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();