Check for association with an executor before removing nodes in ComponentManager

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);
+        }
       }
     }
   }

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<rclcpp::executors::EventsCBGExecutor>();
+  auto manager = std::make_shared<DoubleRemoveComponentManager>(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<composition_interfaces::srv::LoadNode>(
+    "/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<composition_interfaces::srv::LoadNode::Request>();
+  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());
+}