carla_environment.py

""" Carla.org environment"""
import glob
import os
import random
import sys

import numpy as np
import pygame

from steering_wheel import Controller

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

import carla


class Camera:
    """ Add camera sensor to the carla world """
    sensor = None
    surface = None
    display = None
    width = None
    height = None
    surface = None
    camera_type = None

    def __init__(self, world, camera_type='rgb', width=1280, height=720):
        self.width = width
        self.height = height
        self.camera_type = camera_type
        self.display = pygame.display.set_mode(
            (width, height),
            pygame.HWSURFACE | pygame.DOUBLEBUF)
        self.display.fill((0,0,0))
        camera_blueprint = world.world.get_blueprint_library().find('sensor.camera.' + camera_type)
        camera_blueprint.set_attribute('image_size_x', f'{self.width}')
        camera_blueprint.set_attribute('image_size_y', f'{self.height}')
        camera_blueprint.set_attribute('fov', '95')
        camera_transform = carla.Transform(carla.Location(x=-3.0, z=2.2))
        self.sensor = world.world.spawn_actor(camera_blueprint, camera_transform, attach_to=world.player)
        self.sensor.listen(lambda data: self.process_img(data))
    
    def process_img(self, data):
        """ Callback for rgb-camera sensor. """
        if self.camera_type == 'semantic_segmentation':
            data.convert(carla.ColorConverter.CityScapesPalette)
        array = np.frombuffer(data.raw_data, dtype=np.dtype("uint8"))
        array = np.reshape(array, (self.height, self.width, 4))
        array = array[:, :, :3]
        array = array[:, :, ::-1]
        self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
    
    def on_update(self):
        """ Update the pygame display. """
        if self.surface is not None:
            self.display.blit(self.surface, (0, 0))
            pygame.display.flip()


class World:
    """ Wrapper for the carla environment, incl. player/vehicle """
    player = None
    world = None
    blueprint_library = None
    spawn_points = None

    def __init__(self, world):
        self.world = world
        self.blueprint_library = self.world.get_blueprint_library()
        self.spawn_points = self.world.get_map().get_spawn_points()
        self.reset()
    
    def reset(self):
        """ Remove and create new player/vehicle. """
        self.destroy()
        self.spawn_player()

    def spawn_player(self):
        """ Add a vehicle to the world. """
        while self.player is None:
            blueprint = random.choice(self.blueprint_library.filter('model3'))
            position = random.choice(self.spawn_points)
            self.player = self.world.try_spawn_actor(blueprint, position)
            start_location = self.player.get_location()
            print(str(start_location))
            # start_location.x = 288.0
            # start_location.y = 55.0
            # self.player.set_location(start_location)

    def destroy(self):
        """ Remove vehicle from the world. """
        if self.player is not None:
            self.player.destroy()

    def step(self, action):
        """ Apply controls to vehicle. """
        self.player.apply_control(action)
        # print(str(self.player.get_location()))
        # print(str(self.player.get_velocity()))


class CarlaEnvironment:
    world = None
    client = None

    def __init__(self, host="127.0.0.1", port=2000):
        self.client = carla.Client(host, port)
        self.client.set_timeout(2.0)
        self.client.load_world('Town07')
        self.world = World(self.client.get_world())
    
    def step(self, action):
        control = carla.VehicleControl(throttle=action[0], steer=action[2], brake=action[1], reverse=action[3])
        self.world.step(control)
        
if __name__ == "__main__":
    LOCALHOST="127.0.0.1"
    SERVER="192.168.188.20"

    pygame.init()
    clock = pygame.time.Clock()

    env = CarlaEnvironment(host=LOCALHOST)
    cam = Camera(env.world, camera_type='semantic_segmentation')
    ctrl = Controller()

    while ctrl.is_running():
        clock.tick(60)
        ctrl.on_update()
        env.step(ctrl.get_action())
        cam.on_update()
        
    env.world.destroy()
    pygame.quit()