diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 02d3c0963..28fb2a238 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -116,7 +116,8 @@ class TransformListener rclcpp::Subscription::SharedPtr message_subscription_tf_; rclcpp::Subscription::SharedPtr message_subscription_tf_static_; tf2::BufferCore & buffer_; - std::atomic stop_thread_{false}; + std::unique_ptr executor_; + bool spin_thread_{false}; tf2::TimePoint last_update_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 854436726..7426f1e97 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -54,19 +54,24 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) TransformListener::~TransformListener() { - stop_thread_ = true; + if (spin_thread_) { + executor_->cancel(); + } } void TransformListener::initThread( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { - stop_thread_ = false; + executor_ = std::make_unique(); + spin_thread_ = 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 = [&](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { - while (!stop_thread_ && rclcpp::ok()) {rclcpp::spin_some(node_base_interface);} + executor_->add_node(node_base_interface); + executor_->spin(); + executor_->remove_node(node_base_interface); }; dedicated_listener_thread_ = thread_ptr( new std::thread(run_func, node_base_interface),