自动驾驶面经总结（二）

如何设计一个十字路口左转的算法，包括路径规划与决策两个部分

本文将以apollo为例设计一个十字路口左转的算法，包含从规划模块的输入、输出，具体的几个主要代码模块的讲述，还有细节的十字路口左转的算法。

1、十字路口左转算法在apollo框架的位置

上面的图片是apollo 的planning模块的整体框架，十字路口左转算法是众多scenario的一种。
1、planning 的输入输出

在Apollo CyberRT框架下，输出输出由Reader和Writer构成。CyberRT框架定义了两种模式，分别为消息触发和时间触发，而planning 中采用的消息触发，因此要接到特定的上游消息后，才会进入内部主逻辑，消息触发的上游消息，localview定义在common文件里.
planning接受的Reader输入包括traffic_light，routing,pad,relative_map, story-telling;接受的Process（）入参为Prediction,Localization,Chassis等。
planning的输出就比较简单，主要是给ADCTrajectory数据，包含里一条带时间。速度的轨迹点集。

apollo的主框架分为两个线程，子线程ReferenceLinePrivider 以20HZ的频率运行，用于计算planning中最重要的数据结构reference_line;主线程则是基于场景划分的思路，多数场景下是采用基于ReferenceLine的规划算法，对于泊车相关场景，则利用openspace算法。

2、planning中的场景管理

apollo中的规划里的一个重要思想就是基于场景划分来调用不同的task处理，而其中如何进行场景分配便是实现该思想的核心。下面的图就是scenario_manager的具体流程逻辑，可以看到scenario_manager会从Observe(frame)获取reference_line所遇到的第一个overlap的类型，从SenarioDispatch(frame)获取对有场景的判断（这里的场景会是依次判断的：ParkandGo,intersection,Pullover,ValetParking,Deadend等）。然后scenario_manager_Update()会开始scenario、stage、task这三个。
（具体来说，scenario代表当前所处的场景，包括正常的路上行驶场景、停车场景、红绿灯路口场景等。stage则是在当前场景下的阶段，例如在红绿灯路口场景下，stage可以分为等待绿灯、行驶中等。task则是在当前阶段下的任务，例如在等待绿灯阶段下，task可以分为等待时间到达、检测绿灯等）

这里重点介绍十字路口场景
首先，根据先前计算的地图第一个遇到的overlap来确定大类型是包含交通标识的交叉口，还是其他交叉口。其次，若是包含交通标识的交叉口，还细分为stop_sign、traffic_light以及yield_sign.

3、规划模块里对scenario,stage,task的理解

apollo在每个panning 周期首先决策当前处于哪个场景scenario下面的哪个状态stage当中，当确认好stage之后，便会调用这个stage的Process（）函数来执行具体的规划逻辑。
stage::Process();主要逻辑是根据配置文件，来依次执行每一个注册的task的Execute（）函数，从而把具体的规划任务分散到每个task当中。
stage不属于某个特定的scenario，task也不属于某个特定的stage，这些任务通过配置文件进行配置，这样便保证了整个规划模块的解耦和可扩展性

每个stage都要做速度决策，速度优化，路径决策，路径优化等task

4、intersection的具体执行

因为intersection有很多种情况，比如在trafiic_sign的scenario下就有protected,unprotected_left_turn,unprotected_right_turn三种情况；还有bare_intersection/unprotected,stop_sign/unprotcted这两种情况

以traffic_sign下protected场景为例，分析apollo的代码
在protected场景下可以看到有几个代码文件，一个是stage_approach,一个是stage_intersection_cruise,一个是traffic_light_protected_scenario;

首先我们看stage_approach

namespace apollo {
namespace planning {
namespace scenario {
namespace traffic_light {

struct TrafficLightProtectedContext;

class TrafficLightProtectedStageApproach : public Stage {
 public:
  TrafficLightProtectedStageApproach(
      const ScenarioConfig::StageConfig& config,
      const std::shared_ptr<DependencyInjector>& injector)
      : Stage(config, injector) {}

 private:
  Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point,
                             Frame* frame) override;
  TrafficLightProtectedContext* GetContext() {
    return GetContextAs<TrafficLightProtectedContext>();
  }

 private:
  Stage::StageStatus FinishStage();

 private:
  ScenarioTrafficLightProtectedConfig scenario_config_;
};

}  // namespace traffic_light
}  // namespace scenario
}  // namespace planning
}  // namespace apollo

这段代码是Apollo Auto中的一个交通灯保护场景的实现。TrafficLightProtectedStageApproach类是该场景中的一个阶段，用于处理车辆接近交通灯的情况。Process函数是TrafficLightProtectedStageApproach类的一个成员函数，用于处理车辆接近交通灯的情况。FinishStage函数是TrafficLightProtectedStageApproach类的另一个成员函数，用于完成该阶段。TrafficLightProtectedContext结构体是TrafficLightProtectedStageApproach类中使用的一个结构体。

