From 8723196bb916ce52db623bb55b1828bb72fe36c6 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Tue, 15 Aug 2023 17:26:18 +0900
Subject: [PATCH] fix(avoidance): fix trimmed shift line alignment

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../avoidance/avoidance_module.cpp            | 27 ++++++++++++++++---
 1 file changed, 23 insertions(+), 4 deletions(-)

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 3fb333036da26..cde5437148cd7 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -1421,12 +1421,19 @@ void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const dou
   AvoidLineArray input = shift_lines;
   shift_lines.clear();
 
-  for (const auto & s : input) {
-    if (s.getRelativeLongitudinal() < threshold) {
+  for (size_t i = 0; i < input.size(); ++i) {
+    if (input.at(i).getRelativeLongitudinal() > threshold) {
+      shift_lines.push_back(input.at(i));
       continue;
     }
 
-    shift_lines.push_back(s);
+    if (shift_lines.empty()) {
+      continue;
+    }
+
+    utils::avoidance::setEndData(
+      shift_lines.back(), input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx,
+      input.at(i).end_longitudinal);
   }
 }
 
@@ -2347,12 +2354,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat
           std::abs(candidates.at(i).getRelativeLength()) >
           parameters_->lateral_small_shift_threshold) {
           if (has_large_shift) {
-            break;
+            return;
           }
 
           has_large_shift = true;
         }
 
+        if (!isComfortable(AvoidLineArray{candidates.at(i)})) {
+          return;
+        }
+
         subsequent.push_back(candidates.at(i));
       }
     };
@@ -2361,10 +2372,18 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat
   const auto get_subsequent_shift = [&, this](size_t i) {
     AvoidLineArray subsequent{candidates.at(i)};
 
+    if (!isComfortable(subsequent)) {
+      return subsequent;
+    }
+
     if (candidates.size() == i + 1) {
       return subsequent;
     }
 
+    if (!isComfortable(AvoidLineArray{candidates.at(i + 1)})) {
+      return subsequent;
+    }
+
     if (
       std::abs(candidates.at(i).getRelativeLength()) < parameters_->lateral_small_shift_threshold) {
       const auto has_large_shift =