refactor path, discretized_path and frenet_frame_path

unify function names

modules/planning/common/path/discretized_path.cc

@@ -49,7 +49,7 @@ common::PathPoint DiscretizedPath::evaluate(const double param) const {
   return util::interpolate(*(it_lower - 1), *it_lower, param);
 }
 
-double DiscretizedPath::param_length() const {
+double DiscretizedPath::length() const {
   if (path_points_.empty()) {
     return 0.0;
   }
@@ -93,11 +93,7 @@ int DiscretizedPath::query_closest_point(const double param) const {
   }
 }
 
-std::vector<common::PathPoint> *DiscretizedPath::mutable_path_points() {
-  return &path_points_;
-}
-
-const std::vector<common::PathPoint> &DiscretizedPath::path_points() const {
+const std::vector<common::PathPoint> &DiscretizedPath::points() const {
   return path_points_;
 }
 

modules/planning/common/path/discretized_path.h

@@ -40,7 +40,7 @@ class DiscretizedPath : public Path {
 
   common::PathPoint evaluate(const double param) const override;
 
-  double param_length() const override;
+  double length() const override;
 
   common::PathPoint start_point() const override;
 
@@ -50,9 +50,7 @@ class DiscretizedPath : public Path {
 
   int query_closest_point(const double param) const;
 
-  std::vector<common::PathPoint> *mutable_path_points();
-
-  const std::vector<common::PathPoint> &path_points() const;
+  const std::vector<common::PathPoint> &points() const;
 
   std::uint32_t num_of_points() const;
 

modules/planning/common/path/frenet_frame_path.cc

@@ -32,19 +32,22 @@ FrenetFramePath::FrenetFramePath(
   points_ = std::move(sl_points);
 }
 
-void FrenetFramePath::set_frenet_points(
+void FrenetFramePath::set_points(
     const std::vector<common::FrenetFramePoint> &points) {
   points_ = points;
 }
 
-std::vector<common::FrenetFramePoint> *FrenetFramePath::mutable_points() {
-  return &points_;
-}
-
 const std::vector<common::FrenetFramePoint> &FrenetFramePath::points() const {
   return points_;
 }
 
+double FrenetFramePath::length() const {
+  if (points_.empty()) {
+    return 0.0;
+  }
+  return points_.back().s() - points_.front().s();
+}
+
 std::uint32_t FrenetFramePath::number_of_points() const {
   return points_.size();
 }

modules/planning/common/path/frenet_frame_path.h

@@ -34,10 +34,10 @@ class FrenetFramePath {
   explicit FrenetFramePath(std::vector<common::FrenetFramePoint> sl_points);
   virtual ~FrenetFramePath() = default;
 
-  void set_frenet_points(const std::vector<common::FrenetFramePoint> &points);
-  std::vector<common::FrenetFramePoint> *mutable_points();
+  void set_points(const std::vector<common::FrenetFramePoint> &points);
   const std::vector<common::FrenetFramePoint> &points() const;
   std::uint32_t number_of_points() const;
+  double length() const;
   const common::FrenetFramePoint &point_at(const std::uint32_t index) const;
   common::FrenetFramePoint &point_at(const std::uint32_t index);
 

modules/planning/common/path/path.h

@@ -34,7 +34,7 @@ class Path {
 
   virtual common::PathPoint evaluate(const double param) const = 0;
 
-  virtual double param_length() const = 0;
+  virtual double length() const = 0;
 
   virtual common::PathPoint start_point() const = 0;
 

modules/planning/common/path/path_data.cc

@@ -31,22 +31,22 @@
 namespace apollo {
 namespace planning {
 
-void PathData::set_path(const DiscretizedPath &path) { path_ = path; }
+void PathData::set_discretized_path(const DiscretizedPath &path) {
+  discretized_path_ = path;
+}
 
 void PathData::set_frenet_path(const FrenetFramePath &frenet_path) {
   frenet_path_ = frenet_path;
 }
 
-DiscretizedPath *PathData::mutable_path() { return &path_; }
-
-void PathData::set_path_points(
+void PathData::set_discretized_path(
     const std::vector<common::PathPoint> &path_points) {
-  path_.set_points(path_points);
+  discretized_path_.set_points(path_points);
 }
 
-const DiscretizedPath &PathData::path() const { return path_; }
-
-FrenetFramePath *PathData::mutable_frenet_frame_path() { return &frenet_path_; }
+const DiscretizedPath &PathData::discretized_path() const {
+  return discretized_path_;
+}
 
 const FrenetFramePath &PathData::frenet_frame_path() const {
   return frenet_path_;
@@ -54,7 +54,7 @@ const FrenetFramePath &PathData::frenet_frame_path() const {
 
 bool PathData::get_path_point_with_path_s(
     const double s, common::PathPoint *const path_point) const {
-  *path_point = path_.evaluate_linear_approximation(s);
+  *path_point = discretized_path_.evaluate_linear_approximation(s);
   return true;
 }
 
@@ -72,7 +72,7 @@ bool PathData::get_path_point_with_ref_s(
   auto it_lower =
       std::lower_bound(frenet_points.begin(), frenet_points.end(), ref_s, comp);
   if (it_lower == frenet_points.begin()) {
-    *path_point = path_.path_points().front();
+    *path_point = discretized_path_.points().front();
   } else {
     //        std::uint32_t index_lower = (std::uint32_t)(it_lower -
     //        frenet_points.begin());
@@ -93,7 +93,7 @@ bool PathData::get_path_point_with_ref_s(
 std::string PathData::DebugString() const {
   std::ostringstream sout;
   sout << "[" << std::endl;
-  const auto &path_points = path_.path_points();
+  const auto &path_points = discretized_path_.points();
   for (std::size_t i = 0;
        i < path_points.size() &&
        i < static_cast<std::size_t>(FLAGS_trajectory_point_num_for_debug);

modules/planning/common/path/path_data.h

@@ -33,17 +33,13 @@ class PathData {
  public:
   PathData() = default;
 
-  void set_path(const DiscretizedPath &path);
+  void set_discretized_path(const DiscretizedPath &path);
 
-  void set_path_points(const std::vector<common::PathPoint> &path_points);
+  void set_discretized_path(const std::vector<common::PathPoint> &path_points);
 
   void set_frenet_path(const FrenetFramePath &frenet_path);
 
-  DiscretizedPath *mutable_path();
-
-  const DiscretizedPath &path() const;
-
-  FrenetFramePath *mutable_frenet_frame_path();
+  const DiscretizedPath &discretized_path() const;
 
   const FrenetFramePath &frenet_frame_path() const;
 
@@ -57,7 +53,7 @@ class PathData {
 
   // TODO(fanhaoyang) add check if the path data is valid
  private:
-  DiscretizedPath path_;
+  DiscretizedPath discretized_path_;
 
   FrenetFramePath frenet_path_;
 };

modules/planning/common/planning_data.cc

@@ -88,13 +88,15 @@ bool PlanningData::aggregate(const double time_resolution,
     common::PathPoint path_point;
     // TODO(all): temp fix speed point s out of path point bound, need further
     // refine later
-    if (speed_point.s() > path_data_.path().param_length()) {
+    if (speed_point.s() > path_data_.discretized_path().length()) {
       break;
     }
-    QUIT_IF(
-        !path_data_.get_path_point_with_path_s(speed_point.s(), &path_point),
-        false, ERROR, "Fail to get path data with s %f, path total length %f",
-        speed_point.s(), path_data_.path().param_length());
+    if (!path_data_.get_path_point_with_path_s(speed_point.s(), &path_point)) {
+      AERROR << "Fail to get path data with s " << speed_point.s()
+             << "path total length " << path_data_.discretized_path().length();
+      ;
+      return false;
+    }
 
     common::TrajectoryPoint trajectory_point;
     trajectory_point.mutable_path_point()->CopyFrom(path_point);

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -125,8 +125,7 @@ bool DpRoadGraph::find_tunnel(const ReferenceLine &reference_line,
     }
     path_points.push_back(std::move(path_point));
   }
-  DiscretizedPath discretized_path(path_points);
-  path_data->set_path(discretized_path);
+  path_data->set_discretized_path(path_points);
   return true;
 }
 

modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc

@@ -86,7 +86,7 @@ TEST_F(DpRoadGraphTest, speed_road_graph) {
   bool result =
       road_graph.find_tunnel(*reference_line_, &decision_data_, &path_data_);
   EXPECT_TRUE(result);
-  EXPECT_EQ(706, path_data_.path().num_of_points());
+  EXPECT_EQ(706, path_data_.discretized_path().num_of_points());
   EXPECT_EQ(706, path_data_.frenet_frame_path().number_of_points());
   EXPECT_FLOAT_EQ(69.9, path_data_.frenet_frame_path().points().back().s());
   EXPECT_FLOAT_EQ(0.0, path_data_.frenet_frame_path().points().back().l());

modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc

@@ -49,9 +49,9 @@ Status DpStBoundaryMapper::get_graph_boundary(
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (path_data.path().num_of_points() < 2) {
+  if (path_data.discretized_path().num_of_points() < 2) {
     AERROR << "Fail to get params since path has "
-           << path_data.path().num_of_points() << " points.";
+           << path_data.discretized_path().num_of_points() << " points.";
     return Status(ErrorCode::PLANNING_ERROR, "Fail to get params");
   }
 
@@ -103,7 +103,8 @@ Status DpStBoundaryMapper::map_obstacle_with_trajectory(
   // lower and upper bound for st boundary
   std::vector<STPoint> lower_points;
   std::vector<STPoint> upper_points;
-  const std::vector<PathPoint>& veh_path = path_data.path().path_points();
+  const std::vector<PathPoint>& veh_path =
+      path_data.discretized_path().points();
   for (std::uint32_t i = 0; i < obstacle.prediction_trajectories().size();
        ++i) {
     PredictionTrajectory pred_traj = obstacle.prediction_trajectories()[i];
@@ -181,7 +182,8 @@ Status DpStBoundaryMapper::map_obstacle_without_trajectory(
     const double planning_distance, const double planning_time,
     std::vector<StGraphBoundary>* const boundary) const {
   // Static obstacle only have yield option
-  const std::vector<PathPoint>& veh_path = path_data.path().path_points();
+  const std::vector<PathPoint>& veh_path =
+      path_data.discretized_path().points();
 
   ::apollo::common::math::Box2d obs_box = obstacle.BoundingBox();
 

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -65,9 +65,10 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
     return Status(ErrorCode::PLANNING_ERROR, "Not inited.");
   }
 
+  const double path_length = path_data.discretized_path().length();
   // step 1 get boundaries
-  double planning_distance = std::min(dp_st_speed_config_.total_path_length(),
-                                      path_data.path().param_length());
+  double planning_distance =
+      std::min(dp_st_speed_config_.total_path_length(), path_length);
   std::vector<StGraphBoundary> boundaries;
   common::TrajectoryPoint initial_planning_point = DataCenter::instance()
                                                        ->current_frame()
@@ -100,8 +101,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  StGraphData st_graph_data(boundaries, init_point, speed_limit,
-                            path_data.path().param_length());
+  StGraphData st_graph_data(boundaries, init_point, speed_limit, path_length);
 
   DpStGraph st_graph(dp_st_speed_config_);
   if (!st_graph.search(st_graph_data, decision_data, speed_data).ok()) {

modules/planning/optimizer/optimizer_test_base.cc

@@ -85,7 +85,7 @@ void OptimizerTestBase::export_path_data(const PathData& path_data,
   std::ofstream ofs(filename);
   ofs << "s, l, dl, ddl, x, y, z" << std::endl;
   const auto& frenet_path = path_data.frenet_frame_path();
-  const auto& discrete_path = path_data.path();
+  const auto& discrete_path = path_data.discretized_path();
   if (frenet_path.number_of_points() != discrete_path.num_of_points()) {
     AERROR << "frenet_path and discrete path have different number of points";
     return;

modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc

@@ -134,7 +134,7 @@ bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line,
     path_points.push_back(std::move(path_point));
     s += s_resolution;
   }
-  path_data->set_path_points(path_points);
+  path_data->set_discretized_path(path_points);
   return true;
 }
 

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc

@@ -67,10 +67,10 @@ Status QpSplineStBoundaryMapper::get_graph_boundary(
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (path_data.path().num_of_points() < 2) {
+  if (path_data.discretized_path().num_of_points() < 2) {
     AERROR << "Fail to get params because of too few path points. path points "
               "size: "
-           << path_data.path().num_of_points() << ".";
+           << path_data.discretized_path().num_of_points() << ".";
     return Status(ErrorCode::PLANNING_ERROR,
                   "Fail to get params because of too few path points");
   }
@@ -250,7 +250,7 @@ Status QpSplineStBoundaryMapper::map_obstacle_with_prediction_trajectory(
 
   bool skip = true;
   std::vector<STPoint> boundary_points;
-  const auto& adc_path_points = path_data.path().path_points();
+  const auto& adc_path_points = path_data.discretized_path().points();
   if (obstacle.prediction_trajectories().size() == 0) {
     AWARN << "Obstacle (id = " << obstacle.Id()
           << ") has NO prediction trajectory.";
@@ -271,7 +271,7 @@ Status QpSplineStBoundaryMapper::map_obstacle_with_prediction_trajectory(
           obstacle.Length() * st_boundary_config().expending_coeff(),
           obstacle.Width() * st_boundary_config().expending_coeff());
       int64_t low = 0;
-      int64_t high = static_cast<int64_t>(path_data.path().num_of_points()) - 1;
+      int64_t high = adc_path_points.size() - 1;
       bool find_low = false;
       bool find_high = false;
       while (low < high) {

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -71,7 +71,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   }
 
   double total_length = std::min(qp_spline_st_speed_config_.total_path_length(),
-                                 path_data.path().param_length());
+                                 path_data.discretized_path().length());
 
   // step 1 get boundaries
   std::vector<StGraphBoundary> boundaries;
@@ -102,7 +102,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   QpSplineStGraph st_graph(qp_spline_st_speed_config_, veh_param);
 
   StGraphData st_graph_data(boundaries, init_point, speed_limits,
-                            path_data.path().param_length());
+                            path_data.discretized_path().length());
   if (st_graph.search(st_graph_data, path_data, speed_data) != Status::OK()) {
     return Status(ErrorCode::PLANNING_ERROR,
                   "Failed to search graph with dynamic programming!");

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -66,10 +66,10 @@ double StBoundaryMapper::get_area(
   return fabs(area);
 }
 
-bool StBoundaryMapper::check_overlap(
-    const PathPoint& path_point,
-    const apollo::common::VehicleParam& params,
-    const apollo::common::math::Box2d& obs_box, const double buffer) const {
+bool StBoundaryMapper::check_overlap(const PathPoint& path_point,
+                                     const apollo::common::VehicleParam& params,
+                                     const apollo::common::math::Box2d& obs_box,
+                                     const double buffer) const {
   const double mid_to_rear_center =
       params.length() / 2.0 - params.front_edge_to_center();
   const double x =
@@ -103,7 +103,8 @@ Status StBoundaryMapper::get_speed_limits(
   }
 
   std::vector<double> speed_limits;
-  for (const auto& point : path_data.path().path_points()) {
+  const auto& path_points = path_data.discretized_path().points();
+  for (const auto& point : path_points) {
     speed_limits.push_back(_st_boundary_config.maximal_speed());
     for (auto& lane : lanes) {
       if (lane->is_on_lane({point.x(), point.y()})) {
@@ -117,7 +118,7 @@ Status StBoundaryMapper::get_speed_limits(
     }
   }
 
-  if (planning_distance > path_data.path().path_points().back().s()) {
+  if (planning_distance > path_points.back().s()) {
     const std::string msg = "path length cannot be less than planning_distance";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
@@ -127,7 +128,6 @@ Status StBoundaryMapper::get_speed_limits(
   const double unit_s = planning_distance / matrix_dimension_s;
   std::uint32_t i = 0;
   std::uint32_t j = 1;
-  const auto& path_points = path_data.path().path_points();
   while (i < matrix_dimension_s && j < path_points.size()) {
     const auto& point = path_points[j];
     const auto& pre_point = path_points[j - 1];
@@ -153,7 +153,6 @@ Status StBoundaryMapper::get_speed_limits(
             std::fmin(curr_speed_limit,
                       std::fmin(_st_boundary_config.maximal_speed(), speed));
       }
-
       curr_speed_limit *= _st_boundary_config.speed_multiply_buffer();
       curr_speed_limit =
           std::fmax(curr_speed_limit, _st_boundary_config.lowest_speed());

modules/planning/planner/em/em_planner.cc

@@ -84,7 +84,7 @@ Status EMPlanner::Init(const PlanningConfig& config) {
 }
 
 Status EMPlanner::Plan(const TrajectoryPoint& start_point,
-                           ADCTrajectory* trajectory_pb) {
+                       ADCTrajectory* trajectory_pb) {
   DataCenter* data_center = DataCenter::instance();
   Frame* frame = data_center->current_frame();
 
@@ -109,12 +109,12 @@ Status EMPlanner::Plan(const TrajectoryPoint& start_point,
       CHECK(OptimizerType_Parse(optimizer->name(), &type));
       if (type == DP_POLY_PATH_OPTIMIZER || type == QP_SPLINE_PATH_OPTIMIZER) {
         const auto& path_points =
-            planning_data->path_data().path().path_points();
+            planning_data->path_data().discretized_path().points();
         auto* optimized_path =
             trajectory_pb->mutable_debug()->mutable_planning_data()->add_path();
         optimized_path->set_name(optimizer->name());
-        optimized_path->mutable_path_point()->CopyFrom({path_points.begin(),
-                                                        path_points.end()});
+        optimized_path->mutable_path_point()->CopyFrom(
+            {path_points.begin(), path_points.end()});
       }
     }
   }
@@ -135,8 +135,8 @@ Status EMPlanner::Plan(const TrajectoryPoint& start_point,
     reference_line->set_name("planning_reference_line");
     const auto& reference_points =
         planning_data->reference_line().reference_points();
-    reference_line->mutable_path_point()->CopyFrom({reference_points.begin(),
-                                                    reference_points.end()});
+    reference_line->mutable_path_point()->CopyFrom(
+        {reference_points.begin(), reference_points.end()});
   }
 
   return Status::OK();