Planning: add sampling for neighbor points

modules/planning/lattice/behavior_decider/behavior_decider.cc

@@ -47,14 +47,14 @@ PlanningTarget BehaviorDecider::Analyze(
   CHECK(frame != nullptr);
   CHECK_GT(discretized_reference_line.size(), 0);
 
-  PlanningTarget ret;
+  PlanningTarget planning_target;
   if (ScenarioManager::instance()->ComputeWorldDecision(
-          frame, reference_line_info, &ret) != 0) {
+          frame, reference_line_info, &planning_target) != 0) {
     AERROR << "ComputeWorldDecision error!";
   }
 
   for (const auto& reference_point : discretized_reference_line) {
-    ret.mutable_discretized_reference_line()
+    planning_target.mutable_discretized_reference_line()
         ->add_discretized_reference_line_point()
         ->CopyFrom(reference_point);
   }
@@ -64,14 +64,56 @@ PlanningTarget BehaviorDecider::Analyze(
                                    FLAGS_planning_upper_speed_limit,
                                    path_time_neighborhood_);
 
+  // AddSampleBounds(condition_filter, &planning_target);
+
+  AddNeighborPoints(condition_filter, &planning_target);
+
+  return planning_target;
+}
+
+void BehaviorDecider::AddSampleBounds(
+    const ConditionFilter& condition_filter,
+    PlanningTarget* const planning_target) {
   std::vector<SampleBound> sample_bounds = condition_filter.QuerySampleBounds();
+  for (const auto& sample_bound : sample_bounds) {
+    planning_target->add_sample_bound()->CopyFrom(sample_bound);
+  }
+
+  if (sample_bounds.empty()) {
+    ADEBUG << "Sample_bounds empty";
+  } else {
+    for (const SampleBound& sample_bound : sample_bounds) {
+      ADEBUG << "Sample_bound: " << sample_bound.ShortDebugString();
+    }
+  }
+}
+
+void BehaviorDecider::AddNeighborPoints(
+    const ConditionFilter& condition_filter,
+    PlanningTarget* const planning_target) {
+  std::vector<SamplePoint> neighbor_points =
+      condition_filter.QueryNeighborPoints();
+  for (const auto& neighbor_point : neighbor_points) {
+    planning_target->add_neighbor_point()->CopyFrom(neighbor_point);
+  }
+
+  if (neighbor_points.empty()) {
+    ADEBUG << "Sample_bounds empty";
+  } else {
+    for (const SamplePoint& neighbor_point : neighbor_points) {
+      ADEBUG << "Neighbor point: " << neighbor_point.ShortDebugString();
+    }
+  }
+}
 
-  static int decision_cycles = 0;
-  if (FLAGS_enable_lattice_st_image_dump) {
+void BehaviorDecider::DumpLatticeImage(const int index,
+    const common::TrajectoryPoint& init_planning_point,
+    const ConditionFilter& condition_filter,
+    ReferenceLineInfo* const reference_line_info) {
   apollo::planning_internal::LatticeStTraining st_data;
   double timestamp = init_planning_point.relative_time();
   std::string st_img_name = "DecisionCycle_" +
-                              std::to_string(decision_cycles) + "_" +
+                            std::to_string(index) + "_" +
                             std::to_string(timestamp);
   if (condition_filter.GenerateLatticeStPixels(&st_data, timestamp,
                                                st_img_name)) {
@@ -83,22 +125,6 @@ PlanningTarget BehaviorDecider::Analyze(
     ptr_debug->mutable_planning_data()->mutable_lattice_st_image()->CopyFrom(
         st_data);
   }
-  }
-  decision_cycles += 1;
-
-  // Debug SampleBound
-  if (sample_bounds.empty()) {
-    ADEBUG << "Sample_bounds empty";
-  } else {
-    for (const SampleBound& sample_bound : sample_bounds) {
-      ADEBUG << "Sample_bound: " << sample_bound.ShortDebugString();
-    }
-  }
-
-  for (const auto& sample_bound : sample_bounds) {
-    ret.add_sample_bound()->CopyFrom(sample_bound);
-  }
-  return ret;
 }
 
 }  // namespace planning

