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>

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -64,9 +64,14 @@ class StaticExecutorEntitiesCollector final
   void
   init(
     rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+    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

@@ -193,6 +193,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor
     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

@@ -61,7 +61,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 void
 StaticExecutorEntitiesCollector::init(
   rcl_wait_set_t * p_wait_set,
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
   rcl_guard_condition_t * executor_guard_condition)
 {
   // Empty initialize executable list
@@ -80,6 +80,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

@@ -42,6 +42,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

@@ -410,6 +410,14 @@ class TestWaitable : public rclcpp::Waitable
     }
   }
 
+  ~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

@@ -170,6 +170,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());
@@ -217,6 +218,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());
@@ -278,6 +280,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,
@@ -356,6 +359,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_
   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());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
@@ -365,7 +369,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_
   }
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize_error) {
@@ -386,6 +389,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize
   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());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
@@ -395,7 +399,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize
   }
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_not_initialized) {
@@ -423,6 +426,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
   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());
 
   {
     auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
@@ -432,7 +436,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
   }
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait_set_failed) {
@@ -472,6 +475,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait
   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());
 
   {
     auto mock = mocking_utils::patch_and_return(
@@ -483,7 +487,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait
   }
 
   entities_collector_->remove_node(node->get_node_base_interface());
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
@@ -504,14 +507,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
   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());
 
   RCLCPP_EXPECT_THROW_EQ(
     entities_collector_->add_to_wait_set(nullptr),
     std::runtime_error("Executor waitable: couldn't add guard condition to wait set"));
   rcl_reset_error();
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) {
@@ -540,10 +543,10 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group)
   cb_group.reset();
 
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
   ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) {
@@ -610,10 +613,10 @@ TEST_F(
   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());
 
   cb_group->get_associated_with_executor_atomic().exchange(false);
   entities_collector_->execute();
 
   EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
-  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
 }