Apollo path_bounds_decider解析
PathReuseDecider 是lanefollow 场景下，所调用的第 4 个 task，它的作用是采用重决策方式来生成fallback、pullover、changelane、borrowlane四种场景下的path_bound。
代码逻辑分析：proc(frame,reference_line_info)，frame存储referencelineinfo信息，从referencelineinfo->pathDecision()中可以拿到障碍物信息， frame.PlanningStartPoint()能拿到当前帧规划的planning_start_point信息。【好奇这些信息在哪存进去的？】
通过判断（FLAGS_enable_smarter_lane_change &&
reference_line_info->IsChangeLanePath()）来是否计算ChangeLane path_bound。在计算换道地点时，通过获取auto* lane_change_status=injector_->planning_context()->mutable_planning_status()->mutable_change_lane()；当lane_change_status->is_clear_to_change_lane()为true时表示车辆此时可以立即换道，否则只能从lane_change_status->lane_change_start_position()里拿（如果有），没有的话只能自己根据车的位置来计算+FLAGS_lane_change_prepare_length来算出换道起点。【好奇lane_change_status->is_clear_to_change_lane()在哪被置位的，怎么被置为true的？】
通过获取auto* pull_over_status = injector_->planning_context()->mutable_planning_status()->mutable_pull_over();来判断是否计算pull over path_bound。
通过判断reference_line_info->is_path_lane_borrow()和injector_->planning_context()->planning_status().path_decider()来确定是否产生借道path_boudn以及从那边借道的path_bound。当reference_line_info->is_path_lane_borrow()为false时，则只计算不借道的path_bound,当reference_line_info->is_path_lane_borrow()为true,则根据injector_->planning_context()->planning_status().path_decider()的内容来确定是产生向左借道path_bound还是向右借道path_bound。【好奇reference_line_info->is_path_lane_borrow()和injector_->planning_context()->planning_status().path_decider()的内容又是在哪填的？】
小总结：在看其他decider代码时，要特别注重下述几个数据的操作过程。
injector_->planning_context()->mutable_planning_status()->mutable_change_lane()
injector_->planning_context()->mutable_planning_status()->mutable_pull_over()
reference_line_info->is_path_lane_borrow()
injector_->planning_context()->planning_status().path_decider()
# 路径边界决策算法解析，以下信息摘自apollo/docs
## 1.fallback
fallback场景生成过程如上图所示。
fallback只考虑adc信息和静态道路信息，主要调用两个函数
- InitPathBoundary
```C++
bool PathBoundsDecider::InitPathBoundary(
...
// Starting from ADC's current position, increment until the horizon, and
// set lateral bounds to be infinite at every spot.
// 从adc当前位置开始，以0.5m为间隔取点，直到终点，将 [左, 右] 边界设置为double的 [lowerst, max]
for (double curr_s = adc_frenet_s_;
curr_s < std::fmin(adc_frenet_s_ +
std::fmax(kPathBoundsDeciderHorizon,
reference_line_info.GetCruiseSpeed() *
FLAGS_trajectory_time_length),
reference_line.Length());
curr_s += kPathBoundsDeciderResolution) {
path_bound->emplace_back(curr_s, std::numeric_limits::lowest(),
std::numeric_limits::max());
}
...}
```
- GetBoundaryFromLanesAndADC
```C++
// TODO(jiacheng): this function is to be retired soon.
bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
...
for (size_t i = 0; i < path_bound->size(); ++i) {
double curr_s = std::get<0>((*path_bound)[i]);
// 1. Get the current lane width at current point.获取当前点车道的宽度
if (!reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
&curr_lane_right_width)) {
AWARN << "Failed to get lane width at s = " << curr_s;
curr_lane_left_width = past_lane_left_width;
curr_lane_right_width = past_lane_right_width;
} else {...}
// 2. Get the neighbor lane widths at the current point.获取当前点相邻车道的宽度
double curr_neighbor_lane_width = 0.0;
if (CheckLaneBoundaryType(reference_line_info, curr_s, lane_borrow_info)) {
hdmap::Id neighbor_lane_id;
if (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW) {
// 借左车道
...
} else if (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW) {
// 借右车道
...
}
}
// 3. 根据道路宽度，adc的位置和速度计算合适的边界。
static constexpr double kMaxLateralAccelerations = 1.5;
double offset_to_map = 0.0;
reference_line.GetOffsetToMap(curr_s, &offset_to_map);
double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
adc_frenet_ld_ * adc_frenet_ld_ /
kMaxLateralAccelerations / 2.0;
// 向左车道借到，左边界会变成左侧车道左边界
double curr_left_bound_lane =
curr_lane_left_width + (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW
? curr_neighbor_lane_width
: 0.0);
// 和上面类似
double curr_right_bound_lane =
-curr_lane_right_width -
(lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW
? curr_neighbor_lane_width
: 0.0);
double curr_left_bound = 0.0; // 左边界
double curr_right_bound = 0.0; // 右边界
// 计算左边界和右边界
if (config_.path_bounds_decider_config()
.is_extend_lane_bounds_to_include_adc() ||
is_fallback_lanechange) {
// extend path bounds to include ADC in fallback or change lane path
// bounds.
double curr_left_bound_adc =
std::fmax(adc_l_to_lane_center_,
adc_l_to_lane_center_ + ADC_speed_buffer) +
GetBufferBetweenADCCenterAndEdge() + ADC_buffer;
curr_left_bound =
std::fmax(curr_left_bound_lane, curr_left_bound_adc) - offset_to_map;
double curr_right_bound_adc =
std::fmin(adc_l_to_lane_center_,
adc_l_to_lane_center_ + ADC_speed_buffer) -
GetBufferBetweenADCCenterAndEdge() - ADC_buffer;
curr_right_bound =
std::fmin(curr_right_bound_lane, curr_right_bound_adc) -
offset_to_map;
} else {
curr_left_bound = curr_left_bound_lane - offset_to_map;
curr_right_bound = curr_right_bound_lane - offset_to_map;
}
// 4. 更新边界.
if (!UpdatePathBoundaryWithBuffer(i, curr_left_bound, curr_right_bound,
path_bound, is_left_lane_boundary,
is_right_lane_boundary)) {
path_blocked_idx = static_cast(i);
}
... }
``` ## 2.pull over 靠边停车
当 bool plan_pull_over_path = pull_over_status->plan_pull_over_path();为true时才会计算靠边停车的path_bound
### （1）GetBoundaryFromRoads
与`GetBoundaryFromLanesAndADC`不同，`GetBoundaryFromRoads`函数根据道路信息计算出边界:
- 获取参考线信息
- 对路径上的点，逐点计算
+ 边界
+ 更新
### （2）GetBoundaryFromStaticObstacles
根据障碍车调整边界：
- 计算障碍车在frenet坐标系下的坐标
- 扫描线排序，S方向扫描
+ 只关注在路径边界内的障碍物
+ 只关注在adc前方的障碍物
+ 将障碍物分解为两个边界，开始和结束
- 映射障碍物ID
+ Adc能从左边通过为True，否则为False
- 逐个点的检查path路径上的障碍物
+ 根据新来的障碍物
+ 根据已有的障碍物
### （3）SearchPullOverPosition
搜索pull over位置的过程：
- 根据pull_over_status.pull_over_type()判断是前向搜索（pull over开头第一个点），还是后向搜索（pull over末尾后一个点）
- 两层循环，外层控制搜索的索引idx，内层控制进一步的索引（前向idx+1，后向idx-1）。
- 根据内外两层循环的索引，判断搜索到的空间是否满足宽度和长度要求，判断是否可以pull over
代码如下：
```C++
bool PathBoundsDecider::SearchPullOverPosition(
const Frame& frame, const ReferenceLineInfo& reference_line_info,
const std::vector>& path_bound,
std::tuple* const pull_over_configuration) {
const auto& pull_over_status =
injector_->planning_context()->planning_status().pull_over();
// 搜索方向，默认前向搜索
bool search_backward = false; // search FORWARD by default
double pull_over_s = 0.0;
if (pull_over_status.pull_over_type() ==
PullOverStatus::EMERGENCY_PULL_OVER) {...}
int idx = 0;
if (search_backward) {
// 后向搜索，定位pull over末尾的一个点.
idx = static_cast(path_bound.size()) - 1;
while (idx >= 0 && std::get<0>(path_bound[idx]) > pull_over_s) {
--idx;
}
} else {
// 前向搜索，定位emergency pull over开头后的第一个点.
while (idx < static_cast(path_bound.size()) &&
std::get<0>(path_bound[idx]) < pull_over_s) {
++idx;
}
}
// 为pull over搜索到一个可行的位置，主要是确定该区域的宽度和长度
const double pull_over_space_length =
kPulloverLonSearchCoeff *
VehicleConfigHelper::GetConfig().vehicle_param().length() -
FLAGS_obstacle_lon_start_buffer - FLAGS_obstacle_lon_end_buffer;
const double pull_over_space_width =
(kPulloverLatSearchCoeff - 1.0) *
VehicleConfigHelper::GetConfig().vehicle_param().width();
const double adc_half_width =
VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
// 2. Find a window that is close to road-edge.
/*
这里用了内外两层循环进行搜索，外层循环控制搜索的开始的端点idx。
内层控制另一个端点。根据找到的两个端点，判断区域是否可以pull over
*/
bool has_a_feasible_window = false;
while ((search_backward && idx >= 0 &&
std::get<0>(path_bound[idx]) - std::get<0>(path_bound.front()) >
pull_over_space_length) ||
(!search_backward && idx < static_cast(path_bound.size()) &&
std::get<0>(path_bound.back()) - std::get<0>(path_bound[idx]) >
pull_over_space_length)) {
while ((search_backward && j >= 0 &&
std::get<0>(path_bound[idx]) - std::get<0>(path_bound[j]) <
pull_over_space_length) ||
(!search_backward && j < static_cast(path_bound.size()) &&
std::get<0>(path_bound[j]) - std::get<0>(path_bound[idx]) <
pull_over_space_length)) {...}
// 找到可行区域，获取停车区域的位置和姿态
if (is_feasible_window) {
...
break;}
...} // 外层while
...
}
``` ## 3.lane change
当FLAGS_enable_smarter_lane_change &&reference_line_info->IsChangeLanePath()都为true时才会触发LaneChange path_bound计算
代码流程如下：
```C++
Status PathBoundsDecider::GenerateLaneChangePathBound(
const ReferenceLineInfo& reference_line_info,
std::vector>* const path_bound) {
// 1.初始化，和前面的步骤类似
if (!InitPathBoundary(reference_line_info, path_bound)) {...}
// 2. 根据道路和adc的信息获取一个大致的路径边界
std::string dummy_borrow_lane_type;
if (!GetBoundaryFromLanesAndADC(reference_line_info,
LaneBorrowInfo::NO_BORROW, 0.1, path_bound,
&dummy_borrow_lane_type, true)) {...}
// 3. Remove the S-length of target lane out of the path-bound.
GetBoundaryFromLaneChangeForbiddenZone(reference_line_info, path_bound);
// 根据静态障碍物调整边界.
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, &blocking_obstacle_id)) {...}
...
}
```
GetBoundaryFromLaneChangeForbiddenZone函数是lane change重要的函数。运行过程如下：
- 如果当前位置可以变道，则直接变道
- 如果有一个lane-change的起点，则直接使用它
- 逐个检查变道前的点的边界，改变边界的值（如果已经过了变道点，则返回）
```C++
void PathBoundsDecider::GetBoundaryFromLaneChangeForbiddenZone(
const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
// 1.当前位置直接变道。
auto* lane_change_status = injector_->planning_context()
->mutable_planning_status()
->mutable_change_lane();
if (lane_change_status->is_clear_to_change_lane()) {
ADEBUG << "Current position is clear to change lane. No need prep s.";
lane_change_status->set_exist_lane_change_start_position(false);
return;
}
// 2.如果已经有一个lane-change的起点，就直接使用它，否则再找一个
double lane_change_start_s = 0.0;
if (lane_change_status->exist_lane_change_start_position()) {
common::SLPoint point_sl;
reference_line.XYToSL(lane_change_status->lane_change_start_position(),
&point_sl);
lane_change_start_s = point_sl.s();
} else {
// TODO(jiacheng): train ML model to learn this.
// 设置为adc前方一段距离为变道起始点
lane_change_start_s = FLAGS_lane_change_prepare_length + adc_frenet_s_;
// Update the decided lane_change_start_s into planning-context.
// 更新变道起始点的信息
common::SLPoint lane_change_start_sl;
lane_change_start_sl.set_s(lane_change_start_s);
lane_change_start_sl.set_l(0.0);
common::math::Vec2d lane_change_start_xy;
reference_line.SLToXY(lane_change_start_sl, &lane_change_start_xy);
lane_change_status->set_exist_lane_change_start_position(true);
lane_change_status->mutable_lane_change_start_position()->set_x(
lane_change_start_xy.x());
lane_change_status->mutable_lane_change_start_position()->set_y(
lane_change_start_xy.y());
}
// Remove the target lane out of the path-boundary, up to the decided S.
// 逐个检查变道前的点的边界，改变边界的值
for (size_t i = 0; i < path_bound->size(); ++i) {
double curr_s = std::get<0>((*path_bound)[i]);
if (curr_s > lane_change_start_s) {
break;
}
double curr_lane_left_width = 0.0;
double curr_lane_right_width = 0.0;
double offset_to_map = 0.0;
reference_line.GetOffsetToMap(curr_s, &offset_to_map);
if (reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
&curr_lane_right_width)) {
double offset_to_lane_center = 0.0;
reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);
curr_lane_left_width += offset_to_lane_center;
curr_lane_right_width -= offset_to_lane_center;
}
curr_lane_left_width -= offset_to_map;
curr_lane_right_width += offset_to_map;
std::get<1>((*path_bound)[i]) =
adc_frenet_l_ > curr_lane_left_width
? curr_lane_left_width + GetBufferBetweenADCCenterAndEdge()
: std::get<1>((*path_bound)[i]);
std::get<1>((*path_bound)[i]) =
std::fmin(std::get<1>((*path_bound)[i]), adc_frenet_l_ - 0.1);
std::get<2>((*path_bound)[i]) =
adc_frenet_l_ < -curr_lane_right_width
? -curr_lane_right_width - GetBufferBetweenADCCenterAndEdge()
: std::get<2>((*path_bound)[i]);
std::get<2>((*path_bound)[i]) =
std::fmax(std::get<2>((*path_bound)[i]), adc_frenet_l_ + 0.1);
}
}
``` ## 4.Regular
通过判断reference_line_info->is_path_lane_borrow()存储的借道信息来生成path_bound
代码流程如下：
```C++
Status PathBoundsDecider::GenerateRegularPathBound(
const ReferenceLineInfo& reference_line_info,
const LaneBorrowInfo& lane_borrow_info, PathBound* const path_bound,
std::string* const blocking_obstacle_id,
std::string* const borrow_lane_type) {
// 1.初始化边界.
if (!InitPathBoundary(reference_line_info, path_bound)) {...}
// 2.根据adc位置和lane信息确定大致的边界
if (!GetBoundaryFromLanesAndADC(reference_line_info, lane_borrow_info, 0.1,
path_bound, borrow_lane_type)) {...}
// PathBoundsDebugString(*path_bound);
// 3.根据障碍物调整道路边界
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, blocking_obstacle_id)) {...}
...
}
```
流程和上面的几个基本类似，借道有三种类型
```C++
enum class LaneBorrowInfo {
LEFT_BORROW,
NO_BORROW,
RIGHT_BORROW,
};
```