From 438e233008ac291497f103413806ac42e720495e Mon Sep 17 00:00:00 2001
From: Janosch Machowinski <j.machowinski@cellumation.com>
Date: Mon, 8 Apr 2024 20:40:58 +0200
Subject: [PATCH] fix(Executor): Fixed lost of entities rebuild request

Signed-off-by: Janosch Machowinski <j.machowinski@cellumation.com>
---
 rclcpp/src/rclcpp/executor.cpp                           | 9 +++------
 .../rclcpp/executors/static_single_threaded_executor.cpp | 2 +-
 2 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
index 06a2d2e150..62f2f64595 100644
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -123,7 +123,7 @@ void Executor::trigger_entity_recollect(bool notify)
 {
   this->entities_need_rebuild_.store(true);
 
-  if (!spinning.load()) {
+  if (!spinning.load() && entities_need_rebuild_.exchange(false)) {
     std::lock_guard<std::mutex> guard(mutex_);
     this->collect_entities();
   }
@@ -387,9 +387,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   }
 
   // Reset the callback_group, regardless of type
-  if (any_exec.callback_group) {
-    any_exec.callback_group->can_be_taken_from().store(true);
-  }
+  any_exec.callback_group->can_be_taken_from().store(true);
 }
 
 template<typename Taker, typename Handler>
@@ -626,7 +624,6 @@ Executor::collect_entities()
   // In the case that an entity already has an expired weak pointer
   // before being removed from the waitset, additionally prune the waitset.
   this->wait_set_.prune_deleted_entities();
-  this->entities_need_rebuild_.store(false);
 }
 
 void
@@ -639,7 +636,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 
   {
     std::lock_guard<std::mutex> guard(mutex_);
-    if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
+    if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
       this->collect_entities();
     }
   }
diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
index 831076c61c..b8fe92cb09 100644
--- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
+++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
@@ -110,7 +110,7 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
 std::optional<rclcpp::WaitResult<rclcpp::WaitSet>>
 StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout)
 {
-  if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
+  if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) {
     this->collect_entities();
   }
   auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));