fix(avoidance): fix invalid optional access

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -40,41 +40,41 @@
   return policy == "per_avoidance_maneuver";
 }
 
 AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const UUID id)
 {
   AvoidLine ret{};
 
   ret.start_idx = line1.start_idx;
   ret.start_shift_length = line1.start_shift_length;
   ret.start_longitudinal = line1.start_longitudinal;
 
   ret.end_idx = line2.end_idx;
   ret.end_shift_length = line2.end_shift_length;
   ret.end_longitudinal = line2.end_longitudinal;
 
   ret.id = id;
   ret.object = line1.object;
 
   return ret;
 }
 
 AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const UUID id)
 {
   AvoidLine ret{};
 
   ret.start_idx = line1.end_idx;
   ret.start_shift_length = line1.end_shift_length;
   ret.start_longitudinal = line1.end_longitudinal;
 
   ret.end_idx = line2.start_idx;
   ret.end_shift_length = line2.start_shift_length;
   ret.end_longitudinal = line2.start_longitudinal;
 
   ret.id = id;
   ret.object = line1.object;
 
   return ret;
 }
 
 AvoidLineArray toArray(const AvoidOutlines & outlines)
 {
@@ -88,7 +88,7 @@
       [&ret](const auto & line) { ret.push_back(line); });
   }
   return ret;
 }
 }  // namespace
 
 void ShiftLineGenerator::update(AvoidancePlanningData & data, DebugData & debug)
@@ -125,26 +125,26 @@
 
   // Calculate feasible shift length
   const auto get_shift_profile =
     [&](
       auto & object, const auto & desire_shift_length) -> std::optional<std::pair<double, double>> {
     // use each object param
     const auto object_type = utils::getHighestProbLabel(object.object.classification);
     const auto object_parameter = parameters_->object_parameters.at(object_type);
     const auto is_object_on_right = utils::avoidance::isOnRight(object);
 
     // use absolute dist for return-to-center, relative dist from current for avoiding.
     const auto avoiding_shift = desire_shift_length - current_ego_shift;
     const auto nominal_avoid_distance = helper_->getMaxAvoidanceDistance(avoiding_shift);
 
     // calculate remaining distance.
     const auto prepare_distance = helper_->getNominalPrepareDistance();
     const auto & additional_buffer_longitudinal =
       object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
                                                             : 0.0;
     const auto constant = object_parameter.safety_buffer_longitudinal +
                           additional_buffer_longitudinal + prepare_distance;
     const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
     const auto remaining_distance = object.longitudinal - constant;
     const auto avoidance_distance =
       has_enough_distance ? nominal_avoid_distance : remaining_distance;
 
@@ -175,7 +175,7 @@
     }
 
     // the avoidance path is already approved
     const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
     const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) ||
                              (helper_->getShift(object_pos) < 0.0 && !is_object_on_right);
     if (is_approved) {
@@ -184,12 +184,12 @@
 
     // prepare distance is not enough. unavoidable.
     if (remaining_distance < 1e-3) {
       object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
       return std::nullopt;
     }
 
     // calculate lateral jerk.
     const auto required_jerk = PathShifter::calcJerkFromLatLonDistance(
       avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed());
 
     // relax lateral jerk limit. avoidable.
@@ -199,36 +199,36 @@
 
     // avoidance distance is not enough. unavoidable.
     if (!isBestEffort(parameters_->policy_deceleration)) {
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return std::nullopt;
     }
 
     // output avoidance path under lateral jerk constraints.
     const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
       remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
 
     if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
       object.reason = "LessThanExecutionThreshold";
       return std::nullopt;
     }
 
     const auto feasible_shift_length =
       desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
                                 : -1.0 * feasible_relative_shift_length + current_ego_shift;
 
     const auto infeasible =
       std::abs(feasible_shift_length - object.overhang_dist) <
       0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return std::nullopt;
     }
 
     return std::make_pair(feasible_shift_length, avoidance_distance);
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
 
   const auto is_valid_shift_line = [](const auto & s) {
     return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
@@ -241,7 +241,7 @@
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
         continue;
       }
     }
 
