remove sl_point input in path sampler

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -167,8 +167,7 @@ bool DpRoadGraph::generate_graph(const ReferenceLine &reference_line,
   }
   std::vector<std::vector<common::SLPoint>> points;
   PathSampler path_sampler(config_);
-  if (!path_sampler.sample(reference_line, init_point_, init_sl_point_,
-                           &points)) {
+  if (!path_sampler.sample(reference_line, init_point_, &points)) {
     AERROR << "Fail to sampling point with path sampler!";
     return false;
   }

modules/planning/optimizer/dp_poly_path/path_sampler.cc

@@ -39,9 +39,20 @@ PathSampler::PathSampler(const DpPolyPathConfig& config) : config_(config) {}
 bool PathSampler::sample(
     const ReferenceLine& reference_line,
     const ::apollo::common::TrajectoryPoint& init_point,
-    const ::apollo::common::SLPoint& init_sl_point,
     std::vector<std::vector<::apollo::common::SLPoint>>* const points) {
-  CHECK_NOTNULL(points);
+  if (!points) {
+    AERROR << "The provided points are null";
+    return false;
+  }
+  ::apollo::common::SLPoint init_sl_point;
+  common::math::Vec2d init_point_vec2d{init_point.path_point().x(),
+                                       init_point.path_point().y()};
+  if (!reference_line.get_point_in_frenet_frame(init_point_vec2d,
+                                                &init_sl_point)) {
+    AERROR << "Failed to get sl point from point "
+           << init_point_vec2d.DebugString();
+    return false;
+  }
   const double reference_line_length =
       reference_line.reference_map_line().accumulated_s().back();
   double level_distance =

modules/planning/optimizer/dp_poly_path/path_sampler.h

@@ -36,7 +36,6 @@ class PathSampler {
   ~PathSampler() = default;
   bool sample(const ReferenceLine &reference_line,
               const common::TrajectoryPoint &init_point,
-              const common::SLPoint &init_sl_point,
               std::vector<std::vector<common::SLPoint>> *const points);
 
  private:

modules/planning/optimizer/dp_poly_path/path_sampler_test.cc

@@ -39,49 +39,49 @@ class PathSamplerTest : public OptimizerTestBase {
   virtual void SetUp() {
     OptimizerTestBase::SetUp();
     sampled_points_.clear();
-    init_point_.mutable_path_point()->set_x(586392.84003);
-    init_point_.mutable_path_point()->set_y(4140673.01232);
-    init_sl_point_.set_s(0.0);
-    init_sl_point_.set_l(0.0);
+    init_point_.mutable_path_point()->set_x(pose_.position().x());
+    init_point_.mutable_path_point()->set_y(pose_.position().y());
+    init_point_.mutable_path_point()->set_z(0.0);
+    init_point_.set_a(0.0);
+    init_point_.set_v(0.0);
     reference_line_ = &(frame_->planning_data().reference_line());
   }
 
  protected:
   std::vector<std::vector<common::SLPoint>> sampled_points_;
   common::TrajectoryPoint init_point_;
-  common::SLPoint init_sl_point_;
   const ReferenceLine* reference_line_ = nullptr;
 };
 
 TEST_F(PathSamplerTest, sample_one_point) {
   dp_poly_path_config_.set_sample_points_num_each_level(1);
   PathSampler sampler(dp_poly_path_config_);
-  bool sample_result = sampler.sample(*reference_line_, init_point_,
-                                      init_sl_point_, &sampled_points_);
+  bool sample_result =
+      sampler.sample(*reference_line_, init_point_, &sampled_points_);
   EXPECT_TRUE(sample_result);
   // export_sl_points(sampled_points_, "/tmp/points.txt");
   ASSERT_EQ(8, sampled_points_.size());
   ASSERT_EQ(1, sampled_points_[0].size());
   ASSERT_EQ(1, sampled_points_[7].size());
-  EXPECT_FLOAT_EQ(64, sampled_points_[7][0].s());
+  EXPECT_FLOAT_EQ(70.419151, sampled_points_[7][0].s());
   EXPECT_FLOAT_EQ(0, sampled_points_[7][0].l());
 }
 
 TEST_F(PathSamplerTest, sample_three_points) {
   dp_poly_path_config_.set_sample_points_num_each_level(3);
   PathSampler sampler(dp_poly_path_config_);
-  bool sample_result = sampler.sample(*reference_line_, init_point_,
-                                      init_sl_point_, &sampled_points_);
+  bool sample_result =
+      sampler.sample(*reference_line_, init_point_, &sampled_points_);
   EXPECT_TRUE(sample_result);
   // export_sl_points(sampled_points_, "/tmp/points.txt");
   ASSERT_EQ(8, sampled_points_.size());
   ASSERT_EQ(3, sampled_points_[0].size());
   ASSERT_EQ(3, sampled_points_[7].size());
-  EXPECT_FLOAT_EQ(64, sampled_points_[7][0].s());
+  EXPECT_FLOAT_EQ(70.419151, sampled_points_[7][0].s());
   EXPECT_FLOAT_EQ(-0.5, sampled_points_[7][0].l());
-  EXPECT_FLOAT_EQ(64, sampled_points_[7][1].s());
+  EXPECT_FLOAT_EQ(70.419151, sampled_points_[7][1].s());
   EXPECT_FLOAT_EQ(0, sampled_points_[7][1].l());
-  EXPECT_FLOAT_EQ(64, sampled_points_[7][2].s());
+  EXPECT_FLOAT_EQ(70.419151, sampled_points_[7][2].s());
   EXPECT_FLOAT_EQ(0.5, sampled_points_[7][2].l());
 }