Revert "Planning: remove code related to front obs distance"

This reverts commit d284446629dbeec90bafc32a8f3e7cc671ed143e.

Conflicts:
	modules/planning/tasks/deciders/path_bounds_decider.cc

modules/planning/common/ego_info.cc

@@ -59,6 +59,49 @@ void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) {
 void EgoInfo::Clear() {
   start_point_.Clear();
   vehicle_state_.Clear();
+  front_clear_distance_ = FLAGS_default_front_clear_distance;
+}
+
+// TODO(all): remove this function and "front_clear_distance" related.
+// It doesn't make sense when:
+// 1. the heading is not necessaries align with the road
+// 2. the road is not necessaries straight
+void EgoInfo::CalculateFrontObstacleClearDistance(
+    const std::vector<const Obstacle*>& obstacles) {
+  Vec2d position(vehicle_state_.x(), vehicle_state_.y());
+
+  const auto& param = ego_vehicle_config_.vehicle_param();
+  Vec2d vec_to_center(
+      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
+      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);
+
+  Vec2d center(position + vec_to_center.rotate(vehicle_state_.heading()));
+
+  Vec2d unit_vec_heading = Vec2d::CreateUnitVec2d(vehicle_state_.heading());
+
+  // Due to the error of ego heading, only short range distance is meaningful
+  constexpr double kDistanceThreshold = 50.0;
+  constexpr double buffer = 0.1;  // in meters
+  const double impact_region_length =
+      param.length() + buffer + kDistanceThreshold;
+  Box2d ego_front_region(center + unit_vec_heading * kDistanceThreshold / 2.0,
+                         vehicle_state_.heading(), impact_region_length,
+                         param.width() + buffer);
+
+  for (const auto& obstacle : obstacles) {
+    if (obstacle->IsVirtual() ||
+        !ego_front_region.HasOverlap(obstacle->PerceptionBoundingBox())) {
+      continue;
+    }
+
+    double dist = ego_box_.center().DistanceTo(
+                      obstacle->PerceptionBoundingBox().center()) -
+                  ego_box_.diagonal() / 2.0;
+
+    if (front_clear_distance_ < 0.0 || dist < front_clear_distance_) {
+      front_clear_distance_ = dist;
+    }
+  }
 }
 
 }  // namespace planning

modules/planning/common/ego_info.h