@@ -253,20 +253,20 @@
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
         continue;
       }
     }
 
     // use each object param
     const auto object_type = utils::getHighestProbLabel(o.object.classification);
     const auto object_parameter = parameters_->object_parameters.at(object_type);
     const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
 
     if (!feasible_shift_profile.has_value()) {
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
         continue;
       }
     }
 
@@ -279,13 +279,13 @@
       const auto & additional_buffer_longitudinal =
         object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
                                                               : 0.0;
       const auto offset =
         object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
       const auto to_shift_end = o.longitudinal - offset;
       const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);
 
       // start point (use previous linear shift length as start shift length.)
       al_avoid.start_longitudinal = [&]() {
         const auto nearest_avoid_distance =
           std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3);
 
@@ -293,8 +293,8 @@
           return nearest_avoid_distance;
         }
 
         const auto minimum_avoid_distance = helper_->getMinAvoidanceDistance(
           feasible_shift_profile.value().first - current_ego_shift);
         const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);
 
         return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
@@ -306,8 +306,8 @@
       al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position);
 
       // end point
       al_avoid.end_shift_length = feasible_shift_profile.value().first;
       al_avoid.end_longitudinal = to_shift_end;
 
       // misc
       al_avoid.id = generateUUID();
@@ -317,23 +317,23 @@
 
     AvoidLine al_return;
     {
       const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
       const auto to_shift_start = o.longitudinal + offset;
 
       // start point
       al_return.start_shift_length = feasible_shift_profile.value().first;
       al_return.start_longitudinal = to_shift_start;
 
       // end point
       al_return.end_longitudinal = [&]() {
         if (data.to_return_point > to_shift_start) {
           return std::clamp(
             data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start);
         }
 
         return to_shift_start + feasible_return_distance;
       }();
       al_return.end_shift_length = 0.0;
 
       // misc
       al_return.id = generateUUID();
@@ -341,22 +341,22 @@
       al_return.object_on_right = utils::avoidance::isOnRight(o);
     }
 
     if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) {
       outlines.emplace_back(al_avoid, al_return);
     } else {
       o.reason = "InvalidShiftLine";
       continue;
     }
 
     o.is_avoidable = true;
   }
 
   utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines);
 
   debug.step1_current_shift_line = toArray(outlines);
 
   return outlines;
 }
 
 AvoidLineArray ShiftLineGenerator::applyPreProcess(
   const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const
@@ -454,22 +454,22 @@
     const auto & al = avoid_lines.at(j);
     for (size_t i = 0; i < N; ++i) {
       // calc current interpolated shift
       const auto i_shift = utils::avoidance::lerpShiftLengthOnArc(arcs.at(i), al);
 
       // update maximum shift for positive direction
       if (i_shift > sl.pos_shift_line.at(i)) {
         sl.pos_shift_line.at(i) = i_shift;
         sl.pos_shift_line_grad.at(i) = al.getGradient();
       }
 
       // update minumum shift for negative direction
       if (i_shift < sl.neg_shift_line.at(i)) {
         sl.neg_shift_line.at(i) = i_shift;
         sl.neg_shift_line_grad.at(i) = al.getGradient();
       }
 
       // store for debug print
       sl.shift_line_history.at(j).at(i) = i_shift;
     }
   }
 
@@ -509,15 +509,15 @@
     return;
   }
 
   const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) /
                                      avoid_lines.front().start_longitudinal;
 
   for (size_t i = data.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) {
     sl.shift_line.at(i) = helper_->getLinearShift(getPoint(path.points.at(i)));
     sl.shift_line_grad.at(i) = grad_first_shift_line;
   }
 
   sl.shift_line_history.push_back(sl.shift_line);
 }
 
 AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine(
@@ -538,7 +538,7 @@
     }
     const double ds = arcs.at(i) - arcs.at(i - 1);
     if (ds < 1.0e-5) {
       return sl.shift_line_grad.at(i);
     }  // use theoretical value when ds is too small.
     return (sl.shift_line.at(i) - sl.shift_line.at(i - 1)) / ds;
   };
@@ -549,7 +549,7 @@
     }
     const double ds = arcs.at(i + 1) - arcs.at(i);
     if (ds < 1.0e-5) {
       return sl.shift_line_grad.at(i);
     }  // use theoretical value when ds is too small.
     return (sl.shift_line.at(i + 1) - sl.shift_line.at(i)) / ds;
   };
