From 2ba3dd2c7fe6dfaf0218c8f1189b8aa93cb5dddb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 11 Aug 2023 14:16:16 +0200 Subject: [PATCH 1/2] Controller sorting and proper execution in a chain (Fixes #853) (#1063) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added test for the reordering controllers case * Perform controller sorting at the end of switch_controller * fix the list_chained_controllers_srv test case for the new controller sorting * move the logic to a separate function * Apply suggestions from code review Co-authored-by: Christoph Fröhlich * remove obsolete TODO comments in the controller chaining tests * Add a test case to sort 6 controllers in chained mode * Added a method to retrieve the following controller names given a controller name * Update the controller_sorting method to support progressive chaining ability * Added test case to sort chained controllers with branching * Added a method to retrieve the preceding controller names given a controller name * Added logic to controller_sorting to support sorting branched controller chains * Added some documentation to the newly added functions * Add debug print of reordered controllers list once they are sorted * Add the condition to skip the non-configured controllers * remove logging for every command interface * Improve the complex chain test case checking * added a test case to sort independent chains * Added fixes for the independent chains sorting * better randomization in independent chains testing * Fix minor logic issues * Add 3rd chain for better complex testing * Apply suggestions from code review - Denis Co-authored-by: Dr. Denis * address pull request review comments --------- Co-authored-by: Christoph Fröhlich Co-authored-by: Dr. Denis Co-authored-by: Bence Magyar --- .../controller_manager/controller_manager.hpp | 23 + controller_manager/src/controller_manager.cpp | 224 +++++++ .../test/test_controller_manager_srvs.cpp | 601 +++++++++++++++++- ...llers_chaining_with_controller_manager.cpp | 130 +++- 4 files changed, 965 insertions(+), 13 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index d33d146cd8..32e8e13df8 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -349,6 +349,29 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// A method to be used in the std::sort method to sort the controllers to be able to + /// execute them in a proper order + /** + * 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 + * 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 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. + * + * @return true, if ctrl_a needs to execute first, else false + */ + bool controller_sorting( + const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, + const std::vector & controllers); + std::shared_ptr executor_; std::shared_ptr> loader_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 69af65961e..bea276087a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -124,6 +124,123 @@ bool command_interface_is_reference_interface_of_controller( return true; } +/** + * A method to retrieve the names of all it's following controllers given a controller name + * 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 + * empty. + */ +std::vector get_following_controller_names( + const std::string controller_name, + const std::vector & controllers) +{ + std::vector following_controllers; + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); + + 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) + { + controller_manager::ControllersListIterator ctrl_it; + if (command_interface_is_reference_interface_of_controller(itf, controllers, ctrl_it)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "The interface is a reference interface of controller : %s", ctrl_it->info.name.c_str()); + following_controllers.push_back(ctrl_it->info.name); + const std::vector ctrl_names = + get_following_controller_names(ctrl_it->info.name, controllers); + for (const std::string & controller : ctrl_names) + { + if ( + std::find(following_controllers.begin(), following_controllers.end(), controller) == + following_controllers.end()) + { + // Only add to the list if it doesn't exist + following_controllers.push_back(controller); + } + } + } + } + return following_controllers; +} + +/** + * A method to retrieve the names of all it's preceding controllers given a controller name + * 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 + * empty. + */ +std::vector get_preceding_controller_names( + const std::string controller_name, + const std::vector & controllers) +{ + std::vector preceding_controllers; + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); + return preceding_controllers; + } + for (const auto & ctrl : controllers) + { + // If the controller is not configured, return empty + if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) + return preceding_controllers; + auto cmd_itfs = ctrl.c->command_interface_configuration().names; + for (const auto & itf : cmd_itfs) + { + if (itf.find(controller_name) != std::string::npos) + { + preceding_controllers.push_back(ctrl.info.name); + auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); + for (const std::string & controller : ctrl_names) + { + if ( + std::find(preceding_controllers.begin(), preceding_controllers.end(), controller) == + preceding_controllers.end()) + { + // Only add to the list if it doesn't exist + preceding_controllers.push_back(controller); + } + } + } + } + } + return preceding_controllers; +} + } // namespace namespace controller_manager @@ -936,6 +1053,20 @@ controller_interface::return_type ControllerManager::switch_controller( controller.info.claimed_interfaces.clear(); } } + + // Reordering the controllers + std::sort( + to.begin(), to.end(), + std::bind( + &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, + to)); + + RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); + for (const auto & ctrl : to) + { + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); + } + // switch lists rt_controllers_wrapper_.switch_updated_list(guard); // clear unused list @@ -2078,6 +2209,99 @@ 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, + const std::vector & controllers) +{ + // If the controllers are not active, then should be at the end of the list + if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) + { + if (is_controller_active(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()) + { + // The case of the controllers that don't have any command interfaces. For instance, + // joint_state_broadcaster + return true; + } + else + { + auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers); + if (following_ctrls.empty()) return false; + // If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b + if ( + std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) != + following_ctrls.end()) + return true; + else + { + auto ctrl_a_preceding_ctrls = get_preceding_controller_names(ctrl_a.info.name, controllers); + // This is to check that the ctrl_b is in the preceding controllers list of ctrl_a - This + // check is useful when there is a chained controller branching, but they belong to same + // branch + if ( + std::find(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), ctrl_b.info.name) != + ctrl_a_preceding_ctrls.end()) + { + return false; + } + + // This is to handle the cases where, the parsed ctrl_a and ctrl_b are not directly related + // but might have a common parent - happens in branched chained controller + auto ctrl_b_preceding_ctrls = get_preceding_controller_names(ctrl_b.info.name, controllers); + std::sort(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end()); + std::sort(ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end()); + std::list intersection; + std::set_intersection( + ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), + ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end(), + std::back_inserter(intersection)); + if (!intersection.empty()) + { + // If there is an intersection, then there is a common parent controller for both ctrl_a and + // ctrl_b + return true; + } + + // If there is no common parent, then they belong to 2 different sets + auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); + if (following_ctrls_b.empty()) return true; + auto find_first_element = [&](const auto & controllers_list) + { + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back())); + if (it != controllers.end()) + { + int dist = std::distance(controllers.begin(), it); + return dist; + } + }; + const int ctrl_a_chain_first_controller = find_first_element(following_ctrls); + const int ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); + if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) return true; + } + + // 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 + 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); }); + if (state_it != state_itfs.end()) + { + return false; + } + + // The rest of the cases, basically end up at the end of the list + return false; + } }; } // namespace controller_manager diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 68fe01946e..52d4f45a5c 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -291,19 +291,19 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); // get controller list after activate result = call_service_and_wait(*client, request, srv_executor); - // check chainable controller - ASSERT_EQ(result->controller[0].state, "active"); - ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 1u); - ASSERT_EQ(result->controller[0].is_chained, true); // check test controller + ASSERT_EQ(result->controller[0].state, "active"); + ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 3u); + // check chainable controller ASSERT_EQ(result->controller[1].state, "active"); - ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 1u); + ASSERT_EQ(result->controller[1].is_chained, true); ASSERT_EQ( test_chainable_controller::TEST_CONTROLLER_NAME, - result->controller[1].chain_connections[0].name); - ASSERT_EQ(2u, result->controller[1].chain_connections[0].reference_interfaces.size()); - ASSERT_EQ("joint1/position", result->controller[1].chain_connections[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[1].chain_connections[0].reference_interfaces[1]); + result->controller[0].chain_connections[0].name); + ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size()); + ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]); } TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) @@ -482,3 +482,586 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, cm_->get_loaded_controllers()[0].c->get_state().id()); } + +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; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + static constexpr char TEST_CHAINED_CONTROLLER_3[] = "test_chainable_controller_name_3"; + static constexpr char TEST_CHAINED_CONTROLLER_4[] = "test_chainable_controller_name_4"; + static constexpr char TEST_CHAINED_CONTROLLER_5[] = "test_chainable_controller_name_5"; + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_3 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position"}}; + test_chained_controller_3->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_3->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_3->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position"}}; + test_chained_controller_4->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_4->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_4->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint1/position"}}; + test_chained_controller_5->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_5->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_5->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_5) + "/joint1/velocity", "joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(6u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[5].name, "test_controller_name"); + + // configure controllers + for (const auto & controller : + {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, + TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, + test_controller::TEST_CONTROLLER_NAME}) + cm_->configure_controller(controller); + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(6u, result->controller.size()); + + // activate controllers + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, + TEST_CHAINED_CONTROLLER_4}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + // get controller list after activate + result = call_service_and_wait(*client, request, srv_executor); + + ASSERT_EQ(6u, result->controller.size()); + // reordered controllers + ASSERT_EQ(result->controller[0].name, "test_controller_name"); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); + ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_1); + RCLCPP_ERROR(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) +{ + /// The simulated controller chain branching is: + /// 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; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + static constexpr char TEST_CHAINED_CONTROLLER_3[] = "test_chainable_controller_name_3"; + static constexpr char TEST_CHAINED_CONTROLLER_4[] = "test_chainable_controller_name_4"; + static constexpr char TEST_CHAINED_CONTROLLER_5[] = "test_chainable_controller_name_5"; + static constexpr char TEST_CHAINED_CONTROLLER_6[] = "test_chainable_controller_name_6"; + static constexpr char TEST_CHAINED_CONTROLLER_7[] = "test_chainable_controller_name_7"; + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity", "joint2/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_3 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_chained_controller_3->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_3->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_3->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint2/velocity"}}; + test_chained_controller_4->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_4->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_4->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint2/velocity"}}; + test_chained_controller_5->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_5->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_5->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_6 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_5) + "/joint2/velocity"}}; + test_chained_controller_6->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_6->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_6->set_reference_interface_names({"joint1/position", "joint2/velocity"}); + + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/velocity"}}; + test_chained_controller_7->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_7->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_7->set_reference_interface_names({"joint1/position", "joint2/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_7) + "/joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(8u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_6); + ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[6].name, "test_controller_name"); + ASSERT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_7); + + // configure controllers + for (const auto & controller : + {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, + TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, + TEST_CHAINED_CONTROLLER_6, test_controller::TEST_CONTROLLER_NAME}) + cm_->configure_controller(controller); + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(8u, result->controller.size()); + + // activate controllers + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_3}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, + TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + // get controller list after activate + result = call_service_and_wait(*client, request, srv_executor); + + ASSERT_EQ(8u, result->controller.size()); + // reordered controllers + ASSERT_EQ(result->controller[0].name, "test_controller_name"); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_6); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + auto ctrl_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + + // Extra check to see that they are index only after their parent controller (ctrl_6) + ASSERT_GT(ctrl_3_pos, ctrl_6_pos); + ASSERT_GT(ctrl_1_pos, ctrl_6_pos); + + // first branch + ASSERT_GT(ctrl_2_pos, ctrl_6_pos); + ASSERT_GT(ctrl_1_pos, ctrl_2_pos); + + // second branch + ASSERT_GT(ctrl_5_pos, ctrl_6_pos); + ASSERT_GT(ctrl_4_pos, ctrl_5_pos); + ASSERT_GT(ctrl_3_pos, ctrl_4_pos); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) +{ + /// The simulated controller chaining is: + /// test_controller_name_1 -> chain_ctrl_3 -> chain_ctrl_2 -> chain_ctrl_1 + /// && + /// 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; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + static constexpr char TEST_CHAINED_CONTROLLER_3[] = "test_chainable_controller_name_3"; + static constexpr char TEST_CHAINED_CONTROLLER_4[] = "test_chainable_controller_name_4"; + static constexpr char TEST_CHAINED_CONTROLLER_5[] = "test_chainable_controller_name_5"; + static constexpr char TEST_CHAINED_CONTROLLER_6[] = "test_chainable_controller_name_6"; + static constexpr char TEST_CHAINED_CONTROLLER_7[] = "test_chainable_controller_name_7"; + static constexpr char TEST_CHAINED_CONTROLLER_8[] = "test_chainable_controller_name_8"; + static constexpr char TEST_CONTROLLER_1[] = "test_controller_name_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_name_2"; + + // First chain + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_3 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position"}}; + test_chained_controller_3->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_3->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_3->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + // Second chain + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_chained_controller_4->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_4->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_4->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint2/velocity"}}; + test_chained_controller_5->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_5->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_5->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_6 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint2/velocity"}}; + test_chained_controller_6->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_6->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_6->set_reference_interface_names({"joint2/velocity"}); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/velocity"}}; + state_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // Third chain + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}}; + test_chained_controller_7->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_7->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_7->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_8 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint3/velocity"}}; + test_chained_controller_8->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_8->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_8->set_reference_interface_names({"joint3/velocity"}); + + // add controllers + /// @todo add controllers in random order + /// For now, adding the ordered case to see that current sorting doesn't change order + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ(10u, result->controller.size()); + EXPECT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_2); + EXPECT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_6); + EXPECT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1); + EXPECT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_7); + EXPECT_EQ(result->controller[4].name, TEST_CONTROLLER_1); + + EXPECT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_5); + EXPECT_EQ(result->controller[6].name, TEST_CHAINED_CONTROLLER_3); + EXPECT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_4); + EXPECT_EQ(result->controller[8].name, TEST_CONTROLLER_2); + EXPECT_EQ(result->controller[9].name, TEST_CHAINED_CONTROLLER_8); + + // configure controllers + auto ctrls_order = {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, + TEST_CHAINED_CONTROLLER_1, TEST_CONTROLLER_1, + TEST_CHAINED_CONTROLLER_4, TEST_CONTROLLER_2, + TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, + TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; + for (const auto & controller : ctrls_order) cm_->configure_controller(controller); + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(10u, result->controller.size()); + + // activate controllers + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_1}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_4}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_7}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, + TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8}, + {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + cm_->switch_controller( + {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + // get controller list after activate + result = call_service_and_wait(*client, request, srv_executor); + + ASSERT_EQ(10u, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + auto ctrl_chain_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_chain_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_chain_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_chain_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_chain_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_chain_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + auto ctrl_chain_7_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_7); + auto ctrl_chain_8_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_8); + + auto ctrl_1_pos = get_ctrl_pos(TEST_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CONTROLLER_2); + + // Extra check to see that they are indexed after their parent controller + // first chain + ASSERT_GT(ctrl_chain_1_pos, ctrl_chain_2_pos); + ASSERT_GT(ctrl_chain_2_pos, ctrl_chain_3_pos); + ASSERT_GT(ctrl_chain_3_pos, ctrl_1_pos); + + // second tree + ASSERT_GT(ctrl_chain_4_pos, ctrl_chain_5_pos); + ASSERT_GT(ctrl_chain_5_pos, ctrl_chain_6_pos); + ASSERT_GT(ctrl_chain_6_pos, ctrl_2_pos); + + // third tree + ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); +} diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 0d798a1b79..31d2cadcf7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -42,6 +42,8 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -65,6 +67,8 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); public: TestableControllerManager( @@ -610,11 +614,129 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); } -// TODO(destogl): Add test case with controllers added in "random" order +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) +{ + SetupControllers(); -// TODO(destogl): Think about strictness and chaining controllers -// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain -// + // add all controllers in random order to test the sorting + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER + // (otherwise, interface will be missing) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); + + // Diff-Drive Controller claims the reference interfaces of PID controllers + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); + + // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + // 'rot_z' reference interface is not claimed + for (const auto & interface : {"diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // update controllers + std::vector reference = {32.0, 128.0}; + + // update 'Position Tracking' controller + for (auto & value : diff_drive_controller->reference_interfaces_) + { + ASSERT_EQ(value, 0.0); // default reference values are 0.0 + } + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + + ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller + ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + + // update 'Diff Drive' Controller + diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + // default reference values are 0.0 - they should be changed now + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + // update PID controllers that are writing to hardware + pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + + // update hardware ('read' is sufficient for test hardware) + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // 32 - 0 + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + // 32 / 2 + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + // 128 - 0 + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + // 128 / 2 + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + + // update all controllers at once and see that all have expected values --> also checks the order + // of controller execution + + reference = {1024.0, 4096.0}; + UpdateAllControllerAndCheck(reference, 3u); +} INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, From 3995e8f121d84dfc424d197cf5a3c1d9841d12ea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 Nov 2023 15:19:20 +0100 Subject: [PATCH 2/2] Sort controllers while configuring instead of while activating (#1107) --- controller_manager/src/controller_manager.cpp | 62 ++++++++------- .../test_chainable_controller.cpp | 24 +----- .../test/test_controller/test_controller.cpp | 24 +----- .../test/test_controller_manager.cpp | 22 ++++-- .../test/test_controller_manager_srvs.cpp | 79 +++---------------- .../test/test_load_controller.cpp | 15 +++- 6 files changed, 77 insertions(+), 149 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index bea276087a..acf24095b7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -154,9 +154,6 @@ 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) { @@ -215,13 +212,11 @@ std::vector get_preceding_controller_names( } for (const auto & ctrl : controllers) { - // If the controller is not configured, return empty - if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) - return preceding_controllers; auto cmd_itfs = ctrl.c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { - if (itf.find(controller_name) != std::string::npos) + auto split_pos = itf.find_first_of('/'); + if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name)) { preceding_controllers.push_back(ctrl.info.name); auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); @@ -629,10 +624,35 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + } + + // Now let's reorder the controllers + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + + // Copy all controllers from the 'from' list to the 'to' list + to = from; + + // Reordering the controllers + std::sort( + to.begin(), to.end(), + std::bind( + &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, + to)); - // TODO(destogl): check and resort controllers in the vector + RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); + for (const auto & ctrl : to) + { + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); } + // switch lists + rt_controllers_wrapper_.switch_updated_list(guard); + // clear unused list + rt_controllers_wrapper_.get_unused_list(guard).clear(); + return controller_interface::return_type::OK; } @@ -1054,19 +1074,6 @@ controller_interface::return_type ControllerManager::switch_controller( } } - // Reordering the controllers - std::sort( - to.begin(), to.end(), - std::bind( - &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, - to)); - - RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); - for (const auto & ctrl : to) - { - RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); - } - // switch lists rt_controllers_wrapper_.switch_updated_list(guard); // clear unused list @@ -2215,13 +2222,6 @@ bool ControllerManager::controller_sorting( const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers) { - // If the controllers are not active, then should be at the end of the list - if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) - { - if (is_controller_active(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()) @@ -2293,7 +2293,11 @@ bool ControllerManager::controller_sorting( // 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); }); + [ctrl_b](auto itf) + { + auto index = itf.find_first_of('/'); + return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name)); + }); if (state_it != state_itfs.end()) { return false; 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 ba1132e68b..3065fcf2eb 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -32,33 +32,13 @@ TestChainableController::TestChainableController() controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { - 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."); - } + return cmd_iface_cfg_; } controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { - 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."); - } + return state_iface_cfg_; } 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 b6eac9b689..cee8296bb1 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -29,32 +29,12 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { - 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."); - } + return cmd_iface_cfg_; } controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { - 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."); - } + return state_iface_cfg_; } controller_interface::return_type TestController::update( diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6db4cfd1b2..a5566ee9b5 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - cm_->configure_controller(TEST_CONTROLLER2_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); @@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); @@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); @@ -390,7 +399,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 52d4f45a5c..e598165eab 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(2u, result->controller.size()); + // At this stage, the controllers are already reordered // check chainable controller ASSERT_EQ(result->controller[0].state, "inactive"); ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u); + ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u); ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[0].is_chainable, true); + ASSERT_EQ(result->controller[0].is_chainable, false); ASSERT_EQ(result->controller[0].is_chained, false); - ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]); + ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u); + ASSERT_EQ(result->controller[0].chain_connections.size(), 1u); - ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u); ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[1].is_chainable, false); + ASSERT_EQ(result->controller[1].is_chainable, true); ASSERT_EQ(result->controller[1].is_chained, false); - ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].chain_connections.size(), 1u); + ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u); + ASSERT_EQ(result->controller[1].chain_connections.size(), 0u); + ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]); // activate controllers cm_->switch_controller( {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, @@ -602,22 +603,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(6u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_4}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(6u, result->controller.size()); // reordered controllers ASSERT_EQ(result->controller[0].name, "test_controller_name"); ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5); @@ -775,25 +760,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(8u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(8u, result->controller.size()); // reordered controllers ASSERT_EQ(result->controller[0].name, "test_controller_name"); ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); @@ -1008,29 +974,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(10u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_4}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_7}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(10u, result->controller.size()); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int { auto it = std::find_if( diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ca2f7f6b1c..390a7365f0 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Activate configured controller - cm_->configure_controller(controller_name1); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name1); + } start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state test_controller->simulate_cleanup_failure = false; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + } ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); - cm_->configure_controller(controller_name2); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name2); + } // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1");