Handle SIGINT properly in the controller manager (#2014)

(cherry picked from commit 882b489)

# Conflicts:
#	controller_manager/include/controller_manager/controller_manager.hpp
#	controller_manager/src/controller_manager.cpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -80,8 +80,18 @@ class ControllerManager : public rclcpp::Node
     const std::string & namespace_ = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
+<<<<<<< HEAD
   CONTROLLER_MANAGER_PUBLIC
   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();
+>>>>>>> 882b489 (Handle SIGINT properly in the controller manager (#2014))
 
   CONTROLLER_MANAGER_PUBLIC
   void robot_description_callback(const std_msgs::msg::String & msg);
@@ -475,6 +485,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_;
   /// mutex copied from ROS1 Control, protects service callbacks
   /// not needed if we're guaranteed that the callbacks don't come from multiple threads

controller_manager/src/controller_manager.cpp

@@ -307,7 +307,58 @@ ControllerManager::ControllerManager(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
+<<<<<<< HEAD
   if (!get_parameter("update_rate", update_rate_))
+=======
+  initialize_parameters();
+  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
+  if (is_resource_manager_initialized())
+>>>>>>> 882b489 (Handle SIGINT properly in the controller manager (#2014))
   {
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
@@ -320,6 +371,53 @@ ControllerManager::ControllerManager(
   {
     init_services();
   }
+<<<<<<< HEAD
+=======
+
+  // set QoS to transient local to get messages that have already been published
+  // (if robot state publisher starts before controller manager)
+  robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
+    "robot_description", rclcpp::QoS(1).transient_local(),
+    std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1));
+  RCLCPP_INFO(
+    get_logger(), "Subscribing to '%s' topic for robot description.",
+    robot_description_subscription_->get_topic_name());
+
+  // Setup diagnostics
+  periodicity_stats_.Reset();
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+  diagnostics_updater_.add(
+    "Hardware Components Activity", this,
+    &ControllerManager::hardware_components_diagnostic_callback);
+  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.");
+      }));
+>>>>>>> 882b489 (Handle SIGINT properly in the controller manager (#2014))
 }
 
 void ControllerManager::subscribe_to_robot_description_topic()

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -70,6 +70,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 
   ~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

@@ -397,7 +397,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;
@@ -675,6 +675,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
 void ResourceManager::load_urdf(
   const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components)