Add cleanup_controller_exported_interfaces method for proper cleanup

ros-controls · Feb 25, 2025 · 9dd8d75 · 9dd8d75
1 parent 1f01268
commit 9dd8d75
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 10 deletions.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -445,6 +445,13 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
 
+  /**
+   * @brief cleanup_controller_exported_interfaces - A method that cleans up the exported interfaces
+   * of a chainable controller
+   * @param controller - controller info
+   */
+  void cleanup_controller_exported_interfaces(const ControllerSpec & controller);
+
   std::shared_ptr<controller_manager::ParamListener> cm_param_listener_;
   std::shared_ptr<controller_manager::Params> params_;
   diagnostic_updater::Updater diagnostics_updater_;

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -812,6 +812,7 @@ controller_interface::return_type ControllerManager::unload_controller(
 
   RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
+  cleanup_controller_exported_interfaces(controller);
   if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
   {
     RCLCPP_DEBUG(
@@ -838,11 +839,6 @@ void ControllerManager::shutdown_controller(controller_manager::ControllerSpec &
 {
   try
   {
-    if (is_controller_inactive(*controller.c) && controller.c->is_chainable())
-    {
-      resource_manager_->remove_controller_reference_interfaces(controller.info.name);
-      resource_manager_->remove_controller_exported_state_interfaces(controller.info.name);
-    }
     const auto new_state = controller.c->get_node()->shutdown();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
     {
@@ -911,11 +907,7 @@ controller_interface::return_type ControllerManager::configure_controller(
       get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
     try
     {
-      if (controller->is_chainable())
-      {
-        resource_manager_->remove_controller_reference_interfaces(controller_name);
-        resource_manager_->remove_controller_exported_state_interfaces(controller_name);
-      }
+      cleanup_controller_exported_interfaces(*found_it);
       new_state = controller->get_node()->cleanup();
       if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
       {
@@ -3646,4 +3638,16 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
   return controller_node_options;
 }
 
+void ControllerManager::cleanup_controller_exported_interfaces(const ControllerSpec & controller)
+{
+  if (is_controller_inactive(controller.c) && controller.c->is_chainable())
+  {
+    RCLCPP_DEBUG(
+      get_logger(), "Removing controller '%s' exported interfaces from resource manager.",
+      controller.info.name.c_str());
+    resource_manager_->remove_controller_exported_state_interfaces(controller.info.name);
+    resource_manager_->remove_controller_reference_interfaces(controller.info.name);
+  }
+}
+
 }  // namespace controller_manager