Fix correct usage of angular velocity in update_odometry() function (#…

…1118) (cherry picked from commit d2e926b)
ros-controls · May 27, 2024 · e587218 · e587218
1 parent cb40e0b
commit e587218
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 2 deletions.
diff --git 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
@@ -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);
+}