【仿真】Carla之Traffic Manager [3]

引用与前言

1. 官方网页【部分文字来源于此翻译】：https://carla.readthedocs.io/en/0.9.11/
2. 知乎较为完全的介绍【部分文字来源于此】：https://www.zhihu.com/column/c_1324712096148516864

首先第一篇初入Carla请见：https://blog.csdn.net/qq_39537898/article/details/115921745
第二篇深入Carla时间设置见：https://blog.csdn.net/qq_39537898/article/details/117190371

Notion外链及后续小错误更新等见：https://www.notion.so/kinzhang/3-Carla-Traffic-Manager-9c2825b20f714e0e8cd7fac1f6d489c3

介绍

定义 What’s it?

交通管理器（简称TM），其实感觉叫车辆管理器比较好，就是控制仿真世界里所有的车辆的。根据官方的介绍是：写在CARLA C++底层API里的。主要的功能目标呢就是为了构建像真实世界里的交通情况【我寻思着这也没法说真实就真实呀，比如超车 别车 等 再看看】用户可以设置自己的想要的行为方式，例如set specific learning circumstances [这里的学习环境是指场景设置吧 吧 吧]
每一个车辆管理器可以将其他车辆设置为自动模式，并添加进入自己的管理列表中

结构设计 Structure design

TM呢 是写在client这边的，也就是说如果你要设置在某个python的client中调用就行，这样直接能覆盖掉server那边的自动驾驶方式，因为TM这边是你可以进行设置的
这边的execution flow is divided in stages with independent operations and goals. [这里估计在说内核那边C++的设计了] 主要是这样好对应phase-related functionalities and data structures 以此提高计算的效率。每一个stage都是在不同的线程上运行的，通过我们上次说到的同步管理来进行stage之间的消息同步，注意此处的消息流是单方向的不是双向的

个性化设置 User customization

用户需要设置一些其他参数 以此实现控制自己的traffic flow方式，类似于车辆的行为模式，比如：

1. 允许一些车辆直接忽略限速要求
2. 允许一些车辆可以强制切换车道 [超车嘛？]

TM车辆管理的目的呢，也就是使得自己可以设置车辆的行为方式，以进行场景测试 [我默认就场景测试了]

---

Architecture 架构

这一块主要是前面引用参考中官方的英文翻译和知乎里的翻译摘取了 [更为详细的建议查看官方文档 里面有更多的超链]，首先是这幅图，配合下面文字可能更清楚点

1. 获取仿真器里的状态 就是提取仿真器里有哪些车辆 Store and update the current state of the simulation

   • Vehicle Registry.
     首先Server端的Lifecycle管理器（ALSM）会扫描simulation里所有的汽车、行人的信息，删除掉已经died的物体，然后Traffic Manager会把部分车辆注册到自己旗下（至于哪些会被选中是由用户决定的), 这些被选中的车辆的行为从此以后就会被该traffic manager控制了!（可以有多个Traffic manager同时存在，后面会详细讲述）

     Related .cpp files: MotionPlannerStage.cpp

2. 根据上面在TM里注册好了的车辆进行计算和更新，主要有以下几个部分，Calculate the movement of every registered vehicle.

   • Stage 1: Localization Stage.
     In-Memory Map 这个外部模块会缓存整张地图的拓扑结构以及每辆汽车的短期路线规划（实质上是一系列的waypoint), TM在这一阶段做的就是读取in memory map，把自己管辖的车辆的位置、路径规划存到内部模块path buffer & vehicle tracking(PBVT)里面，这样方便后面的函数快速便捷读取相关信息。

     Related .cpp files: LocalizationStage.cpp and LocalizationUtils.cpp

   • Stage 2: Collision Stage.
     TM会根据每个管辖车辆的周边环境和他们的路线规划来判断是否有潜在车祸可能，如果有的话会将此信息传递给motion planner阶段。

     Related .cpp files: CollisionStage.cpp

   • Stage 3: Traffic Light Stage.
     TM会探测每一辆车是否接近红灯或路口，将信息传给下一阶段。

     Related .cpp files: TrafficLightStage.cpp

   • Stage 4: Motion Planner.
     将上个两阶段的信息整合到一起，制定每一辆所管辖的车下一步的举动（比如变道、急刹或者正常行驶），设立下一个目标点，将该点传给PID控制器，得到方向盘应该转动的角度和踩油门的力度、刹车的力度。 [所以主要可以从这里改每辆车的行为方式]

     Related .cpp files: MotionPlannerStage.cpp

