文章目录
- 前言
- RULE_BASED_STOP_DECIDER相关配置
- RULE_BASED_STOP_DECIDER总体流程
-
- StopOnSidePass
-
- CheckClearDone
- CheckSidePassStop
- IsPerceptionBlocked
- IsClearToChangeLane
- CheckSidePassStop
- BuildStopDecision
- ELSE:涉及到的一些其他函数
-
- NormalizeAngle
- SelfRotate
- CheckLaneChangeUrgency
- AddPathEndStop
- 参考
前言
在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine
函数会依次调用task_list中的TASK,本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限,文章可能有纰漏的地方,还请批评斧正。
在modules/planning/conf/scenario/lane_follow_config.pb.txt
配置文件中,我们可以看到LaneFollow所需要执行的所有task。
stage_config: {
stage_type: LANE_FOLLOW_DEFAULT_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_REUSE_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
task_type: RSS_DECIDER
本文将继续介绍LaneFollow的第8个TASK——RULE_BASED_STOP_DECIDER
基于规则的停止决策是规划模块的任务,属于task中的decider类别。基于规则的停止决策根据一些规则来设置停止标志。
RULE_BASED_STOP_DECIDER相关配置
modules/planning/conf/planning_config.pb.txt
default_task_config: {
task_type: RULE_BASED_STOP_DECIDER
rule_based_stop_decider_config {
max_adc_stop_speed: 0.5
max_valid_stop_distance: 1.0
search_beam_length: 20.0
search_beam_radius_intensity: 0.08
search_range: 3.14
is_block_angle_threshold: 0.5
}
}
modules/planning/proto/task_config.proto
// RuleBasedStopDeciderConfig
message RuleBasedStopDeciderConfig {
optional double max_adc_stop_speed = 1 [default = 0.3];
optional double max_valid_stop_distance = 2 [default = 0.5];
optional double search_beam_length = 3 [default = 5.0];
optional double search_beam_radius_intensity = 4 [default = 0.08];
optional double search_range = 5 [default = 3.14];
optional double is_block_angle_threshold = 6 [default = 1.57];
optional double approach_distance_for_lane_change = 10 [default = 80.0];
optional double urgent_distance_for_lane_change = 11 [default = 50.0];
}
RULE_BASED_STOP_DECIDER总体流程
-
输入
apollo::common::Status RuleBasedStopDecider::Process(Frame *const frame, ReferenceLineInfo *const reference_line_info)
输入是frame和reference_line_info。 -
输出
输出保存到reference_line_info中。
代码流程及框架
Process中的代码流程如下图所示。
apollo::common::Status RuleBasedStopDecider::Process(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
// 1. Rule_based stop for side pass onto reverse lane
StopOnSidePass(frame, reference_line_info);
// 2. Rule_based stop for urgent lane change
if (FLAGS_enable_lane_change_urgency_checking) {
CheckLaneChangeUrgency(frame);
}
// 3. Rule_based stop at path end position
AddPathEndStop(frame, reference_line_info);
return Status::OK();
}
主要核心的函数就是StopOnSidePass
、CheckLaneChangeUrgency
以及AddPathEndStop
,接着分别对三者进行剖析。
StopOnSidePass
void RuleBasedStopDecider::StopOnSidePass(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
static bool check_clear;// 默认false
static common::PathPoint change_lane_stop_path_point;
// 获取path data
const PathData &path_data = reference_line_info->path_data();
double stop_s_on_pathdata = 0.0;
// 找到"self"类型的路径,return
if (path_data.path_label().find("self") != std::string::npos) {
check_clear = false;
change_lane_stop_path_point.Clear();
return;
}
// CheckClearDone:Check if needed to check clear again for side pass
// 如果check_clear为true,且CheckClearDone成功。设置check_clear为false
if (check_clear &&
CheckClearDone(*reference_line_info, change_lane_stop_path_point)) {
check_clear = false;
}
// CheckSidePassStop:Check if necessary to set stop fence used for nonscenario side pass
// 如果check_clear为false,CheckSidePassStop为true
if (!check_clear &&
CheckSidePassStop(path_data, *reference_line_info, &stop_s_on_pathdata)) {
// 如果障碍物没有阻塞且可以换道,直接return
if (!LaneChangeDecider::IsPerceptionBlocked(
*reference_line_info,
rule_based_stop_decider_config_.search_beam_length(),
rule_based_stop_decider_config_.search_beam_radius_intensity(),
rule_based_stop_decider_config_.search_range(),
rule_based_stop_decider_config_.is_block_angle_threshold()) &&
LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
return;
}
// 检查adc是否停在了stop fence前,否返回true
if (!CheckADCStop(path_data, *reference_line_info, stop_s_on_pathdata)) {
// 设置stop fence,成功就执行 check_clear = true;
if (!BuildSidePassStopFence(path_data, stop_s_on_pathdata,
&change_lane_stop_path_point, frame,
reference_line_info)) {
AERROR "Set side pass stop fail";
}
} else {
if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
check_clear = true;
}
}
}
}
CheckClearDone
// Check if needed to check clear again for side pass
bool RuleBasedStopDecider::CheckClearDone(
const ReferenceLineInfo &reference_line_info,
const common::PathPoint &stop_point) {
// 获取ADC的SL坐标
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line_info.reference_line().GetLaneWidth(
(adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
&lane_right_width);
SLPoint stop_sl_point;
// 获取停止点的SL坐标
reference_line_info.reference_line().XYToSL(stop_point, &stop_sl_point);
// use distance to last stop point to determine if needed to check clear
// again
if (adc_back_edge_s > stop_sl_point.s()) {
if (adc_start_l > -lane_right_width || adc_end_l lane_left_width) {
return true;
}
}
return false;
}
CheckSidePassStop
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
const PathData &path_data, const ReferenceLineInfo &reference_line_info,
double *stop_s_on_pathdata) {
const std::vectorstd::tupledouble, PathData::PathPointType, double>>
&path_point_decision_guide = path_data.path_point_decision_guide();
// 初始化类型
PathData::PathPointType last_path_point_type =
PathData::PathPointType::UNKNOWN;
// 遍历 path_point_decision_guide
for (const auto &point_guide : path_point_decision_guide) {
// 若上一点在车道内,这一点在逆行车道上
if (last_path_point_type == PathData::PathPointType::IN_LANE &&
std::get1>(point_guide) ==
PathData::PathPointType::OUT_ON_REVERSE_LANE) {
*stop_s_on_pathdata = std::get0>(point_guide);
// Approximate the stop fence s based on the vehicle position
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double ego_front_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
common::PathPoint stop_pathpoint;
// 获取stop point
if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
&stop_pathpoint)) {
AERROR "Can't get stop point on path data";
return false;
}
const double ego_theta = stop_pathpoint.theta();
Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
ego_front_to_center * std::sin(ego_theta)};
// stop_fence的位置
const Vec2d stop_fence_pose =
服务器托管网shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
double stop_l_on_pathdata = 0.0;
const auto &nearby_path = reference_line_info.reference_line().map_path();
nearby_path.GetNearestPoint(stop_fence_pose, stop_s_on_pathdata,
&stop_l_on_pathdata);
return true;
}
last_path_point_type = std::get1>(point_guide);
}
return false;
}
IsPerceptionBlocked
参数解释:
search_beam_length
扫描长度
search_beam_radius_intensity
扫描间隔
search_range
依据ADC中心的扫描范围
is_block_angle_threshold
筛选障碍物所占角度大小的阈值
bool LaneChangeDecider::IsPerceptionBlocked(
const ReferenceLineInfo& reference_line_info,
const double search_beam_length, const double search_beam_radius_intensity,
const double search_range, const double is_block_angle_threshold) {
// search_beam_length: 20.0 //is the length of scanning beam
// search_beam_radius_intensity: 0.08 //is the resolution of scanning
// search_range: 3.14 //is the scanning range centering at ADV heading
// is_block_angle_threshold: 0.5 //is the threshold to tell how big a block angle range is perception blocking
// 获取车辆状态、位置、航向角
const auto& vehicle_state = reference_line_info.vehicle_state();
const common::math::Vec2d adv_pos(vehicle_state.x(), vehicle_state.y());
const double adv_heading = vehicle_state.heading();
// 遍历障碍物
for (auto* obstacle :
reference_line_info.path_decision().obstacles().Items()) {
// NormalizeAngle将给定的角度值规范化到一个特定的范围内(-到之间)
double left_most_angle =
common::math::NormalizeAngle(adv_heading + 0.5 * search_range);
double right_most_angle =
common::math::NormalizeAngle(adv_heading - 0.5 * search_range);
bool right_most_found = false;
// 跳过虚拟障碍物
if (obstacle->IsVirtual()) {
ADEBUG "skip one virtual obstacle";
continue;
}
// 获取障碍物多边形
const auto& obstacle_polygon = obstacle->PerceptionPolygon();
// 按角度进行搜索
for (double search_angle = 0.0; search_angle search_range;
search_angle += search_beam_radius_intensity) {
common::math::Vec2d search_beam_end(search_beam_length, 0.0);
const double beam_heading = common::math::NormalizeAngle(
adv_heading - 0.5 * search_range + search_angle);
// search_beam_end绕adv_pos旋转beam_heading角度
search_beam_end.SelfRotate(beam_heading);
search_beam_end += adv_pos;
// 构造线段
common::math::LineSegment2d search_beam(adv_pos, search_beam_end);
// 判断最右边界是否找到,并更新右边界角度
if (!right_most_found && obstacle_polygon.HasOverlap(search_beam)) {
right_most_found = true;
right_most_angle = beam_heading;
}
// 如果最右边界已找到,且障碍物的感知多边形与搜索光束无重叠,则更新左边界角度并跳出循环。
if (right_most_found && !obstacle_polygon.HasOverlap(search_beam)) {
left_most_angle = beam_heading;
break;
}
}
// 如果最右边界未找到,则继续处理下一个障碍物。(说明该障碍物不在搜索范围内)
if (!right_most_found) {
// obstacle is not in search range
continue;
}
// 判断阈值,过滤掉小的障碍物
if (std::fabs(common::math::NormalizeAngle(
left_most_angle - right_most_angle)) > is_block_angle_threshold) {
return true;
}
}
return false;
}
IsClearToChangeLane
这个在【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER已经有过介绍。
CheckSidePassStop
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
const PathData &path_data, const ReferenceLineInfo &reference_line_info,
double *stop_s_on_pathdata) {
const std::vectorstd::tupledouble, PathData::PathPointType, double>>
&path_point_decision_guide = path_data.path_point_decision_guide();
// 初始化类型
PathData::PathPointType last_path_point_type =
PathData::PathPointType::UNKNOWN;
// 遍历 path_point_decision_guide
for (const auto &point_guide : path_point_decision_guide) {
// 若上一点在车道内,这一点在逆行车道上
if (last_path_point_type == PathData::PathPointType::IN_LANE &&
std::get1>(point_guide) ==
PathData::PathPointType::OUT_ON_REVERSE_LANE) {
*stop_s_on_pathdata = std::get0>(point_guide);
// Approximate the stop fence s based on the vehicle position
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double ego_front_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
common::PathPoint stop_pathpoint;
// 获取stop point
if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
&stop_pathpoint)) {
AERROR "Can't get stop point on path data";
return false;
}
const double ego_theta = stop_pathpoint.theta();
Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
ego_front_to_center * std::sin(ego_theta)};
// stop_fence的位置
const Vec2d stop_fence_pose =
shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
double stop_l_on_pathdata = 0.0;
const auto &nearby_path = reference_line_info.reference_line().map_path();
nearby_path.GetNearestPoint(stop_fence_pose, stop_s_on_pathdata,
&stop_l_on_pathdata);
return true;
}
last_path_point_type = std::get1>(point_guide);
}
return false;
}
BuildStopDecision
/*
* @brief: build virtual obstac服务器托管网le of stop wall, and add STOP decision
*/
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
const double stop_distance,
const StopReasonCode& stop_reason_code,
const std::vectorstd::string>& wait_for_obstacles,
const std::string& decision_tag, Frame* const frame,
ReferenceLineInfo* const reference_line_info) {
CHECK_NOTNULL(frame);
CHECK_NOTNULL(reference_line_info);
// 检查停止线是否在参考线上
const auto& reference_line = reference_line_info->reference_line();
if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
AERROR "stop_line_s[" stop_line_s "] is not on reference line";
return 0;
}
// create virtual stop wall
const auto* obstacle =
frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
if (!obstacle) {
AERROR "Failed to create obstacle [" stop_wall_id "]";
return -1;
}
const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
if (!stop_wall) {
AERROR "Failed to add obstacle[" stop_wall_id "]";
return -1;
}
// build stop decision
const double stop_s = stop_line_s - stop_distance;
const auto& stop_point = reference_line.GetReferencePoint(stop_s);
const double stop_heading =
reference_line.GetReferencePoint(stop_s).heading();
ObjectDecisionType stop;
auto* stop_decision = stop.mutable_stop();
stop_decision->set_reason_code(stop_reason_code);
stop_decision->set_distance_s(-stop_distance);
stop_decision->set_stop_heading(stop_heading);
stop_decision->mutable_stop_point()->set_x(stop_point.x());
stop_decision->mutable_stop_point()->set_y(stop_point.y());
stop_decision->mutable_stop_point()->set_z(0.0);
for (size_t i = 0; i wait_for_obstacles.size(); ++i) {
stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
}
auto* path_decision = reference_line_info->path_decision();
path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
return 0;
}
ELSE:涉及到的一些其他函数
NormalizeAngle
NormalizeAngle
将给定的角度值规范化到一个特定的范围内(-到之间)
double NormalizeAngle(const double angle) {
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
if (a 0.0) {
a += (2.0 * M_PI);
}
return a - M_PI;
}
SelfRotate
将向量绕原点旋转
a
n
g
l
e
angle
angle角。
void Vec2d::SelfRotate(const double angle) {
double tmp_x = x_;
x_ = x_ * cos(angle) - y_ * sin(angle);
y_ = tmp_x * sin(angle) + y_ * cos(angle);
}
CheckLaneChangeUrgency
检查紧急换道,当FLAGS_enable_lane_change_urgency_checking
为true时,启用函数。
void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) {
// 直接进入循环,检查每个reference_line_info
for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
// Check if the target lane is blocked or not
// 1. 检查目标道路是否阻塞,如果在change lane path上,就跳过
if (reference_line_info.IsChangeLanePath()) {
is_clear_to_change_lane_ =
LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
is_change_lane_planning_succeed_ =
reference_line_info.Cost() kStraightForwardLineCost;
continue;
}
// If it's not in lane-change scenario || (target lane is not blocked &&
// change lane planning succeed), skip
// 2.如果不是换道的场景,或者(目标lane没有阻塞)并且换道规划成功,跳过
if (frame->reference_line_info().size() 1 ||
(is_clear_to_change_lane_ && is_change_lane_planning_succeed_)) {
continue;
}
// When the target lane is blocked in change-lane case, check the urgency
// Get the end point of current routing
const auto &route_end_waypoint =
reference_line_info.Lanes().RouteEndWaypoint();
// If can't get lane from the route's end waypoint, then skip
// 3.在route的末端无法获得lane,跳过
if (!route_end_waypoint.lane) {
continue;
}
auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
auto *reference_line = reference_line_info.mutable_reference_line();
common::SLPoint sl_point;
// Project the end point to sl_point on current reference lane
// 将当前参考线的点映射到frenet坐标系下
if (reference_line->XYToSL(point, &sl_point) &&
reference_line->IsOnLane(sl_point)) {
// Check the distance from ADC to the end point of current routing
double distance_to_passage_end =
sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
// If ADC is still far from the end of routing, no need to stop, skip
// 4. 如果adc距离routing终点较远,不需要停止,跳过
if (distance_to_passage_end >
rule_based_stop_decider_config_.approach_distance_for_lane_change()) {
continue;
}
// In urgent case, set a temporary stop fence and wait to change lane
// TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
// 5.如果遇到紧急情况,设置临时的stop fence,等待换道
const std::string stop_wall_id = "lane_change_stop";
std::vectorstd::string> wait_for_obstacles;
util::BuildStopDecision(
stop_wall_id, sl_point.s(),
rule_based_stop_decider_config_.urgent_distance_for_lane_change(),
StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
"RuleBasedStopDecider", frame, &reference_line_info);
}
}
}
AddPathEndStop
void RuleBasedStopDecider::AddPathEndStop(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
// 路径不为空且起点到终点的距离不小于20m
if (!reference_line_info->path_data().path_label().empty() &&
reference_line_info->path_data().frenet_frame_path().back().s() -
reference_line_info->path_data().frenet_frame_path().front().s()
FLAGS_short_path_length_threshold) {
// FLAGS_short_path_length_threshold: Threshold for too short path length(20m)
// 创建虚拟墙的ID
const std::string stop_wall_id =
PATH_END_VO_ID_PREFIX + reference_line_info->path_data().path_label();
std::vectorstd::string> wait_for_obstacles;
// 创建stop fence
util::BuildStopDecision(
stop_wall_id,
reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
0.0, StopReasonCode::STOP_REASON_REFERENCE_END, wait_for_obstacles,
"RuleBasedStopDecider", frame, reference_line_info);
}
}
参考
[1] 基于规则的停止决策
服务器托管,北京服务器托管,服务器租用 http://www.fwqtg.net
1.系统配置 硬件配置基本要求 资源 大小 硬盘 >= 20G cpu >= 2核 内存 >= 2G 本教程配置 主机名 IP 配置 master 192.168.10.155 3核+2G+20G worker1 192.168.10.234…