Incorrect odometry when turning vehicle with bicycle steering controller.

[reydelsol]

Describe the bug Driving a robot with the bicycle steering controller produces incorrect odometry when turning.

To Reproduce This was found during development of a hardware interface for a vehicle modeled with a bicycle controller. A minimal example is not provided, but appears to demonstrate similar behavior to #675. The bug is believed to stem from a mismatch of units in steering_odometry.cpp and may affect other ROS2 controllers:

1. The function update_odometry calls an integrate_exact function with inputs of linear change and angular velocity, SteeringOdometry::integrate_exact(linear_velocity * dt, angular); a. This differs from a call to the same function in update_open_loop which passes linear and angular change, SteeringOdometry::integrate_exact(linear * dt, angular * dt); b. The integrate_exact function calculates odometry’s heading_, x_, and y_ from a division of linear change and angular velocity. The heading_ angle is incremented by units of angular velocity. For a controller update rate of 100 Hz, dt = .01, the heading would change 100x faster than the reality. c. This issue was not found for a linear-only change, as integrate_exact calls integrate_runge_kutta_2 for near-zero angular change.

2. The function update_from_velocity appears to calculate the angular velocity. a. The equation matches that of theta_dot for a bicycle kinematic model, 𝜃_dot = 𝑣 / (L / tan(𝛿)) = 𝑣 * tan(𝛿) / L b. The variable is named, “angular,” unlike the linear_velocity counterpart in this function. The unit 1/s appears to be lost as it is passed to integrate_exact() via update_odometry(). c. Note that other calls to update_odometry in this file also pass “angular” to update_odometry, so the issue may affect other models.

Expected behavior It is expected that the odometry would update at a slower rate and better represent the position of the robot’s dead reckoning. The odometry reported for our vehicle had it spin 5+ times in a circle within seconds (see screenshots).

Screenshots The odometry appears to loop multiple times in 10 seconds, forming a circle:

The expected behavior for a 20 degree steer would be more of a slope away from the forward direction:

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version: ROS2 Humble
• Run in docker container on Ubuntu 22.04

Additional context A comment on this closed issue may also be relevant, #611.

[christophfroehlich]

I tried to understand the code, but failed..

• In the header file, it is written that it should always be of unit m and rad, but it is called velocity. https://github.com/ros-controls/ros2_controllers/blob/ca38c06f712663449ddf1372234d74a983b7d5f5/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp#L195-L201
• From update_from_position/update_from_velocity I'd say that both are velocities and not displacements. https://github.com/ros-controls/ros2_controllers/blob/ca38c06f712663449ddf1372234d74a983b7d5f5/steering_controllers_library/src/steering_odometry.cpp#L146-L149 because docs say that inputs are rad/s and rad https://github.com/ros-controls/ros2_controllers/blob/ca38c06f712663449ddf1372234d74a983b7d5f5/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp#L92-L99
• Why is there the tan() missing, comparing it to the other functions https://github.com/ros-controls/ros2_controllers/blob/ca38c06f712663449ddf1372234d74a983b7d5f5/steering_controllers_library/src/steering_odometry.cpp#L172
• The library lacks any tests for these functions.

@reydelsol would you like to help to sort that stuff out, ideally including simple tests for the functions?

[christophfroehlich]

or maybe @petkovich is around and can clarify this?

[ARK3r]

I am also confused so I started studying the steering libraries, and I'll leave here what I find in hopes that this might be useful in expediting the process of implementing the proposal here:

• one confusing item I found was the difference between reference_callback and reference_callback_unstamped in the steering_controllers_library:

https://github.com/ros-controls/ros2_controllers/blob/f62fc3ac5f480b5a74765e642cd33d17600f7f5a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp#L144-L146

should reference_callback_unstamped also get a STEERING_CONTROLLERS__VISIBILITY_LOCAL?

Also, in reference_callback we have:

https://github.com/ros-controls/ros2_controllers/blob/f62fc3ac5f480b5a74765e642cd33d17600f7f5a/steering_controllers_library/src/steering_controllers_library.cpp#L229-L232

but in reference_callback_unstamped, after making a new twist message we're never writing to input_ref_:

https://github.com/ros-controls/ros2_controllers/blob/f62fc3ac5f480b5a74765e642cd33d17600f7f5a/steering_controllers_library/src/steering_controllers_library.cpp#L265-L268

Am I missing something or is this a bug?

• Then, assuming the twist message has been saved, inside update_reference_from_subscribers we set reference_interfaces_ to the linear and angular components of the twist msg:

https://github.com/ros-controls/ros2_controllers/blob/f62fc3ac5f480b5a74765e642cd33d17600f7f5a/steering_controllers_library/src/steering_controllers_library.cpp#L411-L418

• Then, inside update_and_write_commands we run update_odometry which is virtual inside steering_controllers_library and each steering controller should define for itself (ackermann, bicycle, and tricycle), which inside each update_odometry two seperate logics are implemented based on open_loop param being set to true or not.

  • If open_loop is true, odometry will update x_, y_, and heading_. I'm not sure if the calculations are correct considering the this current issue but I added a test here and the file can be found at build/steering_controllers_library/odom_test after colcon build. Two cases of going straight and moving-and-turning a little seem to be working correctly. With open_loop set to true, the steering_odometry object should strictly calculate x_, y_, and heading_, which to the best of my knowledge is what's being done, then each steering_controller subclass has to create values for all the joints it has available.
  • With this PR a buggy piece of the code will also be fixed.
  • If open_loop is false, then the values from the states should be read, and from them should the odometry be updated.

More investigation is needed to fully figure out the structure of the code before the proposal can be implemented, but I'm afraid I don't have the time to continue right now.