Add PreShutdownCallbackHandle to remove upon destruction

ros-controls · Jan 23, 2025 · e37e1fe · e37e1fe
1 parent 750ade3
commit e37e1fe
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 17 deletions.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -80,7 +80,7 @@ class ControllerManager : public rclcpp::Node
     const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
-  virtual ~ControllerManager() = default;
+  virtual ~ControllerManager();
 
   /// Shutdown all controllers in the controller manager.
   /**
@@ -537,6 +537,7 @@ class ControllerManager : public rclcpp::Node
     int used_by_realtime_controllers_index_ = -1;
   };
 
+  std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
   RTControllerListWrapper rt_controllers_wrapper_;
   std::unordered_map<std::string, ControllerChainSpec> controller_chain_spec_;
   std::vector<std::string> ordered_controllers_names_;

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -291,6 +291,16 @@ ControllerManager::ControllerManager(
   init_controller_manager();
 }
 
+ControllerManager::~ControllerManager()
+{
+  if (preshutdown_cb_handle_)
+  {
+    rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+    context->remove_pre_shutdown_callback(*(preshutdown_cb_handle_.get()));
+    preshutdown_cb_handle_.reset();
+  }
+}
+
 bool ControllerManager::shutdown_controllers()
 {
   RCLCPP_INFO(get_logger(), "Shutting down all controllers in the controller manager.");
@@ -361,22 +371,24 @@ void ControllerManager::init_controller_manager()
     &ControllerManager::controller_manager_diagnostic_callback);
 
   // Add on_shutdown callback to stop the controller manager
-  this->get_node_base_interface()->get_context()->add_pre_shutdown_callback(
-    [this]()
-    {
-      RCLCPP_INFO(get_logger(), "Shutdown request received....");
-      executor_->remove_node(this->get_node_base_interface());
-      executor_->cancel();
-      if (!this->shutdown_controllers())
-      {
-        RCLCPP_ERROR(get_logger(), "Failed shutting down the controllers.");
-      }
-      if (!resource_manager_->shutdown_components())
-      {
-        RCLCPP_ERROR(get_logger(), "Failed shutting down hardware components.");
-      }
-      RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
-    });
+  rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+  preshutdown_cb_handle_ =
+    std::make_unique<rclcpp::PreShutdownCallbackHandle>(context->add_pre_shutdown_callback(
+      [this]()
+      {
+        RCLCPP_INFO(get_logger(), "Shutdown request received....");
+        executor_->remove_node(this->get_node_base_interface());
+        executor_->cancel();
+        if (!this->shutdown_controllers())
+        {
+          RCLCPP_ERROR(get_logger(), "Failed shutting down the controllers.");
+        }
+        if (!resource_manager_->shutdown_components())
+        {
+          RCLCPP_ERROR(get_logger(), "Failed shutting down hardware components.");
+        }
+        RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
+      }));
 }
 
 void ControllerManager::initialize_parameters()