From 4f6c428319ed7d0963b403d52c4f49f5a461a0a7 Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Tue, 8 Aug 2017 00:53:18 -0700 Subject: [PATCH] Planning: restructured path_data. TODO: transfer discretized path to frenet path when set. --- modules/planning/common/path/BUILD | 1 + .../planning/common/path/discretized_path.h | 2 +- modules/planning/common/path/path_data.cc | 59 ++++++++++++++++++- modules/planning/common/path/path_data.h | 9 ++- modules/planning/optimizer/dp_poly_path/BUILD | 1 - .../optimizer/dp_poly_path/dp_road_graph.cc | 53 ----------------- .../optimizer/dp_poly_path/dp_road_graph.h | 3 - 7 files changed, 68 insertions(+), 60 deletions(-) diff --git a/modules/planning/common/path/BUILD b/modules/planning/common/path/BUILD index 87ecde42fa..e805659f41 100644 --- a/modules/planning/common/path/BUILD +++ b/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", ], ) diff --git a/modules/planning/common/path/discretized_path.h b/modules/planning/common/path/discretized_path.h index 30428a27cd..c7a8141a2b 100644 --- a/modules/planning/common/path/discretized_path.h +++ b/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_ diff --git a/modules/planning/common/path/path_data.cc b/modules/planning/common/path/path_data.cc index 658ff1d36f..fe3b47d46c 100644 --- a/modules/planning/common/path/path_data.cc +++ b/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 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 diff --git a/modules/planning/common/path/path_data.h b/modules/planning/common/path/path_data.h index 93c51cee0b..6641bd60fe 100644 --- a/modules/planning/common/path/path_data.h +++ b/modules/planning/common/path/path_data.h @@ -38,7 +38,7 @@ class PathData { void set_discretized_path(const std::vector &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_; diff --git a/modules/planning/optimizer/dp_poly_path/BUILD b/modules/planning/optimizer/dp_poly_path/BUILD index 65be531ca0..5e13aee4df 100644 --- a/modules/planning/optimizer/dp_poly_path/BUILD +++ b/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", diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc index 43832f4268..638d3b63a0 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc +++ b/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 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; } diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.h b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h index 8f785d40fd..bd0b73d521 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.h +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.h @@ -107,9 +107,6 @@ class DPRoadGraph { const std::vector &path_obstacles, std::vector> *const decisions); - bool FrenetToCartesian(const FrenetFramePath &frenet_path, - DiscretizedPath *const discretized_path); - private: DpPolyPathConfig config_; common::TrajectoryPoint init_point_; -- GitLab