【Apollo学习笔记】——规划模块TASK之PATH_REUSE_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的第二个TASK——PATH_REUSE_DECIDER

PATH_REUSE_DECIDER功能简介

主要功能：检查路径是否可重用,提高帧间平顺性。
主要逻辑：主要判断是否可以重用上一帧规划的路径。若上一帧的路径未与障碍物发生碰撞，则可以重用，提高稳定性，节省计算量。若上一帧的规划出的路径发生碰撞，则重新规划路径。

PATH_REUSE_DECIDER相关配置

PATH_REUSE_DECIDER的相关配置集中在以下两个文件：modules/planning/conf/planning_config.pb.txt和modules/planning/conf/scenario/lane_follow_config.pb.txt

// modules/planning/conf/planning_config.pb.txt
default_task_config: {task_type: PATH_REUSE_DECIDERpath_reuse_decider_config {reuse_path: false}
}
// modules/planning/conf/scenario/lane_follow_config.pb.txttask_config: {task_type: PATH_REUSE_DECIDERpath_reuse_decider_config {reuse_path: false}}

可以看到，默认情况不启用PATH_REUSE，改为true后启用。

PATH_REUSE_DECIDER总体流程

接着来看一看PATH_REUSE_DECIDER的代码逻辑。代码路径：modules/planning/tasks/deciders/path_reuse_decider/path_reuse_decider.cc
主函数逻辑集中在Process函数中：

Status PathReuseDecider::Process(Frame* const frame,ReferenceLineInfo* const reference_line_info) {// Sanity checks.CHECK_NOTNULL(frame);CHECK_NOTNULL(reference_line_info);if (!Decider::config_.path_reuse_decider_config().reuse_path()) {ADEBUG << "skipping reusing path: conf";reference_line_info->set_path_reusable(false);return Status::OK();}// skip path reuse if not in LANE_FOLLOW_SCENARIOconst auto scenario_type = injector_->planning_context()->planning_status().scenario().scenario_type();if (scenario_type != ScenarioType::LANE_FOLLOW) {ADEBUG << "skipping reusing path: not in LANE_FOLLOW scenario";reference_line_info->set_path_reusable(false);return Status::OK();}// active path reuse during change_lane onlyauto* lane_change_status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane();ADEBUG << "lane change status: " << lane_change_status->ShortDebugString();// skip path reuse if not in_change_laneif (lane_change_status->status() != ChangeLaneStatus::IN_CHANGE_LANE &&!FLAGS_enable_reuse_path_in_lane_follow) {ADEBUG << "skipping reusing path: not in lane_change";reference_line_info->set_path_reusable(false);return Status::OK();}// for hybrid model: skip reuse path for valid path referenceconst bool valid_model_output =reference_line_info->path_data().is_valid_path_reference();if (valid_model_output) {ADEBUG << "skipping reusing path: path reference is valid";reference_line_info->set_path_reusable(false);return Status::OK();}/*count total_path_ when in_change_lane && reuse_path*/++total_path_counter_;/*reuse path when in non_change_lane reference line oroptimization succeeded in change_lane reference line*/bool is_change_lane_path = reference_line_info->IsChangeLanePath();if (is_change_lane_path && !lane_change_status->is_current_opt_succeed()) {reference_line_info->set_path_reusable(false);ADEBUG << "reusable_path_counter[" << reusable_path_counter_<< "] total_path_counter[" << total_path_counter_ << "]";ADEBUG << "Stop reusing path when optimization failed on change lane path";return Status::OK();}// stop reusing current path:// 1. replan path// 2. collision// 3. failed to trim previous path// 4. speed optimization failed on previous pathbool speed_optimization_successful = false;const auto& history_frame = injector_->frame_history()->Latest();if (history_frame) {const auto history_trajectory_type =history_frame->reference_line_info().front().trajectory_type();speed_optimization_successful =(history_trajectory_type != ADCTrajectory::SPEED_FALLBACK);}// const auto history_trajectory_type = injector_->FrameHistory()s//                                          ->Latest()//                                          ->reference_line_info()//                                          .front()//                                          .trajectory_type();if (path_reusable_) {if (!frame->current_frame_planned_trajectory().is_replan() &&speed_optimization_successful && IsCollisionFree(reference_line_info) &&TrimHistoryPath(frame, reference_line_info)) {ADEBUG << "reuse path";++reusable_path_counter_;  // count reusable path} else {// stop reuse pathADEBUG << "stop reuse path";path_reusable_ = false;}} else {// F -> Tauto* mutable_path_decider_status = injector_->planning_context()->mutable_planning_status()->mutable_path_decider();static constexpr int kWaitCycle = -2;  // wait 2 cycleconst int front_static_obstacle_cycle_counter =mutable_path_decider_status->front_static_obstacle_cycle_counter();const bool ignore_blocking_obstacle =IsIgnoredBlockingObstacle(reference_line_info);ADEBUG << "counter[" << front_static_obstacle_cycle_counter<< "] IsIgnoredBlockingObstacle[" << ignore_blocking_obstacle << "]";// stop reusing current path:// 1. blocking obstacle disappeared or moving far away// 2. trimming successful// 3. no statical obstacle collision.if ((front_static_obstacle_cycle_counter <= kWaitCycle ||ignore_blocking_obstacle) &&speed_optimization_successful && IsCollisionFree(reference_line_info) &&TrimHistoryPath(frame, reference_line_info)) {// enable reuse pathADEBUG << "reuse path: front_blocking_obstacle ignorable";path_reusable_ = true;++reusable_path_counter_;}}reference_line_info->set_path_reusable(path_reusable_);ADEBUG << "reusable_path_counter[" << reusable_path_counter_<< "] total_path_counter[" << total_path_counter_ << "]";return Status::OK();
}

