Significant error in odometry

[Narukara]

Describe the bug
The odometer of the demo car has significant errors. Not sure what causes this.

To Reproduce

1. Launch diff_drive or ackermann demo.
2. Run example_ackermann_drive or example_diff_drive for a few seconds. Then stop it.
3. Check the pose in Gazebo and the odometry of controller.

Expected behavior
The x, y coordinates should be close enough.

Screenshots

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version: ROS2 Humble, Ignition Gazebo Fortress
• humble branch of this repo

Additional context
Is this a problem with the odometer itself, or with the demo? Or am I using it incorrectly? (I'm new to ROS2 Control, sorry if I made a mistake)

[Amronos]

This is a problem that I have also seen a lot of with the demos. It is one of the primary reasons I am working on #531, so that this could be more easily tested and resolved in the future.

[christophfroehlich]

How can we distinguish between physics-related errors (wheel slipping etc) and errors in the odometry calculations? The odometry plugin does not help here. We have to improve the tests in the controller implementations and be sure that they are fine..

[Amronos]

I think the demos should be more perfect with very less wheel slippage as that would really show the controller properly working. The odometry plugin is more so intended to be used after one is sure of the odometry calculations and wants to check if nothing has been put incorrectly in the controller parameters / no wheel slippage is happening.
The controller tests should/can also be definitely improved.

[Narukara]

After doing some tests based on the ackermann demo, I think I have figured out the cause of the error.

I wrote a tool for collecting gazebo data (via ign topic -e -t /world/empty/dynamic_pose/info). In addition, for ease of analysis, I set the controller's velocity_rolling_window_size to 1.

First, for straight-line driving, the error mainly comes from wheel slippage. Currently, the velocity command interface directly sets the velocity of the joint (wheel), but the chassis cannot reach the target speed instantly. Therefore, during acceleration and deceleration, the wheels will slip. This can be clearly seen from the curve below.

[Narukara]

Second, for turning, I found that the odometer always tends to underestimate the angular velocity of the vehicle. As shown in the following curve:

Then I read the source code of the odometry and found that it inexplicably uses a small angle approximation when calculating the angular velocity of the Ackermann vehicle:

const double angular_velocity = steer_pos_ * linear_velocity / wheel_base_;

https://github.com/ros-controls/ros2_controllers/blob/a88bf0af29308448424d0112cba2b0560d1a7305/steering_controllers_library/src/steering_odometry.cpp#L183

The correct formula should be:

$\omega$ angular_velocity
$v$ linear_velocity
$\delta$ steer_pos_
$L$ wheel_base_

$$ \omega = \frac{v}{R}=\frac{v\tan \delta}{L} $$

But it actually uses:

$$ \omega = \frac{v \delta}{L} $$

Since $\tan\delta$ is greater than $\delta$, this causes the odometry to tend to underestimate the angular velocity.

Taking the above test ($L=1.7,v=2,\omega=0.5,R=\frac{v}{\omega}=4,\delta=\arctan\frac LR=0.40$) as an example, the correct angular velocity calculation process is:

$$ \hat\omega=\frac{v\tan \delta}{L}=\frac{2\times(1.7/4)}{1.7}=0.5 $$

But the current odometer will miscalculate it as:

$$ \hat\omega=\frac{v \delta}{L} =\frac{2\times0.40}{1.7}=0.47 $$

which is consistent with the test result above.

[Narukara]

The first error always exists as long as the velocity command interface sets the joint velocity directly. Maybe #122 can solve this problem.

For the second error, I have submitted a PR.

[Narukara]

The changes in ros-controls/ros2_controllers#1777 have been distributed via apt (humble version). I can confirm that the angular velocity is now calculated correctly. 😃

Now the error of the odometer should mainly come from the wheel slip caused by the velocity command interface, as mentioned above. Due to the slip at the start, there is an offset between the estimated path and the actual path:

[christophfroehlich]

Great news. So there is not much we can do about that now. Can I close the issue? Do you want to add a note to the documentation discussing the resulting behavior?

[Narukara]

I guess if the command interface controls the joints by applying forces (#122), errors caused by slippage can be avoided. 🤔

Anyway, it would be good to document how the command interface works and the potential odometry errors. I’d be happy to volunteer if needed.

[christophfroehlich]

yes please do so!