Fix MultiThreadedExecutor busy wait on busy callback groups

rclcpp/src/rclcpp/executor.cpp

@@ -808,6 +808,19 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
     return false;
   }
 
+  // If an entity is ready but its callback group cannot be taken from (e.g. it
+  // is a mutually exclusive group currently executing in another thread), the
+  // entities collection must be rebuilt so the group is excluded from the next
+  // wait. Otherwise a level-triggered entity wakes the wait set immediately and
+  // the calling thread busy waits until the callback group is released.
+  auto can_be_taken_from = [this](const rclcpp::CallbackGroup::SharedPtr & callback_group) {
+      if (!callback_group || !callback_group->can_be_taken_from()) {
+        entities_need_rebuild_.store(true);
+        return false;
+      }
+      return true;
+    };
+
   if (!valid_executable) {
     size_t current_timer_index = 0;
     while (true) {
@@ -819,7 +832,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());
       if (entity_iter != current_collection_.timers.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (!callback_group || !callback_group->can_be_taken_from()) {
+        if (!can_be_taken_from(callback_group)) {
           current_timer_index++;
           continue;
         }
@@ -849,7 +862,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
         subscription->get_subscription_handle().get());
       if (entity_iter != current_collection_.subscriptions.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (!callback_group || !callback_group->can_be_taken_from()) {
+        if (!can_be_taken_from(callback_group)) {
           continue;
         }
         any_executable.subscription = subscription;
@@ -865,7 +878,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.services.find(service->get_service_handle().get());
       if (entity_iter != current_collection_.services.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (!callback_group || !callback_group->can_be_taken_from()) {
+        if (!can_be_taken_from(callback_group)) {
           continue;
         }
         any_executable.service = service;
@@ -881,7 +894,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.clients.find(client->get_client_handle().get());
       if (entity_iter != current_collection_.clients.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (!callback_group || !callback_group->can_be_taken_from()) {
+        if (!can_be_taken_from(callback_group)) {
           continue;
         }
         any_executable.client = client;
@@ -897,7 +910,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.waitables.find(waitable.get());
       if (entity_iter != current_collection_.waitables.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (!callback_group || !callback_group->can_be_taken_from()) {
+        if (!can_be_taken_from(callback_group)) {
           continue;
         }
         any_executable.waitable = waitable;

rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <atomic>
 #include <chrono>
 #include <condition_variable>
 #include <functional>
@@ -23,6 +24,7 @@
 #include <thread>
 
 #include "rclcpp/callback_group.hpp"
+#include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 
@@ -213,3 +215,59 @@ TYPED_TEST(TestBusyWaiting, test_spin)
   // this should get the initial trigger, and the follow up from in the callback
   ASSERT_EQ(this->waitable->get_count(), 2u);
 }
+
+// Regression test for a busy wait in the MultiThreadedExecutor: if an entity
+// becomes ready while its mutually exclusive callback group is executing in
+// another thread, the idle threads must not spin on it until the group is
+// released.
+TEST(TestBusyWaitingMultiThreaded, no_busy_wait_while_callback_group_is_executing)
+{
+  rclcpp::init(0, nullptr);
+  {
+    auto node = std::make_shared<rclcpp::Node>(
+      "node", "TestBusyWaitingMultiThreaded_no_busy_wait_while_callback_group_is_executing");
+    auto callback_group = node->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      /* automatically_add_to_executor_with_node =*/ false);
+    auto waitable = std::make_shared<TestWaitable>();
+    node->get_node_waitables_interface()->add_waitable(waitable, callback_group);
+
+    std::atomic_bool has_executed{false};
+    waitable->set_on_execute_callback([&waitable, &has_executed]() {
+        if (!has_executed.exchange(true)) {
+          // Make the waitable ready again while its callback group is still
+          // executing, and keep the group busy for a while. During this time
+          // the other executor thread must block instead of busy waiting.
+          waitable->trigger();
+          std::this_thread::sleep_for(100ms);
+        }
+      });
+
+    rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2);
+    executor.add_callback_group(callback_group, node->get_node_base_interface());
+
+    std::thread spinner([&executor]() {executor.spin();});
+
+    // Let the executor settle, so that the waitable is triggered while the
+    // executor threads are idle and their entities collection is up to date.
+    const auto settle_deadline = std::chrono::steady_clock::now() + 10s;
+    while (!executor.is_spinning() && std::chrono::steady_clock::now() < settle_deadline) {
+      std::this_thread::sleep_for(1ms);
+    }
+    std::this_thread::sleep_for(100ms);
+
+    waitable->trigger();
+
+    const auto deadline = std::chrono::steady_clock::now() + 10s;
+    while (waitable->get_count() < 2u && std::chrono::steady_clock::now() < deadline) {
+      std::this_thread::sleep_for(1ms);
+    }
+
+    executor.cancel();
+    spinner.join();
+
+    EXPECT_EQ(waitable->get_count(), 2u);
+    EXPECT_LT(waitable->get_is_ready_call_count(), 10u * waitable->get_count());
+  }
+  rclcpp::shutdown();
+}