From e1076b3ceb7e3d722864b7b23e49720894113ce7 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 7 Feb 2025 12:46:16 +0000 Subject: [PATCH] Handle SIGINT properly in the controller manager (backport #2014) (#2040) --- .../controller_manager/controller_manager.hpp | 10 ++- controller_manager/src/controller_manager.cpp | 69 +++++++++++++++++++ .../hardware_interface/resource_manager.hpp | 7 ++ hardware_interface/src/resource_manager.cpp | 18 ++++- 4 files changed, 102 insertions(+), 2 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4f28bf933b..4b03a3b4f4 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -81,7 +81,14 @@ class ControllerManager : public rclcpp::Node const rclcpp::NodeOptions & options = get_cm_node_options()); 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. + */ + CONTROLLER_MANAGER_PUBLIC + bool shutdown_controllers(); CONTROLLER_MANAGER_PUBLIC void robot_description_callback(const std_msgs::msg::String & msg); @@ -475,6 +482,7 @@ class ControllerManager : public rclcpp::Node int used_by_realtime_controllers_index_ = -1; }; + std::unique_ptr 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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 700456aac8..463a323baf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -48,6 +48,12 @@ static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false}; +inline bool is_controller_unconfigured( + const controller_interface::ControllerInterfaceBase & controller) +{ + return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; +} + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -322,6 +328,46 @@ ControllerManager::ControllerManager( } } +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 guard(rt_controllers_wrapper_.controllers_lock_); + std::vector 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()); + controller.c->get_node()->shutdown(); + } + 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::subscribe_to_robot_description_topic() { // set QoS to transient local to get messages that have already been published @@ -529,6 +575,29 @@ void ControllerManager::init_services() "~/set_hardware_component_state", std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + + // 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(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."); + })); } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 46723b3f72..78e0baf461 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/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. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 487a6cacb5..9a003540b4 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/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 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)