From e9f7989e0fec886e9686f345e56a1560d40b6d38 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 24 Mar 2024 15:39:46 +0100 Subject: [PATCH 01/25] Squash all commits of fallback controllers for upstream version add fallback controllers list to the ControllerInterfaceBase do not activate the controller if it's fallback controllers are not found or not in inactive state add formatting changes added the first logic of switching to the fallback controller upon error Added a method to get the active controllers that use the command interfaces of the given controller Compute the complete list of active controllers that already use the command interfaces needed by the fallback controllers added fixes for the compilation retouch for the new structure --- controller_manager/src/controller_manager.cpp | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b8a587f92e..2c6c3b0aa5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -170,6 +170,45 @@ void controller_chain_spec_cleanup( } ctrl_chain_spec.erase(controller); } + +// Gets the list of active controllers that use the command interface of the given controller +std::vector get_active_controllers_using_command_interfaces_of_controller( + const std::string & controller_name, + const std::vector & controllers) +{ + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (it == controllers.end()) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the list of controllers.", controller_name.c_str()); + return {}; + } + std::vector controllers_using_command_interfaces; + const auto cmd_itfs = it->c->command_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + for (const auto & controller : controllers) + { + const auto ctrl_cmd_itfs = controller.c->command_interface_configuration().names; + // check if the controller is active and has the command interface and make sure that it + // doesn't exist in the list already + if ( + is_controller_active(controller.c) && + std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end() && + std::find( + controllers_using_command_interfaces.begin(), controllers_using_command_interfaces.end(), + controller.info.name) == controllers_using_command_interfaces.end()) + { + controllers_using_command_interfaces.push_back(controller.info.name); + } + } + } + return controllers_using_command_interfaces; +} + } // namespace namespace controller_manager @@ -1082,6 +1121,38 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } + const auto & fallback_list = controller_it->info.fallback_controllers_names; + if (!fallback_list.empty()) + { + for (const auto & fb_ctrl : fallback_list) + { + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) + { + RCLCPP_WARN( + get_logger(), + "Unable to find the fallback controller : %s of the controller : %s " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + else + { + if (!is_controller_inactive(fb_ctrl_it->c)) + { + RCLCPP_WARN( + get_logger(), + "Controller with name '%s' cannot be activated, as it's fallback controller : %s" + " need to be configured and in inactive state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + status = controller_interface::return_type::ERROR; + } + } + } + } + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( @@ -2353,6 +2424,35 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { failed_controllers_list.push_back(loaded_controller.info.name); + if (!loaded_controller.info.fallback_controllers_names.empty()) + { + RCLCPP_ERROR( + get_logger(), "Error updating controller '%s', switching to fallback controllers", + loaded_controller.info.name.c_str()); + + std::vector active_controllers_using_interfaces; + for (const auto & fallback_controller : + loaded_controller.info.fallback_controllers_names) + { + auto controllers_list = get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list); + for (const auto & controller : controllers_list) + { + if ( + std::find( + active_controllers_using_interfaces.begin(), + active_controllers_using_interfaces.end(), + loaded_controller.info.name) == active_controllers_using_interfaces.end()) + { + active_controllers_using_interfaces.push_back(loaded_controller.info.name); + } + } + } + + deactivate_controllers(rt_controller_list, {loaded_controller.info.name}); + activate_controllers( + rt_controller_list, loaded_controller.info.fallback_controllers_names); + } ret = controller_ret; } } From 11b3b1185913329f20d76735d2d990eda4704d9b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Oct 2024 17:27:35 +0200 Subject: [PATCH 02/25] remove the unnecessary if condition --- controller_manager/src/controller_manager.cpp | 42 +++++++++---------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2c6c3b0aa5..335057e4aa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1121,35 +1121,31 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } - const auto & fallback_list = controller_it->info.fallback_controllers_names; - if (!fallback_list.empty()) + for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) { - for (const auto & fb_ctrl : fallback_list) + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) { - auto fb_ctrl_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); - if (fb_ctrl_it == controllers.end()) + RCLCPP_ERROR( + get_logger(), + "Unable to find the fallback controller : '%s' of the controller : '%s' " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + else + { + if (!is_controller_inactive(fb_ctrl_it->c)) { - RCLCPP_WARN( + RCLCPP_ERROR( get_logger(), - "Unable to find the fallback controller : %s of the controller : %s " - "within the controller list", - fb_ctrl.c_str(), controller_it->info.name.c_str()); + "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" + " need to be configured and be in inactive state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); status = controller_interface::return_type::ERROR; } - else - { - if (!is_controller_inactive(fb_ctrl_it->c)) - { - RCLCPP_WARN( - get_logger(), - "Controller with name '%s' cannot be activated, as it's fallback controller : %s" - " need to be configured and in inactive state!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); - status = controller_interface::return_type::ERROR; - } - } } } From df9f696c487b1015f470c62ee1ad1553fd947223 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Oct 2024 18:20:25 +0200 Subject: [PATCH 03/25] check that all command and state interfaces are available for fallback controllers --- controller_manager/src/controller_manager.cpp | 20 +++++++++++ .../hardware_interface/resource_manager.hpp | 14 ++++++++ hardware_interface/src/resource_manager.cpp | 36 +++++++++++++++++++ 3 files changed, 70 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 335057e4aa..2443405ff6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1146,6 +1146,26 @@ controller_interface::return_type ControllerManager::switch_controller( controller_it->info.name.c_str(), fb_ctrl.c_str()); status = controller_interface::return_type::ERROR; } + if (!resource_manager_->command_interfaces_are_available( + fb_ctrl_it->c->command_interface_configuration().names)) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + status = controller_interface::return_type::ERROR; + } + if (!resource_manager_->state_interfaces_are_available( + fb_ctrl_it->c->state_interface_configuration().names)) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + status = controller_interface::return_type::ERROR; + } } } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5592fc492..644ecf835a 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -130,6 +130,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + /// Checks whether the given list of state interface are available or not. + /** + * \param[in] interface_names a vector of strings identifying the interface to check. + * \return true if all interfaces are available, false otherwise. + */ + bool state_interfaces_are_available(const std::vector & interface_names) const; + /// Add controllers' exported state interfaces to resource manager. /** * Interface for transferring management of exported state interfaces to resource manager. @@ -300,6 +307,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool command_interface_is_available(const std::string & interface) const; + /// Checks whether the given list of command interface are available or not. + /** + * \param[in] interface_names a vector of strings identifying the interface to check. + * \return true if all interfaces are available, false otherwise. + */ + bool command_interfaces_are_available(const std::vector & interface_names) const; + /// Return the number size_t of loaded actuator components. /** * \return number of actuator components. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e5445491c8..513477ffaf 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1240,6 +1240,24 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +bool ResourceManager::state_interfaces_are_available( + const std::vector & interface_names) const +{ + std::lock_guard guard(resource_interfaces_lock_); + for (const auto & interface : interface_names) + { + if ( + std::find( + resource_storage_->available_state_interfaces_.begin(), + resource_storage_->available_state_interfaces_.end(), + interface) == resource_storage_->available_state_interfaces_.end()) + { + return false; + } + } + return true; +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) @@ -1481,6 +1499,24 @@ bool ResourceManager::command_interface_is_available(const std::string & name) c name) != resource_storage_->available_command_interfaces_.end(); } +bool ResourceManager::command_interfaces_are_available( + const std::vector & interface_names) const +{ + std::lock_guard guard(resource_interfaces_lock_); + for (const auto & interface : interface_names) + { + if ( + std::find( + resource_storage_->available_command_interfaces_.begin(), + resource_storage_->available_command_interfaces_.end(), + interface) == resource_storage_->available_command_interfaces_.end()) + { + return false; + } + } + return true; +} + void ResourceManager::import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info) { From 825e5915c068c2e4083fe45e98b17aa6effbd14f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 01:55:35 +0200 Subject: [PATCH 04/25] change the logic of the get_active_controllers_using_command_interfaces_of_controller method --- controller_manager/src/controller_manager.cpp | 30 +++++++------------ 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2443405ff6..8c0799143f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -172,9 +172,10 @@ void controller_chain_spec_cleanup( } // Gets the list of active controllers that use the command interface of the given controller -std::vector get_active_controllers_using_command_interfaces_of_controller( +void get_active_controllers_using_command_interfaces_of_controller( const std::string & controller_name, - const std::vector & controllers) + const std::vector & controllers, + std::vector controllers_using_command_interfaces) { auto it = std::find_if( controllers.begin(), controllers.end(), @@ -184,9 +185,8 @@ std::vector get_active_controllers_using_command_interfaces_of_cont RCLCPP_ERROR( rclcpp::get_logger("ControllerManager::utils"), "Controller '%s' not found in the list of controllers.", controller_name.c_str()); - return {}; + return; } - std::vector controllers_using_command_interfaces; const auto cmd_itfs = it->c->command_interface_configuration().names; for (const auto & cmd_itf : cmd_itfs) { @@ -202,11 +202,10 @@ std::vector get_active_controllers_using_command_interfaces_of_cont controllers_using_command_interfaces.begin(), controllers_using_command_interfaces.end(), controller.info.name) == controllers_using_command_interfaces.end()) { - controllers_using_command_interfaces.push_back(controller.info.name); + add_element_to_list(controllers_using_command_interfaces, controller.info.name); } } } - return controllers_using_command_interfaces; } } // namespace @@ -2446,23 +2445,14 @@ controller_interface::return_type ControllerManager::update( get_logger(), "Error updating controller '%s', switching to fallback controllers", loaded_controller.info.name.c_str()); - std::vector active_controllers_using_interfaces; + std::vector active_controllers_using_interfaces{ + loaded_controller.info.name}; + active_controllers_using_interfaces.reserve(500); for (const auto & fallback_controller : loaded_controller.info.fallback_controllers_names) { - auto controllers_list = get_active_controllers_using_command_interfaces_of_controller( - fallback_controller, rt_controller_list); - for (const auto & controller : controllers_list) - { - if ( - std::find( - active_controllers_using_interfaces.begin(), - active_controllers_using_interfaces.end(), - loaded_controller.info.name) == active_controllers_using_interfaces.end()) - { - active_controllers_using_interfaces.push_back(loaded_controller.info.name); - } - } + get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list, active_controllers_using_interfaces); } deactivate_controllers(rt_controller_list, {loaded_controller.info.name}); From edcd56c9b1fc093e5fd3ee39d01c91cc513ba7c9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 01:56:11 +0200 Subject: [PATCH 05/25] deactivate all related controller together --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8c0799143f..ccb39434f2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2455,7 +2455,7 @@ controller_interface::return_type ControllerManager::update( fallback_controller, rt_controller_list, active_controllers_using_interfaces); } - deactivate_controllers(rt_controller_list, {loaded_controller.info.name}); + deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); activate_controllers( rt_controller_list, loaded_controller.info.fallback_controllers_names); } From e84fbdcf3aa20fac5abf91f1bceef99b3401a5e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 08:42:22 +0200 Subject: [PATCH 06/25] cleanup some redundant conditioning --- controller_manager/src/controller_manager.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ccb39434f2..a8e32968d7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -197,10 +197,7 @@ void get_active_controllers_using_command_interfaces_of_controller( // doesn't exist in the list already if ( is_controller_active(controller.c) && - std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end() && - std::find( - controllers_using_command_interfaces.begin(), controllers_using_command_interfaces.end(), - controller.info.name) == controllers_using_command_interfaces.end()) + std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end()) { add_element_to_list(controllers_using_command_interfaces, controller.info.name); } From 73f03de611b85db8d3b228b25f7aae71685ff2b6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 09:15:11 +0200 Subject: [PATCH 07/25] add more clear checks on the command interfaces of fallback controllers --- controller_manager/src/controller_manager.cpp | 67 ++++++++++++++++--- 1 file changed, 59 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a8e32968d7..e4140b9e14 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1142,15 +1142,66 @@ controller_interface::return_type ControllerManager::switch_controller( controller_it->info.name.c_str(), fb_ctrl.c_str()); status = controller_interface::return_type::ERROR; } - if (!resource_manager_->command_interfaces_are_available( - fb_ctrl_it->c->command_interface_configuration().names)) + for (const auto fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as not all of its fallback " - "controller's : '%s' command interfaces are currently available!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); - status = controller_interface::return_type::ERROR; + if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_ref_itfs = + resource_manager_->get_controller_reference_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == + exported_ref_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + status = controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + status = controller_interface::return_type::ERROR; + } + } } if (!resource_manager_->state_interfaces_are_available( fb_ctrl_it->c->state_interface_configuration().names)) From 6ecbec5182597dd31b7a688dfc77ac8d61d6097c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 09:33:22 +0200 Subject: [PATCH 08/25] added a separate method to check fallback controllers state --- .../controller_manager/controller_manager.hpp | 12 ++ controller_manager/src/controller_manager.cpp | 204 +++++++++--------- 2 files changed, 118 insertions(+), 98 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b8c05efcc0..d58561efb9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -418,6 +418,18 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// The methood to check if the fallback controllers of the given controllers are in the right + /// state, so they can be activated immediately + /** + * \param[in] controllers is a list of controllers to activate. + * \param[in] controller_it is the iterator pointing to the controller to be activated. + * \return return_type::OK if all fallback controllers are in the right state, otherwise + * return_type::ERROR. + */ + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it); + /** * @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 e4140b9e14..e55a90f46a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1117,104 +1117,7 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } - for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) - { - auto fb_ctrl_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); - if (fb_ctrl_it == controllers.end()) - { - RCLCPP_ERROR( - get_logger(), - "Unable to find the fallback controller : '%s' of the controller : '%s' " - "within the controller list", - fb_ctrl.c_str(), controller_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; - } - else - { - if (!is_controller_inactive(fb_ctrl_it->c)) - { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" - " need to be configured and be in inactive state!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); - status = controller_interface::return_type::ERROR; - } - for (const auto fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) - { - if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) - { - ControllersListIterator following_ctrl_it; - if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) - { - // if following_ctrl_it is inactive and it is in the fallback list of the - // controller_it and then check it it's exported reference interface names list if - // it's available - if (is_controller_inactive(following_ctrl_it->c)) - { - if ( - std::find( - controller_it->info.fallback_controllers_names.begin(), - controller_it->info.fallback_controllers_names.end(), - following_ctrl_it->info.name) != - controller_it->info.fallback_controllers_names.end()) - { - const auto exported_ref_itfs = - resource_manager_->get_controller_reference_interface_names( - following_ctrl_it->info.name); - if ( - std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == - exported_ref_itfs.end()) - { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as the command interface : " - "'%s' required by its fallback controller : '%s' is not exported by the " - "controller : '%s' in the current fallback list!", - controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), - following_ctrl_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; - } - } - else - { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as the command interface : " - "'%s' required by its fallback controller : '%s' is not available as the " - "controller is not in active state!. May be consider adding this controller to " - "the fallback list of the controller : '%s' or already have it activated.", - controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), - following_ctrl_it->info.name.c_str()); - status = controller_interface::return_type::ERROR; - } - } - } - else - { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as not all of its fallback " - "controller's : '%s' command interfaces are currently available!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); - status = controller_interface::return_type::ERROR; - } - } - } - if (!resource_manager_->state_interfaces_are_available( - fb_ctrl_it->c->state_interface_configuration().names)) - { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as not all of its fallback " - "controller's : '%s' state interfaces are currently available!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); - status = controller_interface::return_type::ERROR; - } - } - } + status = check_fallback_controllers_state_pre_activation(controllers, controller_it); if (status != controller_interface::return_type::OK) { @@ -2919,6 +2822,111 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +controller_interface::return_type +ControllerManager::check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it) +{ + for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) + { + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) + { + RCLCPP_ERROR( + get_logger(), + "Unable to find the fallback controller : '%s' of the controller : '%s' " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + if (!is_controller_inactive(fb_ctrl_it->c)) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" + " need to be configured and be in inactive state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + for (const auto fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) + { + if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_ref_itfs = + resource_manager_->get_controller_reference_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == + exported_ref_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + if (!resource_manager_->state_interfaces_are_available( + fb_ctrl_it->c->state_interface_configuration().names)) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + return controller_interface::return_type::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { From c82303b9dc90c3b915c1df23a23c0e54063f3f91 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 09:37:57 +0200 Subject: [PATCH 09/25] extend the checks for the needed state interfaces --- controller_manager/src/controller_manager.cpp | 67 ++++++++++++++++--- 1 file changed, 59 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e55a90f46a..1e07d2242f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2912,15 +2912,66 @@ ControllerManager::check_fallback_controllers_state_pre_activation( } } } - if (!resource_manager_->state_interfaces_are_available( - fb_ctrl_it->c->state_interface_configuration().names)) + for (const auto fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) { - RCLCPP_ERROR( - get_logger(), - "Controller with name '%s' cannot be activated, as not all of its fallback " - "controller's : '%s' state interfaces are currently available!", - controller_it->info.name.c_str(), fb_ctrl.c_str()); - return controller_interface::return_type::ERROR; + if (!resource_manager_->state_interface_is_available(fb_state_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_state_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_state_itfs = + resource_manager_->get_controller_exported_state_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == + exported_state_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } } } } From f4732782e85290f207097fe65e2b499126242439 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 09:43:12 +0200 Subject: [PATCH 10/25] remove the newly added unused helper functions --- .../hardware_interface/resource_manager.hpp | 14 -------- hardware_interface/src/resource_manager.cpp | 36 ------------------- 2 files changed, 50 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 644ecf835a..a5592fc492 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -130,13 +130,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; - /// Checks whether the given list of state interface are available or not. - /** - * \param[in] interface_names a vector of strings identifying the interface to check. - * \return true if all interfaces are available, false otherwise. - */ - bool state_interfaces_are_available(const std::vector & interface_names) const; - /// Add controllers' exported state interfaces to resource manager. /** * Interface for transferring management of exported state interfaces to resource manager. @@ -307,13 +300,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool command_interface_is_available(const std::string & interface) const; - /// Checks whether the given list of command interface are available or not. - /** - * \param[in] interface_names a vector of strings identifying the interface to check. - * \return true if all interfaces are available, false otherwise. - */ - bool command_interfaces_are_available(const std::vector & interface_names) const; - /// Return the number size_t of loaded actuator components. /** * \return number of actuator components. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 513477ffaf..e5445491c8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1240,24 +1240,6 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } -bool ResourceManager::state_interfaces_are_available( - const std::vector & interface_names) const -{ - std::lock_guard guard(resource_interfaces_lock_); - for (const auto & interface : interface_names) - { - if ( - std::find( - resource_storage_->available_state_interfaces_.begin(), - resource_storage_->available_state_interfaces_.end(), - interface) == resource_storage_->available_state_interfaces_.end()) - { - return false; - } - } - return true; -} - // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) @@ -1499,24 +1481,6 @@ bool ResourceManager::command_interface_is_available(const std::string & name) c name) != resource_storage_->available_command_interfaces_.end(); } -bool ResourceManager::command_interfaces_are_available( - const std::vector & interface_names) const -{ - std::lock_guard guard(resource_interfaces_lock_); - for (const auto & interface : interface_names) - { - if ( - std::find( - resource_storage_->available_command_interfaces_.begin(), - resource_storage_->available_command_interfaces_.end(), - interface) == resource_storage_->available_command_interfaces_.end()) - { - return false; - } - } - return true; -} - void ResourceManager::import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info) { From 2f9685639301e04a12953e4a36dec5e8fc648aab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 09:43:36 +0200 Subject: [PATCH 11/25] Use references while iterating in loop --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1e07d2242f..f6aee5333e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2851,7 +2851,7 @@ ControllerManager::check_fallback_controllers_state_pre_activation( controller_it->info.name.c_str(), fb_ctrl.c_str()); return controller_interface::return_type::ERROR; } - for (const auto fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) + for (const auto & fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) { if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) { @@ -2912,7 +2912,7 @@ ControllerManager::check_fallback_controllers_state_pre_activation( } } } - for (const auto fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) + for (const auto & fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) { if (!resource_manager_->state_interface_is_available(fb_state_itf)) { From d1daa53bd5fd49153ec73cf322dc661ce2723e07 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 12 Oct 2024 10:42:35 +0200 Subject: [PATCH 12/25] update the logic in update to activate controllers altogether --- controller_manager/src/controller_manager.cpp | 50 +++++++++++-------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f6aee5333e..1ee4bae194 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2390,26 +2390,6 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { failed_controllers_list.push_back(loaded_controller.info.name); - if (!loaded_controller.info.fallback_controllers_names.empty()) - { - RCLCPP_ERROR( - get_logger(), "Error updating controller '%s', switching to fallback controllers", - loaded_controller.info.name.c_str()); - - std::vector active_controllers_using_interfaces{ - loaded_controller.info.name}; - active_controllers_using_interfaces.reserve(500); - for (const auto & fallback_controller : - loaded_controller.info.fallback_controllers_names) - { - get_active_controllers_using_command_interfaces_of_controller( - fallback_controller, rt_controller_list, active_controllers_using_interfaces); - } - - deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); - activate_controllers( - rt_controller_list, loaded_controller.info.fallback_controllers_names); - } ret = controller_ret; } } @@ -2423,10 +2403,36 @@ controller_interface::return_type ControllerManager::update( failed_controllers += "\n\t- " + controller; } RCLCPP_ERROR( - get_logger(), "Deactivating following controllers as their update resulted in an error :%s", + get_logger(), + "Deactivating following controllers as their update resulted in an error! Will try to switch " + "to their fallback controllers if available :%s", failed_controllers.c_str()); - deactivate_controllers(rt_controller_list, failed_controllers_list); + std::vector active_controllers_using_interfaces(failed_controllers_list); + active_controllers_using_interfaces.reserve(500); + std::vector cumulative_fallback_controllers; + cumulative_fallback_controllers.reserve(500); + + for (const auto & failed_ctrl : failed_controllers_list) + { + auto ctrl_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, failed_ctrl)); + if (ctrl_it != rt_controller_list.end()) + { + for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names) + { + cumulative_fallback_controllers.push_back(fallback_controller); + get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list, active_controllers_using_interfaces); + } + } + } + deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); + if (!cumulative_fallback_controllers.empty()) + { + activate_controllers(rt_controller_list, cumulative_fallback_controllers); + } } // there are controllers to (de)activate From fbf9b685e38791d9cd4f1750a7d1ea2e891fc6bb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 13 Oct 2024 22:22:39 +0200 Subject: [PATCH 13/25] fix the logic for existing tests --- controller_manager/src/controller_manager.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1ee4bae194..5e5c3fafb0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1117,7 +1117,10 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } - status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + if (status == controller_interface::return_type::OK) + { + status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + } if (status != controller_interface::return_type::OK) { From bea7f4d613b88f4aede1869113c9a3556d79285d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 13 Oct 2024 22:39:18 +0200 Subject: [PATCH 14/25] add set_external_commands_for_testing method to the TestController class --- controller_manager/test/test_controller/test_controller.cpp | 5 +++++ controller_manager/test/test_controller/test_controller.hpp | 2 ++ 2 files changed, 7 insertions(+) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7b9af6ecfc..ac89239e09 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -137,6 +137,11 @@ std::vector TestController::get_state_interface_data() const return state_intr_data; } +void TestController::set_external_commands_for_testing(const std::vector & commands) +{ + external_commands_for_testing_ = commands; +} + } // namespace test_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e5f827cd0..d57fd9ddd9 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -70,6 +70,8 @@ class TestController : public controller_interface::ControllerInterface const std::string & getRobotDescription() const; + void set_external_commands_for_testing(const std::vector & commands); + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller From 92914066db7ab166b51f7bf3d77c9a4a30f645fb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 13 Oct 2024 23:35:24 +0200 Subject: [PATCH 15/25] Add a add_controller method to controller_manager that accepts ControllerSpec --- .../include/controller_manager/controller_manager.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index d58561efb9..d54a441b49 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -128,6 +128,12 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller_spec); } + controller_interface::ControllerInterfaceBaseSharedPtr add_controller( + const ControllerSpec & controller_spec) + { + return add_controller_impl(controller_spec); + } + /// configure_controller Configure controller by name calling their "configure" method. /** * \param[in] controller_name as a string. From e2e622c300348a97ad18bc48422b6cd984702ca0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 13 Oct 2024 23:35:48 +0200 Subject: [PATCH 16/25] Add first basic fallback controller test --- .../test/test_controller_manager.cpp | 101 ++++++++++++++++++ 1 file changed, 101 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 28e9f0477c..432082135a 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -531,3 +531,104 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + +class TestControllerManagerFallbackControllers +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); +} From e3023588bd61b4ab5a93fe914476c2b2e8ee2451 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 10:22:09 +0200 Subject: [PATCH 17/25] Add a test case for the controller cannot be a fallback controller for itself --- controller_manager/src/controller_manager.cpp | 9 ++++++++ .../test/test_controller_manager.cpp | 21 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5e5c3fafb0..7356c9f34a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -571,6 +571,15 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) { + if ( + std::find(fallback_controllers.begin(), fallback_controllers.end(), controller_name) != + fallback_controllers.end()) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' cannot be a fallback controller for itself.", + controller_name.c_str()); + return nullptr; + } controller_spec.info.fallback_controllers_names = fallback_controllers; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 432082135a..a8cb9b3ddb 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -538,6 +538,27 @@ class TestControllerManagerFallbackControllers { }; +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_same_controller_in_fallback_list) +{ + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + + const std::vector fallback_controllers = {test_controller_1_name, "random_ctrl2"}; + rclcpp::Parameter fallback_ctrls_parameter( + test_controller_1_name + std::string(".fallback_controllers"), fallback_controllers); + cm_->set_parameter(fallback_ctrls_parameter); + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + nullptr, + cm_->load_controller(test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME)); + } +} + TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) { const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; From 8178ecdb077785c24eaa02cc3fad8532be2598bb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 10:41:33 +0200 Subject: [PATCH 18/25] Added a fail test case when the fallback controllers need non existing interface to activate --- .../test/test_controller_manager.cpp | 86 +++++++++++++++++++ 1 file changed, 86 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a8cb9b3ddb..c94fcde89a 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -653,3 +653,89 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_2->get_lifecycle_state().id()); } + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_command_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"random_non_existing_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} From 8a23238ff3676d6368d97e10df74852071760bb1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 11:59:35 +0200 Subject: [PATCH 19/25] Added a fail test case when the fallback controllers need non existing state interface to activate --- .../test/test_controller_manager.cpp | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index c94fcde89a..9eb381d946 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -739,3 +739,92 @@ TEST_F( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_2->get_lifecycle_state().id()); } + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_state_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.names = {"non_existing_state_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} From 01d21b125eb56d59f0807d4ea4ad5638dfeddfd8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 12:54:27 +0200 Subject: [PATCH 20/25] Added test to consider when one or more fallback controllers are already active --- controller_manager/src/controller_manager.cpp | 6 +- .../test/test_controller_manager.cpp | 137 ++++++++++++++++++ 2 files changed, 140 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7356c9f34a..fa0840abfc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -175,7 +175,7 @@ void controller_chain_spec_cleanup( void get_active_controllers_using_command_interfaces_of_controller( const std::string & controller_name, const std::vector & controllers, - std::vector controllers_using_command_interfaces) + std::vector &controllers_using_command_interfaces) { auto it = std::find_if( controllers.begin(), controllers.end(), @@ -2860,12 +2860,12 @@ ControllerManager::check_fallback_controllers_state_pre_activation( } else { - if (!is_controller_inactive(fb_ctrl_it->c)) + if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) { RCLCPP_ERROR( get_logger(), "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" - " need to be configured and be in inactive state!", + " need to be configured and be in inactive/active state!", controller_it->info.name.c_str(), fb_ctrl.c_str()); return controller_interface::return_type::ERROR; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 9eb381d946..6d04574003 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -828,3 +828,140 @@ TEST_F( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_2->get_lifecycle_state().id()); } + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_valid_activation_if_one_or_more_fallback_controllers_are_already_active) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_3->set_command_interface_configuration(itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + } + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + /// @note Here the order is important do not change the starting order + for (const auto & start_controller : {test_controller_3_name, test_controller_1_name}) + { + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {start_controller}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); +} From c071a98e56915e3829dfc1333991fc70228f6d88 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 13:10:35 +0200 Subject: [PATCH 21/25] Improve logging for better introspection --- controller_manager/src/controller_manager.cpp | 49 ++++++++++++++----- 1 file changed, 37 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fa0840abfc..49844d1041 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -175,7 +175,7 @@ void controller_chain_spec_cleanup( void get_active_controllers_using_command_interfaces_of_controller( const std::string & controller_name, const std::vector & controllers, - std::vector &controllers_using_command_interfaces) + std::vector & controllers_using_command_interfaces) { auto it = std::find_if( controllers.begin(), controllers.end(), @@ -2409,17 +2409,6 @@ controller_interface::return_type ControllerManager::update( } if (!failed_controllers_list.empty()) { - std::string failed_controllers; - for (const auto & controller : failed_controllers_list) - { - failed_controllers += "\n\t- " + controller; - } - RCLCPP_ERROR( - get_logger(), - "Deactivating following controllers as their update resulted in an error! Will try to switch " - "to their fallback controllers if available :%s", - failed_controllers.c_str()); - std::vector active_controllers_using_interfaces(failed_controllers_list); active_controllers_using_interfaces.reserve(500); std::vector cumulative_fallback_controllers; @@ -2440,6 +2429,42 @@ controller_interface::return_type ControllerManager::update( } } } + std::string controllers_string; + controllers_string.reserve(500); + for (const auto & controller : failed_controllers_list) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!", + controllers_string.c_str()); + if (active_controllers_using_interfaces.size() > failed_controllers_list.size()) + { + controllers_string.clear(); + for (size_t i = failed_controllers_list.size(); + i < active_controllers_using_interfaces.size(); i++) + { + controllers_string.append(active_controllers_using_interfaces[i]); + controllers_string.append(" "); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !controllers_string.empty(), + "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " + "controllers to activate.", + controllers_string.c_str()); + } + if (!cumulative_fallback_controllers.empty()) + { + controllers_string.clear(); + for (const auto & controller : cumulative_fallback_controllers) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); + } deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); if (!cumulative_fallback_controllers.empty()) { From 0d4698e085fe3805da0753ba2de8a224a514a846 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 15:03:19 +0200 Subject: [PATCH 22/25] Add test on the fallback controllers need to be in configured state already --- .../test/test_controller_manager.cpp | 82 +++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6d04574003..5b63cf9d93 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -559,6 +559,88 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_same_controller } } +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_controller_not_configured) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); +} + TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) { const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; From cf309a30773b566fa4b0be02196a89691fc67f01 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 23:23:20 +0200 Subject: [PATCH 23/25] Added test cases when using with chainable controllers --- controller_manager/CMakeLists.txt | 1 + .../test/test_controller_manager.cpp | 403 ++++++++++++++++++ 2 files changed, 404 insertions(+) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5dea15c0d1..e44fb5fb9b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -91,6 +91,7 @@ if(BUILD_TESTING) target_link_libraries(test_controller_manager controller_manager test_controller + test_chainable_controller ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5b63cf9d93..7d4b1ebb9e 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -20,6 +20,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -1047,3 +1048,405 @@ TEST_F( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_3->get_lifecycle_state().id()); } + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_multiple_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"joint1/position"}); + test_controller_4->set_exported_state_interface_names({"joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + + controller_spec.c = test_controller_4; + controller_spec.info.name = test_controller_4_name; + controller_spec.info.type = "test_chainable_controller::TestChainableController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_3 alone and it should fail + // as it doesn't have the chained state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_2, test_controller_3 and + // test_controller_4, and now it should work as all the controllers are in the list + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + // It is expected to place all the chainable interfaces first, so they can make the interfaces + // available + controller_spec.info.fallback_controllers_names = { + test_controller_4_name, test_controller_3_name, test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing( + {std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_4->get_lifecycle_state().id()); + } +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_other_failing_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"modified_joint1/position"}); + test_controller_4->set_exported_state_interface_names({"modified_joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + + controller_spec.c = test_controller_4; + controller_spec.info.name = test_controller_4_name; + controller_spec.info.type = "test_chainable_controller::TestChainableController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + { + // Now the controller_1 is set with test_controller_2 and test_controller_4_name and it should + // fail as it has an non existing state interface Start controller, will take effect at the end + // of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + } + + // Now unload the controller_1 and set it up again with test_controller_3 and + // test_controller_4_name and it should fail as it has an non existing state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + cm_->unload_controller(test_controller_4_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_3_name, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } +} From dae549c6785dff9c94b50f1804b60b20197f124b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 15 Oct 2024 09:54:15 +0200 Subject: [PATCH 24/25] added documentation and update release_note.rst --- controller_manager/doc/userdoc.rst | 12 ++++++++++++ doc/release_notes.rst | 1 + 2 files changed, 13 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index bc9f75384e..d3dd5e90c8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -87,6 +87,18 @@ update_rate (mandatory; integer) Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. +.params_file + The absolute path to the YAML file with parameters for the controller. + The file should contain the parameters for the controller in the standard ROS 2 YAML format. + +.fallback_controllers + List of controllers that are activated as a fallback strategy, when the spawned controllers fail by returning ``return_type::ERROR`` during the ``update`` cycle. + It is recommended to add all the controllers needed for the fallback strategy to the list, including the chainable controllers whose interfaces are used by the main fallback controllers. + +.. warning:: + The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. + It is recommended to test the fallback strategy in simulation before deploying it on the real robot. + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7d28b4390d..7fa7ae7f29 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -26,6 +26,7 @@ For details see the controller_manager section. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) +* The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) controller_manager ****************** From 17ec65f0ef6f300342331ab3cdacb3bf536759ea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 14:45:26 +0200 Subject: [PATCH 25/25] Apply suggestions from code review Co-authored-by: Bence Magyar --- .../include/controller_manager/controller_manager.hpp | 2 +- controller_manager/src/controller_manager.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index d54a441b49..273b48b022 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -424,7 +424,7 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); - /// The methood to check if the fallback controllers of the given controllers are in the right + /// Checks if the fallback controllers of the given controllers are in the right /// state, so they can be activated immediately /** * \param[in] controllers is a list of controllers to activate. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 49844d1041..72cebaa1e5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2409,10 +2409,11 @@ controller_interface::return_type ControllerManager::update( } if (!failed_controllers_list.empty()) { + const auto FALLBACK_STACK_MAX_SIZE = 500; std::vector active_controllers_using_interfaces(failed_controllers_list); - active_controllers_using_interfaces.reserve(500); + active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE); std::vector cumulative_fallback_controllers; - cumulative_fallback_controllers.reserve(500); + cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE); for (const auto & failed_ctrl : failed_controllers_list) {