@@ -623,7 +623,7 @@
   };
 
   const auto same_side_shift = [](const auto & line1, const auto & line2) {
     return line1.object_on_right == line2.object_on_right;
   };
 
   const auto within = [](const auto & line, const size_t idx) {
@@ -636,42 +636,42 @@
     auto & last_outline = ret.back();
     auto & next_outline = outlines.at(i);
 
     const auto & return_line = last_outline.return_line;
     const auto & avoid_line = next_outline.avoid_line;
 
     if (no_conflict(return_line, avoid_line)) {
       ret.push_back(outlines.at(i));
       continue;
     }
 
     const auto merged_shift_line = merge(return_line, avoid_line, generateUUID());
 
     if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) {
       ret.push_back(outlines.at(i));
       continue;
     }
 
     if (same_side_shift(return_line, avoid_line)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
 
     if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
 
     if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) {
       last_outline.middle_lines.push_back(merged_shift_line);
       last_outline.return_line = next_outline.return_line;
       debug.step1_merged_shift_line.push_back(merged_shift_line);
       continue;
     }
   }
 
   utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret);
   utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_merged_shift_line);
@@ -689,7 +689,7 @@
       const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID());
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
 
     helper_->alignShiftLinesOrder(outline.middle_lines, false);
 
@@ -697,7 +697,7 @@
       const auto new_line = fill(outline.avoid_line, outline.middle_lines.front(), generateUUID());
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
 
     helper_->alignShiftLinesOrder(outline.middle_lines, false);
 
@@ -705,7 +705,7 @@
       const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID());
       outline.middle_lines.push_back(new_line);
       debug.step1_filled_shift_line.push_back(new_line);
     }
 
     helper_->alignShiftLinesOrder(outline.middle_lines, false);
   }
@@ -714,7 +714,7 @@
   utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_filled_shift_line);
 
   return ret;
 }
 
 AvoidLineArray ShiftLineGenerator::applyFillGapProcess(
   const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, DebugData & debug) const
@@ -739,20 +739,20 @@
     const auto new_line = fill(ego_line, sorted.front(), generateUUID());
     ret.push_back(new_line);
     debug.step1_front_shift_line.push_back(new_line);
   }
 
   helper_->alignShiftLinesOrder(sorted, false);
 
   // fill gap among shift lines.
   for (size_t i = 0; i < sorted.size() - 1; ++i) {
     if (sorted.at(i + 1).start_longitudinal < sorted.at(i).end_longitudinal) {
       continue;
     }
 
     const auto new_line = fill(sorted.at(i), sorted.at(i + 1), generateUUID());
     ret.push_back(new_line);
     debug.step1_front_shift_line.push_back(new_line);
   }
 
   helper_->alignShiftLinesOrder(ret, false);
 
@@ -810,7 +810,7 @@
     return shift_lines;
   }
 
   AvoidLineArray sl_array_trimmed = shift_lines;
 
   // sort shift points from front to back.
   helper_->alignShiftLinesOrder(sl_array_trimmed);
@@ -824,7 +824,7 @@
   // - Combine avoid points that have almost same gradient.
   // this is to remove the noise.
   {
     const auto THRESHOLD = parameters_->same_grad_filter_1_threshold;
     applySimilarGradFilter(sl_array_trimmed, THRESHOLD);
     debug.step3_grad_filtered_1st = sl_array_trimmed;
   }
@@ -832,7 +832,7 @@
   // - Quantize the shift length to reduce the shift point noise
   // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc.
   {
     const auto THRESHOLD = parameters_->quantize_filter_threshold;
     applyQuantizeProcess(sl_array_trimmed, THRESHOLD);
     debug.step3_quantize_filtered = sl_array_trimmed;
   }
