Skip to content

[#16] avoidSuddenDeceleration function incorrectly set the velocity during acceleration phase

Jilin Zhou requested to merge astuff/autoware.ai/core_planning:jz/upstream_fix_avoid_sudden_deceleration into master

Here is the code snippet of function VelocitySetPath::avoidSuddenDeceleration():

void VelocitySetPath::avoidSuddenDeceleration(double velocity_change_limit, double deceleration, int closest_waypoint)
{
  if (closest_waypoint < 0)
    return;

  const double closest_vel = updated_waypoints_.waypoints[closest_waypoint].twist.twist.linear.x;

  // accelerate or decelerate within the limit
  if (std::abs(current_vel_ - closest_vel) < velocity_change_limit)
    return;

  // bring up the forward waypoints' velocity to avoid sudden deceleration.
  for (int i = 0;; i++)
  {
    if (!checkWaypoint(closest_waypoint + i))
      return;

    // sqrt(v^2 - 2ax)
    std::array<int, 2> range = {closest_waypoint, closest_waypoint + i};
    double changed_vel = calcChangedVelocity(std::abs(current_vel_) - velocity_change_limit, -deceleration, range);
    const double target_vel = updated_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x;

    if (std::isnan(changed_vel))
    {
      break;
    }
    const int sgn = (target_vel < 0) ? -1 : 1;
    updated_waypoints_.waypoints[closest_waypoint + i].twist.twist.linear.x = sgn * changed_vel;
  }

}

The function suppose to prevent abrupt deceleration for the ego-vehicle. However, when the target_vel is larger than the current_vel of ego_vehicle (acceleration phase) and its delta is bigger than velocity_change_limit, it also modifies the velocity of waypoints ahead. This is just wrong and make the vehicle move at turtle speed.

The fix is verified at Ottawa L5 track.

Merge request reports