@@ -451,7 +451,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
451
451
452
452
// Calculate the distance traveled until stopping
453
453
auto distance_to_reach_zero_speed =
454
- v * t_stop + a * t_stop * (1.0 / lambda) + a * (1.0 / std::pow (lambda, 2 )) * (std::exp (-lambda * t_h) - 1 );
454
+ v * t_stop + a * t_stop * (1.0 / lambda) +
455
+ a * (1.0 / std::pow (lambda, 2 )) * (std::exp (-lambda * t_h) - 1 );
455
456
// Output an equivalent constant speed
456
457
return distance_to_reach_zero_speed / t_h;
457
458
}
@@ -461,7 +462,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
461
462
// assume it will continue accelerating (reckless driving)
462
463
const bool object_has_surpassed_limit_already = v > speed_limit;
463
464
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
464
- return v + a * (1.0 / lambda) + (a / (t_h * std::pow (lambda, 2 ))) * (std::exp (-lambda * t_h) - 1 );
465
+ return v + a * (1.0 / lambda) +
466
+ (a / (t_h * std::pow (lambda, 2 ))) * (std::exp (-lambda * t_h) - 1 );
465
467
466
468
// It is assumed the vehicle accelerates until final_speed is reached and
467
469
// then continues at constant speed for the rest of the time horizon
@@ -471,7 +473,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
471
473
const double t_f = (-1.0 / lambda) * std::log (1 - ((speed_limit - v) * lambda) / a);
472
474
const double distance_covered =
473
475
// Distance covered while accelerating
474
- a * (1.0 / lambda) * t_f + a * (1.0 / std::pow (lambda, 2 )) * (std::exp (-lambda * t_f) - 1 ) + v * t_f +
476
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow (lambda, 2 )) * (std::exp (-lambda * t_f) - 1 ) +
477
+ v * t_f +
475
478
// Distance covered at constant speed for the rest of the horizon time
476
479
speed_limit * (t_h - t_f);
477
480
return distance_covered / t_h;
0 commit comments