diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index fc3cfb0f5..430b82100 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -45,6 +45,7 @@ #include #include #include +#include namespace tf2_ros @@ -155,6 +156,7 @@ class TransformListener using thread_ptr = std::unique_ptr>; thread_ptr dedicated_listener_thread_; + std::atomic_bool keep_running_; rclcpp::Node::SharedPtr optional_default_node_ = nullptr; rclcpp::Subscription::SharedPtr message_subscription_tf_; diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 844bf3453..99671286e 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -67,19 +67,25 @@ void TransformListener::initThread( { auto executor = std::make_shared(); + 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; 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(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);