diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b6c277126e..e61e3c769b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -148,6 +148,9 @@ std::vector get_following_controller_names( 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) { @@ -206,6 +209,8 @@ std::vector get_preceding_controller_names( } 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) { @@ -2436,6 +2441,14 @@ 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())