diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8b90c6d..910f304 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -267,8 +267,7 @@ public: using ResultCallback = typename GoalHandle::ResultCallback; using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; - using CancelOneCallback = std::function; - using CancelMultipleCallback = std::function; + using CancelCallback = std::function; /// Options for sending a goal. /** @@ -422,42 +421,26 @@ public: * terminal state. * \param[in] goal_handle The goal handle requesting to be canceled. * \param[in] cancel_callback Optional callback that is called when the response is received. - * The callback function takes two parameters: a shared pointer to the goal handle and a bool - * indicating if the action server accepted the cancel request or not. - * \return A future whose result indicates whether or not the cancel request was accepted. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. */ - std::shared_future + std::shared_future async_cancel_goal( typename GoalHandle::SharedPtr goal_handle, - CancelOneCallback cancel_callback = nullptr) + CancelCallback cancel_callback = nullptr) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } - // Put promise in the heap to move it around. - auto promise = std::make_shared>(); - std::shared_future future(promise->get_future()); auto cancel_request = std::make_shared(); // cancel_request->goal_info.goal_id = goal_handle->get_goal_id(); cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); - this->send_cancel_request( - std::static_pointer_cast(cancel_request), - [goal_handle, cancel_callback, promise](std::shared_ptr response) mutable - { - auto cancel_response = std::static_pointer_cast(response); - bool goal_canceled = false; - if (!cancel_response->goals_canceling.empty()) { - const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0]; - // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id()); - goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id()); - } - promise->set_value(goal_canceled); - if (cancel_callback) { - cancel_callback(goal_handle, goal_canceled); - } - }); - return future; + return async_cancel(cancel_request, cancel_callback); } /// Asynchronously request for all goals to be canceled. @@ -471,7 +454,7 @@ public: * action_msgs/CancelGoal.srv. */ std::shared_future - async_cancel_all_goals(CancelMultipleCallback cancel_callback = nullptr) + async_cancel_all_goals(CancelCallback cancel_callback = nullptr) { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); @@ -495,7 +478,7 @@ public: std::shared_future async_cancel_goals_before( const rclcpp::Time & stamp, - CancelMultipleCallback cancel_callback = nullptr) + CancelCallback cancel_callback = nullptr) { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); @@ -636,7 +619,7 @@ private: std::shared_future async_cancel( typename CancelRequest::SharedPtr cancel_request, - CancelMultipleCallback cancel_callback = nullptr) + CancelCallback cancel_callback = nullptr) { // Put promise in the heap to move it around. auto promise = std::make_shared>(); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 025495d..c7237cd 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -325,6 +325,9 @@ ServerBase::execute_cancel_request_received() pimpl_->action_server_.get(), &cancel_request, &cancel_response); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } RCLCPP_SCOPE_EXIT({ ret = rcl_action_cancel_response_fini(&cancel_response); @@ -335,6 +338,7 @@ ServerBase::execute_cancel_request_received() auto response = std::make_shared(); + response->return_code = cancel_response.msg.return_code; auto & goals = cancel_response.msg.goals_canceling; // For each canceled goal, call cancel callback for (size_t i = 0; i < goals.size; ++i) { @@ -351,6 +355,12 @@ ServerBase::execute_cancel_request_received() } } + // If the user rejects all individual requests to cancel goals, + // then we consider the top-level cancel request as rejected. + if (goals.size >= 1u && 0u == response->goals_canceling.size()) { + response->return_code = action_msgs::srv::CancelGoal::Response::ERROR_REJECTED; + } + if (!response->goals_canceling.empty()) { // at least one goal state changed, publish a new status message publish_status(); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 5d402af..bae1720 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -464,8 +464,8 @@ TEST_F(TestClient, async_cancel_one_goal) auto future_cancel = action_client->async_cancel_goal(goal_handle); dual_spin_until_future_complete(future_cancel); - bool goal_canceled = future_cancel.get(); - EXPECT_TRUE(goal_canceled); + ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); } TEST_F(TestClient, async_cancel_one_goal_with_callback) @@ -484,18 +484,21 @@ TEST_F(TestClient, async_cancel_one_goal_with_callback) auto future_cancel = action_client->async_cancel_goal( goal_handle, [&cancel_response_received, goal_handle]( - typename ActionGoalHandle::SharedPtr goal_handle_canceled, bool cancel_accepted) mutable + ActionCancelGoalResponse::SharedPtr response) mutable { if ( - goal_handle_canceled->get_goal_id() == goal_handle->get_goal_id() && - cancel_accepted) + ActionCancelGoalResponse::ERROR_NONE == response->return_code && + 1ul == response->goals_canceling.size() && + goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid) { cancel_response_received = true; } }); dual_spin_until_future_complete(future_cancel); - bool goal_canceled = future_cancel.get(); - EXPECT_TRUE(goal_canceled); + auto cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); EXPECT_TRUE(cancel_response_received); } @@ -527,6 +530,7 @@ TEST_F(TestClient, async_cancel_all_goals) dual_spin_until_future_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid); @@ -575,6 +579,7 @@ TEST_F(TestClient, async_cancel_all_goals_with_callback) dual_spin_until_future_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); EXPECT_TRUE(cancel_callback_received); ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); @@ -608,6 +613,7 @@ TEST_F(TestClient, async_cancel_some_goals) dual_spin_until_future_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); @@ -649,6 +655,7 @@ TEST_F(TestClient, async_cancel_some_goals_with_callback) dual_spin_until_future_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); EXPECT_TRUE(cancel_callback_received); ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 398f91d..0fe664b 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -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( @@ -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("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) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(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("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) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server(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("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) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + handle->succeed(std::make_shared()); + }; + + auto as = rclcpp_action::create_server(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("status_accept_node", "/rclcpp_action/status_accept");