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,29 +277,29 @@ Status StBoundaryMapper::MapObstacleWithoutDecision(
           adc_path_points[high].s() + st_boundary_config_.point_extension(),
           trajectory_point_time);
     }
-    if (lower_points.size() > 0) {
-      boundary_points.clear();
-      const double buffer = st_boundary_config_.follow_buffer();
-      boundary_points.emplace_back(lower_points.at(0).s() - buffer,
-                                   lower_points.at(0).t());
-      boundary_points.emplace_back(lower_points.back().s() - buffer,
-                                   lower_points.back().t());
-      boundary_points.emplace_back(upper_points.back().s() + buffer +
-                                       st_boundary_config_.boundary_buffer(),
-                                   upper_points.back().t());
-      boundary_points.emplace_back(upper_points.at(0).s() + buffer,
-                                   upper_points.at(0).t());
-      if (lower_points.at(0).t() > lower_points.back().t() ||
-          upper_points.at(0).t() > upper_points.back().t()) {
-        AWARN << "lower/upper points are reversed.";
-      }
+  }
+  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,
+                                 lower_points.at(0).t());
+    boundary_points.emplace_back(lower_points.back().s() - buffer,
+                                 lower_points.back().t());
+    boundary_points.emplace_back(upper_points.back().s() + buffer +
+                                     st_boundary_config_.boundary_buffer(),
+                                 upper_points.back().t());
+    boundary_points.emplace_back(upper_points.at(0).s() + buffer,
+                                 upper_points.at(0).t());
+    if (lower_points.at(0).t() > lower_points.back().t() ||
+        upper_points.at(0).t() > upper_points.back().t()) {
+      AWARN << "lower/upper points are reversed.";
+    }
 
-      const double area = GetArea(boundary_points);
-      if (Double::Compare(area, 0.0) > 0) {
-        boundary->emplace_back(&path_obstacle, boundary_points);
-        boundary->back().set_id(obstacle->Id());
-        skip = false;
-      }
+    const double area = GetArea(boundary_points);
+    if (Double::Compare(area, 0.0) > 0) {
+      boundary->emplace_back(&path_obstacle, boundary_points);
+      boundary->back().set_id(obstacle->Id());
+      skip = false;
     }
   }
   return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP")

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;
   }