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 <[email protected]>
ros2 · Aug 2, 2024 · f938f9a · f938f9a
1 parent 45adf65
commit f938f9a
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -726,6 +726,22 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
   TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count());
 
+  // 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<executors::ExecutorNotifyWaitable>(*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();