Skip to content

Commit

Permalink
Added test case to sort chained controllers with branching
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Jun 27, 2023
1 parent cf20fd7 commit 860d486
Showing 1 changed file with 197 additions and 0 deletions.
197 changes: 197 additions & 0 deletions controller_manager/test/test_controller_manager_srvs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -621,3 +621,200 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
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

// create server client and request
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<ListControllers>::SharedPtr client =
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
auto request = std::make_shared<ListControllers::Request>();

// 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<TestChainableController>();
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<TestChainableController>();
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<TestChainableController>();
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<TestChainableController>();
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<TestChainableController>();
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<TestChainableController>();
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<TestChainableController>();
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<TestController>();
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)
{
return std::find_if(
result->controller.begin(), result->controller.end(),
[controller_name](auto itf)
{ return (itf.name.find(controller_name) != std::string::npos); });
};
auto ctrl_1_it = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1);
auto ctrl_3_it = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3);
auto ctrl_6_it = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6);
ASSERT_NE(ctrl_1_it, result->controller.end());
ASSERT_NE(ctrl_3_it, result->controller.end());
auto ctrl_1_pos = std::distance(result->controller.begin(), ctrl_1_it);
auto ctrl_3_pos = std::distance(result->controller.begin(), ctrl_3_it);
auto ctrl_6_pos = std::distance(result->controller.begin(), ctrl_6_it);

// 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_EQ(result->controller[ctrl_1_pos].name, TEST_CHAINED_CONTROLLER_1);
ASSERT_EQ(result->controller[ctrl_1_pos - 1].name, TEST_CHAINED_CONTROLLER_2);

// second branch
ASSERT_EQ(result->controller[ctrl_3_pos].name, TEST_CHAINED_CONTROLLER_3);
ASSERT_EQ(result->controller[ctrl_3_pos - 1].name, TEST_CHAINED_CONTROLLER_4);
ASSERT_EQ(result->controller[ctrl_3_pos - 2].name, TEST_CHAINED_CONTROLLER_5);
}

0 comments on commit 860d486

Please sign in to comment.