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 22db71ba7be63..8465dda939294 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 @@ -1249,7 +1249,7 @@ bool NormalLaneChange::get_path_using_path_shifter( PathWithLaneId prepare_segment; try { if (!utils::lane_change::get_prepare_segment( - common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { + common_data_ptr_, prev_module_output_.path, prep_metric.length, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1268,6 +1268,11 @@ bool NormalLaneChange::get_path_using_path_shifter( 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(