Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (#1303)

* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>
Signed-off-by: Stephen Brawner <brawner@gmail.com>

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -62,6 +62,11 @@ public:
     rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
     rcl_guard_condition_t * executor_guard_condition);
 
+  /// Finalize StaticExecutorEntitiesCollector to clear resources
+  RCLCPP_PUBLIC
+  void
+  fini();
+
   RCLCPP_PUBLIC
   void
   execute() override;

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -163,6 +163,7 @@ public:
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
     entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+    RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
     while (rclcpp::ok(this->context_)) {
       // Do one set of work.

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -62,6 +62,13 @@ StaticExecutorEntitiesCollector::init(
   execute();
 }
 
+void
+StaticExecutorEntitiesCollector::fini()
+{
+  memory_strategy_->clear_handles();
+  exec_list_.clear();
+}
+
 void
 StaticExecutorEntitiesCollector::execute()
 {

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -41,6 +41,7 @@ StaticSingleThreadedExecutor::spin()
   // Set memory_strategy_ and exec_list_ based on weak_nodes_
   // Prepare wait_set_ based on memory_strategy_
   entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -259,6 +259,14 @@ public:
     }
   }
 
+  ~TestWaitable()
+  {
+    rcl_ret_t ret = rcl_guard_condition_fini(&gc_);
+    if (RCL_RET_OK != ret) {
+      fprintf(stderr, "failed to call rcl_guard_condition_fini\n");
+    }
+  }
+
   bool
   add_to_wait_set(rcl_wait_set_t * wait_set) override
   {

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -159,6 +159,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
   rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
   EXPECT_EQ(
     expected_number_of_entities->subscriptions,
     entities_collector_->get_number_of_subscriptions());
@@ -206,6 +207,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
 
   // Expect weak_node pointers to be cleaned up and used
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
   EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
   EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
   EXPECT_EQ(0u, entities_collector_->get_number_of_services());
@@ -267,6 +269,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   EXPECT_EQ(
     1u + expected_number_of_entities->subscriptions,