diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index b7dd1487f9..2b558e82c1 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -79,7 +79,6 @@ EventsExecutor::setup_notify_waitable() // ---> we need to wake up the executor so that it can terminate // - a node or callback group guard condition is triggered: // ---> the entities collection is changed, we need to update callbacks - entities_need_rebuild_ = false; this->handle_updated_entities(false); }); @@ -168,6 +167,14 @@ EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhau return false; }; + // If this spin is not exhaustive (e.g. spin_some), we need to explicitly check + // if entities need to be rebuilt here rather than letting the notify waitable event do it. + // A non-exhaustive spin would not check for work a second time, thus delaying the execution + // of some entities to the next invocation of spin. + if (!exhaustive) { + this->handle_updated_entities(false); + } + // Get the number of events and timers ready at start const size_t ready_events_at_start = events_queue_->size(); size_t executed_events = 0; @@ -311,9 +318,18 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } void -EventsExecutor::handle_updated_entities(bool notify) +EventsExecutor::handle_updated_entities(bool /*notify*/) { - (void)notify; + // Do not rebuild if we don't need to. + // This could happen if a rebuild event gets generated, but then + // this function gets called before the event gets processed. + // This could happen if a node or callback group is manually added to the executor. + const bool notify_waitable_triggered = entities_need_rebuild_.exchange(false); + const bool nodes_or_cb_groups_modified = this->collector_.has_pending(); + if (!notify_waitable_triggered && !nodes_or_cb_groups_modified) { + return; + } + // Build the new collection this->collector_.update_collections(); auto callback_groups = this->collector_.get_all_callback_groups();