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/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 93e607bd08..904bcf08d1 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -107,6 +107,12 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( REAR_LEFT, params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); + // 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 wheel params for the odometry computation odometry_.setWheelsParams( params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 580b4058d7..c764c6c063 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: 1.0, y: 2.0, theta: 3.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..00c1f5a74b 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -562,6 +562,92 @@ TEST_F( EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } +TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) +{ + // Initialize controller + SetUpController("test_mecanum_drive_controller_with_rotation"); + + // 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_to_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 + }; + + // Setup reference interfaces for 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 + }; + + 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: + side_to_side_motion(2.0); + break; + case 1: + rotation_motion(-0.5); + break; + case 2: + side_to_side_motion(-2.0); + break; + case 3: + rotation_motion(0.5); + } + + // Call 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 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(); + size_t rr_index = controller_->get_rear_right_wheel_index(); + 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( + 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..7b4e69aaf4 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, SideToSideAndRotationOdometryTest); public: controller_interface::CallbackReturn on_configure(