diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 16347cae70..2850b2be80 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -392,8 +392,7 @@ class ControllerManager : public rclcpp::Node /// A method to be used in the std::sort method to sort the controllers to be able to /// execute them in a proper order /** - * @brief controller_sorting - A functor that compares the controllers ctrl_a and ctrl_b and then - * returns which comes first in the sequence + * Compares the controllers ctrl_a and ctrl_b and then returns 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 @@ -402,7 +401,7 @@ class ControllerManager : public rclcpp::Node * 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 + * 5. The controllers that only use the hardware command interfaces are updated last * 6. All inactive controllers go at the end of the list * * \param[in] controllers list of controllers to compare their names to interface's prefix. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b388ea836b..bcd42ada74 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -123,6 +123,10 @@ bool command_interface_is_reference_interface_of_controller( * For instance, for the following case * A -> B -> C -> D * When called with B, returns C and D + * NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported from + * the controller B (or) the controller B is utilizing the expected interfaces exported from the + * controller A + * * @param controller_name - Name of the controller for checking the tree * \param[in] controllers list of controllers to compare their names to interface's prefix. * @return list of controllers that are following the given controller in a chain. If none, return @@ -179,6 +183,10 @@ std::vector get_following_controller_names( * For instance, for the following case * A -> B -> C -> D * When called with C, returns A and B + * NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported from + * the controller B (or) the controller B is utilizing the expected interfaces exported from the + * controller A + * * @param controller_name - Name of the controller for checking the tree * \param[in] controllers list of controllers to compare their names to interface's prefix. * @return list of controllers that are preceding the given controller in a chain. If none, return @@ -2403,7 +2411,7 @@ bool ControllerManager::controller_sorting( 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()) + 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 @@ -2469,7 +2477,7 @@ bool ControllerManager::controller_sorting( // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be // infront of ctrl_a - //TODO(saikishor): deal with the state interface chaining in the sorting algorithm + // TODO(saikishor): deal with the state interface chaining in the sorting algorithm auto state_it = std::find_if( state_itfs.begin(), state_itfs.end(), [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index ccc5dfd290..583480d494 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -486,6 +486,10 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) /// The simulated controller chaining is: /// test_controller_name -> chain_ctrl_5 -> chain_ctrl_4 -> chain_ctrl_3 -> chain_ctrl_2 -> /// chain_ctrl_1 + /// + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported + /// from the controller B (or) the controller B is utilizing the expected interfaces exported from + /// the controller A // create server client and request rclcpp::executors::SingleThreadedExecutor srv_executor; @@ -628,6 +632,10 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) /// test_controller_name -> chain_ctrl_7 -> chain_ctrl_6 -> chain_ctrl_2 -> chain_ctrl_1 /// & /// chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 -> chain_ctrl_3 + /// + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported + /// from the controller B (or) the controller B is utilizing the expected interfaces exported from + /// the controller A // create server client and request rclcpp::executors::SingleThreadedExecutor srv_executor; @@ -826,6 +834,10 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) /// test_controller_name_2 -> chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 /// && /// test_controller_name_7 -> test_controller_name_8 + /// + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported + /// from the controller B (or) the controller B is utilizing the expected interfaces exported from + /// the controller A // create server client and request rclcpp::executors::SingleThreadedExecutor srv_executor;