fix(Executor): Fixed lost of entities rebuild request

Signed-off-by: Janosch Machowinski <j.machowinski@cellumation.com>

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();
  }
  }

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));