Prediction: move callback functions outside prediction component for more flexible usage

modules/prediction/BUILD

@@ -14,6 +14,7 @@ cc_library(
     deps = [
         "//cyber/common:file",
         "//modules/common/adapters:adapter_gflags",
+        "//modules/prediction/common:message_process",
         "//modules/prediction/evaluator:evaluator_manager",
         "//modules/prediction/predictor:predictor_manager",
         "//modules/prediction/proto:offline_features_proto",

modules/prediction/common/BUILD

@@ -183,4 +183,20 @@ cc_test(
     ],
 )
 
+cc_library(
+    name = "message_process",
+    srcs = ["message_process.cc"],
+    hdrs = [
+        "message_process.h",
+    ],
+    deps = [
+        "//modules/common/adapters:adapter_gflags",
+        "//modules/prediction/evaluator:evaluator_manager",
+        "//modules/prediction/predictor:predictor_manager",
+        "//modules/prediction/proto:offline_features_proto",
+        "//modules/prediction/scenario:scenario_manager",
+        "//modules/prediction/util:data_extraction",
+    ],
+)
+
 cpplint()

modules/prediction/common/message_process.cc

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/prediction/common/message_process.h"
+
+#include <algorithm>
+#include <vector>
+
+#include "modules/common/adapters/adapter_gflags.h"
+
+#include "modules/prediction/common/feature_output.h"
+#include "modules/prediction/common/junction_analyzer.h"
+#include "modules/prediction/common/prediction_gflags.h"
+#include "modules/prediction/common/prediction_system_gflags.h"
+#include "modules/prediction/common/validation_checker.h"
+#include "modules/prediction/evaluator/evaluator_manager.h"
+#include "modules/prediction/predictor/predictor_manager.h"
+#include "modules/prediction/proto/offline_features.pb.h"
+#include "modules/prediction/scenario/scenario_manager.h"
+#include "modules/prediction/util/data_extraction.h"
+
+namespace apollo {
+namespace prediction {
+
+using apollo::common::adapter::AdapterConfig;
+using apollo::localization::LocalizationEstimate;
+using apollo::perception::PerceptionObstacle;
+using apollo::perception::PerceptionObstacles;
+using apollo::planning::ADCTrajectory;
+
+void MessageProcess::OnPerception(
+    const perception::PerceptionObstacles &perception_obstacles,
+    PredictionObstacles* const prediction_obstacles) {
+  // Insert obstacle
+  auto end_time1 = std::chrono::system_clock::now();
+  auto ptr_obstacles_container = ContainerManager::Instance()->GetContainer<
+      ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
+  CHECK(ptr_obstacles_container != nullptr);
+
+  ptr_obstacles_container->Insert(perception_obstacles);
+  auto end_time2 = std::chrono::system_clock::now();
+  std::chrono::duration<double> diff = end_time2 - end_time1;
+  ADEBUG << "Time to insert obstacles: "
+         << diff.count() * 1000 << " msec.";
+
+  // Scenario analysis
+  ScenarioManager::Instance()->Run();
+  auto end_time3 = std::chrono::system_clock::now();
+  diff = end_time3 - end_time2;
+  ADEBUG << "Time for scenario_manager: "
+         << diff.count() * 1000 << " msec.";
+
+  // If in junction, BuildJunctionFeature();
+  // If not, BuildLaneGraph().
+  const Scenario& scenario = ScenarioManager::Instance()->scenario();
+  if (scenario.type() == Scenario::JUNCTION && scenario.has_junction_id() &&
+      FLAGS_enable_junction_feature) {
+    JunctionAnalyzer::Init(scenario.junction_id());
+    ptr_obstacles_container->BuildJunctionFeature();
+  }
+  auto end_time4 = std::chrono::system_clock::now();
+  diff = end_time4 - end_time3;
+  ADEBUG << "Time to build junction features: "
+         << diff.count() * 1000 << " msec.";
+  ptr_obstacles_container->BuildLaneGraph();
+  auto end_time5 = std::chrono::system_clock::now();
+  diff = end_time5 - end_time4;
+  ADEBUG << "Time to build cruise features: "
+         << diff.count() * 1000 << " msec.";
+  ADEBUG << "Received a perception message ["
+         << perception_obstacles.ShortDebugString() << "].";
+
+  // Insert ADC into the obstacle_container as well.
+  auto ptr_ego_pose_container = ContainerManager::Instance()->GetContainer<
+      PoseContainer>(AdapterConfig::LOCALIZATION);
+  auto ptr_ego_trajectory_container =
+      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
+          AdapterConfig::PLANNING_TRAJECTORY);
+  CHECK(ptr_ego_pose_container != nullptr &&
+      ptr_ego_trajectory_container != nullptr);
+  const PerceptionObstacle* ptr_ego_vehicle =
+      ptr_ego_pose_container->ToPerceptionObstacle();
+  if (ptr_ego_vehicle != nullptr) {
+    ptr_obstacles_container->InsertPerceptionObstacle(*ptr_ego_vehicle,
+        ptr_ego_vehicle->timestamp());
+    double x = ptr_ego_vehicle->position().x();
+    double y = ptr_ego_vehicle->position().y();
+    ADEBUG << "Get ADC position [" << std::fixed << std::setprecision(6) << x
+              << ", " << std::fixed << std::setprecision(6) << y << "].";
+    ptr_ego_trajectory_container->SetPosition({x, y});
+  }
+  auto end_time6 = std::chrono::system_clock::now();
+
+  // Insert features to FeatureOutput for offline_mode
+  if (FLAGS_prediction_offline_mode) {
+    for (const int id :
+        ptr_obstacles_container->curr_frame_predictable_obstacle_ids()) {
+      Obstacle* obstacle_ptr = ptr_obstacles_container->GetObstacle(id);
+      if (obstacle_ptr == nullptr) {
+        AERROR << "Null obstacle found.";
+        continue;
+      } else if (!obstacle_ptr->latest_feature().IsInitialized()) {
+        AERROR << "Obstacle [" << id << "] has no latest feature.";
+        return;
+      }
+      FeatureOutput::Insert(obstacle_ptr->latest_feature());
+      ADEBUG << "Insert feature into feature output";
+    }
+    // Not doing evaluation on offline mode
+    return;
+  }
+
+  // Make evaluations
+  EvaluatorManager::Instance()->Run();
+  auto end_time7 = std::chrono::system_clock::now();
+  diff = end_time7 - end_time6;
+  ADEBUG << "Time to evaluate: "
+        << diff.count() * 1000 << " msec.";
+
+  // Make predictions
+  PredictorManager::Instance()->Run();
+  auto end_time8 = std::chrono::system_clock::now();
+  diff = end_time8 - end_time7;
+  ADEBUG << "Time to predict: "
+        << diff.count() * 1000 << " msec.";
+
+  // Get predicted obstacles
+  *prediction_obstacles =
+      PredictorManager::Instance()->prediction_obstacles();
+}
+
+void MessageProcess::OnLocalization(
+    const localization::LocalizationEstimate &localization) {
+  auto ptr_ego_pose_container =
+      ContainerManager::Instance()->GetContainer<PoseContainer>(
+          AdapterConfig::LOCALIZATION);
+  CHECK(ptr_ego_pose_container != nullptr);
+  ptr_ego_pose_container->Insert(localization);
+
+  ADEBUG << "Received a localization message ["
+         << localization.ShortDebugString() << "].";
+}
+
+void MessageProcess::OnPlanning(
+    const planning::ADCTrajectory &adc_trajectory) {
+  auto ptr_ego_trajectory_container =
+      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
+          AdapterConfig::PLANNING_TRAJECTORY);
+  CHECK(ptr_ego_trajectory_container != nullptr);
+  ptr_ego_trajectory_container->Insert(adc_trajectory);
+
+  ADEBUG << "Received a planning message ["
+         << adc_trajectory.ShortDebugString() << "].";
+}
+
+}  // namespace prediction
+}  // namespace apollo

