提交 c4ff1f45 编写于 作者: Y Yajia Zhang 提交者: Calvin Miao

planning: deprecate some gflags

上级 81ed6350
......@@ -73,8 +73,6 @@ DEFINE_bool(enable_scenario_bare_intersection, false,
DEFINE_bool(enable_scenario_pull_over, false,
"enable side pass scenario in planning");
DEFINE_bool(enable_scenario_side_pass, true,
"enable side pass scenario in planning");
DEFINE_double(side_pass_min_signal_intersection_distance, 50.0,
"meter, for intersection has signal, ADC will enter side pass "
"scenario only when it is farther than this threshoold");
......@@ -331,14 +329,11 @@ DEFINE_uint32(max_planning_thread_pool_size, 15,
"num of thread used in planning thread pool.");
DEFINE_bool(use_multi_thread_to_add_obstacles, false,
"use multiple thread to add obstacles.");
DEFINE_bool(
enable_multi_thread_in_dp_poly_path, false,
"Enable multiple thread to calculation curve cost in dp_poly_path.");
DEFINE_bool(enable_multi_thread_in_dp_st_graph, false,
"Enable multiple thread to calculation curve cost in dp_st_graph.");
/// Lattice Planner
DEFINE_double(lattice_epsilon, 1e-6, "Epsilon in lattice planner.");
DEFINE_double(numerical_epsilon, 1e-6, "Epsilon in lattice planner.");
DEFINE_double(default_cruise_speed, 5.0, "default cruise speed");
DEFINE_bool(enable_auto_tuning, false, "enable auto tuning data emission");
DEFINE_double(trajectory_time_resolution, 0.1,
......@@ -502,8 +497,6 @@ DEFINE_bool(
enable_parallel_trajectory_smoothing, false,
"Whether to partition the trajectory first and do smoothing in parallel");
DEFINE_bool(use_osqp_optimizer_for_qp_st, false,
"Use OSQP optimizer for QpSt speed optimization.");
DEFINE_bool(use_osqp_optimizer_for_reference_line, true,
"Use OSQP optimizer for reference line optimization.");
DEFINE_bool(enable_osqp_debug, false,
......
......@@ -38,7 +38,6 @@ DECLARE_string(scenario_valet_parking_config_file);
DECLARE_bool(enable_scenario_bare_intersection);
DECLARE_bool(enable_scenario_pull_over);
DECLARE_bool(enable_scenario_side_pass);
DECLARE_double(side_pass_min_signal_intersection_distance);
DECLARE_bool(enable_scenario_side_pass_multiple_parked_obstacles);
DECLARE_bool(enable_scenario_stop_sign);
......@@ -162,11 +161,9 @@ DECLARE_bool(enable_sqp_solver);
/// thread pool
DECLARE_uint32(max_planning_thread_pool_size);
DECLARE_bool(use_multi_thread_to_add_obstacles);
DECLARE_bool(enable_multi_thread_in_dp_poly_path);
DECLARE_bool(enable_multi_thread_in_dp_st_graph);
// lattice planner
DECLARE_double(lattice_epsilon);
DECLARE_double(numerical_epsilon);
DECLARE_double(default_cruise_speed);
DECLARE_bool(enable_auto_tuning);
......@@ -248,7 +245,6 @@ DECLARE_double(destination_to_adc_buffer);
DECLARE_double(destination_to_pathend_buffer);
DECLARE_double(pull_over_road_edge_buffer);
DECLARE_bool(use_osqp_optimizer_for_qp_st);
DECLARE_bool(use_osqp_optimizer_for_reference_line);
DECLARE_bool(enable_osqp_debug);
DECLARE_bool(export_chart);
......
......@@ -31,7 +31,7 @@ namespace planning {
ConstantDecelerationTrajectory1d::ConstantDecelerationTrajectory1d(
const double init_s, const double init_v, const double a)
: init_s_(init_s), init_v_(init_v), deceleration_(-a) {
if (init_v_ < -FLAGS_lattice_epsilon) {
if (init_v_ < -FLAGS_numerical_epsilon) {
AERROR << "negative init v = " << init_v_;
}
init_v_ = std::fabs(init_v_);
......
......@@ -32,7 +32,7 @@ ConstantJerkTrajectory1d::ConstantJerkTrajectory1d(const double p0,
const double j,
const double param)
: p0_(p0), v0_(v0), a0_(a0), param_(param), jerk_(j) {
CHECK_GT(param, FLAGS_lattice_epsilon);
CHECK_GT(param, FLAGS_numerical_epsilon);
p1_ = Evaluate(0, param_);
v1_ = Evaluate(1, param_);
a1_ = Evaluate(2, param_);
......@@ -40,7 +40,7 @@ ConstantJerkTrajectory1d::ConstantJerkTrajectory1d(const double p0,
double ConstantJerkTrajectory1d::Evaluate(const std::uint32_t order,
const double param) const {
CHECK_GT(param, -FLAGS_lattice_epsilon);
CHECK_GT(param, -FLAGS_numerical_epsilon);
switch (order) {
case 0: {
return p0_ + v0_ * param + 0.5 * a0_ * param * param +
......
......@@ -45,13 +45,13 @@ void PiecewiseAccelerationTrajectory1d::AppendSegment(const double a,
double t0 = t_.back();
double v1 = v0 + a * t_duration;
CHECK(v1 >= -FLAGS_lattice_epsilon);
CHECK(v1 >= -FLAGS_numerical_epsilon);
double delta_s = (v0 + v1) * t_duration * 0.5;
double s1 = s0 + delta_s;
double t1 = t0 + t_duration;
CHECK(s1 >= s0 - FLAGS_lattice_epsilon);
CHECK(s1 >= s0 - FLAGS_numerical_epsilon);
s1 = std::max(s1, s0);
s_.push_back(s1);
v_.push_back(v1);
......
......@@ -38,7 +38,7 @@ PiecewiseJerkTrajectory1d::PiecewiseJerkTrajectory1d(const double p,
void PiecewiseJerkTrajectory1d::AppendSegment(const double jerk,
const double param) {
CHECK_GT(param, FLAGS_lattice_epsilon);
CHECK_GT(param, FLAGS_numerical_epsilon);
param_.push_back(param_.back() + param);
......@@ -53,7 +53,7 @@ void PiecewiseJerkTrajectory1d::AppendSegment(const double jerk,
double PiecewiseJerkTrajectory1d::Evaluate(const std::uint32_t order,
const double param) const {
CHECK_GE(param, -FLAGS_lattice_epsilon);
CHECK_GE(param, -FLAGS_numerical_epsilon);
auto it_lower = std::lower_bound(param_.begin(), param_.end(), param);
......
......@@ -8,11 +8,8 @@
--enable_reference_line_stitching
--min_length_for_lane_change=5.0
--nouse_multi_thread_to_add_obstacles
--enable_multi_thread_in_dp_poly_path
--enable_multi_thread_in_dp_st_graph
--nouse_osqp_optimizer_for_qp_st
--use_osqp_optimizer_for_reference_line
--enable_scenario_side_pass
--enable_scenario_traffic_light
# --smoother_config_filename=/apollo/modules/planning/conf/spiral_smoother_config.pb.txt
......
......@@ -35,7 +35,6 @@ class GarageTest : public PlanningTestBase {
public:
virtual void SetUp() {
FLAGS_use_multi_thread_to_add_obstacles = false;
FLAGS_enable_multi_thread_in_dp_poly_path = false;
FLAGS_enable_multi_thread_in_dp_st_graph = false;
FLAGS_use_navigation_mode = false;
FLAGS_map_dir = "modules/planning/testdata/garage_map";
......@@ -50,7 +49,6 @@ class GarageTest : public PlanningTestBase {
FLAGS_enable_lag_prediction = false;
FLAGS_enable_rss_info = false;
FLAGS_enable_scenario_side_pass = false;
FLAGS_enable_scenario_stop_sign = false;
FLAGS_enable_scenario_traffic_light = false;
}
......
......@@ -51,7 +51,6 @@ DEFINE_string(test_previous_planning_file, "",
void PlanningTestBase::SetUpTestCase() {
FLAGS_use_multi_thread_to_add_obstacles = false;
FLAGS_enable_multi_thread_in_dp_poly_path = false;
FLAGS_enable_multi_thread_in_dp_st_graph = false;
FLAGS_planning_config_file =
"/apollo/modules/planning/conf/planning_config.pb.txt";
......@@ -73,7 +72,6 @@ void PlanningTestBase::SetUpTestCase() {
FLAGS_enable_trajectory_check = false;
FLAGS_planning_test_mode = true;
FLAGS_enable_lag_prediction = false;
FLAGS_use_osqp_optimizer_for_qp_st = false;
FLAGS_use_osqp_optimizer_for_reference_line = false;
}
......
......@@ -49,9 +49,7 @@ class SunnyvaleBigLoopTest : public PlanningTestBase {
FLAGS_test_base_map_filename = "base_map.bin";
FLAGS_test_data_dir = "modules/planning/testdata/sunnyvale_big_loop_test";
FLAGS_planning_upper_speed_limit = 12.5;
FLAGS_enable_multi_thread_in_dp_poly_path = false;
FLAGS_enable_scenario_side_pass = false;
FLAGS_enable_scenario_stop_sign = false;
FLAGS_enable_scenario_traffic_light = false;
FLAGS_enable_rss_info = false;
......
......@@ -39,7 +39,6 @@ class SunnyvaleLoopTest : public PlanningTestBase {
FLAGS_planning_upper_speed_limit = 12.5;
FLAGS_use_multi_thread_to_add_obstacles = false;
FLAGS_enable_scenario_side_pass = false;
FLAGS_enable_scenario_stop_sign = false;
FLAGS_enable_scenario_traffic_light = false;
FLAGS_enable_rss_info = false;
......
......@@ -287,7 +287,7 @@ std::vector<STPoint> PathTimeGraph::GetObstacleSurroundingPoints(
}
double time_gap = t1 - t0;
CHECK(time_gap > -FLAGS_lattice_epsilon);
CHECK(time_gap > -FLAGS_numerical_epsilon);
time_gap = std::fabs(time_gap);
size_t num_sections = static_cast<size_t>(time_gap / t_min_density + 1);
......@@ -314,7 +314,7 @@ bool PathTimeGraph::IsObstacleInGraph(const std::string& obstacle_id) {
std::vector<std::pair<double, double>> PathTimeGraph::GetLateralBounds(
const double s_start, const double s_end, const double s_resolution) {
CHECK_LT(s_start, s_end);
CHECK_GT(s_resolution, FLAGS_lattice_epsilon);
CHECK_GT(s_resolution, FLAGS_numerical_epsilon);
std::vector<std::pair<double, double>> bounds;
std::vector<double> discretized_path;
double s_range = s_end - s_start;
......@@ -369,15 +369,15 @@ void PathTimeGraph::UpdateLateralBoundsByObstacle(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
size_t start_index = start_iter - discretized_path.begin();
size_t end_index = end_iter - discretized_path.begin();
if (sl_boundary.end_l() > -FLAGS_lattice_epsilon &&
sl_boundary.start_l() < FLAGS_lattice_epsilon) {
if (sl_boundary.end_l() > -FLAGS_numerical_epsilon &&
sl_boundary.start_l() < FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < end_index; ++i) {
bounds->operator[](i).first = -FLAGS_lattice_epsilon;
bounds->operator[](i).second = FLAGS_lattice_epsilon;
bounds->operator[](i).first = -FLAGS_numerical_epsilon;
bounds->operator[](i).second = FLAGS_numerical_epsilon;
}
return;
}
if (sl_boundary.end_l() < FLAGS_lattice_epsilon) {
if (sl_boundary.end_l() < FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
bounds->operator[](i).first =
......@@ -386,7 +386,7 @@ void PathTimeGraph::UpdateLateralBoundsByObstacle(
}
return;
}
if (sl_boundary.start_l() > -FLAGS_lattice_epsilon) {
if (sl_boundary.start_l() > -FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
bounds->operator[](i).second =
......
......@@ -159,7 +159,7 @@ void EndConditionSampler::QueryFollowPathTimePoints(
std::vector<SamplePoint>* const sample_points) const {
std::vector<STPoint> follow_path_time_points =
ptr_path_time_graph_->GetObstacleSurroundingPoints(
obstacle_id, -FLAGS_lattice_epsilon, FLAGS_time_min_density);
obstacle_id, -FLAGS_numerical_epsilon, FLAGS_time_min_density);
for (const auto& path_time_point : follow_path_time_points) {
double v = ptr_prediction_querier_->ProjectVelocityAlongReferenceLine(
......@@ -188,7 +188,7 @@ void EndConditionSampler::QueryOvertakePathTimePoints(
std::vector<SamplePoint>* sample_points) const {
std::vector<STPoint> overtake_path_time_points =
ptr_path_time_graph_->GetObstacleSurroundingPoints(
obstacle_id, FLAGS_lattice_epsilon, FLAGS_time_min_density);
obstacle_id, FLAGS_numerical_epsilon, FLAGS_time_min_density);
for (const auto& path_time_point : overtake_path_time_points) {
double v = ptr_prediction_querier_->ProjectVelocityAlongReferenceLine(
......
......@@ -40,7 +40,7 @@ DiscretizedTrajectory TrajectoryCombiner::Combine(
double accumulated_trajectory_s = 0.0;
PathPoint prev_trajectory_point;
double last_s = -FLAGS_lattice_epsilon;
double last_s = -FLAGS_numerical_epsilon;
double t_param = 0.0;
while (t_param < FLAGS_trajectory_time_length) {
// linear extrapolation is handled internally in LatticeTrajectory1d;
......@@ -52,7 +52,7 @@ DiscretizedTrajectory TrajectoryCombiner::Combine(
last_s = s;
double s_dot =
std::max(FLAGS_lattice_epsilon, lon_trajectory.Evaluate(1, t_param));
std::max(FLAGS_numerical_epsilon, lon_trajectory.Evaluate(1, t_param));
double s_ddot = lon_trajectory.Evaluate(2, t_param);
if (s > s_ref_max) {
break;
......
......@@ -179,7 +179,7 @@ double TrajectoryEvaluator::LatOffsetCost(
cost_abs_sum += std::fabs(cost) * FLAGS_weight_same_side_offset;
}
}
return cost_sqr_sum / (cost_abs_sum + FLAGS_lattice_epsilon);
return cost_sqr_sum / (cost_abs_sum + FLAGS_numerical_epsilon);
}
double TrajectoryEvaluator::LatComfortCost(
......@@ -212,7 +212,7 @@ double TrajectoryEvaluator::LonComfortCost(
cost_sqr_sum += cost * cost;
cost_abs_sum += std::fabs(cost);
}
return cost_sqr_sum / (cost_abs_sum + FLAGS_lattice_epsilon);
return cost_sqr_sum / (cost_abs_sum + FLAGS_numerical_epsilon);
}
double TrajectoryEvaluator::LonObjectiveCost(
......@@ -232,7 +232,7 @@ double TrajectoryEvaluator::LonObjectiveCost(
speed_cost_weight_sum += t * t;
}
double speed_cost =
speed_cost_sqr_sum / (speed_cost_weight_sum + FLAGS_lattice_epsilon);
speed_cost_sqr_sum / (speed_cost_weight_sum + FLAGS_numerical_epsilon);
double dist_travelled_cost = 1.0 / (1.0 + dist_s);
return (speed_cost * FLAGS_weight_target_speed +
dist_travelled_cost * FLAGS_weight_dist_travelled) /
......@@ -266,7 +266,7 @@ double TrajectoryEvaluator::LonCollisionCost(
cost_abs_sum += cost;
}
}
return cost_sqr_sum / (cost_abs_sum + FLAGS_lattice_epsilon);
return cost_sqr_sum / (cost_abs_sum + FLAGS_numerical_epsilon);
}
double TrajectoryEvaluator::CentripetalAccelerationCost(
......@@ -286,7 +286,7 @@ double TrajectoryEvaluator::CentripetalAccelerationCost(
}
return centripetal_acc_sqr_sum /
(centripetal_acc_sum + FLAGS_lattice_epsilon);
(centripetal_acc_sum + FLAGS_numerical_epsilon);
}
std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
......@@ -298,7 +298,7 @@ std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
if (!planning_target.has_stop_point()) {
PiecewiseAccelerationTrajectory1d lon_traj(init_s_[0], cruise_v);
lon_traj.AppendSegment(
0.0, FLAGS_trajectory_time_length + +FLAGS_lattice_epsilon);
0.0, FLAGS_trajectory_time_length + FLAGS_numerical_epsilon);
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
......@@ -306,10 +306,10 @@ std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
}
} else {
double dist_s = planning_target.stop_point().s() - init_s_[0];
if (dist_s < FLAGS_lattice_epsilon) {
if (dist_s < FLAGS_numerical_epsilon) {
PiecewiseAccelerationTrajectory1d lon_traj(init_s_[0], 0.0);
lon_traj.AppendSegment(
0.0, FLAGS_trajectory_time_length + FLAGS_lattice_epsilon);
0.0, FLAGS_trajectory_time_length + FLAGS_numerical_epsilon);
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
......@@ -327,7 +327,7 @@ std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
PiecewiseBrakingTrajectoryGenerator::Generate(
planning_target.stop_point().s(), init_s_[0],
planning_target.cruise_speed(), init_s_[1], a_comfort, d_comfort,
FLAGS_trajectory_time_length + FLAGS_lattice_epsilon);
FLAGS_trajectory_time_length + FLAGS_numerical_epsilon);
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册