fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (#2267)

* fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks

This prevents deadlocks in cases, were e.g. get_status() would
be called on the handle in a callback of the handle.

* test(rclcpp_action): Added test for deadlocks during access of a goal handle

This test checks, if the code deadlocks, if methods on the goal handle are
called from the callbacks.

Signed-off-by: Janosch Machowinski <[email protected]>
Co-authored-by: Tomoya Fujita <[email protected]>

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -163,7 +163,7 @@ class ClientGoalHandle
   ResultCallback result_callback_{nullptr};
   int8_t status_{GoalStatus::STATUS_ACCEPTED};
 
-  std::mutex handle_mutex_;
+  std::recursive_mutex handle_mutex_;
 };
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -59,7 +59,7 @@ template<typename ActionT>
 std::shared_future<typename ClientGoalHandle<ActionT>::WrappedResult>
 ClientGoalHandle<ActionT>::async_get_result()
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   if (!is_result_aware_) {
     throw exceptions::UnawareGoalHandleError();
   }
@@ -70,7 +70,7 @@ template<typename ActionT>
 void
 ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   status_ = static_cast<int8_t>(wrapped_result.code);
   result_promise_.set_value(wrapped_result);
   if (result_callback_) {
@@ -82,55 +82,55 @@ template<typename ActionT>
 void
 ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   feedback_callback_ = callback;
 }
 
 template<typename ActionT>
 void
 ClientGoalHandle<ActionT>::set_result_callback(ResultCallback callback)
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   result_callback_ = callback;
 }
 
 template<typename ActionT>
 int8_t
 ClientGoalHandle<ActionT>::get_status()
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   return status_;
 }
 
 template<typename ActionT>
 void
 ClientGoalHandle<ActionT>::set_status(int8_t status)
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   status_ = status;
 }
 
 template<typename ActionT>
 bool
 ClientGoalHandle<ActionT>::is_feedback_aware()
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   return feedback_callback_ != nullptr;
 }
 
 template<typename ActionT>
 bool
 ClientGoalHandle<ActionT>::is_result_aware()
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   return is_result_aware_;
 }
 
 template<typename ActionT>
 bool
 ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   bool previous = is_result_aware_;
   is_result_aware_ = awareness;
   return previous;
@@ -140,7 +140,7 @@ template<typename ActionT>
 void
 ClientGoalHandle<ActionT>::invalidate(const exceptions::UnawareGoalHandleError & ex)
 {
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   // Guard against multiple calls
   if (is_invalidated()) {
     return;
@@ -168,7 +168,7 @@ ClientGoalHandle<ActionT>::call_feedback_callback(
     RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle.");
     return;
   }
-  std::lock_guard<std::mutex> guard(handle_mutex_);
+  std::lock_guard<std::recursive_mutex> guard(handle_mutex_);
   if (nullptr == feedback_callback_) {
     // Normal, some feedback messages may arrive after the goal result.
     RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it.");

rclcpp_action/test/test_client.cpp

@@ -852,6 +852,86 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback)
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
 }
 
+TEST_F(TestClientAgainstServer, deadlock_in_callbacks)
+{
+  std::atomic<bool> feedback_callback_called = false;
+  std::atomic<bool> response_callback_called = false;
+  std::atomic<bool> result_callback_called = false;
+  std::atomic<bool> no_deadlock = false;
+
+  std::thread tr = std::thread(
+    [&]() {
+      auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+      ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+      ActionGoal goal;
+
+      using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
+      rclcpp_action::Client<ActionType>::SendGoalOptions ops;
+      ops.feedback_callback =
+      [&feedback_callback_called](const GoalHandle::SharedPtr handle,
+      ActionType::Feedback::ConstSharedPtr) {
+        // call functions on the handle that acquire the lock
+        handle->get_status();
+        handle->is_feedback_aware();
+        handle->is_result_aware();
+
+        feedback_callback_called = true;
+      };
+      ops.goal_response_callback = [&response_callback_called](
+        const GoalHandle::SharedPtr & handle) {
+        // call functions on the handle that acquire the lock
+        handle->get_status();
+        handle->is_feedback_aware();
+        handle->is_result_aware();
+
+        response_callback_called = true;
+      };
+      ops.result_callback = [&result_callback_called](
+        const GoalHandle::WrappedResult &) {
+        result_callback_called = true;
+      };
+
+      goal.order = 6;
+      auto future_goal_handle = action_client->async_send_goal(goal, ops);
+      dual_spin_until_future_complete(future_goal_handle);
+      auto goal_handle = future_goal_handle.get();
+
+      ASSERT_TRUE(goal_handle);
+
+      ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+      auto result_future = action_client->async_get_result(goal_handle);
+      dual_spin_until_future_complete(result_future);
+
+      EXPECT_TRUE(result_future.valid());
+      auto result = result_future.get();
+
+      no_deadlock = true;
+    });
+
+  auto start_time = std::chrono::system_clock::now();
+
+  while (std::chrono::system_clock::now() - start_time < std::chrono::milliseconds(2000) &&
+    !no_deadlock)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  if (no_deadlock) {
+    tr.join();
+  } else {
+    // In case of a failure, the thread is assumed to be in a deadlock.
+    // We detach the thread so we don't block further tests.
+    tr.detach();
+  }
+
+  EXPECT_TRUE(no_deadlock);
+  EXPECT_TRUE(response_callback_called);
+  EXPECT_TRUE(result_callback_called);
+  EXPECT_TRUE(feedback_callback_called);
+}
+
 TEST_F(TestClientAgainstServer, send_rcl_errors)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);