planning: add lon. stop trajectory generation; removed virtual obstacle...

planning: add lon. stop trajectory generation; removed virtual obstacle construction for traffic light

modules/planning/lattice/behavior_decider/signal_light_scenario.cc

@@ -62,17 +62,8 @@ int SignalLightScenario::ComputeScenarioDecision(
       continue;
     }
 
-    double stop_deceleration =
-        EstimateStopDeceleration(reference_line_info, signal_from_map);
-
-    if ((it_signal->second->color() == TrafficLight::RED &&
-         stop_deceleration < FLAGS_max_stop_deceleration) ||
-        // (signal.color() == TrafficLight::UNKNOWN &&
-        //  stop_deceleration < FLAGS_max_stop_deceleration) ||
-        (it_signal->second->color() == TrafficLight::YELLOW &&
-         stop_deceleration < FLAGS_max_stop_deacceleration_for_yellow_light)) {
-      CreateStopObstacle(frame, reference_line_info, signal_from_map);
-
+    if ((it_signal->second->color() == TrafficLight::RED) ||
+        (it_signal->second->color() == TrafficLight::YELLOW)) {
       stop_s = std::min(stop_s, signal_from_map->start_s);
     }
   }
@@ -125,57 +116,5 @@ SignalLightScenario::GetPerceptionDetectedSignals() {
   return detected_signals;
 }
 
-double SignalLightScenario::EstimateStopDeceleration(
-    ReferenceLineInfo* const reference_line_info,
-    const hdmap::PathOverlap* signal_light) {
-  double ego_speed =
-      common::VehicleStateProvider::instance()->linear_velocity();
-
-  double ego_front_s = reference_line_info->AdcSlBoundary().end_s();
-  double stop_s = signal_light->start_s;
-
-  if (stop_s >= ego_front_s) {
-    double stop_distance = stop_s - ego_front_s;
-    return (ego_speed * ego_speed) / stop_distance * 0.5;
-  } else {
-    // longitudinal_acceleration_lower_bound is a negative value.
-    return -FLAGS_longitudinal_acceleration_lower_bound;
-  }
-}
-
-void SignalLightScenario::CreateStopObstacle(
-    Frame* frame, ReferenceLineInfo* const reference_line_info,
-    const hdmap::PathOverlap* signal_light) {
-  // check
-  const auto& reference_line = reference_line_info->reference_line();
-  const double stop_s =
-      signal_light->start_s - FLAGS_traffic_light_stop_distance;
-  const double box_center_s =
-      signal_light->start_s + FLAGS_virtual_stop_wall_length / 2.0;
-  if (!WithinBound(0.0, reference_line.Length(), stop_s) ||
-      !WithinBound(0.0, reference_line.Length(), box_center_s)) {
-    ADEBUG << "signal " << signal_light->object_id
-           << " is not on reference line";
-    return;
-  }
-
-  // create virtual stop wall
-  std::string virtual_obstacle_id =
-      FLAGS_signal_light_virtual_obstacle_id_prefix + signal_light->object_id;
-  auto* obstacle = frame->CreateVirtualStopObstacle(
-      reference_line_info, virtual_obstacle_id, signal_light->start_s);
-  if (!obstacle) {
-    AERROR << "Failed to create obstacle [" << virtual_obstacle_id << "]";
-    return;
-  }
-  PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
-  if (!stop_wall) {
-    AERROR << "Failed to create path_obstacle for: " << virtual_obstacle_id;
-    return;
-  }
-
-  return;
-}
-
 }  // namespace planning
 }  // namespace apollo

modules/planning/lattice/behavior_decider/signal_light_scenario.h

@@ -50,13 +50,6 @@ class SignalLightScenario : public Scenario {
   std::vector<const hdmap::PathOverlap*> FindValidSignalLightFromMap(
       ReferenceLineInfo* const reference_line_info);
 
-  double EstimateStopDeceleration(ReferenceLineInfo* const reference_line_info,
-                                  const hdmap::PathOverlap* signal_light);
-
-  void CreateStopObstacle(Frame* frame,
-                          ReferenceLineInfo* const reference_line_info,
-                          const hdmap::PathOverlap* signal_light);
-
   DECLARE_SCENARIO(SignalLightScenario);
 };
 

