Planning: fixed a bug in st_boundary_mapper: should process upper/lower points out of for loop.

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -80,7 +80,6 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
-
   // step 2 perform graph search
   SpeedLimit speed_limit;
   if (!boundary_mapper.GetSpeedLimits(&speed_limit).ok()) {

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -234,8 +234,10 @@ Status StBoundaryMapper::MapObstacleWithoutDecision(
   }
   const auto& trajectory = obstacle->Trajectory();
   if (trajectory.trajectory_point_size() == 0) {
-    AWARN << "Obstacle (id = " << obstacle->Id()
-          << ") has NO prediction trajectory.";
+    std::string msg = common::util::StrCat("Obstacle (id = ", obstacle->Id(),
+                                           ") has NO prediction trajectory.");
+    AERROR << msg;
+    return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
   for (int j = 0; j < trajectory.trajectory_point_size(); ++j) {
@@ -275,7 +277,8 @@ Status StBoundaryMapper::MapObstacleWithoutDecision(
           adc_path_points[high].s() + st_boundary_config_.point_extension(),
           trajectory_point_time);
     }
-    if (lower_points.size() > 0) {
+  }
+  if (lower_points.size() > 0 && upper_points.size() > 0) {
     boundary_points.clear();
     const double buffer = st_boundary_config_.follow_buffer();
     boundary_points.emplace_back(lower_points.at(0).s() - buffer,
@@ -299,7 +302,6 @@ Status StBoundaryMapper::MapObstacleWithoutDecision(
       skip = false;
     }
   }
-  }
   return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP")
               : Status::OK();
 }

modules/planning/optimizer/st_graph/st_graph_boundary.cc

@@ -118,7 +118,7 @@ bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
   Vec2d p_s_second;
 
   if (!GetOverlap(segment, &p_s_first, &p_s_second)) {
-    AERROR << "curr_time[" << curr_time
+    ADEBUG << "curr_time[" << curr_time
            << "] is out of the coverage scope of the boundary.";
     return false;
   }
@@ -147,7 +147,7 @@ bool StGraphBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
   Vec2d p_s_first;
   Vec2d p_s_second;
   if (!GetOverlap(segment, &p_s_first, &p_s_second)) {
-    AERROR << "curr_time[" << curr_time
+    ADEBUG << "curr_time[" << curr_time
            << "] is out of the coverage scope of the boundary.";
     return false;
   }