Prediction: Fix trim bug by using PNC junction

modules/prediction/common/prediction_gflags.cc

@@ -183,7 +183,7 @@ DEFINE_double(lane_sequence_threshold_junction, 0.5,
 DEFINE_double(lane_change_dist, 10.0, "Lane change distance with ADC");
 DEFINE_bool(enable_lane_sequence_acc, false,
             "If use acceleration in lane sequence.");
-DEFINE_bool(enable_trim_prediction_trajectory, false,
+DEFINE_bool(enable_trim_prediction_trajectory, true,
             "If trim the prediction trajectory to avoid crossing"
             "protected adc planning trajectory.");
 DEFINE_bool(enable_trajectory_validation_check, false,

modules/prediction/common/prediction_map.cc

@@ -43,6 +43,7 @@ using apollo::hdmap::HDMapUtil;
 using apollo::hdmap::Id;
 using apollo::hdmap::LaneInfo;
 using apollo::hdmap::JunctionInfo;
+using apollo::hdmap::PNCJunctionInfo;
 using apollo::hdmap::OverlapInfo;
 using apollo::hdmap::MapPathPoint;
 
@@ -303,6 +304,17 @@ std::vector<std::shared_ptr<const JunctionInfo>> PredictionMap::GetJunctions(
   return junctions;
 }
 
+std::vector<std::shared_ptr<const PNCJunctionInfo>>
+PredictionMap::GetPNCJunctions(
+    const Eigen::Vector2d& point, const double radius) {
+  common::PointENU hdmap_point;
+  hdmap_point.set_x(point.x());
+  hdmap_point.set_y(point.y());
+  std::vector<std::shared_ptr<const PNCJunctionInfo>> junctions;
+  HDMapUtil::BaseMap().GetPNCJunctions(hdmap_point, radius, &junctions);
+  return junctions;
+}
+
 bool PredictionMap::InJunction(const Eigen::Vector2d& point,
                                const double radius) {
   auto junction_infos = GetJunctions(point, radius);

modules/prediction/common/prediction_map.h

@@ -233,6 +233,15 @@ class PredictionMap {
   static std::vector<std::shared_ptr<const apollo::hdmap::JunctionInfo>>
   GetJunctions(const Eigen::Vector2d& point, const double radius);
 
+  /**
+   * @brief Get a list of junctions given a point and a search radius
+   * @param Point
+   * @param Search radius
+   * @return A list of junctions
+   */
+  static std::vector<std::shared_ptr<const apollo::hdmap::PNCJunctionInfo>>
+  GetPNCJunctions(const Eigen::Vector2d& point, const double radius);
+
   /**
    * @brief Get the lane heading on a point.
    * @param lane_info The target lane.

modules/prediction/conf/prediction.conf

@@ -6,7 +6,6 @@
 --noenable_kf_tracking
 --noprediction_offline_mode
 
---enable_trim_prediction_trajectory
 --lane_change_dist=10.0
 
 --lane_search_radius=5.0

modules/prediction/container/adc_trajectory/adc_trajectory_container.cc

@@ -32,6 +32,7 @@ using ::apollo::common::math::LineSegment2d;
 using ::apollo::common::math::Polygon2d;
 using ::apollo::common::math::Vec2d;
 using ::apollo::hdmap::JunctionInfo;
+using ::apollo::hdmap::PNCJunctionInfo;
 using ::apollo::planning::ADCTrajectory;
 
 ADCTrajectoryContainer::ADCTrajectoryContainer()
@@ -54,6 +55,9 @@ void ADCTrajectoryContainer::Insert(
   SetJunctionPolygon();
   ADEBUG << "Generate a polygon [" << adc_junction_polygon_.DebugString()
          << "].";
+  SetPNCJunctionPolygon();
+  ADEBUG << "Generate a polygon [" << adc_pnc_junction_polygon_.DebugString()
+         << "].";
 
   // Find ADC lane sequence
   SetLaneSequence();
@@ -61,7 +65,16 @@ void ADCTrajectoryContainer::Insert(
          << "].";
 }
 
-bool ADCTrajectoryContainer::IsPointInJunction(const PathPoint& point) const {
+bool ADCTrajectoryContainer::IsPointInJunction(
+    const PathPoint& point) const {
+  if (adc_pnc_junction_info_ptr_ != nullptr) {
+    return IsPointInPNCJunction(point);
+  }
+  return IsPointInRegularJunction(point);
+}
+
+bool ADCTrajectoryContainer::IsPointInRegularJunction(
+    const PathPoint& point) const {
   if (adc_junction_polygon_.points().size() < 3) {
     return false;
   }
@@ -78,6 +91,14 @@ bool ADCTrajectoryContainer::IsPointInJunction(const PathPoint& point) const {
   return in_polygon && on_virtual_lane;
 }
 
+bool ADCTrajectoryContainer::IsPointInPNCJunction(
+    const PathPoint& point) const {
+  if (adc_pnc_junction_polygon_.points().size() < 3) {
+    return false;
+  }
+  return adc_pnc_junction_polygon_.IsPointIn({point.x(), point.y()});
+}
+
 bool ADCTrajectoryContainer::IsProtected() const {
   return adc_trajectory_.has_right_of_way_status() &&
          adc_trajectory_.right_of_way_status() == ADCTrajectory::PROTECTED;
@@ -125,6 +146,48 @@ void ADCTrajectoryContainer::SetJunctionPolygon() {
   }
 }
 
+void ADCTrajectoryContainer::SetPNCJunctionPolygon() {
+  std::shared_ptr<const PNCJunctionInfo> junction_info(nullptr);
+
+  double s_start = 0.0;
+  double s_at_junction = 0.0;
+  if (adc_trajectory_.trajectory_point_size() > 0) {
+    s_start = adc_trajectory_.trajectory_point(0).path_point().s();
+  }
+  for (int i = 0; i < adc_trajectory_.trajectory_point_size(); ++i) {
+    double s = adc_trajectory_.trajectory_point(i).path_point().s();
+
+    if (s > FLAGS_adc_trajectory_search_length) {
+      break;
+    }
+
+    if (junction_info != nullptr) {
+      s_at_junction = s;
+      break;
+    }
+
+    double x = adc_trajectory_.trajectory_point(i).path_point().x();
+    double y = adc_trajectory_.trajectory_point(i).path_point().y();
+    std::vector<std::shared_ptr<const PNCJunctionInfo>> junctions =
+        PredictionMap::GetPNCJunctions({x, y}, FLAGS_junction_search_radius);
+    if (!junctions.empty() && junctions.front() != nullptr) {
+      junction_info = junctions.front();
+    }
+  }
+
+  if (junction_info != nullptr && junction_info->pnc_junction().has_polygon()) {
+    std::vector<Vec2d> vertices;
+    for (const auto& point : junction_info->pnc_junction().polygon().point()) {
+      vertices.emplace_back(point.x(), point.y());
+    }
+    if (vertices.size() >= 3) {
+      adc_pnc_junction_info_ptr_ = junction_info;
+      s_dist_to_junction_ = s_at_junction - s_start;
+      adc_pnc_junction_polygon_ = Polygon2d{vertices};
+    }
+  }
+}
+
 std::shared_ptr<const apollo::hdmap::JunctionInfo>
 ADCTrajectoryContainer::ADCJunction() const {
   return adc_junction_info_ptr_;

modules/prediction/container/adc_trajectory/adc_trajectory_container.h

@@ -103,6 +103,22 @@ class ADCTrajectoryContainer : public Container {
  private:
   void SetJunctionPolygon();
 
+  void SetPNCJunctionPolygon();
+
+  /**
+   * @brief Check if a point is in the first junction of the adc trajectory
+   * @param Point
+   * @return True if the point is in the first junction of the adc trajectory
+   */
+  bool IsPointInRegularJunction(const common::PathPoint& point) const;
+
+  /**
+   * @brief Check if a point is in the first PNC junction of the adc trajectory
+   * @param Point
+   * @return True if the point is in the first PNC junction of the adc trajectory
+   */
+  bool IsPointInPNCJunction(const common::PathPoint& point) const;
+
   void SetLaneSequence();
 
   std::string ToString(const std::unordered_set<std::string>& lane_ids);
@@ -112,7 +128,9 @@ class ADCTrajectoryContainer : public Container {
  private:
   planning::ADCTrajectory adc_trajectory_;
   common::math::Polygon2d adc_junction_polygon_;
+  common::math::Polygon2d adc_pnc_junction_polygon_;
   std::shared_ptr<const hdmap::JunctionInfo> adc_junction_info_ptr_;
+  std::shared_ptr<const hdmap::PNCJunctionInfo> adc_pnc_junction_info_ptr_;
   double s_dist_to_junction_;
   std::unordered_set<std::string> adc_lane_ids_;
   std::vector<std::string> adc_lane_seq_;

modules/prediction/container/obstacles/obstacle.cc

@@ -302,7 +302,7 @@ void Obstacle::SetJunctionFeatureWithEnterLane(
 void Obstacle::SetJunctionFeatureWithoutEnterLane(
     Feature* const feature_ptr) {
   if (!feature_ptr->has_lane()) {
-    AERROR << "Obstacle [" << id_ << "] has no lane.";
+    ADEBUG << "Obstacle [" << id_ << "] has no lane.";
     return;
   }
   std::vector<std::string> start_lane_ids;