然后看stage_intersection_cruise

namespace apollo {
namespace planning {
namespace scenario {
namespace traffic_light {

struct TrafficLightProtectedContext;

class TrafficLightProtectedStageIntersectionCruise : public Stage {
 public:
  TrafficLightProtectedStageIntersectionCruise(
      const ScenarioConfig::StageConfig& config,
      const std::shared_ptr<DependencyInjector>& injector)
      : Stage(config, injector) {}

 private:
  Stage::StageStatus Process(const common::TrajectoryPoint& planning_init_point,
                             Frame* frame) override;

  TrafficLightProtectedContext* GetContext() {
    return GetContextAs<TrafficLightProtectedContext>();
  }

  Stage::StageStatus FinishStage();

 private:
  ScenarioTrafficLightProtectedConfig scenario_config_;
  StageIntersectionCruiseImpl stage_impl_;
};

}  // namespace traffic_light
}  // namespace scenario
}  // namespace planning
}  // namespace apollo

这段代码是Apollo Auto中的一个交通灯保护场景的实现。TrafficLightProtectedStageIntersectionCruise类是该场景中的一个阶段，用于处理车辆在交通灯保护下通过路口的情况。Process函数是TrafficLightProtectedStageIntersectionCruise类的一个成员函数，用于处理车辆在交通灯保护下通过路口的情况。FinishStage函数是TrafficLightProtectedStageIntersectionCruise类的另一个成员函数，用于完成该阶段。TrafficLightProtectedContext结构体是TrafficLightProtectedStageIntersectionCruise类中使用的一个结构体。

最后看traffic_light_protected_scenario;

namespace apollo {
namespace planning {
namespace scenario {
namespace traffic_light {

// stage context
struct TrafficLightProtectedContext {
  ScenarioTrafficLightProtectedConfig scenario_config;
  std::vector<std::string> current_traffic_light_overlap_ids;
};

class TrafficLightProtectedScenario : public Scenario {
 public:
  TrafficLightProtectedScenario(
      const ScenarioConfig& config, const ScenarioContext* context,
      const std::shared_ptr<DependencyInjector>& injector)
      : Scenario(config, context, injector) {}

  void Init() override;

  std::unique_ptr<Stage> CreateStage(
      const ScenarioConfig::StageConfig& stage_config,
      const std::shared_ptr<DependencyInjector>& injector);

  TrafficLightProtectedContext* GetContext() { return &context_; }

 private:
  static void RegisterStages();
  bool GetScenarioConfig();

 private:
  static apollo::common::util::Factory<
      StageType, Stage,
      Stage* (*)(const ScenarioConfig::StageConfig& stage_config,
                 const std::shared_ptr<DependencyInjector>& injector)>
      s_stage_factory_;
  bool init_ = false;
  TrafficLightProtectedContext context_;
};

}  // namespace traffic_light
}  // namespace scenario
}  // namespace planning
}  // namespace apollo

这段代码是Apollo Auto中的一个交通灯保护场景的实现。TrafficLightProtectedScenario类是该场景的一个实现，用于处理车辆在交通灯保护下通过路口的情况。TrafficLightProtectedContext结构体是TrafficLightProtectedScenario类中使用的一个结构体，用于存储场景中的一些信息。CreateStage函数是TrafficLightProtectedScenario类的一个成员函数，用于创建场景中的阶段。Init函数是TrafficLightProtectedScenario类的另一个成员函数，用于初始化场景。
这段代码是TrafficLightProtectedScenario类的实现，它包括了初始化函数Init()和RegisterStages()函数。Init()函数初始化了场景，并且获取了当前交通灯的信息。RegisterStages()函数注册了两个阶段：TRAFFIC_LIGHT_PROTECTED_APPROACH和TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE。

这个代码片段中，traffic_light_status是一个planning_status结构体中的traffic_light成员变量，它包含了当前交通灯的信息。如果当前交通灯的id为空，那么就会输出错误信息并返回。如果当前交通灯的id不为空，那么就会将当前交通灯的id存储到context_.current_traffic_light_overlap_ids中。
这个代码片段中还使用了HDMapUtil::BaseMap().GetSignalById()函数来获取交通灯的信息。HDMapUtil是一个工具类，它提供了一些与HDMap相关的函数。GetSignalById()函数可以根据交通灯的id获取交通灯的信息。

5以一个stage为例分析stge如何注册task，以及一个场景如何工作

task的注册发生在的注册发生在stage对象的简历阶段，stage的构造函数继承了stage类的构造函数，在建立对象时读取配置文件，并对当前stage包含的task进行注册；
stage的构造函数对stage的构造函数进行继承；
stage的构造函数读取配置文件，并且注册每个task，并保存在全局变量task_list_中；
Process()函数执行主要的规划逻辑，另外一个重要的函数是PlanOnReferenceLine(),在这个函数中实现了对每个task的依次调用
下面的图是task的被调用的逻辑

6、交规决策的运行逻辑