Skip to content

Commit

Permalink
Fix correct usage of angular velocity in update_odometry() function (#…
Browse files Browse the repository at this point in the history
…1118) (#1153)

(cherry picked from commit d2e926b)

Co-authored-by: Ferry Schoenmakers <[email protected]>
  • Loading branch information
mergify[bot] and MCFurry authored May 27, 2024
1 parent 11e8ad7 commit 9e83d63
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 2 deletions.
4 changes: 2 additions & 2 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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();
Expand Down
36 changes: 36 additions & 0 deletions steering_controllers_library/test/test_steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

0 comments on commit 9e83d63

Please sign in to comment.