fix(Executor): Fixed lost of entities rebuild request

Signed-off-by: Janosch Machowinski <[email protected]>

Author: Janosch Machowinski

Diff for: 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 for: 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));