From 2e32546398c194afd952e2a30d376be3a8c88478 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 25 Nov 2023 00:14:21 +0100 Subject: [PATCH] fix the sorting logic for complex and large use-cases (fixes #1170) --- controller_manager/src/controller_manager.cpp | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index da1f5a5399..869253ffe4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2444,7 +2444,15 @@ bool ControllerManager::controller_sorting( (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; + // When both the controllers are inactive, do not change their initial order + auto ctrl_a_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl_a.info.name)); + auto ctrl_b_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl_b.info.name)); + return std::distance(controllers.begin(), ctrl_a_it) < + std::distance(controllers.begin(), ctrl_b_it); } const std::vector cmd_itfs = ctrl_a.c->command_interface_configuration().names; @@ -2453,8 +2461,25 @@ bool ControllerManager::controller_sorting( { // 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 + if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + { + auto ctrl_a_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl_a.info.name)); + auto ctrl_b_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, ctrl_b.info.name)); + return std::distance(controllers.begin(), ctrl_a_it) < + std::distance(controllers.begin(), ctrl_b_it); + } return true; } + 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);