本文主要记录一下自己最近学习开源项目OpenCDA代码的笔记。简而言之，该项目用Python实现了全栈式自动驾驶算法的开发，也实现了CARLA-SUMO的联合仿真（该部分参考了CARLA官方给出的联合仿真实例代码）。学习该项目代码的主要目的是为了能够与现有的MARL开源算法库pymarl2结合，然后测试自己开发的算法。本文第一部分先介绍一下OpenCDA的主要文件结构，项目中类的定义及相关关系，以及总结官方文档中进行自定义算法植入customization的思路。第二部分主要对其中的DEMO: platoon_joining2lanefree_cosim进行具体的代码分析。本文所涉及的CARLA和SUMO traci的接口函数都可以在官网中查到。
以思维导图（树状图）的方式记录看代码的过程真是非常高效，也是受这篇博客（pymarl代码讲解）的启发。本文中完整的思维导图文件欢迎从我的github repo自取。
OpenCDA的主要文件结构就是根目录下的opencda，其中assets包含了各类测试用例的地图文件或对应SUMO中的.net.xml和.rou.xml文件等；co_simulation文件夹中的sumo_integration对应CARLA 0.9.11中的Co-simulation部分中同名的文件夹；core文件夹非常重要，包含了从感知到决策规划的所有模块，此外还实现了地图管理等功能；customize文件夹中包含了一些自定义的算法，例如yolov5模型的导入就在customize/ml_libs中；最后，scenario_testing文件夹包含了OpenCDA所有的测试用demo源文件（.yaml和.py文件），包含了ScenarioManager的scenario_testing/utils/sim_api.py和联合仿真中的CoScenarioManager的scenario_testing/utils/cosim_api.py。
OpenCDA
├── docs # documents of opencda, no need to pay attention.
├── opencda
│ ├── assests # customized map and sumo xml package.
│ ├── co_simulation # source codes for sumo background traffic generation.
│ ├── core # the core part of the code
│ │ └── actutation # implementation of control algorithms
│ │ ├── application # implementation of cooperative driving applications
│ │ ├── common # base class and communication class for cavs
│ │ ├── map # HDMap manager
│ │ ├── plan # planning algorithms
│ │ └── sensing # perception and localization algorithms.
│ ├── customize # where the users put their customized algorithm to replace the default modules
│ ├── scenario_testing # where all scenario testing scripts and yaml files exist
│ │ └── config_yaml # the yaml file defining the testing scenarios
│ │ ├── evluations # evluation scripts for different modules' performance
│ │ ├── utils # utility functions to construct scenarios based on given yaml files.
├── scripts # installation scripts
├── tests # unit tests
OpenCDA中的类结构见下图所示，其中最顶层的类是ScenarioManager和从之继承的CoScenarioManager（用于某个具体测试样例场景的管理）；前者用于只用CARLA的仿真任务，后者用于CARLA-SUMO的联合仿真任务，类的定义代码路径见上文所述。定义了ScenarioManager或CoScenarioManager之后，针对某个具体任务（比如联合仿真中的DEMO是platoon merging），调用该类下的方法create_platoon_manager和create_vehicle_manager分别创建PlatooningManager和VehicleManager，前者是根据车队中包含的车辆数为其中每一辆车都构造一个VehicleManager，而VehicleManager类中又包含了感知，定位等相关任务的管理器类，详细的可以参考OpenCDA的官方文档。本文的第二部分将借助联合仿真DEMO中的代码对这些类中具体的属性和方法进行记录。
另外，OpenCDA也提供了用户植入自定义算法的接口，把自定义的算法模块加入到路径opencda/customize/...并利用inheritance重写默认算法即可，主要就是两个方法，一个是update_information，一个是run_step。之后，将自定义的模块导入到VehicleManager类中。
这一部分记录联合仿真DEMO中具体的代码实现，对应scenario_testing/中的platoon_joining2lanefree_cosim.py文件和同名的yaml文件。这一部分的记录形式主要是思维导图配合解释性的文字。首先，先了解一下该DEMO的具体任务：在SUMO控制的交通流下，辅路行驶的CAV（Connected and Autonomous Vehicle）将汇入行驶在主路，由四辆CAVs组成的车队（其中CAVs由 CARLA控制），最终形成新的车队驶往目的地。这一任务又可以分为两个子任务，第一个是单CAV汇入主路，第二是加入原先的车队。此外，在仿真中，CAVs被标记为蓝色，对应CARLA中的lincoln车型（blueprint为vehicle.lincoln.mkz2017），SUMO交通流中的车辆被标记为白色，在CARLA中类型随机。该DEMO的输出包括两部分：1）运动学相关（速度，加速度，TTC）；2）定位相关（x,y坐标，yaw angle, 对应的误差）。
接下来是代码整体框架及对应的函数。根目录下的opencda.py中的main()包括了以下步骤：1）找到运行时命令行中的场景参数所对应的py文件和配置yaml文件；2）运行测试文件中对应的run_scenario()函数。下图首先给出run_scenario()中所需要的参数，一类参数对应选择哪一类场景opt，另一类参数对应选择某个场景后，该场景本身的参数config_yaml。run_scenario()函数又包含了三个主要步骤：1）初始化类和相关变量；2）开启循环，其中包括CoScenarioManager.tick()函数，该函数的作用类似于强化学习中的env.step()，设置CARLA中的spectator，最后分别对platoon和single CAV运行一步；3）构造evaluation manager用以记录实验结果，并最终销毁CARLA中的actor结束程序。
首先记录run_scenario()中的初始化部分的框架，第一步，将上文中提到的yaml文件转化为dictionary类型，之后构建CavWorld类的一个实例，目的是为了存储所用车辆的信息和在联合仿真中用到的sumo-carla ID关系的dictionary。接下来找到测试用例所使用的SUMO配置文件（.sumocfg）和对应的OpenDrive文件（.xodr），并将其输入给CoScenarioManager类的实例构造。构建该实例的过程不仅包含了CoScenarioManager的初始化，还包含了其父类ScenarioManager的初始化。实例化之后，调用ScenarioManager中的create_platoon_manager和create_vehicle_manager方法分别构造车队成员的VehicleManager和单CAV的VehicleManager。初始化的最后构建EvaluationManager的实例，并设置好spectator和spectator_vehicle用于监测雷达点云和相机的输出。
CoScenarioManager类的实例化介绍完初始化部分的整体框架后，对其中CoScenarioManager类的实例化进行记录。首先对其父类ScenarioManager进行实例化，其主要负责在CARLA中生成车辆。具体而言，在初始化相关变量和设置随机种子之后构造carla.Client()。如果使用的是自定义的xodr地图（该DEMO使用的是自定义OpenDrive地图），则用CARLA中的APIcarla.Client.generate_opendrive_world()导入地图，该函数对应的函数是opencda/scenario_testing/utils/customized_map_api中的load_customized_world。对于不使用自定义OpenDrive地图的情况，直接使用carla.Client.load_world('town_name')即可。接下来对CARLA服务器进行一些设定，其中包括设置同步模式和天气，分别对应设置carla.World.get_settings().sychronous_mode，将天气参数传给carla.World.set_weather()。
CoScenarioManager类的初始化主要对SUMO仿真器和相关参数进行配置（见下图），其包括以下几个步骤：1）相关dictionaries的初始化；2）配置SUMO启动相关的参数（例如port, host, gui等），并对SumoSimulation类进行初始化，构建实例（该类对应的文件路径是opencda/co_simulation/sumo_integration/sumo_simulation.py）；3）SUMO-CARLA的ID mapping；4）设置BridgeHelper类中的相关属性（blueprint library和地图网络相关的offset），该类对应的文件路径是：opencda/co_simulation/sumo_integration/bridge_helper.py。
VehicleManager
CoScenarioManager这一主要类初始化完成之后，则要为每个车队成员和单CAV构建VehicleManager。对每个车队成员VehicleManager的构建包括了对blueprint，spawn point，color，任务目标地点等变量的定义，详细过程见下图。
单CAVVehicleManager的构造流程与上文车队成员类似，但多出来了一步是要利用PlatooningPlugin.set_platoon()对其当前的车队状态进行标记。
本节主要对仿真循环部分的CoScenarioManager.tick()和run step部分进行记录（整体框架见下图）。前者主要包含两部分，其一是将由SUMO车流控制生成的车辆同步到CARLA中，其二是将CARLA中CAVs对应生成到SUMO中，形成一个闭环。
后者则主要对应于PlatooningManager.update_information()和PlatooningManager.run_step()两个方法。这两个方法又包含了VehicleManager.update_info()和VehicleManager.run_step()，这两个方法与单CAV的方法等价。
CoScenarioManager.tick()
CoScenarioManager.tick()的作用就相当于RL环境中常见的step函数功能。先来看下SUMO同步到CARLA的具体步骤，如下图所示：
第一步先调用SumoSimulation.tick()方法，该方法用traci.simulationStep()接口将SUMO，和其中的信号灯运行一步。之后利用traci.simulation.getDepartedIDList()，traci.simulation.getArrivedIDList()获取当前交通流下的车辆ID和已经到达目的地的车辆ID。第二步，将SUMO中的车辆在CARLA中生成，具体步骤包括：1）在SUMO中订阅对应车辆ID的具体信息（订阅需要利用SUMO中预先定义好的整数值，见SUMO官方doc即可）；2）使用traci.vehicle.getSubscriptionResults(id)获取订阅的相关信息；3）获取该车辆在CARLA中的对应blueprint（随机），利用BridgeHelper.get_carla_transform()方法获取CARLA中对应的transform，并调用CoScenarioManager.spawn_actor()方法对该车辆进行生成（主要使用carla.command.SpawnActor，carla.command.SetSimulatPhysics指令）；4）最后，将CARLA中生成的车辆ID复制到CoScenarioManager.sumo2carla_ids 字典中。
接下来介绍CARLA同步到SUMO的具体步骤，流程见下图：
主要步骤有：1）调用carla.World.tick()；2）更新相关的数据结构；3）将CARLA中生成的CAVs同步生成到SUMO中，其中包括用carla.World.get_actor()获取CARLA中的车辆信息，获取车辆类型信息，用SumoSimulation.spawn_actor()将该车辆在SUMO中生成，并用SumoSimulation.subscribe()订阅其相关信息。4）销毁掉已经到达目的地的CARLA CAVs；5）更新相关数据结构，字典CoScenarioManager.carla2sumo_ids；6）基于CARLA中的信息更新SUMO中交通流的信息。
run step的主要步骤就是两个循环，第一个循环构造PlatooningManager，其中最主要的两个函数是update_information()和run_step()，在这两个函数的内部会分别调用VehicleManager的update_info()和run_step()方法。第二个则是为单CAV构造VehicleManager，最主要的两个函数是update_info()和run_step()，这两个函数与PlatooningManager中的流程是完全等价的。
PlatooningManager
下图是第一个循环的主要框架，每一个车队一开始由4辆车组成，每一辆车对应的车辆管理器VehicleManager要对核心方法进行调用update_info()和run_step()。接下来分别对这两个方法的主要逻辑进行记录。
首先是车辆管理器中的update_info方法，其主要目的就是调用感知和定位模块以提取ego vehicle及周围车辆的信息。其可以分为以下几个步骤：1）定位，获取ego的速度（ego_spd）和位姿（ego_pos），如果定位模块没被激活，则直接从CARLA服务器获取速度和位姿，否则利用GNSS和卡尔曼滤波进行定位。2）目标检测：DEMO中没有启用YOLO5和LIDAR进行融合目标检测，而是直接利用CARLA服务器中的相关信息，DEMO中只是用了前向相机；3）更新MapManager中的位姿信息，调用MapManager.update_information()方法；4）更新V2XManager中ego的速度和位姿信息，调用V2XManager.update_info(ego_pos, ego_spd)，主要目的就是获取ego车辆周围的车辆，并更新PlatooningPlugin中的速度与位姿信息；5）更新PlatooningBehaviorAgent，该类继承于BehaviorAgent，其中的详细逻辑见下文；6）更新ControlManager（PID控制器，文件路径为：opencda/core/actuation/pid_controller.py），将速度和位姿传入控制器，并判断是否采用动态调整PID参数的机制（DEMO中没有采用动态调整PID参数的机制）。
关于PlatooningBehaviorAgent.update_information()函数的具体细节见下图：
第二个核心函数是VehicleManager.run_step()方法，基本框架如下图：
其包含1）MapManager.run_step()，如果激活的话使用rasterization，此DEMO并没有激活该功能。2）最核心的部分：PlatooningBehaviorAgent.run_step()。其输入是目标速度，输出是目标速度和目标位姿（或者称之为waypoint），其主要逻辑就是处理以下几种cases：
3）调用ControlManager.run_step()，输入是目标速度和waypoint，输出是控制指令，其类型为carla.VehicleControl。其Controller.run_step(target_speed, waypoint) 逻辑是首先处理紧急停车的情况，之后进行水平和纵向控制，最终进行加速度和转角的clipping操作，将其限定在预先设定的范围内。
单CAV所调用的核心方法在platoon情况下都有涉及，所以这里只记录一下单CAV的大致框架。其框架与上文类似，不一样的地方在于需要每一步对单CAV是否加入车队进行检查。如果车辆还没有加入车队，则依次调用VehicleManager.update_info()和VehicleManager.run_step()。
