fix(avoidance): return shift path was not generated expectedly (autow…

…arefoundation#6017) fix(avoidance): output return shift path properly Signed-off-by: satoshi-ota <[email protected]>
tier4 · Jan 11, 2024 · 8331124 · 8331124
1 parent 14c6562
commit 8331124
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 4 deletions.
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -1390,7 +1390,8 @@ void AvoidanceModule::insertReturnDeadLine(
     shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1);
   const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end);
 
-  const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length);
+  const auto min_return_distance =
+    helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance();
   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

diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -1101,12 +1101,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   const double variable_prepare_distance =
     std::max(nominal_prepare_distance - last_sl_distance, 0.0);
 
-  double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance);
+  double prepare_distance_scaled =
+    std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), 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 = last_sl_distance + scale * nominal_prepare_distance;
+    prepare_distance_scaled = std::max(
+      helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance);
     avoid_distance_scaled *= scale;
   }
 
@@ -1219,7 +1221,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine(
     const auto & candidate = shift_lines.at(i);
 
     // prevent sudden steering.
-    if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) {
+    if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) {
       break;
     }