Prediction: refactor functions for caution pedestrian, add caution pe…

…destrian in front for new model testing
wuqunyong · May 13, 2020 · 44bd2d4 · 44bd2d4
1 parent 9b1c8c9
commit 44bd2d4
Show file tree

Hide file tree

Showing 5 changed files with 60 additions and 5 deletions.
diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc
@@ -64,6 +64,10 @@ DEFINE_double(junction_distance_threshold, 10.0,
               "to junction to consider as junction scenario");
 DEFINE_bool(enable_all_junction, false,
             "If consider all junction with junction_mlp_model.");
+DEFINE_bool(enable_all_pedestrian_caution_in_front, false,
+            "If true, then all pedestrian in front of ADC are marked caution.");
+DEFINE_bool(enable_rank_caution_obstacles, true,
+            "Rank the caution-level obstacles.");
 DEFINE_int32(caution_obs_max_nums, 6,
              "The max number of caution-level obstacles");
 DEFINE_double(caution_distance_threshold, 60.0,

diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h
@@ -49,6 +49,8 @@ DECLARE_bool(img_show_semantic_map);
 // Scenario
 DECLARE_double(junction_distance_threshold);
 DECLARE_bool(enable_all_junction);
+DECLARE_bool(enable_all_pedestrian_caution_in_front);
+DECLARE_bool(enable_rank_caution_obstacles);
 DECLARE_int32(caution_obs_max_nums);
 DECLARE_double(caution_distance_threshold);
 DECLARE_double(caution_search_distance_ahead);

diff --git a/modules/prediction/scenario/prioritization/obstacles_prioritizer.cc b/modules/prediction/scenario/prioritization/obstacles_prioritizer.cc
@@ -197,9 +197,14 @@ void ObstaclesPrioritizer::AssignCautionLevel() {
   AssignCautionLevelCruiseKeepLane(*ego_vehicle, obstacles_container);
   AssignCautionLevelCruiseChangeLane(*ego_vehicle, obstacles_container);
   AssignCautionLevelByEgoReferenceLine(*ego_vehicle, obstacles_container);
-
-  // Ranking Caution Obstacles
-  RankingCautionLevelObstacles(*ego_vehicle, obstacles_container);
+  AssignCautionLevelPedestrianByEgoReferenceLine(*ego_vehicle,
+                                                 obstacles_container);
+  if (FLAGS_enable_all_pedestrian_caution_in_front) {
+    AssignCautionLevelPedestrianInFront(*ego_vehicle, obstacles_container);
+  }
+  if (FLAGS_enable_rank_caution_obstacles) {
+    RankingCautionLevelObstacles(*ego_vehicle, obstacles_container);
+  }
 }
 
 void ObstaclesPrioritizer::AssignCautionLevelInJunction(
@@ -366,8 +371,17 @@ void ObstaclesPrioritizer::AssignCautionLevelByEgoReferenceLine(
       break;
     }
   }
+}
 
-  // finally loop through all pedestrian to AssignCaution
+void ObstaclesPrioritizer::AssignCautionLevelPedestrianByEgoReferenceLine(
+    const Obstacle& ego_vehicle, ObstaclesContainer* obstacles_container) {
+  ADCTrajectoryContainer* adc_trajectory_container =
+      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
+          AdapterConfig::PLANNING_TRAJECTORY);
+  if (adc_trajectory_container == nullptr) {
+    AERROR << "adc_trajectory_container is nullptr";
+    return;
+  }
   for (const int obstacle_id :
        obstacles_container->curr_frame_movable_obstacle_ids()) {
     Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
@@ -423,6 +437,33 @@ void ObstaclesPrioritizer::AssignCautionLevelByEgoReferenceLine(
   }
 }
 
+void ObstaclesPrioritizer::AssignCautionLevelPedestrianInFront(
+    const Obstacle& ego_vehicle, ObstaclesContainer* obstacles_container) {
+  const Point3D& ego_position = ego_vehicle.latest_feature().position();
+  double ego_heading = ego_vehicle.latest_feature().theta();
+  const auto& obstacle_ids =
+      obstacles_container->curr_frame_movable_obstacle_ids();
+  for (const int obstacle_id : obstacle_ids) {
+    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
+    if (!obstacle_ptr->IsPedestrian() ||
+        obstacle_ptr->history_size() == 0) {
+      continue;
+    }
+    const Feature& obs_feature = obstacle_ptr->latest_feature();
+    double obs_x = obs_feature.position().x();
+    double obs_y = obs_feature.position().y();
+    double diff_x = obs_x - ego_position.x();
+    double diff_y = obs_y - ego_position.y();
+    double inner_prod = std::cos(ego_heading) * diff_x +
+                        std::sin(ego_heading) * diff_y;
+    if (inner_prod < 0.0) {
+      continue;
+    }
+    SetCautionIfCloseToEgo(ego_vehicle, FLAGS_caution_distance_threshold,
+                           obstacle_ptr);
+  }
+}
+
 void ObstaclesPrioritizer::RankingCautionLevelObstacles(
     const Obstacle& ego_vehicle, ObstaclesContainer* obstacles_container) {
   const Point3D& ego_position = ego_vehicle.latest_feature().position();

diff --git a/modules/prediction/scenario/prioritization/obstacles_prioritizer.h b/modules/prediction/scenario/prioritization/obstacles_prioritizer.h
@@ -48,6 +48,12 @@ class ObstaclesPrioritizer {
   void AssignCautionLevelByEgoReferenceLine(
       const Obstacle& ego_vehicle, ObstaclesContainer* obstacles_container);
 
+  void AssignCautionLevelPedestrianByEgoReferenceLine(
+    const Obstacle& ego_vehicle, ObstaclesContainer* obstacles_container);
+
+  void AssignCautionLevelPedestrianInFront(
+      const Obstacle& ego_vehicle, ObstaclesContainer* obstacles_container);
+
   void RankingCautionLevelObstacles(const Obstacle& ego_vehicle,
                                     ObstaclesContainer* obstacles_container);
 

diff --git a/modules/tools/prediction/data_pipelines/scripts/records_to_dump_records.sh b/modules/tools/prediction/data_pipelines/scripts/records_to_dump_records.sh
@@ -39,4 +39,6 @@ fi
     --prediction_offline_mode=6 \
     --prediction_offline_bags=${SRC_DIR} \
     --noenable_multi_thread \
-    --noenable_async_draw_base_image
+    --noenable_async_draw_base_image \
+    --enable_all_pedestrian_caution_in_front \
+    --noenable_rank_caution_obstacles