fix(Executor): Fixed entities not beeing executed after just beeing added (#2724)

Fixes #2589

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

Author: jmachowinski

rclcpp/src/rclcpp/executor.cpp

@@ -371,7 +371,14 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
   // both spin_some and spin_all wait for work at the beginning
   wait_result_.reset();
   wait_for_work(std::chrono::milliseconds(0));
-  bool just_waited = true;
+  bool entity_states_fully_polled = true;
+
+  if (entities_need_rebuild_) {
+    // if the last wait triggered a collection rebuild, we need to call
+    // wait_for_work once more, in order to do a collection rebuild and collect
+    // events from the just added entities
+    entity_states_fully_polled = false;
+  }
 
   // The logic of this while loop is as follows:
   //
@@ -393,12 +400,14 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
     AnyExecutable any_exec;
     if (get_next_ready_executable(any_exec)) {
       execute_any_executable(any_exec);
-      just_waited = false;
+      // during the execution some entity might got ready therefore we need
+      // to repoll the states of all entities
+      entity_states_fully_polled = false;
     } else {
       // if nothing is ready, reset the result to clear it
       wait_result_.reset();
 
-      if (just_waited) {
+      if (entity_states_fully_polled) {
         // there was no work after just waiting, always exit in this case
         // before the exhaustive condition can be checked
         break;
@@ -408,7 +417,13 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
         // if exhaustive, wait for work again
         // this only happens for spin_all; spin_some only waits at the start
         wait_for_work(std::chrono::milliseconds(0));
-        just_waited = true;
+        entity_states_fully_polled = true;
+        if (entities_need_rebuild_) {
+          // if the last wait triggered a collection rebuild, we need to call
+          // wait_for_work once more, in order to do a collection rebuild and
+          // collect events from the just added entities
+          entity_states_fully_polled = false;
+        }
       } else {
         break;
       }

rclcpp/test/rclcpp/executors/test_executors_warmup.cpp

@@ -99,14 +99,6 @@ TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup_with_cbgroup)
 {
   using ExecutorType = TypeParam;
 
-  // TODO(alsora): Enable when https://github.com/ros2/rclcpp/pull/2595 gets merged
-  if (
-    std::is_same<ExecutorType, rclcpp::executors::SingleThreadedExecutor>() ||
-    std::is_same<ExecutorType, rclcpp::executors::MultiThreadedExecutor>())
-  {
-    GTEST_SKIP();
-  }
-
   ExecutorType executor;
 
   // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event