Skip to content

Commit e7ec195

Browse files
veqccpre-commit-ci[bot]
authored andcommitted
fix(autoware_map_based_prediction): fix syntaxError (autowarefoundation#7813)
* fix(autoware_map_based_prediction): fix syntaxError Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * style(pre-commit): autofix * fix spellcheck Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * fix new cppcheck warnings Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent fb6c0eb commit e7ec195

File tree

2 files changed

+20
-17
lines changed

2 files changed

+20
-17
lines changed

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

+7-6
Original file line numberDiff line numberDiff line change
@@ -1771,11 +1771,12 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
17711771
object.kinematics.acceleration_with_covariance.accel.linear.y)
17721772
: 0.0;
17731773
const double t_h = time_horizon;
1774-
const double λ = std::log(2) / acceleration_exponential_half_life_;
1774+
const double lambda = std::log(2) / acceleration_exponential_half_life_;
17751775

17761776
auto get_search_distance_with_decaying_acc = [&]() -> double {
17771777
const double acceleration_distance =
1778-
obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
1778+
obj_acc * (1.0 / lambda) * t_h +
1779+
obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
17791780
double search_dist = acceleration_distance + obj_vel * t_h;
17801781
return search_dist;
17811782
};
@@ -1787,13 +1788,13 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
17871788
return obj_vel * t_h;
17881789
}
17891790
// Time to reach final speed
1790-
const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
1791+
const double t_f = (-1.0 / lambda) * std::log(1 - ((final_speed - obj_vel) * lambda) / obj_acc);
17911792
// It is assumed the vehicle accelerates until final_speed is reached and
17921793
// then continues at constant speed for the rest of the time horizon
17931794
const double search_dist =
17941795
// Distance covered while accelerating
1795-
obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
1796-
obj_vel * t_f +
1796+
obj_acc * (1.0 / lambda) * t_f +
1797+
obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) + obj_vel * t_f +
17971798
// Distance covered at constant speed
17981799
final_speed * (t_h - t_f);
17991800
return search_dist;
@@ -1807,7 +1808,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
18071808
const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
18081809

18091810
double final_speed_after_acceleration =
1810-
obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
1811+
obj_vel + obj_acc * (1.0 / lambda) * (1.0 - std::exp(-lambda * t_h));
18111812

18121813
const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
18131814
const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;

perception/autoware_map_based_prediction/src/path_generator.cpp

+13-11
Original file line numberDiff line numberDiff line change
@@ -421,7 +421,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
421421

422422
// Using a decaying acceleration model. Consult the README for more information about the model.
423423
const double t_h = duration;
424-
const float λ = std::log(2) / acceleration_exponential_half_life_;
424+
const float lambda = std::log(2) / acceleration_exponential_half_life_;
425425

426426
auto have_same_sign = [](double a, double b) -> bool {
427427
return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0);
@@ -434,7 +434,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
434434
return v;
435435
}
436436
// Get velocity after time horizon
437-
const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h));
437+
const auto terminal_velocity = v + a * (1.0 / lambda) * (1 - std::exp(-lambda * t_h));
438438

439439
// If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
440440
// most stop, not reverse its direction)
@@ -443,15 +443,16 @@ FrenetPoint PathGenerator::getFrenetPoint(
443443
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
444444
// time t_stop
445445

446-
// 0 = Vo + acc(1/λ)(1-e^(-λt_stop))
447-
// e^(-λt_stop) = 1 - (-Vo* λ)/acc
448-
// t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc)
449-
// t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc)
450-
auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a));
446+
// 0 = Vo + acc(1/lambda)(1-e^(-lambda t_stop))
447+
// e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
448+
// t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
449+
// t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
450+
auto t_stop = (-1.0 / lambda) * std::log1p(v * lambda / a);
451451

452452
// Calculate the distance traveled until stopping
453453
auto distance_to_reach_zero_speed =
454-
v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
454+
v * t_stop + a * t_stop * (1.0 / lambda) +
455+
a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
455456
// Output an equivalent constant speed
456457
return distance_to_reach_zero_speed / t_h;
457458
}
@@ -461,17 +462,18 @@ FrenetPoint PathGenerator::getFrenetPoint(
461462
// assume it will continue accelerating (reckless driving)
462463
const bool object_has_surpassed_limit_already = v > speed_limit;
463464
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
464-
return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1);
465+
return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);
465466

466467
// It is assumed the vehicle accelerates until final_speed is reached and
467468
// then continues at constant speed for the rest of the time horizon
468469
// So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
469470
// would go if it accelerated until reaching the speed limit, and then continued at a constant
470471
// speed.
471-
const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a);
472+
const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);
472473
const double distance_covered =
473474
// Distance covered while accelerating
474-
a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f +
475+
a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) +
476+
v * t_f +
475477
// Distance covered at constant speed for the rest of the horizon time
476478
speed_limit * (t_h - t_f);
477479
return distance_covered / t_h;

0 commit comments

Comments
 (0)