Pass goal handle to goal response callback instead of a future

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,20 @@ 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_);
+  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;
       }