3. 发送第二大部分里的更新指令到仿真器Server里 Apply the commands in the simulation.

---

具体使用方式 Using the Traffic Manager

下面介绍的TM车辆管理器里的注册车辆性质呢，是默认模式下的哈： [下面是英文的原文]

• 车辆没有什么目的地之说 主要就是沿着路走，什么时候遇到路口了随机选一条继续走，所以整个仿真过程中开启自动驾驶（这里是指CARLA里的auto模式）后车辆的路径是endless 无穷无尽的，没有终点
  Vehicles are not goal-oriented they follow a trajectory and whenever approaching a junction, choose a path randomly. Their path is endless, and will never stop roaming around the city.
• 车辆的速度一般都是当前限速的70%，比如这条路限速60km/h，则TM内的自动行车速度一般是42km/h
  Vehicles’ target speed is 70% their current speed limit: unless any other value is set.
• 交叉路口不会遵循交通规则：TM里的车都是更高优先级的，也就是说他们不会为了其他管理器外的车辆让路，比如我自己一辆车在直行，TM中有辆车想左转他不管你的，直接左转撞到就撞到了
  Junction priority does not follow traffic regulations: the TM has a priority system to be used while junction complexity is solved. This may cause some issues such as a vehicle inside a roundabout yielding to a vehicle trying to get in.

简要总结

混合物理模式，主要呢：This feature removes the vehicle physics bottleneck from the simulator. Since vehicle physics are not active, all cars move by teleportation. 大概就是提高仿真效率之说？

如下图表示的是：有一辆(头顶橙色标标)车，就是我们的控制车辆，只有橙色标标车附近的车 我开启Hybird Physics 功能(头顶绿色标志)，其他范围外的不开物理(蓝紫色标示意)，这样可以提高整个仿真器的计算吧

简要总结

Title	Description
通用	1. 主要就是在client里创建TM 2. 通过端口读取创建好的TM 比如创建TM时设置的port在8000 (默认是8000哈)
相关安全设置	1. 设置最小的停车距离，TM里的车会保持这个最小停车距离的 2. 设置速度，比如上面提到的现在路限速60km/h，设置70% (默认是70% 可以改) 3. 重置交通灯
碰撞管理等	1. Enable/Disable collisions between a vehicle and a specific actor. 2. Make a vehicle ignore all the other vehicles. 3. Make a vehicle ignore all the walkers. 4. Make a vehicle ignore all the traffic lights.
变道	1. Force a lane change disregarding possible collisions. 2. Enable/Disable lane changes for a vehicle.

代码部分

介绍完了这么多了，也该直接上代码试一试了

1. 打开tm，这一步是可以在任意的client里使用，只要端口一致即可，默认端口一般为8000

   tm = client.get_trafficmanager(port)
   

2. 接着可以通过端口进行修改其中的车辆状态，比如在这里是将vehicles_list里的车都通过这个tm端口设置成自动驾驶

   tm_port = tm.get_port()
    for v in vehicles_list:
        v.set_autopilot(True,tm_port)
   

