fix for failing scenario caused by prepare velocity

Signed-off-by: Zulfaqar Azmi <[email protected]>

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);

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(

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;
 }
 

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)