diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp index 09dfa03f90..32872482c4 100644 --- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -102,3 +102,66 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { executor.add_node(node); executor.spin(); } + +/* + Test that no tasks are starved + */ +TEST_F(TestMultiThreadedExecutor, starvation) { + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), + 2u); + + std::shared_ptr node = + std::make_shared("test_multi_threaded_executor_starvation"); + + std::atomic_int timer_one_count{0}; + std::atomic_int timer_two_count{0}; + + rclcpp::TimerBase::SharedPtr timer_one; + 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) { + } + + timer_one_count++; + + auto diff = std::abs(timer_one_count - timer_two_count); + + std::cout << "Difference in counts: " << diff << std::endl; + + if (diff > 1) { + 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) { + } + + timer_two_count++; + + auto diff = std::abs(timer_one_count - timer_two_count); + + std::cout << "Difference in counts: " << diff << std::endl; + + if (diff > 1) { + 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); + + executor.add_node(node); + executor.spin(); +}