Skip to content

Commit

Permalink
Add 3rd chain for better complex testing
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Jul 7, 2023
1 parent 9d33f4a commit 3ef00f3
Showing 1 changed file with 79 additions and 27 deletions.
106 changes: 79 additions & 27 deletions controller_manager/test/test_controller_manager_srvs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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";

Expand Down Expand Up @@ -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<TestChainableController>();
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<TestChainableController>();
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
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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);
}

0 comments on commit 3ef00f3

Please sign in to comment.