Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择

本文重点讲解Apollo代码中怎样配置Scenario以及选择当前Scenario，Scenario场景决策是Apollo规划算法的第一步，本文会对代码进行详细解析，也会梳理整个决策流程，码字不易，喜欢的朋友们麻烦点个关注与赞。 

Apollo Planning决策规划系列文章：

Apollo Planning决策规划代码详细解析 (2)：Scenario执行

Apollo Planning决策规划代码详细解析 (3)：stage执行

Apollo Planning决策规划代码详细解析 (4)：Stage逻辑详解

Apollo Planning决策规划代码详细解析 (5)：规划算法流程介绍

Apollo Planning决策规划代码详细解析 (6): LaneChangeDecider

Apollo Planning决策规划代码详细解析 (7): PathReuseDecider

Apollo Planning决策规划代码详细解析 (8): PathLaneBorrowDecide

Apollo Planning决策规划代码详细解析 (9): PathBoundsDecider

Apollo Planning决策规划代码详细解析 (10)：PiecewiseJerkPathOptimizer

Apollo Planning决策规划代码详细解析 (11): PathAssessmentDecider

Apollo Planning决策规划代码详细解析 (12): PathDecider

Apollo Planning决策规划代码详细解析 (13): RuleBasedStopDecider

Apollo Planning决策规划算法代码详细解析 (14):SPEED_BOUNDS_PRIORI_DECIDER

Apollo Planning决策规划算法代码解析(15): 速度动态规划SPEED_HEURISTIC_OPTIMIZER 上

Apollo Planning决策规划算法代码解析 (16):SPEED_HEURISTIC_OPTIMIZER 速度动态规划中

Apollo Planning决策规划算法代码解析 (17):SPEED_HEURISTIC_OPTIMIZER 速度动态规划下

算法介绍文章： 

Apollo决策规划算法Planning : Piecewise Jerk Path Optimizer的python实现

仿真技术介绍文章： 

prescan联合simulink进行FCW的仿真_自动驾驶 Player的博客-CSDN博客

 如果对apollo规划算法感兴趣，想学习完整的系列文章，可以订阅下面专栏：https://blog.csdn.net/nn243823163/category_11685852.htmlhttps://blog.csdn.net/nn243823163/category_11685852.html

正文如下： 

本文重点讲解Apollo代码中怎样配置Scenario以及选择当前Scenario，Scenario决策是Apollo规划算法的第一步，本文会对代码进行详细解析，也会梳理整个决策流程，码字不易，喜欢的朋友们麻烦点个关注与赞。  

在本文你将学到下面这些内容：

1. 规划器planer的种类；
2. 规划器planer的主要函数及逻辑；
3. 场景管理类ScenarioManager的运行机制；
4. 场景注册方法；
5. 场景决策流程，如何选择当前场景
6. 详细的apollo决策规划代码分析

代码具体过程如下：

0、规划算法的入口

（1）规划模块的入口函数是PlanningComponent的Proc。

（2）以规划模式OnLanePlanning，执行RunOnce。在RunOnce中先执行交通规则，再规划轨迹。规划轨迹的函数是Plan。

1、Scenario的判断在Planer中进行，目前Apollo共有下面这些planer，其中最常用的就是EM规划器，即PublicRoadPlanner，本系列主要介绍PublicRoadPlanner这个Planer。

​

2、Apollo会根据配置调用PublicRoadPlanner这个planer，关于配置方法，之后会在另外一篇博文进行更新。PublicRoadPlanner主要有init()与plan()两个重要的函数，inti()是规划器的初始化，plan就是具体的规划过程。

class PublicRoadPlanner : public PlannerWithReferenceLine {
 public:
  /**
   * @brief Constructor
   */
  PublicRoadPlanner() = delete;

  explicit PublicRoadPlanner(
      const std::shared_ptr& 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() 函数的update阶段，主要用的是ScenarioManager类的updata函数。ScenarioManager并不属于某个特定的planer，这个类只针对于scenario，每个planer都可以调用它来管理场景。下面代码片段是PublicRoadPlanner的Plan()函数。

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame,
                               ADCTrajectory* ptr_computed_trajectory) {
  // 决策当前应该执行哪个场景
  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_DONE
    scenario_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初始化时会调用配置文件里的参数来建立这个对象。

Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}

调用ScenarioManager类的init()函数，并且根据当前planer的配置来注册场景。

bool ScenarioManager::Init(
    const std::set& supported_scenarios) {
  // 注册场景
  RegisterScenarios();
  default_scenario_type_ = ScenarioConfig::LANE_FOLLOW;
  supported_scenarios_ = supported_scenarios;
  // 创建场景，默认为lane_follow
  current_scenario_ = CreateScenario(default_scenario_type_);
  return true;
}

目前PublicRoadPlanner支持下面这些场景

// 还是在"/conf/planning_config.pb.txt"中
standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_type: OPEN_SPACE
  planner_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()函数，用来决策当前处在什么场景。如果进入了新的场景，会创建一个新的对象来进行之后的规划逻辑。

void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                             const Frame& frame) {
  ACHECK(!frame.reference_line_info().empty());

  Observe(frame);

  ScenarioDispatch(frame);
}

场景决策逻辑在ScenarioDispatch(frame)当中，会根据配置选择基于规则还是基于学习的决策方法。

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 PlanningContext
  UpdatePlanningContext(frame, scenario_type);

  if (current_scenario_->scenario_type() != scenario_type) {
    current_scenario_ = CreateScenario(scenario_type);
  }
}

6、ScenarioDispatchNonLearning()函数默认从lanefollow场景开始判断，首先根据驾驶员的意图来安排场景，如果不是默认的lanefollow场景，直接输出当前场景；如果是lanefollow场景，会依次判断是否属于别的场景；即剩余场景之间的跳转必须经过lanefollow这个场景。

ScenarioConfig::ScenarioType ScenarioManager::ScenarioDispatchNonLearning(
    const Frame& frame) {
  
  // default: LANE_FOLLOW
  ScenarioConfig::ScenarioType scenario_type = default_scenario_type_;
  
  // Pad Msg scenario
  scenario_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 logic
        if (JudgeReachTargetPoint(car_position, target_point)) {
          scenario_type = ScenarioConfig::LANE_FOLLOW;
          reach_target_pose_ = true;
        }
      case ScenarioConfig::YIELD_SIGN:
        // must continue until finish
        if (current_scenario_->GetStatus() !=
            Scenario::ScenarioStatus::STATUS_DONE) {
          scenario_type = current_scenario_->scenario_type();
        }
        break;
      default:
        break;
    }
  }
  
  // ParkAndGo / starting scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_park_and_go && !reach_target_pose_) {
      scenario_type = SelectParkAndGoScenario(frame);
    }
  }

  
  // intersection scenarios
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectInterceptionScenario(frame);
  }

  
  // pull-over scenario
  if (scenario_type == default_scenario_type_) {
    if (FLAGS_enable_scenario_pull_over) {
      scenario_type = SelectPullOverScenario(frame);
    }
  }

  
  // VALET_PARKING scenario
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectValetParkingScenario(frame);
  }
  
  // dead end
  if (scenario_type == default_scenario_type_) {
    scenario_type = SelectDeadEndScenario(frame);
  }
  
  return scenario_type;
}

7、在场景判断时，首先调用函数SelectPadMsgScenario()，根据驾驶员意图来安排场景.

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场景决策逻辑。后续文章会讲场景选择之后，怎么进行下一步的规划算法。