#!/usr/bin/env python # Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de # Barcelona (UAB). # # This work is licensed under the terms of the MIT license. # For a copy, see . # Allows visualising a 2D map generated by vehicles. """ Welcome to CARLA No Rendering Mode Visualizer I : Toggle HUD H : Hero Mode Mouse Wheel : Zoom In / Zoom Out Mouse Drag : Move Map in Map Mode ESC : quit """ # ============================================================================== # -- find carla module --------------------------------------------------------- # ============================================================================== import glob import os import sys try: sys.path.append(glob.glob('**/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 # ============================================================================== # -- imports ------------------------------------------------------------------- # ============================================================================== import carla import argparse import logging import weakref import math import random try: import pygame from pygame.locals import K_h from pygame.locals import K_i from pygame.locals import K_ESCAPE except ImportError: raise RuntimeError('cannot import pygame, make sure pygame package is installed') # ============================================================================== # -- Constants ----------------------------------------------------------------- # ============================================================================== # Colors COLOR_RED = pygame.Color(255, 0, 0) COLOR_BLUE = pygame.Color(0, 0, 255) COLOR_GREEN = pygame.Color(0, 255, 0) COLOR_YELLOW = pygame.Color(255, 255, 0) COLOR_DARK_YELLOW = pygame.Color(150, 150, 0) COLOR_MAGENTA = pygame.Color(255, 0, 255) COLOR_CYAN = pygame.Color(0, 255, 255) COLOR_WHITE = pygame.Color(255, 255, 255) COLOR_BLACK = pygame.Color(0, 0, 0) COLOR_GREY = pygame.Color(127, 127, 127) COLOR_LIGHT_GREY = pygame.Color(200, 200, 200) COLOR_DARK_GREY = pygame.Color(50, 50, 50) COLOR_ORANGE = pygame.Color(255, 127, 0) COLOR_BROWN = pygame.Color(139, 69, 19) # Legend names LEGEND_NAME = 'LEGEND' VEHICLE_NAME = 'Vehicle' TRAFFIC_LIGHT_NAME = 'Traffic Light' SPEED_LIMIT_NAME = 'Speed Limit' WALKER_NAME = 'Walker' # Module Defines MODULE_WORLD = 'WORLD' MODULE_HUD = 'HUD' MODULE_INPUT = 'INPUT' MODULE_RENDER = 'RENDER' # Input MIN_WHEEL = 0.1 MAX_WHEEL = 3.0 # ============================================================================== # -- TransformHelper ----------------------------------------------------------- # ============================================================================== class Util(object): @staticmethod def rotate_surface(img, pos, angle): w, h = img.get_size() img2 = pygame.Surface((w * 2, h * 2), pygame.SRCALPHA).convert() img2.set_clip(pygame.Rect(w - pos[0], h - pos[1], w, h)) img2.blit(img, (w - pos[0], h - pos[1])) rotated_surface = pygame.transform.rotate(img2, angle) return rotated_surface @staticmethod def normalize_vector(vector): length_vector = math.sqrt(vector[0] ** 2 + vector[1] ** 2) normalized_vector = (vector[0] / length_vector, vector[1] / length_vector) return normalized_vector @staticmethod def blits(destination_surface, source_surfaces): if hasattr(destination_surface, 'blits'): destination_surface.blits(source_surfaces) else: for surface in source_surfaces: destination_surface.blit(surface[0], surface[1]) @staticmethod def get_parallel_line_at_distance(line, unit_vector, distance): parallel_line = [(line[0][0] + unit_vector[0] * distance, line[0][1] + unit_vector[1] * distance), (line[1][0] + unit_vector[0] * distance, line[1][1] + unit_vector[1] * distance)] return parallel_line @staticmethod def get_lateral_lines_from_lane(line, distance): front_vector = (line[1][0] - line[0][0], line[1][1] - line[0][1]) left_vector = (-front_vector[1], front_vector[0]) unit_left_vector = Util.normalize_vector(left_vector) unit_right_vector = (-unit_left_vector[0], -unit_left_vector[1]) distance = distance / 2.0 # Get lateral lines lateral_left = Util.get_parallel_line_at_distance(line, unit_left_vector, distance) lateral_right = Util.get_parallel_line_at_distance(line, unit_right_vector, distance) return lateral_left, lateral_right @staticmethod def distance_between_points(p1, p2): return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2) class TransformHelper(object): def __init__(self, min_map_point, max_map_point, map_size): self.min_map_point = min_map_point self.max_map_point = max_map_point self.map_size = map_size def convert_world_to_screen_point(self, point): screen_point = (int(float(point[0] - self.min_map_point[0]) / float((self.max_map_point[0] - self.min_map_point[0])) * self.map_size), int(float(point[1] - self.min_map_point[1]) / float((self.max_map_point[1] - self.min_map_point[1])) * self.map_size)) return (max(screen_point[0], 1), max(screen_point[1], 1)) def convert_world_to_screen_line(self, line): return (self.convert_world_to_screen_point(line[0]), self.convert_world_to_screen_point(line[1])) def convert_world_to_screen_size(self, size): screen_size = (int(size[0] / float((self.max_map_point[0] - self.min_map_point[0])) * self.map_size), int(size[1] / float((self.max_map_point[1] - self.min_map_point[1])) * self.map_size)) return (max(screen_size[0], 1), max(screen_size[1], 1)) # ============================================================================== # -- Waypoint ---------------------------------------------------------------------- # ============================================================================== class Waypoint(object): def __init__(self, color, width_world, line_world, transform_helper, arrow_lines_world=None, road_id=None, lane_id=None): self.color = color self.width_world = width_world self.line_world = line_world self.transform_helper = transform_helper self.line_screen = self.transform_helper.convert_world_to_screen_line(self.line_world) self.width_screen = int( self.transform_helper.convert_world_to_screen_size( (self.width_world, self.width_world))[0]) self.arrow_lines_world = arrow_lines_world self.arrow_lines_screen = [] self.road_id = road_id self.lane_id = lane_id if self.arrow_lines_world is not None: # We add also the central line of the arrow self.arrow_lines_world.append(self.line_world) for line in self.arrow_lines_world: self.arrow_lines_screen.append(self.transform_helper.convert_world_to_screen_line(line)) def refresh_conversion_during_scale(self): self.line_screen = self.transform_helper.convert_world_to_screen_line(self.line_world) self.width_screen = int( self.transform_helper.convert_world_to_screen_size( (self.width_world, self.width_world))[0]) if self.arrow_lines_world is not None: del self.arrow_lines_screen[:] self.arrow_lines_world.append(self.line_world) for line in self.arrow_lines_world: self.arrow_lines_screen.append(self.transform_helper.convert_world_to_screen_line(line)) # ============================================================================== # -- Vehicle ---------------------------------------------------------------------- # ============================================================================== class Vehicle(object): def __init__(self, actor, color, map_transform_helper): self.actor = actor self.color = color self.map_transform_helper = map_transform_helper # Compute bounding box points bb_extent = self.actor.bounding_box.extent original_size = [bb_extent.x * 2.0, bb_extent.y * 2.0] self.surface_size = map_transform_helper.convert_world_to_screen_size(original_size) self.color = color surface = pygame.Surface((self.surface_size[0], self.surface_size[1]), pygame.SRCALPHA) surface.set_colorkey(COLOR_BLACK) pygame.draw.polygon(surface, color, [(0, 0), (self.surface_size[0], 0), (self.surface_size[0], self.surface_size[1]), (0, self.surface_size[1])]) center = (self.surface_size[0] / 2, self.surface_size[1] / 2) arrow_tip = (self.surface_size[0], self.surface_size[1] / 2) arrow_half = self.surface_size[1] / 2 + arrow_tip[0] / 2 line_0 = [center, arrow_tip] line_1 = [arrow_tip, (arrow_half - 1, 0)] line_2 = [arrow_tip, (arrow_half - 1, self.surface_size[1])] arrow_width = map_transform_helper.convert_world_to_screen_size((0.5, 0.5))[0] render_module = module_manager.get_module(MODULE_RENDER) render_module.draw_arrow(surface, COLOR_BLUE, [line_0, line_1, line_2], arrow_width) actor_location = self.actor.get_location() self.x, self.y = self.map_transform_helper.convert_world_to_screen_point((actor_location.x, actor_location.y)) self.surface = pygame.transform.rotate(surface, -self.actor.get_transform().rotation.yaw).convert() class TrafficLight(object): def __init__(self, actor, map_transform_helper): self.actor = actor self.map_transform_helper = map_transform_helper pos = self.actor.get_location() self.x, self.y = self.map_transform_helper.convert_world_to_screen_point((pos.x, pos.y)) self.color = COLOR_BLACK if actor.state == carla.libcarla.TrafficLightState.Green: self.color = COLOR_GREEN elif actor.state == carla.libcarla.TrafficLightState.Yellow: self.color = COLOR_YELLOW else: self.color = COLOR_RED # Compute bounding box points # bb_extent = self.actor.bounding_box.extent # original_size = [bb_extent.x * 2.0, bb_extent.y * 2.0] original_size = [2, 2] self.surface_size = map_transform_helper.convert_world_to_screen_size(original_size) self.surface = pygame.Surface((self.surface_size[0], self.surface_size[1]), pygame.SRCALPHA) self.surface.set_colorkey(COLOR_BLACK) pygame.draw.polygon(self.surface, self.color, [(0, 0), (self.surface_size[0], 0), (self.surface_size[0], self.surface_size[1]), (0, self.surface_size[1])]) class SpeedLimit(object): def __init__(self, actor, radius, map_transform_helper, hero_actor): self.actor = actor self.speed_limit = actor.type_id.split('.')[2] self.font = pygame.font.SysFont('Arial', radius) actor_location = actor.get_location() self.x, self.y = map_transform_helper.convert_world_to_screen_point((actor_location.x, actor_location.y)) self.surface = pygame.Surface((radius * 2, radius * 2)).convert() # Render speed limit white_circle_radius = int(radius * 0.75) pygame.draw.circle(self.surface, COLOR_RED, (radius, radius), radius) pygame.draw.circle(self.surface, COLOR_WHITE, (radius, radius), white_circle_radius) font_surface = self.font.render(self.speed_limit, False, COLOR_DARK_GREY) # Blit if hero_actor is not None: # Rotate font surface with respect to hero vehicle front angle = -hero_actor.get_transform().rotation.yaw - 90.0 font_surface = Util.rotate_surface(font_surface, (radius / 2, radius / 2), angle) font_surface.set_colorkey(COLOR_BLACK) final_offset = font_surface.get_rect(center=(radius, radius)) self.surface.blit(font_surface, final_offset) else: self.surface.blit(font_surface, (radius / 2, radius / 2)) class Walker(object): def __init__(self, actor, map_transform_helper): self.actor = actor actor_location = actor.get_location() self.x, self.y = map_transform_helper.convert_world_to_screen_point((actor_location.x, actor_location.y)) self.color = COLOR_WHITE # Compute bounding box points bb_extent = self.actor.bounding_box.extent original_size = [bb_extent.x * 2.0, bb_extent.y * 2.0] self.surface_size = map_transform_helper.convert_world_to_screen_size(original_size) self.surface = pygame.Surface((self.surface_size[0], self.surface_size[1]), pygame.SRCALPHA) self.surface.set_colorkey(COLOR_BLACK) pygame.draw.polygon(self.surface, self.color, [(0, 0), (self.surface_size[0], 0), (self.surface_size[0], self.surface_size[1]), (0, self.surface_size[1])]) # ============================================================================== # -- ModuleManager ------------------------------------------------------------- # ============================================================================== class ModuleManager(object): def __init__(self): self.modules = [] def register_module(self, module): self.modules.append(module) def clear_modules(self): del self.modules[:] def tick(self, clock): # Update all the modules for module in self.modules: module.tick(clock) def render(self, display): display.fill(COLOR_BROWN) for module in self.modules: module.render(display) def get_module(self, name): for module in self.modules: if module.name == name: return module def start_modules(self): for module in self.modules: module.start() # ============================================================================== # -- ModuleRender ------------------------------------------------------------- # ============================================================================== class ModuleRender(object): def __init__(self, name): self.name = name def start(self): pass def render(self, display): pass def tick(self, clock): pass def draw_arrow(self, surface, color, lines, arrow_width): self.draw_line(surface, color, False, lines[0], arrow_width) self.draw_line(surface, color, False, lines[1], arrow_width) self.draw_line(surface, color, False, lines[2], arrow_width) def draw_rect_from_line(self, surface, line, distance, transform_helper): lateral_left, lateral_right = Util.get_lateral_lines_from_lane(line, distance) # Convert to screen space lateral_left_screen = transform_helper.convert_world_to_screen_line(lateral_left) lateral_right_screen = transform_helper.convert_world_to_screen_line(lateral_right) pygame.draw.polygon(surface, COLOR_DARK_GREY, [lateral_left_screen[0], lateral_left_screen[1], lateral_right_screen[1], lateral_right_screen[0]]) def draw_line_for_lane(self, index, town_map, road_id, lane_id, surface, width, color, transform_helper, location, line): gen_wp = town_map.get_waypoint(location) if gen_wp is not None: is_central_line = (gen_wp.road_id == road_id and gen_wp.lane_id * lane_id < 0) is_lateral_line = (gen_wp.road_id == road_id and gen_wp.lane_id == lane_id) line_screen = transform_helper.convert_world_to_screen_line(line) if is_central_line or is_lateral_line: self.draw_line(surface, color, False, line_screen, width) else: if math.fmod(index, 3) == 0: self.draw_line(surface, COLOR_WHITE, False, line_screen, width) def draw_lateral_line_at_distance(self, index, town_map, road_id, lane_id, surface, line, distance, width, color, transform_helper): left_lateral, right_lateral = Util.get_lateral_lines_from_lane(line, distance + 0.1) left_location = carla.Location(x=left_lateral[0][0], y=left_lateral[0][1]) self.draw_line_for_lane( index, town_map, road_id, lane_id, surface, width, color, transform_helper, left_location, left_lateral) right_location = carla.Location(x=right_lateral[0][0], y=right_lateral[0][1]) self.draw_line_for_lane( index, town_map, road_id, lane_id, surface, width, color, transform_helper, right_location, right_lateral) def draw_line(self, surface, color, closed, line, width): pygame.draw.lines(surface, color, closed, line, width) def drawCircle(self, surface, x, y, radius, color): pygame.draw.circle(surface, color, (x, y), radius) # ============================================================================== # -- HUD ----------------------------------------------------------------------- # ============================================================================== class Legend(object): def __init__(self, list_keys, header_font, font): self.header_surface = header_font.render(LEGEND_NAME, True, COLOR_LIGHT_GREY) self.legend_surfaces = [] self.surface_size = 25 for key in list_keys: color_surface = pygame.Surface((self.surface_size, self.surface_size)) color_surface.fill(key[0]) font_surface = font.render(key[1], True, COLOR_LIGHT_GREY) self.legend_surfaces.append((color_surface, font_surface)) def render(self, display): h_offset = 20 v_offset = 235 h_space = 10 display.blit(self.header_surface, (8 + 100 / 2, v_offset)) for surface in self.legend_surfaces: v_offset = v_offset + surface[0].get_height() + 10 display.blit(surface[0], (h_offset, v_offset)) display.blit(surface[1], (surface[0].get_width() + h_offset + h_space, v_offset + 5)) class ModuleHUD (object): def __init__(self, name, width, height): self.name = name self._init_hud_params() self._init_data_params(width, height) def start(self): pass def _init_hud_params(self): fonts = [x for x in pygame.font.get_fonts() if 'mono' 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, 14) self._header_font = pygame.font.SysFont('Arial', 14) def _init_data_params(self, height, width): self.dim = (height, width) self.show_info = True self._info_text = {} self.legend = Legend(((COLOR_MAGENTA, VEHICLE_NAME), (COLOR_WHITE, WALKER_NAME)), self._header_font, self._font_mono) def tick(self, clock): pass def add_info(self, module_name, info): self._info_text[module_name] = info def render_actors_ids(self, vehicle_id_surface, list_actors, transform_helper, hero_actor): vehicle_id_surface.fill(COLOR_BLACK) if self.show_info: vehicle_id_surface.set_alpha(150) v_offset = 4 for actor in list_actors: location = actor.get_location() x, y = transform_helper.convert_world_to_screen_point((location.x, location.y - v_offset)) angle = 0 if hero_actor is not None: angle = -hero_actor.get_transform().rotation.yaw - 90 color_surface = pygame.Surface((len(str(actor.id)) * 8, 14)) color_surface.fill(COLOR_BLACK) color_surface.set_alpha(200) rotated_color_surface = pygame.transform.rotate(color_surface, angle) vehicle_id_surface.blit(rotated_color_surface, (x, y)) font_surface = self._font_mono.render(str(actor.id), True, COLOR_WHITE) font_surface.set_colorkey(COLOR_BLACK) font_surface.set_alpha(255) rotated_font_surface = pygame.transform.rotate(font_surface, angle).convert_alpha() vehicle_id_surface.blit(rotated_font_surface, (x, y)) return vehicle_id_surface def render(self, display): if self.show_info: info_surface = pygame.Surface((240, self.dim[1])) info_surface.set_alpha(100) display.blit(info_surface, (0, 0)) v_offset = 4 bar_h_offset = 100 bar_width = 106 i = 0 for module_name, module_info in self._info_text.items(): surface = self._header_font.render(module_name, True, COLOR_LIGHT_GREY).convert_alpha() display.blit(surface, (8 + bar_width / 2, 18 * i + v_offset)) i += 1 for item in module_info: 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, COLOR_WHITE, 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, COLOR_WHITE, 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, COLOR_WHITE, rect) item = item[0] if item: # At this point has to be a str. surface = self._font_mono.render(item, True, COLOR_WHITE).convert_alpha() display.blit(surface, (8, 18 * i + v_offset)) v_offset += 18 self.legend.render(display) # ============================================================================== # -- World --------------------------------------------------------------------- # ============================================================================== class ModuleWorld(object): def __init__(self, name, host, port, timeout): self.client = None self.name = name self.host = host self.port = port self.timeout = timeout self.server_fps = 0.0 self.server_clock = pygame.time.Clock() # World data self.world = None self.town_map = None self.actors = None # Store necessary modules self.hud_module = None self.module_input = None self.render_module = None self.surface_size = [0, 0] self.prev_scaled_size = 0 self.scaled_size = 0 # Hero actor self.hero_actor = None self.filter_radius = 50 self.map_rendered = False self.accum_offset = [0, 0] self.scale_offset = [0, 0] self.map_surface = None self.vehicles_surface = None self.traffic_light_surface = None self.speed_limits_surface = None self.walkers_surface = None self.hero_actor_surface = None self.vehicle_id_surface = None self.result_surface = None self.waypoint_length = 1.5 self.map_waypoints = None self.road_render_data_list = [] self.intersection_render_data_list = [] # Map Bounding box self.x_min = 0.0 self.y_min = 0.0 self.x_max = 0.0 self.y_max = 0.0 # Transform helper self.transform_helper = None def _get_data_from_carla(self, host, port, timeout): try: self.client = carla.Client(host, port) self.client.set_timeout(timeout) world = self.client.get_world() town_map = world.get_map() actors = world.get_actors() return (world, town_map, actors) except Exception as ex: logging.error(ex) exit_game() def _create_world_surfaces(self): self.map_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.vehicles_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.vehicles_surface.set_colorkey((0, 0, 0)) self.traffic_light_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.traffic_light_surface.set_colorkey((0, 0, 0)) self.speed_limits_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.speed_limits_surface.set_colorkey((0, 0, 0)) self.walkers_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.walkers_surface.set_colorkey((0, 0, 0)) self.hero_actor_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.hero_actor_surface.set_colorkey((0, 0, 0)) self.vehicle_id_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.vehicle_id_surface.set_colorkey((0, 0, 0)) self.result_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() self.result_surface.set_colorkey((0, 0, 0)) def _compute_map_bounding_box(self, map_waypoints): x_min = float('inf') y_min = float('inf') x_max = 0 y_max = 0 for waypoint in map_waypoints: x_max = max(x_max, waypoint.transform.location.x) x_min = min(x_min, waypoint.transform.location.x) y_max = max(y_max, waypoint.transform.location.y) y_min = min(y_min, waypoint.transform.location.y) return (x_min, y_min, x_max, y_max) def prepare_waypoints_data(self): # compute bounding boxes self.x_min, self.y_min, self.x_max, self.y_max = self._compute_map_bounding_box(self.map_waypoints) # Feed map bounding box and surface size to transform helper shrink_map_factor = 1.02 self.transform_helper = TransformHelper( (self.x_min * shrink_map_factor, self.y_min * shrink_map_factor), (self.x_max * shrink_map_factor, self.y_max * shrink_map_factor), self.surface_size) # Retrieve data from waypoints orientation, width and length and do conversions into another list del self.road_render_data_list[:] del self.intersection_render_data_list[:] for waypoint in self.map_waypoints: # Waypoint front wf = waypoint.transform.get_forward_vector() wp_0 = (waypoint.transform.location.x, waypoint.transform.location.y) wp_1 = (wp_0[0] + wf.x * self.waypoint_length, wp_0[1] + wf.y * self.waypoint_length) wp_half = (wp_0[0] + wf.x * self.waypoint_length / 2, wp_0[1] + wf.y * self.waypoint_length / 2) # Orientation of road if waypoint.is_intersection: intersection_render_data = Waypoint( COLOR_DARK_GREY, waypoint.lane_width, (wp_0, wp_1), self.transform_helper) self.intersection_render_data_list.append(intersection_render_data) else: # Get arrow lines wl = (-wf.y, wf.x) line_0 = [wp_1, (wp_half[0] + wl[0] * self.waypoint_length / 2, wp_half[1] + wl[1] * self.waypoint_length / 2)] line_1 = [wp_1, (wp_half[0] - wl[0] * self.waypoint_length / 2, wp_half[1] - wl[1] * self.waypoint_length / 2)] arrow_lines = [line_0, line_1] road_render_data = Waypoint( COLOR_DARK_GREY, waypoint.lane_width, (wp_0, wp_1), self.transform_helper, arrow_lines, waypoint.road_id, waypoint.lane_id) self.road_render_data_list.append(road_render_data) def start(self): self.world, self.town_map, self.actors = self._get_data_from_carla(self.host, self.port, self.timeout) # Store necessary modules self.hud_module = module_manager.get_module(MODULE_HUD) self.module_input = module_manager.get_module(MODULE_INPUT) self.surface_size = min(self.hud_module.dim[0], self.hud_module.dim[1]) self.prev_scaled_size = int(self.surface_size) self._create_world_surfaces() # Generate waypoints self.map_waypoints = self.town_map.generate_waypoints(self.waypoint_length) self.prepare_waypoints_data() # Module render self.render_module = module_manager.get_module(MODULE_RENDER) weak_self = weakref.ref(self) self.world.on_tick(lambda timestamp: ModuleWorld.on_world_tick(weak_self, timestamp)) def select_random_hero(self): hero_vehicles = [ actor for actor in self.actors if 'vehicle' in actor.type_id and actor.attributes['role_name'] == 'hero'] if len(hero_vehicles) > 0: self.hero_actor = random.choice(hero_vehicles) else: print("There are no hero vehicles spawned") def tick(self, clock): self.update_hud_info(clock) def update_hud_info(self, clock): hero_mode_text = [] if self.hero_actor is not None: vehicle_name, vehicle_brand, vehicle_model = self.hero_actor.type_id.split('.') type_id_text = vehicle_brand + ' ' + vehicle_model hero_speed = self.hero_actor.get_velocity() hero_speed_text = 3.6 * math.sqrt(hero_speed.x ** 2 + hero_speed.y ** 2 + hero_speed.z ** 2) state = self.hero_actor.get_traffic_light_state() affected_traffic_light = 'None' if state == carla.libcarla.TrafficLightState.Green: affected_traffic_light = 'GREEN' elif state == carla.libcarla.TrafficLightState.Yellow: affected_traffic_light = 'YELLOW' else: affected_traffic_light = 'RED' affected_speed_limit = self.hero_actor.get_speed_limit() hero_mode_text = [ 'Hero Mode: ON', 'Hero ID: %4d' % self.hero_actor.id, 'Hero Type ID:%12s' % type_id_text, 'Hero speed: %3d km/h' % hero_speed_text, 'Hero Affected by:', ' Traffic Light:%12s' % affected_traffic_light, ' Speed Limit: %3d km/h' % affected_speed_limit ] else: hero_mode_text = ['Hero Mode: OFF'] self.server_fps = self.server_clock.get_fps() module_info_text = [ 'Server: % 16.0f FPS' % self.server_fps, 'Client: % 16.0f FPS' % clock.get_fps(), 'Map Name: %10s' % self.world.map_name, ] module_info_text = module_info_text + hero_mode_text module_hud = module_manager.get_module(MODULE_HUD) module_hud.add_info(self.name, module_info_text) @staticmethod def on_world_tick(weak_self, timestamp): self = weak_self() if not self: return self.server_clock.tick() self.server_fps = self.server_clock.get_fps() self.world = self.client.get_world() self.actors = self.world.get_actors() def render_map(self, map_surface): map_surface.fill(COLOR_GREY) i = 0 # Draw Roads for road_render_data in self.road_render_data_list: road_render_data.refresh_conversion_during_scale() self.render_module.draw_rect_from_line(map_surface, road_render_data.line_world, road_render_data.width_world, self.transform_helper) border_line_width = self.transform_helper.convert_world_to_screen_size((0.3, 0.3))[0] self.render_module.drawCircle(map_surface, road_render_data.line_screen[0][0], road_render_data.line_screen[0][1], int(road_render_data.width_screen / 2), road_render_data.color) self.render_module.drawCircle(map_surface, road_render_data.line_screen[0][0], road_render_data.line_screen[0][1], int(road_render_data.width_screen / 2), road_render_data.color) self.render_module.draw_lateral_line_at_distance(i, self.town_map, road_render_data.road_id, road_render_data.lane_id, map_surface, road_render_data.line_world, road_render_data.width_world, border_line_width, COLOR_DARK_YELLOW, self.transform_helper) i = i + 1 # Draw Intersections for intersection_render_data in self.intersection_render_data_list: intersection_render_data.refresh_conversion_during_scale() self.render_module.draw_line(map_surface, intersection_render_data.color, False, intersection_render_data.line_screen, intersection_render_data.width_screen) self.render_module.drawCircle(map_surface, intersection_render_data.line_screen[0][0], intersection_render_data.line_screen[0][1], int(intersection_render_data.width_screen / 2), intersection_render_data.color) self.render_module.drawCircle(map_surface, intersection_render_data.line_screen[1][0], intersection_render_data.line_screen[1][1], int(intersection_render_data.width_screen / 2), intersection_render_data.color) # Draw Arrows for road orientation i = 0 for road_render_data in self.road_render_data_list: if math.fmod(i, 17) == 0: self.render_module.draw_arrow(map_surface, COLOR_CYAN, road_render_data.arrow_lines_screen, 1) i = i + 1 def render_hero_actor(self, translation_offset): self.hero_actor_surface.set_alpha(100) hero_diameter_screen = self.transform_helper.convert_world_to_screen_size( (self.filter_radius * 2.0, self.filter_radius * 2.0))[0] self.render_module.drawCircle(self.hero_actor_surface, translation_offset[0], translation_offset[1], int(hero_diameter_screen / 2), COLOR_ORANGE) def is_actor_inside_hero_radius(self, actor): actor_location = actor.get_location() hero_location = self.hero_actor.get_location() return Util.distance_between_points([actor_location.x, actor_location.y], [ hero_location.x, hero_location.y]) <= self.filter_radius def _split_actors(self, actors): vehicles = [] traffic_lights = [] speed_limits = [] walkers = [] for actor in actors: if 'vehicle' in actor.type_id: vehicles.append(actor) elif 'traffic_light' in actor.type_id: traffic_lights.append(actor) elif 'speed_limit' in actor.type_id: speed_limits.append(actor) elif 'walker' in actor.type_id: walkers.append(actor) return (vehicles, traffic_lights, speed_limits, walkers) def refresh_surface(self, surface, size): surface.fill(COLOR_BLACK) new_surface = pygame.Surface(size).convert() new_surface.set_colorkey(COLOR_BLACK) return new_surface def render_actors(self, vehicles, traffic_lights, speed_limits, walkers): # Render Vehicles vehicle_renderer = [] for actor in vehicles: vehicle = Vehicle(actor, COLOR_MAGENTA, self.transform_helper) vehicle_renderer.append((vehicle.surface, (vehicle.x, vehicle.y))) Util.blits(self.vehicles_surface, vehicle_renderer) # Render Traffic Lights traffic_lights_renderer = [] for actor in traffic_lights: traffic_light = TrafficLight(actor, self.transform_helper) traffic_lights_renderer.append((traffic_light.surface, (traffic_light.x, traffic_light.y))) Util.blits(self.traffic_light_surface, traffic_lights_renderer) # Render Speed limit speed_limit_renderer = [] speed_limit_width = self.transform_helper.convert_world_to_screen_size((3, 3))[0] for actor in speed_limits: speed_limit = SpeedLimit(actor, speed_limit_width, self.transform_helper, self.hero_actor) speed_limit_renderer.append((speed_limit.surface, (speed_limit.x, speed_limit.y))) Util.blits(self.speed_limits_surface, speed_limit_renderer) # Render Walkers walkers_renderer = [] for actor in walkers: walker = Walker(actor, self.transform_helper) walkers_renderer.append((walker.surface, (walker.x, walker.y))) Util.blits(self.walkers_surface, walkers_renderer) def clip_surfaces(self, clipping_rect): self.map_surface.set_clip(clipping_rect) self.vehicles_surface.set_clip(clipping_rect) self.traffic_light_surface.set_clip(clipping_rect) self.speed_limits_surface.set_clip(clipping_rect) self.walkers_surface.set_clip(clipping_rect) self.vehicle_id_surface.set_clip(clipping_rect) self.hero_actor_surface.set_clip(clipping_rect) self.result_surface.set_clip(clipping_rect) def render(self, display): if not self.map_rendered: self.render_map(self.map_surface) self.map_rendered = True self.vehicles_surface.fill(COLOR_BLACK) self.traffic_light_surface.fill(COLOR_BLACK) self.speed_limits_surface.fill(COLOR_BLACK) self.walkers_surface.fill(COLOR_BLACK) self.hero_actor_surface.fill(COLOR_BLACK) vehicles, traffic_lights, speed_limits, walkers = self._split_actors(self.actors) if self.hero_actor is not None: vehicles = [vehicle for vehicle in vehicles if self.is_actor_inside_hero_radius(vehicle)] traffic_lights = [traffic_light for traffic_light in traffic_lights if self.is_actor_inside_hero_radius(traffic_light)] speed_limits = [speed_limit for speed_limit in speed_limits if self.is_actor_inside_hero_radius(speed_limit)] scale_factor = self.module_input.wheel_offset self.scaled_size = int(self.surface_size * scale_factor) # Scale surfaces if needed if self.scaled_size != self.prev_scaled_size: m = self.module_input.mouse_pos # Percentage of surface where mouse position is actually px = (m[0] - self.accum_offset[0]) / float(self.prev_scaled_size) py = (m[1] - self.accum_offset[1]) / float(self.prev_scaled_size) # Offset will be the previously accumulated offset added with the # difference of mouse positions in the old and new scales diff_between_scales = ((float(self.prev_scaled_size) * px) - (float(self.scaled_size) * px), (float(self.prev_scaled_size) * py) - (float(self.scaled_size) * py)) self.scale_offset = (self.accum_offset[0] + diff_between_scales[0], self.accum_offset[1] + diff_between_scales[1]) # Accumulate offset self.accum_offset = (self.accum_offset[0] + diff_between_scales[0], self.accum_offset[1] + diff_between_scales[1]) # Update previous scale self.prev_scaled_size = self.scaled_size # Scale performed self.transform_helper.map_size = self.scaled_size self.map_surface.fill(COLOR_BLACK) new_map_size = (self.transform_helper.map_size, self.transform_helper.map_size) new_map_surface = pygame.Surface(new_map_size).convert() self.render_map(new_map_surface) self.map_surface = new_map_surface self.vehicles_surface = self.refresh_surface(self.vehicles_surface, new_map_size).convert() self.traffic_light_surface = self.refresh_surface(self.traffic_light_surface, new_map_size).convert() self.speed_limits_surface = self.refresh_surface(self.speed_limits_surface, new_map_size).convert() self.walkers_surface = self.refresh_surface(self.walkers_surface, new_map_size).convert() self.vehicle_id_surface = self.refresh_surface(self.vehicle_id_surface, new_map_size).convert() self.hero_actor_surface = self.refresh_surface(self.hero_actor_surface, new_map_size).convert() self.result_surface = self.refresh_surface(self.result_surface, new_map_size).convert() # Render Vehicles self.render_actors(vehicles, traffic_lights, speed_limits, walkers) angle = 0 center_offset = (0, 0) # Translation offset if self.hero_actor is None: translation_offset = ((self.module_input.mouse_offset[0]) * scale_factor + self.scale_offset[0], self.module_input.mouse_offset[1] * scale_factor + self.scale_offset[1]) center_offset = ((display.get_width() - self.surface_size) / 2 * scale_factor, 0) else: hero_location = (self.hero_actor.get_location().x, self.hero_actor.get_location().y) hero_location_screen = self.transform_helper.convert_world_to_screen_point(hero_location) translation_offset = (-hero_location_screen[0], (-hero_location_screen[1])) selected_hero_actor = [vehicle for vehicle in vehicles if vehicle.id == self.hero_actor.id] if len(selected_hero_actor) != 0: self.render_hero_actor(hero_location_screen) angle = self.hero_actor.get_transform().rotation.yaw + 90.0 center_offset = (display.get_width() / 2, display.get_height() / 2) else: self.hero_actor = None # Blit surfaces surfaces = ((self.map_surface, (0, 0)), (self.vehicles_surface, (0, 0)), (self.traffic_light_surface, (0, 0)), (self.speed_limits_surface, (0, 0)), (self.walkers_surface, (0, 0)), (self.vehicle_id_surface, (0, 0)), (self.hero_actor_surface, (0, 0)) ) self.hud_module.render_actors_ids(self.vehicle_id_surface, vehicles, self.transform_helper, self.hero_actor) rotated_result_surface = self.result_surface if self.hero_actor is not None: hero_surface = pygame.Surface((self.surface_size, self.surface_size), pygame.SRCALPHA) # Apply clipping rect clipping_rect = pygame.Rect(-translation_offset[0] - hero_surface.get_width() / 2, -translation_offset[1] - hero_surface.get_height() / 2, self.hud_module.dim[0], self.hud_module.dim[1]) self.clip_surfaces(clipping_rect) Util.blits(self.result_surface, surfaces) hero_surface.fill(COLOR_BROWN) hero_surface.blit(self.result_surface, (translation_offset[0] + hero_surface.get_width() / 2, translation_offset[1] + hero_surface.get_height() / 2)) rotated_result_surface = Util.rotate_surface(hero_surface, (hero_surface.get_width() / 2, hero_surface.get_height() / 2), angle) final_offset = rotated_result_surface.get_rect(center=center_offset) display.blit(rotated_result_surface, final_offset) else: # Apply clipping rect clipping_rect = pygame.Rect(-translation_offset[0] - center_offset[0], -translation_offset[1], self.hud_module.dim[0], self.hud_module.dim[1]) self.clip_surfaces(clipping_rect) Util.blits(self.result_surface, surfaces) display.blit(rotated_result_surface, (translation_offset[0] + center_offset[0], translation_offset[1])) del vehicles[:] del traffic_lights[:] del speed_limits[:] del walkers[:] # ============================================================================== # -- Input ----------------------------------------------------------- # ============================================================================== class ModuleInput(object): def __init__(self, name): self.name = name self.mouse_pos = (0, 0) self.mouse_offset = [0.0, 0.0] self.wheel_offset = 1.0 self.wheel_amount = 0.1 def start(self): pass def render(self, display): pass def tick(self, clock): self.parse_input() def _parse_events(self): self.mouse_pos = pygame.mouse.get_pos() for event in pygame.event.get(): if event.type == pygame.QUIT: exit_game() elif event.type == pygame.KEYUP: if event.key == K_ESCAPE: exit_game() if event.key == K_h: module_world = module_manager.get_module(MODULE_WORLD) if module_world.hero_actor is None: module_world.select_random_hero() else: module_world.hero_actor = None if event.key == K_i: module_hud = module_manager.get_module(MODULE_HUD) module_hud.show_info = not module_hud.show_info elif event.type == pygame.MOUSEBUTTONDOWN: if event.button == 4: self.wheel_offset += self.wheel_amount if self.wheel_offset >= MAX_WHEEL: self.wheel_offset = MAX_WHEEL if event.button == 5: self.wheel_offset -= self.wheel_amount if self.wheel_offset <= MIN_WHEEL: self.wheel_offset = MIN_WHEEL def _parse_mouse(self): if pygame.mouse.get_pressed()[0]: x, y = pygame.mouse.get_pos() self.mouse_offset[0] = self.mouse_offset[0] + x - self.mouse_pos[0] self.mouse_offset[1] += y - self.mouse_pos[1] self.mouse_pos = (x, y) def parse_input(self): self._parse_events() self._parse_mouse() # ============================================================================== # -- Global Objects ------------------------------------------------------------ # ============================================================================== module_manager = ModuleManager() # ============================================================================== # -- Game Loop --------------------------------------------------------------- # ============================================================================== def game_loop(args): # Init Pygame pygame.init() display = pygame.display.set_mode( (args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF) pygame.display.set_caption(args.description) # Init modules input_module = ModuleInput(MODULE_INPUT) hud_module = ModuleHUD(MODULE_HUD, args.width, args.height) world_module = ModuleWorld(MODULE_WORLD, args.host, args.port, 2.0) render_module = ModuleRender(MODULE_RENDER) # Register Modules module_manager.register_module(input_module) module_manager.register_module(render_module) module_manager.register_module(world_module) module_manager.register_module(hud_module) module_manager.start_modules() clock = pygame.time.Clock() while True: clock.tick_busy_loop(120) module_manager.tick(clock) module_manager.render(display) pygame.display.flip() def exit_game(): module_manager.clear_modules() pygame.quit() sys.exit() # ============================================================================== # -- Main -------------------------------------------------------------------- # ============================================================================== def main(): # Parse arguments argparser = argparse.ArgumentParser( description='CARLA No Rendering Mode Visualizer') 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)') args = argparser.parse_args() args.description = argparser.description 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()