modules/planning/lattice/trajectory_generator/end_condition_sampler.cc

@@ -88,6 +88,37 @@ EndConditionSampler::SampleLonEndConditionsForCruising(
   return end_s_conditions;
 }
 
+
+std::vector<std::pair<std::array<double, 3>, double>>
+EndConditionSampler::SampleLonEndConditionsForStopping(
+    const double ref_stop_point) const {
+  // time interval is one second plus the last one 0.01
+  constexpr std::size_t num_time_section = 9;
+  std::array<double, num_time_section> time_sections;
+  for (std::size_t i = 0; i + 1 < num_time_section; ++i) {
+    time_sections[i] = FLAGS_trajectory_time_length - i;
+  }
+  time_sections[num_time_section - 1] = 0.01;
+
+  constexpr std::size_t num_stop_section = 3;
+  std::array<double, num_stop_section> s_offsets;
+  for (std::size_t i = 0; i < num_stop_section; ++i) {
+    s_offsets[i] = -1 * i;
+  }
+
+  std::vector<std::pair<std::array<double, 3>, double>> end_s_conditions;
+  for (const auto& time : time_sections) {
+    for (const auto& s_offset : s_offsets) {
+      std::array<double, 3> end_s;
+      end_s[0] = std::max(init_s_[0], ref_stop_point + s_offset);
+      end_s[1] = 0.0;
+      end_s[2] = 0.0;
+      end_s_conditions.emplace_back(end_s, time);
+    }
+  }
+  return end_s_conditions;
+}
+
 std::vector<std::pair<std::array<double, 3>, double>>
 EndConditionSampler::SampleLonEndConditionsForPathTimeBounds(
     const PlanningTarget& planning_target) const {

modules/planning/lattice/trajectory_generator/end_condition_sampler.h

@@ -47,6 +47,9 @@ class EndConditionSampler {
   std::vector<std::pair<std::array<double, 3>, double>>
   SampleLonEndConditionsForCruising(const double ref_cruise_speed) const;
 
+  std::vector<std::pair<std::array<double, 3>, double>>
+  SampleLonEndConditionsForStopping(const double ref_stop_point) const;
+
   std::vector<std::pair<std::array<double, 3>, double>>
   SampleLonEndConditionsForPathTimeBounds(
       const PlanningTarget& planning_target) const;

modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc

@@ -63,7 +63,7 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForCruising(
     const double target_speed,
     std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const {
   ADEBUG << "cruise speed is  " << target_speed;
-  std::vector<std::pair<std::array<double, 3>, double>> end_conditions =
+  auto end_conditions =
       end_condition_sampler_->SampleLonEndConditionsForCruising(target_speed);
 
   for (const auto& end_condition : end_conditions) {
@@ -77,6 +77,25 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForCruising(
             std::shared_ptr<Curve1d>(new QuarticPolynomialCurve1d(
                 init_lon_state_, end_state, end_condition.second))));
 
+    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::GenerateSpeedProfileForStopping(
+    const double stop_point,
+    std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const {
+  ADEBUG << "stop point is " << stop_point;
+  auto end_conditions =
+      end_condition_sampler_->SampleLonEndConditionsForStopping(stop_point);
+
+  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);
@@ -114,6 +133,11 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
   //
   GenerateSpeedProfilesForPathTimeBound(planning_target,
                                         ptr_lon_trajectory_bundle);
+
+  if (planning_target.has_stop_point()) {
+    GenerateSpeedProfileForStopping(planning_target.stop_point(),
+                                    ptr_lon_trajectory_bundle);
+  }
 }
 
 void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(

modules/planning/lattice/trajectory_generator/trajectory1d_generator.h

@@ -51,6 +51,10 @@ class Trajectory1dGenerator {
       const double target_speed,
       std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
 
+  void GenerateSpeedProfileForStopping(
+      const double stop_point,
+      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
+
   void GenerateSpeedProfilesForPathTimeBound(
       const PlanningTarget& planning_target,
       std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;