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 df45da3f08fa1..69a9bb26a6619 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 @@ -195,7 +195,7 @@ FrenetState init_frenet_state( std::optional init_sampling_parameters( const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics, - const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose) + const FrenetState & initial_state, const Spline2D & ref_spline) { const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory; const auto min_lc_vel = trajectory.min_lane_changing_velocity; @@ -206,15 +206,11 @@ std::optional init_sampling_parameters( const auto final_velocity = std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration); const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5; - const auto target_s = initial_state.position.s + lc_length; - const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation); - const auto target_lat_vel = - (1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) * - std::tan(initial_yaw - ref_spline.yaw(target_s)); - - if (std::isnan(target_lat_vel)) { - return std::nullopt; - } + const auto target_s = std::min(ref_spline.lastS(), initial_state.position.s + lc_length); + // for smooth lateral motion we want a constant lateral acceleration profile + // this means starting from a 0 lateral velocity and setting a positive target lateral velocity + const auto max_lat_vel = duration * max_lateral_acc; + const auto target_lat_vel = std::min(max_lat_vel, initial_state.position.d / duration); SamplingParameters sampling_parameters; const auto & safety = common_data_ptr->lc_param_ptr->safety; @@ -222,9 +218,8 @@ std::optional init_sampling_parameters( sampling_parameters.parameters.emplace_back(); sampling_parameters.parameters.back().target_duration = duration; sampling_parameters.parameters.back().target_state.position = {target_s, 0.0}; - // TODO(Maxime): not sure if we should use curvature at initial or target s sampling_parameters.parameters.back().target_state.lateral_velocity = - sign(common_data_ptr->direction) * target_lat_vel; + std::copysign(target_lat_vel, sign(common_data_ptr->direction)); sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0; sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity; sampling_parameters.parameters.back().target_state.longitudinal_acceleration = @@ -571,8 +566,8 @@ std::vector generate_frenet_candidates( initial_state.lateral_velocity, initial_state.longitudinal_acceleration, initial_state.lateral_acceleration); - const auto sampling_parameters_opt = init_sampling_parameters( - common_data_ptr, metric, initial_state, reference_spline, lc_start_pose); + const auto sampling_parameters_opt = + init_sampling_parameters(common_data_ptr, metric, initial_state, reference_spline); if (!sampling_parameters_opt) { continue;