Use options struct for passing callbacks to async_send_goal

Now supports callbacks for the goal response and result.
This also makes it easier to incorporate action clients in composable nodes since we don't have to rely on waiting on futures.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

rclcpp_action/include/rclcpp_action/client.hpp

@@ -261,10 +261,44 @@ public:
   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 FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
+  using ResultCallback = typename ClientGoalHandle<ActionT>::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<typename GoalHandle::SharedPtr>
-  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<std::promise<typename GoalHandle::SharedPtr>>();
@@ -318,31 +350,38 @@ public:
     goal_request->goal = goal;
     this->send_goal_request(
       std::static_pointer_cast<void>(goal_request),
-      [this, goal_request, callback, ignore_result, promise](
-        std::shared_ptr<void> response) mutable
+      [this, goal_request, options, promise, future](std::shared_ptr<void> response) mutable
       {
         using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
         auto goal_response = std::static_pointer_cast<GoalResponse>(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<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
-        if (!ignore_result) {
+        std::shared_ptr<GoalHandle> 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<std::mutex> 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;
   }

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<void (
         typename ClientGoalHandle<ActionT>::SharedPtr,
         const std::shared_ptr<const Feedback>)>;
+  using ResultCallback = std::function<void (const WrappedResult & result)>;
 
   virtual ~ClientGoalHandle();
 
@@ -116,7 +118,10 @@ private:
   // The templated Client creates goal handles
   friend Client<ActionT>;
 
-  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<WrappedResult> result_future_;
 
   FeedbackCallback feedback_callback_{nullptr};
+  ResultCallback result_callback_{nullptr};
   int8_t status_{GoalStatus::STATUS_ACCEPTED};
 
   std::mutex handle_mutex_;

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -28,8 +28,11 @@ namespace rclcpp_action
 
 template<typename ActionT>
 ClientGoalHandle<ActionT>::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<typename ActionT>
 const GoalUUID &
 ClientGoalHandle<ActionT>::get_goal_id() const
 {
-  // return info_.goal_id;
   return info_.goal_id.uuid;
 }
 
@@ -71,6 +73,9 @@ ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
   std::lock_guard<std::mutex> guard(handle_mutex_);
   status_ = static_cast<int8_t>(wrapped_result.code);
   result_promise_.set_value(wrapped_result);
+  if (result_callback_) {
+    result_callback_(wrapped_result);
+  }
 }
 
 template<typename ActionT>

rclcpp_action/test/test_client.cpp

@@ -268,20 +268,20 @@ TEST_F(TestClient, construction_and_destruction)
   ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(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<ActionType>(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<ActionType>(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<ActionType>(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<ActionType>::SendGoalOptions();
+  send_goal_ops.goal_response_callback =
+    [&goal_response_received]
+      (std::shared_future<typename ActionGoalHandle::SharedPtr> 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<ActionType>(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<ActionType>::SendGoalOptions();
+  send_goal_ops.feedback_callback =
     [&feedback_count](
-      typename ActionGoalHandle::SharedPtr goal_handle,
-      const std::shared_ptr<const ActionFeedback> feedback) mutable
+    typename ActionGoalHandle::SharedPtr goal_handle,
+    const std::shared_ptr<const ActionFeedback> 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<ActionType>(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<ActionType>::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<ActionType>(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();