【含源码】基于Carla的跟驰仿真环境设计

Carla实现跟车模型

1. 需求说明

本文实现在Carla仿真环境中的跟车模型，具体为在仿真环境中放置两个车辆，前车与后车，通过 Carla API设置前车的自动驾驶模式，然后我们通过编程手段实现后车跟随前车的路线行动，目前有两种手段完成，第一种为使用安全距离以及速度差的方式驱动油门和刹车，逼近方法是PID；第二种手段是同样设置后车的自动驾驶模式，但是接管自动驾驶车辆的路口转向的自由，将其路口出路的选择变为前车所选择的路口而不是随机选择。

> Carla Python API 版本：0.9.13
>
> Python API：3.7.12

2. 跟车实现

###　2.1 类设计以及源码

在环境类的设计中，参数debug指定模式，实际上就是采用同步方式还是异步方式运行Carla，采用同步方式会逐帧处理所需要的逻辑。参数mode指定驱动模式，有两个参数可以选择，分别是auto和pid，其中auto模式为Carla自带自动驾驶API，但是其不能直接用于跟车模型，因为岔路口的道路选择策略是随机的。PID的方式是根据设定的安全距离和当前速度，进行PID的油门/刹车逼近。源码如下：

class Env:
    def __init__(self, client, world, tau=0.1):
        self.client = client
        self.world = world
        self.screen = None
        self.camera_bp = None
        self.mode = 'auto'
        self.debug = False
        self.green_light = True
        self.front_image = np.zeros((600, 800, 3), dtype=np.uint8)
        self.back_image = np.zeros((600, 800, 3), dtype=np.uint8)
        self.camera_front = None
        self.camera_back = None
        self.warning_img = None
        self.ego_vehicle = None  # 后车
        self.front_vehicle_bp = None  # 前车
        self.front_switch_road_waypoints = deque()
        self.relative_distance = None
        self.relative_speed = None
        self.ego_speed = None
        self.ego_acceleration = None
        self.tau = 0.1
        self.destination = None
        self.control_flag = True
        self.map = self.world.get_map()
        self.spawn_vehicles()
        self.frenet = FrenetCoordinateSystem(self.map.generate_waypoints(2.0))
        # 初始化RSS参数
        self.t_response = 3  # 反应时间 (s)
        self.a_max_brake = 3  # 最大制动减速度 (m/s^2)
        if self.debug is True:
            settings = self.world.get_settings()
            settings.synchronous_mode = True
            settings.fixed_delta_seconds = 0.1
            world.apply_settings(settings)
        self.agent = BasicAgent(self.ego_vehicle)
        self.front_agent = BasicAgent(self.front_vehicle_bp)
        self.device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
        self.ddpg = torch.load('/home/moresweet/gitCloneZone/DIRL/checkpoints/RSS/weights/ddpg_105.pt')
        self.ddpg_acc = 0
        self.cur_acc = 0
        self.e2f_waypoints_queue = deque()
        self.last_location1 = self.front_vehicle_bp.get_location()
        self.last_location2 = self.ego_vehicle.get_location()

    """
    生成carla场景
    """

    def spawn_vehicles(self):
        # 加载场景
        blueprint_library = self.world.get_blueprint_library()
        vehicle_bp = blueprint_library.filter('vehicle.*')[0]
        vehicle_bp_rear = blueprint_library.filter('vehicle.*')[0]
        spawn_points = self.world.get_map().get_spawn_points()
        front_spawn_point = spawn_points[0]
        # 获取前车的车道信息
        front_waypoint = self.map.get_waypoint(front_spawn_point.location)

        # 计算后车生成点（在前车后20米处）
        distance = 10  # 距离前车20米
        rear_location = front_waypoint.transform.location - front_waypoint.transform.get_forward_vector() * distance
        rear_spawn_point = carla.Transform(rear_location, front_waypoint.transform.rotation)
        # 创建前车和后车
        self.front_vehicle_bp = self.world.spawn_actor(vehicle_bp, front_spawn_point)
        self.ego_vehicle = self.world.spawn_actor(vehicle_bp_rear, rear_spawn_point)
        pygame.init()
        # 设置前车匀速行驶
        self.front_vehicle_bp.set_autopilot(True)
        if self.mode == 'auto':
            self.ego_vehicle.set_autopilot(True)
        pygame.init()
        # 创建pygame窗口
        self.screen = pygame.display.set_mode((800, 600))
        pygame.display.set_caption("CARLA Manual Control")
        # 设置前后相机视角
        self.camera_bp = blueprint_library.find('sensor.camera.rgb')
        self.camera_bp.set_attribute('image_size_x', '800')
        self.camera_bp.set_attribute('image_size_y', '600')
        self.camera_bp.set_attribute('fov', '110')
        camera_transform_front = carla.Transform(carla.Location(x=1.5, y=0, z=2.4), carla.Rotation(yaw=30))
        self.camera_front = world.spawn_actor(self.camera_bp, camera_transform_front, attach_to=self.ego_vehicle)
        camera_transform_back = carla.Transform(carla.Location(x=-0.2, y=-0.2, z=1.3), carla.Rotation(yaw=28))
        self.camera_back = world.spawn_actor(self.camera_bp, camera_transform_back, attach_to=self.ego_vehicle)
        self.camera_front.listen(lambda image: self.front_camera_callback(image))
        self.camera_back.listen(lambda image: self.back_camera_callback(image))
        self.warning_img = pygame.image.load('./EARLY_WARNING.png')
        self.warning_img = pygame.transform.scale(self.warning_img, (50, 50))

    """
    相机图像处理
    :param image: 相机图像
    """

    def process_img(self, image):
        array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
        array = np.reshape(array, (image.height, image.width, 4))
        array = array[:, :, :3]
        array = array[:, :, ::-1]
        return array

    """
    摄像头回调函数
    :param image: 相机图像
    """

    def front_camera_callback(self, image)