diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1c1f028705..d12159dc86 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -85,7 +85,7 @@ repos: entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - args: ["--linelength=100", "--filter=-whitespace/newline"] + args: ["--linelength=100", "--filter=-whitespace/newline,-readability/fn_size"] # Cmake hooks - repo: local diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 407aa0c61e..24b54d516b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -366,6 +366,10 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy protected: pal_statistics::RegistrationsRAII stats_registrations_; + +public: + using SharedPtr = std::shared_ptr; + using WeakPtr = std::weak_ptr; }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 21e8c71f3b..7ad2511418 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -501,6 +501,14 @@ class ControllerManager : public rclcpp::Node const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller); + /** + * @brief Build the controller chain topology information based on the provided controllers. + * This method constructs a directed graph representing the dependencies between controllers. + * It analyzes the relationships between controllers, such as which controllers depend on others, + * and builds a directed graph to represent these dependencies. + */ + void build_controllers_topology_info(const std::vector & controllers); + /** * @brief Method to publish the state of the controller manager. * The state includes the list of controllers and the list of hardware interfaces along with @@ -635,6 +643,7 @@ class ControllerManager : public rclcpp::Node std::unique_ptr preshutdown_cb_handle_{nullptr}; RTControllerListWrapper rt_controllers_wrapper_; std::unordered_map controller_chain_spec_; + ControllerChainDependencyGraph controller_chain_dependency_graph_; std::vector ordered_controllers_names_; /// mutex copied from ROS1 Control, protects service callbacks /// not needed if we're guaranteed that the callbacks don't come from multiple threads diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index cb7a31cfe9..33f6c91b43 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -13,23 +13,518 @@ // limitations under the License. /* - * Author: Wim Meeussen + * Author: Wim Meeussen, Sai Kishor Kothakota */ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +#include +#include +#include #include #include +#include +#include #include + #include "controller_interface/controller_interface_base.hpp" #include "hardware_interface/controller_info.hpp" +#include "hardware_interface/helpers.hpp" #include "hardware_interface/types/statistics_types.hpp" namespace controller_manager { using MovingAverageStatistics = ros2_control::MovingAverageStatistics; + +struct ControllerPeerInfo +{ + std::string name = ""; + std::vector predecessors = {}; + std::vector successors = {}; + controller_interface::ControllerInterfaceBase::WeakPtr controller; + std::unordered_set command_interfaces = {}; + std::unordered_set state_interfaces = {}; + std::unordered_set reference_interfaces = {}; + std::vector> mutually_exclusive_predecessor_groups = {}; + std::vector> mutually_exclusive_successor_groups = {}; + + void build_mutually_exclusive_predecessor_groups() + { + // Build mutually exclusive groups of predecessor controllers, that could utilize all the + // reference interfaces of the current controller. This is used to determine which predecessor + // controllers can be activated together with the current controller. + + mutually_exclusive_predecessor_groups.clear(); + const auto are_all_reference_interfaces_found = + []( + const std::unordered_set & ref_itfs, + const std::unordered_set & cmd_itfs) + { + return std::all_of( + ref_itfs.begin(), ref_itfs.end(), [&cmd_itfs](const std::string & reference_itf) + { return cmd_itfs.find(reference_itf) != cmd_itfs.end(); }); + }; + const auto & current_reference_interfaces = reference_interfaces; + std::for_each( + predecessors.begin(), predecessors.end(), + [this, &are_all_reference_interfaces_found](const ControllerPeerInfo * p) + { + // check if all the command interfaces of the predecessor are in the current controller's + // reference interfaces If they are, add them as individual group + std::unordered_set predecessor_group = {}; + bool all_predecessor_interfaces_match = + are_all_reference_interfaces_found(reference_interfaces, p->command_interfaces); + if (all_predecessor_interfaces_match) + { + // If the predecessor's command interfaces are all in the current controller's reference + // interfaces, add it as individual group + predecessor_group.insert(p->name); + mutually_exclusive_predecessor_groups.push_back(predecessor_group); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "Adding predecessor: '%s' as individual group, as all its command interfaces are in " + "the current controller's reference interfaces.", + p->name.c_str()); + } + }); + + // If the predecessor's command interfaces are not all in the current controller's reference + // interfaces, then check other predecessors and see if they can be grouped together + + // generate combinations of predecessors that can be grouped together + for (const auto & predecessor : predecessors) + { + // check if the predessort is already in the mutually exclusive group as single group + if (std::any_of( + mutually_exclusive_predecessor_groups.begin(), + mutually_exclusive_predecessor_groups.end(), + [&predecessor](const std::unordered_set & group) + { return group.find(predecessor->name) != group.end() && group.size() == 1; })) + { + continue; // skip this predecessor, as it is already in a group as individual + } + + // create all combinations of predecessors that can be grouped together + // For instance, predecessors A,B,C,D. Get combinations like: + // A,B; A,C; A,D; B,C; B,D; C,D; A,B,C; A,B,D; A,C,D; B,C,D; A,B,C,D + // Note: This is a simplified version, in practice you would want to check if the + // command interfaces of the predecessors do not overlap and are unique + + std::vector> combinations; + std::vector current_combination; + std::function generate_combinations = [&](size_t start_index) + { + if (current_combination.size() > 1) + { + // check if the current combination's command interfaces are all in the + // current controller's reference interfaces + std::unordered_set combined_command_interfaces; + for (const auto & predecessor_name : current_combination) + { + const auto & predecessor_itf = std::find_if( + predecessors.begin(), predecessors.end(), + [&predecessor_name](const ControllerPeerInfo * p) + { return p->name == predecessor_name; }); + if (predecessor_itf != predecessors.end()) + { + combined_command_interfaces.insert( + (*predecessor_itf)->command_interfaces.begin(), + (*predecessor_itf)->command_interfaces.end()); + } + } + if (are_all_reference_interfaces_found( + current_reference_interfaces, combined_command_interfaces)) + { + combinations.push_back(current_combination); + } + } + for (size_t i = start_index; i < predecessors.size(); ++i) + { + if (std::any_of( + mutually_exclusive_predecessor_groups.begin(), + mutually_exclusive_predecessor_groups.end(), + [&](const std::unordered_set & group) + { return group.find(predecessors[i]->name) != group.end() && group.size() == 1; })) + { + continue; // skip this predecessor, as it is already in a group as individual + } + + if ( + std::find( + current_combination.begin(), current_combination.end(), predecessors[i]->name) != + current_combination.end()) + { + continue; // skip this predecessor, as it is already in the current combination + } + + current_combination.push_back(predecessors[i]->name); + generate_combinations(i + 1); + current_combination.pop_back(); + } + }; + + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "Generating combinations of predecessors for controller: %s (%s)", + predecessor->name.c_str(), name.c_str()); + generate_combinations(0); + // Add the combinations to the mutually exclusive predecessor groups + for (const auto & combination : combinations) + { + std::unordered_set group(combination.begin(), combination.end()); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + fmt::format( + "Adding predecessor group: [{}] with size: {}", fmt::join(combination, ", "), + combination.size()) + .c_str()); + if (!group.empty()) + { + mutually_exclusive_predecessor_groups.push_back(group); + } + } + } + } + + void build_mutually_exclusive_successor_groups() + { + // Build mutually exclusive groups of successor controllers, that could utilize all the + // reference interfaces of the current controller. This is used to determine which successor + // controllers can be activated together with the current controller. + + mutually_exclusive_successor_groups.clear(); + const auto are_all_command_interfaces_found = + []( + const std::unordered_set & ref_itfs, + const std::unordered_set & cmd_itfs) + { + return std::all_of( + cmd_itfs.begin(), cmd_itfs.end(), [&ref_itfs](const std::string & command_itf) + { return ref_itfs.find(command_itf) != ref_itfs.end(); }); + }; + const auto & current_reference_interfaces = reference_interfaces; + std::for_each( + successors.begin(), successors.end(), + [this, &are_all_command_interfaces_found](const ControllerPeerInfo * s) + { + // check if all the command interfaces of the successor are in the current controller's + // reference interfaces If they are, add them as individual group + std::unordered_set successor_group = {}; + bool all_successor_interfaces_match = + are_all_command_interfaces_found(s->reference_interfaces, command_interfaces); + if (all_successor_interfaces_match) + { + // If the successor's command interfaces are all in the current controller's reference + // interfaces, add it as individual group + successor_group.insert(s->name); + mutually_exclusive_successor_groups.push_back(successor_group); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "Adding successor: '%s' as individual group, as all its command interfaces are in the " + "current controller's reference interfaces.", + s->name.c_str()); + } + }); + + // If the successor's command interfaces are not all in the current controller's reference + // interfaces, then check other successors and see if they can be grouped together + + // generate combinations of successors that can be grouped together + for (const auto & successor : successors) + { + // check if the successor is already in the mutually exclusive group as single group + if (std::any_of( + mutually_exclusive_successor_groups.begin(), mutually_exclusive_successor_groups.end(), + [&successor](const std::unordered_set & group) + { return group.find(successor->name) != group.end() && group.size() == 1; })) + { + continue; // skip this successor, as it is already in a group as individual + } + // create all combinations of successors that can be grouped together + // For instance, successors A,B,C,D. Get combinations like: + // A,B; A,C; A,D; B,C; B,D; C,D; A,B,C; A,B,D; A,C,D; B,C,D; A,B,C,D + std::vector> combinations; + std::vector current_combination; + std::function generate_combinations = [&](size_t start_index) + { + if (current_combination.size() > 1) + { + // check if the current combination's command interfaces are all in the + // current controller's reference interfaces + std::unordered_set combined_reference_interfaces; + for (const auto & successor_name : current_combination) + { + const auto & successor_itf = std::find_if( + successors.begin(), successors.end(), [&successor_name](const ControllerPeerInfo * s) + { return s->name == successor_name; }); + if (successor_itf != successors.end()) + { + combined_reference_interfaces.insert( + (*successor_itf)->reference_interfaces.begin(), + (*successor_itf)->reference_interfaces.end()); + } + } + if (are_all_command_interfaces_found(combined_reference_interfaces, command_interfaces)) + { + combinations.push_back(current_combination); + } + } + for (size_t i = start_index; i < successors.size(); ++i) + { + if (std::any_of( + mutually_exclusive_successor_groups.begin(), + mutually_exclusive_successor_groups.end(), + [&](const std::unordered_set & group) + { return group.find(successors[i]->name) != group.end() && group.size() == 1; })) + { + continue; // skip this successor, as it is already in a group as individual + } + + if ( + std::find( + current_combination.begin(), current_combination.end(), successors[i]->name) != + current_combination.end()) + { + continue; // skip this successor, as it is already in the current combination + } + current_combination.push_back(successors[i]->name); + generate_combinations(i + 1); + current_combination.pop_back(); + } + }; + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "Generating combinations of successors for controller: %s", name.c_str()); + generate_combinations(0); + // Add the combinations to the mutually exclusive successor groups + for (const auto & combination : combinations) + { + std::unordered_set group(combination.begin(), combination.end()); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + fmt::format( + "Adding successor group: [{}] with size: {}", fmt::join(combination, ", "), + combination.size()) + .c_str()); + if (!group.empty()) + { + mutually_exclusive_successor_groups.push_back(group); + } + } + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "Mutually exclusive successor groups for controller: '%s' are: %zu", name.c_str(), + mutually_exclusive_successor_groups.size()); + } + } + + void get_controllers_to_activate(std::vector & controllers_to_activate) const + { + // Check the predecessors of the controller and check if they belong to the controller's state + // interfaces If they do, add them to the list of controllers to activatestate_itf + /// @todo Handle the cases where the predecessor is not active in the current state + std::unordered_set predecessor_command_interfaces_set = {}; + std::vector predecessor_in_active_list = {}; + std::for_each( + predecessors.begin(), predecessors.end(), + [&predecessor_command_interfaces_set, &predecessor_in_active_list, &controllers_to_activate, + this](const ControllerPeerInfo * predecessor) + { + if (ros2_control::has_item(controllers_to_activate, predecessor->name)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The predecessor: '%s' is already in the active list.", predecessor->name.c_str()); + ros2_control::add_item(predecessor_in_active_list, predecessor->name); + + // Only insert those that has name of the current controller in their command interfaces + std::for_each( + predecessor->command_interfaces.begin(), predecessor->command_interfaces.end(), + [&predecessor_command_interfaces_set, &predecessor, + this](const std::string & command_itf) + { + if (command_itf.find(name) != std::string::npos) + { + predecessor_command_interfaces_set.insert(command_itf); + } + }); + // break; + } + }); + + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The predecessor command interfaces of the predecessor: '%s' are: %zu", name.c_str(), + predecessor_command_interfaces_set.size()); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The reference interfaces of the controller: '%s' are: %zu", name.c_str(), + reference_interfaces.size()); + if ( + !predecessor_in_active_list.empty() && + (predecessor_command_interfaces_set.size() != reference_interfaces.size())) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The predecessor command interfaces of the predecessor: '%s' are not equal to the " + "reference interfaces of the controller: '%s' : %zu != %zu", + name.c_str(), name.c_str(), predecessor_command_interfaces_set.size(), + reference_interfaces.size()); + for (const auto & predecessor : predecessors) + { + if (!ros2_control::has_item(predecessor_in_active_list, predecessor->name)) + { + ros2_control::add_item(controllers_to_activate, predecessor->name); + predecessor->get_controllers_to_activate(controllers_to_activate); + } + } + } + for (const auto & predecessor : predecessors) + { + for (const auto & state_itf : state_interfaces) + { + if (state_itf.find(predecessor->name) != std::string::npos) + { + ros2_control::add_item(controllers_to_activate, predecessor->name); + break; + } + } + } + + std::unordered_set command_interfaces_set( + command_interfaces.begin(), command_interfaces.end()); + size_t successors_reference_interfaces_count = 0; + for (const auto & successor : successors) + { + successors_reference_interfaces_count += successor->reference_interfaces.size(); + } + for (const auto & successor : successors) + { + // check if all the successors reference interfaces are in the current controller's command + // interfaces If they are, add them to the list of controllers to activate + + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The command interfaces of the predecessor: '%s' are: %zu", name.c_str(), + command_interfaces_set.size()); + for (const auto & command_itf : command_interfaces_set) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The command interfaces of the predecessor: '%s' are: %s", name.c_str(), + command_itf.c_str()); + } + + for (const auto & reference_itf : successor->reference_interfaces) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The reference interfaces of the successor: '%s' are: %s", successor->name.c_str(), + reference_itf.c_str()); + } + + bool all_successor_interfaces_match = false; + std::for_each( + command_interfaces.begin(), command_interfaces.end(), + [&successor, &all_successor_interfaces_match](const std::string & command_itf) + { + if ( + successor->reference_interfaces.find(command_itf) != + successor->reference_interfaces.end()) + { + all_successor_interfaces_match = true; + } + }); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The reference interfaces of the successor: '%s' are within the command interfaces of the " + "predecessor: '%s' : %s", + successor->name.c_str(), name.c_str(), all_successor_interfaces_match ? "true" : "false"); + if (all_successor_interfaces_match) + { + ros2_control::add_item(controllers_to_activate, successor->name); + successor->get_controllers_to_activate(controllers_to_activate); + continue; + } + else + { + RCLCPP_WARN( + rclcpp::get_logger("controller_manager"), + "Controller %s has a successor %s who has more reference interfaces that use different " + "controllers. This is not supported now.", + name.c_str(), successor->name.c_str()); + } + } + } + + void get_controllers_to_deactivate(std::vector & controllers_to_deactivate) const + { + // All predecessors of the controller should be deactivated except the state interface ones + for (const auto & predecessor : predecessors) + { + if (ros2_control::has_item(controllers_to_deactivate, predecessor->name)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The predecessor: '%s' is already in the deactivation list.", predecessor->name.c_str()); + continue; + } + ros2_control::add_item(controllers_to_deactivate, predecessor->name); + std::for_each( + state_interfaces.begin(), state_interfaces.end(), + [&predecessor, &controllers_to_deactivate](const std::string & state_itf) + { + if (state_itf.find(predecessor->name) != std::string::npos) + { + ros2_control::remove_item(controllers_to_deactivate, predecessor->name); + } + }); + predecessor->get_controllers_to_deactivate(controllers_to_deactivate); + } + + // All successors of controller with no command interfaces should be deactivated + for (const auto & successor : successors) + { + if (ros2_control::has_item(controllers_to_deactivate, successor->name)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The successor: '%s' is already in the deactivation list.", successor->name.c_str()); + continue; + } + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + fmt::format( + "The controllers to deactivate list is {}", fmt::join(controllers_to_deactivate, ", ")) + .c_str()); + + // Check if the successor is an individual exclusive group, if so, then return + if (std::any_of( + mutually_exclusive_successor_groups.begin(), mutually_exclusive_successor_groups.end(), + [&successor](const std::unordered_set & group) + { return group.find(successor->name) != group.end() && group.size() == 1; })) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "The successor: '%s' is in a mutually exclusive group, skipping further deactivation.", + successor->name.c_str()); + continue; + } + + if (successor->command_interfaces.empty()) + { + ros2_control::add_item(controllers_to_deactivate, successor->name); + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "Adding successor: '%s' to the deactivation list, as it has no command interfaces.", + successor->name.c_str()); + } + successor->get_controllers_to_deactivate(controllers_to_deactivate); + } + } +}; /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -57,5 +552,78 @@ struct ControllerChainSpec std::vector following_controllers; std::vector preceding_controllers; }; + +class ControllerChainDependencyGraph +{ +public: + void add_dependency(const ControllerPeerInfo & predecessor, const ControllerPeerInfo & successor) + { + if (controller_graph_.count(predecessor.name) == 0) + { + controller_graph_[predecessor.name] = predecessor; + } + if (controller_graph_.count(successor.name) == 0) + { + controller_graph_[successor.name] = successor; + } + if ( + std::find_if( + controller_graph_[predecessor.name].successors.begin(), + controller_graph_[predecessor.name].successors.end(), + [&successor](const ControllerPeerInfo * s) { return s->name == successor.name; }) == + controller_graph_[predecessor.name].successors.end()) + { + controller_graph_[predecessor.name].successors.push_back(&controller_graph_[successor.name]); + } + if ( + std::find_if( + controller_graph_[successor.name].predecessors.begin(), + controller_graph_[successor.name].predecessors.end(), + [&predecessor](const ControllerPeerInfo * p) { return p->name == predecessor.name; }) == + controller_graph_[successor.name].predecessors.end()) + { + controller_graph_[successor.name].predecessors.push_back( + &controller_graph_[predecessor.name]); + } + } + + std::vector get_dependencies_to_activate(const std::string & controller_name) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "+++++++++++++++++++++++++++++++ Getting dependencies to ACTIVATE " + "+++++++++++++++++++++++++++++++"); + std::vector controllers_to_activate({controller_name}); + if (controller_graph_.count(controller_name) == 0) + { + return {}; + } + controller_graph_[controller_name].build_mutually_exclusive_predecessor_groups(); + controller_graph_[controller_name].build_mutually_exclusive_successor_groups(); + controller_graph_[controller_name].get_controllers_to_activate(controllers_to_activate); + return controllers_to_activate; + } + + std::vector get_dependencies_to_deactivate(const std::string & controller_name) + { + RCLCPP_DEBUG( + rclcpp::get_logger("controller_manager"), + "+++++++++++++++++++++++++++++++ Getting dependencies to DEACTIVATE " + "+++++++++++++++++++++++++++++++"); + std::vector controllers_to_deactivate({controller_name}); + if (controller_graph_.count(controller_name) == 0) + { + return {}; + } + controller_graph_[controller_name].build_mutually_exclusive_predecessor_groups(); + controller_graph_[controller_name].build_mutually_exclusive_successor_groups(); + controller_graph_[controller_name].get_controllers_to_deactivate(controllers_to_deactivate); + return controllers_to_deactivate; + } + +private: + std::unordered_map controller_graph_; +}; + } // namespace controller_manager #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d7b482246e..6f020494d3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1336,33 +1336,7 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - for (const auto & cmd_itf : cmd_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) - { - ros2_control::add_item( - controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); - ros2_control::add_item( - controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name); - ros2_control::add_item( - controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller_name); - } - } - // This is needed when we start exporting the state interfaces from the controllers - for (const auto & state_itf : state_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) - { - ros2_control::add_item( - controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); - ros2_control::add_item( - controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name); - ros2_control::add_item( - controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller_name); - } - } + build_controllers_topology_info(controllers); // Now let's reorder the controllers // lock controllers @@ -1493,22 +1467,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb( get_logger(), "Controller Manager: to switch controllers you need to specify a " "strictness level of controller_manager_msgs::SwitchController::STRICT " - "(%d) or ::BEST_EFFORT (%d). When unspecified, the default is %s", + "(%d) or ::BEST_EFFORT (%d) or ::AUTO (%d). When unspecified, the default is %s", controller_manager_msgs::srv::SwitchController::Request::STRICT, controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, - default_strictness.c_str()); + controller_manager_msgs::srv::SwitchController::Request::AUTO, default_strictness.c_str()); strictness = params_->defaults.switch_controller.strictness == "strict" ? controller_manager_msgs::srv::SwitchController::Request::STRICT : controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - else if (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO) - { - RCLCPP_WARN( - get_logger(), - "Controller Manager: AUTO is not currently implemented. " - "Defaulting to BEST_EFFORT"); - strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; - } else if (strictness == controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO) { RCLCPP_DEBUG( @@ -1517,37 +1483,69 @@ controller_interface::return_type ControllerManager::switch_controller_cb( "Defaulting to BEST_EFFORT"); strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - - std::string activate_list, deactivate_list; - activate_list.reserve(500); - deactivate_list.reserve(500); - for (const auto & controller : activate_controllers) - { - activate_list.append(controller); - activate_list.append(" "); - } - for (const auto & controller : deactivate_controllers) + else if (strictness > controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO) { - deactivate_list.append(controller); - deactivate_list.append(" "); + message = fmt::format( + FMT_COMPILE( + "Unknown strictness level '%d'. Please use controller_manager_msgs::SwitchController::" + "STRICT (%d) or ::BEST_EFFORT (%d) or ::AUTO (%d) or ::FORCE_AUTO (%d)."), + strictness, controller_manager_msgs::srv::SwitchController::Request::STRICT, + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT, + controller_manager_msgs::srv::SwitchController::Request::AUTO, + controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO); + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; } + + const std::string strictness_string = + strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT + ? "STRICT" + : (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT + ? "BEST_EFFORT" + : (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO + ? "AUTO" + : "FORCE_AUTO")); + RCLCPP_INFO_EXPRESSION( - get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str()); + get_logger(), !activate_controllers.empty(), + fmt::format("Activating controllers: [ {} ]", fmt::join(activate_controllers, " ")).c_str()); RCLCPP_INFO_EXPRESSION( - get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", - deactivate_list.c_str()); + get_logger(), !deactivate_controllers.empty(), + fmt::format("Deactivating controllers: [ {} ]", fmt::join(deactivate_controllers, " ")) + .c_str()); const auto list_controllers = - [this, strictness]( + [this, strictness, &strictness_string]( const std::vector & controller_list, std::vector & request_list, const std::string & action, std::string & msg) -> controller_interface::return_type { // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); auto result = controller_interface::return_type::OK; + std::vector new_controller_list = controller_list; + + if (strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO) + { + const bool is_activate = (action == "activate"); + std::vector dependencies = {}; + for (const auto & controller : controller_list) + { + const auto ctrl_dependencies = + is_activate + ? controller_chain_dependency_graph_.get_dependencies_to_activate(controller) + : controller_chain_dependency_graph_.get_dependencies_to_deactivate(controller); + RCLCPP_DEBUG( + get_logger(), fmt::format( + "Controller {} has '{}' dependencies to {}", controller, + fmt::join(ctrl_dependencies, ", "), action) + .c_str()); + ros2_control::add_unique_items(dependencies, ctrl_dependencies); + } + ros2_control::add_unique_items(new_controller_list, dependencies); + } // list all controllers to (de)activate - for (const auto & controller : controller_list) + for (const auto & controller : new_controller_list) { const auto & updated_controllers = rt_controllers_wrapper_.get_updated_list(guard); @@ -1567,10 +1565,12 @@ controller_interface::return_type ControllerManager::switch_controller_cb( // not a critical error result = request_list.empty() ? controller_interface::return_type::ERROR : controller_interface::return_type::OK; - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + if (strictness != controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) { msg = error_msg; - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! ('STRICT' switch)"); + RCLCPP_ERROR( + get_logger(), "Aborting, no controller is switched! ('%s' switch)", + strictness_string.c_str()); return controller_interface::return_type::ERROR; } } @@ -1606,6 +1606,19 @@ controller_interface::return_type ControllerManager::switch_controller_cb( activate_request_.clear(); return ret; } + + const bool print_updated_list = + (strictness != controller_manager_msgs::srv::SwitchController::Request::AUTO || + strictness != controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO); + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_request_.empty() && print_updated_list, + fmt::format("Updated Activating controllers: [ {} ]", fmt::join(activate_request_, " ")) + .c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_request_.empty() && print_updated_list, + fmt::format("Updated Deactivating controllers: [ {} ]", fmt::join(deactivate_request_, " ")) + .c_str()); + // If it is a best effort switch, we can remove the controllers log that could not be activated message.clear(); @@ -1647,7 +1660,6 @@ controller_interface::return_type ControllerManager::switch_controller_cb( message = fmt::format( FMT_COMPILE("Controller with name '{}' is already active."), controller_it->info.name); RCLCPP_WARN(get_logger(), "%s", message.c_str()); - RCLCPP_WARN(get_logger(), "%s", message.c_str()); status = controller_interface::return_type::ERROR; } } @@ -1680,7 +1692,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb( "Check the state of the controllers and their required interfaces using " "`ros2 control list_controllers -v` CLI to get more information.", (*ctrl_it).c_str()); - if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + if ( + strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT || + strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO) { // TODO(destogl): automatic manipulation of the chain: // || strictness == @@ -1691,9 +1705,11 @@ controller_interface::return_type ControllerManager::switch_controller_cb( message.clear(); --ctrl_it; } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + else { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + RCLCPP_ERROR( + get_logger(), "Aborting, no controller is switched! (::%s switch)", + strictness_string.c_str()); // reset all lists clear_requests(); return controller_interface::return_type::ERROR; @@ -1732,7 +1748,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb( "Check the state of the controllers and their required interfaces using " "`ros2 control list_controllers -v` CLI to get more information.", (*ctrl_it).c_str()); - if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + if ( + strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT || + strictness == controller_manager_msgs::srv::SwitchController::Request::AUTO) { // remove controller that can not be activated from the activation request and step-back // iterator to correctly step to the next element in the list in the loop @@ -1740,9 +1758,11 @@ controller_interface::return_type ControllerManager::switch_controller_cb( message.clear(); --ctrl_it; } - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + else { - RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + RCLCPP_ERROR( + get_logger(), "Aborting, no controller is switched! (::%s switch)", + strictness_string.c_str()); // reset all lists clear_requests(); return controller_interface::return_type::ERROR; @@ -1798,10 +1818,10 @@ controller_interface::return_type ControllerManager::switch_controller_cb( auto handle_conflict = [&](const std::string & msg) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + if (strictness != controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) { message = msg; - RCLCPP_ERROR(get_logger(), "%s", msg.c_str()); + RCLCPP_ERROR(get_logger(), "%s (::%s switch)", message.c_str(), strictness_string.c_str()); deactivate_request_.clear(); deactivate_command_interface_request_.clear(); activate_request_.clear(); @@ -2113,6 +2133,8 @@ void ControllerManager::deactivate_controllers( { resource_manager_->make_controller_exported_state_interfaces_unavailable(controller_name); resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + // TODO(saikishor): Review this part later + controller->set_chained_mode(false); } if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -4314,6 +4336,113 @@ void ControllerManager::update_list_with_controller_chain( } } +void ControllerManager::build_controllers_topology_info( + const std::vector & controllers) +{ + std::for_each( + controller_chain_spec_.begin(), controller_chain_spec_.end(), + [](auto & pair) + { + pair.second.following_controllers.clear(); + pair.second.preceding_controllers.clear(); + }); + std::for_each( + controller_chained_reference_interfaces_cache_.begin(), + controller_chained_reference_interfaces_cache_.end(), [](auto & pair) { pair.second.clear(); }); + std::for_each( + controller_chained_state_interfaces_cache_.begin(), + controller_chained_state_interfaces_cache_.end(), [](auto & pair) { pair.second.clear(); }); + + const auto get_controller_peer_info = [&](const ControllerSpec & controller) -> ControllerPeerInfo + { + const auto cmd_itfs = controller.c->command_interface_configuration().names; + const auto state_itfs = controller.c->state_interface_configuration().names; + ControllerPeerInfo controller_peer_info; + controller_peer_info.name = controller.info.name; + controller_peer_info.command_interfaces = + std::unordered_set(cmd_itfs.begin(), cmd_itfs.end()); + controller_peer_info.state_interfaces = + std::unordered_set(state_itfs.begin(), state_itfs.end()); + controller_peer_info.controller = controller.c; + if (controller.c->is_chainable()) + { + const auto ref_interface = + resource_manager_->get_controller_reference_interface_names(controller.info.name); + controller_peer_info.reference_interfaces = + std::unordered_set(ref_interface.begin(), ref_interface.end()); + } + return controller_peer_info; + }; + for (const auto & controller : controllers) + { + if (is_controller_unconfigured(*controller.c)) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' is unconfigured, skipping chain building.", + controller.info.name.c_str()); + continue; + } + const auto cmd_itfs = controller.c->command_interface_configuration().names; + const auto state_itfs = controller.c->state_interface_configuration().names; + + ControllerPeerInfo controller_peer_info = get_controller_peer_info(controller); + for (const auto & cmd_itf : cmd_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) + { + if (is_controller_unconfigured(*ctrl_it->c)) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' is unconfigured, skipping chain building.", + ctrl_it->info.name.c_str()); + continue; + } + ControllerPeerInfo succeeding_peer_info = get_controller_peer_info(*ctrl_it); + controller_chain_dependency_graph_.add_dependency( + controller_peer_info, succeeding_peer_info); + ros2_control::add_item( + controller_chain_spec_[controller.info.name].following_controllers, ctrl_it->info.name); + ros2_control::add_item( + controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller.info.name); + ros2_control::add_item( + controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller.info.name); + } + } + // This is needed when we start exporting the state interfaces from the controllers + for (const auto & state_itf : state_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) + { + ros2_control::add_item( + controller_chain_spec_[controller.info.name].preceding_controllers, ctrl_it->info.name); + ros2_control::add_item( + controller_chain_spec_[ctrl_it->info.name].following_controllers, controller.info.name); + ros2_control::add_item( + controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller.info.name); + } + } + } + for (const auto & [controller_name, controller_chain] : controller_chain_spec_) + { + RCLCPP_DEBUG( + get_logger(), "Controller '%s' has %ld following controllers and %ld preceding controllers.", + controller_name.c_str(), controller_chain.following_controllers.size(), + controller_chain.preceding_controllers.size()); + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain.following_controllers.empty(), + fmt::format( + "\tFollowing controllers are : {}", fmt::join(controller_chain.following_controllers, ", ")) + .c_str()); + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain.preceding_controllers.empty(), "%s", + fmt::format( + "\tPreceding controllers are : {}", fmt::join(controller_chain.preceding_controllers, ", ")) + .c_str()); + } +} + rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const ControllerSpec & controller) const { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 00afa58186..a28f8260c7 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -73,6 +73,139 @@ class TestControllerManagerWithStrictness } }; +TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) +{ + { + controller_manager::ControllerChainDependencyGraph graph; + // Let's test the case of A -> B -> C -> D && B -> E -> F + controller_manager::ControllerPeerInfo A; + A.name = "A"; + A.command_interfaces = {"B3", "B4"}; + A.reference_interfaces = {"A3", "A4"}; + A.state_interfaces = {"A5", "A6"}; + controller_manager::ControllerPeerInfo B; + B.name = "B"; + B.command_interfaces = {"C3", "C4", "E3", "E4"}; + B.reference_interfaces = {"B3", "B4"}; + B.state_interfaces = {"B5", "B6"}; + controller_manager::ControllerPeerInfo C; + C.name = "C"; + C.command_interfaces = {"D3", "D4"}; + C.reference_interfaces = {"C3", "C4"}; + C.state_interfaces = {"C5", "C6"}; + controller_manager::ControllerPeerInfo D; + D.name = "D"; + D.command_interfaces = {"D1", "D2"}; + D.reference_interfaces = {"D3", "D4"}; + D.state_interfaces = {"D5", "D6"}; + controller_manager::ControllerPeerInfo E; + E.name = "E"; + E.command_interfaces = {"F3", "F4"}; + E.reference_interfaces = {"E3", "E4", "E7", "E8"}; + E.state_interfaces = {"E5", "E6"}; + controller_manager::ControllerPeerInfo F; + F.name = "F"; + F.command_interfaces = {"F1", "F2"}; + F.reference_interfaces = {"F3", "F4"}; + F.state_interfaces = {"F5", "F6"}; + + graph.add_dependency(A, B); + graph.add_dependency(B, C); + graph.add_dependency(C, D); + graph.add_dependency(B, E); + graph.add_dependency(E, F); + + EXPECT_THAT( + graph.get_dependencies_to_activate("A"), + testing::UnorderedElementsAre("A", "B", "C", "D", "E", "F")); + EXPECT_THAT( + graph.get_dependencies_to_activate("B"), + testing::UnorderedElementsAre("B", "C", "D", "E", "F")); + EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("C", "D")); + EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre("D")); + EXPECT_THAT(graph.get_dependencies_to_activate("E"), testing::UnorderedElementsAre("E", "F")); + EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre("F")); + + EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre("A")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", "B")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("B", "A", "C")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A", "D")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("B", "A", "E")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", "B", "A", "F")); + } + + { + RCLCPP_INFO(rclcpp::get_logger("test"), "Testing controller chain dependency graph"); + RCLCPP_INFO(rclcpp::get_logger("test"), "+++++++++++++++++++++++++++++++++++++++++++++++++"); + controller_manager::ControllerChainDependencyGraph graph; + // Let's test the case of A -> B -> C -> D && E -> B && E -> F + controller_manager::ControllerPeerInfo A; + A.name = "A"; + A.command_interfaces = {"B3", "B4"}; + A.reference_interfaces = {"A3", "A4"}; + A.state_interfaces = {"A5", "A6"}; + controller_manager::ControllerPeerInfo B; + B.name = "B"; + B.command_interfaces = {"C3", "C4"}; + B.reference_interfaces = {"B3", "B4", "B5", "B6"}; + B.state_interfaces = {"B5", "B6"}; + controller_manager::ControllerPeerInfo C; + C.name = "C"; + C.command_interfaces = {"D3", "D4"}; + C.reference_interfaces = {"C3", "C4"}; + C.state_interfaces = {"C5", "C6"}; + controller_manager::ControllerPeerInfo D; + D.name = "D"; + D.command_interfaces = {"D1", "D2"}; + D.reference_interfaces = {"D3", "D4"}; + D.state_interfaces = {"D5", "D6"}; + controller_manager::ControllerPeerInfo E; + E.name = "E"; + E.command_interfaces = {"F3", "F4", "B5", "B6"}; + E.reference_interfaces = {"E3", "E4"}; + E.state_interfaces = {"E5", "E6"}; + controller_manager::ControllerPeerInfo F; + F.name = "F"; + F.command_interfaces = {"F1", "F2"}; + F.reference_interfaces = {"F3", "F4"}; + F.state_interfaces = {"F5", "F6"}; + + graph.add_dependency(A, B); + graph.add_dependency(B, C); + graph.add_dependency(C, D); + graph.add_dependency(E, B); + graph.add_dependency(E, F); + + EXPECT_THAT( + graph.get_dependencies_to_activate("A"), + testing::UnorderedElementsAre("A", "B", "C", "D", "E", "F")); + EXPECT_THAT( + graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("B", "C", "D")); + EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("C", "D")); + EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre("D")); + EXPECT_THAT( + graph.get_dependencies_to_activate("E"), + testing::UnorderedElementsAre("E", "F", "B", "C", "D", "A")); + EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre("F")); + + EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre("A")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", "B", "E")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("C", "B", "A", "E")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("D"), + testing::UnorderedElementsAre("D", "C", "B", "A", "E")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("E", "A")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("F", "E", "A")); + } +} + class TestControllerManagerRobotDescription : public ControllerManagerFixture { diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 54c34bd55d..39e9707dd9 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -836,6 +836,133 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_1); RCLCPP_ERROR(srv_node->get_logger(), "Check successful!"); + + auto check_lifecycle_state = [&](const std::vector & expected_states) + { + ASSERT_EQ(expected_states[0], test_controller->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_controller->get_name(); + ASSERT_EQ(expected_states[1], test_chained_controller_5->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_5->get_name(); + ASSERT_EQ(expected_states[2], test_chained_controller_4->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_4->get_name(); + ASSERT_EQ(expected_states[3], test_chained_controller_3->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_3->get_name(); + ASSERT_EQ(expected_states[4], test_chained_controller_2->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_2->get_name(); + ASSERT_EQ(expected_states[5], test_chained_controller_1->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_1->get_name(); + }; + + // Activate the whole chain by activating the top level controller + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO, + true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // redoing the switch should be a no-op and not change the states and it should be successful + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO, + true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // Now, deactivate the last controller in the chain and see that all controllers are deactivated + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_1}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}); + + // Now activate the middle controller in the chain and see that all controllers below it are + // activated + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_4}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // Now, deactivate 3, and see that 3 and 4 are deactivated + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_3}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // Redoing it should be a no-op and not change the states and it should be successful + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_3}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_4}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO, + true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); } TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) @@ -1018,6 +1145,202 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) ASSERT_GT(ctrl_5_pos, ctrl_6_pos); ASSERT_GT(ctrl_4_pos, ctrl_5_pos); ASSERT_GT(ctrl_3_pos, ctrl_4_pos); + + auto check_lifecycle_state = [&](const std::vector & expected_states) + { + ASSERT_EQ(expected_states[0], test_controller->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_controller->get_name(); + ASSERT_EQ(expected_states[1], test_chained_controller_7->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_7->get_name(); + ASSERT_EQ(expected_states[2], test_chained_controller_6->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_6->get_name(); + ASSERT_EQ(expected_states[3], test_chained_controller_5->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_5->get_name(); + ASSERT_EQ(expected_states[4], test_chained_controller_4->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_4->get_name(); + ASSERT_EQ(expected_states[5], test_chained_controller_3->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_3->get_name(); + ASSERT_EQ(expected_states[6], test_chained_controller_2->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_2->get_name(); + ASSERT_EQ(expected_states[7], test_chained_controller_1->get_lifecycle_state().id()) + << "Unexpected state for controller: " << test_chained_controller_1->get_name(); + }; + + // Now let's activate the top level controller and see if the whole chain is activated + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO, + true, rclcpp::Duration(0, 0))); + + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // Now deactivate the chained_controller_1 and everything should deactivate + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_1}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_6}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // Now deactivate the chained_controller_1 and 3 and everything should deactivate + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_3}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_2}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_5}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_7}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // repeating the last should also result in same state and succeed + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_7}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); + + // Deactivate the whole chain by deactivating 1 and 3 + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_3}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE}); + + ASSERT_EQ( + controller_interface::return_type::OK, + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_5}, {}, + controller_manager_msgs::srv::SwitchController::Request::AUTO, true, rclcpp::Duration(0, 0))); + check_lifecycle_state( + {lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE}); } TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index 605ecb9216..10da0f3a85 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace ros2_control @@ -36,7 +37,8 @@ namespace ros2_control template [[nodiscard]] auto get_item_iterator(const Container & container, const T & item) { - if constexpr (std::is_same_v>) + if constexpr ( + std::is_same_v> || std::is_same_v>) { return std::find(container.begin(), container.end(), item); } @@ -52,10 +54,11 @@ template using is_map = std::is_same>; using is_unordered_map = std::is_same>; + using is_unordered_set = std::is_same>; // Handle unsupported container types static_assert( - is_vector::value || is_map::value || is_unordered_map::value, - "Only std::vector, std::map and std::unordered_map are supported."); + is_vector::value || is_map::value || is_unordered_map::value || is_unordered_set::value, + "Only std::vector, std::unordered_set, std::map and std::unordered_map are supported."); } } @@ -85,6 +88,15 @@ void add_item(std::vector & vector, const T & item) } } +template +void add_unique_items(std::vector & vector, const std::vector & items) +{ + for (const auto & item : items) + { + add_item(vector, item); + } +} + /** * @brief Remove the item from the container if it is in it. * @param container The container to remove the item from. @@ -175,6 +187,22 @@ template return std::adjacent_find(container.cbegin(), container.cend()) == container.cend(); } +// Create a function that checks that two containers do not have any common items +/** + * @brief Check if the two containers have no common items. + * @param container1 The first container to search in. + * @param container2 The second container to search in. + * @return True if the two containers have no common items, false otherwise. + */ +template +[[nodiscard]] bool has_no_common_items(const Container1 & container1, const Container2 & container2) +{ + return std::none_of( + container1.cbegin(), container1.cend(), + [&container2](const typename Container1::value_type & item) + { return has_item(container2, item); }); +} + } // namespace ros2_control #endif // HARDWARE_INTERFACE__HELPERS_HPP_ diff --git a/hardware_interface/test/test_helpers.cpp b/hardware_interface/test/test_helpers.cpp index 0ec043a24d..abb065092e 100644 --- a/hardware_interface/test/test_helpers.cpp +++ b/hardware_interface/test/test_helpers.cpp @@ -54,4 +54,33 @@ TEST(TestHelper, test_helper_methods) ASSERT_TRUE(ros2_control::is_unique(vec)); ASSERT_FALSE(ros2_control::is_unique(std::vector({"aa", "bb", "cc", "aa"}))); + + std::vector vec1({"aa", "bb", "cc"}); + std::vector vec2({"dd", "ee"}); + std::vector vec3({"aa", "bb", "dd"}); + + std::unordered_set set1({"aa", "bb", "cc"}); + std::unordered_set set2({"dd", "ee"}); + std::unordered_set set3({"aa", "bb", "dd"}); + + ASSERT_TRUE(ros2_control::has_no_common_items(vec1, vec2)); + ASSERT_TRUE(ros2_control::has_no_common_items(set1, set2)); + ASSERT_TRUE(ros2_control::has_no_common_items(vec1, set2)); + ASSERT_TRUE(ros2_control::has_no_common_items(vec2, set1)); + ASSERT_TRUE(ros2_control::has_no_common_items(set1, vec2)); + ASSERT_TRUE(ros2_control::has_no_common_items(set2, vec1)); + + ASSERT_FALSE(ros2_control::has_no_common_items(vec1, vec3)); + ASSERT_FALSE(ros2_control::has_no_common_items(set1, set3)); + ASSERT_FALSE(ros2_control::has_no_common_items(vec1, vec1)); + ASSERT_FALSE(ros2_control::has_no_common_items(set1, set1)); + ASSERT_FALSE(ros2_control::has_no_common_items(vec2, vec2)); + ASSERT_FALSE(ros2_control::has_no_common_items(set2, set2)); + ASSERT_FALSE(ros2_control::has_no_common_items(vec1, set3)); + ASSERT_FALSE(ros2_control::has_no_common_items(set1, vec3)); + + std::vector vec4 = vec3; + ASSERT_THAT(vec4, ::testing::ElementsAre("aa", "bb", "dd")); + ros2_control::add_unique_items(vec4, vec2); + ASSERT_THAT(vec4, ::testing::ElementsAre("aa", "bb", "dd", "ee")); }