From 729212915aecfe9f631bf3f5705e0d266227eb14 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 29 Jan 2025 20:05:58 +0100 Subject: [PATCH 1/3] Handle SIGINT properly in the controller manager (#2014) --- .../controller_manager/controller_manager.hpp | 11 +++- controller_manager/src/controller_manager.cpp | 63 +++++++++++++++++++ .../hardware_interface/resource_manager.hpp | 7 +++ hardware_interface/src/resource_manager.cpp | 18 +++++- 4 files changed, 95 insertions(+), 4 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4f28bf933b..6fcd2724ee 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -80,10 +80,14 @@ class ControllerManager : public rclcpp::Node const std::string & namespace_ = "", 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. + */ + bool shutdown_controllers(); - CONTROLLER_MANAGER_PUBLIC void robot_description_callback(const std_msgs::msg::String & msg); CONTROLLER_MANAGER_PUBLIC @@ -475,6 +479,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..13a97aec2b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -322,6 +322,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()); + 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::subscribe_to_robot_description_topic() { // set QoS to transient local to get messages that have already been published @@ -529,6 +569,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) From 3e0fefe8ed3dcd692b29294c0f9408698da7117c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 6 Feb 2025 12:27:38 +0100 Subject: [PATCH 2/3] fix the shutdown and missing is_controller_unconfigured --- controller_manager/src/controller_manager.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 13a97aec2b..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; @@ -352,7 +358,7 @@ bool ControllerManager::shutdown_controllers() { RCLCPP_INFO( get_logger(), "Shutting down controller '%s'", controller.c->get_node()->get_name()); - shutdown_controller(controller); + controller.c->get_node()->shutdown(); } ctrls_shutdown_status &= (controller.c->get_node()->get_current_state().id() == From 2d8fa813e03c3d22effd3ded205993c8a4395481 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 6 Feb 2025 13:08:46 +0100 Subject: [PATCH 3/3] add CONTROLLER_MANAGER_PUBLIC visibility macros --- .../include/controller_manager/controller_manager.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 6fcd2724ee..4b03a3b4f4 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -80,14 +80,17 @@ class ControllerManager : public rclcpp::Node const std::string & namespace_ = "", const rclcpp::NodeOptions & options = get_cm_node_options()); + CONTROLLER_MANAGER_PUBLIC 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); CONTROLLER_MANAGER_PUBLIC