diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 71839208bf..8b90c6df75 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -261,9 +261,45 @@ class Client : public ClientBase using Feedback = typename ActionT::Feedback; using GoalHandle = ClientGoalHandle; using WrappedResult = typename GoalHandle::WrappedResult; - using FeedbackCallback = typename ClientGoalHandle::FeedbackCallback; + using GoalResponseCallback = + std::function)>; + using FeedbackCallback = typename GoalHandle::FeedbackCallback; + 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. + /** + * This struct is used to pass parameters to the function `async_send_goal`. + */ + struct SendGoalOptions + { + SendGoalOptions() + : goal_response_callback(nullptr), + feedback_callback(nullptr), + result_callback(nullptr) + { + } + + /// Function called when the goal is accepted or rejected. + /** + * Takes a single argument that is a future to a goal handle shared pointer. + * If the goal is accepted, then the pointer points to a valid goal handle. + * If the goal is rejected, then pointer has the value `nullptr`. + * If an error occurs while waiting for the goal response an exception will be thrown + * when calling `future::get()`. + * Possible exceptions include `rclcpp::RCLError` and `rclcpp::RCLBadAlloc`. + */ + GoalResponseCallback goal_response_callback; + + /// Function called whenever feedback is received for the goal. + FeedbackCallback feedback_callback; + + /// Function called when the result for the goal is received. + ResultCallback result_callback; + }; /// Construct an action client. /** @@ -299,15 +335,13 @@ class Client : public ClientBase * The goal handle is used to monitor the status of the goal and get the final result. * * \param[in] goal The goal request. - * \param[in] callback Optional user callback for feedback associated with the goal. - * \param[in] ignore_result If `true`, then the result for the goal will not be requested and - * therefore inaccessible from the goal handle. + * \param[in] options Options for sending the goal request. Contains references to callbacks for + * the goal response (accepted/rejected), feedback, and the final result. * \return A future that completes when the goal has been accepted or rejected. * If the goal is rejected, then the result will be a `nullptr`. */ std::shared_future - async_send_goal( - const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) + async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) { // Put promise in the heap to move it around. auto promise = std::make_shared>(); @@ -318,31 +352,38 @@ class Client : public ClientBase goal_request->goal = goal; this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, callback, ignore_result, promise]( - std::shared_ptr response) mutable + [this, goal_request, options, promise, future](std::shared_ptr response) mutable { using GoalResponse = typename ActionT::Impl::SendGoalService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { promise->set_value(nullptr); + if (options.goal_response_callback) { + options.goal_response_callback(future); + } return; } GoalInfo goal_info; goal_info.goal_id.uuid = goal_request->goal_id.uuid; goal_info.stamp = goal_response->stamp; // Do not use std::make_shared as friendship cannot be forwarded. - std::shared_ptr goal_handle(new GoalHandle(goal_info, callback)); - if (!ignore_result) { + std::shared_ptr goal_handle( + new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); + if (options.result_callback) { try { this->make_result_aware(goal_handle); } catch (...) { promise->set_exception(std::current_exception()); + options.goal_response_callback(future); return; } } std::lock_guard guard(goal_handles_mutex_); goal_handles_[goal_handle->get_goal_id()] = goal_handle; promise->set_value(goal_handle); + if (options.goal_response_callback) { + options.goal_response_callback(future); + } }); return future; } @@ -352,15 +393,22 @@ class Client : public ClientBase * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal * state. * \param[in] goal_handle The goal handle for which to get the result. + * \param[in] result_callback Optional callback that is called when the result is received. * \return A future that is set to the goal result when the goal is finished. */ std::shared_future - async_get_result(typename GoalHandle::SharedPtr goal_handle) + async_get_result( + typename GoalHandle::SharedPtr goal_handle, + ResultCallback result_callback = nullptr) { std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } + if (result_callback) { + // This will override any previously registered callback + goal_handle->set_result_callback(result_callback); + } // If the user chose to ignore the result before, then ask the server for the result now. if (!goal_handle->is_result_aware()) { this->make_result_aware(goal_handle); @@ -373,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) { @@ -390,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; @@ -400,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 @@ -413,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 @@ -433,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); @@ -441,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 @@ -572,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/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index c6522f88a7..28210fad5b 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -72,10 +72,12 @@ class ClientGoalHandle } WrappedResult; using Feedback = typename ActionT::Feedback; + using Result = typename ActionT::Result; using FeedbackCallback = std::function::SharedPtr, const std::shared_ptr)>; + using ResultCallback = std::function; virtual ~ClientGoalHandle(); @@ -116,11 +118,17 @@ class ClientGoalHandle // The templated Client creates goal handles friend Client; - ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback); + ClientGoalHandle( + const GoalInfo & info, + FeedbackCallback feedback_callback, + ResultCallback result_callback); void set_feedback_callback(FeedbackCallback callback); + void + set_result_callback(ResultCallback callback); + void call_feedback_callback( typename ClientGoalHandle::SharedPtr shared_this, @@ -145,6 +153,7 @@ class ClientGoalHandle std::shared_future result_future_; FeedbackCallback feedback_callback_{nullptr}; + ResultCallback result_callback_{nullptr}; int8_t status_{GoalStatus::STATUS_ACCEPTED}; std::mutex handle_mutex_; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 73f850dd47..2a491014f1 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -28,8 +28,11 @@ namespace rclcpp_action template ClientGoalHandle::ClientGoalHandle( - const GoalInfo & info, FeedbackCallback callback) -: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback) + const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback) +: info_(info), + result_future_(result_promise_.get_future()), + feedback_callback_(feedback_callback), + result_callback_(result_callback) { } @@ -42,7 +45,6 @@ template const GoalUUID & ClientGoalHandle::get_goal_id() const { - // return info_.goal_id; return info_.goal_id.uuid; } @@ -71,6 +73,9 @@ ClientGoalHandle::set_result(const WrappedResult & wrapped_result) std::lock_guard guard(handle_mutex_); status_ = static_cast(wrapped_result.code); result_promise_.set_value(wrapped_result); + if (result_callback_) { + result_callback_(wrapped_result); + } } template @@ -81,6 +86,14 @@ ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) feedback_callback_ = callback; } +template +void +ClientGoalHandle::set_result_callback(ResultCallback callback) +{ + std::lock_guard guard(handle_mutex_); + result_callback_ = callback; +} + template int8_t ClientGoalHandle::get_status() diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 32afe75310..5d402afaf9 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -268,20 +268,20 @@ TEST_F(TestClient, construction_and_destruction) ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); } -TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) +TEST_F(TestClient, async_send_goal_no_callbacks) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); ActionGoal bad_goal; bad_goal.order = -5; - auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true); + auto future_goal_handle = action_client->async_send_goal(bad_goal); dual_spin_until_future_complete(future_goal_handle); EXPECT_EQ(nullptr, future_goal_handle.get().get()); ActionGoal good_goal; good_goal.order = 5; - future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true); + future_goal_handle = action_client->async_send_goal(good_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()); @@ -290,7 +290,7 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result) EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError); } -TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) +TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -302,8 +302,9 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - auto future_result = goal_handle->async_result(); dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(6ul, wrapped_result.result->sequence.size()); @@ -312,7 +313,41 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) EXPECT_EQ(5, wrapped_result.result->sequence[5]); } -TEST_F(TestClient, async_send_goal_with_feedback_and_result) +TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result) +{ + 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 = 4; + bool goal_response_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.goal_response_callback = + [&goal_response_received] + (std::shared_future future) mutable + { + auto goal_handle = future.get(); + if (goal_handle) { + goal_response_received = true; + } + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); @@ -320,22 +355,24 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result) ActionGoal goal; goal.order = 4; int feedback_count = 0; - auto future_goal_handle = action_client->async_send_goal( - goal, + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.feedback_callback = [&feedback_count]( - typename ActionGoalHandle::SharedPtr goal_handle, - const std::shared_ptr feedback) mutable + typename ActionGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) mutable { (void)goal_handle; (void)feedback; feedback_count++; - }); + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); 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()); EXPECT_TRUE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - auto future_result = goal_handle->async_result(); dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); @@ -344,6 +381,75 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result) EXPECT_EQ(5, feedback_count); } +TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result) +{ + 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 = 4; + bool result_callback_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.result_callback = + [&result_callback_received]( + const typename ActionGoalHandle::WrappedResult & result) mutable + { + if (rclcpp_action::ResultCode::SUCCEEDED == result.code && + result.result->sequence.size() == 5u) + { + result_callback_received = true; + } + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + 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()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_TRUE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClient, async_get_result_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 = 4; + 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_NE(goal_handle, nullptr); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + bool result_callback_received = false; + auto future_result = action_client->async_get_result( + goal_handle, + [&result_callback_received]( + const typename ActionGoalHandle::WrappedResult & result) mutable + { + if ( + rclcpp_action::ResultCode::SUCCEEDED == result.code && + result.result->sequence.size() == 5u) + { + result_callback_received = true; + } + }); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + TEST_F(TestClient, async_cancel_one_goal) { auto action_client = rclcpp_action::create_client(client_node, action_name); @@ -351,7 +457,7 @@ TEST_F(TestClient, async_cancel_one_goal) ActionGoal goal; goal.order = 5; - auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true); + 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()); @@ -362,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); @@ -369,14 +506,14 @@ TEST_F(TestClient, async_cancel_all_goals) ActionGoal goal; goal.order = 6; - auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + 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, nullptr, true); + 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(); @@ -397,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); @@ -404,14 +590,14 @@ TEST_F(TestClient, async_cancel_some_goals) ActionGoal goal; goal.order = 6; - auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true); + 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, nullptr, true); + 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(); @@ -426,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()); +}