diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index e7f3a6b345..13749ad980 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -133,9 +133,13 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ( - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 2288f675fd..4ed178fa1e 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -160,8 +160,13 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = - controller_->init(controller_name, controller_->robot_description_, 0, "", options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = controller_->robot_description_; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = options; + auto result = controller_->init(params); controller_->export_reference_interfaces(); assign_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index d594bef3e9..dfe07b294b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -131,9 +131,13 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ( - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/chained_filter_controller/test/test_chained_filter.cpp b/chained_filter_controller/test/test_chained_filter.cpp index 32ff4ffd38..c0ca7982c3 100644 --- a/chained_filter_controller/test/test_chained_filter.cpp +++ b/chained_filter_controller/test/test_chained_filter.cpp @@ -48,7 +48,13 @@ void ChainedFilterTest::SetUpController( auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameters); - const auto result = controller_->init(node_name, "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = node_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -61,8 +67,13 @@ TEST_F(ChainedFilterTest, InitReturnsSuccess) { SetUpController(); } TEST_F(ChainedFilterTest, InitFailureWithNoParams) { - const auto result = controller_->init( - "test_chained_filter_no_params", "", 0, "", controller_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_chained_filter_no_params"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + const auto result = controller_->init(params); EXPECT_EQ(result, controller_interface::return_type::ERROR); } diff --git a/chained_filter_controller/test/test_multiple_chained_filter.cpp b/chained_filter_controller/test/test_multiple_chained_filter.cpp index c29d52feed..31848d9b04 100644 --- a/chained_filter_controller/test/test_multiple_chained_filter.cpp +++ b/chained_filter_controller/test/test_multiple_chained_filter.cpp @@ -48,7 +48,13 @@ void MultipleChainedFilterTest::SetUpController( auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameters); - const auto result = controller_->init(node_name, "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = node_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 250d434405..49a7493e42 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -206,8 +206,13 @@ class TestDiffDriveController : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - - return controller_->init(controller_name, urdf_, 0, ns, node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = urdf_; + params.update_rate = 0; + params.node_namespace = ns; + params.node_options = node_options; + return controller_->init(params); } std::string controller_name; @@ -243,8 +248,13 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, init_fails_without_parameters) { - const auto ret = - controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = urdf_; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + const auto ret = controller_->init(params); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 0a74913ebb..83a576a16d 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -42,8 +42,13 @@ void JointGroupEffortControllerTest::SetUpController( auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameters); - const auto result = - controller_->init("test_joint_group_effort_controller", "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_joint_group_effort_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 3c36ece783..c1f24fa51b 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -54,8 +54,13 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster(std::string node_name) { - const auto result = - fts_broadcaster_->init(node_name, "", 0, "", fts_broadcaster_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = node_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = fts_broadcaster_->define_custom_node_options(); + const auto result = fts_broadcaster_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 75bb97d18b..4cf7532e1e 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -54,8 +54,13 @@ void ForwardCommandControllerTest::SetUpController( { auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameters); - - const auto result = controller_->init("forward_command_controller", "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "forward_command_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index e4f6a5b735..a1fd896418 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -65,8 +65,13 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController( auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameter_overrides); - const auto result = - controller_->init("multi_interface_forward_command_controller", "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "multi_interface_forward_command_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 24190edc01..f3d8eeda55 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -199,6 +199,17 @@ class GpioCommandControllerTestSuite : public ::testing::Test std::unique_ptr controller_; + controller_interface::ControllerInterfaceParams create_ctrl_params( + const rclcpp::NodeOptions & node_options, const std::string & robot_description = "") + { + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_gpio_command_controller"; + params.robot_description = robot_description; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + return params; + } const std::vector gpio_names{"gpio1", "gpio2"}; std::vector gpio_commands{1.0, 0.0, 3.1}; std::vector gpio_states{1.0, 0.0, 3.1}; @@ -221,9 +232,8 @@ class GpioCommandControllerTestSuite : public ::testing::Test TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) { - const auto result = controller_->init( - "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", - controller_->define_custom_node_options()); + const auto result = controller_->init(create_ctrl_params( + controller_->define_custom_node_options(), ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -231,7 +241,7 @@ TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) { const auto node_options = create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -242,7 +252,7 @@ TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInit {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -251,7 +261,7 @@ TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetIni { const auto node_options = create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -267,7 +277,7 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -280,8 +290,7 @@ TEST_F( {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); const auto result = controller_->init( - "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", - node_options); + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -294,8 +303,8 @@ TEST_F( {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller_->init( - "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + const auto result = + controller_->init(create_ctrl_params(node_options, minimal_robot_urdf_with_gpio)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -309,7 +318,7 @@ TEST_F( {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -323,7 +332,7 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -341,7 +350,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -369,7 +378,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -398,7 +407,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -428,8 +437,7 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); assert_default_command_and_state_values(); update_controller_loop(); assert_default_command_and_state_values(); @@ -445,8 +453,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), @@ -467,8 +474,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); const auto command = createGpioCommand( {"gpio1", "gpio2"}, @@ -489,8 +495,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), @@ -513,8 +518,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); const auto command = createGpioCommand( {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), @@ -537,8 +541,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); @@ -560,8 +563,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); const auto command = createGpioCommand( {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), @@ -584,8 +586,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); auto command_pub = node->create_publisher( std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); @@ -609,8 +610,7 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state( - controller_->init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state(controller_->init(create_ctrl_params(node_options))); auto subscription = node->create_subscription( std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, @@ -649,8 +649,8 @@ TEST_F( state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); state_interfaces.emplace_back(gpio_2_ana_state, nullptr); - const auto result_of_initialization = controller_->init( - "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + const auto result_of_initialization = + controller_->init(create_ctrl_params(node_options, minimal_robot_urdf_with_gpio)); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); @@ -684,9 +684,7 @@ TEST_F( std::vector state_interfaces; state_interfaces.emplace_back(gpio_1_1_dig_state, nullptr); state_interfaces.emplace_back(gpio_2_ana_state, nullptr); - - const auto result_of_initialization = - controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result_of_initialization = controller_->init(create_ctrl_params(node_options)); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); controller_->assign_interfaces({}, std::move(state_interfaces)); diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp index ccc0c6dcc4..d3bb1773d7 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -105,6 +105,18 @@ class GPSSensorBroadcasterTest : public ::testing::Test return gps_msg; } + controller_interface::ControllerInterfaceParams create_ctrl_params( + const rclcpp::NodeOptions & node_options, const std::string & robot_description = "") + { + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_gps_sensor_broadcaster"; + params.robot_description = robot_description; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + return params; + } + protected: const rclcpp::Parameter sensor_name_param_ = rclcpp::Parameter("sensor_name", "gps_sensor"); const std::string sensor_name_ = sensor_name_param_.get_value(); @@ -140,9 +152,8 @@ class GPSSensorBroadcasterTest : public ::testing::Test TEST_F(GPSSensorBroadcasterTest, whenNoParamsAreSetThenInitShouldFail) { - const auto result = gps_broadcaster_->init( - "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - gps_broadcaster_->define_custom_node_options()); + const auto result = gps_broadcaster_->init(create_ctrl_params( + gps_broadcaster_->define_custom_node_options(), ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -150,8 +161,7 @@ TEST_F(GPSSensorBroadcasterTest, whenOnlySensorNameIsSetThenInitShouldFail) { const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_}); const auto result = gps_broadcaster_->init( - "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - node_options); + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -162,8 +172,7 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_}); const auto result = gps_broadcaster_->init( - "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - node_options); + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); @@ -177,8 +186,7 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_}); const auto result = gps_broadcaster_->init( - "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - node_options); + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); @@ -209,8 +217,7 @@ TEST_F( {"static_position_covariance", std::vector{static_covariance.begin(), static_covariance.end()}}}); const auto result = gps_broadcaster_->init( - "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - node_options); + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); @@ -236,9 +243,9 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {sensor_name_param_, frame_id_, {"read_covariance_from_interface", true}}); + const auto result = gps_broadcaster_->init( - "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - node_options); + create_ctrl_params(node_options, ros2_control_test_assets::minimal_robot_urdf)); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 2246e69b43..daf0b7f89a 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -59,9 +59,13 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster( { auto node_options = imu_broadcaster_->define_custom_node_options(); node_options.parameter_overrides(parameters); - - const auto result = - imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_imu_sensor_broadcaster"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = imu_broadcaster_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 58a34d7aa8..d5c01625b6 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -70,9 +70,13 @@ void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::string & robot_description, const std::vector & joint_names, const std::vector & interfaces) { - const auto result = state_broadcaster_->init( - "joint_state_broadcaster", robot_description, 0, "", - state_broadcaster_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "joint_state_broadcaster"; + params.robot_description = robot_description; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = state_broadcaster_->define_custom_node_options(); + const auto result = state_broadcaster_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); @@ -952,8 +956,13 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest) { - const auto result = state_broadcaster_->init( - "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "joint_state_broadcaster"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = state_broadcaster_->define_custom_node_options(); + const auto result = state_broadcaster_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); custom_joint_value_ = 12.34; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index ef3c8b8054..5b7da75390 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -284,8 +284,13 @@ class TrajectoryControllerTest : public ::testing::Test node_options.parameter_overrides(parameter_overrides); traj_controller_->set_node_options(node_options); - return traj_controller_->init( - controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name_; + params.robot_description = urdf; + params.update_rate = 100; + params.node_namespace = ""; + params.node_options = traj_controller_->define_custom_node_options(); + return traj_controller_->init(params); } void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 4c9cad780b..c519e00eb0 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -173,10 +173,13 @@ class MecanumDriveControllerFixture : public ::testing::Test const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions(), const std::string ns = "") { const auto urdf = ""; - - ASSERT_EQ( - controller_->init(controller_name, urdf, 0, ns, node_options), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = urdf; + params.update_rate = 0; + params.node_namespace = ns; + params.node_options = node_options; + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); std::vector loaned_command_ifs; command_itfs_.reserve(joint_command_values_.size()); diff --git a/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp index 3030e74d42..52b53ff486 100644 --- a/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp +++ b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp @@ -78,11 +78,13 @@ class MotionPrimitivesForwardControllerFixture : public ::testing::Test void SetUp() override { controller_ = std::make_unique(); - ASSERT_EQ( - controller_->init( - "test_motion_primitives_forward_controller", "", 0, "", - controller_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_motion_primitives_forward_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); node_ = std::make_shared("test_node"); diff --git a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp index d94d201378..a4214e3ac1 100644 --- a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp +++ b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp @@ -26,8 +26,14 @@ class OmniWheelDriveControllerTest TEST_F(OmniWheelDriveControllerTest, init_fails_without_parameters) { - const auto ret = - controller_->init(controller_name_, urdf_, 0, "", controller_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name_; + params.robot_description = urdf_; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + + const auto ret = controller_->init(params); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp index 896d76b911..649d9f65e0 100644 --- a/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp +++ b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp @@ -227,7 +227,13 @@ class OmniWheelDriveControllerFixture : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - return controller_->init(controller_name_, urdf_, 0, ns, node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name_; + params.robot_description = urdf_; + params.update_rate = 0; + params.node_namespace = ns; + params.node_options = node_options; + return controller_->init(params); } std::vector wheels_pos_states_ = {1, 1, 1, 1}; diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 1af3576462..bbe99c7b46 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -50,8 +50,14 @@ void GripperControllerTest::SetUpController( const std::string & controller_name = "test_gripper_action_position_controller", controller_interface::return_type expected_result = controller_interface::return_type::OK) { - const auto result = - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + + const auto result = controller_->init(params); ASSERT_EQ(result, expected_result); std::vector command_ifs; diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 3efb170af5..5d2afc9871 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -151,9 +151,13 @@ class PidControllerFixture : public ::testing::Test command_publisher_ = command_publisher_node_->create_publisher( "/" + controller_name + "/reference", rclcpp::SystemDefaultsQoS()); - ASSERT_EQ( - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); std::vector loaned_command_ifs; command_itfs_.reserve(dof_names_.size()); diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 6006cae8b4..737661ac78 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -26,10 +26,14 @@ void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } void PoseBroadcasterTest::SetUpPoseBroadcaster() { - ASSERT_EQ( - pose_broadcaster_->init( - "test_pose_broadcaster", "", 0, "", pose_broadcaster_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_pose_broadcaster"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = pose_broadcaster_->define_custom_node_options(); + + ASSERT_EQ(pose_broadcaster_->init(params), controller_interface::return_type::OK); std::vector state_interfaces; state_interfaces.emplace_back(pose_position_x_, nullptr); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index bf0d315bac..d0125c7e14 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -42,8 +42,13 @@ void JointGroupPositionControllerTest::SetUpController( auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameters); - const auto result = - controller_->init("test_joint_group_position_controller", "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_joint_group_position_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index e2fa00685f..93aae56ac6 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -39,8 +39,14 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) { controller_interface::return_type result = controller_interface::return_type::ERROR; - result = range_broadcaster_->init( - broadcaster_name, "", 0, "", range_broadcaster_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = broadcaster_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = range_broadcaster_->define_custom_node_options(); + + result = range_broadcaster_->init(params); if (controller_interface::return_type::OK == result) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index e1b22ae0fa..30b8e36bec 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -153,9 +153,13 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ( - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 583f086412..545283d5f2 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -168,7 +168,13 @@ class TestTricycleController : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - return controller_->init(controller_name, urdf_, 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = urdf_; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + return controller_->init(params); } const std::string controller_name = "test_tricycle_controller"; @@ -201,8 +207,14 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, init_fails_without_parameters) { - const auto ret = - controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "controller_name"; + params.robot_description = urdf_; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + + const auto ret = controller_->init(params); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 8517773e96..91597ea89d 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -132,9 +132,13 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ( - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), - controller_interface::return_type::OK); + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index de138571af..718de55dee 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -42,8 +42,13 @@ void JointGroupVelocityControllerTest::SetUpController( auto node_options = controller_->define_custom_node_options(); node_options.parameter_overrides(parameters); - const auto result = - controller_->init("test_joint_group_velocity_controller", "", 0, "", node_options); + controller_interface::ControllerInterfaceParams params; + params.controller_name = "test_joint_group_velocity_controller"; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = node_options; + const auto result = controller_->init(params); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs;