Add optional cancel callback to asynchronous cancel goal methods

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

rclcpp_action/include/rclcpp_action/client.hpp

@@ -267,6 +267,8 @@ public:
   using ResultCallback = typename GoalHandle::ResultCallback;
   using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
   using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
+  using CancelOneCallback = std::function<void (typename GoalHandle::SharedPtr, bool)>;
+  using CancelMultipleCallback = std::function<void (typename CancelResponse::SharedPtr)>;
 
   /// Options for sending a goal.
   /**
@@ -419,10 +421,15 @@ public:
    * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a
    *   terminal state.
    * \param[in] goal_handle The goal handle requesting to be canceled.
+   * \param[in] cancel_callback Optional callback that is called when the response is received.
+   *   The callback function takes two parameters: a shared pointer to the goal handle and a bool
+   *   indicating if the action server accepted the cancel request or not.
    * \return A future whose result indicates whether or not the cancel request was accepted.
    */
   std::shared_future<bool>
-  async_cancel_goal(typename GoalHandle::SharedPtr goal_handle)
+  async_cancel_goal(
+    typename GoalHandle::SharedPtr goal_handle,
+    CancelOneCallback cancel_callback = nullptr)
   {
     std::lock_guard<std::mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
@@ -436,7 +443,7 @@ public:
     cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
     this->send_cancel_request(
       std::static_pointer_cast<void>(cancel_request),
-      [goal_handle, promise](std::shared_ptr<void> response) mutable
+      [goal_handle, cancel_callback, promise](std::shared_ptr<void> response) mutable
       {
         auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
         bool goal_canceled = false;
@@ -446,12 +453,17 @@ public:
           goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id());
         }
         promise->set_value(goal_canceled);
+        if (cancel_callback) {
+          cancel_callback(goal_handle, goal_canceled);
+        }
       });
     return future;
   }
 
   /// Asynchronously request for all goals to be canceled.
   /**
+   * \param[in] cancel_callback Optional callback that is called when the response is received.
+   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
    * \return A future to a CancelResponse message that is set when the request has been
    * acknowledged by an action server.
    * See
@@ -459,19 +471,21 @@ public:
    * action_msgs/CancelGoal.srv</a>.
    */
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel_all_goals()
+  async_cancel_all_goals(CancelMultipleCallback cancel_callback = nullptr)
   {
     auto cancel_request = std::make_shared<CancelRequest>();
     // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
     std::fill(
       cancel_request->goal_info.goal_id.uuid.begin(),
       cancel_request->goal_info.goal_id.uuid.end(), 0u);
-    return async_cancel(cancel_request);
+    return async_cancel(cancel_request, cancel_callback);
   }
 
   /// Asynchronously request all goals at or before a specified time be canceled.
   /**
    * \param[in] stamp The timestamp for the cancel goal request.
+   * \param[in] cancel_callback Optional callback that is called when the response is received.
+   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
    * \return A future to a CancelResponse message that is set when the request has been
    * acknowledged by an action server.
    * See
@@ -479,7 +493,9 @@ public:
    * action_msgs/CancelGoal.srv</a>.
    */
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel_goals_before(const rclcpp::Time & stamp)
+  async_cancel_goals_before(
+    const rclcpp::Time & stamp,
+    CancelMultipleCallback cancel_callback = nullptr)
   {
     auto cancel_request = std::make_shared<CancelRequest>();
     // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
@@ -487,7 +503,7 @@ public:
       cancel_request->goal_info.goal_id.uuid.begin(),
       cancel_request->goal_info.goal_id.uuid.end(), 0u);
     cancel_request->goal_info.stamp = stamp;
-    return async_cancel(cancel_request);
+    return async_cancel(cancel_request, cancel_callback);
   }
 
   virtual
@@ -618,17 +634,22 @@ private:
 
   /// \internal
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel(typename CancelRequest::SharedPtr cancel_request)
+  async_cancel(
+    typename CancelRequest::SharedPtr cancel_request,
+    CancelMultipleCallback cancel_callback = nullptr)
   {
     // Put promise in the heap to move it around.
     auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
     std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
     this->send_cancel_request(
       std::static_pointer_cast<void>(cancel_request),
-      [promise](std::shared_ptr<void> response) mutable
+      [cancel_callback, promise](std::shared_ptr<void> response) mutable
       {
         auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
         promise->set_value(cancel_response);
+        if (cancel_callback) {
+          cancel_callback(cancel_response);
+        }
       });
     return future;
   }

rclcpp_action/test/test_client.cpp

@@ -468,6 +468,37 @@ TEST_F(TestClient, async_cancel_one_goal)
   EXPECT_TRUE(goal_canceled);
 }
 
+TEST_F(TestClient, async_cancel_one_goal_with_callback)
+{
+  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 = 5;
+  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());
+
+  bool cancel_response_received = false;
+  auto future_cancel = action_client->async_cancel_goal(
+    goal_handle,
+    [&cancel_response_received, goal_handle](
+      typename ActionGoalHandle::SharedPtr goal_handle_canceled, bool cancel_accepted) mutable
+    {
+      if (
+        goal_handle_canceled->get_goal_id() == goal_handle->get_goal_id() &&
+        cancel_accepted)
+      {
+        cancel_response_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_cancel);
+  bool goal_canceled = future_cancel.get();
+  EXPECT_TRUE(goal_canceled);
+  EXPECT_TRUE(cancel_response_received);
+}
+
 TEST_F(TestClient, async_cancel_all_goals)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
@@ -503,6 +534,55 @@ TEST_F(TestClient, async_cancel_all_goals)
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
 }
 
+TEST_F(TestClient, async_cancel_all_goals_with_callback)
+{
+  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 = 6;
+  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);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
+    goal_handle0.swap(goal_handle1);
+  }
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  bool cancel_callback_received = false;
+  auto future_cancel_all = action_client->async_cancel_all_goals(
+    [&cancel_callback_received, goal_handle0, goal_handle1](
+      ActionCancelGoalResponse::SharedPtr response)
+    {
+      if (
+        response &&
+        2ul == response->goals_canceling.size() &&
+        goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid &&
+        goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid)
+      {
+        cancel_callback_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_cancel_all);
+  auto cancel_response = future_cancel_all.get();
+
+  EXPECT_TRUE(cancel_callback_received);
+  ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
+}
+
 TEST_F(TestClient, async_cancel_some_goals)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
@@ -532,3 +612,45 @@ TEST_F(TestClient, async_cancel_some_goals)
   EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
 }
+
+TEST_F(TestClient, async_cancel_some_goals_with_callback)
+{
+  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 = 6;
+  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);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  bool cancel_callback_received = false;
+  auto future_cancel_some = action_client->async_cancel_goals_before(
+    goal_handle1->get_goal_stamp(),
+    [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response)
+    {
+      if (
+        response &&
+        1ul == response->goals_canceling.size() &&
+        goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid)
+      {
+        cancel_callback_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_cancel_some);
+  auto cancel_response = future_cancel_some.get();
+
+  EXPECT_TRUE(cancel_callback_received);
+  ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+}