Fix ABBA deadlock between waitForTransform and testTransformableRequests

tf2/src/buffer_core.cpp

@@ -1508,57 +1508,80 @@ void BufferCore::_getFrameStrings(std::vector<std::string> & vec) const
 
 void BufferCore::testTransformableRequests()
 {
-  std::unique_lock<std::mutex> lock(transformable_requests_mutex_);
-  size_t i = 0;
-  while (i < transformable_requests_.size()) {
-    TransformableRequest & req = transformable_requests_[i];
+  struct PendingCallback
+  {
+    TransformableCallback cb;
+    TransformableRequestHandle request_handle;
+    std::string target_frame;
+    std::string source_frame;
+    TimePoint time;
+    TransformableResult result;
+  };
+  std::vector<PendingCallback> pending;
 
-    // One or both of the frames may not have existed when the request was originally made.
-    if (req.target_id == 0) {
-      req.target_id = lookupFrameNumber(req.target_string);
-    }
+  {
+    std::unique_lock<std::mutex> lock(transformable_requests_mutex_);
+    size_t i = 0;
+    while (i < transformable_requests_.size()) {
+      TransformableRequest & req = transformable_requests_[i];
+
+      // One or both of the frames may not have existed when the request was originally made.
+      if (req.target_id == 0) {
+        req.target_id = lookupFrameNumber(req.target_string);
+      }
 
-    if (req.source_id == 0) {
-      req.source_id = lookupFrameNumber(req.source_string);
-    }
+      if (req.source_id == 0) {
+        req.source_id = lookupFrameNumber(req.source_string);
+      }
 
-    TimePoint latest_time;
-    bool do_cb = false;
-    TransformableResult result = TransformAvailable;
-    // TODO(anyone): This is incorrect, but better than nothing. Really we want the latest time for
-    // any of the frames
-    getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
-    if ((latest_time != TimePointZero) && (req.time + cache_time_ < latest_time)) {
-      do_cb = true;
-      result = TransformFailure;
-    } else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) {
-      do_cb = true;
-      result = TransformAvailable;
-    }
+      TimePoint latest_time;
+      bool do_cb = false;
+      TransformableResult result = TransformAvailable;
+      // TODO(anyone): This is incorrect, but better than nothing.
+      // Really we want the latest time for any of the frames.
+      getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
+      if ((latest_time != TimePointZero) && (req.time + cache_time_ < latest_time)) {
+        do_cb = true;
+        result = TransformFailure;
+      } else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) {
+        do_cb = true;
+        result = TransformAvailable;
+      }
+
+      if (do_cb) {
+        TransformableCallback cb;
+        {
+          std::unique_lock<std::mutex> lock2(transformable_callbacks_mutex_);
+          auto cb_it = transformable_callbacks_.find(req.cb_handle);
+          if (cb_it != transformable_callbacks_.end()) {
+            cb = std::move(cb_it->second);
+            transformable_callbacks_.erase(cb_it);
+          }
+        }
 
-    if (do_cb) {
-      {
-        std::unique_lock<std::mutex> lock2(transformable_callbacks_mutex_);
-        M_TransformableCallback::iterator cb_it = transformable_callbacks_.find(req.cb_handle);
-        if (cb_it != transformable_callbacks_.end()) {
-          const TransformableCallback & cb = cb_it->second;
-          cb(
-            req.request_handle, lookupFrameString(req.target_id), lookupFrameString(
-              req.source_id), req.time, result);
-          transformable_callbacks_.erase(req.cb_handle);
+        if (cb) {
+          pending.push_back(
+            {std::move(cb), req.request_handle,
+              lookupFrameString(req.target_id), lookupFrameString(req.source_id),
+              req.time, result});
         }
-      }
 
-      // Swap with the last element and pop to remove in O(1).
-      // Do not advance i: the element swapped in from the back is examined in the next iteration.
-      if (i < transformable_requests_.size() - 1) {
-        transformable_requests_[i] = transformable_requests_.back();
+        // Swap with the last element and pop to remove in O(1).
+        // Do not advance i: the element swapped in from the back is examined in the next iteration.
+        if (i < transformable_requests_.size() - 1) {
+          transformable_requests_[i] = transformable_requests_.back();
+        }
+        transformable_requests_.pop_back();
+      } else {
+        ++i;
       }
-      transformable_requests_.pop_back();
-    } else {
-      ++i;
     }
   }
+
+  // Invoke callbacks without holding any mutex to prevent lock-order inversions.
+  for (auto & p : pending) {
+    p.cb(p.request_handle, p.target_frame, p.source_frame, p.time, p.result);
+  }
 }
 
 std::string BufferCore::_allFramesAsDot(TimePoint current_time) const

tf2_ros/test/test_buffer.cpp

@@ -617,6 +617,109 @@ TEST(test_buffer, wait_for_transform_race_during_setup)
   }
 }
 