PATH_REUSE_DECIDER相关子函数

IsCollisionFree

bool PathReuseDecider::IsCollisionFree(ReferenceLineInfo* const reference_line_info) {const ReferenceLine& reference_line = reference_line_info->reference_line();static constexpr double kMinObstacleArea = 1e-4;const double kSBuffer = 0.5;static constexpr int kNumExtraTailBoundPoint = 21;static constexpr double kPathBoundsDeciderResolution = 0.5;// current vehicle sl positioncommon::SLPoint adc_position_sl;GetADCSLPoint(reference_line, &adc_position_sl);// current obstaclesstd::vector<Polygon2d> obstacle_polygons;for (auto obstacle :reference_line_info->path_decision()->obstacles().Items()) {// filtered all non-static objects and virtual obstacleif (!obstacle->IsStatic() || obstacle->IsVirtual()) {if (!obstacle->IsStatic()) {ADEBUG << "SPOT a dynamic obstacle";}if (obstacle->IsVirtual()) {ADEBUG << "SPOT a virtual obstacle";}continue;}const auto& obstacle_sl = obstacle->PerceptionSLBoundary();// Ignore obstacles behind ADCif ((obstacle_sl.end_s() < adc_position_sl.s() - kSBuffer) ||// Ignore too small obstacles.(obstacle_sl.end_s() - obstacle_sl.start_s()) *(obstacle_sl.end_l() - obstacle_sl.start_l()) <kMinObstacleArea) {continue;}obstacle_polygons.push_back(Polygon2d({Vec2d(obstacle_sl.start_s(), obstacle_sl.start_l()),Vec2d(obstacle_sl.start_s(), obstacle_sl.end_l()),Vec2d(obstacle_sl.end_s(), obstacle_sl.end_l()),Vec2d(obstacle_sl.end_s(), obstacle_sl.start_l())}));}if (obstacle_polygons.empty()) {return true;}const auto& history_frame = injector_->frame_history()->Latest();if (!history_frame) {return false;}const DiscretizedPath& history_path =history_frame->current_frame_planned_path();// path end point// 将上一段轨迹的终点投影到SL坐标系下common::SLPoint path_end_position_sl;common::math::Vec2d path_end_position = {history_path.back().x(),history_path.back().y()};reference_line.XYToSL(path_end_position, &path_end_position_sl);for (size_t i = 0; i < history_path.size(); ++i) {common::SLPoint path_position_sl;common::math::Vec2d path_position = {history_path[i].x(),history_path[i].y()};reference_line.XYToSL(path_position, &path_position_sl);if (path_end_position_sl.s() - path_position_sl.s() <=kNumExtraTailBoundPoint * kPathBoundsDeciderResolution) {break;}if (path_position_sl.s() < adc_position_sl.s() - kSBuffer) {continue;}const auto& vehicle_box =common::VehicleConfigHelper::Instance()->GetBoundingBox(history_path[i]);std::vector<Vec2d> ABCDpoints = vehicle_box.GetAllCorners();for (const auto& corner_point : ABCDpoints) {// For each corner point, project it onto reference_linecommon::SLPoint curr_point_sl;if (!reference_line.XYToSL(corner_point, &curr_point_sl)) {AERROR << "Failed to get the projection from point onto ""reference_line";return false;}auto curr_point = Vec2d(curr_point_sl.s(), curr_point_sl.l());// Check if it's in any polygon of other static obstacles.for (const auto& obstacle_polygon : obstacle_polygons) {if (obstacle_polygon.IsPointIn(curr_point)) {// for debugADEBUG << "s distance to end point:" << path_end_position_sl.s();ADEBUG << "s distance to end point:" << path_position_sl.s();ADEBUG << "[" << i << "]"<< ", history_path[i].x(): " << std::setprecision(9)<< history_path[i].x() << ", history_path[i].y()"<< std::setprecision(9) << history_path[i].y();ADEBUG << "collision:" << curr_point.x() << ", " << curr_point.y();Vec2d xy_point;reference_line.SLToXY(curr_point_sl, &xy_point);ADEBUG << "collision:" << xy_point.x() << ", " << xy_point.y();return false;}}}}return true;
}

