Add return code to CancelGoal service response (ros2#710)

* Populate return code of CancelGoal service response Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Throw if there is an error processing a cancel goal request Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Make cancel callback signature consistent across cancel methods and add tests Refactored the callback signature for canceling one goal. Now it is the same as the other cancel methods. This makes it easier to communicate the error code to the user. Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Address review Signed-off-by: Jacob Perron <jacob@openrobotics.org>
cho3 · Jun 3, 2019 · 1298239 · 1298239
1 parent 370f1cc
commit 1298239
Show file tree

Hide file tree

Showing 4 changed files with 165 additions and 38 deletions.
diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/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>>();

diff --git a/rclcpp_action/src/server.cpp b/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();

diff --git a/rclcpp_action/test/test_client.cpp b/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);

diff --git a/rclcpp_action/test/test_server.cpp b/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");