optmized code in st_boundary_mapper.

modules/planning/optimizer/dp_st_speed/dp_st_graph.cc

@@ -335,8 +335,7 @@ Status DpStGraph::get_object_decision(const StGraphData& st_graph_data,
 
       double s_upper = dp_st_speed_config_.total_path_length();
       double s_lower = 0.0;
-      if (boundary_it->get_boundary_s_range_by_time(st_it->t(), &s_upper,
-                                                    &s_lower)) {
+      if (boundary_it->GetBoundarySRange(st_it->t(), &s_upper, &s_lower)) {
         if (s_lower > st_it->s()) {
           go_down = true;
         } else if (s_upper < st_it->s()) {

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -271,7 +271,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
   for (const double curr_t : t_knots_) {
     double s_upper = 0.0;
     double s_lower = 0.0;
-    if (follow_boundary.get_s_boundary_position(curr_t, &s_upper, &s_lower)) {
+    if (follow_boundary.GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
       ref_s.push_back(s_upper - follow_boundary.characteristic_length());
     }
     ref_s.push_back(s_upper);
@@ -292,7 +292,7 @@ Status QpSplineStGraph::GetSConstraintByTime(
     double s_upper = 0.0;
     double s_lower = 0.0;
 
-    if (!boundary.get_s_boundary_position(time, &s_upper, &s_lower)) {
+    if (!boundary.GetUnblockSRange(time, &s_upper, &s_lower)) {
       continue;
     }
 

modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc

@@ -80,9 +80,9 @@ Status StBoundaryMapperImpl::GetGraphBoundary(
 
   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, st_graph_boundaries);
+    ret = map_main_decision_stop(main_decision.stop(), reference_line,
+                                 planning_distance, planning_time,
+                                 st_graph_boundaries);
     if (!ret.ok() && ret.code() != ErrorCode::PLANNING_SKIP) {
       return Status(ErrorCode::PLANNING_ERROR);
     }
@@ -120,7 +120,8 @@ 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, st_graph_boundaries);
+            reference_line, planning_distance, planning_time,
+            st_graph_boundaries);
         if (!ret.ok()) {
           AERROR << "Fail to map follow dynamic obstacle with id " << obs->Id()
                  << ".";
@@ -253,8 +254,8 @@ Status StBoundaryMapperImpl::map_obstacle_with_prediction_trajectory(
 
   for (uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) {
     const auto& trajectory = obstacle.prediction_trajectories()[i];
-    for (int j = 0; j < trajectory.trajectory_point_size(); ++i) {
-      const auto& trajectory_point = trajectory.trajectory_point(i);
+    for (int j = 0; j < trajectory.trajectory_point_size(); ++j) {
+      const auto& trajectory_point = trajectory.trajectory_point(j);
       // TODO(all): fix trajectory point relative time issue.
       double trajectory_point_time = trajectory_point.relative_time();
       const Box2d obs_box(
@@ -273,7 +274,7 @@ Status StBoundaryMapperImpl::map_obstacle_with_prediction_trajectory(
         }
         if (!find_low) {
           if (!CheckOverlap(adc_path_points[low], vehicle_param(), obs_box,
-                             st_boundary_config().boundary_buffer())) {
+                            st_boundary_config().boundary_buffer())) {
             ++low;
           } else {
             find_low = true;
@@ -281,7 +282,7 @@ Status StBoundaryMapperImpl::map_obstacle_with_prediction_trajectory(
         }
         if (!find_high) {
           if (!CheckOverlap(adc_path_points[high], vehicle_param(), obs_box,
-                             st_boundary_config().boundary_buffer())) {
+                            st_boundary_config().boundary_buffer())) {
             --high;
           } else {
             find_high = true;

modules/planning/optimizer/st_graph/st_graph_boundary.cc

@@ -15,8 +15,8 @@
  *****************************************************************************/
 
 /**
-*   @file: obstacle_st_boundary.cc
-**/
+  * @file: obstacle_st_boundary.cc
+  **/
 
 #include "modules/planning/optimizer/st_graph/st_graph_boundary.h"
 
@@ -60,8 +60,6 @@ Vec2d StGraphBoundary::point(const uint32_t index) const {
 
 const std::vector<Vec2d>& StGraphBoundary::points() const { return points_; }
 
-bool StGraphBoundary::is_empty() const { return points_.empty(); }
-
 StGraphBoundary::BoundaryType StGraphBoundary::boundary_type() const {
   return _boundary_type;
 }
@@ -83,16 +81,15 @@ void StGraphBoundary::set_characteristic_length(
   _characteristic_length = characteristic_length;
 }
 
-bool StGraphBoundary::get_s_boundary_position(const double curr_time,
-                                              double* s_upper,
-                                              double* s_lower) const {
+bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
+                                       double* s_lower) const {
   const common::math::LineSegment2d segment = {Vec2d(curr_time, 0.0),
                                                Vec2d(curr_time, _s_high_limit)};
   *s_upper = _s_high_limit;
   *s_lower = 0.0;
-
   Vec2d p_s_first;
   Vec2d p_s_second;
+
   if (!GetOverlap(segment, &p_s_first, &p_s_second)) {
     AERROR << "curr_time[" << curr_time
            << "] is out of the coverage scope of the boundary.";
@@ -103,16 +100,19 @@ bool StGraphBoundary::get_s_boundary_position(const double curr_time,
       _boundary_type == BoundaryType::FOLLOW ||
       _boundary_type == BoundaryType::UNKNOWN) {
     *s_upper = std::fmin(*s_upper, std::fmin(p_s_first.y(), p_s_second.y()));
-  } else {
+  } else if (_boundary_type == BoundaryType::OVERTAKE) {
     // overtake
     *s_lower = std::fmax(*s_lower, std::fmax(p_s_first.y(), p_s_second.y()));
+  } else {
+    AERROR << "boundary_type is not supported. boundary_type: "
+           << static_cast<int>(_boundary_type);
+    return false;
   }
   return true;
 }
 
-bool StGraphBoundary::get_boundary_s_range_by_time(const double curr_time,
-                                                   double* s_upper,
-                                                   double* s_lower) const {
+bool StGraphBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
+                                        double* s_lower) const {
   const common::math::LineSegment2d segment = {Vec2d(curr_time, 0.0),
                                                Vec2d(curr_time, _s_high_limit)};
   *s_upper = _s_high_limit;
@@ -121,7 +121,7 @@ bool StGraphBoundary::get_boundary_s_range_by_time(const double curr_time,
   Vec2d p_s_first;
   Vec2d p_s_second;
   if (!GetOverlap(segment, &p_s_first, &p_s_second)) {
-    AERROR << "curr_time[ " << curr_time
+    AERROR << "curr_time[" << curr_time
            << "] is out of the coverage scope of the boundary.";
     return false;
   }

modules/planning/optimizer/st_graph/st_graph_boundary.h

@@ -15,8 +15,8 @@
  *****************************************************************************/
 
 /**
-*   @file: st_graph_boundary.h
-**/
+ *   @file: st_graph_boundary.h
+ **/
 
 #ifndef MODULES_PLANNING_OPTIMIZER_ST_GRAPH_ST_GRAPH_BOUNDARY_H_
 #define MODULES_PLANNING_OPTIMIZER_ST_GRAPH_ST_GRAPH_BOUNDARY_H_
@@ -47,7 +47,6 @@ class StGraphBoundary : public common::math::Polygon2d {
 
   ~StGraphBoundary() = default;
 
-  bool is_empty() const;
   bool IsPointInBoundary(const StGraphPoint& st_graph_point) const;
   bool IsPointInBoundary(const STPoint& st_point) const;
 
@@ -62,11 +61,11 @@ class StGraphBoundary : public common::math::Polygon2d {
   void set_boundary_type(const BoundaryType& boundary_type);
   void set_characteristic_length(const double characteristic_length);
 
-  bool get_s_boundary_position(const double curr_time, double* s_upper,
-                               double* s_lower) const;
+  bool GetUnblockSRange(const double curr_time, double* s_upper,
+                        double* s_lower) const;
 
-  bool get_boundary_s_range_by_time(const double curr_time, double* s_upper,
-                                    double* s_lower) const;
+  bool GetBoundarySRange(const double curr_time, double* s_upper,
+                         double* s_lower) const;
 
   void get_boundary_time_scope(double* start_t, double* end_t) const;
 
@@ -80,4 +79,4 @@ class StGraphBoundary : public common::math::Polygon2d {
 }  // end namespace planning
 }  // end namespace apollo
 
-#endif  // BAIDU_ADU_PLANNING_OPTIMIZER_COMMON_ST_GRAPH_BOUNDARY_H_
+#endif  // MODULES_PLANNING_OPTIMIZER_ST_GRAPH_ST_GRAPH_BOUNDARY_H_