Prediction: enabled replay mode by timestamp gap (#454)

modules/prediction/common/prediction_gflags.cc

@@ -34,6 +34,10 @@ DEFINE_double(double_precision, 1e-6, "precision of double");
 DEFINE_double(min_prediction_length, 5.0,
               "Minimal length of prediction trajectory");
 
+// Bag replay timestamp gap
+DEFINE_double(replay_timestamp_gap, 10.0,
+              "Max timestamp gap for rosbag replay");
+
 // Map
 DEFINE_double(search_radius, 3.0, "Search radius for a candidate lane");
 
@@ -72,7 +76,7 @@ DEFINE_double(still_speed, 0.01, "speed considered to be still");
 DEFINE_string(vehicle_model_file,
               "modules/prediction/data/mlp_vehicle_model.bin",
               "Vehicle model file");
-DEFINE_int32(max_num_obstacles_stored, 100,
+DEFINE_int32(max_num_obstacles, 100,
              "maximal number of obstacles stored in obstacles container.");
 
 // Obstacle trajectory

modules/prediction/common/prediction_gflags.h

@@ -29,6 +29,9 @@ DECLARE_double(prediction_freq);
 DECLARE_double(double_precision);
 DECLARE_double(min_prediction_length);
 
+// Bag replay timestamp gap
+DECLARE_double(replay_timestamp_gap);
+
 // Map
 DECLARE_double(search_radius);
 
@@ -57,7 +60,7 @@ DECLARE_double(prediction_pedestrian_total_time);
 DECLARE_int32(num_trajectory_still_pedestrian);
 DECLARE_double(still_speed);
 DECLARE_string(vehicle_model_file);
-DECLARE_int32(max_num_obstacles_stored);
+DECLARE_int32(max_num_obstacles);
 
 // Obstacle trajectory
 DECLARE_double(lane_sequence_threshold);

modules/prediction/container/obstacles/obstacles_container.cc

@@ -30,19 +30,21 @@ using apollo::perception::PerceptionObstacles;
 std::mutex ObstaclesContainer::g_mutex_;
 
 ObstaclesContainer::ObstaclesContainer()
-    : obstacles_(FLAGS_max_num_obstacles_stored) {}
+    : obstacles_(FLAGS_max_num_obstacles) {}
 
 void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   ADEBUG << "message: " << message.ShortDebugString();
   const PerceptionObstacles& perception_obstacles =
       dynamic_cast<const PerceptionObstacles&>(message);
-  ADEBUG << "perception obstacles: " << perception_obstacles.ShortDebugString();
   double timestamp = 0.0;
   if (perception_obstacles.has_header() &&
       perception_obstacles.header().has_timestamp_sec()) {
     timestamp = perception_obstacles.header().timestamp_sec();
   }
-  if (timestamp < timestamp_) {
+  if (timestamp <= timestamp_ - FLAGS_replay_timestamp_gap) {
+    obstacles_.Clear();
+    ADEBUG << "Replay mode is enabled.";
+  } else if (timestamp <= timestamp_) {
     AERROR << "Invalid timestamp curr [" << timestamp << "] v.s. prev ["
            << timestamp_ << "].";
     return;
@@ -52,11 +54,11 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   ADEBUG << "Current timestamp is [" << timestamp_ << "]";
   for (const PerceptionObstacle& perception_obstacle :
        perception_obstacles.perception_obstacle()) {
-    ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "]"
-           << " was detected";
+    ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "] "
+           << "was detected";
     InsertPerceptionObstacle(perception_obstacle, timestamp_);
-    ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "]"
-           << " was inserted";
+    ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "] "
+           << "was inserted";
   }
 }
 
@@ -64,6 +66,11 @@ Obstacle* ObstaclesContainer::GetObstacle(const int id) {
   return obstacles_.GetSilently(id);
 }
 
+void ObstaclesContainer::Clear() {
+  obstacles_.Clear();
+  timestamp_ = -1.0;
+}
+
 void ObstaclesContainer::InsertPerceptionObstacle(
     const PerceptionObstacle& perception_obstacle, const double timestamp) {
   std::lock_guard<std::mutex> lock(g_mutex_);

modules/prediction/container/obstacles/obstacles_container.h

@@ -66,6 +66,11 @@ class ObstaclesContainer : public Container {
    */
   Obstacle* GetObstacle(const int id);
 
+  /**
+   * @brief Clear obstacle container
+   */
+  void Clear();
+
  private:
   /**
    * @brief Check if an obstacle is predictable
@@ -75,7 +80,7 @@ class ObstaclesContainer : public Container {
   bool IsPredictable(const perception::PerceptionObstacle& perception_obstacle);
 
  private:
-  double timestamp_ = 0.0;
+  double timestamp_ = -1.0;
   common::util::LRUCache<int, Obstacle> obstacles_;
   static std::mutex g_mutex_;
 };

modules/prediction/container/obstacles/obstacles_container_test.cc

@@ -81,5 +81,15 @@ TEST_F(ObstaclesContainerTest, Pedestrian) {
   EXPECT_TRUE(obstacle_ptr103 == nullptr);
 }
 
+TEST_F(ObstaclesContainerTest, ClearAll) {
+  container_.Clear();
+  EXPECT_TRUE(container_.GetObstacle(0) == nullptr);
+  EXPECT_TRUE(container_.GetObstacle(1) == nullptr);
+  EXPECT_TRUE(container_.GetObstacle(2) == nullptr);
+  EXPECT_TRUE(container_.GetObstacle(3) == nullptr);
+  EXPECT_TRUE(container_.GetObstacle(101) == nullptr);
+  EXPECT_TRUE(container_.GetObstacle(102) == nullptr);
+}
+
 }  // namespace prediction
 }  // namespace apollo