3. 整体可以查看spawn_npc.py文件内的tm使用方式：

   traffic_manager = client.get_trafficmanager(args.tm-port)
   tm_port = traffic_manager.get_port()
   # ...
   batch.append(SpawnActor(blueprint, transform).then(SetAutopilot(FutureActor, True,tm_port)))
   # ...
   traffic_manager.global_percentage_speed_difference(30.0)
   

   可以查看一下文档得知：global_percentage_speed_difference 例如这条路的限速是60km/h，设为30.0的话就是车辆运行速度会降为60*0.7=42km/h emm 看起来还挺慢的哎，然后如果想TM内车辆超速行驶的话，可以直接-10，负百分比来设定超过速度限制多少

更多设置函数与方法见：https://carla.readthedocs.io/en/0.9.11/python_api/#carlatrafficmanager 官方文档

仿真实验

设置了几个方式但是没什么直观的对比图，以下是几个问题待解决

• 首先是关于混合物理模式开启的时候，没找到怎么判断其他车辆是否开启物理；就像上面的gif那样可以得知
• 测试车辆无法再设为自动驾驶模式？
  可以开启，但是这样无法get到周围车辆的速度了

import glob
import os
import sys

# ==============================================================================
# -- Find CARLA module ---------------------------------------------------------
# ==============================================================================
try:
    sys.path.append(glob.glob('../../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

import carla

import random
import time
import numpy as np
import cv2
import queue
import logging
import math

actor_list = []
vehicles_id_list = []
set_synchronous = True # 同步模式
tm_port=8000 # tm端口

try:
    
    client = carla.Client('localhost', 3000)
    client.set_timeout(5.0)
    world = client.get_world()
    tm = client.get_trafficmanager(tm_port)
    
    # 设置车辆直接停车距离为3m
    tm.set_global_distance_to_leading_vehicle(0.1)

    # 设置车辆速度限速的百分之30
    tm.global_percentage_speed_difference(30.0)

    if set_synchronous:
        print('Pay attention this client set mode to synchronous')
        settings = world.get_settings()
        settings.synchronous_mode = True
        settings.fixed_delta_seconds = 0.05
        world.apply_settings(settings)
        # 需要同时为TM开启同步
        tm.set_synchronous_mode(True)

    blueprint_library = world.get_blueprint_library()

    bp = blueprint_library.find('vehicle.audi.a2')

    # 必须将测试车辆的role name设置为hero 才开启了混合物理模式
    bp.set_attribute('role_name', 'hero')

    spawn_point = random.choice(world.get_map().get_spawn_points())
    print('my vehicle blueprint:',bp,'my car position:',spawn_point)
    vehicle = world.spawn_actor(bp, spawn_point)

    actor_list.append(vehicle)

    # 设置混合物理模式开启仅在测试车辆附近
    tm.set_hybrid_physics_mode(True)

    #  默认是70，这里设置了半径50m内开启
    tm.set_hybrid_physics_radius(50.0)

    # ------------- 此部分来源于spawn_npc.py ---------------------------- #
    SpawnActor = carla.command.SpawnActor
    SetAutopilot = carla.command.SetAutopilot
    SetVehicleLightState = carla.command.SetVehicleLightState
    FutureActor = carla.command.FutureActor

    blueprints_vehicle = blueprint_library.filter("vehicle.*")
    blueprints = sorted(blueprints_vehicle, key=lambda bp: bp.id)
    spawn_points = world.get_map().get_spawn_points()
    number_of_spawn_points = len(spawn_points)
    # --------------
    # Spawn vehicles
    # --------------
    
    batch = []
    number_of_vehicles = 100
    for n, transform in enumerate(spawn_points):
        if n >= number_of_vehicles:
            break
        blueprint = random.choice(blueprints)
        if blueprint.has_attribute('color'):
            color = random.choice(blueprint.get_attribute('color').recommended_values)
            blueprint.set_attribute('color', color)
        if blueprint.has_attribute('driver_id'):
            driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
            blueprint.set_attribute('driver_id', driver_id)
        blueprint.set_attribute('role_name', 'autopilot')

        # spawn the cars and set their autopilot and light state all together
        batch.append(SpawnActor(blueprint, transform)
            .then(SetAutopilot(FutureActor, True, tm.get_port())))

    for response in client.apply_batch_sync(batch, set_synchronous):
        if response.error:
            logging.error(response.error)
        else:
            vehicles_id_list.append(response.actor_id)
    # ------------- 此部分来源于spawn_npc.py ---------------------------- #

    # 获得所有车辆
    vehicles_list = world.get_actors().filter('vehicle.*')
    for v in vehicles_list:
        if v.attributes['role_name'] != "hero":
            # 设置可切车道
            tm.auto_lane_change(v, True)

    # vehicle.set_autopilot(True)
    spectator = world.get_spectator()

    while True:
        if set_synchronous:
            world.tick()
        else:
            world.wait_for_tick()

        transform = vehicle.get_transform()
        spectator.set_transform(carla.Transform(transform.location + carla.Location(z=50), 
                                carla.Rotation(yaw=int(transform.rotation.yaw), roll=0, pitch=-90)))

        # 打印距离测试车辆10m以内所有车辆的速度
        for v in vehicles_list:
            if v.attributes['role_name'] != "hero":
                distance = math.sqrt((transform.location.x-v.get_transform().location.x)**2 + (transform.location.y - v.get_transform().location.y)**2)
                if distance<20 and (int(v.get_velocity().y) or int(v.get_velocity().x) or int(v.get_velocity().z) != 0):
                    print('type:',v.type_id ,'id',v.id,'v:', v.get_velocity())

finally:
    if set_synchronous:
        print('set mode to asynchronous')
        settings = world.get_settings()
        settings.synchronous_mode = False
        settings.fixed_delta_seconds = None
        world.apply_settings(settings)
        tm.set_synchronous_mode(False)

    
    print('\ndestroying %d vehicles' % len(vehicles_id_list))
    client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_id_list])
    print('destroying actors')
    for actor in actor_list:
        actor.destroy()
    print('done.')
    time.sleep(0.5)

