[motion_velocity_smoother] steer rate calculation looks wrong

[VRichardJP]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

Here:

autoware.universe/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

Lines 218 to 235 in 2edd55e

	for (size_t i = 1; i < output.size() - 1; i++) {
	// velocity
	const auto & v_front = output.at(i + 1).longitudinal_velocity_mps;
	const auto & v_back = output.at(i).longitudinal_velocity_mps;
	// steer
	auto & steer_front = output.at(i + 1).front_wheel_angle_rad;
	auto & steer_back = output.at(i).front_wheel_angle_rad;
	// calculate the just 2 steering angle
	steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i));
	steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i + 1));
	const auto mean_vel = 0.5 * (v_front + v_back);
	const auto dt = std::max(points_interval / mean_vel, std::numeric_limits<double>::epsilon());
	const auto steering_diff = std::fabs(steer_front - steer_back);
	steer_rate_arr.at(i) = steering_diff / dt;
	}

we go through trajectory points with a sliding window of 2 points. There are few issues with this loop:

• fix(motion_velocity_smoother): calc first point front_wheel_angle_rad #5989 loop does not start at 0 but 1. I don't think it is necessary
• (minor?) front_wheel_angle_rad values are calculated twice, once with i+1 and on the next loop with i. The loop does 2 things at the same time: calculating front_wheel_angle_rad values and steer_rate. Duplicated computation could be avoiding by spliting the loop into 2.
• fix(motion_velocity_smoother): front wheel steer rate calculation #5965 "front" values are for index i+1 and "back" values are for index i, however for the atan lines the calculation looks reversed:

    steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i));
    steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i + 1));

I guess it should be:

    steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i + 1));
    steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i));

Expected behavior

N/A

Actual behavior

N/A

Steps to reproduce

N/A

Versions

No response

Possible causes

No response

Additional context

No response

[rej55]

@VRichardJP

Thank you for your information.
I agree it should be fixed.
I will approve the PR.

However, the behavior of code is not changed because

    const auto steering_diff = std::fabs(steer_front - steer_back);

is not changed whether or not change the atan calculation.

[VRichardJP]

Yes, however the PR shifts front_wheel_angle_rad by 1 point backward, so this may have a little impact on Autoware turning behavior (if the value is used to compute steering).

[VRichardJP]

Edit: I still get the issue with the fix, so it may not be the root cause.

Regarding the loop starting at 1 instead of 0, I think this is the cause of one of the issues I am facing with the freespace planner. When the vehicle is stopped half way in a turning manoeuver, Autoware sometimes refuse to restart the vehicle. The PID longitudinal controller reports this error: target speed > 0, but keep stop condition is met. Keep STOPPED.. The stop condition is met because lateral_sync_data_.is_steer_converged is false and I guess this is because the current steering is far from the target steering of the trajectory (which is 0.0 because not updated by the motion_velocity_smoother).

For example, here is one trajectory calculated by motion_velocity_smoother while the vehicle is stopped with the steering wheel turned:

header:
  stamp:
    sec: 1703740309
    nanosec: 80912131
  frame_id: map
points:
- time_from_start:
    sec: 0
    nanosec: 0
  pose:
    position:
      x: 3738.7482156691976
      y: 73759.76434660016
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.36787980716724306
      w: 0.9298733502357146
  longitudinal_velocity_mps: 0.25
  lateral_velocity_mps: 0.0
  acceleration_mps2: 0.10000000149011612
  heading_rate_rps: 0.060412731021642685
  front_wheel_angle_rad: 0.0
  rear_wheel_angle_rad: 0.0
- time_from_start:
    sec: 0
    nanosec: 325953602
  pose:
    position:
      x: 3738.821148558692
      y: 73759.6959302744
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.35793588138765237
      w: 0.9337461672292126
  longitudinal_velocity_mps: 0.28722813725471497
  lateral_velocity_mps: 0.0
  acceleration_mps2: 0.37421149015426636
  heading_rate_rps: 0.11836133897304535
  front_wheel_angle_rad: 0.500284731388092
  rear_wheel_angle_rad: 0.0
---
[...]

As you can see, the first point front_wheel_angle_rad is 0.0, while other points have a valid value.

This problem may be limited to the planning simulator, as the simulator seems not able to turn the steering in place.

Anyway, I think it is better not to miss the first point front_wheel_angle_rad value.

[mehmetdogru]

@VRichardJP

If this PR solves the issue completely please close the issue.