Planning: restructured path_data. TODO: transfer discretized path to frenet path when set.

modules/planning/common/path/BUILD

@@ -43,6 +43,7 @@ cc_library(
         ":frenet_frame_path",
         "//modules/planning/common:planning_gflags",
         "//modules/planning/math:double",
+        "//modules/planning/math:sl_analytic_transformation",
         "//modules/planning/reference_line",
     ],
 )

modules/planning/common/path/discretized_path.h

@@ -68,4 +68,4 @@ class DiscretizedPath {
 }  // namespace planning
 }  // namespace apollo
 
-#endif /* MODULES_PLANNING_COMMON_PATH_PATH_H_ */
+#endif  // MODULES_PLANNING_COMMON_PATH_PATH_H_

modules/planning/common/path/path_data.cc

@@ -28,6 +28,7 @@
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/common/planning_util.h"
 #include "modules/planning/math/double.h"
+#include "modules/planning/math/sl_analytic_transformation.h"
 
 namespace apollo {
 namespace planning {
@@ -39,8 +40,18 @@ void PathData::set_discretized_path(const DiscretizedPath &path) {
   discretized_path_ = path;
 }
 
-void PathData::set_frenet_path(const FrenetFramePath &frenet_path) {
+bool PathData::set_frenet_path(const FrenetFramePath &frenet_path) {
+  if (reference_line_ == nullptr) {
+    AERROR << "Should NOT set frenet path with reference line is nullptr. "
+              "Please set reference line first.";
+    return false;
+  }
   frenet_path_ = frenet_path;
+  if (!FrenetToCartesian(frenet_path_, &discretized_path_)) {
+    AERROR << "Fail to transfer frenet path to discretized path.";
+    return false;
+  }
+  return true;
 }
 
 void PathData::set_discretized_path(
@@ -57,6 +68,7 @@ const FrenetFramePath &PathData::frenet_frame_path() const {
 }
 
 void PathData::set_reference_line(const ReferenceLine *reference_line) {
+  // Clear();
   reference_line_ = reference_line;
 }
 
@@ -112,5 +124,50 @@ std::string PathData::DebugString() const {
       "]\n");
 }
 
+bool PathData::FrenetToCartesian(const FrenetFramePath &frenet_path,
+                                 DiscretizedPath *const discretized_path) {
+  DCHECK_NOTNULL(discretized_path);
+  std::vector<common::PathPoint> path_points;
+  for (const common::FrenetFramePoint &frenet_point : frenet_path.points()) {
+    common::SLPoint sl_point;
+    common::math::Vec2d cartesian_point;
+    sl_point.set_s(frenet_point.s());
+    sl_point.set_l(frenet_point.l());
+    if (!reference_line_->get_point_in_cartesian_frame(sl_point,
+                                                       &cartesian_point)) {
+      AERROR << "Fail to convert sl point to xy point";
+      return false;
+    }
+    ReferencePoint ref_point =
+        reference_line_->get_reference_point(frenet_point.s());
+    double theta = SLAnalyticTransformation::calculate_theta(
+        ref_point.heading(), ref_point.kappa(), frenet_point.l(),
+        frenet_point.dl());
+    double kappa = SLAnalyticTransformation::calculate_kappa(
+        ref_point.kappa(), ref_point.dkappa(), frenet_point.l(),
+        frenet_point.dl(), frenet_point.ddl());
+
+    common::PathPoint path_point;
+    path_point.set_x(cartesian_point.x());
+    path_point.set_y(cartesian_point.y());
+    path_point.set_z(0.0);
+    path_point.set_theta(theta);
+    path_point.set_kappa(kappa);
+    path_point.set_dkappa(0.0);
+    path_point.set_ddkappa(0.0);
+    path_point.set_s(0.0);
+
+    if (!path_points.empty()) {
+      common::math::Vec2d last(path_points.back().x(), path_points.back().y());
+      common::math::Vec2d current(path_point.x(), path_point.y());
+      double distance = (last - current).Length();
+      path_point.set_s(path_points.back().s() + distance);
+    }
+    path_points.push_back(std::move(path_point));
+  }
+  *discretized_path = DiscretizedPath(path_points);
+  return true;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/path/path_data.h

@@ -38,7 +38,7 @@ class PathData {
 
   void set_discretized_path(const std::vector<common::PathPoint> &path_points);
 
-  void set_frenet_path(const FrenetFramePath &frenet_path);
+  bool set_frenet_path(const FrenetFramePath &frenet_path);
 
   void set_reference_line(const ReferenceLine *reference_line);
 
@@ -61,6 +61,13 @@ class PathData {
   std::string DebugString() const;
 
  private:
+  /*
+   * convert frenet path to cartesian path by reference line
+   */
+  bool FrenetToCartesian(const FrenetFramePath &frenet_path,
+                         DiscretizedPath *const discretized_path);
+  bool CartesianToFrenet(const DiscretizedPath &discretized_path,
+                         FrenetFramePath *const frenet_path);
   const ReferenceLine *reference_line_ = nullptr;
   DiscretizedPath discretized_path_;
   FrenetFramePath frenet_path_;

modules/planning/optimizer/dp_poly_path/BUILD

@@ -23,7 +23,6 @@ cc_library(
         "//modules/planning/common:planning_gflags",
         "//modules/planning/common/path:path_data",
         "//modules/planning/common/speed:speed_data",
-        "//modules/planning/math:sl_analytic_transformation",
         "//modules/planning/math/curve1d:polynomial_curve1d",
         "//modules/planning/math/curve1d:quintic_polynomial_curve1d",
         "//modules/planning/proto:dp_poly_path_config_proto",

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -36,7 +36,6 @@
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
 #include "modules/planning/math/double.h"
-#include "modules/planning/math/sl_analytic_transformation.h"
 #include "modules/planning/optimizer/dp_poly_path/trajectory_cost.h"
 
 namespace apollo {
@@ -97,60 +96,8 @@ bool DPRoadGraph::FindPathTunnel(const common::TrajectoryPoint &init_point,
   }
   FrenetFramePath tunnel(frenet_path);
 
-  DiscretizedPath discretized_path;
-  if (!FrenetToCartesian(tunnel, &discretized_path)) {
-    AERROR << "Failed to convert dp graph tunnel to PathData";
-    return false;
-  }
   path_data->set_reference_line(&reference_line_);
   path_data->set_frenet_path(tunnel);
-  path_data->set_discretized_path(discretized_path);
-  return true;
-}
-
-bool DPRoadGraph::FrenetToCartesian(const FrenetFramePath &frenet_path,
-                                    DiscretizedPath *const discretized_path) {
-  DCHECK_NOTNULL(discretized_path);
-  // convert frenet path to cartesian path by reference line
-  std::vector<common::PathPoint> path_points;
-  for (const common::FrenetFramePoint &frenet_point : frenet_path.points()) {
-    common::SLPoint sl_point;
-    common::math::Vec2d cartesian_point;
-    sl_point.set_s(frenet_point.s());
-    sl_point.set_l(frenet_point.l());
-    if (!reference_line_.get_point_in_cartesian_frame(sl_point,
-                                                      &cartesian_point)) {
-      AERROR << "Fail to convert sl point to xy point";
-      return false;
-    }
-    ReferencePoint ref_point =
-        reference_line_.get_reference_point(frenet_point.s());
-    double theta = SLAnalyticTransformation::calculate_theta(
-        ref_point.heading(), ref_point.kappa(), frenet_point.l(),
-        frenet_point.dl());
-    double kappa = SLAnalyticTransformation::calculate_kappa(
-        ref_point.kappa(), ref_point.dkappa(), frenet_point.l(),
-        frenet_point.dl(), frenet_point.ddl());
-
-    common::PathPoint path_point;
-    path_point.set_x(cartesian_point.x());
-    path_point.set_y(cartesian_point.y());
-    path_point.set_z(0.0);
-    path_point.set_theta(theta);
-    path_point.set_kappa(kappa);
-    path_point.set_dkappa(0.0);
-    path_point.set_ddkappa(0.0);
-    path_point.set_s(0.0);
-
-    if (!path_points.empty()) {
-      common::math::Vec2d last(path_points.back().x(), path_points.back().y());
-      common::math::Vec2d current(path_point.x(), path_point.y());
-      double distance = (last - current).Length();
-      path_point.set_s(path_points.back().s() + distance);
-    }
-    path_points.push_back(std::move(path_point));
-  }
-  *discretized_path = DiscretizedPath(path_points);
   return true;
 }
 

modules/planning/optimizer/dp_poly_path/dp_road_graph.h

@@ -107,9 +107,6 @@ class DPRoadGraph {
       const std::vector<const PathObstacle *> &path_obstacles,
       std::vector<std::pair<std::string, ObjectDecisionType>> *const decisions);
 
-  bool FrenetToCartesian(const FrenetFramePath &frenet_path,
-                         DiscretizedPath *const discretized_path);
-
  private:
   DpPolyPathConfig config_;
   common::TrajectoryPoint init_point_;