---

注意事项

1. 如果CARLA的服务端开启了同步模式，TM必须也要手动开同步模式的！destory掉了也记得disable掉这个同步模式

   ...
   
   # Set the simulation to sync mode
   init_settings = world.get_settings()
   settings = world.get_settings()
   settings.synchronous_mode = True
   # Right after that, set the Traffic Manager to sync mode
   my_tm.set_synchronous_mode(True)
   
   ...
   
   # Tick the world in the same client
   world.apply_settings(init_settings)
   world.tick()
   ...
   
   # Disable the sync mode always, before the script ends
   settings.synchronous_mode = False
   my_tm.set_synchronous_mode(False)
   

2. 这又有个文档写错了的… 但是dev lastest分支没错，对应到版本就错了，见issue链接：https://github.com/carla-simulator/carla/issues/3926

   # github上有人提issue更正的：
   tm.set_hybrid_physics_radius(50.0)
   
   # 9.11文档里写的 这样会报错
   tm.set_hybrid_mode_radius(50.0)
   

3. 在tm开启切车道的动作，也不会使得车辆有超车行为，而是随机的切车道（但是会注意到是否能切，也有不要命的切车道行为：force_lane_change 也就是这个）

     # 获得所有车辆
     vehicles_list = world.get_actors().filter('vehicle.*')
     for v in vehicles_list:
         if v.attributes['role_name'] is not 'hero':
             # 设置可切车道
             tm.auto_lane_change(v, True)
   

4. 在开启混合物理模式下，一定要记得给自己的测试车辆设role_name不然的话会不知道是哪个为中心来开

   # 必须将测试车辆的role name设置为hero 才开启了混合物理模式
   bp.set_attribute('role_name', 'hero')
   
   # 设置混合物理模式开启仅在测试车辆附近
   tm.set_hybrid_physics_mode(True)
   
   #  默认是70，这里设置了半径50m内开启
   tm.set_hybrid_physics_radius(50.0)
   

   官方文档处：

   • Ego vehicle — A vehicle tagged with role_name='hero' that will act of the radius.
     • If there is none, all the vehicles will disable physics.
     • If there are many, the radius will be considered for all of them. That will create different areas of influence with physics enabled.

