From 0115236067628a8d5e02d33b7e07fdce50db5d65 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Wed, 26 Jun 2024 15:53:06 +0900 Subject: [PATCH] refactor (de)activate request check --- .../controller_manager/controller_manager.hpp | 42 +++ controller_manager/src/controller_manager.cpp | 262 +++++++++--------- 2 files changed, 178 insertions(+), 126 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 845fa2dee0..6cb8a567ae 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -329,6 +329,13 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager_; private: + enum class CheckDeActivateRequestResult + { + OK, + ERROR, + RETRY + }; + std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); @@ -339,6 +346,7 @@ class ControllerManager : public rclcpp::Node * and "control loop" threads. */ void clear_requests(); + void clear_chained_mode_requests(); /** * If a controller is deactivated all following controllers (if any exist) should be switched @@ -377,6 +385,23 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /** + * Check if all activate requests are valid. + * Perform a detailed check internally by calling check_following_controllers_for_activate. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following + * controllers will be automatically added to the activate request list if they are not in the + * deactivate request. + * + * \returns If all activate requests pass the check, return CheckDeActivateRequestResult::OK. If + * an error occurs during the check, the processing will differ based on the value of strictness: + * if BEST_EFFORT, erase the target controller from the activate_request and return + * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR. + */ + CheckDeActivateRequestResult check_activate_requests( + const std::vector & controllers, int strictness); + /// Check if all the preceding controllers will be in inactive state after controllers' switch. /** * Check that all preceding controllers of the @controller_it @@ -400,6 +425,23 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /** + * Check if all deactivate requests are valid. + * Perform a detailed check internally by calling check_preceeding_controllers_for_deactivate. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following + * controllers will be automatically added to the activate request list if they are not in the + * deactivate request. + * + * \returns If all deactivate requests pass the check, return CheckDeActivateRequestResult::OK. If + * an error occurs during the check, the processing will differ based on the value of strictness: + * if BEST_EFFORT, erase the target controller from the deactivate_request and return + * CheckDeActivateRequestResult::RETRY; if STRICT, return CheckDeActivateRequestResult::ERROR. + */ + CheckDeActivateRequestResult check_deactivate_requests( + const std::vector & controllers, int strictness); + /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c4efe9ed95..f18c353680 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -831,16 +831,21 @@ void ControllerManager::clear_requests() { deactivate_request_.clear(); activate_request_.clear(); - // Set these interfaces as unavailable when clearing requests to avoid leaving them in available - // state without the controller being in active state + clear_chained_mode_requests(); + activate_command_interface_request_.clear(); + deactivate_command_interface_request_.clear(); +} + +void ControllerManager::clear_chained_mode_requests() +{ + // Set these interfaces as unavailable when clearing requests to avoid leaving them in + // available state without the controller being in active state for (const auto & controller_name : to_chained_mode_request_) { resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); } to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); - activate_command_interface_request_.clear(); - deactivate_command_interface_request_.clear(); } controller_interface::return_type ControllerManager::switch_controller( @@ -963,136 +968,25 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); - enum class CreateRequestResult - { - OK, - ERROR, - RETRY - }; - const auto check_de_activate_request_and_create_chained_mode_request = - [this, &strictness, &controllers]() -> CreateRequestResult + [this, &strictness, &controllers]() -> CheckDeActivateRequestResult { - const auto clear_chained_mode_requests = [this]() - { - // Set these interfaces as unavailable when clearing requests to avoid leaving them in - // available state without the controller being in active state - for (const auto & controller_name : to_chained_mode_request_) - { - resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); - } - to_chained_mode_request_.clear(); - from_chained_mode_request_.clear(); - }; - // if a preceding controller is deactivated, all first-level controllers should be switched // 'from' chained mode propagate_deactivation_of_chained_mode(controllers); - // check if controllers should be switched 'to' chained mode when controllers are activated - for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + if (const auto result = check_activate_requests(controllers, strictness); + result != CheckDeActivateRequestResult::OK) { - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type status = controller_interface::return_type::OK; - - // if controller is not inactive then do not do any following-controllers checks - if (!is_controller_inactive(controller_it->c)) - { - RCLCPP_WARN( - get_logger(), - "Controller with name '%s' is not inactive so its following " - "controllers do not have to be checked, because it cannot be activated.", - controller_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; - } - else - { - status = check_following_controllers_for_activate(controllers, strictness, controller_it); - } - - if (status != controller_interface::return_type::OK) - { - RCLCPP_WARN( - get_logger(), - "Could not activate controller with name '%s'. Check above warnings for more details. " - "Check the state of the controllers and their required interfaces using " - "`ros2 control list_controllers -v` CLI to get more information.", - (*ctrl_it).c_str()); - if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) - { - // TODO(destogl): automatic manipulation of the chain: - // || strictness == - // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); - // remove controller that can not be activated from the activation request and step-back - // iterator to correctly step to the next element in the list in the loop - activate_request_.erase(ctrl_it); - // reset chained mode request lists and will retry the creation of the request - clear_chained_mode_requests(); - return CreateRequestResult::RETRY; - } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); - // reset all lists - clear_requests(); - return CreateRequestResult::ERROR; - } - } + return result; } - - // check if controllers should be deactivated if used in chained mode - for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); - ++ctrl_it) + if (const auto result = check_deactivate_requests(controllers, strictness); + result != CheckDeActivateRequestResult::OK) { - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type status = controller_interface::return_type::OK; - - // if controller is not active then skip preceding-controllers checks - if (!is_controller_active(controller_it->c)) - { - RCLCPP_WARN( - get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", - controller_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; - } - else - { - status = - check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); - } - - if (status != controller_interface::return_type::OK) - { - RCLCPP_WARN( - get_logger(), - "Could not deactivate controller with name '%s'. Check above warnings for more details. " - "Check the state of the controllers and their required interfaces using " - "`ros2 control list_controllers -v` CLI to get more information.", - (*ctrl_it).c_str()); - if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) - { - // remove controller that can not be activated from the activation request and step-back - // iterator to correctly step to the next element in the list in the loop - deactivate_request_.erase(ctrl_it); - // reset chained mode request lists and will retry the creation of the request - clear_chained_mode_requests(); - return CreateRequestResult::RETRY; - } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); - // reset all lists - clear_requests(); - return CreateRequestResult::ERROR; - } - } + return result; } - return CreateRequestResult::OK; + return CheckDeActivateRequestResult::OK; }; // Validate the (de)activate request and create from/to chained mode requests as needed. @@ -1104,15 +998,15 @@ controller_interface::return_type ControllerManager::switch_controller( while (true) { const auto result = check_de_activate_request_and_create_chained_mode_request(); - if (result == CreateRequestResult::RETRY) + if (result == CheckDeActivateRequestResult::RETRY) { continue; } - if (result == CreateRequestResult::ERROR) + if (result == CheckDeActivateRequestResult::ERROR) { return controller_interface::return_type::ERROR; } - // if result == CreateRequestResult::OK -> break the loop + // if result == CheckDeActivateRequestResult::OK -> break the loop break; } @@ -2550,6 +2444,68 @@ controller_interface::return_type ControllerManager::check_following_controllers return controller_interface::return_type::OK; }; +ControllerManager::CheckDeActivateRequestResult ControllerManager::check_activate_requests( + const std::vector & controllers, int strictness) +{ + // check if controllers should be switched 'to' chained mode when controllers are activated + for (auto ctrl_it = activate_request_.begin(); ctrl_it != activate_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type status = controller_interface::return_type::OK; + + // if controller is not inactive then do not do any following-controllers checks + if ( + !is_controller_inactive(controller_it->c) && + std::find(deactivate_request_.begin(), deactivate_request_.end(), *ctrl_it) == + deactivate_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Controller with name '%s' is not inactive so its following " + "controllers do not have to be checked, because it cannot be activated.", + controller_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + else + { + status = check_following_controllers_for_activate(controllers, strictness, controller_it); + } + + if (status != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not activate controller with name '%s'. Check above warnings for more details. " + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` CLI to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // TODO(destogl): automatic manipulation of the chain: + // || strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + activate_request_.erase(ctrl_it); + // reset chained mode request lists and will retry the creation of the request + clear_chained_mode_requests(); + return CheckDeActivateRequestResult::RETRY; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return CheckDeActivateRequestResult::ERROR; + } + } + } + + return CheckDeActivateRequestResult::OK; +} + controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate( const std::vector & controllers, int /*strictness*/, const ControllersListIterator controller_it) @@ -2636,6 +2592,60 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +ControllerManager::CheckDeActivateRequestResult ControllerManager::check_deactivate_requests( + const std::vector & controllers, int strictness) +{ + // check if controllers should be deactivated if used in chained mode + for (auto ctrl_it = deactivate_request_.begin(); ctrl_it != deactivate_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type status = controller_interface::return_type::OK; + + // if controller is not active then skip preceding-controllers checks + if (!is_controller_active(controller_it->c)) + { + RCLCPP_WARN( + get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", + controller_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + else + { + status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + } + + if (status != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s'. Check above warnings for more details. " + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` CLI to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + deactivate_request_.erase(ctrl_it); + // reset chained mode request lists and will retry the creation of the request + clear_chained_mode_requests(); + return CheckDeActivateRequestResult::RETRY; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return CheckDeActivateRequestResult::ERROR; + } + } + } + + return CheckDeActivateRequestResult::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) {