#!/usr/bin/env python # Copyright (c) 2018 Intel Labs. # authors: German Ros (german.ros@intel.com) # # This work is licensed under the terms of the MIT license. # For a copy, see . """ Example of automatic vehicle control from client side. """ from __future__ import print_function import argparse import collections import datetime import glob import logging import math import os import random import re import sys import weakref try: import pygame from pygame.locals import KMOD_CTRL from pygame.locals import K_ESCAPE from pygame.locals import K_q except ImportError: raise RuntimeError('cannot import pygame, make sure pygame package is installed') try: import numpy as np except ImportError: raise RuntimeError( 'cannot import numpy, make sure numpy package is installed') # ============================================================================== # -- 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 # ============================================================================== # -- add PythonAPI for release mode -------------------------------------------- # ============================================================================== try: sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') except IndexError: pass import carla from carla import ColorConverter as cc from agents.navigation.roaming_agent import RoamingAgent from agents.navigation.basic_agent import BasicAgent # ============================================================================== # -- Global functions ---------------------------------------------------------- # ============================================================================== def find_weather_presets(): rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] def get_actor_display_name(actor, truncate=250): name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name # ============================================================================== # -- World --------------------------------------------------------------- # ============================================================================== class World(object): def __init__(self, carla_world, hud, actor_filter): self.world = carla_world self.map = self.world.get_map() self.hud = hud self.player = None self.collision_sensor = None self.lane_invasion_sensor = None self.gnss_sensor = None self.camera_manager = None self._weather_presets = find_weather_presets() self._weather_index = 0 self._actor_filter = actor_filter self.restart() self.world.on_tick(hud.on_world_tick) self.recording_enabled = False self.recording_start = 0 def restart(self): # Keep same camera config if the camera manager exists. cam_index = self.camera_manager.index if self.camera_manager is not None else 0 cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 # Get a random blueprint. blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) blueprint.set_attribute('role_name', 'hero') if blueprint.has_attribute('color'): color = random.choice(blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) # Spawn the player. if self.player is not None: spawn_point = self.player.get_transform() spawn_point.location.z += 2.0 spawn_point.rotation.roll = 0.0 spawn_point.rotation.pitch = 0.0 self.destroy() self.player = self.world.try_spawn_actor(blueprint, spawn_point) while self.player is None: spawn_points = self.map.get_spawn_points() spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() self.player = self.world.try_spawn_actor(blueprint, spawn_point) # Set up the sensors. self.collision_sensor = CollisionSensor(self.player, self.hud) self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) self.gnss_sensor = GnssSensor(self.player) self.camera_manager = CameraManager(self.player, self.hud) self.camera_manager.transform_index = cam_pos_index self.camera_manager.set_sensor(cam_index, notify=False) actor_type = get_actor_display_name(self.player) self.hud.notification(actor_type) def next_weather(self, reverse=False): self._weather_index += -1 if reverse else 1 self._weather_index %= len(self._weather_presets) preset = self._weather_presets[self._weather_index] self.hud.notification('Weather: %s' % preset[1]) self.player.get_world().set_weather(preset[0]) def tick(self, clock): self.hud.tick(self, clock) def render(self, display): self.camera_manager.render(display) self.hud.render(display) def destroy_sensors(self): self.camera_manager.sensor.destroy() self.camera_manager.sensor = None self.camera_manager.index = None def destroy(self): actors = [ self.camera_manager.sensor, self.collision_sensor.sensor, self.lane_invasion_sensor.sensor, self.gnss_sensor.sensor, self.player] for actor in actors: if actor is not None: actor.destroy() # ============================================================================== # -- KeyboardControl ----------------------------------------------------------- # ============================================================================== class KeyboardControl(object): def __init__(self, world): world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) def parse_events(self): for event in pygame.event.get(): if event.type == pygame.QUIT: return True elif event.type == pygame.KEYUP: if self._is_quit_shortcut(event.key): return True @staticmethod def _is_quit_shortcut(key): return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) # ============================================================================== # -- HUD ----------------------------------------------------------------------- # ============================================================================== class HUD(object): def __init__(self, width, height): self.dim = (width, height) font = pygame.font.Font(pygame.font.get_default_font(), 20) font_name = 'courier' if os.name == 'nt' else 'mono' fonts = [x for x in pygame.font.get_fonts() if font_name in x] default_font = 'ubuntumono' mono = default_font if default_font in fonts else fonts[0] mono = pygame.font.match_font(mono) self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) self._notifications = FadingText(font, (width, 40), (0, height - 40)) self.help = HelpText(pygame.font.Font(mono, 24), width, height) self.server_fps = 0 self.frame = 0 self.simulation_time = 0 self._show_info = True self._info_text = [] self._server_clock = pygame.time.Clock() def on_world_tick(self, timestamp): self._server_clock.tick() self.server_fps = self._server_clock.get_fps() self.frame = timestamp.frame self.simulation_time = timestamp.elapsed_seconds def tick(self, world, clock): self._notifications.tick(world, clock) if not self._show_info: return t = world.player.get_transform() v = world.player.get_velocity() c = world.player.get_control() heading = 'N' if abs(t.rotation.yaw) < 89.5 else '' heading += 'S' if abs(t.rotation.yaw) > 90.5 else '' heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else '' heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else '' colhist = world.collision_sensor.get_collision_history() collision = [colhist[x + self.frame - 200] for x in range(0, 200)] max_col = max(1.0, max(collision)) collision = [x / max_col for x in collision] vehicles = world.world.get_actors().filter('vehicle.*') self._info_text = [ 'Server: % 16.0f FPS' % self.server_fps, 'Client: % 16.0f FPS' % clock.get_fps(), '', 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), 'Map: % 20s' % world.map.name, 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), '', 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x ** 2 + v.y ** 2 + v.z ** 2)), u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading), 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), 'Height: % 18.0f m' % t.location.z, ''] if isinstance(c, carla.VehicleControl): self._info_text += [ ('Throttle:', c.throttle, 0.0, 1.0), ('Steer:', c.steer, -1.0, 1.0), ('Brake:', c.brake, 0.0, 1.0), ('Reverse:', c.reverse), ('Hand brake:', c.hand_brake), ('Manual:', c.manual_gear_shift), 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] elif isinstance(c, carla.WalkerControl): self._info_text += [ ('Speed:', c.speed, 0.0, 5.556), ('Jump:', c.jump)] self._info_text += [ '', 'Collision:', collision, '', 'Number of vehicles: % 8d' % len(vehicles)] if len(vehicles) > 1: self._info_text += ['Nearby vehicles:'] def distance(l): return math.sqrt( (l.x - t.location.x) ** 2 + (l.y - t.location.y) ** 2 + (l.z - t.location.z) ** 2) vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] for d, vehicle in sorted(vehicles): if d > 200.0: break vehicle_type = get_actor_display_name(vehicle, truncate=22) self._info_text.append('% 4dm %s' % (d, vehicle_type)) def toggle_info(self): self._show_info = not self._show_info def notification(self, text, seconds=2.0): self._notifications.set_text(text, seconds=seconds) def error(self, text): self._notifications.set_text('Error: %s' % text, (255, 0, 0)) def render(self, display): if self._show_info: info_surface = pygame.Surface((220, self.dim[1])) info_surface.set_alpha(100) display.blit(info_surface, (0, 0)) v_offset = 4 bar_h_offset = 100 bar_width = 106 for item in self._info_text: if v_offset + 18 > self.dim[1]: break if isinstance(item, list): if len(item) > 1: points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] pygame.draw.lines(display, (255, 136, 0), False, points, 2) item = None v_offset += 18 elif isinstance(item, tuple): if isinstance(item[1], bool): rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) else: rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) pygame.draw.rect(display, (255, 255, 255), rect_border, 1) f = (item[1] - item[2]) / (item[3] - item[2]) if item[2] < 0.0: rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) else: rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) pygame.draw.rect(display, (255, 255, 255), rect) item = item[0] if item: # At this point has to be a str. surface = self._font_mono.render(item, True, (255, 255, 255)) display.blit(surface, (8, v_offset)) v_offset += 18 self._notifications.render(display) self.help.render(display) # ============================================================================== # -- FadingText ---------------------------------------------------------------- # ============================================================================== class FadingText(object): def __init__(self, font, dim, pos): self.font = font self.dim = dim self.pos = pos self.seconds_left = 0 self.surface = pygame.Surface(self.dim) def set_text(self, text, color=(255, 255, 255), seconds=2.0): text_texture = self.font.render(text, True, color) self.surface = pygame.Surface(self.dim) self.seconds_left = seconds self.surface.fill((0, 0, 0, 0)) self.surface.blit(text_texture, (10, 11)) def tick(self, _, clock): delta_seconds = 1e-3 * clock.get_time() self.seconds_left = max(0.0, self.seconds_left - delta_seconds) self.surface.set_alpha(500.0 * self.seconds_left) def render(self, display): display.blit(self.surface, self.pos) # ============================================================================== # -- HelpText ------------------------------------------------------------------ # ============================================================================== class HelpText(object): def __init__(self, font, width, height): lines = __doc__.split('\n') self.font = font self.dim = (680, len(lines) * 22 + 12) self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) self.seconds_left = 0 self.surface = pygame.Surface(self.dim) self.surface.fill((0, 0, 0, 0)) for n, line in enumerate(lines): text_texture = self.font.render(line, True, (255, 255, 255)) self.surface.blit(text_texture, (22, n * 22)) self._render = False self.surface.set_alpha(220) def toggle(self): self._render = not self._render def render(self, display): if self._render: display.blit(self.surface, self.pos) # ============================================================================== # -- CollisionSensor ----------------------------------------------------------- # ============================================================================== class CollisionSensor(object): def __init__(self, parent_actor, hud): self.sensor = None self.history = [] self._parent = parent_actor self.hud = hud world = self._parent.get_world() bp = world.get_blueprint_library().find('sensor.other.collision') self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid circular # reference. weak_self = weakref.ref(self) self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) def get_collision_history(self): history = collections.defaultdict(int) for frame, intensity in self.history: history[frame] += intensity return history @staticmethod def _on_collision(weak_self, event): self = weak_self() if not self: return actor_type = get_actor_display_name(event.other_actor) self.hud.notification('Collision with %r' % actor_type) impulse = event.normal_impulse intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) self.history.append((event.frame, intensity)) if len(self.history) > 4000: self.history.pop(0) # ============================================================================== # -- LaneInvasionSensor -------------------------------------------------------- # ============================================================================== class LaneInvasionSensor(object): def __init__(self, parent_actor, hud): self.sensor = None self._parent = parent_actor self.hud = hud world = self._parent.get_world() bp = world.get_blueprint_library().find('sensor.other.lane_invasion') self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid circular # reference. weak_self = weakref.ref(self) self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) @staticmethod def _on_invasion(weak_self, event): self = weak_self() if not self: return lane_types = set(x.type for x in event.crossed_lane_markings) text = ['%r' % str(x).split()[-1] for x in lane_types] self.hud.notification('Crossed line %s' % ' and '.join(text)) # ============================================================================== # -- GnssSensor -------------------------------------------------------- # ============================================================================== class GnssSensor(object): def __init__(self, parent_actor): self.sensor = None self._parent = parent_actor self.lat = 0.0 self.lon = 0.0 world = self._parent.get_world() bp = world.get_blueprint_library().find('sensor.other.gnss') self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid circular # reference. weak_self = weakref.ref(self) self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) @staticmethod def _on_gnss_event(weak_self, event): self = weak_self() if not self: return self.lat = event.latitude self.lon = event.longitude # ============================================================================== # -- CameraManager ------------------------------------------------------------- # ============================================================================== class CameraManager(object): def __init__(self, parent_actor, hud): self.sensor = None self.surface = None self._parent = parent_actor self.hud = hud self.recording = False self._camera_transforms = [ carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), carla.Transform(carla.Location(x=1.6, z=1.7))] self.transform_index = 1 self.sensors = [ ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] world = self._parent.get_world() bp_library = world.get_blueprint_library() for item in self.sensors: bp = bp_library.find(item[0]) if item[0].startswith('sensor.camera'): bp.set_attribute('image_size_x', str(hud.dim[0])) bp.set_attribute('image_size_y', str(hud.dim[1])) elif item[0].startswith('sensor.lidar'): bp.set_attribute('range', '50') item.append(bp) self.index = None def toggle_camera(self): self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) self.sensor.set_transform(self._camera_transforms[self.transform_index]) def set_sensor(self, index, notify=True): index = index % len(self.sensors) needs_respawn = True if self.index is None \ else self.sensors[index][0] != self.sensors[self.index][0] if needs_respawn: if self.sensor is not None: self.sensor.destroy() self.surface = None self.sensor = self._parent.get_world().spawn_actor( self.sensors[index][-1], self._camera_transforms[self.transform_index], attach_to=self._parent) # We need to pass the lambda a weak reference to self to avoid # circular reference. weak_self = weakref.ref(self) self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) if notify: self.hud.notification(self.sensors[index][2]) self.index = index def next_sensor(self): self.set_sensor(self.index + 1) def toggle_recording(self): self.recording = not self.recording self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) def render(self, display): if self.surface is not None: display.blit(self.surface, (0, 0)) @staticmethod def _parse_image(weak_self, image): self = weak_self() if not self: return if self.sensors[self.index][0].startswith('sensor.lidar'): points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) points = np.reshape(points, (int(points.shape[0] / 3), 3)) lidar_data = np.array(points[:, :2]) lidar_data *= min(self.hud.dim) / 100.0 lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 lidar_data = lidar_data.astype(np.int32) lidar_data = np.reshape(lidar_data, (-1, 2)) lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) lidar_img = np.zeros(lidar_img_size) lidar_img[tuple(lidar_data.T)] = (255, 255, 255) self.surface = pygame.surfarray.make_surface(lidar_img) else: image.convert(self.sensors[self.index][1]) 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] self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) if self.recording: image.save_to_disk('_out/%08d' % image.frame) # ============================================================================== # -- game_loop() --------------------------------------------------------- # ============================================================================== def game_loop(args): pygame.init() pygame.font.init() world = None try: client = carla.Client(args.host, args.port) client.set_timeout(4.0) display = pygame.display.set_mode( (args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF) hud = HUD(args.width, args.height) world = World(client.get_world(), hud, args.filter) controller = KeyboardControl(world) if args.agent == "Roaming": agent = RoamingAgent(world.player) else: agent = BasicAgent(world.player) spawn_point = world.map.get_spawn_points()[0] agent.set_destination((spawn_point.location.x, spawn_point.location.y, spawn_point.location.z)) clock = pygame.time.Clock() while True: if controller.parse_events(): return # as soon as the server is ready continue! world.world.wait_for_tick(10.0) world.tick(clock) world.render(display) pygame.display.flip() control = agent.run_step() control.manual_gear_shift = False world.player.apply_control(control) finally: if world is not None: world.destroy() pygame.quit() # ============================================================================== # -- main() -------------------------------------------------------------- # ============================================================================== def main(): argparser = argparse.ArgumentParser( description='CARLA Manual Control Client') argparser.add_argument( '-v', '--verbose', action='store_true', dest='debug', help='print debug information') argparser.add_argument( '--host', metavar='H', default='127.0.0.1', help='IP of the host server (default: 127.0.0.1)') argparser.add_argument( '-p', '--port', metavar='P', default=2000, type=int, help='TCP port to listen to (default: 2000)') argparser.add_argument( '--res', metavar='WIDTHxHEIGHT', default='1280x720', help='window resolution (default: 1280x720)') argparser.add_argument( '--filter', metavar='PATTERN', default='vehicle.*', help='actor filter (default: "vehicle.*")') argparser.add_argument("-a", "--agent", type=str, choices=["Roaming", "Basic"], help="select which agent to run", default="Basic") args = argparser.parse_args() args.width, args.height = [int(x) for x in args.res.split('x')] log_level = logging.DEBUG if args.debug else logging.INFO logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) logging.info('listening to server %s:%s', args.host, args.port) print(__doc__) try: game_loop(args) except KeyboardInterrupt: print('\nCancelled by user. Bye!') if __name__ == '__main__': main()