updated path sampler

modules/planning/optimizer/dp_poly_path/path_sampler.cc

@@ -24,6 +24,7 @@
 #include "modules/common/log.h"
 #include "modules/planning/math/double.h"
 #include "modules/planning/reference_line/reference_line.h"
+#include "modules/common/proto/path_point.pb.h"
 
 namespace apollo {
 namespace planning {
@@ -33,37 +34,46 @@ using apollo::common::Status;
 PathSampler::PathSampler(const DpPolyPathConfig &config) : _config(config) {}
 
 Status PathSampler::sample(
-    const ReferenceLine &reference_line,
-    const ::apollo::common::TrajectoryPoint &init_point,
-    const common::SLPoint &init_sl_point,
-    std::vector<std::vector<common::SLPoint>> *const points) {
+    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);
+  double reference_line_length = reference_line.reference_map_line().accumulated_s().back();
   double step_length = init_point.v();
   step_length = std::min(step_length, _config.step_length_max());
   step_length = std::max(step_length, _config.step_length_min());
   double center_l = init_sl_point.l();
   double accumulated_s = init_sl_point.s();
-  for (uint32_t i = 0; i < _config.sample_level(); ++i) {
-    std::vector<common::SLPoint> level_points;
-    if (center_l < _config.lateral_sample_offset()) {
+  for (size_t i = 0; i < _config.sample_level(); ++i) {
+    std::vector<::apollo::common::SLPoint> level_points;
+    if (std::abs(center_l) < _config.lateral_sample_offset()) {
       center_l = 0.0;
     } else {
       center_l = center_l * _config.lateral_adjust_coeff();
     }
     accumulated_s += step_length;
-    double level_start_l =
-        center_l -
-        _config.lateral_sample_offset() *
-            ((_config.sample_points_num_each_level() - 1) >> 1);
-    for (uint32_t j = 0; j < _config.sample_points_num_each_level(); ++j) {
-      // TODO(haoyang): the lateral value is incorrect
-
-      // TODO(haoyang): no checker no protection
-      common::SLPoint sl_point;
+    double level_start_l = center_l
+        - _config.lateral_sample_offset() * ((_config.sample_points_num_each_level() - 1) >> 1);
+    for (size_t j = 0; j < _config.sample_points_num_each_level(); ++j) {
+      ::apollo::common::SLPoint sl_point;
       sl_point.set_s(accumulated_s);
-      sl_point.set_l(level_start_l + _config.lateral_sample_offset());
+      sl_point.set_l(level_start_l + j * _config.lateral_sample_offset());
       if (reference_line.is_on_road(sl_point)) {
-        level_points.push_back(sl_point);
+        level_points.push_back(std::move(sl_point));
+      }
+    }
+    if (level_points.empty()) {
+      if (accumulated_s > reference_line_length) {
+        level_points.emplace_back(reference_line_length, -_config.lateral_sample_offset());
+        level_points.emplace_back(reference_line_length, 0.0);
+        level_points.emplace_back(reference_line_length, _config.lateral_sample_offset());
+        points->push_back(level_points);
+        return Status::OK();
+      } else {
+        level_points.emplace_back(accumulated_s, -_config.lateral_sample_offset());
+        level_points.emplace_back(accumulated_s, 0.0);
+        level_points.emplace_back(accumulated_s, _config.lateral_sample_offset());
       }
     }
     points->push_back(level_points);