From 5d58f1c1c09488e318648c4b1b308b3c47ca2a98 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Date: Thu, 4 Apr 2024 22:52:42 +0900
Subject: [PATCH] fix(lane_change): limit prepare and lane changing length
 (#6734)

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../utils/utils.hpp                              | 16 ++++++++++++++++
 .../src/scene.cpp                                | 11 +++++------
 .../src/utils/utils.cpp                          | 10 ++++++++++
 3 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
index 67506326d92aa..7b7e424c36d71 100644
--- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
+++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
@@ -220,5 +220,21 @@ lanelet::ConstLanelets generateExpandedLanelets(
  * @return rclcpp::Logger The logger instance configured for the specified lane change type.
  */
 rclcpp::Logger getLogger(const std::string & type);
+/**
+ * @brief Calculates the distance required during a lane change operation.
+ *
+ * Used for computing prepare or lane change length based on current and maximum velocity,
+ * acceleration, and duration, returning the lesser of accelerated distance or distance at max
+ * velocity.
+ *
+ * @param velocity The current velocity of the vehicle in meters per second (m/s).
+ * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s).
+ * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2).
+ * @param duration The duration of the lane change in seconds (s).
+ * @return The calculated minimum distance in meters (m).
+ */
+double calcPhaseLength(
+  const double velocity, const double maximum_velocity, const double acceleration,
+  const double time);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index ce44ee27b6638..2d88003f2b0ee 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -1180,9 +1180,8 @@ bool NormalLaneChange::getLaneChangePaths(
         (prepare_duration < 1e-3) ? 0.0
                                   : ((prepare_velocity - current_velocity) / prepare_duration);
 
-      const double prepare_length =
-        current_velocity * prepare_duration +
-        0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2);
+      const auto prepare_length = utils::lane_change::calcPhaseLength(
+        current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration);
 
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
@@ -1234,9 +1233,9 @@ bool NormalLaneChange::getLaneChangePaths(
           utils::lane_change::calcLaneChangingAcceleration(
             initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
             sampled_longitudinal_acc);
-        const auto lane_changing_length =
-          initial_lane_changing_velocity * lane_changing_time +
-          0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
+        const auto lane_changing_length = utils::lane_change::calcPhaseLength(
+          initial_lane_changing_velocity, getCommonParam().max_vel,
+          longitudinal_acc_on_lane_changing, lane_changing_time);
         const auto terminal_lane_changing_velocity = std::min(
           initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
           getCommonParam().max_vel);
diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
index df73990b770a3..3a9d20e1adf03 100644
--- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -1168,4 +1168,14 @@ rclcpp::Logger getLogger(const std::string & type)
 {
   return rclcpp::get_logger("lane_change").get_child(type);
 }
+
+double calcPhaseLength(
+  const double velocity, const double maximum_velocity, const double acceleration,
+  const double duration)
+{
+  const auto length_with_acceleration =
+    velocity * duration + 0.5 * acceleration * std::pow(duration, 2);
+  const auto length_with_max_velocity = maximum_velocity * duration;
+  return std::min(length_with_acceleration, length_with_max_velocity);
+}
 }  // namespace behavior_path_planner::utils::lane_change