update qp spline path optimizer test

modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer_test.cc

@@ -20,26 +20,113 @@
 #include "gtest/gtest.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h"
+#include "modules/common/util/file.h"
+#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
 
 namespace apollo {
 namespace planning {
 
+using PlanningPb = planning::ADCTrajectory;
+
 class QpSplinePathOptimizerTest : public ::testing::Test {
- public:
   virtual void SetUp() {
     FLAGS_reference_line_smoother_config_file =
         "modules/planning/testdata/conf/reference_line_smoother_config.pb.txt";
-    qp_spline_path_optimizer_.reset(new QpSplinePathOptimizer("QP_PATH"));
+
+    frame.reset(new Frame(1));
+    hdmap::PncMap* pnc_map;
+    pnc_map = new hdmap::PncMap("modules/planning/testdata/base_map.txt");
+    Frame::SetMap(pnc_map);
+
+    planning_pb = LoadPlanningPb(
+        "modules/planning/testdata"
+            "/qp_spline_path/42_apollo_planning.pb.txt");
+
+    frame->SetVehicleInitPose(
+        planning_pb.debug().planning_data().adc_position().pose());
+    frame->SetRoutingResponse(
+        planning_pb.debug().planning_data().routing());
   }
 
  protected:
-  std::unique_ptr<QpSplinePathOptimizer> qp_spline_path_optimizer_;
+  PlanningPb LoadPlanningPb(const std::string &filename) {
+    PlanningPb planning_pb;
+    CHECK(apollo::common::util::GetProtoFromFile(filename, &planning_pb))
+    << "Failed to open file " << filename;
+    return std::move(planning_pb);
+  }
+
+  common::TrajectoryPoint GetInitPoint() {
+    common::TrajectoryPoint init_point;
+    common::VehicleState::instance()->Update(
+        planning_pb.debug().planning_data().adc_position(),
+        planning_pb.debug().planning_data().chassis());
+    init_point.mutable_path_point()->set_x(
+        common::VehicleState::instance()->x());
+    init_point.mutable_path_point()->set_y(
+        common::VehicleState::instance()->y());
+    init_point.set_v(common::VehicleState::instance()->linear_velocity());
+    init_point.set_a(common::VehicleState::instance()->linear_acceleration());
+    init_point.mutable_path_point()->set_theta(
+        common::VehicleState::instance()->heading());
+    init_point.mutable_path_point()->set_kappa(
+        common::VehicleState::instance()->kappa());
+
+    init_point.set_relative_time(0.0);
+    return std::move(init_point);
+  }
+
+  double timestamp_ = 0.0;
+  std::unique_ptr<Frame> frame;
+  PlanningPb planning_pb;
 };
 
 TEST_F(QpSplinePathOptimizerTest, Process) {
-  Frame frame(1);
+  EXPECT_EQ(frame->Init(), true);
 
-  EXPECT_EQ(qp_spline_path_optimizer_.get()->name(), "QP_PATH");
+  common::TrajectoryPoint init_point = GetInitPoint();
+  frame->SetPlanningStartPoint(init_point);
+
+  PlanningData* planning_data = frame->mutable_planning_data();
+  EXPECT_EQ(planning_data->path_data().discretized_path().points().size(), 0);
+
+  QpSplinePathConfig qp_spline_path_config;
+  EXPECT_TRUE(common::util::GetProtoFromFile(
+      "modules/planning/testdata/qp_spline_path/qp_spline_path_config.pb.txt",
+      &qp_spline_path_config));
+
+  QpSplinePathGenerator path_generator(frame->reference_line(),
+                                       qp_spline_path_config);
+  EXPECT_TRUE(path_generator.Generate(
+      frame->path_decision()->path_obstacles().Items(),
+      planning_data->speed_data(),
+      frame->PlanningStartPoint(),
+      planning_data->mutable_path_data()));
+
+  common::Path qp_path_ground_truth;
+  for (common::Path path : planning_pb.debug().planning_data().path()) {
+    if (path.name() == "QP_SPLINE_PATH_OPTIMIZER") {
+      qp_path_ground_truth = path;
+    }
+  }
+
+  EXPECT_EQ(qp_path_ground_truth.path_point().size(), 101);
+  EXPECT_EQ(planning_data->path_data().discretized_path().points().size(), 101);
+
+  for (std::uint32_t i = 0; i < qp_path_ground_truth.path_point().size(); i++) {
+    common::PathPoint ground_truth_point =
+        qp_path_ground_truth.path_point().Get(i);
+    common::PathPoint computed_point = planning_data
+        ->path_data().discretized_path().points()[i];
+    EXPECT_NEAR(ground_truth_point.x(), computed_point.x(), 1.0e-3);
+    EXPECT_NEAR(ground_truth_point.y(), computed_point.y(), 1.0e-3);
+    EXPECT_NEAR(ground_truth_point.kappa(), computed_point.kappa(), 1.0e-3);
+    EXPECT_NEAR(ground_truth_point.theta(), computed_point.theta(), 1.0e-3);
+    EXPECT_NEAR(ground_truth_point.s(), computed_point.s(), 1.0e-3);
+    EXPECT_NEAR(ground_truth_point.dkappa(), computed_point.dkappa(), 1.0e-3);
+    EXPECT_NEAR(ground_truth_point.ddkappa(), computed_point.ddkappa(), 1.0e-3);
+  }
 }
 
 }  // namespace planning

modules/planning/testdata/conf/reference_line_smoother_config.pb.txt

@@ -9,3 +9,4 @@ ref_line_weight: 0.0
 derivative_weight: 0.0
 second_derivative_weight: 0.0
 third_derivative_weight: 100
+num_of_total_points: 500
\ No newline at end of file

modules/planning/testdata/qp_spline_path/qp_spline_path_config.pb.txt

+spline_order: 6
+number_of_knots: 5
+number_of_fx_constraint_knots: 13
+time_resolution: 0.1
+regularization_weight: 0.001
+derivative_weight: 1.0
+second_derivative_weight: 0.0
+third_derivative_weight: 0.5
+reference_line_weight: 0.0
+num_refline_point: 10
+num_output: 100