Carla学习（七）收集同步的传感器数据

为了收集同一时刻的图像雷达数据，利用到了两个点：同步模式和固定时间步
这块挺疑惑的，到底是客户端的world.tick()驱动服务器更新step，还是服务器按照固定的时间步更新step，后面写了自己的猜测，不知道对不对。。。
不过最后收集到的传感器数据是同步的

time-step

time-step是服务器更新两个step之间的时间间隔，分为两种：

• variable time-step：取决于计算机计算的时间，通常很短
• fixed time-step：将两个step之间的时间间隔固定为一个常数值，那么每个simulated second包含1/fixed time-step 帧。

Carla同步模式

官方解释是服务器只有在等待一个客户端发送的world.tick()后才会计算下一个step。

为了收集同步的数据，参照官方的示例synchronous_mode.py，同时设置同步模式和固定时间步：

init_setting = world.get_settings()
 settings = world.get_settings()
 settings.synchronous_mode = True
 settings.fixed_delta_seconds = 0.05  
 world.apply_settings(settings)

然后用pygame将收集到的图片播放出来，打印出服务器的fps和客户端的fps：

display.blit(
                font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)),
                (8, 10))
display.blit(
                font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)),
                (8, 28))

发现服务器的fps是20，客户端的fps是10左右。

思考一下这里是如何进行同步的：

• 同步模式下客户端不会自动计算下一个step，它只有在接受一个world.tick()后才会计算下一个step
• 当客户端发送一个tick后，服务器耗费0.05s计算下一个step，然后暂停仿真，继续等待下一个tick

最后客户端的fps是10左右，所以在每一轮循环大概花费0.1s，而服务器是20fps。在一秒内客户端发送10个tick，那么服务器不也是更新10个step，也就是10fps吗？这里服务器的20fps是指simulated second，所以实际上tick驱动服务器更新一个step后，服务器暂停模拟。它更新10个fps实际花费的时间是1s，而花费的模拟时间是0.5s。所以循环的2s才相当于模拟的1s。

代码如下：

import glob
import os
import sys

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 pygame
import random
from constants import *
import queue
import numpy as np


class SynchronyModel(object):
    def __init__(self):
        self.world,self.init_setting = self._make_setting()
        self.blueprint_library = self.world.get_blueprint_library()
        self.non_player = []
        self.actor_list = []
        self.frame = None
        self.player = None
        self.sensors = []
        self._queues = []
        self._span_player()

    def __enter__(self):
        # set the sensor listener function
        def make_queue(register_event):
            q = queue.Queue()
            register_event(q.put)
            self._queues.append(q)

        make_queue(self.world.on_tick)
        for sensor in self.sensors:
            make_queue(sensor.listen)
        return self

    def tick(self, timeout):
        self.frame = self.world.tick()
        data = [self._retrieve_data(q, timeout) for q in self._queues]
        assert all(x.frame == self.frame for x in data)
        return data

    def _retrieve_data(self, sensor_queue, timeout):
        while True:
            data = sensor_queue.get(timeout=timeout)
            if data.frame == self.frame:
                return data

    def __exit__(self, *args, **kwargs):
        # cover the world settings
        self.world.apply_settings(self.init_setting)

    def _make_setting(self):
        client = carla.Client('localhost', 2000)
        client.set_timeout(5.0)
        world = client.get_world()
        # synchrony model and fixed time step
        init_setting = world.get_settings()
        settings = world.get_settings()
        settings.synchronous_mode = True
        settings.fixed_delta_seconds = 0.05  # 20 steps  per second
        world.apply_settings(settings)
        return world, init_setting

    def _span_player(self):
        my_vehicle_bp = random.choice(self.blueprint_library.filter('vehicle.bmw.*'))

        location = carla.Location(198, 10, 0.5)
        rotation = carla.Rotation(0, 0, 0)
        transform_vehicle = carla.Transform(location, rotation)

        my_vehicle = self.world.spawn_actor(my_vehicle_bp, transform_vehicle)
        self._span_sensor(my_vehicle)
        self.actor_list.append(my_vehicle)
        self.player = my_vehicle

    def _span_sensor(self, player):
        camera_bp = self.blueprint_library.find('sensor.camera.rgb')
        camera_d_bp = self.blueprint_library.find('sensor.camera.depth')
        lidar_bp = self.blueprint_library.find('sensor.lidar.ray_cast')

        camera_bp.set_attribute('image_size_x', str(WINDOW_WIDTH))
        camera_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT))
        camera_bp.set_attribute('fov', '90')

        camera_d_bp.set_attribute('image_size_x', str(WINDOW_WIDTH))
        camera_d_bp.set_attribute('image_size_y', str(WINDOW_HEIGHT))
        camera_d_bp.set_attribute('fov', '90')

        lidar_bp.set_attribute('rotation_frequency', '20')

        transform_sensor = carla.Transform(carla.Location(x=0, y=0, z=CAMERA_HEIGHT_POS))

        my_camera = self.world.spawn_actor(camera_bp, transform_sensor, attach_to=player)
        my_camera_d = self.world.spawn_actor(camera_d_bp, transform_sensor, attach_to=player)
        my_lidar = self.world.spawn_actor(lidar_bp, transform_sensor, attach_to=player)

        self.actor_list.append(my_camera)
        self.actor_list.append(my_camera_d)
        self.actor_list.append(my_lidar)
        self.sensors.append(my_camera)
        self.sensors.append(my_camera_d)
        self.sensors.append(my_lidar)


def draw_image(surface, image, blend=False):
    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]
    image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
    if blend:
        image_surface.set_alpha(100)
    surface.blit(image_surface, (0, 0))


def should_quit():
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            return True
        elif event.type == pygame.KEYUP:
            if event.key == pygame.K_ESCAPE:
                return True
    return False


def get_font():
    fonts = [x for x in pygame.font.get_fonts()]
    default_font = 'ubuntumono'
    font = default_font if default_font in fonts else fonts[0]
    font = pygame.font.match_font(font)
    return pygame.font.Font(font, 14)


def main():
    pygame.init()
    display = pygame.display.set_mode(
        (WINDOW_WIDTH, WINDOW_HEIGHT),
        pygame.HWSURFACE | pygame.DOUBLEBUF)
    font = get_font()
    clock = pygame.time.Clock()

    with SynchronyModel() as sync_mode:
        waypoint = sync_mode.world.get_map().get_waypoint(sync_mode.player.get_transform().location)
        while True:
            if should_quit():
                break
            clock.tick()
            snapshot, image_rgb, image_depth, point_cloud = sync_mode.tick(timeout=2.0)
            waypoint = random.choice(waypoint.next(1.5))
            sync_mode.player.set_transform(waypoint.transform)
            fps = round(1.0 / snapshot.timestamp.delta_seconds)
            draw_image(display, image_rgb)
            display.blit(
                font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)),
                (8, 10))
            display.blit(
                font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)),
                (8, 28))
            pygame.display.flip()

        print('destroying actors.')
        for actor in sync_mode.actor_list:
            actor.destroy()
        pygame.quit()
        print('done.')


if __name__ == '__main__':

    main()