modules/prediction/common/message_process.h

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include "modules/localization/proto/localization.pb.h"
+#include "modules/planning/proto/planning.pb.h"
+#include "modules/prediction/proto/prediction_obstacle.pb.h"
+
+namespace apollo {
+namespace prediction {
+
+class MessageProcess {
+ public:
+  static void OnPerception(
+      const perception::PerceptionObstacles &perception_obstacles,
+      PredictionObstacles* const prediction_obstacles);
+
+  static void OnLocalization(
+      const localization::LocalizationEstimate &localization);
+
+  static void OnPlanning(const planning::ADCTrajectory &adc_trajectory);
+};
+
+}  // namespace prediction
+}  // namespace apollo

modules/prediction/prediction_component.cc

@@ -60,17 +60,19 @@ void PredictionComponent::ProcessOfflineData(const std::string& filename) {
     if (message.channel_name == FLAGS_perception_obstacle_topic) {
       PerceptionObstacles perception_obstacles;
       if (perception_obstacles.ParseFromString(message.content)) {
-        OnPerception(perception_obstacles);
+        PredictionObstacles prediction_obstacles;
+        MessageProcess::OnPerception(
+            perception_obstacles, &prediction_obstacles);
       }
     } else if (message.channel_name == FLAGS_localization_topic) {
       LocalizationEstimate localization;
       if (localization.ParseFromString(message.content)) {
-        OnLocalization(localization);
+        MessageProcess::OnLocalization(localization);
       }
     } else if (message.channel_name == FLAGS_planning_trajectory_topic) {
       ADCTrajectory adc_trajectory;
       if (adc_trajectory.ParseFromString(message.content)) {
-        OnPlanning(adc_trajectory);
+        MessageProcess::OnPlanning(adc_trajectory);
       }
     }
   }
