From ede2c6e5bb767110e50d19f09ae98a52cebae12c Mon Sep 17 00:00:00 2001
From: Kaven Yau <kavenyau@foxmail.com>
Date: Sun, 25 Apr 2021 15:32:30 +0800
Subject: [PATCH] Fix action server deadlock issue that caused by other mutexes
 locked in CancelCallback (#1635)

* Fix deadlock issue that caused by other mutexes locked in CancelCallback

Signed-off-by: Kaven Yau <love29881460@qq.com>

* Add unit test for rclcpp action server deadlock

Signed-off-by: Kaven Yau <love29881460@qq.com>

* Update rclcpp_action/test/test_server.cpp

Co-authored-by: William Woodall <william+github@osrfoundation.org>

Co-authored-by: Kaven Yau <love29881460@qq.com>
Co-authored-by: Jacob Perron <jacob@openrobotics.org>
Co-authored-by: William Woodall <william+github@osrfoundation.org>
(cherry picked from commit fba080cf34256243705e45b1eadc1c49057b5896)
---
 .../include/rclcpp_action/server.hpp          | 22 +++++++++++--------
 rclcpp_action/test/test_server.cpp            | 15 ++++++++++++-
 2 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp
index b3871060cd..adc38c7b96 100644
--- 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
index 7ff9838658..af94d67cbf 100644
--- 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();
+}