+
+// Reproduces the ABBA deadlock:
+//
+//   Thread A – waitForTransform:
+//     holds timer_to_request_map_mutex_
+//       -> BufferCore::addTransformableRequest
+//         -> waits for transformable_requests_mutex_
+//
+//   Thread B – setTransform -> testTransformableRequests:
+//     holds transformable_requests_mutex_
+//       -> waitForTransform ready-callback
+//         -> waits for timer_to_request_map_mutex_
+
+TEST(test_buffer, wait_for_transform_does_not_deadlock_with_set_transform)
+{
+  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  tf2_ros::Buffer buffer(clock);
+  buffer.setUsingDedicatedThread(true);
+  auto mock_create_timer = std::make_shared<MockCreateTimer>();
+  buffer.setCreateTimerInterface(mock_create_timer);
+
+  const tf2::TimePoint time_point = tf2::timeFromSec(1.0);
+  const std::string target_frame = "foo";
+  const std::string source_frame = "bar";
+
+  std::promise<void> in_transformable_callback;
+  std::promise<void> waiter_finished;
+  auto waiter_finished_future = waiter_finished.get_future();
+  std::thread waiter_thread;
+
+  // First request becomes ready together with the waitForTransform below. While
+  // testTransformableRequests still holds transformable_requests_mutex_,
+  // this callback starts a concurrent waitForTransform that takes
+  // timer_to_request_map_mutex_ and then blocks in addTransformableRequest.
+  auto gate_cb =
+    [&buffer, &in_transformable_callback, &waiter_thread, &waiter_finished, time_point,
+      target_frame](
+    tf2::TransformableRequestHandle, const std::string &, const std::string &,
+    tf2::TimePoint, tf2::TransformableResult)
+    {
+      waiter_thread = std::thread(
+        [&buffer, &in_transformable_callback, &waiter_finished, time_point, target_frame]()
+        {
+          // Wait until the gate callback is running so addTransformableRequest
+          // contends with testTransformableRequests.
+          in_transformable_callback.get_future().wait();
+          buffer.waitForTransform(
+            target_frame, "other", time_point, tf2::durationFromSec(1.0),
+            [](const tf2_ros::TransformStampedFuture &) {});
+          waiter_finished.set_value();
+        });
+      in_transformable_callback.set_value();
+      // Give the waiter time to enter waitForTransform.
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    };
+
+  ASSERT_NE(
+    buffer.addTransformableRequest(gate_cb, target_frame, source_frame, time_point),
+    0u);
+
+  bool wait_callback_called = false;
+  auto future = buffer.waitForTransform(
+    target_frame, source_frame, time_point, tf2::durationFromSec(1.0),
+    [&wait_callback_called](const tf2_ros::TransformStampedFuture &) {
+      wait_callback_called = true;
+    });
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = target_frame;
+  transform.header.stamp.sec = 1;
+  transform.child_frame_id = source_frame;
+  transform.transform.rotation.w = 1.0;
+
+  std::promise<void> set_transform_done;
+  std::thread setter([&buffer, &transform, &set_transform_done]() {
+      EXPECT_TRUE(buffer.setTransform(transform, "unittest"));
+      set_transform_done.set_value();
+    });
+
+  const auto set_status = set_transform_done.get_future().wait_for(std::chrono::seconds(5));
+  EXPECT_EQ(set_status, std::future_status::ready) <<
+    "Deadlock between waitForTransform (timer_to_request_map_mutex_ -> "
+    "transformable_requests_mutex_) and testTransformableRequests "
+    "(transformable_requests_mutex_ -> timer_to_request_map_mutex_). ";
+  if (set_status != std::future_status::ready) {
+    // Threads still hold the two mutexes; abort so gtest does not hang on join.
+    std::_Exit(1);
+  }
+
+  setter.join();
+  ASSERT_TRUE(waiter_thread.joinable());
+  const auto waiter_status = waiter_finished_future.wait_for(std::chrono::seconds(1));
+  EXPECT_EQ(waiter_status, std::future_status::ready);
+  if (waiter_status != std::future_status::ready) {
+    std::_Exit(1);
+  }
+  waiter_thread.join();
+
+  EXPECT_TRUE(wait_callback_called);
+  EXPECT_EQ(future.wait_for(std::chrono::seconds(1)), std::future_status::ready);
+}
+
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);