From 931fc4321768073beef5fa746a901cf2cdd3ef52 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 3 Dec 2023 19:12:44 +0100 Subject: [PATCH 01/31] added reference lists of following and preeceding controllers to ControllerSpec --- .../include/controller_manager/controller_spec.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 6f7483f3ec..1afd8feb4a 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -39,6 +39,9 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr next_update_cycle_time; + + std::vector> following_controllers; + std::vector> preceding_controllers; }; } // namespace controller_manager From 72f6ca567a962c3979a5bc0b39b8ec460d4b06ba Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 21:46:02 +0100 Subject: [PATCH 02/31] change to a different struct for easiness and ownership --- .../include/controller_manager/controller_spec.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 1afd8feb4a..d13e9c56bd 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -39,10 +39,12 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr next_update_cycle_time; - - std::vector> following_controllers; - std::vector> preceding_controllers; }; +struct ControllerChainSpec +{ + std::vector following_controllers; + std::vector preceding_controllers; +}; } // namespace controller_manager #endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ From 34b56945dd4c0953e1788353efa52b1c08374405 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 22:06:53 +0100 Subject: [PATCH 03/31] add a utility method to add elements to the list if they don't exist --- controller_manager/src/controller_manager.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1911bd6ff8..8cf50869f1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -240,6 +240,16 @@ std::vector get_preceding_controller_names( return preceding_controllers; } +template +void add_element_to_list(std::vector & list, const T & element) +{ + if (std::find(list.begin(), list.end(), element) == list.end()) + { + // Only add to the list if it doesn't exist + list.push_back(element); + } +} + } // namespace namespace controller_manager From dd9119c04c8a6ad6885f2cd3c2444f05fc2749db Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 22:07:28 +0100 Subject: [PATCH 04/31] Add controller_chain_spec and update the following controllers based on the command interfaces --- .../controller_manager/controller_manager.hpp | 1 + controller_manager/src/controller_manager.cpp | 12 ++++++++++++ 2 files changed, 13 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f5ee6854a1..b07801b987 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -515,6 +515,7 @@ class ControllerManager : public rclcpp::Node }; RTControllerListWrapper rt_controllers_wrapper_; + std::unordered_map controller_chain_spec_; /// mutex copied from ROS1 Control, protects service callbacks /// not needed if we're guaranteed that the callbacks don't come from multiple threads std::mutex services_lock_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8cf50869f1..5584a2f319 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -816,6 +816,18 @@ controller_interface::return_type ControllerManager::configure_controller( resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); } + // let's update the list of following and preceding controllers + const auto cmd_itfs = controller->command_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (command_interface_is_reference_interface_of_controller(cmd_itf, controllers, ctrl_it)) + { + add_element_to_list( + controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); + } + } + // Now let's reorder the controllers // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); From 6a2ab8b2c1fc955d411c8e1ad3650c24159f34cd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 22:10:45 +0100 Subject: [PATCH 05/31] Add current configured controller to the preceding controller list of the controller exposing reference interface --- controller_manager/src/controller_manager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5584a2f319..2e157c6fb8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -825,6 +825,8 @@ controller_interface::return_type ControllerManager::configure_controller( { add_element_to_list( controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name); + add_element_to_list( + controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name); } } From b5a0c74cfbfd439d9b68ae06ec47bfedd38fa79d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 22:23:13 +0100 Subject: [PATCH 06/31] Apply the same type of logic for state interfaces --- controller_manager/src/controller_manager.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2e157c6fb8..da73c8aed7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -818,6 +818,7 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; + const auto state_itfs = controller->state_interface_configuration().names; for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; @@ -829,6 +830,18 @@ controller_interface::return_type ControllerManager::configure_controller( controller_chain_spec_[ctrl_it->info.name].preceding_controllers, 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 (command_interface_is_reference_interface_of_controller(state_itf, controllers, ctrl_it)) + { + add_element_to_list( + controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name); + add_element_to_list( + controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name); + } + } // Now let's reorder the controllers // lock controllers From 1d93659f2e6c067b272f34572cce49fc60ad205f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 22:37:58 +0100 Subject: [PATCH 07/31] Add utility method to remove elements from the list --- controller_manager/src/controller_manager.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index da73c8aed7..ce724dccee 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -250,6 +250,16 @@ void add_element_to_list(std::vector & list, const T & element) } } +template +void remove_element_from_list(std::vector & list, const T & element) +{ + auto itr = std::find(list.begin(), list.end(), element); + if (itr != list.end()) + { + list.erase(itr); + } +} + } // namespace namespace controller_manager From 95c70265902ef5b0335b0ac040e299b5c1dd2c41 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Dec 2023 22:38:43 +0100 Subject: [PATCH 08/31] Added a method to cleanup the controller chain spec upon node cleanup (reconfigure and unloading the controller) --- controller_manager/src/controller_manager.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ce724dccee..1d1294a339 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -260,6 +260,23 @@ void remove_element_from_list(std::vector & list, const T & element) } } +void controller_chain_spec_cleanup( + std::unordered_map & ctrl_chain_spec, + const std::string & controller) +{ + const auto following_controllers = ctrl_chain_spec[controller].following_controllers; + const auto preceding_controllers = ctrl_chain_spec[controller].preceding_controllers; + for (const auto & flwg_ctrl : following_controllers) + { + remove_element_from_list(ctrl_chain_spec[flwg_ctrl].preceding_controllers, controller); + } + for (const auto & preced_ctrl : preceding_controllers) + { + remove_element_from_list(ctrl_chain_spec[preced_ctrl].following_controllers, controller); + } + ctrl_chain_spec.erase(controller); +} + } // namespace namespace controller_manager @@ -682,6 +699,7 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + controller_chain_spec_cleanup(controller_chain_spec_, controller_name); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? if (is_controller_inactive(*controller.c)) @@ -759,6 +777,7 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? + controller_chain_spec_cleanup(controller_chain_spec_, controller_name); new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { From 2ba75ffa8188b696666008fb15ffcbdf2b82fe79 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 Dec 2023 23:12:02 +0100 Subject: [PATCH 09/31] added a lambda to find the controllerSpec by controller name --- controller_manager/src/controller_manager.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1d1294a339..a456abf098 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -880,6 +880,18 @@ controller_interface::return_type ControllerManager::configure_controller( // Copy all controllers from the 'from' list to the 'to' list to = from; + auto find_controller_spec_by_name = []( + const std::vector & specs_list, + const std::string & controller_name) -> ControllerSpec + { + auto it = std::find( + specs_list.begin(), specs_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (it != specs_list.end()) + return *it; + else + return ControllerSpec(); + }; // Reordering the controllers std::stable_sort( From de7f97fc27e8bfa86dc2b32e821080b4cee6f6a0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 5 Dec 2023 23:12:32 +0100 Subject: [PATCH 10/31] added a lambda to check if a ControllerSpec for a given controller name --- controller_manager/src/controller_manager.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a456abf098..5ca6525210 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -880,6 +880,17 @@ controller_interface::return_type ControllerManager::configure_controller( // Copy all controllers from the 'from' list to the 'to' list to = from; + std::vector sorted_list; + + auto is_controller_in_list = + [](const std::vector & specs_list, const std::string & controller_name) -> bool + { + return std::find( + specs_list.begin(), specs_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)) != + specs_list.end(); + }; + auto find_controller_spec_by_name = []( const std::vector & specs_list, const std::string & controller_name) -> ControllerSpec From f728a3a5bae41f738f6211250295f47fd4922ef5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 16 Dec 2023 04:01:26 +0100 Subject: [PATCH 11/31] comment out the lambdas --- controller_manager/src/controller_manager.cpp | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5ca6525210..a3ace686be 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -882,27 +882,27 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; std::vector sorted_list; - auto is_controller_in_list = - [](const std::vector & specs_list, const std::string & controller_name) -> bool - { - return std::find( - specs_list.begin(), specs_list.end(), - std::bind(controller_name_compare, std::placeholders::_1, controller_name)) != - specs_list.end(); - }; - - auto find_controller_spec_by_name = []( - const std::vector & specs_list, - const std::string & controller_name) -> ControllerSpec - { - auto it = std::find( - specs_list.begin(), specs_list.end(), - std::bind(controller_name_compare, std::placeholders::_1, controller_name)); - if (it != specs_list.end()) - return *it; - else - return ControllerSpec(); - }; + // auto is_controller_in_list = + // [](const std::vector & specs_list, const std::string & ctrl_name) -> bool + // { + // return std::find( + // specs_list.begin(), specs_list.end(), + // std::bind(controller_name_compare, std::placeholders::_1, ctrl_name)) != + // specs_list.end(); + // }; + + // auto find_controller_spec_by_name = []( + // const std::vector & specs_list, + // const std::string & ctrl_name) -> ControllerSpec + // { + // auto it = std::find( + // specs_list.begin(), specs_list.end(), + // std::bind(controller_name_compare, std::placeholders::_1, ctrl_name)); + // if (it != specs_list.end()) + // return *it; + // else + // return ControllerSpec(); + // }; // Reordering the controllers std::stable_sort( From 3042a85ad5fe64e7d4875d8d1195dc7cf4b8339f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 16 Dec 2023 04:01:55 +0100 Subject: [PATCH 12/31] Added 2 new methods to perform controller sorting --- .../controller_manager/controller_manager.hpp | 7 +++ controller_manager/src/controller_manager.cpp | 61 +++++++++++++++++++ 2 files changed, 68 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b07801b987..57b7992cd4 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -413,6 +413,12 @@ class ControllerManager : public rclcpp::Node const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers); + void perform_controller_sorting(); + + void insert_controller( + const std::string & ctrl_name, std::vector::iterator controller_iterator, + bool append_to_controller); + void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); /** @@ -516,6 +522,7 @@ class ControllerManager : public rclcpp::Node RTControllerListWrapper rt_controllers_wrapper_; std::unordered_map controller_chain_spec_; + 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 std::mutex services_lock_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a3ace686be..fb7ec74db4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2714,6 +2714,67 @@ void ControllerManager::controller_activity_diagnostic_callback( } } +void controller_manager::ControllerManager::perform_controller_sorting() +{ + for (const auto & [controller_name, chain_spec] : controller_chain_spec_) + { + auto it = std::find( + ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); + if (it == ordered_controllers_names_.end()) + { + ordered_controllers_names_.push_back(controller_name); + RCLCPP_INFO(get_logger(), "Adding controller : %s", controller_name.c_str()); + it = std::find( + ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); + } + + RCLCPP_INFO(get_logger(), "\tFollowing controllers : "); + for (const std::string & flwg_ctrl : chain_spec.following_controllers) + { + RCLCPP_INFO(get_logger(), "\t\t%s", flwg_ctrl.c_str()); + insert_controller(flwg_ctrl, it, true); + } + + RCLCPP_INFO(get_logger(), "\tPreceding controllers : "); + for (const std::string & preced_ctrl : chain_spec.preceding_controllers) + { + RCLCPP_INFO(get_logger(), "\t\t%s", preced_ctrl.c_str()); + insert_controller(preced_ctrl, it, false); + } + } +} + +void ControllerManager::insert_controller( + const std::string & ctrl_name, std::vector::iterator controller_iterator, + bool append_to_controller) +{ + auto new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + if (new_ctrl_it == ordered_controllers_names_.end()) + { + if (append_to_controller) + { + ordered_controllers_names_.insert(controller_iterator + 1, ctrl_name); + } + else + { + ordered_controllers_names_.insert(controller_iterator, ctrl_name); + } + new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + } + if (append_to_controller) + { + for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) + insert_controller(flwg_ctrl, new_ctrl_it, true); + } + else + { + for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + insert_controller(preced_ctrl, new_ctrl_it, false); + } +} + rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const ControllerSpec & controller) const { From 7ab583d9a30fced26ae1831338e11d6bee149ff6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 16 Dec 2023 04:02:40 +0100 Subject: [PATCH 13/31] added lines to start checking out the list --- controller_manager/src/controller_manager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fb7ec74db4..2544792430 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -848,6 +848,7 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; + controller_chain_spec_[controller_name] = ControllerChainSpec(); for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; @@ -904,6 +905,8 @@ controller_interface::return_type ControllerManager::configure_controller( // return ControllerSpec(); // }; + ordered_controllers_names_.clear(); + perform_controller_sorting(); // Reordering the controllers std::stable_sort( to.begin(), to.end(), From 9bb56bbd58d4791bd43fc5514149c712c51540a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Dec 2023 16:36:27 +0100 Subject: [PATCH 14/31] remove resetting the list on configure as there might be another controller added as predecessor --- controller_manager/src/controller_manager.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2544792430..fef0da6d7e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -848,7 +848,6 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; - controller_chain_spec_[controller_name] = ControllerChainSpec(); for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; From 708b42f8a39ef0bc41b3c8d12763c2f3f9e8f07d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Dec 2023 16:49:29 +0100 Subject: [PATCH 15/31] initialize empty spec for the broadcasters --- controller_manager/src/controller_manager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fef0da6d7e..8096a62e93 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -848,6 +848,8 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; + if (cmd_itfs.empty() && state_itfs.empty()) + controller_chain_spec_[controller_name] = ControllerChainSpec(); for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; From 0a31887914b3167a575d50283f5b46a769b17d2f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 21 Dec 2023 18:14:41 +0100 Subject: [PATCH 16/31] add formatting changes --- controller_manager/src/controller_manager.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8096a62e93..e6dc876916 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -849,7 +849,9 @@ controller_interface::return_type ControllerManager::configure_controller( const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; if (cmd_itfs.empty() && state_itfs.empty()) + { controller_chain_spec_[controller_name] = ControllerChainSpec(); + } for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; @@ -2770,12 +2772,16 @@ void ControllerManager::insert_controller( if (append_to_controller) { for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) + { insert_controller(flwg_ctrl, new_ctrl_it, true); + } } else { for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + { insert_controller(preced_ctrl, new_ctrl_it, false); + } } } From 6a43be74a6941905fa8120a1d656b662ae366232 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 4 Feb 2024 20:06:05 +0100 Subject: [PATCH 17/31] remove the unnecessary conditioning in the insert_controller method --- controller_manager/src/controller_manager.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e6dc876916..c081309278 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2758,6 +2758,7 @@ void ControllerManager::insert_controller( std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); if (new_ctrl_it == ordered_controllers_names_.end()) { + RCLCPP_DEBUG(get_logger(), "Adding controller : %s", ctrl_name.c_str()); if (append_to_controller) { ordered_controllers_names_.insert(controller_iterator + 1, ctrl_name); @@ -2768,18 +2769,23 @@ void ControllerManager::insert_controller( } new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); - } - if (append_to_controller) - { + + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), + "\t[%s]Following controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].following_controllers.size()); for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) { + RCLCPP_DEBUG(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); insert_controller(flwg_ctrl, new_ctrl_it, true); } - } - else - { + RCLCPP_DEBUG_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), + "\t[%s]Preceding controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].preceding_controllers.size()); for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) { + RCLCPP_DEBUG(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); insert_controller(preced_ctrl, new_ctrl_it, false); } } From aa228555d0ecb24214b6723db76a64778bbea007 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 4 Feb 2024 22:59:38 +0100 Subject: [PATCH 18/31] fix the typo in the tests documentation --- controller_manager/test/test_controller_manager_srvs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index cdec4806c1..c7786285d2 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -865,7 +865,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) /// && /// test_controller_name_2 -> chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 /// && - /// test_controller_name_7 -> test_controller_name_8 + /// test_controller_name_8 -> test_controller_name_7 /// /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported /// from the controller B (or) the controller B is utilizing the expected interfaces exported from From 81f43f182d76942d6d01376362071343714c4419 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 4 Feb 2024 23:00:54 +0100 Subject: [PATCH 19/31] change the conditioning to set the default ControllerChainSpec --- 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 c081309278..2056c04499 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -848,7 +848,7 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; - if (cmd_itfs.empty() && state_itfs.empty()) + if (controller_chain_spec_.find(controller_name) == controller_chain_spec_.end()) { controller_chain_spec_[controller_name] = ControllerChainSpec(); } From a1134b418b33fea1118a3da6aa269ed2c5b8e002 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 6 Feb 2024 01:07:16 +0100 Subject: [PATCH 20/31] add propagate option to the method --- .../controller_manager/controller_manager.hpp | 2 +- controller_manager/src/controller_manager.cpp | 118 +++++++++++++++--- 2 files changed, 99 insertions(+), 21 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 57b7992cd4..fbf180c6d9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -417,7 +417,7 @@ class ControllerManager : public rclcpp::Node void insert_controller( const std::string & ctrl_name, std::vector::iterator controller_iterator, - bool append_to_controller); + bool append_to_controller, bool propagate = true); void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2056c04499..8b74cd12e3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2752,41 +2752,119 @@ void controller_manager::ControllerManager::perform_controller_sorting() void ControllerManager::insert_controller( const std::string & ctrl_name, std::vector::iterator controller_iterator, - bool append_to_controller) + bool append_to_controller, bool propagate) { + // auto get_inserting_index = [this]( + // const auto init_iterator, const ControllerChainSpec & spec, + // const std::vector & list) + // { + // auto iterator = list.end(); + // for (const auto & ctrl : spec.following_controllers) + // { + // auto it = std::find(list.begin(), list.end(), ctrl); + // if (it != list.end()) + // { + // if (std::distance(list.begin(), it) < std::distance(list.begin(), iterator)) + // { + // iterator = it; + // } + // } + // } + // for (const auto & ctrl : spec.preceding_controllers) + // { + // auto it = std::find(list.begin(), list.end(), ctrl); + // if (it != list.end()) + // { + // if (std::distance(list.begin(), it) < std::distance(list.begin(), iterator)) + // { + // RCLCPP_INFO( + // get_logger(), "THIS CASE CAN NEVER HAPPEN, INSETING CONTROLLER IN WRONG POSITION : + // %s", ctrl.c_str()); + // } + // if (std::distance(list.begin(), it) > std::distance(list.begin(), iterator)) + // { + // iterator = it; + // } + // } + // } + // return iterator; + // }; auto new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); if (new_ctrl_it == ordered_controllers_names_.end()) { - RCLCPP_DEBUG(get_logger(), "Adding controller : %s", ctrl_name.c_str()); + RCLCPP_INFO(get_logger(), "Adding controller : %s", ctrl_name.c_str()); + + auto iterator = controller_iterator; + for (const auto & ctrl : controller_chain_spec_[ctrl_name].following_controllers) + { + auto it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl); + if (it != ordered_controllers_names_.end()) + { + if ( + std::distance(ordered_controllers_names_.begin(), it) < + std::distance(ordered_controllers_names_.begin(), iterator)) + { + iterator = it; + } + } + } + for (const auto & ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + { + auto it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl); + if (it != ordered_controllers_names_.end()) + { + if ( + std::distance(ordered_controllers_names_.begin(), it) < + std::distance(ordered_controllers_names_.begin(), iterator)) + { + RCLCPP_INFO( + get_logger(), "THIS CASE CAN NEVER HAPPEN, INSETING CONTROLLER IN WRONG POSITION : %s", + ctrl.c_str()); + } + if ( + std::distance(ordered_controllers_names_.begin(), it) > + std::distance(ordered_controllers_names_.begin(), iterator)) + { + iterator = it; + } + } + } + if (append_to_controller) { - ordered_controllers_names_.insert(controller_iterator + 1, ctrl_name); + ordered_controllers_names_.insert(iterator + 1, ctrl_name); } else { - ordered_controllers_names_.insert(controller_iterator, ctrl_name); + ordered_controllers_names_.insert(iterator, ctrl_name); } new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); - RCLCPP_DEBUG_EXPRESSION( - get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), - "\t[%s]Following controllers : %ld", ctrl_name.c_str(), - controller_chain_spec_[ctrl_name].following_controllers.size()); - for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) - { - RCLCPP_DEBUG(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); - insert_controller(flwg_ctrl, new_ctrl_it, true); - } - RCLCPP_DEBUG_EXPRESSION( - get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), - "\t[%s]Preceding controllers : %ld", ctrl_name.c_str(), - controller_chain_spec_[ctrl_name].preceding_controllers.size()); - for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + if (propagate) { - RCLCPP_DEBUG(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); - insert_controller(preced_ctrl, new_ctrl_it, false); + RCLCPP_INFO_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), + "\t[%s]Following controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].following_controllers.size()); + for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) + { + RCLCPP_INFO(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); + insert_controller(flwg_ctrl, new_ctrl_it, true, false); + } + RCLCPP_INFO_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), + "\t[%s]Preceding controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].preceding_controllers.size()); + for (const std::string & preced_ctrl : + controller_chain_spec_[ctrl_name].preceding_controllers) + { + RCLCPP_INFO(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); + insert_controller(preced_ctrl, new_ctrl_it, false, false); + } } } } From e82b1f9380eef362c4a48735f166b0e2cf4ca5ad Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 6 Feb 2024 01:08:14 +0100 Subject: [PATCH 21/31] comment out unnecessary code and only use insert_controller method --- controller_manager/src/controller_manager.cpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8b74cd12e3..27c6433326 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2728,25 +2728,26 @@ void controller_manager::ControllerManager::perform_controller_sorting() ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); if (it == ordered_controllers_names_.end()) { - ordered_controllers_names_.push_back(controller_name); - RCLCPP_INFO(get_logger(), "Adding controller : %s", controller_name.c_str()); + insert_controller(controller_name, ordered_controllers_names_.end(), false); + // ordered_controllers_names_.push_back(controller_name); + // RCLCPP_DEBUG(get_logger(), "Adding controller : %s", controller_name.c_str()); it = std::find( ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); } - RCLCPP_INFO(get_logger(), "\tFollowing controllers : "); - for (const std::string & flwg_ctrl : chain_spec.following_controllers) - { - RCLCPP_INFO(get_logger(), "\t\t%s", flwg_ctrl.c_str()); - insert_controller(flwg_ctrl, it, true); - } + // RCLCPP_DEBUG(get_logger(), "\tFollowing controllers : "); + // for (const std::string & flwg_ctrl : chain_spec.following_controllers) + // { + // RCLCPP_DEBUG(get_logger(), "\t\t%s", flwg_ctrl.c_str()); + // insert_controller(flwg_ctrl, it, true); + // } - RCLCPP_INFO(get_logger(), "\tPreceding controllers : "); - for (const std::string & preced_ctrl : chain_spec.preceding_controllers) - { - RCLCPP_INFO(get_logger(), "\t\t%s", preced_ctrl.c_str()); - insert_controller(preced_ctrl, it, false); - } + // RCLCPP_DEBUG(get_logger(), "\tPreceding controllers : "); + // for (const std::string & preced_ctrl : chain_spec.preceding_controllers) + // { + // RCLCPP_DEBUG(get_logger(), "\t\t%s", preced_ctrl.c_str()); + // insert_controller(preced_ctrl, it, false); + // } } } From 81ed774c76be240be5bf346b9122454c4a1a258a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 6 Feb 2024 01:10:32 +0100 Subject: [PATCH 22/31] comment out old sorting and test the current implementation --- controller_manager/src/controller_manager.cpp | 47 +++++++++++++++---- 1 file changed, 39 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 27c6433326..8e36b38373 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -908,20 +908,47 @@ controller_interface::return_type ControllerManager::configure_controller( // return ControllerSpec(); // }; + // RCLCPP_INFO(get_logger(), "Full controllers list is:"); + // for (const auto & ctrl : to) + // { + // RCLCPP_INFO(this->get_logger(), "\t%s", ctrl.info.name.c_str()); + // } ordered_controllers_names_.clear(); perform_controller_sorting(); - // Reordering the controllers - std::stable_sort( - to.begin(), to.end(), - std::bind( - &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, - to)); + std::vector new_list; + for (const auto & ctrl : ordered_controllers_names_) + { + auto controller_it = std::find_if( + to.begin(), to.end(), std::bind(controller_name_compare, std::placeholders::_1, ctrl)); + if (controller_it != to.end()) new_list.push_back(*controller_it); + } - RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); for (const auto & ctrl : to) { - RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); + auto controller_it = std::find_if( + new_list.begin(), new_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl.info.name)); + if (controller_it == new_list.end()) new_list.push_back(ctrl); + } + + // Reordering the controllers + // std::stable_sort( + // to.begin(), to.end(), + // std::bind( + // &ControllerManager::controller_sorting, this, std::placeholders::_1, + // std::placeholders::_2, to)); + RCLCPP_INFO(get_logger(), "New Reordered controllers list is:"); + for (const auto & ctrl : ordered_controllers_names_) + { + RCLCPP_INFO(this->get_logger(), "\t%s", ctrl.c_str()); } + to = new_list; + + // RCLCPP_INFO(get_logger(), "Reordered controllers list is:"); + // for (const auto & ctrl : to) + // { + // RCLCPP_INFO(this->get_logger(), "\t%s", ctrl.info.name.c_str()); + // } // switch lists rt_controllers_wrapper_.switch_updated_list(guard); @@ -1449,6 +1476,10 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } + // initialize the data for the controller chain spec once it is loaded. It is needed, so when we + // sort the controllers later, they will be added at the end + // controller_chain_spec_[controller.info.name] = ControllerChainSpec(); + executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); From eba4f7e4df85873eb7bf4778077d256cbffeb74a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 8 Feb 2024 00:16:33 +0100 Subject: [PATCH 23/31] remove propagate option and check the new controller iterator every iteration --- .../controller_manager/controller_manager.hpp | 5 +- controller_manager/src/controller_manager.cpp | 52 ++++++++----------- 2 files changed, 24 insertions(+), 33 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index fbf180c6d9..c690fd5195 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -415,9 +415,8 @@ class ControllerManager : public rclcpp::Node void perform_controller_sorting(); - void insert_controller( - const std::string & ctrl_name, std::vector::iterator controller_iterator, - bool append_to_controller, bool propagate = true); + void insert_controller(const std::string & ctrl_name, std::vector::iterator controller_iterator, + bool append_to_controller); void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8e36b38373..4e87cd2e5b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2784,7 +2784,7 @@ void controller_manager::ControllerManager::perform_controller_sorting() void ControllerManager::insert_controller( const std::string & ctrl_name, std::vector::iterator controller_iterator, - bool append_to_controller, bool propagate) + bool append_to_controller) { // auto get_inserting_index = [this]( // const auto init_iterator, const ControllerChainSpec & spec, @@ -2848,14 +2848,6 @@ void ControllerManager::insert_controller( std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl); if (it != ordered_controllers_names_.end()) { - if ( - std::distance(ordered_controllers_names_.begin(), it) < - std::distance(ordered_controllers_names_.begin(), iterator)) - { - RCLCPP_INFO( - get_logger(), "THIS CASE CAN NEVER HAPPEN, INSETING CONTROLLER IN WRONG POSITION : %s", - ctrl.c_str()); - } if ( std::distance(ordered_controllers_names_.begin(), it) > std::distance(ordered_controllers_names_.begin(), iterator)) @@ -2876,27 +2868,27 @@ void ControllerManager::insert_controller( new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); - if (propagate) - { - RCLCPP_INFO_EXPRESSION( - get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), - "\t[%s]Following controllers : %ld", ctrl_name.c_str(), - controller_chain_spec_[ctrl_name].following_controllers.size()); - for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) - { - RCLCPP_INFO(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); - insert_controller(flwg_ctrl, new_ctrl_it, true, false); - } - RCLCPP_INFO_EXPRESSION( - get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), - "\t[%s]Preceding controllers : %ld", ctrl_name.c_str(), - controller_chain_spec_[ctrl_name].preceding_controllers.size()); - for (const std::string & preced_ctrl : - controller_chain_spec_[ctrl_name].preceding_controllers) - { - RCLCPP_INFO(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); - insert_controller(preced_ctrl, new_ctrl_it, false, false); - } + RCLCPP_INFO_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), + "\t[%s]Following controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].following_controllers.size()); + for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) + { + new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + RCLCPP_INFO(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); + insert_controller(flwg_ctrl, new_ctrl_it, true); + } + RCLCPP_INFO_EXPRESSION( + get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), + "\t[%s]Preceding controllers : %ld", ctrl_name.c_str(), + controller_chain_spec_[ctrl_name].preceding_controllers.size()); + for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) + { + new_ctrl_it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + RCLCPP_INFO(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); + insert_controller(preced_ctrl, new_ctrl_it, false); } } } From d5f60d60591bd772ba5b5d9247e1f00b17190fe4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 8 Feb 2024 00:19:49 +0100 Subject: [PATCH 24/31] remove commented part of the code --- controller_manager/src/controller_manager.cpp | 35 ------------------- 1 file changed, 35 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4e87cd2e5b..90a2ac5227 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2786,41 +2786,6 @@ void ControllerManager::insert_controller( const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller) { - // auto get_inserting_index = [this]( - // const auto init_iterator, const ControllerChainSpec & spec, - // const std::vector & list) - // { - // auto iterator = list.end(); - // for (const auto & ctrl : spec.following_controllers) - // { - // auto it = std::find(list.begin(), list.end(), ctrl); - // if (it != list.end()) - // { - // if (std::distance(list.begin(), it) < std::distance(list.begin(), iterator)) - // { - // iterator = it; - // } - // } - // } - // for (const auto & ctrl : spec.preceding_controllers) - // { - // auto it = std::find(list.begin(), list.end(), ctrl); - // if (it != list.end()) - // { - // if (std::distance(list.begin(), it) < std::distance(list.begin(), iterator)) - // { - // RCLCPP_INFO( - // get_logger(), "THIS CASE CAN NEVER HAPPEN, INSETING CONTROLLER IN WRONG POSITION : - // %s", ctrl.c_str()); - // } - // if (std::distance(list.begin(), it) > std::distance(list.begin(), iterator)) - // { - // iterator = it; - // } - // } - // } - // return iterator; - // }; auto new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); if (new_ctrl_it == ordered_controllers_names_.end()) From 5a4c46fa8274269a0851b40b93d75e116cf29d67 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 8 Feb 2024 23:11:54 +0100 Subject: [PATCH 25/31] cleanup the code and remove commented part --- .../controller_manager/controller_manager.hpp | 3 +- controller_manager/src/controller_manager.cpp | 72 +------------------ 2 files changed, 4 insertions(+), 71 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index c690fd5195..57b7992cd4 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -415,7 +415,8 @@ class ControllerManager : public rclcpp::Node void perform_controller_sorting(); - void insert_controller(const std::string & ctrl_name, std::vector::iterator controller_iterator, + void insert_controller( + const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller); void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 90a2ac5227..90257e9fce 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -777,7 +777,6 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - controller_chain_spec_cleanup(controller_chain_spec_, controller_name); new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { @@ -848,10 +847,6 @@ controller_interface::return_type ControllerManager::configure_controller( // let's update the list of following and preceding controllers const auto cmd_itfs = controller->command_interface_configuration().names; const auto state_itfs = controller->state_interface_configuration().names; - if (controller_chain_spec_.find(controller_name) == controller_chain_spec_.end()) - { - controller_chain_spec_[controller_name] = ControllerChainSpec(); - } for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; @@ -886,33 +881,6 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; std::vector sorted_list; - // auto is_controller_in_list = - // [](const std::vector & specs_list, const std::string & ctrl_name) -> bool - // { - // return std::find( - // specs_list.begin(), specs_list.end(), - // std::bind(controller_name_compare, std::placeholders::_1, ctrl_name)) != - // specs_list.end(); - // }; - - // auto find_controller_spec_by_name = []( - // const std::vector & specs_list, - // const std::string & ctrl_name) -> ControllerSpec - // { - // auto it = std::find( - // specs_list.begin(), specs_list.end(), - // std::bind(controller_name_compare, std::placeholders::_1, ctrl_name)); - // if (it != specs_list.end()) - // return *it; - // else - // return ControllerSpec(); - // }; - - // RCLCPP_INFO(get_logger(), "Full controllers list is:"); - // for (const auto & ctrl : to) - // { - // RCLCPP_INFO(this->get_logger(), "\t%s", ctrl.info.name.c_str()); - // } ordered_controllers_names_.clear(); perform_controller_sorting(); std::vector new_list; @@ -923,20 +891,6 @@ controller_interface::return_type ControllerManager::configure_controller( if (controller_it != to.end()) new_list.push_back(*controller_it); } - for (const auto & ctrl : to) - { - auto controller_it = std::find_if( - new_list.begin(), new_list.end(), - std::bind(controller_name_compare, std::placeholders::_1, ctrl.info.name)); - if (controller_it == new_list.end()) new_list.push_back(ctrl); - } - - // Reordering the controllers - // std::stable_sort( - // to.begin(), to.end(), - // std::bind( - // &ControllerManager::controller_sorting, this, std::placeholders::_1, - // std::placeholders::_2, to)); RCLCPP_INFO(get_logger(), "New Reordered controllers list is:"); for (const auto & ctrl : ordered_controllers_names_) { @@ -944,12 +898,6 @@ controller_interface::return_type ControllerManager::configure_controller( } to = new_list; - // RCLCPP_INFO(get_logger(), "Reordered controllers list is:"); - // for (const auto & ctrl : to) - // { - // RCLCPP_INFO(this->get_logger(), "\t%s", ctrl.info.name.c_str()); - // } - // switch lists rt_controllers_wrapper_.switch_updated_list(guard); // clear unused list @@ -1477,8 +1425,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } // initialize the data for the controller chain spec once it is loaded. It is needed, so when we - // sort the controllers later, they will be added at the end - // controller_chain_spec_[controller.info.name] = ControllerChainSpec(); + // sort the controllers later, they will be added to the list + controller_chain_spec_[controller.info.name] = ControllerChainSpec(); executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); @@ -2760,25 +2708,9 @@ void controller_manager::ControllerManager::perform_controller_sorting() if (it == ordered_controllers_names_.end()) { insert_controller(controller_name, ordered_controllers_names_.end(), false); - // ordered_controllers_names_.push_back(controller_name); - // RCLCPP_DEBUG(get_logger(), "Adding controller : %s", controller_name.c_str()); it = std::find( ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); } - - // RCLCPP_DEBUG(get_logger(), "\tFollowing controllers : "); - // for (const std::string & flwg_ctrl : chain_spec.following_controllers) - // { - // RCLCPP_DEBUG(get_logger(), "\t\t%s", flwg_ctrl.c_str()); - // insert_controller(flwg_ctrl, it, true); - // } - - // RCLCPP_DEBUG(get_logger(), "\tPreceding controllers : "); - // for (const std::string & preced_ctrl : chain_spec.preceding_controllers) - // { - // RCLCPP_DEBUG(get_logger(), "\t\t%s", preced_ctrl.c_str()); - // insert_controller(preced_ctrl, it, false); - // } } } From fa7c972d49ef09f47ffa88243dcec9fc70e26b54 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Feb 2024 19:02:08 +0100 Subject: [PATCH 26/31] remove old controller sorting code --- .../controller_manager/controller_manager.hpp | 23 -- controller_manager/src/controller_manager.cpp | 242 ------------------ 2 files changed, 265 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 57b7992cd4..c2db0782e0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -390,29 +390,6 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); - /// A method to be used in the std::sort method to sort the controllers to be able to - /// execute them in a proper order - /** - * Compares the controllers ctrl_a and ctrl_b and then returns which comes first in the sequence - * - * @note The following conditions needs to be handled while ordering the controller list - * 1. The controllers that do not use any state or command interfaces are updated first - * 2. The controllers that use only the state system interfaces only are updated next - * 3. The controllers that use any of an another controller's reference interface are updated - * before the preceding controller - * 4. The controllers that use the controller's estimated interfaces are updated after the - * preceding controller - * 5. The controllers that only use the hardware command interfaces are updated last - * 6. All inactive controllers go at the end of the list - * - * \param[in] controllers list of controllers to compare their names to interface's prefix. - * - * @return true, if ctrl_a needs to execute first, else false - */ - bool controller_sorting( - const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, - const std::vector & controllers); - void perform_controller_sorting(); void insert_controller( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 90257e9fce..6b810e63db 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -118,128 +118,6 @@ bool command_interface_is_reference_interface_of_controller( return true; } -/** - * A method to retrieve the names of all it's following controllers given a controller name - * For instance, for the following case - * A -> B -> C -> D - * When called with B, returns C and D - * NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported from - * the controller B (or) the controller B is utilizing the expected interfaces exported from the - * controller A - * - * @param controller_name - Name of the controller for checking the tree - * \param[in] controllers list of controllers to compare their names to interface's prefix. - * @return list of controllers that are following the given controller in a chain. If none, return - * empty. - */ -std::vector get_following_controller_names( - const std::string controller_name, - const std::vector & controllers) -{ - std::vector following_controllers; - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, controller_name)); - if (controller_it == controllers.end()) - { - RCLCPP_DEBUG( - rclcpp::get_logger("ControllerManager::utils"), - "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); - - return following_controllers; - } - // If the controller is not configured, return empty - if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c))) - { - return following_controllers; - } - const auto cmd_itfs = controller_it->c->command_interface_configuration().names; - for (const auto & itf : cmd_itfs) - { - controller_manager::ControllersListIterator ctrl_it; - if (command_interface_is_reference_interface_of_controller(itf, controllers, ctrl_it)) - { - RCLCPP_DEBUG( - rclcpp::get_logger("ControllerManager::utils"), - "The interface is a reference interface of controller : %s", ctrl_it->info.name.c_str()); - following_controllers.push_back(ctrl_it->info.name); - const std::vector ctrl_names = - get_following_controller_names(ctrl_it->info.name, controllers); - for (const std::string & controller : ctrl_names) - { - if ( - std::find(following_controllers.begin(), following_controllers.end(), controller) == - following_controllers.end()) - { - // Only add to the list if it doesn't exist - following_controllers.push_back(controller); - } - } - } - } - return following_controllers; -} - -/** - * A method to retrieve the names of all it's preceding controllers given a controller name - * For instance, for the following case - * A -> B -> C -> D - * When called with C, returns A and B - * NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported from - * the controller B (or) the controller B is utilizing the expected interfaces exported from the - * controller A - * - * @param controller_name - Name of the controller for checking the tree - * \param[in] controllers list of controllers to compare their names to interface's prefix. - * @return list of controllers that are preceding the given controller in a chain. If none, return - * empty. - */ -std::vector get_preceding_controller_names( - const std::string controller_name, - const std::vector & controllers) -{ - std::vector preceding_controllers; - auto controller_it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, controller_name)); - if (controller_it == controllers.end()) - { - RCLCPP_DEBUG( - rclcpp::get_logger("ControllerManager::utils"), - "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); - return preceding_controllers; - } - for (const auto & ctrl : controllers) - { - // If the controller is not configured, then continue - if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) - { - continue; - } - auto cmd_itfs = ctrl.c->command_interface_configuration().names; - for (const auto & itf : cmd_itfs) - { - auto split_pos = itf.find_first_of('/'); - if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name)) - { - preceding_controllers.push_back(ctrl.info.name); - auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); - for (const std::string & controller : ctrl_names) - { - if ( - std::find(preceding_controllers.begin(), preceding_controllers.end(), controller) == - preceding_controllers.end()) - { - // Only add to the list if it doesn't exist - preceding_controllers.push_back(controller); - } - } - } - } - } - return preceding_controllers; -} - template void add_element_to_list(std::vector & list, const T & element) { @@ -2553,126 +2431,6 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } -bool ControllerManager::controller_sorting( - const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, - const std::vector & controllers) -{ - // If the neither of the controllers are configured, then return false - if (!((is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) && - (is_controller_active(ctrl_b.c) || is_controller_inactive(ctrl_b.c)))) - { - if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) - { - return true; - } - return false; - } - - const std::vector cmd_itfs = ctrl_a.c->command_interface_configuration().names; - const std::vector state_itfs = ctrl_a.c->state_interface_configuration().names; - if (cmd_itfs.empty() || !ctrl_a.c->is_chainable()) - { - // The case of the controllers that don't have any command interfaces. For instance, - // joint_state_broadcaster - // If the controller b is also under the same condition, then maintain their initial order - const auto command_interfaces_exist = - !ctrl_b.c->command_interface_configuration().names.empty(); - return ctrl_b.c->is_chainable() && command_interfaces_exist; - } - else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) - { - // If only the controller b is a broadcaster or non chainable type , then swap the controllers - return false; - } - else - { - auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers); - if (following_ctrls.empty()) - { - return false; - } - // If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b - if ( - std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) != - following_ctrls.end()) - { - return true; - } - else - { - auto ctrl_a_preceding_ctrls = get_preceding_controller_names(ctrl_a.info.name, controllers); - // This is to check that the ctrl_b is in the preceding controllers list of ctrl_a - This - // check is useful when there is a chained controller branching, but they belong to same - // branch - if ( - std::find(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), ctrl_b.info.name) != - ctrl_a_preceding_ctrls.end()) - { - return false; - } - - // This is to handle the cases where, the parsed ctrl_a and ctrl_b are not directly related - // but might have a common parent - happens in branched chained controller - auto ctrl_b_preceding_ctrls = get_preceding_controller_names(ctrl_b.info.name, controllers); - std::sort(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end()); - std::sort(ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end()); - std::list intersection; - std::set_intersection( - ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), - ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end(), - std::back_inserter(intersection)); - if (!intersection.empty()) - { - // If there is an intersection, then there is a common parent controller for both ctrl_a and - // ctrl_b - return true; - } - - // If there is no common parent, then they belong to 2 different sets - auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); - if (following_ctrls_b.empty()) - { - return true; - } - auto find_first_element = [&](const auto & controllers_list) -> size_t - { - auto it = std::find_if( - controllers.begin(), controllers.end(), - std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back())); - if (it != controllers.end()) - { - return std::distance(controllers.begin(), it); - } - return 0; - }; - const auto ctrl_a_chain_first_controller = find_first_element(following_ctrls); - const auto ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); - if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) - { - return true; - } - } - - // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be - // infront of ctrl_a - // TODO(saikishor): deal with the state interface chaining in the sorting algorithm - auto state_it = std::find_if( - state_itfs.begin(), state_itfs.end(), - [ctrl_b](auto itf) - { - auto index = itf.find_first_of('/'); - return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name)); - }); - if (state_it != state_itfs.end()) - { - return false; - } - - // The rest of the cases, basically end up at the end of the list - return false; - } -}; - void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { From 4c2f54ea223544355f1e4c1bcdec13707d4a1d2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Feb 2024 19:57:25 +0100 Subject: [PATCH 27/31] rename the method to update_list_with_controller_chain --- .../controller_manager/controller_manager.hpp | 22 +++++++++++++++++-- controller_manager/src/controller_manager.cpp | 9 ++++---- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index c2db0782e0..80e6a8b875 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -391,8 +391,26 @@ class ControllerManager : public rclcpp::Node const ControllersListIterator controller_it); void perform_controller_sorting(); - - void insert_controller( + /** + * @brief Inserts a controller into an ordered list based on dependencies to compute the + * controller chain. + * + * This method computes the controller chain by inserting the provided controller name into an + * ordered list of controllers based on dependencies. It ensures that controllers are inserted in + * the correct order so that dependencies are satisfied. + * + * @param ctrl_name The name of the controller to be inserted into the chain. + * @param controller_iterator An iterator pointing to the position in the ordered list where the + * controller should be inserted. + * @param append_to_controller Flag indicating whether the controller should be appended or + * prepended to the parsed iterator. + * @note The specification of controller dependencies is in the ControllerChainSpec, + * containing information about following and preceding controllers. This struct should include + * the neighboring controllers with their relationships to the provided controller. + * `following_controllers` specify controllers that come after the provided controller. + * `preceding_controllers` specify controllers that come before the provided controller. + */ + void update_list_with_controller_chain( const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6b810e63db..93ac193a7f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2465,14 +2465,13 @@ void controller_manager::ControllerManager::perform_controller_sorting() ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); if (it == ordered_controllers_names_.end()) { - insert_controller(controller_name, ordered_controllers_names_.end(), false); + update_list_with_controller_chain(controller_name, ordered_controllers_names_.end(), false); it = std::find( ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); } } } - -void ControllerManager::insert_controller( +void ControllerManager::update_list_with_controller_chain( const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller) { @@ -2532,7 +2531,7 @@ void ControllerManager::insert_controller( new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); RCLCPP_INFO(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); - insert_controller(flwg_ctrl, new_ctrl_it, true); + update_list_with_controller_chain(flwg_ctrl, new_ctrl_it, true); } RCLCPP_INFO_EXPRESSION( get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), @@ -2543,7 +2542,7 @@ void ControllerManager::insert_controller( new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); RCLCPP_INFO(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); - insert_controller(preced_ctrl, new_ctrl_it, false); + update_list_with_controller_chain(preced_ctrl, new_ctrl_it, false); } } } From 72180123b15482b69c9ded6e06de71f0aefc2955 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Feb 2024 20:03:17 +0100 Subject: [PATCH 28/31] remove unnecessary method --- .../controller_manager/controller_manager.hpp | 1 - controller_manager/src/controller_manager.cpp | 30 ++++++++----------- 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 80e6a8b875..9a709c5f8f 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -390,7 +390,6 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); - void perform_controller_sorting(); /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 93ac193a7f..dc2bd521ca 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -759,8 +759,18 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; std::vector sorted_list; + // clear the list before reordering it again ordered_controllers_names_.clear(); - perform_controller_sorting(); + for (const auto & [ctrl_name, chain_spec] : controller_chain_spec_) + { + auto it = + std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); + if (it == ordered_controllers_names_.end()) + { + update_list_with_controller_chain(ctrl_name, ordered_controllers_names_.end(), false); + } + } + std::vector new_list; for (const auto & ctrl : ordered_controllers_names_) { @@ -769,10 +779,10 @@ controller_interface::return_type ControllerManager::configure_controller( if (controller_it != to.end()) new_list.push_back(*controller_it); } - RCLCPP_INFO(get_logger(), "New Reordered controllers list is:"); + RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); for (const auto & ctrl : ordered_controllers_names_) { - RCLCPP_INFO(this->get_logger(), "\t%s", ctrl.c_str()); + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.c_str()); } to = new_list; @@ -2457,20 +2467,6 @@ void ControllerManager::controller_activity_diagnostic_callback( } } -void controller_manager::ControllerManager::perform_controller_sorting() -{ - for (const auto & [controller_name, chain_spec] : controller_chain_spec_) - { - auto it = std::find( - ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); - if (it == ordered_controllers_names_.end()) - { - update_list_with_controller_chain(controller_name, ordered_controllers_names_.end(), false); - it = std::find( - ordered_controllers_names_.begin(), ordered_controllers_names_.end(), controller_name); - } - } -} void ControllerManager::update_list_with_controller_chain( const std::string & ctrl_name, std::vector::iterator controller_iterator, bool append_to_controller) From 3cef552377b3dd0c4e1abdd3fd9d343548287240 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Feb 2024 20:07:47 +0100 Subject: [PATCH 29/31] change the logging tot DEBUG --- controller_manager/src/controller_manager.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index dc2bd521ca..3edd30bc0c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2475,7 +2475,7 @@ void ControllerManager::update_list_with_controller_chain( std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); if (new_ctrl_it == ordered_controllers_names_.end()) { - RCLCPP_INFO(get_logger(), "Adding controller : %s", ctrl_name.c_str()); + RCLCPP_DEBUG(get_logger(), "Adding controller chain : %s", ctrl_name.c_str()); auto iterator = controller_iterator; for (const auto & ctrl : controller_chain_spec_[ctrl_name].following_controllers) @@ -2515,29 +2515,27 @@ void ControllerManager::update_list_with_controller_chain( { ordered_controllers_names_.insert(iterator, ctrl_name); } - new_ctrl_it = - std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); - RCLCPP_INFO_EXPRESSION( + RCLCPP_DEBUG_EXPRESSION( get_logger(), !controller_chain_spec_[ctrl_name].following_controllers.empty(), - "\t[%s]Following controllers : %ld", ctrl_name.c_str(), + "\t[%s] Following controllers : %ld", ctrl_name.c_str(), controller_chain_spec_[ctrl_name].following_controllers.size()); for (const std::string & flwg_ctrl : controller_chain_spec_[ctrl_name].following_controllers) { new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); - RCLCPP_INFO(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); + RCLCPP_DEBUG(get_logger(), "\t\t[%s] : %s", ctrl_name.c_str(), flwg_ctrl.c_str()); update_list_with_controller_chain(flwg_ctrl, new_ctrl_it, true); } - RCLCPP_INFO_EXPRESSION( + RCLCPP_DEBUG_EXPRESSION( get_logger(), !controller_chain_spec_[ctrl_name].preceding_controllers.empty(), - "\t[%s]Preceding controllers : %ld", ctrl_name.c_str(), + "\t[%s] Preceding controllers : %ld", ctrl_name.c_str(), controller_chain_spec_[ctrl_name].preceding_controllers.size()); for (const std::string & preced_ctrl : controller_chain_spec_[ctrl_name].preceding_controllers) { new_ctrl_it = std::find(ordered_controllers_names_.begin(), ordered_controllers_names_.end(), ctrl_name); - RCLCPP_INFO(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); + RCLCPP_DEBUG(get_logger(), "\t\t[%s]: %s", ctrl_name.c_str(), preced_ctrl.c_str()); update_list_with_controller_chain(preced_ctrl, new_ctrl_it, false); } } From 0b9584f4ee656e90ea997a874e2a266b770c9165 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Feb 2024 20:14:42 +0100 Subject: [PATCH 30/31] modify a bit the logging of reordered list --- controller_manager/src/controller_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3edd30bc0c..6659464122 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -779,12 +779,12 @@ controller_interface::return_type ControllerManager::configure_controller( if (controller_it != to.end()) new_list.push_back(*controller_it); } + to = new_list; RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); - for (const auto & ctrl : ordered_controllers_names_) + for (const auto & ctrl : to) { - RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.c_str()); + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); } - to = new_list; // switch lists rt_controllers_wrapper_.switch_updated_list(guard); From f23139c3ecf2e6301946a169e05c3b7b28d3e699 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Feb 2024 20:22:40 +0100 Subject: [PATCH 31/31] fix formatting --- controller_manager/src/controller_manager.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6659464122..319f576c50 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -776,7 +776,10 @@ controller_interface::return_type ControllerManager::configure_controller( { auto controller_it = std::find_if( to.begin(), to.end(), std::bind(controller_name_compare, std::placeholders::_1, ctrl)); - if (controller_it != to.end()) new_list.push_back(*controller_it); + if (controller_it != to.end()) + { + new_list.push_back(*controller_it); + } } to = new_list;