Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择
本文重點講解Apollo代碼中怎樣配置Scenario以及選擇當前Scenario，Scenario場景決策是Apollo規(guī)劃算法的第一步，本文會對代碼進行詳細解析，也會梳理整個決策流程，碼字不易，喜歡的朋友們麻煩點個關注與贊。?
Apollo Planning決策規(guī)劃系列文章：
Apollo Planning決策規(guī)劃代碼詳細解析 (2)：Scenario執(zhí)行
Apollo Planning決策規(guī)劃代碼詳細解析 (3)：stage執(zhí)行
Apollo Planning決策規(guī)劃代碼詳細解析 (4)：Stage邏輯詳解
Apollo Planning決策規(guī)劃代碼詳細解析 (5)：規(guī)劃算法流程介紹
Apollo Planning決策規(guī)劃代碼詳細解析 (6): LaneChangeDecider
Apollo Planning決策規(guī)劃代碼詳細解析 (7): PathReuseDecider
Apollo Planning決策規(guī)劃代碼詳細解析 (8): PathLaneBorrowDecide
Apollo Planning決策規(guī)劃代碼詳細解析 (9): PathBoundsDecider
Apollo Planning決策規(guī)劃代碼詳細解析 (10)：PiecewiseJerkPathOptimizer
Apollo Planning決策規(guī)劃代碼詳細解析 (11): PathAssessmentDecider
Apollo Planning決策規(guī)劃代碼詳細解析 (12): PathDecider
Apollo Planning決策規(guī)劃代碼詳細解析 (13): RuleBasedStopDecider
Apollo Planning決策規(guī)劃算法代碼詳細解析 (14):SPEED_BOUNDS_PRIORI_DECIDER
Apollo Planning決策規(guī)劃算法代碼解析(15): 速度動態(tài)規(guī)劃SPEED_HEURISTIC_OPTIMIZER 上
Apollo Planning決策規(guī)劃算法代碼解析 (16):SPEED_HEURISTIC_OPTIMIZER 速度動態(tài)規(guī)劃中
Apollo Planning決策規(guī)劃算法代碼解析 (17):SPEED_HEURISTIC_OPTIMIZER 速度動態(tài)規(guī)劃下
算法介紹文章：?
Apollo決策規(guī)劃算法Planning : Piecewise Jerk Path Optimizer的python實現(xiàn)
仿真技術介紹文章：?
prescan聯(lián)合simulink進行FCW的仿真_自動駕駛 Player的博客-CSDN博客
?如果對apollo規(guī)劃算法感興趣，想學習完整的系列文章，可以訂閱下面專欄：https://blog.csdn.net/nn243823163/category_11685852.htmlhttps://blog.csdn.net/nn243823163/category_11685852.html
正文如下：?
本文重點講解Apollo代碼中怎樣配置Scenario以及選擇當前Scenario，Scenario決策是Apollo規(guī)劃算法的第一步，本文會對代碼進行詳細解析，也會梳理整個決策流程，碼字不易，喜歡的朋友們麻煩點個關注與贊。??
在本文你將學到下面這些內容：
代碼具體過程如下：
0、規(guī)劃算法的入口
（1）規(guī)劃模塊的入口函數(shù)是PlanningComponent的Proc。
（2）以規(guī)劃模式OnLanePlanning，執(zhí)行RunOnce。在RunOnce中先執(zhí)行交通規(guī)則，再規(guī)劃軌跡。規(guī)劃軌跡的函數(shù)是Plan。
1、Scenario的判斷在Planer中進行，目前Apollo共有下面這些planer，其中最常用的就是EM規(guī)劃器，即PublicRoadPlanner，本系列主要介紹PublicRoadPlanner這個Planer。
?
2、Apollo會根據(jù)配置調用PublicRoadPlanner這個planer，關于配置方法，之后會在另外一篇博文進行更新。PublicRoadPlanner主要有init()與plan()兩個重要的函數(shù)，inti()是規(guī)劃器的初始化，plan就是具體的規(guī)劃過程。
class PublicRoadPlanner : public PlannerWithReferenceLine {public:/*** @brief Constructor*/PublicRoadPlanner() = delete;explicit PublicRoadPlanner(const std::shared_ptr<DependencyInjector>& injector): PlannerWithReferenceLine(injector) {}/*** @brief Destructor*/virtual ~PublicRoadPlanner() = default;void Stop() override {}std::string Name() override { return "PUBLIC_ROAD"; }common::Status Init(const PlanningConfig& config) override;/*** @brief Override function Plan in parent class Planner.* @param planning_init_point The trajectory point where planning starts.* @param frame Current planning frame.* @return OK if planning succeeds; error otherwise.*/common::Status Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) override; };3、scenario的選擇在Plan() 函數(shù)的update階段，主要用的是ScenarioManager類的updata函數(shù)。ScenarioManager并不屬于某個特定的planer，這個類只針對于scenario，每個planer都可以調用它來管理場景。下面代碼片段是PublicRoadPlanner的Plan()函數(shù)。
Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {// 決策當前應該執(zhí)行哪個場景scenario_manager_.Update(planning_start_point, *frame);// 獲取當前場景scenario_ = scenario_manager_.mutable_scenario();// 處理當前場景auto result = scenario_->Process(planning_start_point, frame);// 打印debug信息if (FLAGS_enable_record_debug) {auto scenario_debug = ptr_computed_trajectory->mutable_debug()->mutable_planning_data()->mutable_scenario();scenario_debug->set_scenario_type(scenario_->scenario_type());scenario_debug->set_stage_type(scenario_->GetStage());scenario_debug->set_msg(scenario_->GetMsg());}// 場景處理成功if (result == scenario::Scenario::STATUS_DONE) {// only updates scenario manager when previous scenario's status is// STATUS_DONEscenario_manager_.Update(planning_start_point, *frame);} // 場景處理失敗else if (result == scenario::Scenario::STATUS_UNKNOWN) {return Status(common::PLANNING_ERROR, "scenario returned unknown");}return Status::OK(); }4、ScenarioManager會實例化一個全局的scenario_manager_對象來進行場景管理，在PublicRoadPlanner初始化時會調用配置文件里的參數(shù)來建立這個對象。
Status PublicRoadPlanner::Init(const PlanningConfig& config) {config_ = config;scenario_manager_.Init(config);return Status::OK(); }調用ScenarioManager類的init()函數(shù)，并且根據(jù)當前planer的配置來注冊場景。
bool ScenarioManager::Init(const std::set<ScenarioConfig::ScenarioType>& supported_scenarios) {// 注冊場景RegisterScenarios();default_scenario_type_ = ScenarioConfig::LANE_FOLLOW;supported_scenarios_ = supported_scenarios;// 創(chuàng)建場景，默認為lane_followcurrent_scenario_ = CreateScenario(default_scenario_type_);return true; }目前PublicRoadPlanner支持下面這些場景
// 還是在"/conf/planning_config.pb.txt"中 standard_planning_config {planner_type: PUBLIC_ROADplanner_type: OPEN_SPACEplanner_public_road_config {// 支持的場景scenario_type: LANE_FOLLOW // 車道線保持scenario_type: SIDE_PASS // 超車scenario_type: STOP_SIGN_UNPROTECTED // 停止scenario_type: TRAFFIC_LIGHT_PROTECTED // 紅綠燈scenario_type: TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN // 紅綠燈左轉scenario_type: TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN // 紅綠燈右轉scenario_type: VALET_PARKING // 代客泊車}5、ScenarioManager類的Update()函數(shù)，用來決策當前處在什么場景。如果進入了新的場景，會創(chuàng)建一個新的對象來進行之后的規(guī)劃邏輯。
void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,const Frame& frame) {ACHECK(!frame.reference_line_info().empty());Observe(frame);ScenarioDispatch(frame); }場景決策邏輯在ScenarioDispatch(frame)當中，會根據(jù)配置選擇基于規(guī)則還是基于學習的決策方法。
void ScenarioManager::ScenarioDispatch(const Frame& frame) {ACHECK(!frame.reference_line_info().empty());ScenarioConfig::ScenarioType scenario_type;int history_points_len = 0;if (injector_->learning_based_data() &&injector_->learning_based_data()->GetLatestLearningDataFrame()) {history_points_len = injector_->learning_based_data()->GetLatestLearningDataFrame()->adc_trajectory_point_size();}if ((planning_config_.learning_mode() == PlanningConfig::E2E ||planning_config_.learning_mode() == PlanningConfig::E2E_TEST) &&history_points_len >= FLAGS_min_past_history_points_len) {scenario_type = ScenarioDispatchLearning();} else {scenario_type = ScenarioDispatchNonLearning(frame);}ADEBUG << "select scenario: "<< ScenarioConfig::ScenarioType_Name(scenario_type);// update PlanningContextUpdatePlanningContext(frame, scenario_type);if (current_scenario_->scenario_type() != scenario_type) {current_scenario_ = CreateScenario(scenario_type);} }6、ScenarioDispatchNonLearning()函數(shù)默認從lanefollow場景開始判斷，首先根據(jù)駕駛員的意圖來安排場景，如果不是默認的lanefollow場景，直接輸出當前場景；如果是lanefollow場景，會依次判斷是否屬于別的場景；即剩余場景之間的跳轉必須經(jīng)過lanefollow這個場景。
ScenarioConfig::ScenarioType ScenarioManager::ScenarioDispatchNonLearning(const Frame& frame) {// default: LANE_FOLLOWScenarioConfig::ScenarioType scenario_type = default_scenario_type_;// Pad Msg scenarioscenario_type = SelectPadMsgScenario(frame);const auto vehicle_state_provider = injector_->vehicle_state();common::VehicleState vehicle_state = vehicle_state_provider->vehicle_state();const common::PointENU& target_point =frame.local_view().routing->routing_request().dead_end_info().target_point();const common::VehicleState& car_position = frame.vehicle_state();if (scenario_type == default_scenario_type_) {// check current_scenario (not switchable)switch (current_scenario_->scenario_type()) {case ScenarioConfig::LANE_FOLLOW:case ScenarioConfig::PULL_OVER:break;case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED:case ScenarioConfig::EMERGENCY_PULL_OVER:case ScenarioConfig::PARK_AND_GO:case ScenarioConfig::STOP_SIGN_PROTECTED:case ScenarioConfig::STOP_SIGN_UNPROTECTED:case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED:case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:case ScenarioConfig::VALET_PARKING:case ScenarioConfig::DEADEND_TURNAROUND:// transfer dead_end to lane follow, should enhance transfer logicif (JudgeReachTargetPoint(car_position, target_point)) {scenario_type = ScenarioConfig::LANE_FOLLOW;reach_target_pose_ = true;}case ScenarioConfig::YIELD_SIGN:// must continue until finishif (current_scenario_->GetStatus() !=Scenario::ScenarioStatus::STATUS_DONE) {scenario_type = current_scenario_->scenario_type();}break;default:break;}}// ParkAndGo / starting scenarioif (scenario_type == default_scenario_type_) {if (FLAGS_enable_scenario_park_and_go && !reach_target_pose_) {scenario_type = SelectParkAndGoScenario(frame);}}// intersection scenariosif (scenario_type == default_scenario_type_) {scenario_type = SelectInterceptionScenario(frame);}// pull-over scenarioif (scenario_type == default_scenario_type_) {if (FLAGS_enable_scenario_pull_over) {scenario_type = SelectPullOverScenario(frame);}}// VALET_PARKING scenarioif (scenario_type == default_scenario_type_) {scenario_type = SelectValetParkingScenario(frame);}// dead endif (scenario_type == default_scenario_type_) {scenario_type = SelectDeadEndScenario(frame);}return scenario_type; }7、在場景判斷時，首先調用函數(shù)SelectPadMsgScenario()，根據(jù)駕駛員意圖來安排場景.
ScenarioConfig::ScenarioType ScenarioManager::SelectPadMsgScenario(const Frame& frame) {const auto& pad_msg_driving_action = frame.GetPadMsgDrivingAction();switch (pad_msg_driving_action) {case DrivingAction::PULL_OVER:if (FLAGS_enable_scenario_emergency_pull_over) {return ScenarioConfig::EMERGENCY_PULL_OVER;}break;case DrivingAction::STOP:if (FLAGS_enable_scenario_emergency_stop) {return ScenarioConfig::EMERGENCY_STOP;}break;case DrivingAction::RESUME_CRUISE:if (current_scenario_->scenario_type() ==ScenarioConfig::EMERGENCY_PULL_OVER ||current_scenario_->scenario_type() ==ScenarioConfig::EMERGENCY_STOP) {return ScenarioConfig::PARK_AND_GO;}break;default:break;}return default_scenario_type_; }8、可以看到，除了駕駛員行為相關的兩個場景外，每次切換場景必須是從默認場景(LANE_FOLLOW)開始，即每次場景切換之后都會回到默認場景。
?
9、以上即為apollo場景決策邏輯。后續(xù)文章會講場景選擇之后，怎么進行下一步的規(guī)劃算法。
總結
以上是生活随笔為你收集整理的Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择的全部內容，希望文章能夠幫你解決所遇到的問題。
