From 65160edbad309c1bc27b5c0ee84865f3e71ea0d6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 23 Apr 2019 17:11:32 -0700 Subject: [PATCH] Add optional cancel callback to asynchronous cancel goal methods Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 37 ++++-- rclcpp_action/test/test_client.cpp | 122 ++++++++++++++++++ 2 files changed, 151 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 36e01933a7..8b90c6df75 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -267,6 +267,8 @@ class Client : public ClientBase 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; /// Options for sending a goal. /** @@ -419,10 +421,15 @@ class Client : public ClientBase * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a * 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. */ std::shared_future - async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) + async_cancel_goal( + typename GoalHandle::SharedPtr goal_handle, + CancelOneCallback cancel_callback = nullptr) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { @@ -436,7 +443,7 @@ class Client : public ClientBase cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [goal_handle, promise](std::shared_ptr response) mutable + [goal_handle, cancel_callback, promise](std::shared_ptr response) mutable { auto cancel_response = std::static_pointer_cast(response); bool goal_canceled = false; @@ -446,12 +453,17 @@ class Client : public ClientBase 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; } /// Asynchronously request for all goals to be canceled. /** + * \param[in] cancel_callback Optional callback that is called when the response is received. + * 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 @@ -459,19 +471,21 @@ class Client : public ClientBase * action_msgs/CancelGoal.srv. */ std::shared_future - async_cancel_all_goals() + async_cancel_all_goals(CancelMultipleCallback cancel_callback = nullptr) { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); std::fill( cancel_request->goal_info.goal_id.uuid.begin(), cancel_request->goal_info.goal_id.uuid.end(), 0u); - return async_cancel(cancel_request); + return async_cancel(cancel_request, cancel_callback); } /// Asynchronously request all goals at or before a specified time be canceled. /** * \param[in] stamp The timestamp for the cancel goal request. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * 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 @@ -479,7 +493,9 @@ class Client : public ClientBase * action_msgs/CancelGoal.srv. */ std::shared_future - async_cancel_goals_before(const rclcpp::Time & stamp) + async_cancel_goals_before( + const rclcpp::Time & stamp, + CancelMultipleCallback cancel_callback = nullptr) { auto cancel_request = std::make_shared(); // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); @@ -487,7 +503,7 @@ class Client : public ClientBase cancel_request->goal_info.goal_id.uuid.begin(), cancel_request->goal_info.goal_id.uuid.end(), 0u); cancel_request->goal_info.stamp = stamp; - return async_cancel(cancel_request); + return async_cancel(cancel_request, cancel_callback); } virtual @@ -618,17 +634,22 @@ class Client : public ClientBase /// \internal std::shared_future - async_cancel(typename CancelRequest::SharedPtr cancel_request) + async_cancel( + typename CancelRequest::SharedPtr cancel_request, + CancelMultipleCallback cancel_callback = nullptr) { // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); this->send_cancel_request( std::static_pointer_cast(cancel_request), - [promise](std::shared_ptr response) mutable + [cancel_callback, promise](std::shared_ptr response) mutable { auto cancel_response = std::static_pointer_cast(response); promise->set_value(cancel_response); + if (cancel_callback) { + cancel_callback(cancel_response); + } }); return future; } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index e17ba6a73b..5d402afaf9 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -468,6 +468,37 @@ TEST_F(TestClient, async_cancel_one_goal) EXPECT_TRUE(goal_canceled); } +TEST_F(TestClient, async_cancel_one_goal_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + + bool cancel_response_received = false; + 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 + { + if ( + goal_handle_canceled->get_goal_id() == goal_handle->get_goal_id() && + cancel_accepted) + { + cancel_response_received = true; + } + }); + dual_spin_until_future_complete(future_cancel); + bool goal_canceled = future_cancel.get(); + EXPECT_TRUE(goal_canceled); + EXPECT_TRUE(cancel_response_received); +} + TEST_F(TestClient, async_cancel_all_goals) { auto action_client = rclcpp_action::create_client(client_node, action_name); @@ -503,6 +534,55 @@ TEST_F(TestClient, async_cancel_all_goals) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); } +TEST_F(TestClient, async_cancel_all_goals_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { + goal_handle0.swap(goal_handle1); + } + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + bool cancel_callback_received = false; + auto future_cancel_all = action_client->async_cancel_all_goals( + [&cancel_callback_received, goal_handle0, goal_handle1]( + ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 2ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid && + goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + 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); + EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); +} + TEST_F(TestClient, async_cancel_some_goals) { auto action_client = rclcpp_action::create_client(client_node, action_name); @@ -532,3 +612,45 @@ TEST_F(TestClient, async_cancel_some_goals) 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()); } + +TEST_F(TestClient, async_cancel_some_goals_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + bool cancel_callback_received = false; + auto future_cancel_some = action_client->async_cancel_goals_before( + goal_handle1->get_goal_stamp(), + [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 1ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_some); + auto cancel_response = future_cancel_some.get(); + + 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); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); +}