Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (#1635)

KavenYau · Kaven Yau · jacobperron · web-flow · commit fba080cf3425 · 2021-04-25T16:32:30.000+09:00
* Fix deadlock issue that caused by other mutexes locked in CancelCallback

Signed-off-by: Kaven Yau &lt;love29881460@qq.com&gt;

* Add unit test for rclcpp action server deadlock

Signed-off-by: Kaven Yau &lt;love29881460@qq.com&gt;

* Update rclcpp_action/test/test_server.cpp

Co-authored-by: William Woodall &lt;william+github@osrfoundation.org&gt;

Co-authored-by: Kaven Yau &lt;love29881460@qq.com&gt;
Co-authored-by: Jacob Perron &lt;jacob@openrobotics.org&gt;
Co-authored-by: William Woodall &lt;william+github@osrfoundation.org&gt;
diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp
@@ -356,16 +356,20 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
   CancelResponse
   call_handle_cancel_callback(const GoalUUID & uuid) override
   {
-    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
+    {
+      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+      auto element = goal_handles_.find(uuid);
+      if (element != goal_handles_.end()) {
+        goal_handle = element->second.lock();
+      }
+    }
+
     CancelResponse resp = CancelResponse::REJECT;
-    auto element = goal_handles_.find(uuid);
-    if (element != goal_handles_.end()) {
-      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
-      if (goal_handle) {
-        resp = handle_cancel_(goal_handle);
-        if (CancelResponse::ACCEPT == resp) {
-          goal_handle->_cancel_goal();
-        }
+    if (goal_handle) {
+      resp = handle_cancel_(goal_handle);
+      if (CancelResponse::ACCEPT == resp) {
+        goal_handle->_cancel_goal();
       }
     }
     return resp;
diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp
@@ -1234,10 +1234,14 @@ class TestDeadlockServer : public TestServer
         this->TryLockFor(lock, std::chrono::milliseconds(1000));
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [this](std::shared_ptr<GoalHandle>) {
+      [this](std::shared_ptr<GoalHandle> handle) {
         // 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) {
@@ -1306,3 +1310,12 @@ TEST_F(TestDeadlockServer, deadlock_while_canceled)
   send_goal_request(node_, uuid2_);  // deadlock here
   t.join();
 }
+
+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_);
+  t.join();
+}
