diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4f28bf933b..b7220d8c75 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/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 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..72acf29bfb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -307,7 +307,58 @@ ControllerManager::ControllerManager( std::make_shared>( 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 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::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( + "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(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() 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)