Fix race between timeout and transform ready callback (#143)

* Add test for race in tf2_ros::Buffer

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Fix race between timeout and transform ready callback

The timer-request handle map is used to check if the promise to the user has already been set.
For the transformable request callback, we loop over the values of the map to check if the request is still valid.
Alternatively, we could implement a bi-directional map for efficiency, but I don't expect the number of entries in
the timer-request map to be large so it seems like a pre-mature optimization.

Fixes #141

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

tf2_ros/src/buffer.cpp

@@ -202,14 +202,33 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou
   auto promise = std::make_shared<std::promise<geometry_msgs::msg::TransformStamped>>();
   TransformStampedFuture future(promise->get_future());
 
-  auto cb_handle = addTransformableCallback([&, promise, callback, future](
+  auto cb_handle = addTransformableCallback([this, promise, callback, future](
     tf2::TransformableRequestHandle request_handle, const std::string& target_frame,
     const std::string& source_frame, tf2::TimePoint time, tf2::TransformableResult result)
     {
       (void) request_handle;
 
+      bool timeout_occurred = true;
+      {
+        std::lock_guard<std::mutex> lock(this->timer_to_request_map_mutex_);
+        // Check if a timeout already occurred
+        for (auto it = timer_to_request_map_.begin(); it != timer_to_request_map_.end(); ++it) {
+          if (request_handle == it->second) {
+            // The request handle was found, so a timeout has not occurred
+            this->timer_interface_->remove(it->first);
+            this->timer_to_request_map_.erase(it->first);
+            timeout_occurred = false;
+            break;
+          }
+        }
+      }
+
+      if (timeout_occurred) {
+        return;
+      }
+
       if (result == tf2::TransformAvailable) {
-      geometry_msgs::msg::TransformStamped msg_stamped = lookupTransform(target_frame, source_frame, time);
+        geometry_msgs::msg::TransformStamped msg_stamped = this->lookupTransform(target_frame, source_frame, time);
         promise->set_value(msg_stamped);
       } else {
         promise->set_exception(std::make_exception_ptr<tf2::LookupException>(
@@ -246,17 +265,25 @@ Buffer::timerCallback(const TimerHandle & timer_handle,
                       TransformStampedFuture future,
                       TransformReadyCallback callback)
 {
+  bool timer_is_valid = false;
   tf2::TransformableRequestHandle request_handle;
   {
     std::lock_guard<std::mutex> lock(timer_to_request_map_mutex_);
-    request_handle = timer_to_request_map_.at(timer_handle);
+    auto timer_and_request_it = timer_to_request_map_.find(timer_handle);
+    timer_is_valid = (timer_to_request_map_.end() != timer_and_request_it);
+    if (timer_is_valid) {
+      request_handle = timer_and_request_it->second;
+    }
     timer_to_request_map_.erase(timer_handle);
+    timer_interface_->remove(timer_handle);
+  }
+
+  if (timer_is_valid) {
+    cancelTransformableRequest(request_handle);
+    promise->set_exception(
+      std::make_exception_ptr<tf2::TimeoutException>(std::string("Timed out waiting for transform")));
+    callback(future);
   }
-  cancelTransformableRequest(request_handle);
-  promise->set_exception(
-    std::make_exception_ptr<tf2::TimeoutException>(std::string("Timed out waiting for transform")));
-  callback(future);
-  timer_interface_->remove(timer_handle);
 }
 
 bool Buffer::getFrames(tf2_msgs::srv::FrameGraph::Request& req, tf2_msgs::srv::FrameGraph::Response& res) 

tf2_ros/test/test_buffer.cpp

@@ -223,6 +223,52 @@ TEST(test_buffer, wait_for_transform_timeout)
   EXPECT_TRUE(callback_timeout);
 }
 
+// Regression test for https://github.com/ros2/geometry2/issues/141
+TEST(test_buffer, wait_for_transform_race)
+{
+  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  tf2_ros::Buffer buffer(clock);
+  auto mock_create_timer = std::make_shared<MockCreateTimer>();
+  buffer.setCreateTimerInterface(mock_create_timer);
+
+  rclcpp::Time rclcpp_time = clock->now();
+  tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds()));
+
+  bool callback_timeout = false;
+  auto future = buffer.waitForTransform(
+    "foo",
+    "bar",
+    tf2_time, tf2::durationFromSec(1.0),
+    [&callback_timeout](const tf2_ros::TransformStampedFuture & future)
+    {
+      try {
+        // We don't expect this throw, even though a timeout will occur
+        future.get();
+      } catch (...) {
+        callback_timeout = true;
+      }
+    });
+
+  auto status = future.wait_for(std::chrono::milliseconds(1));
+  EXPECT_EQ(status, std::future_status::timeout);
+
+  // Set the valid transform during the timeout
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = "foo";
+  transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time);
+  transform.child_frame_id = "bar";
+  transform.transform.rotation.w = 1.0;
+  EXPECT_TRUE(buffer.setTransform(transform, "unittest"));
+
+  // Fake a time out (race with setTransform above)
+  mock_create_timer->execute_timers();
+
+  EXPECT_TRUE(buffer.canTransform("bar", "foo", tf2_time));
+  status = future.wait_for(std::chrono::milliseconds(1));
+  EXPECT_EQ(status, std::future_status::ready);
+  EXPECT_FALSE(callback_timeout);
+}
+
 int main(int argc, char **argv){
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();