Prediction: look ahead lane sequence start from ego_vehicle

modules/prediction/scenario/prioritization/obstacles_prioritizer.cc

@@ -337,13 +337,53 @@ void ObstaclesPrioritizer::AssignCautionLevelByEgoReferenceLine() {
   ADCTrajectoryContainer* adc_trajectory_container =
       ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
           AdapterConfig::PLANNING_TRAJECTORY);
+  if (adc_trajectory_container == nullptr) {
+    AERROR << "adc_trajectory_container is nullptr";
+    return;
+  }
   const std::vector<std::string>& lane_ids =
       adc_trajectory_container->GetADCLaneIDSequence();
   if (lane_ids.empty()) {
     return;
   }
 
+  auto pose_container =
+      ContainerManager::Instance()->GetContainer<PoseContainer>(
+          AdapterConfig::LOCALIZATION);
+  if (pose_container == nullptr) {
+    AERROR << "Pose container pointer is a null pointer.";
+    return;
+  }
+  const PerceptionObstacle* pose_obstacle_ptr =
+      pose_container->ToPerceptionObstacle();
+  if (pose_obstacle_ptr == nullptr) {
+    AERROR << "Pose obstacle pointer is a null pointer.";
+    return;
+  }
+  double pose_x = pose_obstacle_ptr->position().x();
+  double pose_y = pose_obstacle_ptr->position().y();
+  double ego_vehicle_s = std::numeric_limits<double>::max();
+  double ego_vehicle_l = std::numeric_limits<double>::max();
   double accumulated_s = 0.0;
+  // first loop of lane_ids to findout ego_vehicle_s
+  for (const std::string& lane_id : lane_ids) {
+    std::shared_ptr<const LaneInfo> lane_info_ptr =
+        PredictionMap::LaneById(lane_id);
+    if (lane_info_ptr == nullptr) {
+      AERROR << "Null lane info pointer found.";
+      continue;
+    }
+    double s, l;
+    PredictionMap::GetProjection({pose_x, pose_y}, lane_info_ptr, &s, &l);
+    if (l < ego_vehicle_l) {
+      ego_vehicle_s = accumulated_s + s;
+      ego_vehicle_l = l;
+    }
+    accumulated_s += lane_info_ptr->total_length();
+  }
+
+  // then loop of lane_ids to AssignCaution
+  accumulated_s = 0.0;
   for (const std::string& lane_id : lane_ids) {
     std::shared_ptr<const LaneInfo> lane_info_ptr =
         PredictionMap::LaneById(lane_id);
@@ -354,7 +394,7 @@ void ObstaclesPrioritizer::AssignCautionLevelByEgoReferenceLine() {
     accumulated_s += lane_info_ptr->total_length();
     AssignCautionByMerge(lane_info_ptr);
     AssignCautionByOverlap(lane_info_ptr);
-    if (accumulated_s > FLAGS_caution_search_distance_ahead) {
+    if (accumulated_s > FLAGS_caution_search_distance_ahead + ego_vehicle_s) {
       break;
     }
   }