From 96025916dc1c04a086512422bf05b04db1e68d31 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 11 Sep 2020 12:20:50 -0700 Subject: [PATCH 1/6] Pass goal handle to goal response callback instead of a future This resolves an issue where `promise->set_value` is called before a potential call to `promise->set_exception`. If there is an issue sending a result request, set the exception on the future to the result in the goal handle, instead of the future to the goal handle itself. Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 59 +++++++++---------- rclcpp_action/test/test_client.cpp | 3 +- 2 files changed, 30 insertions(+), 32 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a46e008f33..8c969e45b8 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); } }); @@ -632,22 +625,28 @@ 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 &) { + goal_handle->invalidate(); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + } } /// \internal 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; } From a920315486cdca9f255e78659b48d4147890e985 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 14 Sep 2020 11:42:20 -0700 Subject: [PATCH 2/6] Do not remove goal handle from list if result request fails This way the user can still interact with the goal (e.g. send a cancel request). Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8c969e45b8..0bcc986b30 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -643,9 +643,8 @@ class Client : public ClientBase goal_handles_.erase(goal_handle->get_goal_id()); }); } catch (rclcpp::exceptions::RCLError &) { + // This will cause an exception when the user tries to access the result goal_handle->invalidate(); - std::lock_guard lock(goal_handles_mutex_); - goal_handles_.erase(goal_handle->get_goal_id()); } } From 973da954cce13774cefb4b1bb9f81d03a0e8e601 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 14 Sep 2020 11:44:17 -0700 Subject: [PATCH 3/6] Set the result awareness to false if goal handle is invalidated This will cause an exception when trying to get the future to result, in addition to the exception when trying to access values for existing references to the future. Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp | 1 + 1 file changed, 1 insertion(+) 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..03b6f832ce 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -142,6 +142,7 @@ ClientGoalHandle::invalidate() { std::lock_guard guard(handle_mutex_); status_ = GoalStatus::STATUS_UNKNOWN; + is_result_aware_ = false; result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError())); } From 68cd6e9e3fb0ccd0411a83e5d2a318ccd7377890 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 14 Sep 2020 16:50:41 -0700 Subject: [PATCH 4/6] Revert "Set the result awareness to false if goal handle is invalidated" This reverts commit d444e09131c42d6eece1338443b8ffb4f5f17370. --- rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp | 1 - 1 file changed, 1 deletion(-) 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 03b6f832ce..f36c2e471b 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -142,7 +142,6 @@ ClientGoalHandle::invalidate() { std::lock_guard guard(handle_mutex_); status_ = GoalStatus::STATUS_UNKNOWN; - is_result_aware_ = false; result_promise_.set_exception(std::make_exception_ptr(exceptions::UnawareGoalHandleError())); } From 9cb17766a66ed6e75292a6a7d8649ca8e2dcb3eb Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 15 Sep 2020 11:13:42 -0700 Subject: [PATCH 5/6] Throw from Client::async_get_result if the goal handle was invalidated due to a failed result request Propagate error message from a failed result request. Also set result awareness to false if the result request fails so the user can also check before being hit with an exception. Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client.hpp | 13 +++++++++---- .../include/rclcpp_action/client_goal_handle.hpp | 8 +++++++- .../rclcpp_action/client_goal_handle_impl.hpp | 13 +++++++++++-- rclcpp_action/include/rclcpp_action/exceptions.hpp | 6 ++++-- 4 files changed, 31 insertions(+), 9 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 0bcc986b30..43acf3ebf5 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -407,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. @@ -421,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); @@ -511,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); } @@ -642,9 +647,9 @@ class Client : public ClientBase std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); - } catch (rclcpp::exceptions::RCLError &) { + } catch (rclcpp::exceptions::RCLError & ex) { // This will cause an exception when the user tries to access the result - goal_handle->invalidate(); + goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message)); } } 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..c4923974d2 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,20 @@ ClientGoalHandle::set_result_awareness(bool awareness) template void -ClientGoalHandle::invalidate() +ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) { std::lock_guard guard(handle_mutex_); + 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) { } }; From b145c738aacdf6b5bddfcf57883021e4a47b2f01 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 16 Sep 2020 14:48:18 -0700 Subject: [PATCH 6/6] Guard against mutliple calls to invalidate Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client_goal_handle_impl.hpp | 4 ++++ 1 file changed, 4 insertions(+) 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 c4923974d2..0c25e52433 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -141,6 +141,10 @@ void 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;