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);
+}
 
-    if (lower_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 && 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,165 +448,165 @@ 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();
   }
-  return skip ? Status(ErrorCode::PLANNING_SKIP, "PLANNING_SKIP")
-              : Status::OK();
-}
 
-Status StBoundaryMapper::MapFollowDecision(
-    const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
-    StGraphBoundary* const boundary) const {
-  if (!obj_decision.has_follow()) {
-    std::string msg = common::util::StrCat(
-        "Map obstacle without prediction trajectory is ONLY supported when "
-        "the "
-        "object decision is follow. The current object decision is: \n",
-        obj_decision.DebugString());
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
+  Status StBoundaryMapper::MapFollowDecision(
+      const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
+      StGraphBoundary* const boundary) const {
+    if (!obj_decision.has_follow()) {
+      std::string msg = common::util::StrCat(
+          "Map obstacle without prediction trajectory is ONLY supported when "
+          "the "
+          "object decision is follow. The current object decision is: \n",
+          obj_decision.DebugString());
+      AERROR << msg;
+      return Status(ErrorCode::PLANNING_ERROR, msg);
+    }
 
-  const auto* obstacle = path_obstacle.Obstacle();
+    const auto* obstacle = path_obstacle.Obstacle();
 
-  const auto& speed = obstacle->Perception().velocity();
-  const double scalar_speed = std::hypot(speed.x(), speed.y());
-
-  const auto& perception = obstacle->Perception();
-  const PathPoint ref_point = reference_line_.get_reference_point(
-      perception.position().x(), perception.position().y());
-  const double speed_coeff = std::cos(perception.theta() - ref_point.theta());
-  if (speed_coeff < 0.0) {
-    AERROR << "Obstacle is moving opposite to the reference line. Obstacle: "
-           << perception.DebugString();
-    return common::Status(ErrorCode::PLANNING_ERROR,
-                          "obstacle is moving opposite the reference line");
-  }
+    const auto& speed = obstacle->Perception().velocity();
+    const double scalar_speed = std::hypot(speed.x(), speed.y());
 
-  const auto& point = path_data_.discretized_path().StartPoint();
-  const PathPoint curr_point =
-      reference_line_.get_reference_point(point.x(), point.y());
-  const double distance_to_obstacle = ref_point.s() - curr_point.s() -
-                                      vehicle_param_.front_edge_to_center() -
-                                      st_boundary_config_.follow_buffer();
-
-  if (distance_to_obstacle > planning_distance_) {
-    std::string msg = "obstacle is out of range.";
-    AINFO << msg;
-    return Status(ErrorCode::PLANNING_SKIP, msg);
-  }
+    const auto& perception = obstacle->Perception();
+    const PathPoint ref_point = reference_line_.get_reference_point(
+        perception.position().x(), perception.position().y());
+    const double speed_coeff = std::cos(perception.theta() - ref_point.theta());
+    if (speed_coeff < 0.0) {
+      AERROR << "Obstacle is moving opposite to the reference line. Obstacle: "
+             << perception.DebugString();
+      return common::Status(ErrorCode::PLANNING_ERROR,
+                            "obstacle is moving opposite the reference line");
+    }
 
-  double follow_speed = 0.0;
-  if (scalar_speed > st_boundary_config_.follow_speed_threshold()) {
-    follow_speed = st_boundary_config_.follow_speed_threshold() * speed_coeff;
-  } else {
-    follow_speed = scalar_speed * speed_coeff *
-                   st_boundary_config_.follow_speed_damping_factor();
-  }
+    const auto& point = path_data_.discretized_path().StartPoint();
+    const PathPoint curr_point =
+        reference_line_.get_reference_point(point.x(), point.y());
+    const double distance_to_obstacle = ref_point.s() - curr_point.s() -
+                                        vehicle_param_.front_edge_to_center() -
+                                        st_boundary_config_.follow_buffer();
+
+    if (distance_to_obstacle > planning_distance_) {
+      std::string msg = "obstacle is out of range.";
+      AINFO << msg;
+      return Status(ErrorCode::PLANNING_SKIP, msg);
+    }
 
-  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_lower = s_min_lower + planning_time_ * follow_speed;
+    double follow_speed = 0.0;
+    if (scalar_speed > st_boundary_config_.follow_speed_threshold()) {
+      follow_speed = st_boundary_config_.follow_speed_threshold() * speed_coeff;
+    } else {
+      follow_speed = scalar_speed * speed_coeff *
+                     st_boundary_config_.follow_speed_damping_factor();
+    }
 
-  std::vector<STPoint> boundary_points;
-  boundary_points.emplace_back(s_min_lower, 0.0);
-  boundary_points.emplace_back(s_max_lower, planning_time_);
-  boundary_points.emplace_back(s_max_upper, planning_time_);
-  boundary_points.emplace_back(s_min_upper, 0.0);
-
-  const double area = GetArea(boundary_points);
-  if (Double::Compare(area, 0.0) <= 0) {
-    std::string msg = "Do not need to map because area is zero.";
-    AINFO << msg;
-    return Status(ErrorCode::PLANNING_SKIP, msg);
-  }
-  *boundary = StGraphBoundary(&path_obstacle, boundary_points);
-
-  const double characteristic_length =
-      std::fmax(scalar_speed * speed_coeff *
-                    st_boundary_config_.minimal_follow_time(),
-                std::fabs(obj_decision.follow().distance_s())) +
-      vehicle_param_.front_edge_to_center() +
-      st_boundary_config_.follow_buffer();
-
-  boundary->SetCharacteristicLength(characteristic_length *
-                                    st_boundary_config_.follow_coeff());
-  boundary->SetBoundaryType(StGraphBoundary::BoundaryType::FOLLOW);
-  boundary->set_id(obstacle->Id());
-  return Status::OK();
-}
+    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_lower = s_min_lower + planning_time_ * follow_speed;
 
-double StBoundaryMapper::GetArea(
-    const std::vector<STPoint>& boundary_points) const {
-  if (boundary_points.size() < 3) {
-    return 0.0;
+    std::vector<STPoint> boundary_points;
+    boundary_points.emplace_back(s_min_lower, 0.0);
+    boundary_points.emplace_back(s_max_lower, planning_time_);
+    boundary_points.emplace_back(s_max_upper, planning_time_);
+    boundary_points.emplace_back(s_min_upper, 0.0);
+
+    const double area = GetArea(boundary_points);
+    if (Double::Compare(area, 0.0) <= 0) {
+      std::string msg = "Do not need to map because area is zero.";
+      AINFO << msg;
+      return Status(ErrorCode::PLANNING_SKIP, msg);
+    }
+    *boundary = StGraphBoundary(&path_obstacle, boundary_points);
+
+    const double characteristic_length =
+        std::fmax(scalar_speed * speed_coeff *
+                      st_boundary_config_.minimal_follow_time(),
+                  std::fabs(obj_decision.follow().distance_s())) +
+        vehicle_param_.front_edge_to_center() +
+        st_boundary_config_.follow_buffer();
+
+    boundary->SetCharacteristicLength(characteristic_length *
+                                      st_boundary_config_.follow_coeff());
+    boundary->SetBoundaryType(StGraphBoundary::BoundaryType::FOLLOW);
+    boundary->set_id(obstacle->Id());
+    return Status::OK();
   }
 
-  double area = 0.0;
-  for (uint32_t i = 2; i < boundary_points.size(); ++i) {
-    const double ds1 = boundary_points[i - 1].s() - boundary_points[0].s();
-    const double dt1 = boundary_points[i - 1].t() - boundary_points[0].t();
+  double StBoundaryMapper::GetArea(const std::vector<STPoint>& boundary_points)
+      const {
+    if (boundary_points.size() < 3) {
+      return 0.0;
+    }
+
+    double area = 0.0;
+    for (uint32_t i = 2; i < boundary_points.size(); ++i) {
+      const double ds1 = boundary_points[i - 1].s() - boundary_points[0].s();
+      const double dt1 = boundary_points[i - 1].t() - boundary_points[0].t();
 
-    const double ds2 = boundary_points[i].s() - boundary_points[0].s();
-    const double dt2 = boundary_points[i].t() - boundary_points[0].t();
-    // cross product
-    area += (ds1 * dt2 - ds2 * dt1);
+      const double ds2 = boundary_points[i].s() - boundary_points[0].s();
+      const double dt2 = boundary_points[i].t() - boundary_points[0].t();
+      // cross product
+      area += (ds1 * dt2 - ds2 * dt1);
+    }
+    return fabs(area * 0.5);
   }
-  return fabs(area * 0.5);
-}
 
-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 =
-      path_point.x() - mid_to_rear_center * std::cos(path_point.theta());
-  const double y =
-      path_point.y() - mid_to_rear_center * std::sin(path_point.theta());
-  const Box2d adc_box =
-      Box2d({x, y}, path_point.theta(), vehicle_param_.length() + 2 * buffer,
-            vehicle_param_.width() + 2 * buffer);
-  return obs_box.HasOverlap(adc_box);
-}
+  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 =
+        path_point.x() - mid_to_rear_center * std::cos(path_point.theta());
+    const double y =
+        path_point.y() - mid_to_rear_center * std::sin(path_point.theta());
+    const Box2d adc_box =
+        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 {
-  CHECK_NOTNULL(speed_limit_data);
+  Status StBoundaryMapper::GetSpeedLimits(SpeedLimit * const speed_limit_data)
+      const {
+    CHECK_NOTNULL(speed_limit_data);
 
-  std::vector<double> speed_limits;
-  for (const auto& path_point : path_data_.discretized_path().path_points()) {
-    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.");
-      AWARN << msg;
-      break;
-    }
+    std::vector<double> speed_limits;
+    for (const auto& path_point : path_data_.discretized_path().path_points()) {
+      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.");
+        AWARN << msg;
+        break;
+      }
 
-    double speed_limit_on_reference_line =
-        reference_line_.GetSpeedLimitFromS(path_point.s());
+      double speed_limit_on_reference_line =
+          reference_line_.GetSpeedLimitFromS(path_point.s());
 
-    // speed limit from path curvature
-    double speed_limit_on_path =
-        std::sqrt(st_boundary_config_.centric_acceleration_limit() /
-                  std::fmax(std::fabs(path_point.kappa()),
-                            st_boundary_config_.minimal_kappa()));
+      // speed limit from path curvature
+      double speed_limit_on_path =
+          std::sqrt(st_boundary_config_.centric_acceleration_limit() /
+                    std::fmax(std::fabs(path_point.kappa()),
+                              st_boundary_config_.minimal_kappa()));
 
-    const double curr_speed_limit = std::fmax(
-        st_boundary_config_.lowest_speed(),
-        std::fmin(speed_limit_on_path, speed_limit_on_reference_line));
+      const double curr_speed_limit = std::fmax(
+          st_boundary_config_.lowest_speed(),
+          std::fmin(speed_limit_on_path, speed_limit_on_reference_line));
 
-    speed_limit_data->AddSpeedLimit(path_point.s(), curr_speed_limit);
+      speed_limit_data->AddSpeedLimit(path_point.s(), curr_speed_limit);
+    }
+    return Status::OK();
   }
-  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;