概述
apollo planning模块下modules\planning\common\obstacle_blocking_analyzer.cc/.h实现了一个障碍物阻塞分析器功能?但是没有封装成类,就是几个函数。
这几个函数的作用如下,主要都是实现对障碍物的判断,可以用来判断是否可以对前方障碍物进行绕行
obstacle_blocking_analyzer.h
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/common/frame.h"
namespace apollo {
namespace planning {
//判断是否为非移动的障碍物
//输入参数 道路参考线信息类对象, 被判断的障碍物对象
//判断是否足够远,是否是停止的车辆,是否是被其他障碍物阻塞了?通过这些条件来判断输入障碍物是否为非移动也就是静态障碍物。
bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info,
const Obstacle& obstacle);
/**
* @brief 判断一个障碍物是否阻塞路径,需要自车绕行?
* @param frame包含了一个周期内规划所有信息包括道路参考线等
* @param 感兴趣的障碍物obstacle
* @param 判断一个障碍物是否停止的速度阈值block_obstacle_min_speed
* @param 到前方阻塞障碍物需要绕行的最小距离min_front_sidepass_distance
* 如果太近了,出于安全考虑,不绕行。
* @param 是否要考虑被阻塞障碍物本身被其他障碍物阻塞了enable_obstacle_blocked_check
* 如果前方的障碍物也是被前面的障碍物阻塞,那么就不要尝试绕行它?
*/
bool IsBlockingObstacleToSidePass(const Frame& frame, const Obstacle* obstacle,
double block_obstacle_min_speed,
double min_front_sidepass_distance,
bool enable_obstacle_blocked_check);
//获取自车和障碍物之间的距离?
//输入参数frame,包含一个周期规划的所有数据
//需要计算距离的障碍物对象
double GetDistanceBetweenADCAndObstacle(const Frame& frame,
const Obstacle* obstacle);
//判断障碍物是否阻塞驾驶路径
//输入参数 道路参考线类对象,障碍物类对象
bool IsBlockingDrivingPathObstacle(const ReferenceLine& reference_line,
const Obstacle* obstacle);
//判断是否为停止的车辆
//输入参数 道路参考线类对象,障碍物对象
//用障碍物对象的xy坐标去判断是否处在停车车道?然后又离右边道路边缘足够近?然后障碍物还属于静态障碍物,都满足则说明是停在边上的静止的车
bool IsParkedVehicle(const ReferenceLine& reference_line,
const Obstacle* obstacle);
} // namespace planning
} // namespace apollo
obstacle_blocking_analyzer.cc
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include <algorithm>
#include <memory>
#include <vector>
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/point_factory.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"
namespace apollo {
namespace planning {
using apollo::common::VehicleConfigHelper;
using apollo::hdmap::HDMapUtil;
//这类定义了两个阈值,自车距离阈值35m,障碍物距离阈值15m,干什么用的?
constexpr double kAdcDistanceThreshold = 35.0; // unit: m
constexpr double kObstaclesDistanceThreshold = 15.0;
//判断是否为非移动的障碍物
//输入参数 道路参考线信息类对象, 被判断的障碍物对象
//判断是否足够远,是否是停止的车辆,是否是被其他障碍物阻塞了?通过这些条件来判断输入障碍物是否为非移动也就是静态障碍物。
bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info,
const Obstacle& obstacle) {
//障碍物足够远?获取道路参考线信息类对象的AdcSlBoundary自车的SL边界
//SLBoundary又是.proto文件生成的类 modules\planning\proto\sl_boundary.proto
//其包含信息:S的上下边界,L的上下边界,其实就是自车SL边界矩形?
const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
//如果障碍物的感知SL边界的起始s比自车SL边界终点s+35m还要大
//就说明障碍物离自车非常远?无需考虑直接返回false?
if (obstacle.PerceptionSLBoundary().start_s() >
adc_sl_boundary.end_s() + kAdcDistanceThreshold) {
ADEBUG << " - It is too far ahead and we are not so sure of its status.";
return false;
}
// 静止障碍物,调用下面的函数IsParkedVehicle()判断是否为停止车辆,是的话直接返回true
if (IsParkedVehicle(reference_line_info.reference_line(), &obstacle)) {
ADEBUG << "It is Parked and NON-MOVABLE.";
return true;
}
//障碍物也被其他障碍物阻塞了?
//遍历道路参考线信息对象上的路径决策对象上的障碍物对象列表
for (const auto* other_obstacle :
reference_line_info.path_decision().obstacles().Items()) {
//如果遍历的这个障碍物刚好是输入障碍物对象,判断其id是否相同,直接跳到下一个障碍物
//因为就是要判断输入的这个障碍物是否被其他障碍物阻塞,它自己当然跳过
if (other_obstacle->Id() == obstacle.Id()) {
continue;
}
//如果其他障碍物是虚拟的也直接跳到下一个障碍物
if (other_obstacle->IsVirtual()) {
continue;
}
//判断其他障碍物的SL边界(主要是横向)是否没有与输入参数障碍物SL边界干涉
//如果没有也直接跳到下一个障碍物
if (other_obstacle->PerceptionSLBoundary().start_l() >
obstacle.PerceptionSLBoundary().end_l() ||
other_obstacle->PerceptionSLBoundary().end_l() <
obstacle.PerceptionSLBoundary().start_l()) {
// not blocking the backside vehicle
continue;
}
//判断其他障碍物的SL边界(主要是纵向),其他障碍物的SL边界起始s小于输入障碍物的
//SL边界的终点s?那这样不是SL边界有干涉吗?这个快速排斥实验条件是不是写反了?或者
//其他障碍物的SL边界起始s大于输入障碍物的SL边界的终点s超过15m就认为足够远无影响
//直接跳到下一个障碍物
//判断条件delta_s < 0.0 根据下面函数来看是遍历的其他障碍物在输入障碍物后面无影响?
double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -
obstacle.PerceptionSLBoundary().end_s();
if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) {
continue;
}
// TODO(All): Fix the segmentation bug for large vehicles, otherwise
// the follow line will be problematic.
//修复大型车辆的分段错误,否则下面的行可能是有问题的
//如果执行到这里还没返回,就是找到第一个不会干涉输入障碍物的障碍物,就直接返回false
//逻辑没有太看明白?
ADEBUG << " - It is blocked by others, and will move later.";
return false;
}
//执行到这里还没返回,说明确实输入障碍物是非移动的。
ADEBUG << "IT IS NON-MOVABLE!";
return true;
}
/**
* @brief 判断一个障碍物是否阻塞路径,需要自车绕行?
* @param frame包含了一个周期内规划所有信息包括道路参考线等
* @param 感兴趣的障碍物obstacle
* @param 判断一个障碍物是否停止的速度阈值block_obstacle_min_speed
* @param 到前方阻塞障碍物需要绕行的最小距离min_front_sidepass_distance
* 如果太近了,出于安全考虑,不绕行。
* @param 是否要考虑被阻塞障碍物本身被其他障碍物阻塞了enable_obstacle_blocked_check
* 如果前方的障碍物也是被前面的障碍物阻塞,那么就不要尝试绕行它?
*/
bool IsBlockingObstacleToSidePass(const Frame& frame, const Obstacle* obstacle,
double block_obstacle_min_speed,
double min_front_sidepass_distance,
bool enable_obstacle_blocked_check) {
// 获取必要的信息
//frame里的第一条参考线信息类对象?
const auto& reference_line_info = frame.reference_line_info().front();
//道路参考线
const auto& reference_line = reference_line_info.reference_line();
//自车SL边界
const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
//路径决策类对象
const PathDecision& path_decision = reference_line_info.path_decision();
ADEBUG << "Evaluating Obstacle: " << obstacle->Id();
// 如果障碍物是虚拟的,直接返回false,无需绕行
if (obstacle->IsVirtual()) {
ADEBUG << " - It is virtual.";
return false;
}
// 如果障碍物非静态,或者障碍物的速度大于判断停车的速度阈值,也认为是非静态障碍物
//无需绕行
if (!obstacle->IsStatic() || obstacle->speed() > block_obstacle_min_speed) {
ADEBUG << " - It is non-static.";
return false;
}
// 如果障碍物在自车后面,障碍物的SL边界起始s < 自车SL边界终点s
//也直接返回false无需绕行
if (obstacle->PerceptionSLBoundary().start_s() <= adc_sl_boundary.end_s()) {
ADEBUG << " - It is behind ADC.";
return false;
}
// 障碍物足够远
static constexpr double kAdcDistanceSidePassThreshold = 15.0;
//障碍物的SL边界的起始s 比 自车SL边界的终点s + 15m就认为障碍物足够远,无需绕行
if (obstacle->PerceptionSLBoundary().start_s() >
adc_sl_boundary.end_s() + kAdcDistanceSidePassThreshold) {
ADEBUG << " - It is too far ahead.";
return false;
}
// 障碍物太近了
//如果自车SL边界的终点s + 最小绕行距离阈值 > 输入障碍物SL边界的起点s,出于安全考虑,放弃绕行
if (adc_sl_boundary.end_s() + min_front_sidepass_distance >
obstacle->PerceptionSLBoundary().start_s()) {
ADEBUG << " - It is too close to side-pass.";
return false;
}
// 障碍物没有阻塞我们的驾驶路径,直接返回false,调用IsBlockingDrivingPathObstacle函数进行判断
if (!IsBlockingDrivingPathObstacle(reference_line, obstacle)) {
ADEBUG << " - It is not blocking our way.";
return false;
}
// 障碍物也被其他障碍物阻塞了,也放弃绕行
//如果打开障碍物被其他障碍物阻塞的检查开关 且 输入障碍物不是个静止车辆
if (enable_obstacle_blocked_check &&
!IsParkedVehicle(reference_line, obstacle)) {
//遍历路径决策对象上的障碍物列表
for (const auto* other_obstacle : path_decision.obstacles().Items()) {
//因为要判断输入障碍物是否被其他障碍物阻塞,如果是自己则直接跳到下一个障碍物
if (other_obstacle->Id() == obstacle->Id()) {
continue;
}
//如果障碍物是虚拟的也直接跳到下一个障碍物
if (other_obstacle->IsVirtual()) {
continue;
}
//快速排斥实验,判断其他障碍物的SL边界的横向坐标L区间和自车的SL边界的横向坐标L区间是否有重叠,如果没有也直接跳到下一个障碍物
if (other_obstacle->PerceptionSLBoundary().start_l() >
obstacle->PerceptionSLBoundary().end_l() ||
other_obstacle->PerceptionSLBoundary().end_l() <
obstacle->PerceptionSLBoundary().start_l()) {
// not blocking the backside vehicle
continue;
}
//判断其他障碍物是否在输入障碍物后方 或 其他障碍物距离输入障碍物距离足够远超过35m,则认为输入障碍物并没有被其他障碍物影响/阻塞,直接跳到下一个障碍物
double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -
obstacle->PerceptionSLBoundary().end_s();
if (delta_s < 0.0 || delta_s > kAdcDistanceThreshold) {
continue;
}
// TODO(All): Fix the segmentation bug for large vehicles, otherwise
// the follow line will be problematic.
//如果前面都没有直接跳到下一个障碍物的话,说明输入障碍物确实是被其他障碍物阻塞
//放弃绕行
ADEBUG << " - It is blocked by others, too.";
return false;
}
}
//如果上面都没有返回的话说明可以选择绕行?
ADEBUG << "IT IS BLOCKING!";
return true;
}
//获取自车和障碍物之间的距离?
//输入参数frame,包含一个周期规划的所有数据
//需要计算距离的障碍物对象
double GetDistanceBetweenADCAndObstacle(const Frame& frame,
const Obstacle* obstacle) {
//首先获取道路参考线信息类对象
const auto& reference_line_info = frame.reference_line_info().front();
//获取自车的SL边界
const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
//获取自车和障碍物在纵向s上的距离并返回
double distance_between_adc_and_obstacle =
obstacle->PerceptionSLBoundary().start_s() - adc_sl_boundary.end_s();
return distance_between_adc_and_obstacle;
}
//判断障碍物是否阻塞驾驶路径
//输入参数 道路参考线类对象,障碍物类对象
bool IsBlockingDrivingPathObstacle(const ReferenceLine& reference_line,
const Obstacle* obstacle) {
//驾驶宽度,道路参考线根据障碍物SL边界获取驾驶宽度
const double driving_width =
reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());
//获取自车的车宽
const double adc_width =
VehicleConfigHelper::GetConfig().vehicle_param().width();
ADEBUG << " (driving width = " << driving_width
<< ", adc_width = " << adc_width << ")";
//如果驾驶宽度 > 自车宽度 + 0.3m(左右一起0.3m障碍物缓冲) + 0.1m(绕行缓冲),那么则说明障碍物没有阻塞我们的驾驶路径
//FLAGS_static_obstacle_nudge_l_buffer google gflags的老用法
//FLAGS_代表去modules\planning\common\planning_gflags.cc取出static_obstacle_nudge_l_buffer的值,默认为0.3m
if (driving_width > adc_width + FLAGS_static_obstacle_nudge_l_buffer +
FLAGS_side_pass_driving_width_l_buffer) {
// TODO(jiacheng): make this a GFLAG:
// side_pass_context_.scenario_config_.min_l_nudge_buffer()
ADEBUG << "It is NOT blocking our path.";
return false;
}
//如果执行到这里之前没有返回,说明障碍物阻塞了我们的路径。
ADEBUG << "It is blocking our path.";
return true;
}
//判断是否为停止的车辆
//输入参数 道路参考线类对象,障碍物对象
//用障碍物对象的xy坐标去判断是否处在停车车道?然后又离右边道路边缘足够近?然后障碍物还属于静态障碍物,都满足则说明是停在边上的静止的车
bool IsParkedVehicle(const ReferenceLine& reference_line,
const Obstacle* obstacle) {
//enable_scenario_side_pass_multiple_parked_obstacles默认值为true
//所以不会进入该if,使能场景绕行多辆停止车辆?
if (!FLAGS_enable_scenario_side_pass_multiple_parked_obstacles) {
return false;
}
//初始定义道路的左宽度,右宽度,最大道路右边宽度为0.0
double road_left_width = 0.0;
double road_right_width = 0.0;
double max_road_right_width = 0.0;
//参考线根据障碍物感知的SL边界 的起始点s获取道路宽度,道路中心线左边宽度放入
//road_left_width,道路中心线右边宽度放入road_right_width
reference_line.GetRoadWidth(obstacle->PerceptionSLBoundary().start_s(),
&road_left_width, &road_right_width);
//道路中心线右边的最大宽度为道路中心线右边宽度
max_road_right_width = road_right_width;
//道路参考线根据障碍物感知的SL边界 的终点s获取道路宽度,中心线左边的宽度放入road_left_width,右边放入road_right_width
reference_line.GetRoadWidth(obstacle->PerceptionSLBoundary().end_s(),
&road_left_width, &road_right_width);
//最大道路中心线右边宽度 = 中心线右边最大宽度和右边道路宽度之间的较大值
max_road_right_width = std::max(max_road_right_width, road_right_width);
//判断是否在道路边缘,如果障碍物的SL边界的起始点横坐标l > 最大中心线右边宽度-0.1,说明是离边界线很近
bool is_at_road_edge = std::abs(obstacle->PerceptionSLBoundary().start_l()) >
max_road_right_width - 0.1;
//车道vector列表
std::vector<std::shared_ptr<const hdmap::LaneInfo>> lanes;
//障碍物盒,障碍物对象去访问成员函数获取感知边界盒
auto obstacle_box = obstacle->PerceptionBoundingBox();
//这个是高精度地图模块,根据障碍物边界盒中心的xy坐标,获取其所在车道
HDMapUtil::BaseMapPtr()->GetLanes(
common::util::PointFactory::ToPointENU(obstacle_box.center().x(),
obstacle_box.center().y()),
std::min(obstacle_box.width(), obstacle_box.length()), &lanes);
//初始定义在停止车道上的布尔量为false
bool is_on_parking_lane = false;
//如果障碍物边界盒所处的车道里只有一个数 且 车道列表里第一个车道的车道类型为PARKING,就说明在停车车道上
if (lanes.size() == 1 &&
lanes.front()->lane().type() == apollo::hdmap::Lane::PARKING) {
is_on_parking_lane = true;
}
//在停车车道上又在道路边缘则说明是停车了
bool is_parked = is_on_parking_lane || is_at_road_edge;
//停住了而且障碍物为静态说明是一辆停住的车
return is_parked && obstacle->IsStatic();
}
} // namespace planning
} // namespace apollo