From 38b9d2d2d36a9c8fc68331b0ec1ad47a9cc3eef1 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Mar 2025 00:33:26 -0800 Subject: [PATCH 1/4] Add the missing odometry init and a test case --- .../src/mecanum_drive_controller.cpp | 27 +++-- .../test/mecanum_drive_controller_params.yaml | 21 ++++ .../test/test_mecanum_drive_controller.cpp | 105 +++++++++++++++++- .../test/test_mecanum_drive_controller.hpp | 1 + 4 files changed, 139 insertions(+), 15 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 93e607bd08..3f583ef613 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -107,7 +107,13 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( REAR_LEFT, params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); - // Set wheel params for the odometry computation + // initialize odometry + std::array base_frame_offset = { + {params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, + params_.kinematics.base_frame_offset.theta}}; + odometry_.init(get_node()->now(), base_frame_offset); + + // Set wheels params for the odometry computation odometry_.setWheelsParams( params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, params_.kinematics.wheels_radius); @@ -375,10 +381,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma const rclcpp::Time & time, const rclcpp::Duration & period) { // FORWARD KINEMATICS (odometry). - const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); - const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); - const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); - const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); + const double wheel_front_left_state_vel = + state_interfaces_[FRONT_LEFT].get_value().value(); + const double wheel_front_right_state_vel = + state_interfaces_[FRONT_RIGHT].get_value().value(); + const double wheel_rear_right_state_vel = + state_interfaces_[REAR_RIGHT].get_value().value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value().value(); if ( !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && @@ -498,13 +507,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma { controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.front_left_wheel_velocity = - state_interfaces_[FRONT_LEFT].get_value(); + state_interfaces_[FRONT_LEFT].get_value().value(); controller_state_publisher_->msg_.front_right_wheel_velocity = - state_interfaces_[FRONT_RIGHT].get_value(); + state_interfaces_[FRONT_RIGHT].get_value().value(); controller_state_publisher_->msg_.back_right_wheel_velocity = - state_interfaces_[REAR_RIGHT].get_value(); + state_interfaces_[REAR_RIGHT].get_value().value(); controller_state_publisher_->msg_.back_left_wheel_velocity = - state_interfaces_[REAR_LEFT].get_value(); + state_interfaces_[REAR_LEFT].get_value().value(); controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 580b4058d7..2b5e21afa5 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -17,3 +17,24 @@ test_mecanum_drive_controller: enable_odom_tf: true twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + + +test_mecanum_drive_controller_with_rotation: + ros__parameters: + reference_timeout: 5.0 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "rear_right_wheel_joint" + rear_left_wheel_command_joint_name: "rear_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.05 + sum_of_robot_center_projection_on_X_Y_axis: 0.2925 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 079a431e03..902a550eb9 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -161,11 +161,13 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value().value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_TRUE( + std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_TRUE( + std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value().value())); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -383,7 +385,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value().value(), 0.0); } std::shared_ptr msg_2 = std::make_shared(); @@ -422,7 +424,7 @@ TEST_F( for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value().value(), 3.0); } } @@ -481,7 +483,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value().value(), 6.0); } } @@ -562,6 +564,97 @@ TEST_F( EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } +TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) +{ + // Initialize controller + SetUpController("test_mecanum_drive_controller_with_rotation"); + + // Configure + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Activate in chained mode + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->is_in_chained_mode(), true); + + // create closure to set side by side motion, linear_y should be a parameter + auto side_by_side_motion = [this](double linear_y) + { + controller_->reference_interfaces_[0] = 0; // linear x + controller_->reference_interfaces_[1] = linear_y; // linear y + controller_->reference_interfaces_[2] = 0; // angular z + }; + + // create rotation + auto rotation_motion = [this](double rotation_velocity) + { + controller_->reference_interfaces_[0] = 0; // linear x + controller_->reference_interfaces_[1] = 0; // linear y + controller_->reference_interfaces_[2] = rotation_velocity; // angular z + }; + + // check the odometry + + const double update_rate = 50.0; // 50 Hz + const double dt = 1.0 / update_rate; + const double test_duration = 1.0; // 1 second test + auto current_time = controller_->get_node()->now(); + + auto count = 0; + for (double t = 0; t < test_duration; t += dt) + { + switch (count % 4) + { + case 0: + // create side to side motion + side_by_side_motion(2.0); + break; + case 1: + // rotation motion + rotation_motion(-0.5); + break; + case 2: + // side to side motion + side_by_side_motion(-2.0); + break; + case 3: + // rotation motion + rotation_motion(0.5); + } + + // update method + ASSERT_EQ( + controller_->update(current_time, rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + + current_time += rclcpp::Duration::from_seconds(dt); + count++; + + // update the state of the wheels + size_t fl_index = controller_->get_front_left_wheel_index(); + size_t fr_index = controller_->get_front_right_wheel_index(); + size_t rl_index = controller_->get_rear_left_wheel_index(); + size_t rr_index = controller_->get_rear_right_wheel_index(); + joint_state_values_[fl_index] = + controller_->command_interfaces_[fl_index].get_value().value(); + joint_state_values_[fr_index] = + controller_->command_interfaces_[fr_index].get_value().value(); + joint_state_values_[rl_index] = + controller_->command_interfaces_[rl_index].get_value().value(); + joint_state_values_[rr_index] = + controller_->command_interfaces_[rr_index].get_value().value(); + } + + RCLCPP_INFO( + controller_->get_node()->get_logger(), "odometry: %f, %f, %f", controller_->odometry_.getX(), + controller_->odometry_.getY(), controller_->odometry_.getRz()); + + // Verify odometry remains bounded + EXPECT_LT(std::abs(controller_->odometry_.getX()), 1.0); + EXPECT_LT(std::abs(controller_->odometry_.getY()), 1.0); + EXPECT_LT(std::abs(controller_->odometry_.getRz()), M_PI); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 766c3ec554..b3c1d26a16 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -82,6 +82,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST( MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); + FRIEND_TEST(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest); public: controller_interface::CallbackReturn on_configure( From 7a784d96d686b4991f3ec3b12b5b657ec18513e5 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Mar 2025 08:45:01 -0800 Subject: [PATCH 2/4] Revert the change related to deprecated get_value calls --- .../src/mecanum_drive_controller.cpp | 23 +++++++--------- .../test/test_mecanum_drive_controller.cpp | 26 +++++++------------ 2 files changed, 20 insertions(+), 29 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3f583ef613..904bcf08d1 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -107,13 +107,13 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( REAR_LEFT, params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); - // initialize odometry + // Initialize odometry std::array base_frame_offset = { {params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, params_.kinematics.base_frame_offset.theta}}; odometry_.init(get_node()->now(), base_frame_offset); - // Set wheels params for the odometry computation + // Set wheel params for the odometry computation odometry_.setWheelsParams( params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, params_.kinematics.wheels_radius); @@ -381,13 +381,10 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma const rclcpp::Time & time, const rclcpp::Duration & period) { // FORWARD KINEMATICS (odometry). - const double wheel_front_left_state_vel = - state_interfaces_[FRONT_LEFT].get_value().value(); - const double wheel_front_right_state_vel = - state_interfaces_[FRONT_RIGHT].get_value().value(); - const double wheel_rear_right_state_vel = - state_interfaces_[REAR_RIGHT].get_value().value(); - const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value().value(); + const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); + const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); + const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); if ( !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && @@ -507,13 +504,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma { controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.front_left_wheel_velocity = - state_interfaces_[FRONT_LEFT].get_value().value(); + state_interfaces_[FRONT_LEFT].get_value(); controller_state_publisher_->msg_.front_right_wheel_velocity = - state_interfaces_[FRONT_RIGHT].get_value().value(); + state_interfaces_[FRONT_RIGHT].get_value(); controller_state_publisher_->msg_.back_right_wheel_velocity = - state_interfaces_[REAR_RIGHT].get_value().value(); + state_interfaces_[REAR_RIGHT].get_value(); controller_state_publisher_->msg_.back_left_wheel_velocity = - state_interfaces_[REAR_LEFT].get_value().value(); + state_interfaces_[REAR_LEFT].get_value(); controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 902a550eb9..e69efe125f 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -161,13 +161,11 @@ TEST_F( ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value().value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE( - std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value().value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE( - std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value().value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -385,7 +383,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value().value(), 0.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } std::shared_ptr msg_2 = std::make_shared(); @@ -424,7 +422,7 @@ TEST_F( for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value().value(), 3.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); } } @@ -483,7 +481,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value().value(), 6.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); } } @@ -635,14 +633,10 @@ TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) size_t fr_index = controller_->get_front_right_wheel_index(); size_t rl_index = controller_->get_rear_left_wheel_index(); size_t rr_index = controller_->get_rear_right_wheel_index(); - joint_state_values_[fl_index] = - controller_->command_interfaces_[fl_index].get_value().value(); - joint_state_values_[fr_index] = - controller_->command_interfaces_[fr_index].get_value().value(); - joint_state_values_[rl_index] = - controller_->command_interfaces_[rl_index].get_value().value(); - joint_state_values_[rr_index] = - controller_->command_interfaces_[rr_index].get_value().value(); + joint_state_values_[fl_index] = controller_->command_interfaces_[fl_index].get_value(); + joint_state_values_[fr_index] = controller_->command_interfaces_[fr_index].get_value(); + joint_state_values_[rl_index] = controller_->command_interfaces_[rl_index].get_value(); + joint_state_values_[rr_index] = controller_->command_interfaces_[rr_index].get_value(); } RCLCPP_INFO( From 936e4c4518881c2977da35e5ee692fe326af2662 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Wed, 5 Mar 2025 09:22:55 -0800 Subject: [PATCH 3/4] Update code comments for new test added. --- .../test/test_mecanum_drive_controller.cpp | 16 +++++----------- .../test/test_mecanum_drive_controller.hpp | 2 +- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index e69efe125f..7ddeb55f77 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -562,7 +562,7 @@ TEST_F( EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) +TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) { // Initialize controller SetUpController("test_mecanum_drive_controller_with_rotation"); @@ -575,7 +575,7 @@ TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->is_in_chained_mode(), true); - // create closure to set side by side motion, linear_y should be a parameter + // Setup reference interfaces for side to side motion auto side_by_side_motion = [this](double linear_y) { controller_->reference_interfaces_[0] = 0; // linear x @@ -583,7 +583,7 @@ TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) controller_->reference_interfaces_[2] = 0; // angular z }; - // create rotation + // Setup reference interfaces for rotation auto rotation_motion = [this](double rotation_velocity) { controller_->reference_interfaces_[0] = 0; // linear x @@ -591,8 +591,6 @@ TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) controller_->reference_interfaces_[2] = rotation_velocity; // angular z }; - // check the odometry - const double update_rate = 50.0; // 50 Hz const double dt = 1.0 / update_rate; const double test_duration = 1.0; // 1 second test @@ -604,23 +602,19 @@ TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) switch (count % 4) { case 0: - // create side to side motion side_by_side_motion(2.0); break; case 1: - // rotation motion rotation_motion(-0.5); break; case 2: - // side to side motion side_by_side_motion(-2.0); break; case 3: - // rotation motion rotation_motion(0.5); } - // update method + // Call update method ASSERT_EQ( controller_->update(current_time, rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); @@ -628,7 +622,7 @@ TEST_F(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest) current_time += rclcpp::Duration::from_seconds(dt); count++; - // update the state of the wheels + // Update the state of the wheels for subsequent loop size_t fl_index = controller_->get_front_left_wheel_index(); size_t fr_index = controller_->get_front_right_wheel_index(); size_t rl_index = controller_->get_rear_left_wheel_index(); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index b3c1d26a16..7b4e69aaf4 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -82,7 +82,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST( MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); - FRIEND_TEST(MecanumDriveControllerTest, SideBySideAndRotationOdometryTest); + FRIEND_TEST(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest); public: controller_interface::CallbackReturn on_configure( From fb1772ed9f7b33000f98449883399227d07adacf Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Mon, 10 Mar 2025 14:07:42 -0700 Subject: [PATCH 4/4] Add test steps to verify the base_frame_offset --- .../include/mecanum_drive_controller/odometry.hpp | 5 +++++ .../test/mecanum_drive_controller_params.yaml | 2 +- .../test/test_mecanum_drive_controller.cpp | 11 ++++++++--- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 241809e8c2..79a1b54846 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -68,6 +68,11 @@ class Odometry ; } + const std::array & getBaseFrameOffset() const + { + return base_frame_offset_; + } + /// \brief Sets the wheels parameters: mecanum geometric param and radius /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param /// (used in mecanum wheels' ik) [m] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 2b5e21afa5..c764c6c063 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -29,7 +29,7 @@ test_mecanum_drive_controller_with_rotation: rear_left_wheel_command_joint_name: "rear_left_wheel_joint" kinematics: - base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + base_frame_offset: { x: 1.0, y: 2.0, theta: 3.0 } wheels_radius: 0.05 sum_of_robot_center_projection_on_X_Y_axis: 0.2925 diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 7ddeb55f77..00c1f5a74b 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -570,13 +570,18 @@ TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) // Configure ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + // Check on the base frame offset is set by the params + EXPECT_EQ(controller_->odometry_.getBaseFrameOffset()[0], 1.0); + EXPECT_EQ(controller_->odometry_.getBaseFrameOffset()[1], 2.0); + EXPECT_EQ(controller_->odometry_.getBaseFrameOffset()[2], 3.0); + // Activate in chained mode controller_->set_chained_mode(true); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->is_in_chained_mode(), true); // Setup reference interfaces for side to side motion - auto side_by_side_motion = [this](double linear_y) + auto side_to_side_motion = [this](double linear_y) { controller_->reference_interfaces_[0] = 0; // linear x controller_->reference_interfaces_[1] = linear_y; // linear y @@ -602,13 +607,13 @@ TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) switch (count % 4) { case 0: - side_by_side_motion(2.0); + side_to_side_motion(2.0); break; case 1: rotation_motion(-0.5); break; case 2: - side_by_side_motion(-2.0); + side_to_side_motion(-2.0); break; case 3: rotation_motion(0.5);