/******************************************************************************
* Copyright 2023 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/planning/tasks/lane_borrow_path/lane_borrow_path.h"
#include <algorithm>
#include <functional>
#include <memory>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/planning_base/common/obstacle_blocking_analyzer.h"
#include "modules/planning/planning_base/common/planning_context.h"
#include "modules/planning/planning_interface_base/task_base/common/path_generation.h"
#include "modules/planning/planning_interface_base/task_base/common/path_util/path_assessment_decider_util.h"
#include "modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.h"
#include "modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.h"
namespace apollo {
namespace planning {
using apollo::common::Status;
using apollo::common::VehicleConfigHelper;
using apollo::common::math::Box2d;
using apollo::common::math::Polygon2d;
using apollo::common::math::Vec2d;
constexpr double kIntersectionClearanceDist = 20.0;
constexpr double kJunctionClearanceDist = 15.0;
// bool LaneBorrowPath::Init(
// const std::string& config_dir,
// const std::string& name,
// const std::shared_ptr<DependencyInjector>& injector) {
// if (!Task::Init(config_dir, name, injector)) {
// return false;
// }
// // Load the config this task.
// return Task::LoadConfig<LaneBorrowPathConfig>(&config_);
// }
// LaneBorrowPath的初始化函数
bool LaneBorrowPath::Init(
const std::string& config_dir, // 配置文件的目录
const std::string& name, // 任务的名称
const std::shared_ptr<DependencyInjector>& injector) { // 依赖注入器
// 调用父类Task的初始化函数，如果初始化失败，则返回false
if (!Task::Init(config_dir, name, injector)) {
return false;
}
// 加载此任务的配置，使用LaneBorrowPathConfig类型的config_作为参数
return Task::LoadConfig<LaneBorrowPathConfig>(&config_);
}
// apollo::common::Status LaneBorrowPath::Process(Frame* frame, ReferenceLineInfo* reference_line_info) {
// if (!config_.is_allow_lane_borrowing() || reference_line_info->path_reusable()) {
// ADEBUG << "path reusable" << reference_line_info->path_reusable() << ",skip";
// return Status::OK();
// }
// if (!IsNecessaryToBorrowLane()) {
// ADEBUG << "No need to borrow lane";
// return Status::OK();
// }
// std::vector<PathBoundary> candidate_path_boundaries;
// std::vector<PathData> candidate_path_data;
// GetStartPointSLState();
// if (!DecidePathBounds(&candidate_path_boundaries)) {
// return Status::OK();
// }
// if (!OptimizePath(candidate_path_boundaries, &candidate_path_data)) {
// return Status::OK();
// }
// if (AssessPath(&candidate_path_data, reference_line_info->mutable_path_data())) {
// ADEBUG << "lane borrow path success";
// }
// return Status::OK();
// }
// LaneBorrowPath的处理函数
apollo::common::Status LaneBorrowPath::Process(Frame* frame, ReferenceLineInfo* reference_line_info) {
// 如果配置不允许借道，或者参考线信息的路径可重用，则跳过后续处理
if (!config_.is_allow_lane_borrowing() || reference_line_info->path_reusable()) {
ADEBUG << "path reusable" << reference_line_info->path_reusable() << ",skip";
return Status::OK();
}
// 如果不需要借道，则跳过后续处理
if (!IsNecessaryToBorrowLane()) {
ADEBUG << "No need to borrow lane";
return Status::OK();
}
// 候选路径边界和候选路径数据的向量
std::vector<PathBoundary> candidate_path_boundaries;
std::vector<PathData> candidate_path_data;
// 获取起始点的SL状态
GetStartPointSLState();
// 如果决定路径边界失败，则返回OK状态
if (!DecidePathBounds(&candidate_path_boundaries)) {
return Status::OK();
}
// 如果优化路径失败，则返回OK状态
if (!OptimizePath(candidate_path_boundaries, &candidate_path_data)) {
return Status::OK();
}
// 如果评估路径成功，则打印"lane borrow path success"
if (AssessPath(&candidate_path_data, reference_line_info->mutable_path_data())) {
ADEBUG << "lane borrow path success";
}
// 返回OK状态
return Status::OK();
}
// bool LaneBorrowPath::DecidePathBounds(std::vector<PathBoundary>* boundary) {
// for (size_t i = 0; i < decided_side_pass_direction_.size(); i++) {
// boundary->emplace_back();
// auto& path_bound = boundary->back();
// std::string blocking_obstacle_id = "";
// std::string borrow_lane_type = "";
// double path_narrowest_width = 0;
// // 1. Initialize the path boundaries to be an indefinitely large area.
// if (!PathBoundsDeciderUtil::InitPathBoundary(*reference_line_info_, &path_bound, init_sl_state_)) {
// const std::string msg = "Failed to initialize path boundaries.";
// AERROR << msg;
// boundary->pop_back();
// continue;
// }
// // 2. Decide a rough boundary based on lane info and ADC's position
// if (!GetBoundaryFromNeighborLane(decided_side_pass_direction_[i], &path_bound, &borrow_lane_type)) {
// AERROR << "Failed to decide a rough boundary based on lane and adc.";
// boundary->pop_back();
// continue;
// }
// // path_bound.DebugString("after_neighbor_lane");
// // 3. Fine-tune the boundary based on static obstacles
// PathBound temp_path_bound = path_bound;
// if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles(
// *reference_line_info_, init_sl_state_, &path_bound, &blocking_obstacle_id, &path_narrowest_width)) {
// const std::string msg
// = "Failed to decide fine tune the boundaries after "
// "taking into consideration all static obstacles.";
// AERROR << msg;
// boundary->pop_back();
// continue;
// }
// // path_bound.DebugString("after_obs");
// // 4. 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 < FLAGS_num_extra_tail_bound_point) {
// path_bound.push_back(temp_path_bound[path_bound.size()]);
// counter++;
// }
// ADEBUG << "Completed generating path boundaries.";
// std::string label;
// if (decided_side_pass_direction_[i] == SidePassDirection::LEFT_BORROW) {
// label = "regular/right" + borrow_lane_type;
// } else {
// label = "regular/left" + borrow_lane_type;
// }
// path_bound.set_label(label);
// path_bound.set_blocking_obstacle_id(blocking_obstacle_id);
// RecordDebugInfo(path_bound, path_bound.label(), reference_line_info_);
// }
// return !boundary->empty();
// }
// 决定路径边界的函数
bool LaneBorrowPath::DecidePathBounds(std::vector<PathBoundary>* boundary) {
// 遍历已决定的侧向通行方向
for (size_t i = 0; i < decided_side_pass_direction_.size(); i++) {
// 在边界向量中添加一个新元素
boundary->emplace_back();
// 获取新添加的边界引用
auto& path_bound = boundary->back();
// 初始化阻塞障碍物ID、借道类型和路径最窄宽度
std::string blocking_obstacle_id = "";
std::string borrow_lane_type = "";
double path_narrowest_width = 0;
// 1. 初始化路径边界为无限大的区域
if (!PathBoundsDeciderUtil::InitPathBoundary(*reference_line_info_, &path_bound, init_sl_state_)) {
const std::string msg = "Failed to initialize path boundaries.";
AERROR << msg;
// 如果初始化失败，删除新添加的边界并跳过当前循环
boundary->pop_back();
continue;
}
// 2. 根据车道信息和自动驾驶车辆的位置决定一个粗略的边界
if (!GetBoundaryFromNeighborLane(decided_side_pass_direction_[i], &path_bound, &borrow_lane_type)) {
AERROR << "Failed to decide a rough boundary based on lane and adc.";
// 如果决定失败，删除新添加的边界并跳过当前循环
boundary->pop_back();
continue;
}
// 3. 根据静态障碍物微调边界
PathBound temp_path_bound = path_bound;
if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles(
*reference_line_info_, init_sl_state_, &path_bound, &blocking_obstacle_id, &path_narrowest_width)) {
const std::string msg
= "Failed to decide fine tune the boundaries after "
"taking into consideration all static obstacles.";
AERROR << msg;
// 如果微调失败，删除新添加的边界并跳过当前循环
boundary->pop_back();
continue;
}
// 4. 添加一些额外的路径边界点以避免零长度的路径数据
int counter = 0;
while (!blocking_obstacle_id.empty() && path_bound.size() < temp_path_bound.size()
&& counter < FLAGS_num_extra_tail_bound_point) {
path_bound.push_back(temp_path_bound[path_bound.size()]);
counter++;
}
ADEBUG << "Completed generating path boundaries.";
// 根据侧向通行方向设置标签
std::string label;
if (decided_side_pass_direction_[i] == SidePassDirection::LEFT_BORROW) {
label = "regular/right" + borrow_lane_type;
} else {
label = "regular/left" + borrow_lane_type;
}
// 设置路径边界的标签和阻塞障碍物ID
path_bound.set_label(label);
path_bound.set_blocking_obstacle_id(blocking_obstacle_id);
// 记录调试信息
RecordDebugInfo(path_bound, path_bound.label(), reference_line_info_);
}
// 如果边界向量不为空，则返回true，否则返回false
return !boundary->empty();
}
// bool LaneBorrowPath::OptimizePath(
// const std::vector<PathBoundary>& path_boundaries,
// std::vector<PathData>* candidate_path_data) {
// const auto& config = config_.path_optimizer_config();
// const ReferenceLine& reference_line = reference_line_info_->reference_line();
// std::array<double, 3> end_state = {0.0, 0.0, 0.0};
// for (const auto& path_boundary : path_boundaries) {
// std::vector<double> opt_l, opt_dl, opt_ddl;
// std::vector<std::pair<double, double>> ddl_bounds;
// PathOptimizerUtil::CalculateAccBound(path_boundary, reference_line, &ddl_bounds);
// const double jerk_bound = PathOptimizerUtil::EstimateJerkBoundary(std::fmax(init_sl_state_.first[1], 1e-12));
// std::vector<double> ref_l;
// std::vector<double> weight_ref_l;
// PathOptimizerUtil::UpdatePathRefWithBound(
// path_boundary, config.path_reference_l_weight(), &ref_l, &weight_ref_l);
// bool res_opt = PathOptimizerUtil::OptimizePath(
// init_sl_state_,
// end_state,
// ref_l,
// weight_ref_l,
// path_boundary,
// ddl_bounds,
// jerk_bound,
// config,
// &opt_l,
// &opt_dl,
// &opt_ddl);
// if (res_opt) {
// auto frenet_frame_path = PathOptimizerUtil::ToPiecewiseJerkPath(
// opt_l, opt_dl, opt_ddl, path_boundary.delta_s(), path_boundary.start_s());
// PathData path_data;
// path_data.SetReferenceLine(&reference_line);
// path_data.SetFrenetPath(std::move(frenet_frame_path));
// if (FLAGS_use_front_axe_center_in_path_planning) {
// auto discretized_path
// = DiscretizedPath(PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe(path_data));
// path_data.SetDiscretizedPath(discretized_path);
// }
// path_data.set_path_label(path_boundary.label());
// path_data.set_blocking_obstacle_id(path_boundary.blocking_obstacle_id());
// candidate_path_data->push_back(std::move(path_data));
// }
// }
// if (candidate_path_data->empty()) {
// return false;
// }
// return true;
// }
// 优化路径的函数
bool LaneBorrowPath::OptimizePath(
const std::vector<PathBoundary>& path_boundaries,
std::vector<PathData>* candidate_path_data) {
// 获取路径优化器配置
const auto& config = config_.path_optimizer_config();
// 获取参考线
const ReferenceLine& reference_line = reference_line_info_->reference_line();
// 初始化结束状态
std::array<double, 3> end_state = {0.0, 0.0, 0.0};
// 遍历路径边界
for (const auto& path_boundary : path_boundaries) {
// 初始化优化后的l, dl, ddl
std::vector<double> opt_l, opt_dl, opt_ddl;
// 初始化ddl边界
std::vector<std::pair<double, double>> ddl_bounds;
// 计算加速度边界
PathOptimizerUtil::CalculateAccBound(path_boundary, reference_line, &ddl_bounds);
// 估计jerk边界
const double jerk_bound = PathOptimizerUtil::EstimateJerkBoundary(std::fmax(init_sl_state_.first[1], 1e-12));
// 初始化参考l和权重
std::vector<double> ref_l;
std::vector<double> weight_ref_l;
// 更新路径参考与边界
PathOptimizerUtil::UpdatePathRefWithBound(
path_boundary, config.path_reference_l_weight(), &ref_l, &weight_ref_l);
// 优化路径
bool res_opt = PathOptimizerUtil::OptimizePath(
init_sl_state_,
end_state,
ref_l,
weight_ref_l,
path_boundary,
ddl_bounds,
jerk_bound,
config,
&opt_l,
&opt_dl,
&opt_ddl);
// 如果优化成功
if (res_opt) {
// 转换为分段jerk路径
auto frenet_frame_path = PathOptimizerUtil::ToPiecewiseJerkPath(
opt_l, opt_dl, opt_ddl, path_boundary.delta_s(), path_boundary.start_s());
// 初始化路径数据
PathData path_data;
// 设置参考线和frenet路径
path_data.SetReferenceLine(&reference_line);
path_data.SetFrenetPath(std::move(frenet_frame_path));
// 如果在路径规划中使用前轴中心
if (FLAGS_use_front_axe_center_in_path_planning) {
// 转换路径点参考从前轴到后轴
auto discretized_path
= DiscretizedPath(PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe(path_data));
// 设置离散化路径
path_data.SetDiscretizedPath(discretized_path);
}
// 设置路径标签和阻塞障碍物ID
path_data.set_path_label(path_boundary.label());
path_data.set_blocking_obstacle_id(path_boundary.blocking_obstacle_id());
// 将路径数据添加到候选路径数据中
candidate_path_data->push_back(std::move(path_data));
}
}
// 如果候选路径数据为空，则返回false，否则返回true
if (candidate_path_data->empty()) {
return false;
}
return true;
}
// bool LaneBorrowPath::AssessPath(std::vector<PathData>* candidate_path_data, PathData* final_path) {
// std::vector<PathData> valid_path_data;
// for (auto& curr_path_data : *candidate_path_data) {
// if (PathAssessmentDeciderUtil::IsValidRegularPath(*reference_line_info_, curr_path_data)) {
// SetPathInfo(&curr_path_data);
// PathAssessmentDeciderUtil::TrimTailingOutLanePoints(&curr_path_data);
// if (curr_path_data.Empty()) {
// AINFO << "lane borrow path is empty after trimed";
// continue;
// }
// valid_path_data.push_back(curr_path_data);
// }
// }
// if (valid_path_data.empty()) {
// AINFO << "All lane borrow path are not valid";
// return false;
// }
// auto* mutable_path_decider_status
// = injector_->planning_context()->mutable_planning_status()->mutable_path_decider();
// const std::string blocking_obstacle_id = mutable_path_decider_status->front_static_obstacle_id();
// const Obstacle* blocking_obstacle = reference_line_info_->path_decision()->obstacles().Find(blocking_obstacle_id);
// if (valid_path_data.size() > 1) {
// if (ComparePathData(valid_path_data[0], valid_path_data[1], blocking_obstacle)) {
// *final_path = valid_path_data[0];
// } else {
// *final_path = valid_path_data[1];
// }
// } else {
// *final_path = valid_path_data[0];
// }
// RecordDebugInfo(*final_path, final_path->path_label(), reference_line_info_);
// return true;
// }
// 评估路径的函数
bool LaneBorrowPath::AssessPath(std::vector<PathData>* candidate_path_data, PathData* final_path) {
// 初始化有效路径数据
std::vector<PathData> valid_path_data;
// 遍历候选路径数据
for (auto& curr_path_data : *candidate_path_data) {
// 如果当前路径是有效的常规路径
if (PathAssessmentDeciderUtil::IsValidRegularPath(*reference_line_info_, curr_path_data)) {
// 设置路径信息
SetPathInfo(&curr_path_data);
// 剪除尾部超出车道的点
PathAssessmentDeciderUtil::TrimTailingOutLanePoints(&curr_path_data);
// 如果剪除后的路径为空，则跳过当前循环
if (curr_path_data.Empty()) {
AINFO << "lane borrow path is empty after trimed";
continue;
}
// 将当前路径添加到有效路径数据中
valid_path_data.push_back(curr_path_data);
}
}
// 如果有效路径数据为空，则返回false
if (valid_path_data.empty()) {
AINFO << "All lane borrow path are not valid";
return false;
}
// 获取路径决策状态
auto* mutable_path_decider_status
= injector_->planning_context()->mutable_planning_status()->mutable_path_decider();
// 获取阻塞障碍物ID
const std::string blocking_obstacle_id = mutable_path_decider_status->front_static_obstacle_id();
// 查找阻塞障碍物
const Obstacle* blocking_obstacle = reference_line_info_->path_decision()->obstacles().Find(blocking_obstacle_id);
// 如果有效路径数据的大小大于1
if (valid_path_data.size() > 1) {
// 比较两条路径数据，选择更优的一条作为最终路径
if (ComparePathData(valid_path_data[0], valid_path_data[1], blocking_obstacle)) {
*final_path = valid_path_data[0];
} else {
*final_path = valid_path_data[1];
}
} else {
// 如果只有一条有效路径，直接作为最终路径
*final_path = valid_path_data[0];
}
// 记录调试信息
RecordDebugInfo(*final_path, final_path->path_label(), reference_line_info_);
// 返回true
return true;
}
// bool LaneBorrowPath::GetBoundaryFromNeighborLane(
// const SidePassDirection pass_direction,
// PathBoundary* const path_bound,
// std::string* borrow_lane_type) {
// // Sanity checks.
// CHECK_NOTNULL(path_bound);
// ACHECK(!path_bound->empty());
// const ReferenceLine& reference_line = reference_line_info_->reference_line();
// double adc_lane_width = PathBoundsDeciderUtil::GetADCLaneWidth(reference_line, init_sl_state_.first[0]);
// double offset_to_map = 0;
// bool borrowing_reverse_lane = false;
// reference_line.GetOffsetToMap(init_sl_state_.first[0], &offset_to_map);
// // Go through every point, update the boundary based on lane info and
// // ADC's position.
// double past_lane_left_width = adc_lane_width / 2.0;
// double past_lane_right_width = adc_lane_width / 2.0;
// int path_blocked_idx = -1;
// for (size_t i = 0; i < path_bound->size(); ++i) {
// double curr_s = (*path_bound)[i].s;
// // 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 {
// 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;
// }
bool LaneBorrowPath::GetBoundaryFromNeighborLane(
const SidePassDirection pass_direction, // 侧向通过方向
PathBoundary* const path_bound, // 路径边界
std::string* borrow_lane_type) { // 借用车道类型
// Sanity checks.
CHECK_NOTNULL(path_bound); // 检查路径边界是否为空
ACHECK(!path_bound->empty()); // 检查路径边界是否为空
const ReferenceLine& reference_line = reference_line_info_->reference_line(); // 获取参考线
double adc_lane_width = PathBoundsDeciderUtil::GetADCLaneWidth(reference_line, init_sl_state_.first[0]); // 获取ADC车道宽度
double offset_to_map = 0; // 到地图的偏移量
bool borrowing_reverse_lane = false; // 是否借用反向车道
reference_line.GetOffsetToMap(init_sl_state_.first[0], &offset_to_map); // 获取到地图的偏移量
// Go through every point, update the boundary based on lane info and
// ADC's position.
double past_lane_left_width = adc_lane_width / 2.0; // 过去的车道左宽度
double past_lane_right_width = adc_lane_width / 2.0; // 过去的车道右宽度
int path_blocked_idx = -1; // 路径阻塞索引
for (size_t i = 0; i < path_bound->size(); ++i) { // 遍历路径边界
double curr_s = (*path_bound)[i].s; // 当前s值
// 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 {
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;
// if (CheckLaneBoundaryType(*reference_line_info_, curr_s, pass_direction)) {
// hdmap::Id neighbor_lane_id;
// if (pass_direction == SidePassDirection::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." << neighbor_lane_id.id();
// } 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." << neighbor_lane_id.id();
// } else {
// ADEBUG << "There is no left neighbor lane.";
// }
// } else if (pass_direction == SidePassDirection::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." << neighbor_lane_id.id();
// } 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." << neighbor_lane_id.id();
// } else {
// ADEBUG << "There is no right neighbor lane.";
// }
// }
// }
// 2. 获取当前点的邻近车道宽度。
double curr_neighbor_lane_width = 0.0;
// 检查车道边界类型
if (CheckLaneBoundaryType(*reference_line_info_, curr_s, pass_direction)) {
hdmap::Id neighbor_lane_id; // 邻近车道ID
// 如果是向左借用
if (pass_direction == SidePassDirection::LEFT_BORROW) {
// 借用左邻近车道。
// 如果能获取到左前方的邻近车道信息
if (reference_line_info_->GetNeighborLaneInfo(
ReferenceLineInfo::LaneType::LeftForward,
curr_s,
&neighbor_lane_id,
&curr_neighbor_lane_width)) {
ADEBUG << "Borrow left forward neighbor lane." << neighbor_lane_id.id();
// 如果能获取到左后方的邻近车道信息
} 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." << neighbor_lane_id.id();
} else {
ADEBUG << "There is no left neighbor lane."; // 没有左邻近车道
}
// 如果是向右借用
} else if (pass_direction == SidePassDirection::RIGHT_BORROW) {
// 借用右邻近车道。
// 如果能获取到右前方的邻近车道信息
if (reference_line_info_->GetNeighborLaneInfo(
ReferenceLineInfo::LaneType::RightForward,
curr_s,
&neighbor_lane_id,
&curr_neighbor_lane_width)) {
ADEBUG << "Borrow right forward neighbor lane." << neighbor_lane_id.id();
// 如果能获取到右后方的邻近车道信息
} 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." << neighbor_lane_id.id();
} else {
ADEBUG << "There is no right neighbor lane."; // 没有右邻近车道
}
}
}
// // 3. Calculate the proper boundary based on lane-width, ADC's position,
// // and ADC's velocity.
// double offset_to_map = 0.0;
// reference_line.GetOffsetToMap(curr_s, &offset_to_map);
// double curr_left_bound_lane = curr_lane_left_width
// + (pass_direction == SidePassDirection::LEFT_BORROW ? curr_neighbor_lane_width : 0.0);
// double curr_right_bound_lane = -curr_lane_right_width
// - (pass_direction == SidePassDirection::RIGHT_BORROW ? curr_neighbor_lane_width : 0.0);
// double curr_left_bound = 0.0;
// double curr_right_bound = 0.0;
// curr_left_bound = curr_left_bound_lane - offset_to_map;
// curr_right_bound = curr_right_bound_lane - offset_to_map;
// // 4. Update the boundary.
// if (!PathBoundsDeciderUtil::UpdatePathBoundaryWithBuffer(
// curr_left_bound, curr_right_bound, BoundType::LANE, BoundType::LANE, "", "", &path_bound->at(i))) {
// path_blocked_idx = static_cast<int>(i);
// }
// if (path_blocked_idx != -1) {
// break;
// }
// }
// PathBoundsDeciderUtil::TrimPathBounds(path_blocked_idx, path_bound);
// *borrow_lane_type = borrowing_reverse_lane ? "reverse" : "forward";
// return true;
// }
// 3. 根据车道宽度、ADC的位置和ADC的速度计算适当的边界。
double offset_to_map = 0.0;
reference_line.GetOffsetToMap(curr_s, &offset_to_map); // 获取到地图的偏移量
double curr_left_bound_lane = curr_lane_left_width
+ (pass_direction == SidePassDirection::LEFT_BORROW ? curr_neighbor_lane_width : 0.0); // 计算当前左边界车道
double curr_right_bound_lane = -curr_lane_right_width
- (pass_direction == SidePassDirection::RIGHT_BORROW ? curr_neighbor_lane_width : 0.0); // 计算当前右边界车道
double curr_left_bound = 0.0;
double curr_right_bound = 0.0;
curr_left_bound = curr_left_bound_lane - offset_to_map; // 计算当前左边界
curr_right_bound = curr_right_bound_lane - offset_to_map; // 计算当前右边界
// 4. 更新边界。
if (!PathBoundsDeciderUtil::UpdatePathBoundaryWithBuffer(
curr_left_bound, curr_right_bound, BoundType::LANE, BoundType::LANE, "", "", &path_bound->at(i))) {
path_blocked_idx = static_cast<int>(i); // 如果更新失败，记录阻塞的路径索引
}
if (path_blocked_idx != -1) {
break; // 如果路径被阻塞，跳出循环
}
}
PathBoundsDeciderUtil::TrimPathBounds(path_blocked_idx, path_bound); // 裁剪路径边界
*borrow_lane_type = borrowing_reverse_lane ? "reverse" : "forward"; // 设置借用车道类型
return true;
}
// void LaneBorrowPath::UpdateSelfPathInfo() {
// auto cur_path = reference_line_info_->path_data();
// if (!cur_path.Empty() && cur_path.path_label().find("self") != std::string::npos
// && cur_path.blocking_obstacle_id().empty()) {
// use_self_lane_ = std::min(use_self_lane_ + 1, 10);
// } else {
// use_self_lane_ = 0;
// }
// blocking_obstacle_id_ = cur_path.blocking_obstacle_id();
// }
void LaneBorrowPath::UpdateSelfPathInfo() {
auto cur_path = reference_line_info_->path_data(); // 获取当前路径数据
// 如果当前路径不为空，且路径标签包含"self"，且没有阻塞障碍物ID
if (!cur_path.Empty() && cur_path.path_label().find("self") != std::string::npos
&& cur_path.blocking_obstacle_id().empty()) {
use_self_lane_ = std::min(use_self_lane_ + 1, 10); // 使用自身车道，最多增加到10
} else {
use_self_lane_ = 0; // 否则，不使用自身车道
}
blocking_obstacle_id_ = cur_path.blocking_obstacle_id(); // 更新阻塞障碍物ID
}
// bool LaneBorrowPath::IsNecessaryToBorrowLane() {
// auto* mutable_path_decider_status
// = injector_->planning_context()->mutable_planning_status()->mutable_path_decider();
// if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) {
// UpdateSelfPathInfo();
// // If originally borrowing neighbor lane:
// if (use_self_lane_ >= 6) {
// // If have been able to use self-lane for some time, then switch to
// // non-lane-borrowing.
// mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
// decided_side_pass_direction_.clear();
// AINFO << "Switch from LANE-BORROW path to SELF-LANE path.";
// }
// } else {
// // If originally not borrowing neighbor lane:
// AINFO << "Blocking obstacle ID[" << mutable_path_decider_status->front_static_obstacle_id() << "]";
// // 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
// if (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);
// AINFO << "LEFT AND RIGHT LANE CAN NOT BORROW";
// return false;
// } else {
// mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
// if (left_borrowable) {
// decided_side_pass_direction_.push_back(SidePassDirection::LEFT_BORROW);
// }
// if (right_borrowable) {
// decided_side_pass_direction_.push_back(SidePassDirection::RIGHT_BORROW);
// }
// }
// }
// use_self_lane_ = 0;
// AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
// }
// return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
// }
bool LaneBorrowPath::IsNecessaryToBorrowLane() {
auto* mutable_path_decider_status
= injector_->planning_context()->mutable_planning_status()->mutable_path_decider(); // 获取路径决策状态
if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) { // 如果当前在借用车道的场景中
UpdateSelfPathInfo(); // 更新自身路径信息
// 如果原本在借用邻居车道：
if (use_self_lane_ >= 6) {
// 如果已经能够使用自身车道一段时间，那么切换到非借用车道
mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
decided_side_pass_direction_.clear();
AINFO << "Switch from LANE-BORROW path to SELF-LANE path.";
}
} else {
// 如果原本没有借用邻居车道：
AINFO << "Blocking obstacle ID[" << mutable_path_decider_status->front_static_obstacle_id() << "]";
// ADC对借用车道的要求检查：
if (!HasSingleReferenceLine(*frame_)) {
return false;
}
if (!IsWithinSidePassingSpeedADC(*frame_)) {
return false;
}
// 对借用车道的障碍物条件检查：
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;
}
// // 切换到借用车道
// if (decided_side_pass_direction_.empty()) {
// // 第一次初始化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);
// AINFO << "LEFT AND RIGHT LANE CAN NOT BORROW";
// return false;
// } else {
// mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
// if (left_borrowable) {
// decided_side_pass_direction_.push_back(SidePassDirection::LEFT_BORROW);
// }
// if (right_borrowable) {
// decided_side_pass_direction_.push_back(SidePassDirection::RIGHT_BORROW);
// }
// }
// }
// use_self_lane_ = 0;
// AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
// }
// return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
// }
// 切换到借用车道
if (decided_side_pass_direction_.empty()) {
// 第一次初始化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);
AINFO << "LEFT AND RIGHT LANE CAN NOT BORROW";
return false;
} else {
// 如果可以借用左侧或右侧车道，则设置为借用车道场景
mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
if (left_borrowable) {
// 如果可以借用左侧车道，则将左侧借用添加到决定的侧向通过方向中
decided_side_pass_direction_.push_back(SidePassDirection::LEFT_BORROW);
}
if (right_borrowable) {
// 如果可以借用右侧车道，则将右侧借用添加到决定的侧向通过方向中
decided_side_pass_direction_.push_back(SidePassDirection::RIGHT_BORROW);
}
}
}
// 设置使用自身车道的次数为0
use_self_lane_ = 0;
AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
}
// 返回当前是否在借用车道的场景中
return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
}
// bool LaneBorrowPath::HasSingleReferenceLine(const Frame& frame) {
// return frame.reference_line_info().size() == 1;
// }
// bool LaneBorrowPath::IsWithinSidePassingSpeedADC(const Frame& frame) {
// return frame.PlanningStartPoint().v() < config_.lane_borrow_max_speed();
// }
// bool LaneBorrowPath::IsLongTermBlockingObstacle() {
// if (injector_->planning_context()->planning_status().path_decider().front_static_obstacle_cycle_counter()
// >= config_.long_term_blocking_obstacle_cycle_threshold()) {
// ADEBUG << "The blocking obstacle is long-term existing.";
// return true;
// } else {
// ADEBUG << "The blocking obstacle is not long-term existing.";
// return false;
// }
// }
// 判断是否只有一条参考线
bool LaneBorrowPath::HasSingleReferenceLine(const Frame& frame) {
return frame.reference_line_info().size() == 1;
}
// 判断当前速度是否在侧向通过的最大速度之内
bool LaneBorrowPath::IsWithinSidePassingSpeedADC(const Frame& frame) {
return frame.PlanningStartPoint().v() < config_.lane_borrow_max_speed();
}
// 判断是否存在长期阻塞的障碍物
bool LaneBorrowPath::IsLongTermBlockingObstacle() {
// 如果前方静态障碍物的周期计数器大于等于长期阻塞障碍物的周期阈值，则认为存在长期阻塞的障碍物
if (injector_->planning_context()->planning_status().path_decider().front_static_obstacle_cycle_counter()
>= config_.long_term_blocking_obstacle_cycle_threshold()) {
ADEBUG << "The blocking obstacle is long-term existing.";
return true;
} else {
ADEBUG << "The blocking obstacle is not long-term existing.";
return false;
}
}
// bool LaneBorrowPath::IsBlockingObstacleWithinDestination(const ReferenceLineInfo& reference_line_info) {
// const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
// const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id();
// if (blocking_obstacle_id.empty()) {
// ADEBUG << "There is no blocking obstacle.";
// return true;
// }
// const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);
// if (blocking_obstacle == nullptr) {
// ADEBUG << "Blocking obstacle is no longer there.";
// return true;
// }
// double blocking_obstacle_s = blocking_obstacle->PerceptionSLBoundary().start_s();
// double adc_end_s = reference_line_info.AdcSlBoundary().end_s();
// ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
// ADEBUG << "ADC is at s = " << adc_end_s;
// ADEBUG << "Destination is at s = " << reference_line_info.SDistanceToDestination() + adc_end_s;
// if (blocking_obstacle_s - adc_end_s > reference_line_info.SDistanceToDestination()) {
// return false;
// }
// return true;
// }
// 判断阻塞障碍物是否在目的地之内
bool LaneBorrowPath::IsBlockingObstacleWithinDestination(const ReferenceLineInfo& reference_line_info) {
// 获取路径决策状态
const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
// 获取前方静态障碍物的ID
const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id();
if (blocking_obstacle_id.empty()) {
// 如果没有阻塞障碍物，返回true
ADEBUG << "There is no blocking obstacle.";
return true;
}
// 在障碍物列表中查找阻塞障碍物
const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);
if (blocking_obstacle == nullptr) {
// 如果阻塞障碍物已经不存在，返回true
ADEBUG << "Blocking obstacle is no longer there.";
return true;
}
// 获取阻塞障碍物的起始s坐标
double blocking_obstacle_s = blocking_obstacle->PerceptionSLBoundary().start_s();
// 获取自动驾驶车辆的结束s坐标
double adc_end_s = reference_line_info.AdcSlBoundary().end_s();
ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
ADEBUG << "ADC is at s = " << adc_end_s;
ADEBUG << "Destination is at s = " << reference_line_info.SDistanceToDestination() + adc_end_s;
// 如果阻塞障碍物距离自动驾驶车辆的距离大于自动驾驶车辆到目的地的距离，返回false
if (blocking_obstacle_s - adc_end_s > reference_line_info.SDistanceToDestination()) {
return false;
}
// 否则，返回true
return true;
}
/*
这段代码的主要功能是
判断阻塞障碍物是否在目的地之内。它首先获取路径决策状态和前方静态障碍物的ID，
然后在障碍物列表中查找阻塞障碍物。如果没有阻塞障碍物或阻塞障碍物已经不存在，
返回true。然后，获取阻塞障碍物的起始s坐标和自动驾驶车辆的结束s坐标，如果
阻塞障碍物距离自动驾驶车辆的距离大于自动驾驶车辆到目的地的距离，返回false，
否则，返回true。*/
// bool LaneBorrowPath::IsBlockingObstacleFarFromIntersection(const ReferenceLineInfo& reference_line_info) {
// // 这个函数返回一个布尔值，表示阻塞障碍物是否远离交叉口。它接受一个ReferenceLineInfo类型的常量引用作为参数。
// const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
// const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id();
// if (blocking_obstacle_id.empty()) {
// ADEBUG << "There is no blocking obstacle.";
// return true;
// }
// const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);
// if (blocking_obstacle == nullptr) {
// ADEBUG << "Blocking obstacle is no longer there.";
// return true;
// }
// // Get blocking obstacle's s.
// double blocking_obstacle_s = blocking_obstacle->PerceptionSLBoundary().end_s();
// ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
// // Get intersection's s and compare with threshold.
// const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();
// for (const auto& overlap : first_encountered_overlaps) {
// ADEBUG << overlap.first << ", " << overlap.second.DebugString();
// if (overlap.first != ReferenceLineInfo::SIGNAL && overlap.first != ReferenceLineInfo::STOP_SIGN) {
// continue;
// }
// auto distance = overlap.second.start_s - blocking_obstacle_s;
// if (overlap.first == ReferenceLineInfo::SIGNAL || overlap.first == ReferenceLineInfo::STOP_SIGN) {
// if (distance < kIntersectionClearanceDist) {
// ADEBUG << "Too close to signal intersection (" << distance << "m); don't SIDE_PASS.";
// return false;
// }
// } else {
// if (distance < kJunctionClearanceDist) {
// ADEBUG << "Too close to overlap_type[" << overlap.first << "] (" << distance << "m); don't SIDE_PASS";
// return false;
// }
// }
// }
// return true;
// }
// 判断阻塞障碍物是否远离交叉口
bool LaneBorrowPath::IsBlockingObstacleFarFromIntersection(const ReferenceLineInfo& reference_line_info) {
// 获取路径决策状态
const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
// 获取前方静态障碍物的ID
const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id();
if (blocking_obstacle_id.empty()) {
// 如果没有阻塞障碍物，返回true
ADEBUG << "There is no blocking obstacle.";
return true;
}
// 在障碍物列表中查找阻塞障碍物
const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);
if (blocking_obstacle == nullptr) {
// 如果阻塞障碍物已经不存在，返回true
ADEBUG << "Blocking obstacle is no longer there.";
return true;
}
// 获取阻塞障碍物的s坐标
double blocking_obstacle_s = blocking_obstacle->PerceptionSLBoundary().end_s();
ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
// 获取交叉口的s坐标并与阈值进行比较
const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();
for (const auto& overlap : first_encountered_overlaps) {
ADEBUG << overlap.first << ", " << overlap.second.DebugString();
if (overlap.first != ReferenceLineInfo::SIGNAL && overlap.first != ReferenceLineInfo::STOP_SIGN) {
continue;
}
// 计算阻塞障碍物与交叉口的距离
auto distance = overlap.second.start_s - blocking_obstacle_s;
if (overlap.first == ReferenceLineInfo::SIGNAL || overlap.first == ReferenceLineInfo::STOP_SIGN) {
// 如果距离小于交叉口清除距离，返回false
if (distance < kIntersectionClearanceDist) {
ADEBUG << "Too close to signal intersection (" << distance << "m); don't SIDE_PASS.";
return false;
}
} else {
// 如果距离小于交叉口清除距离，返回false
if (distance < kJunctionClearanceDist) {
ADEBUG << "Too close to overlap_type[" << overlap.first << "] (" << distance << "m); don't SIDE_PASS";
return false;
}
}
}
// 如果阻塞障碍物远离交叉口，返回true
return true;
}
// bool LaneBorrowPath::IsSidePassableObstacle(const ReferenceLineInfo& reference_line_info) {
// const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
// const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id();
// if (blocking_obstacle_id.empty()) {
// ADEBUG << "There is no blocking obstacle.";
// return false;
// }
// const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);
// if (blocking_obstacle == nullptr) {
// ADEBUG << "Blocking obstacle is no longer there.";
// return false;
// }
// return IsNonmovableObstacle(reference_line_info, *blocking_obstacle);
// }
// 判断是否可以绕过阻塞障碍物
bool LaneBorrowPath::IsSidePassableObstacle(const ReferenceLineInfo& reference_line_info) {
// 获取路径决策状态
const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
// 获取前方静态障碍物的ID
const std::string blocking_obstacle_id = path_decider_status.front_static_obstacle_id();
if (blocking_obstacle_id.empty()) {
// 如果没有阻塞障碍物，返回false
ADEBUG << "There is no blocking obstacle.";
return false;
}
// 在障碍物列表中查找阻塞障碍物
const Obstacle* blocking_obstacle = reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);
if (blocking_obstacle == nullptr) {
// 如果阻塞障碍物已经不存在，返回false
ADEBUG << "Blocking obstacle is no longer there.";
return false;
}
// 判断阻塞障碍物是否是不可移动的障碍物
return IsNonmovableObstacle(reference_line_info, *blocking_obstacle);
}
// void LaneBorrowPath::CheckLaneBorrow(
// const ReferenceLineInfo& reference_line_info,
// bool* left_neighbor_lane_borrowable,
// bool* right_neighbor_lane_borrowable) {
// const ReferenceLine& reference_line = reference_line_info.reference_line();
// *left_neighbor_lane_borrowable = true;
// //*right_neighbor_lane_borrowable = true;
// *right_neighbor_lane_borrowable = false;
// // Check if the left/right neighbor lane is borrowable.
// // 2024.06.18.22.12GOLDMINER修改此处
// static constexpr double kLookforwardDistance = 100.0;
// double check_s = reference_line_info.AdcSlBoundary().end_s();
// const double lookforward_distance = std::min(check_s + kLookforwardDistance, reference_line.Length());
// while (check_s < lookforward_distance) {
// auto ref_point = reference_line.GetNearestReferencePoint(check_s);
// if (ref_point.lane_waypoints().empty()) {
// *left_neighbor_lane_borrowable = false;
// *right_neighbor_lane_borrowable = false;
// return;
// }
// auto ptr_lane_info = reference_line_info.LocateLaneInfo(check_s);
// if (ptr_lane_info->lane().left_neighbor_forward_lane_id().empty()
// && ptr_lane_info->lane().left_neighbor_reverse_lane_id().empty()) {
// *left_neighbor_lane_borrowable = false;
// }
// if (ptr_lane_info->lane().right_neighbor_forward_lane_id().empty()
// && ptr_lane_info->lane().right_neighbor_reverse_lane_id().empty()) {
// *right_neighbor_lane_borrowable = false;
// }
// const auto waypoint = ref_point.lane_waypoints().front();
// hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
// if (*left_neighbor_lane_borrowable) {
// lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
// if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
// || lane_boundary_type == hdmap::LaneBoundaryType::DOUBLE_YELLOW
// || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
// *left_neighbor_lane_borrowable = false;
// }
// ADEBUG << "s[" << check_s << "] left_lane_boundary_type[" << LaneBoundaryType_Type_Name(lane_boundary_type)
// << "]";
// }
// if (*right_neighbor_lane_borrowable) {
// lane_boundary_type = hdmap::RightBoundaryType(waypoint);
// if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
// || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
// *right_neighbor_lane_borrowable = false;
// }
// ADEBUG << "s[" << check_s << "] right_neighbor_lane_borrowable["
// << LaneBoundaryType_Type_Name(lane_boundary_type) << "]";
// }
// check_s += 2.0;
// }
// }
// 检查是否可以借用左/右邻近车道
void LaneBorrowPath::CheckLaneBorrow(
const ReferenceLineInfo& reference_line_info,
bool* left_neighbor_lane_borrowable,
bool* right_neighbor_lane_borrowable) {
// 获取参考线
const ReferenceLine& reference_line = reference_line_info.reference_line();
// 默认左邻近车道可以借用，右邻近车道不可以借用
*left_neighbor_lane_borrowable = true;
*right_neighbor_lane_borrowable = false;
// 定义向前查看的距离
static constexpr double kLookforwardDistance = 100.0;
// 获取自动驾驶车辆的s坐标
double check_s = reference_line_info.AdcSlBoundary().end_s();
// 计算向前查看的距离
const double lookforward_distance = std::min(check_s + kLookforwardDistance, reference_line.Length());
// 在向前查看的距离内检查每个参考点
while (check_s < lookforward_distance) {
// 获取最近的参考点
auto ref_point = reference_line.GetNearestReferencePoint(check_s);
// 如果参考点没有车道信息，则不能借用左/右邻近车道
if (ref_point.lane_waypoints().empty()) {
*left_neighbor_lane_borrowable = false;
*right_neighbor_lane_borrowable = false;
return;
}
// 定位车道信息
auto ptr_lane_info = reference_line_info.LocateLaneInfo(check_s);
// 如果左邻近车道不存在，则不能借用左邻近车道
if (ptr_lane_info->lane().left_neighbor_forward_lane_id().empty()
&& ptr_lane_info->lane().left_neighbor_reverse_lane_id().empty()) {
*left_neighbor_lane_borrowable = false;
}
// 如果右邻近车道不存在，则不能借用右邻近车道
if (ptr_lane_info->lane().right_neighbor_forward_lane_id().empty()
&& ptr_lane_info->lane().right_neighbor_reverse_lane_id().empty()) {
*right_neighbor_lane_borrowable = false;
}
// 获取车道的第一个航点
const auto waypoint = ref_point.lane_waypoints().front();
// 初始化车道边界类型
hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
// 如果可以借用左邻近车道，检查左边界类型
if (*left_neighbor_lane_borrowable) {
lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
// 如果左边界是实线黄线、双黄线或实线白线，则不能借用左邻近车道
if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
|| lane_boundary_type == hdmap::LaneBoundaryType::DOUBLE_YELLOW
|| lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
*left_neighbor_lane_borrowable = false;
}
}
// 如果可以借用右邻近车道，检查右边界类型
if (*right_neighbor_lane_borrowable) {
lane_boundary_type = hdmap::RightBoundaryType(waypoint);
// 如果右边界是实线黄线或实线白线，则不能借用右邻近车道
if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
|| lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
*right_neighbor_lane_borrowable = false;
}
}
// 每次检查后，s坐标向前移动2.0
check_s += 2.0;
}
}
// bool LaneBorrowPath::CheckLaneBoundaryType(
// const ReferenceLineInfo& reference_line_info,
// const double check_s,
// const SidePassDirection& lane_borrow_info) {
// const ReferenceLine& reference_line = reference_line_info.reference_line();
// auto ref_point = reference_line.GetNearestReferencePoint(check_s);
// if (ref_point.lane_waypoints().empty()) {
// return false;
// }
// const auto waypoint = ref_point.lane_waypoints().front();
// hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
// if (lane_borrow_info == SidePassDirection::LEFT_BORROW) {
// lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
// } else if (lane_borrow_info == SidePassDirection::RIGHT_BORROW) {
// lane_boundary_type = hdmap::RightBoundaryType(waypoint);
// }
// if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
// || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
// return false;
// }
// return true;
// }
// 检查车道边界类型
bool LaneBorrowPath::CheckLaneBoundaryType(
const ReferenceLineInfo& reference_line_info,
const double check_s,
const SidePassDirection& lane_borrow_info) {
// 获取参考线
const ReferenceLine& reference_line = reference_line_info.reference_line();
// 获取最近的参考点
auto ref_point = reference_line.GetNearestReferencePoint(check_s);
// 如果参考点没有车道信息，返回false
if (ref_point.lane_waypoints().empty()) {
return false;
}
// 获取车道的第一个航点
const auto waypoint = ref_point.lane_waypoints().front();
// 初始化车道边界类型
hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
// 如果是左侧借道，检查左边界类型
if (lane_borrow_info == SidePassDirection::LEFT_BORROW) {
lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
}
// 如果是右侧借道，检查右边界类型
else if (lane_borrow_info == SidePassDirection::RIGHT_BORROW) {
lane_boundary_type = hdmap::RightBoundaryType(waypoint);
}
// 如果边界类型是实线黄线或实线白线，返回false
if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW
|| lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
return false;
}
// 其他情况返回true
return true;
}
// void LaneBorrowPath::SetPathInfo(PathData* const path_data) {
// std::vector<PathPointDecision> path_decision;
// PathAssessmentDeciderUtil::InitPathPointDecision(*path_data, PathData::PathPointType::IN_LANE, &path_decision);
// // Go through every path_point, and add in-lane/out-of-lane info.
// const auto& discrete_path = path_data->discretized_path();
// bool is_prev_point_out_lane = false;
// SLBoundary ego_sl_boundary;
// for (size_t i = 0; i < discrete_path.size(); ++i) {
// if (!GetSLBoundary(*path_data, i, reference_line_info_, &ego_sl_boundary)) {
// ADEBUG << "Unable to get SL-boundary of ego-vehicle.";
// continue;
// }
// double lane_left_width = 0.0;
// double lane_right_width = 0.0;
// double middle_s = (ego_sl_boundary.start_s() + ego_sl_boundary.end_s()) / 2.0;
// if (reference_line_info_->reference_line().GetLaneWidth(middle_s, &lane_left_width, &lane_right_width)) {
// // Rough sl boundary estimate using single point lane width
// double back_to_inlane_extra_buffer = 0.2;
// double in_and_out_lane_hysteresis_buffer = is_prev_point_out_lane ? back_to_inlane_extra_buffer : 0.0;
// // For lane-borrow path, as long as ADC is not on the lane of
// // reference-line, it is out on other lanes. It might even be
// // on reverse lane!
// if (ego_sl_boundary.end_l() > lane_left_width + in_and_out_lane_hysteresis_buffer
// || ego_sl_boundary.start_l() < -lane_right_width - in_and_out_lane_hysteresis_buffer) {
// if (path_data->path_label().find("reverse") != std::string::npos) {
// std::get<1>((path_decision)[i]) = PathData::PathPointType::OUT_ON_REVERSE_LANE;
// } else if (path_data->path_label().find("forward") != std::string::npos) {
// std::get<1>((path_decision)[i]) = PathData::PathPointType::OUT_ON_FORWARD_LANE;
// } else {
// std::get<1>((path_decision)[i]) = PathData::PathPointType::UNKNOWN;
// }
// if (!is_prev_point_out_lane) {
// if (ego_sl_boundary.end_l() > lane_left_width + back_to_inlane_extra_buffer
// || ego_sl_boundary.start_l() < -lane_right_width - back_to_inlane_extra_buffer) {
// is_prev_point_out_lane = true;
// }
// }
// } else {
// // The path point is within the reference_line's lane.
// std::get<1>((path_decision)[i]) = PathData::PathPointType::IN_LANE;
// if (is_prev_point_out_lane) {
// is_prev_point_out_lane = false;
// }
// }
// } else {
// AERROR << "reference line not ready when setting path point guide, middle_s" << middle_s << ",index" << i
// << "path point" << discrete_path[i].DebugString();
// break;
// }
// }
// path_data->SetPathPointDecisionGuide(std::move(path_decision));
// }
// 设置路径信息
void LaneBorrowPath::SetPathInfo(PathData* const path_data) {
// 初始化路径决策向量
std::vector<PathPointDecision> path_decision;
// 初始化路径点决策
PathAssessmentDeciderUtil::InitPathPointDecision(*path_data, PathData::PathPointType::IN_LANE, &path_decision);
// 获取离散路径
const auto& discrete_path = path_data->discretized_path();
// 初始化前一个点是否在车道外的标志
bool is_prev_point_out_lane = false;
// 初始化自车的SL边界
SLBoundary ego_sl_boundary;
// 遍历离散路径的每一个点
for (size_t i = 0; i < discrete_path.size(); ++i) {
// 获取自车的SL边界，如果获取失败，打印调试信息并跳过当前循环
if (!GetSLBoundary(*path_data, i, reference_line_info_, &ego_sl_boundary)) {
ADEBUG << "Unable to get SL-boundary of ego-vehicle.";
continue;
}
// 初始化车道左宽和右宽
double lane_left_width = 0.0;
double lane_right_width = 0.0;
// 计算SL边界的中点
double middle_s = (ego_sl_boundary.start_s() + ego_sl_boundary.end_s()) / 2.0;
// 获取车道宽度，如果获取成功，进行以下操作
if (reference_line_info_->reference_line().GetLaneWidth(middle_s, &lane_left_width, &lane_right_width)) {
// 使用单点车道宽度进行粗略的SL边界估计
double back_to_inlane_extra_buffer = 0.2;
double in_and_out_lane_hysteresis_buffer = is_prev_point_out_lane ? back_to_inlane_extra_buffer : 0.0;
// 对于借道路径，只要自车不在参考线的车道上，就认为它在其他车道上。甚至可能在反向车道上！
if (ego_sl_boundary.end_l() > lane_left_width + in_and_out_lane_hysteresis_buffer
|| ego_sl_boundary.start_l() < -lane_right_width - in_and_out_lane_hysteresis_buffer) {
// 如果路径标签包含"reverse"，则设置路径点类型为反向车道外
if (path_data->path_label().find("reverse") != std::string::npos) {
std::get<1>((path_decision)[i]) = PathData::PathPointType::OUT_ON_REVERSE_LANE;
}
// 如果路径标签包含"forward"，则设置路径点类型为正向车道外
else if (path_data->path_label().find("forward") != std::string::npos) {
std::get<1>((path_decision)[i]) = PathData::PathPointType::OUT_ON_FORWARD_LANE;
}
// 否则，设置路径点类型为未知
else {
std::get<1>((path_decision)[i]) = PathData::PathPointType::UNKNOWN;
}
// 如果前一个点不在车道外，再次检查当前点是否在车道外
if (!is_prev_point_out_lane) {
if (ego_sl_boundary.end_l() > lane_left_width + back_to_inlane_extra_buffer
|| ego_sl_boundary.start_l() < -lane_right_width - back_to_inlane_extra_buffer) {
is_prev_point_out_lane = true;
}
}
} else {
// 路径点在参考线的车道内
std::get<1>((path_decision)[i]) = PathData::PathPointType::IN_LANE;
if (is_prev_point_out_lane) {
is_prev_point_out_lane = false;
}
}
} else {
// 如果在设置路径点指南时参考线还未准备好，打印错误信息并跳出循环
AERROR << "reference line not ready when setting path point guide, middle_s" << middle_s << ",index" << i
<< "path point" << discrete_path[i].DebugString();
break;
}
}
// 设置路径点决策指南
path_data->SetPathPointDecisionGuide(std::move(path_decision));
}
// bool ComparePathData(const PathData& lhs, const PathData& rhs, const Obstacle* blocking_obstacle) {
// ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();
// 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();
// // Select longer path.
// // If roughly same length, then select self-lane path.
// if (std::fabs(lhs_path_length - rhs_path_length) > kNeighborPathLengthComparisonTolerance) {
// return lhs_path_length > rhs_path_length;
// }
// // 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;
// }
// // For two lane-borrow directions, based on ADC's position,
// // select the more convenient one.
// 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;
// return lhs_on_leftlane;
// }
bool ComparePathData(const PathData& lhs, const PathData& rhs, const Obstacle* blocking_obstacle) {
// 打印正在比较的两条路径的标签
ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();
// 定义一个常量，用于比较两条路径长度的容忍度
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 (std::fabs(lhs_path_length - rhs_path_length) > kNeighborPathLengthComparisonTolerance) {
return lhs_path_length > rhs_path_length;
}
// 如果两条路径长度大致相同，且必须借用邻车道，则优先借用前行车道而不是逆行车道
int lhs_on_reverse = ContainsOutOnReverseLane(lhs.path_point_decision_guide());
int rhs_on_reverse = ContainsOutOnReverseLane(rhs.path_point_decision_guide());
if (std::abs(lhs_on_reverse - rhs_on_reverse) > 6) {
return lhs_on_reverse < rhs_on_reverse;
}
// 如果存在阻塞障碍物，根据障碍物的位置选择左/右路径
if (blocking_obstacle) {
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 {
// 如果没有阻塞障碍物，根据ADC的位置选择左/右路径
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;
}
}
// 如果两条路径长度相同，且都是前行车道，选择先返回自车道的路径
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;
}
// 如果两条路径长度相同，都是前行车道，且同时返回自车道，选择左侧路径进行侧向通行
bool lhs_on_leftlane = lhs.path_label().find("left") != std::string::npos;
return lhs_on_leftlane;
}
// int ContainsOutOnReverseLane(const std::vector<PathPointDecision>& path_point_decision) {
// int ret = 0;
// for (const auto& curr_decision : path_point_decision) {
// if (std::get<1>(curr_decision) == PathData::PathPointType::OUT_ON_REVERSE_LANE) {
// ++ret;
// }
// }
// return ret;
// }
int ContainsOutOnReverseLane(const std::vector<PathPointDecision>& path_point_decision) {
// 初始化返回值为0
int ret = 0;
// 遍历输入向量
for (const auto& curr_decision : path_point_decision) {
// 如果当前元素的第二个字段（PathPointType）为OUT_ON_REVERSE_LANE，则增加计数
if (std::get<1>(curr_decision) == PathData::PathPointType::OUT_ON_REVERSE_LANE) {
++ret;
}
}
// 返回计数结果
return ret;
}
// int GetBackToInLaneIndex(const std::vector<PathPointDecision>& path_point_decision) {
// // ACHECK(!path_point_decision.empty());
// // ACHECK(std::get<1>(path_point_decision.back()) ==
// // PathData::PathPointType::IN_LANE);
// for (int i = static_cast<int>(path_point_decision.size()) - 1; i >= 0; --i) {
// if (std::get<1>(path_point_decision[i]) != PathData::PathPointType::IN_LANE) {
// return i;
// }
// }
// return 0;
// }
int GetBackToInLaneIndex(const std::vector<PathPointDecision>& path_point_decision) {
// 检查输入向量是否为空，如果为空则抛出异常（注释掉的代码）
// ACHECK(!path_point_decision.empty());
// 检查输入向量的最后一个元素的PathPointType是否为IN_LANE，如果不是则抛出异常（注释掉的代码）
// ACHECK(std::get<1>(path_point_decision.back()) ==
// PathData::PathPointType::IN_LANE);
// 从后向前遍历输入向量
for (int i = static_cast<int>(path_point_decision.size()) - 1; i >= 0; --i) {
// 如果当前元素的PathPointType不为IN_LANE，则返回当前索引
if (std::get<1>(path_point_decision[i]) != PathData::PathPointType::IN_LANE) {
return i;
}
}
// 如果所有元素的PathPointType都为IN_LANE，则返回0
return 0;
}
} // namespace planning
} // namespace apollo
12-24
2821
2821
07-12
315
315
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
