Prediction: trim prediction trajectory by virtual lane

modules/prediction/common/prediction_gflags.cc

@@ -108,6 +108,7 @@ DEFINE_double(distance_beyond_junction, 0.5,
               "consider it in junction.");
 DEFINE_double(adc_trajectory_search_length, 10.0,
               "How far to search junction along adc planning trajectory");
+DEFINE_double(virtual_lane_radius, 0.5, "Radius to search virtual lanes");
 
 // move sequence prediction
 DEFINE_double(time_upper_bound_to_lane_center, 5.0,

modules/prediction/common/prediction_gflags.h

@@ -76,6 +76,7 @@ DECLARE_bool(enable_lane_sequence_acc);
 DECLARE_bool(enable_trim_prediction_trajectory);
 DECLARE_double(distance_beyond_junction);
 DECLARE_double(adc_trajectory_search_length);
+DECLARE_double(virtual_lane_radius);
 
 // move sequence prediction
 DECLARE_double(time_upper_bound_to_lane_center);

modules/prediction/common/prediction_map.cc

@@ -90,6 +90,37 @@ bool PredictionMap::ProjectionFromLane(
   return true;
 }
 
+bool PredictionMap::IsVirtualLane(const std::string& lane_id) {
+  std::shared_ptr<const LaneInfo> lane_info =
+      HDMapUtil::BaseMap().GetLaneById(hdmap::MakeMapId(lane_id));
+  if (lane_info == nullptr) {
+    return false;
+  }
+  const apollo::hdmap::Lane& lane = lane_info->lane();
+  bool left_virtual = lane.has_left_boundary() &&
+                      lane.left_boundary().has_virtual_() &&
+                      lane.left_boundary().virtual_();
+  bool right_virtual = lane.has_right_boundary() &&
+                       lane.right_boundary().has_virtual_() &&
+                       lane.right_boundary().virtual_();
+  return left_virtual && right_virtual;
+}
+
+bool PredictionMap::OnVirtualLane(const Eigen::Vector2d& point,
+                                  const double radius) {
+  std::vector<std::shared_ptr<const LaneInfo>> lanes;
+  common::PointENU hdmap_point;
+  hdmap_point.set_x(point[0]);
+  hdmap_point.set_y(point[1]);
+  HDMapUtil::BaseMap().GetLanes(hdmap_point, radius, &lanes);
+  for (const auto& lane : lanes) {
+    if (IsVirtualLane(lane->id().id())) {
+      return true;
+    }
+  }
+  return false;
+}
+
 void PredictionMap::OnLane(
     const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
     const Eigen::Vector2d& point, const double heading, const double radius,

modules/prediction/common/prediction_map.h

@@ -89,6 +89,21 @@ class PredictionMap {
       std::shared_ptr<const hdmap::LaneInfo> lane_info, const double s,
       hdmap::MapPathPoint* path_point);
 
+  /**
+   * @brief Determine if a lane is a virtual lane.
+   * @param The lane ID of the lane.
+   * @return If the lane is a virtual lane.
+   */
+  static bool IsVirtualLane(const std::string& lane_id);
+
+  /**
+   * @brief Determine if a point is on a virtual lane.
+   * @param The point coordinate.
+   * @return If the point is on a virtual lane.
+   */
+  static bool OnVirtualLane(const Eigen::Vector2d& position,
+                            const double radius);
+
   /**
    * @brief Get the connected lanes from some specified lanes.
    * @param prev_lanes The lanes from which to search their connected lanes.

modules/prediction/predictor/predictor.cc

@@ -87,7 +87,9 @@ bool Predictor::TrimTrajectory(
                    forward_length * std::cos(heading);
   double start_y = trajectory->trajectory_point(0).path_point().y() +
                    forward_length * std::sin(heading);
-  if (adc_trajectory_container->IsPointInJunction({start_x, start_y})) {
+  if (adc_trajectory_container->IsPointInJunction({start_x, start_y}) &&
+      PredictionMap::instance()->OnVirtualLane({start_x, start_y},
+                                               FLAGS_virtual_lane_radius)) {
     return false;
   }
   int index = 0;
@@ -97,6 +99,14 @@ bool Predictor::TrimTrajectory(
     if (adc_trajectory_container->IsPointInJunction({x, y})) {
       break;
     }
+    if (!trajectory->trajectory_point(index).path_point().has_lane_id()) {
+      continue;
+    }
+    const std::string& lane_id =
+        trajectory->trajectory_point(index).path_point().lane_id();
+    if (PredictionMap::instance()->IsVirtualLane(lane_id)) {
+      break;
+    }
     ++index;
   }