TrimHistoryPath

bool PathReuseDecider::TrimHistoryPath(Frame* frame, ReferenceLineInfo* const reference_line_info) {const ReferenceLine& reference_line = reference_line_info->reference_line();const auto& history_frame = injector_->frame_history()->Latest();if (!history_frame) {ADEBUG << "no history frame";return false;}// 找到上一帧轨迹的起始点const common::TrajectoryPoint history_planning_start_point =history_frame->PlanningStartPoint();common::PathPoint history_init_path_point =history_planning_start_point.path_point();ADEBUG << "history_init_path_point x:[" << std::setprecision(9)<< history_init_path_point.x() << "], y["<< history_init_path_point.y() << "], s: ["<< history_init_path_point.s() << "]";// 当前周期规划的起点const common::TrajectoryPoint planning_start_point =frame->PlanningStartPoint();common::PathPoint init_path_point = planning_start_point.path_point();ADEBUG << "init_path_point x:[" << std::setprecision(9) << init_path_point.x()<< "], y[" << init_path_point.y() << "], s: [" << init_path_point.s()<< "]";const DiscretizedPath& history_path =history_frame->current_frame_planned_path();DiscretizedPath trimmed_path;// 获取自车的SL坐标common::SLPoint adc_position_sl;  // current vehicle sl positionGetADCSLPoint(reference_line, &adc_position_sl);ADEBUG << "adc_position_sl.s(): " << adc_position_sl.s();size_t path_start_index = 0;for (size_t i = 0; i < history_path.size(); ++i) {// find previous init point// 找到上周期轨迹规划的起点索引if (history_path[i].s() > 0) {path_start_index = i;break;}}ADEBUG << "!!!path_start_index[" << path_start_index << "]";// get current s=0common::SLPoint init_path_position_sl;// 当前轨迹的起点reference_line.XYToSL(init_path_point, &init_path_position_sl);bool inserted_init_point = false;//匹配当前规划起点位置，裁剪该点之后的轨迹for (size_t i = path_start_index; i < history_path.size(); ++i) {common::SLPoint path_position_sl;common::math::Vec2d path_position = {history_path[i].x(),history_path[i].y()};reference_line.XYToSL(path_position, &path_position_sl);double updated_s = path_position_sl.s() - init_path_position_sl.s();// insert init pointif (updated_s > 0 && !inserted_init_point) {trimmed_path.emplace_back(init_path_point);trimmed_path.back().set_s(0);inserted_init_point = true;}trimmed_path.emplace_back(history_path[i]);// if (i < 50) {//   ADEBUG << "path_point:[" << i << "]" << updated_s;//   path_position_sl.s();//   ADEBUG << std::setprecision(9) << "path_point:[" << i << "]"//          << "x: [" << history_path[i].x() << "], y:[" <<//          history_path[i].y()//          << "]. s[" << history_path[i].s() << "]";// }trimmed_path.back().set_s(updated_s);}ADEBUG << "trimmed_path[0]: " << trimmed_path.front().s();ADEBUG << "[END] trimmed_path.size(): " << trimmed_path.size();// 检查裁剪出来的轨迹是不是过短if (!NotShortPath(trimmed_path)) {ADEBUG << "short path: " << trimmed_path.size();return false;}// set pathauto path_data = reference_line_info->mutable_path_data();ADEBUG << "previous path_data size: " << history_path.size();path_data->SetReferenceLine(&reference_line);ADEBUG << "previous path_data size: " << path_data->discretized_path().size();path_data->SetDiscretizedPath(DiscretizedPath(std::move(trimmed_path)));ADEBUG << "not short path: " << trimmed_path.size();ADEBUG << "current path size: "<< reference_line_info->path_data().discretized_path().size();return true;
}

