Multi-threaded Executor starvation fix

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<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("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();
+}