From 2e939bd5727c07a927f2d1645f3a7e50bde8e3da Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 6 May 2024 20:58:22 +0200 Subject: [PATCH 1/4] Fix correct usage of angular velocity in update_odometry() function --- steering_controllers_library/src/steering_odometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index aadd047f2e..6fb20478a4 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(); From 860e24417a532e450fdfb487849c19d5e311eba3 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 6 May 2024 23:17:55 +0200 Subject: [PATCH 2/4] Test update_odometry() functions --- .../test/test_steering_odometry.cpp | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 173c76baef..1420a87dd7 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); +} \ No newline at end of file From 67a16fa86c89959fdca7b8b8fae53ce89d8985be Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Sat, 25 May 2024 06:31:30 +0200 Subject: [PATCH 3/4] Satisfy the linters MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- steering_controllers_library/test/test_steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 1420a87dd7..9a4d626de0 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -139,7 +139,7 @@ TEST(TestSteeringOdometry, ackermann_odometry) 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_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); From bed6be3c1c8146bc3f46bb9e68b2cc08331280ca Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 27 May 2024 09:38:26 +0200 Subject: [PATCH 4/4] Satisfy pre-commit checks, adding newline --- steering_controllers_library/test/test_steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 9a4d626de0..3e4adc59fe 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -143,4 +143,4 @@ TEST(TestSteeringOdometry, ackermann_odometry) EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); -} \ No newline at end of file +}