diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 872141d..fa4ee21 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -167,14 +167,12 @@ def run_scenario( agent_class_name = self.module_aw_agent.__name__.title().replace("_", "") try: print(getattr(self.module_aw_agent, agent_class_name)) + # call the agent method to notify the bridge of the ego spawn + # this will loop until the bridge is ready self.aw_agent = getattr(self.module_aw_agent, agent_class_name)( env_config ) route_config.agent = self.aw_agent - - # call the agent method to notify the bridge of the ego spawn - # only continue once state changes! - except Exception as e: # Forces the simulation to run synchronously # pylint: disable=broad-except traceback.print_exc() print("Could not setup required agent due to {}".format(e)) @@ -182,7 +180,11 @@ def run_scenario( return False # only set synchronous mode once bridge is ready - # tick synchronously until then + # tick asynchronously until then + settings = CarlaDataProvider.get_world().get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = self._carla_config["fixed_delta_seconds"] + CarlaDataProvider.get_world().apply_settings(settings) # ADD TRAFFIC MANAGER SEED TO CONFIG tm_port = int(self._tm_config["port"]) # type: ignore diff --git a/config.yaml b/config.yaml index f6bf1cb..88bc9d5 100644 --- a/config.yaml +++ b/config.yaml @@ -17,3 +17,6 @@ scenario_runner: debug: false route_id: 0 in_docker: true # used for dev +algorithm: + iterations: 1000 + hyperparmaters: # will be passed into the algorithm class diff --git a/docker/autoware_msgs.tar b/docker/autoware_msgs.tar index 79cd6e2..bec9450 100644 Binary files a/docker/autoware_msgs.tar and b/docker/autoware_msgs.tar differ diff --git a/manual_control.py b/manual_control.py deleted file mode 100755 index 9a8e623..0000000 --- a/manual_control.py +++ /dev/null @@ -1,999 +0,0 @@ -#!/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 controlling a vehicle with a keyboard. For a simpler and more -# documented example, please take a look at tutorial.py. - -""" -Welcome to CARLA manual control. - -Use ARROWS or WASD keys for control. - - W : throttle - S : brake - A/D : steer left/right - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - - L : toggle next light type - SHIFT + L : toggle high beam - Z/X : toggle right/left blinker - I : toggle interior light - - TAB : change camera position - - R : toggle recording images to disk - - CTRL + R : toggle recording of simulation (replacing any previous) - CTRL + P : start replaying last recorded simulation - CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) - CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) - - F1 : toggle HUD - H/? : toggle help - ESC : quit -""" - -from __future__ import print_function - -# ============================================================================== -# -- imports ------------------------------------------------------------------- -# ============================================================================== - -import carla -from carla import ColorConverter as cc - -import argparse -import os -import sys -import time -import collections -import datetime -import logging -import math -import weakref - -try: - import pygame - from pygame.locals import K_ESCAPE - from pygame.locals import K_F1 - from pygame.locals import KMOD_CTRL - from pygame.locals import KMOD_SHIFT - from pygame.locals import K_TAB - from pygame.locals import K_SPACE - from pygame.locals import K_UP - from pygame.locals import K_DOWN - from pygame.locals import K_LEFT - from pygame.locals import K_RIGHT - from pygame.locals import K_w - from pygame.locals import K_a - from pygame.locals import K_s - from pygame.locals import K_d - from pygame.locals import K_q - from pygame.locals import K_m - from pygame.locals import K_COMMA - from pygame.locals import K_PERIOD - from pygame.locals import K_p - from pygame.locals import K_i - from pygame.locals import K_l - from pygame.locals import K_z - from pygame.locals import K_x - from pygame.locals import K_r - from pygame.locals import K_MINUS - from pygame.locals import K_EQUALS -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') - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - -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, args): - self.world = carla_world - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.imu_sensor = None - self.radar_sensor = None - self.camera_manager = None - self.restart(args) - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - - def restart(self, args): - - self.player_max_speed = 1.589 - self.player_max_speed_fast = 3.713 - - # 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 the ego vehicle - while self.player is None: - print("Waiting for the ego vehicle...") - time.sleep(1) - possible_vehicles = self.world.get_actors().filter('vehicle.*') - for vehicle in possible_vehicles: - if vehicle.attributes['role_name'] == args.rolename: - print("Ego vehicle found") - self.player = vehicle - break - - self.player_name = self.player.type_id - - # 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.imu_sensor = IMUSensor(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) - - self.world.wait_for_tick() - - def tick(self, clock, wait_for_repetitions): - if len(self.world.get_actors().filter(self.player_name)) < 1: - if not wait_for_repetitions: - return False - else: - self.player = None - self.destroy() - self.restart() - - self.hud.tick(self, clock) - return True - - 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): - sensors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.imu_sensor.sensor] - for sensor in sensors: - if sensor is not None: - sensor.stop() - sensor.destroy() - if self.player is not None: - self.player.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - """Class that handles keyboard input.""" - def __init__(self, world, start_in_autopilot): - self._autopilot_enabled = start_in_autopilot - self._control = carla.VehicleControl() - self._lights = carla.VehicleLightState.NONE - self._steer_cache = 0.0 - world.player.set_autopilot(self._autopilot_enabled) - world.player.set_light_state(self._lights) - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - - def parse_events(self, client, world, clock): - current_lights = self._lights - 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 - elif event.key == K_F1: - world.hud.toggle_info() - elif event.key == K_TAB: - world.camera_manager.toggle_camera() - elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): - world.camera_manager.toggle_recording() - elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): - if (world.recording_enabled): - client.stop_recorder() - world.recording_enabled = False - world.hud.notification("Recorder is OFF") - else: - client.start_recorder("manual_recording.rec") - world.recording_enabled = True - world.hud.notification("Recorder is ON") - elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): - # stop recorder - client.stop_recorder() - world.recording_enabled = False - # work around to fix camera at start of replaying - current_index = world.camera_manager.index - world.destroy_sensors() - # disable autopilot - self._autopilot_enabled = False - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification("Replaying file 'manual_recording.rec'") - # replayer - client.replay_file("manual_recording.rec", world.recording_start, 0, 0) - world.camera_manager.set_sensor(current_index) - elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start -= 10 - else: - world.recording_start -= 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start += 10 - else: - world.recording_start += 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - elif event.key == K_q: - self._control.gear = 1 if self._control.reverse else -1 - elif event.key == K_m: - self._control.manual_gear_shift = not self._control.manual_gear_shift - self._control.gear = world.player.get_control().gear - world.hud.notification('%s Transmission' % - ('Manual' if self._control.manual_gear_shift else 'Automatic')) - elif self._control.manual_gear_shift and event.key == K_COMMA: - self._control.gear = max(-1, self._control.gear - 1) - elif self._control.manual_gear_shift and event.key == K_PERIOD: - self._control.gear = self._control.gear + 1 - elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL: - self._autopilot_enabled = not self._autopilot_enabled - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification( - 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) - elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: - current_lights ^= carla.VehicleLightState.Special1 - elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: - current_lights ^= carla.VehicleLightState.HighBeam - elif event.key == K_l: - # Use 'L' key to switch between lights: - # closed -> position -> low beam -> fog - if not self._lights & carla.VehicleLightState.Position: - world.hud.notification("Position lights") - current_lights |= carla.VehicleLightState.Position - else: - world.hud.notification("Low beam lights") - current_lights |= carla.VehicleLightState.LowBeam - if self._lights & carla.VehicleLightState.LowBeam: - world.hud.notification("Fog lights") - current_lights |= carla.VehicleLightState.Fog - if self._lights & carla.VehicleLightState.Fog: - world.hud.notification("Lights off") - current_lights ^= carla.VehicleLightState.Position - current_lights ^= carla.VehicleLightState.LowBeam - current_lights ^= carla.VehicleLightState.Fog - elif event.key == K_i: - current_lights ^= carla.VehicleLightState.Interior - elif event.key == K_z: - current_lights ^= carla.VehicleLightState.LeftBlinker - elif event.key == K_x: - current_lights ^= carla.VehicleLightState.RightBlinker - - if not self._autopilot_enabled: - self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) - self._control.reverse = self._control.gear < 0 - # Set automatic control-related vehicle lights - if self._control.brake: - current_lights |= carla.VehicleLightState.Brake - else: # Remove the Brake flag - current_lights &= ~carla.VehicleLightState.Brake - if self._control.reverse: - current_lights |= carla.VehicleLightState.Reverse - else: # Remove the Reverse flag - current_lights &= ~carla.VehicleLightState.Reverse - if current_lights != self._lights: # Change the light state only if necessary - self._lights = current_lights - world.player.set_light_state(carla.VehicleLightState(self._lights)) - world.player.apply_control(self._control) - - def _parse_vehicle_keys(self, keys, milliseconds): - if keys[K_UP] or keys[K_w]: - self._control.throttle = min(self._control.throttle + 0.1, 1.00) - else: - self._control.throttle = 0.0 - - if keys[K_DOWN] or keys[K_s]: - self._control.brake = min(self._control.brake + 0.2, 1) - else: - self._control.brake = 0 - - steer_increment = 5e-4 * milliseconds - if keys[K_LEFT] or keys[K_a]: - if self._steer_cache > 0: - self._steer_cache = 0 - else: - self._steer_cache -= steer_increment - elif keys[K_RIGHT] or keys[K_d]: - if self._steer_cache < 0: - self._steer_cache = 0 - else: - self._steer_cache += steer_increment - else: - self._steer_cache = 0.0 - self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) - self._control.steer = round(self._steer_cache, 1) - self._control.hand_brake = keys[K_SPACE] - - @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, 16), 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() - compass = world.imu_sensor.compass - heading = 'N' if compass > 270.5 or compass < 89.5 else '' - heading += 'S' if 90.5 < compass < 269.5 else '' - heading += 'E' if 0.5 < compass < 179.5 else '' - heading += 'W' if 180.5 < compass < 359.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.split('/')[-1], - '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'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), - 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), - 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), - '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, - ''] - 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)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - if len(vehicles) > 1: - self._info_text += ['Nearby vehicles:'] - distance = lambda l: 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, key=lambda vehicles: vehicles[0]): - 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): - """Helper class to handle text output using pygame""" - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.line_space = 18 - self.dim = (780, len(lines) * self.line_space + 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 * self.line_space)) - 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 - - # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor - if parent_actor.type_id.startswith("vehicle."): - 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 - - -# ============================================================================== -# -- IMUSensor ----------------------------------------------------------------- -# ============================================================================== - - -class IMUSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.accelerometer = (0.0, 0.0, 0.0) - self.gyroscope = (0.0, 0.0, 0.0) - self.compass = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.imu') - 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 sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) - - @staticmethod - def _IMU_callback(weak_self, sensor_data): - self = weak_self() - if not self: - return - limits = (-99.9, 99.9) - self.accelerometer = ( - max(limits[0], min(limits[1], sensor_data.accelerometer.x)), - max(limits[0], min(limits[1], sensor_data.accelerometer.y)), - max(limits[0], min(limits[1], sensor_data.accelerometer.z))) - self.gyroscope = ( - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) - self.compass = math.degrees(sensor_data.compass) - - -# ============================================================================== -# -- RadarSensor --------------------------------------------------------------- -# ============================================================================== - - -class RadarSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - bound_x = 0.5 + self._parent.bounding_box.extent.x - bound_y = 0.5 + self._parent.bounding_box.extent.y - bound_z = 0.5 + self._parent.bounding_box.extent.z - - self.velocity_range = 7.5 # m/s - world = self._parent.get_world() - self.debug = world.debug - bp = world.get_blueprint_library().find('sensor.other.radar') - bp.set_attribute('horizontal_fov', str(35)) - bp.set_attribute('vertical_fov', str(20)) - self.sensor = world.spawn_actor( - bp, - carla.Transform( - carla.Location(x=bound_x + 0.05, z=bound_z+0.05), - carla.Rotation(pitch=5)), - attach_to=self._parent) - # We need a weak reference to self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen( - lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) - - @staticmethod - def _Radar_callback(weak_self, radar_data): - self = weak_self() - if not self: - return - # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: - # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) - # points = np.reshape(points, (len(radar_data), 4)) - - current_rot = radar_data.transform.rotation - for detect in radar_data: - azi = math.degrees(detect.azimuth) - alt = math.degrees(detect.altitude) - # The 0.25 adjusts a bit the distance so the dots can - # be properly seen - fw_vec = carla.Vector3D(x=detect.depth - 0.25) - carla.Transform( - carla.Location(), - carla.Rotation( - pitch=current_rot.pitch + alt, - yaw=current_rot.yaw + azi, - roll=current_rot.roll)).transform(fw_vec) - - def clamp(min_v, max_v, value): - return max(min_v, min(value, max_v)) - - norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] - r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) - g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) - b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) - self.debug.draw_point( - radar_data.transform.location + fw_vec, - size=0.075, - life_time=0.06, - persistent_lines=False, - color=carla.Color(r, g, b)) - -# ============================================================================== -# -- 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 - bound_x = 0.5 + self._parent.bounding_box.extent.x - bound_y = 0.5 + self._parent.bounding_box.extent.y - bound_z = 0.5 + self._parent.bounding_box.extent.z - Attachment = carla.AttachmentType - - self._camera_transforms = [ - (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), - (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] - - self.transform_index = 1 - self.sensors = [['sensor.camera.rgb', cc.Raw, 'Camera RGB']] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - bp = bp_library.find(item[0]) - bp.set_attribute('image_size_x', str(hud.dim[0])) - bp.set_attribute('image_size_y', str(hud.dim[1])) - bp.set_attribute('gamma', '2.2') - item.append(bp) - self.index = None - - def toggle_camera(self): - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - index = index % len(self.sensors) - needs_respawn = True if self.index is None else \ - (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) - 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][0], - attach_to=self._parent, - attachment_type=self._camera_transforms[self.transform_index][1]) - # 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 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] / 4), 4)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) - 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), dtype=np.uint8) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): - # Example of converting the raw_data from a carla.DVSEventArray - # sensor into a NumPy array and using it as an image - dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ - ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) - dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) - # Blue is positive, red is negative - dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 - self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) - elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): - image = image.get_color_coded_flow() - 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)) - 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(20.0) - sim_world = client.get_world() - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - display.fill((0,0,0)) - pygame.display.flip() - - hud = HUD(args.width, args.height) - world = World(client.get_world(), hud, args) - controller = KeyboardControl(world, args.autopilot) - - sim_world.wait_for_tick() - - clock = pygame.time.Clock() - while True: - clock.tick_busy_loop(60) - if controller.parse_events(client, world, clock): - return - if not world.tick(clock, args.wait_for_repetitions): - return - world.render(display) - pygame.display.flip() - - finally: - - if (world and world.recording_enabled): - client.stop_recorder() - - if world is not None: - # prevent destruction of ego vehicle - if args.keep_ego_vehicle: - world.player = 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( - '-a', '--autopilot', - action='store_true', - help='enable autopilot. This does not autocomplete the scenario') - argparser.add_argument( - '--rolename', - metavar='NAME', - default='hero', - help='role name of ego vehicle to control (default: "hero")') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='window resolution (default: 1280x720)') - argparser.add_argument( - '--keep_ego_vehicle', - action='store_true', - help='do not destroy ego vehicle on exit') - argparser.add_argument( - '--wait-for-repetitions', - action='store_true', - help='Avoids stopping the manual control when the scenario ends.') - 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!') - except Exception as error: - logging.exception(error) - - -if __name__ == '__main__': - - main() diff --git a/no_rendering_mode.py b/no_rendering_mode.py deleted file mode 100755 index ec24d1d..0000000 --- a/no_rendering_mode.py +++ /dev/null @@ -1,1494 +0,0 @@ -#!/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 - - TAB : toggle hero mode - Mouse Wheel : zoom in / zoom out - Mouse Drag : move map (map mode only) - - W : throttle - S : brake - AD : steer - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - - F1 : toggle HUD - I : toggle actor ids - H/? : toggle help - ESC : quit -""" - -# ============================================================================== -# -- find carla module --------------------------------------------------------- -# ============================================================================== - -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 - -# ============================================================================== -# -- imports ------------------------------------------------------------------- -# ============================================================================== - -import carla -from carla import TrafficLightState as tls - -import argparse -import logging -import datetime -import weakref -import math -import random - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import KMOD_SHIFT - from pygame.locals import K_COMMA - from pygame.locals import K_DOWN - from pygame.locals import K_ESCAPE - from pygame.locals import K_F1 - from pygame.locals import K_LEFT - from pygame.locals import K_PERIOD - from pygame.locals import K_RIGHT - from pygame.locals import K_SLASH - from pygame.locals import K_SPACE - from pygame.locals import K_TAB - from pygame.locals import K_UP - from pygame.locals import K_a - from pygame.locals import K_d - from pygame.locals import K_h - from pygame.locals import K_i - from pygame.locals import K_m - from pygame.locals import K_p - from pygame.locals import K_q - from pygame.locals import K_s - from pygame.locals import K_w -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -# ============================================================================== -# -- Constants ----------------------------------------------------------------- -# ============================================================================== - -# Colors - -# We will use the color palette used in Tango Desktop Project (Each color is indexed depending on brightness level) -# See: https://en.wikipedia.org/wiki/Tango_Desktop_Project - -COLOR_BUTTER_0 = pygame.Color(252, 233, 79) -COLOR_BUTTER_1 = pygame.Color(237, 212, 0) -COLOR_BUTTER_2 = pygame.Color(196, 160, 0) - -COLOR_ORANGE_0 = pygame.Color(252, 175, 62) -COLOR_ORANGE_1 = pygame.Color(245, 121, 0) -COLOR_ORANGE_2 = pygame.Color(209, 92, 0) - -COLOR_CHOCOLATE_0 = pygame.Color(233, 185, 110) -COLOR_CHOCOLATE_1 = pygame.Color(193, 125, 17) -COLOR_CHOCOLATE_2 = pygame.Color(143, 89, 2) - -COLOR_CHAMELEON_0 = pygame.Color(138, 226, 52) -COLOR_CHAMELEON_1 = pygame.Color(115, 210, 22) -COLOR_CHAMELEON_2 = pygame.Color(78, 154, 6) - -COLOR_SKY_BLUE_0 = pygame.Color(114, 159, 207) -COLOR_SKY_BLUE_1 = pygame.Color(52, 101, 164) -COLOR_SKY_BLUE_2 = pygame.Color(32, 74, 135) - -COLOR_PLUM_0 = pygame.Color(173, 127, 168) -COLOR_PLUM_1 = pygame.Color(117, 80, 123) -COLOR_PLUM_2 = pygame.Color(92, 53, 102) - -COLOR_SCARLET_RED_0 = pygame.Color(239, 41, 41) -COLOR_SCARLET_RED_1 = pygame.Color(204, 0, 0) -COLOR_SCARLET_RED_2 = pygame.Color(164, 0, 0) - -COLOR_ALUMINIUM_0 = pygame.Color(238, 238, 236) -COLOR_ALUMINIUM_1 = pygame.Color(211, 215, 207) -COLOR_ALUMINIUM_2 = pygame.Color(186, 189, 182) -COLOR_ALUMINIUM_3 = pygame.Color(136, 138, 133) -COLOR_ALUMINIUM_4 = pygame.Color(85, 87, 83) -COLOR_ALUMINIUM_4_5 = pygame.Color(66, 62, 64) -COLOR_ALUMINIUM_5 = pygame.Color(46, 52, 54) - - -COLOR_WHITE = pygame.Color(255, 255, 255) -COLOR_BLACK = pygame.Color(0, 0, 0) - - -# Module Defines -MODULE_WORLD = 'WORLD' -MODULE_HUD = 'HUD' -MODULE_INPUT = 'INPUT' - -PIXELS_PER_METER = 12 - -MAP_DEFAULT_SCALE = 0.1 -HERO_DEFAULT_SCALE = 1.0 - -PIXELS_AHEAD_VEHICLE = 150 - -# ============================================================================== -# -- Util ----------------------------------------------------------- -# ============================================================================== - - -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 - - -class Util(object): - - @staticmethod - def blits(destination_surface, source_surfaces, rect=None, blend_mode=0): - for surface in source_surfaces: - destination_surface.blit(surface[0], surface[1], rect, blend_mode) - - @staticmethod - def length(v): - return math.sqrt(v.x**2 + v.y**2 + v.z**2) - - @staticmethod - def get_bounding_box(actor): - bb = actor.trigger_volume.extent - corners = [carla.Location(x=-bb.x, y=-bb.y), - carla.Location(x=bb.x, y=-bb.y), - carla.Location(x=bb.x, y=bb.y), - carla.Location(x=-bb.x, y=bb.y), - carla.Location(x=-bb.x, y=-bb.y)] - corners = [x + actor.trigger_volume.location for x in corners] - t = actor.get_transform() - t.transform(corners) - return corners - -# ============================================================================== -# -- 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_ALUMINIUM_4) - 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() - - -# ============================================================================== -# -- 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=COLOR_WHITE, seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill(COLOR_BLACK) - 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(COLOR_BLACK) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, COLOR_WHITE) - 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) - - -# ============================================================================== -# -- ModuleHUD ----------------------------------------------------------------- -# ============================================================================== - - -class ModuleHUD (object): - - def __init__(self, name, width, height): - self.name = name - self.dim = (width, height) - self._init_hud_params() - self._init_data_params() - - 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, True) - self.help = HelpText(pygame.font.Font(mono, 24), *self.dim) - self._notifications = FadingText( - pygame.font.Font(pygame.font.get_default_font(), 20), - (self.dim[0], 40), (0, self.dim[1] - 40)) - - def _init_data_params(self): - self.show_info = True - self.show_actor_ids = False - self._info_text = {} - - def notification(self, text, seconds=2.0): - self._notifications.set_text(text, seconds=seconds) - - def tick(self, clock): - self._notifications.tick(clock) - - def add_info(self, module_name, info): - self._info_text[module_name] = info - - def render_vehicles_ids(self, vehicle_id_surface, list_actors, world_to_pixel, hero_actor, hero_transform): - vehicle_id_surface.fill(COLOR_BLACK) - if self.show_actor_ids: - vehicle_id_surface.set_alpha(150) - for actor in list_actors: - x, y = world_to_pixel(actor[1].location) - - angle = 0 - if hero_actor is not None: - angle = -hero_transform.rotation.yaw - 90 - - color = COLOR_SKY_BLUE_0 - if int(actor[0].attributes['number_of_wheels']) == 2: - color = COLOR_CHOCOLATE_0 - if actor[0].attributes['role_name'] == 'hero': - color = COLOR_CHAMELEON_0 - - font_surface = self._header_font.render(str(actor[0].id), True, color) - rotated_font_surface = pygame.transform.rotate(font_surface, angle) - rect = rotated_font_surface.get_rect(center=(x, y)) - vehicle_id_surface.blit(rotated_font_surface, rect) - - 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(): - if not module_info: - continue - surface = self._header_font.render(module_name, True, COLOR_ALUMINIUM_0).convert_alpha() - display.blit(surface, (8 + bar_width / 2, 18 * i + v_offset)) - v_offset += 12 - 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 - 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_ALUMINIUM_0, 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_ALUMINIUM_0, 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_ALUMINIUM_0, rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, COLOR_ALUMINIUM_0).convert_alpha() - display.blit(surface, (8, 18 * i + v_offset)) - v_offset += 18 - v_offset += 24 - self._notifications.render(display) - self.help.render(display) - - -# ============================================================================== -# -- TrafficLightSurfaces ------------------------------------------------------ -# ============================================================================== - - -class TrafficLightSurfaces(object): - """Holds the surfaces (scaled and rotated) for painting traffic lights""" - - def __init__(self): - def make_surface(tl): - w = 40 - surface = pygame.Surface((w, 3 * w), pygame.SRCALPHA) - surface.fill(COLOR_ALUMINIUM_5 if tl != 'h' else COLOR_ORANGE_2) - if tl != 'h': - hw = int(w / 2) - off = COLOR_ALUMINIUM_4 - red = COLOR_SCARLET_RED_0 - yellow = COLOR_BUTTER_0 - green = COLOR_CHAMELEON_0 - pygame.draw.circle(surface, red if tl == tls.Red else off, (hw, hw), int(0.4 * w)) - pygame.draw.circle(surface, yellow if tl == tls.Yellow else off, (hw, w + hw), int(0.4 * w)) - pygame.draw.circle(surface, green if tl == tls.Green else off, (hw, 2 * w + hw), int(0.4 * w)) - return pygame.transform.smoothscale(surface, (15, 45) if tl != 'h' else (19, 49)) - self._original_surfaces = { - 'h': make_surface('h'), - tls.Red: make_surface(tls.Red), - tls.Yellow: make_surface(tls.Yellow), - tls.Green: make_surface(tls.Green), - tls.Off: make_surface(tls.Off), - tls.Unknown: make_surface(tls.Unknown) - } - self.surfaces = dict(self._original_surfaces) - - def rotozoom(self, angle, scale): - for key, surface in self._original_surfaces.items(): - self.surfaces[key] = pygame.transform.rotozoom(surface, angle, scale) - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== - - -class MapImage(object): - def __init__(self, carla_world, carla_map, pixels_per_meter, show_triggers, show_connections, show_spawn_points): - self._pixels_per_meter = pixels_per_meter - self.scale = 1.0 - self.show_triggers = show_triggers - self.show_connections = show_connections - self.show_spawn_points = show_spawn_points - - waypoints = carla_map.generate_waypoints(2) - margin = 50 - max_x = max(waypoints, key=lambda x: x.transform.location.x).transform.location.x + margin - max_y = max(waypoints, key=lambda x: x.transform.location.y).transform.location.y + margin - min_x = min(waypoints, key=lambda x: x.transform.location.x).transform.location.x - margin - min_y = min(waypoints, key=lambda x: x.transform.location.y).transform.location.y - margin - - self.width = max(max_x - min_x, max_y - min_y) - self._world_offset = (min_x, min_y) - - width_in_pixels = int(self._pixels_per_meter * self.width) - - self.big_map_surface = pygame.Surface((width_in_pixels, width_in_pixels)).convert() - self.draw_road_map(self.big_map_surface, carla_world, carla_map, self.world_to_pixel, self.world_to_pixel_width) - self.surface = self.big_map_surface - - def draw_road_map(self, map_surface, carla_world, carla_map, world_to_pixel, world_to_pixel_width): - map_surface.fill(COLOR_ALUMINIUM_4) - precision = 0.05 - - def lane_marking_color_to_tango(lane_marking_color): - tango_color = COLOR_BLACK - - if lane_marking_color == carla.LaneMarkingColor.White: - tango_color = COLOR_ALUMINIUM_2 - - elif lane_marking_color == carla.LaneMarkingColor.Blue: - tango_color = COLOR_SKY_BLUE_0 - - elif lane_marking_color == carla.LaneMarkingColor.Green: - tango_color = COLOR_CHAMELEON_0 - - elif lane_marking_color == carla.LaneMarkingColor.Red: - tango_color = COLOR_SCARLET_RED_0 - - elif lane_marking_color == carla.LaneMarkingColor.Yellow: - tango_color = COLOR_ORANGE_0 - - return tango_color - - def draw_solid_line(surface, color, closed, points, width): - if len(points) >= 2: - pygame.draw.lines(surface, color, closed, points, width) - - def draw_broken_line(surface, color, closed, points, width): - broken_lines = [x for n, x in enumerate(zip(*(iter(points),) * 20)) if n % 3 == 0] - for line in broken_lines: - pygame.draw.lines(surface, color, closed, line, width) - - def get_lane_markings(lane_marking_type, lane_marking_color, waypoints, sign): - margin = 0.20 - if lane_marking_type == carla.LaneMarkingType.Broken or (lane_marking_type == carla.LaneMarkingType.Solid): - marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints] - return [(lane_marking_type, lane_marking_color, marking_1)] - elif lane_marking_type == carla.LaneMarkingType.SolidBroken or lane_marking_type == carla.LaneMarkingType.BrokenSolid: - marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints] - marking_2 = [world_to_pixel(lateral_shift(w.transform, - sign * (w.lane_width * 0.5 + margin * 2))) for w in waypoints] - return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1), - (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)] - elif lane_marking_type == carla.LaneMarkingType.BrokenBroken: - marking = [world_to_pixel(lateral_shift(w.transform, - sign * (w.lane_width * 0.5 - margin))) for w in waypoints] - return [(carla.LaneMarkingType.Broken, lane_marking_color, marking)] - elif lane_marking_type == carla.LaneMarkingType.SolidSolid: - marking = [world_to_pixel(lateral_shift(w.transform, - sign * ((w.lane_width * 0.5) - margin))) for w in waypoints] - return [(carla.LaneMarkingType.Solid, lane_marking_color, marking)] - - return [(carla.LaneMarkingType.NONE, carla.LaneMarkingColor.Other, [])] - - def draw_lane_marking(surface, waypoints, is_left): - sign = -1 if is_left else 1 - lane_marking = None - - marking_type = carla.LaneMarkingType.NONE - previous_marking_type = carla.LaneMarkingType.NONE - - marking_color = carla.LaneMarkingColor.Other - previous_marking_color = carla.LaneMarkingColor.Other - - waypoints_list = [] - temp_waypoints = [] - current_lane_marking = carla.LaneMarkingType.NONE - for sample in waypoints: - lane_marking = sample.left_lane_marking if sign < 0 else sample.right_lane_marking - - if lane_marking is None: - continue - - marking_type = lane_marking.type - marking_color = lane_marking.color - - if current_lane_marking != marking_type: - markings = get_lane_markings( - previous_marking_type, - lane_marking_color_to_tango(previous_marking_color), - temp_waypoints, - sign) - current_lane_marking = marking_type - - for marking in markings: - waypoints_list.append(marking) - - temp_waypoints = temp_waypoints[-1:] - - else: - temp_waypoints.append((sample)) - previous_marking_type = marking_type - previous_marking_color = marking_color - - # Add last marking - last_markings = get_lane_markings( - previous_marking_type, - lane_marking_color_to_tango(previous_marking_color), - temp_waypoints, - sign) - for marking in last_markings: - waypoints_list.append(marking) - - for markings in waypoints_list: - if markings[0] == carla.LaneMarkingType.Solid: - draw_solid_line(surface, markings[1], False, markings[2], 2) - elif markings[0] == carla.LaneMarkingType.Broken: - draw_broken_line(surface, markings[1], False, markings[2], 2) - - def draw_arrow(surface, transform, color=COLOR_ALUMINIUM_2): - transform.rotation.yaw += 180 - forward = transform.get_forward_vector() - transform.rotation.yaw += 90 - right_dir = transform.get_forward_vector() - end = transform.location - start = end - 2.0 * forward - right = start + 0.8 * forward + 0.4 * right_dir - left = start + 0.8 * forward - 0.4 * right_dir - pygame.draw.lines( - surface, color, False, [ - world_to_pixel(x) for x in [ - start, end]], 4) - pygame.draw.lines( - surface, color, False, [ - world_to_pixel(x) for x in [ - left, start, right]], 4) - - def draw_traffic_signs(surface, font_surface, actor, color=COLOR_ALUMINIUM_2, trigger_color=COLOR_PLUM_0): - transform = actor.get_transform() - waypoint = carla_map.get_waypoint(transform.location) - - angle = -waypoint.transform.rotation.yaw - 90.0 - font_surface = pygame.transform.rotate(font_surface, angle) - pixel_pos = world_to_pixel(waypoint.transform.location) - offset = font_surface.get_rect(center=(pixel_pos[0], pixel_pos[1])) - surface.blit(font_surface, offset) - - # Draw line in front of stop - forward_vector = carla.Location(waypoint.transform.get_forward_vector()) - left_vector = carla.Location(-forward_vector.y, forward_vector.x, - forward_vector.z) * waypoint.lane_width / 2 * 0.7 - - line = [(waypoint.transform.location + (forward_vector * 1.5) + (left_vector)), - (waypoint.transform.location + (forward_vector * 1.5) - (left_vector))] - - line_pixel = [world_to_pixel(p) for p in line] - pygame.draw.lines(surface, color, True, line_pixel, 2) - - # draw bounding box - if self.show_triggers: - corners = Util.get_bounding_box(actor) - corners = [world_to_pixel(p) for p in corners] - pygame.draw.lines(surface, trigger_color, True, corners, 2) - - def lateral_shift(transform, shift): - transform.rotation.yaw += 90 - return transform.location + shift * transform.get_forward_vector() - - def draw_topology(carla_topology, index): - topology = [x[index] for x in carla_topology] - topology = sorted(topology, key=lambda w: w.transform.location.z) - for waypoint in topology: - # if waypoint.road_id == 150 or waypoint.road_id == 16: - waypoints = [waypoint] - - nxt = waypoint.next(precision) - if len(nxt) > 0: - nxt = nxt[0] - while nxt.road_id == waypoint.road_id: - waypoints.append(nxt) - nxt = nxt.next(precision) - if len(nxt) > 0: - nxt = nxt[0] - else: - break - - # Draw Road - road_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in waypoints] - road_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in waypoints] - - polygon = road_left_side + [x for x in reversed(road_right_side)] - polygon = [world_to_pixel(x) for x in polygon] - - if len(polygon) > 2: - pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon, 5) - pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon) - - # Draw Shoulders and Parkings - PARKING_COLOR = COLOR_ALUMINIUM_4_5 - SHOULDER_COLOR = COLOR_ALUMINIUM_5 - - final_color = SHOULDER_COLOR - - # Draw Right - shoulder = [] - for w in waypoints: - r = w.get_right_lane() - if r is not None and ( - r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking): - if r.lane_type == carla.LaneType.Parking: - final_color = PARKING_COLOR - shoulder.append(r) - - shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder] - shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder] - - polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)] - polygon = [world_to_pixel(x) for x in polygon] - - if len(polygon) > 2: - pygame.draw.polygon(map_surface, final_color, polygon, 5) - pygame.draw.polygon(map_surface, final_color, polygon) - - draw_lane_marking( - map_surface, - shoulder, - False) - - # Draw Left - shoulder = [] - for w in waypoints: - r = w.get_left_lane() - if r is not None and ( - r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking): - if r.lane_type == carla.LaneType.Parking: - final_color = PARKING_COLOR - shoulder.append(r) - - shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder] - shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder] - - polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)] - polygon = [world_to_pixel(x) for x in polygon] - - if len(polygon) > 2: - pygame.draw.polygon(map_surface, final_color, polygon, 5) - pygame.draw.polygon(map_surface, final_color, polygon) - - draw_lane_marking( - map_surface, - shoulder, - True) - - # Draw Lane Markings and Arrows - if not waypoint.is_intersection: - draw_lane_marking( - map_surface, - waypoints, - True) - draw_lane_marking( - map_surface, - waypoints, - False) - for n, wp in enumerate(waypoints): - if ((n + 1) % 400) == 0: - draw_arrow(map_surface, wp.transform) - - topology = carla_map.get_topology() - draw_topology(topology, 0) - draw_topology(topology, 1) - - if self.show_spawn_points: - for sp in carla_map.get_spawn_points(): - draw_arrow(map_surface, sp, color=COLOR_CHOCOLATE_0) - - if self.show_connections: - dist = 1.5 - to_pixel = lambda wp: world_to_pixel(wp.transform.location) - for wp in carla_map.generate_waypoints(dist): - col = (0, 255, 255) if wp.is_intersection else (0, 255, 0) - for nxt in wp.next(dist): - pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(nxt), 2) - if wp.lane_change & carla.LaneChange.Right: - r = wp.get_right_lane() - if r and r.lane_type == carla.LaneType.Driving: - pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(r), 2) - if wp.lane_change & carla.LaneChange.Left: - l = wp.get_left_lane() - if l and l.lane_type == carla.LaneType.Driving: - pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(l), 2) - - actors = carla_world.get_actors() - - # Draw Traffic Signs - font_size = world_to_pixel_width(1) - font = pygame.font.SysFont('Arial', font_size, True) - - stops = [actor for actor in actors if 'stop' in actor.type_id] - yields = [actor for actor in actors if 'yield' in actor.type_id] - - stop_font_surface = font.render("STOP", False, COLOR_ALUMINIUM_2) - stop_font_surface = pygame.transform.scale( - stop_font_surface, (stop_font_surface.get_width(), stop_font_surface.get_height() * 2)) - - yield_font_surface = font.render("YIELD", False, COLOR_ALUMINIUM_2) - yield_font_surface = pygame.transform.scale( - yield_font_surface, (yield_font_surface.get_width(), yield_font_surface.get_height() * 2)) - - for ts_stop in stops: - draw_traffic_signs(map_surface, stop_font_surface, ts_stop, trigger_color=COLOR_SCARLET_RED_1) - - for ts_yield in yields: - draw_traffic_signs(map_surface, yield_font_surface, ts_yield, trigger_color=COLOR_ORANGE_1) - - def world_to_pixel(self, location, offset=(0, 0)): - x = self.scale * self._pixels_per_meter * (location.x - self._world_offset[0]) - y = self.scale * self._pixels_per_meter * (location.y - self._world_offset[1]) - return [int(x - offset[0]), int(y - offset[1])] - - def world_to_pixel_width(self, width): - return int(self.scale * self._pixels_per_meter * width) - - def scale_map(self, scale): - if scale != self.scale: - self.scale = scale - width = int(self.big_map_surface.get_width() * self.scale) - self.surface = pygame.transform.smoothscale(self.big_map_surface, (width, width)) - - -class ModuleWorld(object): - def __init__(self, name, args, timeout): - self.client = None - self.name = name - self.args = args - self.timeout = timeout - self.server_fps = 0.0 - self.simulation_time = 0 - self.server_clock = pygame.time.Clock() - - # World data - self.world = None - self.town_map = None - self.actors_with_transforms = [] - # Store necessary modules - self.module_hud = None - self.module_input = None - - self.surface_size = [0, 0] - self.prev_scaled_size = 0 - self.scaled_size = 0 - # Hero actor - self.hero_actor = None - self.spawned_hero = None - self.hero_transform = None - - self.scale_offset = [0, 0] - - self.vehicle_id_surface = None - self.result_surface = None - - self.traffic_light_surfaces = TrafficLightSurfaces() - self.affected_traffic_light = None - - # Map info - self.map_image = None - self.border_round_surface = None - self.original_surface_size = None - self.hero_surface = None - self.actors_surface = None - - def _get_data_from_carla(self): - try: - self.client = carla.Client(self.args.host, self.args.port) - self.client.set_timeout(self.timeout) - - if self.args.map is None: - world = self.client.get_world() - else: - world = self.client.load_world(self.args.map) - - town_map = world.get_map() - return (world, town_map) - - except RuntimeError as ex: - logging.error(ex) - exit_game() - - def start(self): - self.world, self.town_map = self._get_data_from_carla() - - # Create Surfaces - self.map_image = MapImage( - carla_world=self.world, - carla_map=self.town_map, - pixels_per_meter=PIXELS_PER_METER, - show_triggers=self.args.show_triggers, - show_connections=self.args.show_connections, - show_spawn_points=self.args.show_spawn_points) - - # Store necessary modules - self.module_hud = module_manager.get_module(MODULE_HUD) - self.module_input = module_manager.get_module(MODULE_INPUT) - - self.original_surface_size = min(self.module_hud.dim[0], self.module_hud.dim[1]) - self.surface_size = self.map_image.big_map_surface.get_width() - - self.scaled_size = int(self.surface_size) - self.prev_scaled_size = int(self.surface_size) - - # Render Actors - self.actors_surface = pygame.Surface((self.map_image.surface.get_width(), self.map_image.surface.get_height())) - self.actors_surface.set_colorkey(COLOR_BLACK) - - self.vehicle_id_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() - self.vehicle_id_surface.set_colorkey(COLOR_BLACK) - - self.border_round_surface = pygame.Surface(self.module_hud.dim, pygame.SRCALPHA).convert() - self.border_round_surface.set_colorkey(COLOR_WHITE) - self.border_round_surface.fill(COLOR_BLACK) - - center_offset = (int(self.module_hud.dim[0] / 2), int(self.module_hud.dim[1] / 2)) - pygame.draw.circle(self.border_round_surface, COLOR_ALUMINIUM_1, center_offset, int(self.module_hud.dim[1] / 2)) - pygame.draw.circle(self.border_round_surface, COLOR_WHITE, center_offset, int((self.module_hud.dim[1] - 8) / 2)) - - scaled_original_size = self.original_surface_size * (1.0 / 0.9) - self.hero_surface = pygame.Surface((scaled_original_size, scaled_original_size)).convert() - - self.result_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() - self.result_surface.set_colorkey(COLOR_BLACK) - - # Start hero mode by default - self.select_hero_actor() - self.hero_actor.set_autopilot(False) - self.module_input.wheel_offset = HERO_DEFAULT_SCALE - self.module_input.control = carla.VehicleControl() - - weak_self = weakref.ref(self) - self.world.on_tick(lambda timestamp: ModuleWorld.on_world_tick(weak_self, timestamp)) - - def select_hero_actor(self): - hero_vehicles = [actor for actor in self.world.get_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) - self.hero_transform = self.hero_actor.get_transform() - else: - self._spawn_hero() - - def _spawn_hero(self): - # Get a random blueprint. - blueprint = random.choice(self.world.get_blueprint_library().filter(self.args.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. - while self.hero_actor is None: - spawn_points = self.world.get_map().get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - self.hero_actor = self.world.try_spawn_actor(blueprint, spawn_point) - self.hero_transform = self.hero_actor.get_transform() - - # Save it in order to destroy it when closing program - self.spawned_hero = self.hero_actor - - def tick(self, clock): - actors = self.world.get_actors() - self.actors_with_transforms = [(actor, actor.get_transform()) for actor in actors] - if self.hero_actor is not None: - self.hero_transform = self.hero_actor.get_transform() - self.update_hud_info(clock) - - def update_hud_info(self, clock): - hero_mode_text = [] - if self.hero_actor is not None: - 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) - - affected_traffic_light_text = 'None' - if self.affected_traffic_light is not None: - state = self.affected_traffic_light.state - if state == carla.TrafficLightState.Green: - affected_traffic_light_text = 'GREEN' - elif state == carla.TrafficLightState.Yellow: - affected_traffic_light_text = 'YELLOW' - else: - affected_traffic_light_text = 'RED' - - affected_speed_limit_text = self.hero_actor.get_speed_limit() - - hero_mode_text = [ - 'Hero Mode: ON', - 'Hero ID: %7d' % self.hero_actor.id, - 'Hero Vehicle: %14s' % get_actor_display_name(self.hero_actor, truncate=14), - 'Hero Speed: %3d km/h' % hero_speed_text, - 'Hero Affected by:', - ' Traffic Light: %12s' % affected_traffic_light_text, - ' Speed Limit: %3d km/h' % affected_speed_limit_text - ] - else: - hero_mode_text = ['Hero Mode: OFF'] - - self.server_fps = self.server_clock.get_fps() - self.server_fps = 'inf' if self.server_fps == float('inf') else round(self.server_fps) - module_info_text = [ - 'Server: % 16s FPS' % self.server_fps, - 'Client: % 16s FPS' % round(clock.get_fps()), - 'Simulation Time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), - 'Map Name: %10s' % self.town_map.name.split('/')[-1], - ] - - module_info_text = module_info_text - module_hud = module_manager.get_module(MODULE_HUD) - module_hud.add_info(self.name, module_info_text) - module_hud.add_info('HERO', hero_mode_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.simulation_time = timestamp.elapsed_seconds - - def _split_actors(self): - vehicles = [] - traffic_lights = [] - speed_limits = [] - walkers = [] - - for actor_with_transform in self.actors_with_transforms: - actor = actor_with_transform[0] - if 'vehicle' in actor.type_id: - vehicles.append(actor_with_transform) - elif 'traffic_light' in actor.type_id: - traffic_lights.append(actor_with_transform) - elif 'speed_limit' in actor.type_id: - speed_limits.append(actor_with_transform) - elif 'walker' in actor.type_id: - walkers.append(actor_with_transform) - - info_text = [] - if self.hero_actor is not None and len(vehicles) > 1: - location = self.hero_transform.location - vehicle_list = [x[0] for x in vehicles if x[0].id != self.hero_actor.id] - - def distance(v): return location.distance(v.get_location()) - for n, vehicle in enumerate(sorted(vehicle_list, key=distance)): - if n > 15: - break - vehicle_type = get_actor_display_name(vehicle, truncate=22) - info_text.append('% 5d %s' % (vehicle.id, vehicle_type)) - module_manager.get_module(MODULE_HUD).add_info( - 'NEARBY VEHICLES', - info_text) - - return (vehicles, traffic_lights, speed_limits, walkers) - - def _render_traffic_lights(self, surface, list_tl, world_to_pixel): - self.affected_traffic_light = None - - for tl in list_tl: - world_pos = tl.get_location() - pos = world_to_pixel(world_pos) - - if self.args.show_triggers: - corners = Util.get_bounding_box(tl) - corners = [world_to_pixel(p) for p in corners] - pygame.draw.lines(surface, COLOR_BUTTER_1, True, corners, 2) - - if self.hero_actor is not None: - corners = Util.get_bounding_box(tl) - corners = [world_to_pixel(p) for p in corners] - tl_t = tl.get_transform() - - transformed_tv = tl_t.transform(tl.trigger_volume.location) - hero_location = self.hero_actor.get_location() - d = hero_location.distance(transformed_tv) - s = Util.length(tl.trigger_volume.extent) + Util.length(self.hero_actor.bounding_box.extent) - if (d <= s): - # Highlight traffic light - self.affected_traffic_light = tl - srf = self.traffic_light_surfaces.surfaces['h'] - surface.blit(srf, srf.get_rect(center=pos)) - - srf = self.traffic_light_surfaces.surfaces[tl.state] - surface.blit(srf, srf.get_rect(center=pos)) - - def _render_speed_limits(self, surface, list_sl, world_to_pixel, world_to_pixel_width): - - font_size = world_to_pixel_width(2) - radius = world_to_pixel_width(2) - font = pygame.font.SysFont('Arial', font_size) - - for sl in list_sl: - - x, y = world_to_pixel(sl.get_location()) - - # Render speed limit - white_circle_radius = int(radius * 0.75) - - pygame.draw.circle(surface, COLOR_SCARLET_RED_1, (x, y), radius) - pygame.draw.circle(surface, COLOR_ALUMINIUM_0, (x, y), white_circle_radius) - - limit = sl.type_id.split('.')[2] - font_surface = font.render(limit, True, COLOR_ALUMINIUM_5) - - if self.args.show_triggers: - corners = Util.get_bounding_box(sl) - corners = [world_to_pixel(p) for p in corners] - pygame.draw.lines(surface, COLOR_PLUM_2, True, corners, 2) - - # Blit - if self.hero_actor is not None: - # Rotate font surface with respect to hero vehicle front - angle = -self.hero_transform.rotation.yaw - 90.0 - font_surface = pygame.transform.rotate(font_surface, angle) - offset = font_surface.get_rect(center=(x, y)) - surface.blit(font_surface, offset) - - else: - surface.blit(font_surface, (x - radius / 2, y - radius / 2)) - - def _render_walkers(self, surface, list_w, world_to_pixel): - for w in list_w: - color = COLOR_PLUM_0 - - # Compute bounding box points - bb = w[0].bounding_box.extent - corners = [ - carla.Location(x=-bb.x, y=-bb.y), - carla.Location(x=bb.x, y=-bb.y), - carla.Location(x=bb.x, y=bb.y), - carla.Location(x=-bb.x, y=bb.y)] - - w[1].transform(corners) - corners = [world_to_pixel(p) for p in corners] - pygame.draw.polygon(surface, color, corners) - - def _render_vehicles(self, surface, list_v, world_to_pixel): - - for v in list_v: - color = COLOR_SKY_BLUE_0 - if int(v[0].attributes['number_of_wheels']) == 2: - color = COLOR_CHOCOLATE_1 - if v[0].attributes['role_name'] == 'hero': - color = COLOR_CHAMELEON_0 - # Compute bounding box points - bb = v[0].bounding_box.extent - corners = [carla.Location(x=-bb.x, y=-bb.y), - carla.Location(x=bb.x - 0.8, y=-bb.y), - carla.Location(x=bb.x, y=0), - carla.Location(x=bb.x - 0.8, y=bb.y), - carla.Location(x=-bb.x, y=bb.y), - carla.Location(x=-bb.x, y=-bb.y) - ] - v[1].transform(corners) - corners = [world_to_pixel(p) for p in corners] - pygame.draw.lines(surface, color, False, corners, int(math.ceil(4.0 * self.map_image.scale))) - - def render_actors(self, surface, vehicles, traffic_lights, speed_limits, walkers): - # Static actors - self._render_traffic_lights(surface, [tl[0] for tl in traffic_lights], self.map_image.world_to_pixel) - self._render_speed_limits(surface, [sl[0] for sl in speed_limits], self.map_image.world_to_pixel, - self.map_image.world_to_pixel_width) - - # Dynamic actors - self._render_vehicles(surface, vehicles, self.map_image.world_to_pixel) - self._render_walkers(surface, walkers, self.map_image.world_to_pixel) - - def clip_surfaces(self, clipping_rect): - self.actors_surface.set_clip(clipping_rect) - self.vehicle_id_surface.set_clip(clipping_rect) - self.result_surface.set_clip(clipping_rect) - - def _compute_scale(self, scale_factor): - m = self.module_input.mouse_pos - - # Percentage of surface where mouse position is actually - px = (m[0] - self.scale_offset[0]) / float(self.prev_scaled_size) - py = (m[1] - self.scale_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.scale_offset[0] + diff_between_scales[0], - self.scale_offset[1] + diff_between_scales[1]) - - # Update previous scale - self.prev_scaled_size = self.scaled_size - - # Scale performed - self.map_image.scale_map(scale_factor) - - def render(self, display): - if self.actors_with_transforms is None: - return - self.result_surface.fill(COLOR_BLACK) - vehicles, traffic_lights, speed_limits, walkers = self._split_actors() - - scale_factor = self.module_input.wheel_offset - self.scaled_size = int(self.map_image.width * scale_factor) - if self.scaled_size != self.prev_scaled_size: - self._compute_scale(scale_factor) - - # Render Actors - - self.actors_surface.fill(COLOR_BLACK) - self.render_actors( - self.actors_surface, - vehicles, - traffic_lights, - speed_limits, - walkers) - - # Render Ids - self.module_hud.render_vehicles_ids(self.vehicle_id_surface, vehicles, - self.map_image.world_to_pixel, self.hero_actor, self.hero_transform) - - # Blit surfaces - surfaces = ((self.map_image.surface, (0, 0)), - (self.actors_surface, (0, 0)), - (self.vehicle_id_surface, (0, 0)), - ) - - angle = 0.0 if self.hero_actor is None else self.hero_transform.rotation.yaw + 90.0 - self.traffic_light_surfaces.rotozoom(-angle, self.map_image.scale) - - center_offset = (0, 0) - if self.hero_actor is not None: - - hero_location_screen = self.map_image.world_to_pixel(self.hero_transform.location) - hero_front = self.hero_transform.get_forward_vector() - translation_offset = ( - hero_location_screen[0] - - self.hero_surface.get_width() / - 2 + - hero_front.x * - PIXELS_AHEAD_VEHICLE, - (hero_location_screen[1] - - self.hero_surface.get_height() / - 2 + - hero_front.y * - PIXELS_AHEAD_VEHICLE)) - - # Apply clipping rect - clipping_rect = pygame.Rect(translation_offset[0], - translation_offset[1], - self.hero_surface.get_width(), - self.hero_surface.get_height()) - self.clip_surfaces(clipping_rect) - - Util.blits(self.result_surface, surfaces) - - self.border_round_surface.set_clip(clipping_rect) - - self.hero_surface.fill(COLOR_ALUMINIUM_4) - self.hero_surface.blit(self.result_surface, (-translation_offset[0], - -translation_offset[1])) - - rotated_result_surface = pygame.transform.rotozoom(self.hero_surface, angle, 0.9).convert() - - center = (display.get_width() / 2, display.get_height() / 2) - rotation_pivot = rotated_result_surface.get_rect(center=center) - display.blit(rotated_result_surface, rotation_pivot) - - display.blit(self.border_round_surface, (0, 0)) - else: - # Translation offset - 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 = (abs(display.get_width() - self.surface_size) / 2 * scale_factor, 0) - - # Apply clipping rect - clipping_rect = pygame.Rect(-translation_offset[0] - center_offset[0], -translation_offset[1], - self.module_hud.dim[0], self.module_hud.dim[1]) - self.clip_surfaces(clipping_rect) - Util.blits(self.result_surface, surfaces) - - display.blit(self.result_surface, (translation_offset[0] + center_offset[0], - translation_offset[1])) - - def destroy(self): - if self.spawned_hero is not None: - self.spawned_hero.destroy() -# ============================================================================== -# -- 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 = 0.1 - self.wheel_amount = 0.025 - self._steer_cache = 0.0 - self.control = None - self._autopilot_enabled = False - - def start(self): - hud = module_manager.get_module(MODULE_HUD) - hud.notification("Press 'H' or '?' for help.", seconds=4.0) - - def render(self, display): - pass - - def tick(self, clock): - self.parse_input(clock) - - 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 self._is_quit_shortcut(event.key): - exit_game() - elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): - module_hud = module_manager.get_module(MODULE_HUD) - module_hud.help.toggle() - elif event.key == K_TAB: - module_world = module_manager.get_module(MODULE_WORLD) - module_hud = module_manager.get_module(MODULE_HUD) - if module_world.hero_actor is None: - module_world.select_hero_actor() - self.wheel_offset = HERO_DEFAULT_SCALE - self.control = carla.VehicleControl() - module_hud.notification('Hero Mode') - else: - self.wheel_offset = MAP_DEFAULT_SCALE - self.mouse_offset = [0, 0] - self.mouse_pos = [0, 0] - module_world.scale_offset = [0, 0] - module_world.hero_actor = None - module_hud.notification('Map Mode') - elif event.key == K_F1: - module_hud = module_manager.get_module(MODULE_HUD) - module_hud.show_info = not module_hud.show_info - elif event.key == K_i: - module_hud = module_manager.get_module(MODULE_HUD) - module_hud.show_actor_ids = not module_hud.show_actor_ids - elif isinstance(self.control, carla.VehicleControl): - if event.key == K_q: - self.control.gear = 1 if self.control.reverse else -1 - elif event.key == K_m: - self.control.manual_gear_shift = not self.control.manual_gear_shift - world = module_manager.get_module(MODULE_WORLD) - self.control.gear = world.hero_actor.get_control().gear - module_hud = module_manager.get_module(MODULE_HUD) - module_hud.notification('%s Transmission' % ( - 'Manual' if self.control.manual_gear_shift else 'Automatic')) - elif self.control.manual_gear_shift and event.key == K_COMMA: - self.control.gear = max(-1, self.control.gear - 1) - elif self.control.manual_gear_shift and event.key == K_PERIOD: - self.control.gear = self.control.gear + 1 - elif event.key == K_p: - world = module_manager.get_module(MODULE_WORLD) - if world.hero_actor is not None: - self._autopilot_enabled = not self._autopilot_enabled - world.hero_actor.set_autopilot(self._autopilot_enabled) - module_hud = module_manager.get_module(MODULE_HUD) - module_hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) - elif event.type == pygame.MOUSEBUTTONDOWN: - if event.button == 4: - self.wheel_offset += self.wheel_amount - if self.wheel_offset >= 1.0: - self.wheel_offset = 1.0 - elif event.button == 5: - self.wheel_offset -= self.wheel_amount - if self.wheel_offset <= 0.1: - self.wheel_offset = 0.1 - - def _parse_keys(self, milliseconds): - keys = pygame.key.get_pressed() - self.control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0 - steer_increment = 5e-4 * milliseconds - if keys[K_LEFT] or keys[K_a]: - self._steer_cache -= steer_increment - elif keys[K_RIGHT] or keys[K_d]: - self._steer_cache += steer_increment - else: - self._steer_cache = 0.0 - self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) - self.control.steer = round(self._steer_cache, 1) - self.control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0 - self.control.hand_brake = keys[K_SPACE] - - def _parse_mouse(self): - if pygame.mouse.get_pressed()[0]: - x, y = pygame.mouse.get_pos() - self.mouse_offset[0] += (1.0 / self.wheel_offset) * (x - self.mouse_pos[0]) - self.mouse_offset[1] += (1.0 / self.wheel_offset) * (y - self.mouse_pos[1]) - self.mouse_pos = (x, y) - - def parse_input(self, clock): - self._parse_events() - self._parse_mouse() - if not self._autopilot_enabled: - if isinstance(self.control, carla.VehicleControl): - self._parse_keys(clock.get_time()) - self.control.reverse = self.control.gear < 0 - world = module_manager.get_module(MODULE_WORLD) - if (world.hero_actor is not None): - world.hero_actor.apply_control(self.control) - - @staticmethod - def _is_quit_shortcut(key): - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - - -# ============================================================================== -# -- Global Objects ------------------------------------------------------------ -# ============================================================================== -module_manager = ModuleManager() - - -# ============================================================================== -# -- Game Loop --------------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - try: - # Init Pygame - pygame.init() - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - pygame.display.set_caption(args.description) - - font = pygame.font.Font(pygame.font.get_default_font(), 20) - text_surface = font.render('Rendering map...', True, COLOR_WHITE) - display.blit(text_surface, text_surface.get_rect(center=(args.width / 2, args.height / 2))) - pygame.display.flip() - - # Init modules - input_module = ModuleInput(MODULE_INPUT) - hud_module = ModuleHUD(MODULE_HUD, args.width, args.height) - world_module = ModuleWorld(MODULE_WORLD, args, timeout=2.0) - - # Register Modules - module_manager.register_module(world_module) - module_manager.register_module(hud_module) - module_manager.register_module(input_module) - - module_manager.start_modules() - - clock = pygame.time.Clock() - while True: - clock.tick_busy_loop(60) - - module_manager.tick(clock) - module_manager.render(display) - - pygame.display.flip() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - finally: - if world_module is not None: - world_module.destroy() - -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)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='actor filter (default: "vehicle.*")') - argparser.add_argument( - '--map', - metavar='TOWN', - default=None, - help='start a new episode at the given TOWN') - argparser.add_argument( - '--no-rendering', - action='store_true', - default=True, - help='switch off server rendering') - argparser.add_argument( - '--show-triggers', - action='store_true', - help='show trigger boxes of traffic signs') - argparser.add_argument( - '--show-connections', - action='store_true', - help='show waypoint connections') - argparser.add_argument( - '--show-spawn-points', - action='store_true', - help='show recommended spawn points') - - 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__) - - game_loop(args) - -if __name__ == '__main__': - main() diff --git a/scenario_runner.py b/scenario_runner.py deleted file mode 100755 index 6ecda75..0000000 --- a/scenario_runner.py +++ /dev/null @@ -1,771 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2018-2020 Intel Corporation -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -""" -Welcome to CARLA scenario_runner - -This is the main script to be executed when running a scenario. -It loads the scenario configuration, loads the scenario and manager, -and finally triggers the scenario execution. -""" - -from __future__ import print_function - -import glob -import traceback -import argparse -from argparse import RawTextHelpFormatter -from datetime import datetime -import importlib -import inspect -import os -import signal -import sys -import time -import json - -import carla - -from srunner.scenarioconfigs.openscenario_configuration import OpenScenarioConfiguration -from srunner.scenariomanager.carla_data_provider import CarlaDataProvider -from srunner.scenariomanager.scenario_manager import ScenarioManager -from srunner.scenarios.open_scenario import OpenScenario -from srunner.scenarios.route_scenario import RouteScenario -from srunner.tools.scenario_parser import ScenarioConfigurationParser -from srunner.tools.route_parser import RouteParser -from srunner.tools.osc2_helper import OSC2Helper -from srunner.scenarios.osc2_scenario import OSC2Scenario -from srunner.scenarioconfigs.osc2_scenario_configuration import ( - OSC2ScenarioConfiguration, -) - -# Version of scenario_runner -VERSION = "0.9.13" - - -class ScenarioRunner(object): - """ - This is the core scenario runner module. It is responsible for - running (and repeating) a single scenario or a list of scenarios. - - Usage: - scenario_runner = ScenarioRunner(args) - scenario_runner.run() - del scenario_runner - """ - - ego_vehicles = [] - - # Tunable parameters - client_timeout = 10.0 # in seconds - wait_for_world = 20.0 # in seconds - frame_rate = 20.0 # in Hz - - # CARLA world and scenario handlers - world = None - manager = None - - finished = False - - additional_scenario_module = None - - agent_instance = None - module_agent = None - - def __init__(self, args): - """ - Setup CARLA client and world - Setup ScenarioManager - """ - self._args = args - - if args.timeout: - self.client_timeout = float(args.timeout) - - # First of all, we need to create the client that will send the requests - # to the simulator. Here we'll assume the simulator is accepting - # requests in the localhost at port 2000. - # self.client = carla.Client(args.host, int(args.port)) - # self.client.set_timeout(self.client_timeout) - # dist = pkg_resources.get_distribution("carla") - # if LooseVersion(dist.version) < LooseVersion('0.9.15'): - # raise ImportError("CARLA version 0.9.15 or newer required. CARLA version found: {}".format(dist)) - - # Load agent if requested via command line args - # If something goes wrong an exception will be thrown by importlib (ok here) - if self._args.agent is not None: - module_name = os.path.basename(args.agent).split(".")[0] - sys.path.insert(0, os.path.dirname(args.agent)) - self.module_agent = importlib.import_module(module_name) - - # Create the ScenarioManager - self.manager = ScenarioManager( - self._args.debug, self._args.sync, self._args.timeout - ) - - # Create signal handler for SIGINT - self._shutdown_requested = False - if sys.platform != "win32": - signal.signal(signal.SIGHUP, self._signal_handler) - signal.signal(signal.SIGINT, self._signal_handler) - signal.signal(signal.SIGTERM, self._signal_handler) - - self._start_wall_time = datetime.now() - - def destroy(self): - """ - Cleanup and delete actors, ScenarioManager and CARLA world - """ - - self._cleanup() - if self.manager is not None: - del self.manager - if self.world is not None: - del self.world - if self.client is not None: - del self.client - - def _signal_handler(self, signum, frame): - """ - Terminate scenario ticking when receiving a signal interrupt - """ - self._shutdown_requested = True - if self.manager: - self.manager.stop_scenario() - self._cleanup() - if not self.manager.get_running_status(): - raise RuntimeError("Timeout occurred during scenario execution") - - def _get_scenario_class_or_fail(self, scenario): - """ - Get scenario class by scenario name - If scenario is not supported or not found, exit script - """ - - # Path of all scenario at "srunner/scenarios" folder + the path of the additional scenario argument - scenarios_list = glob.glob( - "{}/srunner/scenarios/*.py".format(os.getenv("SCENARIO_RUNNER_ROOT", "./")) - ) - scenarios_list.append(self._args.additionalScenario) - - for scenario_file in scenarios_list: - # Get their module - module_name = os.path.basename(scenario_file).split(".")[0] - sys.path.insert(0, os.path.dirname(scenario_file)) - scenario_module = importlib.import_module(module_name) - - # And their members of type class - for member in inspect.getmembers(scenario_module, inspect.isclass): - if scenario in member: - return member[1] - - # Remove unused Python paths - sys.path.pop(0) - - print("Scenario '{}' not supported ... Exiting".format(scenario)) - sys.exit(-1) - - def _cleanup(self): - """ - Remove and destroy all actors - """ - if self.finished: - return - - self.finished = True - - # Simulation still running and in synchronous mode? - if self.world is not None and self._args.sync: - try: - # Reset to asynchronous mode - settings = self.world.get_settings() - settings.synchronous_mode = False - settings.fixed_delta_seconds = None - self.world.apply_settings(settings) - self.client.get_trafficmanager( - int(self._args.trafficManagerPort) - ).set_synchronous_mode(False) - except RuntimeError: - sys.exit(-1) - - self.manager.cleanup() - - CarlaDataProvider.cleanup() - - for i, _ in enumerate(self.ego_vehicles): - if self.ego_vehicles[i]: - if ( - not self._args.waitForEgo - and self.ego_vehicles[i] is not None - and self.ego_vehicles[i].is_alive - ): - print("Destroying ego vehicle {}".format(self.ego_vehicles[i].id)) - self.ego_vehicles[i].destroy() - self.ego_vehicles[i] = None - self.ego_vehicles = [] - - if self.agent_instance: - self.agent_instance.destroy() - self.agent_instance = None - - def _prepare_ego_vehicles(self, ego_vehicles): - """ - Spawn or update the ego vehicles - """ - - if not self._args.waitForEgo: - for vehicle in ego_vehicles: - self.ego_vehicles.append( - CarlaDataProvider.request_new_actor( - vehicle.model, - vehicle.transform, - vehicle.rolename, - random_location=vehicle.random_location, - color=vehicle.color, - actor_category=vehicle.category, - ) - ) - else: - ego_vehicle_missing = True - while ego_vehicle_missing: - self.ego_vehicles = [] - ego_vehicle_missing = False - for ego_vehicle in ego_vehicles: - ego_vehicle_found = False - carla_vehicles = ( - CarlaDataProvider.get_world().get_actors().filter("vehicle.*") - ) - for carla_vehicle in carla_vehicles: - if ( - carla_vehicle.attributes["role_name"] - == ego_vehicle.rolename - ): - ego_vehicle_found = True - self.ego_vehicles.append(carla_vehicle) - break - if not ego_vehicle_found: - ego_vehicle_missing = True - break - - for i, _ in enumerate(self.ego_vehicles): - self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) - self.ego_vehicles[i].set_target_velocity(carla.Vector3D()) - self.ego_vehicles[i].set_target_angular_velocity(carla.Vector3D()) - self.ego_vehicles[i].apply_control(carla.VehicleControl()) - CarlaDataProvider.register_actor( - self.ego_vehicles[i], ego_vehicles[i].transform - ) - - # sync state - if CarlaDataProvider.is_sync_mode(): - self.world.tick() - else: - self.world.wait_for_tick() - - def _analyze_scenario(self, config): - """ - Provide feedback about success/failure of a scenario - """ - - # Create the filename - current_time = str(datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) - junit_filename = None - json_filename = None - config_name = config.name - if self._args.outputDir != "": - config_name = os.path.join(self._args.outputDir, config_name) - - if self._args.junit: - junit_filename = config_name + current_time + ".xml" - if self._args.json: - json_filename = config_name + current_time + ".json" - filename = None - if self._args.file: - filename = config_name + current_time + ".txt" - - if not self.manager.analyze_scenario( - self._args.output, filename, junit_filename, json_filename - ): - print("All scenario tests were passed successfully!") - else: - print("Not all scenario tests were successful") - if not (self._args.output or filename or junit_filename): - print("Please run with --output for further information") - - def _record_criteria(self, criteria, name): - """ - Filter the JSON serializable attributes of the criterias and - dumps them into a file. This will be used by the metrics manager, - in case the user wants specific information about the criterias. - """ - file_name = name[:-4] + ".json" - - # Filter the attributes that aren't JSON serializable - with open("temp.json", "w", encoding="utf-8") as fp: - criteria_dict = {} - for criterion in criteria: - criterion_dict = criterion.__dict__ - criteria_dict[criterion.name] = {} - - for key in criterion_dict: - if key != "name": - try: - key_dict = {key: criterion_dict[key]} - json.dump(key_dict, fp, sort_keys=False, indent=4) - criteria_dict[criterion.name].update(key_dict) - except TypeError: - pass - - os.remove("temp.json") - - # Save the criteria dictionary into a .json file - with open(file_name, "w", encoding="utf-8") as fp: - json.dump(criteria_dict, fp, sort_keys=False, indent=4) - - def _load_and_wait_for_world(self, town, ego_vehicles=None): - """ - Load a new CARLA world and provide data to CarlaDataProvider - """ - - if self._args.reloadWorld: - self.world = self.client.load_world(town) - else: - # if the world should not be reloaded, wait at least until all ego vehicles are ready - ego_vehicle_found = False - if self._args.waitForEgo: - while not ego_vehicle_found and not self._shutdown_requested: - vehicles = self.client.get_world().get_actors().filter("vehicle.*") - for ego_vehicle in ego_vehicles: - ego_vehicle_found = False - for vehicle in vehicles: - if vehicle.attributes["role_name"] == ego_vehicle.rolename: - ego_vehicle_found = True - break - if not ego_vehicle_found: - print("Not all ego vehicles ready. Waiting ... ") - time.sleep(1) - break - - self.world = self.client.get_world() - - if self._args.sync: - settings = self.world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 1.0 / self.frame_rate - self.world.apply_settings(settings) - - CarlaDataProvider.set_client(self.client) - CarlaDataProvider.set_world(self.world) - - # Wait for the world to be ready - if CarlaDataProvider.is_sync_mode(): - self.world.tick() - else: - self.world.wait_for_tick() - - map_name = CarlaDataProvider.get_map().name.split("/")[-1] - if map_name not in (town, "OpenDriveMap"): - print("The CARLA server uses the wrong map: {}".format(map_name)) - print("This scenario requires to use map: {}".format(town)) - return False - - return True - - def _load_and_run_scenario(self, config): - """ - Load and run the scenario given by config - """ - result = False - if not self._load_and_wait_for_world(config.town, config.ego_vehicles): - self._cleanup() - return False - - if self._args.agent: - agent_class_name = self.module_agent.__name__.title().replace("_", "") - try: - self.agent_instance = getattr(self.module_agent, agent_class_name)( - self._args.agentConfig - ) - config.agent = self.agent_instance - except Exception as e: # Forces the simulation to run synchronously # pylint: disable=broad-except - traceback.print_exc() - print("Could not setup required agent due to {}".format(e)) - self._cleanup() - return False - - CarlaDataProvider.set_traffic_manager_port(int(self._args.trafficManagerPort)) - tm = self.client.get_trafficmanager(int(self._args.trafficManagerPort)) - tm.set_random_device_seed(int(self._args.trafficManagerSeed)) - if self._args.sync: - tm.set_synchronous_mode(True) - - # Prepare scenario - print("Preparing scenario: " + config.name) - try: - self._prepare_ego_vehicles(config.ego_vehicles) - if self._args.openscenario: - scenario = OpenScenario( - world=self.world, - ego_vehicles=self.ego_vehicles, - config=config, - config_file=self._args.openscenario, - timeout=100000, - ) - elif self._args.route: - scenario = RouteScenario( - world=self.world, config=config, debug_mode=self._args.debug - ) - elif self._args.openscenario2: - scenario = OSC2Scenario( - world=self.world, - ego_vehicles=self.ego_vehicles, - config=config, - osc2_file=self._args.openscenario2, - timeout=100000, - ) - else: - scenario_class = self._get_scenario_class_or_fail(config.type) - scenario = scenario_class( - world=self.world, - ego_vehicles=self.ego_vehicles, - config=config, - randomize=self._args.randomize, - debug_mode=self._args.debug, - ) - except Exception as exception: # pylint: disable=broad-except - print("The scenario cannot be loaded") - traceback.print_exc() - print(exception) - self._cleanup() - return False - - try: - if self._args.record: - recorder_name = "{}/{}/{}.log".format( - os.getenv("SCENARIO_RUNNER_ROOT", "./"), - self._args.record, - config.name, - ) - self.client.start_recorder(recorder_name, True) - - # Load scenario and run it - self.manager.load_scenario(scenario, self.agent_instance) - self.manager.run_scenario() - - # Provide outputs if required - self._analyze_scenario(config) - - # Remove all actors, stop the recorder and save all criterias (if needed) - scenario.remove_all_actors() - if self._args.record: - self.client.stop_recorder() - self._record_criteria( - self.manager.scenario.get_criteria(), recorder_name - ) - - result = True - - except Exception as e: # pylint: disable=broad-except - traceback.print_exc() - print(e) - result = False - - self._cleanup() - return result - - def _run_scenarios(self): - """ - Run conventional scenarios (e.g. implemented using the Python API of ScenarioRunner) - """ - result = False - - # Load the scenario configurations provided in the config file - scenario_configurations = ( - ScenarioConfigurationParser.parse_scenario_configuration( - self._args.scenario, self._args.configFile - ) - ) - if not scenario_configurations: - print( - "Configuration for scenario {} cannot be found!".format( - self._args.scenario - ) - ) - return result - - # Execute each configuration - for config in scenario_configurations: - for _ in range(self._args.repetitions): - self.finished = False - result = self._load_and_run_scenario(config) - - self._cleanup() - return result - - def _run_route(self): - """ - Run the route scenario - """ - result = False - - # retrieve routes - route_configurations = RouteParser.parse_routes_file( - self._args.route, self._args.route_id - ) - - for config in route_configurations: - for _ in range(self._args.repetitions): - result = self._load_and_run_scenario(config) - - self._cleanup() - return result - - def _run_openscenario(self): - """ - Run a scenario based on OpenSCENARIO - """ - - # Load the scenario configurations provided in the config file - if not os.path.isfile(self._args.openscenario): - print("File does not exist") - self._cleanup() - return False - - openscenario_params = {} - if self._args.openscenarioparams is not None: - for entry in self._args.openscenarioparams.split(","): - [key, val] = [m.strip() for m in entry.split(":")] - openscenario_params[key] = val - config = OpenScenarioConfiguration( - self._args.openscenario, self.client, openscenario_params - ) - - result = self._load_and_run_scenario(config) - self._cleanup() - return result - - def _run_osc2(self): - """ - Run a scenario based on ASAM OpenSCENARIO 2.0. - https://www.asam.net/static_downloads/public/asam-openscenario/2.0.0/welcome.html - """ - # Load the scenario configurations provided in the config file - if not os.path.isfile(self._args.openscenario2): - print("File does not exist") - self._cleanup() - return False - - config = OSC2ScenarioConfiguration(self._args.openscenario2, self.client) - - result = self._load_and_run_scenario(config) - self._cleanup() - - return result - - def run(self): - """ - Run all scenarios according to provided commandline args - """ - result = True - if self._args.openscenario: - result = self._run_openscenario() - elif self._args.route: - result = self._run_route() - elif self._args.openscenario2: - result = self._run_osc2() - else: - result = self._run_scenarios() - - print("No more scenarios .... Exiting") - return result - - -def main(): - """ - main function - """ - description = ( - "CARLA Scenario Runner: Setup, Run and Evaluate scenarios using CARLA\n" - "Current version: " + VERSION - ) - - # pylint: disable=line-too-long - parser = argparse.ArgumentParser( - description=description, formatter_class=RawTextHelpFormatter - ) - parser.add_argument( - "-v", "--version", action="version", version="%(prog)s " + VERSION - ) - parser.add_argument( - "--host", default="127.0.0.1", help="IP of the host server (default: localhost)" - ) - parser.add_argument( - "--port", default="2000", help="TCP port to listen to (default: 2000)" - ) - parser.add_argument( - "--timeout", - default="10.0", - help="Set the CARLA client timeout value in seconds", - ) - parser.add_argument( - "--trafficManagerPort", - default="8000", - help="Port to use for the TrafficManager (default: 8000)", - ) - parser.add_argument( - "--trafficManagerSeed", - default="0", - help="Seed used by the TrafficManager (default: 0)", - ) - parser.add_argument( - "--sync", action="store_true", help="Forces the simulation to run synchronously" - ) - parser.add_argument( - "--list", action="store_true", help="List all supported scenarios and exit" - ) - - parser.add_argument( - "--scenario", - help="Name of the scenario to be executed. Use the preposition 'group:' to run all scenarios of one class, e.g. ControlLoss or FollowLeadingVehicle", - ) - parser.add_argument("--openscenario", help="Provide an OpenSCENARIO definition") - parser.add_argument( - "--openscenarioparams", help="Overwrited for OpenSCENARIO ParameterDeclaration" - ) - parser.add_argument("--openscenario2", help="Provide an openscenario2 definition") - parser.add_argument("--route", help="Run a route as a scenario", type=str) - parser.add_argument( - "--route-id", - help="Run a specific route inside that 'route' file", - default="", - type=str, - ) - parser.add_argument( - "--agent", - help="Agent used to execute the route. Not compatible with non-route-based scenarios.", - ) - parser.add_argument( - "--agentConfig", type=str, help="Path to Agent's configuration file", default="" - ) - - parser.add_argument( - "--output", action="store_true", help="Provide results on stdout" - ) - parser.add_argument( - "--file", action="store_true", help="Write results into a txt file" - ) - parser.add_argument( - "--junit", action="store_true", help="Write results into a junit file" - ) - parser.add_argument( - "--json", action="store_true", help="Write results into a JSON file" - ) - parser.add_argument( - "--outputDir", - default="", - help="Directory for output files (default: this directory)", - ) - - parser.add_argument( - "--configFile", - default="", - help="Provide an additional scenario configuration file (*.xml)", - ) - parser.add_argument( - "--additionalScenario", - default="", - help="Provide additional scenario implementations (*.py)", - ) - - parser.add_argument("--debug", action="store_true", help="Run with debug output") - parser.add_argument( - "--reloadWorld", - action="store_true", - help="Reload the CARLA world before starting a scenario (default=True)", - ) - parser.add_argument( - "--record", - type=str, - default="", - help="Path were the files will be saved, relative to SCENARIO_RUNNER_ROOT.\nActivates the CARLA recording feature and saves to file all the criteria information.", - ) - parser.add_argument( - "--randomize", action="store_true", help="Scenario parameters are randomized" - ) - parser.add_argument( - "--repetitions", default=1, type=int, help="Number of scenario executions" - ) - parser.add_argument( - "--waitForEgo", - action="store_true", - help="Connect the scenario to an existing ego vehicle", - ) - - arguments = parser.parse_args() - # pylint: enable=line-too-long - - OSC2Helper.wait_for_ego = arguments.waitForEgo - - if arguments.list: - print("Currently the following scenarios are supported:") - print( - *ScenarioConfigurationParser.get_list_of_scenarios(arguments.configFile), - sep="\n", - ) - return 1 - - if ( - not arguments.scenario - and not arguments.openscenario - and not arguments.route - and not arguments.openscenario2 - ): - print("Please specify either a scenario or use the route mode\n\n") - parser.print_help(sys.stdout) - return 1 - - if arguments.route and (arguments.openscenario or arguments.scenario): - print( - "The route mode cannot be used together with a scenario (incl. OpenSCENARIO)'\n\n" - ) - parser.print_help(sys.stdout) - return 1 - - if arguments.agent and (arguments.openscenario or arguments.scenario): - print("Agents are currently only compatible with route scenarios'\n\n") - parser.print_help(sys.stdout) - return 1 - - if arguments.openscenarioparams and not arguments.openscenario: - print( - "WARN: Ignoring --openscenarioparams when --openscenario is not specified" - ) - - if arguments.route: - arguments.reloadWorld = True - - if arguments.agent: - arguments.sync = True - - scenario_runner = None - result = True - print(arguments.sync) - try: - scenario_runner = ScenarioRunner(arguments) - result = scenario_runner.run() - except Exception: # pylint: disable=broad-except - traceback.print_exc() - - finally: - if scenario_runner is not None: - scenario_runner.destroy() - del scenario_runner - return not result - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 9edf6ec..2c884ef 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -66,6 +66,7 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: sensor_config_msg.sensor_id = sensor_config.id ego_config_msg.sensors.append(sensor_config_msg) + # keep publishing ego_sensor config until the bridge is ready while not self.autoware_state.bridge_ready: time.sleep(1) self.autoware_state.ego_config_publisher.publish(ego_config_msg)