modules/planning/lattice/behavior_decider/behavior_decider.h

@@ -29,6 +29,7 @@
 #include "modules/planning/common/frame.h"
 #include "modules/planning/common/reference_line_info.h"
 #include "modules/planning/common/trajectory/discretized_trajectory.h"
+#include "modules/planning/lattice/behavior_decider/condition_filter.h"
 #include "modules/planning/lattice/behavior_decider/path_time_graph.h"
 #include "modules/planning/proto/lattice_structure.pb.h"
 #include "modules/planning/proto/planning.pb.h"
@@ -51,6 +52,19 @@ class BehaviorDecider {
       const std::vector<common::PathPoint>& discretized_reference_line);
 
  private:
+  void AddSampleBounds(
+    const ConditionFilter& condition_filter,
+    PlanningTarget* const plannint_target);
+
+  void AddNeighborPoints(
+    const ConditionFilter& condition_filter,
+    PlanningTarget* const plannint_target);
+
+  void DumpLatticeImage(const int index,
+      const common::TrajectoryPoint& init_planning_point,
+      const ConditionFilter& condition_filter,
+      ReferenceLineInfo* const reference_line_info);
+
   std::shared_ptr<PathTimeGraph> path_time_neighborhood_;
 };
 

modules/planning/lattice/behavior_decider/condition_filter.cc

@@ -159,7 +159,7 @@ PathTimePointPair ConditionFilter::QueryPathTimeObstacleIntervals(
   return block_interval;
 }
 
