PATH_BOUNDS_DECIDER
在这个decider的主要工作在Process()中完成，在该函数中，分4种场景对PathBound进行计算，按处理的顺序分别是fallback、pull over、lane change、regular，不同的boundary对应不同的应用场景，其中fallback对应的path bound一定会生成，其余3个只有一个被激活，即按照顺序一旦有有效的boundary生成，就结束该task。
输出和在阅读代码时常用到的数据结构：
// PathBoundPoint contains: (s, l_min, l_max).
using PathBoundPoint = std::tuple<double, double, double>;
// PathBound contains a vector of PathBoundPoints.
using PathBound = std::vector<PathBoundPoint>;
PATH_BOUNDS_DECIDER中主要处理逻辑在process函数中：
Status PathBoundsDecider::Process(
Frame* const frame, ReferenceLineInfo* const reference_line_info) {
// 检查输入信息
CHECK_NOTNULL(frame);
CHECK_NOTNULL(reference_line_info);
// 如果重复使用路径，则跳过路径边界决策。
if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {
return Status::OK();
}
std::vector<PathBoundary> candidate_path_boundaries;
// Initialization，这个函数后面会讲到
InitPathBoundsDecider(*frame, *reference_line_info);
// 生成fallback路径边界。
PathBound fallback_path_bound;
Status ret =
GenerateFallbackPathBound(*reference_line_info, &fallback_path_bound);
if (!ret.ok()) {
ADEBUG << "Cannot generate a fallback path bound.";
return Status(ErrorCode::PLANNING_ERROR, ret.error_message());
}
if (fallback_path_bound.empty()) {
const std::string msg = "Failed to get a valid fallback path boundary";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
if (!fallback_path_bound.empty()) {
CHECK_LE(adc_frenet_l_, std::get<2>(fallback_path_bound[0]));
CHECK_GE(adc_frenet_l_, std::get<1>(fallback_path_bound[0]));
}
// Update the fallback path boundary into the reference_line_info.
std::vector<std::pair<double, double>> fallback_path_bound_pair;
for (size_t i = 0; i < fallback_path_bound.size(); ++i) {
fallback_path_bound_pair.emplace_back(std::get<1>(fallback_path_bound[i]),
std::get<2>(fallback_path_bound[i]));
}
candidate_path_boundaries.emplace_back(std::get<0>(fallback_path_bound[0]),
kPathBoundsDeciderResolution,
fallback_path_bound_pair);
// 生成fallback界限，存入candidate_path_boundaries，并标记为“fallback”
candidate_path_boundaries.back().set_label("fallback");
// 以上的一套检查生成的path bound的流程，几乎四种情况大致逻辑类似，后面分析
// 剩下三个的情况的时候，忽略这些检查步骤
// If pull-over is requested, generate pull-over path boundary.
auto* pull_over_status = injector_->planning_context()
->mutable_planning_status()
->mutable_pull_over();
const bool plan_pull_over_path = pull_over_status->plan_pull_over_path();
// 生成停车边界
if (plan_pull_over_path) {
PathBound pull_over_path_bound;
Status ret = GeneratePullOverPathBound(*frame, *reference_line_info,
&pull_over_path_bound);
if (!ret.ok()) {
AWARN << "Cannot generate a pullover path bound, do regular planning.";
} else {
ACHECK(!pull_over_path_bound.empty());
CHECK_LE(adc_frenet_l_, std::get<2>(pull_over_path_bound[0]));
CHECK_GE(adc_frenet_l_, std::get<1>(pull_over_path_bound[0]));
// Update the fallback path boundary into the reference_line_info.
std::vector<std::pair<double, double>> pull_over_path_bound_pair;
for (size_t i = 0; i < pull_over_path_bound.size(); ++i) {
pull_over_path_bound_pair.emplace_back(
std::get<1>(pull_over_path_bound[i]),
std::get<2>(pull_over_path_bound[i]));
}
candidate_path_boundaries.emplace_back(
std::get<0>(pull_over_path_bound[0]), kPathBoundsDeciderResolution,
pull_over_path_bound_pair);
candidate_path_boundaries.back().set_label("regular/pullover");
// 生成pullover界限，存入candidate_path_boundaries，并标记为“regular/pullover”
reference_line_info->SetCandidatePathBoundaries(
std::move(candidate_path_boundaries));
ADEBUG << "Completed pullover and fallback path boundaries generation.";
// set debug info in planning_data
auto* pull_over_debug = reference_line_info->mutable_debug()
->mutable_planning_data()
->mutable_pull_over();
pull_over_debug->mutable_position()->CopyFrom(
pull_over_status->position());
pull_over_debug->set_theta(pull_over_status->theta());
pull_over_debug->set_length_front(pull_over_status->length_front());
pull_over_debug->set_length_back(pull_over_status->length_back());
pull_over_debug->set_width_left(pull_over_status->width_left());
pull_over_debug->set_width_right(pull_over_status->width_right());
return Status::OK();
}
}
// If it's a lane-change reference-line, generate lane-change path boundary.
// 生成换道边界
if (FLAGS_enable_smarter_lane_change &&
reference_line_info->IsChangeLanePath()) {
PathBound lanechange_path_bound;
Status ret = GenerateLaneChangePathBound(*reference_line_info,
&lanechange_path_bound);
if (!ret.ok()) {
ADEBUG << "Cannot generate a lane-change path bound.";
return Status(ErrorCode::PLANNING_ERROR, ret.error_message());
}
if (lanechange_path_bound.empty()) {
const std::string msg = "Failed to get a valid fallback path boundary";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// disable this change when not extending lane bounds to include adc
if (config_.path_bounds_decider_config()
.is_extend_lane_bounds_to_include_adc()) {
CHECK_LE(adc_frenet_l_, std::get<2>(lanechange_path_bound[0]));
CHECK_GE(adc_frenet_l_, std::get<1>(lanechange_path_bound[0]));
}
// Update the fallback path boundary into the reference_line_info.
std::vector<std::pair<double, double>> lanechange_path_bound_pair;
for (size_t i = 0; i < lanechange_path_bound.size(); ++i) {
lanechange_path_bound_pair.emplace_back(
std::get<1>(lanechange_path_bound[i]),
std::get<2>(lanechange_path_bound[i]));
}
candidate_path_boundaries.emplace_back(
std::get<0>(lanechange_path_bound[0]), kPathBoundsDeciderResolution,
lanechange_path_bound_pair);
candidate_path_boundaries.back().set_label("regular/lanechange");
RecordDebugInfo(lanechange_path_bound, "", reference_line_info);
// 生成lanechange界限，存入candidate_path_boundaries，并标记为“regular/lanechange”
reference_line_info->SetCandidatePathBoundaries(
std::move(candidate_path_boundaries));
ADEBUG << "Completed lanechange and fallback path boundaries generation.";
return Status::OK();
}
// Generate regular path boundaries.
// 生成regular路径边界
std::vector<LaneBorrowInfo> lane_borrow_info_list;
lane_borrow_info_list.push_back(LaneBorrowInfo::NO_BORROW);
if (reference_line_info->is_path_lane_borrow()) {
const auto& path_decider_status =
injector_->planning_context()->planning_status().path_decider();
for (const auto& lane_borrow_direction :
path_decider_status.decided_side_pass_direction()) {
if (lane_borrow_direction == PathDeciderStatus::LEFT_BORROW) {
lane_borrow_info_list.push_back(LaneBorrowInfo::LEFT_BORROW);
} else if (lane_borrow_direction == PathDeciderStatus::RIGHT_BORROW) {
lane_borrow_info_list.push_back(LaneBorrowInfo::RIGHT_BORROW);
}
}
}
// Try every possible lane-borrow option:
// PathBound regular_self_path_bound;
// bool exist_self_path_bound = false;
for (const auto& lane_borrow_info : lane_borrow_info_list) {
PathBound regular_path_bound;
std::string blocking_obstacle_id = "";
std::string borrow_lane_type = "";
Status ret = GenerateRegularPathBound(
*reference_line_info, lane_borrow_info, ®ular_path_bound,
&blocking_obstacle_id, &borrow_lane_type);
if (!ret.ok()) {
continue;
}
if (regular_path_bound.empty()) {
continue;
}
// disable this change when not extending lane bounds to include adc
if (config_.path_bounds_decider_config()
.is_extend_lane_bounds_to_include_adc()) {
CHECK_LE(adc_frenet_l_, std::get<2>(regular_path_bound[0]));
CHECK_GE(adc_frenet_l_, std::get<1>(regular_path_bound[0]));
}
// Update the path boundary into the reference_line_info.
std::vector<std::pair<double, double>> regular_path_bound_pair;
for (size_t i = 0; i < regular_path_bound.size(); ++i) {
regular_path_bound_pair.emplace_back(std::get<1>(regular_path_bound[i]),
std::get<2>(regular_path_bound[i]));
}
candidate_path_boundaries.emplace_back(std::get<0>(regular_path_bound[0]),
kPathBoundsDeciderResolution,
regular_path_bound_pair);
std::string path_label = "";
switch (lane_borrow_info) {
case LaneBorrowInfo::LEFT_BORROW:
path_label = "left";
break;
case LaneBorrowInfo::RIGHT_BORROW:
path_label = "right";
break;
default:
path_label = "self";
// exist_self_path_bound = true;
// regular_self_path_bound = regular_path_bound;
break;
}
// RecordDebugInfo(regular_path_bound, "", reference_line_info);
// 生成laneborrow界限，存入candidate_path_boundaries，并标记为“regular/...”
candidate_path_boundaries.back().set_label(
absl::StrCat("regular/", path_label, "/", borrow_lane_type));
candidate_path_boundaries.back().set_blocking_obstacle_id(
blocking_obstacle_id);
}
// Remove redundant boundaries.
// RemoveRedundantPathBoundaries(&candidate_path_boundaries);
// Success
reference_line_info->SetCandidatePathBoundaries(
std::move(candidate_path_boundaries));
ADEBUG << "Completed regular and fallback path boundaries generation.";
return Status::OK();
}
1.fall back
fallback是其他3种场景计算PathBound失败时的备选，只考虑自车信息和静态道路信息，不考虑静态障碍物。因此，speed decider负责让自车在障碍物前停车。
// fallback倒车场景
Status PathBoundsDecider::GenerateFallbackPathBound(
const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
// 1.将路径边界初始化为无限大的区域。
// InitPathBoundary(reference_line_info, path_bound))这个函数较好理解，就不单独拿出来分析
// 就是先将InitPathBoundary()在合理的s范围内，以kPathBoundsDeciderResolution为采样间隔对s采样，
// 将每个采样点处的横向边界（右边界，左边界）设定为最小值和最大值
if (!InitPathBoundary(reference_line_info, path_bound)) {
const std::string msg = "Failed to initialize fallback path boundaries.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// 2.根据车道信息和ADC位置确定粗略边界
// GetBoundaryFromLanesAndADC（...）这个函数在后文单独拿出来说一下
std::string dummy_borrow_lane_type;
if (!GetBoundaryFromLanesAndADC(reference_line_info,
LaneBorrowInfo::NO_BORROW, 0.5, path_bound,
&dummy_borrow_lane_type, true)) {
const std::string msg =
"Failed to decide a rough fallback boundary based on "
"road information.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
ADEBUG << "Completed generating fallback path boundaries.";
return Status::OK();
}
GetBoundaryFromLanesAndADC则包含了根据自车信息、车道信息计算PathBound的具体细节，具体逻辑在代码中体现出来了。
bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
const ReferenceLineInfo& reference_line_info,
const LaneBorrowInfo& lane_borrow_info, double ADC_buffer,
PathBound* const path_bound, std::string* const borrow_lane_type,
bool is_fallback_lanechange) {
// 输入信息检查
CHECK_NOTNULL(path_bound);
ACHECK(!path_bound->empty());
const ReferenceLine& reference_line = reference_line_info.reference_line();
bool is_left_lane_boundary = true;
bool is_right_lane_boundary = true;
const double boundary_buffer = 0.05; // meter
// 通过每个点，根据车道信息和ADC的位置更新边界。
double past_lane_left_width = adc_lane_width_ / 2.0;
double past_lane_right_width = adc_lane_width_ / 2.0;
// 路径阻塞idx
int path_blocked_idx = -1;
bool borrowing_reverse_lane = false;
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.
double curr_lane_left_width = 0.0;
double curr_lane_right_width = 0.0;
double offset_to_lane_center = 0.0;
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 {
// 检查车道边界是否也是道路边界
double curr_road_left_width = 0.0;
double curr_road_right_width = 0.0;
if (reference_line.GetRoadWidth(curr_s, &curr_road_left_width,
&curr_road_right_width)) {
// 获取每一点boundary处对应的车道宽度及道路宽度，然后比较左右车道及道路宽度的值，
// 判断车道边界是否就是道路边界，
is_left_lane_boundary =
(std::abs(curr_road_left_width - curr_lane_left_width) >
boundary_buffer);
is_right_lane_boundary =
(std::abs(curr_road_right_width - curr_lane_right_width) >
boundary_buffer);
}
/*
最后根据相对地图的偏移量计算当前边界点所对应的车道宽度；
计算结果为 车道的左右宽度curr_lane_left_width， curr_lane_right_width
*/
/*
GetLaneWidth获取的地图中设定的道路的左右侧宽度，
GetOffsetToMap获取参考线相对于车道中心线的偏移量，
根据两个函数得到的结果计算得到的是相对于参考线的宽度
*/
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;
past_lane_left_width = curr_lane_left_width;
past_lane_right_width = curr_lane_right_width;
}
// 2. Get the neighbor lane widths at the current point.
double curr_neighbor_lane_width = 0.0;
//CheckLaneBoundaryType()在向左或向右借道时，返回true
if (CheckLaneBoundaryType(reference_line_info, curr_s, lane_borrow_info)) {
hdmap::Id neighbor_lane_id;
if (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW) {
// Borrowing left neighbor lane. 向左侧同向顺行车道借道
if (reference_line_info.GetNeighborLaneInfo(
ReferenceLineInfo::LaneType::LeftForward, curr_s,
&neighbor_lane_id, &curr_neighbor_lane_width)) {
ADEBUG << "Borrow left forward neighbor lane.";
} else if (reference_line_info.GetNeighborLaneInfo(
ReferenceLineInfo::LaneType::LeftReverse, curr_s,
&neighbor_lane_id, &curr_neighbor_lane_width)) {
borrowing_reverse_lane = true;//向左侧反向逆行车道借道
ADEBUG << "Borrow left reverse neighbor lane.";
} else {
ADEBUG << "There is no left neighbor lane.";
}
}
// 右侧同理左侧
else if (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW) {
// Borrowing right neighbor lane.
if (reference_line_info.GetNeighborLaneInfo(
ReferenceLineInfo::LaneType::RightForward, curr_s,
&neighbor_lane_id, &curr_neighbor_lane_width)) {
ADEBUG << "Borrow right forward neighbor lane.";
} else if (reference_line_info.GetNeighborLaneInfo(
ReferenceLineInfo::LaneType::RightReverse, curr_s,
&neighbor_lane_id, &curr_neighbor_lane_width)) {
borrowing_reverse_lane = true;
ADEBUG << "Borrow right reverse neighbor lane.";
} else {
ADEBUG << "There is no right neighbor lane.";
}
}
}
// 根据车道宽度、ADC位置和ADC速度计算适当的边界。
static constexpr double kMaxLateralAccelerations = 1.5;
double offset_to_map = 0.0;
reference_line.GetOffsetToMap(curr_s, &offset_to_map);
//给定初始速度和加速度，刹停行驶的距离（横向）
//对横向来说，就是横向速度变为0的过程行驶的横向位移
//ADC_speed_buffer是按照最大横向加速度车辆横向速度降为0所需要的横向距离
double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
adc_frenet_ld_ * adc_frenet_ld_ /
kMaxLateralAccelerations / 2.0;
// curr_left_bound_lane、curr_right_bound_lane基于车道边界的左右边界，如果需要借道，则为当前车道宽度加上相邻车道宽度；
// 如果要向左借道，可行驶道路左边界会变为左侧道路左边界
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;
// 根据配置文件中的is_extend_lane_bounds_to_include_adc（计算的边界值是否需要考虑自车状态）以及上述三组变量计算左右边界值
/*
如果需要考虑自车状态，则在计算时边界值为同时考虑基于自车信息的左右边界和基于车道边界的左右边界，取两者的边界较大值；若不需要考虑自车状态，
则只需以基于车道边界的左右边界作为边界值。
另外需要注意的是，得到的边界值curr_left_bound、 curr_right_bound是基于参考线的，因此需要考虑参考线相对map的偏移量offset_to_map。
*/
if (config_.path_bounds_decider_config()
.is_extend_lane_bounds_to_include_adc() ||
is_fallback_lanechange) {
//扩展路径边界以将ADC包括在回退或更改通道路径边界中。
/*
curr_left_bound_adc、curr_right_bound_adc，基于自车信息的左右边界，
根据ADC_speed_buffer，ADC_buffer（0.5，可以认为是横向安全区间）,
半车宽，adc_l_to_lane_center_（自车相对于车道中心的偏移量）计算
*/
double curr_left_bound_adc =
std::fmax(adc_l_to_lane_center_,
adc_l_to_lane_center_ + ADC_speed_buffer) +
// 获取ADC中心和边缘之间的缓冲区
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) -
// 获取ADC中心和边缘之间的缓冲区
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;
}
ADEBUG << "At s = " << curr_s
<< ", left_lane_bound = " << curr_lane_left_width
<< ", right_lane_bound = " << curr_lane_right_width
<< ", offset = " << offset_to_map;
// 4. Update the boundary.
/*
调用函数UpdatePathBoundaryWithBuffer（）根据上述得到的边界值curr_left_bound、 curr_right_bound，
以及车道边界是否是道路边界的判定结果is_left_lane_boundary，is_right_lane_boundary更新boundary
*/
if (!UpdatePathBoundaryWithBuffer(i, curr_left_bound, curr_right_bound,
path_bound, is_left_lane_boundary,
is_right_lane_boundary)) {
path_blocked_idx = static_cast<int>(i);
}
if (path_blocked_idx != -1) {
break;
}
}
// 从boundary中删除被截断之后的边界点
TrimPathBounds(path_blocked_idx, path_bound);
if (lane_borrow_info == LaneBorrowInfo::NO_BORROW) {
*borrow_lane_type = "";
} else {
*borrow_lane_type = borrowing_reverse_lane ? "reverse" : "forward";
}
return true;
}
2.pull over
Status PathBoundsDecider::GeneratePullOverPathBound(
const Frame& frame, const ReferenceLineInfo& reference_line_info,
PathBound* const path_bound) {
// 1. Initialize the path boundaries to be an indefinitely large area.
// 和fallback中的函数作用一样
if (!InitPathBoundary(reference_line_info, path_bound)) {
const std::string msg = "Failed to initialize path boundaries.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// 2. Decide a rough boundary based on road boundary
// 根据道路确定一个大致粗略的边界，此函数较好理解不单独分析了
if (!GetBoundaryFromRoads(reference_line_info, path_bound)) {
const std::string msg =
"Failed to decide a rough boundary based on road boundary.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
//将边界S轴从车道中心转换为参照线（这就说明之前的s轴都是车道中心线）
ConvertBoundarySAxisFromLaneCenterToRefLine(reference_line_info, path_bound);
// ADC已超出道路边界。无法生成转移路径
if (adc_frenet_l_ < std::get<1>(path_bound->front()) ||
adc_frenet_l_ > std::get<2>(path_bound->front())) {
const std::string msg =
"ADC is outside road boundary already. Cannot generate pull-over path";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// 2. Update boundary by lane boundary for pull_over
// 这个函数较好理解就不单独分析了
UpdatePullOverBoundaryByLaneBoundary(reference_line_info, path_bound);
// 3. Fine-tune the boundary based on static obstacles
// 基于静态障碍物微调边界，后面单独分析GetBoundaryFromStaticObstacles（...）
PathBound temp_path_bound = *path_bound;
std::string blocking_obstacle_id;
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, &blocking_obstacle_id)) {
const std::string msg =
"Failed to decide fine tune the boundaries after "
"taking into consideration all static obstacles.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// PathBoundsDebugString(*path_bound);
auto* pull_over_status = injector_->planning_context()
->mutable_planning_status()
->mutable_pull_over();
// If already found a pull-over position, simply check if it's valid.
int curr_idx = -1;
if (pull_over_status->has_position()) {
curr_idx = IsPointWithinPathBound(
reference_line_info, pull_over_status->position().x(),
pull_over_status->position().y(), *path_bound);
}
// If haven't found a pull-over position, search for one.
// 如果点不在bound限制的可行驶区域内，搜索可以停车的地点
if (curr_idx < 0) {
auto pull_over_type = pull_over_status->pull_over_type();
pull_over_status->Clear();
pull_over_status->set_pull_over_type(pull_over_type);
pull_over_status->set_plan_pull_over_path(true);
// <pull_over_x, pull_over_y, pull_over_theta, index_in_bounds>
std::tuple<double, double, double, int> pull_over_configuration;
// 这个函数单独在后面说一下
if (!SearchPullOverPosition(frame, reference_line_info, *path_bound,
&pull_over_configuration)) {
const std::string msg = "Failed to find a proper pull-over position.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
curr_idx = std::get<3>(pull_over_configuration);
// If have found a pull-over position, update planning-context
// 设定停车地点的相关信息
pull_over_status->mutable_position()->set_x(
std::get<0>(pull_over_configuration));
pull_over_status->mutable_position()->set_y(
std::get<1>(pull_over_configuration));
pull_over_status->mutable_position()->set_z(0.0);
pull_over_status->set_theta(std::get<2>(pull_over_configuration));
pull_over_status->set_length_front(FLAGS_obstacle_lon_start_buffer);
pull_over_status->set_length_back(FLAGS_obstacle_lon_end_buffer);
pull_over_status->set_width_left(
VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0);
pull_over_status->set_width_right(
VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0);
ADEBUG << "Pull Over: x[" << pull_over_status->position().x() << "] y["
<< pull_over_status->position().y() << "] theta["
<< pull_over_status->theta() << "]";
}
// Trim path-bound properly
// 停车后面的bounds就没用了，删除一部分，修改一部分
while (static_cast<int>(path_bound->size()) - 1 >
curr_idx + kNumExtraTailBoundPoint) {
path_bound->pop_back();
}
// 停车后面的bounds全部修改为停车点处的bound
for (size_t idx = curr_idx + 1; idx < path_bound->size(); ++idx) {
std::get<1>((*path_bound)[idx]) = std::get<1>((*path_bound)[curr_idx]);
std::get<2>((*path_bound)[idx]) = std::get<2>((*path_bound)[curr_idx]);
}
return Status::OK();
}
GetBoundaryFromStaticObstacles具体逻辑在代码中体现
这个函数我的理解中他有写错的部分，不知道理解的对不对
// Currently, it processes each obstacle based on its frenet-frame
// projection. Therefore, it might be overly conservative when processing
// obstacles whose headings differ from road-headings a lot.
// TODO(all): (future work) this can be improved in the future.
// 目前，它基于每个障碍物的法语框架投影来处理每个障碍物。因此，在处理方向与道路方向相差很大的障碍物时，可能过于保守。
bool PathBoundsDecider::GetBoundaryFromStaticObstacles(
const PathDecision& path_decision, PathBound* const path_boundaries,
std::string* const blocking_obstacle_id) {
// Preprocessing.
auto indexed_obstacles = path_decision.obstacles();
auto sorted_obstacles = SortObstaclesForSweepLine(indexed_obstacles);
ADEBUG << "There are " << sorted_obstacles.size() << " obstacles.";
double center_line = adc_frenet_l_;
size_t obs_idx = 0;
int path_blocked_idx = -1;
std::multiset<double, std::greater<double>> right_bounds;
right_bounds.insert(std::numeric_limits<double>::lowest());
// 这个left_bounds定义我个人认为应该是std::multiset<double, std::lesser<double>>
std::multiset<double> left_bounds;
left_bounds.insert(std::numeric_limits<double>::max());
// 将障碍物ID映射到已确定的ADC通过方向，如果ADC应从左侧通过，则为真；否则为false。
std::unordered_map<std::string, bool> obs_id_to_direction;
// 将障碍物ID映射到是否允许在该障碍物上侧向通过的决定。如果允许，则为真；否则为false。
std::unordered_map<std::string, bool> obs_id_to_sidepass_decision;
// Step through every path point.
for (size_t i = 1; i < path_boundaries->size(); ++i) {
double curr_s = std::get<0>((*path_boundaries)[i]);
// obs_idx障碍物索引
if (obs_idx < sorted_obstacles.size() &&
std::get<1>(sorted_obstacles[obs_idx]) < curr_s) {
while (obs_idx < sorted_obstacles.size() &&
std::get<1>(sorted_obstacles[obs_idx]) < curr_s) {
// 更新障碍物信息
const auto& curr_obstacle = sorted_obstacles[obs_idx];
const double curr_obstacle_s = std::get<1>(curr_obstacle);
const double curr_obstacle_l_min = std::get<2>(curr_obstacle);
const double curr_obstacle_l_max = std::get<3>(curr_obstacle);
const std::string curr_obstacle_id = std::get<4>(curr_obstacle);
ADEBUG << "id[" << curr_obstacle_id << "] s[" << curr_obstacle_s
<< "] curr_obstacle_l_min[" << curr_obstacle_l_min
<< "] curr_obstacle_l_max[" << curr_obstacle_l_max
<< "] center_line[" << center_line << "]";
if (std::get<0>(curr_obstacle) == 1) {
// 一个新的障碍进入我们的范围：
//-确定ADC通过的方向。
//-相应地更新左/右边界。
//-如果边界被阻挡，则决定是否可以侧向通过。
//-如果是，则借用相邻车道至侧车道。
if (curr_obstacle_l_min + curr_obstacle_l_max < center_line * 2) {
// 就是中心线在障碍物中心线的左侧，那么就应该从障碍物左侧通过，同理另一种情况
// Obstacle is to the right of center-line, should pass from left.
obs_id_to_direction[curr_obstacle_id] = true;
right_bounds.insert(curr_obstacle_l_max);
} else {
// Obstacle is to the left of center-line, should pass from right.
obs_id_to_direction[curr_obstacle_id] = false;
left_bounds.insert(curr_obstacle_l_min);
}
// 使用缓冲区更新路径边界和中心线
// 就这个地方我感觉既然是传入begin()就应该两个都是排序的情况
// 不知道自己理解的是否正确
if (!UpdatePathBoundaryAndCenterLineWithBuffer(
i, *left_bounds.begin(), *right_bounds.begin(),
path_boundaries, ¢er_line)) {
// 如果没更新成功，就将此id记录此为阻塞的id
path_blocked_idx = static_cast<int>(i);
*blocking_obstacle_id = curr_obstacle_id;
break;
}
} else {
// 已经存在的情况
// An existing obstacle exits our scope.
if (obs_id_to_direction[curr_obstacle_id]) {
right_bounds.erase(right_bounds.find(curr_obstacle_l_max));
} else {
left_bounds.erase(left_bounds.find(curr_obstacle_l_min));
}
obs_id_to_direction.erase(curr_obstacle_id);
}
// Update the bounds and center_line.
std::get<1>((*path_boundaries)[i]) = std::fmax(
std::get<1>((*path_boundaries)[i]),
*right_bounds.begin() + GetBufferBetweenADCCenterAndEdge());
std::get<2>((*path_boundaries)[i]) = std::fmin(
std::get<2>((*path_boundaries)[i]),
*left_bounds.begin() - GetBufferBetweenADCCenterAndEdge());
// 阻塞情况
if (std::get<1>((*path_boundaries)[i]) >
std::get<2>((*path_boundaries)[i])) {
ADEBUG << "Path is blocked at s = " << curr_s;
path_blocked_idx = static_cast<int>(i);
if (!obs_id_to_direction.empty()) {
*blocking_obstacle_id = obs_id_to_direction.begin()->first;
}
break;
} else {
center_line = (std::get<1>((*path_boundaries)[i]) +
std::get<2>((*path_boundaries)[i])) /
2.0;
}
++obs_idx;
}
} else {
// If no obstacle change, update the bounds and center_line.
// 如果没有障碍物更改，请更新边界和center_line。
std::get<1>((*path_boundaries)[i]) =
std::fmax(std::get<1>((*path_boundaries)[i]),
*right_bounds.begin() + GetBufferBetweenADCCenterAndEdge());
std::get<2>((*path_boundaries)[i]) =
std::fmin(std::get<2>((*path_boundaries)[i]),
*left_bounds.begin() - GetBufferBetweenADCCenterAndEdge());
if (std::get<1>((*path_boundaries)[i]) >
std::get<2>((*path_boundaries)[i])) {
ADEBUG << "Path is blocked at s = " << curr_s;
path_blocked_idx = static_cast<int>(i);
if (!obs_id_to_direction.empty()) {
*blocking_obstacle_id = obs_id_to_direction.begin()->first;
}
} else {
center_line = (std::get<1>((*path_boundaries)[i]) +
std::get<2>((*path_boundaries)[i])) /
2.0;
}
}
// Early exit if path is blocked.
if (path_blocked_idx != -1) {
break;
}
}
TrimPathBounds(path_blocked_idx, path_boundaries);
return true;
}
SearchPullOverPosition它的目的是搜索停车点，为了能够容纳车的尺寸，因为要先搜索可以停车的区域，然后在该区域内取一点作为停车点。搜索区域时，要先确定一个端点，然后向前或向后考察另一个端点，以及考察两端点之间的区域是否符合要求。
处理逻辑在代码里体现了
bool PathBoundsDecider::SearchPullOverPosition(
const Frame& frame, const ReferenceLineInfo& reference_line_info,
const std::vector<std::tuple<double, double, double>>& path_bound,
std::tuple<double, double, double, int>* const pull_over_configuration) {
const auto& pull_over_status =
injector_->planning_context()->planning_status().pull_over();
// 定义搜索方向
// 搜索方向默认为向前搜索
bool search_backward = false;
// A.先根据不同场景搜索一个可停车区域的大致端点，然后从该端点出发确定可停车区域
double pull_over_s = 0.0;
// 如果是紧急情况，前方停车
if (pull_over_status.pull_over_type() ==
PullOverStatus::EMERGENCY_PULL_OVER) {
if (!FindEmergencyPullOverS(reference_line_info, &pull_over_s)) {
AERROR << "Failed to find emergency_pull_over s";
return false;
}
// 从目标位置向前搜索
search_backward = false;
} else if (pull_over_status.pull_over_type() == PullOverStatus::PULL_OVER) {
// 在这个FindDestinationPullOverS函数中，是将理想的停车点当作route的终点，所以search_backward = true
if (!FindDestinationPullOverS(frame, reference_line_info, path_bound,
&pull_over_s)) {
AERROR << "Failed to find pull_over s upon destination arrival";
return false;
}
search_backward = true; // search BACKWARD from target position
} else {
return false;
}
int idx = 0;
if (search_backward) {
// 1. 找到目的地之前的第一个点。
idx = static_cast<int>(path_bound.size()) - 1;
while (idx >= 0 && std::get<0>(path_bound[idx]) > pull_over_s) {
--idx;
}
} else {
// 反向搜索时，idx表示停车区域末端
// 1. 找到emergency_pull_over s后的第一个点。
while (idx < static_cast<int>(path_bound.size()) &&
std::get<0>(path_bound[idx]) < pull_over_s) {
++idx;
}
}
if (idx < 0 || idx >= static_cast<int>(path_bound.size())) {
AERROR << "Failed to find path_bound index for pull over s";
return false;
}
// Search for a feasible location for pull-over.
// 根据一些条件计算pull_over_space_length 和 pull_over_space_width
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.
// (not in any intersection)
// B.搜索可停车的区域始末。内外2层while循环，外循环控制一个开始搜索的端点idx，
// 因为当考察的区域不符合安全性和尺寸条件时，idx也要变化。内循环控制另一个端点j。
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<int>(path_bound.size()) &&
std::get<0>(path_bound.back()) - std::get<0>(path_bound[idx]) >
pull_over_space_length)) {
int j = idx;
bool is_feasible_window = true;
// Check if the point of idx is within intersection.
double pt_ref_line_s = std::get<0>(path_bound[idx]);
double pt_ref_line_l = 0.0;
common::SLPoint pt_sl;
pt_sl.set_s(pt_ref_line_s);
pt_sl.set_l(pt_ref_line_l);
common::math::Vec2d pt_xy;
reference_line_info.reference_line().SLToXY(pt_sl, &pt_xy);
common::PointENU hdmap_point;
hdmap_point.set_x(pt_xy.x());
hdmap_point.set_y(pt_xy.y());
ADEBUG << "Pull-over position might be around (" << pt_xy.x() << ", "
<< pt_xy.y() << ")";
std::vector<std::shared_ptr<const JunctionInfo>> junctions;
HDMapUtil::BaseMap().GetJunctions(hdmap_point, 1.0, &junctions);
// 如果遇到路口，调整开始搜索的端点idx，重新搜索
if (!junctions.empty()) {
AWARN << "Point is in PNC-junction.";
idx = search_backward ? idx - 1 : idx + 1;
continue;
}
// 搜索一段宽度达标、长度达标的可停车区域。即，搜索合适的端点j
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<int>(path_bound.size()) &&
std::get<0>(path_bound[j]) - std::get<0>(path_bound[idx]) <
pull_over_space_length)) {
double curr_s = std::get<0>(path_bound[j]);
double curr_right_bound = std::fabs(std::get<1>(path_bound[j]));
double curr_road_left_width = 0;
double curr_road_right_width = 0;
reference_line_info.reference_line().GetRoadWidth(
curr_s, &curr_road_left_width, &curr_road_right_width);
ADEBUG << "s[" << curr_s << "] curr_road_left_width["
<< curr_road_left_width << "] curr_road_right_width["
<< curr_road_right_width << "]";
if (curr_road_right_width - (curr_right_bound + adc_half_width) >
config_.path_bounds_decider_config().pull_over_road_edge_buffer()) {
AERROR << "Not close enough to road-edge. Not feasible for pull-over.";
is_feasible_window = false;
break;
}
const double right_bound = std::get<1>(path_bound[j]);
const double left_bound = std::get<2>(path_bound[j]);
ADEBUG << "left_bound[" << left_bound << "] right_bound[" << right_bound
<< "]";
if (left_bound - right_bound < pull_over_space_width) {
AERROR << "Not wide enough to fit ADC. Not feasible for pull-over.";
is_feasible_window = false;
break;
}
j = search_backward ? j - 1 : j + 1;
}
if (j < 0) {
return false;
}
// C.找到可停车区域后，获取停车目标点的位姿
if (is_feasible_window) {
has_a_feasible_window = true;
const auto& reference_line = reference_line_info.reference_line();
// 估计停车点，使车辆前后保持相同的安全距离
const auto& vehicle_param =
VehicleConfigHelper::GetConfig().vehicle_param();
const double back_clear_to_total_length_ratio =
(0.5 * (kPulloverLonSearchCoeff - 1.0) * vehicle_param.length() +
vehicle_param.back_edge_to_center()) /
vehicle_param.length() / kPulloverLonSearchCoeff;
int start_idx = j;
int end_idx = idx;
if (!search_backward) {
start_idx = idx;
end_idx = j;
}
// 根据start_idx和end_idx计算pull_over_idx。注意index是相对于bounds的
auto pull_over_idx = static_cast<size_t>(
back_clear_to_total_length_ratio * static_cast<double>(end_idx) +
(1.0 - back_clear_to_total_length_ratio) *
static_cast<double>(start_idx));
const auto& pull_over_point = path_bound[pull_over_idx];
const double pull_over_s = std::get<0>(pull_over_point);
const double pull_over_l =
std::get<1>(pull_over_point) + pull_over_space_width / 2.0;
common::SLPoint pull_over_sl_point;
pull_over_sl_point.set_s(pull_over_s);
pull_over_sl_point.set_l(pull_over_l);
common::math::Vec2d pull_over_xy_point;
reference_line.SLToXY(pull_over_sl_point, &pull_over_xy_point);
const double pull_over_x = pull_over_xy_point.x();
const double pull_over_y = pull_over_xy_point.y();
// 如果参考线θ与车道不对齐，则将靠边θ设置为最近的车道θ，而不是参考线θ
// 根据找到的停车点，设定相关信息，并根据参考线计算停车点的朝向角
const auto& reference_point =
reference_line.GetReferencePoint(pull_over_s);
double pull_over_theta = reference_point.heading();
hdmap::LaneInfoConstPtr lane;
double s = 0.0;
double l = 0.0;
auto point =
common::util::PointFactory::ToPointENU(pull_over_x, pull_over_y);
if (HDMapUtil::BaseMap().GetNearestLaneWithHeading(
point, 5.0, pull_over_theta, M_PI_2, &lane, &s, &l) == 0) {
pull_over_theta = lane->Heading(s);
}
*pull_over_configuration =
std::make_tuple(pull_over_x, pull_over_y, pull_over_theta,
static_cast<int>(pull_over_idx));
// 一旦找到可停车区域，退出最外层while循环，返回结果
break;
}
idx = search_backward ? idx - 1 : idx + 1;
}
return has_a_feasible_window;
}
3.lane change
计算lane change中的PathBound的步骤大体和前面的类似，函数GetBoundaryFromLaneChangeForbiddenZone()是前面没有的，后面单独介绍。
分析这个情况的时候切记一点：变道情况下的参考线不是本车所在车道的参考线，而是目标车道的参考线。
Status PathBoundsDecider::GenerateLaneChangePathBound(
const ReferenceLineInfo& reference_line_info,
std::vector<std::tuple<double, double, double>>* const path_bound) {
// 1. Initialize the path boundaries to be an indefinitely large area.
if (!InitPathBoundary(reference_line_info, path_bound)) {
const std::string msg = "Failed to initialize path boundaries.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// PathBoundsDebugString(*path_bound);
// 2. Decide a rough boundary based on lane info and ADC's position
std::string dummy_borrow_lane_type;
if (!GetBoundaryFromLanesAndADC(reference_line_info,
LaneBorrowInfo::NO_BORROW, 0.1, path_bound,
&dummy_borrow_lane_type, true)) {
const std::string msg =
"Failed to decide a rough boundary based on "
"road information.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// 3. Remove the S-length of target lane out of the path-bound.
GetBoundaryFromLaneChangeForbiddenZone(reference_line_info, path_bound);
PathBound temp_path_bound = *path_bound;
std::string blocking_obstacle_id;
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, &blocking_obstacle_id)) {
const std::string msg =
"Failed to decide fine tune the boundaries after "
"taking into consideration all static obstacles.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// 附加一些额外的路径绑定点以避免零长度路径数据。
// Append some extra path bound points to avoid zero-length path data.
int counter = 0;
while (!blocking_obstacle_id.empty() &&
path_bound->size() < temp_path_bound.size() &&
counter < kNumExtraTailBoundPoint) {
path_bound->push_back(temp_path_bound[path_bound->size()]);
counter++;
}
ADEBUG << "Completed generating path boundaries.";
return Status::OK();
}
GetBoundaryFromLaneChangeForbiddenZone处理逻辑在代码中体现
void PathBoundsDecider::GetBoundaryFromLaneChangeForbiddenZone(
const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
// Sanity checks.
CHECK_NOTNULL(path_bound);
const ReferenceLine& reference_line = reference_line_info.reference_line();
// If there is a pre-determined lane-change starting position, then use it;
// otherwise, decide one.
// 当前位置可以直接变道
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;
}
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.
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.
if (lane_change_start_s < adc_frenet_s_) {
// If already passed the decided S, then return.
// lane_change_status->set_exist_lane_change_start_position(false);
return;
}
// 逐个考察lane_change_start_s之前的bound
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;
/*
right：adc_frenet_l_ > curr_lane_left_width
表示右变道才去更新右边界为原车道右边线，在变道起点前不给它右侧车道的空间，左变道时右侧边界维持原值
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]) =
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（lane borrow）
处理步骤非常相似，不同之处在于调用GetBoundaryFromLanesAndADC()时，会将3种LaneBorrowInfo传入，即不借道、向左借道、向右借道。而之前的场景在调用该函数时，均设定不借道。
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. Initialize the path boundaries to be an indefinitely large area.
if (!InitPathBoundary(reference_line_info, path_bound)) {
const std::string msg = "Failed to initialize path boundaries.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// PathBoundsDebugString(*path_bound);
// 2. Decide a rough boundary based on lane info and ADC's position
if (!GetBoundaryFromLanesAndADC(reference_line_info, lane_borrow_info, 0.1,
path_bound, borrow_lane_type)) {
const std::string msg =
"Failed to decide a rough boundary based on "
"road information.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// PathBoundsDebugString(*path_bound);
// TODO(jiacheng): once ready, limit the path boundary based on the
// actual road boundary to avoid getting off-road.
// 3. Fine-tune the boundary based on static obstacles
PathBound temp_path_bound = *path_bound;
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, blocking_obstacle_id)) {
const std::string msg =
"Failed to decide fine tune the boundaries after "
"taking into consideration all static obstacles.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// Append some extra path bound points to avoid zero-length path data.
int counter = 0;
while (!blocking_obstacle_id->empty() &&
path_bound->size() < temp_path_bound.size() &&
counter < kNumExtraTailBoundPoint) {
path_bound->push_back(temp_path_bound[path_bound->size()]);
counter++;
}
// PathBoundsDebugString(*path_bound);
// 4. Adjust the boundary considering dynamic obstacles
// TODO(all): may need to implement this in the future.
ADEBUG << "Completed generating path boundaries.";
return Status::OK();
}
借道的三种类型：
enum class LaneBorrowInfo {
LEFT_BORROW,
NO_BORROW,
RIGHT_BORROW,
};
1396
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
