diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index bd767191884ad..5c8522b9e31ba 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1184,9 +1184,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); @@ -1238,9 +1237,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);