diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1eb545d0d7..3e3e7e6ecd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -154,6 +154,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) { @@ -212,6 +215,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) { @@ -2273,6 +2278,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()) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index 3065fcf2eb..ba1132e68b 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -32,13 +32,33 @@ TestChainableController::TestChainableController() controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { - return cmd_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return cmd_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get command interface configuration until the controller is configured."); + } } controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { - return state_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return state_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get state interface configuration until the controller is configured."); + } } controller_interface::return_type TestChainableController::update_reference_from_subscribers() diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index cee8296bb1..b6eac9b689 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -29,12 +29,32 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { - return cmd_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return cmd_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get command interface configuration until the controller is configured."); + } } controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { - return state_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return state_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get state interface configuration until the controller is configured."); + } } controller_interface::return_type TestController::update(