fix(avoidance): rebuild prepare distance calculation/validation logic

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -97,16 +97,18 @@ class AvoidanceHelper
     return std::max(getEgoSpeed(), values.at(idx));
   }
 
-  double getMinimumPrepareDistance() const
+  double getNominalPrepareDistance(const double velocity) const
   {
     const auto & p = parameters_;
-    return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance);
+    const auto & values = p->velocity_map;
+    const auto idx = getConstraintsMapIndex(velocity, values);
+    return std::max(values.at(idx) * p->max_prepare_time, p->min_prepare_distance);
   }
 
   double getNominalPrepareDistance() const
   {
     const auto & p = parameters_;
-    return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance);
+    return std::max(getAvoidanceEgoSpeed() * p->max_prepare_time, p->min_prepare_distance);
   }
 
   double getNominalAvoidanceDistance(const double shift_length) const
@@ -298,6 +300,13 @@ class AvoidanceHelper
     return std::numeric_limits<double>::max();
   }
 
+  bool isEnoughPrepareDistance(const double prepare_distance) const
+  {
+    const auto & p = parameters_;
+    return prepare_distance >
+           std::max(getEgoSpeed() * p->min_prepare_distance, p->min_prepare_distance);
+  }
+
   bool isComfortable(const AvoidLineArray & shift_lines) const
   {
     const auto JERK_BUFFER = 0.1;  // [m/sss]
@@ -328,7 +337,7 @@ class AvoidanceHelper
     const auto desire_shift_length =
       getShiftLength(object, is_object_on_right, object.avoid_margin.value());
 
-    const auto prepare_distance = getMinimumPrepareDistance();
+    const auto prepare_distance = getNominalPrepareDistance(0.0);
     const auto constant_distance = getFrontConstantDistance(object);
     const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
 

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -409,9 +409,9 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
   const auto registered_lines = path_shifter_.getShiftLines();
   if (!registered_lines.empty()) {
     const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
-    const auto to_shift_start_point = motion_utils::calcSignedArcLength(
+    const auto prepare_distance = motion_utils::calcSignedArcLength(
       path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx);
-    if (to_shift_start_point < helper_->getMinimumPrepareDistance()) {
+    if (!helper_->isEnoughPrepareDistance(prepare_distance)) {
       RCLCPP_DEBUG(
         getLogger(),
         "Distance to shift start point is less than minimum prepare distance. The distance is not "
@@ -1418,10 +1418,11 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
   const auto avoidance_distance = helper_->getMinAvoidanceDistance(
     helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
   const auto constant_distance = helper_->getFrontConstantDistance(object);
+  const auto prepare_distance = helper_->getNominalPrepareDistance(0.0);
 
   return object.longitudinal -
          std::min(
-           avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer,
+           avoidance_distance + constant_distance + prepare_distance + p->stop_buffer,
            p->stop_max_distance);
 }
 
@@ -1450,7 +1451,7 @@ void AvoidanceModule::insertReturnDeadLine(
   const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end);
 
   const auto min_return_distance =
-    helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance();
+    helper_->getMinAvoidanceDistance(shift_length) + helper_->getNominalPrepareDistance(0.0);
   const auto to_stop_line = data.to_return_point - min_return_distance - buffer;
 
   // If we don't need to consider deceleration constraints, insert a deceleration point

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -929,7 +929,7 @@ void ShiftLineGenerator::applySmallShiftFilter(
       continue;
     }
 
-    if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) {
+    if (!helper_->isEnoughPrepareDistance(s.start_longitudinal)) {
       continue;
     }
 
@@ -1173,13 +1173,13 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     std::max(nominal_prepare_distance - last_sl_distance, 0.0);
 
   double prepare_distance_scaled = std::max(
-    helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance));
+    helper_->getNominalPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance));
   double avoid_distance_scaled = nominal_avoid_distance;
   if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) {
     const auto scale = (remaining_distance - last_sl_distance) /
                        std::max(nominal_avoid_distance + variable_prepare_distance, 0.1);
     prepare_distance_scaled = std::max(
-      helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance);
+      helper_->getNominalPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance);
     avoid_distance_scaled *= scale;
   }
 
@@ -1292,7 +1292,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine(
     const auto & candidate = shift_lines.at(i);
 
     // prevent sudden steering.
-    if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) {
+    if (!helper_->isEnoughPrepareDistance(candidate.start_longitudinal)) {
       break;
     }