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..833a813ed0 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{ - 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{ + ExecutableEntity{sync_function, nullptr}, + 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{ - 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{ + ExecutableEntity{sync_function, nullptr}, + 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;