changes in st_boundary and mapper.

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -72,7 +72,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
       std::min(dp_st_speed_config_.total_path_length(), path_length);
   std::vector<StGraphBoundary> boundaries;
   if (!boundary_mapper_
-           .get_graph_boundary(init_point, *decision_data, path_data,
+           .GetGraphBoundary(init_point, *decision_data, path_data,
                                reference_line,
                                dp_st_speed_config_.total_path_length(),
                                dp_st_speed_config_.total_time(), &boundaries)
@@ -86,7 +86,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
   // step 2 perform graph search
   SpeedLimit speed_limit;
   if (!boundary_mapper_
-           .get_speed_limits(common::VehicleState::instance()->pose(),
+           .GetSpeedLimits(common::VehicleState::instance()->pose(),
                                   Frame::PncMap(), path_data, planning_distance,
                                   dp_st_speed_config_.matrix_dimension_s(),
                                   dp_st_speed_config_.max_speed(), &speed_limit)

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -37,9 +37,10 @@ QpSplineStGraph::QpSplineStGraph(
     const QpSplineStSpeedConfig& qp_spline_st_speed_config,
     const apollo::common::VehicleParam& veh_param)
     : qp_spline_st_speed_config_(qp_spline_st_speed_config),
-      time_resolution_(qp_spline_st_speed_config_.total_time() /
-                       qp_spline_st_speed_config_.number_of_discrete_graph_t()),
-      evaluation_time_resolution_(
+      t_knots_resolution_(
+          qp_spline_st_speed_config_.total_time() /
+          qp_spline_st_speed_config_.number_of_discrete_graph_t()),
+      t_evaluated_resolution_(
           qp_spline_st_speed_config_.total_time() /
           qp_spline_st_speed_config_.number_of_evaluated_graph_t())
 
@@ -51,7 +52,7 @@ void QpSplineStGraph::Init() {
   for (uint32_t i = 0;
        i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) {
     t_knots_.push_back(curr_t);
-    curr_t += time_resolution_;
+    curr_t += t_knots_resolution_;
   }
 
   // init evaluated t positions
@@ -59,7 +60,7 @@ void QpSplineStGraph::Init() {
   for (uint32_t i = 0;
        i <= qp_spline_st_speed_config_.number_of_evaluated_graph_t(); ++i) {
     t_evaluated_.push_back(curr_t);
-    curr_t += evaluation_time_resolution_;
+    curr_t += t_evaluated_resolution_;
   }
 
   // init spline generator
@@ -106,15 +107,16 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
   speed_data->Clear();
   const Spline1d& spline = spline_generator_->spline();
 
-  double time_resolution = qp_spline_st_speed_config_.output_time_resolution();
+  double t_output_resolution =
+      qp_spline_st_speed_config_.output_time_resolution();
   double time = 0.0;
-  while (time < qp_spline_st_speed_config_.total_time() + time_resolution) {
+  while (time < qp_spline_st_speed_config_.total_time() + t_output_resolution) {
     double s = spline(time);
     double v = spline.derivative(time);
     double a = spline.second_order_derivative(time);
     double da = spline.third_order_derivative(time);
     speed_data->add_speed_point(s, time, v, a, da);
-    time += time_resolution;
+    time += t_output_resolution;
   }
 
   return Status::OK();
@@ -228,7 +230,8 @@ Status QpSplineStGraph::ApplyKernel(
   for (uint32_t i = 0;
        i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) {
     s_vec.push_back(dist_ref);
-    dist_ref += time_resolution_ * speed_limit.get_speed_limit_by_s(dist_ref);
+    dist_ref +=
+        t_knots_resolution_ * speed_limit.get_speed_limit_by_s(dist_ref);
   }
   // TODO: change reference line kernel to configurable version
   spline_kernel->add_reference_line_kernel_matrix(t_knots_, s_vec, 1);

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h

@@ -90,12 +90,12 @@ class QpSplineStGraph {
   std::unique_ptr<Spline1dGenerator> spline_generator_ = nullptr;
 
   // time resolution
-  double time_resolution_ = 0.0;
+  double t_knots_resolution_ = 0.0;
 
   // knots
   std::vector<double> t_knots_;
 
-  double evaluation_time_resolution_ = 0.0;
+  double t_evaluated_resolution_ = 0.0;
   // evaluated points
   std::vector<double> t_evaluated_;
 };

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -74,7 +74,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   // step 1 get boundaries
   std::vector<StGraphBoundary> boundaries;
   if (!boundary_mapper_
-           .get_graph_boundary(
+           .GetGraphBoundary(
                init_point, *decision_data, path_data, reference_line,
                qp_spline_st_speed_config_.total_path_length(),
                qp_spline_st_speed_config_.total_time(), &boundaries)
@@ -86,7 +86,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   SpeedLimit speed_limits;
   const auto& pose = apollo::common::VehicleState::instance()->pose();
   const auto* pnc_map = frame_->PncMap();
-  if (boundary_mapper_.get_speed_limits(pose, pnc_map, path_data, total_length,
+  if (boundary_mapper_.GetSpeedLimits(pose, pnc_map, path_data, total_length,
                                         qp_spline_st_speed_config_.total_time(),
                                         qp_spline_st_speed_config_.max_speed(),
                                         &speed_limits) != Status::OK()) {

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -83,7 +83,7 @@ bool StBoundaryMapper::check_overlap(const PathPoint& path_point,
   return obs_box.HasOverlap(adc_box);
 }
 
-Status StBoundaryMapper::get_speed_limits(
+Status StBoundaryMapper::GetSpeedLimits(
     const Pose& pose, const PncMap* pnc_map, const PathData& path_data,
     const double planning_distance, const std::uint32_t matrix_dimension_s,
     const double default_speed_limit, SpeedLimit* const speed_limit_data) {
@@ -93,14 +93,14 @@ Status StBoundaryMapper::get_speed_limits(
   adc_point.set_x(adc_position.x());
   adc_point.set_y(adc_position.y());
   adc_point.set_z(adc_position.z());
-  // std::vector<const apollo::hdmap::LaneInfo*> lanes;
+
   std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>> lanes;
   int ret = pnc_map->HDMap()->get_lanes(adc_point, 1.0, &lanes);
   if (ret != 0) {
     AERROR << "Fail to get lanes for point [" << adc_position.x() << ", "
            << adc_position.y() << "].";
     return Status(ErrorCode::PLANNING_ERROR,
-                  "StBoundaryMapper::get_speed_limits");
+                  "StBoundaryMapper::GetSpeedLimits");
   }
 
   std::vector<double> speed_limits;

modules/planning/optimizer/st_graph/st_boundary_mapper.h

@@ -47,14 +47,14 @@ class StBoundaryMapper {
   virtual ~StBoundaryMapper() = default;
 
   bool Init(const std::string& config_file);
-  virtual apollo::common::Status get_graph_boundary(
+  virtual apollo::common::Status GetGraphBoundary(
       const common::TrajectoryPoint& initial_planning_point,
       const DecisionData& decision_data, const PathData& path_data,
       const ReferenceLine& reference_line, const double planning_distance,
       const double planning_time,
       std::vector<StGraphBoundary>* const boundary) const = 0;
 
-  virtual apollo::common::Status get_speed_limits(
+  virtual apollo::common::Status GetSpeedLimits(
       const apollo::localization::Pose& pose, const apollo::hdmap::PncMap* map,
       const PathData& path_data, const double planning_distance,
       const std::uint32_t matrix_dimension_s, const double default_speed_limit,

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc

@@ -49,7 +49,7 @@ using Box2d = apollo::common::math::Box2d;
 using Vec2d = apollo::common::math::Vec2d;
 using VehicleConfigHelper = apollo::common::VehicleConfigHelper;
 
-Status StBoundaryMapperImpl::get_graph_boundary(
+Status StBoundaryMapperImpl::GetGraphBoundary(
     const common::TrajectoryPoint& initial_planning_point,
     const DecisionData& decision_data, const PathData& path_data,
     const ReferenceLine& reference_line, const double planning_distance,

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.h

@@ -35,7 +35,7 @@ namespace planning {
 
 class StBoundaryMapperImpl : public StBoundaryMapper {
  public:
-  apollo::common::Status get_graph_boundary(
+  apollo::common::Status GetGraphBoundary(
       const common::TrajectoryPoint& initial_planning_point,
       const DecisionData& decision_data, const PathData& path_data,
       const ReferenceLine& reference_line, const double planning_distance,

modules/planning/optimizer/st_graph/st_graph_boundary.cc

@@ -29,42 +29,36 @@ namespace planning {
 
 using Vec2d = common::math::Vec2d;
 
-StGraphBoundary::StGraphBoundary(const std::vector<STPoint>& points) {
-  CHECK_GE(points.size(), 4);
-  for (const auto& point : points) {
-    points_.emplace_back(point.t(), point.s());
-  }
-  BuildFromPoints();
+StGraphBoundary::StGraphBoundary(const std::vector<STPoint>& points)
+    : Polygon2d(std::vector<Vec2d>(points.begin(), points.end())) {
+  CHECK_EQ(points.size(), 4)
+      << "StGraphBoundary must have exactly four points. Input points size: "
+      << points.size();
 }
 
 StGraphBoundary::StGraphBoundary(
     const std::vector<::apollo::common::math::Vec2d>& points)
     : Polygon2d(points) {
-  CHECK_GE(points.size(), 4);
+  CHECK_EQ(points.size(), 4)
+      << "StGraphBoundary must have exactly four points. Input points size: "
+      << points.size();
 }
 
 bool StGraphBoundary::IsPointInBoundary(
     const StGraphPoint& st_graph_point) const {
-  ::apollo::common::math::Vec2d vec2d = {st_graph_point.point().t(),
-                                         st_graph_point.point().s()};
-  return IsPointIn(vec2d);
+  return IsPointInBoundary(st_graph_point.point());
 }
 
 bool StGraphBoundary::IsPointInBoundary(const STPoint& st_point) const {
-  ::apollo::common::math::Vec2d vec2d = {st_point.t(), st_point.s()};
-  return IsPointIn(vec2d);
+  return IsPointIn(st_point);
 }
 
-const ::apollo::common::math::Vec2d StGraphBoundary::point(
-    const uint32_t index) const {
-  CHECK_LT(index, points_.size());
+Vec2d StGraphBoundary::point(const uint32_t index) const {
+  CHECK_LT(index, points_.size()) << "Index[" << index << "] is out of range.";
   return points_[index];
 }
 
-const std::vector<::apollo::common::math::Vec2d>& StGraphBoundary::points()
-    const {
-  return points_;
-}
+const std::vector<Vec2d>& StGraphBoundary::points() const { return points_; }
 
 bool StGraphBoundary::is_empty() const { return points_.empty(); }
 

modules/planning/optimizer/st_graph/st_graph_boundary.h

@@ -40,8 +40,6 @@ class StGraphBoundary : public common::math::Polygon2d {
     FOLLOW,
     YIELD,
     OVERTAKE,
-    SIDEPASSFOLLOW,
-    SIDEPASSLEAD,
   };
 
   explicit StGraphBoundary(const std::vector<STPoint>& points);
@@ -53,7 +51,7 @@ class StGraphBoundary : public common::math::Polygon2d {
   bool IsPointInBoundary(const StGraphPoint& st_graph_point) const;
   bool IsPointInBoundary(const STPoint& st_point) const;
 
-  const common::math::Vec2d point(const uint32_t index) const;
+  common::math::Vec2d point(const uint32_t index) const;
   const std::vector<common::math::Vec2d>& points() const;
 
   BoundaryType boundary_type() const;
@@ -82,4 +80,4 @@ class StGraphBoundary : public common::math::Polygon2d {
 }  // end namespace planning
 }  // end namespace apollo
 
-#endif  // BAIDU_ADU_PLANNING_OTIMIZER_COMMON_ST_GRAPH_BOUNDARY_H_
+#endif  // BAIDU_ADU_PLANNING_OPTIMIZER_COMMON_ST_GRAPH_BOUNDARY_H_