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 81.08108% with 14 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.03%. Comparing base (42e7ba4) to head (8ecb548).

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

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1854      +/-   ##
==========================================
- Coverage   85.25%   85.03%   -0.22%     
==========================================
  Files         143      143              
  Lines       13790    13819      +29     
  Branches     1197     1199       +2     
==========================================
- Hits        11756    11751       -5     
- Misses       1636     1671      +35     
+ Partials      398      397       -1

Flag	Coverage Δ
unittests	`85.03% <81.08%> (-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	`82.79% <83.33%> (+0.21%)`	⬆️
diff_drive_controller/src/odometry.cpp	`50.00% <78.12%> (-26.32%)`	⬇️

... and 1 file with indirect coverage changes

🚀 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

christophfroehlich · 2025-08-17T18:09:59Z

diff_drive_controller/src/odometry.cpp

+bool Odometry::updateFromVel(
+  const double & left_vel, const double & right_vel, const rclcpp::Time & time)
+{
+  const double dt = time.seconds() - timestamp_.seconds();


this is exactly the pattern mentioned with #271 to be removed?

Sorry, I should have explained this in the PR description.
The reasoning here is the same as that of this #260 (comment).
Additionally, timestamp_ records the last time an update was run; therefore, the correct value of dt can only be obtained from there.
It is used in this function despite (std::fabs(dt) < 1e-6) not being used, as it is also called from updateFromPos() which uses the timestamp_ variable.

Sorry, I still don't understand. Why not using the period argument from the update method and pass it to the update method?

Basically due to the if (std::fabs(dt) < 1e-6) {return false;} check in updateFromPos() we need to maintain the timestamp_ variable.

timestamp_ is only updated when the check returns false, and we continue with the code. If timestamp_ was updated every time, it would yield incorrect results, as we only update odometry when the code passes through that check.

If period was directly passed, then it would yield an incorrect dt (dt will now be replaced with period), as the current way to calculate dt is through the timestamp_, which is not updated every time the method is called.

The updateFromVel() function can be changed to use period if the arguments of updateFromPos() remain the same, and we pass it dt from there when using position feedback, and when using velocity feedback, we pass period from the main update method. This can only be done, though, if we are sure that the user can't/won't switch between velocity and position feedback while the controller is running (add read_only parameters to diff_drive_controller #1781).

Thanks, I understand the motivation now.

Is the period argument ever that small so we need the check? (might come from ROS 1 days)

If so (maybe the first update call after activation or something like this. This can't happen on a regular basis), Is the error really that high if we use the period and still skip if it is close to zero?

ros-controls/ros2_control#2336

I also thought about this, as the period argument would only be this small if the update rate is 10^6; I don't think people set the update rate that high, but I still kept the check. Because of Period is zero at first loop of ros controller ros2_control#2336, ig we will either way need it.

Didn't think about it like that :). There would only be a minor 10^-6 or less seconds unaccounted for, which shouldn't have any effect on the odometry. That is assuming that the update rate would never be set that high to make this error accumulate.

So should I pass period to both the functions, in updateFromPos() keep the zero check, and stop the use of timestamp_ in the new functions?

yes please do so 👍

Done in 77eade3.

christophfroehlich · 2025-08-17T18:10:54Z

diff_drive_controller/src/odometry.cpp

+  linear_ = linear_vel;
+  angular_ = angular_vel;
+
+  return true;


why a return value, which is always true?

This is there to maintain consistency with other update methods, make it easier to add code to this function in the future that may return false, and make the diff_drive_controller.cpp code simpler.
updateFromVel() also has this line for the same reason.
I can change everything back to the normal updateOpenLoop() function if needed, though.

diff_drive_controller/src/odometry.cpp

diff_drive_controller/src/diff_drive_controller.cpp

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

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

christophfroehlich · 2025-09-08T18:35:22Z

diff_drive_controller/src/odometry.cpp

+bool Odometry::updateFromVel(
+  const double & left_vel, const double & right_vel, const rclcpp::Time & time)
+{
+  const double dt = time.seconds() - timestamp_.seconds();


Sorry, I still don't understand. Why not using the period argument from the update method and pass it to the update method?

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

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!

Update odometry implementation in diff_drive

fe3a568

github-actions bot requested review from bijoua29, christophfroehlich, erickisos, fmauch, harderthan, Maverobot 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 <christophfroehlich@users.noreply.github.com>

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

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 <christophfroehlich@users.noreply.github.com>

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

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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Amronos Sep 9, 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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

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!

Choose a reason for hiding this comment

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!

codecov bot commented Aug 6, 2025 •

edited

Loading

Amronos Sep 9, 2025 •

edited

Loading