Planning: refactored path_bounds_decider.

modules/planning/tasks/deciders/path_bounds_decider.cc

@@ -61,6 +61,9 @@ Status PathBoundsDecider::Process(
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(reference_line_info);
 
+  // Initialize.
+  InitPathBoundsDecider(*frame, *reference_line_info);
+
   // The decided path bounds should be in the format of: (s, l_min, l_max).
   PathBoundary fallback_path_boundaries;
   PathBoundary path_boundaries;
@@ -92,8 +95,9 @@ Status PathBoundsDecider::Process(
   }
 
   // Generate path boundaries.
-  std::string path_bounds_msg =
-      GenerateRegularPathBoundary(frame, reference_line_info, &path_boundaries);
+  std::string path_bounds_msg = GenerateRegularPathBoundary(
+      *frame, *reference_line_info, LaneBorrowInfo::NO_BORROW,
+      &path_boundaries);
   if (path_bounds_msg != "") {
     return Status(ErrorCode::PLANNING_ERROR, path_bounds_msg);
   }
@@ -122,44 +126,73 @@ Status PathBoundsDecider::Process(
   return Status::OK();
 }
 
-std::string PathBoundsDecider::GenerateRegularPathBoundary(
-    Frame* frame, ReferenceLineInfo* reference_line_info,
-    std::vector<std::tuple<double, double, double>>* const path_boundaries) {
-  // Sanity checks.
-  CHECK_NOTNULL(frame);
-  CHECK_NOTNULL(reference_line_info);
+void PathBoundsDecider::InitPathBoundsDecider(
+    const Frame& frame, const ReferenceLineInfo& reference_line_info) {
+  const ReferenceLine& reference_line =
+      reference_line_info.reference_line();
+  const common::TrajectoryPoint& planning_start_point =
+      frame.PlanningStartPoint();
+  // Reset variables.
+  blocking_obstacle_id_ = "";
+  adc_frenet_s_ = 0.0;
+  adc_frenet_l_ = 0.0;
+  adc_lane_width_ = 0.0;
 
+  // Initialize some private variables.
+  // ADC s/l info.
+  auto adc_frenet_position =
+      reference_line.GetFrenetPoint(planning_start_point.path_point());
+  adc_frenet_s_ = adc_frenet_position.s();
+  adc_frenet_l_ = adc_frenet_position.l();
+  auto adc_sl_info = reference_line.ToFrenetFrame(planning_start_point);
+  adc_frenet_sd_ = adc_sl_info.first[1];
+  adc_frenet_ld_ = adc_sl_info.second[1] * adc_frenet_sd_;
+  // ADC's lane width.
+  double lane_left_width = 0.0;
+  double lane_right_width = 0.0;
+  if (!reference_line.GetLaneWidth(adc_frenet_s_, &lane_left_width,
+                                   &lane_right_width)) {
+    AWARN << "Failed to get lane width at planning start point.";
+    adc_lane_width_ = kDefaultLaneWidth;
+  } else {
+    adc_lane_width_ = lane_left_width + lane_right_width;
+  }
+}
+
+std::string PathBoundsDecider::GenerateRegularPathBoundary(
+    const Frame& frame, const ReferenceLineInfo& reference_line_info,
+    const LaneBorrowInfo lane_borrow_info,
+    PathBoundary* const path_boundary) {
   // 1. Initialize the path boundaries to be an indefinitely large area.
-  if (!InitPathBoundary(reference_line_info->reference_line(),
-                        frame->PlanningStartPoint(), path_boundaries)) {
+  if (!InitPathBoundary(reference_line_info.reference_line(),
+                        path_boundary)) {
     const std::string msg = "Failed to initialize path boundaries.";
     AERROR << msg;
     return msg;
   }
-  // PathBoundsDebugString(*path_boundaries);
+  // PathBoundsDebugString(*path_boundary);
 
   // 2. Decide a rough boundary based on road info and ADC's position
-  if (!GetBoundaryFromLanesAndADC(reference_line_info->reference_line(), 0, 0.1,
-                                  path_boundaries)) {
+  if (!GetBoundaryFromLanesAndADC(reference_line_info.reference_line(),
+                                  0, 0.1, path_boundary)) {
     const std::string msg =
         "Failed to decide a rough boundary based on "
         "road information.";
     AERROR << msg;
     return msg;
   }
-  // PathBoundsDebugString(*path_boundaries);
+  // PathBoundsDebugString(*path_boundary);
 
   // 3. Fine-tune the boundary based on static obstacles
-  // TODO(all): in the future, add side-pass functionality.
-  if (!GetBoundaryFromStaticObstacles(reference_line_info->path_decision(),
-                                      path_boundaries)) {
+  if (!GetBoundaryFromStaticObstacles(
+          reference_line_info.path_decision(), path_boundary)) {
     const std::string msg =
         "Failed to decide fine tune the boundaries after "
         "taking into consideration all static obstacles.";
     AERROR << msg;
     return msg;
   }
-  // PathBoundsDebugString(*path_boundaries);
+  // PathBoundsDebugString(*path_boundary);
 
   // 4. Adjust the boundary considering dynamic obstacles
   // TODO(all): may need to implement this in the future.
@@ -170,14 +203,14 @@ std::string PathBoundsDecider::GenerateRegularPathBoundary(
 
 std::string PathBoundsDecider::GenerateFallbackPathBoundary(
     Frame* frame, ReferenceLineInfo* reference_line_info,
-    std::vector<std::tuple<double, double, double>>* const path_boundaries) {
+    PathBoundary* const path_boundaries) {
   // Sanity checks.
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(reference_line_info);
 
   // 1. Initialize the path boundaries to be an indefinitely large area.
   if (!InitPathBoundary(reference_line_info->reference_line(),
-                        frame->PlanningStartPoint(), path_boundaries)) {
+                        path_boundaries)) {
     const std::string msg = "Failed to initialize fallback path boundaries.";
     AERROR << msg;
     return msg;
@@ -200,38 +233,10 @@ std::string PathBoundsDecider::GenerateFallbackPathBoundary(
 }
 
 bool PathBoundsDecider::InitPathBoundary(
-    const ReferenceLine& reference_line,
-    const common::TrajectoryPoint& planning_start_point,
-    std::vector<std::tuple<double, double, double>>* const path_boundaries) {
+    const ReferenceLine& reference_line, PathBoundary* const path_boundary) {
   // Sanity checks.
-  CHECK_NOTNULL(path_boundaries);
-  path_boundaries->clear();
-
-  // Reset variables.
-  blocking_obstacle_id_ = "";
-  adc_frenet_s_ = 0.0;
-  adc_frenet_l_ = 0.0;
-  adc_lane_width_ = 0.0;
-
-  // Initialize some private variables.
-  // ADC s/l info.
-  auto adc_frenet_position =
-      reference_line.GetFrenetPoint(planning_start_point.path_point());
-  adc_frenet_s_ = adc_frenet_position.s();
-  adc_frenet_l_ = adc_frenet_position.l();
-  auto adc_sl_info = reference_line.ToFrenetFrame(planning_start_point);
-  adc_frenet_sd_ = adc_sl_info.first[1];
-  adc_frenet_ld_ = adc_sl_info.second[1] * adc_frenet_sd_;
-  // ADC's lane width.
-  double lane_left_width = 0.0;
-  double lane_right_width = 0.0;
-  if (!reference_line.GetLaneWidth(adc_frenet_s_, &lane_left_width,
-                                   &lane_right_width)) {
-    AWARN << "Failed to get lane width at planning start point.";
-    adc_lane_width_ = kDefaultLaneWidth;
-  } else {
-    adc_lane_width_ = lane_left_width + lane_right_width;
-  }
+  CHECK_NOTNULL(path_boundary);
+  path_boundary->clear();
 
   // Starting from ADC's current position, increment until the horizon, and
   // set lateral bounds to be infinite at every spot.
@@ -239,10 +244,15 @@ bool PathBoundsDecider::InitPathBoundary(
        curr_s < std::min(adc_frenet_s_ + kPathBoundsDeciderHorizon,
                          reference_line.Length());
        curr_s += kPathBoundsDeciderResolution) {
-    path_boundaries->emplace_back(curr_s, std::numeric_limits<double>::lowest(),
+    path_boundary->emplace_back(curr_s, std::numeric_limits<double>::lowest(),
                                   std::numeric_limits<double>::max());
   }
 
+  // return.
+  if (path_boundary->empty()) {
+    ADEBUG << "Empty path boundary in InitPathBoundary";
+    return false;
+  }
   return true;
 }
 
@@ -374,10 +384,10 @@ bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
 // obstacles whose headings differ from road-headings a lot.
 // TODO(all): (future work) this can be improved in the future.
 bool PathBoundsDecider::GetBoundaryFromStaticObstacles(
-    PathDecision* const path_decision,
+    const PathDecision& path_decision,
     std::vector<std::tuple<double, double, double>>* const path_boundaries) {
   // Preprocessing.
-  auto indexed_obstacles = path_decision->obstacles();
+  auto indexed_obstacles = path_decision.obstacles();
   auto sorted_obstacles = SortObstaclesForSweepLine(indexed_obstacles);
   double center_line = adc_frenet_l_;
   size_t obs_idx = 0;

modules/planning/tasks/deciders/path_bounds_decider.h

@@ -36,62 +36,75 @@ namespace planning {
 
 class PathBoundsDecider : public Decider {
  public:
+  enum class LaneBorrowInfo {
+    LEFT_BORROW,
+    NO_BORROW,
+    RIGHT_BORROW,
+  };
   explicit PathBoundsDecider(const TaskConfig& config);
 
  private:
   common::Status Process(Frame* frame,
                          ReferenceLineInfo* reference_line_info) override;
 
-  /**
-   * @brief: The regular path boundary generation considers the ADC itself
-   *   and other static environments:
-   *   - ADC's position (lane-changing considerations)
-   *   - lane info
-   *   - static obstacles
-   *   The philosophy is: static environment must be and can only be taken
-   *   care of by the path planning.
-   * @param: frame
-   * @param: reference_line_info
-   * @param: The generated regular path_boundary, if there is one.
-   * @return: A failure message. If succeeded, return "" (empty string).
-   */
+  void InitPathBoundsDecider(
+      const Frame& frame, const ReferenceLineInfo& reference_line_info);
+
+  /** @brief: The regular path boundary generation considers the ADC itself
+    *   and other static environments:
+    *   - ADC's position (lane-changing considerations)
+    *   - lane info
+    *   - static obstacles
+    *   The philosophy is: static environment must be and can only be taken
+    *   care of by the path planning.
+    * @param: frame
+    * @param: reference_line_info
+    * @param: lane_borrow_info: which lane to borrow.
+    * @param: The generated regular path_boundary, if there is one.
+    * @return: A failure message. If succeeded, return "" (empty string).
+    */
   std::string GenerateRegularPathBoundary(
-      Frame* frame, ReferenceLineInfo* reference_line_info,
-      std::vector<std::tuple<double, double, double>>* const path_boundaries);
-
-  /**
-   * @brief: The fallback path only considers:
-   *   - ADC's position (so that boundary must contain ADC's position)
-   *   - lane info
-   *   It is supposed to be the last resort in case regular path generation
-   *   fails so that speed decider can at least have some path and won't
-   *   fail drastically.
-   *   Therefore, it be reliable so that optimizer will not likely to
-   *   fail with this boundary, and therefore doesn't consider any static
-   *   obstacle. When the fallback path is used, stopping before static
-   *   obstacles should be taken care of by the speed decider. Also, it
-   *   doesn't consider any lane-borrowing.
-   * @param: frame
-   * @param: reference_line_info
-   * @param: The generated fallback path_boundary, if there is one.
-   * @return: A failure message. If succeeded, return "" (empty string).
-   */
+      const Frame& frame, const ReferenceLineInfo& reference_line_info,
+      const LaneBorrowInfo lane_borrow_info,
+      std::vector<std::tuple<double, double, double>>* const path_boundary);
+
+  /** @brief: The fallback path only considers:
+    *   - ADC's position (so that boundary must contain ADC's position)
+    *   - lane info
+    *   It is supposed to be the last resort in case regular path generation
+    *   fails so that speed decider can at least have some path and won't
+    *   fail drastically.
+    *   Therefore, it be reliable so that optimizer will not likely to
+    *   fail with this boundary, and therefore doesn't consider any static
+    *   obstacle. When the fallback path is used, stopping before static
+    *   obstacles should be taken care of by the speed decider. Also, it
+    *   doesn't consider any lane-borrowing.
+    * @param: frame
+    * @param: reference_line_info
+    * @param: The generated fallback path_boundary, if there is one.
+    * @return: A failure message. If succeeded, return "" (empty string).
+    */
   std::string GenerateFallbackPathBoundary(
       Frame* frame, ReferenceLineInfo* reference_line_info,
       std::vector<std::tuple<double, double, double>>* const path_boundaries);
 
-  bool InitPathBoundary(
-      const ReferenceLine& reference_line,
-      const common::TrajectoryPoint& planning_start_point,
-      std::vector<std::tuple<double, double, double>>* const path_boundaries);
+  /** @brief: Initializes an empty path boundary.
+    */
+  bool InitPathBoundary(const ReferenceLine& reference_line,
+      std::vector<std::tuple<double, double, double>>* const path_boundary);
 
+  /** @brief: Refine the boundary based on lane-info and ADC's location.
+    *   It will comply to the lane-boundary. However, if the ADC itself
+    *   is out of the given lane(s), it will adjust the boundary
+    *   accordingly to include ADC's current position.
+    */
   bool GetBoundaryFromLanesAndADC(
       const ReferenceLine& reference_line, int lane_borrowing,
       double ADC_buffer,
       std::vector<std::tuple<double, double, double>>* const path_boundaries);
 
   bool GetBoundaryFromStaticObstacles(
-      PathDecision* const path_decision,
+      const PathDecision& path_decision,
       std::vector<std::tuple<double, double, double>>* const path_boundaries);
 
   bool GetLaneInfoFromPoint(double point_x, double point_y, double point_z,
@@ -118,16 +131,15 @@ class PathBoundsDecider : public Decider {
       const std::vector<std::tuple<int, double, double, double, std::string>>&
           new_entering_obstacles);
 
-  /**
-   * @brief Update the path_boundary at "idx", as well as the new center-line.
-   *        It also checks if ADC is blocked (lmax < lmin).
-   * @param The current index of the path_bounds
-   * @param The minimum left boundary (l_max)
-   * @param The maximum right boundary (l_min)
-   * @param The path_boundaries (its content at idx will be updated)
-   * @param The center_line (to be updated)
-   * @return If path is good, true; if path is blocked, false.
-   */
+  /** @brief Update the path_boundary at "idx", as well as the new center-line.
+    *        It also checks if ADC is blocked (lmax < lmin).
+    * @param The current index of the path_bounds
+    * @param The minimum left boundary (l_max)
+    * @param The maximum right boundary (l_min)
+    * @param The path_boundaries (its content at idx will be updated)
+    * @param The center_line (to be updated)
+    * @return If path is good, true; if path is blocked, false.
+    */
   bool UpdatePathBoundaryAndCenterLine(
       size_t idx, double left_bound, double right_bound,
       std::vector<std::tuple<double, double, double>>* const path_boundaries,