-std::vector<SamplePoint> ConditionFilter::QuerySamplePoints() const {
+std::vector<SamplePoint> ConditionFilter::QueryNeighborPoints() const {
   std::vector<SamplePoint> sample_points;
   for (const auto& path_time_obstacle :
        ptr_path_time_neighborhood_->GetPathTimeObstacles()) {
@@ -250,7 +250,7 @@ std::vector<double> ConditionFilter::UniformTimeStamps(
 // Compute pixel img for lattice st
 bool ConditionFilter::GenerateLatticeStPixels(
     apollo::planning_internal::LatticeStTraining* st_data, double timestamp,
-    std::string st_img_name) {
+    std::string st_img_name) const {
   int num_rows = 250;
   int num_cols = 160;
   double s_step = 100.0 / static_cast<double>(num_rows);
@@ -297,7 +297,7 @@ bool ConditionFilter::GenerateLatticeStPixels(
   return true;
 }
 
-bool ConditionFilter::WithinObstacleSt(double s, double t) {
+bool ConditionFilter::WithinObstacleSt(double s, double t) const {
   const auto& path_time_obstacles =
       ptr_path_time_neighborhood_->GetPathTimeObstacles();
 

modules/planning/lattice/behavior_decider/condition_filter.h

@@ -45,16 +45,16 @@ class ConditionFilter {
 
   std::vector<SampleBound> QuerySampleBounds() const;
 
-  std::vector<SamplePoint> QuerySamplePoints() const;
+  std::vector<SamplePoint> QueryNeighborPoints() const;
 
   std::vector<std::pair<PathTimePoint, PathTimePoint>>
   QueryPathTimeObstacleIntervals(const double t) const;
 
   bool GenerateLatticeStPixels(
       apollo::planning_internal::LatticeStTraining* st_data, double timestamp,
-      std::string st_img_name);
+      std::string st_img_name) const;
 
-  bool WithinObstacleSt(double s, double t);
+  bool WithinObstacleSt(double s, double t) const;
 
  private:
   // Return true only if t is within the range of time slot,

modules/planning/lattice/trajectory_generator/end_condition_sampler.cc

@@ -147,5 +147,19 @@ EndConditionSampler::SampleLonEndConditionsForPathTimeBounds(
   return end_s_conditions;
 }
 
+std::vector<std::pair<std::array<double, 3>, double>>
+EndConditionSampler::SampleLonEndConditionsForNeighborPoints(
+    const PlanningTarget& planning_target) const {
+  std::vector<std::pair<std::array<double, 3>, double>> end_s_conditions;
+  for (const SamplePoint& neighbor_point : planning_target.neighbor_point()) {
+    double s = neighbor_point.path_time_point().s();
+    double v = neighbor_point.v();
+    std::array<double, 3> end_state = {s, v, 0.0};
+    end_s_conditions.push_back({end_state,
+                                neighbor_point.path_time_point().t()});
+  }
+  return end_s_conditions;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/lattice/trajectory_generator/end_condition_sampler.h

@@ -54,6 +54,10 @@ class EndConditionSampler {
   SampleLonEndConditionsForPathTimeBounds(
       const PlanningTarget& planning_target) const;
 
+  std::vector<std::pair<std::array<double, 3>, double>>
+  SampleLonEndConditionsForNeighborPoints(
+      const PlanningTarget& planning_target) const;
+
  private:
   std::array<double, 3> init_s_;
 

modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc

@@ -123,6 +123,26 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForPathTimeBound(
   }
 }
 
+void Trajectory1dGenerator::GenerateSpeedProfilesForNeighborPoints(
+    const PlanningTarget& planning_target,
+    std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const {
+  std::vector<std::pair<std::array<double, 3>, double>> end_conditions =
+      end_condition_sampler_->SampleLonEndConditionsForNeighborPoints(
+          planning_target);
+
+  for (const auto& end_condition : end_conditions) {
+    std::shared_ptr<LatticeTrajectory1d> lattice_traj_ptr(
+        new LatticeTrajectory1d(
+            std::shared_ptr<Curve1d>(new QuinticPolynomialCurve1d(
+                init_lon_state_, end_condition.first, end_condition.second))));
+
+    lattice_traj_ptr->set_target_position(end_condition.first[0]);
+    lattice_traj_ptr->set_target_velocity(end_condition.first[1]);
+    lattice_traj_ptr->set_target_time(end_condition.second);
+    ptr_lon_trajectory_bundle->push_back(lattice_traj_ptr);
+  }
+}
+
 void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
     const PlanningTarget& planning_target,
     std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const {
@@ -130,8 +150,10 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
   GenerateSpeedProfilesForCruising(planning_target.cruise_speed(),
                                    ptr_lon_trajectory_bundle);
 
-  //
-  GenerateSpeedProfilesForPathTimeBound(planning_target,
+  // GenerateSpeedProfilesForPathTimeBound(planning_target,
+  //                                       ptr_lon_trajectory_bundle);
+
+  GenerateSpeedProfilesForNeighborPoints(planning_target,
                                          ptr_lon_trajectory_bundle);
 
   if (planning_target.has_stop_point()) {

modules/planning/lattice/trajectory_generator/trajectory1d_generator.h

@@ -59,6 +59,10 @@ class Trajectory1dGenerator {
       const PlanningTarget& planning_target,
       std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
 
+  void GenerateSpeedProfilesForNeighborPoints(
+      const PlanningTarget& planning_target,
+      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
+
   void GenerateLongitudinalTrajectoryBundle(
       const PlanningTarget& planning_target,
       std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;

modules/planning/proto/lattice_structure.proto

@@ -46,6 +46,7 @@ message SampleBound {
 message PlanningTarget {
   optional DiscretizedReferenceLine discretized_reference_line = 1;
   repeated SampleBound sample_bound = 2;
-  optional double stop_point = 3;
-  optional double cruise_speed = 4;
+  repeated SamplePoint neighbor_point = 3;
+  optional double stop_point = 4;
+  optional double cruise_speed = 5;
 }