Incorrect odometry when turning vehicle with Ackermann steering controller (ROS2 Humble).

[aizmailovs]

Description of the bug
When turning the vehicle with Ackermann steering controller, the odometry gets wrong. This issue looks similar to the #789.

To Reproduce
To reproduce the bug, launch the car-like robot with the ackermann_steering_controller. My .yaml file is:

controller_manager:
  ros__parameters:
    update_rate: 50

    use_sim_time: true

    joint_state_controller:
      type: joint_state_broadcaster/JointStateBroadcaster

    ackermann_steering_controller:
      type: ackermann_steering_controller/AckermannSteeringController

ackermann_steering_controller:
  ros__parameters:

    reference_timeout: 2.0
    front_steering: true
    open_loop: false
    velocity_rolling_window_size: 10
    position_feedback: true
    use_stamped_vel: true   # Use true to make controller topic /ackermann_steering_controller/reference listen to TwistStamped type data
                            # Use false ro make controller topic /ackermann_steering_controller/reference_unstamped listen to Twist type data
    rear_wheels_names: [right_rear_joint, left_rear_joint]
    front_wheels_names: [front_steer_right_joint, front_steer_left_joint]

    wheelbase: 0.65               # Distance between front and rear wheel axes
    front_wheel_track: 0.605      # Distance between front wheels
    rear_wheel_track: 0.605       # Distance between rear wheels
    front_wheels_radius: 0.165
    rear_wheels_radius: 0.165

joint_state_controller:
  ros__parameters:
    type: joint_state_controller/JointStateController

Reality and expected behavior
While the robot is going in a straight line, the odometry is pretty accurate, but when the robot turns the odometry starts to spin with much shorter radius than expected.

odometry2.webm

Just curious if anybody else have similar issue with Ackermann steering controller.

[aizmailovs]

Two (out of three) ways to calculate the odometry is to use position of the steering whells with position of the traction wheels or with the velocity of the traction wheels according to:

ros2_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp

Lines 57 to 91 in 13a43c3

	bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
	{
	if (params_.open_loop)
	{
	odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
	}
	else
	{
	const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
	const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
	const double front_right_steer_position =
	state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
	const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
	if (
	!std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
	!std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
	{
	if (params_.position_feedback)
	{
	// Estimate linear and angular velocity using joint information
	odometry_.update_from_position(
	rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
	front_left_steer_position, period.seconds());
	}
	else
	{
	// Estimate linear and angular velocity using joint information
	odometry_.update_from_velocity(
	rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
	front_left_steer_position, period.seconds());
	}
	}
	}
	return true;
	}

ros2_controllers/steering_controllers_library/src/steering_odometry.cpp

Lines 116 to 140 in 13a43c3

	bool SteeringOdometry::update_from_position(
	const double traction_right_wheel_pos, const double traction_left_wheel_pos,
	const double right_steer_pos, const double left_steer_pos, const double dt)
	{
	/// Get current wheel joint positions:
	const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_;
	const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_;
	const double traction_right_wheel_est_pos_diff =
	traction_right_wheel_cur_pos - traction_right_wheel_old_pos_;
	const double traction_left_wheel_est_pos_diff =
	traction_left_wheel_cur_pos - traction_left_wheel_old_pos_;
	/// Update old position with current:
	traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos;
	traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos;
	/// Compute linear and angular diff:
	const double linear_velocity =
	(traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
	steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
	const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
	return update_odometry(linear_velocity, angular, dt);
	}

ros2_controllers/steering_controllers_library/src/steering_odometry.cpp

Lines 165 to 175 in 13a43c3

	bool SteeringOdometry::update_from_velocity(
	const double right_traction_wheel_vel, const double left_traction_wheel_vel,
	const double right_steer_pos, const double left_steer_pos, const double dt)
	{
	steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
	double linear_velocity =
	(right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
	const double angular = steer_pos_ * linear_velocity / wheelbase_;
	return update_odometry(linear_velocity, angular, dt);
	}

The thing I do not really understand why the same value (rear_right_wheel_value, rear_left_wheel_value) is used as inputs for two different functions (update_from_position and update_from_velocity), which expect positions or velocities of the traction wheels respectively.

Both ways to calculate the odometry result in similar outcome as on a video above.

[kartman88]

I'm trying to reproduce your error but when I launch everything I don't have odom fixed frame in Rviz. But the odometry topic is published. Is there some param to set to enable odometry in the controller settings?

[aizmailovs]

I did not use Rviz, refer to the #937 (comment).

I have only looked at the topic "/ackermann_steering_controller/odometry" for odometry data from the controller. Then, I visualized it.

[BalazsPh21]

I think I have the same issue and also looking for a solution. Someone installed my project and reported the same issue. Also, running it in Docker doesn't help.

Here is a recording I made of what I believe is a really similar problem:

Screencast.from.2024.01.18.18_52_49.mp4

In case you want to try it yourself, you can find my project here. I hope it helps.

[rbretmounet]

I think I have the same issue and also looking for a solution. Someone installed my project and reported the same issue. Also, running it in Docker doesn't help.

Here is a recording I made of what I believe is a really similar problem:

Screencast.from.2024.01.18.18_52_49.mp4
In case you want to try it yourself, you can find my project here. I hope it helps.

@BalazsPh21 @aleksandrsizmailovs Did you ever resolve this issue?

[christophfroehlich]

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