Planning: allow empty st_graph_boundary; do not return error when no mapping...

Planning: allow empty st_graph_boundary; do not return error when no mapping of obstacle. abstract redundant code to seperate function.

modules/planning/common/obstacle.h

@@ -69,6 +69,7 @@ class Obstacle {
   const common::math::Box2d &PerceptionBoundingBox() const;
 
   const prediction::Trajectory &Trajectory() const;
+  bool has_trajectory() const { return has_trajectory_; }
 
   const perception::PerceptionObstacle &Perception() const;
 

modules/planning/common/path_obstacle.h

@@ -78,6 +78,7 @@ class PathObstacle {
   std::vector<ObjectDecisionType> decisions_;
   std::vector<std::string> decider_tags_;
   SLBoundary sl_boundary_;
+  StGraphBoundary st_boundary_;
 };
 
 }  // namespace planning

modules/planning/optimizer/st_graph/st_boundary_mapper.cc

@@ -106,7 +106,7 @@ Status StBoundaryMapper::GetGraphBoundary(
     if (path_obstacle->Decisions().empty()) {
       const auto ret =
           MapObstacleWithoutDecision(*path_obstacle, st_graph_boundaries);
-      if (!ret.ok()) {
+      if (ret.code() == ErrorCode::PLANNING_ERROR) {
         std::string msg = common::util::StrCat(
             "Fail to map obstacle ", path_obstacle->Id(), " without decision.");
         AERROR << msg;
@@ -306,45 +306,31 @@ Status StBoundaryMapper::MapObstacleWithoutDecision(
               : Status::OK();
 }
 
-Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
-    const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
-    std::vector<StGraphBoundary>* const boundary) const {
-  std::vector<STPoint> lower_points;
-  std::vector<STPoint> upper_points;
-
-  const auto* obstacle = path_obstacle.Obstacle();
-  const auto& speed = obstacle->Perception().velocity();
-  const double scalar_speed = std::hypot(speed.x(), speed.y());
-  const double minimal_follow_time = st_boundary_config_.minimal_follow_time();
-  double follow_distance = -1.0;
-  if (obj_decision.has_follow()) {
-    follow_distance = std::fmax(scalar_speed * minimal_follow_time,
-                                std::fabs(obj_decision.follow().distance_s())) +
-                      vehicle_param_.front_edge_to_center();
-  }
-
-  bool skip = true;
-  std::vector<STPoint> boundary_points;
-  const auto& adc_path_points = path_data_.discretized_path().path_points();
-  if (adc_path_points.size() == 0) {
+bool StBoundaryMapper::GetOverlapBoundaryPoints(
+    const std::vector<PathPoint>& path_points, const Obstacle& obstacle,
+    std::vector<STPoint>* upper_points,
+    std::vector<STPoint>* lower_points) const {
+  DCHECK_NOTNULL(upper_points);
+  DCHECK_NOTNULL(lower_points);
+  DCHECK(upper_points->empty());
+  DCHECK(lower_points->empty());
+  DCHECK_GT(path_points.size(), 0);
+
+  if (path_points.size() == 0) {
     std::string msg = common::util::StrCat(
         "Too few points in path_data_.discretized_path(); size = ",
-        adc_path_points.size());
+        path_points.size());
     AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-  const auto& trajectory = obstacle->Trajectory();
-  if (trajectory.trajectory_point_size() == 0) {
-    AWARN << "Obstacle (id = " << obstacle->Id()
-          << ") has NO prediction trajectory.";
+    return false;
   }
 
+  const auto& trajectory = obstacle.Trajectory();
   for (int j = 0; j < trajectory.trajectory_point_size(); ++j) {
     const auto& trajectory_point = trajectory.trajectory_point(j);
     double trajectory_point_time = trajectory_point.relative_time();
-    const Box2d obs_box = obstacle->GetBoundingBox(trajectory_point);
+    const Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
     int64_t low = 0;
-    int64_t high = adc_path_points.size() - 1;
+    int64_t high = path_points.size() - 1;
     bool find_low = false;
     bool find_high = false;
     while (low < high) {
@@ -352,7 +338,7 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
         break;
       }
       if (!find_low) {
-        if (!CheckOverlap(adc_path_points[low], obs_box,
+        if (!CheckOverlap(path_points[low], obs_box,
                           st_boundary_config_.boundary_buffer())) {
           ++low;
         } else {
@@ -360,7 +346,7 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
         }
       }
       if (!find_high) {
-        if (!CheckOverlap(adc_path_points[high], obs_box,
+        if (!CheckOverlap(path_points[high], obs_box,
                           st_boundary_config_.boundary_buffer())) {
           --high;
         } else {
@@ -369,19 +355,34 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
       }
     }
     if (find_high && find_low) {
-      lower_points.emplace_back(
-          adc_path_points[low].s() - st_boundary_config_.point_extension(),
+      lower_points->emplace_back(
+          path_points[low].s() - st_boundary_config_.point_extension(),
           trajectory_point_time);
-      upper_points.emplace_back(
-          adc_path_points[high].s() + st_boundary_config_.point_extension(),
+      upper_points->emplace_back(
+          path_points[high].s() + st_boundary_config_.point_extension(),
           trajectory_point_time);
-    } else {
-      if (obj_decision.has_yield() || obj_decision.has_overtake()) {
-        AINFO << "Point[" << j << "] cannot find low or high index.";
     }
   }
+  DCHECK_EQ(lower_points->size(), upper_points->size());
+  return (lower_points->size() > 0 && upper_points->size() > 0);
+}
+
+Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
+    const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
+    std::vector<StGraphBoundary>* const boundary) const {
+  std::vector<STPoint> lower_points;
+  std::vector<STPoint> upper_points;
+
+  bool skip = true;
+  std::vector<STPoint> boundary_points;
+  if (!GetOverlapBoundaryPoints(path_data_.discretized_path().path_points(),
+                                *(path_obstacle.Obstacle()), &upper_points,
+                                &lower_points)) {
+    return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP")
+                : Status::OK();
+    ;
 
-    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,
@@ -401,15 +402,24 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
       // change boundary according to obj_decision.
       StGraphBoundary::BoundaryType b_type =
           StGraphBoundary::BoundaryType::UNKNOWN;
+      double characteristic_length = 0.0;
       if (obj_decision.has_follow()) {
-        boundary_points.at(0).set_s(boundary_points.at(0).s() -
-                                    follow_distance);
-        boundary_points.at(1).set_s(boundary_points.at(1).s() -
-                                    follow_distance);
+        const auto& speed = obstacle->Perception().velocity();
+        const double scalar_speed = std::hypot(speed.x(), speed.y());
+        const double minimal_follow_time =
+            st_boundary_config_.minimal_follow_time();
+        characteristic_length =
+            std::fmax(scalar_speed * minimal_follow_time,
+                      std::fabs(obj_decision.follow().distance_s())) +
+            vehicle_param_.front_edge_to_center();
+
+        boundary_points.at(0).set_s(boundary_points.at(0).s());
+        boundary_points.at(1).set_s(boundary_points.at(1).s());
         boundary_points.at(3).set_t(-1.0);
         b_type = StGraphBoundary::BoundaryType::FOLLOW;
       } else if (obj_decision.has_yield()) {
         const double dis = std::fabs(obj_decision.yield().distance_s());
+        characteristic_length = dis;
         // TODO(all): remove the arbitrary numbers in this part.
         if (boundary_points.at(0).s() - dis < 0.0) {
           boundary_points.at(0).set_s(
@@ -428,6 +438,7 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
         b_type = StGraphBoundary::BoundaryType::YIELD;
       } else if (obj_decision.has_overtake()) {
         const double dis = std::fabs(obj_decision.overtake().distance_s());
+        characteristic_length = dis;
         boundary_points.at(2).set_s(boundary_points.at(2).s() + dis);
         boundary_points.at(3).set_s(boundary_points.at(3).s() + dis);
       }
@@ -437,15 +448,15 @@ Status StBoundaryMapper::MapObstacleWithPredictionTrajectory(
         boundary->emplace_back(&path_obstacle, boundary_points);
         boundary->back().SetBoundaryType(b_type);
         boundary->back().set_id(obstacle->Id());
+        boundary->back().SetCharacteristicLength(characteristic_length);
         skip = false;
       }
     }
-  }
     return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP")
                 : Status::OK();
-}
+  }
 
-Status StBoundaryMapper::MapFollowDecision(
+  Status StBoundaryMapper::MapFollowDecision(
       const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
       StGraphBoundary* const boundary) const {
     if (!obj_decision.has_follow()) {
@@ -498,8 +509,8 @@ Status StBoundaryMapper::MapFollowDecision(
     const double s_min_lower = distance_to_obstacle;
     const double s_min_upper =
         std::max(distance_to_obstacle + 1.0, planning_distance_);
-  const double s_max_upper =
-      std::max(s_min_upper + planning_time_ * follow_speed, planning_distance_);
+    const double s_max_upper = std::max(
+        s_min_upper + planning_time_ * follow_speed, planning_distance_);
     const double s_max_lower = s_min_lower + planning_time_ * follow_speed;
 
     std::vector<STPoint> boundary_points;
@@ -528,10 +539,10 @@ Status StBoundaryMapper::MapFollowDecision(
     boundary->SetBoundaryType(StGraphBoundary::BoundaryType::FOLLOW);
     boundary->set_id(obstacle->Id());
     return Status::OK();
-}
+  }
 
-double StBoundaryMapper::GetArea(
-    const std::vector<STPoint>& boundary_points) const {
+  double StBoundaryMapper::GetArea(const std::vector<STPoint>& boundary_points)
+      const {
     if (boundary_points.size() < 3) {
       return 0.0;
     }
@@ -547,11 +558,11 @@ double StBoundaryMapper::GetArea(
       area += (ds1 * dt2 - ds2 * dt1);
     }
     return fabs(area * 0.5);
-}
+  }
 
-bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point,
-                                    const Box2d& obs_box,
-                                    const double buffer) const {
+  bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point,
+                                      const Box2d& obs_box, const double buffer)
+      const {
     const double mid_to_rear_center =
         vehicle_param_.length() / 2.0 - vehicle_param_.front_edge_to_center();
     const double x =
@@ -562,10 +573,10 @@ bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point,
         Box2d({x, y}, path_point.theta(), vehicle_param_.length() + 2 * buffer,
               vehicle_param_.width() + 2 * buffer);
     return obs_box.HasOverlap(adc_box);
-}
+  }
 
-Status StBoundaryMapper::GetSpeedLimits(
-    SpeedLimit* const speed_limit_data) const {
+  Status StBoundaryMapper::GetSpeedLimits(SpeedLimit * const speed_limit_data)
+      const {
     CHECK_NOTNULL(speed_limit_data);
 
     std::vector<double> speed_limits;
@@ -573,8 +584,8 @@ Status StBoundaryMapper::GetSpeedLimits(
       if (Double::Compare(path_point.s(), reference_line_.length()) > 0) {
         std::string msg = common::util::StrCat(
             "path length [", path_data_.discretized_path().Length(),
-          "] is LARGER than reference_line_ length [", reference_line_.length(),
-          "]. Please debug before proceeding.");
+            "] is LARGER than reference_line_ length [",
+            reference_line_.length(), "]. Please debug before proceeding.");
         AWARN << msg;
         break;
       }
@@ -595,7 +606,7 @@ Status StBoundaryMapper::GetSpeedLimits(
       speed_limit_data->AddSpeedLimit(path_point.s(), curr_speed_limit);
     }
     return Status::OK();
-}
+  }
 
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/st_graph/st_boundary_mapper.h

@@ -57,6 +57,11 @@ class StBoundaryMapper {
 
   double GetArea(const std::vector<STPoint>& boundary_points) const;
 
+  bool GetOverlapBoundaryPoints(
+      const std::vector<apollo::common::PathPoint>& path_points,
+      const Obstacle& obstacle, std::vector<STPoint>* upper_points,
+      std::vector<STPoint>* lower_points) const;
+
   apollo::common::Status MapObstacleWithoutDecision(
       const PathObstacle& path_obstacle,
       std::vector<StGraphBoundary>* const boundary) const;

modules/planning/optimizer/st_graph/st_graph_boundary.h

@@ -55,6 +55,7 @@ class StGraphBoundary : public common::math::Polygon2d {
 
   ~StGraphBoundary() = default;
 
+  bool IsEmpty() const { return points.empty(); }
   bool IsPointInBoundary(const StGraphPoint& st_graph_point) const;
   bool IsPointInBoundary(const STPoint& st_point) const;