diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2b6c69d1d0..3f3bd064c2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2448,7 +2448,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; @@ -2457,8 +2465,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); @@ -2516,7 +2541,7 @@ bool ControllerManager::controller_sorting( 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; + return false; } }