diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 034c6582ec..73926a7aac 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -217,12 +217,16 @@ class Executor * This also means that future callback groups created by the given node are no longer * automatically added to this executor. * + * If the node is not associated with any executor, this logs a warning and returns without + * doing anything instead of throwing. This makes redundant removals safe and non-fatal. + * Removing a node that is associated with a different executor + * is still treated as a programming error and throws. + * * \param[in] node_ptr Shared pointer to the node to remove. * \param[in] notify True to trigger the interrupt guard condition and wake up the executor. * This is useful if the last node was removed from the executor while the executor was blocked * waiting for work in another thread, because otherwise the executor would never be notified. - * \throw std::runtime_error if the node is not associated with an executor. - * \throw std::runtime_error if the node is not associated with this executor. + * \throw std::runtime_error if the node is associated with a different executor. */ RCLCPP_PUBLIC virtual void diff --git a/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp b/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp index b791f6f5da..04d422473b 100644 --- a/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp +++ b/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp @@ -102,6 +102,15 @@ class EventsCBGExecutor : public rclcpp::Executor void add_node(const std::shared_ptr & node_ptr, bool notify = true) override; + /// Remove a node from the executor. + /** + * Note that this executor also removes every added node during shutdown(), so a node may + * already be disassociated by the time an owner removes it. + * Removing a node that is not associated with any executor logs a warning and returns. + * \throw std::runtime_error if trying to remove a node associated with a different executor + * + * \see rclcpp::Executor::remove_node + */ RCLCPP_PUBLIC void remove_node( diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp index 972c9e69bb..f859046f94 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -86,9 +86,12 @@ class ExecutorEntitiesCollector /// Remove a node from the entity collector /** + * If the node is not associated with any executor, this logs a warning and returns without + * doing anything instead of throwing, so redundant removals are safe. Removing a node that + * is associated with a different executor still throws. + * * \param[in] node_ptr a shared pointer that points to a node base interface - * \throw std::runtime_error if the node is associated with an executor - * \throw std::runtime_error if the node is associated with this executor + * \throw std::runtime_error if the node is associated with a different executor */ RCLCPP_PUBLIC void diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp index 80f87ac57a..7ea72eea42 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp @@ -746,19 +746,30 @@ EventsCBGExecutor::remove_node( { std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); if (!has_executor.exchange(false)) { - throw std::runtime_error( - std::string("Node '") + node_ptr->get_fully_qualified_name() + - "' needs to be associated with an executor."); + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "Node '%s' is not associated with any executor, ignoring remove_node request.", + node_ptr->get_fully_qualified_name()); + return; } { std::lock_guard lock{added_nodes_mutex_}; - added_nodes.erase( - std::remove_if( - added_nodes.begin(), added_nodes.end(), [&node_ptr](const auto & weak_ptr) { - const auto shr_ptr = weak_ptr.lock(); - return shr_ptr && shr_ptr == node_ptr; - }), added_nodes.end()); + auto new_end = std::remove_if( + added_nodes.begin(), added_nodes.end(), [&node_ptr](const auto & weak_ptr) { + const auto shr_ptr = weak_ptr.lock(); + return shr_ptr && shr_ptr == node_ptr; + }); + if (new_end == added_nodes.end()) { + // The node's association flag was set, but the node is not tracked by + // this executor, so it belongs to a different executor. Restore the flag + // we cleared above and throw, mirroring Executor::remove_node. + has_executor.store(true); + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with this executor."); + } + added_nodes.erase(new_end, added_nodes.end()); } node_ptr->for_each_callback_group( diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 9ed6f32882..f561e1318f 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -18,6 +18,8 @@ #include "rclcpp/executors/executor_notify_waitable.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rcutils/logging_macros.h" + namespace rclcpp { namespace executors @@ -114,9 +116,11 @@ ExecutorEntitiesCollector::remove_node( { std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); if (!has_executor.exchange(false)) { - throw std::runtime_error( - std::string("Node '") + node_ptr->get_fully_qualified_name() + - "' needs to be associated with an executor."); + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "Node '%s' is not associated with any executor, ignoring remove_node request.", + node_ptr->get_fully_qualified_name()); + return; } std::lock_guard lock(mutex_); @@ -125,6 +129,9 @@ ExecutorEntitiesCollector::remove_node( bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; if (!(associated || add_queued) || remove_queued) { + // The node is associated with a different executor. Restore the flag we + // cleared above so the owning executor is left untouched before throwing. + has_executor.store(true); throw std::runtime_error( std::string("Node '") + node_ptr->get_fully_qualified_name() + "' needs to be associated with this executor."); diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp index 930dc68aff..972cb4c93b 100644 --- a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -83,19 +83,21 @@ TEST_F(TestExecutorEntitiesCollector, add_associated_node) { TEST_F(TestExecutorEntitiesCollector, remove_unassociated_node) { auto node1 = std::make_shared("node1", "ns"); - // Add an already-associated node - RCLCPP_EXPECT_THROW_EQ( - entities_collector->remove_node(node1->get_node_base_interface()), - std::runtime_error("Node '/ns/node1' needs to be associated with an executor.")); + EXPECT_NO_THROW(entities_collector->remove_node(node1->get_node_base_interface())); // Simulate node being associated somewhere else auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); has_executor.store(true); - // Add an already-associated node + // Removing a node that is associated with a *different* executor is still an + // error and is expected to throw. RCLCPP_EXPECT_THROW_EQ( entities_collector->remove_node(node1->get_node_base_interface()), std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); + + // The failed removal must not steal the node from its owning executor. + EXPECT_TRUE(has_executor.load()); + has_executor.store(false); } TEST_F(TestExecutorEntitiesCollector, add_remove_node_before_update) { diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 8911c0a28c..37cdca87d9 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -146,6 +146,27 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) executor1.remove_node(this->node, true); } +TYPED_TEST(TestExecutors, removeNodeNotAssociated) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + EXPECT_NO_THROW(executor.remove_node(this->node, false)); +} + +// Removing a node that is associated with a *different* executor is still an +// error and throws, and must not steal the node from its owning executor. +TYPED_TEST(TestExecutors, removeNodeDifferentExecutor) +{ + using ExecutorType = TypeParam; + ExecutorType executor1; + ExecutorType executor2; + EXPECT_NO_THROW(executor1.add_node(this->node)); + EXPECT_THROW(executor2.remove_node(this->node, false), std::runtime_error); + EXPECT_TRUE( + this->node->get_node_base_interface()->get_associated_with_executor_atomic().load()); + EXPECT_NO_THROW(executor1.remove_node(this->node, false)); +} + // Check simple spin example TYPED_TEST(TestExecutors, spinWithTimer) { diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 8dd798dbab..8d6ce654ed 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -188,9 +188,7 @@ TEST_F(TestExecutor, remove_node_not_associated) { rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); - RCLCPP_EXPECT_THROW_EQ( - dummy.remove_node(node->get_node_base_interface(), false), - std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); + EXPECT_NO_THROW(dummy.remove_node(node->get_node_base_interface(), false)); } TEST_F(TestExecutor, remove_node_associated_with_different_executor) { diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index f9793dc307..ab9bece121 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -83,7 +83,10 @@ ComponentManager::~ComponentManager() RCLCPP_DEBUG(get_logger(), "Removing components from executor"); if (auto exec = executor_.lock()) { for (auto & wrapper : node_wrappers_) { - exec->remove_node(wrapper.second.get_node_base_interface()); + auto node_interface = wrapper.second.get_node_base_interface(); + if (node_interface->get_associated_with_executor_atomic().load()) { + exec->remove_node(node_interface); + } } } } diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index d87c89ea04..a8d1b4e768 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -22,6 +22,7 @@ #include "composition_interfaces/srv/unload_node.hpp" #include "composition_interfaces/srv/list_nodes.hpp" +#include "rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp" #include "rclcpp_components/component_manager.hpp" #include "rclcpp_components/component_manager_isolated.hpp" @@ -372,3 +373,50 @@ TEST_F(TestComponentManager, components_api) test_components_api(true); } } + +// Test fixture for the component manager which exercises node removal like on +// shutdown. EventsCBGExecutor::shutdown() is protected, so this is to +// reproduce its effect (dropping every node from the executor while the manager +// still tracks them in node_wrappers_) +class DoubleRemoveComponentManager : public rclcpp_components::ComponentManager +{ +public: + using rclcpp_components::ComponentManager::ComponentManager; + + void remove_all_nodes_from_executor() + { + if (auto exec = executor_.lock()) { + for (auto & wrapper : node_wrappers_) { + exec->remove_node(wrapper.second.get_node_base_interface()); + } + } + } +}; + +TEST_F(TestComponentManager, no_throw_remove_node_twice_on_shutdown) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + auto client_node = rclcpp::Node::make_shared("test_component_manager_3186"); + + exec->add_node(manager); + exec->add_node(client_node); + + auto composition_client = client_node->create_client( + "/ComponentManager/_container/load_node"); + ASSERT_TRUE(composition_client->wait_for_service(20s)) << "service not available after waiting"; + + // Load a component so the manager tracks a node that is associated with the + // executor (~ComponentManager only removes nodes when node_wrappers_ is + // non-empty). + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + auto future = composition_client->async_send_request(request); + ASSERT_EQ(exec->spin_until_future_complete(future, 5s), rclcpp::FutureReturnCode::SUCCESS); + ASSERT_TRUE(future.get()->success); + + // the executor removes all of its nodes on shutdown + manager->remove_all_nodes_from_executor(); + EXPECT_NO_THROW(manager.reset()); +}