Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (#1641)

KavenYau · web-flow · commit d051b8aa20fa · 2021-04-30T17:56:38.000-03:00
Signed-off-by: Kaven Yau &lt;kavenyau@foxmail.com&gt;
diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp
@@ -369,7 +369,14 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
     if (goal_handle) {
       resp = handle_cancel_(goal_handle);
       if (CancelResponse::ACCEPT == resp) {
-        goal_handle->_cancel_goal();
+        try {
+          goal_handle->_cancel_goal();
+        } catch (const rclcpp::exceptions::RCLError & ex) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("rclcpp_action"),
+            "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what());
+          return CancelResponse::REJECT;
+        }
       }
     }
     return resp;
diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp
@@ -1234,14 +1234,10 @@ class TestDeadlockServer : public TestServer
         this->TryLockFor(lock, std::chrono::milliseconds(1000));
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [this](std::shared_ptr<GoalHandle> handle) {
+      [this](std::shared_ptr<GoalHandle>) {
         // instead of making a deadlock, check if it can acquire the lock in a second
         std::unique_lock<std::recursive_timed_mutex> lock(server_mutex_, std::defer_lock);
         this->TryLockFor(lock, std::chrono::milliseconds(1000));
-        // TODO(KavenYau): this check may become obsolete with https://github.com/ros2/rclcpp/issues/1599
-        if (!handle->is_active()) {
-          return rclcpp_action::CancelResponse::REJECT;
-        }
         return rclcpp_action::CancelResponse::ACCEPT;
       },
       [this](std::shared_ptr<GoalHandle> handle) {
@@ -1316,6 +1312,9 @@ TEST_F(TestDeadlockServer, deadlock_while_succeed_and_canceled)
   send_goal_request(node_, uuid1_);
   std::thread t(&TestDeadlockServer::GoalSucceeded, this);
   rclcpp::sleep_for(std::chrono::milliseconds(50));
-  send_cancel_request(node_, uuid1_);
+  auto response_ptr = send_cancel_request(node_, uuid1_);
+
+  // current goal handle is not cancelable, so it returns ERROR_REJECTED
+  EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code);
   t.join();
 }
