@@ -195,7 +195,7 @@ FrenetState init_frenet_state(
195
195
196
196
std::optional<SamplingParameters> init_sampling_parameters (
197
197
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)
199
199
{
200
200
const auto & trajectory = common_data_ptr->lc_param_ptr ->trajectory ;
201
201
const auto min_lc_vel = trajectory.min_lane_changing_velocity ;
@@ -206,25 +206,20 @@ std::optional<SamplingParameters> init_sampling_parameters(
206
206
const auto final_velocity =
207
207
std::max (min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration);
208
208
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);
218
214
219
215
SamplingParameters sampling_parameters;
220
216
const auto & safety = common_data_ptr->lc_param_ptr ->safety ;
221
217
sampling_parameters.resolution = safety.collision_check .prediction_time_resolution ;
222
218
sampling_parameters.parameters .emplace_back ();
223
219
sampling_parameters.parameters .back ().target_duration = duration;
224
220
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
226
221
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 )) ;
228
223
sampling_parameters.parameters .back ().target_state .lateral_acceleration = 0.0 ;
229
224
sampling_parameters.parameters .back ().target_state .longitudinal_velocity = final_velocity;
230
225
sampling_parameters.parameters .back ().target_state .longitudinal_acceleration =
@@ -571,8 +566,8 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
571
566
initial_state.lateral_velocity , initial_state.longitudinal_acceleration ,
572
567
initial_state.lateral_acceleration );
573
568
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);
576
571
577
572
if (!sampling_parameters_opt) {
578
573
continue ;
0 commit comments