5. 关于自身车辆开启自动驾驶后，TM内车辆get_velocity()的时候都是0的现象在0.9.11还是会出现：

   有关其他相似问题的issue链接见：https://github.com/carla-simulator/carla/issues/4238 但是我仔细看了一下不太相关，这个issue下是指：在开启混合物理模式后 hero 也就是测试车辆get不到速度

   但是我的问题是在开启自动驾驶后无法get到周围车辆的速度了，但是只要不开就可以获得，具体重现方式在上面实验部分 开启注释标的# vehicle.set_autopilot(True) 就能发现了

Hybrid physics mode 混合物理模式

这个点我总是觉得很奇怪，这里的描述有点针对于车辆的运行是否遵循动力学，比如不在范围内的，车辆可以瞬间移动这种；在开启范围内的，就会老老实实运行，但是这个和我一开始想的穿模好像不太一样… 也就是碰撞属性，可以需要后面测试一下，这个physics是否包含这一项

还有个就是关于车辆物理的设置，比如设置轮胎的摩擦系数等等，具体官方文档里有写过，但是论坛上有人提到好像车不动… 不过我们也很少用就先算了吧

import carla
import random

def main():
    # Connect to client
    client = carla.Client('127.0.0.1', 2000)
    client.set_timeout(2.0)

    # Get World and Actors
    world = client.get_world()
    current_map = world.get_map()
    actors = world.get_actors()

    # Get a random vehicle from world (there should be one at least)
    vehicle = random.choice([actor for actor in actors if 'vehicle' in actor.type_id])

    # Create Wheels Physics Control
    front_left_wheel  = carla.WheelPhysicsControl(tire_friction=4.5, damping_rate=1.0, max_steer_angle=70.0, radius=30.0)
    front_right_wheel = carla.WheelPhysicsControl(tire_friction=2.5, damping_rate=1.5, max_steer_angle=70.0, radius=25.0)
    rear_left_wheel   = carla.WheelPhysicsControl(tire_friction=1.0, damping_rate=0.2, max_steer_angle=0.0,  radius=15.0)
    rear_right_wheel  = carla.WheelPhysicsControl(tire_friction=1.5, damping_rate=1.3, max_steer_angle=0.0,  radius=20.0)

    wheels = [front_left_wheel, front_right_wheel, rear_left_wheel, rear_right_wheel]

    # Change Vehicle Physics Control parameters of the vehicle
    physics_control = vehicle.get_physics_control()

    physics_control.torque_curve = [carla.Vector2D(x=0, y=400), carla.Vector2D(x=1300, y=600)]
    physics_control.max_rpm = 10000
    physics_control.moi = 1.0
    physics_control.damping_rate_full_throttle = 0.0
    physics_control.use_gear_autobox = True
    physics_control.gear_switch_time = 0.5
    physics_control.clutch_strength = 10
    physics_control.mass = 10000
    physics_control.drag_coefficient = 0.25
    physics_control.steering_curve = [carla.Vector2D(x=0, y=1), carla.Vector2D(x=100, y=1), carla.Vector2D(x=300, y=1)]
    physics_control.wheels = wheels
    physics_control.use_sweep_wheel_collision = True

    # Apply Vehicle Physics Control for the vehicle
    vehicle.apply_physics_control(physics_control)

if __name__ == '__main__':
    main()

官方设置：https://carla.readthedocs.io/en/0.9.11/tuto_G_control_vehicle_physics/
论坛：https://forum.carla.org/t/vehicle-does-not-move-when-i-apply-physics-control/391

