Prediction: avoid deep copy of perception in lego

modules/prediction/common/message_process.cc

@@ -236,8 +236,9 @@ void MessageProcess::OnPerception(
     return;
   }
   // Make predictions
-  PredictorManager::Instance()->Run(ptr_ego_trajectory_container,
-                                    ptr_obstacles_container);
+  PredictorManager::Instance()->Run(
+      perception_obstacles, ptr_ego_trajectory_container,
+      ptr_obstacles_container);
 
   // Get predicted obstacles
   *prediction_obstacles = PredictorManager::Instance()->prediction_obstacles();

modules/prediction/common/prediction_system_gflags.cc

@@ -81,3 +81,6 @@ DEFINE_string(container_submodule_name, "container_submodule",
               "Container submodule name");
 DEFINE_string(evaluator_submodule_name, "evaluator_submodule",
               "Evaluator submodule name");
+DEFINE_string(perception_obstacles_topic_name,
+              "/apollo/prediction/perception_obstacles",
+              "Internal topic of perception obstacles");

modules/prediction/common/prediction_system_gflags.h

@@ -50,3 +50,4 @@ DECLARE_string(adccontainer_topic_name);
 DECLARE_string(evaluator_topic_name);
 DECLARE_string(container_submodule_name);
 DECLARE_string(evaluator_submodule_name);
+DECLARE_string(perception_obstacles_topic_name);

modules/prediction/container/obstacles/obstacles_container.cc

@@ -50,11 +50,6 @@ ObstaclesContainer::ObstaclesContainer(const SubmoduleOutput& submodule_output)
   std::unique_ptr<Obstacle> ptr_ego_vehicle(new Obstacle(ego_vehicle));
   ptr_obstacles_.Put(ego_vehicle.id(), std::move(ptr_ego_vehicle));
 
-  for (const auto& perception_obstacle :
-       submodule_output.curr_frame_perception_obstacles()) {
-    int id = perception_obstacle.id();
-    curr_frame_id_perception_obstacle_map_[id] = perception_obstacle;
-  }
   curr_frame_movable_obstacle_ids_ =
       submodule_output.curr_frame_movable_obstacle_ids();
   curr_frame_unmovable_obstacle_ids_ =
