From 4b26290978cdd0e1a859adb2ac61de2e7c1ecf99 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 27 May 2024 18:50:45 +0200 Subject: [PATCH] Fix correct usage of angular velocity in update_odometry() function (#1118) (cherry picked from commit d2e926be5d1ecc31fc5966322c59afdd17bb1c3f) --- .../src/steering_odometry.cpp | 4 +-- .../test/test_steering_odometry.cpp | 36 +++++++++++++++++++ 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 3e1b586807..352ba5c792 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -52,7 +52,7 @@ bool SteeringOdometry::update_odometry( const double linear_velocity, const double angular, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +62,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular / dt); + angular_acc_.accumulate(angular); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean(); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 173c76baef..3e4adc59fe 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -108,3 +108,39 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) EXPECT_LT(cmd1[0], 0); } + +TEST(TestSteeringOdometry, bicycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, tricycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +}