planning: prepare planning_test_base for end-to-end planning integration tests

modules/planning/integration_tests/planning_test_base.cc

@@ -33,6 +33,7 @@ DEFINE_string(test_localization_file,
 DEFINE_string(test_chassis_file,
               "modules/planning/testdata/garage_chassis.pb.txt",
               "The chassis test file");
+DEFINE_string(test_prediction_file, "", "The prediction module test file");
 
 void PlanningTestBase::SetDataConfigs() {
   FLAGS_qp_spline_path_config_file =
@@ -82,35 +83,24 @@ bool PlanningTestBase::SetUpAdapters() {
     AERROR << "Failed to load chassis file: " << FLAGS_test_chassis_file;
     return false;
   }
-  AdapterManager::Observe();
+  if (!FLAGS_test_prediction_file.empty() &&
+      AdapterManager::FeedPredictionFile(FLAGS_test_prediction_file)) {
+    AERROR << "Failed to load prediction file: " << FLAGS_test_prediction_file;
+    return false;
+  }
   return true;
 }
 
 void PlanningTestBase::SetUp() {
   SetDataConfigs();
+  adc_trajectory_ = nullptr;
+  SetUpAdapters();
   planning_.Init();
-  if (!SetUpAdapters()) {
-    AERROR << "Failed to setup adapters";
-    return;
-  }
-  common::VehicleState::instance()->Update(
-      AdapterManager::GetLocalization()->GetLatestObserved(),
-      AdapterManager::GetChassis()->GetLatestObserved());
-  pose_ = common::VehicleState::instance()->pose();
-  if (!common::util::GetProtoFromFile(FLAGS_dp_poly_path_config_file,
-                                      &dp_poly_path_config_)) {
-    AERROR << "Failed to load file " << FLAGS_dp_poly_path_config_file;
-    return;
-  }
-  if (!common::util::GetProtoFromFile(FLAGS_dp_st_speed_config_file,
-                                      &dp_st_speed_config_)) {
-    AERROR << "Failed to load file " << FLAGS_dp_st_speed_config_file;
-    return;
-  }
-  if (!planning_.InitFrame(1)) {
-    AERROR << "planning init frame failed";
-    return;
-  }
+}
+
+void PlanningTestBase::RunPlanning() {
+  planning_.RunOnce();
+  adc_trajectory_ = AdapterManager::GetPlanning()->GetLatestPublished();
 }
 
 void PlanningTestBase::export_sl_points(

modules/planning/integration_tests/planning_test_base.h

@@ -37,6 +37,7 @@ using common::adapter::AdapterManager;
 DECLARE_string(test_routing_response_file);
 DECLARE_string(test_localization_file);
 DECLARE_string(test_chassis_file);
+DECLARE_string(test_prediction_file);
 
 class PlanningTestBase : public ::testing::Test {
  public:
@@ -56,6 +57,13 @@ class PlanningTestBase : public ::testing::Test {
   virtual void SetDataConfigs();
   virtual void SetUp();
 
+  /**
+   * Execute the planning code.
+   * @return true if planning is success. The ADCTrajectory will be used to
+   * store the planing results.  Otherwise false.
+   */
+  void RunPlanning();
+
   /**
    * @brief Print out the points to a file for debug and visualization purpose.
    * User can see the file, or feed it
@@ -74,6 +82,7 @@ class PlanningTestBase : public ::testing::Test {
   DpStSpeedConfig dp_st_speed_config_;
   localization::Pose pose_;
   Planning planning_;
+  ADCTrajectory* adc_trajectory_ = nullptr;
 };
 
 }  // namespace planning

modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc

@@ -67,6 +67,7 @@ class DpRoadGraphTest : public PlanningTestBase {
   virtual void SetUp() {
     google::InitGoogleLogging("DpRoadGraphTest");
     PlanningTestBase::SetUp();
+    RunPlanning();
     SetInitPoint();
     SetSpeedDataWithConstVeolocity(10.0);
     const auto* frame = planning_.GetFrame();
@@ -86,9 +87,9 @@ TEST_F(DpRoadGraphTest, speed_road_graph) {
   ASSERT_TRUE(reference_line_ != nullptr);
   bool result = road_graph.FindPathTunnel(init_point_, &path_data_);
   EXPECT_TRUE(result);
-  EXPECT_EQ(648, path_data_.discretized_path().num_of_points());
-  EXPECT_EQ(648, path_data_.frenet_frame_path().number_of_points());
-  EXPECT_FLOAT_EQ(70.450378,
+  EXPECT_EQ(765, path_data_.discretized_path().num_of_points());
+  EXPECT_EQ(765, path_data_.frenet_frame_path().number_of_points());
+  EXPECT_FLOAT_EQ(75.900002,
                   path_data_.frenet_frame_path().points().back().s());
   EXPECT_FLOAT_EQ(0.0, path_data_.frenet_frame_path().points().back().l());
   // export_path_data(path_data_, "/tmp/path.txt");

modules/planning/planning.cc

@@ -88,7 +88,9 @@ Status Planning::Init() {
         "failed to load planning config file: " + FLAGS_planning_config_file);
   }
 
-  AdapterManager::Init(FLAGS_adapter_config_path);
+  if (!AdapterManager::Initialized()) {
+    AdapterManager::Init(FLAGS_adapter_config_path);
+  }
   if (AdapterManager::GetLocalization() == nullptr) {
     std::string error_msg("Localization is not registered");
     AERROR << error_msg;
@@ -195,7 +197,8 @@ void Planning::RunOnce() {
   const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
   auto trajectory_pb = frame_->MutableADCTrajectory();
   trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
-  ADEBUG << "Planning latency: " << trajectory_pb->latency_stats().DebugString();
+  ADEBUG << "Planning latency: "
+         << trajectory_pb->latency_stats().DebugString();
 
   if (res_planning) {
     AdapterManager::FillPlanningHeader("planning", trajectory_pb);
@@ -211,10 +214,8 @@ void Planning::RunOnce() {
 
 void Planning::Stop() {}
 
-bool Planning::Plan(const bool is_on_auto_mode,
-                    const double current_time_stamp,
+bool Planning::Plan(const bool is_on_auto_mode, const double current_time_stamp,
                     const double planning_cycle_time) {
-
   const auto& stitching_trajectory =
       TrajectoryStitcher::compute_stitching_trajectory(
           is_on_auto_mode, current_time_stamp, planning_cycle_time,
@@ -223,8 +224,8 @@ bool Planning::Plan(const bool is_on_auto_mode,
   frame_->SetPlanningStartPoint(stitching_trajectory.back());
 
   PublishableTrajectory publishable_trajectory;
-  auto status = planner_->Plan(stitching_trajectory.back(),
-      frame_.get(), &publishable_trajectory);
+  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
+                               &publishable_trajectory);
 
   if (status != Status::OK()) {
     AERROR << "planner failed to make a driving plan";

modules/planning/planning.h

@@ -75,17 +75,17 @@ class Planning : public apollo::common::ApolloApp {
    * @brief Plan the trajectory given current vehicle state
    * @param is_on_auto_mode whether the current system is on auto-driving mode
    */
-  bool Plan(const bool is_on_auto_mode,
-            const double current_time_stamp,
+  bool Plan(const bool is_on_auto_mode, const double current_time_stamp,
             const double planning_cycle_time);
 
   const Frame* GetFrame() const;
   const hdmap::PncMap* GetPncMap() const;
   bool InitFrame(const uint32_t sequence_num);
 
+  void RunOnce();
+
  private:
   void RegisterPlanners();
-  void RunOnce();
 
   apollo::common::util::Factory<PlanningConfig::PlannerType, Planner>
       planner_factory_;