velocity_set: avoidSuddenDeceleration function behaves incorrectly.
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.
Edited by Jilin Zhou