@@ -68,7 +63,6 @@ void ObstaclesContainer::CleanUp() {
   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
@@ -184,13 +178,6 @@ void ObstaclesContainer::Clear() {
   timestamp_ = -1.0;
 }
 
-const PerceptionObstacle& ObstaclesContainer::GetPerceptionObstacle(
-    const int id) {
-  CHECK(curr_frame_id_perception_obstacle_map_.find(id) !=
-        curr_frame_id_perception_obstacle_map_.end());
-  return curr_frame_id_perception_obstacle_map_[id];
-}
-
 const std::vector<int>& ObstaclesContainer::curr_frame_movable_obstacle_ids() {
   return curr_frame_movable_obstacle_ids_;
 }
@@ -237,7 +224,6 @@ void ObstaclesContainer::InsertPerceptionObstacle(
     AERROR << "Invalid ID [" << id << "]";
     return;
   }
-  curr_frame_id_perception_obstacle_map_[id] = perception_obstacle;
   if (!IsMovable(perception_obstacle)) {
     ADEBUG << "Perception obstacle [" << perception_obstacle.id()
            << "] is unmovable.";
@@ -350,10 +336,7 @@ double ObstaclesContainer::timestamp() const { return timestamp_; }
 
 SubmoduleOutput ObstaclesContainer::GetSubmoduleOutput() {
   SubmoduleOutput container_output;
-  for (const auto& perception_obstacle_pair :
-       curr_frame_id_perception_obstacle_map_) {
-    int id = perception_obstacle_pair.first;
-    container_output.InsertPerceptionObstacle(perception_obstacle_pair.second);
+  for (int id : curr_frame_considered_obstacle_ids_) {
     Obstacle* obstacle = GetObstacle(id);
     if (obstacle == nullptr) {
       AERROR << "Nullptr found for obstacle [" << id << "]";

modules/prediction/container/obstacles/obstacles_container.h

@@ -151,8 +151,6 @@ class ObstaclesContainer : public Container {
   std::vector<int> curr_frame_movable_obstacle_ids_;
   std::vector<int> curr_frame_unmovable_obstacle_ids_;
   std::vector<int> curr_frame_considered_obstacle_ids_;
-  std::unordered_map<int, apollo::perception::PerceptionObstacle>
-      curr_frame_id_perception_obstacle_map_;
 };
 
 }  // namespace prediction

modules/prediction/dag/prediction_lego.dag

@@ -42,6 +42,12 @@ module_config {
             config_file_path: "/apollo/modules/prediction/conf/prediction_conf.pb.txt"
             flag_file_path: "/apollo/modules/prediction/conf/prediction.conf"
             readers: [
+                {
+                    channel: "/apollo/prediction/perception_obstacles"
+                    qos_profile: {
+                        depth : 1
+                    }
+                },
                 {
                     channel:"/apollo/prediction/adccontainer"
                     qos_profile: {

modules/prediction/prediction_component.cc

@@ -94,6 +94,9 @@ bool PredictionComponent::Init() {
   adc_container_writer_ = node_->CreateWriter<ADCTrajectoryContainer>(
       FLAGS_adccontainer_topic_name);
 
+  perception_obstacles_writer_ = node_->CreateWriter<PerceptionObstacles>(
+      FLAGS_perception_obstacles_topic_name);
+
   return true;
 }
 
@@ -107,7 +110,7 @@ bool PredictionComponent::Proc(
 
 bool PredictionComponent::ContainerSubmoduleProcess(
     const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
-  const double frame_start_time = Clock::NowInSeconds();
+  const double frame_start_time = apollo::cyber::Time::Now().ToNanosecond();
   // Read localization info. and call OnLocalization to update
   // the PoseContainer.
   localization_reader_->Observe();
@@ -119,6 +122,8 @@ bool PredictionComponent::ContainerSubmoduleProcess(
   auto localization_msg = *ptr_localization_msg;
   MessageProcess::OnLocalization(localization_msg);
 
+  perception::PerceptionObstacles perception_msg = *perception_obstacles;
+
   // Read planning info. of last frame and call OnPlanning to update
   // the ADCTrajectoryContainer
   planning_reader_->Observe();
@@ -141,14 +146,13 @@ bool PredictionComponent::ContainerSubmoduleProcess(
 
   SubmoduleOutput submodule_output =
       obstacles_container_ptr->GetSubmoduleOutput();
-  submodule_output.set_perception_header(perception_obstacles->header());
-  submodule_output.set_perception_error_code(
-      perception_obstacles->error_code());
   submodule_output.set_frame_start_time(frame_start_time);
   container_writer_->Write(std::make_shared<SubmoduleOutput>(submodule_output));
   ADCTrajectoryContainer adc_container = *adc_trajectory_container_ptr;
   adc_container_writer_->Write(
       std::make_shared<ADCTrajectoryContainer>(adc_container));
+  perception_obstacles_writer_->Write(std::make_shared<PerceptionObstacles>(
+      perception_msg));
   return true;
 }
 

modules/prediction/prediction_component.h

@@ -92,6 +92,9 @@ class PredictionComponent
   std::shared_ptr<cyber::Writer<SubmoduleOutput>> container_writer_;
 
   std::shared_ptr<cyber::Writer<ADCTrajectoryContainer>> adc_container_writer_;
+
+  std::shared_ptr<cyber::Writer<perception::PerceptionObstacles>>
+      perception_obstacles_writer_;
 };
 
 CYBER_REGISTER_COMPONENT(PredictionComponent)

modules/prediction/predictor/predictor_manager.cc

@@ -41,6 +41,7 @@ namespace apollo {
 namespace prediction {
 
 using apollo::common::adapter::AdapterConfig;
+using apollo::perception::PerceptionObstacles;
 using apollo::perception::PerceptionObstacle;
 using IdObstacleListMap = std::unordered_map<int, std::list<Obstacle*>>;
 using IdPredictionObstacleMap =
@@ -134,21 +135,27 @@ Predictor* PredictorManager::GetPredictor(
 }
 
 void PredictorManager::Run(
+    const PerceptionObstacles& perception_obstacles,
     const ADCTrajectoryContainer* adc_trajectory_container,
     ObstaclesContainer* obstacles_container) {
   prediction_obstacles_.Clear();
 
   if (FLAGS_enable_multi_thread) {
-    PredictObstaclesInParallel(adc_trajectory_container, obstacles_container);
+    PredictObstaclesInParallel(
+        perception_obstacles, adc_trajectory_container, obstacles_container);
   } else {
-    PredictObstacles(adc_trajectory_container, obstacles_container);
+    PredictObstacles(
+        perception_obstacles, adc_trajectory_container, obstacles_container);
   }
 }
 
 void PredictorManager::PredictObstacles(
+    const PerceptionObstacles& perception_obstacles,
     const ADCTrajectoryContainer* adc_trajectory_container,
     ObstaclesContainer* obstacles_container) {
-  for (const int id : obstacles_container->curr_frame_obstacle_ids()) {
+  for (const PerceptionObstacle& perception_obstacle :
+       perception_obstacles.perception_obstacle()) {
+    int id = perception_obstacle.id();
     if (id < 0) {
       ADEBUG << "The obstacle has invalid id [" << id << "].";
       continue;
@@ -157,8 +164,6 @@ void PredictorManager::PredictObstacles(
     PredictionObstacle prediction_obstacle;
     Obstacle* obstacle = obstacles_container->GetObstacle(id);
 
-    PerceptionObstacle perception_obstacle =
-        obstacles_container->GetPerceptionObstacle(id);
     // if obstacle == nullptr, that means obstacle is unmovable
     // Checkout the logic of unmovable in obstacle.cc
     if (obstacle != nullptr) {
@@ -180,17 +185,20 @@ void PredictorManager::PredictObstacles(
 }
 
 void PredictorManager::PredictObstaclesInParallel(
+    const PerceptionObstacles& perception_obstacles,
     const ADCTrajectoryContainer* adc_trajectory_container,
     ObstaclesContainer* obstacles_container) {
   IdPredictionObstacleMap id_prediction_obstacle_map;
-  for (int id : obstacles_container->curr_frame_obstacle_ids()) {
+  for (const PerceptionObstacle& perception_obstacle :
+       perception_obstacles.perception_obstacle()) {
+    int id = perception_obstacle.id();
     id_prediction_obstacle_map[id] = std::make_shared<PredictionObstacle>();
   }
   IdObstacleListMap id_obstacle_map;
-  for (int id : obstacles_container->curr_frame_obstacle_ids()) {
+  for (const auto& perception_obstacle :
+       perception_obstacles.perception_obstacle()) {
+    int id = perception_obstacle.id();
     Obstacle* obstacle = obstacles_container->GetObstacle(id);
-    const PerceptionObstacle& perception_obstacle =
-        obstacles_container->GetPerceptionObstacle(id);
     if (obstacle == nullptr) {
       std::shared_ptr<PredictionObstacle> prediction_obstacle_ptr =
           id_prediction_obstacle_map[id];
@@ -210,11 +218,14 @@ void PredictorManager::PredictObstaclesInParallel(
                           id_prediction_obstacle_map[id].get());
         }
       });
-  for (auto& item : id_prediction_obstacle_map) {
-    int id = item.first;
-    auto prediction_obstacle_ptr = item.second;
-    const PerceptionObstacle& perception_obstacle =
-        obstacles_container->GetPerceptionObstacle(id);
+  for (const PerceptionObstacle& perception_obstacle :
+       perception_obstacles.perception_obstacle()) {
+    int id = perception_obstacle.id();
+    auto prediction_obstacle_ptr = id_prediction_obstacle_map[id];
+    if (prediction_obstacle_ptr == nullptr) {
+      AERROR << "Prediction obstacle [" << id << "] not found.";
+      continue;
+    }
     prediction_obstacle_ptr->set_predicted_period(
         FLAGS_prediction_trajectory_time_length);
     prediction_obstacle_ptr->mutable_perception_obstacle()->CopyFrom(

modules/prediction/predictor/predictor_manager.h

@@ -59,7 +59,8 @@ class PredictorManager {
    * @param Adc trajectory container
    * @param Obstacles container
    */
-  void Run(const ADCTrajectoryContainer* adc_trajectory_container,
+  void Run(const apollo::perception::PerceptionObstacles& perception_obstacles,
+           const ADCTrajectoryContainer* adc_trajectory_container,
            ObstaclesContainer* obstacles_container);
 
   /**
@@ -100,10 +101,13 @@ class PredictorManager {
    */
   void RegisterPredictors();
 
-  void PredictObstacles(const ADCTrajectoryContainer* adc_trajectory_container,
-                        ObstaclesContainer* obstacles_container);
+  void PredictObstacles(
+      const apollo::perception::PerceptionObstacles& perception_obstacles,
+      const ADCTrajectoryContainer* adc_trajectory_container,
+      ObstaclesContainer* obstacles_container);
 
   void PredictObstaclesInParallel(
+      const apollo::perception::PerceptionObstacles& perception_obstacles,
       const ADCTrajectoryContainer* adc_trajectory_container,
       ObstaclesContainer* obstacles_container);
 

modules/prediction/predictor/predictor_manager_test.cc

@@ -65,8 +65,9 @@ TEST_F(PredictorManagerTest, General) {
           AdapterConfig::PLANNING_TRAJECTORY);
 
   EvaluatorManager::Instance()->Run(obstacles_container);
-  PredictorManager::Instance()->Run(adc_trajectory_container,
-                                    obstacles_container);
+  PredictorManager::Instance()->Run(
+      perception_obstacles_, adc_trajectory_container,
+      obstacles_container);
 
   const PredictionObstacles& prediction_obstacles =
       PredictorManager::Instance()->prediction_obstacles();

modules/prediction/submodules/evaluator_submodule.cc

@@ -46,16 +46,10 @@ bool EvaluatorSubmodule::Init() {
 
 bool EvaluatorSubmodule::Proc(
     const std::shared_ptr<SubmoduleOutput>& container_output) {
-  const apollo::common::Header& perception_header =
-      container_output->perception_header();
-  const apollo::common::ErrorCode& perception_error_code =
-      container_output->perception_error_code();
   const double frame_start_time = container_output->frame_start_time();
   ObstaclesContainer obstacles_container(*container_output);
   EvaluatorManager::Instance()->Run(&obstacles_container);
   SubmoduleOutput submodule_output = obstacles_container.GetSubmoduleOutput();
-  submodule_output.set_perception_header(perception_header);
-  submodule_output.set_perception_error_code(perception_error_code);
   submodule_output.set_frame_start_time(frame_start_time);
   evaluator_writer_->Write(std::make_shared<SubmoduleOutput>(submodule_output));
   return true;

modules/prediction/submodules/predictor_submodule.cc

@@ -29,6 +29,7 @@ namespace apollo {
 namespace prediction {
 
 using apollo::common::time::Clock;
+using apollo::perception::PerceptionObstacles;
 
 PredictorSubmodule::~PredictorSubmodule() {}
 
@@ -46,16 +47,18 @@ bool PredictorSubmodule::Init() {
 }
 
 bool PredictorSubmodule::Proc(
+    const std::shared_ptr<PerceptionObstacles>& perception_obstacles,
     const std::shared_ptr<ADCTrajectoryContainer>& adc_trajectory_container,
     const std::shared_ptr<SubmoduleOutput>& submodule_output) {
   const apollo::common::Header& perception_header =
-      submodule_output->perception_header();
+      perception_obstacles->header();
   const apollo::common::ErrorCode& perception_error_code =
-      submodule_output->perception_error_code();
+      perception_obstacles->error_code();
   const double frame_start_time = submodule_output->frame_start_time();
   ObstaclesContainer obstacles_container(*submodule_output);
-  PredictorManager::Instance()->Run(adc_trajectory_container.get(),
-                                    &obstacles_container);
+  PredictorManager::Instance()->Run(
+      *perception_obstacles, adc_trajectory_container.get(),
+      &obstacles_container);
   PredictionObstacles prediction_obstacles =
       PredictorManager::Instance()->prediction_obstacles();
 
@@ -73,6 +76,10 @@ bool PredictorSubmodule::Proc(
   predictor_writer_->Write(
       std::make_shared<PredictionObstacles>(prediction_obstacles));
 
+  double end_time = apollo::cyber::Time::Now().ToNanosecond();
+  double start_time = submodule_output->frame_start_time();
+  ADEBUG << "End to end time = " << (end_time - start_time) / 1e6 << " ms";
+
   return true;
 }
 

modules/prediction/submodules/predictor_submodule.h

@@ -33,7 +33,9 @@ namespace apollo {
 namespace prediction {
 
 class PredictorSubmodule
-    : public cyber::Component<ADCTrajectoryContainer, SubmoduleOutput> {
+    : public cyber::Component<apollo::perception::PerceptionObstacles,
+                              ADCTrajectoryContainer,
+                              SubmoduleOutput> {
  public:
   /**
    * @brief Destructor
@@ -57,7 +59,8 @@ class PredictorSubmodule
    * @param Prediction adc trajectory container.
    * @param Prediction evaluator output.
    */
-  bool Proc(const std::shared_ptr<ADCTrajectoryContainer>&,
+  bool Proc(const std::shared_ptr<apollo::perception::PerceptionObstacles>&,
+            const std::shared_ptr<ADCTrajectoryContainer>&,
             const std::shared_ptr<SubmoduleOutput>&) override;
 
  private:

modules/prediction/submodules/submodule_output.cc

@@ -28,11 +28,6 @@ void SubmoduleOutput::InsertEgoVehicle(const Obstacle& ego_vehicle) {
   ego_vehicle_ = ego_vehicle;
 }
 
-void SubmoduleOutput::InsertPerceptionObstacle(
-    const apollo::perception::PerceptionObstacle& perception_obstacle) {
-  curr_frame_perception_obstacles_.push_back(perception_obstacle);
-}
-
 void SubmoduleOutput::set_curr_frame_movable_obstacle_ids(
     const std::vector<int>& curr_frame_movable_obstacle_ids) {
   curr_frame_movable_obstacle_ids_ = curr_frame_movable_obstacle_ids;
@@ -48,16 +43,6 @@ void SubmoduleOutput::set_curr_frame_considered_obstacle_ids(
   curr_frame_considered_obstacle_ids_ = curr_frame_considered_obstacle_ids;
 }
 
-void SubmoduleOutput::set_perception_header(
-    const apollo::common::Header& perception_header) {
-  perception_header_ = perception_header;
-}
-
-void SubmoduleOutput::set_perception_error_code(
-    const apollo::common::ErrorCode& perception_error_code) {
-  perception_error_code_ = perception_error_code;
-}
-
 void SubmoduleOutput::set_frame_start_time(const double frame_start_time) {
   frame_start_time_ = frame_start_time;
 }
@@ -68,11 +53,6 @@ const std::vector<Obstacle>& SubmoduleOutput::curr_frame_obstacles() const {
 
 const Obstacle& SubmoduleOutput::GetEgoVehicle() const { return ego_vehicle_; }
 
-const std::vector<apollo::perception::PerceptionObstacle>&
-SubmoduleOutput::curr_frame_perception_obstacles() const {
-  return curr_frame_perception_obstacles_;
-}
-
 std::vector<int> SubmoduleOutput::curr_frame_movable_obstacle_ids() const {
   return curr_frame_movable_obstacle_ids_;
 }
@@ -85,14 +65,6 @@ std::vector<int> SubmoduleOutput::curr_frame_considered_obstacle_ids() const {
   return curr_frame_considered_obstacle_ids_;
 }
 
-apollo::common::Header SubmoduleOutput::perception_header() const {
-  return perception_header_;
-}
-
-apollo::common::ErrorCode SubmoduleOutput::perception_error_code() const {
-  return perception_error_code_;
-}
-
 double SubmoduleOutput::frame_start_time() const { return frame_start_time_; }
 
 }  // namespace prediction

modules/prediction/submodules/submodule_output.h

@@ -46,9 +46,6 @@ class SubmoduleOutput {
 
   void InsertEgoVehicle(const Obstacle& ego_vehicle);
 
-  void InsertPerceptionObstacle(
-      const apollo::perception::PerceptionObstacle& perception_obstacle);
-
   void set_curr_frame_movable_obstacle_ids(
       const std::vector<int>& curr_frame_movable_obstacle_ids);
 
@@ -58,10 +55,6 @@ class SubmoduleOutput {
   void set_curr_frame_considered_obstacle_ids(
       const std::vector<int>& curr_frame_considered_obstacle_ids);
 
-  void set_perception_header(const apollo::common::Header& perception_header);
-
-  void set_perception_error_code(const apollo::common::ErrorCode&);
-
   void set_frame_start_time(const double frame_start_time);
 
   const std::vector<Obstacle>& curr_frame_obstacles() const;
@@ -77,22 +70,14 @@ class SubmoduleOutput {
 
   std::vector<int> curr_frame_considered_obstacle_ids() const;
 
-  apollo::common::Header perception_header() const;
-
-  apollo::common::ErrorCode perception_error_code() const;
-
   double frame_start_time() const;
 
  protected:
   std::vector<Obstacle> curr_frame_obstacles_;
   Obstacle ego_vehicle_;
-  std::vector<apollo::perception::PerceptionObstacle>
-      curr_frame_perception_obstacles_;
   std::vector<int> curr_frame_movable_obstacle_ids_;
   std::vector<int> curr_frame_unmovable_obstacle_ids_;
   std::vector<int> curr_frame_considered_obstacle_ids_;
-  apollo::common::Header perception_header_;
-  apollo::common::ErrorCode perception_error_code_;
   double frame_start_time_;
 };