宁波哪家建网站好,苏州网站建设的一般流程,网站标题在线制作,抖音代运营谈判技巧文章目录 前言PATH_REUSE_DECIDER功能简介PATH_REUSE_DECIDER相关配置PATH_REUSE_DECIDER总体流程PATH_REUSE_DECIDER相关子函数IsCollisionFreeTrimHistoryPathIsIgnoredBlockingObstacle和GetBlockingObstacleS Else参考 前言
在Apollo星火计划学习笔记——Apollo路径规划算… 文章目录 前言PATH_REUSE_DECIDER功能简介PATH_REUSE_DECIDER相关配置PATH_REUSE_DECIDER总体流程PATH_REUSE_DECIDER相关子函数IsCollisionFreeTrimHistoryPathIsIgnoredBlockingObstacle和GetBlockingObstacleS Else参考 前言
在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::vectorPolygon2d 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::vectorVec2d 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 its 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 s0common::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 IndexedListstd::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流程与代码解析
