From ce4c324b6097998a116ea5b6353ce705fb45db8c Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 1 Apr 2021 01:50:37 -0700 Subject: [PATCH] Do not use rclcpp::spin_some in tight loops as it makes the context shutdown_callbacks huge, sometimes triggers bad_function on shutdown Signed-off-by: Emerson Knapp --- .../include/rosbag2_test_common/subscription_manager.hpp | 6 ++++-- rosbag2_transport/test/rosbag2_transport/test_record.cpp | 4 +++- .../test/rosbag2_transport/test_rosbag2_node.cpp | 4 +++- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp index 33a262199..7172e2790 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp @@ -77,10 +77,12 @@ class SubscriptionManager std::future spin_subscriptions() { + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(subscriber_node_); return async( - std::launch::async, [this]() { + std::launch::async, [this, &exec]() { while (continue_spinning(expected_topics_with_size_)) { - rclcpp::spin_some(subscriber_node_); + exec.spin_some(); } }); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index 81bbba4db..2002ac203 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -151,9 +151,11 @@ TEST_F(RecordIntegrationTestFixture, receives_latched_messages) // Publish messages before starting recording test_msgs::msg::Strings msg; msg.string_value = "Hello"; + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(publisher_node); for (size_t i = 0; i < num_latched_messages; i++) { publisher_transient_local->publish(msg); - rclcpp::spin_some(publisher_node); + exec.spin_some(); } start_recording({false, false, {topic}, "rmw_format", 100ms}); auto & writer = static_cast(writer_->get_implementation_handle()); diff --git a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp index 3efd326e7..33429eb0c 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp @@ -83,8 +83,10 @@ class RosBag2NodeFixture : public Test counter++; }); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node_); while (counter < expected_messages_number) { - rclcpp::spin_some(node_); + exec.spin_some(); } return messages; }