@@ -46,8 +46,13 @@ class EgoInfo {
 
   common::VehicleState vehicle_state() const { return vehicle_state_; }
 
+  double front_clear_distance() const { return front_clear_distance_; }
+
   common::math::Box2d ego_box() const { return ego_box_; }
 
+  void CalculateFrontObstacleClearDistance(
+      const std::vector<const Obstacle*>& obstacles);
+
  private:
   FRIEND_TEST(EgoInfoTest, EgoInfoSimpleTest);
 
@@ -68,6 +73,8 @@ class EgoInfo {
   // ego vehicle state
   common::VehicleState vehicle_state_;
 
+  double front_clear_distance_ = FLAGS_default_front_clear_distance;
+
   common::VehicleConfig ego_vehicle_config_;
 
   common::math::Box2d ego_box_;

modules/planning/common/ego_info_test.cc

@@ -40,6 +40,16 @@ TEST(EgoInfoTest, EgoInfoSimpleTest) {
   EXPECT_DOUBLE_EQ(ego_info->start_point().path_point().x(), p.x());
   EXPECT_DOUBLE_EQ(ego_info->start_point().path_point().y(), p.y());
   EXPECT_DOUBLE_EQ(ego_info->start_point().path_point().z(), p.z());
+
+  uint32_t sequence_num = 0;
+  common::TrajectoryPoint planning_start_point;
+  common::VehicleState vehicle_state;
+  ReferenceLineProvider reference_line_provider;
+
+  LocalView dummy_local_view;
+  Frame frame(sequence_num, dummy_local_view, planning_start_point,
+              vehicle_state, &reference_line_provider);
+  ego_info->CalculateFrontObstacleClearDistance(frame.obstacles());
 }
 
 }  // namespace planning

modules/planning/common/planning_gflags.cc

@@ -461,7 +461,7 @@ DEFINE_bool(enable_record_debug, true,
             "True to enable record debug info in chart format");
 
 DEFINE_double(
-    default_front_clear_distance, 100.0,
+    default_front_clear_distance, 300.0,
     "default front clear distance value in case there is no obstacle around.");
 
 DEFINE_double(max_trajectory_len, 1000.0,

modules/planning/common/reference_line_info.h

@@ -79,8 +79,6 @@ class ReferenceLineInfo {
   void SetCost(double cost) { cost_ = cost; }
   double PriorityCost() const { return priority_cost_; }
   void SetPriorityCost(double cost) { priority_cost_ = cost; }
-  double ReachableS() const { return reachable_s_; }
-  void SetReachableS(double reachable_s) { reachable_s_ = reachable_s; }
   // For lattice planner'speed planning target
   void SetStopPoint(const StopPoint& stop_point);
   void SetCruiseSpeed(double speed);
@@ -270,8 +268,6 @@ class ReferenceLineInfo {
    */
   double cost_ = 0.0;
 
-  double reachable_s_ = 0.0;
-
   bool is_inited_ = false;
 
   bool is_drivable_ = true;

modules/planning/common/speed_profile_generator.cc

@@ -121,13 +121,11 @@ std::vector<SpeedPoint> SpeedProfileGenerator::GenerateSpeedHotStart(
   return hot_start_speed_profile;
 }
 
-SpeedData SpeedProfileGenerator::GenerateFallbackSpeedProfile(
-    const double reachable_s) {
+SpeedData SpeedProfileGenerator::GenerateFallbackSpeedProfile() {
   const double init_v = EgoInfo::Instance()->start_point().v();
   const double init_a = EgoInfo::Instance()->start_point().a();
   if (init_v > FLAGS_polynomial_speed_fallback_velocity) {
-    auto speed_data = GenerateStopProfileFromPolynomial(init_v, init_a,
-                                                        reachable_s);
+    auto speed_data = GenerateStopProfileFromPolynomial(init_v, init_a);
     if (!speed_data.empty()) {
       return speed_data;
     }
@@ -161,11 +159,13 @@ SpeedData SpeedProfileGenerator::GenerateStopProfile(const double init_speed,
 }
 
 SpeedData SpeedProfileGenerator::GenerateStopProfileFromPolynomial(
-    const double init_speed, const double init_acc, const double reachable_s) {
+    const double init_speed, const double init_acc) {
   AERROR << "Slowing down the car with polynomial.";
   constexpr double kMaxT = 4.0;
   for (double t = 2.0; t <= kMaxT; t += 0.5) {
-    for (double s = 0.0; s < std::min(50.0, reachable_s - 0.3); s += 1.0) {
+    for (double s = 0.0;
+         s < std::min(50.0, EgoInfo::Instance()->front_clear_distance() - 0.3);
+         s += 1.0) {
       QuinticPolynomialCurve1d curve(0.0, init_speed, init_acc, s, 0.0, 0.0, t);
       if (!IsValidProfile(curve)) {
         continue;

modules/planning/common/speed_profile_generator.h

@@ -44,7 +44,7 @@ class SpeedProfileGenerator {
   static std::vector<common::SpeedPoint> GenerateSpeedHotStart(
       const common::TrajectoryPoint& planning_init_point);
 
-  static SpeedData GenerateFallbackSpeedProfile(const double reachable_s);
+  static SpeedData GenerateFallbackSpeedProfile();
 
   static SpeedData GenerateFixedDistanceCreepProfile(const double distance,
                                                      const double max_speed);
@@ -57,8 +57,7 @@ class SpeedProfileGenerator {
                                        const double init_acc);
 
   static SpeedData GenerateStopProfileFromPolynomial(const double init_speed,
-                                                     const double init_acc,
-                                                     const double reachable_s);
+                                                     const double init_acc);
 
   static bool IsValidProfile(const QuinticPolynomialCurve1d& curve);
 };

modules/planning/common/speed_profile_generator_test.cc

@@ -29,7 +29,7 @@ namespace apollo {
 namespace planning {
 
 TEST(SpeedProfileGeneratorTest, GenerateFallbackSpeedProfile) {
-  auto speed_data = SpeedProfileGenerator::GenerateFallbackSpeedProfile(100.0);
+  auto speed_data = SpeedProfileGenerator::GenerateFallbackSpeedProfile();
   EXPECT_FALSE(speed_data.empty());
 
   common::TrajectoryPoint adc_planning_point;
@@ -38,7 +38,7 @@ TEST(SpeedProfileGeneratorTest, GenerateFallbackSpeedProfile) {
   common::VehicleState vs;
 
   EgoInfo::Instance()->Update(adc_planning_point, vs);
-  auto speed_data2 = SpeedProfileGenerator::GenerateFallbackSpeedProfile(100.0);
+  auto speed_data2 = SpeedProfileGenerator::GenerateFallbackSpeedProfile();
   EXPECT_FALSE(speed_data2.empty());
 }
 

modules/planning/on_lane_planning.cc

@@ -241,6 +241,11 @@ void OnLanePlanning::RunOnce(const LocalView& local_view,
       EgoInfo::Instance()->Update(stitching_trajectory.back(), vehicle_state);
   status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
 
+  if (update_ego_info && status.ok()) {
+    EgoInfo::Instance()->CalculateFrontObstacleClearDistance(
+        frame_->obstacles());
+  }
+
   if (FLAGS_enable_record_debug) {
     frame_->RecordInputDebug(trajectory_pb->mutable_debug());
   }
@@ -367,11 +372,10 @@ Status OnLanePlanning::Plan(
   auto status =
       planner_->Plan(stitching_trajectory.back(), frame_.get(), trajectory_pb);
 
-  const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();
-
   ptr_debug->mutable_planning_data()->set_front_clear_distance(
-      best_ref_info->ReachableS());
+      EgoInfo::Instance()->front_clear_distance());
 
+  const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();
   if (!best_ref_info) {
     std::string msg("planner failed to make a driving plan");
     AERROR << msg;

modules/planning/scenarios/lane_follow/lane_follow_stage.cc

@@ -233,8 +233,7 @@ Status LaneFollowStage::PlanOnReferenceLine(
     AERROR << "Speed fallback.";
 
     *reference_line_info->mutable_speed_data() =
-        SpeedProfileGenerator::GenerateFallbackSpeedProfile(
-            reference_line_info->ReachableS());
+        SpeedProfileGenerator::GenerateFallbackSpeedProfile();
     reference_line_info->AddCost(kSpeedOptimizationFallbackCost);
     reference_line_info->set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
   } else {

modules/planning/scenarios/side_pass/stage_stop_on_wait_point.cc

@@ -118,8 +118,7 @@ Stage::StageStatus StageStopOnWaitPoint::Process(
     auto& rfl_info = frame->mutable_reference_line_info()->front();
     *(rfl_info.mutable_path_data()) = GetContext()->path_data_;
     *(rfl_info.mutable_speed_data()) =
-        SpeedProfileGenerator::GenerateFallbackSpeedProfile(
-            rfl_info.ReachableS());
+        SpeedProfileGenerator::GenerateFallbackSpeedProfile();
 
     rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
     DiscretizedTrajectory trajectory;

modules/planning/scenarios/stage.cc

@@ -93,8 +93,7 @@ bool Stage::ExecuteTaskOnReferenceLine(
 
     if (reference_line_info.speed_data().empty()) {
       *reference_line_info.mutable_speed_data() =
-          SpeedProfileGenerator::GenerateFallbackSpeedProfile(
-              reference_line_info.ReachableS());
+          SpeedProfileGenerator::GenerateFallbackSpeedProfile();
       reference_line_info.AddCost(kSpeedOptimizationFallbackCost);
       reference_line_info.set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
     } else {

modules/planning/tasks/deciders/path_bounds_decider.cc

@@ -179,6 +179,7 @@ std::string PathBoundsDecider::GenerateFallbackPathBoundaries(
     AERROR << msg;
     return msg;
   }
+<<<<<<< HEAD
   PathBoundsDebugString(path_boundaries);
 
   // 2. Decide a rough boundary based on road info and ADC's position
@@ -194,6 +195,18 @@ std::string PathBoundsDecider::GenerateFallbackPathBoundaries(
 
   ADEBUG << "Completed generating fallback path boundaries.";
   return "";
+=======
+  reference_line_info->SetPathBoundaries(path_boundaries_pair,
+                                         std::get<0>(path_boundaries[0]),
+                                         kPathBoundsDeciderResolution);
+  reference_line_info->SetBlockingObstacleId(blocking_obstacle_id_);
+  if (!path_boundaries.empty()) {
+    CHECK_LE(adc_frenet_l_, std::get<2>(path_boundaries[0]));
+    CHECK_GE(adc_frenet_l_, std::get<1>(path_boundaries[0]));
+  }
+  ADEBUG << "Completed path boundaries generation.";
+  return Status::OK();
+>>>>>>> parent of d284446... Planning: remove code related to front obs distance
 }
 
 bool PathBoundsDecider::InitPathBoundaries(