diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index b00d29c774..6525fa360b 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/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; diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index d286ee150b..cbfcd00a1c 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/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. diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index 0b81c10069..ba9f8fdcce 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/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() { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 772cd8e260..7f018a51ce 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/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 diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index f2493f4f2a..fa85dc94ba 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/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 { 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 01b0082e5b..9947ad8d95 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/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,6 +507,7 @@ 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), @@ -511,7 +515,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { 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); }