From bde033998fa1b2826230a6e3bdcea658790b6866 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 22 Jan 2024 16:13:03 +0100 Subject: [PATCH] Optimize cleanup controllers method. --- controller_manager/src/controller_manager.cpp | 117 ++++++++++-------- 1 file changed, 65 insertions(+), 52 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 26a9112a9a..8c7946fbef 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -41,6 +41,18 @@ rclcpp::QoS qos_services = .reliable() .durability_volatile(); +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_unconfigured( + const controller_interface::ControllerInterfaceBaseSharedPtr & controller) +{ + return is_controller_unconfigured(*controller); +} + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -606,41 +618,46 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c return load_controller(controller_name, controller_type); } -controller_interface::return_type ControllerManager::unload_controller( +controller_interface::return_type ControllerManager::cleanup_controller( const std::string & controller_name) { - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); - const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + RCLCPP_INFO(get_logger(), "Cleanup controller '%s'", controller_name.c_str()); - // Transfers the active controllers over, skipping the one to be removed and the active ones. - to = from; + const auto & controllers = get_loaded_controllers(); auto found_it = std::find_if( - to.begin(), to.end(), + controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controller_name)); - if (found_it == to.end()) + + if (found_it == controllers.end()) { - // Fails if we could not remove the controllers - to.clear(); RCLCPP_ERROR( get_logger(), - "Could not unload controller with name '%s' because no controller with this name exists", + "Could not cleanup controller with name '%s' because no controller with this name exists", controller_name.c_str()); return controller_interface::return_type::ERROR; } + auto controller = found_it->c; - auto & controller = *found_it; + if (is_controller_unconfigured(controller)) + { + // all good nothing to do! + return controller_interface::return_type::OK; + } - if (is_controller_active(*controller.c)) + auto state = controller->get_state(); + if ( + state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || + state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - to.clear(); RCLCPP_ERROR( - get_logger(), "Could not unload controller with name '%s' because it is still active", - controller_name.c_str()); + get_logger(), "Controller '%s' can not be cleaned-up from '%s' state.", + controller_name.c_str(), state.label().c_str()); return controller_interface::return_type::ERROR; } - if (controller.c->is_async()) + + // ASYNCHRONOUS CONTROLLERS: Stop background thread for update + if (controller->is_async()) { RCLCPP_DEBUG( get_logger(), "Removing controller '%s' from the list of async controllers", @@ -648,36 +665,45 @@ controller_interface::return_type ControllerManager::unload_controller( async_controller_threads_.erase(controller_name); } - RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? - controller.c->get_node()->cleanup(); - executor_->remove_node(controller.c->get_node()->get_node_base_interface()); - to.erase(found_it); + // CHAINABLE CONTROLLERS: remove reference interfaces of chainable controllers + if (controller->is_chainable()) + { + RCLCPP_DEBUG( + get_logger(), + "Controller '%s' is chainable. Interfaces are being removed from resource manager.", + controller_name.c_str()); + resource_manager_->remove_controller_reference_interfaces(controller_name); + } - // Destroys the old controllers list when the realtime thread is finished with it. - RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list"); - rt_controllers_wrapper_.switch_updated_list(guard); - std::vector & new_unused_list = rt_controllers_wrapper_.get_unused_list(guard); - RCLCPP_DEBUG(get_logger(), "Destruct controller"); - new_unused_list.clear(); - RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); + RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + auto new_state = controller->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + get_logger(), "After cleanup-up, controller '%s' is in state '%s', expected 'unconfigured'.", + controller_name.c_str(), new_state.label().c_str()); + return controller_interface::return_type::ERROR; + } - RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Successfully cleaned-up controller '%s'", controller_name.c_str()); return controller_interface::return_type::OK; } -controller_interface::return_type ControllerManager::cleanup_controller( +controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { + // first find and clean controller if it is inactive + if (cleanup_controller(controller_name) != controller_interface::return_type::OK) + { + return controller_interface::return_type::ERROR; + } + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); - // Transfers the active controllers over, skipping the one to be removed and the active ones. to = from; - auto found_it = std::find_if( to.begin(), to.end(), std::bind(controller_name_compare, std::placeholders::_1, controller_name)); @@ -687,26 +713,14 @@ controller_interface::return_type ControllerManager::cleanup_controller( to.clear(); RCLCPP_ERROR( get_logger(), - "Could not clear controller with name '%s' because no controller with this name exists", + "Could not unload controller with name '%s' because no controller with this name exists", controller_name.c_str()); return controller_interface::return_type::ERROR; } auto & controller = *found_it; - if (is_controller_active(*controller.c)) - { - to.clear(); - RCLCPP_ERROR( - get_logger(), "Could not clear controller with name '%s' because it is still active", - controller_name.c_str()); - return controller_interface::return_type::ERROR; - } - - RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for - // cleaning-up controllers? - controller.c->get_node()->cleanup(); + RCLCPP_DEBUG(get_logger(), "Unload controller"); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -718,7 +732,7 @@ controller_interface::return_type ControllerManager::cleanup_controller( new_unused_list.clear(); RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); - RCLCPP_DEBUG(get_logger(), "Successfully cleaned controller '%s'", controller_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); return controller_interface::return_type::OK; } @@ -782,7 +796,7 @@ controller_interface::return_type ControllerManager::configure_controller( if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( - get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", + get_logger(), "After configuring, controller '%s' is in state '%s', expected 'inactive'.", controller_name.c_str(), new_state.label().c_str()); return controller_interface::return_type::ERROR; } @@ -1910,8 +1924,7 @@ void ControllerManager::cleanup_controller_service_cb( std::shared_ptr response) { // lock services - RCLCPP_DEBUG( - get_logger(), "cleanup service called for controller '%s' ", request->name.c_str()); + RCLCPP_DEBUG(get_logger(), "cleanup service called for controller '%s' ", request->name.c_str()); std::lock_guard guard(services_lock_); RCLCPP_DEBUG(get_logger(), "cleanup service locked");