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.
  

  ros2_controllers/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

  Lines 195 to 201 in ca38c06

  	/**
  	* \brief Uses precomputed linear and angular velocities to compute dometry and update
  	* accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt)
  	* computed by previous odometry method \param angular Angular velocity [rad] (angular
  	* displacement, i.e. m/s * dt) computed by previous odometry method
  	*/
  	bool update_odometry(const double linear_velocity, const double angular, const double dt);

• From update_from_position/update_from_velocity I'd say that both are velocities and not displacements.
  

  ros2_controllers/steering_controllers_library/src/steering_odometry.cpp

  Lines 146 to 149 in ca38c06

  	double linear_velocity = traction_wheel_vel * wheel_radius_;
  	const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
  	return update_odometry(linear_velocity, angular, dt);

  
  because docs say that inputs are rad/s and rad
  

  ros2_controllers/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

  Lines 92 to 99 in ca38c06

  	* \brief Updates the odometry class with latest wheels position
  	* \param traction_wheel_vel Traction wheel velocity [rad/s]
  	* \param front_steer_pos Steer wheel position [rad]
  	* \param dt time difference to last call
  	* \return true if the odometry is actually updated
  	*/
  	bool update_from_velocity(
  	const double traction_wheel_vel, const double steer_pos, const double dt);

• Why is there the tan() missing, comparing it to the other functions
  

  ros2_controllers/steering_controllers_library/src/steering_odometry.cpp

  Line 172 in ca38c06

  const double angular = steer_pos_ * linear_velocity / wheelbase_;

• 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:

ros2_controllers/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

Lines 144 to 146 in f62fc3a

	STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback(
	const std::shared_ptr<ControllerTwistReferenceMsg> msg);
	void reference_callback_unstamped(const std::shared_ptr<geometry_msgs::msg::Twist> msg);

should reference_callback_unstamped also get a STEERING_CONTROLLERS__VISIBILITY_LOCAL?

Also, in reference_callback we have:

ros2_controllers/steering_controllers_library/src/steering_controllers_library.cpp

Lines 229 to 232 in f62fc3a

	if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
	{
	input_ref_.writeFromNonRT(msg);
	}

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

ros2_controllers/steering_controllers_library/src/steering_controllers_library.cpp

Lines 265 to 268 in f62fc3a

	if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
	{
	twist_stamped->twist = *msg;
	}

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:

ros2_controllers/steering_controllers_library/src/steering_controllers_library.cpp

Lines 411 to 418 in f62fc3a

	if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0))
	{
	if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z))
	{
	reference_interfaces_[0] = current_ref->twist.linear.x;
	reference_interfaces_[1] = current_ref->twist.angular.z;
	}
	}

• 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.

[christophfroehlich]

This should be fixed with #1150 et al. Please reopen if this still doesn't work out.