【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER

前言

在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_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZERtask_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_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_DECIDERrule_based_stop_decider_config {max_adc_stop_speed: 0.5max_valid_stop_distance: 1.0search_beam_length: 20.0search_beam_radius_intensity: 0.08search_range: 3.14is_block_angle_threshold: 0.5}
}

modules/planning/proto/task_config.proto

// RuleBasedStopDeciderConfigmessage 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 laneStopOnSidePass(frame, reference_line_info);// 2. Rule_based stop for urgent lane changeif (FLAGS_enable_lane_change_urgency_checking) {CheckLaneChangeUrgency(frame);}// 3. Rule_based stop at path end positionAddPathEndStop(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;// 默认falsestatic common::PathPoint change_lane_stop_path_point;// 获取path dataconst PathData &path_data = reference_line_info->path_data();double stop_s_on_pathdata = 0.0;// 找到"self"类型的路径，returnif (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为falseif (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为trueif (!check_clear &&CheckSidePassStop(path_data, *reference_line_info, &stop_s_on_pathdata)) {// 如果障碍物没有阻塞且可以换道，直接returnif (!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前，否返回trueif (!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// againif (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::vector<std::tuple<double, 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_guidefor (const auto &point_guide : path_point_decision_guide) {// 若上一点在车道内，这一点在逆行车道上if (last_path_point_type == PathData::PathPointType::IN_LANE &&std::get<1>(point_guide) ==PathData::PathPointType::OUT_ON_REVERSE_LANE) {*stop_s_on_pathdata = std::get<0>(point_guide);// Approximate the stop fence s based on the vehicle positionconst 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 pointif (!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::get<1>(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 rangecontinue;}// 判断阈值，过滤掉小的障碍物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::vector<std::tuple<double, 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_guidefor (const auto &point_guide : path_point_decision_guide) {// 若上一点在车道内，这一点在逆行车道上if (last_path_point_type == PathData::PathPointType::IN_LANE &&std::get<1>(point_guide) ==PathData::PathPointType::OUT_ON_REVERSE_LANE) {*stop_s_on_pathdata = std::get<0>(point_guide);// Approximate the stop fence s based on the vehicle positionconst 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 pointif (!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::get<1>(point_guide);}return false;
}

BuildStopDecision

/** @brief: build virtual obstacle 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::vector<std::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 wallconst 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 decisionconst 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

将向量绕原点旋转$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_infofor (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 routingconst 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 routingdouble 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::vector<std::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) {// 路径不为空且起点到终点的距离不小于20mif (!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)// 创建虚拟墙的IDconst std::string stop_wall_id =PATH_END_VO_ID_PREFIX + reference_line_info->path_data().path_label();std::vector<std::string> wait_for_obstacles;// 创建stop fenceutil::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] 基于规则的停止决策