planning: code clean up for lattice planner

modules/planning/common/planning_gflags.cc

@@ -381,11 +381,10 @@ DEFINE_int32(num_lattice_traj_to_plot, 5,
 DEFINE_double(default_cruise_speed, 5.0, "default cruise speed");
 DEFINE_double(spiral_downsample_curvature_thred, 0.02,
               "curvature threshold for down sampling reference line points");
-DEFINE_bool(enable_sample_bound_planning, true,
-            "enable SampleBound based motion planning");
 DEFINE_bool(enable_lattice_st_image_dump, false,
             "enable sending the lattice st image");
 DEFINE_bool(enable_auto_tuning, false, "enable auto tuning data emission");
+
 DEFINE_double(trajectory_time_resolution, 0.1,
               "Trajectory time resolution in planning");
 DEFINE_double(trajectory_space_resolution, 1.0,
@@ -395,9 +394,6 @@ DEFINE_double(collision_buffer_expansion_ratio, 0.2,
               "to expand in collision checking");
 DEFINE_double(decision_horizon, 200.0,
               "Longitudinal horizon for decision making");
-DEFINE_double(lateral_obstacle_ignore_thred, 2.0,
-              "Lateral threshold for "
-              "obstacles to be considered in path-time graph");
 DEFINE_bool(enable_backup_trajectory, false,
             "If generate backup trajectory when planning fail");
 DEFINE_double(backup_trajectory_cost, 1000.0,

modules/planning/common/planning_gflags.h

@@ -198,7 +198,6 @@ DECLARE_double(lattice_epsilon);
 DECLARE_int32(num_lattice_traj_to_plot);
 DECLARE_double(default_cruise_speed);
 DECLARE_double(spiral_downsample_curvature_thred);
-DECLARE_bool(enable_sample_bound_planning);
 DECLARE_bool(enable_lattice_st_image_dump);
 DECLARE_bool(enable_auto_tuning);
 DECLARE_double(trajectory_time_resolution);
@@ -206,7 +205,6 @@ DECLARE_double(trajectory_space_resolution);
 DECLARE_double(lateral_acceleration_bound);
 DECLARE_double(collision_buffer_expansion_ratio);
 DECLARE_double(decision_horizon);
-DECLARE_double(lateral_obstacle_ignore_thred);
 DECLARE_bool(enable_backup_trajectory);
 DECLARE_double(backup_trajectory_cost);
 

modules/planning/constraint_checker/collision_checker.cc

@@ -106,23 +106,20 @@ void CollisionChecker::BuildPredictedEnv(
 
 bool CollisionChecker::IgnoreObstaclesBehind(
     const std::array<double, 3>& adc_init_d) {
-  bool ignore = false;
-  if (std::abs(adc_init_d[0]) < FLAGS_lateral_obstacle_ignore_thred) {
-    ignore = true;
-  }
-  return ignore;
+  return std::abs(adc_init_d[0]) < FLAGS_default_reference_line_width * 0.5;
 }
 
 bool CollisionChecker::IsBehind(
     const Obstacle* obstacle,
     const std::array<double, 3>& adc_init_s,
     const std::vector<PathPoint>& discretized_reference_line) {
+  double half_lane_width = FLAGS_default_reference_line_width * 0.5;
   TrajectoryPoint point = obstacle->GetPointAtTime(0.0);
   std::pair<double, double> sl_point =
       ReferenceLineFrameConverter::CartesianToFrenet(discretized_reference_line,
           point.path_point().x(), point.path_point().y());
   if (sl_point.first < adc_init_s[0] &&
-      std::abs(sl_point.second) < FLAGS_lateral_obstacle_ignore_thred) {
+      std::abs(sl_point.second) < half_lane_width) {
     ADEBUG << "Ignore obstacle [" << obstacle->Id() << "]";
     return true;
   }

modules/planning/lattice/behavior_decider/path_time_neighborhood.cc

@@ -112,6 +112,8 @@ SLBoundary PathTimeNeighborhood::ComputeObstacleBoundary(
 void PathTimeNeighborhood::SetupObstacles(
     const std::vector<const Obstacle*>& obstacles,
     const std::vector<PathPoint>& discretized_ref_points) {
+
+  double half_lane_width = FLAGS_default_reference_line_width * 0.5;
   for (const Obstacle* obstacle : obstacles) {
     if (prediction_traj_map_.find(obstacle->Id()) ==
         prediction_traj_map_.end()) {
@@ -135,8 +137,8 @@ void PathTimeNeighborhood::SetupObstacles(
       // the obstacle is not shown on the region to be considered.
       if (sl_boundary.end_s() < path_range_.first ||
           sl_boundary.start_s() > path_range_.second ||
-          (sl_boundary.start_l() > FLAGS_lateral_obstacle_ignore_thred &&
-           sl_boundary.end_l() < -FLAGS_lateral_obstacle_ignore_thred)) {
+          (sl_boundary.start_l() > half_lane_width &&
+           sl_boundary.end_l() < -half_lane_width)) {
         if (path_time_obstacle_map_.find(obstacle->Id()) !=
             path_time_obstacle_map_.end()) {
           break;

modules/planning/lattice/trajectory_generator/end_condition_sampler.cc

@@ -42,7 +42,7 @@ std::vector<std::pair<std::array<double, 3>, double>>
 EndConditionSampler::SampleLatEndConditions() const {
   std::vector<std::pair<std::array<double, 3>, double>> end_d_conditions;
   std::array<double, 5> end_d_candidates = {0.0, -0.25, 0.25, -0.5, 0.5};
-  std::array<double, 4> end_s_candidates = {5.0, 10.0, 20.0, 30.0};
+  std::array<double, 4> end_s_candidates = {10.0, 20.0, 30.0, 40.0};
 
   for (const auto& s : end_s_candidates) {
     for (const auto& d : end_d_candidates) {

modules/planning/lattice/trajectory_generator/trajectory_evaluator.cc

@@ -42,6 +42,12 @@ TrajectoryEvaluator::TrajectoryEvaluator(
     std::shared_ptr<PathTimeNeighborhood> pathtime_neighborhood)
     : is_auto_tuning_(is_auto_tuning),
       pathtime_neighborhood_(pathtime_neighborhood) {
+
+  double start_time = 0.0;
+  double end_time = FLAGS_trajectory_time_length;
+  path_time_intervals_ = pathtime_neighborhood_->GetPathBlockingIntervals(
+      start_time, end_time, FLAGS_trajectory_time_resolution);
+
   for (const auto lon_trajectory : lon_trajectories) {
     if (!ConstraintChecker1d::IsValidLongitudinalTrajectory(*lon_trajectory)) {
       continue;
@@ -53,7 +59,7 @@ TrajectoryEvaluator::TrajectoryEvaluator(
       }
       if (!is_auto_tuning_) {
         double cost =
-            Evaluate(planning_target, lon_trajectory, lat_trajectory, nullptr);
+            Evaluate(planning_target, lon_trajectory, lat_trajectory);
         cost_queue_.push(PairCost({lon_trajectory, lat_trajectory}, cost));
       } else {
         std::vector<double> cost_components;
@@ -143,7 +149,7 @@ double TrajectoryEvaluator::Evaluate(
   double lat_offset_cost = LatOffsetCost(lat_trajectory, s_values);
   double lat_comfort_cost = LatComfortCost(lon_trajectory, lat_trajectory);
 
-  if (cost_components) {
+  if (cost_components != nullptr) {
     cost_components->push_back(lon_travel_cost);
     cost_components->push_back(lon_jerk_cost);
     cost_components->push_back(lon_collision_cost);
@@ -236,35 +242,27 @@ double TrajectoryEvaluator::LonObjectiveCost(
 // while constructing trajectory evaluator
 double TrajectoryEvaluator::LonCollisionCost(
     const std::shared_ptr<Trajectory1d>& lon_trajectory) const {
-  double start_time = 0.0;
-  double end_time = FLAGS_trajectory_time_length;
-  const auto& pt_intervals = pathtime_neighborhood_->GetPathBlockingIntervals(
-      start_time, end_time, FLAGS_trajectory_time_resolution);
   double cost_sqr_sum = 0.0;
   double cost_abs_sum = 0.0;
-  for (std::size_t i = 0; i < pt_intervals.size(); ++i) {
-    const auto& pt_interval = pt_intervals[i];
+  for (std::size_t i = 0; i < path_time_intervals_.size(); ++i) {
+    const auto& pt_interval = path_time_intervals_[i];
     if (pt_interval.empty()) {
       continue;
     }
-
-    double t = start_time + i * FLAGS_trajectory_time_resolution;
+    double t = i * FLAGS_trajectory_time_resolution;
     double traj_s = lon_trajectory->Evaluate(0, t);
     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_yield_buffer &&
-          traj_s < m.second + FLAGS_lon_collision_overtake_buffer) {
-        cost = 1.0;
-      } else if (traj_s < m.first) {
-        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 = traj_s - (m.second + FLAGS_lon_collision_overtake_buffer);
-        cost = std::exp(-dist * dist / (2.0 * sigma * sigma));
+      double dist = 0.0;
+      if (traj_s < m.first - FLAGS_lon_collision_yield_buffer) {
+        dist = m.first - FLAGS_lon_collision_yield_buffer - traj_s;
+      } else if (traj_s > m.second + FLAGS_lon_collision_overtake_buffer) {
+        dist = traj_s - m.second - FLAGS_lon_collision_overtake_buffer;
       }
+      double cost = std::exp(-dist * dist / (2.0 * sigma * sigma));
+
       cost_sqr_sum += cost * cost;
-      cost_abs_sum += std::abs(cost);
+      cost_abs_sum += cost;
     }
   }
   return cost_sqr_sum / (cost_abs_sum + FLAGS_lattice_epsilon);

modules/planning/lattice/trajectory_generator/trajectory_evaluator.h

@@ -77,7 +77,7 @@ class TrajectoryEvaluator {
   double Evaluate(const PlanningTarget& planning_target,
                   const std::shared_ptr<Curve1d>& lon_trajectory,
                   const std::shared_ptr<Curve1d>& lat_trajectory,
-                  std::vector<double>* cost_components) const;
+                  std::vector<double>* cost_components = nullptr) const;
 
   double LatOffsetCost(const std::shared_ptr<Curve1d>& lat_trajectory,
                        const std::vector<double>& s_values) const;
@@ -119,6 +119,8 @@ class TrajectoryEvaluator {
   bool is_auto_tuning_ = false;
 
   std::shared_ptr<PathTimeNeighborhood> pathtime_neighborhood_;
+
+  std::vector<std::vector<std::pair<double, double>>> path_time_intervals_;
 };
 
 }  // namespace planning

modules/planning/planner/lattice/lattice_planner.cc

@@ -26,7 +26,6 @@
 #include <utility>
 #include <vector>
 
-#include "modules/planning/lattice/trajectory_generator/backup_trajectory_generator.h"
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/log.h"
 #include "modules/common/macro.h"
@@ -35,6 +34,7 @@
 #include "modules/planning/constraint_checker/collision_checker.h"
 #include "modules/planning/constraint_checker/constraint_checker.h"
 #include "modules/planning/lattice/behavior_decider/path_time_neighborhood.h"
+#include "modules/planning/lattice/trajectory_generator/backup_trajectory_generator.h"
 #include "modules/planning/lattice/trajectory_generator/trajectory1d_generator.h"
 #include "modules/planning/lattice/trajectory_generator/trajectory_combiner.h"
 #include "modules/planning/lattice/trajectory_generator/trajectory_evaluator.h"