@@ -846,22 +846,22 @@
 
   // - Combine avoid points that have almost same gradient (again)
   {
     const auto THRESHOLD = parameters_->same_grad_filter_2_threshold;
     applySimilarGradFilter(sl_array_trimmed, THRESHOLD);
     debug.step3_grad_filtered_2nd = sl_array_trimmed;
   }
 
   // - Combine avoid points that have almost same gradient (again)
   {
     const auto THRESHOLD = parameters_->same_grad_filter_3_threshold;
     applySimilarGradFilter(sl_array_trimmed, THRESHOLD);
     debug.step3_grad_filtered_3rd = sl_array_trimmed;
   }
 
   return sl_array_trimmed;
 }
 
 void ShiftLineGenerator::applyQuantizeProcess(
   AvoidLineArray & shift_lines, const double threshold) const
 {
   if (threshold < 1.0e-5) {
@@ -869,43 +869,43 @@
   }
 
   for (auto & sl : shift_lines) {
     sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold;
   }
 
   helper_->alignShiftLinesOrder(shift_lines);
 }
 
 void ShiftLineGenerator::applySmallShiftFilter(
   AvoidLineArray & shift_lines, const double threshold) const
 {
   if (shift_lines.empty()) {
     return;
   }
 
   AvoidLineArray input = shift_lines;
   shift_lines.clear();
 
   for (const auto & s : input) {
     if (s.getRelativeLongitudinal() < threshold) {
       continue;
     }
 
     if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) {
       continue;
     }
 
     shift_lines.push_back(s);
   }
 }
 
 void ShiftLineGenerator::applySimilarGradFilter(
   AvoidLineArray & avoid_lines, const double threshold) const
 {
   if (avoid_lines.empty()) {
     return;
   }
 
   AvoidLineArray input = avoid_lines;
   avoid_lines.clear();
   avoid_lines.push_back(input.front());  // Take the first one anyway (think later)
 
@@ -914,12 +914,12 @@
   AvoidLineArray combine_buffer;
   combine_buffer.push_back(input.front());
 
   constexpr auto SHIFT_THR = 1e-3;
   const auto is_negative_shift = [&](const auto & s) {
     return s.getRelativeLength() < -1.0 * SHIFT_THR;
   };
 
   const auto is_positive_shift = [&](const auto & s) { return s.getRelativeLength() > SHIFT_THR; };
 
   for (size_t i = 1; i < input.size(); ++i) {
     AvoidLine combine{};
@@ -927,13 +927,13 @@
     utils::avoidance::setStartData(
       combine, base_line.start_shift_length, base_line.start, base_line.start_idx,
       base_line.start_longitudinal);
     utils::avoidance::setEndData(
       combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx,
       input.at(i).end_longitudinal);
 
     combine_buffer.push_back(input.at(i));
 
     const auto violates = [&]() {
       if (is_negative_shift(input.at(i)) && is_positive_shift(base_line)) {
         return true;
       }
@@ -942,9 +942,9 @@
         return true;
       }
 
       const auto lon_combine = combine.getRelativeLongitudinal();
       const auto base_length = base_line.getGradient() * lon_combine;
       return std::abs(combine.getRelativeLength() - base_length) > threshold;
     }();
 
     if (violates) {
@@ -955,10 +955,10 @@
     } else {
       avoid_lines.back() = combine;
     }
   }
 
   helper_->alignShiftLinesOrder(avoid_lines);
 }
 
 AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, DebugData & debug) const
@@ -966,25 +966,27 @@
   AvoidLineArray ret = shift_lines;
 
   constexpr double ep = 1.0e-3;
+  constexpr double RETURN_SHIFT_THRESHOLD = 0.1;
   const bool has_candidate_point = !ret.empty();
   const bool has_registered_point = last_.has_value();
 
   const auto exist_unavoidable_object = std::any_of(
     data.target_objects.begin(), data.target_objects.end(),
     [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; });
 
   if (exist_unavoidable_object) {
     return ret;
   }
 
-  // If the return-to-center shift points are already registered, do nothing.
-  if (!has_registered_point && std::fabs(base_offset_) < ep) {
-    return ret;
-  }
-
-  constexpr double RETURN_SHIFT_THRESHOLD = 0.1;
-  if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) {
-    return ret;
+  if (last_.has_value()) {
+    if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) {
+      return ret;
+    }
+  } else {
+    // If the return-to-center shift points are already registered, do nothing.
+    if (std::abs(base_offset_) < ep) {
+      return ret;
+    }
   }
 
   // From here, the return-to-center is not registered. But perhaps the candidate is
