Potential Inaccuracy in Odometry::updateFromVelocity Linear Displacement Calculation

[AnEx-ofc]

https://github.com/ros-controls/ros2_controllers/blob/a09e7af552c873ce29940b10167af99568252fb2/diff_drive_controller/src/odometry.cpp#L81C26-L81C34

I've noticed that the Odometry::updateFromVelocity function calculates linear displacement using the following formula:

linear = (left_vel + right_vel) * 0.5

While this correctly calculates the average linear velocity, it might not be directly equivalent to linear displacement without considering the time interval (dt) between measurements.

It's worth double-checking this calculation.

[christophfroehlich]

I think this is only implemented very confusing but actually correct (proof me wrong ;))

This is related to the refactoring in #1394
could you have a look there and leave a review to @josephduchesne there?

[AnEx-ofc]

@christophfroehlich,

This is working correctly because when calling

ros2_controllers/diff_drive_controller/src/odometry.cpp

Line 71 in a09e7af

updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time);

it actually passing left and right displacement with unit "m" not actual velocity with "m/s" unit. I believe the main error is here:

ros2_controllers/diff_drive_controller/src/odometry.cpp

Line 64 in a09e7af

const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;

and next line

ros2_controllers/diff_drive_controller/src/odometry.cpp

Line 65 in a09e7af

const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;

.

To convert pose diff to vel, dt must have been used. Like this: left_wheel_est_vel = (left_wheel_cur_pos - left_wheel_old_pos_)/dt.
Latter, those velocities should have get converted to displacement using dt inside updateFromVelocity fn.

To convert pose difference to velocity, you correctly identified the need to divide by the time difference (dt).
And convert these velocities back to displacements within the updateFromVelocity function by multiplying the time difference (dt).

The primary goal of the updateFromVelocity function is to update the odometry based on the provided velocities. These velocities, when multiplied by the time step (dt), directly give the displacements.

Here's a revised approach:

1. Calculate velocities:

   left_wheel_est_vel = (left_wheel_cur_pos - left_wheel_old_pos_) / dt;
   right_wheel_est_vel = (right_wheel_cur_pos - right_wheel_old_pos_) / dt;

2. Update odometry in updateFromVelocity:

   // ... other calculations ...
   
   // Calculate displacements using velocities and dt
   left_wheel_displacement = left_wheel_vel * dt;
   right_wheel_displacement = right_wheel_vel * dt;
   
   // Update odometry using the calculated displacements
   // ... odometry update logic ...

By following this approach, you ensure that the odometry is updated accurately based on the velocities and the time step.

[christophfroehlich]

Isn't this exactly what is changed in the linked PR?

[AnEx-ofc]

Yes exactly.
Just checked the code from that PR.