Prediction: correct a few bugs and add ADC into obstacle-container.

modules/prediction/common/message_process.cc

@@ -17,6 +17,8 @@
 #include "modules/prediction/common/message_process.h"
 
 #include <algorithm>
+#include <iomanip>
+#include <limits>
 #include <vector>
 
 #include "cyber/common/file.h"
@@ -94,6 +96,7 @@ void MessageProcess::OnPerception(
       ContainerManager::Instance()->GetContainer<ObstaclesContainer>(
           AdapterConfig::PERCEPTION_OBSTACLES);
   CHECK_NOTNULL(ptr_obstacles_container);
+  ptr_obstacles_container->CleanUp();
 
   // Get pose_container
   auto ptr_ego_pose_container =
@@ -111,8 +114,17 @@ void MessageProcess::OnPerception(
   const PerceptionObstacle* ptr_ego_vehicle =
       ptr_ego_pose_container->ToPerceptionObstacle();
   if (ptr_ego_vehicle != nullptr) {
+    double perception_obs_timestamp = ptr_ego_vehicle->timestamp();
+    if (perception_obstacles.has_header() &&
+        perception_obstacles.header().has_timestamp_sec()) {
+      ADEBUG << "Correcting " << std::fixed << std::setprecision(6)
+             << ptr_ego_vehicle->timestamp() << " to "
+             << std::fixed << std::setprecision(6)
+             << perception_obstacles.header().timestamp_sec();
+      perception_obs_timestamp = perception_obstacles.header().timestamp_sec();
+    }
     ptr_obstacles_container->InsertPerceptionObstacle(
-        *ptr_ego_vehicle, ptr_ego_vehicle->timestamp());
+        *ptr_ego_vehicle, perception_obs_timestamp);
     double x = ptr_ego_vehicle->position().x();
     double y = ptr_ego_vehicle->position().y();
     ADEBUG << "Get ADC position [" << std::fixed << std::setprecision(6) << x

modules/prediction/container/obstacles/obstacle.cc

@@ -443,10 +443,6 @@ void Obstacle::SetType(const PerceptionObstacle& perception_obstacle,
 void Obstacle::SetTimestamp(const PerceptionObstacle& perception_obstacle,
                             const double timestamp, Feature* feature) {
   double ts = timestamp;
-  if (perception_obstacle.has_timestamp() &&
-      perception_obstacle.timestamp() > 0.0) {
-    ts = perception_obstacle.timestamp();
-  }
   feature->set_timestamp(ts);
 
   ADEBUG << "Obstacle [" << id_ << "] has timestamp [" << std::fixed

modules/prediction/container/obstacles/obstacles_container.cc

@@ -16,6 +16,8 @@
 
 #include "modules/prediction/container/obstacles/obstacles_container.h"
 
+#include <iomanip>
+#include <limits>
 #include <unordered_set>
 #include <utility>
 
@@ -37,16 +39,18 @@ ObstaclesContainer::ObstaclesContainer()
     : ptr_obstacles_(FLAGS_max_num_obstacles),
       id_mapping_(FLAGS_max_num_obstacles) {}
 
-// This is called by Perception module at every frame to insert all
-// detected obstacles.
-void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
+void ObstaclesContainer::CleanUp() {
   // Clean up the history and get the PerceptionObstacles
   curr_frame_id_mapping_.clear();
   curr_frame_movable_obstacle_ids_.clear();
   curr_frame_unmovable_obstacle_ids_.clear();
   curr_frame_considered_obstacle_ids_.clear();
   curr_frame_id_perception_obstacle_map_.clear();
+}
 
+// This is called by Perception module at every frame to insert all
+// detected obstacles.
+void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   PerceptionObstacles perception_obstacles;
   perception_obstacles.CopyFrom(
       dynamic_cast<const PerceptionObstacles&>(message));
@@ -56,7 +60,6 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   //   obstacle history.
   // - If it's not a valid time (earlier than history), continue.
   // - Also consider the offline_mode case.
-
   double timestamp = 0.0;
   if (perception_obstacles.has_header() &&
       perception_obstacles.header().has_timestamp_sec()) {
@@ -116,7 +119,8 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   }
 
   timestamp_ = timestamp;
-  ADEBUG << "Current timestamp is [" << timestamp_ << "]";
+  ADEBUG << "Current timestamp is [" << std::fixed << std::setprecision(6)
+         << timestamp_ << "]";
 
   // Prediction tracking adaptation
   if (FLAGS_enable_tracking_adaptation) {
@@ -136,7 +140,8 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
     ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "] "
            << "was inserted";
   }
-  // 3. Sort the Obstacles
+
+  SetConsideredObstacleIds();
   ObstacleClusters::SortObstacles();
 }
 
@@ -230,6 +235,8 @@ void ObstaclesContainer::InsertPerceptionObstacle(
   // Insert the obstacle and also update the LRUCache.
   auto obstacle_ptr = GetObstacleWithLRUUpdate(id);
   if (obstacle_ptr != nullptr) {
+    ADEBUG << "Current time = " << std::fixed << std::setprecision(6)
+           << timestamp;
     obstacle_ptr->Insert(perception_obstacle, timestamp, id);
     ADEBUG << "Refresh obstacle [" << id << "]";
   } else {
@@ -242,10 +249,11 @@ void ObstaclesContainer::InsertPerceptionObstacle(
     ADEBUG << "Insert obstacle [" << id << "]";
   }
 
-  if (id != FLAGS_ego_vehicle_id) {
+  if (FLAGS_prediction_offline_mode ==
+          PredictionConstants::kDumpDataForLearning ||
+      id != FLAGS_ego_vehicle_id) {
     curr_frame_movable_obstacle_ids_.push_back(id);
   }
-  SetConsideredObstacleIds();
 }
 
 void ObstaclesContainer::InsertFeatureProto(const Feature& feature) {
@@ -354,6 +362,7 @@ void ObstaclesContainer::BuildLaneGraph() {
       obstacle_ptr->BuildLaneGraphFromLeftToRight();
     } else {
       ADEBUG << "Building ordered Lane Graph.";
+      ADEBUG << "Building lane graph for id = " << id;
       obstacle_ptr->BuildLaneGraphFromLeftToRight();
     }
     obstacle_ptr->SetNearbyObstacles();

modules/prediction/container/obstacles/obstacles_container.h

@@ -94,6 +94,8 @@ class ObstaclesContainer : public Container {
    */
   void Clear();
 
+  void CleanUp();
+
   size_t NumOfObstacles() { return ptr_obstacles_.size(); }
 
   const apollo::perception::PerceptionObstacle& GetPerceptionObstacle(

modules/prediction/evaluator/evaluator_manager.cc

@@ -145,6 +145,11 @@ void EvaluatorManager::Init(const PredictionConf& config) {
             } else {
               vehicle_in_junction_evaluator_ = obstacle_conf.evaluator_type();
             }
+            if (FLAGS_prediction_offline_mode ==
+                PredictionConstants::kDumpDataForLearning) {
+              vehicle_in_junction_evaluator_ =
+                  ObstacleConf::LANE_SCANNING_EVALUATOR;
+            }
           }
           break;
         }
@@ -222,10 +227,6 @@ void EvaluatorManager::Run() {
         });
   } else {
     for (int id : obstacles_container->curr_frame_considered_obstacle_ids()) {
-      if (id < 0) {
-        ADEBUG << "The obstacle has invalid id [" << id << "].";
-        continue;
-      }
       Obstacle* obstacle = obstacles_container->GetObstacle(id);
 
       if (obstacle == nullptr) {

modules/prediction/evaluator/vehicle/lane_scanning_evaluator.cc

@@ -92,9 +92,12 @@ bool LaneScanningEvaluator::Evaluate(Obstacle* obstacle_ptr,
   std::vector<double> labels = {0.0};
   if (FLAGS_prediction_offline_mode ==
       PredictionConstants::kDumpDataForLearning) {
+    std::string learning_data_tag = "cruise";
+    if (latest_feature_ptr->has_junction_feature()) {
+      learning_data_tag = "junction";
+    }
     FeatureOutput::InsertDataForLearning(*latest_feature_ptr, feature_values,
-                                         string_feature_values, "cruise",
-                                         nullptr);
+        string_feature_values, learning_data_tag, nullptr);
     ADEBUG << "Save extracted features for learning locally.";
     return true;
   }