diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index 44e591a1c929acb41aa24011b3b6b3083e5666ec..1b307e9690db5699440295e71bd91531b7545080 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/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 diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 21268b7ac6e8af0686097479a1e114185c5608b0..f1364672c14dc20f019105b62b43b51be91237b1 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/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); diff --git a/modules/prediction/container/obstacles/obstacles_container.cc b/modules/prediction/container/obstacles/obstacles_container.cc index d56ff95aaa37a9e83f934c401732695d1d91c45a..6c2401280bcd439902605e0ccc6f953947babcae 100644 --- a/modules/prediction/container/obstacles/obstacles_container.cc +++ b/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(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 lock(g_mutex_); diff --git a/modules/prediction/container/obstacles/obstacles_container.h b/modules/prediction/container/obstacles/obstacles_container.h index 94cf95958eb12e514fdebc9b47446f7847dedb57..cf3878035f250b96b646250f77004f59fd496424 100644 --- a/modules/prediction/container/obstacles/obstacles_container.h +++ b/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 obstacles_; static std::mutex g_mutex_; }; diff --git a/modules/prediction/container/obstacles/obstacles_container_test.cc b/modules/prediction/container/obstacles/obstacles_container_test.cc index e3bcd66aa16a2c886df8cc2525fd96102c24f56f..8b1e13ec368a6410eb2a7eabc71df202f55aa6f7 100644 --- a/modules/prediction/container/obstacles/obstacles_container_test.cc +++ b/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