From 69efebb3dc8bed751fe6a66fa07c4aba4c66a260 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 May 2025 23:59:49 +0200 Subject: [PATCH 01/29] Add controllers chain dependency graph to know the successors and predecessors --- .../controller_manager/controller_manager.hpp | 1 + .../controller_manager/controller_spec.hpp | 79 +++++++++++++++++++ controller_manager/src/controller_manager.cpp | 2 + 3 files changed, 82 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 21e8c71f3b..87aa153dff 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -635,6 +635,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..c76b6c3f6f 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -19,11 +19,16 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +#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 @@ -57,5 +62,79 @@ struct ControllerChainSpec std::vector following_controllers; std::vector preceding_controllers; }; + +class ControllerChainDependencyGraph +{ +public: + void add_dependency(const std::string & predecessor, const std::string & successor) + { + if (predecessors.count(predecessor) == 0) + { + predecessors[predecessor] = {}; + } + if (successors.count(successor) == 0) + { + successors[successor] = {}; + } + + ros2_control::add_item(predecessors[successor], predecessor); + ros2_control::add_item(successors[predecessor], successor); + } + + void remove_controller(const std::string & controller_name) + { + predecessors.erase(controller_name); + successors.erase(controller_name); + for (auto & [_, succ] : predecessors) + { + succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end()); + } + for (auto & [_, preds] : successors) + { + preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end()); + } + } + + void depth_first_search( + const std::string & controller_name, std::unordered_set & visited, + std::unordered_map> & graph) + { + if (visited.find(controller_name) != visited.end()) + { + return; + } + visited.insert(controller_name); + for (const auto & neighbor : graph[controller_name]) + { + if (visited.find(neighbor) == visited.end()) + { + depth_first_search(neighbor, visited, graph); + } + } + } + + std::vector get_all_predecessors(const std::string & controller_name) + { + std::unordered_set visited; + depth_first_search(controller_name, visited, predecessors); + std::vector predecessors_list; + std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list)); + return predecessors_list; + } + + std::vector get_all_successors(const std::string & controller_name) + { + std::unordered_set visited; + depth_first_search(controller_name, visited, successors); + std::vector successors_list; + std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list)); + return successors_list; + } + +private: + std::unordered_map> predecessors = {}; + std::unordered_map> successors = {}; +}; + } // 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..29fc7ec231 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1341,6 +1341,7 @@ controller_interface::return_type ControllerManager::configure_controller( controller_manager::ControllersListIterator ctrl_it; if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) { + controller_chain_dependency_graph_.add_dependency(controller_name, ctrl_it->info.name); ros2_control::add_item( controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); ros2_control::add_item( @@ -1355,6 +1356,7 @@ controller_interface::return_type ControllerManager::configure_controller( controller_manager::ControllersListIterator ctrl_it; if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) { + controller_chain_dependency_graph_.add_dependency(ctrl_it->info.name, controller_name); ros2_control::add_item( controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); ros2_control::add_item( From 37b69b94995c9c6783ab7deac6576ed3d9282125 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 13 May 2025 23:51:37 +0200 Subject: [PATCH 02/29] Add some tests --- .../controller_manager/controller_spec.hpp | 2 ++ .../test/test_controller_manager.cpp | 28 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index c76b6c3f6f..ec8390fbd0 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -119,6 +119,7 @@ class ControllerChainDependencyGraph depth_first_search(controller_name, visited, predecessors); std::vector predecessors_list; std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list)); + predecessors_list.pop_back(); // Remove the controller itself return predecessors_list; } @@ -128,6 +129,7 @@ class ControllerChainDependencyGraph depth_first_search(controller_name, visited, successors); std::vector successors_list; std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list)); + successors_list.pop_back(); // Remove the controller itself return successors_list; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 00afa58186..d6873d2e96 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -73,6 +73,34 @@ class TestControllerManagerWithStrictness } }; +TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) +{ + { + controller_manager::ControllerChainDependencyGraph graph; + // Let's test the case of A -> B -> C -> D && B -> E && E -> F + 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_all_successors("A"), testing::UnorderedElementsAre("B", "C", "D", "E", "F")); + EXPECT_THAT(graph.get_all_successors("B"), testing::UnorderedElementsAre("C", "D", "E", "F")); + EXPECT_THAT(graph.get_all_successors("C"), testing::UnorderedElementsAre("D")); + EXPECT_THAT(graph.get_all_successors("D"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_all_successors("E"), testing::UnorderedElementsAre("F")); + EXPECT_THAT(graph.get_all_successors("F"), testing::UnorderedElementsAre()); + + EXPECT_THAT(graph.get_all_predecessors("A"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_all_predecessors("B"), testing::UnorderedElementsAre("A")); + EXPECT_THAT(graph.get_all_predecessors("C"), testing::UnorderedElementsAre("B", "A")); + EXPECT_THAT(graph.get_all_predecessors("D"), testing::UnorderedElementsAre("C", "B", "A")); + EXPECT_THAT(graph.get_all_predecessors("E"), testing::UnorderedElementsAre("B", "A")); + EXPECT_THAT(graph.get_all_predecessors("F"), testing::UnorderedElementsAre("E", "B", "A")); + } +} + class TestControllerManagerRobotDescription : public ControllerManagerFixture { From 1a5a1ad74c2270318c6277abeab704513f3f0813 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 16 May 2025 00:31:42 +0200 Subject: [PATCH 03/29] WIP: Add dependencies finder class --- .../controller_manager/controller_spec.hpp | 331 +++++++++++++++--- .../test/test_controller_manager.cpp | 55 ++- 2 files changed, 321 insertions(+), 65 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index ec8390fbd0..341d154519 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -35,6 +35,133 @@ namespace controller_manager { using MovingAverageStatistics = ros2_control::MovingAverageStatistics; + +struct ControllerPeerInfo +{ + std::string name = ""; + std::vector predecessors = {}; + std::vector successors = {}; + + std::vector get_controllers_to_activate() const + { + std::vector controllers_to_activate; + for (const auto & successor : successors) + { + ros2_control::add_item(controllers_to_activate, successor->name); + successor->get_successors_and_predecessors(controllers_to_activate, name); + } + ros2_control::remove_item(controllers_to_activate, name); + return controllers_to_activate; + } + + std::vector get_controllers_to_deactivate() const + { + std::vector controllers_to_deactivate; + for (const auto & predecessor : predecessors) + { + ros2_control::add_item(controllers_to_deactivate, predecessor->name); + predecessor->get_predecessors(controllers_to_deactivate, name); + predecessor->get_successors(controllers_to_deactivate, name); + } + ros2_control::remove_item(controllers_to_deactivate, name); + return controllers_to_deactivate; + } + + void get_predecessors(std::vector &total_list, const std::string &untill_controller) const + { + for (const auto & predecessor : predecessors) + { + if(!predecessor) + { + continue; + } + if(predecessor->name == untill_controller) + { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), "skipping predecessor: %s - %s", + predecessor->name.c_str(), untill_controller.c_str()); + continue; + } + const std::string predecessor_name = predecessor->name; + if(!ros2_control::has_item(total_list, predecessor_name)) + { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "Getting Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), untill_controller.c_str()); + total_list.push_back(predecessor_name); + predecessor->get_predecessors(total_list, predecessor_name); + } + } + } + + void get_successors(std::vector &total_list, const std::string &untill_controller) const + { + for (const auto & successor : successors) + { + if(!successor) + { + continue; + } + if(successor->name == untill_controller) + { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), "skipping successor: %s - %s", + successor->name.c_str(), untill_controller.c_str()); + continue; + } + const std::string successor_name = successor->name; + if(!ros2_control::has_item(total_list, successor_name)) + { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "Getting Successor: %s, Predecessor: %s - %s", successor_name.c_str(), name.c_str(), untill_controller.c_str()); + total_list.push_back(successor_name); + successor->get_successors(total_list, successor_name); + } + } + } + + void get_successors_and_predecessors(std::vector &total_list, const std::string &untill_controller) const + { + for (const auto & predecessor : predecessors) + { + if(!predecessor) + { + continue; + } + if(predecessor->name == untill_controller) + { + continue; + } + const std::string predecessor_name = predecessor->name; + if(!ros2_control::has_item(total_list, predecessor_name)) + { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), untill_controller.c_str()); + total_list.push_back(predecessor_name); + predecessor->get_successors_and_predecessors(total_list, predecessor_name); + } + } + for (const auto & successor : successors) + { + if(!successor) + { + continue; + } + if(successor->name == untill_controller) + { + continue; + } + const std::string successor_name = successor->name; + if(!ros2_control::has_item(total_list, successor_name)) + { + total_list.push_back(successor_name); + successor->get_successors_and_predecessors(total_list, successor_name); + } + } + } +}; /// Controller Specification /** * This struct contains both a pointer to a given controller, \ref c, as well @@ -68,75 +195,177 @@ class ControllerChainDependencyGraph public: void add_dependency(const std::string & predecessor, const std::string & successor) { - if (predecessors.count(predecessor) == 0) + if(controller_graph_.count(predecessor) == 0) { - predecessors[predecessor] = {}; + controller_graph_[predecessor] = ControllerPeerInfo(); + controller_graph_[predecessor].name = predecessor; } - if (successors.count(successor) == 0) + if(controller_graph_.count(successor) == 0) { - successors[successor] = {}; + controller_graph_[successor] = ControllerPeerInfo(); + controller_graph_[successor].name = successor; } - - ros2_control::add_item(predecessors[successor], predecessor); - ros2_control::add_item(successors[predecessor], successor); + controller_graph_[predecessor].successors.push_back(&controller_graph_[successor]); + controller_graph_[successor].predecessors.push_back(&controller_graph_[predecessor]); } - void remove_controller(const std::string & controller_name) + std::vector get_dependencies_to_activate( + const std::string & controller_name) { - predecessors.erase(controller_name); - successors.erase(controller_name); - for (auto & [_, succ] : predecessors) - { - succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end()); - } - for (auto & [_, preds] : successors) + if (controller_graph_.count(controller_name) == 0) { - preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end()); + return {}; } + return controller_graph_[controller_name].get_controllers_to_activate(); } - void depth_first_search( - const std::string & controller_name, std::unordered_set & visited, - std::unordered_map> & graph) + std::vector get_dependencies_to_deactivate( + const std::string & controller_name) { - if (visited.find(controller_name) != visited.end()) - { - return; - } - visited.insert(controller_name); - for (const auto & neighbor : graph[controller_name]) + if (controller_graph_.count(controller_name) == 0) { - if (visited.find(neighbor) == visited.end()) - { - depth_first_search(neighbor, visited, graph); - } + return {}; } - } - - std::vector get_all_predecessors(const std::string & controller_name) - { - std::unordered_set visited; - depth_first_search(controller_name, visited, predecessors); - std::vector predecessors_list; - std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list)); - predecessors_list.pop_back(); // Remove the controller itself - return predecessors_list; - } - - std::vector get_all_successors(const std::string & controller_name) - { - std::unordered_set visited; - depth_first_search(controller_name, visited, successors); - std::vector successors_list; - std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list)); - successors_list.pop_back(); // Remove the controller itself - return successors_list; + return controller_graph_[controller_name].get_controllers_to_deactivate(); } private: - std::unordered_map> predecessors = {}; - std::unordered_map> successors = {}; + std::unordered_map controller_graph_; }; + +// class ControllerChainDependencyGraph +// { +// public: +// void add_dependency(const std::string & predecessor, const std::string & successor) +// { +// if (predecessors.count(predecessor) == 0) +// { +// predecessors[predecessor] = {}; +// } +// if (successors.count(successor) == 0) +// { +// successors[successor] = {}; +// } + +// ros2_control::add_item(predecessors[successor], predecessor); +// ros2_control::add_item(successors[predecessor], successor); +// } + +// void remove_controller(const std::string & controller_name) +// { +// predecessors.erase(controller_name); +// successors.erase(controller_name); +// for (auto & [_, succ] : predecessors) +// { +// succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end()); +// } +// for (auto & [_, preds] : successors) +// { +// preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end()); +// } +// } + +// void depth_first_search( +// const std::string & controller_name, std::unordered_set & visited, +// std::unordered_map> & graph, const std::string & untill_node = "") +// { +// if (visited.find(controller_name) != visited.end()) +// { +// return; +// } +// if(!untill_node.empty() && controller_name == untill_node) +// { +// return; +// } +// visited.insert(controller_name); +// for (const auto & neighbor : graph[controller_name]) +// { +// if (visited.find(neighbor) == visited.end()) +// { +// depth_first_search(neighbor, visited, graph); +// } +// } +// } + +// std::vector get_all_predecessors(const std::string & controller_name, const std::string & untill_node = "") +// { +// std::unordered_set visited; +// depth_first_search(controller_name, visited, predecessors, untill_node); +// std::vector predecessors_list; +// std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list)); +// ros2_control::remove_item(predecessors_list, controller_name); +// return predecessors_list; +// } + +// std::vector get_all_successors(const std::string & controller_name, const std::string & untill_node = "") +// { +// std::unordered_set visited; +// depth_first_search(controller_name, visited, successors, untill_node); +// std::vector successors_list; +// std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list)); +// ros2_control::remove_item(successors_list, controller_name); +// return successors_list; +// } + +// std::vector get_dependents_to_activate(const std::string & controller_name) +// { +// std::unordered_set visited; +// depth_first_search(controller_name, visited, successors); +// // now for every visited controller look for all it's predecessors and their predecessors and +// // add to the dependents list +// std::vector dependents_to_activate; +// std::copy(visited.begin(), visited.end(), std::back_inserter(dependents_to_activate)); +// ros2_control::remove_item(dependents_to_activate, controller_name); +// for (const auto & controller : visited) +// { +// std::vector predecessors_list = get_all_predecessors(controller, controller_name); +// ros2_control::remove_item(predecessors_list, controller_name); + +// for(const auto & predecessor : predecessors_list) +// { +// RCLCPP_INFO( +// rclcpp::get_logger("controller_manager"), +// "Predecessor of %s is %s", controller_name.c_str(), predecessor.c_str()); +// std::vector successors_of_predecessor = get_all_successors(predecessor, controller_name); +// ros2_control::remove_item(successors_of_predecessor, predecessor); + +// for (const auto & succ_pred : successors_of_predecessor) +// { +// RCLCPP_INFO( +// rclcpp::get_logger("controller_manager"), +// "Successor of predecessor %s is %s", predecessor.c_str(), succ_pred.c_str()); +// } +// // insert if not already in the list +// std::copy_if( +// successors_of_predecessor.begin(), successors_of_predecessor.end(), +// std::back_inserter(dependents_to_activate), +// [&dependents_to_activate](const std::string & succ_pred) +// { +// return std::find( +// dependents_to_activate.begin(), dependents_to_activate.end(), succ_pred) == +// dependents_to_activate.end(); +// }); +// } +// // insert if not already in the list +// std::copy_if( +// predecessors_list.begin(), predecessors_list.end(), +// std::back_inserter(dependents_to_activate), +// [&dependents_to_activate](const std::string & predecessor) +// { +// return std::find( +// dependents_to_activate.begin(), dependents_to_activate.end(), predecessor) == +// dependents_to_activate.end(); +// }); +// } +// ros2_control::remove_item(dependents_to_activate, controller_name); +// return dependents_to_activate; +// } + +// private: +// std::unordered_map> predecessors = {}; +// std::unordered_map> successors = {}; +// }; + } // namespace controller_manager #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index d6873d2e96..e2f0bd4efc 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -77,7 +77,7 @@ TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) { { controller_manager::ControllerChainDependencyGraph graph; - // Let's test the case of A -> B -> C -> D && B -> E && E -> F + // Let's test the case of A -> B -> C -> D && B -> E -> F graph.add_dependency("A", "B"); graph.add_dependency("B", "C"); graph.add_dependency("C", "D"); @@ -85,19 +85,46 @@ TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) graph.add_dependency("E", "F"); EXPECT_THAT( - graph.get_all_successors("A"), testing::UnorderedElementsAre("B", "C", "D", "E", "F")); - EXPECT_THAT(graph.get_all_successors("B"), testing::UnorderedElementsAre("C", "D", "E", "F")); - EXPECT_THAT(graph.get_all_successors("C"), testing::UnorderedElementsAre("D")); - EXPECT_THAT(graph.get_all_successors("D"), testing::UnorderedElementsAre()); - EXPECT_THAT(graph.get_all_successors("E"), testing::UnorderedElementsAre("F")); - EXPECT_THAT(graph.get_all_successors("F"), testing::UnorderedElementsAre()); - - EXPECT_THAT(graph.get_all_predecessors("A"), testing::UnorderedElementsAre()); - EXPECT_THAT(graph.get_all_predecessors("B"), testing::UnorderedElementsAre("A")); - EXPECT_THAT(graph.get_all_predecessors("C"), testing::UnorderedElementsAre("B", "A")); - EXPECT_THAT(graph.get_all_predecessors("D"), testing::UnorderedElementsAre("C", "B", "A")); - EXPECT_THAT(graph.get_all_predecessors("E"), testing::UnorderedElementsAre("B", "A")); - EXPECT_THAT(graph.get_all_predecessors("F"), testing::UnorderedElementsAre("E", "B", "A")); + graph.get_dependencies_to_activate("A"), testing::UnorderedElementsAre("B", "C", "D", "E", "F")); + EXPECT_THAT(graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("C", "D", "E", "F")); + EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("D")); + EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_dependencies_to_activate("E"), testing::UnorderedElementsAre("F")); + EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre()); + + EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("B", "A")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("B", "A")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", "B", "A")); + } + + { + controller_manager::ControllerChainDependencyGraph graph; + // Let's test the case of A -> B -> C -> D && E -> B && E -> F + 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("B", "C", "D", "E", "F")); + EXPECT_THAT(graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("C", "D")); + EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("D")); + EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_dependencies_to_activate("E"), testing::UnorderedElementsAre("F", "B", "C", "D", "A")); + EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre()); + + EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", "E")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("B", "A", "E")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A", "E")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre()); + EXPECT_THAT(graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", "B", "A")); } } From 7359136493ab644636da7b2cbe4da90bc6384f81 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 23 May 2025 00:00:42 +0200 Subject: [PATCH 04/29] Add Shared and WeakPtr attributed to controller interface base --- .../controller_interface/controller_interface_base.hpp | 4 ++++ 1 file changed, 4 insertions(+) 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; From f03e5dccbaa61cba85df3e255ee045ef6191f0aa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 23 May 2025 02:10:59 +0200 Subject: [PATCH 05/29] first decent version --- .../controller_manager/controller_spec.hpp | 380 +++++++++++++----- .../test/test_controller_manager.cpp | 180 +++++++-- 2 files changed, 421 insertions(+), 139 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 341d154519..06fa17e126 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -41,126 +41,278 @@ 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 get_controllers_to_activate() const + void get_controllers_to_activate(std::vector & controllers_to_activate) const { - std::vector controllers_to_activate; - for (const auto & successor : successors) - { - ros2_control::add_item(controllers_to_activate, successor->name); - successor->get_successors_and_predecessors(controllers_to_activate, name); - } - ros2_control::remove_item(controllers_to_activate, name); - return controllers_to_activate; - } + // 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_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The predecessor: " << predecessor->name << " is already in the active list."); + ros2_control::add_item(predecessor_in_active_list, predecessor->name); - std::vector get_controllers_to_deactivate() const - { - std::vector controllers_to_deactivate; - for (const auto & predecessor : predecessors) - { - ros2_control::add_item(controllers_to_deactivate, predecessor->name); - predecessor->get_predecessors(controllers_to_deactivate, name); - predecessor->get_successors(controllers_to_deactivate, name); - } - ros2_control::remove_item(controllers_to_deactivate, name); - return controllers_to_deactivate; - } + // 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; + } + }); - void get_predecessors(std::vector &total_list, const std::string &untill_controller) const - { - for (const auto & predecessor : predecessors) + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The predecessor command interfaces of the predecessor:" + << name << " are: " << predecessor_command_interfaces_set.size()); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The reference interfaces of the controller:" << name + << " are: " << reference_interfaces.size()); + if ( + !predecessor_in_active_list.empty() && + (predecessor_command_interfaces_set.size() != reference_interfaces.size())) { - if(!predecessor) - { - continue; - } - if(predecessor->name == untill_controller) + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The predecessor command interfaces of the predecessor:" + << name << " are not equal to the reference interfaces of the controller:" << name + << " : " << predecessor_command_interfaces_set.size() + << " != " << reference_interfaces.size()); + for (const auto & predecessor : predecessors) { - RCLCPP_INFO( - rclcpp::get_logger("controller_manager"), "skipping predecessor: %s - %s", - predecessor->name.c_str(), untill_controller.c_str()); - continue; + 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); + } } - const std::string predecessor_name = predecessor->name; - if(!ros2_control::has_item(total_list, predecessor_name)) + } + for (const auto & predecessor : predecessors) + { + for (const auto & state_itf : state_interfaces) { - RCLCPP_INFO( - rclcpp::get_logger("controller_manager"), - "Getting Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), untill_controller.c_str()); - total_list.push_back(predecessor_name); - predecessor->get_predecessors(total_list, predecessor_name); + if (state_itf.find(predecessor->name) != std::string::npos) + { + ros2_control::add_item(controllers_to_activate, predecessor->name); + break; + } } } - } - void get_successors(std::vector &total_list, const std::string &untill_controller) const - { + std::unordered_set command_interfaces_set( + command_interfaces.begin(), command_interfaces.end()); + size_t successors_reference_interfaces_count = 0; for (const auto & successor : successors) { - if(!successor) + 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_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The command interfaces of the predecessor:" << name + << " are: " << command_interfaces_set.size()); + for (const auto & command_itf : command_interfaces_set) { - continue; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The command interfaces of the predecessor:" << name << " are: " << command_itf); } - if(successor->name == untill_controller) + + for (const auto & reference_itf : successor->reference_interfaces) { - RCLCPP_INFO( - rclcpp::get_logger("controller_manager"), "skipping successor: %s - %s", - successor->name.c_str(), untill_controller.c_str()); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The reference interfaces of the successor:" << successor->name + << " are: " << reference_itf); + } + + 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_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The reference interfaces of the successor: " + << successor->name << " are within the command interfaces of the predecessor: " << name + << " : " << std::boolalpha << all_successor_interfaces_match); + if (all_successor_interfaces_match) + { + ros2_control::add_item(controllers_to_activate, successor->name); + successor->get_controllers_to_activate(controllers_to_activate); continue; } - const std::string successor_name = successor->name; - if(!ros2_control::has_item(total_list, successor_name)) + else { - RCLCPP_INFO( + RCLCPP_ERROR( rclcpp::get_logger("controller_manager"), - "Getting Successor: %s, Predecessor: %s - %s", successor_name.c_str(), name.c_str(), untill_controller.c_str()); - total_list.push_back(successor_name); - successor->get_successors(total_list, successor_name); + "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_successors_and_predecessors(std::vector &total_list, const std::string &untill_controller) const + 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(!predecessor) - { - continue; - } - if(predecessor->name == untill_controller) - { - continue; - } - const std::string predecessor_name = predecessor->name; - if(!ros2_control::has_item(total_list, predecessor_name)) - { - RCLCPP_INFO( - rclcpp::get_logger("controller_manager"), - "Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), untill_controller.c_str()); - total_list.push_back(predecessor_name); - predecessor->get_successors_and_predecessors(total_list, predecessor_name); - } + 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(!successor) + if (successor->command_interfaces.empty()) { - continue; - } - if(successor->name == untill_controller) - { - continue; - } - const std::string successor_name = successor->name; - if(!ros2_control::has_item(total_list, successor_name)) - { - total_list.push_back(successor_name); - successor->get_successors_and_predecessors(total_list, successor_name); + ros2_control::add_item(controllers_to_deactivate, successor->name); + successor->get_controllers_to_deactivate(controllers_to_deactivate); } } } + + // void get_predecessors(std::vector &total_list, const std::string + // &untill_controller) const + // { + // for (const auto & predecessor : predecessors) + // { + // if(!predecessor) + // { + // continue; + // } + // if(predecessor->name == untill_controller) + // { + // RCLCPP_INFO( + // rclcpp::get_logger("controller_manager"), "skipping predecessor: %s - %s", + // predecessor->name.c_str(), untill_controller.c_str()); + // continue; + // } + // const std::string predecessor_name = predecessor->name; + // if(!ros2_control::has_item(total_list, predecessor_name)) + // { + // RCLCPP_INFO( + // rclcpp::get_logger("controller_manager"), + // "Getting Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), + // untill_controller.c_str()); + // total_list.push_back(predecessor_name); + // predecessor->get_predecessors(total_list, predecessor_name); + // } + // } + // } + + // void get_successors(std::vector &total_list, const std::string &untill_controller) + // const + // { + // for (const auto & successor : successors) + // { + // if(!successor) + // { + // continue; + // } + // if(successor->name == untill_controller) + // { + // RCLCPP_INFO( + // rclcpp::get_logger("controller_manager"), "skipping successor: %s - %s", + // successor->name.c_str(), untill_controller.c_str()); + // continue; + // } + // const std::string successor_name = successor->name; + // if(!ros2_control::has_item(total_list, successor_name)) + // { + // RCLCPP_INFO( + // rclcpp::get_logger("controller_manager"), + // "Getting Successor: %s, Predecessor: %s - %s", successor_name.c_str(), name.c_str(), + // untill_controller.c_str()); + // total_list.push_back(successor_name); + // successor->get_successors(total_list, successor_name); + // } + // } + // } + + // void get_successors_and_predecessors(std::vector &total_list, const std::string + // &untill_controller) const + // { + // for (const auto & predecessor : predecessors) + // { + // if(!predecessor) + // { + // continue; + // } + // if(predecessor->name == untill_controller) + // { + // continue; + // } + // const std::string predecessor_name = predecessor->name; + // if(!ros2_control::has_item(total_list, predecessor_name)) + // { + // RCLCPP_INFO( + // rclcpp::get_logger("controller_manager"), + // "Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), + // untill_controller.c_str()); + // total_list.push_back(predecessor_name); + // predecessor->get_successors_and_predecessors(total_list, predecessor_name); + // } + // } + // for (const auto & successor : successors) + // { + // if(!successor) + // { + // continue; + // } + // if(successor->name == untill_controller) + // { + // continue; + // } + // const std::string successor_name = successor->name; + // if(!ros2_control::has_item(total_list, successor_name)) + // { + // total_list.push_back(successor_name); + // successor->get_successors_and_predecessors(total_list, successor_name); + // } + // } + // } }; /// Controller Specification /** @@ -195,12 +347,12 @@ class ControllerChainDependencyGraph public: void add_dependency(const std::string & predecessor, const std::string & successor) { - if(controller_graph_.count(predecessor) == 0) + if (controller_graph_.count(predecessor) == 0) { controller_graph_[predecessor] = ControllerPeerInfo(); controller_graph_[predecessor].name = predecessor; } - if(controller_graph_.count(successor) == 0) + if (controller_graph_.count(successor) == 0) { controller_graph_[successor] = ControllerPeerInfo(); controller_graph_[successor].name = successor; @@ -209,31 +361,50 @@ class ControllerChainDependencyGraph controller_graph_[successor].predecessors.push_back(&controller_graph_[predecessor]); } - std::vector get_dependencies_to_activate( - const std::string & controller_name) + 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; + } + controller_graph_[predecessor.name].successors.push_back(&controller_graph_[successor.name]); + controller_graph_[successor.name].predecessors.push_back(&controller_graph_[predecessor.name]); + } + + std::vector get_dependencies_to_activate(const std::string & controller_name) { + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "+++++++++++++++++++++++++++++++ Getting dependencies to ACTIVATE " + "+++++++++++++++++++++++++++++++"); + std::vector controllers_to_activate({controller_name}); if (controller_graph_.count(controller_name) == 0) { return {}; } - return controller_graph_[controller_name].get_controllers_to_activate(); + 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) + std::vector get_dependencies_to_deactivate(const std::string & controller_name) { + std::vector controllers_to_deactivate({controller_name}); if (controller_graph_.count(controller_name) == 0) { return {}; } - return controller_graph_[controller_name].get_controllers_to_deactivate(); + controller_graph_[controller_name].get_controllers_to_deactivate(controllers_to_deactivate); + return controllers_to_deactivate; } private: std::unordered_map controller_graph_; }; - // class ControllerChainDependencyGraph // { // public: @@ -268,7 +439,8 @@ class ControllerChainDependencyGraph // void depth_first_search( // const std::string & controller_name, std::unordered_set & visited, -// std::unordered_map> & graph, const std::string & untill_node = "") +// std::unordered_map> & graph, const std::string & +// untill_node = "") // { // if (visited.find(controller_name) != visited.end()) // { @@ -288,7 +460,8 @@ class ControllerChainDependencyGraph // } // } -// std::vector get_all_predecessors(const std::string & controller_name, const std::string & untill_node = "") +// std::vector get_all_predecessors(const std::string & controller_name, const +// std::string & untill_node = "") // { // std::unordered_set visited; // depth_first_search(controller_name, visited, predecessors, untill_node); @@ -298,7 +471,8 @@ class ControllerChainDependencyGraph // return predecessors_list; // } -// std::vector get_all_successors(const std::string & controller_name, const std::string & untill_node = "") +// std::vector get_all_successors(const std::string & controller_name, const +// std::string & untill_node = "") // { // std::unordered_set visited; // depth_first_search(controller_name, visited, successors, untill_node); @@ -319,16 +493,16 @@ class ControllerChainDependencyGraph // ros2_control::remove_item(dependents_to_activate, controller_name); // for (const auto & controller : visited) // { -// std::vector predecessors_list = get_all_predecessors(controller, controller_name); -// ros2_control::remove_item(predecessors_list, controller_name); - +// std::vector predecessors_list = get_all_predecessors(controller, +// controller_name); ros2_control::remove_item(predecessors_list, controller_name); + // for(const auto & predecessor : predecessors_list) // { // RCLCPP_INFO( // rclcpp::get_logger("controller_manager"), // "Predecessor of %s is %s", controller_name.c_str(), predecessor.c_str()); -// std::vector successors_of_predecessor = get_all_successors(predecessor, controller_name); -// ros2_control::remove_item(successors_of_predecessor, predecessor); +// std::vector successors_of_predecessor = get_all_successors(predecessor, +// controller_name); ros2_control::remove_item(successors_of_predecessor, predecessor); // for (const auto & succ_pred : successors_of_predecessor) // { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index e2f0bd4efc..a6edee851d 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -78,54 +78,162 @@ TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) { controller_manager::ControllerChainDependencyGraph graph; // Let's test the case of A -> B -> C -> D && B -> E -> F - 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"); + 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("B", "C", "D", "E", "F")); - EXPECT_THAT(graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("C", "D", "E", "F")); - EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("D")); - EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre()); - EXPECT_THAT(graph.get_dependencies_to_activate("E"), testing::UnorderedElementsAre("F")); - EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre()); + 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()); - EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A")); - EXPECT_THAT(graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("B", "A")); - EXPECT_THAT(graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A")); - EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("B", "A")); - EXPECT_THAT(graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", "B", "A")); + 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 - 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"); + 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("B", "C", "D", "E", "F")); - EXPECT_THAT(graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("C", "D")); - EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("D")); - EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre()); - EXPECT_THAT(graph.get_dependencies_to_activate("E"), testing::UnorderedElementsAre("F", "B", "C", "D", "A")); - EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre()); - - EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre()); - EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", "E")); - EXPECT_THAT(graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("B", "A", "E")); + testing::UnorderedElementsAre("A", "B", "C", "D", "E", "F")); EXPECT_THAT( - graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A", "E")); - EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre()); - EXPECT_THAT(graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", "B", "A")); - } + 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")); + EXPECT_THAT( + graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("F", "E", "B", "A")); + } + + // { + // controller_manager::ControllerChainDependencyGraph graph; + // // Let's test the case of A -> B -> C -> D && E -> B && E -> F + // 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("B", "C", "D", "E", "F")); + // EXPECT_THAT(graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("C", + // "D")); EXPECT_THAT(graph.get_dependencies_to_activate("C"), + // testing::UnorderedElementsAre("D")); EXPECT_THAT(graph.get_dependencies_to_activate("D"), + // testing::UnorderedElementsAre()); EXPECT_THAT(graph.get_dependencies_to_activate("E"), + // testing::UnorderedElementsAre("F", "B", "C", "D", "A")); + // EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre()); + + // EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre()); + // EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", + // "E")); EXPECT_THAT(graph.get_dependencies_to_deactivate("C"), + // testing::UnorderedElementsAre("B", "A", "E")); EXPECT_THAT( + // graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A", + // "E")); + // EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre()); + // EXPECT_THAT(graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", + // "B", "A")); + // } } class TestControllerManagerRobotDescription From a0ffcff7c9abef642f9ca78204ff462bfa0879dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 25 May 2025 08:55:08 +0200 Subject: [PATCH 06/29] Add a utility method to check if 2 containers have common items or not --- .../include/hardware_interface/helpers.hpp | 25 ++++++++++++++++--- hardware_interface/test/test_helpers.cpp | 24 ++++++++++++++++++ 2 files changed, 46 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index 605ecb9216..3e91d8b1e1 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."); } } @@ -175,6 +178,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..d9afc667c0 100644 --- a/hardware_interface/test/test_helpers.cpp +++ b/hardware_interface/test/test_helpers.cpp @@ -54,4 +54,28 @@ 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)); } From 75375deec918205e6bde92a2f8360f079572de5f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 25 May 2025 21:57:44 +0200 Subject: [PATCH 07/29] remove old code --- .../controller_manager/controller_spec.hpp | 101 ------------------ 1 file changed, 101 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 06fa17e126..a2d49f8a6d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -212,107 +212,6 @@ struct ControllerPeerInfo } } } - - // void get_predecessors(std::vector &total_list, const std::string - // &untill_controller) const - // { - // for (const auto & predecessor : predecessors) - // { - // if(!predecessor) - // { - // continue; - // } - // if(predecessor->name == untill_controller) - // { - // RCLCPP_INFO( - // rclcpp::get_logger("controller_manager"), "skipping predecessor: %s - %s", - // predecessor->name.c_str(), untill_controller.c_str()); - // continue; - // } - // const std::string predecessor_name = predecessor->name; - // if(!ros2_control::has_item(total_list, predecessor_name)) - // { - // RCLCPP_INFO( - // rclcpp::get_logger("controller_manager"), - // "Getting Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), - // untill_controller.c_str()); - // total_list.push_back(predecessor_name); - // predecessor->get_predecessors(total_list, predecessor_name); - // } - // } - // } - - // void get_successors(std::vector &total_list, const std::string &untill_controller) - // const - // { - // for (const auto & successor : successors) - // { - // if(!successor) - // { - // continue; - // } - // if(successor->name == untill_controller) - // { - // RCLCPP_INFO( - // rclcpp::get_logger("controller_manager"), "skipping successor: %s - %s", - // successor->name.c_str(), untill_controller.c_str()); - // continue; - // } - // const std::string successor_name = successor->name; - // if(!ros2_control::has_item(total_list, successor_name)) - // { - // RCLCPP_INFO( - // rclcpp::get_logger("controller_manager"), - // "Getting Successor: %s, Predecessor: %s - %s", successor_name.c_str(), name.c_str(), - // untill_controller.c_str()); - // total_list.push_back(successor_name); - // successor->get_successors(total_list, successor_name); - // } - // } - // } - - // void get_successors_and_predecessors(std::vector &total_list, const std::string - // &untill_controller) const - // { - // for (const auto & predecessor : predecessors) - // { - // if(!predecessor) - // { - // continue; - // } - // if(predecessor->name == untill_controller) - // { - // continue; - // } - // const std::string predecessor_name = predecessor->name; - // if(!ros2_control::has_item(total_list, predecessor_name)) - // { - // RCLCPP_INFO( - // rclcpp::get_logger("controller_manager"), - // "Predecessor: %s, Successor: %s - %s", predecessor_name.c_str(), name.c_str(), - // untill_controller.c_str()); - // total_list.push_back(predecessor_name); - // predecessor->get_successors_and_predecessors(total_list, predecessor_name); - // } - // } - // for (const auto & successor : successors) - // { - // if(!successor) - // { - // continue; - // } - // if(successor->name == untill_controller) - // { - // continue; - // } - // const std::string successor_name = successor->name; - // if(!ros2_control::has_item(total_list, successor_name)) - // { - // total_list.push_back(successor_name); - // successor->get_successors_and_predecessors(total_list, successor_name); - // } - // } - // } }; /// Controller Specification /** From 4ec10ced3045922435752f1271517d21017c223f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 25 May 2025 22:13:17 +0200 Subject: [PATCH 08/29] Generate mutually exclusive predecessor groups --- .../controller_manager/controller_spec.hpp | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index a2d49f8a6d..7d15466e45 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -19,6 +19,8 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +#include +#include #include #include #include @@ -45,6 +47,137 @@ struct ControllerPeerInfo std::unordered_set command_interfaces = {}; std::unordered_set state_interfaces = {}; std::unordered_set reference_interfaces = {}; + std::vector> mutually_exclusive_predecessor_groups = {}; + + void build_mutually_exclusive_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_INFO_STREAM( + rclcpp::get_logger("controller_manager"), + "Adding predecessor: " + << p->name + << " as individual group, as all its command " + "interfaces are in the current controller's reference interfaces."); + } + }); + + // 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 + } + + current_combination.push_back(predecessors[i]->name); + generate_combinations(i + 1); + current_combination.pop_back(); + } + }; + + RCLCPP_INFO( + rclcpp::get_logger("controller_manager"), + "Generating combinations of predecessors for controller: %s", 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_INFO( + 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 get_controllers_to_activate(std::vector & controllers_to_activate) const { From 584123253ddeaab2f89cd705a62b1b2cf1808fb2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 25 May 2025 22:13:42 +0200 Subject: [PATCH 09/29] Use the `build_mutually_exclusive_groups` method --- .../include/controller_manager/controller_spec.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 7d15466e45..f5b348585d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -418,17 +418,23 @@ class ControllerChainDependencyGraph { return {}; } + controller_graph_[controller_name].build_mutually_exclusive_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_INFO( + 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_groups(); controller_graph_[controller_name].get_controllers_to_deactivate(controllers_to_deactivate); return controllers_to_deactivate; } From 3515c3e5610c36553f3d666488a4c21961c81532 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 May 2025 22:37:58 +0200 Subject: [PATCH 10/29] Rename to build_mutually_exclusive_predecessor_groups --- .../include/controller_manager/controller_spec.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index f5b348585d..659b7fa6c1 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -49,7 +49,7 @@ struct ControllerPeerInfo std::unordered_set reference_interfaces = {}; std::vector> mutually_exclusive_predecessor_groups = {}; - void build_mutually_exclusive_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 @@ -418,7 +418,7 @@ class ControllerChainDependencyGraph { return {}; } - controller_graph_[controller_name].build_mutually_exclusive_groups(); + controller_graph_[controller_name].build_mutually_exclusive_predecessor_groups(); controller_graph_[controller_name].get_controllers_to_activate(controllers_to_activate); return controllers_to_activate; } @@ -434,7 +434,7 @@ class ControllerChainDependencyGraph { return {}; } - controller_graph_[controller_name].build_mutually_exclusive_groups(); + controller_graph_[controller_name].build_mutually_exclusive_predecessor_groups(); controller_graph_[controller_name].get_controllers_to_deactivate(controllers_to_deactivate); return controllers_to_deactivate; } From 1335bdb2f7eee4e15fac0a35b793f4591eedc4a4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 May 2025 22:38:58 +0200 Subject: [PATCH 11/29] Add build_mutually_exclusive_successor_groups method --- .../controller_manager/controller_spec.hpp | 125 ++++++++++++++++++ 1 file changed, 125 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 659b7fa6c1..7833408ac4 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -48,6 +48,7 @@ struct ControllerPeerInfo 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() { @@ -179,6 +180,128 @@ struct ControllerPeerInfo } } + 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_INFO_STREAM( + rclcpp::get_logger("controller_manager"), + "Adding successor: " + << s->name + << " as individual group, as all its command " + "interfaces are in the current controller's reference interfaces."); + } + }); + + // 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 + } + + current_combination.push_back(successors[i]->name); + generate_combinations(i + 1); + current_combination.pop_back(); + } + }; + RCLCPP_INFO( + 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_INFO( + 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); + } + } + } + } + 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 @@ -419,6 +542,7 @@ class ControllerChainDependencyGraph 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; } @@ -435,6 +559,7 @@ class ControllerChainDependencyGraph 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; } From 1b216bc064ea06673551f812a3fb4684a56269f4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 1 Jun 2025 11:41:33 +0200 Subject: [PATCH 12/29] Fix the test expectations --- controller_manager/test/test_controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a6edee851d..4c3e829538 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -200,9 +200,9 @@ TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) 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")); + EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("E", "A")); EXPECT_THAT( - graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("F", "E", "B", "A")); + graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("F", "E", "A")); } // { From c935d0393e4cdfb4152b41d6ce02b1ca010f3b50 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 1 Jun 2025 11:43:25 +0200 Subject: [PATCH 13/29] add first complete working version --- .../controller_manager/controller_spec.hpp | 46 ++++++++++++++++++- 1 file changed, 45 insertions(+), 1 deletion(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 7833408ac4..297ffc91cd 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -445,6 +445,13 @@ struct ControllerPeerInfo // 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_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The predecessor: " << predecessor->name << " is already in the deactivation list."); + continue; + } ros2_control::add_item(controllers_to_deactivate, predecessor->name); std::for_each( state_interfaces.begin(), state_interfaces.end(), @@ -461,11 +468,48 @@ struct ControllerPeerInfo // 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_ERROR_STREAM( + rclcpp::get_logger("controller_manager"), + "The successor: " << successor->name << " is already in the deactivation list."); + continue; + } + RCLCPP_INFO( + 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_INFO_STREAM( + rclcpp::get_logger("controller_manager"), + "The successor: " << successor->name + << " is in a mutually exclusive group, skipping further deactivation."); + continue; + } + if (successor->command_interfaces.empty()) { ros2_control::add_item(controllers_to_deactivate, successor->name); - successor->get_controllers_to_deactivate(controllers_to_deactivate); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("controller_manager"), + "Adding successor: " << successor->name + << " to the deactivation list, as it has no command interfaces."); + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("controller_manager"), + "Controller %s has a successor %s who has command interfaces. This is not supported now.", + name.c_str(), successor->name.c_str()); } + successor->get_controllers_to_deactivate(controllers_to_deactivate); } } }; From 4b8ff0cc2efce986a0ec6b2b1079175d5bde2dbb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 1 Jun 2025 11:49:38 +0200 Subject: [PATCH 14/29] Remove unused code --- .../controller_manager/controller_spec.hpp | 136 ------------------ 1 file changed, 136 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 297ffc91cd..104c8fa0a9 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -612,141 +612,5 @@ class ControllerChainDependencyGraph std::unordered_map controller_graph_; }; -// class ControllerChainDependencyGraph -// { -// public: -// void add_dependency(const std::string & predecessor, const std::string & successor) -// { -// if (predecessors.count(predecessor) == 0) -// { -// predecessors[predecessor] = {}; -// } -// if (successors.count(successor) == 0) -// { -// successors[successor] = {}; -// } - -// ros2_control::add_item(predecessors[successor], predecessor); -// ros2_control::add_item(successors[predecessor], successor); -// } - -// void remove_controller(const std::string & controller_name) -// { -// predecessors.erase(controller_name); -// successors.erase(controller_name); -// for (auto & [_, succ] : predecessors) -// { -// succ.erase(std::remove(succ.begin(), succ.end(), controller_name), succ.end()); -// } -// for (auto & [_, preds] : successors) -// { -// preds.erase(std::remove(preds.begin(), preds.end(), controller_name), preds.end()); -// } -// } - -// void depth_first_search( -// const std::string & controller_name, std::unordered_set & visited, -// std::unordered_map> & graph, const std::string & -// untill_node = "") -// { -// if (visited.find(controller_name) != visited.end()) -// { -// return; -// } -// if(!untill_node.empty() && controller_name == untill_node) -// { -// return; -// } -// visited.insert(controller_name); -// for (const auto & neighbor : graph[controller_name]) -// { -// if (visited.find(neighbor) == visited.end()) -// { -// depth_first_search(neighbor, visited, graph); -// } -// } -// } - -// std::vector get_all_predecessors(const std::string & controller_name, const -// std::string & untill_node = "") -// { -// std::unordered_set visited; -// depth_first_search(controller_name, visited, predecessors, untill_node); -// std::vector predecessors_list; -// std::copy(visited.begin(), visited.end(), std::back_inserter(predecessors_list)); -// ros2_control::remove_item(predecessors_list, controller_name); -// return predecessors_list; -// } - -// std::vector get_all_successors(const std::string & controller_name, const -// std::string & untill_node = "") -// { -// std::unordered_set visited; -// depth_first_search(controller_name, visited, successors, untill_node); -// std::vector successors_list; -// std::copy(visited.begin(), visited.end(), std::back_inserter(successors_list)); -// ros2_control::remove_item(successors_list, controller_name); -// return successors_list; -// } - -// std::vector get_dependents_to_activate(const std::string & controller_name) -// { -// std::unordered_set visited; -// depth_first_search(controller_name, visited, successors); -// // now for every visited controller look for all it's predecessors and their predecessors and -// // add to the dependents list -// std::vector dependents_to_activate; -// std::copy(visited.begin(), visited.end(), std::back_inserter(dependents_to_activate)); -// ros2_control::remove_item(dependents_to_activate, controller_name); -// for (const auto & controller : visited) -// { -// std::vector predecessors_list = get_all_predecessors(controller, -// controller_name); ros2_control::remove_item(predecessors_list, controller_name); - -// for(const auto & predecessor : predecessors_list) -// { -// RCLCPP_INFO( -// rclcpp::get_logger("controller_manager"), -// "Predecessor of %s is %s", controller_name.c_str(), predecessor.c_str()); -// std::vector successors_of_predecessor = get_all_successors(predecessor, -// controller_name); ros2_control::remove_item(successors_of_predecessor, predecessor); - -// for (const auto & succ_pred : successors_of_predecessor) -// { -// RCLCPP_INFO( -// rclcpp::get_logger("controller_manager"), -// "Successor of predecessor %s is %s", predecessor.c_str(), succ_pred.c_str()); -// } -// // insert if not already in the list -// std::copy_if( -// successors_of_predecessor.begin(), successors_of_predecessor.end(), -// std::back_inserter(dependents_to_activate), -// [&dependents_to_activate](const std::string & succ_pred) -// { -// return std::find( -// dependents_to_activate.begin(), dependents_to_activate.end(), succ_pred) == -// dependents_to_activate.end(); -// }); -// } -// // insert if not already in the list -// std::copy_if( -// predecessors_list.begin(), predecessors_list.end(), -// std::back_inserter(dependents_to_activate), -// [&dependents_to_activate](const std::string & predecessor) -// { -// return std::find( -// dependents_to_activate.begin(), dependents_to_activate.end(), predecessor) == -// dependents_to_activate.end(); -// }); -// } -// ros2_control::remove_item(dependents_to_activate, controller_name); -// return dependents_to_activate; -// } - -// private: -// std::unordered_map> predecessors = {}; -// std::unordered_map> successors = {}; -// }; - } // namespace controller_manager #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ From c55262afdec144879098effabff683fc3a21713a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 1 Jun 2025 14:10:59 +0200 Subject: [PATCH 15/29] Remove more unused code --- .../controller_manager/controller_spec.hpp | 16 ---------------- controller_manager/src/controller_manager.cpp | 2 -- 2 files changed, 18 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 104c8fa0a9..8a08b2eb20 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -544,22 +544,6 @@ struct ControllerChainSpec class ControllerChainDependencyGraph { public: - void add_dependency(const std::string & predecessor, const std::string & successor) - { - if (controller_graph_.count(predecessor) == 0) - { - controller_graph_[predecessor] = ControllerPeerInfo(); - controller_graph_[predecessor].name = predecessor; - } - if (controller_graph_.count(successor) == 0) - { - controller_graph_[successor] = ControllerPeerInfo(); - controller_graph_[successor].name = successor; - } - controller_graph_[predecessor].successors.push_back(&controller_graph_[successor]); - controller_graph_[successor].predecessors.push_back(&controller_graph_[predecessor]); - } - void add_dependency(const ControllerPeerInfo & predecessor, const ControllerPeerInfo & successor) { if (controller_graph_.count(predecessor.name) == 0) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 29fc7ec231..d7b482246e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1341,7 +1341,6 @@ controller_interface::return_type ControllerManager::configure_controller( controller_manager::ControllersListIterator ctrl_it; if (is_interface_a_chained_interface(cmd_itf, controllers, ctrl_it)) { - controller_chain_dependency_graph_.add_dependency(controller_name, ctrl_it->info.name); ros2_control::add_item( controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); ros2_control::add_item( @@ -1356,7 +1355,6 @@ controller_interface::return_type ControllerManager::configure_controller( controller_manager::ControllersListIterator ctrl_it; if (is_interface_a_chained_interface(state_itf, controllers, ctrl_it)) { - controller_chain_dependency_graph_.add_dependency(ctrl_it->info.name, controller_name); ros2_control::add_item( controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); ros2_control::add_item( From f1d61041c83c8f3d5bdc9fdb009ac1b76792e749 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Jun 2025 23:30:59 +0200 Subject: [PATCH 16/29] Add new `build_controllers_topology_info` method and move common information --- .../controller_manager/controller_manager.hpp | 8 ++ controller_manager/src/controller_manager.cpp | 103 +++++++++++++----- 2 files changed, 84 insertions(+), 27 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 87aa153dff..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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d7b482246e..8b12ea40e2 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 @@ -4314,6 +4288,81 @@ 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(); }); + 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; + + 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.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_INFO( + 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_INFO_EXPRESSION( + get_logger(), !controller_chain.following_controllers.empty(), + fmt::format( + "\tFollowing controllers are : {}", fmt::join(controller_chain.following_controllers, ", ")) + .c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !controller_chain.preceding_controllers.empty(), + fmt::format( + "\tPreceding controllers are : {}", fmt::join(controller_chain.preceding_controllers, ", ")) + .c_str()); + } +} + rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const ControllerSpec & controller) const { From b13dc1c5e69899ad77d88edab74622403babe88e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 17 Jun 2025 23:33:03 +0200 Subject: [PATCH 17/29] Create ControllerPeerInfo and add the dependency --- controller_manager/src/controller_manager.cpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8b12ea40e2..8ebd51ef18 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -4304,6 +4304,27 @@ void ControllerManager::build_controllers_topology_info( 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)) @@ -4316,11 +4337,15 @@ void ControllerManager::build_controllers_topology_info( 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)) { + 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( From 05cccf408f23c3830e93e43ed6b122dcd201fb92 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 21 Jul 2025 23:30:02 +0200 Subject: [PATCH 18/29] Add some debug prints --- controller_manager/src/controller_manager.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8ebd51ef18..a175fda813 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1492,6 +1492,32 @@ controller_interface::return_type ControllerManager::switch_controller_cb( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } + if (!activate_controllers.empty()) + { + for (const auto & controller : activate_controllers) + { + const auto x = controller_chain_dependency_graph_.get_dependencies_to_activate(controller); + RCLCPP_INFO( + get_logger(), + fmt::format( + "Controller {} has '{}' dependencies to activate", controller, fmt::join(x, ", ")) + .c_str()); + } + } + + if (!deactivate_controllers.empty()) + { + for (const auto & controller : deactivate_controllers) + { + const auto x = controller_chain_dependency_graph_.get_dependencies_to_deactivate(controller); + RCLCPP_INFO( + get_logger(), + fmt::format( + "Controller {} has '{}' dependencies to deactivate", controller, fmt::join(x, ", ")) + .c_str()); + } + } + std::string activate_list, deactivate_list; activate_list.reserve(500); deactivate_list.reserve(500); From a7c08ad8b885a8564b21429c08222a1bfbea1609 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 23 Jul 2025 00:03:46 +0200 Subject: [PATCH 19/29] remove commented tests --- .../test/test_controller_manager.cpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 4c3e829538..a28f8260c7 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -204,36 +204,6 @@ TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph) EXPECT_THAT( graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("F", "E", "A")); } - - // { - // controller_manager::ControllerChainDependencyGraph graph; - // // Let's test the case of A -> B -> C -> D && E -> B && E -> F - // 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("B", "C", "D", "E", "F")); - // EXPECT_THAT(graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("C", - // "D")); EXPECT_THAT(graph.get_dependencies_to_activate("C"), - // testing::UnorderedElementsAre("D")); EXPECT_THAT(graph.get_dependencies_to_activate("D"), - // testing::UnorderedElementsAre()); EXPECT_THAT(graph.get_dependencies_to_activate("E"), - // testing::UnorderedElementsAre("F", "B", "C", "D", "A")); - // EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre()); - - // EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre()); - // EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", - // "E")); EXPECT_THAT(graph.get_dependencies_to_deactivate("C"), - // testing::UnorderedElementsAre("B", "A", "E")); EXPECT_THAT( - // graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A", - // "E")); - // EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre()); - // EXPECT_THAT(graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", - // "B", "A")); - // } } class TestControllerManagerRobotDescription From 5edda8591c9829684c715d8436f7ae13e8eb7670 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 23 Jul 2025 00:04:20 +0200 Subject: [PATCH 20/29] Add more conditions to protect the infinity builds --- .../controller_manager/controller_spec.hpp | 43 +++++++++++++++++-- 1 file changed, 40 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 8a08b2eb20..ea848f0f32 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -152,6 +152,14 @@ struct ControllerPeerInfo 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(); @@ -160,7 +168,8 @@ struct ControllerPeerInfo RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "Generating combinations of predecessors for controller: %s", name.c_str()); + "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) @@ -275,6 +284,13 @@ struct ControllerPeerInfo 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(); @@ -299,6 +315,10 @@ struct ControllerPeerInfo mutually_exclusive_successor_groups.push_back(group); } } + RCLCPP_INFO_STREAM( + rclcpp::get_logger("controller_manager"), + "Mutually exclusive successor groups for controller: " + << name << " are: " << mutually_exclusive_successor_groups.size()); } } @@ -554,8 +574,25 @@ class ControllerChainDependencyGraph { controller_graph_[successor.name] = successor; } - controller_graph_[predecessor.name].successors.push_back(&controller_graph_[successor.name]); - controller_graph_[successor.name].predecessors.push_back(&controller_graph_[predecessor.name]); + 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) From 853a28b90ad10e5341584afda0db2d91187c43db Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 20 Sep 2025 01:33:17 +0200 Subject: [PATCH 21/29] Add the missing check --- controller_manager/src/controller_manager.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a175fda813..92d1ec0630 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -4369,6 +4369,13 @@ void ControllerManager::build_controllers_topology_info( 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); From b60f45dfd97fdc421833d2cf58df786dedee10de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Sep 2025 23:46:53 +0200 Subject: [PATCH 22/29] Add logic to have a functional AUTO mode --- controller_manager/src/controller_manager.cpp | 139 ++++++++++-------- .../include/hardware_interface/helpers.hpp | 9 ++ 2 files changed, 83 insertions(+), 65 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 92d1ec0630..8e37fcbe22 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1467,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( @@ -1491,63 +1483,61 @@ controller_interface::return_type ControllerManager::switch_controller_cb( "Defaulting to BEST_EFFORT"); strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - - if (!activate_controllers.empty()) + else if (strictness > controller_manager_msgs::srv::SwitchController::Request::FORCE_AUTO) { - for (const auto & controller : activate_controllers) - { - const auto x = controller_chain_dependency_graph_.get_dependencies_to_activate(controller); - RCLCPP_INFO( - get_logger(), - fmt::format( - "Controller {} has '{}' dependencies to activate", controller, fmt::join(x, ", ")) - .c_str()); - } - } - - if (!deactivate_controllers.empty()) - { - for (const auto & controller : deactivate_controllers) - { - const auto x = controller_chain_dependency_graph_.get_dependencies_to_deactivate(controller); - RCLCPP_INFO( - get_logger(), - fmt::format( - "Controller {} has '{}' dependencies to deactivate", controller, fmt::join(x, ", ")) - .c_str()); - } + 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; } - 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) - { - deactivate_list.append(controller); - deactivate_list.append(" "); - } - RCLCPP_INFO_EXPRESSION( - get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str()); - RCLCPP_INFO_EXPRESSION( - get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", - deactivate_list.c_str()); + 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")); 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_INFO( + 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 +1557,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 +1598,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb( activate_request_.clear(); return ret; } + + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_request_.empty(), + fmt::format("Activating controllers: [ {} ]", fmt::join(activate_request_, " ")).c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_request_.empty(), + fmt::format("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 +1647,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 +1679,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 +1692,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 +1735,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 +1745,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 +1805,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 +2120,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) { diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index 3e91d8b1e1..10da0f3a85 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -88,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. From 3a68481a5f9336f0167e68b97b2d44d81cba5d5e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Sep 2025 23:47:14 +0200 Subject: [PATCH 23/29] Add some tests to test the logic of the AUTO mode --- .../test/test_controller_manager_srvs.cpp | 168 ++++++++++++++++++ 1 file changed, 168 insertions(+) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 54c34bd55d..17cac20304 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -836,6 +836,117 @@ 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 +1129,63 @@ 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); + + // Now let's activate the top level controller and see if the whole chain is activated + cm_->switch_controller( + {"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO, + true, rclcpp::Duration(0, 0)); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_7->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_6->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_5->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_4->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_3->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); + + // Now deactivate the chained_controller_1 and everything should deactivate + cm_->switch_controller( + {}, {TEST_CHAINED_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::AUTO, + true, rclcpp::Duration(0, 0)); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_7->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_6->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_5->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_4->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_chained_controller_3->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_2->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_chained_controller_1->get_lifecycle_state().id()); } TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) From 71515892ae6c2f564dc2702eaecf5e41d0a38244 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Sep 2025 21:55:50 +0200 Subject: [PATCH 24/29] Add tests to the newly added add_unique_items method --- hardware_interface/test/test_helpers.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/hardware_interface/test/test_helpers.cpp b/hardware_interface/test/test_helpers.cpp index d9afc667c0..abb065092e 100644 --- a/hardware_interface/test/test_helpers.cpp +++ b/hardware_interface/test/test_helpers.cpp @@ -78,4 +78,9 @@ TEST(TestHelper, test_helper_methods) 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")); } From f7255c6a4ae1035453bff27fe0d5cf8aacdeb310 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Sep 2025 22:19:19 +0200 Subject: [PATCH 25/29] Added more tests for the chaining activation --- .../test/test_controller_manager_srvs.cpp | 289 ++++++++++++++---- 1 file changed, 222 insertions(+), 67 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 17cac20304..39e9707dd9 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -854,9 +854,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) }; // 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))); + 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, @@ -866,9 +868,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) 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))); + 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, @@ -878,9 +882,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) 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))); + 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, @@ -891,9 +897,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) // 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))); + 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, @@ -903,9 +911,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) 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))); + 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, @@ -915,9 +925,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) 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))); + 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, @@ -926,9 +938,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) 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))); + 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, @@ -937,9 +951,11 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) 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))); + 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, @@ -1130,62 +1146,201 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) 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 - cm_->switch_controller( - {"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO, - true, rclcpp::Duration(0, 0)); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_7->get_lifecycle_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_6->get_lifecycle_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_5->get_lifecycle_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_4->get_lifecycle_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_3->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_2->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_1->get_lifecycle_state().id()); + 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 everything should deactivate - cm_->switch_controller( - {}, {TEST_CHAINED_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::AUTO, - true, rclcpp::Duration(0, 0)); + // Now deactivate the chained_controller_1 and 3 and everything should deactivate ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_chained_controller_7->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_chained_controller_6->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_chained_controller_5->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_chained_controller_4->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - test_chained_controller_3->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_chained_controller_2->get_lifecycle_state().id()); + 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( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_chained_controller_1->get_lifecycle_state().id()); + 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) From 19d06c074d2b02991a8cc57d10c51fc38874c81d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Sep 2025 22:42:32 +0200 Subject: [PATCH 26/29] move to RCLCPP_INFO in logging --- .../controller_manager/controller_spec.hpp | 101 +++++++++--------- 1 file changed, 50 insertions(+), 51 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index ea848f0f32..124c445ce7 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -13,7 +13,7 @@ // limitations under the License. /* - * Author: Wim Meeussen + * Author: Wim Meeussen, Sai Kishor Kothakota */ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ @@ -82,12 +82,11 @@ struct ControllerPeerInfo // interfaces, add it as individual group predecessor_group.insert(p->name); mutually_exclusive_predecessor_groups.push_back(predecessor_group); - RCLCPP_INFO_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "Adding predecessor: " - << p->name - << " as individual group, as all its command " - "interfaces are in the current controller's reference interfaces."); + "Adding predecessor: '%s' as individual group, as all its command interfaces are in " + "the current controller's reference interfaces.", + p->name.c_str()); } }); @@ -221,12 +220,11 @@ struct ControllerPeerInfo // interfaces, add it as individual group successor_group.insert(s->name); mutually_exclusive_successor_groups.push_back(successor_group); - RCLCPP_INFO_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "Adding successor: " - << s->name - << " as individual group, as all its command " - "interfaces are in the current controller's reference interfaces."); + "Adding successor: '%s' as individual group, as all its command interfaces are in the " + "current controller's reference interfaces.", + s->name.c_str()); } }); @@ -315,10 +313,10 @@ struct ControllerPeerInfo mutually_exclusive_successor_groups.push_back(group); } } - RCLCPP_INFO_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "Mutually exclusive successor groups for controller: " - << name << " are: " << mutually_exclusive_successor_groups.size()); + "Mutually exclusive successor groups for controller: '%s' are: %zu", name.c_str(), + mutually_exclusive_successor_groups.size()); } } @@ -336,9 +334,9 @@ struct ControllerPeerInfo { if (ros2_control::has_item(controllers_to_activate, predecessor->name)) { - RCLCPP_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The predecessor: " << predecessor->name << " is already in the active list."); + "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 @@ -356,24 +354,24 @@ struct ControllerPeerInfo } }); - RCLCPP_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The predecessor command interfaces of the predecessor:" - << name << " are: " << predecessor_command_interfaces_set.size()); - RCLCPP_ERROR_STREAM( + "The predecessor command interfaces of the predecessor: '%s' are: %zu", name.c_str(), + predecessor_command_interfaces_set.size()); + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The reference interfaces of the controller:" << name - << " are: " << reference_interfaces.size()); + "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_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The predecessor command interfaces of the predecessor:" - << name << " are not equal to the reference interfaces of the controller:" << name - << " : " << predecessor_command_interfaces_set.size() - << " != " << reference_interfaces.size()); + "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)) @@ -407,23 +405,24 @@ struct ControllerPeerInfo // 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_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The command interfaces of the predecessor:" << name - << " are: " << command_interfaces_set.size()); + "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_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The command interfaces of the predecessor:" << name << " are: " << command_itf); + "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_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The reference interfaces of the successor:" << successor->name - << " are: " << reference_itf); + "The reference interfaces of the successor: '%s' are: %s", successor->name.c_str(), + reference_itf.c_str()); } bool all_successor_interfaces_match = false; @@ -438,11 +437,11 @@ struct ControllerPeerInfo all_successor_interfaces_match = true; } }); - RCLCPP_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The reference interfaces of the successor: " - << successor->name << " are within the command interfaces of the predecessor: " << name - << " : " << std::boolalpha << all_successor_interfaces_match); + "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); @@ -451,7 +450,7 @@ struct ControllerPeerInfo } else { - RCLCPP_ERROR( + RCLCPP_INFO( 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.", @@ -467,9 +466,9 @@ struct ControllerPeerInfo { if (ros2_control::has_item(controllers_to_deactivate, predecessor->name)) { - RCLCPP_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The predecessor: " << predecessor->name << " is already in the deactivation list."); + "The predecessor: '%s' is already in the deactivation list.", predecessor->name.c_str()); continue; } ros2_control::add_item(controllers_to_deactivate, predecessor->name); @@ -490,9 +489,9 @@ struct ControllerPeerInfo { if (ros2_control::has_item(controllers_to_deactivate, successor->name)) { - RCLCPP_ERROR_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The successor: " << successor->name << " is already in the deactivation list."); + "The successor: '%s' is already in the deactivation list.", successor->name.c_str()); continue; } RCLCPP_INFO( @@ -507,24 +506,24 @@ struct ControllerPeerInfo [&successor](const std::unordered_set & group) { return group.find(successor->name) != group.end() && group.size() == 1; })) { - RCLCPP_INFO_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "The successor: " << successor->name - << " is in a mutually exclusive group, skipping further deactivation."); + "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_INFO_STREAM( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), - "Adding successor: " << successor->name - << " to the deactivation list, as it has no command interfaces."); + "Adding successor: '%s' to the deactivation list, as it has no command interfaces.", + successor->name.c_str()); } else { - RCLCPP_ERROR( + RCLCPP_INFO( rclcpp::get_logger("controller_manager"), "Controller %s has a successor %s who has command interfaces. This is not supported now.", name.c_str(), successor->name.c_str()); From 36bebe8fda1ec2629a30c60cf309ccb064516055 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Sep 2025 22:58:41 +0200 Subject: [PATCH 27/29] Change to DEBUG logging --- .../controller_manager/controller_spec.hpp | 57 ++++++++----------- 1 file changed, 25 insertions(+), 32 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 124c445ce7..33f6c91b43 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -82,7 +82,7 @@ struct ControllerPeerInfo // interfaces, add it as individual group predecessor_group.insert(p->name); mutually_exclusive_predecessor_groups.push_back(predecessor_group); - RCLCPP_INFO( + 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.", @@ -165,7 +165,7 @@ struct ControllerPeerInfo } }; - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "Generating combinations of predecessors for controller: %s (%s)", predecessor->name.c_str(), name.c_str()); @@ -174,10 +174,10 @@ struct ControllerPeerInfo for (const auto & combination : combinations) { std::unordered_set group(combination.begin(), combination.end()); - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), fmt::format( - "Adding predecessor group: {} with size: {}", fmt::join(combination, ", "), + "Adding predecessor group: [{}] with size: {}", fmt::join(combination, ", "), combination.size()) .c_str()); if (!group.empty()) @@ -220,7 +220,7 @@ struct ControllerPeerInfo // interfaces, add it as individual group successor_group.insert(s->name); mutually_exclusive_successor_groups.push_back(successor_group); - RCLCPP_INFO( + 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.", @@ -294,7 +294,7 @@ struct ControllerPeerInfo current_combination.pop_back(); } }; - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "Generating combinations of successors for controller: %s", name.c_str()); generate_combinations(0); @@ -302,10 +302,10 @@ struct ControllerPeerInfo for (const auto & combination : combinations) { std::unordered_set group(combination.begin(), combination.end()); - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), fmt::format( - "Adding successor group: {} with size: {}", fmt::join(combination, ", "), + "Adding successor group: [{}] with size: {}", fmt::join(combination, ", "), combination.size()) .c_str()); if (!group.empty()) @@ -313,7 +313,7 @@ struct ControllerPeerInfo mutually_exclusive_successor_groups.push_back(group); } } - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "Mutually exclusive successor groups for controller: '%s' are: %zu", name.c_str(), mutually_exclusive_successor_groups.size()); @@ -334,7 +334,7 @@ struct ControllerPeerInfo { if (ros2_control::has_item(controllers_to_activate, predecessor->name)) { - RCLCPP_INFO( + 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); @@ -354,11 +354,11 @@ struct ControllerPeerInfo } }); - RCLCPP_INFO( + 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_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The reference interfaces of the controller: '%s' are: %zu", name.c_str(), reference_interfaces.size()); @@ -366,7 +366,7 @@ struct ControllerPeerInfo !predecessor_in_active_list.empty() && (predecessor_command_interfaces_set.size() != reference_interfaces.size())) { - RCLCPP_INFO( + 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", @@ -405,13 +405,13 @@ struct ControllerPeerInfo // 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_INFO( + 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_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The command interfaces of the predecessor: '%s' are: %s", name.c_str(), command_itf.c_str()); @@ -419,7 +419,7 @@ struct ControllerPeerInfo for (const auto & reference_itf : successor->reference_interfaces) { - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The reference interfaces of the successor: '%s' are: %s", successor->name.c_str(), reference_itf.c_str()); @@ -437,7 +437,7 @@ struct ControllerPeerInfo all_successor_interfaces_match = true; } }); - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The reference interfaces of the successor: '%s' are within the command interfaces of the " "predecessor: '%s' : %s", @@ -450,7 +450,7 @@ struct ControllerPeerInfo } else { - RCLCPP_INFO( + 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.", @@ -466,7 +466,7 @@ struct ControllerPeerInfo { if (ros2_control::has_item(controllers_to_deactivate, predecessor->name)) { - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The predecessor: '%s' is already in the deactivation list.", predecessor->name.c_str()); continue; @@ -489,12 +489,12 @@ struct ControllerPeerInfo { if (ros2_control::has_item(controllers_to_deactivate, successor->name)) { - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The successor: '%s' is already in the deactivation list.", successor->name.c_str()); continue; } - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), fmt::format( "The controllers to deactivate list is {}", fmt::join(controllers_to_deactivate, ", ")) @@ -506,7 +506,7 @@ struct ControllerPeerInfo [&successor](const std::unordered_set & group) { return group.find(successor->name) != group.end() && group.size() == 1; })) { - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "The successor: '%s' is in a mutually exclusive group, skipping further deactivation.", successor->name.c_str()); @@ -516,18 +516,11 @@ struct ControllerPeerInfo if (successor->command_interfaces.empty()) { ros2_control::add_item(controllers_to_deactivate, successor->name); - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "Adding successor: '%s' to the deactivation list, as it has no command interfaces.", successor->name.c_str()); } - else - { - RCLCPP_INFO( - rclcpp::get_logger("controller_manager"), - "Controller %s has a successor %s who has command interfaces. This is not supported now.", - name.c_str(), successor->name.c_str()); - } successor->get_controllers_to_deactivate(controllers_to_deactivate); } } @@ -596,7 +589,7 @@ class ControllerChainDependencyGraph std::vector get_dependencies_to_activate(const std::string & controller_name) { - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "+++++++++++++++++++++++++++++++ Getting dependencies to ACTIVATE " "+++++++++++++++++++++++++++++++"); @@ -613,7 +606,7 @@ class ControllerChainDependencyGraph std::vector get_dependencies_to_deactivate(const std::string & controller_name) { - RCLCPP_INFO( + RCLCPP_DEBUG( rclcpp::get_logger("controller_manager"), "+++++++++++++++++++++++++++++++ Getting dependencies to DEACTIVATE " "+++++++++++++++++++++++++++++++"); From 9c9ef20e59c63e03f567b314cf9c45775a8af24f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Sep 2025 23:55:32 +0200 Subject: [PATCH 28/29] Update print list --- .pre-commit-config.yaml | 2 +- controller_manager/src/controller_manager.cpp | 29 ++++++++++++++----- 2 files changed, 22 insertions(+), 9 deletions(-) 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_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8e37fcbe22..6afd1d0f02 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1506,6 +1506,14 @@ controller_interface::return_type ControllerManager::switch_controller_cb( ? "AUTO" : "FORCE_AUTO")); + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_controllers.empty(), + fmt::format("Activating controllers: [ {} ]", fmt::join(activate_controllers, " ")).c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_controllers.empty(), + fmt::format("Deactivating controllers: [ {} ]", fmt::join(deactivate_controllers, " ")) + .c_str()); + const auto list_controllers = [this, strictness, &strictness_string]( const std::vector & controller_list, std::vector & request_list, @@ -1526,7 +1534,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( is_activate ? controller_chain_dependency_graph_.get_dependencies_to_activate(controller) : controller_chain_dependency_graph_.get_dependencies_to_deactivate(controller); - RCLCPP_INFO( + RCLCPP_DEBUG( get_logger(), fmt::format( "Controller {} has '{}' dependencies to {}", controller, fmt::join(ctrl_dependencies, ", "), action) @@ -1599,12 +1607,17 @@ controller_interface::return_type ControllerManager::switch_controller_cb( 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(), - fmt::format("Activating controllers: [ {} ]", fmt::join(activate_request_, " ")).c_str()); + 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(), - fmt::format("Deactivating controllers: [ {} ]", fmt::join(deactivate_request_, " ")).c_str()); + 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(); @@ -4413,16 +4426,16 @@ void ControllerManager::build_controllers_topology_info( } for (const auto & [controller_name, controller_chain] : controller_chain_spec_) { - RCLCPP_INFO( + 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_INFO_EXPRESSION( + RCLCPP_DEBUG_EXPRESSION( get_logger(), !controller_chain.following_controllers.empty(), fmt::format( "\tFollowing controllers are : {}", fmt::join(controller_chain.following_controllers, ", ")) .c_str()); - RCLCPP_INFO_EXPRESSION( + RCLCPP_DEBUG_EXPRESSION( get_logger(), !controller_chain.preceding_controllers.empty(), fmt::format( "\tPreceding controllers are : {}", fmt::join(controller_chain.preceding_controllers, ", ")) From cb75751b6cc51124d441f77859fa00889daac1e0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 24 Sep 2025 21:27:28 +0200 Subject: [PATCH 29/29] Update controller_manager/src/controller_manager.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- 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 6afd1d0f02..6f020494d3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -4436,7 +4436,7 @@ void ControllerManager::build_controllers_topology_info( "\tFollowing controllers are : {}", fmt::join(controller_chain.following_controllers, ", ")) .c_str()); RCLCPP_DEBUG_EXPRESSION( - get_logger(), !controller_chain.preceding_controllers.empty(), + get_logger(), !controller_chain.preceding_controllers.empty(), "%s", fmt::format( "\tPreceding controllers are : {}", fmt::join(controller_chain.preceding_controllers, ", ")) .c_str());