Planning: relaxed the mapped boundary from 4 points to multiple points.

modules/planning/common/speed/st_boundary.cc

@@ -308,5 +308,22 @@ bool StBoundary::GetIndexRange(const std::vector<STPoint>& points,
   return true;
 }
 
+StBoundary StBoundary::GenerateStBoundary(
+    const std::vector<STPoint> lower_points,
+    const std::vector<STPoint> upper_points) {
+  if (lower_points.size() != upper_points.size() || lower_points.size() < 2) {
+    AERROR << "Fail to generate StBoundary because input points are not valid.";
+    return StBoundary();
+  }
+
+  std::vector<std::pair<STPoint, STPoint>> point_pairs;
+  for (size_t i = 0; i < lower_points.size() && i < upper_points.size(); ++i) {
+    point_pairs.emplace_back(
+        STPoint(lower_points.at(i).s(), lower_points.at(i).t()),
+        STPoint(upper_points.at(i).s(), upper_points.at(i).t()));
+  }
+  return StBoundary(point_pairs);
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/speed/st_boundary.h

@@ -96,6 +96,9 @@ class StBoundary : public common::math::Polygon2d {
   std::vector<STPoint> upper_points() const { return upper_points_; }
   std::vector<STPoint> lower_points() const { return lower_points_; }
 
+  static StBoundary GenerateStBoundary(const std::vector<STPoint> lower_points,
+                                       const std::vector<STPoint> upper_points);
+
  private:
   bool IsValid(
       const std::vector<std::pair<STPoint, STPoint>>& point_pairs) const;

modules/planning/tasks/dp_st_speed/dp_st_graph.cc

@@ -406,7 +406,6 @@ Status DpStGraph::MakeObjectDecision(const SpeedData& speed_profile,
     if (path_obstacle->HasLongitudinalDecision()) {
       continue;
     }
-    CHECK_EQ(boundary.points().size(), 4);
 
     double start_t = boundary.min_t();
     double end_t = boundary.max_t();
@@ -640,8 +639,6 @@ double DpStGraph::CalculateEdgeCostForThirdCol(const uint32_t curr_row,
 }
 
 bool DpStGraph::CheckIsFollowByT(const StBoundary& boundary) const {
-  DCHECK_EQ(boundary.points().size(), 4);
-
   if (boundary.BottomLeftPoint().s() > boundary.BottomRightPoint().s()) {
     return false;
   }

modules/planning/tasks/speed_optimizer.cc

@@ -95,7 +95,6 @@ void SpeedOptimizer::RecordSTGraphDebug(
       point_debug->set_t(point.x());
       point_debug->set_s(point.y());
     }
-    std::cout << boundary.DebugString() << std::endl;
   }
 
   for (const auto& point : speed_limits.speed_limit_points()) {

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -234,21 +234,17 @@ Status StBoundaryMapper::MapWithoutDecision(const PathObstacle& path_obstacle,
     return Status::OK();
   }
 
-  if (lower_points.size() > 0 && upper_points.size() > 0) {
-    std::vector<std::pair<STPoint, STPoint>> point_pairs;
-    point_pairs.emplace_back(
-        STPoint(lower_points.at(0).s() - boundary_s_buffer,
-                lower_points.at(0).t() - boundary_t_buffer),
-        STPoint(upper_points.at(0).s() + boundary_s_buffer,
-                upper_points.at(0).t() - boundary_t_buffer));
-
-    point_pairs.emplace_back(
-        STPoint(lower_points.back().s() - boundary_s_buffer,
-                lower_points.back().t() + boundary_t_buffer),
-        STPoint(upper_points.back().s() + boundary_s_buffer,
-                upper_points.back().t() + boundary_t_buffer));
-
-    *boundary = StBoundary(point_pairs);
+  if (lower_points.size() != upper_points.size()) {
+    std::string msg = common::util::StrCat(
+        "lower_points.size()[", lower_points.size(),
+        "] and upper_points.size()[", upper_points.size(), "] does NOT match.");
+    return Status(ErrorCode::PLANNING_ERROR, msg);
+  }
+
+  if (lower_points.size() > 1 && upper_points.size() > 1) {
+    *boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
+                    .ExpandByS(boundary_s_buffer)
+                    .ExpandByT(boundary_t_buffer);
     boundary->SetId(path_obstacle.obstacle()->Id());
   }
   return Status::OK();
@@ -350,21 +346,18 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
                                 &lower_points)) {
     return Status(ErrorCode::PLANNING_ERROR, "PLANNING_ERROR");
   }
-  if (lower_points.size() > 0 && upper_points.size() > 0) {
-    std::vector<std::pair<STPoint, STPoint>> point_pairs;
-    point_pairs.emplace_back(
-        STPoint(lower_points.at(0).s() - boundary_s_buffer,
-                lower_points.at(0).t() - boundary_t_buffer),
-        STPoint(upper_points.at(0).s() + boundary_s_buffer,
-                upper_points.at(0).t() - boundary_t_buffer));
-
-    point_pairs.emplace_back(
-        STPoint(lower_points.back().s() - boundary_s_buffer,
-                lower_points.back().t() + boundary_t_buffer),
-        STPoint(upper_points.back().s() + boundary_s_buffer,
-                upper_points.back().t() + boundary_t_buffer));
-
-    *boundary = StBoundary(point_pairs);
+
+  if (lower_points.size() != upper_points.size()) {
+    std::string msg = common::util::StrCat(
+        "lower_points.size()[", lower_points.size(),
+        "] and upper_points.size()[", upper_points.size(), "] does NOT match.");
+    return Status(ErrorCode::PLANNING_ERROR, msg);
+  }
+
+  if (lower_points.size() > 1 && upper_points.size() > 1) {
+    *boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
+                    .ExpandByS(boundary_s_buffer)
+                    .ExpandByT(boundary_t_buffer);
 
     // get characteristic_length and boundary_type.
     StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN;
@@ -394,7 +387,8 @@ Status StBoundaryMapper::MapFollowDecision(
     StBoundary* const boundary) const {
   DCHECK_NOTNULL(boundary);
   DCHECK(obj_decision.has_follow())
-      << "Map obstacle without prediction trajectory is ONLY supported when "
+      << "Map obstacle without prediction trajectory is ONLY supported "
+         "when "
          "the object decision is follow. The current object decision is: "
       << obj_decision.DebugString();