From b02a20f07d66030179dbfd8b36a475802993ccaa Mon Sep 17 00:00:00 2001
From: Ryuta Kambe <ryuta.kambe@tier4.jp>
Date: Wed, 3 Jul 2024 23:07:27 +0900
Subject: [PATCH 1/5] fix(autoware_map_based_prediction): fix syntaxError
Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp>
---
.../src/map_based_prediction_node.cpp | 10 ++++-----
.../src/path_generator.cpp | 22 +++++++++----------
2 files changed, 16 insertions(+), 16 deletions(-)
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index c8c4e3bed2d01..3b74d7d34899b 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -1771,11 +1771,11 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
object.kinematics.acceleration_with_covariance.accel.linear.y)
: 0.0;
const double t_h = time_horizon;
- const double λ = std::log(2) / acceleration_exponential_half_life_;
+ const double lambda = std::log(2) / acceleration_exponential_half_life_;
auto get_search_distance_with_decaying_acc = [&]() -> double {
const double acceleration_distance =
- obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
+ obj_acc * (1.0 / lambda) * t_h + obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
double search_dist = acceleration_distance + obj_vel * t_h;
return search_dist;
};
@@ -1787,12 +1787,12 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
return obj_vel * t_h;
}
// Time to reach final speed
- const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
+ const double t_f = (-1.0 / lambda) * std::log(1 - ((final_speed - obj_vel) * lambda) / obj_acc);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
const double search_dist =
// Distance covered while accelerating
- obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
+ obj_acc * (1.0 / lambda) * t_f + obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) +
obj_vel * t_f +
// Distance covered at constant speed
final_speed * (t_h - t_f);
@@ -1807,7 +1807,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
double final_speed_after_acceleration =
- obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
+ obj_vel + obj_acc * (1.0 / lambda) * (1.0 - std::exp(-lambda * t_h));
const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index aeb1b0fedd33f..0077457daa20d 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -421,7 +421,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
// Using a decaying acceleration model. Consult the README for more information about the model.
const double t_h = duration;
- const float λ = std::log(2) / acceleration_exponential_half_life_;
+ const float lambda = std::log(2) / acceleration_exponential_half_life_;
auto have_same_sign = [](double a, double b) -> bool {
return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0);
@@ -434,7 +434,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
return v;
}
// Get velocity after time horizon
- const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h));
+ const auto terminal_velocity = v + a * (1.0 / lambda) * (1 - std::exp(-lambda * t_h));
// If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
// most stop, not reverse its direction)
@@ -443,15 +443,15 @@ FrenetPoint PathGenerator::getFrenetPoint(
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
// time t_stop
- // 0 = Vo + acc(1/λ)(1-e^(-λt_stop))
- // e^(-λt_stop) = 1 - (-Vo* λ)/acc
- // t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc)
- // t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc)
- auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a));
+ // 0 = Vo + acc(1/lambda)(1-e^(-lambdat_stop))
+ // e^(-lambdat_stop) = 1 - (-Vo* lambda)/acc
+ // t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
+ // t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
+ auto t_stop = (-1.0 / lambda) * std::log(1 + (v * lambda / a));
// Calculate the distance traveled until stopping
auto distance_to_reach_zero_speed =
- v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
+ v * t_stop + a * t_stop * (1.0 / lambda) + a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
// Output an equivalent constant speed
return distance_to_reach_zero_speed / t_h;
}
@@ -461,17 +461,17 @@ FrenetPoint PathGenerator::getFrenetPoint(
// assume it will continue accelerating (reckless driving)
const bool object_has_surpassed_limit_already = v > speed_limit;
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
- return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1);
+ return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * (std::exp(-lambda * t_h) - 1);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
// So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
// would go if it accelerated until reaching the speed limit, and then continued at a constant
// speed.
- const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a);
+ const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);
const double distance_covered =
// Distance covered while accelerating
- a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f +
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) + v * t_f +
// Distance covered at constant speed for the rest of the horizon time
speed_limit * (t_h - t_f);
return distance_covered / t_h;
From d745303d093c85234d40379877ee6be54b9b913a Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Wed, 3 Jul 2024 14:10:53 +0000
Subject: [PATCH 2/5] style(pre-commit): autofix
---
.../src/map_based_prediction_node.cpp | 7 ++++---
.../autoware_map_based_prediction/src/path_generator.cpp | 9 ++++++---
2 files changed, 10 insertions(+), 6 deletions(-)
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index 3b74d7d34899b..3361240be4c2f 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -1775,7 +1775,8 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
auto get_search_distance_with_decaying_acc = [&]() -> double {
const double acceleration_distance =
- obj_acc * (1.0 / lambda) * t_h + obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
+ obj_acc * (1.0 / lambda) * t_h +
+ obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
double search_dist = acceleration_distance + obj_vel * t_h;
return search_dist;
};
@@ -1792,8 +1793,8 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
// then continues at constant speed for the rest of the time horizon
const double search_dist =
// Distance covered while accelerating
- obj_acc * (1.0 / lambda) * t_f + obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) +
- obj_vel * t_f +
+ obj_acc * (1.0 / lambda) * t_f +
+ obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) + obj_vel * t_f +
// Distance covered at constant speed
final_speed * (t_h - t_f);
return search_dist;
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index 0077457daa20d..c7a1725aaa6b4 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -451,7 +451,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
// Calculate the distance traveled until stopping
auto distance_to_reach_zero_speed =
- v * t_stop + a * t_stop * (1.0 / lambda) + a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
+ v * t_stop + a * t_stop * (1.0 / lambda) +
+ a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
// Output an equivalent constant speed
return distance_to_reach_zero_speed / t_h;
}
@@ -461,7 +462,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
// assume it will continue accelerating (reckless driving)
const bool object_has_surpassed_limit_already = v > speed_limit;
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
- return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * (std::exp(-lambda * t_h) - 1);
+ return v + a * (1.0 / lambda) +
+ (a / (t_h * std::pow(lambda, 2))) * (std::exp(-lambda * t_h) - 1);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
@@ -471,7 +473,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);
const double distance_covered =
// Distance covered while accelerating
- a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) + v * t_f +
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) +
+ v * t_f +
// Distance covered at constant speed for the rest of the horizon time
speed_limit * (t_h - t_f);
return distance_covered / t_h;
From 4e90378c31c6bc5f4d310fc8dd6b85a36fcb3f4e Mon Sep 17 00:00:00 2001
From: Ryuta Kambe <ryuta.kambe@tier4.jp>
Date: Wed, 3 Jul 2024 23:22:38 +0900
Subject: [PATCH 3/5] fix spellcheck
Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp>
---
.../autoware_map_based_prediction/src/path_generator.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index c7a1725aaa6b4..2f0dbec61b3d4 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -443,8 +443,8 @@ FrenetPoint PathGenerator::getFrenetPoint(
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
// time t_stop
- // 0 = Vo + acc(1/lambda)(1-e^(-lambdat_stop))
- // e^(-lambdat_stop) = 1 - (-Vo* lambda)/acc
+ // 0 = Vo + acc(1/lambda)(1-e^(-lambda t_stop))
+ // e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
// t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
// t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
auto t_stop = (-1.0 / lambda) * std::log(1 + (v * lambda / a));
From 84f242b5a812b385e1b516c004cac60e3df564ec Mon Sep 17 00:00:00 2001
From: Ryuta Kambe <ryuta.kambe@tier4.jp>
Date: Wed, 3 Jul 2024 23:29:40 +0900
Subject: [PATCH 4/5] fix new cppcheck warnings
Signed-off-by: Ryuta Kambe <ryuta.kambe@tier4.jp>
---
.../src/map_based_prediction_node.cpp | 4 ++--
.../autoware_map_based_prediction/src/path_generator.cpp | 8 ++++----
2 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index 3361240be4c2f..2b8358eb52e4e 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -1776,7 +1776,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
auto get_search_distance_with_decaying_acc = [&]() -> double {
const double acceleration_distance =
obj_acc * (1.0 / lambda) * t_h +
- obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
+ obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
double search_dist = acceleration_distance + obj_vel * t_h;
return search_dist;
};
@@ -1794,7 +1794,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
const double search_dist =
// Distance covered while accelerating
obj_acc * (1.0 / lambda) * t_f +
- obj_acc * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) + obj_vel * t_f +
+ obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) + obj_vel * t_f +
// Distance covered at constant speed
final_speed * (t_h - t_f);
return search_dist;
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index 2f0dbec61b3d4..d9b401a9325fe 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -447,12 +447,12 @@ FrenetPoint PathGenerator::getFrenetPoint(
// e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
// t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
// t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
- auto t_stop = (-1.0 / lambda) * std::log(1 + (v * lambda / a));
+ auto t_stop = (-1.0 / lambda) * std::log1p(v * lambda / a);
// Calculate the distance traveled until stopping
auto distance_to_reach_zero_speed =
v * t_stop + a * t_stop * (1.0 / lambda) +
- a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_h) - 1);
+ a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
// Output an equivalent constant speed
return distance_to_reach_zero_speed / t_h;
}
@@ -463,7 +463,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
const bool object_has_surpassed_limit_already = v > speed_limit;
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
return v + a * (1.0 / lambda) +
- (a / (t_h * std::pow(lambda, 2))) * (std::exp(-lambda * t_h) - 1);
+ (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
@@ -473,7 +473,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);
const double distance_covered =
// Distance covered while accelerating
- a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * (std::exp(-lambda * t_f) - 1) +
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) +
v * t_f +
// Distance covered at constant speed for the rest of the horizon time
speed_limit * (t_h - t_f);
From 2ad9c58b854e1957fddfb1ff7cfe9560cb703e3a Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Wed, 3 Jul 2024 14:32:49 +0000
Subject: [PATCH 5/5] style(pre-commit): autofix
---
.../autoware_map_based_prediction/src/path_generator.cpp | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index d9b401a9325fe..dac034a6b7a7d 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -462,8 +462,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
// assume it will continue accelerating (reckless driving)
const bool object_has_surpassed_limit_already = v > speed_limit;
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
- return v + a * (1.0 / lambda) +
- (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);
+ return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon