diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4ffaef24aa..b144cd68f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -777,7 +777,7 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; // Reordering the controllers - std::sort( + std::stable_sort( to.begin(), to.end(), std::bind( &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, @@ -2453,7 +2453,16 @@ bool ControllerManager::controller_sorting( { // The case of the controllers that don't have any command interfaces. For instance, // joint_state_broadcaster - return true; + // If the controller b is also under the same condition, then maintain their initial order + if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + return false; + else + return true; + } + else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + { + // If only the controller b is a broadcaster or non chainable type , then swap the controllers + return false; } else { diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index ef1640a1d9..bc20236306 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1050,3 +1050,499 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // third tree ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); } + +TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) +{ + /// 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 + /// && + /// There are 100 more other basic controllers and 100 more different broadcasters to check for + /// crashing + /// 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_CHAINED_CONTROLLER_9[] = "test_chainable_controller_name_9"; + 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"}); + + auto test_chained_controller_9 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/max_acceleration"}}; + test_chained_controller_9->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_9->set_state_interface_configuration(state_cfg); + + unsigned int num_of_random_broadcasters = 100; + unsigned int num_of_random_controllers = 100; + std::vector chained_ref_interfaces; + for (size_t i = 0; i < num_of_random_controllers; i++) + { + chained_ref_interfaces.push_back("ref_" + std::to_string(i) + "/joint_2/acceleration"); + } + test_chained_controller_9->set_reference_interface_names(chained_ref_interfaces); + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_9) + std::string("/ref_") + std::to_string(i) + + std::string("/joint_2/acceleration")}}); + } + + // 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); + cm_->add_controller( + test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + 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( + 11u + num_of_random_broadcasters + num_of_random_controllers, 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_9, + 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}; + { + ControllerManagerRunner cm_runner(this); + for (const auto & controller : ctrls_order) + { + cm_->configure_controller(controller); + } + + for (auto random_ctrl : random_controllers_list) + { + cm_->configure_controller(random_ctrl.first); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + 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_chain_9_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_9); + + 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); + + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + ASSERT_GT(ctrl_chain_9_pos, get_ctrl_pos(controller_name)); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) +{ + /// The simulated controller chain is like every joint has its own controller exposing interfaces + /// and then a controller chain using those interfaces + /// + /// There are 20 more broadcasters + 20 more normal controllers for complexity + /// 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(); + + const unsigned int joints_count = 20; + + static constexpr char JOINT_CONTROLLER_PREFIX[] = "test_chainable_controller_name_joint_"; + static constexpr char FWD_CONTROLLER_PREFIX[] = "forward_controller_joint_"; + static constexpr char JOINT_SENSOR_BROADCASTER_PREFIX[] = "test_broadcaster_joint_"; + std::vector controllers_list; + std::vector fwd_joint_position_interfaces_list; + std::vector fwd_joint_velocity_interfaces_list; + std::vector fwd_joint_position_ref_interfaces_list; + std::vector fwd_joint_velocity_ref_interfaces_list; + std::unordered_map> + random_chainable_controllers_list; + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < joints_count; i++) + { + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position", "joint" + std::to_string(i) + "/velocity"}}; + // Joint controller + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[controller_name] = + std::make_shared(); + random_chainable_controllers_list[controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[controller_name]->set_command_interface_configuration( + chained_cmd_cfg); + random_chainable_controllers_list[controller_name]->set_reference_interface_names( + chained_state_cfg.names); + controllers_list.push_back(controller_name); + + // Forward Joint interfaces controller + fwd_joint_position_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/position"); + fwd_joint_velocity_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/velocity"); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[fwd_controller_name] = + std::make_shared(); + random_chainable_controllers_list[fwd_controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[fwd_controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {fwd_joint_position_interfaces_list.back(), fwd_joint_velocity_interfaces_list.back()}}); + random_chainable_controllers_list[fwd_controller_name]->set_reference_interface_names( + chained_state_cfg.names); + fwd_joint_position_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_position_interfaces_list.back()); + fwd_joint_velocity_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_velocity_interfaces_list.back()); + controllers_list.push_back(fwd_controller_name); + + // Add a broadcaster for every joint assuming it as a sensor (just for the tests) + const std::string broadcaster_name = JOINT_SENSOR_BROADCASTER_PREFIX + std::to_string(i); + random_controllers_list[broadcaster_name] = std::make_shared(); + random_controllers_list[broadcaster_name]->set_state_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/torque", "joint" + std::to_string(i) + "/torque"}}); + controllers_list.push_back(broadcaster_name); + } + + // create set of chained controllers + static constexpr char POSITION_REFERENCE_CONTROLLER[] = "position_reference_chainable_controller"; + static constexpr char VELOCITY_REFERENCE_CONTROLLER[] = "velocity_reference_chainable_controller"; + static constexpr char HIGHER_LEVEL_REFERENCE_CONTROLLER[] = "task_level_controller"; + + // Position reference controller + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_position_ref_interfaces_list}); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/position"}); + controllers_list.push_back(POSITION_REFERENCE_CONTROLLER); + + // Velocity reference controller + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_velocity_ref_interfaces_list}); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/velocity"}); + controllers_list.push_back(VELOCITY_REFERENCE_CONTROLLER); + + // Higher level task level controller + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER] = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(POSITION_REFERENCE_CONTROLLER) + "/joint/position", + std::string(VELOCITY_REFERENCE_CONTROLLER) + "/joint/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_command_interface_configuration( + cmd_cfg); + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_state_interface_configuration( + state_cfg); + controllers_list.push_back(HIGHER_LEVEL_REFERENCE_CONTROLLER); + + const unsigned int num_of_random_broadcasters = 20; + const unsigned int num_of_random_controllers = 20; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + controllers_list.push_back(controller_name); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_reference_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string("ref_") + std::to_string(i) + std::string("/joint_2/acceleration")}}); + controllers_list.push_back(controller_name); + } + + // Now shuffle the list to be able to configure controller later randomly + std::random_shuffle(controllers_list.begin(), controllers_list.end()); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_chain_ctrl : random_chainable_controllers_list) + { + cm_->add_controller( + random_chain_ctrl.second, random_chain_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + 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(controllers_list.size(), result->controller.size()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : controllers_list) + { + cm_->configure_controller(random_ctrl); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + 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); + }; + + // Check the controller indexing + auto pos_ref_pos = get_ctrl_pos(POSITION_REFERENCE_CONTROLLER); + auto vel_ref_pos = get_ctrl_pos(VELOCITY_REFERENCE_CONTROLLER); + auto task_level_ctrl_pos = get_ctrl_pos(HIGHER_LEVEL_REFERENCE_CONTROLLER); + ASSERT_GT(vel_ref_pos, task_level_ctrl_pos); + ASSERT_GT(pos_ref_pos, task_level_ctrl_pos); + + for (size_t i = 0; i < joints_count; i++) + { + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + + ASSERT_GT(get_ctrl_pos(fwd_controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(fwd_controller_name), vel_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), vel_ref_pos); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +}