Call shutdown instead of cleanup during controller unloading

ros-controls · Jan 11, 2025 · 8cadf7c · 8cadf7c
1 parent d501f1b
commit 8cadf7c
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 24 deletions.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -317,6 +317,13 @@ class ControllerManager : public rclcpp::Node
 
   void initialize_parameters();
 
+  /**
+   * Call shutdown and move given controller to the finalized state.
+   *
+   * \param[in] controller controller to be shutdown.
+   */
+  void shutdown_controller(controller_manager::ControllerSpec & controller) const;
+
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"
    * and "control loop" threads.

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -695,7 +695,7 @@ controller_interface::return_type ControllerManager::unload_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  RCLCPP_DEBUG(get_logger(), "Cleanup controller");
+  RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
   // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
   // cleaning-up controllers?
@@ -705,29 +705,7 @@ controller_interface::return_type ControllerManager::unload_controller(
       get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
     // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
     // cleaning-up controllers?
-    try
-    {
-      const auto new_state = controller.c->get_node()->cleanup();
-      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
-      {
-        RCLCPP_WARN(
-          get_logger(), "Failed to clean-up the controller '%s' before unloading!",
-          controller_name.c_str());
-      }
-    }
-    catch (const std::exception & e)
-    {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s",
-        typeid(e).name(), controller_name.c_str(), e.what());
-    }
-    catch (...)
-    {
-      RCLCPP_ERROR(
-        get_logger(), "Failed to clean-up the controller '%s' before unloading",
-        controller_name.c_str());
-    }
+    shutdown_controller(controller);
   }
   executor_->remove_node(controller.c->get_node()->get_node_base_interface());
   to.erase(found_it);
@@ -745,6 +723,31 @@ controller_interface::return_type ControllerManager::unload_controller(
   return controller_interface::return_type::OK;
 }
 
+void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const
+try
+{
+  const auto new_state = controller.c->get_node()->shutdown();
+  if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  {
+    RCLCPP_WARN(
+      get_logger(), "Failed to shut-down the controller '%s' before unloading!",
+      controller.info.name.c_str());
+  }
+}
+catch (const std::exception & e)
+{
+  RCLCPP_ERROR(
+    get_logger(),
+    "Caught exception of type : %s while shut down the controller '%s' before unloading: %s",
+    typeid(e).name(), controller.info.name.c_str(), e.what());
+}
+catch (...)
+{
+  RCLCPP_ERROR(
+    get_logger(), "Failed to shut-down the controller '%s' before unloading",
+    controller.info.name.c_str());
+}
+
 std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const
 {
   std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);