From 0da966b9814214d0077be49ad5686fd8d7012076 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 17 Apr 2019 22:23:55 -0700 Subject: [PATCH] Use options struct for passing callbacks to async_send_goal Now supports callbacks for the goal response and result. This also makes it easier to incorporate action clients in composable nodes since we don't have to rely on waiting on futures. Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 57 ++++++++-- .../rclcpp_action/client_goal_handle.hpp | 8 +- .../rclcpp_action/client_goal_handle_impl.hpp | 11 +- rclcpp_action/test/test_client.cpp | 105 +++++++++++++++--- 4 files changed, 151 insertions(+), 30 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 7183920..a9cb8e6 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -261,10 +261,44 @@ public: using Feedback = typename ActionT::Feedback; using GoalHandle = ClientGoalHandle; using WrappedResult = typename GoalHandle::WrappedResult; + using GoalResponseCallback = + std::function)>; using FeedbackCallback = typename ClientGoalHandle::FeedbackCallback; + using ResultCallback = typename ClientGoalHandle::ResultCallback; using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; + /// 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. /** * This constructs an action client, but it will not work until it has been added to a node. @@ -299,15 +333,13 @@ public: * 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 +350,38 @@ public: 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; } diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index c6522f8..f367fef 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 @@ public: } 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,7 +118,10 @@ private: // 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); @@ -145,6 +150,7 @@ private: 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 73f850d..23fb200 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 diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 32afe75..61d07da 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,40 @@ 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_cancel_one_goal) { auto action_client = rclcpp_action::create_client(client_node, action_name); @@ -351,7 +422,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()); @@ -369,14 +440,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(); @@ -404,14 +475,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();