Improve diff_drive_controller odometry velocity and timestep handling #1394
Conversation
use velocity where it claims to, and pass timestep in to every method that makes use of it. This was requested in ros-controls#271. - The result is that the class is very slightly less efficient (but still quite trivial) due to passing around a extra double. - The other result is that it is no longer possible for velocity based odometry updates calculated incorrectly when the timestep fails to keep up. To prevent position based updates from now suffering the inverse of the velocity timestep/dt mismatch, the measured delta is used exclusively for position based updates (retaining the existing behavior), while the configured timestep is used for open and closed loop velocity updates. I also added a 1s throttled warning if there is a major mismatch between measured and configured timestep (+/-20%) since this would cause a slight accumulation of integration errors on curved paths (I believe), depending on velocity, curvature and mismatch size. Similar requested period/measured period mismatches may be a nuisance for open loop (which is velocity based) and closed loop velocity based updates, but the impact is likely negligible (in this class).
github-actions
bot
requested review from
anfemosa,
ARK3r,
bijoua29,
christophfroehlich,
DasRoteSkelett,
destogl and
LukasMacha97
There was a problem hiding this comment.
Thanks for picking this old issue, I agree this should be more explicit.
But I have comments in the code here, I'm not sure if this is really an issue what you tried to fix.
In general:
- Please check the pre-commit failures
- We should deprecate the old methods but keep them for a while
- If we are breaking API here, we should rename all odometry class methods to have snake case which is the standard of ros2_control (and ROS 2 in general afaik)
| // Compute linear and angular diff: | ||
| const double linear = (left_vel + right_vel) * 0.5; | ||
| // Now there is a bug about scout angular velocity | ||
| const double angular = (right_vel - left_vel) / wheel_separation_; | ||
|
|
||
| // Integrate odometry: | ||
| integrateExact(linear, angular); | ||
| integrateExact(linear, angular, dt); | ||
|
|
||
| timestamp_ = time; |
There was a problem hiding this comment.
timestamp_ class variable should be removed now (marked as deprecated to be removed with the old methods)
| odometry_.update(left_feedback_mean, right_feedback_mean, time); | ||
| // Warn if actual timestep doesn't match period, | ||
| // since | ||
| const double measured_period = ( |
There was a problem hiding this comment.
Why should the measured period be different from the period argument of the update method? This is calculated from the controller_manager, we can assume that this is always valid?
There was a problem hiding this comment.
I was thinking on this some more myself and came to the same conclusion.
I'm hoping to rework this PR sometime next week focusing on the code quality/cleanup aspects suggested here and remove the inconsistent time handling (which as you suggest is probably a distinction without a difference).
Recently I've switched to using the RT_Linux kernel and set up permissions for real-time and found a fairly dramatic difference in perceived responsiveness with it enabled in the diff drive controller. Something isn't handling time/dates/update timesteps properly and I think I incorrectly identified this as the culprit. It still might be, but I should have started off with unit tests (or at least minimum steps to reproduce) rather than complicating an otherwise straightforward code cleanup.
Codecov ReportAttention: Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## master #1394 +/- ##
==========================================
+ Coverage 83.58% 83.81% +0.23%
==========================================
Files 122 122
Lines 10990 10987 -3
Branches 935 936 +1
==========================================
+ Hits 9186 9209 +23
+ Misses 1492 1464 -28
- Partials 312 314 +2
Flags with carried forward coverage won't be shown. Click here to find out more.
|
Thanks, this is my first PR for the repo and I understand the value of consistency for a big open source project like this. I'll try to get things better aligned/more narrowly focused and either update the PR branch, or close this if I won't have the time in the next two weeks. |
|
This pull request is in conflict. Could you fix it @josephduchesne? |
This PR updates diff_drive odometry class to
This was requested in #271, although I took a more expansive approach since the odometry class is was at high risk of new bugs due the unclear units of variables at play.
The result is that the class is very slightly less efficient (but still quite trivial) due to passing around a extra double.
The other result is that it is no longer possible for velocity based odometry updates calculated incorrectly when the timestep fails to keep up.
To prevent position based updates from now suffering the inverse of the velocity timestep/dt mismatch, the measured delta is used exclusively for position based updates (retaining the existing behavior), while the configured timestep is used for open and closed loop velocity updates. This decision is made by diff_drive_controller, and out of scope of the odometry class.
I also added a 1s throttled warning if there is a major mismatch between measured and configured timestep (+/-20%) since this would cause a slight accumulation of integration errors on curved paths (I believe), depending on velocity, curvature and mismatch size. This could probably be expanded to a more general diagnostics WARN output, since controller/odometry updates failing to keep up can be serious.
Similar requested period/measured period mismatches may be a nuisance for open loop (which is velocity based) and closed loop velocity based updates, but the impact is likely negligible (in this class).