From 743766a3cef9aaffacfe6373d032098de1cc51c1 Mon Sep 17 00:00:00 2001
From: Zygfryd Wieszok <zwieszok@autonomous-systems.pl>
Date: Thu, 14 Apr 2022 13:23:32 +0000
Subject: [PATCH 1/3] Added additional flag to fix race condition in
 TransformListener

---
 tf2_ros/include/tf2_ros/transform_listener.h |  3 +++
 tf2_ros/src/transform_listener.cpp           | 11 ++++++++---
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h
index fc3cfb0f5..1da4d127b 100644
--- a/tf2_ros/include/tf2_ros/transform_listener.h
+++ b/tf2_ros/include/tf2_ros/transform_listener.h
@@ -36,6 +36,7 @@
 #include <tf2/time.h>
 #include <tf2_ros/visibility_control.h>
 
+#include <atomic>
 #include <tf2_msgs/msg/tf_message.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -45,6 +46,7 @@
 #include <memory>
 #include <thread>
 #include <utility>
+#include <atomic>
 
 
 namespace tf2_ros
@@ -155,6 +157,7 @@ class TransformListener
   using thread_ptr =
     std::unique_ptr<std::thread, std::function<void (std::thread *)>>;
   thread_ptr dedicated_listener_thread_;
+  std::atomic_bool keep_running_;
 
   rclcpp::Node::SharedPtr optional_default_node_ = nullptr;
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr message_subscription_tf_;
diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp
index 844bf3453..b8209a9ea 100644
--- a/tf2_ros/src/transform_listener.cpp
+++ b/tf2_ros/src/transform_listener.cpp
@@ -67,19 +67,24 @@ void TransformListener::initThread(
 {
   auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 
+  keep_running_ = true;
   // This lambda is required because `std::thread` cannot infer the correct
   // rclcpp::spin, since there are more than one versions of it (overloaded).
   // see: http://stackoverflow.com/a/27389714/671658
   // I (wjwwood) chose to use the lamda rather than the static cast solution.
   auto run_func =
-    [executor](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) {
+    [executor, &keep_running =keep_running_](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) {
       executor->add_node(node_base_interface);
-      executor->spin();
+      while(keep_running.load())
+      {
+        executor->spin_once();
+      }
       executor->remove_node(node_base_interface);
     };
   dedicated_listener_thread_ = thread_ptr(
     new std::thread(run_func, node_base_interface),
-    [executor](std::thread * t) {
+    [executor, &keep_running = keep_running_](std::thread * t) {
+      keep_running.store(false);
       executor->cancel();
       t->join();
       delete t;

From 933609a5ec045661e5e77ada4ae8021ae2eb898c Mon Sep 17 00:00:00 2001
From: Zygfryd Wieszok <zwieszok@autonomous-systems.pl>
Date: Thu, 14 Apr 2022 14:55:44 +0000
Subject: [PATCH 2/3] Uncrustify transform_listener

---
 tf2_ros/include/tf2_ros/transform_listener.h | 1 -
 tf2_ros/src/transform_listener.cpp           | 9 +++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h
index 1da4d127b..430b82100 100644
--- a/tf2_ros/include/tf2_ros/transform_listener.h
+++ b/tf2_ros/include/tf2_ros/transform_listener.h
@@ -36,7 +36,6 @@
 #include <tf2/time.h>
 #include <tf2_ros/visibility_control.h>
 
-#include <atomic>
 #include <tf2_msgs/msg/tf_message.hpp>
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp
index b8209a9ea..99671286e 100644
--- a/tf2_ros/src/transform_listener.cpp
+++ b/tf2_ros/src/transform_listener.cpp
@@ -73,17 +73,18 @@ void TransformListener::initThread(
   // see: http://stackoverflow.com/a/27389714/671658
   // I (wjwwood) chose to use the lamda rather than the static cast solution.
   auto run_func =
-    [executor, &keep_running =keep_running_](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) {
+    [executor,
+      & keep_running =
+      keep_running_](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) {
       executor->add_node(node_base_interface);
-      while(keep_running.load())
-      {
+      while (keep_running.load()) {
         executor->spin_once();
       }
       executor->remove_node(node_base_interface);
     };
   dedicated_listener_thread_ = thread_ptr(
     new std::thread(run_func, node_base_interface),
-    [executor, &keep_running = keep_running_](std::thread * t) {
+    [executor, & keep_running = keep_running_](std::thread * t) {
       keep_running.store(false);
       executor->cancel();
       t->join();

From 5326e5a2667abeb7cd02faffe795059aea7ccb78 Mon Sep 17 00:00:00 2001
From: Zygfryd Wieszok <zwieszok@autonomous-systems.pl>
Date: Thu, 14 Apr 2022 15:06:34 +0000
Subject: [PATCH 3/3] Add unit test to chcek if destructor works

---
 tf2_ros/test/test_transform_listener.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp
index 14a166a1f..80ca2a2bd 100644
--- a/tf2_ros/test/test_transform_listener.cpp
+++ b/tf2_ros/test/test_transform_listener.cpp
@@ -77,6 +77,14 @@ TEST(tf2_test_transform_listener, transform_listener_as_member)
   custom_node->init_tf_listener();
 }
 
+TEST(tf2_test_transform_listener, transform_listener_should_destroy_correctly)
+{
+  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  tf2_ros::Buffer buffer(clock);
+  tf2_ros::TransformListener tfl(buffer, true);
+  (void) tfl;
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);