re-structured st_graph_data

modules/planning/optimizer/dp_st_speed/dp_st_cost.cc

@@ -39,9 +39,9 @@ DpStCost::DpStCost(const DpStSpeedConfig& dp_st_speed_config)
 // TODO(all): normalize cost with time
 double DpStCost::GetObstacleCost(
     const STPoint& point,
-    const std::vector<StGraphBoundary>& obs_boundary) const {
+    const std::vector<StGraphBoundary>& st_graph_boundaries) const {
   double total_cost = 0.0;
-  for (const StGraphBoundary& boundary : obs_boundary) {
+  for (const StGraphBoundary& boundary : st_graph_boundaries) {
     if (point.s() < 0 || boundary.IsPointInBoundary(point)) {
       total_cost += _dp_st_speed_config.st_graph_default_point_cost();
     } else {

modules/planning/optimizer/dp_st_speed/dp_st_cost.h

@@ -38,7 +38,7 @@ class DpStCost {
 
   double GetObstacleCost(
       const STPoint& point,
-      const std::vector<StGraphBoundary>& obs_boundary) const;
+      const std::vector<StGraphBoundary>& st_graph_boundaries) const;
 
   double GetReferenceCost(const STPoint& point,
                           const STPoint& reference_point) const;

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -54,7 +54,7 @@ Status DpStGraph::Search(const StGraphData& st_graph_data,
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  CalculatePointwiseCost(st_graph_data.obs_boundary());
+  CalculatePointwiseCost(st_graph_data.st_graph_boundaries());
 
   if (!CalculateTotalCost(st_graph_data).ok()) {
     const std::string msg = "Calculate total cost failed.";
@@ -291,7 +291,7 @@ Status DpStGraph::get_object_decision(const StGraphData& st_graph_data,
   }
 
   const std::vector<StGraphBoundary>& obs_boundaries =
-      st_graph_data.obs_boundary();
+      st_graph_data.st_graph_boundaries();
   const std::vector<SpeedPoint>& speed_points = speed_profile.speed_vector();
 
   for (std::vector<StGraphBoundary>::const_iterator boundary_it =
@@ -328,7 +328,7 @@ Status DpStGraph::get_object_decision(const StGraphData& st_graph_data,
       STPoint st_point(st_it->s(), st_it->t());
       if (boundary_it->IsPointInBoundary(st_point)) {
         const std::string msg =
-            "dp_st_graph failed: speed profile cross obs_boundary.";
+            "dp_st_graph failed: speed profile cross st_graph_boundaries.";
         AERROR << msg;
         return Status(ErrorCode::PLANNING_ERROR, msg);
       }

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -84,14 +84,14 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
   Init();
 
   if (!ApplyConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(),
-                       st_graph_data.obs_boundary())
+                       st_graph_data.st_graph_boundaries())
            .ok()) {
     const std::string msg = "Apply constraint failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (!ApplyKernel(st_graph_data.obs_boundary(), st_graph_data.speed_limit())
+  if (!ApplyKernel(st_graph_data.st_graph_boundaries(), st_graph_data.speed_limit())
            .ok()) {
     const std::string msg = "Apply kernel failed!";
     AERROR << msg;
@@ -191,8 +191,6 @@ Status QpSplineStGraph::ApplyConstraint(
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  // TODO(Liangliang):
-  // add speed constraint and other limits according to adu/planning
   std::vector<double> speed_constraint;
   if (!EstimateSpeedConstraint(init_point, speed_limit, &speed_constraint)
            .ok()) {

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc

@@ -54,9 +54,9 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
     const DecisionData& decision_data, const PathData& path_data,
     const ReferenceLine& reference_line, const double planning_distance,
     const double planning_time,
-    std::vector<StGraphBoundary>* const obs_boundary) const {
-  if (obs_boundary == nullptr) {
-    const std::string msg = "obs_boundary is NULL.";
+    std::vector<StGraphBoundary>* const st_graph_boundaries) const {
+  if (st_graph_boundaries == nullptr) {
+    const std::string msg = "st_graph_boundaries is NULL.";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
@@ -75,20 +75,20 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
                   "Fail to get params because of too few path points");
   }
 
-  obs_boundary->clear();
+  st_graph_boundaries->clear();
   Status ret = Status::OK();
 
   const auto& main_decision = decision_data.Decision().main_decision();
   if (main_decision.has_stop()) {
     ret =
         map_main_decision_stop(main_decision.stop(), reference_line,
-                               planning_distance, planning_time, obs_boundary);
+                               planning_distance, planning_time, st_graph_boundaries);
     if (!ret.ok() && ret.code() != ErrorCode::PLANNING_SKIP) {
       return Status(ErrorCode::PLANNING_ERROR);
     }
   } else if (main_decision.has_mission_complete()) {
     ret = map_mission_complete(reference_line, planning_distance, planning_time,
-                               obs_boundary);
+                               st_graph_boundaries);
     if (!ret.ok() && ret.code() != ErrorCode::PLANNING_SKIP) {
       return Status(ErrorCode::PLANNING_ERROR);
     }
@@ -103,7 +103,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
     }
     ret = map_obstacle_without_prediction_trajectory(
         initial_planning_point, *obs, path_data, planning_distance,
-        planning_time, obs_boundary);
+        planning_time, st_graph_boundaries);
     if (!ret.ok()) {
       AERROR << "Fail to map static obstacle with id[" << obs->Id() << "].";
       return Status(ErrorCode::PLANNING_ERROR, "Fail to map static obstacle");
@@ -120,7 +120,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
       if (obj_decision.has_follow()) {
         ret = map_obstacle_without_prediction_trajectory(
             initial_planning_point, *obs, obj_decision, path_data,
-            reference_line, planning_distance, planning_time, obs_boundary);
+            reference_line, planning_distance, planning_time, st_graph_boundaries);
         if (!ret.ok()) {
           AERROR << "Fail to map follow dynamic obstacle with id " << obs->Id()
                  << ".";
@@ -130,7 +130,7 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
       } else if (obj_decision.has_overtake() || obj_decision.has_yield()) {
         ret = map_obstacle_with_prediction_trajectory(
             initial_planning_point, *obs, obj_decision, path_data,
-            planning_distance, planning_time, obs_boundary);
+            planning_distance, planning_time, st_graph_boundaries);
         if (!ret.ok()) {
           AERROR << "Fail to map dynamic obstacle with id " << obs->Id() << ".";
           // Return OK by intention.

modules/planning/optimizer/st_graph/st_graph_data.cc

@@ -25,41 +25,24 @@ namespace planning {
 
 using TrajectoryPoint = apollo::common::TrajectoryPoint;
 
-StGraphData::StGraphData(const std::vector<StGraphBoundary>& obs_boundary,
-                         const TrajectoryPoint& init_point,
-                         const SpeedLimit& speed_limit,
-                         const double path_data_length)
-    : _obs_boundary(obs_boundary),
-      _init_point(init_point),
-      _speed_limit(speed_limit),
-      _path_data_length(path_data_length) {}
-
-const std::vector<StGraphBoundary>& StGraphData::obs_boundary() const {
-  return _obs_boundary;
+StGraphData::StGraphData(
+    const std::vector<StGraphBoundary>& st_graph_boundaries,
+    const TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
+    const double path_data_length)
+    : st_graph_boundaries_(st_graph_boundaries),
+      init_point_(init_point),
+      speed_limit_(speed_limit),
+      path_data_length_(path_data_length) {}
+
+const std::vector<StGraphBoundary>& StGraphData::st_graph_boundaries() const {
+  return st_graph_boundaries_;
 }
 
-const TrajectoryPoint& StGraphData::init_point() const { return _init_point; }
-
-const SpeedLimit StGraphData::speed_limit() const { return _speed_limit; }
+const TrajectoryPoint& StGraphData::init_point() const { return init_point_; }
 
-double StGraphData::path_data_length() const { return _path_data_length; }
+const SpeedLimit StGraphData::speed_limit() const { return speed_limit_; }
 
-void StGraphData::set_speed_limit(const SpeedLimit& speed_limit) {
-  _speed_limit = speed_limit;
-}
-
-void StGraphData::set_obs_boundary(
-    const std::vector<StGraphBoundary>& obs_boundary) {
-  _obs_boundary = obs_boundary;
-}
-
-void StGraphData::set_init_point(const TrajectoryPoint& init_point) {
-  _init_point = init_point;
-}
-
-void StGraphData::set_path_data_length(const double path_data_length) {
-  _path_data_length = path_data_length;
-}
+double StGraphData::path_data_length() const { return path_data_length_; }
 
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/st_graph/st_graph_data.h

@@ -36,12 +36,12 @@ class StGraphData {
  public:
   StGraphData() = default;
 
-  explicit StGraphData(const std::vector<StGraphBoundary>& obs_boundary,
+  explicit StGraphData(const std::vector<StGraphBoundary>& st_graph_boundaries,
                        const apollo::common::TrajectoryPoint& init_point,
                        const SpeedLimit& speed_limit,
                        const double path_data_length);
 
-  const std::vector<StGraphBoundary>& obs_boundary() const;
+  const std::vector<StGraphBoundary>& st_graph_boundaries() const;
 
   const apollo::common::TrajectoryPoint& init_point() const;
 
@@ -49,20 +49,12 @@ class StGraphData {
 
   double path_data_length() const;
 
-  void set_speed_limit(const SpeedLimit& speed_limit);
-
-  void set_obs_boundary(const std::vector<StGraphBoundary>& obs_boundary);
-
-  void set_init_point(const apollo::common::TrajectoryPoint& init_point);
-
-  void set_path_data_length(const double path_data_length);
-
  private:
-  std::vector<StGraphBoundary> _obs_boundary;
-  apollo::common::TrajectoryPoint _init_point;
+  std::vector<StGraphBoundary> st_graph_boundaries_;
+  apollo::common::TrajectoryPoint init_point_;
 
-  SpeedLimit _speed_limit;
-  double _path_data_length = 0.0;
+  SpeedLimit speed_limit_;
+  double path_data_length_ = 0.0;
 };
 
 }  // namespace planning