diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc index 5da5374873a419f25c1bf8706aae0eeb5dd870d5..1475d84ab52d272c289910d2e7bb4701ec3787cf 100644 --- a/modules/prediction/container/obstacles/obstacle.cc +++ b/modules/prediction/container/obstacles/obstacle.cc @@ -196,7 +196,7 @@ bool Obstacle::Insert(const PerceptionObstacle& perception_obstacle, } // Trim historical features - Trim(); + DiscardOutdatedHistory(); return true; } @@ -392,7 +392,7 @@ void Obstacle::UpdateStatus(Feature* feature) { } bool Obstacle::SetId(const PerceptionObstacle& perception_obstacle, - Feature* feature, int prediction_obstacle_id) { + Feature* feature, const int prediction_obstacle_id) { int id = prediction_obstacle_id > 0 ? prediction_obstacle_id : perception_obstacle.id(); if (id_ < 0) { @@ -1486,20 +1486,16 @@ bool Obstacle::ReceivedNewerMessage(const double timestamp) const { return timestamp <= last_timestamp_received; } -void Obstacle::Trim() { - if (feature_history_.size() < 2) { - return; - } - int count = 0; +void Obstacle::DiscardOutdatedHistory() { + auto num_of_frames = feature_history_.size(); const double latest_ts = feature_history_.front().timestamp(); - while (!feature_history_.empty() && - latest_ts - feature_history_.back().timestamp() >= + while (latest_ts - feature_history_.back().timestamp() >= FLAGS_max_history_time) { feature_history_.pop_back(); - ++count; } - if (count > 0) { - ADEBUG << "Obstacle [" << id_ << "] trimmed " << count + auto num_of_discarded_frames = num_of_frames - feature_history_.size(); + if (num_of_discarded_frames > 0) { + ADEBUG << "Obstacle [" << id_ << "] discards " << num_of_discarded_frames << " historical features"; } } diff --git a/modules/prediction/container/obstacles/obstacle.h b/modules/prediction/container/obstacles/obstacle.h index 4c4767807cda2c810108cee855a41e09efeb4be8..d566ef923f35dd440f8e3ca27c30b29ac8603317 100644 --- a/modules/prediction/container/obstacles/obstacle.h +++ b/modules/prediction/container/obstacles/obstacle.h @@ -234,7 +234,7 @@ class Obstacle { void UpdateStatus(Feature* feature); bool SetId(const perception::PerceptionObstacle& perception_obstacle, - Feature* feature, int prediction_id = -1); + Feature* feature, const int prediction_id = -1); bool SetType(const perception::PerceptionObstacle& perception_obstacle, Feature* feature); @@ -305,7 +305,7 @@ class Obstacle { void SetJunctionFeatureWithoutEnterLane(Feature* const feature_ptr); - void Trim(); + void DiscardOutdatedHistory(); private: int id_ = -1; diff --git a/modules/prediction/container/obstacles/obstacles_container.cc b/modules/prediction/container/obstacles/obstacles_container.cc index f528ead551ad2fe723e5c3fe49e30739dbed7000..38b17f7f6c97233fb01f4f95ffc96d169bfe3b8d 100644 --- a/modules/prediction/container/obstacles/obstacles_container.cc +++ b/modules/prediction/container/obstacles/obstacles_container.cc @@ -108,8 +108,8 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) { } -Obstacle* ObstaclesContainer::GetObstacle(const int id) { - auto ptr_obstacle = ptr_obstacles_.GetSilently(id); +Obstacle* ObstaclesContainer::GetObstacle(const int perception_id) { + auto ptr_obstacle = ptr_obstacles_.GetSilently(perception_id); if (ptr_obstacle != nullptr) { return ptr_obstacle->get(); }