From 30d68eb14b961fd1737714e608f91cdc7fd82fb8 Mon Sep 17 00:00:00 2001
From: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Mon, 23 Dec 2024 17:34:07 +0900
Subject: [PATCH] fix for failing scenario caused by prepare velocity

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../utils/utils.hpp                             |  3 +++
 .../src/scene.cpp                               |  5 +++++
 .../src/utils/path.cpp                          | 13 -------------
 .../src/utils/utils.cpp                         | 17 +++++++++++++++++
 4 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
index 4114ae2a5ae46..69362994dbbac 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -66,6 +66,9 @@ rclcpp::Logger get_logger();
 
 bool is_mandatory_lane_change(const ModuleType lc_type);
 
+void set_prepare_velocity(
+  PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity);
+
 std::vector<int64_t> replaceWithSortedIds(
   const std::vector<int64_t> & original_lane_ids,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
index d6b2506f38c86..f8a200a9685a7 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -1145,6 +1145,11 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths)
     const auto lane_changing_metrics = get_lane_changing_metrics(
       prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element);
 
+    // set_prepare_velocity must only be called after computing lane change metrics, as lane change
+    // metrics rely on the prepare segment's original velocity as max_path_velocity.
+    utils::lane_change::set_prepare_velocity(
+      prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity);
+
     for (const auto & lc_metric : lane_changing_metrics) {
       const auto debug_print_lat = [&](const std::string & s) {
         RCLCPP_DEBUG(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp
index da6522c05fff2..e46fb74856fec 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp
@@ -187,19 +187,6 @@ bool get_prepare_segment(
     throw std::logic_error("lane change start is behind target lanelet!");
   }
 
-  if (common_data_ptr->get_ego_speed() >= prep_metric.velocity) {
-    // deceleration
-    prepare_segment.points.back().point.longitudinal_velocity_mps = std::min(
-      prepare_segment.points.back().point.longitudinal_velocity_mps,
-      static_cast<float>(prep_metric.velocity));
-  } else {
-    // acceleration
-    for (auto & point : prepare_segment.points) {
-      point.point.longitudinal_velocity_mps =
-        std::min(point.point.longitudinal_velocity_mps, static_cast<float>(prep_metric.velocity));
-    }
-  }
-
   return true;
 }
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
index 77cdf01ec5165..147ef3856ac4e 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -85,6 +85,23 @@ bool is_mandatory_lane_change(const ModuleType lc_type)
          lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE;
 }
 
+void set_prepare_velocity(
+  PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity)
+{
+  if (current_velocity >= prepare_velocity) {
+    // deceleration
+    prepare_segment.points.back().point.longitudinal_velocity_mps = std::min(
+      prepare_segment.points.back().point.longitudinal_velocity_mps,
+      static_cast<float>(prepare_velocity));
+    return;
+  }
+  // acceleration
+  for (auto & point : prepare_segment.points) {
+    point.point.longitudinal_velocity_mps =
+      std::min(point.point.longitudinal_velocity_mps, static_cast<float>(prepare_velocity));
+  }
+}
+
 lanelet::ConstLanelets get_target_neighbor_lanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const LaneChangeModuleType & type)