From 791c23afe5a4b23bfbacbb2c83eb6f0abe4aa490 Mon Sep 17 00:00:00 2001
From: Kaven Yau <kavenyau@foxmail.com>
Date: Wed, 5 May 2021 21:28:42 +0800
Subject: [PATCH] Fix action server deadlock issue that caused by other mutexes
 locked in CancelCallback (#1635) (#1654)

Signed-off-by: Kaven Yau <kavenyau@foxmail.com>
---
 .../include/rclcpp_action/server.hpp          | 22 +++++++++++--------
 1 file changed, 13 insertions(+), 9 deletions(-)

diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp
index 7aede9a..a6059f1 100644
--- a/rclcpp_action/include/rclcpp_action/server.hpp
+++ b/rclcpp_action/include/rclcpp_action/server.hpp
@@ -352,16 +352,20 @@ protected:
   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;