@@ -997,7 +999,7 @@
     // avoidance points: Yes, shift points: No -> select last avoidance point.
     if (has_candidate_point && !has_registered_point) {
       helper_->alignShiftLinesOrder(ret, false);
       last_sl = ret.back();
     }
 
     // avoidance points: No, shift points: Yes -> select last shift point.
@@ -1011,14 +1013,14 @@
       const auto & al = ret.back();
       const auto & sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()});
       last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al;
     }
 
     // avoidance points: No, shift points: No -> set the ego position to the last shift point
     // so that the return-shift will be generated from ego position.
     if (!has_candidate_point && !has_registered_point) {
       last_sl.end_idx = data.ego_closest_path_index;
       last_sl.end = data.reference_path.points.at(last_sl.end_idx).point.pose;
       last_sl.end_shift_length = base_offset_;
     }
   }
 
@@ -1052,13 +1054,13 @@
 
     // set the return-shift from ego.
     last_sl.end_idx = data.ego_closest_path_index;
     last_sl.end = data.reference_path.points.at(last_sl.end_idx).point.pose;
     last_sl.end_shift_length = current_base_shift;
   }
 
   const auto & arclength_from_ego = data.arclength_from_ego;
 
   const auto nominal_prepare_distance = helper_->getNominalPrepareDistance();
   const auto nominal_avoid_distance = helper_->getMaxAvoidanceDistance(last_sl.end_shift_length);
 
   if (arclength_from_ego.empty()) {
@@ -1069,7 +1071,7 @@
     arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point);
 
   // If the avoidance point has already been set, the return shift must be set after the point.
   const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx);
 
   // check if there is enough distance for return.
   if (last_sl_distance > remaining_distance) {  // tmp: add some small number (+1.0)
@@ -1096,31 +1098,31 @@
   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 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;
     avoid_distance_scaled *= scale;
   }
 
   // shift point for prepare distance: from last shift to return-start point.
   if (nominal_prepare_distance > last_sl_distance) {
     AvoidLine al;
     al.id = generateUUID();
     al.start_idx = last_sl.end_idx;
     al.start = last_sl.end;
     al.start_longitudinal = arclength_from_ego.at(al.start_idx);
     al.end_idx =
       utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled);
     al.end = data.reference_path.points.at(al.end_idx).point.pose;
     al.end_longitudinal = arclength_from_ego.at(al.end_idx);
     al.end_shift_length = last_sl.end_shift_length;
     al.start_shift_length = last_sl.end_shift_length;
     ret.push_back(al);
     debug.step1_return_shift_line.push_back(al);
   }
 
   // shift point for return to center line
   {
@@ -1129,19 +1131,19 @@
     al.start_idx =
       utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled);
     al.start = data.reference_path.points.at(al.start_idx).point.pose;
     al.start_longitudinal = arclength_from_ego.at(al.start_idx);
     al.end_idx = utils::avoidance::findPathIndexFromArclength(
       arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled);
     al.end = data.reference_path.points.at(al.end_idx).point.pose;
     al.end_longitudinal = arclength_from_ego.at(al.end_idx);
     al.end_shift_length = 0.0;
     al.start_shift_length = last_sl.end_shift_length;
     ret.push_back(al);
     debug.step1_return_shift_line.push_back(al);
   }
 
   return ret;
 }
 
 AvoidLineArray ShiftLineGenerator::findNewShiftLine(
   const AvoidLineArray & shift_lines, DebugData & debug) const
