From 8a20e2ac5bd1dbf7193db8a30e10ffbcc40b0a7b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 9 Feb 2024 17:16:32 +0000 Subject: [PATCH] Adjust tests and add wheel names size verification to init --- .../src/diff_drive_controller.cpp | 9 + .../test/test_diff_drive_controller.cpp | 187 ++---------------- 2 files changed, 26 insertions(+), 170 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index b1c74625cb..4fa44d6a71 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -60,6 +60,15 @@ controller_interface::CallbackReturn DiffDriveController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); + + if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The number of left wheels [%zu] and the number of right wheels [%zu] are different", + params_.left_wheel_names.size(), params_.right_wheel_names.size()); + return controller_interface::CallbackReturn::ERROR; + } } catch (const std::exception & e) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index b005655aad..34550eef68 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"; @@ -214,67 +214,32 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Publisher::SharedPtr velocity_publisher; }; -TEST_F(TestDiffDriveController, init_fails_without_parameters) +TEST_F(TestDiffDriveController, 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) +TEST_F(TestDiffDriveController, fails_with_only_left_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); + const auto ret = InitController(left_wheel_names, {}); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) +TEST_F(TestDiffDriveController, 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); + const auto ret = InitController(left_wheel_names, extended_right_wheel_names); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { -<<<<<<< HEAD - const auto ret = controller_->init(controller_name); + const auto ret = InitController(left_wheel_names, right_wheel_names); 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_THAT( @@ -284,15 +249,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) controller_->command_interface_configuration().names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); } - 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"; @@ -311,19 +269,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + // tf_frame_prefix_enable is false so no modifications to the frame ids ASSERT_EQ(test_odom_frame_id, odom_id); ASSERT_EQ(test_base_frame_id, base_link_id); } 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"; @@ -343,20 +295,13 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's */ + // tf_frame_prefix_enable true and frame_prefix not blank, should be appended to the frame ids ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } 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 = ""; @@ -375,8 +320,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame - * id's */ + // tf_frame_prefix_enable true but frame_prefix blank, should not be appended to the frame ids ASSERT_EQ(test_odom_frame_id, odom_id); ASSERT_EQ(test_base_frame_id, base_link_id); } @@ -384,13 +328,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) { 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"; @@ -410,7 +347,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + // tf_frame_prefix_enable is false so no modifications to the frame ids ASSERT_EQ(test_odom_frame_id, odom_id); ASSERT_EQ(test_base_frame_id, base_link_id); } @@ -418,13 +355,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) { 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"; @@ -445,8 +375,8 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame - * id's instead of the namespace*/ + // tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + // ids instead of the namespace ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } @@ -454,13 +384,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) { 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 = ""; @@ -480,25 +403,15 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; - /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the - * frame id's */ + // tf_frame_prefix_enable true but frame_prefix blank, namespace should be appended to the frame + // ids ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); } 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 +419,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,24 +429,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(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -546,23 +441,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 +454,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 +467,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 +512,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());