diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 631d5173aa5..752111147fa 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -389,6 +389,26 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// A functor to be used in the std::sort method inorder to sort the controllers to be able to + /// execute then in a proper order + /** + * @brief controller_sorting - A functor that compares the controllers ctrl_a and ctrl_b and then + * return 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 system's command interfaces are updated last + * 6. All inactive controllers go at the end of the list + * + * @return true, if ctrl_a needs to execute first, else false + */ + bool controller_sorting(const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b); + void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); diagnostic_updater::Updater diagnostics_updater_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 00c801498d9..ce7c67289a6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1036,59 +1036,7 @@ controller_interface::return_type ControllerManager::switch_controller( } // Reordering the controllers - /// @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 system's command interfaces are updated last - /// 6. All inactive controllers go at the end of the list - auto sorting_lambda = [](ControllerSpec ctrl_a, ControllerSpec ctrl_b) - { - // If the controllers are not active, then go to the end of the list - if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) 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 - return true; - } - else - { - if (ctrl_b.c->is_chainable()) - { - // If the ctrl_a's command interface is the one exported by the ctrl_b then ctrl_a should be - // infront of ctrl_b - auto it = std::find_if( - cmd_itfs.begin(), cmd_itfs.end(), - [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); - if (it != cmd_itfs.end()) - { - return true; - } - else - { - // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be - // infront of ctrl_a - auto state_it = std::find_if( - cmd_itfs.begin(), cmd_itfs.end(), - [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); - if (it != cmd_itfs.end()) - { - return false; - } - } - } - // The rest of the cases, basically end up at the end of the list - return false; - } - }; - std::sort(to.begin(), to.end(), sorting_lambda); + std::sort(to.begin(), to.end(), &ControllerManager::controller_sorting); // switch lists rt_controllers_wrapper_.switch_updated_list(guard); @@ -2248,6 +2196,51 @@ 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) +{ + // If the controllers are not active, then go to the end of the list + if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) 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 + return true; + } + else + { + if (ctrl_b.c->is_chainable()) + { + // If the ctrl_a's command interface is the one exported by the ctrl_b then ctrl_a should be + // infront of ctrl_b + auto it = std::find_if( + cmd_itfs.begin(), cmd_itfs.end(), + [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); + if (it != cmd_itfs.end()) + { + return true; + } + else + { + // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be + // infront of ctrl_a + auto state_it = std::find_if( + cmd_itfs.begin(), cmd_itfs.end(), + [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); + if (it != cmd_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(