From 11a98b058f85f3da1178ab2673e7fe00b63f012e Mon Sep 17 00:00:00 2001 From: panjiacheng Date: Wed, 6 Nov 2019 17:20:16 -0800 Subject: [PATCH] Planning: refactor side-pass condition check to make room for side-passing moving obstacles. --- .../path_lane_borrow_decider.cc | 78 +++++++++++-------- 1 file changed, 47 insertions(+), 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 index 5e72ad01e0c..ba8f49c0782 100644 --- 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(); }