path optimizer根據boundaries生成了很多條空間上的path
在進行速度規划之前，需要先驗證生成的path的合理性
file:modules/planning/tasks/deciders/path_assessment_decider/path_assessment_decider.cc
代碼的注釋比較清晰，decider主要分成四部分：
- Remove invalid path.
- Analyze and add important info for speed decider to use
- Pick the optimal path.
- Update necessary info for lane-borrow decider's future uses.
1.Remove invalid path
Is valid fallback path
設置了3個條件來驗證fallbackpath的合理性
- 軌跡點不為空
- 距離參考線不是特別遠(20m)
- 距離左右車道線不是特別遠（10m）
// Basic sanity checks.
if (path_data.Empty()) {
ADEBUG << "Fallback Path: path data is empty.";
return false;
}
// Check if the path is greatly off the reference line.
if (IsGreatlyOffReferenceLine(path_data)) {
ADEBUG << "Fallback Path: ADC is greatly off reference line.";
return false;
}
// Check if the path is greatly off the road.
if (IsGreatlyOffRoad(reference_line_info, path_data)) {
ADEBUG << "Fallback Path: ADC is greatly off road.";
return false;
}
return true;
Is valid regular path
設置了5個條件來驗證常規path的合理性
- 軌跡點不為空
- 距離參考線不是特別遠(20m)
- 距離左右車道線不是特別遠（10m）
- 與靜態障礙物沒有發生碰撞：path不應當與靜態障礙物發生碰撞
- 不會停在反向道路上：檢查終點不在目標車道上
// Basic sanity checks.
if (path_data.Empty()) {
ADEBUG << path_data.path_label() << ": path data is empty.";
return false;
}
// Check if the path is greatly off the reference line.
if (IsGreatlyOffReferenceLine(path_data)) {
ADEBUG << path_data.path_label() << ": ADC is greatly off reference line.";
return false;
}
// Check if the path is greatly off the road.
if (IsGreatlyOffRoad(reference_line_info, path_data)) {
ADEBUG << path_data.path_label() << ": ADC is greatly off road.";
return false;
}
// Check if there is any collision.
if (IsCollidingWithStaticObstacles(reference_line_info, path_data)) {
ADEBUG << path_data.path_label() << ": ADC has collision.";
return false;
}
if (IsStopOnReverseNeighborLane(reference_line_info, path_data)) {
ADEBUG << path_data.path_label() << ": stop at reverse neighbor lane";
return false;
}
2.Analyze and add important info for speed decider to use
添加一些信息供speed decider使用
對於涉及換道的path，需要標記出換道點的位置（in lane 、 out lane）
對於block的軌跡，需要標記出block的位置
3. Pick the optimal path.
挑選最好的path，根據設置的規則對所有路徑按優先級進行排序
std::sort(valid_path_data.begin(), valid_path_data.end(),
std::bind(ComparePathData, std::placeholders::_1,
std::placeholders::_2, blocking_obstacle_on_selflane));
看一下ComparePathData中設置的一些規則：
- fallback的path放在最后：
// Regular path goes before fallback path.
bool lhs_is_regular = lhs.path_label().find("regular") != std::string::npos;
bool rhs_is_regular = rhs.path_label().find("regular") != std::string::npos;
if (lhs_is_regular != rhs_is_regular) {
return lhs_is_regular;
}
- 挑選更長的path： 例如在借道避讓場景下，向左避讓一般比向右避讓優先級更高；但是當右避讓的path比左避讓的path更長時（tolerance = 15m），優先選擇右避讓的path；對於換道的path，可以設置更高的tolerance（25m ）
// Select longer path.
// If roughly same length, then select self-lane path.
bool lhs_on_selflane = lhs.path_label().find("self") != std::string::npos;
bool rhs_on_selflane = rhs.path_label().find("self") != std::string::npos;
static constexpr double kSelfPathLengthComparisonTolerance = 15.0;
static constexpr double kNeighborPathLengthComparisonTolerance = 25.0;
double lhs_path_length = lhs.frenet_frame_path().back().s();
double rhs_path_length = rhs.frenet_frame_path().back().s();
if (lhs_on_selflane || rhs_on_selflane) {
if (std::fabs(lhs_path_length - rhs_path_length) >
kSelfPathLengthComparisonTolerance) {
return lhs_path_length > rhs_path_length;
} else {
return lhs_on_selflane;
}
} else {
if (std::fabs(lhs_path_length - rhs_path_length) >
kNeighborPathLengthComparisonTolerance) {
return lhs_path_length > rhs_path_length;
}
}
- 第二步留了一種換道的場景：自車必須要進行換道避障，左右避障的path長度又差不多，該怎么選？首先排除有一側（一般左側）有逆行的道路的情況
// If roughly same length, and must borrow neighbor lane,
// then prefer to borrow forward lane rather than reverse lane.
int lhs_on_reverse =
ContainsOutOnReverseLane(lhs.path_point_decision_guide());
int rhs_on_reverse =
ContainsOutOnReverseLane(rhs.path_point_decision_guide());
// TODO(jiacheng): make this a flag.
if (std::abs(lhs_on_reverse - rhs_on_reverse) > 6) {
return lhs_on_reverse < rhs_on_reverse;
}
剩下的情況就是兩邊都可以避，path也差不多的情況。這種情況下盡量選擇一條更為方便的路徑（convenient path），如果當前的path有blocking Obstacle，那就看Obstacle的位置，Obstacle靠左就右避讓，靠右的左避讓；如果沒有Obstacle，就選擇較近的path進行避讓（l值更小）。
/ For two lane-borrow directions, based on ADC's position,
// select the more convenient one.
if ((lhs.path_label().find("left") != std::string::npos &&
rhs.path_label().find("right") != std::string::npos) ||
(lhs.path_label().find("right") != std::string::npos &&
rhs.path_label().find("left") != std::string::npos)) {
if (blocking_obstacle) {
// select left/right path based on blocking_obstacle's position
const double obstacle_l =
(blocking_obstacle->PerceptionSLBoundary().start_l() +
blocking_obstacle->PerceptionSLBoundary().end_l()) /
2;
ADEBUG << "obstacle[" << blocking_obstacle->Id() << "] l[" << obstacle_l
<< "]";
return (obstacle_l > 0.0
? (lhs.path_label().find("right") != std::string::npos)
: (lhs.path_label().find("left") != std::string::npos));
} else {
// select left/right path based on ADC's position
double adc_l = lhs.frenet_frame_path().front().l();
if (adc_l < -1.0) {
return lhs.path_label().find("right") != std::string::npos;
} else if (adc_l > 1.0) {
return lhs.path_label().find("left") != std::string::npos;
}
}
}
// If same length, both neighbor lane are forward,
// then select the one that returns to in-lane earlier.
static constexpr double kBackToSelfLaneComparisonTolerance = 20.0;
int lhs_back_idx = GetBackToInLaneIndex(lhs.path_point_decision_guide());
int rhs_back_idx = GetBackToInLaneIndex(rhs.path_point_decision_guide());
double lhs_back_s = lhs.frenet_frame_path()[lhs_back_idx].s();
double rhs_back_s = rhs.frenet_frame_path()[rhs_back_idx].s();
if (std::fabs(lhs_back_s - rhs_back_s) > kBackToSelfLaneComparisonTolerance) {
return lhs_back_idx < rhs_back_idx;
}
// If same length, both forward, back to inlane at same time,
// select the left one to side-pass.
bool lhs_on_leftlane = lhs.path_label().find("left") != std::string::npos;
bool rhs_on_leftlane = rhs.path_label().find("left") != std::string::npos;
if (lhs_on_leftlane != rhs_on_leftlane) {
ADEBUG << "Select " << (lhs_on_leftlane ? "left" : "right") << " lane over "
<< (!lhs_on_leftlane ? "left" : "right") << " lane.";
return lhs_on_leftlane;
}
- Update necessary info for lane-borrow decider's future uses.
在借道的路徑上添加一些信息，包括：
- 前方障礙物的counter
- 當前車道可用的counter
- 當前的借道方向
這里只是添加對應的status，具體如何使用還需要看后續的speed decider