Add return code to CancelGoal service response

rclcpp_action/include/rclcpp_action/client.hpp

@@ -267,8 +267,7 @@ class Client : public ClientBase
   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)>;
+  using CancelCallback = std::function<void (typename CancelResponse::SharedPtr)>;
 
   /// Options for sending a goal.
   /**
@@ -422,42 +421,26 @@ class Client : public ClientBase
    *   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.
+   *   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
+   * <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
+   * action_msgs/CancelGoal.srv</a>.
    */
-  std::shared_future<bool>
+  std::shared_future<typename CancelResponse::SharedPtr>
   async_cancel_goal(
     typename GoalHandle::SharedPtr goal_handle,
-    CancelOneCallback cancel_callback = nullptr)
+    CancelCallback cancel_callback = nullptr)
   {
     std::lock_guard<std::mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
-    // Put promise in the heap to move it around.
-    auto promise = std::make_shared<std::promise<bool>>();
-    std::shared_future<bool> future(promise->get_future());
     auto cancel_request = std::make_shared<CancelRequest>();
     // cancel_request->goal_info.goal_id = goal_handle->get_goal_id();
     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, cancel_callback, promise](std::shared_ptr<void> response) mutable
-      {
-        auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
-        bool goal_canceled = false;
-        if (!cancel_response->goals_canceling.empty()) {
-          const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0];
-          // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id());
-          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;
+    return async_cancel(cancel_request, cancel_callback);
   }
 
   /// Asynchronously request for all goals to be canceled.
@@ -471,7 +454,7 @@ class Client : public ClientBase
    * action_msgs/CancelGoal.srv</a>.
    */
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel_all_goals(CancelMultipleCallback cancel_callback = nullptr)
+  async_cancel_all_goals(CancelCallback cancel_callback = nullptr)
   {
     auto cancel_request = std::make_shared<CancelRequest>();
     // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
@@ -495,7 +478,7 @@ class Client : public ClientBase
   std::shared_future<typename CancelResponse::SharedPtr>
   async_cancel_goals_before(
     const rclcpp::Time & stamp,
-    CancelMultipleCallback cancel_callback = nullptr)
+    CancelCallback cancel_callback = nullptr)
   {
     auto cancel_request = std::make_shared<CancelRequest>();
     // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
@@ -636,7 +619,7 @@ class Client : public ClientBase
   std::shared_future<typename CancelResponse::SharedPtr>
   async_cancel(
     typename CancelRequest::SharedPtr cancel_request,
-    CancelMultipleCallback cancel_callback = nullptr)
+    CancelCallback cancel_callback = nullptr)
   {
     // Put promise in the heap to move it around.
     auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();

rclcpp_action/src/server.cpp

@@ -325,6 +325,9 @@ ServerBase::execute_cancel_request_received()
     pimpl_->action_server_.get(),
     &cancel_request,
     &cancel_response);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
 
   RCLCPP_SCOPE_EXIT({
     ret = rcl_action_cancel_response_fini(&cancel_response);
@@ -335,6 +338,7 @@ ServerBase::execute_cancel_request_received()
 
   auto response = std::make_shared<action_msgs::srv::CancelGoal::Response>();
 
+  response->return_code = cancel_response.msg.return_code;
   auto & goals = cancel_response.msg.goals_canceling;
   // For each canceled goal, call cancel callback
   for (size_t i = 0; i < goals.size; ++i) {
@@ -351,6 +355,12 @@ ServerBase::execute_cancel_request_received()
     }
   }
 
+  // If the user rejects all individual requests to cancel goals,
+  // then we consider the top-level cancel request as rejected.
+  if (goals.size >= 1u && 0u == response->goals_canceling.size()) {
+    response->return_code = action_msgs::srv::CancelGoal::Response::ERROR_REJECTED;
+  }
+
   if (!response->goals_canceling.empty()) {
     // at least one goal state changed, publish a new status message
     publish_status();

rclcpp_action/test/test_client.cpp

@@ -464,8 +464,8 @@ TEST_F(TestClient, async_cancel_one_goal)
 
   auto future_cancel = action_client->async_cancel_goal(goal_handle);
   dual_spin_until_future_complete(future_cancel);
-  bool goal_canceled = future_cancel.get();
-  EXPECT_TRUE(goal_canceled);
+  ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get();
+  EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
 }
 
 TEST_F(TestClient, async_cancel_one_goal_with_callback)
@@ -484,18 +484,21 @@ TEST_F(TestClient, async_cancel_one_goal_with_callback)
   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
+      ActionCancelGoalResponse::SharedPtr response) mutable
     {
       if (
-        goal_handle_canceled->get_goal_id() == goal_handle->get_goal_id() &&
-        cancel_accepted)
+        ActionCancelGoalResponse::ERROR_NONE == response->return_code &&
+        1ul == response->goals_canceling.size() &&
+        goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid)
       {
         cancel_response_received = true;
       }
     });
   dual_spin_until_future_complete(future_cancel);
