diff --git a/modules/planning/integration_tests/planning_test_base.cc b/modules/planning/integration_tests/planning_test_base.cc index b9232b6c706060d8a0213fbec6cff326751e3812..0023b6676d3dd5b68b167492b4d355ffcc153ba9 100644 --- a/modules/planning/integration_tests/planning_test_base.cc +++ b/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( diff --git a/modules/planning/integration_tests/planning_test_base.h b/modules/planning/integration_tests/planning_test_base.h index a5e9e9791527e1ce546954b25d060d0069bcb44d..e983e903183109c953ba86df5eef777923bce8c0 100644 --- a/modules/planning/integration_tests/planning_test_base.h +++ b/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 diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc index 3b6bc559386869ff3604ca01de2e705c387fbd73..77d2a2f5db451ed2d151600fe4a8f8d65ff11082 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc +++ b/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"); diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index 4db2dfea2e705f8272604ab7cc85f90ccbee16fc..67edf03b4cec8f29ae80595521f577e20a093877 100644 --- a/modules/planning/planning.cc +++ b/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"; diff --git a/modules/planning/planning.h b/modules/planning/planning.h index 370eae6973bb0b3e0fa221cf710aae02895b2534..4ceb8ae6ad4cba7f6480645bfbd4149ba00ba63a 100644 --- a/modules/planning/planning.h +++ b/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 planner_factory_;