IsIgnoredBlockingObstacle和GetBlockingObstacleS

前方堵塞的障碍物是否离开足够远的距离

bool PathReuseDecider::IsIgnoredBlockingObstacle(ReferenceLineInfo* const reference_line_info) {const ReferenceLine& reference_line = reference_line_info->reference_line();static constexpr double kSDistBuffer = 30.0;  // meterstatic constexpr int kTimeBuffer = 3;         // second// vehicle speeddouble adc_speed = injector_->vehicle_state()->linear_velocity();double final_s_buffer = std::max(kSDistBuffer, kTimeBuffer * adc_speed);// current vehicle s positioncommon::SLPoint adc_position_sl;GetADCSLPoint(reference_line, &adc_position_sl);// blocking obstacle start sdouble blocking_obstacle_start_s;if (GetBlockingObstacleS(reference_line_info, &blocking_obstacle_start_s) &&// distance to blocking obstacle(blocking_obstacle_start_s - adc_position_sl.s() > final_s_buffer)) {ADEBUG << "blocking obstacle distance: "<< blocking_obstacle_start_s - adc_position_sl.s();return true;} else {return false;}
}

bool PathReuseDecider::GetBlockingObstacleS(ReferenceLineInfo* const reference_line_info, double* blocking_obstacle_s) {auto* mutable_path_decider_status = injector_->planning_context()->mutable_planning_status()->mutable_path_decider();// get blocking obstacle ID (front_static_obstacle_id)const std::string& blocking_obstacle_ID =mutable_path_decider_status->front_static_obstacle_id();const IndexedList<std::string, Obstacle>& indexed_obstacles =reference_line_info->path_decision()->obstacles();const auto* blocking_obstacle = indexed_obstacles.Find(blocking_obstacle_ID);if (blocking_obstacle == nullptr) {return false;}const auto& obstacle_sl = blocking_obstacle->PerceptionSLBoundary();*blocking_obstacle_s = obstacle_sl.start_s();ADEBUG << "blocking obstacle distance: " << obstacle_sl.start_s();return true;
}

Else

在启用reuse之后，之后的task会有这样一段代码，用以跳过以下流程，沿用之前的path

  // skip path_lane_borrow_decider if reused pathif (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {// for debugAINFO << "skip due to reusing path";return Status::OK();}

参考

[1] Apollo Planning决策规划代码详细解析 (7): PathReuseDecider
[2] Apollo6.0 PathReuseDecider流程与代码解析