但是论坛里的那位仁兄没有贴出他的运行是不是直接的control_apply的方式所以… 我也不知道问题出在哪里，这段没有进行测试

Homework

至上次留个一个设置行人过马路的场景，其实在这个章节我们可以发现，当时写的应该是有个bug，也就是行人的物理没有开… 我是因为开Autoware的时候有一次车辆直接从行人那里穿过去发现的，所以要完整还得把行人的物理开起来set_simulate_physics() 具体是这个

设置生成车辆在自身测试车辆周围

题目：在生成测试车时，同时在测试车周围生成NPC玩家用来测试等…

https://github.com/carla-simulator/carla/issues/3876 对应issue链接

import carla
import numpy as np
import math

def spawn_vehicles_around_ego_vehicles(ego_vehicle, radius, spawn_points, numbers_of_vehicles):
    # parameters:
    # ego_vehicle :: your target vehicle
    # radius :: the distance limitation between ego-vehicle and other free-vehicles
    # spawn_points  :: the available spawn points in current map
    # numbers_of_vehicles :: the number of free-vehicles around ego-vehicle that you need
    np.random.shuffle(spawn_points)  # shuffle  all the spawn points
    ego_location = ego_vehicle.get_location()
    accessible_points = []
    for spawn_point in spawn_points:
        dis = math.sqrt((ego_location.x-spawn_point.location.x)**2 + (ego_location.y-spawn_point.location.y)**2)
        # it also can include z-coordinate,but it is unnecessary
        if dis < radius:
            print(dis)
            accessible_points.append(spawn_point)

    vehicle_bps = world.get_blueprint_library().filter('vehicle.*.*')   # don't specify the type of vehicle
    vehicle_bps = [x for x in vehicle_bps if int(x.get_attribute('number_of_wheels')) == 4]  # only choose car with 4 wheels

    vehicle_list = []  # keep the spawned vehicle in vehicle_list, because we need to link them with traffic_manager
    if len(accessible_points) < numbers_of_vehicles:
        # if your radius is relatively small,the satisfied points may be insufficient
        numbers_of_vehicles = len(accessible_points)

    for i in range(numbers_of_vehicles):  # generate the free vehicle
        point = accessible_points[i]
        vehicle_bp = np.random.choice(vehicle_bps)
        try:
            vehicle = world.spawn_actor(vehicle_bp, point)
            vehicle_list.append(vehicle)
        except:
            print('failed')  # if failed, print the hints.
            pass
# you also can add those free vehicle into trafficemanager,and set them to autopilot.
# Only need to get rid of comments for below code. Otherwise, the those vehicle will be static
    tm = client.get_trafficmanager()  # create a TM object
    tm.global_percentage_speed_difference(10.0)  # set the global speed limitation
    tm_port = tm.get_port()  # get the port of tm. we need add vehicle to tm by this port
    for v in vehicle_list:  # set every vehicle's mode
        v.set_autopilot(True, tm_port)  # you can get those functions detail in carla document
        tm.ignore_lights_percentage(v, 0)
        tm.distance_to_leading_vehicle(v, 0.5)
        tm.vehicle_percentage_speed_difference(v, -20)

# create client object
client = carla.Client("localhost", 2000)
client.set_timeout(10)

# create world object
world = client.load_world('Town05')
spawn_points = world.get_map().get_spawn_points()
model3_bp = world.get_blueprint_library().find('vehicle.tesla.model3')
model3_bp.set_attribute('color', '255,0,0')   # set vehicle's color to red
model3_spawn_point = np.random.choice(spawn_points)
model3_actor = world.spawn_actor(model3_bp, model3_spawn_point)
spawn_vehicles_around_ego_vehicles(ego_vehicle=model3_actor, radius=100,
                                   spawn_points=spawn_points, numbers_of_vehicles=10)
model3_actor.set_autopilot(True)  # set the ego-vehicle to autopilot