Planning: added path reference evaluation to convert evenly dt distribution to...

Planning: added path reference evaluation to convert evenly dt distribution to evenly ds distribution.

modules/planning/tasks/deciders/path_reference_decider/path_reference_decider.cc

@@ -26,6 +26,7 @@
 #include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/math/box2d.h"
 #include "modules/common/math/line_segment2d.h"
+#include "modules/planning/common/path/discretized_path.h"
 #include "modules/planning/common/path_boundary.h"
 #include "modules/planning/common/planning_context.h"
 #include "modules/planning/proto/planning_config.pb.h"
@@ -34,6 +35,7 @@
 namespace apollo {
 namespace planning {
 
+using common::PathPoint;
 using common::SLPoint;
 using common::Status;
 using common::TrajectoryPoint;
@@ -68,6 +70,7 @@ Status PathReferenceDecider::Process(
   if (IsValidPathReference(path_reference, path_boundaries)) {
     // mark learning trajectory as path reference
     frame->set_learning_trajectory_valid(true);
+    ADEBUG << "valid learning trajectory";
   }
 
   return Status::OK();
@@ -175,5 +178,26 @@ void PathReferenceDecider::PathBoundToLineSegments(
   path_bound_segments.emplace_back(cur_right_bound_segments);
 }
 
+void PathReferenceDecider::EvaluatePathReference(
+    const PathBoundary *path_bound,
+    const std::vector<TrajectoryPoint> &path_reference,
+    std::vector<PathPoint> *evaluated_path_reference) {
+  const double start_s = path_bound->start_s();
+  const double delta_s = path_bound->delta_s();
+  const double path_reference_end_s = path_reference.back().path_point().s();
+  DiscretizedPath discrete_path_reference;
+  for (auto trajectory_point : path_reference) {
+    discrete_path_reference.emplace_back(trajectory_point.path_point());
+  }
+  for (size_t idx = 0; idx < path_bound->boundary().size(); ++idx) {
+    double cur_s = start_s + idx * delta_s;
+    if (cur_s > path_reference_end_s) {
+      break;
+    }
+    evaluated_path_reference->emplace_back(
+        discrete_path_reference.Evaluate(cur_s));
+  }
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/deciders/path_reference_decider/path_reference_decider.h

@@ -59,6 +59,18 @@ class PathReferenceDecider : public Task {
       const PathBoundary *path_bound,
       std::vector<std::vector<common::math::LineSegment2d>>
           &path_bound_segments);
+
+  /**
+   * @brief convert path points from evenly dt to evenly ds distribution
+   *
+   * @param path_bound
+   * @param path_reference
+   * @param evaluated_path_reference
+   */
+  void EvaluatePathReference(
+      const PathBoundary *path_bound,
+      const std::vector<common::TrajectoryPoint> &path_reference,
+      std::vector<common::PathPoint> *evaluated_path_reference);
 };
 
 }  // namespace planning