Handle SIGINT properly in the controller manager (#2014)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -80,7 +80,13 @@ 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.
+  /**
+   * \return true if all controllers are successfully shutdown, false otherwise.
+   */
+  bool shutdown_controllers();
 
   void robot_description_callback(const std_msgs::msg::String & msg);
 
@@ -531,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_;

controller_manager/src/controller_manager.cpp

@@ -291,6 +291,46 @@ 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.");
+  // Shutdown all controllers
+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  std::vector<ControllerSpec> controllers_list = rt_controllers_wrapper_.get_updated_list(guard);
+  bool ctrls_shutdown_status = true;
+  for (auto & controller : controllers_list)
+  {
+    if (is_controller_active(controller.c))
+    {
+      RCLCPP_INFO(
+        get_logger(), "Deactivating controller '%s'", controller.c->get_node()->get_name());
+      controller.c->get_node()->deactivate();
+      controller.c->release_interfaces();
+    }
+    if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
+    {
+      RCLCPP_INFO(
+        get_logger(), "Shutting down controller '%s'", controller.c->get_node()->get_name());
+      shutdown_controller(controller);
+    }
+    ctrls_shutdown_status &=
+      (controller.c->get_node()->get_current_state().id() ==
+       lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+    executor_->remove_node(controller.c->get_node()->get_node_base_interface());
+  }
+  return ctrls_shutdown_status;
+}
+
 void ControllerManager::init_controller_manager()
 {
   // Get parameters needed for RT "update" loop to work
@@ -329,6 +369,29 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
+
+  // Add on_shutdown callback to stop 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....");
+        if (this->get_node_base_interface()->get_associated_with_executor_atomic().load())
+        {
+          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()

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -76,6 +76,13 @@ class ResourceManager
 
   virtual ~ResourceManager();
 
+  /// Shutdown all hardware components, irrespective of their state.
+  /**
+   * The method is called when the controller manager is being shutdown.
+   * @return true if all hardware components are successfully shutdown, false otherwise.
+   */
+  bool shutdown_components();
+
   /// Load resources from on a given URDF.
   /**
    * The resource manager can be post-initialized with a given URDF.

hardware_interface/src/resource_manager.cpp

@@ -573,7 +573,7 @@ class ResourceStorage
             result = shutdown_hardware(hardware);
             break;
           case State::PRIMARY_STATE_ACTIVE:
-            result = shutdown_hardware(hardware);
+            result = deactivate_hardware(hardware) && shutdown_hardware(hardware);
             break;
           case State::PRIMARY_STATE_FINALIZED:
             result = true;
@@ -1040,6 +1040,22 @@ ResourceManager::ResourceManager(
   }
 }
 
+bool ResourceManager::shutdown_components()
+{
+  std::unique_lock<std::recursive_mutex> guard(resource_interfaces_lock_);
+  bool shutdown_status = true;
+  for (auto const & hw_info : resource_storage_->hardware_info_map_)
+  {
+    rclcpp_lifecycle::State finalized_state(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED);
+    if (set_component_state(hw_info.first, finalized_state) != return_type::OK)
+    {
+      shutdown_status = false;
+    }
+  }
+  return shutdown_status;
+}
+
 // CM API: Called in "callback/slow"-thread
 bool ResourceManager::load_and_initialize_components(
   const std::string & urdf, const unsigned int update_rate)