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>

Author: 2 people

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

@@ -1771,11 +1771,12 @@ 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::expm1(-lambda * t_h);
     double search_dist = acceleration_distance + obj_vel * t_h;
     return search_dist;
   };
@@ -1787,13 +1788,13 @@ 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_vel * t_f +
+      obj_acc * (1.0 / lambda) * 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;
@@ -1807,7 +1808,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;

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,16 @@ 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^(-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::log1p(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::expm1(-lambda * t_h);
       // Output an equivalent constant speed
       return distance_to_reach_zero_speed / t_h;
     }
@@ -461,17 +462,18 @@ 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::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
     // 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::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);
     return distance_covered / t_h;