diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 0df25fd..604f20d 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/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; diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 9d39ba8..d007988 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/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. diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index 5461e66..7d514b2 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/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() { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 728a090..ced3880 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/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 diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index ac24459..e6beba6 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/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 { diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 92ded7f..6b2d0d1 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/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,