Skip to content

Commit 7eabe12

Browse files
authored
feat(lane_change): improve the calculation of the target lateral velocity in Frenet frame (autowarefoundation#10078)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 1d2a0c7 commit 7eabe12

File tree

1 file changed

+9
-14
lines changed
  • planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils

1 file changed

+9
-14
lines changed

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

+9-14
Original file line numberDiff line numberDiff line change
@@ -195,7 +195,7 @@ FrenetState init_frenet_state(
195195

196196
std::optional<SamplingParameters> init_sampling_parameters(
197197
const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics,
198-
const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose)
198+
const FrenetState & initial_state, const Spline2D & ref_spline)
199199
{
200200
const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory;
201201
const auto min_lc_vel = trajectory.min_lane_changing_velocity;
@@ -206,25 +206,20 @@ std::optional<SamplingParameters> init_sampling_parameters(
206206
const auto final_velocity =
207207
std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration);
208208
const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5;
209-
const auto target_s = initial_state.position.s + lc_length;
210-
const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation);
211-
const auto target_lat_vel =
212-
(1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) *
213-
std::tan(initial_yaw - ref_spline.yaw(target_s));
214-
215-
if (std::isnan(target_lat_vel)) {
216-
return std::nullopt;
217-
}
209+
const auto target_s = std::min(ref_spline.lastS(), initial_state.position.s + lc_length);
210+
// for smooth lateral motion we want a constant lateral acceleration profile
211+
// this means starting from a 0 lateral velocity and setting a positive target lateral velocity
212+
const auto max_lat_vel = duration * max_lateral_acc;
213+
const auto target_lat_vel = std::min(max_lat_vel, initial_state.position.d / duration);
218214

219215
SamplingParameters sampling_parameters;
220216
const auto & safety = common_data_ptr->lc_param_ptr->safety;
221217
sampling_parameters.resolution = safety.collision_check.prediction_time_resolution;
222218
sampling_parameters.parameters.emplace_back();
223219
sampling_parameters.parameters.back().target_duration = duration;
224220
sampling_parameters.parameters.back().target_state.position = {target_s, 0.0};
225-
// TODO(Maxime): not sure if we should use curvature at initial or target s
226221
sampling_parameters.parameters.back().target_state.lateral_velocity =
227-
sign<double>(common_data_ptr->direction) * target_lat_vel;
222+
std::copysign(target_lat_vel, sign<double>(common_data_ptr->direction));
228223
sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0;
229224
sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity;
230225
sampling_parameters.parameters.back().target_state.longitudinal_acceleration =
@@ -571,8 +566,8 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
571566
initial_state.lateral_velocity, initial_state.longitudinal_acceleration,
572567
initial_state.lateral_acceleration);
573568

574-
const auto sampling_parameters_opt = init_sampling_parameters(
575-
common_data_ptr, metric, initial_state, reference_spline, lc_start_pose);
569+
const auto sampling_parameters_opt =
570+
init_sampling_parameters(common_data_ptr, metric, initial_state, reference_spline);
576571

577572
if (!sampling_parameters_opt) {
578573
continue;

0 commit comments

Comments
 (0)