fix bug in StaticSTE that broke the add callback groups to executor tests

Signed-off-by: William Woodall <[email protected]>

Author: wjwwood

rclcpp/src/rclcpp/executor.cpp

@@ -72,6 +72,10 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
       }
     });
 
+  notify_waitable_->set_on_ready_callback([this](auto, auto) {
+    this->entities_need_rebuild_.store(true);
+  });
+
   notify_waitable_->add_guard_condition(interrupt_guard_condition_);
   notify_waitable_->add_guard_condition(shutdown_guard_condition_);
 }

rclcpp/src/rclcpp/executors/executor_entities_collector.cpp

@@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector()
     if (group_ptr) {
       group_ptr->get_associated_with_executor_atomic().store(false);
     }
+    // Disassociate the guard condition from the executor notify waitable
+    auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
+      this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
+      weak_groups_to_guard_conditions_.erase(guard_condition_it);
+    }
   }
   pending_manually_added_groups_.clear();
   pending_manually_removed_groups_.clear();
@@ -144,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g
   }
 
   this->pending_manually_added_groups_.insert(group_ptr);
+
+  // Store callback group notify guard condition in map and add it to the notify waitable
+  auto group_guard_condition = group_ptr->get_notify_guard_condition();
+  weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition});
+  this->notify_waitable_->add_guard_condition(group_guard_condition);
 }
 
 void
@@ -341,6 +352,13 @@ ExecutorEntitiesCollector::process_queues()
     auto group_ptr = weak_group_ptr.lock();
     if (group_ptr) {
       this->add_callback_group_to_collection(group_ptr, manually_added_groups_);
+    } else {
+      // Disassociate the guard condition from the executor notify waitable
+      auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+      if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
+        this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
+        weak_groups_to_guard_conditions_.erase(guard_condition_it);
+      }
     }
   }
   pending_manually_added_groups_.clear();

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -13,7 +13,6 @@
 // limitations under the License.
 
 #include "rclcpp/executors/executor_entities_collection.hpp"
-#include "rclcpp/executors/executor_notify_waitable.hpp"
 #include "rcpputils/scope_exit.hpp"
 
 #include "rclcpp/executors/static_single_threaded_executor.hpp"

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -200,18 +200,25 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t
 {
   using ExecutorType = TypeParam;
 
+  auto count_callback_groups_in_node = [](auto node) {
+      size_t num = 0;
+      node->get_node_base_interface()->for_each_callback_group([&num](auto) {
+          num++;
+        });
+      return num;
+    };
+
   ExecutorType executor;
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   executor.add_node(node->get_node_base_interface());
-  ASSERT_EQ(executor.get_all_callback_groups().size(), 1u);
+  ASSERT_EQ(executor.get_all_callback_groups().size(), count_callback_groups_in_node(node));
   std::atomic_size_t timer_count {0};
   auto timer_callback = [&executor, &timer_count]() {
-      printf("in timer_callback(%zu)\n", timer_count.load());
-      if (timer_count > 0) {
-        ASSERT_GT(executor.get_all_callback_groups().size(), 1u);
+      auto cur_timer_count = timer_count++;
+      printf("in timer_callback(%zu)\n", cur_timer_count);
+      if (cur_timer_count > 0) {
         executor.cancel();
       }
-      timer_count++;
     };
   rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive);