prediction: obstacle.cc refactor round2

modules/prediction/container/obstacles/obstacle.cc

@@ -52,18 +52,13 @@ bool IsClosed(const double x0, const double y0, const double theta0,
 
 }  // namespace
 
-Obstacle::Obstacle() {}
-
 PerceptionObstacle::Type Obstacle::type() const { return type_; }
 
 int Obstacle::id() const { return id_; }
 
 double Obstacle::timestamp() const {
-  if (feature_history_.size() > 0) {
-    return feature_history_.front().timestamp();
-  } else {
-    return 0.0;
-  }
+  CHECK(!feature_history_.empty());
+  return feature_history_.front().timestamp();
 }
 
 const Feature& Obstacle::feature(const size_t i) const {
@@ -72,6 +67,7 @@ const Feature& Obstacle::feature(const size_t i) const {
 }
 
 Feature* Obstacle::mutable_feature(const size_t i) {
+  CHECK(!feature_history_.empty());
   CHECK(i < feature_history_.size());
   return &feature_history_[i];
 }
@@ -132,28 +128,32 @@ bool Obstacle::IsNearJunction() {
                                      FLAGS_junction_search_radius);
 }
 
-void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
+bool Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
                       const double timestamp,
                       const int prediction_obstacle_id) {
-  if (feature_history_.size() > 0 &&
-      timestamp <= feature_history_.front().timestamp()) {
+  if (!perception_obstacle.has_id() || !perception_obstacle.has_type()) {
+    AERROR << "Perception obstacle has incomplete information; "
+        "skip insertion";
+    return false;
+  }
+
+  if (ReceivedNewerMessage(timestamp)) {
     AERROR << "Obstacle [" << id_ << "] received an older frame ["
            << std::setprecision(20) << timestamp
            << "] than the most recent timestamp [ "
-           << feature_history_.front().timestamp() << "].";
-    return;
+           << this->timestamp() << "].";
+    return false;
   }
 
   // Set ID, Type, and Status of the feature.
   Feature feature;
-  if (SetId(perception_obstacle, &feature, prediction_obstacle_id) ==
-      ErrorCode::PREDICTION_ERROR) {
-    return;
+  if (!SetId(perception_obstacle, &feature, prediction_obstacle_id)) {
+    return false;
   }
-  if (SetType(perception_obstacle, &feature) == ErrorCode::PREDICTION_ERROR) {
-    return;
+  if (!SetType(perception_obstacle, &feature)) {
+    return false;
   }
-  SetStatus(perception_obstacle, timestamp, &feature, prediction_obstacle_id);
+  SetStatus(perception_obstacle, timestamp, &feature);
 
   // Set obstacle observation for KF tracking
   if (!FLAGS_use_navigation_mode) {
@@ -197,10 +197,12 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
 
   // Trim historical features
   Trim();
+  return true;
 }
 
-void Obstacle::InsertFeature(const Feature& feature) {
+bool Obstacle::InsertFeature(const Feature& feature) {
   InsertFeatureToHistory(feature);
+  return true;
 }
 
 bool Obstacle::IsInJunction(const std::string& junction_id) {
@@ -303,13 +305,7 @@ void Obstacle::SetJunctionFeatureWithoutEnterLane(
 }
 
 void Obstacle::SetStatus(const PerceptionObstacle& perception_obstacle,
-                         const double timestamp, Feature* feature,
-                         int pred_id) {
-  if (SetId(perception_obstacle, feature, pred_id) ==
-      ErrorCode::PREDICTION_ERROR) {
-    AERROR << "Obstacle has no ID";
-    return;
-  }
+                         const double timestamp, Feature* feature) {
   SetTimestamp(perception_obstacle, timestamp, feature);
   SetPolygonPoints(perception_obstacle, feature);
   SetPosition(perception_obstacle, feature);
@@ -395,13 +391,8 @@ void Obstacle::UpdateStatus(Feature* feature) {
   }
 }
 
-ErrorCode Obstacle::SetId(const PerceptionObstacle& perception_obstacle,
-                          Feature* feature, int prediction_obstacle_id) {
-  if (!perception_obstacle.has_id()) {
-    AERROR << "Obstacle has no ID.";
-    return ErrorCode::PREDICTION_ERROR;
-  }
-
+bool Obstacle::SetId(const PerceptionObstacle& perception_obstacle,
+                     Feature* feature, int prediction_obstacle_id) {
   int id = prediction_obstacle_id > 0 ?
       prediction_obstacle_id : perception_obstacle.id();
   if (id_ < 0) {
@@ -411,24 +402,19 @@ ErrorCode Obstacle::SetId(const PerceptionObstacle& perception_obstacle,
     if (id_ != id) {
       AERROR << "Obstacle [" << id_ << "] has a mismatched ID [" << id
              << "] from perception obstacle.";
-      return ErrorCode::PREDICTION_ERROR;
+      return false;
     }
   }
   feature->set_id(id);
-  return ErrorCode::OK;
+  return true;
 }
 
-ErrorCode Obstacle::SetType(const PerceptionObstacle& perception_obstacle,
-                            Feature* feature) {
-  if (perception_obstacle.has_type()) {
-    type_ = perception_obstacle.type();
-    ADEBUG << "Obstacle [" << id_ << "] has type [" << type_ << "].";
-    feature->set_type(type_);
-  } else {
-    AERROR << "Obstacle [" << id_ << "] has no type.";
-    return ErrorCode::PREDICTION_ERROR;
-  }
-  return ErrorCode::OK;
+bool Obstacle::SetType(const PerceptionObstacle& perception_obstacle,
+                       Feature* feature) {
+  type_ = perception_obstacle.type();
+  ADEBUG << "Obstacle [" << id_ << "] has type [" << type_ << "].";
+  feature->set_type(type_);
+  return true;
 }
 
 void Obstacle::SetTimestamp(const PerceptionObstacle& perception_obstacle,
@@ -1355,10 +1341,7 @@ void Obstacle::SetLaneSequencePath(LaneGraph* const lane_graph) {
 void Obstacle::SetNearbyObstacles() {
   // This function runs after all basic features have been set up
   Feature* feature_ptr = mutable_latest_feature();
-  if (feature_ptr == nullptr) {
-    AERROR << "Null feature received.";
-    return;
-  }
+
   LaneGraph* lane_graph =
       feature_ptr->mutable_lane()->mutable_lane_graph();
   for (int i = 0; i < lane_graph->lane_sequence_size(); ++i) {
@@ -1452,7 +1435,7 @@ void Obstacle::SetMotionStatus() {
 }
 
 void Obstacle::SetMotionStatusBySpeed() {
-  int history_size = static_cast<int>(feature_history_.size());
+  auto history_size = feature_history_.size();
   if (history_size < 2) {
     ADEBUG << "Obstacle [" << id_ << "] has no history and "
            << "is considered moving.";
@@ -1479,6 +1462,30 @@ void Obstacle::InsertFeatureToHistory(const Feature& feature) {
   ADEBUG << "Obstacle [" << id_ << "] inserted a frame into the history.";
 }
 
+std::unique_ptr<Obstacle> Obstacle::Create(
+    const PerceptionObstacle& perception_obstacle,
+    const double timestamp, const int prediction_id) {
+  std::unique_ptr<Obstacle> ptr_obstacle(new Obstacle());
+  if (!ptr_obstacle->Insert(perception_obstacle, timestamp, prediction_id)) {
+    return nullptr;
+  }
+  return ptr_obstacle;
+}
+
+std::unique_ptr<Obstacle> Obstacle::Create(const Feature& feature) {
+  std::unique_ptr<Obstacle> ptr_obstacle(new Obstacle());
+  ptr_obstacle->InsertFeatureToHistory(feature);
+  return ptr_obstacle;
+}
+
+bool Obstacle::ReceivedNewerMessage(const double timestamp) const {
+  if (feature_history_.empty()) {
+    return false;
+  }
+  auto last_timestamp_received = feature_history_.front().timestamp();
+  return timestamp <= last_timestamp_received;
+}
+
 void Obstacle::Trim() {
   if (feature_history_.size() < 2) {
     return;

modules/prediction/container/obstacles/obstacle.h

@@ -47,7 +47,12 @@ class Obstacle {
   /**
    * @brief Constructor
    */
-  Obstacle();
+  static std::unique_ptr<Obstacle>
+  Create(const perception::PerceptionObstacle& perception_obstacle,
+      const double timestamp, const int prediction_id);
+
+  static std::unique_ptr<Obstacle>
+  Create(const Feature& feature);
 
   /**
    * @brief Destructor
@@ -59,14 +64,14 @@ class Obstacle {
    * @param perception_obstacle The obstacle from perception.
    * @param timestamp The timestamp when the perception obstacle was detected.
    */
-  void Insert(const perception::PerceptionObstacle& perception_obstacle,
+  bool Insert(const perception::PerceptionObstacle& perception_obstacle,
               const double timestamp, const int prediction_id);
 
   /**
    * @brief Insert a feature proto message.
    * @param feature proto message.
    */
-  void InsertFeature(const Feature& feature);
+  bool InsertFeature(const Feature& feature);
 
   /**
    * @brief Get the type of perception obstacle's type.
@@ -86,6 +91,8 @@ class Obstacle {
    */
   double timestamp() const;
 
+  bool ReceivedNewerMessage(const double timestamp) const;
+
   /**
    * @brief Get the ith feature from latest to earliest.
    * @param i The index of the feature.
@@ -219,18 +226,18 @@ class Obstacle {
   bool RNNEnabled() const;
 
  private:
+  Obstacle() = default;
+
   void SetStatus(const perception::PerceptionObstacle& perception_obstacle,
-                 double timestamp, Feature* feature, int prediction_id);
+                 double timestamp, Feature* feature);
 
   void UpdateStatus(Feature* feature);
 
-  common::ErrorCode SetId(
-      const perception::PerceptionObstacle& perception_obstacle,
-      Feature* feature, int prediction_id = -1);
+  bool SetId(const perception::PerceptionObstacle& perception_obstacle,
+             Feature* feature, int prediction_id = -1);
 
-  common::ErrorCode SetType(
-      const perception::PerceptionObstacle& perception_obstacle,
-      Feature* feature);
+  bool SetType(const perception::PerceptionObstacle& perception_obstacle,
+               Feature* feature);
 
   void SetIsNearJunction(
       const perception::PerceptionObstacle& perception_obstacle,

modules/prediction/container/obstacles/obstacles_container.cc

@@ -31,9 +31,8 @@ namespace prediction {
 using apollo::perception::PerceptionObstacle;
 using apollo::perception::PerceptionObstacles;
 
-
 ObstaclesContainer::ObstaclesContainer()
-    : obstacles_(FLAGS_max_num_obstacles),
+    : ptr_obstacles_(FLAGS_max_num_obstacles),
       id_mapping_(FLAGS_max_num_obstacles) {}
 
 // This is called by Perception module at every frame to insert all
@@ -58,7 +57,7 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
     timestamp = perception_obstacles.header().timestamp_sec();
   }
   if (std::fabs(timestamp - timestamp_) > FLAGS_replay_timestamp_gap) {
-    obstacles_.Clear();
+    ptr_obstacles_.Clear();
     ADEBUG << "Replay mode is enabled.";
   } else if (timestamp <= timestamp_) {
     AERROR << "Invalid timestamp curr [" << timestamp << "] v.s. prev ["
@@ -82,6 +81,7 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   // Set up the ObstacleClusters:
   // 1. Initialize ObstacleClusters
   ObstacleClusters::Init();
+
   // 2. Insert the Obstacles one by one
   for (const PerceptionObstacle& perception_obstacle :
        perception_obstacles.perception_obstacle()) {
@@ -109,7 +109,10 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
 
 
 Obstacle* ObstaclesContainer::GetObstacle(const int id) {
-  return obstacles_.GetSilently(id);
+  auto ptr_obstacle = ptr_obstacles_.GetSilently(id);
+  if (ptr_obstacle != nullptr)
+    return ptr_obstacles_.GetSilently(id)->get();
+  return nullptr;
 }
 
 
@@ -120,7 +123,7 @@ ObstaclesContainer::GetCurrentFramePredictableObstacleIds() const {
 
 
 void ObstaclesContainer::Clear() {
-  obstacles_.Clear();
+  ptr_obstacles_.Clear();
   id_mapping_.Clear();
   timestamp_ = -1.0;
 }
@@ -151,14 +154,18 @@ void ObstaclesContainer::InsertPerceptionObstacle(
 
   // Insert the obstacle and also update the LRUCache.
   curr_frame_predictable_obstacle_ids_.push_back(prediction_id);
-  Obstacle* obstacle_ptr = obstacles_.Get(prediction_id);
+  auto obstacle_ptr = GetObstacle(prediction_id);
   if (obstacle_ptr != nullptr) {
     obstacle_ptr->Insert(perception_obstacle, timestamp, prediction_id);
     ADEBUG << "Refresh obstacle [" << prediction_id << "]";
   } else {
-    Obstacle obstacle;
-    obstacle.Insert(perception_obstacle, timestamp, prediction_id);
-    obstacles_.Put(prediction_id, std::move(obstacle));
+    auto ptr_obstacle = Obstacle::Create(
+        perception_obstacle, timestamp, prediction_id);
+    if (ptr_obstacle == nullptr) {
+      AERROR << "Failed to insert obstacle into container";
+      return;
+    }
+    ptr_obstacles_.Put(prediction_id, std::move(ptr_obstacle));
     ADEBUG << "Insert obstacle [" << prediction_id << "]";
   }
 }
@@ -169,13 +176,16 @@ void ObstaclesContainer::InsertFeatureProto(const Feature& feature) {
     return;
   }
   int id = feature.id();
-  Obstacle* obstacle_ptr = obstacles_.Get(id);
+  auto obstacle_ptr = GetObstacle(id);
   if (obstacle_ptr != nullptr) {
     obstacle_ptr->InsertFeature(feature);
   } else {
-    Obstacle obstacle;
-    obstacle.InsertFeature(feature);
-    obstacles_.Put(id, std::move(obstacle));
+    auto ptr_obstacle = Obstacle::Create(feature);
+    if (ptr_obstacle == nullptr) {
+      AERROR << "Failed to insert obstacle into container";
+      return;
+    }
+    ptr_obstacles_.Put(id, std::move(ptr_obstacle));
   }
 }
 
@@ -213,7 +223,8 @@ void ObstaclesContainer::BuildCurrentFrameIdMapping(
         seen_prediction_ids.insert(prediction_id);
       }
     } else {  // process adaption
-      common::util::Node<int, Obstacle>* curr = obstacles_.First();
+      common::util::Node<int, std::unique_ptr<Obstacle>>*
+      curr = ptr_obstacles_.First();
       while (curr != nullptr) {
         int obs_id = curr->key;
         curr = curr->next;
@@ -223,7 +234,7 @@ void ObstaclesContainer::BuildCurrentFrameIdMapping(
           // this obs_id has already been processed
           continue;
         }
-        Obstacle* obstacle_ptr = obstacles_.GetSilently(obs_id);
+        Obstacle* obstacle_ptr = GetObstacle(obs_id);
         if (obstacle_ptr == nullptr) {
           AERROR << "Obstacle id [" << obs_id << "] with empty obstacle_ptr.";
           break;
@@ -247,7 +258,7 @@ void ObstaclesContainer::BuildLaneGraph() {
   // Go through every obstacle in the current frame, after some
   // sanity checks, build lane graph for non-junction cases.
   for (const int id : curr_frame_predictable_obstacle_ids_) {
-    Obstacle* obstacle_ptr = obstacles_.GetSilently(id);
+    Obstacle* obstacle_ptr = GetObstacle(id);
     if (obstacle_ptr == nullptr) {
       AERROR << "Null obstacle found.";
       continue;
@@ -265,7 +276,7 @@ void ObstaclesContainer::BuildJunctionFeature() {
   // Go through every obstacle in the current frame, after some
   // sanit checks, build junction features for those that are in junction.
   for (const int id : curr_frame_predictable_obstacle_ids_) {
-    Obstacle* obstacle_ptr = obstacles_.GetSilently(id);
+    Obstacle* obstacle_ptr = GetObstacle(id);
     if (obstacle_ptr == nullptr) {
       AERROR << "Null obstacle found.";
       continue;

modules/prediction/container/obstacles/obstacles_container.h

@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <memory>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -99,6 +100,8 @@ class ObstaclesContainer : public Container {
    */
   void Clear();
 
+  size_t NumOfObstacles() { return ptr_obstacles_.size(); }
+
  private:
   /**
    * @brief Check if a perception_obstacle is an old existed obstacle
@@ -118,7 +121,7 @@ class ObstaclesContainer : public Container {
 
  private:
   double timestamp_ = -1.0;
-  common::util::LRUCache<int, Obstacle> obstacles_;
+  common::util::LRUCache<int, std::unique_ptr<Obstacle>> ptr_obstacles_;
   // an id_mapping from perception_id to prediction_id
   common::util::LRUCache<int, int> id_mapping_;
   std::vector<int> curr_frame_predictable_obstacle_ids_;