Planning: refactor side-pass condition check to make room for side-pa…

…ssing moving obstacles.
seekme-ai · Nov 7, 2019 · 11a98b0 · 11a98b0
1 parent 53107ec
commit 11a98b0
Showing 1 changed file with 47 additions and 31 deletions.
diff --git a/modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc b/modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc
@@ -79,40 +79,56 @@ bool PathLaneBorrowDecider::IsNecessaryToBorrowLane(
     // If originally not borrowing neighbor lane:
     ADEBUG << "Blocking obstacle ID["
            << mutable_path_decider_status->front_static_obstacle_id() << "]";
-    if (HasSingleReferenceLine(frame) && IsWithinSidePassingSpeedADC(frame) &&
-        IsBlockingObstacleFarFromIntersection(reference_line_info) &&
-        IsLongTermBlockingObstacle() &&
-        IsBlockingObstacleWithinDestination(reference_line_info) &&
-        IsSidePassableObstacle(reference_line_info)) {
-      // switch to lane-borrowing
-      // set side-pass direction
-      const auto& path_decider_status =
-          PlanningContext::Instance()->planning_status().path_decider();
-      if (path_decider_status.decided_side_pass_direction().empty()) {
-        // first time init decided_side_pass_direction
-        bool left_borrowable;
-        bool right_borrowable;
-        CheckLaneBorrow(reference_line_info, &left_borrowable,
-                        &right_borrowable);
-        if (!left_borrowable && !right_borrowable) {
-          mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(
-              false);
-        } else {
-          mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(
-              true);
-          if (left_borrowable) {
-            mutable_path_decider_status->add_decided_side_pass_direction(
-                PathDeciderStatus::LEFT_BORROW);
-          }
-          if (right_borrowable) {
-            mutable_path_decider_status->add_decided_side_pass_direction(
-                PathDeciderStatus::RIGHT_BORROW);
-          }
+    // ADC requirements check for lane-borrowing:
+    if (!HasSingleReferenceLine(frame)) {
+      return false;
+    }
+    if (!IsWithinSidePassingSpeedADC(frame)) {
+      return false;
+    }
+
+    // Obstacle condition check for lane-borrowing:
+    if (!IsBlockingObstacleFarFromIntersection(reference_line_info)) {
+      return false;
+    }
+    if (!IsLongTermBlockingObstacle()) {
+      return false;
+    }
+    if (!IsBlockingObstacleWithinDestination(reference_line_info)) {
+      return false;
+    }
+    if (!IsSidePassableObstacle(reference_line_info)) {
+      return false;
+    }
+
+    // switch to lane-borrowing
+    // set side-pass direction
+    const auto& path_decider_status =
+        PlanningContext::Instance()->planning_status().path_decider();
+    if (path_decider_status.decided_side_pass_direction().empty()) {
+      // first time init decided_side_pass_direction
+      bool left_borrowable;
+      bool right_borrowable;
+      CheckLaneBorrow(reference_line_info, &left_borrowable,
+                      &right_borrowable);
+      if (!left_borrowable && !right_borrowable) {
+        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(
+            false);
+      } else {
+        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(
+            true);
+        if (left_borrowable) {
+          mutable_path_decider_status->add_decided_side_pass_direction(
+              PathDeciderStatus::LEFT_BORROW);
+        }
+        if (right_borrowable) {
+          mutable_path_decider_status->add_decided_side_pass_direction(
+              PathDeciderStatus::RIGHT_BORROW);
         }
       }
-
-      AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
     }
+
+    AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
   }
   return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
 }