diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp index 32872482c4..59c9735458 100644 --- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -120,12 +120,7 @@ TEST_F(TestMultiThreadedExecutor, starvation) { rclcpp::TimerBase::SharedPtr timer_two; auto timer_one_callback = [&timer_one_count, &timer_two_count]() { - std::cout << "Timer one callback executed. Count: " - << timer_one_count.load() << std::endl; - - auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 100ms) { - } + std::this_thread::sleep_for(100ms); timer_one_count++; @@ -133,34 +128,27 @@ TEST_F(TestMultiThreadedExecutor, starvation) { std::cout << "Difference in counts: " << diff << std::endl; - if (diff > 1) { + if (timer_one_count > 10 || timer_two_count > 10) { rclcpp::shutdown(); ASSERT_LE(diff, 1); } }; auto timer_two_callback = [&timer_one_count, &timer_two_count]() { - std::cout << "Timer two callback executed. Count: " - << timer_two_count.load() << std::endl; - - auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < 100ms) { - } + std::this_thread::sleep_for(100ms); timer_two_count++; auto diff = std::abs(timer_one_count - timer_two_count); - std::cout << "Difference in counts: " << diff << std::endl; - - if (diff > 1) { + if (timer_one_count > 10 || timer_two_count > 10) { rclcpp::shutdown(); ASSERT_LE(diff, 1); } }; - timer_one = node->create_wall_timer(0ms, timer_one_callback); - timer_two = node->create_wall_timer(0ms, timer_two_callback); + timer_one = node->create_wall_timer(10ms, timer_one_callback); + timer_two = node->create_wall_timer(10ms, timer_two_callback); executor.add_node(node); executor.spin();