Prediction: implement const acc trajectory for stop sign

modules/prediction/common/prediction_gflags.cc

@@ -73,6 +73,8 @@ DEFINE_double(r_var, 0.25, "Measurement noise covariance");
 DEFINE_double(p_var, 0.1, "Error covariance");
 DEFINE_double(go_approach_rate, 0.995,
               "The rate to approach to the reference line of going straight");
+DEFINE_double(cutin_approach_rate, 0.95,
+              "The rate to approach to the reference line of cutin");
 
 DEFINE_int32(min_still_obstacle_history_length, 4,
              "Min # historical frames for still obstacles");
@@ -155,6 +157,8 @@ DEFINE_double(distance_beyond_junction, 0.5,
               "consider it in junction.");
 DEFINE_double(defualt_junction_range, 10.0,
               "Default value for the range of a junction.");
+DEFINE_double(distance_to_slow_down_at_stop_sign, 40.0,
+              "The distance to slow down at stop sign");
 
 // Evaluator
 DEFINE_double(time_to_center_if_not_reach, 10.0,

modules/prediction/common/prediction_gflags.h

@@ -53,6 +53,7 @@ DECLARE_double(q_var);
 DECLARE_double(r_var);
 DECLARE_double(p_var);
 DECLARE_double(go_approach_rate);
+DECLARE_double(cutin_approach_rate);
 
 DECLARE_int32(min_still_obstacle_history_length);
 DECLARE_int32(max_still_obstacle_history_length);
@@ -99,6 +100,7 @@ DECLARE_double(centripetal_acc_coeff);
 DECLARE_double(junction_exit_lane_threshold);
 DECLARE_double(distance_beyond_junction);
 DECLARE_double(defualt_junction_range);
+DECLARE_double(distance_to_slow_down_at_stop_sign);
 
 // Evaluator
 DECLARE_double(time_to_center_if_not_reach);

modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc

@@ -74,9 +74,23 @@ void LaneSequencePredictor::Predict(Obstacle* obstacle) {
            << "] with probability [" << sequence.probability() << "].";
 
     std::vector<TrajectoryPoint> points;
-    DrawLaneSequenceTrajectoryPoints(*obstacle, sequence,
-        FLAGS_prediction_trajectory_time_length,
-        FLAGS_prediction_trajectory_time_resolution, &points);
+    bool is_about_to_stop = false;
+    double acceleration = 0.0;
+    if (sequence.has_stop_sign()) {
+      double stop_distance = sequence.stop_sign().lane_sequence_s() -
+                             sequence.lane_s();
+      is_about_to_stop = SupposedToStop(feature, stop_distance, &acceleration);
+    }
+
+    if (is_about_to_stop) {
+      DrawConstantAccelerationTrajectory(
+          *obstacle, sequence, FLAGS_prediction_trajectory_time_length,
+          FLAGS_prediction_trajectory_time_resolution, acceleration, &points);
+    } else {
+      DrawLaneSequenceTrajectoryPoints(*obstacle, sequence,
+          FLAGS_prediction_trajectory_time_length,
+          FLAGS_prediction_trajectory_time_resolution, &points);
+    }
 
     if (points.empty()) {
       continue;
@@ -121,6 +135,10 @@ void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints(
     AERROR << "Failed in getting lane s and lane l";
     return;
   }
+  double approach_rate = FLAGS_go_approach_rate;
+  if (!lane_sequence.vehicle_on_lane()) {
+    approach_rate = FLAGS_cutin_approach_rate;
+  }
   size_t total_num = static_cast<size_t>(total_time / period);
   for (size_t i = 0; i < total_num; ++i) {
     double relative_time = static_cast<double>(i) * period;
@@ -154,7 +172,7 @@ void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints(
       lane_id = lane_sequence.lane_segment(lane_segment_index).lane_id();
     }
 
-    lane_l *= FLAGS_go_approach_rate;
+    lane_l *= approach_rate;
   }
 }
 

modules/prediction/predictor/move_sequence/move_sequence_predictor.cc

@@ -90,17 +90,30 @@ void MoveSequencePredictor::Predict(Obstacle* obstacle) {
            << "] with probability [" << sequence.probability() << "].";
 
     std::vector<TrajectoryPoint> points;
-
     auto end_time1 = std::chrono::system_clock::now();
-    DrawMoveSequenceTrajectoryPointsUsingBestTrajectorySelection(
-        *obstacle, sequence, FLAGS_prediction_trajectory_time_length,
-        FLAGS_prediction_trajectory_time_resolution, &points);
+
+    bool is_about_to_stop = false;
+    double acceleration = 0.0;
+    if (sequence.has_stop_sign()) {
+      double stop_distance = sequence.stop_sign().lane_sequence_s() -
+                             sequence.lane_s();
+      is_about_to_stop = SupposedToStop(feature, stop_distance, &acceleration);
+    }
+
+    if (is_about_to_stop) {
+      DrawConstantAccelerationTrajectory(
+          *obstacle, sequence, FLAGS_prediction_trajectory_time_length,
+          FLAGS_prediction_trajectory_time_resolution, acceleration, &points);
+    } else {
+      DrawMoveSequenceTrajectoryPointsUsingBestTrajectorySelection(
+          *obstacle, sequence, FLAGS_prediction_trajectory_time_length,
+          FLAGS_prediction_trajectory_time_resolution, &points);
+    }
     auto end_time2 = std::chrono::system_clock::now();
     std::chrono::duration<double> diff = end_time2 - end_time1;
     ADEBUG << " Time to draw trajectory: "
            << diff.count() * 1000 << " msec.";
 
-
     Trajectory trajectory = GenerateTrajectory(points);
     trajectory.set_probability(sequence.probability());
     trajectories_.push_back(std::move(trajectory));

modules/prediction/predictor/predictor.cc

@@ -32,6 +32,7 @@ using apollo::common::PathPoint;
 using apollo::common::TrajectoryPoint;
 using apollo::common::math::LineSegment2d;
 using apollo::common::math::Vec2d;
+using apollo::hdmap::LaneInfo;
 using apollo::planning::ADCTrajectory;
 
 const std::vector<Trajectory>& Predictor::trajectories() {
@@ -128,18 +129,21 @@ bool Predictor::TrimTrajectory(
   return true;
 }
 
-void Predictor::DrawConstantAccelerationTrajectory(
-    const Obstacle& obstacle, const LaneSequence& lane_sequence,
-    const double total_time, const double period,
-    const double acceleration,
-    std::vector<TrajectoryPoint>* points) {
-  // TODO(kechxu) implement
-}
-
-bool Predictor::SupposedToStop(const Obstacle& obstacle,
-    const double stop_distance) {
-  // TODO(kechxu) implement
-  return false;
+bool Predictor::SupposedToStop(const Feature& feature,
+                               const double stop_distance,
+                               double* acceleration) {
+  if (stop_distance < std::max(feature.length() * 0.5, 1.0)) {
+    return false;
+  }
+  if (stop_distance > FLAGS_distance_to_slow_down_at_stop_sign) {
+    return false;
+  }
+  double speed = feature.speed();
+  *acceleration = -speed * speed / (2.0 * stop_distance);
+  if (*acceleration > -FLAGS_double_precision) {
+    return false;
+  }
+  return true;
 }
 
 }  // namespace prediction

modules/prediction/predictor/predictor.h

@@ -105,29 +105,15 @@ class Predictor {
                       const ADCTrajectoryContainer* adc_trajectory_container,
                       Trajectory* trajectory);
 
-  /**
-   * @brief Draw constant acceleration trajectory points
-   * @param Obstacle
-   * @param Lane sequence
-   * @param Total prediction time
-   * @param Prediction period
-   * @param acceleration
-   * @param A vector of generated trajectory points
-   */
-  void DrawConstantAccelerationTrajectory(
-    const Obstacle& obstacle, const LaneSequence& lane_sequence,
-    const double total_time, const double period,
-    const double acceleration,
-    std::vector<apollo::common::TrajectoryPoint>* points);
-
   /**
    * @brief Determine if an obstacle is supposed to stop within a distance
-   * @param The specific obstacle
+   * @param The latest feature of obstacle
    * @param The distance to stop
+   * @param The output param of acceleration
    * @return If the obstacle is supposed to stop within a distance
    */
-  bool SupposedToStop(const Obstacle& obstacle,
-    const double stop_distance);
+  bool SupposedToStop(const Feature& feature, const double stop_distance,
+                      double* acceleration);
 
  protected:
   std::vector<Trajectory> trajectories_;

modules/prediction/predictor/sequence/sequence_predictor.cc

@@ -261,5 +261,73 @@ bool SequencePredictor::LaneChangeWithMaxProb(const LaneChangeType& type,
   return false;
 }
 
+void SequencePredictor::DrawConstantAccelerationTrajectory(
+    const Obstacle& obstacle, const LaneSequence& lane_sequence,
+    const double total_time, const double period,
+    const double acceleration,
+    std::vector<TrajectoryPoint>* points) {
+  const Feature& feature = obstacle.latest_feature();
+  if (!feature.has_position() || !feature.has_velocity() ||
+      !feature.position().has_x() || !feature.position().has_y()) {
+    AERROR << "Obstacle [" << obstacle.id()
+           << " is missing position or velocity";
+    return;
+  }
+
+  Eigen::Vector2d position(feature.position().x(), feature.position().y());
+  double speed = feature.speed();
+
+  int lane_segment_index = 0;
+  std::string lane_id =
+      lane_sequence.lane_segment(lane_segment_index).lane_id();
+  std::shared_ptr<const LaneInfo> lane_info = PredictionMap::LaneById(lane_id);
+  double lane_s = 0.0;
+  double lane_l = 0.0;
+  if (!PredictionMap::GetProjection(position, lane_info, &lane_s, &lane_l)) {
+    AERROR << "Failed in getting lane s and lane l";
+    return;
+  }
+  size_t total_num = static_cast<size_t>(total_time / period);
+  for (size_t i = 0; i < total_num; ++i) {
+    double relative_time = static_cast<double>(i) * period;
+    Eigen::Vector2d point;
+    double theta = M_PI;
+    if (!PredictionMap::SmoothPointFromLane(lane_id, lane_s, lane_l, &point,
+                                            &theta)) {
+      AERROR << "Unable to get smooth point from lane [" << lane_id
+             << "] with s [" << lane_s << "] and l [" << lane_l << "]";
+      break;
+    }
+    TrajectoryPoint trajectory_point;
+    PathPoint path_point;
+    path_point.set_x(point.x());
+    path_point.set_y(point.y());
+    path_point.set_z(0.0);
+    path_point.set_theta(theta);
+    path_point.set_lane_id(lane_id);
+    trajectory_point.mutable_path_point()->CopyFrom(path_point);
+    trajectory_point.set_v(speed);
+    trajectory_point.set_a(0.0);
+    trajectory_point.set_relative_time(relative_time);
+    points->emplace_back(std::move(trajectory_point));
+
+    if (speed < FLAGS_double_precision) {
+      continue;
+    }
+
+    lane_s += speed * period + 0.5 * acceleration * period * period;
+    speed += acceleration * period;
+
+    while (lane_s > PredictionMap::LaneById(lane_id)->total_length() &&
+           lane_segment_index + 1 < lane_sequence.lane_segment_size()) {
+      lane_segment_index += 1;
+      lane_s = lane_s - PredictionMap::LaneById(lane_id)->total_length();
+      lane_id = lane_sequence.lane_segment(lane_segment_index).lane_id();
+    }
+
+    lane_l *= FLAGS_go_approach_rate;
+  }
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/predictor/sequence/sequence_predictor.h

@@ -91,6 +91,21 @@ class SequencePredictor : public Predictor {
    */
   double GetLaneChangeDistanceWithADC(const LaneSequence& lane_sequence);
 
+  /**
+   * @brief Draw constant acceleration trajectory points
+   * @param Obstacle
+   * @param Lane sequence
+   * @param Total prediction time
+   * @param Prediction period
+   * @param acceleration
+   * @param A vector of generated trajectory points
+   */
+  void DrawConstantAccelerationTrajectory(
+    const Obstacle& obstacle, const LaneSequence& lane_sequence,
+    const double total_time, const double period,
+    const double acceleration,
+    std::vector<apollo::common::TrajectoryPoint>* points);
+
   /**
    * @brief Clear private members
    */