diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 197ffa4aa..0dfce0abd 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -32,6 +32,7 @@ #ifndef TF2_ROS__TRANSFORM_LISTENER_H_ #define TF2_ROS__TRANSFORM_LISTENER_H_ +#include #include #include #include @@ -143,7 +144,12 @@ class TransformListener // Create executor with dedicated thread to spin. executor_ = std::make_shared(); executor_->add_callback_group(callback_group_, node_base_interface_); - dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); + running_.store(true); + dedicated_listener_thread_ = std::make_unique([&]() { + while (running_.load()) { + executor_->spin_once(); + } + }); // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } else { @@ -159,6 +165,7 @@ class TransformListener // ros::CallbackQueue tf_message_callback_queue_; bool spin_thread_{false}; + std::atomic running_; std::unique_ptr dedicated_listener_thread_; rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 3f59d5f0c..25e046ff4 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -61,6 +61,7 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) TransformListener::~TransformListener() { if (spin_thread_) { + running_.store(false); executor_->cancel(); dedicated_listener_thread_->join(); }