目录
MakeStaticObstacleDecision()的流程图编辑
MakeStaticObstacleDecision()的代码解析
path_decision->AddLongitudinalDecision()
ObjectDecisionType Obstacle::MergeLongitudinalDecision()
path_decision->MergeWithMainStop()
obstacle.MinRadiusStopDistance()
PathDecider的主要功能
PathDecider路径决策类就是对阻塞自车参考线的障碍物做决策，是否要纵向stop，或者是否可以横向ignore，或者是否要车道内横向让行(nudge)，apollo路径决策时是只考虑静态障碍物，动态障碍物决策在速度规划时考虑。
PathDecider代码分析
Process()
pathdecider的主要功能由Process()这个函数实现。
Process()函数的作用：对自车参考线的障碍物调用MakeObjectDecision()函数去对这些阻碍自车参考线障碍物做决策
Status PathDecider::Process(const ReferenceLineInfo *reference_line_info,
const PathData &path_data,
PathDecision *const path_decision) {
/// 如果复用了之前的path则可以跳过
if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {
return Status::OK();
}
std::string blocking_obstacle_id;
if (reference_line_info->GetBlockingObstacle() != nullptr) {
blocking_obstacle_id = reference_line_info->GetBlockingObstacle()->Id();
}
/// 对障碍物做路径决策
if (!MakeObjectDecision(path_data, blocking_obstacle_id, path_decision)) {
const std::string msg = "Failed to make decision based on tunnel";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
return Status::OK();
}MakeObjectDecision()
MakeObjectDecision()函数的作用：调用MakeStaticObstacleDecision()函数对静态障碍物做决策，路径决策时只考虑静态障碍物
bool PathDecider::MakeObjectDecision(const PathData &path_data,
const std::string &blocking_obstacle_id,
PathDecision *const path_decision) {
if (!MakeStaticObstacleDecision(path_data, blocking_obstacle_id,
path_decision)) {
AERROR << "Failed to make decisions for static obstacles";
return false;
}
return true;
}MakeStaticObstacleDecision()
MakeStaticObstacleDecision()函数的作用：对静态障碍物做决策，若障碍物边界距离自车未来边界最近的横向距离大于3m就横向Ignore，若<0.15m就做纵向stop决策，若[0.15,3.0]m时就做nudge决策，nudge决策也分为车道内向左绕行，车道内向右绕行。
MakeStaticObstacleDecision()的流程图
MakeStaticObstacleDecision()的代码解析
bool PathDecider::MakeStaticObstacleDecision(
const PathData &path_data, const std::string &blocking_obstacle_id,
PathDecision *const path_decision) {
// Sanity checks and get important values.
ACHECK(path_decision);
/// 获取frenet坐标系下的路径坐标
const auto &frenet_path = path_data.frenet_frame_path();
if (frenet_path.empty()) {
AERROR << "Path is empty.";
return false;
}
/// 获取自车半车宽
const double half_width =
common::VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
/// 设置自车横向半径，意思就是里自车边缘横向距离(FLAGS_lateral_ignore_buffer)以上的障碍物忽略
const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer;
// Go through every obstacle and make decisions.
for (const auto *obstacle : path_decision->obstacles().Items()) {
const std::string &obstacle_id = obstacle->Id();
const std::string obstacle_type_name =
PerceptionObstacle_Type_Name(obstacle->Perception().type());
ADEBUG << "obstacle_id[<< " << obstacle_id << "] type["
<< obstacle_type_name << "]";
/// 如果不是静态障碍物或者是虚拟障碍物就跳过
if (!obstacle->IsStatic() || obstacle->IsVirtual()) {
continue;
}
// - skip decision making for obstacles with IGNORE/STOP decisions already.
/// 如果横向纵向决策都已经有了ignore就跳过
if (obstacle->HasLongitudinalDecision() &&
obstacle->LongitudinalDecision().has_ignore() &&
obstacle->HasLateralDecision() &&
obstacle->LateralDecision().has_ignore()) {
continue;
}
/// 如果纵向决策已经有了stop就跳过
if (obstacle->HasLongitudinalDecision() &&
obstacle->LongitudinalDecision().has_stop()) {
// STOP decision
continue;
}
// - add STOP decision for blocking obstacles.
/// 如果障碍物的id是属于静态阻塞障碍物 blocking_obstacle_id，
/// 并且当前injector里的规划状态不处于借道场景的话，就对这个阻塞的障碍物添加stop决策，
/// 并打上PathDecider/blocking_obstacle的标签
if (obstacle->Id() == blocking_obstacle_id &&
!injector_->planning_context()
->planning_status()
.path_decider()
.is_in_path_lane_borrow_scenario()) {
// Add stop decision
ADEBUG << "Blocking obstacle = " << blocking_obstacle_id;
ObjectDecisionType object_decision;
*object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);
path_decision->AddLongitudinalDecision("PathDecider/blocking_obstacle",
obstacle->Id(), object_decision);
continue;
}
// - skip decision making for clear-zone obstacles.
/// 障碍物投影到参考线的ST边界，跳过那些边界类型是禁停区KEEP_CLEAR的障碍物
if (obstacle->reference_line_st_boundary().boundary_type() ==
STBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
// 0. IGNORE by default and if obstacle is not in path s at all.
ObjectDecisionType object_decision;
object_decision.mutable_ignore();
/// 获取障碍物在在参考线上投影的SL边界sl_boundary
/// 获取障碍物的SL边界，其实就是(start_s,end_s,start_l,end_l)
const auto &sl_boundary = obstacle->PerceptionSLBoundary();
/// 如果障碍物的s不在路径path的s范围内，直接对这个障碍物横纵向都添加ignore决策，
/// 并打上tag PathDecider/not-in-s
if (sl_boundary.end_s() < frenet_path.front().s() ||
sl_boundary.start_s() > frenet_path.back().s()) {
path_decision->AddLongitudinalDecision("PathDecider/not-in-s",
obstacle->Id(), object_decision);
path_decision->AddLateralDecision("PathDecider/not-in-s", obstacle->Id(),
object_decision);
continue;
}
/// frenet_point就是在自车path路径上找到距离障碍物SLBoundary最近的点
const auto frenet_point = frenet_path.GetNearestPoint(sl_boundary);
/// curr_l就是frenet_point的L坐标
const double curr_l = frenet_point.l();
/// min_nudge_l = 自车车宽一半 + 静态障碍物缓冲距离的一半
double min_nudge_l =
half_width +
config_.path_decider_config().static_obstacle_buffer() / 2.0;
/// l方向上，静态障碍物离path太远了，就横向决策上ignore，并打上PathDecider/not-in-l的tag
/// 横向距离太远判定是自车和静态障碍物之间的横向距离要大于FLAGS_lateral_ignore_buffer
if (curr_l - lateral_radius > sl_boundary.end_l() ||
curr_l + lateral_radius < sl_boundary.start_l()) {
// 1. IGNORE if laterally too far away.
path_decision->AddLateralDecision("PathDecider/not-in-l", obstacle->Id(),
object_decision);
} else if (sl_boundary.end_l() >= curr_l - min_nudge_l &&
sl_boundary.start_l() <= curr_l + min_nudge_l) {
/// l方向上，静态障碍物离path太近了，而且此障碍物要距离自车最近
/// 就做出纵向决策上的stop，并且打上PathDecider/nearest-stop的tag
/// 横向距离太近判定是自车和静态障碍物之间的横向距离要小于
/// config_.path_decider_config().static_obstacle_buffer()
// 2. STOP if laterally too overlapping.
*object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);
if (path_decision->MergeWithMainStop(
object_decision.stop(), obstacle->Id(),
reference_line_info_->reference_line(),
reference_line_info_->AdcSlBoundary())) {
path_decision->AddLongitudinalDecision("PathDecider/nearest-stop",
obstacle->Id(), object_decision);
} else {
/// l方向上，静态障碍物离path太近了，而且此障碍物要距离自车不是最近的
/// 就做出纵向决策上的ignore，并且打上PathDecider/not-nearest-stop的tag
ObjectDecisionType object_decision;
object_decision.mutable_ignore();
path_decision->AddLongitudinalDecision("PathDecider/not-nearest-stop",
obstacle->Id(), object_decision);
}
} else {
// 3. NUDGE if laterally very close.
/// l方向上，当静态障碍物离自车的距离小于FLAGS_lateral_ignore_buffer
/// 并且大于config_.path_decider_config().static_obstacle_buffer()
/// 那就在横向上做出nudge的决策，并且根据sl_boundary判断出nudge的方向
if (sl_boundary.end_l() < curr_l - min_nudge_l) { // &&
// sl_boundary.end_l() > curr_l - min_nudge_l - 0.3) {
// LEFT_NUDGE
ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);
object_nudge_ptr->set_distance_l(
config_.path_decider_config().static_obstacle_buffer());
path_decision->AddLateralDecision("PathDecider/left-nudge",
obstacle->Id(), object_decision);
} else if (sl_boundary.start_l() > curr_l + min_nudge_l) { // &&
// sl_boundary.start_l() < curr_l + min_nudge_l + 0.3) {
// RIGHT_NUDGE
ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);
object_nudge_ptr->set_distance_l(
-config_.path_decider_config().static_obstacle_buffer());
path_decision->AddLateralDecision("PathDecider/right-nudge",
obstacle->Id(), object_decision);
}
}
}
return true;
}GenerateObjectStopDecision()
GenerateObjectStopDecision()函数的作用：设置停止决策的属性，往里设置停止距离，设置停止点x,y,heading等
ObjectStop PathDecider::GenerateObjectStopDecision(
const Obstacle &obstacle) const {
ObjectStop object_stop;
/// 停止距离的计算就是自车以最小转弯半径可以绕过障碍物而不装上障碍物的最小s
double stop_distance = obstacle.MinRadiusStopDistance(
VehicleConfigHelper::GetConfig().vehicle_param());
object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
object_stop.set_distance_s(-stop_distance);
const double stop_ref_s =
obstacle.PerceptionSLBoundary().start_s() - stop_distance;
const auto stop_ref_point =
reference_line_info_->reference_line().GetReferencePoint(stop_ref_s);
object_stop.mutable_stop_point()->set_x(stop_ref_point.x());
object_stop.mutable_stop_point()->set_y(stop_ref_point.y());
object_stop.set_stop_heading(stop_ref_point.heading());
return object_stop;
} PathDecider里用到的其他函数
path_decision->AddLongitudinalDecision()
path_decision->AddLongitudinalDecision()函数的主要作用：对障碍物加上对应的纵向decision，但是障碍物原本已有decision时进行逻辑上的merge，主要由ObjectDecisionType Obstacle::MergeLongitudinalDecision()实现，对于所有decision，merge的结果永远是安全价值大的decision覆盖掉安全价值小的decision，横纵向的decision安全价值定义在两个unordered_map里面：const std::unordered_map<ObjectDecisionType::ObjectTagCase, int, Obstacle::ObjectTagCaseHash> Obstacle::s_longitudinal_decision_safety_sorter_、const std::unordered_map<ObjectDecisionType::ObjectTagCase, int, Obstacle::ObjectTagCaseHash> Obstacle::s_lateral_decision_safety_sorter_ 。
两个unordered_map
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
Obstacle::ObjectTagCaseHash>
Obstacle::s_longitudinal_decision_safety_sorter_ = {
{ObjectDecisionType::kIgnore, 0},
{ObjectDecisionType::kOvertake, 100},
{ObjectDecisionType::kFollow, 300},
{ObjectDecisionType::kYield, 400},
{ObjectDecisionType::kStop, 500}};
const std::unordered_map<ObjectDecisionType::ObjectTagCase, int,
Obstacle::ObjectTagCaseHash>
Obstacle::s_lateral_decision_safety_sorter_ = {
{ObjectDecisionType::kIgnore, 0}, {ObjectDecisionType::kNudge, 100}};ObjectDecisionType Obstacle::MergeLongitudinalDecision()
ObjectDecisionType Obstacle::MergeLongitudinalDecision(
const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {
if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
return rhs;
}
if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {
return lhs;
}
const auto lhs_val =
FindOrDie(s_longitudinal_decision_safety_sorter_, lhs.object_tag_case());
const auto rhs_val =
FindOrDie(s_longitudinal_decision_safety_sorter_, rhs.object_tag_case());
/// 如果decision的value不同就按value就行decision merge，否者相同的的话就比较决策中的distance_s()
if (lhs_val < rhs_val) {
return rhs;
} else if (lhs_val > rhs_val) {
return lhs;
} else {
if (lhs.has_ignore()) {
return rhs;
} else if (lhs.has_stop()) {
return lhs.stop().distance_s() < rhs.stop().distance_s() ? lhs : rhs;
} else if (lhs.has_yield()) {
return lhs.yield().distance_s() < rhs.yield().distance_s() ? lhs : rhs;
} else if (lhs.has_follow()) {
return lhs.follow().distance_s() < rhs.follow().distance_s() ? lhs : rhs;
} else if (lhs.has_overtake()) {
return lhs.overtake().distance_s() > rhs.overtake().distance_s() ? lhs
: rhs;
} else {
DCHECK(false) << "Unknown decision";
}
}
return lhs; // stop compiler complaining
}path_decision->MergeWithMainStop()
path_decision->MergeWithMainStop()函数的主要作用：对于多个stop的decision，根据stop decision里的stop距离来进行merge，永远选择离我们最近的stop decision。
bool PathDecision::MergeWithMainStop(const ObjectStop &obj_stop,
const std::string &obj_id,
const ReferenceLine &reference_line,
const SLBoundary &adc_sl_boundary) {
common::PointENU stop_point = obj_stop.stop_point();
common::SLPoint stop_line_sl;
reference_line.XYToSL(stop_point, &stop_line_sl);
double stop_line_s = stop_line_sl.s();
/// stop decision必须在reference_line范围内
if (stop_line_s < 0.0 || stop_line_s > reference_line.Length()) {
AERROR << "Ignore object:" << obj_id << " fence route_s[" << stop_line_s
<< "] not in range[0, " << reference_line.Length() << "]";
return false;
}
// check stop_line_s vs adc_s, ignore if it is further way than main stop
/// 计算停止距离s
const auto &vehicle_config = common::VehicleConfigHelper::GetConfig();
stop_line_s = std::fmax(
stop_line_s, adc_sl_boundary.end_s() -
vehicle_config.vehicle_param().front_edge_to_center());
if (stop_line_s >= stop_reference_line_s_) {
ADEBUG << "stop point is farther than current main stop point.";
return false;
}
main_stop_.Clear();
main_stop_.set_reason_code(obj_stop.reason_code());
main_stop_.set_reason("stop by " + obj_id);
main_stop_.mutable_stop_point()->set_x(obj_stop.stop_point().x());
main_stop_.mutable_stop_point()->set_y(obj_stop.stop_point().y());
main_stop_.set_stop_heading(obj_stop.stop_heading());
stop_reference_line_s_ = stop_line_s;
ADEBUG << " main stop obstacle id:" << obj_id
<< " stop_line_s:" << stop_line_s << " stop_point: ("
<< obj_stop.stop_point().x() << obj_stop.stop_point().y()
<< " ) stop_heading: " << obj_stop.stop_heading();
return true;
}obstacle.MinRadiusStopDistance()
obstacle.MinRadiusStopDistance()函数的作用：计算自车以当前最小的转弯半径，绕靠这个障碍物所需要的最小的S距离（函数内的计算将reference_line当作直线近似处理），外加一个安全的stop_distance_buffer ，要想直观的解释计算过程需要画图（不会用电脑画图，图之后奉上）。
double Obstacle::MinRadiusStopDistance(
const common::VehicleParam& vehicle_param) const {
if (min_radius_stop_distance_ > 0) {
return min_radius_stop_distance_;
}
static constexpr double stop_distance_buffer = 0.5;
const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();
double lateral_diff =
vehicle_param.width() / 2.0 + std::max(std::fabs(sl_boundary_.start_l()),
std::fabs(sl_boundary_.end_l()));
const double kEpison = 1e-5;
lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);
double stop_distance =
std::sqrt(std::fabs(min_turn_radius * min_turn_radius -
(min_turn_radius - lateral_diff) *
(min_turn_radius - lateral_diff))) +
stop_distance_buffer;
stop_distance -= vehicle_param.front_edge_to_center();
/// 将stop_distance限制在最大值和最小值之间
stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle);
stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle);
return stop_distance;
}
2336
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
