diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a46e008f33..43acf3ebf5 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ +#include #include #include #include @@ -37,6 +38,7 @@ #include #include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -261,8 +263,7 @@ class Client : public ClientBase using Feedback = typename ActionT::Feedback; using GoalHandle = ClientGoalHandle; using WrappedResult = typename GoalHandle::WrappedResult; - using GoalResponseCallback = - std::function)>; + using GoalResponseCallback = std::function; using FeedbackCallback = typename GoalHandle::FeedbackCallback; using ResultCallback = typename GoalHandle::ResultCallback; using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; @@ -284,12 +285,9 @@ class Client : public ClientBase /// Function called when the goal is accepted or rejected. /** - * Takes a single argument that is a future to a goal handle shared pointer. + * Takes a single argument that is 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; @@ -360,7 +358,7 @@ class Client : public ClientBase if (!goal_response->accepted) { promise->set_value(nullptr); if (options.goal_response_callback) { - options.goal_response_callback(future); + options.goal_response_callback(nullptr); } return; } @@ -376,16 +374,11 @@ class Client : public ClientBase } promise->set_value(goal_handle); if (options.goal_response_callback) { - options.goal_response_callback(future); + options.goal_response_callback(goal_handle); } if (options.result_callback) { - try { - this->make_result_aware(goal_handle); - } catch (...) { - promise->set_exception(std::current_exception()); - return; - } + this->make_result_aware(goal_handle); } }); @@ -414,7 +407,7 @@ class Client : public ClientBase /// Asynchronously get the result for an active goal. /** * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal - * state. + * state, or if there was an error requesting the result. * \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. @@ -428,6 +421,11 @@ class Client : public ClientBase if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } + if (goal_handle->is_invalidated()) { + // This case can happen if there was a failure to send the result request + // during the goal response callback + throw goal_handle->invalidate_exception_; + } if (result_callback) { // This will override any previously registered callback goal_handle->set_result_callback(result_callback); @@ -518,7 +516,7 @@ class Client : public ClientBase while (it != goal_handles_.end()) { typename GoalHandle::SharedPtr goal_handle = it->second.lock(); if (goal_handle) { - goal_handle->invalidate(); + goal_handle->invalidate(exceptions::UnawareGoalHandleError()); } it = goal_handles_.erase(it); } @@ -632,22 +630,27 @@ class Client : public ClientBase using GoalResultRequest = typename ActionT::Impl::GetResultService::Request; auto goal_result_request = std::make_shared(); goal_result_request->goal_id.uuid = goal_handle->get_goal_id(); - this->send_result_request( - std::static_pointer_cast(goal_result_request), - [goal_handle, this](std::shared_ptr response) mutable - { - // Wrap the response in a struct with the fields a user cares about - WrappedResult wrapped_result; - using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; - auto result_response = std::static_pointer_cast(response); - wrapped_result.result = std::make_shared(); - *wrapped_result.result = result_response->result; - wrapped_result.goal_id = goal_handle->get_goal_id(); - wrapped_result.code = static_cast(result_response->status); - goal_handle->set_result(wrapped_result); - std::lock_guard lock(goal_handles_mutex_); - goal_handles_.erase(goal_handle->get_goal_id()); - }); + try { + this->send_result_request( + std::static_pointer_cast(goal_result_request), + [goal_handle, this](std::shared_ptr response) mutable + { + // Wrap the response in a struct with the fields a user cares about + WrappedResult wrapped_result; + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + auto result_response = std::static_pointer_cast(response); + wrapped_result.result = std::make_shared(); + *wrapped_result.result = result_response->result; + wrapped_result.goal_id = goal_handle->get_goal_id(); + wrapped_result.code = static_cast(result_response->status); + goal_handle->set_result(wrapped_result); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }); + } catch (rclcpp::exceptions::RCLError & ex) { + // This will cause an exception when the user tries to access the result + goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message)); + } } /// \internal diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index ea9f04c6e6..e2861e3e79 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -26,6 +26,7 @@ #include #include +#include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -145,10 +146,15 @@ class ClientGoalHandle set_result(const WrappedResult & wrapped_result); void - invalidate(); + invalidate(const exceptions::UnawareGoalHandleError & ex); + + bool + is_invalidated() const; GoalInfo info_; + std::exception_ptr invalidate_exception_{nullptr}; + bool is_result_aware_{false}; std::promise result_promise_; std::shared_future result_future_; 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 f36c2e471b..0c25e52433 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -138,11 +138,24 @@ ClientGoalHandle::set_result_awareness(bool awareness) template void -ClientGoalHandle::invalidate() +ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) { std::lock_guard guard(handle_mutex_); + // Guard against multiple calls + if (is_invalidated()) { + return; + } + is_result_aware_ = false; + invalidate_exception_ = std::make_exception_ptr(ex); status_ = GoalStatus::STATUS_UNKNOWN; - result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError())); + result_promise_.set_exception(invalidate_exception_); +} + +template +bool +ClientGoalHandle::is_invalidated() const +{ + return invalidate_exception_ != nullptr; } template diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp index 33d94c2804..a1fcf50bff 100644 --- a/rclcpp_action/include/rclcpp_action/exceptions.hpp +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -16,6 +16,7 @@ #define RCLCPP_ACTION__EXCEPTIONS_HPP_ #include +#include namespace rclcpp_action { @@ -33,8 +34,9 @@ class UnknownGoalHandleError : public std::invalid_argument class UnawareGoalHandleError : public std::runtime_error { public: - UnawareGoalHandleError() - : std::runtime_error("Goal handle is not tracking the goal result.") + UnawareGoalHandleError( + const std::string & message = "Goal handle is not tracking the goal result.") + : std::runtime_error(message) { } }; diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 84c9dbe7d0..13cd07add4 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -435,9 +435,8 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.goal_response_callback = [&goal_response_received] - (std::shared_future future) mutable + (typename ActionGoalHandle::SharedPtr goal_handle) mutable { - auto goal_handle = future.get(); if (goal_handle) { goal_response_received = true; }