diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 5839c6496..12a8a576f 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -211,6 +211,9 @@ class TransformListener executor_ = std::make_shared(); executor_->add_callback_group(callback_group_, node_base_interface_); dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); + while (!executor_->is_spinning()) { + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } else { diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp index d6e28f6ea..54212b994 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -104,6 +104,15 @@ TEST(tf2_test_transform_listener, transform_listener_with_intraprocess) custom_node->init_tf_listener(); } +TEST(tf2_test_transform_listener, transform_listener_spin_thread) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, node, true); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv);