Update odometry implementation in diff_drive #1854

Amronos · 2025-08-06T14:24:16Z

Fixes #271.
Fixes #357.

I have updated the odometry implementation of the diff_drive_controller to fix the above two issues alongside improving the code's readability. This implementation is similar to that of omni_wheel_drive_controller.
I have created some new methods and deprecated the older ones.

Should I also make similar changes to other controllers? If yes, should I do that in a separate PR?

codecov · 2025-08-06T14:49:43Z

Codecov Report

❌ Patch coverage is 79.71014% with 14 lines in your changes missing coverage. Please review.
✅ Project coverage is 84.91%. Comparing base (ec198b1) to head (b474fde).

Files with missing lines	Patch %	Lines
...iff_drive_controller/src/diff_drive_controller.cpp	80.00%	4 Missing and 3 partials ⚠️
diff_drive_controller/src/odometry.cpp	79.41%	6 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1854      +/-   ##
==========================================
- Coverage   85.13%   84.91%   -0.22%     
==========================================
  Files         144      144              
  Lines       13968    14002      +34     
  Branches     1201     1204       +3     
==========================================
- Hits        11891    11890       -1     
- Misses       1670     1705      +35     
  Partials      407      407

Flag	Coverage Δ
unittests	`84.91% <79.71%> (-0.22%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...troller/include/diff_drive_controller/odometry.hpp	`100.00% <ø> (ø)`
...iff_drive_controller/src/diff_drive_controller.cpp	`83.00% <80.00%> (+0.32%)`	⬆️
diff_drive_controller/src/odometry.cpp	`50.90% <79.41%> (-25.41%)`	⬇️

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

diff_drive_controller/include/diff_drive_controller/odometry.hpp

christophfroehlich

Let's discuss my points first, and merge the outcome. If necessary, let's open a new PR afterwards for the other controllers.

diff_drive_controller/include/diff_drive_controller/odometry.hpp

diff_drive_controller/src/odometry.cpp

diff_drive_controller/src/diff_drive_controller.cpp

Co-authored-by: Christoph Fröhlich <[email protected]>

Amronos · 2025-09-04T04:26:16Z

Sorry for the delay in fixing the failing checks here! I was busy shifting Linux distros and had tried to make the commits that caused the checks to fail through the web.

christophfroehlich

Still not sure about the tryUpdateOpenLoop method, maybe @saikishor or anyone else has an opinion about that.

diff_drive_controller/src/odometry.cpp

Co-authored-by: Christoph Fröhlich <[email protected]>

Amronos · 2025-09-24T17:20:43Z

@christophfroehlich pre-commit isn't working for me for some reason, but besides that I have made the changed you requested. If possible, could you clone the pr, and push a fix for the pre-commit? Thanks!

mergify · 2025-10-03T18:35:54Z

This pull request is in conflict. Could you fix it @Amronos?

Amronos · 2025-10-03T18:58:47Z

Sorry for the delay on this! All requested changes have been made now, please review!

diff_drive_controller/src/diff_drive_controller.cpp

christophfroehlich

Thanks for your patience. This is almost good to be merged, two minor comments only

diff_drive_controller/src/odometry.cpp

diff_drive_controller/include/diff_drive_controller/odometry.hpp

christophfroehlich

Thanks for the latest cleanup! LGTM

Juliaj · 2025-11-29T18:55:08Z

diff_drive_controller/include/diff_drive_controller/odometry.hpp

+  [[deprecated(
+    "Replaced by bool update_from_pos(double left_pos, double right_pos, const "
+    "rclcpp::Time & time).")]]


Suggested change

[[deprecated(

"Replaced by bool update_from_pos(double left_pos, double right_pos, const "

"rclcpp::Time & time).")]]

[[deprecated(

"Replaced by bool update_from_pos(double left_pos, double right_pos, double "

"dt).")]]

Juliaj · 2025-11-29T18:59:51Z

diff_drive_controller/include/diff_drive_controller/odometry.hpp

+  [[deprecated(
+    "Replaced by bool update_from_vel(double left_vel, double right_vel, const "
+    "rclcpp::Time & time).")]]


Suggested change

[[deprecated(

"Replaced by bool update_from_vel(double left_vel, double right_vel, const "

"rclcpp::Time & time).")]]

[[deprecated(

"Replaced by bool update_from_vel(double left_vel, double right_vel, double "

"dt).")]]

Juliaj · 2025-11-29T19:00:46Z

diff_drive_controller/include/diff_drive_controller/odometry.hpp

+  [[deprecated(
+    "Replaced by bool try_update_open_loop(double linear_vel, double angular_vel, const "
+    "rclcpp::Time "
+    "& time).")]]


Suggested change

[[deprecated(

"Replaced by bool try_update_open_loop(double linear_vel, double angular_vel, const "

"rclcpp::Time "

"& time).")]]

[[deprecated(

"Replaced by bool try_update_open_loop(double linear_vel, double angular_vel, double "

"dt).")]]

Juliaj · 2025-11-29T19:08:16Z

diff_drive_controller/src/odometry.cpp


+bool Odometry::update_from_vel(double left_vel, double right_vel, double dt)
+{
+  // Compute linear and angular velocities of the robot:


Let's add this check back

if (dt < 0.0001) { return false; // Interval too small to integrate with }

Juliaj · 2025-11-29T19:17:32Z

diff_drive_controller/src/odometry.cpp


+void Odometry::integrate(double linear_vel, double angular_vel, double dt)
+{
+  const double dx = linear_vel * dt;


Suggested change

const double dx = linear_vel * dt;

// Skip integration for invalid time intervals

if (std::fabs(dt) < 1e-6)

{

return;

}

const double dx = linear_vel * dt;

Juliaj · 2025-11-29T19:26:34Z

diff_drive_controller/src/diff_drive_controller.cpp

    {
-      previous_publish_timestamp_ += publish_period_;
-      should_publish = true;
+      if (previous_publish_timestamp_ + publish_period_ < time)


Suggested change

if (previous_publish_timestamp_ + publish_period_ < time)

if (previous_publish_timestamp_ + publish_period_ <= time)

Juliaj · 2025-11-29T19:33:56Z

diff_drive_controller/src/diff_drive_controller.cpp

+    catch (const std::runtime_error &)
    {
-      odometry_message_.header.stamp = time;
-      odometry_message_.pose.pose.position.x = odometry_.getX();
-      odometry_message_.pose.pose.position.y = odometry_.getY();
-      odometry_message_.pose.pose.orientation.x = orientation.x();
-      odometry_message_.pose.pose.orientation.y = orientation.y();
-      odometry_message_.pose.pose.orientation.z = orientation.z();
-      odometry_message_.pose.pose.orientation.w = orientation.w();
-      odometry_message_.twist.twist.linear.x = odometry_.getLinear();
-      odometry_message_.twist.twist.angular.z = odometry_.getAngular();
-      realtime_odometry_publisher_->try_publish(odometry_message_);
+      // Handle exceptions when the time source changes and initialize publish timestamp
+      previous_publish_timestamp_ = time;
+      should_publish = true;
    }


Is this block meant to catch the error of previous_publish_timestamp as unintialized ? if so, we should initialize it in on_configure. If not, this block may not be reachable.

Juliaj · 2025-11-29T19:40:19Z

@Amronos, thanks for working on this PR. It would be great to add some tests for the odometry changes. If you can't accommodate this within this PR, please consider creating a GitHub issue so the community can pick it up.

Update odometry implementation in diff_drive

fe3a568

github-actions bot requested review from Maverobot, bijoua29, christophfroehlich, erickisos, fmauch, harderthan and saikishor August 6, 2025 14:24

christophfroehlich reviewed Aug 17, 2025

View reviewed changes

diff_drive_controller/include/diff_drive_controller/odometry.hpp Outdated Show resolved Hide resolved

christophfroehlich requested changes Aug 17, 2025

View reviewed changes

christophfroehlich mentioned this pull request Aug 17, 2025

Odometry position approximation: exact integration / rungeKutta2 integration / rungeKutta4 integration? #357

Open

Amronos and others added 4 commits August 20, 2025 19:03

Apply suggestions from code review

13edbec

Co-authored-by: Christoph Fröhlich <[email protected]>

Fix deprecation warning

629cf5e

Multiply by wheel radius in updateFromVel

d387750

Fix pre-commit

bd1283b

Amronos requested a review from christophfroehlich August 20, 2025 13:43

Amronos added 2 commits September 4, 2025 08:19

Merge branch 'master' into update-diff_drive-odom

3048023

Fix build errors

93806d6

christophfroehlich requested changes Sep 8, 2025

View reviewed changes

diff_drive_controller/src/odometry.cpp Outdated Show resolved Hide resolved

diff_drive_controller/src/odometry.cpp Outdated Show resolved Hide resolved

Amronos and others added 2 commits September 9, 2025 15:08

Update diff_drive_controller/src/odometry.cpp

1d47fed

Co-authored-by: Christoph Fröhlich <[email protected]>

Merge branch 'master' into update-diff_drive-odom

a52855b

Amronos requested a review from christophfroehlich September 9, 2025 09:55

Amronos added 4 commits September 24, 2025 11:12

Apply suggestions from code review

77eade3

Merge branch 'master' into update-diff_drive-odom

831c9c7

Fix pre-commit

a95a450

Fix pre-commit

8ecb548

Fix pre-commit

ec25473

Merge branch 'master' into update-diff_drive-odom

3e04f37

Amronos added 2 commits October 5, 2025 15:47

Merge branch 'master' into update-diff_drive-odom

93b0c0c

Merge branch 'master' into update-diff_drive-odom

620aec5

Amronos commented Oct 22, 2025

View reviewed changes

diff_drive_controller/src/diff_drive_controller.cpp Outdated Show resolved Hide resolved

Update diff_drive_controller/src/diff_drive_controller.cpp

574c3ba

Amronos commented Oct 22, 2025

View reviewed changes

diff_drive_controller/src/diff_drive_controller.cpp Outdated Show resolved Hide resolved

Update diff_drive_controller/src/diff_drive_controller.cpp

f52b6b1

Amronos mentioned this pull request Oct 23, 2025

diff_drive_controller publishes non-zero floating-point residual velocities when robot is stopped #1972

Open

christophfroehlich requested changes Nov 1, 2025

View reviewed changes

diff_drive_controller/src/odometry.cpp Outdated Show resolved Hide resolved

diff_drive_controller/include/diff_drive_controller/odometry.hpp Outdated Show resolved Hide resolved

Amronos added 3 commits November 7, 2025 07:13

Merge branch 'master' into update-diff_drive-odom

5c2620c

Apply suggestions from code review

6a6c38c

Merge branch 'master' into update-diff_drive-odom

e28abfa

Amronos requested a review from christophfroehlich November 9, 2025 09:55

christophfroehlich approved these changes Nov 15, 2025

View reviewed changes

Merge branch 'master' into update-diff_drive-odom

b474fde

christophfroehlich requested a review from Juliaj November 29, 2025 12:56

Juliaj reviewed Nov 29, 2025

View reviewed changes

	if (previous_publish_timestamp_ + publish_period_ < time)
	if (previous_publish_timestamp_ + publish_period_ <= time)

Update odometry implementation in diff_drive #1854

Are you sure you want to change the base?

Update odometry implementation in diff_drive #1854

Conversation

Amronos commented Aug 6, 2025

Uh oh!

codecov bot commented Aug 6, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Amronos commented Sep 4, 2025

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Amronos commented Sep 24, 2025

Uh oh!

mergify bot commented Oct 3, 2025

Uh oh!

Amronos commented Oct 3, 2025

Uh oh!

Uh oh!

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025

Choose a reason for hiding this comment

Uh oh!

Juliaj Nov 29, 2025

Choose a reason for hiding this comment

Uh oh!

Juliaj commented Nov 29, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

codecov bot commented Aug 6, 2025 •

edited

Loading

Juliaj Nov 29, 2025 •

edited

Loading