From aae0b0a93d1a0ad7785d05956cb5670c64c6ab08 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Tue, 1 Oct 2024 18:34:30 +0900
Subject: [PATCH] Revert "remove calc maximum lane change length"

This reverts commit e9cc386e1c21321c59f518d2acbe78a3c668471f.
---
 .../utils/calculation.hpp                     |  8 +++
 .../src/utils/calculation.cpp                 | 52 +++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
index 25244e1d30448..021fa16fa6ec1 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
@@ -103,6 +103,14 @@ double calc_ego_dist_to_lanes_start(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
 
+double calc_maximum_lane_change_length(
+  const double current_velocity, const LaneChangeParameters & lane_change_parameters,
+  const std::vector<double> & shift_intervals, const double max_acc);
+
+double calc_maximum_lane_change_length(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
+  const double max_acc);
+
 double calc_lane_changing_acceleration(
   const double initial_lane_changing_velocity, const double max_path_velocity,
   const double lane_changing_time, const double prepare_longitudinal_acc);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
index c4e2e95e812f6..1c4aede51074a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
@@ -140,6 +140,58 @@ double calc_ego_dist_to_lanes_start(
   return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt);
 }
 
+double calc_maximum_lane_change_length(
+  const double current_velocity, const LaneChangeParameters & lane_change_parameters,
+  const std::vector<double> & shift_intervals, const double max_acc)
+{
+  if (shift_intervals.empty()) {
+    return 0.0;
+  }
+
+  const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;
+  const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk;
+  const auto t_prepare = lane_change_parameters.lane_change_prepare_duration;
+
+  auto vel = current_velocity;
+
+  const auto calc_sum = [&](double sum, double shift_interval) {
+    // prepare section
+    const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare;
+    vel = vel + max_acc * t_prepare;
+
+    // lane changing section
+    const auto [min_lat_acc, max_lat_acc] =
+      lane_change_parameters.lane_change_lat_acc_map.find(vel);
+    const auto t_lane_changing =
+      PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc);
+    const auto lane_changing_length =
+      vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing;
+
+    vel = vel + max_acc * t_lane_changing;
+    return sum + (prepare_length + lane_changing_length + finish_judge_buffer);
+  };
+
+  const auto total_length =
+    std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum);
+
+  const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane;
+  return total_length + backward_buffer * (static_cast<double>(shift_intervals.size()) - 1.0);
+}
+
+double calc_maximum_lane_change_length(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
+  const double max_acc)
+{
+  const auto shift_intervals =
+    common_data_ptr->route_handler_ptr->getLateralIntervalsToPreferredLane(
+      current_terminal_lanelet);
+  const auto vel = std::max(
+    common_data_ptr->get_ego_speed(),
+    common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity);
+  return calc_maximum_lane_change_length(
+    vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc);
+}
+
 std::vector<double> calc_all_min_lc_lengths(
   const LCParamPtr & lc_param_ptr, const std::vector<double> & shift_intervals)
 {