planning: fix the length in path_data generated by DP algorithm

modules/planning/tasks/dp_poly_path/dp_road_graph.cc

@@ -72,8 +72,6 @@ bool DPRoadGraph::FindPathTunnel(
   double accumulated_s = init_sl_point_.s();
   const double path_resolution = config_.path_resolution();
 
-  constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
-
   for (std::size_t i = 1; i < min_cost_path.size(); ++i) {
     const auto &prev_node = min_cost_path[i - 1];
     const auto &cur_node = min_cost_path[i];
@@ -81,7 +79,7 @@ bool DPRoadGraph::FindPathTunnel(
     const double path_length = cur_node.sl_point.s() - prev_node.sl_point.s();
     double current_s = 0.0;
     const auto &curve = cur_node.min_cost_curve;
-    while (current_s + kEpsilon < path_length) {
+    while (current_s + path_resolution / 2.0 < path_length) {
       const double l = curve.Evaluate(0, current_s);
       const double dl = curve.Evaluate(1, current_s);
       const double ddl = curve.Evaluate(2, current_s);
@@ -93,7 +91,11 @@ bool DPRoadGraph::FindPathTunnel(
       frenet_path.push_back(std::move(frenet_frame_point));
       current_s += path_resolution;
     }
-    accumulated_s += path_length;
+    if (i == min_cost_path.size() - 1) {
+      accumulated_s += current_s;
+    } else {
+      accumulated_s += path_length;
+    }
   }
   FrenetFramePath tunnel(frenet_path);
   path_data->SetReferenceLine(&reference_line_);
@@ -161,25 +163,16 @@ bool DPRoadGraph::GenerateMinCostPath(
 bool DPRoadGraph::SamplePathWaypoints(
     const common::TrajectoryPoint &init_point,
     std::vector<std::vector<common::SLPoint>> *const points) {
+  constexpr double kSamplePointLookForwardTime = 1.0;
   CHECK(points != nullptr);
 
-  common::math::Vec2d init_cartesian_point(init_point.path_point().x(),
-                                           init_point.path_point().y());
-  common::SLPoint init_sl_point;
-  if (!reference_line_.XYToSL(init_cartesian_point, &init_sl_point)) {
-    AERROR << "Failed to get sl point from point "
-           << init_cartesian_point.DebugString();
-    return false;
-  }
-
-  const double reference_line_length =
-      reference_line_.map_path().accumulated_s().back();
+  const double reference_line_length = reference_line_.Length();
 
   double level_distance =
       std::fmax(config_.step_length_min(),
-                std::fmin(init_point.v(), config_.step_length_max()));
-
-  double accumulated_s = init_sl_point.s();
+                std::fmin(init_point.v() * kSamplePointLookForwardTime,
+                          config_.step_length_max()));
+  double accumulated_s = init_sl_point_.s();
   for (std::size_t i = 0; accumulated_s < reference_line_length; ++i) {
     std::vector<common::SLPoint> level_points;
     accumulated_s += level_distance;

modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt

@@ -1166,11 +1166,11 @@ decision {
         follow {
           distance_s: -10
           fence_point {
-            x: 587257.46692710358
-            y: 4140942.1245008437
+            x: 587257.49634319346
+            y: 4140942.223678661
             z: 0
           }
-          fence_heading: -1.860146918620373
+          fence_heading: -1.8586265241375961
         }
       }
     }