niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add return code to CancelGoal service response (#710)

Browse source 
* Populate return code of CancelGoal service response

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Throw if there is an error processing a cancel goal request

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Make cancel callback signature consistent across cancel methods and add tests

Refactored the callback signature for canceling one goal. Now it is the same as the other cancel methods.
This makes it easier to communicate the error code to the user.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Address review

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
This commit is contained in:
Jacob Perron 2019-05-02 15:25:15 -07:00 • committed by GitHub
parent 7ed130cf7a
commit ecf35114b6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 165 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

129
rclcpp_action/test/test_server.cpp
View file

@ -26,6 +26,7 @@
#include "rclcpp_action/server.hpp"
using Fibonacci = test_msgs::action::Fibonacci;
using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response;
using GoalUUID = rclcpp_action::GoalUUID;
class TestServer : public ::testing::Test
@ -55,7 +56,7 @@ protected:
return request;
}
void
CancelResponse::SharedPtr
send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
{
auto cancel_client = node->create_client<Fibonacci::Impl::CancelGoalService>(
@ -71,6 +72,7 @@ protected:
{
throw std::runtime_error("cancel goal future didn't complete succesfully");
}
return future.get();
}
};
@ -207,6 +209,131 @@ TEST_F(TestServer, handle_cancel_called)
EXPECT_TRUE(received_handle->is_canceling());
}
TEST_F(TestServer, handle_cancel_reject)
{
auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
auto handle_goal = [](
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
};
using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
auto handle_cancel = [](std::shared_ptr<GoalHandle>)
{
return rclcpp_action::CancelResponse::REJECT;
};
std::shared_ptr<GoalHandle> received_handle;
auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
{
received_handle = handle;
};
auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
handle_goal,
handle_cancel,
handle_accepted);
(void)as;
send_goal_request(node, uuid);
ASSERT_TRUE(received_handle);
EXPECT_EQ(uuid, received_handle->get_goal_id());
EXPECT_FALSE(received_handle->is_canceling());
auto response_ptr = send_cancel_request(node, uuid);
EXPECT_FALSE(received_handle->is_canceling());
EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code);
}
TEST_F(TestServer, handle_cancel_unknown_goal)
{
auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
const GoalUUID unknown_uuid{{11, 22, 33, 44, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}};
auto handle_goal = [](
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
};
using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
auto handle_cancel = [](std::shared_ptr<GoalHandle>)
{
return rclcpp_action::CancelResponse::ACCEPT;
};
std::shared_ptr<GoalHandle> received_handle;
auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
{
received_handle = handle;
};
auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
handle_goal,
handle_cancel,
handle_accepted);
(void)as;
send_goal_request(node, uuid);
ASSERT_TRUE(received_handle);
EXPECT_EQ(uuid, received_handle->get_goal_id());
EXPECT_FALSE(received_handle->is_canceling());
auto response_ptr = send_cancel_request(node, unknown_uuid);
EXPECT_FALSE(received_handle->is_canceling());
EXPECT_EQ(CancelResponse::ERROR_UNKNOWN_GOAL_ID, response_ptr->return_code);
}
TEST_F(TestServer, handle_cancel_terminated_goal)
{
auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
auto handle_goal = [](
const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
{
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
};
using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
auto handle_cancel = [](std::shared_ptr<GoalHandle>)
{
return rclcpp_action::CancelResponse::ACCEPT;
};
std::shared_ptr<GoalHandle> received_handle;
auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
{
received_handle = handle;
handle->succeed(std::make_shared<Fibonacci::Result>());
};
auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
handle_goal,
handle_cancel,
handle_accepted);
(void)as;
send_goal_request(node, uuid);
ASSERT_TRUE(received_handle);
EXPECT_EQ(uuid, received_handle->get_goal_id());
EXPECT_FALSE(received_handle->is_canceling());
auto response_ptr = send_cancel_request(node, uuid);
EXPECT_FALSE(received_handle->is_canceling());
EXPECT_EQ(CancelResponse::ERROR_GOAL_TERMINATED, response_ptr->return_code);
}
TEST_F(TestServer, publish_status_accepted)
{
auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept");