Pass goal handle to goal response callback instead of a future (#1311)

* 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 <[email protected]>

* 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 <[email protected]>

* 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 <[email protected]>

* Revert "Set the result awareness to false if goal handle is invalidated"

This reverts commit d444e09.

* 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 <[email protected]>

* Guard against mutliple calls to invalidate

Signed-off-by: Jacob Perron <[email protected]>

Author: jacobperron

rclcpp_action/include/rclcpp_action/client.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP_ACTION__CLIENT_HPP_
 #define RCLCPP_ACTION__CLIENT_HPP_
 
+#include <rclcpp/exceptions.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/node_interfaces/node_base_interface.hpp>
 #include <rclcpp/node_interfaces/node_logging_interface.hpp>
@@ -37,6 +38,7 @@
 #include <utility>
 
 #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<ActionT>;
   using WrappedResult = typename GoalHandle::WrappedResult;
-  using GoalResponseCallback =
-    std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
+  using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
   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<GoalResultRequest>();
     goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
-    this->send_result_request(
-      std::static_pointer_cast<void>(goal_result_request),
-      [goal_handle, this](std::shared_ptr<void> 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<GoalResultResponse>(response);
-        wrapped_result.result = std::make_shared<typename ActionT::Result>();
-        *wrapped_result.result = result_response->result;
-        wrapped_result.goal_id = goal_handle->get_goal_id();
-        wrapped_result.code = static_cast<ResultCode>(result_response->status);
-        goal_handle->set_result(wrapped_result);
-        std::lock_guard<std::mutex> lock(goal_handles_mutex_);
-        goal_handles_.erase(goal_handle->get_goal_id());
-      });
+    try {
+      this->send_result_request(
+        std::static_pointer_cast<void>(goal_result_request),
+        [goal_handle, this](std::shared_ptr<void> 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<GoalResultResponse>(response);
+          wrapped_result.result = std::make_shared<typename ActionT::Result>();
+          *wrapped_result.result = result_response->result;
+          wrapped_result.goal_id = goal_handle->get_goal_id();
+          wrapped_result.code = static_cast<ResultCode>(result_response->status);
+          goal_handle->set_result(wrapped_result);
+          std::lock_guard<std::mutex> 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

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -26,6 +26,7 @@
 #include <memory>
 #include <mutex>
 
+#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<WrappedResult> result_promise_;
   std::shared_future<WrappedResult> result_future_;

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -138,11 +138,24 @@ ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
 
 template<typename ActionT>
 void
-ClientGoalHandle<ActionT>::invalidate()
+ClientGoalHandle<ActionT>::invalidate(const exceptions::UnawareGoalHandleError & ex)
 {
   std::lock_guard<std::mutex> 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<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_invalidated() const
+{
+  return invalidate_exception_ != nullptr;
 }
 
 template<typename ActionT>

rclcpp_action/include/rclcpp_action/exceptions.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP_ACTION__EXCEPTIONS_HPP_
 
 #include <stdexcept>
+#include <string>
 
 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)
   {
   }
 };

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<ActionType>::SendGoalOptions();
   send_goal_ops.goal_response_callback =
     [&goal_response_received]
-      (std::shared_future<typename ActionGoalHandle::SharedPtr> future) mutable
+      (typename ActionGoalHandle::SharedPtr goal_handle) mutable
     {
-      auto goal_handle = future.get();
       if (goal_handle) {
         goal_response_received = true;
       }