diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc index df9133cd197903c51b9c1670d38c4a9ee271f189..e8bf8d9e49637d3465d8c841354422996b32f471 100644 --- a/modules/planning/common/planning_gflags.cc +++ b/modules/planning/common/planning_gflags.cc @@ -409,6 +409,9 @@ DEFINE_double(weight_opposite_side_offset, 10.0, DEFINE_double(weight_dist_travelled, 10.0, "Weight of travelled distance cost"); DEFINE_double(weight_target_speed, 1.0, "Weight of target speed cost"); DEFINE_double(lat_offset_bound, 3.0, "The bound of lateral offset"); -DEFINE_double(lon_collision_buffer, 1.0, "Longitudinal collision buffer"); +DEFINE_double(lon_collision_yield_buffer, 1.0, + "Longitudinal collision buffer for yield"); +DEFINE_double(lon_collision_overtake_buffer, 5.0, + "Longitudinal collision buffer for overtake"); DEFINE_double(lon_collision_cost_std, 0.5, "The standard deviation of logitudinal collision cost function"); diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h index 3a93827da9baec267f529bfbcf00e4fbdb9ac4a3..56524c9265e880ae8456b927ffd1009e112a1383 100644 --- a/modules/planning/common/planning_gflags.h +++ b/modules/planning/common/planning_gflags.h @@ -217,7 +217,8 @@ DECLARE_double(weight_opposite_side_offset); DECLARE_double(weight_dist_travelled); DECLARE_double(weight_target_speed); DECLARE_double(lat_offset_bound); -DECLARE_double(lon_collision_buffer); +DECLARE_double(lon_collision_yield_buffer); +DECLARE_double(lon_collision_overtake_buffer); DECLARE_double(lon_collision_cost_std); #endif // MODULES_PLANNING_COMMON_PLANNING_GFLAGS_H diff --git a/modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc b/modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc index de072c299854d98ea8846854fc4b3a261560fdf4..06db470e803d4e3379bc2281dc17d52b35d39c0f 100644 --- a/modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc +++ b/modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc @@ -233,14 +233,14 @@ double TrajectoryEvaluator::LonCollisionCost( double sigma = FLAGS_lon_collision_cost_std; for (const auto& m : pt_interval) { double cost = 0.0; - if (traj_s > m.first - FLAGS_lon_collision_buffer && - traj_s < m.second + FLAGS_lon_collision_buffer) { + if (traj_s > m.first - FLAGS_lon_collision_yield_buffer && + traj_s < m.second + FLAGS_lon_collision_overtake_buffer) { cost = 1.0; } else if (traj_s < m.first) { - double dist = traj_s - m.first + FLAGS_lon_collision_buffer; + double dist = (m.first - FLAGS_lon_collision_yield_buffer) - traj_s; cost = std::exp(-dist * dist / (2.0 * sigma * sigma)); } else if (traj_s > m.second) { - double dist = m.second + FLAGS_lon_collision_buffer - traj_s; + double dist = traj_s - (m.second + FLAGS_lon_collision_overtake_buffer); cost = std::exp(-dist * dist / (2.0 * sigma * sigma)); } cost_sqr_sum += cost * cost;