Added additional flag to fix race condition in TransformListener

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_;

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;