From 0705e71e1eece71cde7f800c0087ba7b65cf97fc Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Fri, 2 Aug 2024 13:55:17 +0000 Subject: [PATCH] fix: Double spin required since 28.2.0 Rebuild the collection explicitly before the wait, if the notify_waitable_ has been triggered. Signed-off-by: Janosch Machowinski --- rclcpp/src/rclcpp/executor.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 69131cc111..22b195d337 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -726,6 +726,27 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) { TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); + // query, if notify_waitable_ was triggered by a guard condition + // make sure that it is executed before the wait, as this enforces + // a collection rebuild before the wait. + { + // the real one has been added to the main waitset, therefore we need to create a copy + executors::ExecutorNotifyWaitable::SharedPtr notify_cpy = std::make_shared(*notify_waitable_); + + rclcpp::WaitSet notify_only_wait_set_({}, {}, {}, {}, {}, {}, context_); + notify_only_wait_set_.add_waitable(notify_cpy); + auto res = notify_only_wait_set_.wait(std::chrono::nanoseconds(0)); + if(res.kind() == WaitResultKind::Ready) { + auto & rcl_wait_set = res.get_wait_set().get_rcl_wait_set(); + if (notify_cpy->is_ready(rcl_wait_set)) { + notify_cpy->execute(notify_cpy->take_data()); + + // make sure we don't loos a wakeup + interrupt_guard_condition_->trigger(); + } + } + } + // Clear any previous wait result this->wait_result_.reset();