diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index c3be9b92a9..1449a68ea9 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/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; diff --git a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp index 5b4ea137e8..3f6f1a9a31 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -23,6 +24,7 @@ #include #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( + "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(); + 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(); +}