@@ -163,160 +165,6 @@ bool PredictionComponent::Init() {
   return true;
 }
 
-void PredictionComponent::OnLocalization(
-    const LocalizationEstimate& localization_msg) {
-  auto ptr_ego_pose_container =
-      ContainerManager::Instance()->GetContainer<PoseContainer>(
-          AdapterConfig::LOCALIZATION);
-  CHECK(ptr_ego_pose_container != nullptr);
-  ptr_ego_pose_container->Insert(localization_msg);
-
-  ADEBUG << "Received a localization message ["
-         << localization_msg.ShortDebugString() << "].";
-}
-
-void PredictionComponent::OnPlanning(
-    const planning::ADCTrajectory& planning_msg) {
-  auto ptr_ego_trajectory_container =
-      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
-          AdapterConfig::PLANNING_TRAJECTORY);
-  CHECK(ptr_ego_trajectory_container != nullptr);
-  ptr_ego_trajectory_container->Insert(planning_msg);
-
-  ADEBUG << "Received a planning message ["
-         << planning_msg.ShortDebugString() << "].";
-}
-
-void PredictionComponent::OnPerception(
-    const PerceptionObstacles& perception_msg) {
-  // Insert obstacle
-  auto end_time1 = std::chrono::system_clock::now();
-  auto ptr_obstacles_container = ContainerManager::Instance()->GetContainer<
-      ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
-  CHECK(ptr_obstacles_container != nullptr);
-
-  ptr_obstacles_container->Insert(perception_msg);
-  auto end_time2 = std::chrono::system_clock::now();
-  std::chrono::duration<double> diff = end_time2 - end_time1;
-  ADEBUG << "Time to insert obstacles: "
-         << diff.count() * 1000 << " msec.";
-
-  // Scenario analysis
-  ScenarioManager::Instance()->Run();
-  auto end_time3 = std::chrono::system_clock::now();
-  diff = end_time3 - end_time2;
-  ADEBUG << "Time for scenario_manager: "
-         << diff.count() * 1000 << " msec.";
-
-  // If in junction, BuildJunctionFeature();
-  // If not, BuildLaneGraph().
-  const Scenario& scenario = ScenarioManager::Instance()->scenario();
-  if (scenario.type() == Scenario::JUNCTION && scenario.has_junction_id() &&
-      FLAGS_enable_junction_feature) {
-    JunctionAnalyzer::Init(scenario.junction_id());
-    ptr_obstacles_container->BuildJunctionFeature();
-  }
-  auto end_time4 = std::chrono::system_clock::now();
-  diff = end_time4 - end_time3;
-  ADEBUG << "Time to build junction features: "
-         << diff.count() * 1000 << " msec.";
-  ptr_obstacles_container->BuildLaneGraph();
-  auto end_time5 = std::chrono::system_clock::now();
-  diff = end_time5 - end_time4;
-  ADEBUG << "Time to build cruise features: "
-         << diff.count() * 1000 << " msec.";
-  ADEBUG << "Received a perception message ["
-         << perception_msg.ShortDebugString() << "].";
-
-  // Insert ADC into the obstacle_container as well.
-  auto ptr_ego_pose_container = ContainerManager::Instance()->GetContainer<
-      PoseContainer>(AdapterConfig::LOCALIZATION);
-  auto ptr_ego_trajectory_container =
-      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
-          AdapterConfig::PLANNING_TRAJECTORY);
-  CHECK(ptr_ego_pose_container != nullptr &&
-      ptr_ego_trajectory_container != nullptr);
-  const PerceptionObstacle* ptr_ego_vehicle =
-      ptr_ego_pose_container->ToPerceptionObstacle();
-  if (ptr_ego_vehicle != nullptr) {
-    ptr_obstacles_container->InsertPerceptionObstacle(*ptr_ego_vehicle,
-        ptr_ego_vehicle->timestamp());
-    double x = ptr_ego_vehicle->position().x();
-    double y = ptr_ego_vehicle->position().y();
-    ADEBUG << "Get ADC position [" << std::fixed << std::setprecision(6) << x
-              << ", " << std::fixed << std::setprecision(6) << y << "].";
-    ptr_ego_trajectory_container->SetPosition({x, y});
-  }
-  auto end_time6 = std::chrono::system_clock::now();
-
-  // Insert features to FeatureOutput for offline_mode
-  if (FLAGS_prediction_offline_mode) {
-    for (const int id :
-        ptr_obstacles_container->curr_frame_predictable_obstacle_ids()) {
-      Obstacle* obstacle_ptr = ptr_obstacles_container->GetObstacle(id);
-      if (obstacle_ptr == nullptr) {
-        AERROR << "Null obstacle found.";
-        continue;
-      } else if (!obstacle_ptr->latest_feature().IsInitialized()) {
-        AERROR << "Obstacle [" << id << "] has no latest feature.";
-        return;
-      }
-      FeatureOutput::Insert(obstacle_ptr->latest_feature());
-      ADEBUG << "Insert feature into feature output";
-    }
-    // Not doing evaluation on offline mode
-    return;
-  }
-
-  // Make evaluations
-  EvaluatorManager::Instance()->Run();
-  auto end_time7 = std::chrono::system_clock::now();
-  diff = end_time7 - end_time6;
-  ADEBUG << "Time to evaluate: "
-        << diff.count() * 1000 << " msec.";
-
-  // Make predictions
-  PredictorManager::Instance()->Run();
-  auto end_time8 = std::chrono::system_clock::now();
-  diff = end_time8 - end_time7;
-  ADEBUG << "Time to predict: "
-        << diff.count() * 1000 << " msec.";
-
-  // Get predicted obstacles
-  auto prediction_obstacles =
-      PredictorManager::Instance()->prediction_obstacles();
-  prediction_obstacles.set_start_timestamp(frame_start_time_);
-  prediction_obstacles.set_end_timestamp(Clock::NowInSeconds());
-  prediction_obstacles.mutable_header()->set_lidar_timestamp(
-      perception_msg.header().lidar_timestamp());
-  prediction_obstacles.mutable_header()->set_camera_timestamp(
-      perception_msg.header().camera_timestamp());
-  prediction_obstacles.mutable_header()->set_radar_timestamp(
-      perception_msg.header().radar_timestamp());
-
-  prediction_obstacles.set_perception_error_code(perception_msg.error_code());
-
-  if (FLAGS_prediction_test_mode) {
-    for (auto const& prediction_obstacle :
-         prediction_obstacles.prediction_obstacle()) {
-      for (auto const& trajectory : prediction_obstacle.trajectory()) {
-        for (auto const& trajectory_point : trajectory.trajectory_point()) {
-          if (!ValidationChecker::ValidTrajectoryPoint(trajectory_point)) {
-            AERROR << "Invalid trajectory point ["
-                   << trajectory_point.ShortDebugString() << "]";
-            return;
-          }
-        }
-      }
-    }
-  }
-
-  // Publish output
-  common::util::FillHeader(node_->Name(), &prediction_obstacles);
-  prediction_writer_->Write(
-      std::make_shared<PredictionObstacles>(prediction_obstacles));
-}
-
 bool PredictionComponent::Proc(
     const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
   if (FLAGS_prediction_test_mode &&
@@ -344,7 +192,7 @@ bool PredictionComponent::Proc(
     return false;
   }
   auto localization_msg = *ptr_localization_msg;
-  OnLocalization(localization_msg);
+  MessageProcess::OnLocalization(localization_msg);
   auto end_time2 = std::chrono::system_clock::now();
   std::chrono::duration<double> diff = end_time2 - end_time1;
   ADEBUG << "Time for updating PoseContainer: "
@@ -356,7 +204,7 @@ bool PredictionComponent::Proc(
   auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
   if (ptr_trajectory_msg != nullptr) {
     auto trajectory_msg = *ptr_trajectory_msg;
-    OnPlanning(trajectory_msg);
+    MessageProcess::OnPlanning(trajectory_msg);
   }
   auto end_time3 = std::chrono::system_clock::now();
   diff = end_time3 - end_time2;
@@ -366,12 +214,44 @@ bool PredictionComponent::Proc(
   // Get all perception_obstacles of this frame and call OnPerception to
   // process them all.
   auto perception_msg = *perception_obstacles;
-  OnPerception(perception_msg);
+  PredictionObstacles prediction_obstacles;
+  MessageProcess::OnPerception(perception_msg, &prediction_obstacles);
   auto end_time4 = std::chrono::system_clock::now();
   diff = end_time4 - end_time3;
   ADEBUG << "Time for updating PerceptionContainer: "
         << diff.count() * 1000 << " msec.";
 
+  prediction_obstacles.set_start_timestamp(frame_start_time_);
+  prediction_obstacles.set_end_timestamp(Clock::NowInSeconds());
+  prediction_obstacles.mutable_header()->set_lidar_timestamp(
+      perception_msg.header().lidar_timestamp());
+  prediction_obstacles.mutable_header()->set_camera_timestamp(
+      perception_msg.header().camera_timestamp());
+  prediction_obstacles.mutable_header()->set_radar_timestamp(
+      perception_msg.header().radar_timestamp());
+
+  prediction_obstacles.set_perception_error_code(
+      perception_msg.error_code());
+
+  if (FLAGS_prediction_test_mode) {
+    for (auto const& prediction_obstacle :
+         prediction_obstacles.prediction_obstacle()) {
+      for (auto const& trajectory : prediction_obstacle.trajectory()) {
+        for (auto const& trajectory_point : trajectory.trajectory_point()) {
+          if (!ValidationChecker::ValidTrajectoryPoint(trajectory_point)) {
+            AERROR << "Invalid trajectory point ["
+                   << trajectory_point.ShortDebugString() << "]";
+            break;
+          }
+        }
+      }
+    }
+  }
+
+  // Publish output
+  common::util::FillHeader(node_->Name(), &prediction_obstacles);
+  prediction_writer_->Write(
+      std::make_shared<PredictionObstacles>(prediction_obstacles));
   return true;
 }
 

modules/prediction/prediction_component.h

@@ -24,8 +24,7 @@
 #include <string>
 
 #include "cyber/component/component.h"
-#include "modules/planning/proto/planning.pb.h"
-#include "modules/prediction/proto/prediction_obstacle.pb.h"
+#include "modules/prediction/common/message_process.h"
 
 /**
  * @namespace apollo::prediction
@@ -68,13 +67,6 @@ class PredictionComponent
   void OfflineProcessFeatureProtoFile(const std::string& features_proto_file);
 
  private:
-  void OnLocalization(const localization::LocalizationEstimate &localization);
-
-  void OnPlanning(const planning::ADCTrajectory &adc_trajectory);
-
-  void OnPerception(
-      const perception::PerceptionObstacles &perception_obstacles);
-
   void ProcessOfflineData(const std::string &filename);
 
  private: