[Foxy backport] Fixed memory leak in Buffer::waitForTransform (#281) (#330)

* Clear callback from transformable_callbacks_ when callback is done or not needed

Co-authored-by: marting87 <[email protected]>
Co-authored-by: Matthijs den Toom <[email protected]>
Co-authored-by: Chris Lalancette <[email protected]>

Author: 4 people

tf2/src/buffer_core.cpp

@@ -1491,6 +1491,7 @@ void BufferCore::testTransformableRequests()
         {
           const TransformableCallback& cb = it->second;
           cb(req.request_handle, lookupFrameString(req.target_id), lookupFrameString(req.source_id), req.time, result);
+          transformable_callbacks_.erase(req.cb_handle);
         }
       }
 

tf2_ros/src/buffer.cpp

@@ -34,20 +34,40 @@
 
 #include <exception>
 #include <limits>
+#include <map>
 #include <mutex>
 #include <sstream>
 #include <thread>
+#include <unordered_map>
 
 //TODO(tfoote replace these terrible macros)
 #define ROS_ERROR printf
 #define ROS_FATAL printf
 #define ROS_INFO printf
 #define ROS_WARN printf
 
-
 namespace tf2_ros
 {
 
+// Added to backport: https://github.com/ros2/geometry2/pull/281
+static std::mutex g_object_map_to_cb_handle_mutex;
+static std::map<Buffer*, std::unordered_map<TimerHandle, tf2::TransformableCallbackHandle>> g_object_map_to_cb_handle;
+
+void deleteTransformCallbackHandle(Buffer *class_ptr, const TimerHandle &timer_handle)
+{
+  if (g_object_map_to_cb_handle.find(class_ptr) == g_object_map_to_cb_handle.end())
+  {
+    // Return if the object map cb handle is already removed
+    return;
+  }
+
+  g_object_map_to_cb_handle.at(class_ptr).erase(timer_handle);
+  if (g_object_map_to_cb_handle.at(class_ptr).size() == 0)
+  {
+    g_object_map_to_cb_handle.erase(class_ptr);
+  }
+}
+
 Buffer::Buffer(rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) :
   BufferCore(cache_time), clock_(clock), timer_interface_(nullptr)
 {
@@ -215,8 +235,14 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou
         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);
+            auto timer_handle = it->first;
+            this->timer_interface_->remove(timer_handle);
+            this->timer_to_request_map_.erase(timer_handle);
+            {
+              std::lock_guard<std::mutex> lock(g_object_map_to_cb_handle_mutex);
+              // Remove the callback function.
+              deleteTransformCallbackHandle(this, timer_handle);
+            }
             timeout_occurred = false;
             break;
           }
@@ -242,19 +268,31 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou
     // Immediately transformable
     geometry_msgs::msg::TransformStamped msg_stamped = lookupTransform(target_frame, source_frame, time);
     promise->set_value(msg_stamped);
+    removeTransformableCallback(cb_handle);
+    callback(future);
   } else if (0xffffffffffffffffULL == handle) {
     // Never transformable
     promise->set_exception(std::make_exception_ptr(tf2::LookupException(
           "Failed to transform from " + source_frame + " to " + target_frame)));
+    removeTransformableCallback(cb_handle);
+    callback(future);
   } else {
     std::lock_guard<std::mutex> lock(timer_to_request_map_mutex_);
     auto timer_handle = timer_interface_->createTimer(
       clock_,
       timeout,
       std::bind(&Buffer::timerCallback, this, std::placeholders::_1, promise, future, callback));
 
-    // Save association between timer and request handle
+    // Save association between timer and request/callback handle
     timer_to_request_map_[timer_handle] = handle;
+    {
+      std::lock_guard<std::mutex> lock(g_object_map_to_cb_handle_mutex);
+      if (g_object_map_to_cb_handle.find(this) == g_object_map_to_cb_handle.end())
+      {
+        g_object_map_to_cb_handle[this];
+      }
+      g_object_map_to_cb_handle.at(this)[timer_handle] = cb_handle;
+    }
   }
   return future;
 }
@@ -266,20 +304,30 @@ Buffer::timerCallback(const TimerHandle & timer_handle,
                       TransformReadyCallback callback)
 {
   bool timer_is_valid = false;
-  tf2::TransformableRequestHandle request_handle = 0u;
+  tf2::TransformableCallbackHandle callback_handle = 0u;
   {
     std::lock_guard<std::mutex> lock(timer_to_request_map_mutex_);
-    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;
+    {
+      std::lock_guard<std::mutex> lock(g_object_map_to_cb_handle_mutex);
+      if (g_object_map_to_cb_handle.find(this) != g_object_map_to_cb_handle.end())
+      {
+        // Only if the map to callback handle isn't already removed.
+        auto timer_and_callback_it = g_object_map_to_cb_handle.at(this).find(timer_handle);
+        timer_is_valid = (g_object_map_to_cb_handle.at(this).end() != timer_and_callback_it);
+
+        if (timer_is_valid) {
+          callback_handle = timer_and_callback_it->second;
+        }
+
+        deleteTransformCallbackHandle(this, timer_handle);
+      }
     }
     timer_to_request_map_.erase(timer_handle);
     timer_interface_->remove(timer_handle);
   }
 
   if (timer_is_valid) {
-    cancelTransformableRequest(request_handle);
+    removeTransformableCallback(callback_handle);
     promise->set_exception(
       std::make_exception_ptr(tf2::TimeoutException(std::string("Timed out waiting for transform"))));
     callback(future);