@@ -1152,9 +1154,9 @@
 
   // add small shift lines.
   const auto add_straight_shift =
     [&, this](auto & subsequent, bool has_large_shift, const size_t start_idx) {
       for (size_t i = start_idx; i < shift_lines.size(); ++i) {
         if (
           std::abs(shift_lines.at(i).getRelativeLength()) >
           parameters_->lateral_small_shift_threshold) {
           if (has_large_shift) {
@@ -1168,12 +1170,12 @@
           return;
         }
 
         subsequent.push_back(shift_lines.at(i));
       }
     };
 
   // get subsequent shift lines.
   const auto get_subsequent_shift = [&, this](size_t i) {
     AvoidLineArray subsequent{shift_lines.at(i)};
 
     if (!helper_->isComfortable(subsequent)) {
@@ -1188,11 +1190,11 @@
       return subsequent;
     }
 
     if (
       std::abs(shift_lines.at(i).getRelativeLength()) <
       parameters_->lateral_small_shift_threshold) {
       const auto has_large_shift =
         shift_lines.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold;
 
       // candidate.at(i) is small length shift line. add large length shift line.
       subsequent.push_back(shift_lines.at(i + 1));
@@ -1203,12 +1205,12 @@
     }
 
     return subsequent;
   };
 
   // check ignore or not.
   const auto is_ignore_shift = [this](const auto & s) {
     return std::abs(helper_->getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold;
   };
 
   for (size_t i = 0; i < shift_lines.size(); ++i) {
     const auto & candidate = shift_lines.at(i);
@@ -1219,20 +1221,20 @@
     }
 
     if (is_ignore_shift(candidate)) {
       continue;
     }
 
     if (perManeuver(parameters_->policy_approval)) {
       debug.step4_new_shift_line = shift_lines;
       return shift_lines;
     }
 
     const auto new_shift_lines = get_subsequent_shift(i);
     debug.step4_new_shift_line = new_shift_lines;
     return new_shift_lines;
   }
 
   return {};
 }
 
 void ShiftLineGenerator::updateRegisteredRawShiftLines(const AvoidancePlanningData & data)
@@ -1241,20 +1243,20 @@
 
   AvoidLineArray avoid_lines;
 
   const auto has_large_offset = [this](const auto & s) {
     constexpr double THRESHOLD = 0.1;
     const auto ego_shift_length = helper_->getEgoLinearShift();
 
     const auto start_to_ego_longitudinal = -1.0 * s.start_longitudinal;
 
     if (start_to_ego_longitudinal < 0.0) {
       return false;
     }
 
     const auto reg_shift_length =
       s.getGradient() * start_to_ego_longitudinal + s.start_shift_length;
 
     return std::abs(ego_shift_length - reg_shift_length) > THRESHOLD;
   };
 
   const auto ego_idx = data.ego_closest_path_index;
@@ -1262,12 +1264,12 @@
   for (const auto & s : raw_registered_) {
     // invalid
     if (s.end_idx < ego_idx) {
       continue;
     }
 
     // invalid
     if (has_large_offset(s)) {
       continue;
     }
 
     // valid
@@ -1277,41 +1279,41 @@
   raw_registered_ = avoid_lines;
 }
 
 void ShiftLineGenerator::setRawRegisteredShiftLine(
   const AvoidLineArray & shift_lines, const AvoidancePlanningData & data)
 {
   if (shift_lines.empty()) {
     RCLCPP_ERROR(rclcpp::get_logger(""), "future is empty! return.");
     return;
   }
 
   auto future_with_info = shift_lines;
   utils::avoidance::fillAdditionalInfoFromPoint(data, future_with_info);
 
   // sort by longitudinal
   std::sort(future_with_info.begin(), future_with_info.end(), [](auto a, auto b) {
     return a.end_longitudinal < b.end_longitudinal;
   });
 
   // calc relative lateral length
   future_with_info.front().start_shift_length = base_offset_;
   for (size_t i = 1; i < future_with_info.size(); ++i) {
     future_with_info.at(i).start_shift_length = future_with_info.at(i - 1).end_shift_length;
   }
 
   const auto is_registered = [this](const auto id) {
     const auto & r = raw_registered_;
     return std::any_of(r.begin(), r.end(), [id](const auto & s) { return s.id == id; });
   };
 
   const auto same_id_shift_line = [this](const auto id) {
     const auto & r = raw_;
     const auto itr = std::find_if(r.begin(), r.end(), [id](const auto & s) { return s.id == id; });
     if (itr != r.end()) {
       return *itr;
     }
     throw std::logic_error("not found same id current raw shift line.");
   };
 
   for (const auto & s : future_with_info) {
     if (s.parent_ids.empty()) {
@@ -1320,11 +1322,11 @@
 
     for (const auto id : s.parent_ids) {
       if (is_registered(id)) {
         continue;
       }
 
       raw_registered_.push_back(same_id_shift_line(id));
     }
   }
 }
 }  // namespace behavior_path_planner::utils::avoidance