From 2f90b4d03a29984e824c7fb3c4bbba2a59a4f18d Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Tue, 8 Sep 2026 03:05:37 +0200 Subject: [PATCH 1/2] fix: Improved thread usage under congestion (#3258) Signed-off-by: Janosch Machowinski Co-authored-by: Janosch Machowinski (cherry picked from commit c36e550282d41fc151872470b6958507784baa4c) # Conflicts: # rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp --- .../events_cbg_executor.cpp | 4 +++ .../first_in_first_out_scheduler.cpp | 4 --- .../first_in_first_out_scheduler.hpp | 2 +- .../events_cbg_executor/scheduler.hpp | 30 +++++++++++++++++++ 4 files changed, 35 insertions(+), 5 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 329f233125..2328bafe18 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 e00d9c586b..5b7fb316be 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 cbdf4b50c7..503b0a320a 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp @@ -263,6 +263,7 @@ class CBGScheduler */ ExecutableEntityWithInfo get_next_ready_entity() { +<<<<<<< HEAD { std::lock_guard l(ready_callback_groups_mutex); if(needs_sync) { @@ -271,6 +272,16 @@ class CBGScheduler ExecutableEntity{sync_function, nullptr}, 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}; +>>>>>>> c36e550 (fix: Improved thread usage under congestion (#3258)) } return get_next_ready_entity_intern(); @@ -279,6 +290,7 @@ class CBGScheduler ExecutableEntityWithInfo get_next_ready_entity( GlobalEventIdProvider::MonotonicId max_id) { +<<<<<<< HEAD { std::lock_guard l(ready_callback_groups_mutex); if(needs_sync) { @@ -287,11 +299,24 @@ class CBGScheduler ExecutableEntity{sync_function, nullptr}, 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}; +>>>>>>> c36e550 (fix: Improved thread usage under congestion (#3258)) } 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 +363,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 +414,7 @@ class CBGScheduler bool release_workers = false; bool release_worker_once = false; + bool worker_checking_for_work = false; std::condition_variable work_ready_conditional; From a9cde92f8af0046c69abc1f973c56631dd6285dc Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 10 Sep 2026 17:29:15 +0200 Subject: [PATCH 2/2] chore: Fixed merge conflict Signed-off-by: Janosch Machowinski --- .../events_cbg_executor/scheduler.hpp | 34 ++++--------------- 1 file changed, 6 insertions(+), 28 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp index 503b0a320a..833a813ed0 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp @@ -263,25 +263,14 @@ class CBGScheduler */ ExecutableEntityWithInfo get_next_ready_entity() { -<<<<<<< HEAD - { - std::lock_guard l(ready_callback_groups_mutex); - if(needs_sync) { - needs_sync = false; - return ExecutableEntityWithInfo{ - ExecutableEntity{sync_function, nullptr}, - 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}; ->>>>>>> c36e550 (fix: Improved thread usage under congestion (#3258)) + return ExecutableEntityWithInfo{ + ExecutableEntity{sync_function, nullptr}, + false}; } return get_next_ready_entity_intern(); @@ -290,25 +279,14 @@ class CBGScheduler ExecutableEntityWithInfo get_next_ready_entity( GlobalEventIdProvider::MonotonicId max_id) { -<<<<<<< HEAD - { - std::lock_guard l(ready_callback_groups_mutex); - if(needs_sync) { - needs_sync = false; - return ExecutableEntityWithInfo{ - ExecutableEntity{sync_function, nullptr}, - 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}; ->>>>>>> c36e550 (fix: Improved thread usage under congestion (#3258)) + return ExecutableEntityWithInfo{ + ExecutableEntity{sync_function, nullptr}, + false}; } return get_next_ready_entity_intern(max_id);