Prediction: set caution level for keep lane scenario

modules/prediction/common/prediction_gflags.cc

@@ -64,6 +64,7 @@ DEFINE_bool(enable_all_junction, false,
            "If consider all junction with junction_mlp_model.");
 
 // Obstacle features
+DEFINE_int32(ego_vehicle_id, -1, "The obstacle ID of the ego vehicle.");
 DEFINE_double(scan_length, 80.0, "The length of the obstacles scan area");
 DEFINE_double(scan_width, 12.0, "The width of the obstacles scan area");
 DEFINE_double(back_dist_ignore_ped, -2.0,

modules/prediction/common/prediction_gflags.h

@@ -48,6 +48,7 @@ DECLARE_bool(enable_junction_feature);
 DECLARE_bool(enable_all_junction);
 
 // Obstacle features
+DECLARE_int32(ego_vehicle_id);
 DECLARE_double(scan_length);
 DECLARE_double(scan_width);
 DECLARE_double(back_dist_ignore_ped);

modules/prediction/container/obstacles/obstacle.cc

@@ -1528,5 +1528,11 @@ void Obstacle::InitRNNStates() {
 
 bool Obstacle::RNNEnabled() const { return rnn_enabled_; }
 
+void Obstacle::SetCaution() {
+  CHECK_GT(feature_history_.size(), 0);
+  Feature* feature = mutable_latest_feature();
+  feature->mutable_priority()->set_priority(ObstaclePriority::CAUTION);
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/container/obstacles/obstacle.h

@@ -225,6 +225,11 @@ class Obstacle {
    */
   bool RNNEnabled() const;
 
+  /**
+   * @brief Set the obstacle as caution level
+   */
+  void SetCaution();
+
  private:
   Obstacle() = default;
 

modules/prediction/container/obstacles/obstacles_container.cc

@@ -334,6 +334,13 @@ void ObstaclesContainer::BuildLaneGraph() {
     ADEBUG << "Building ordered Lane Graph.";
     obstacle_ptr->BuildLaneGraphFromLeftToRight();
   }
+
+  Obstacle* ego_vehicle = GetObstacle(FLAGS_ego_vehicle_id);
+  if (ego_vehicle == nullptr) {
+    AERROR << "Ego vehicle not inserted";
+    return;
+  }
+  ego_vehicle->BuildLaneGraph();
 }
 
 void ObstaclesContainer::BuildJunctionFeature() {

modules/prediction/scenario/prioritization/BUILD

@@ -10,6 +10,7 @@ cc_library(
         "//modules/prediction/common:environment_features",
         "//modules/prediction/container:container_manager",
         "//modules/prediction/container/obstacles:obstacles_container",
+        "//modules/prediction/container/obstacles:obstacle_clusters",
         "//modules/prediction/container/pose:pose_container",
         "//modules/prediction/scenario/scenario_features:scenario_features",
         "//modules/prediction/scenario/scenario_features:cruise_scenario_features",

modules/prediction/scenario/prioritization/obstacles_prioritizer.cc

@@ -17,10 +17,12 @@
 #include "modules/prediction/scenario/prioritization/obstacles_prioritizer.h"
 
 #include <algorithm>
+#include <limits>
 
 #include "modules/prediction/common/prediction_gflags.h"
 #include "modules/prediction/common/prediction_map.h"
 #include "modules/prediction/container/container_manager.h"
+#include "modules/prediction/container/obstacles/obstacle_clusters.h"
 #include "modules/prediction/container/pose/pose_container.h"
 
 namespace apollo {
@@ -180,5 +182,43 @@ void ObstaclesPrioritizer::AssignIgnoreLevelForCruiseScenario(
   }
 }
 
+void ObstaclesPrioritizer::AssignCautionLevelCruiseKeepLane() {
+  ObstaclesContainer* obstacles_container =
+      ContainerManager::Instance()->GetContainer<
+          ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
+  Obstacle* ego_vehicle =
+      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
+  if (ego_vehicle == nullptr) {
+    AERROR << "Ego vehicle not found";
+    return;
+  }
+  if (ego_vehicle->history_size() == 0) {
+    AERROR << "Ego vehicle has no history";
+    return;
+  }
+  const Feature& ego_latest_feature = ego_vehicle->latest_feature();
+  for (const LaneSequence& lane_sequence :
+       ego_latest_feature.lane().lane_graph().lane_sequence()) {
+    int nearest_front_obstacle_id = -10;
+    double smallest_relative_s = std::numeric_limits<double>::infinity();
+    for (const auto& nearby_obs : lane_sequence.nearby_obstacle()) {
+      if (nearby_obs.s() < 0.0) {
+        continue;
+      }
+      if (nearby_obs.s() < smallest_relative_s) {
+        smallest_relative_s = nearby_obs.s();
+        nearest_front_obstacle_id = nearby_obs.id();
+      }
+    }
+    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(
+        nearest_front_obstacle_id);
+    if (obstacle_ptr == nullptr) {
+      AERROR << "Obstacle [" << nearest_front_obstacle_id << "] Not found";
+      continue;
+    }
+    obstacle_ptr->SetCaution();
+  }
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/scenario/prioritization/obstacles_prioritizer.h

@@ -40,6 +40,8 @@ class ObstaclesPrioritizer {
       const EnvironmentFeatures& environment_features,
       const std::shared_ptr<CruiseScenarioFeatures> scenario_features,
       ObstaclesContainer* ptr_obstacle_contrainer);
+
+  static void AssignCautionLevelCruiseKeepLane();
 };
 
 }  // namespace prediction