fix(avoidance): fix trimmed shift line alignment

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -329,6 +329,8 @@ class AvoidanceModule : public SceneModuleInterface
   AvoidLineArray applyPreProcessToRawShiftLines(
     AvoidLineArray & current_raw_shift_points, DebugData & debug) const;
 
+  AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const;
+
   /*
    * @brief merge negative & positive shift lines.
    * @param original shift lines.

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -704,6 +704,8 @@ void AvoidanceModule::updateRegisteredRawShiftLines()
 AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines(
   AvoidLineArray & raw_shift_lines, DebugData & debug) const
 {
+  const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines);
+
   /**
    * Use all registered points. For the current points, if the similar one of the current
    * points are already registered, will not use it.
@@ -733,6 +735,8 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines(
    * Add gap filled shift lines so that merged shift lines connect smoothly.
    */
   fillShiftLineGap(raw_shift_lines);
+  raw_shift_lines.insert(
+    raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end());
   debug.gap_filled = raw_shift_lines;
 
   /**
@@ -978,10 +982,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
       al_avoid.end_longitudinal = o.longitudinal - offset;
       al_avoid.id = getOriginalShiftLineUniqueId();
       al_avoid.object = o;
-
-      if (is_valid_shift_line(al_avoid)) {
-        avoid_lines.push_back(al_avoid);
-      }
     }
 
     AvoidLine al_return;
@@ -1001,10 +1001,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
         o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance);
       al_return.id = getOriginalShiftLineUniqueId();
       al_return.object = o;
+    }
 
-      if (is_valid_shift_line(al_return)) {
-        avoid_lines.push_back(al_return);
-      }
+    if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
+      avoid_lines.push_back(al_avoid);
+      avoid_lines.push_back(al_return);
     }
 
     o.is_avoidable = true;
@@ -1220,6 +1221,38 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_
   return merged_avoid_lines;
 }
 
+AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const
+{
+  AvoidLineArray ret{};
+
+  if (shift_lines.empty()) {
+    return ret;
+  }
+
+  const auto calc_gap_shift_line = [&](const auto & line1, const auto & line2) {
+    AvoidLine gap_filled_line{};
+    gap_filled_line.start_shift_length = line1.end_shift_length;
+    gap_filled_line.start_longitudinal = line1.end_longitudinal;
+    gap_filled_line.end_shift_length = line2.start_shift_length;
+    gap_filled_line.end_longitudinal = line2.start_longitudinal;
+    gap_filled_line.id = getOriginalShiftLineUniqueId();
+
+    return gap_filled_line;
+  };
+
+  // fill gap among shift lines.
+  for (size_t i = 0; i < shift_lines.size() - 1; i += 2) {
+    if (shift_lines.at(i).end_longitudinal > shift_lines.at(i + 1).start_longitudinal) {
+      continue;
+    }
+    ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1)));
+  }
+
+  utils::avoidance::fillAdditionalInfoFromLongitudinal(avoidance_data_, ret);
+
+  return ret;
+}
+
 void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const
 {
   using utils::avoidance::setEndData;
@@ -2347,12 +2380,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 +2398,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 =