From 9549b42214e67d57641f24687ab9779d41402c39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:47:09 +0100 Subject: [PATCH 1/2] [diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958) (cherry picked from commit 2705ca8530ab9dae7bc77747541a7c35ceb4d1f3) # Conflicts: # diff_drive_controller/test/test_diff_drive_controller.cpp --- diff_drive_controller/CMakeLists.txt | 6 +- .../diff_drive_controller.hpp | 6 + .../src/diff_drive_controller.cpp | 16 +- .../src/diff_drive_controller_parameter.yaml | 15 +- .../config/test_diff_drive_controller.yaml | 3 +- .../test/test_diff_drive_controller.cpp | 264 +++++++++++------- .../test/test_load_diff_drive_controller.cpp | 10 +- 7 files changed, 195 insertions(+), 125 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b944ff1e88..aa71c70745 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -53,8 +53,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller diff_drive_controller ) @@ -69,8 +68,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) ament_target_dependencies(test_load_diff_drive_controller controller_manager diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 5923a27d4d..56d4970945 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface // Parameters from ROS for diff_drive_controller std::shared_ptr param_listener_; Params params_; + /* Number of wheels on each side of the robot. This is important to take the wheels slip into + * account when multiple wheels on each side are present. If there are more wheels then control + * signals for each side, you should enter number for control signals. For example, Husky has two + * wheels on each side, but they use one control signal, in this case '1' is the correct value of + * the parameter. */ + int wheels_per_side_; Odometry odometry_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 23097c0df2..b1c74625cb 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -150,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= static_cast(params_.wheels_per_side); - right_feedback_mean /= static_cast(params_.wheels_per_side); + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); if (params_.position_feedback) { @@ -258,7 +258,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -287,12 +287,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.left_wheel_names.empty()) - { - RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return controller_interface::CallbackReturn::ERROR; - } - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; @@ -322,7 +316,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - params_.wheels_per_side = params_.left_wheel_names.size(); + wheels_per_side_ = static_cast(params_.left_wheel_names.size()); if (publish_limited_velocity_) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index e878ad5481..509a7ab1e1 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,23 +2,24 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } wheel_separation: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", } - wheels_per_side: { - type: int, - default_value: 0, - description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", - } wheel_radius: { type: double, default_value: 0.0, diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index edaaa84e4b..b005655aad 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -166,11 +172,28 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + + 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); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -191,36 +214,34 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Publisher::SharedPtr velocity_publisher; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +======= + const auto ret = controller_->init(controller_name, urdf_, 0); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) } -TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -231,12 +252,18 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size extended_right_wheel_names.push_back("extra_wheel"); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); +======= + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -244,6 +271,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); +======= + ASSERT_EQ(InitController(), controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -257,26 +287,24 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -290,26 +318,24 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -325,26 +351,24 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -361,26 +385,25 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; +<<<<<<< HEAD const auto ret = controller_->init(controller_name, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -396,26 +419,25 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; +<<<<<<< HEAD const auto ret = controller_->init(controller_name, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -433,26 +455,25 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; +<<<<<<< HEAD const auto ret = controller_->init(controller_name, test_namespace); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -467,6 +488,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -474,6 +496,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); +======= + ASSERT_EQ(InitController(), controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -481,15 +506,14 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); +======= + ASSERT_EQ(InitController(), controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -497,6 +521,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -506,6 +531,13 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); +======= + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -514,6 +546,7 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -523,6 +556,13 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); +======= + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -531,6 +571,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -540,6 +581,13 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); controller_->get_node()->set_parameter( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); +======= + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -548,6 +596,7 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -557,6 +606,13 @@ TEST_F(TestDiffDriveController, cleanup) rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); +======= + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -597,6 +653,7 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { +<<<<<<< HEAD const auto ret = controller_->init(controller_name); ASSERT_EQ(ret, controller_interface::return_type::OK); @@ -606,6 +663,13 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); +======= + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); +>>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 1eb8939031..983ec6d98f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -21,8 +21,6 @@ TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -33,6 +31,14 @@ TEST(TestLoadDiffDriveController, load_controller) ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; } From e6fad3cdb7946da14beeb535fd5d8cc192209635 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 19 Feb 2024 22:08:32 +0000 Subject: [PATCH 2/2] Fix merge conflicts --- .../test/test_diff_drive_controller.cpp | 148 +----------------- 1 file changed, 1 insertion(+), 147 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index b005655aad..6de71dbeb8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -188,7 +188,7 @@ 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); + return controller_->init(controller_name, ns, node_options); } const std::string controller_name = "test_diff_drive_controller"; @@ -216,64 +216,29 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, init_fails_without_parameters) { -<<<<<<< HEAD const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -======= - const auto ret = controller_->init(controller_name, urdf_, 0); ASSERT_EQ(ret, controller_interface::return_type::ERROR); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) } TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); -======= ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); -======= ASSERT_EQ( InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); -======= ASSERT_EQ(InitController(), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -287,12 +252,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - -======= ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; @@ -318,12 +277,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - -======= ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; @@ -351,12 +304,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - -======= ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; @@ -385,12 +332,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; -<<<<<<< HEAD - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - -======= ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; @@ -419,12 +360,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; -<<<<<<< HEAD - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - -======= ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; @@ -455,12 +390,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; -<<<<<<< HEAD - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - -======= ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; @@ -488,17 +417,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); -======= ASSERT_EQ(InitController(), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -506,12 +425,7 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); -======= ASSERT_EQ(InitController(), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) // We implicitly test that by default position feedback is required ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -521,23 +435,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); -======= ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -546,23 +448,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); -======= ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -571,23 +461,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); -======= ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -596,23 +474,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); -======= ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -653,23 +519,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); -======= ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), controller_interface::return_type::OK); ->>>>>>> 2705ca8 ([diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958)) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface());