From 3ef00f3386d8b194f133f8936f764e15c218391a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 29 Jun 2023 16:04:15 +0200 Subject: [PATCH] Add 3rd chain for better complex testing --- .../test/test_controller_manager_srvs.cpp | 106 +++++++++++++----- 1 file changed, 79 insertions(+), 27 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 2d4fa787db..ccc5dfd290 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -824,6 +824,8 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) /// 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 // create server client and request rclcpp::executors::SingleThreadedExecutor srv_executor; @@ -840,6 +842,8 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) 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"; @@ -913,6 +917,22 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) 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 @@ -925,6 +945,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) 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( @@ -938,32 +961,38 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) 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(8u, result->controller.size()); + 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_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[4].name, TEST_CHAINED_CONTROLLER_5); - EXPECT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_3); - EXPECT_EQ(result->controller[6].name, TEST_CHAINED_CONTROLLER_4); - EXPECT_EQ(result->controller[7].name, TEST_CONTROLLER_2); + 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, - }; + 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(8u, result->controller.size()); + ASSERT_EQ(10u, result->controller.size()); // activate controllers cm_->switch_controller( @@ -972,9 +1001,12 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) 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_6, TEST_CHAINED_CONTROLLER_8}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); cm_->switch_controller( @@ -983,19 +1015,39 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // get controller list after activate result = call_service_and_wait(*client, request, srv_executor); - ASSERT_EQ(8u, result->controller.size()); - // reordered controllers - for (auto & ctrl : result->controller) + ASSERT_EQ(10u, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int { - RCLCPP_ERROR(srv_node->get_logger(), "\t%s", ctrl.name.c_str()); - } - EXPECT_EQ(result->controller[0].name, TEST_CONTROLLER_1); - EXPECT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_3); - EXPECT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2); - EXPECT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_1); - - EXPECT_EQ(result->controller[4].name, TEST_CONTROLLER_2); - EXPECT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_6); - EXPECT_EQ(result->controller[6].name, TEST_CHAINED_CONTROLLER_5); - EXPECT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_4); + 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); }