From cfb9b246233d4b019e5be007322314c44a67fad2 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 8 Sep 2026 03:05:37 +0200 Subject: [PATCH] fix: Improved thread usage under congestion (#3258) Signed-off-by: Janosch Machowinski Co-authored-by: Janosch Machowinski (cherry picked from commit c36e550282d41fc151872470b6958507784baa4c) --- .../events_cbg_executor.cpp | 4 ++ .../first_in_first_out_scheduler.cpp | 4 -- .../first_in_first_out_scheduler.hpp | 2 +- .../events_cbg_executor/scheduler.hpp | 40 +++++++++++-------- 4 files changed, 29 insertions(+), 21 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp index 80f87ac57a..56bd736572 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp @@ -382,6 +382,10 @@ EventsCBGExecutor::run( continue; } + if(ready_entity.moreEntitiesReady) { + scheduler->unblock_one_worker_thread(); + } + try { ready_entity.entity->execute_function(); } catch (const std::exception & e) { diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.cpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.cpp index 4af06287b7..c758982c4c 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.cpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.cpp @@ -154,8 +154,6 @@ FirstInFirstOutScheduler::get_handle_for_callback_group( CBGScheduler::ExecutableEntityWithInfo FirstInFirstOutScheduler::get_next_ready_entity_intern() { - std::lock_guard l(ready_callback_groups_mutex); - while(!ready_callback_groups.empty()) { FirstInFirstOutCallbackGroupHandle *ready_cbg = static_cast(ready_callback_groups.front()); @@ -183,8 +181,6 @@ CBGScheduler::ExecutableEntityWithInfo FirstInFirstOutScheduler::get_next_ready_ CBGScheduler::ExecutableEntityWithInfo FirstInFirstOutScheduler::get_next_ready_entity_intern( GlobalEventIdProvider::MonotonicId max_id) { - std::lock_guard l(ready_callback_groups_mutex); - // as, we remove an reappend ready callback_groups during execution, // the first ready cbg may not contain the lowest id. Therefore we // need to search the whole deque diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp index df55e22f29..52f5c072f0 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp @@ -68,11 +68,11 @@ class FirstInFirstOutScheduler : public CBGScheduler public: using CBGScheduler::CBGScheduler; +private: ExecutableEntityWithInfo get_next_ready_entity_intern() final; ExecutableEntityWithInfo get_next_ready_entity_intern( GlobalEventIdProvider::MonotonicId max_id) final; -private: std::unique_ptr get_handle_for_callback_group( const rclcpp::CallbackGroup::SharedPtr & callback_group) final; diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp index 6fe052bec0..606b330b24 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp @@ -263,14 +263,14 @@ class CBGScheduler */ ExecutableEntityWithInfo get_next_ready_entity() { - { - std::lock_guard l(ready_callback_groups_mutex); - if(needs_sync) { - needs_sync = false; - return ExecutableEntityWithInfo{.entity = - ExecutableEntity{.execute_function = sync_function, .callback_handle = nullptr}, - .moreEntitiesReady = false}; - } + std::lock_guard l(ready_callback_groups_mutex); + worker_checking_for_work = false; + + if(needs_sync) { + needs_sync = false; + return ExecutableEntityWithInfo{.entity = + ExecutableEntity{.execute_function = sync_function, .callback_handle = nullptr}, + .moreEntitiesReady = false}; } return get_next_ready_entity_intern(); @@ -279,19 +279,22 @@ class CBGScheduler ExecutableEntityWithInfo get_next_ready_entity( GlobalEventIdProvider::MonotonicId max_id) { - { - std::lock_guard l(ready_callback_groups_mutex); - if(needs_sync) { - needs_sync = false; - return ExecutableEntityWithInfo{.entity = - ExecutableEntity{.execute_function = sync_function, .callback_handle = nullptr}, - .moreEntitiesReady = false}; - } + std::lock_guard l(ready_callback_groups_mutex); + worker_checking_for_work = false; + + if(needs_sync) { + needs_sync = false; + return ExecutableEntityWithInfo{.entity = + ExecutableEntity{.execute_function = sync_function, .callback_handle = nullptr}, + .moreEntitiesReady = false}; } return get_next_ready_entity_intern(max_id); } + /** + * Will be called while holding the ready_callback_groups_mutex lock + */ virtual ExecutableEntityWithInfo get_next_ready_entity_intern() = 0; virtual ExecutableEntityWithInfo get_next_ready_entity_intern( GlobalEventIdProvider::MonotonicId max_id) = 0; @@ -338,6 +341,10 @@ class CBGScheduler { { std::lock_guard lk(ready_callback_groups_mutex); + if(worker_checking_for_work) { + return; + } + worker_checking_for_work = true; release_worker_once = true; } work_ready_conditional.notify_one(); @@ -385,6 +392,7 @@ class CBGScheduler bool release_workers = false; bool release_worker_once = false; + bool worker_checking_for_work = false; std::condition_variable work_ready_conditional;