-  bool goal_canceled = future_cancel.get();
-  EXPECT_TRUE(goal_canceled);
+  auto cancel_response = future_cancel.get();
+  EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
+  ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
   EXPECT_TRUE(cancel_response_received);
 }
 
@@ -527,6 +530,7 @@ TEST_F(TestClient, async_cancel_all_goals)
   dual_spin_until_future_complete(future_cancel_all);
   auto cancel_response = future_cancel_all.get();
 
+  EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
   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);
@@ -575,6 +579,7 @@ TEST_F(TestClient, async_cancel_all_goals_with_callback)
   dual_spin_until_future_complete(future_cancel_all);
   auto cancel_response = future_cancel_all.get();
 
+  EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
   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);
@@ -608,6 +613,7 @@ TEST_F(TestClient, async_cancel_some_goals)
   dual_spin_until_future_complete(future_cancel_some);
   auto cancel_response = future_cancel_some.get();
 
+  EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
   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());
@@ -649,6 +655,7 @@ TEST_F(TestClient, async_cancel_some_goals_with_callback)
   dual_spin_until_future_complete(future_cancel_some);
   auto cancel_response = future_cancel_some.get();
 
+  EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code);
   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);

rclcpp_action/test/test_server.cpp

@@ -26,6 +26,7 @@
 #include "rclcpp_action/server.hpp"
 
 using Fibonacci = test_msgs::action::Fibonacci;
+using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response;
 using GoalUUID = rclcpp_action::GoalUUID;
 
 class TestServer : public ::testing::Test
@@ -55,7 +56,7 @@ class TestServer : public ::testing::Test
     return request;
   }
 
-  void
+  CancelResponse::SharedPtr
   send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
   {
     auto cancel_client = node->create_client<Fibonacci::Impl::CancelGoalService>(
@@ -71,6 +72,7 @@ class TestServer : public ::testing::Test
     {
       throw std::runtime_error("cancel goal future didn't complete succesfully");
     }
+    return future.get();
   }
 };
 
@@ -207,6 +209,131 @@ TEST_F(TestServer, handle_cancel_called)
   EXPECT_TRUE(received_handle->is_canceling());
 }
 
+TEST_F(TestServer, handle_cancel_reject)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
+  const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_FALSE(received_handle->is_canceling());
+
+  auto response_ptr = send_cancel_request(node, uuid);
+  EXPECT_FALSE(received_handle->is_canceling());
+  EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code);
+}
+
+TEST_F(TestServer, handle_cancel_unknown_goal)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
+  const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+  const GoalUUID unknown_uuid{{11, 22, 33, 44, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}};
+
+  auto handle_goal = [](
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_FALSE(received_handle->is_canceling());
+
+  auto response_ptr = send_cancel_request(node, unknown_uuid);
+  EXPECT_FALSE(received_handle->is_canceling());
+  EXPECT_EQ(CancelResponse::ERROR_UNKNOWN_GOAL_ID, response_ptr->return_code);
+}
+
+TEST_F(TestServer, handle_cancel_terminated_goal)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
+  const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+      handle->succeed(std::make_shared<Fibonacci::Result>());
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_FALSE(received_handle->is_canceling());
+
+  auto response_ptr = send_cancel_request(node, uuid);
+  EXPECT_FALSE(received_handle->is_canceling());
+  EXPECT_EQ(CancelResponse::ERROR_GOAL_TERMINATED, response_ptr->return_code);
+}
+
 TEST_F(TestServer, publish_status_accepted)
 {
   auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept");