From 5febb2785436ca45578f1f97abd0006d9c31f615 Mon Sep 17 00:00:00 2001 From: rakadu1 Date: Wed, 1 Apr 2026 19:08:46 +0530 Subject: [PATCH] Added Carla_Examples for in depth exploration and experimentation. --- .gitignore | 3 +- carla_examples/V2XDemo.py | 1402 ++++++++++++++ carla_examples/automatic_control.py | 872 +++++++++ carla_examples/bounding_boxes.py | 484 +++++ carla_examples/carla_cosmos_gen.py | 376 ++++ carla_examples/client_bounding_boxes.py | 381 ++++ carla_examples/cosmos_aov.yaml | 84 + carla_examples/draw_skeleton.py | 418 +++++ carla_examples/dynamic_weather.py | 142 ++ carla_examples/follow_vehicle.py | 35 + carla_examples/generate_traffic.py | 366 ++++ carla_examples/get_component_test.py | 24 + carla_examples/invertedai_traffic.py | 695 +++++++ carla_examples/lidar_to_camera.py | 345 ++++ carla_examples/manual_control.py | 1367 ++++++++++++++ carla_examples/manual_control_carsim.py | 1175 ++++++++++++ carla_examples/manual_control_chrono.py | 1196 ++++++++++++ .../manual_control_steeringwheel.py | 848 +++++++++ carla_examples/no_rendering_mode.py | 1627 +++++++++++++++++ carla_examples/open3d_lidar.py | 323 ++++ carla_examples/requirements.txt | 9 + carla_examples/sensor_synchronization.py | 131 ++ .../show_recorder_actors_blocked.py | 66 + carla_examples/show_recorder_collisions.py | 64 + carla_examples/show_recorder_file_info.py | 68 + carla_examples/start_recording.py | 141 ++ carla_examples/start_replaying.py | 109 ++ carla_examples/synchronous_mode.py | 195 ++ carla_examples/test_addsecondvx.py | 83 + carla_examples/tutorial.py | 116 ++ carla_examples/tutorial_gbuffer.py | 116 ++ carla_examples/vehicle_gallery.py | 51 + carla_examples/vehicle_physics.py | 133 ++ carla_examples/visualize_multiple_sensors.py | 375 ++++ intel/old_implement.md | 472 +++++ intel/shenron_implementation_guide.md | 123 ++ intel/shenron_integration.md | 64 + 37 files changed, 14478 insertions(+), 1 deletion(-) create mode 100644 carla_examples/V2XDemo.py create mode 100644 carla_examples/automatic_control.py create mode 100644 carla_examples/bounding_boxes.py create mode 100644 carla_examples/carla_cosmos_gen.py create mode 100644 carla_examples/client_bounding_boxes.py create mode 100644 carla_examples/cosmos_aov.yaml create mode 100644 carla_examples/draw_skeleton.py create mode 100644 carla_examples/dynamic_weather.py create mode 100644 carla_examples/follow_vehicle.py create mode 100644 carla_examples/generate_traffic.py create mode 100644 carla_examples/get_component_test.py create mode 100644 carla_examples/invertedai_traffic.py create mode 100644 carla_examples/lidar_to_camera.py create mode 100644 carla_examples/manual_control.py create mode 100644 carla_examples/manual_control_carsim.py create mode 100644 carla_examples/manual_control_chrono.py create mode 100644 carla_examples/manual_control_steeringwheel.py create mode 100644 carla_examples/no_rendering_mode.py create mode 100644 carla_examples/open3d_lidar.py create mode 100644 carla_examples/requirements.txt create mode 100644 carla_examples/sensor_synchronization.py create mode 100644 carla_examples/show_recorder_actors_blocked.py create mode 100644 carla_examples/show_recorder_collisions.py create mode 100644 carla_examples/show_recorder_file_info.py create mode 100644 carla_examples/start_recording.py create mode 100644 carla_examples/start_replaying.py create mode 100644 carla_examples/synchronous_mode.py create mode 100644 carla_examples/test_addsecondvx.py create mode 100644 carla_examples/tutorial.py create mode 100644 carla_examples/tutorial_gbuffer.py create mode 100644 carla_examples/vehicle_gallery.py create mode 100644 carla_examples/vehicle_physics.py create mode 100644 carla_examples/visualize_multiple_sensors.py create mode 100644 intel/old_implement.md create mode 100644 intel/shenron_implementation_guide.md create mode 100644 intel/shenron_integration.md diff --git a/.gitignore b/.gitignore index 1bfaeb3..43798a2 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,5 @@ __pycache__/ *.mcap logs/ *.pyc -.env \ No newline at end of file +.env +carla_examples/nvidia/ \ No newline at end of file diff --git a/carla_examples/V2XDemo.py b/carla_examples/V2XDemo.py new file mode 100644 index 0000000..7967d2f --- /dev/null +++ b/carla_examples/V2XDemo.py @@ -0,0 +1,1402 @@ +#!/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 + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + O : open/close all doors of vehicle + T : toggle vehicle's telemetry + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + 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 collections +import datetime +import logging +import math +import os +import random +import re +import sys +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + 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_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_f + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_o + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + 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 find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.sync = args.sync + self.actor_role_name = args.rolename + 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.v2x_sensor = None + self.radar_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._actor_generation = args.generation + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.show_vehicle_telemetry = False + self.doors_are_open = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + 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 a random blueprint. + blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('terramechanics'): + blueprint.set_attribute('terramechanics', 'true') + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + # 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.v2x_sensor = V2XSensor(self.player, self.hud) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + 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) + + if self.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + sensors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + # self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.imu_sensor.sensor, + self.v2x_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._ackermann_enabled = False + self._ackermann_reverse = 1 + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._ackermann_control = carla.VehicleAckermannControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock, sync_mode): + if isinstance(self._control, carla.VehicleControl): + 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_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key == K_o: + try: + if world.doors_are_open: + world.hud.notification("Closing Doors") + world.doors_are_open = False + world.player.close_door(carla.VehicleDoor.All) + else: + world.hud.notification("Opening doors") + world.doors_are_open = True + world.player.open_door(carla.VehicleDoor.All) + except Exception: + pass + elif event.key == K_t: + if world.show_vehicle_telemetry: + world.player.show_debug_telemetry(False) + world.show_vehicle_telemetry = False + world.hud.notification("Disabled Vehicle Telemetry") + else: + try: + world.player.show_debug_telemetry(True) + world.show_vehicle_telemetry = True + world.hud.notification("Enabled Vehicle Telemetry") + except Exception: + pass + elif event.key > K_0 and event.key <= K_9: + index_ctrl = 0 + if pygame.key.get_mods() & KMOD_CTRL: + index_ctrl = 9 + world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) + 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.log") + 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.log'") + # replayer + client.replay_file("manual_recording.log", 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)) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_f: + # Toggle ackermann controller + self._ackermann_enabled = not self._ackermann_enabled + world.hud.show_ackermann_info(self._ackermann_enabled) + world.hud.notification("Ackermann Controller %s" % + ("Enabled" if self._ackermann_enabled else "Disabled")) + if event.key == K_q: + if not self._ackermann_enabled: + self._control.gear = 1 if self._control.reverse else -1 + else: + self._ackermann_reverse *= -1 + # Reset ackermann control + self._ackermann_control = carla.VehicleAckermannControl() + 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: + if not self._autopilot_enabled and not sync_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + 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: + if isinstance(self._control, carla.VehicleControl): + 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)) + # Apply control + if not self._ackermann_enabled: + world.player.apply_control(self._control) + else: + world.player.apply_ackermann_control(self._ackermann_control) + # Update control to the last one applied by the ackermann controller. + self._control = world.player.get_control() + # Update hud with the newest ackermann control + world.hud.update_ackermann_control(self._ackermann_control) + + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + if keys[K_UP] or keys[K_w]: + if not self._ackermann_enabled: + self._control.throttle = min(self._control.throttle + 0.01, 1.00) + else: + self._ackermann_control.speed += round(milliseconds * 0.005, 2) * self._ackermann_reverse + else: + if not self._ackermann_enabled: + self._control.throttle = 0.0 + + if keys[K_DOWN] or keys[K_s]: + if not self._ackermann_enabled: + self._control.brake = min(self._control.brake + 0.2, 1) + else: + self._ackermann_control.speed -= min(abs(self._ackermann_control.speed), round(milliseconds * 0.005, 2)) * self._ackermann_reverse + self._ackermann_control.speed = max(0, abs(self._ackermann_control.speed)) * self._ackermann_reverse + else: + if not self._ackermann_enabled: + 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)) + if not self._ackermann_enabled: + self._control.steer = round(self._steer_cache, 1) + self._control.hand_brake = keys[K_SPACE] + else: + self._ackermann_control.steer = round(self._steer_cache, 1) + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @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() + + self._show_ackermann_info = False + self._ackermann_control = carla.VehicleAckermannControl() + + 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, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + if self._show_ackermann_info: + self._info_text += [ + '', + 'Ackermann Controller:', + ' Target speed: % 8.0f km/h' % (3.6*self._ackermann_control.speed), + ] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + 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 show_ackermann_info(self, enabled): + self._show_ackermann_info = enabled + + def update_ackermann_control(self, ackermann_control): + self._ackermann_control = ackermann_control + + 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) + +# ============================================================================== +# -- V2XSensor ----------------------------------------------------------------- +# ============================================================================== + + +class V2XSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + world = self._parent.get_world() + #bp = world.get_blueprint_library().find('sensor.other.v2x_custom') + bp = world.get_blueprint_library().find('sensor.other.v2x') + bp.set_attribute("path_loss_model", "geometric") + self.hud = hud + 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: V2XSensor._V2X_callback(weak_self, sensor_data)) + + @staticmethod + def _V2X_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + print(sensor_data.get_message_count()) + for data in sensor_data: + msg = data.get() + # stationId = msg["Header"]["Station ID"] + power = data.power + print(msg) + # self.hud.notification('Cam message received from %s ' % stationId) + self.hud.notification('Cam message received with power %f ' % power) +# ============================================================================== +# -- 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, gamma_correction): + 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 + + if not self._parent.type_id.startswith("walker.pedestrian"): + 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.SpringArmGhost), + (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.SpringArmGhost), + (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.SpringArmGhost), + (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] + else: + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] + + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], + # ['sensor.other.v2x', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}], + ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], + ['sensor.camera.normals', cc.Raw, 'Camera Normals', {}], + ] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + 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 next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 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 + original_settings = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2000.0) + + sim_world = client.get_world() + if args.sync: + original_settings = sim_world.get_settings() + settings = sim_world.get_settings() + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + + if args.autopilot and not sim_world.get_settings().synchronous_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + display.fill((0,0,0)) + pygame.display.flip() + # monitors = get_monitors() + # resolutions = [(monitor.name, monitor.width, monitor.height) for monitor in monitors] + # return resolutions + hud = HUD(args.width, args.height) + world = World(sim_world, hud, args) + controller = KeyboardControl(world, args.autopilot) + + if args.sync: + sim_world.tick() + else: + sim_world.wait_for_tick() + + clock = pygame.time.Clock() + while True: + if args.sync: + sim_world.tick() + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock, args.sync): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if original_settings: + sim_world.apply_settings(original_settings) + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + 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( + '--generation', + metavar='G', + default='2', + help='restrict to certain actor generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '--sync', + action='store_true', + help='Activate synchronous mode execution') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/automatic_control.py b/carla_examples/automatic_control.py new file mode 100644 index 0000000..26db623 --- /dev/null +++ b/carla_examples/automatic_control.py @@ -0,0 +1,872 @@ +#!/usr/bin/env python + +# Copyright (c) 2018 Intel Labs. +# authors: German Ros (german.ros@intel.com) +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import argparse +import collections +import datetime +import logging +import math +import os +import numpy.random as random +import re +import sys +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError( + 'cannot import numpy, make sure numpy package is installed') + +# ============================================================================== +# -- Add PythonAPI for release mode -------------------------------------------- +# ============================================================================== +try: + sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') +except IndexError: + pass + +import carla +from carla import ColorConverter as cc + +from agents.navigation.behavior_agent import BehaviorAgent # pylint: disable=import-error +from agents.navigation.basic_agent import BasicAgent # pylint: disable=import-error +from agents.navigation.constant_velocity_agent import ConstantVelocityAgent # pylint: disable=import-error + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + """Method to find weather presets""" + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + """Method to get actor display name""" + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2, 3]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + +# ============================================================================== +# -- World --------------------------------------------------------------- +# ============================================================================== + +class World(object): + """ Class representing the surrounding environment """ + + def __init__(self, carla_world, hud, args): + """Constructor method""" + self._args = 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.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._actor_generation = args.generation + self.restart(args) + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self, args): + """Restart the world""" + # 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_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 + + # Get a random blueprint. + blueprint_list = get_actor_blueprints(self.world, self._actor_filter, self._actor_generation) + if not blueprint_list: + raise ValueError("Couldn't find any blueprints with the specified filters") + blueprint = random.choice(blueprint_list) + blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.modify_vehicle_physics(self.player) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.modify_vehicle_physics(self.player) + + if self._args.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + """Get next weather setting""" + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + """Method for every tick""" + self.hud.tick(self, clock) + + def render(self, display): + """Render world""" + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + """Destroy sensors""" + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + """Destroys all actors""" + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.player] + for actor in actors: + if actor is not None: + actor.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + def __init__(self, world): + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + + @staticmethod + def _is_quit_shortcut(key): + """Shortcut for quitting""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + """Class for HUD text""" + + def __init__(self, width, height): + """Constructor method""" + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + """Gets informations from the world at every tick""" + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + """HUD method for every tick""" + self._notifications.tick(world, clock) + if not self._show_info: + return + transform = world.player.get_transform() + vel = world.player.get_velocity() + control = world.player.get_control() + heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name.split('/')[-1], + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), + u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % transform.location.z, + ''] + if isinstance(control, carla.VehicleControl): + self._info_text += [ + ('Throttle:', control.throttle, 0.0, 1.0), + ('Steer:', control.steer, -1.0, 1.0), + ('Brake:', control.brake, 0.0, 1.0), + ('Reverse:', control.reverse), + ('Hand brake:', control.hand_brake), + ('Manual:', control.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] + elif isinstance(control, carla.WalkerControl): + self._info_text += [ + ('Speed:', control.speed, 0.0, 5.556), + ('Jump:', control.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + + def dist(l): + return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) + ** 2 + (l.z - transform.location.z)**2) + vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] + + for dist, vehicle in sorted(vehicles): + if dist > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (dist, vehicle_type)) + + def toggle_info(self): + """Toggle info on or off""" + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + """Notification text""" + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + """Error text""" + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + """Render for HUD class""" + 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 - 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) + fig = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * 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): + """ Class for fading text """ + + def __init__(self, font, dim, pos): + """Constructor method""" + 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): + """Set fading text""" + 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): + """Fading text method for every tick""" + 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): + """Render fading text method""" + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """ Helper class for text render""" + + def __init__(self, font, width, height): + """Constructor method""" + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for i, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, i * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + """Toggle on or off the render help""" + self._render = not self._render + + def render(self, display): + """Render help text method""" + if self._render: + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + """ Class for collision sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(blueprint, 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): + """Gets the history of collisions""" + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + """On collision method""" + 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): + """Class for lane invasion sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + """On invasion method""" + 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): + """ Class for GNSS sensors""" + + def __init__(self, parent_actor): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(blueprint, 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): + """GNSS method""" + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + """ Class for camera management""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + 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.SpringArmGhost), + (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.SpringArmGhost), + (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.SpringArmGhost), + (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'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + blp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + blp.set_attribute('image_size_x', str(hud.dim[0])) + blp.set_attribute('image_size_y', str(hud.dim[1])) + elif item[0].startswith('sensor.lidar'): + blp.set_attribute('range', '50') + item.append(blp) + self.index = None + + def toggle_camera(self): + """Activate a camera""" + 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): + """Set a sensor""" + index = index % len(self.sensors) + needs_respawn = True if self.index is None else ( + force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][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 next_sensor(self): + """Get the next sensor""" + self.set_sensor(self.index + 1) + + def toggle_recording(self): + """Toggle recording on or off""" + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + """Render method""" + 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) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) + +# ============================================================================== +# -- Game Loop --------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + """ + Main loop of the simulation. It handles updating all the HUD information, + ticking the agent and, if needed, the world. + """ + + pygame.init() + pygame.font.init() + world = None + + try: + if args.seed: + random.seed(args.seed) + + client = carla.Client(args.host, args.port) + client.set_timeout(60.0) + + traffic_manager = client.get_trafficmanager() + sim_world = client.get_world() + + if args.sync: + settings = sim_world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager.set_synchronous_mode(True) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + if args.agent == "Basic": + agent = BasicAgent(world.player, 30) + agent.follow_speed_limits(True) + elif args.agent == "Constant": + agent = ConstantVelocityAgent(world.player, 30) + ground_loc = world.world.ground_projection(world.player.get_location(), 5) + if ground_loc: + world.player.set_location(ground_loc.location + carla.Location(z=0.01)) + agent.follow_speed_limits(True) + elif args.agent == "Behavior": + agent = BehaviorAgent(world.player, behavior=args.behavior) + + # Set the agent destination + spawn_points = world.map.get_spawn_points() + destination = random.choice(spawn_points).location + agent.set_destination(destination) + + clock = pygame.time.Clock() + + while True: + clock.tick() + if args.sync: + world.world.tick() + else: + world.world.wait_for_tick() + if controller.parse_events(): + return + + world.tick(clock) + world.render(display) + pygame.display.flip() + + if agent.done(): + if args.loop: + agent.set_destination(random.choice(spawn_points).location) + world.hud.notification("Target reached", seconds=4.0) + print("The target has been reached, searching for another target") + else: + print("The target has been reached, stopping the simulation") + break + + control = agent.run_step() + control.manual_gear_shift = False + world.player.apply_control(control) + + finally: + + if world is not None: + settings = world.world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + world.world.apply_settings(settings) + traffic_manager.set_synchronous_mode(True) + + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------- +# ============================================================================== + + +def main(): + """Main method""" + + argparser = argparse.ArgumentParser( + description='CARLA Automatic Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='Print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='Window resolution (default: 1280x720)') + argparser.add_argument( + '--sync', + action='store_true', + help='Synchronous mode execution') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='Actor filter (default: "vehicle.*")') + argparser.add_argument( + '--generation', + metavar='G', + default='2', + help='restrict to certain actor generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '-l', '--loop', + action='store_true', + dest='loop', + help='Sets a new random destination upon reaching the previous one (default: False)') + argparser.add_argument( + "-a", "--agent", type=str, + choices=["Behavior", "Basic", "Constant"], + help="select which agent to run", + default="Behavior") + argparser.add_argument( + '-b', '--behavior', type=str, + choices=["cautious", "normal", "aggressive"], + help='Choose one of the possible agent behaviors (default: normal) ', + default='normal') + argparser.add_argument( + '-s', '--seed', + help='Set seed for repeating executions (default: None)', + default=None, + type=int) + + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + main() diff --git a/carla_examples/bounding_boxes.py b/carla_examples/bounding_boxes.py new file mode 100644 index 0000000..947e2a7 --- /dev/null +++ b/carla_examples/bounding_boxes.py @@ -0,0 +1,484 @@ +#!/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 . + +# Generates 2D and 3D bounding boxes for a simulation and can save them as JSON +# Instructions: + +""" +Welcome to CARLA bounding boxes. + R : toggle recording images and bounding boxes + 3 : visualize bounding boxes in 3D + 2 : vizualize bounding boxes in 2D + ESC : quit +""" + +import carla +import json +import random +import queue +import pygame +import argparse +import numpy as np +from math import radians + +from pygame.locals import K_ESCAPE +from pygame.locals import K_2 +from pygame.locals import K_3 +from pygame.locals import K_r + +# Bounding box edge topology order +EDGES = [[0,1], [1,3], [3,2], [2,0], [0,4], [4,5], [5,1], [5,7], [7,6], [6,4], [6,2], [7,3]] + +# Map for CARLA semantic labels to class names and colors +SEMANTIC_MAP = {0: ('unlabelled', (0,0,0)), 1: ('road', (128,64,0)),2: ('sidewalk', (244,35,232)), + 3: ('building', (70,70,70)), 4: ('wall', (102,102,156)), 5: ('fence', (190,153,153)), + 6: ('pole', (153,153,153)), 7: ('traffic light', (250,170,30)), + 8: ('traffic sign', (220,220,0)), 9: ('vegetation', (107,142,35)), + 10: ('terrain', (152,251,152)), 11: ('sky', (70,130,180)), + 12: ('pedestrian', (220,20,60)), 13: ('rider', (255,0,0)), + 14: ('car', (0,0,142)), 15: ('truck', (0,0,70)), 16: ('bus', (0,60,100)), + 17: ('train', (0,80,100)), 18: ('motorcycle', (0,0,230)), + 19: ('bicycle', (119,11,32)), 20: ('static', (110,190,160)), + 21: ('dynamic', (170,120,50)), 22: ('other', (55,90,80)), + 23: ('water', (45,60,150)), 24: ('road line', (157,234,50)), + 25: ('ground', (81,0,81)), 26: ('bridge', (150,100,100)), + 27: ('rail track', (230,150,140)), 28: ('guard rail', (180,165,180))} + +# Calculate the camera projection matrix +def build_projection_matrix(w, h, fov, is_behind_camera=False): + focal = w / (2.0 * np.tan(fov * np.pi / 360.0)) + K = np.identity(3) + + if is_behind_camera: + K[0, 0] = K[1, 1] = -focal + else: + K[0, 0] = K[1, 1] = focal + + K[0, 2] = w / 2.0 + K[1, 2] = h / 2.0 + return K + +# Calculate 2D projection of 3D coordinate +def get_image_point(loc, K, w2c): + + # Format the input coordinate (loc is a carla.Position object) + point = np.array([loc.x, loc.y, loc.z, 1]) + # transform to camera coordinates + point_camera = np.dot(w2c, point) + + # New we must change from UE4's coordinate system to an "standard" + # (x, y ,z) -> (y, -z, x) + # and we remove the fourth componebonent also + point_camera = [point_camera[1], -point_camera[2], point_camera[0]] + + # now project 3D->2D using the camera matrix + point_img = np.dot(K, point_camera) + # normalize + point_img[0] /= point_img[2] + point_img[1] /= point_img[2] + + return point_img[0:2] + +# Verify that the point is within the image plane +def point_in_canvas(pos, img_h, img_w): + """Return true if point is in canvas""" + if (pos[0] >= 0) and (pos[0] < img_w) and (pos[1] >= 0) and (pos[1] < img_h): + return True + return False + +# Decode the instance segmentation map into semantic labels and actor IDs +def decode_instance_segmentation(img_rgba: np.ndarray): + semantic_labels = img_rgba[..., 2] # R channel + actor_ids = img_rgba[..., 1].astype(np.uint16) + (img_rgba[..., 0].astype(np.uint16) << 8) + return semantic_labels, actor_ids + +# Generate a 2D bounding box for an actor from the actor ID image +def bbox_2d_for_actor(actor, actor_ids: np.ndarray, semantic_labels: np.ndarray): + mask = (actor_ids == actor.id) + if not np.any(mask): + return None # actor not present + ys, xs = np.where(mask) + xmin, xmax = xs.min(), xs.max() + ymin, ymax = ys.min(), ys.max() + return {'actor_id': actor.id, + 'semantic_label': actor.semantic_tags[0], + 'bbox_2d': (xmin, ymin, xmax, ymax)} + +# Generate a 3D bounding box for an actor from the simulation +def bbox_3d_for_actor(actor, ego, camera_bp, camera): + + # Get the world to camera matrix + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # Get the attributes from the camera + image_w = camera_bp.get_attribute("image_size_x").as_int() + image_h = camera_bp.get_attribute("image_size_y").as_int() + fov = camera_bp.get_attribute("fov").as_float() + + # Calculate the camera projection matrix to project from 3D -> 2D + K = build_projection_matrix(image_w, image_h, fov) + K_b = build_projection_matrix(image_w, image_h, fov, is_behind_camera=True) + + ego_bbox_loc = ego.get_transform().location + ego.bounding_box.location + ego_bbox_transform = carla.Transform(ego_bbox_loc, ego.get_transform().rotation) + + npc_bbox_loc = actor.get_transform().location + actor.bounding_box.location + #npc_bbox_transform = carla.Transform(npc_bbox_loc, actor.get_transform().rotation) + + npc_loc_ego_space = ego_bbox_transform.inverse_transform(npc_bbox_loc) + + verts = [v for v in actor.bounding_box.get_world_vertices(actor.get_transform())] + + projection = [] + for edge in EDGES: + p1 = get_image_point(verts[edge[0]], K, world_2_camera) + p2 = get_image_point(verts[edge[1]], K, world_2_camera) + + p1_in_canvas = point_in_canvas(p1, image_h, image_w) + p2_in_canvas = point_in_canvas(p2, image_h, image_w) + + if not p1_in_canvas and not p2_in_canvas: + continue + + ray0 = verts[edge[0]] - camera.get_transform().location + ray1 = verts[edge[1]] - camera.get_transform().location + cam_forward_vec = camera.get_transform().get_forward_vector() + + # One of the vertexes is behind the camera + if not (cam_forward_vec.dot(ray0) > 0): + p1 = get_image_point(verts[edge[0]], K_b, world_2_camera) + if not (cam_forward_vec.dot(ray1) > 0): + p2 = get_image_point(verts[edge[1]], K_b, world_2_camera) + + projection.append((int(p1[0]), int(p1[1]), int(p2[0]), int(p2[1]))) + + return {'actor_id': actor.id, + 'semantic_label': actor.semantic_tags[0], + 'bbox_3d': { + 'center': { + 'x': npc_loc_ego_space.x, + 'y': npc_loc_ego_space.y, + 'z': npc_loc_ego_space.z + }, + 'dimensions': { + 'length': actor.bounding_box.extent.x*2, + 'width': actor.bounding_box.extent.y*2, + 'height': actor.bounding_box.extent.z*2, + }, + 'rotation_yaw': radians(actor.get_transform().rotation.yaw - ego.get_transform().rotation.yaw) + }, + 'projection': projection + } + +# Visualize 2D bounding boxes in Pygame +def visualize_2d_bboxes(surface, img, bboxes): + + rgb_img = img[:, :, :3][:, :, ::-1] + frame_surface = pygame.surfarray.make_surface(np.transpose(rgb_img[..., 0:3], (1,0,2))) + surface.blit(frame_surface, (0, 0)) + + font = pygame.font.SysFont("Arial", 18) + + for item in bboxes: + bbox = item['2d'] + if bbox is not None: + xmin, ymin, xmax, ymax = [int(v) for v in bbox['bbox_2d']] + label = SEMANTIC_MAP[bbox['semantic_label']][0] + color = SEMANTIC_MAP[bbox['semantic_label']][1] + pygame.draw.rect(surface, color, pygame.Rect(xmin, ymin, xmax-xmin, ymax-ymin), 2) + text_surface = font.render(label, True, (255,255,255), color) + text_rect = text_surface.get_rect(topleft=(xmin, ymin-20)) + surface.blit(text_surface, text_rect) + + return surface + +# Visualize 3D bounding boxes in Pygame +def visualize_3d_bboxes(surface, img, bboxes): + + rgb_img = img[:, :, :3][:, :, ::-1] + frame_surface = pygame.surfarray.make_surface(np.transpose(rgb_img[..., 0:3], (1,0,2))) + surface.blit(frame_surface, (0, 0)) + + for item in bboxes: + bbox = item['3d'] + color = SEMANTIC_MAP[bbox['semantic_label']][1] + + n = 0 + mean_x = 0 + mean_y = 0 + for line in bbox['projection']: + mean_x += line[0] + mean_y += line[1] + n += 1 + pygame.draw.line(surface, color, (line[0], line[1]), (line[2],line[3]), 2) + + if n > 0: + mean_x /= n + mean_y /= n + + # --- Render label --- + font = pygame.font.SysFont("Arial", 18) + text_surface = font.render(SEMANTIC_MAP[bbox['semantic_label']][0], True, (255,255,255), color) # black text, filled bg + text_rect = text_surface.get_rect(topleft=(mean_x, mean_y)) + surface.blit(text_surface, text_rect) + +def calculate_relative_velocity(actor, ego): + # Calculate the relative velocity in world frame + rel_vel = actor.get_velocity() - ego.get_velocity() + # Now convert to local frame of ego + vel_ego_frame = ego.get_transform().inverse_transform(rel_vel) + + return { + 'x': vel_ego_frame.x, + 'y': vel_ego_frame.y, + 'z': vel_ego_frame.z + } + +def vehicle_light_state_to_dict(vehicle: carla.Vehicle): + state = vehicle.get_light_state() + return { + "position": bool(state & carla.VehicleLightState.Position), + "low_beam": bool(state & carla.VehicleLightState.LowBeam), + "high_beam": bool(state & carla.VehicleLightState.HighBeam), + "brake": bool(state & carla.VehicleLightState.Brake), + "reverse": bool(state & carla.VehicleLightState.Reverse), + "left_blinker": bool(state & carla.VehicleLightState.LeftBlinker), + "right_blinker":bool(state & carla.VehicleLightState.RightBlinker), + "fog": bool(state & carla.VehicleLightState.Fog), + "interior": bool(state & carla.VehicleLightState.Interior), + "special1": bool(state & carla.VehicleLightState.Special1), + "special2": bool(state & carla.VehicleLightState.Special2), + } + +def main(): + + argparser = argparse.ArgumentParser( + description='CARLA bounding boxes') + 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( + '-d', '--distance', + metavar='D', + default=50, + type=int, + help='Actor distance threshold') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + pygame.init() + + # State variables + record = False + display_3d = False + run_simulation = True + + clock = pygame.time.Clock() + pygame.display.set_caption("Bounding Box Visualization") + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + display.fill((0,0,0)) + pygame.display.flip() + + # Connect to the CARLA server and get the world object + client = carla.Client(args.host, args.port) + world = client.get_world() + + # Set up the simulator in synchronous mode + settings = world.get_settings() + settings.synchronous_mode = True # Enables synchronous mode + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + # Set the traffic manager to Synchronous mode + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + + bp_lib = world.get_blueprint_library() + + # Get the map spawn points + spawn_points = world.get_map().get_spawn_points() + + # spawn vehicle + vehicle_bp =bp_lib.find('vehicle.lincoln.mkz_2020') + ego_vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + + # spawn RGB camera + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(args.width)) + camera_bp.set_attribute('image_size_y', str(args.height)) + camera_init_trans = carla.Transform(carla.Location(z=2)) + camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=ego_vehicle) + + # spawn instance segmentation camera + inst_camera_bp = bp_lib.find('sensor.camera.instance_segmentation') + inst_camera_bp.set_attribute('image_size_x', str(args.width)) + inst_camera_bp.set_attribute('image_size_y', str(args.height)) + camera_init_trans = carla.Transform(carla.Location(z=2)) + inst_camera = world.spawn_actor(inst_camera_bp, camera_init_trans, attach_to=ego_vehicle) + + ego_vehicle.set_autopilot(True) + + # Add some traffic + npcs = [] + for i in range(100): + vehicle_bp = random.choice(bp_lib.filter('vehicle')) + npc = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points)) + if npc: + npc.set_autopilot(True) + npcs.append(npc) + + # Create queues to store and retrieve the sensor data + image_queue = queue.Queue() + camera.listen(image_queue.put) + + inst_queue = queue.Queue() + inst_camera.listen(inst_queue.put) + + try: + while run_simulation: + for event in pygame.event.get(): + if event.type == pygame.KEYUP: + if event.key == K_r: + record = True + if event.key == K_2: + display_3d = False + if event.key == K_3: + display_3d = True + if event.key == K_ESCAPE: + run_simulation = False + if event.type == pygame.QUIT: + run_simulation = False + + world.tick() + snapshot = world.get_snapshot() + + json_frame_data = { + 'frame_id': snapshot.frame, + 'timestamp': snapshot.timestamp.elapsed_seconds, + 'objects': [] + } + + image = image_queue.get() + img = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)) + + if record: + image.save_to_disk('_out/%08d' % image.frame) + + inst_seg_image = inst_queue.get() + inst_seg = np.reshape(np.copy(inst_seg_image.raw_data), (inst_seg_image.height, inst_seg_image.width, 4)) + + # Decode instance segmentation image + semantic_labels, actor_ids = decode_instance_segmentation(inst_seg) + + # Empty list to collect bounding boxes for this frame + frame_bboxes = [] + + # Loop through the NPCs in the simulation + for npc in world.get_actors().filter('*vehicle*'): + + # Filter out the ego vehicle + if npc.id !=ego_vehicle.id: + + npc_bbox = npc.bounding_box + dist = npc.get_transform().location.distance(ego_vehicle.get_transform().location) + + # Filter for the vehicles within 50m + if dist < args.distance: + + # Limit to vehicles in front of the camera + forward_vec = camera.get_transform().get_forward_vector() + inter_vehicle_vec = npc.get_transform().location - camera.get_transform().location + + if forward_vec.dot(inter_vehicle_vec) > 0: + + # Generate 2D and 2D bounding boxes for each actor + npc_bbox_2d = bbox_2d_for_actor(npc, actor_ids, semantic_labels) + npc_bbox_3d = bbox_3d_for_actor(npc, ego_vehicle, camera_bp, camera) + + frame_bboxes.append({'3d': npc_bbox_3d, '2d': npc_bbox_2d}) + + json_frame_data['objects'].append({ + 'id': npc.id, + 'class': SEMANTIC_MAP[npc.semantic_tags[0]][0], + 'blueprint_id': npc.type_id, + 'velocity': calculate_relative_velocity(npc, ego_vehicle), + 'bbox_3d': npc_bbox_3d['bbox_3d'], + 'bbox_2d': { + 'xmin': int(npc_bbox_2d['bbox_2d'][0]), + 'ymin': int(npc_bbox_2d['bbox_2d'][1]), + 'xmax': int(npc_bbox_2d['bbox_2d'][2]), + 'ymax': int(npc_bbox_2d['bbox_2d'][3]), + } if npc_bbox_2d else None, + 'light_state': vehicle_light_state_to_dict(npc) + + }) + + # Draw the scene in Pygame + display.fill((0,0,0)) + if display_3d: + visualize_3d_bboxes(display, img, frame_bboxes) + else: + visualize_2d_bboxes(display, img, frame_bboxes) + pygame.display.flip() + clock.tick(30) # 30 FPS + if record: + with open(f"_out/{snapshot.frame}.json", 'w') as f: + json.dump(json_frame_data, f) + + except KeyboardInterrupt: + pass + finally: + + ego_vehicle.destroy() + camera.stop() + camera.destroy() + inst_camera.stop() + inst_camera.destroy() + for npc in npcs: + npc.set_autopilot(False) + npc.destroy() + + world.tick() + + # Set up the simulator in synchronous mode + settings = world.get_settings() + settings.synchronous_mode = False # Disables synchronous mode + settings.fixed_delta_seconds = None + world.apply_settings(settings) + + # Set the traffic manager to Synchronous mode + traffic_manager.set_synchronous_mode(False) + + pygame.quit() + + print('\ndone.') + + +if __name__ == '__main__': + print('Bounding boxes script instructions:') + print('R : toggle recording images as PNG and bounding boxes as JSON') + print('3 : view the bounding boxes in 3D') + print('2 : view the bounding boxes in 2D') + print('ESC : quit') + main() diff --git a/carla_examples/carla_cosmos_gen.py b/carla_examples/carla_cosmos_gen.py new file mode 100644 index 0000000..1080c80 --- /dev/null +++ b/carla_examples/carla_cosmos_gen.py @@ -0,0 +1,376 @@ +#!/usr/bin/env python3 + +import argparse +import multiprocessing as mp +import logging +from enum import Enum +from dataclasses import dataclass +from typing import Dict, List, Sequence, Tuple +from pathlib import Path +import yaml +import subprocess +import numpy as np +import cv2 +import carla +from PIL import Image + +# === ENUMS AND DATA STRUCTURES === +class AOV(Enum): + RGB = 0 + DEPTH = 1 + SEMANTIC_SEGMENTATION = 2 + INSTANCE_SEGMENTATION = 3 + NORMALS = 4 + COSMOS_VISUALIZATION = 5 + +@dataclass +class FrameBundle: + index: int + frames: Dict[AOV, np.ndarray] + timestamp: float + +def extract_between(input_string, left_delim, right_delim): + try: + start = input_string.index(left_delim) + len(left_delim) + end = input_string.index(right_delim, start) + return input_string[start:end] + except ValueError: + return None + + +def parse_frames_duration(info): + frames = extract_between(info, "Frames: ", "\n") + duration = extract_between(info, "Duration: ", " seconds") + + if frames and duration: + return int(frames), float(duration) + else: + return -1, -1.0 + +# === CONFIGURATION LOADERS === +CLASSES_TO_KEEP_SHADED_SEG: List[Sequence[int]] = [] +CLASSES_TO_KEEP_CANNY: List[Sequence[int]] = [] + +def load_class_filter_config(path: str): + with open(path, 'r') as f: + config = yaml.safe_load(f) + global CLASSES_TO_KEEP_SHADED_SEG, CLASSES_TO_KEEP_CANNY + CLASSES_TO_KEEP_SHADED_SEG = config.get('shaded_segmentation_classes', []) + CLASSES_TO_KEEP_CANNY = config.get('canny_classes', []) + +# === ORIGINAL POST-PROCESSING FUNCTIONS === +def masked_edges_from_semseg( + rgb_img: np.ndarray, + semseg_img: np.ndarray, + classes: List[Sequence[int]], + *, + gaussian_kernel: Tuple[int, int] = (5, 5), + gaussian_sigma: float = 1.0, + canny_thresh1: int = 100, + canny_thresh2: int = 200, +) -> Tuple[np.ndarray, np.ndarray]: + blurred_rgb = cv2.GaussianBlur(rgb_img, gaussian_kernel, gaussian_sigma) + mask = np.zeros(semseg_img.shape[:2], dtype=np.uint8) + for color in classes: + lower = np.array(color, dtype=np.uint8) + upper = np.array(color, dtype=np.uint8) + mask |= cv2.inRange(semseg_img, lower, upper) + mask_bool = mask.astype(bool) + masked_rgb = np.zeros_like(rgb_img) + masked_rgb[mask_bool] = blurred_rgb[mask_bool] + gray = cv2.cvtColor(masked_rgb, cv2.COLOR_RGB2GRAY) + edges = cv2.Canny(gray, canny_thresh1, canny_thresh2) + return masked_rgb, edges + + +def created_shaded_composition( + sem: np.ndarray, inst: np.ndarray, nor: np.ndarray, classes_to_keep: List[Sequence[int]] +) -> np.ndarray: + semantics = sem[..., ::-1] + instances = inst[..., ::-1] + normals = nor[..., ::-1] + light_source = np.array([1.0, 0.0, 0.0]) + mask = np.zeros(semantics.shape[:2], dtype=bool) + for color in classes_to_keep: + mask |= (semantics == np.array(color)).all(-1) + mask_exp = mask[..., None] + composed = np.where(mask_exp, semantics, instances) + normals_f = normals.astype(np.float32) / 255.0 + shading = np.dot(normals_f, light_source) + shaded_seg = (composed.astype(np.float32) * shading[..., None]).astype(np.uint8) + return shaded_seg + + +def create_shuffled_colormap( + size=65536, base_cmap_name='prism', seed=None, fix_zero=True +) -> np.ndarray: + import matplotlib.pyplot as plt + if seed is not None: + np.random.seed(seed) + try: + cmap_func = plt.get_cmap(base_cmap_name) + except ValueError: + cmap_func = plt.get_cmap('turbo') + base_colors = cmap_func(np.linspace(0, 1, size))[:, :3] + indices = np.arange(size) + if fix_zero: + shuffled = np.concatenate(([0], np.random.permutation(indices[1:]))) + else: + shuffled = np.random.permutation(indices) + shuffled_colors = base_colors[shuffled] + colormap_uint8 = (shuffled_colors * 255).astype(np.uint8) + if fix_zero: + colormap_uint8[0] = [0, 0, 0] + return colormap_uint8 + + +def reconstruct_ids_vectorized(image_data_uint8: np.ndarray) -> np.ndarray: + low = image_data_uint8[:, :, 1].astype(np.uint16) + high = image_data_uint8[:, :, 2].astype(np.uint16) + return (high << 8) | low + + +def apply_colormap_vectorized(ids_uint16: np.ndarray, colormap: np.ndarray) -> np.ndarray: + return colormap[ids_uint16] + + +def depth_to_log_grayscale( + depth_map: np.ndarray, + near_clip=0.01, + far_clip=1000.0, + inverted_depth=True +) -> Image.Image: + clipped = np.clip(depth_map, near_clip, far_clip) + log_depth = np.log(clipped) + norm_log = (log_depth - np.log(near_clip)) / (np.log(far_clip) - np.log(near_clip)) + if inverted_depth: + norm_log = 1.0 - norm_log + gray_img = (norm_log * 255).astype(np.uint8) + return Image.fromarray(gray_img) + +# Pre-generate colormap for instance segmentation +colormap_uint8 = create_shuffled_colormap(seed=140) + +# === SENSOR INFO WRAPPER === +class SensorInfo: + def __init__(self, sensor, stype: AOV): + self.sensor = sensor + self.sensor_type = stype + self.queue = mp.Queue() + sensor.listen(self._callback) + + def _callback(self, data): + conv_map = { + AOV.RGB: carla.ColorConverter.Raw, + AOV.SEMANTIC_SEGMENTATION: carla.ColorConverter.CityScapesPalette, + AOV.COSMOS_VISUALIZATION: carla.ColorConverter.Raw + } + conv = conv_map.get(self.sensor_type, carla.ColorConverter.Raw) + data.convert(conv) + arr = np.frombuffer(data.raw_data, dtype=np.uint8) + h, w = data.height, data.width + raw = arr.reshape((h, w, 4)) + img = raw if self.sensor_type == AOV.DEPTH else raw[:, :, :3] + self.queue.put((img.copy(), data.frame, data.timestamp)) + + def capture_current_frame(self): + try: + return self.queue.get(timeout=1.0) + except Exception: + return None + +# === WORKERS === + +def post_processing_worker(raw_q: mp.Queue, proc_q: mp.Queue): + logging.info(f"[{mp.current_process().name}] starting") + while True: + bundle = raw_q.get() + if bundle is None: + break + processed = {} + frames = bundle.frames + if AOV.RGB in frames: + processed['RGB'] = frames[AOV.RGB] + if AOV.RGB in frames and AOV.SEMANTIC_SEGMENTATION in frames: + masked, edges = masked_edges_from_semseg( + frames[AOV.RGB], frames[AOV.SEMANTIC_SEGMENTATION], CLASSES_TO_KEEP_CANNY + ) + processed['RGB_MASKED'] = masked + processed['RGB_EDGES'] = cv2.cvtColor(edges, cv2.COLOR_GRAY2RGB) + if AOV.DEPTH in frames: + depth_bgra = frames[AOV.DEPTH] + scales = np.array([65536.0, 256.0, 1.0, 0.0]) / (256**3 - 1) * 1000 + depth_map = np.dot(depth_bgra, scales).astype(np.float32) + gray_img = depth_to_log_grayscale(depth_map) + processed['DEPTH'] = np.array(gray_img.convert('RGB')) + if AOV.SEMANTIC_SEGMENTATION in frames: + processed['SEMANTIC_SEGMENTATION'] = frames[AOV.SEMANTIC_SEGMENTATION] + if AOV.INSTANCE_SEGMENTATION in frames: + ids = reconstruct_ids_vectorized(frames[AOV.INSTANCE_SEGMENTATION]) + colored = apply_colormap_vectorized(ids, colormap_uint8) + processed['INSTANCE_SEGMENTATION'] = colored + if AOV.COSMOS_VISUALIZATION in frames: + processed['COSMOS_VISUALIZATION'] = frames[AOV.COSMOS_VISUALIZATION] + proc_q.put((bundle.index, processed)) + logging.info(f"[{mp.current_process().name}] exiting") + + +def video_writer_worker(proc_q: mp.Queue, out_dir: Path, fps: float): + logging.info("[Writer] starting") + writers = {} + paths = {} + write_count = 0 + + def get_writer(key: str, shape: Tuple[int, int]): + if key not in writers: + tmp = out_dir / f"{key.lower()}_tmp.mp4" + final = out_dir / f"{key.lower()}.mp4" + fourcc = cv2.VideoWriter_fourcc(*'mp4v') + w = cv2.VideoWriter(str(tmp), fourcc, fps, (shape[1], shape[0])) + writers[key] = w + paths[key] = (tmp, final) + return writers[key] + + while True: + item = proc_q.get() + if item is None: + break + idx, frames = item + for key, img in frames.items(): + get_writer(key, img.shape[:2]).write(img) + write_count += 1 + if write_count % 100 == 0: + logging.info(f"[Writer] wrote {write_count} frames total") + + for key, w in writers.items(): + w.release() + tmp, final = paths[key] + try: + subprocess.run(['ffmpeg', '-i', str(tmp), '-r', '24', '-c:v', 'libx264', + '-y', '-loglevel', 'error', str(final)], check=True, + stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + except subprocess.CalledProcessError as e: + logging.error(f"FFmpeg failed for {key}: {e}") + tmp.unlink(missing_ok=True) + logging.info("[Writer] exiting") + +# === MAIN === +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--sensors', type=str, required=True) + parser.add_argument('--class-filter-config', type=str) + parser.add_argument('-f','--recorder-filename', type=str, required=True) + parser.add_argument('-o','--output-dir', type=str, required=True) + parser.add_argument('-s','--start', type=float, default=0.0) + parser.add_argument('-d','--duration', type=float, default=0.0) + parser.add_argument('--host', type=str, default='127.0.0.1') + parser.add_argument('--port', type=int, default=2000) + parser.add_argument('-c','--camera', type=int, default=0) + parser.add_argument('--time-factor', type=float, default=1.0) + parser.add_argument('--ignore-hero', action='store_true') + parser.add_argument('--move-spectator', action='store_true') + parser.add_argument('--spawn-sensors', action='store_true') + parser.add_argument('--num-post-workers', type=int, default=max(1, mp.cpu_count()-1)) + args = parser.parse_args() + + logging.basicConfig( + level=logging.INFO, + format='%(asctime)s %(levelname)s %(processName)s: %(message)s' + ) + logging.info("Starting CarlaCosmos-DataAcquisition parallel pipeline") + + if args.class_filter_config: + load_class_filter_config(args.class_filter_config) + + client = carla.Client(args.host, args.port) + client.set_timeout(60.0) + client.reload_world() + + info = client.show_recorder_file_info(args.recorder_filename, False) + log_frames, log_duration = parse_frames_duration(info) + + log_delta = log_duration / log_frames + fps = round(1.0 / log_delta) + logging.info(f"Recorder: {log_frames} frames, {log_duration:.2f}s, fps={fps}") + + client.set_replayer_time_factor(args.time_factor) + client.set_replayer_ignore_hero(args.ignore_hero) + client.set_replayer_ignore_spectator(not args.move_spectator) + client.replay_file( + args.recorder_filename, args.start, args.duration, args.camera, args.spawn_sensors + ) + + world = client.get_world() + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = log_delta + world.apply_settings(settings) + + with open(args.sensors.replace('file:',''), 'r') as f: + sensor_cfg = yaml.safe_load(f) + vehicle = world.get_actor(args.camera) + sensor_infos = [] + for entry in sensor_cfg: + bp = world.get_blueprint_library().find(f"sensor.camera.{entry['sensor']}") + for k,v in entry.get('attributes',{}).items(): bp.set_attribute(k,str(v)) + tf = entry.get('transform',{}) + transform = carla.Transform( + carla.Location(**tf.get('location',{})), + carla.Rotation(**tf.get('rotation',{})) + ) + sensor = world.spawn_actor(bp, transform, attach_to=vehicle) + + # If it's the cosmos visualization sensor, set it to ignore the ego vehicle + if entry['sensor'].upper() == 'COSMOS_VISUALIZATION': + sensor.set_ignored_vehicles([args.camera]) # Only this sensor ignores ego + + sensor_infos.append(SensorInfo(sensor, AOV[entry['sensor'].upper()])) + + raw_q = mp.Queue() + proc_q = mp.Queue() + workers = [] + for i in range(args.num_post_workers): + p = mp.Process( + target=post_processing_worker, + args=(raw_q, proc_q), + name=f"PostProc-{i}" + ) + p.start(); workers.append(p) + + out_dir = Path(args.output_dir) + out_dir.mkdir(parents=True, exist_ok=True) + writer = mp.Process( + target=video_writer_worker, + args=(proc_q, out_dir, fps), + name="Writer" + ) + writer.start() + + timestamp = args.start + total = log_duration if args.duration == 0.0 else args.duration + frame_count = 0 + try: + while timestamp < args.start + total: + idx = world.tick() + frame_dict = {} + for si in sensor_infos: + res = si.capture_current_frame() + if res: + img,_,_ = res + frame_dict[si.sensor_type] = img + raw_q.put(FrameBundle(idx, frame_dict, timestamp)) + frame_count += 1 + if frame_count % 100 == 0: + logging.info(f"Queued frame {frame_count}, timestamp={timestamp:.3f}, idx={idx}") + timestamp += log_delta + finally: + for _ in workers: raw_q.put(None) + for p in workers: p.join() + proc_q.put(None); writer.join() + client.stop_replayer(keep_actors=False) + for si in sensor_infos: si.sensor.stop(); si.sensor.destroy() + settings.synchronous_mode = False; settings.fixed_delta_seconds = None; world.apply_settings(settings) + logging.info("Finished CarlaCosmos-DataAcquisition parallel pipeline") + +if __name__ == '__main__': + main() diff --git a/carla_examples/client_bounding_boxes.py b/carla_examples/client_bounding_boxes.py new file mode 100644 index 0000000..e641b68 --- /dev/null +++ b/carla_examples/client_bounding_boxes.py @@ -0,0 +1,381 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Aptiv +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +An example of client-side bounding boxes with basic car controls. + +Controls: + + W : throttle + S : brake + AD : steer + Space : hand-brake + + ESC : quit +""" + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + +import carla + +import weakref +import random + +try: + import pygame + from pygame.locals import K_ESCAPE + from pygame.locals import K_SPACE + from pygame.locals import K_a + from pygame.locals import K_d + 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') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +VIEW_WIDTH = 1920//2 +VIEW_HEIGHT = 1080//2 +VIEW_FOV = 90 + +BB_COLOR = (248, 64, 24) + +# ============================================================================== +# -- ClientSideBoundingBoxes --------------------------------------------------- +# ============================================================================== + + +class ClientSideBoundingBoxes(object): + """ + This is a module responsible for creating 3D bounding boxes and drawing them + client-side on pygame surface. + """ + + @staticmethod + def get_bounding_boxes(vehicles, camera): + """ + Creates 3D bounding boxes based on carla vehicle list and camera. + """ + + bounding_boxes = [ClientSideBoundingBoxes.get_bounding_box(vehicle, camera) for vehicle in vehicles] + # filter objects behind camera + bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)] + return bounding_boxes + + @staticmethod + def draw_bounding_boxes(display, bounding_boxes): + """ + Draws bounding boxes on pygame display. + """ + + bb_surface = pygame.Surface((VIEW_WIDTH, VIEW_HEIGHT)) + bb_surface.set_colorkey((0, 0, 0)) + for bbox in bounding_boxes: + points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)] + # draw lines + # base + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1]) + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1]) + pygame.draw.line(bb_surface, BB_COLOR, points[1], points[2]) + pygame.draw.line(bb_surface, BB_COLOR, points[2], points[3]) + pygame.draw.line(bb_surface, BB_COLOR, points[3], points[0]) + # top + pygame.draw.line(bb_surface, BB_COLOR, points[4], points[5]) + pygame.draw.line(bb_surface, BB_COLOR, points[5], points[6]) + pygame.draw.line(bb_surface, BB_COLOR, points[6], points[7]) + pygame.draw.line(bb_surface, BB_COLOR, points[7], points[4]) + # base-top + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[4]) + pygame.draw.line(bb_surface, BB_COLOR, points[1], points[5]) + pygame.draw.line(bb_surface, BB_COLOR, points[2], points[6]) + pygame.draw.line(bb_surface, BB_COLOR, points[3], points[7]) + display.blit(bb_surface, (0, 0)) + + @staticmethod + def get_bounding_box(vehicle, camera): + """ + Returns 3D bounding box for a vehicle based on camera view. + """ + + bb_cords = ClientSideBoundingBoxes._create_bb_points(vehicle) + cords_x_y_z = ClientSideBoundingBoxes._vehicle_to_sensor(bb_cords, vehicle, camera)[:3, :] + cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) + bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x)) + camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1) + return camera_bbox + + @staticmethod + def _create_bb_points(vehicle): + """ + Returns 3D bounding box for a vehicle. + """ + + cords = np.zeros((8, 4)) + extent = vehicle.bounding_box.extent + cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1]) + cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1]) + cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1]) + cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1]) + cords[4, :] = np.array([extent.x, extent.y, extent.z, 1]) + cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1]) + cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1]) + cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1]) + return cords + + @staticmethod + def _vehicle_to_sensor(cords, vehicle, sensor): + """ + Transforms coordinates of a vehicle bounding box to sensor. + """ + + world_cord = ClientSideBoundingBoxes._vehicle_to_world(cords, vehicle) + sensor_cord = ClientSideBoundingBoxes._world_to_sensor(world_cord, sensor) + return sensor_cord + + @staticmethod + def _vehicle_to_world(cords, vehicle): + """ + Transforms coordinates of a vehicle bounding box to world. + """ + + bb_transform = carla.Transform(vehicle.bounding_box.location) + bb_vehicle_matrix = ClientSideBoundingBoxes.get_matrix(bb_transform) + vehicle_world_matrix = ClientSideBoundingBoxes.get_matrix(vehicle.get_transform()) + bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix) + world_cords = np.dot(bb_world_matrix, np.transpose(cords)) + return world_cords + + @staticmethod + def _world_to_sensor(cords, sensor): + """ + Transforms world coordinates to sensor. + """ + + sensor_world_matrix = ClientSideBoundingBoxes.get_matrix(sensor.get_transform()) + world_sensor_matrix = np.linalg.inv(sensor_world_matrix) + sensor_cords = np.dot(world_sensor_matrix, cords) + return sensor_cords + + @staticmethod + def get_matrix(transform): + """ + Creates matrix from carla transform. + """ + + rotation = transform.rotation + location = transform.location + c_y = np.cos(np.radians(rotation.yaw)) + s_y = np.sin(np.radians(rotation.yaw)) + c_r = np.cos(np.radians(rotation.roll)) + s_r = np.sin(np.radians(rotation.roll)) + c_p = np.cos(np.radians(rotation.pitch)) + s_p = np.sin(np.radians(rotation.pitch)) + matrix = np.matrix(np.identity(4)) + matrix[0, 3] = location.x + matrix[1, 3] = location.y + matrix[2, 3] = location.z + matrix[0, 0] = c_p * c_y + matrix[0, 1] = c_y * s_p * s_r - s_y * c_r + matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r + matrix[1, 0] = s_y * c_p + matrix[1, 1] = s_y * s_p * s_r + c_y * c_r + matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r + matrix[2, 0] = s_p + matrix[2, 1] = -c_p * s_r + matrix[2, 2] = c_p * c_r + return matrix + + +# ============================================================================== +# -- BasicSynchronousClient ---------------------------------------------------- +# ============================================================================== + + +class BasicSynchronousClient(object): + """ + Basic implementation of a synchronous client. + """ + + def __init__(self): + self.client = None + self.world = None + self.camera = None + self.car = None + + self.display = None + self.image = None + self.capture = True + + def camera_blueprint(self): + """ + Returns camera blueprint. + """ + + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH)) + camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT)) + camera_bp.set_attribute('fov', str(VIEW_FOV)) + return camera_bp + + def set_synchronous_mode(self, synchronous_mode): + """ + Sets synchronous mode. + """ + + settings = self.world.get_settings() + settings.synchronous_mode = synchronous_mode + self.world.apply_settings(settings) + + def setup_car(self): + """ + Spawns actor-vehicle to be controled. + """ + + car_bp = self.world.get_blueprint_library().filter('vehicle.*')[0] + location = random.choice(self.world.get_map().get_spawn_points()) + self.car = self.world.spawn_actor(car_bp, location) + + def setup_camera(self): + """ + Spawns actor-camera to be used to render view. + Sets calibration for client-side boxes rendering. + """ + + camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)) + self.camera = self.world.spawn_actor(self.camera_blueprint(), camera_transform, attach_to=self.car) + weak_self = weakref.ref(self) + self.camera.listen(lambda image: weak_self().set_image(weak_self, image)) + + calibration = np.identity(3) + calibration[0, 2] = VIEW_WIDTH / 2.0 + calibration[1, 2] = VIEW_HEIGHT / 2.0 + calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0)) + self.camera.calibration = calibration + + def control(self, car): + """ + Applies control to main car based on pygame pressed keys. + Will return True If ESCAPE is hit, otherwise False to end main loop. + """ + + keys = pygame.key.get_pressed() + if keys[K_ESCAPE]: + return True + + control = car.get_control() + control.throttle = 0 + if keys[K_w]: + control.throttle = 1 + control.reverse = False + elif keys[K_s]: + control.throttle = 1 + control.reverse = True + if keys[K_a]: + control.steer = max(-1., min(control.steer - 0.05, 0)) + elif keys[K_d]: + control.steer = min(1., max(control.steer + 0.05, 0)) + else: + control.steer = 0 + control.hand_brake = keys[K_SPACE] + + car.apply_control(control) + return False + + @staticmethod + def set_image(weak_self, img): + """ + Sets image coming from camera sensor. + The self.capture flag is a mean of synchronization - once the flag is + set, next coming image will be stored. + """ + + self = weak_self() + if self.capture: + self.image = img + self.capture = False + + def render(self, display): + """ + Transforms image from camera sensor and blits it to main pygame display. + """ + + if self.image is not None: + array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (self.image.height, self.image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + display.blit(surface, (0, 0)) + + def game_loop(self): + """ + Main program loop. + """ + + try: + pygame.init() + + self.client = carla.Client('127.0.0.1', 2000) + self.client.set_timeout(2.0) + self.world = self.client.get_world() + + self.setup_car() + self.setup_camera() + + self.display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF) + pygame_clock = pygame.time.Clock() + + self.set_synchronous_mode(True) + vehicles = self.world.get_actors().filter('vehicle.*') + + while True: + self.world.tick() + + self.capture = True + pygame_clock.tick_busy_loop(20) + + self.render(self.display) + bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(vehicles, self.camera) + ClientSideBoundingBoxes.draw_bounding_boxes(self.display, bounding_boxes) + + pygame.display.flip() + + pygame.event.pump() + if self.control(self.car): + return + + finally: + self.set_synchronous_mode(False) + self.camera.destroy() + self.car.destroy() + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + """ + Initializes the client-side bounding box demo. + """ + + try: + client = BasicSynchronousClient() + client.game_loop() + finally: + print('EXIT') + + +if __name__ == '__main__': + main() diff --git a/carla_examples/cosmos_aov.yaml b/carla_examples/cosmos_aov.yaml new file mode 100644 index 0000000..aa13cf4 --- /dev/null +++ b/carla_examples/cosmos_aov.yaml @@ -0,0 +1,84 @@ +- sensor: rgb + attributes: + image_size_x: 1920 + image_size_y: 1080 + fov: 110 + transform: + location: + x: 0 + y: 0 + z: 1.8 + rotation: + pitch: 8 + yaw: 0 + roll: 0 +- sensor: normals + attributes: + image_size_x: 1920 + image_size_y: 1080 + fov: 110 + transform: + location: + x: 0 + y: 0 + z: 1.8 + rotation: + pitch: 8 + yaw: 0 + roll: 0 +- sensor: depth + attributes: + image_size_x: 1920 + image_size_y: 1080 + fov: 110 + transform: + location: + x: 0 + y: 0 + z: 1.8 + rotation: + pitch: 8 + yaw: 0 + roll: 0 +- sensor: semantic_segmentation + attributes: + image_size_x: 1920 + image_size_y: 1080 + fov: 110 + transform: + location: + x: 0 + y: 0 + z: 1.8 + rotation: + pitch: 8 + yaw: 0 + roll: 0 +- sensor: instance_segmentation + attributes: + image_size_x: 1920 + image_size_y: 1080 + fov: 110 + transform: + location: + x: 0 + y: 0 + z: 1.8 + rotation: + pitch: 8 + yaw: 0 + roll: 0 +- sensor: cosmos_visualization + attributes: + image_size_x: 1920 + image_size_y: 1080 + fov: 110 + transform: + location: + x: 0 + y: 0 + z: 1.8 + rotation: + pitch: 8 + yaw: 0 + roll: 0 diff --git a/carla_examples/draw_skeleton.py b/carla_examples/draw_skeleton.py new file mode 100644 index 0000000..d78bedf --- /dev/null +++ b/carla_examples/draw_skeleton.py @@ -0,0 +1,418 @@ +#!/usr/bin/env python +# Copyright (c) 2021 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 . + +import math +import argparse +import copy +from multiprocessing import Pool +from PIL import Image + +import carla +import random + +try: + import pygame +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') + +try: + import queue +except ImportError: + import Queue as queue + + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + def tick(self, timeout): + self.frame = self.world.tick() + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + # --------------- + +def build_projection_matrix(w, h, fov): + focal = w / (2.0 * np.tan(fov * np.pi / 360.0)) + K = np.identity(3) + K[0, 0] = K[1, 1] = focal + K[0, 2] = w / 2.0 + K[1, 2] = h / 2.0 + return K + +def get_image_as_array(image): + 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] + # make the array writeable doing a deep copy + array2 = copy.deepcopy(array) + return array2 + +def draw_image(surface, array, blend=False): + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + +def get_screen_points(camera, K, image_w, image_h, points3d): + + # get 4x4 matrix to transform points from world to camera coordinates + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # build the points array in numpy format as (x, y, z, 1) to be operable with a 4x4 matrix + points_temp = [] + for p in points3d: + points_temp += [p.x, p.y, p.z, 1] + points = np.array(points_temp).reshape(-1, 4).T + + # convert world points to camera space + points_camera = np.dot(world_2_camera, points) + + # New we must change from UE4's coordinate system to an "standard" + # (x, y ,z) -> (y, -z, x) + # and we remove the fourth component also + points = np.array([ + points_camera[1], + points_camera[2] * -1, + points_camera[0]]) + + # Finally we can use our K matrix to do the actual 3D -> 2D. + points_2d = np.dot(K, points) + + # normalize the values and transpose + points_2d = np.array([ + points_2d[0, :] / points_2d[2, :], + points_2d[1, :] / points_2d[2, :], + points_2d[2, :]]).T + + return points_2d + +def draw_points_on_buffer(buffer, image_w, image_h, points_2d, color, size=4): + half = int(size / 2) + # draw each point + for p in points_2d: + x = int(p[0]) + y = int(p[1]) + for j in range(y - half, y + half): + if (j >=0 and j =0 and i = dy): + err += dy + x0 += sx + if (e2 <= dx): + err += dx + y0 += sy + +def draw_skeleton(buffer, image_w, image_h, boneIndex, points2d, color, size=4): + try: + # draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_root"]], points2d[boneIndex["crl_hips__C"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hips__C"]], points2d[boneIndex["crl_spine__C"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hips__C"]], points2d[boneIndex["crl_thigh__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hips__C"]], points2d[boneIndex["crl_thigh__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_spine__C"]], points2d[boneIndex["crl_spine01__C"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_spine01__C"]], points2d[boneIndex["crl_shoulder__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_spine01__C"]], points2d[boneIndex["crl_neck__C"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_spine01__C"]], points2d[boneIndex["crl_shoulder__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_shoulder__L"]], points2d[boneIndex["crl_arm__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_arm__L"]], points2d[boneIndex["crl_foreArm__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_foreArm__L"]], points2d[boneIndex["crl_hand__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__L"]], points2d[boneIndex["crl_handThumb__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__L"]], points2d[boneIndex["crl_handIndex__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__L"]], points2d[boneIndex["crl_handMiddle__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__L"]], points2d[boneIndex["crl_handRing__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__L"]], points2d[boneIndex["crl_handPinky__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handThumb__L"]], points2d[boneIndex["crl_handThumb01__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handThumb01__L"]], points2d[boneIndex["crl_handThumb02__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handThumb02__L"]], points2d[boneIndex["crl_handThumbEnd__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handIndex__L"]], points2d[boneIndex["crl_handIndex01__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handIndex01__L"]], points2d[boneIndex["crl_handIndex02__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handIndex02__L"]], points2d[boneIndex["crl_handIndexEnd__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handMiddle__L"]], points2d[boneIndex["crl_handMiddle01__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handMiddle01__L"]], points2d[boneIndex["crl_handMiddle02__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handMiddle02__L"]], points2d[boneIndex["crl_handMiddleEnd__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handRing__L"]], points2d[boneIndex["crl_handRing01__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handRing01__L"]], points2d[boneIndex["crl_handRing02__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handRing02__L"]], points2d[boneIndex["crl_handRingEnd__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handPinky__L"]], points2d[boneIndex["crl_handPinky01__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handPinky01__L"]], points2d[boneIndex["crl_handPinky02__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handPinky02__L"]], points2d[boneIndex["crl_handPinkyEnd__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_neck__C"]], points2d[boneIndex["crl_Head__C"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_Head__C"]], points2d[boneIndex["crl_eye__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_Head__C"]], points2d[boneIndex["crl_eye__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_shoulder__R"]], points2d[boneIndex["crl_arm__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_arm__R"]], points2d[boneIndex["crl_foreArm__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_foreArm__R"]], points2d[boneIndex["crl_hand__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__R"]], points2d[boneIndex["crl_handThumb__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__R"]], points2d[boneIndex["crl_handIndex__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__R"]], points2d[boneIndex["crl_handMiddle__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__R"]], points2d[boneIndex["crl_handRing__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_hand__R"]], points2d[boneIndex["crl_handPinky__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handThumb__R"]], points2d[boneIndex["crl_handThumb01__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handThumb01__R"]], points2d[boneIndex["crl_handThumb02__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handThumb02__R"]], points2d[boneIndex["crl_handThumbEnd__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handIndex__R"]], points2d[boneIndex["crl_handIndex01__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handIndex01__R"]], points2d[boneIndex["crl_handIndex02__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handIndex02__R"]], points2d[boneIndex["crl_handIndexEnd__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handMiddle__R"]], points2d[boneIndex["crl_handMiddle01__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handMiddle01__R"]], points2d[boneIndex["crl_handMiddle02__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handMiddle02__R"]], points2d[boneIndex["crl_handMiddleEnd__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handRing__R"]], points2d[boneIndex["crl_handRing01__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handRing01__R"]], points2d[boneIndex["crl_handRing02__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handRing02__R"]], points2d[boneIndex["crl_handRingEnd__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handPinky__R"]], points2d[boneIndex["crl_handPinky01__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handPinky01__R"]], points2d[boneIndex["crl_handPinky02__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_handPinky02__R"]], points2d[boneIndex["crl_handPinkyEnd__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_thigh__R"]], points2d[boneIndex["crl_leg__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_leg__R"]], points2d[boneIndex["crl_foot__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_foot__R"]], points2d[boneIndex["crl_toe__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_toe__R"]], points2d[boneIndex["crl_toeEnd__R"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_thigh__L"]], points2d[boneIndex["crl_leg__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_leg__L"]], points2d[boneIndex["crl_foot__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_foot__L"]], points2d[boneIndex["crl_toe__L"]]), color, size) + draw_line_on_buffer(buffer, image_w, image_h, (points2d[boneIndex["crl_toe__L"]], points2d[boneIndex["crl_toeEnd__L"]]), color, size) + except: + pass + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + +def write_image(frame, id, buffer): + # Save the image using Pillow module. + img = Image.fromarray(buffer) + img.save('_out/%s_%06d.png' % (id, frame)) + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '--fov', + default=60, + type=int, + help='FOV for camera') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + # default='1920x1080', + default='800x600', + help='window resolution (default: 800x600)') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + actor_list = [] + pygame.init() + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(5.0) + + world = client.get_world() + + # spawn a camera + camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') + camera_bp.set_attribute("image_size_x", str(args.width)) + camera_bp.set_attribute("image_size_y", str(args.height)) + camera_bp.set_attribute("fov", str(args.fov)) + camera = world.spawn_actor(camera_bp, carla.Transform()) + + # spawn a pedestrian + world.set_pedestrians_seed(1235) + ped_bp = random.choice(world.get_blueprint_library().filter("walker.pedestrian.*")) + trans = carla.Transform() + trans.location = world.get_random_location_from_navigation() + ped = world.spawn_actor(ped_bp, trans) + walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') + controller = world.spawn_actor(walker_controller_bp, carla.Transform(), ped) + controller.start() + controller.go_to_location(world.get_random_location_from_navigation()) + controller.set_max_speed(1.7) + + # keep tracking of actors to remove + actor_list.append(camera) + actor_list.append(ped) + actor_list.append(controller) + + # get some attributes from the camera + image_w = camera_bp.get_attribute("image_size_x").as_int() + image_h = camera_bp.get_attribute("image_size_y").as_int() + fov = camera_bp.get_attribute("fov").as_float() + + try: + pool = Pool(processes=5) + # Create a synchronous mode context. + with CarlaSyncMode(world, camera, fps=30) as sync_mode: + + # set the projection matrix + K = build_projection_matrix(image_w, image_h, fov) + + blending = 0 + turning = 0 + while True: + if should_quit(): + return + clock.tick() + + # make some transition from custom pose to animation + ped.blend_pose(math.sin(blending)) + + # move the pedestrian + blending += 0.015 + turning += 0.009 + + # move camera around + trans = ped.get_transform() + x = math.cos(turning) * -3 + y = math.sin(turning) * 3 + trans.location.x += x + trans.location.y += y + trans.location.z = 2 + trans.rotation.pitch = -16 + trans.rotation.roll = 0 + trans.rotation.yaw = -360 * (turning/(math.pi*2)) + camera.set_transform(trans) + + # Advance the simulation and wait for the data. + snapshot, image_rgb = sync_mode.tick(timeout=5.0) + + # Draw the display. + buffer = get_image_as_array(image_rgb) + + # get the pedestrian bones + bones = ped.get_bones() + + # prepare the bones (get name and world position) + boneIndex = {} + points = [] + for i, bone in enumerate(bones.bone_transforms): + boneIndex[bone.name] = i + points.append(bone.world.location) + + # project the 3d points to 2d screen + points2d = get_screen_points(camera, K, image_w, image_h, points) + + # draw the skeleton lines + draw_skeleton(buffer, image_w, image_h, boneIndex, points2d, (0, 255, 0), 2) + + # draw the bone points + draw_points_on_buffer(buffer, image_w, image_h, points2d[1:], (255, 0, 0), 4) + + draw_image(display, buffer) + # pool.apply_async(write_image, (snapshot.frame, "ped", buffer)) + + # display.blit(font.render('%d bones' % len(points), True, (255, 255, 255)), (8, 10)) + + pygame.display.flip() + + finally: + # time.sleep(5) + print('destroying actors.') + for actor in actor_list: + actor.destroy() + pygame.quit() + pool.close() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') diff --git a/carla_examples/dynamic_weather.py b/carla_examples/dynamic_weather.py new file mode 100644 index 0000000..7f09692 --- /dev/null +++ b/carla_examples/dynamic_weather.py @@ -0,0 +1,142 @@ +#!/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 . + +""" +CARLA Dynamic Weather: + +Connect to a CARLA Simulator instance and control the weather. Change Sun +position smoothly with time and generate storms occasionally. +""" + +import carla + +import argparse +import math +import sys + + +def clamp(value, minimum=0.0, maximum=100.0): + return max(minimum, min(value, maximum)) + + +class Sun(object): + def __init__(self, azimuth, altitude): + self.azimuth = azimuth + self.altitude = altitude + self._t = 0.0 + + def tick(self, delta_seconds): + self._t += 0.008 * delta_seconds + self._t %= 2.0 * math.pi + self.azimuth += 0.25 * delta_seconds + self.azimuth %= 360.0 + self.altitude = (70 * math.sin(self._t)) - 20 + + def __str__(self): + return 'Sun(alt: %.2f, azm: %.2f)' % (self.altitude, self.azimuth) + + +class Storm(object): + def __init__(self, precipitation): + self._t = precipitation if precipitation > 0.0 else -50.0 + self._increasing = True + self.clouds = 0.0 + self.rain = 0.0 + self.wetness = 0.0 + self.puddles = 0.0 + self.wind = 0.0 + self.fog = 0.0 + + def tick(self, delta_seconds): + delta = (1.3 if self._increasing else -1.3) * delta_seconds + self._t = clamp(delta + self._t, -250.0, 100.0) + self.clouds = clamp(self._t + 40.0, 0.0, 90.0) + self.rain = clamp(self._t, 0.0, 80.0) + delay = -10.0 if self._increasing else 90.0 + self.puddles = clamp(self._t + delay, 0.0, 85.0) + self.wetness = clamp(self._t * 5, 0.0, 100.0) + self.wind = 5.0 if self.clouds <= 20 else 90 if self.clouds >= 70 else 40 + self.fog = clamp(self._t - 10, 0.0, 30.0) + if self._t == -250.0: + self._increasing = True + if self._t == 100.0: + self._increasing = False + + def __str__(self): + return 'Storm(clouds=%d%%, rain=%d%%, wind=%d%%)' % (self.clouds, self.rain, self.wind) + + +class Weather(object): + def __init__(self, weather): + self.weather = weather + self._sun = Sun(weather.sun_azimuth_angle, weather.sun_altitude_angle) + self._storm = Storm(weather.precipitation) + + def tick(self, delta_seconds): + self._sun.tick(delta_seconds) + self._storm.tick(delta_seconds) + self.weather.cloudiness = self._storm.clouds + self.weather.precipitation = self._storm.rain + self.weather.precipitation_deposits = self._storm.puddles + self.weather.wind_intensity = self._storm.wind + self.weather.fog_density = self._storm.fog + self.weather.wetness = self._storm.wetness + self.weather.sun_azimuth_angle = self._sun.azimuth + self.weather.sun_altitude_angle = self._sun.altitude + + def __str__(self): + return '%s %s' % (self._sun, self._storm) + + +def main(): + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-s', '--speed', + metavar='FACTOR', + default=1.0, + type=float, + help='rate at which the weather changes (default: 1.0)') + args = argparser.parse_args() + + speed_factor = args.speed + update_freq = 0.1 / speed_factor + + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + world = client.get_world() + + weather = Weather(world.get_weather()) + + elapsed_time = 0.0 + + while True: + timestamp = world.wait_for_tick(seconds=30.0).timestamp + elapsed_time += timestamp.delta_seconds + if elapsed_time > update_freq: + weather.tick(speed_factor * elapsed_time) + world.set_weather(weather.weather) + sys.stdout.write('\r' + str(weather) + 12 * ' ') + sys.stdout.flush() + elapsed_time = 0.0 + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/follow_vehicle.py b/carla_examples/follow_vehicle.py new file mode 100644 index 0000000..508e4ae --- /dev/null +++ b/carla_examples/follow_vehicle.py @@ -0,0 +1,35 @@ +import carla +import time + +client = carla.Client("localhost", 2000) +client.set_timeout(5.0) + +world = client.get_world() +blueprint_library = world.get_blueprint_library() + +# Spawn vehicle +vehicle_bp = blueprint_library.filter("model3")[0] +spawn_point = world.get_map().get_spawn_points()[0] + +vehicle = world.spawn_actor(vehicle_bp, spawn_point) +vehicle.set_autopilot(True) + +# Get spectator (this is the simulator camera) +spectator = world.get_spectator() + +try: + while True: + transform = vehicle.get_transform() + + # Position camera behind and above vehicle + spectator_transform = carla.Transform( + transform.location + carla.Location(x=-6, z=3), + transform.rotation + ) + + spectator.set_transform(spectator_transform) + + time.sleep(0.05) + +finally: + vehicle.destroy() \ No newline at end of file diff --git a/carla_examples/generate_traffic.py b/carla_examples/generate_traffic.py new file mode 100644 index 0000000..fd0bba7 --- /dev/null +++ b/carla_examples/generate_traffic.py @@ -0,0 +1,366 @@ +#!/usr/bin/env python + +# Copyright (c) 2021 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 . + +"""Example script to generate traffic in the simulation""" + +import carla + +from carla import VehicleLightState as vls +from carla.command import SpawnActor, SetAutopilot, FutureActor, DestroyActor + +import argparse +import logging +from numpy import random +import time + + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2, 3]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + +def main(): + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-n', '--number-of-vehicles', + metavar='N', + default=30, + type=int, + help='Number of vehicles (default: 30)') + argparser.add_argument( + '-w', '--number-of-walkers', + metavar='W', + default=10, + type=int, + help='Number of walkers (default: 10)') + argparser.add_argument( + '--safe', + action='store_true', + help='Avoid spawning vehicles prone to accidents') + argparser.add_argument( + '--filterv', + metavar='PATTERN', + default='vehicle.*', + help='Filter vehicle model (default: "vehicle.*")') + argparser.add_argument( + '--generationv', + metavar='G', + default='All', + help='restrict to certain vehicle generation (values: "1","2","All" - default: "All")') + argparser.add_argument( + '--filterw', + metavar='PATTERN', + default='walker.pedestrian.*', + help='Filter pedestrian type (default: "walker.pedestrian.*")') + argparser.add_argument( + '--generationw', + metavar='G', + default='2', + help='restrict to certain pedestrian generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--tm-port', + metavar='P', + default=8000, + type=int, + help='Port to communicate with TM (default: 8000)') + argparser.add_argument( + '--asynch', + action='store_true', + help='Activate asynchronous mode execution') + argparser.add_argument( + '--hybrid', + action='store_true', + help='Activate hybrid mode for Traffic Manager') + argparser.add_argument( + '-s', '--seed', + metavar='S', + type=int, + help='Set random device seed and deterministic mode for Traffic Manager') + argparser.add_argument( + '--seedw', + metavar='S', + default=0, + type=int, + help='Set the seed for pedestrians module') + argparser.add_argument( + '--car-lights-on', + action='store_true', + default=False, + help='Enable automatic car light management') + argparser.add_argument( + '--hero', + action='store_true', + default=False, + help='Set one of the vehicles as hero') + argparser.add_argument( + '--respawn', + action='store_true', + default=False, + help='Automatically respawn dormant vehicles (only in large maps)') + argparser.add_argument( + '--no-rendering', + action='store_true', + default=False, + help='Activate no rendering mode') + + args = argparser.parse_args() + + logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) + + vehicles_list = [] + walkers_list = [] + all_id = [] + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + synchronous_master = False + random.seed(args.seed if args.seed is not None else int(time.time())) + + try: + world = client.get_world() + + traffic_manager = client.get_trafficmanager(args.tm_port) + traffic_manager.set_global_distance_to_leading_vehicle(2.5) + if args.respawn: + traffic_manager.set_respawn_dormant_vehicles(True) + if args.hybrid: + traffic_manager.set_hybrid_physics_mode(True) + traffic_manager.set_hybrid_physics_radius(70.0) + if args.seed is not None: + traffic_manager.set_random_device_seed(args.seed) + + settings = world.get_settings() + if not args.asynch: + traffic_manager.set_synchronous_mode(True) + if not settings.synchronous_mode: + synchronous_master = True + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + else: + synchronous_master = False + else: + print("You are currently in asynchronous mode. If this is a traffic simulation, \ + you could experience some issues. If it's not working correctly, switch to synchronous \ + mode by using traffic_manager.set_synchronous_mode(True)") + + if args.no_rendering: + settings.no_rendering_mode = True + world.apply_settings(settings) + + blueprints = get_actor_blueprints(world, args.filterv, args.generationv) + if not blueprints: + raise ValueError("Couldn't find any vehicles with the specified filters") + blueprintsWalkers = get_actor_blueprints(world, args.filterw, args.generationw) + if not blueprintsWalkers: + raise ValueError("Couldn't find any walkers with the specified filters") + + if args.safe: + blueprints = [x for x in blueprints if x.get_attribute('base_type') == 'car'] + + blueprints = sorted(blueprints, key=lambda bp: bp.id) + + spawn_points = world.get_map().get_spawn_points() + number_of_spawn_points = len(spawn_points) + + if args.number_of_vehicles < number_of_spawn_points: + random.shuffle(spawn_points) + elif args.number_of_vehicles > number_of_spawn_points: + msg = 'requested %d vehicles, but could only find %d spawn points' + logging.warning(msg, args.number_of_vehicles, number_of_spawn_points) + args.number_of_vehicles = number_of_spawn_points + + # -------------- + # Spawn vehicles + # -------------- + batch = [] + hero = args.hero + for n, transform in enumerate(spawn_points): + if n >= args.number_of_vehicles: + break + blueprint = random.choice(blueprints) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if hero: + blueprint.set_attribute('role_name', 'hero') + hero = False + else: + blueprint.set_attribute('role_name', 'autopilot') + + # spawn the cars and set their autopilot and light state all together + batch.append(SpawnActor(blueprint, transform) + .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))) + + for response in client.apply_batch_sync(batch, synchronous_master): + if response.error: + logging.error(response.error) + else: + vehicles_list.append(response.actor_id) + + # Set automatic vehicle lights update if specified + if args.car_lights_on: + all_vehicle_actors = world.get_actors(vehicles_list) + for actor in all_vehicle_actors: + traffic_manager.update_vehicle_lights(actor, True) + + # ------------- + # Spawn Walkers + # ------------- + # some settings + percentagePedestriansRunning = 0.0 # how many pedestrians will run + percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road + if args.seedw: + world.set_pedestrians_seed(args.seedw) + random.seed(args.seedw) + # 1. take all the random locations to spawn + spawn_points = [] + for i in range(args.number_of_walkers): + spawn_point = carla.Transform() + loc = world.get_random_location_from_navigation() + if (loc != None): + spawn_point.location = loc + spawn_points.append(spawn_point) + # 2. we spawn the walker object + batch = [] + walker_speed = [] + for spawn_point in spawn_points: + walker_bp = random.choice(blueprintsWalkers) + # set as not invincible + probability = random.randint(0,100 + 1); + if walker_bp.has_attribute('is_invincible'): + walker_bp.set_attribute('is_invincible', 'false') + if walker_bp.has_attribute('can_use_wheelchair') and probability < 11: + walker_bp.set_attribute('use_wheelchair', 'true') + # set the max speed + if walker_bp.has_attribute('speed'): + if (random.random() > percentagePedestriansRunning): + # walking + walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) + else: + # running + walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) + else: + print("Walker has no speed") + walker_speed.append(0.0) + batch.append(SpawnActor(walker_bp, spawn_point)) + results = client.apply_batch_sync(batch, True) + walker_speed2 = [] + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list.append({"id": results[i].actor_id}) + walker_speed2.append(walker_speed[i]) + walker_speed = walker_speed2 + # 3. we spawn the walker controller + batch = [] + walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') + for i in range(len(walkers_list)): + batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) + results = client.apply_batch_sync(batch, True) + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list[i]["con"] = results[i].actor_id + # 4. we put together the walkers and controllers id to get the objects from their id + for i in range(len(walkers_list)): + all_id.append(walkers_list[i]["con"]) + all_id.append(walkers_list[i]["id"]) + all_actors = world.get_actors(all_id) + + # wait for a tick to ensure client receives the last transform of the walkers we have just created + if args.asynch or not synchronous_master: + world.wait_for_tick() + else: + world.tick() + + # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) + # set how many pedestrians can cross the road + world.set_pedestrians_cross_factor(percentagePedestriansCrossing) + for i in range(0, len(all_id), 2): + # start walker + all_actors[i].start() + # set walk to random point + all_actors[i].go_to_location(world.get_random_location_from_navigation()) + # max speed + all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) + + print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) + + # Example of how to use Traffic Manager parameters + traffic_manager.global_percentage_speed_difference(30.0) + + while True: + if not args.asynch and synchronous_master: + world.tick() + else: + world.wait_for_tick() + + finally: + + if not args.asynch and synchronous_master: + settings = world.get_settings() + settings.synchronous_mode = False + settings.no_rendering_mode = False + settings.fixed_delta_seconds = None + world.apply_settings(settings) + + print('\ndestroying %d vehicles' % len(vehicles_list)) + client.apply_batch([DestroyActor(x) for x in vehicles_list]) + + # stop walker controllers (list is [controller, actor, controller, actor ...]) + for i in range(0, len(all_id), 2): + all_actors[i].stop() + + print('\ndestroying %d walkers' % len(walkers_list)) + client.apply_batch([DestroyActor(x) for x in all_id]) + + time.sleep(0.5) + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/get_component_test.py b/carla_examples/get_component_test.py new file mode 100644 index 0000000..7d1d4a9 --- /dev/null +++ b/carla_examples/get_component_test.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +# Copyright (c) 2025 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 . + +import carla + +client = carla.Client('localhost', 2000) +world = client.get_world() + +location = carla.Location(200.0, 200.0, 200.0) +rotation = carla.Rotation(0.0, 0.0, 0.0) +transform = carla.Transform(location, rotation) + +bp_library = world.get_blueprint_library() +bp_audi = bp_library.find('vehicle.audi.tt') +audi = world.spawn_actor(bp_audi, transform) + +component_transform = audi.get_component_world_transform('front-blinker-r-1') +print(component_transform) + diff --git a/carla_examples/invertedai_traffic.py b/carla_examples/invertedai_traffic.py new file mode 100644 index 0000000..2df3364 --- /dev/null +++ b/carla_examples/invertedai_traffic.py @@ -0,0 +1,695 @@ +#!/usr/bin/env python + +# Copyright (c) 2024 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 . + +""" +Example script to generate realistic traffic with the InvertedAI API +""" + +import os +import time +import carla +import argparse +import logging +import math +import random +import invertedai as iai +import numpy as np +from tqdm import tqdm +from invertedai.common import AgentProperties, AgentState, TrafficLightState, Point +from carla import command, Location + +#--------- +# CARLA Utils +#--------- + +# Argument parser +def argument_parser(): + + argparser = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to') + argparser.add_argument( + '-n', '--number-of-vehicles', + metavar='N', + default=50, + type=int, + help='Number of vehicles spawned by InvertedAI') + argparser.add_argument( + '-w', '--number-of-walkers', + metavar='W', + default=0, + type=int, + help='Number of walkers') + argparser.add_argument( + '--safe', + type=bool, + default=True, + help='Avoid spawning vehicles prone to accidents') + argparser.add_argument( + '--filterv', + metavar='PATTERN', + default='vehicle.*', + help='Filter vehicle model') + argparser.add_argument( + '--generationv', + metavar='G', + default='All', + help='restrict to certain vehicle generation') + argparser.add_argument( + '--filterw', + metavar='PATTERN', + default='walker.pedestrian.*', + help='Filter pedestrian type') + argparser.add_argument( + '--generationw', + metavar='G', + default='All', + help='restrict to certain pedestrian generation') + argparser.add_argument( + '-s', '--seed', + metavar='S', + type=int, + help='Set random seed') + argparser.add_argument( + '--hero', + action='store_true', + default=False, + help='Set one of the vehicles as hero') + argparser.add_argument( + '--iai-key', + type=str, + help="InvertedAI API key.") + argparser.add_argument( + '--record', + action='store_true', + help="Record the simulation using the CARLA recorder", + default=False) + argparser.add_argument( + '--sim-length', + type=int, + default=60, + help="Length of the simulation in seconds") + argparser.add_argument( + '--location', + type=str, + help=f"IAI formatted map on which to create simulate (default: carla:Town10HD, only tested there)", + default='carla:Town10HD') + argparser.add_argument( + '--capacity', + type=int, + help=f"The capacity parameter of a quadtree leaf before splitting", + default=100) + argparser.add_argument( + '--width', + type=int, + help=f"Full width of the area to initialize", + default=250) + argparser.add_argument( + '--height', + type=int, + help=f"Full height of the area to initialize", + default=250) + argparser.add_argument( + '--map-center', + type=int, + nargs='+', + help=f"Center of the area to initialize", + default=tuple([-50,20])) + argparser.add_argument( + '--iai-async', + type=bool, + help=f"Whether to call drive asynchronously", + default=True) + argparser.add_argument( + '--api-model', + type=str, + help=f"IAI API model version", + default="bI5p") + argparser.add_argument( + '--iai-log', + action="store_true", + help=f"Export a log file for the InvertedAI cosimulation, which can be replayed afterwards") + argparser.add_argument( + '--iai-waypoint-distance', + type=int, + default=15, + help=f"Distance to the next waypoint for IAI agents" + ) + argparser.add_argument( + '--iai-waypoint-detection-threshold', + type=int, + default=2, + help=f"Distance to which an agent is deemed as having reached its waypoint" + ) + argparser.add_argument( + '--iai-max-distance-away', + type=int, + default=20, + help=f"Maximum distance away before a new waypoint is set for an agent" + ) + + args = argparser.parse_args() + + return args + +# Setup CARLA client and world +def setup_carla_environment(host, port, location): + map_name = location.split(":")[-1] + + step_length = 0.1 # 0.1 is the only step length that is supported by invertedai so far + + client = carla.Client(host, port) + client.set_timeout(200.0) + + # Configure the simulation environment + world = client.load_world(map_name) + world_settings = carla.WorldSettings( + synchronous_mode=True, + fixed_delta_seconds=step_length, + ) + world.apply_settings(world_settings) + + return client, world + +# Set spectator view on a hero vehicle +def set_spectator(world, hero_v): + + spectator_offset_x = -6. + spectator_offset_z = 6. + spectator_offset_pitch = 20 + + hero_t = hero_v.get_transform() + + yaw = hero_t.rotation.yaw + spectator_l = hero_t.location + carla.Location( + spectator_offset_x * math.cos(math.radians(yaw)), + spectator_offset_x * math.sin(math.radians(yaw)), + spectator_offset_z, + ) + spectator_t = carla.Transform(spectator_l, hero_t.rotation) + spectator_t.rotation.pitch -= spectator_offset_pitch + world.get_spectator().set_transform(spectator_t) + +#--------- +# Initialize actors +#--------- + +# Initialize IAI agents from CARLA actors +def initialize_iai_agent(actor, agent_type): + + transf = actor.get_transform() + vel = actor.get_velocity() + speed = math.sqrt(vel.x**2. + vel.y**2. +vel.z**2.) + + agent_state = AgentState.fromlist([ + transf.location.x, + transf.location.y, + math.radians(transf.rotation.yaw), + speed + ]) + + bb = actor.bounding_box + length, width = bb.extent.x*2, bb.extent.y*2 + + agent_properties = AgentProperties(length=length, width=width, agent_type=agent_type) + if agent_type=="car": + agent_properties.rear_axis_offset = length*0.38 # Empirical value fitted from InvertedAI initialization + + return agent_state, agent_properties + +# Initialize IAI pedestrians from CARLA actors +def initialize_pedestrians(pedestrians): + + iai_pedestrians_states, iai_pedestrians_properties = [], [] + for actor in pedestrians: + iai_ped_state, iai_ped_properties = initialize_iai_agent(actor,agent_type="pedestrian") + iai_pedestrians_states.append(iai_ped_state) + iai_pedestrians_properties.append(iai_ped_properties) + + return iai_pedestrians_states, iai_pedestrians_properties + +# Spawn pedestrians in the simulation, which are driven by CARLA controllers (not by invertedai) +def spawn_pedestrians(client, world, num_pedestrians, bps): + + batch = [] + + # Get spawn points for pedestrians + spawn_points = [] + for i in range(num_pedestrians): + + loc = world.get_random_location_from_navigation() + if (loc is not None): + spawn_point = carla.Transform(location=loc) + #Apply Offset in vertical to avoid collision spawning + spawn_point.location.z += 1 + spawn_points.append(spawn_point) + + pedestrians = [] + walkers_list = [] + + # Spawn pedestrians + for i in range(len(spawn_points)): + walker_bp = random.choice(bps) + if walker_bp.has_attribute('is_invincible'): + walker_bp.set_attribute('is_invincible', 'false') + spawn_point = spawn_points[i] + batch.append(command.SpawnActor(walker_bp, spawn_point)) + + results = client.apply_batch_sync(batch, True) + pedestrians = world.get_actors().filter('walker.*') + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list.append({"id": results[i].actor_id}) + + # Spawn CARLA IA controllers for pedestrians + batch = [] + walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') + for i in range(len(walkers_list)): + batch.append(command.SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) + results = client.apply_batch_sync(batch, True) + + world.tick() + + for controller in world.get_actors().filter('controller.ai.walker'): + controller.start() + dest = world.get_random_location_from_navigation() + controller.go_to_location(dest) + controller.set_max_speed(0.5 + random.random()) + + return pedestrians + +# Get blueprints according to the given filters +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2, 3, 4]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + +#--------- +# InvertedAI - CARLA synchronization routines +#--------- + +# Get CARLA transform from IAI transform +def transform_iai_to_carla(agent_state): + agent_transform = carla.Transform( + carla.Location( + agent_state.center.x, + agent_state.center.y, + 0.1 + ), + carla.Rotation( + yaw=math.degrees(agent_state.orientation) + ) + ) + + return agent_transform + +# Update transforms of CARLA agents driven by IAI and tick the world +def update_transforms(iai2carla,response): + """ + Tick the carla simulation forward one time step + Assume carla_actors is a list of carla actors controlled by IAI + """ + for agent_id in iai2carla.keys(): + agentdict = iai2carla[agent_id] + if agentdict["is_iai"]: + agent = response.agent_states[agent_id] + agent_transform = transform_iai_to_carla(agent) + try: + actor = agentdict["actor"] + actor.set_transform(agent_transform) + except: + pass + +# Assign existing IAI agents to CARLA vehicle blueprints and add these agents to the CARLA simulation +def assign_carla_blueprints_to_iai_agents(world,vehicle_blueprints,agent_properties,agent_states,recurrent_states,is_iai,noniai_actors): + + agent_properties_new = [] + agent_states_new = [] + recurrent_states_new = [] + iai2carla = {} + + for agent_id, state in enumerate(agent_states): + + if not is_iai[agent_id]: + agent_properties_new.append(agent_properties[agent_id]) + agent_states_new.append(agent_states[agent_id]) + recurrent_states_new.append(recurrent_states[agent_id]) + actor = noniai_actors[agent_id] + iai2carla[len(iai2carla)] = {"actor":actor, "is_iai":False, "type":agent_properties[agent_id].agent_type} + + else: + + blueprint = random.choice(vehicle_blueprints) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + agent_transform = transform_iai_to_carla(state) + + actor = world.try_spawn_actor(blueprint,agent_transform) + + if actor is not None: + bb = actor.bounding_box.extent + + agent_attr = agent_properties[agent_id] + + agent_attr.length = 2*bb.x + agent_attr.width = 2*bb.y + agent_attr.rear_axis_offset = 2*bb.x/3 + + agent_properties_new.append(agent_attr) + agent_states_new.append(agent_states[agent_id]) + recurrent_states_new.append(recurrent_states[agent_id]) + + actor.set_simulate_physics(False) + + iai2carla[len(iai2carla)] = {"actor":actor, "is_iai":True, "type":agent_properties[agent_id].agent_type} + + if len(agent_properties_new) == 0: + raise Exception("No vehicles could be placed in Carla environment.") + + return agent_properties_new, agent_states_new, recurrent_states_new, iai2carla + +# Initialize InvertedAI co-simulation +def initialize_simulation(args, world, agent_states=None, agent_properties=None): + + iai_seed = args.seed if args.seed is not None else random.randint(1,10000) + traffic_lights_states, carla2iai_tl = initialize_tl_states(world) + + ################################################################################################# + # Initialize IAI Agents + map_center = args.map_center + print(f"Call location info.") + location_info_response = iai.location_info( + location = args.location, + rendering_center = map_center + ) + print(f"Begin initialization.") + # Acquire a grid of 100x100m regions in which to initialize vehicles to be controlled by IAI. + regions = iai.get_regions_default( + location = args.location, + total_num_agents = args.number_of_vehicles, + area_shape = (int(args.width/2),int(args.height/2)), + map_center = map_center, + ) + # Place vehicles within the specified regions which will consider the relative states of nearby vehicles in neighbouring regions. + response = iai.large_initialize( + location = args.location, + regions = regions, + traffic_light_state_history = [traffic_lights_states], + agent_states = agent_states, + agent_properties = agent_properties, + random_seed = iai_seed + ) + + return response, carla2iai_tl, location_info_response + +#--------- +# Synchronize InvertedAI and CARLA traffic lights +#--------- + +# Mapping between CARLA and IAI traffic lights IDs +def get_traffic_lights_mapping(world): + tls = world.get_actors().filter('traffic.traffic_light*') + tl_ids = sorted([tl.id for tl in list(tls)]) + carla2iai_tl = {} + # ID for IAI traffic lights, only valid for Town10 for now (in both UE4 and UE5 versions of the map) + iai_tl_id = 4364 + for carla_tl_id in tl_ids: + carla2iai_tl[str(carla_tl_id)] = [str(iai_tl_id), str(iai_tl_id+1000)] + iai_tl_id+=1 + + return carla2iai_tl + +# Returns IAI traffic light state based on CARLA traffic light state +def get_traffic_light_state_from_carla(carla_tl_state): + + if carla_tl_state == carla.TrafficLightState.Red: + return TrafficLightState.red + + elif carla_tl_state == carla.TrafficLightState.Yellow: + return TrafficLightState.yellow + + elif carla_tl_state == carla.TrafficLightState.Green: + return TrafficLightState.green + + else: # Unknown state, turn off traffic light + return TrafficLightState.Off + +# Assign IAI traffic lights based on the CARLA ones +def assign_iai_traffic_lights_from_carla(world, iai_tl, carla2iai_tl): + + traffic_lights = world.get_actors().filter('traffic.traffic_light*') + + carla_tl_dict = {} + for tl in traffic_lights: + carla_tl_dict[str(tl.id)]=tl.state + + for carla_tl_id, carla_state in carla_tl_dict.items(): + iai_tl_id_pair = carla2iai_tl[carla_tl_id] + for iai_tl_id in iai_tl_id_pair: + iai_tl[iai_tl_id] = get_traffic_light_state_from_carla(carla_state) + + return iai_tl + +# Initialize traffic lights states +def initialize_tl_states(world): + carla2iai_tl = get_traffic_lights_mapping(world) + iai_tl_states = {} + for tlpair in carla2iai_tl.values(): + for tl in tlpair: + iai_tl_states[tl] = TrafficLightState.red # Initialize to given value + + iai_tl_states = assign_iai_traffic_lights_from_carla(world, iai_tl_states, carla2iai_tl) + return iai_tl_states, carla2iai_tl + + +def get_distance(point1, point2): + return np.sqrt((point2[0] - point1[0])**2 + (point2[1] - point1[1])**2) + + +def new_waypoint_needed(waypoint, agent_state, detection_threshold, max_distance_away): + distance_from_waypoint = get_distance(waypoint, [agent_state.center.x, agent_state.center.y]) + return distance_from_waypoint <= detection_threshold or distance_from_waypoint >= max_distance_away + + +def get_next_waypoints(carla_map, agent_states, waypoints, args): + indices_of_interest = [] + interested_agent_states = [] + if len(waypoints) == 0: + indices_of_interest = [i for i in range(len(agent_states))] + else: + indices_of_interest = [i for i in range(len(agent_states)) if new_waypoint_needed(waypoints[i], agent_states[i], args.iai_waypoint_detection_threshold, args.iai_max_distance_away)] + vehicle_locations = [Location(x=agent_states[i].center.x, y=agent_states[i].center.y, z=0.1) for i in indices_of_interest] + closest_waypoints = [carla_map.get_waypoint(vehicle_location, project_to_road=True, lane_type=carla.LaneType.Driving) for vehicle_location in vehicle_locations] + next_waypoints = [np.random.choice(wp.next(args.iai_waypoint_distance)) for wp in closest_waypoints] + results = [] + if len(waypoints) == 0: + results = [[waypoint.transform.location.x, waypoint.transform.location.y] for waypoint in next_waypoints] + else: + results = waypoints + for ind in range(len(next_waypoints)): + results[indices_of_interest[ind]] = [next_waypoints[ind].transform.location.x, next_waypoints[ind].transform.location.y] + return results + +#--------- +# Main +#--------- +def main(): + + args = argument_parser() + + # Setup CARLA client and world + client, world = setup_carla_environment(args.host, args.port, args.location) + + # Specify the IAI API key + try: + iai.add_apikey(args.iai_key) + except: + print("\n\tYou need to indicate the InvertedAI API key with the argument --iai-key. To obtain one, please go to https://www.inverted.ai \n") + + num_pedestrians = args.number_of_walkers + + FPS = int(1./world.get_settings().fixed_delta_seconds) + + if args.record: + logfolder = os.getcwd()+"/logs/" + if not os.path.exists(logfolder): + os.system("mkdir "+logfolder) + logfile = logfolder+"carla_record.log" + client.start_recorder(logfile) + print("Recording on file: %s" % logfile) + + seed = args.seed + + if seed: + random.seed(seed) + + vehicle_blueprints = get_actor_blueprints(world, args.filterv, args.generationv) + if args.safe: + vehicle_blueprints = [x for x in vehicle_blueprints if x.get_attribute('base_type') == 'car'] + + agent_states, agent_properties = [], [] + is_iai = [] + noniai_actors = [] + + # Add pedestrians (not driven by IAI) + if num_pedestrians>0: + if seed: + world.set_pedestrians_seed(seed) + blueprintsWalkers = get_actor_blueprints(world, args.filterw, args.generationw) + if not blueprintsWalkers: + raise ValueError("Couldn't find any walkers with the specified filters") + pedestrians = spawn_pedestrians(client, world, num_pedestrians, blueprintsWalkers) + iai_pedestrians_states, iai_pedestrians_properties = initialize_pedestrians(pedestrians) + agent_states.extend(iai_pedestrians_states) + agent_properties.extend(iai_pedestrians_properties) + is_iai.extend( [False]*len(iai_pedestrians_states) ) + noniai_actors.extend(pedestrians) + + else: + pedestrians = [] + + num_noniai = len(agent_properties) + # Initialize InvertedAI co-simulation + response, carla2iai_tl, location_info_response = initialize_simulation(args, world, agent_states=agent_states, agent_properties=agent_properties) + agent_properties = response.agent_properties + is_iai.extend( [True]*(len(agent_properties)-num_noniai) ) + + # Map IAI agents to CARLA actors and update response properties and states + print(f"Number of agents initialized: {len(response.agent_states)}") + agent_properties, agent_states_new, recurrent_states_new, iai2carla = assign_carla_blueprints_to_iai_agents(world,vehicle_blueprints,agent_properties,response.agent_states,response.recurrent_states,is_iai,noniai_actors) + traffic_lights_states = assign_iai_traffic_lights_from_carla(world,response.traffic_lights_states, carla2iai_tl) + response.agent_states = agent_states_new + response.agent_properties = agent_properties + response.recurrent_states = recurrent_states_new + response.traffic_lights_states = traffic_lights_states + + # Write InvertedAI log file, which can be opened afterwards to visualize a gif and further analysis + # See an example of usage here: https://github.com/inverted-ai/invertedai/blob/master/examples/scenario_log_example.py + + if args.iai_log: + log_writer = iai.LogWriter() + log_writer.initialize( + location=args.location, + location_info_response=location_info_response, + init_response=response + ) + iailog_path = os.path.join(os.getcwd(),f"iai_log.json") + + # Perform first CARLA simulation tick + world.tick() + + iai_agent_indices = [i for i in range(len(agent_properties)) if iai2carla[i]["is_iai"]] + try: + + vehicles = world.get_actors().filter('vehicle.*') + print("Total number of agents:",len(agent_properties),"Vehicles",len(vehicles), "Pedestrians:",len(pedestrians)) + for index in iai_agent_indices: + agent_properties[index].max_speed = 10.0 + # Get hero vehicle + hero_v = None + if args.hero: + hero_v = vehicles[0] + + carla_map = world.get_map() + waypoints = [] + for frame in tqdm(range(args.sim_length * FPS)): + response.traffic_lights_states = assign_iai_traffic_lights_from_carla(world, response.traffic_lights_states, carla2iai_tl) + iai_agent_states = [response.agent_states[i] for i in iai_agent_indices] + waypoints = get_next_waypoints(carla_map, iai_agent_states, waypoints, args) + for i in range(len(waypoints)): + agent_properties[iai_agent_indices[i]].waypoint = Point(x=waypoints[i][0], y=waypoints[i][1]) + + # IAI update step + response = iai.large_drive( + location = args.location, + agent_states = response.agent_states, + agent_properties = agent_properties, + recurrent_states = response.recurrent_states, + traffic_lights_states = response.traffic_lights_states, + single_call_agent_limit = args.capacity, + async_api_calls = args.iai_async, + api_model_version = args.api_model, + random_seed = seed + ) + + if args.iai_log: + log_writer.drive(drive_response=response) + + # Update CARLA actors with new transforms from IAI agents + update_transforms(iai2carla,response) + + # Tick CARLA simulation + world.tick() + + # Update agents not driven by IAI in IAI cosimulation, like pedestrians + for agent_id in iai2carla.keys(): + agentdict = iai2carla[agent_id] + + if not agentdict["is_iai"] and agentdict["type"] == "pedestrian": + actor = agentdict["actor"] + state, properties = initialize_iai_agent(actor, agentdict["type"]) + response.agent_states[agent_id] = state + agent_properties[agent_id] = properties + + # Update spectator view if there is hero vehicle + if hero_v is not None: + set_spectator(world, hero_v) + + + + finally: + time.sleep(0.5) + + if args.record: + client.stop_recorder() + + if args.iai_log: + log_writer.export_to_file(log_path=iailog_path) + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/lidar_to_camera.py b/carla_examples/lidar_to_camera.py new file mode 100644 index 0000000..214b59f --- /dev/null +++ b/carla_examples/lidar_to_camera.py @@ -0,0 +1,345 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 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 . + +""" +Lidar projection on RGB camera example +""" + +import os +import sys + +import carla + +import argparse +from queue import Queue +from queue import Empty +from matplotlib import cm + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + from PIL import Image +except ImportError: + raise RuntimeError('cannot import PIL, make sure "Pillow" package is installed') + +VIRIDIS = np.array(cm._colormaps.get_cmap('viridis').colors) +VID_RANGE = np.linspace(0.0, 1.0, VIRIDIS.shape[0]) + +def sensor_callback(data, queue): + """ + This simple callback just stores the data on a thread safe Python Queue + to be retrieved from the "main thread". + """ + queue.put(data) + + +def tutorial(args): + """ + This function is intended to be a tutorial on how to retrieve data in a + synchronous way, and project 3D points from a lidar to a 2D camera. + """ + # Connect to the server + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + world = client.get_world() + bp_lib = world.get_blueprint_library() + + traffic_manager = client.get_trafficmanager(8000) + traffic_manager.set_synchronous_mode(True) + + original_settings = world.get_settings() + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 3.0 + world.apply_settings(settings) + + vehicle = None + camera = None + lidar = None + + try: + if not os.path.isdir('_out'): + os.mkdir('_out') + # Search the desired blueprints + vehicle_bp = bp_lib.filter("vehicle.lincoln.mkz_2017")[0] + camera_bp = bp_lib.filter("sensor.camera.rgb")[0] + lidar_bp = bp_lib.filter("sensor.lidar.ray_cast")[0] + + # Configure the blueprints + camera_bp.set_attribute("image_size_x", str(args.width)) + camera_bp.set_attribute("image_size_y", str(args.height)) + + if args.no_noise: + lidar_bp.set_attribute('dropoff_general_rate', '0.0') + lidar_bp.set_attribute('dropoff_intensity_limit', '1.0') + lidar_bp.set_attribute('dropoff_zero_intensity', '0.0') + lidar_bp.set_attribute('upper_fov', str(args.upper_fov)) + lidar_bp.set_attribute('lower_fov', str(args.lower_fov)) + lidar_bp.set_attribute('channels', str(args.channels)) + lidar_bp.set_attribute('range', str(args.range)) + lidar_bp.set_attribute('points_per_second', str(args.points_per_second)) + + # Spawn the blueprints + vehicle = world.spawn_actor( + blueprint=vehicle_bp, + transform=world.get_map().get_spawn_points()[0]) + vehicle.set_autopilot(True) + camera = world.spawn_actor( + blueprint=camera_bp, + transform=carla.Transform(carla.Location(x=1.6, z=1.6)), + attach_to=vehicle) + lidar = world.spawn_actor( + blueprint=lidar_bp, + transform=carla.Transform(carla.Location(x=1.0, z=1.8)), + attach_to=vehicle) + + # Build the K projection matrix: + # K = [[Fx, 0, image_w/2], + # [ 0, Fy, image_h/2], + # [ 0, 0, 1]] + image_w = camera_bp.get_attribute("image_size_x").as_int() + image_h = camera_bp.get_attribute("image_size_y").as_int() + fov = camera_bp.get_attribute("fov").as_float() + focal = image_w / (2.0 * np.tan(fov * np.pi / 360.0)) + + # In this case Fx and Fy are the same since the pixel aspect + # ratio is 1 + K = np.identity(3) + K[0, 0] = K[1, 1] = focal + K[0, 2] = image_w / 2.0 + K[1, 2] = image_h / 2.0 + + # The sensor data will be saved in thread-safe Queues + image_queue = Queue() + lidar_queue = Queue() + + camera.listen(lambda data: sensor_callback(data, image_queue)) + lidar.listen(lambda data: sensor_callback(data, lidar_queue)) + + for frame in range(args.frames): + world.tick() + world_frame = world.get_snapshot().frame + + try: + # Get the data once it's received. + image_data = image_queue.get(True, 1.0) + lidar_data = lidar_queue.get(True, 1.0) + except Empty: + print("[Warning] Some sensor data has been missed") + continue + + assert image_data.frame == lidar_data.frame == world_frame + # At this point, we have the synchronized information from the 2 sensors. + sys.stdout.write("\r(%d/%d) Simulation: %d Camera: %d Lidar: %d" % + (frame, args.frames, world_frame, image_data.frame, lidar_data.frame) + ' ') + sys.stdout.flush() + + # Get the raw BGRA buffer and convert it to an array of RGB of + # shape (image_data.height, image_data.width, 3). + im_array = np.copy(np.frombuffer(image_data.raw_data, dtype=np.dtype("uint8"))) + im_array = np.reshape(im_array, (image_data.height, image_data.width, 4)) + im_array = im_array[:, :, :3][:, :, ::-1] + + # Get the lidar data and convert it to a numpy array. + p_cloud_size = len(lidar_data) + p_cloud = np.copy(np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))) + p_cloud = np.reshape(p_cloud, (p_cloud_size, 4)) + + # Lidar intensity array of shape (p_cloud_size,) but, for now, let's + # focus on the 3D points. + intensity = np.array(p_cloud[:, 3]) + + # Point cloud in lidar sensor space array of shape (3, p_cloud_size). + local_lidar_points = np.array(p_cloud[:, :3]).T + + # Add an extra 1.0 at the end of each 3d point so it becomes of + # shape (4, p_cloud_size) and it can be multiplied by a (4, 4) matrix. + local_lidar_points = np.r_[ + local_lidar_points, [np.ones(local_lidar_points.shape[1])]] + + # This (4, 4) matrix transforms the points from lidar space to world space. + lidar_2_world = lidar.get_transform().get_matrix() + + # Transform the points from lidar space to world space. + world_points = np.dot(lidar_2_world, local_lidar_points) + + # This (4, 4) matrix transforms the points from world to sensor coordinates. + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + # Transform the points from world space to camera space. + sensor_points = np.dot(world_2_camera, world_points) + + # New we must change from UE4's coordinate system to an "standard" + # camera coordinate system (the same used by OpenCV): + + # ^ z . z + # | / + # | to: +-------> x + # | . x | + # |/ | + # +-------> y v y + + # This can be achieved by multiplying by the following matrix: + # [[ 0, 1, 0 ], + # [ 0, 0, -1 ], + # [ 1, 0, 0 ]] + + # Or, in this case, is the same as swapping: + # (x, y ,z) -> (y, -z, x) + point_in_camera_coords = np.array([ + sensor_points[1], + sensor_points[2] * -1, + sensor_points[0]]) + + # Finally we can use our K matrix to do the actual 3D -> 2D. + points_2d = np.dot(K, point_in_camera_coords) + + # Remember to normalize the x, y values by the 3rd value. + points_2d = np.array([ + points_2d[0, :] / points_2d[2, :], + points_2d[1, :] / points_2d[2, :], + points_2d[2, :]]) + + # At this point, points_2d[0, :] contains all the x and points_2d[1, :] + # contains all the y values of our points. In order to properly + # visualize everything on a screen, the points that are out of the screen + # must be discarted, the same with points behind the camera projection plane. + points_2d = points_2d.T + intensity = intensity.T + points_in_canvas_mask = \ + (points_2d[:, 0] > 0.0) & (points_2d[:, 0] < image_w) & \ + (points_2d[:, 1] > 0.0) & (points_2d[:, 1] < image_h) & \ + (points_2d[:, 2] > 0.0) + points_2d = points_2d[points_in_canvas_mask] + intensity = intensity[points_in_canvas_mask] + + # Extract the screen coords (uv) as integers. + u_coord = points_2d[:, 0].astype(int) + v_coord = points_2d[:, 1].astype(int) + + # Since at the time of the creation of this script, the intensity function + # is returning high values, these are adjusted to be nicely visualized. + intensity = 4 * intensity - 3 + color_map = np.array([ + np.interp(intensity, VID_RANGE, VIRIDIS[:, 0]) * 255.0, + np.interp(intensity, VID_RANGE, VIRIDIS[:, 1]) * 255.0, + np.interp(intensity, VID_RANGE, VIRIDIS[:, 2]) * 255.0]).astype(int).T + + if args.dot_extent <= 0: + # Draw the 2d points on the image as a single pixel using numpy. + im_array[v_coord, u_coord] = color_map + else: + # Draw the 2d points on the image as squares of extent args.dot_extent. + for i in range(len(points_2d)): + # I'm not a NumPy expert and I don't know how to set bigger dots + # without using this loop, so if anyone has a better solution, + # make sure to update this script. Meanwhile, it's fast enough :) + im_array[ + v_coord[i]-args.dot_extent : v_coord[i]+args.dot_extent, + u_coord[i]-args.dot_extent : u_coord[i]+args.dot_extent] = color_map[i] + + # Save the image using Pillow module. + image = Image.fromarray(im_array) + image.save("_out/%08d.png" % image_data.frame) + + finally: + # Apply the original settings when exiting. + world.apply_settings(original_settings) + + # Destroy the actors in the scene. + if camera: + camera.destroy() + if lidar: + lidar.destroy() + if vehicle: + vehicle.destroy() + + +def main(): + """Start function""" + argparser = argparse.ArgumentParser( + description='CARLA Sensor sync and projection tutorial') + 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='680x420', + help='window resolution (default: 1280x720)') + argparser.add_argument( + '-f', '--frames', + metavar='N', + default=500, + type=int, + help='number of frames to record (default: 500)') + argparser.add_argument( + '-d', '--dot-extent', + metavar='SIZE', + default=2, + type=int, + help='visualization dot extent in pixels (Recomended [1-4]) (default: 2)') + argparser.add_argument( + '--no-noise', + action='store_true', + help='remove the drop off and noise from the normal (non-semantic) lidar') + argparser.add_argument( + '--upper-fov', + metavar='F', + default=30.0, + type=float, + help='lidar\'s upper field of view in degrees (default: 15.0)') + argparser.add_argument( + '--lower-fov', + metavar='F', + default=-25.0, + type=float, + help='lidar\'s lower field of view in degrees (default: -25.0)') + argparser.add_argument( + '-c', '--channels', + metavar='C', + default=64.0, + type=float, + help='lidar\'s channel count (default: 64)') + argparser.add_argument( + '-r', '--range', + metavar='R', + default=100.0, + type=float, + help='lidar\'s maximum range in meters (default: 100.0)') + argparser.add_argument( + '--points-per-second', + metavar='N', + default='100000', + type=int, + help='lidar points per second (default: 100000)') + args = argparser.parse_args() + args.width, args.height = [int(x) for x in args.res.split('x')] + args.dot_extent -= 1 + + try: + tutorial(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/manual_control.py b/carla_examples/manual_control.py new file mode 100644 index 0000000..c8997ef --- /dev/null +++ b/carla_examples/manual_control.py @@ -0,0 +1,1367 @@ +#!/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 + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + O : open/close all doors of vehicle + T : toggle vehicle's telemetry + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + 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 collections +import datetime +import logging +import math +import os +import random +import re +import sys +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + 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_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_f + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_o + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + 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 find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2, 3]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.sync = args.sync + self.actor_role_name = args.rolename + 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._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._actor_generation = args.generation + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.show_vehicle_telemetry = False + self.doors_are_open = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + 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 a random blueprint. + blueprint_list = get_actor_blueprints(self.world, self._actor_filter, self._actor_generation) + if not blueprint_list: + raise ValueError("Couldn't find any blueprints with the specified filters") + blueprint = random.choice(blueprint_list) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('terramechanics'): + blueprint.set_attribute('terramechanics', 'true') + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + # 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._gamma) + 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) + + if self.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + 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._ackermann_enabled = False + self._ackermann_reverse = 1 + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._ackermann_control = carla.VehicleAckermannControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock, sync_mode): + if isinstance(self._control, carla.VehicleControl): + 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_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key == K_o: + try: + if world.doors_are_open: + world.hud.notification("Closing Doors") + world.doors_are_open = False + world.player.close_door(carla.VehicleDoor.All) + else: + world.hud.notification("Opening doors") + world.doors_are_open = True + world.player.open_door(carla.VehicleDoor.All) + except Exception: + pass + elif event.key == K_t: + if world.show_vehicle_telemetry: + world.player.show_debug_telemetry(False) + world.show_vehicle_telemetry = False + world.hud.notification("Disabled Vehicle Telemetry") + else: + try: + world.player.show_debug_telemetry(True) + world.show_vehicle_telemetry = True + world.hud.notification("Enabled Vehicle Telemetry") + except Exception: + pass + elif event.key > K_0 and event.key <= K_9: + index_ctrl = 0 + if pygame.key.get_mods() & KMOD_CTRL: + index_ctrl = 9 + world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) + 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.log") + 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.log'") + # replayer + client.replay_file("manual_recording.log", 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)) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_f: + # Toggle ackermann controller + self._ackermann_enabled = not self._ackermann_enabled + world.hud.show_ackermann_info(self._ackermann_enabled) + world.hud.notification("Ackermann Controller %s" % + ("Enabled" if self._ackermann_enabled else "Disabled")) + if event.key == K_q: + if not self._ackermann_enabled: + self._control.gear = 1 if self._control.reverse else -1 + else: + self._ackermann_reverse *= -1 + # Reset ackermann control + self._ackermann_control = carla.VehicleAckermannControl() + 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: + if not self._autopilot_enabled and not sync_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + 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: + if isinstance(self._control, carla.VehicleControl): + 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)) + # Apply control + if not self._ackermann_enabled: + world.player.apply_control(self._control) + else: + world.player.apply_ackermann_control(self._ackermann_control) + # Update control to the last one applied by the ackermann controller. + self._control = world.player.get_control() + # Update hud with the newest ackermann control + world.hud.update_ackermann_control(self._ackermann_control) + + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + if keys[K_UP] or keys[K_w]: + if not self._ackermann_enabled: + self._control.throttle = min(self._control.throttle + 0.1, 1.00) + else: + self._ackermann_control.speed += round(milliseconds * 0.005, 2) * self._ackermann_reverse + else: + if not self._ackermann_enabled: + self._control.throttle = 0.0 + + if keys[K_DOWN] or keys[K_s]: + if not self._ackermann_enabled: + self._control.brake = min(self._control.brake + 0.2, 1) + else: + self._ackermann_control.speed -= min(abs(self._ackermann_control.speed), round(milliseconds * 0.005, 2)) * self._ackermann_reverse + self._ackermann_control.speed = max(0, abs(self._ackermann_control.speed)) * self._ackermann_reverse + else: + if not self._ackermann_enabled: + 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)) + if not self._ackermann_enabled: + self._control.steer = round(self._steer_cache, 1) + self._control.hand_brake = keys[K_SPACE] + else: + self._ackermann_control.steer = round(self._steer_cache, 1) + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @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() + + self._show_ackermann_info = False + self._ackermann_control = carla.VehicleAckermannControl() + + 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, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + if self._show_ackermann_info: + self._info_text += [ + '', + 'Ackermann Controller:', + ' Target speed: % 8.0f km/h' % (3.6*self._ackermann_control.speed), + ] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + 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 show_ackermann_info(self, enabled): + self._show_ackermann_info = enabled + + def update_ackermann_control(self, ackermann_control): + self._ackermann_control = ackermann_control + + 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, gamma_correction): + 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 + + if not self._parent.type_id.startswith("walker.pedestrian"): + 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.SpringArmGhost), + (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.SpringArmGhost), + (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.SpringArmGhost), + (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] + else: + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] + + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], + ['sensor.camera.cosmos_visualization', cc.Raw, 'Cosmos Control Visualization', {}], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}], + ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], + ['sensor.camera.normals', cc.Raw, 'Camera Normals', {}], + ] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + 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 next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 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', 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 + original_settings = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2000.0) + + sim_world = client.get_world() + if args.sync: + original_settings = sim_world.get_settings() + settings = sim_world.get_settings() + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + + if args.autopilot and not sim_world.get_settings().synchronous_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + + 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(sim_world, hud, args) + controller = KeyboardControl(world, args.autopilot) + + if args.sync: + sim_world.tick() + else: + sim_world.wait_for_tick() + + clock = pygame.time.Clock() + while True: + if args.sync: + sim_world.tick() + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock, args.sync): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if original_settings: + sim_world.apply_settings(original_settings) + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + 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( + '--generation', + metavar='G', + default='2', + help='restrict to certain actor generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '--sync', + action='store_true', + help='Activate synchronous mode execution') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/manual_control_carsim.py b/carla_examples/manual_control_carsim.py new file mode 100644 index 0000000..ebe0f31 --- /dev/null +++ b/carla_examples/manual_control_carsim.py @@ -0,0 +1,1175 @@ +#!/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 + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + 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 collections +import datetime +import logging +import math +import os +import random +import re +import sys +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + 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_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_p + from pygame.locals import K_k + from pygame.locals import K_j + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + 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 find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.actor_role_name = args.rolename + 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._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + 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 a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + else: + print("No recommended values for 'speed' attribute") + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.imu_sensor = IMUSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + 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._carsim_enabled = False + self._carsim_road = False + self._autopilot_enabled = start_in_autopilot + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock): + if isinstance(self._control, carla.VehicleControl): + 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_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key > K_0 and event.key <= K_9: + world.camera_manager.set_sensor(event.key - 1 - K_0) + 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.log") + 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.log'") + # replayer + client.replay_file("manual_recording.log", world.recording_start, 0, 0) + world.camera_manager.set_sensor(current_index) + elif event.key == K_k and (pygame.key.get_mods() & KMOD_CTRL): + print("k pressed") + world.player.enable_carsim("d:/CVC/carsim/DataUE4/ue4simfile.sim") + elif event.key == K_j and (pygame.key.get_mods() & KMOD_CTRL): + self._carsim_road = not self._carsim_road + world.player.use_carsim_road(self._carsim_road) + print("j pressed, using carsim road =", self._carsim_road) + # elif event.key == K_i and (pygame.key.get_mods() & KMOD_CTRL): + # print("i pressed") + # imp = carla.Location(z=50000) + # world.player.add_impulse(imp) + 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)) + if 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 + 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: + if isinstance(self._control, carla.VehicleControl): + 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)) + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + 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.01, 1) + 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] + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @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, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + 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 + 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 + 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=2.8, z=1.0), + 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, gamma_correction): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + Attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=5.5, y=1.5, z=1.5)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-1, y=-bound_y, z=0.5)), Attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}]] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + 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 next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 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)) + 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(2.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + 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( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/manual_control_chrono.py b/carla_examples/manual_control_chrono.py new file mode 100644 index 0000000..b9970ea --- /dev/null +++ b/carla_examples/manual_control_chrono.py @@ -0,0 +1,1196 @@ +#!/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 + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + 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 collections +import datetime +import logging +import math +import os +import random +import re +import sys +import weakref +from pathlib import Path + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + 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_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_p + from pygame.locals import K_k + from pygame.locals import K_o + from pygame.locals import K_j + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + 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 find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.actor_role_name = args.rolename + 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._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + 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 a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + else: + print("No recommended values for 'speed' attribute") + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.imu_sensor = IMUSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + 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._carsim_enabled = False + self._carsim_road = False + self._chrono_enabled = False + self._autopilot_enabled = start_in_autopilot + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock): + if isinstance(self._control, carla.VehicleControl): + 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_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key > K_0 and event.key <= K_9: + world.camera_manager.set_sensor(event.key - 1 - K_0) + 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.log") + 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.log'") + # replayer + client.replay_file("manual_recording.log", world.recording_start, 0, 0) + world.camera_manager.set_sensor(current_index) + elif event.key == K_k and (pygame.key.get_mods() & KMOD_CTRL): + print("k pressed") + if not self._carsim_enabled: + self._carsim_enabled = True + world.player.enable_carsim() + else: + self._carsim_enabled = False + world.player.restore_physx_physics() + elif event.key == K_o and (pygame.key.get_mods() & KMOD_CTRL): + print("o pressed") + if not self._chrono_enabled: + self._chrono_enabled = True + vehicle_json = "sedan/vehicle/Sedan_Vehicle.json" + powertrain_json = "sedan/powertrain/Sedan_SimpleMapPowertrain.json" + tire_json = "sedan/tire/Sedan_TMeasyTire.json" + base_path = str(Path(__file__).resolve().parents[2] / "Co-Simulation" / "Chrono" / "Vehicles") + os.sep + world.player.enable_chrono_physics(5000, 0.002, vehicle_json, powertrain_json, tire_json, base_path) + else: + self._chrono_enabled = False + world.player.restore_physx_physics() + elif event.key == K_j and (pygame.key.get_mods() & KMOD_CTRL): + self._carsim_road = not self._carsim_road + world.player.use_carsim_road(self._carsim_road) + print("j pressed, using carsim road =", self._carsim_road) + # elif event.key == K_i and (pygame.key.get_mods() & KMOD_CTRL): + # print("i pressed") + # imp = carla.Location(z=50000) + # world.player.add_impulse(imp) + 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)) + if 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 + 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: + if isinstance(self._control, carla.VehicleControl): + 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)) + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + 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.01, 1) + 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] + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @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, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + 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 + 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 + 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=2.8, z=1.0), + 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, gamma_correction): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + Attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=5.5, y=1.5, z=1.5)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), Attachment.SpringArmGhost), + (carla.Transform(carla.Location(x=-1, y=-bound_y, z=0.5)), Attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}]] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + 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 next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 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)) + 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(200.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + 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( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/manual_control_steeringwheel.py b/carla_examples/manual_control_steeringwheel.py new file mode 100644 index 0000000..4253eaf --- /dev/null +++ b/carla_examples/manual_control_steeringwheel.py @@ -0,0 +1,848 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Intel Labs +# +# 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 with steering wheel Logitech G29. + +To drive start by preshing the brake pedal. +Change your wheel_config.ini according to your steering wheel. + +To find out the values of your steering wheel use jstest-gtk in Ubuntu. + +""" + +from __future__ import print_function + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import logging +import math +import os +import random +import re +import sys +import weakref + +if sys.version_info >= (3, 0): + + from configparser import ConfigParser + +else: + + from ConfigParser import RawConfigParser as ConfigParser + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + 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_c + from pygame.locals import K_d + from pygame.locals import K_h + from pygame.locals import K_m + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + 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') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, actor_filter): + self.world = carla_world + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = actor_filter + self.restart() + self.world.on_tick(hud.on_world_tick) + + def restart(self): + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Get a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) + blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + while self.player is None: + spawn_points = self.world.get_map().get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy(self): + sensors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor] + for sensor in sensors: + if sensor is not None: + sensor.stop() + sensor.destroy() + if self.player is not None: + self.player.destroy() + +# ============================================================================== +# -- DualControl ----------------------------------------------------------- +# ============================================================================== + + +class DualControl(object): + def __init__(self, world, start_in_autopilot): + self._autopilot_enabled = start_in_autopilot + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + world.player.set_autopilot(self._autopilot_enabled) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + # initialize steering wheel + pygame.joystick.init() + + joystick_count = pygame.joystick.get_count() + if joystick_count > 1: + raise ValueError("Please Connect Just One Joystick") + + self._joystick = pygame.joystick.Joystick(0) + self._joystick.init() + + self._parser = ConfigParser() + self._parser.read('wheel_config.ini') + self._steer_idx = int( + self._parser.get('G29 Racing Wheel', 'steering_wheel')) + self._throttle_idx = int( + self._parser.get('G29 Racing Wheel', 'throttle')) + self._brake_idx = int(self._parser.get('G29 Racing Wheel', 'brake')) + self._reverse_idx = int(self._parser.get('G29 Racing Wheel', 'reverse')) + self._handbrake_idx = int( + self._parser.get('G29 Racing Wheel', 'handbrake')) + + def parse_events(self, world, clock): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.JOYBUTTONDOWN: + if event.button == 0: + world.restart() + elif event.button == 1: + world.hud.toggle_info() + elif event.button == 2: + world.camera_manager.toggle_camera() + elif event.button == 3: + world.next_weather() + elif event.button == self._reverse_idx: + self._control.gear = 1 if self._control.reverse else -1 + elif event.button == 23: + world.camera_manager.next_sensor() + + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_BACKSPACE: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key > K_0 and event.key <= K_9: + world.camera_manager.set_sensor(event.key - 1 - K_0) + elif event.key == K_r: + world.camera_manager.toggle_recording() + if 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 + 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: + 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')) + + if not self._autopilot_enabled: + if isinstance(self._control, carla.VehicleControl): + self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) + self._parse_vehicle_wheel() + self._control.reverse = self._control.gear < 0 + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time()) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + 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_vehicle_wheel(self): + numAxes = self._joystick.get_numaxes() + jsInputs = [float(self._joystick.get_axis(i)) for i in range(numAxes)] + # print (jsInputs) + jsButtons = [float(self._joystick.get_button(i)) for i in + range(self._joystick.get_numbuttons())] + + # Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed + # For the steering, it seems fine as it is + K1 = 1.0 # 0.55 + steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx]) + + K2 = 1.6 # 1.6 + throttleCmd = K2 + (2.05 * math.log10( + -0.7 * jsInputs[self._throttle_idx] + 1.4) - 1.2) / 0.92 + if throttleCmd <= 0: + throttleCmd = 0 + elif throttleCmd > 1: + throttleCmd = 1 + + brakeCmd = 1.6 + (2.05 * math.log10( + -0.7 * jsInputs[self._brake_idx] + 1.4) - 1.2) / 0.92 + if brakeCmd <= 0: + brakeCmd = 0 + elif brakeCmd > 1: + brakeCmd = 1 + + self._control.steer = steerCmd + self._control.brake = brakeCmd + self._control.throttle = throttleCmd + + #toggle = jsButtons[self._reverse_idx] + + self._control.hand_brake = bool(jsButtons[self._handbrake_idx]) + + def _parse_walker_keys(self, keys, milliseconds): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = 5.556 if pygame.key.get_mods() & KMOD_SHIFT else 2.778 + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + if not self._show_info: + return + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() + heading = 'N' if abs(t.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(t.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > t.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > t.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.world.get_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'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % t.location.z, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + 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): + if d > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (d, vehicle_type)) + + def toggle_info(self): + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + self._camera_transforms = [ + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + carla.Transform(carla.Location(x=1.6, z=1.7))] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + elif item[0].startswith('sensor.lidar'): + bp.set_attribute('range', '50') + item.append(bp) + self.index = None + + def toggle_camera(self): + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.sensor.set_transform(self._camera_transforms[self.transform_index]) + + def set_sensor(self, index, notify=True): + index = index % len(self.sensors) + needs_respawn = True if self.index is None \ + else self.sensors[index][0] != self.sensors[self.index][0] + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index], + attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args.filter) + controller = DualControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(world, clock): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + 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.*")') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/no_rendering_mode.py b/carla_examples/no_rendering_mode.py new file mode 100644 index 0000000..587b538 --- /dev/null +++ b/carla_examples/no_rendering_mode.py @@ -0,0 +1,1627 @@ +#!/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 +""" + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + +import carla +from carla import TrafficLightState as tls + +import argparse +import logging +import datetime +import glob +import weakref +import math +import os +import random +import sys +import hashlib + +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 +TITLE_WORLD = 'WORLD' +TITLE_HUD = 'HUD' +TITLE_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): + """Function that renders the all the source surfaces in a destination source""" + for surface in source_surfaces: + destination_surface.blit(surface[0], surface[1], rect, blend_mode) + + @staticmethod + def length(v): + """Returns the length of a vector""" + return math.sqrt(v.x**2 + v.y**2 + v.z**2) + + @staticmethod + def get_bounding_box(actor): + """Gets the bounding box corners of an actor in world space""" + 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 + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + """Renders texts that fades out after some seconds that the user specifies""" + + def __init__(self, font, dim, pos): + """Initializes variables such as text font, dimensions and position""" + 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): + """Sets the text, color and seconds until fade out""" + 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): + """Each frame, it shows the displayed text for some specified seconds, if any""" + 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): + """ Renders the text in its surface and its position""" + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + def __init__(self, font, width, height): + """Renders the help text that shows the controls for using no rendering mode""" + 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): + """Toggles display of help text""" + self._render = not self._render + + def render(self, display): + """Renders the help text, if enabled""" + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------- +# ============================================================================== + + +class HUD (object): + """Class encharged of rendering the HUD that displays information about the world and the hero vehicle""" + + def __init__(self, name, width, height): + """Initializes default HUD params and content data parameters that will be displayed""" + self.name = name + self.dim = (width, height) + self._init_hud_params() + self._init_data_params() + + def start(self): + """Does nothing since it does not need to use other modules""" + + def _init_hud_params(self): + """Initialized visual parameters such as font text and size""" + 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, 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): + """Initializes the content data structures""" + self.show_info = True + self.show_actor_ids = False + self._info_text = {} + + def notification(self, text, seconds=2.0): + """Shows fading texts for some specified seconds""" + self._notifications.set_text(text, seconds=seconds) + + def tick(self, clock): + """Updated the fading texts each frame""" + self._notifications.tick(clock) + + def add_info(self, title, info): + """Adds a block of information in the left HUD panel of the visualizer""" + self._info_text[title] = info + + def render_vehicles_ids(self, vehicle_id_surface, list_actors, world_to_pixel, hero_actor, hero_transform): + """When flag enabled, it shows the IDs of the vehicles that are spawned in the world. Depending on the vehicle type, + it will render it in different colors""" + + 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 flag enabled, it renders all the information regarding the left panel of the visualizer""" + 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 title, info in self._info_text.items(): + if not info: + continue + surface = self._header_font.render(title, 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 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): + """Draws a traffic light, which is composed of a dark background surface with 3 circles that indicate its color depending on the state""" + 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 + + # Draws the corresponding color if is on, otherwise it will be gray if its off + 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): + """Rotates and scales the traffic light surface""" + for key, surface in self._original_surfaces.items(): + self.surfaces[key] = pygame.transform.rotozoom(surface, angle, scale) + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class MapImage(object): + """Class encharged of rendering a 2D image from top view of a carla world. Please note that a cache system is used, so if the OpenDrive content + of a Carla town has not changed, it will read and use the stored image if it was rendered in a previous execution""" + + def __init__(self, carla_world, carla_map, pixels_per_meter, show_triggers, show_connections, show_spawn_points): + """ Renders the map image generated based on the world, its map and additional flags that provide extra information about the road network""" + 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) + + # Maximum size of a Pygame surface + width_in_pixels = (1 << 14) - 1 + + # Adapt Pixels per meter to make world fit in surface + surface_pixel_per_meter = int(width_in_pixels / self.width) + if surface_pixel_per_meter > PIXELS_PER_METER: + surface_pixel_per_meter = PIXELS_PER_METER + + self._pixels_per_meter = surface_pixel_per_meter + width_in_pixels = int(self._pixels_per_meter * self.width) + + self.big_map_surface = pygame.Surface((width_in_pixels, width_in_pixels)).convert() + + # Load OpenDrive content + opendrive_content = carla_map.to_opendrive() + + # Get hash based on content + hash_func = hashlib.sha1() + hash_func.update(opendrive_content.encode("UTF-8")) + opendrive_hash = str(hash_func.hexdigest()) + + # Build path for saving or loading the cached rendered map + filename = carla_map.name.split('/')[-1] + "_" + opendrive_hash + ".tga" + dirname = os.path.join("cache", "no_rendering_mode") + full_path = str(os.path.join(dirname, filename)) + + if os.path.isfile(full_path): + # Load Image + self.big_map_surface = pygame.image.load(full_path) + else: + # Render map + self.draw_road_map( + self.big_map_surface, + carla_world, + carla_map, + self.world_to_pixel, + self.world_to_pixel_width) + + # If folders path does not exist, create it + if not os.path.exists(dirname): + os.makedirs(dirname) + + # Remove files if selected town had a previous version saved + list_filenames = glob.glob(os.path.join(dirname, carla_map.name) + "*") + for town_filename in list_filenames: + os.remove(town_filename) + + # Save rendered map for next executions of same map + pygame.image.save(self.big_map_surface, full_path) + + self.surface = self.big_map_surface + + def draw_road_map(self, map_surface, carla_world, carla_map, world_to_pixel, world_to_pixel_width): + """Draws all the roads, including lane markings, arrows and traffic signs""" + map_surface.fill(COLOR_ALUMINIUM_4) + precision = 0.05 + + def lane_marking_color_to_tango(lane_marking_color): + """Maps the lane marking color enum specified in PythonAPI to a Tango 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): + """Draws solid lines in a surface given a set of points, width and color""" + if len(points) >= 2: + pygame.draw.lines(surface, color, closed, points, width) + + def draw_broken_line(surface, color, closed, points, width): + """Draws broken lines in a surface given a set of points, width and color""" + # Select which lines are going to be rendered from the set of lines + broken_lines = [x for n, x in enumerate(zip(*(iter(points),) * 20)) if n % 3 == 0] + + # Draw selected lines + 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): + """For multiple lane marking types (SolidSolid, BrokenSolid, SolidBroken and BrokenBroken), it converts them + as a combination of Broken and Solid lines""" + margin = 0.25 + marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints] + if lane_marking_type == carla.LaneMarkingType.Broken or (lane_marking_type == carla.LaneMarkingType.Solid): + return [(lane_marking_type, lane_marking_color, marking_1)] + else: + marking_2 = [world_to_pixel(lateral_shift(w.transform, + sign * (w.lane_width * 0.5 + margin * 2))) for w in waypoints] + if lane_marking_type == carla.LaneMarkingType.SolidBroken: + return [(carla.LaneMarkingType.Broken, lane_marking_color, marking_1), + (carla.LaneMarkingType.Solid, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.BrokenSolid: + return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1), + (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.BrokenBroken: + return [(carla.LaneMarkingType.Broken, lane_marking_color, marking_1), + (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.SolidSolid: + return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1), + (carla.LaneMarkingType.Solid, lane_marking_color, marking_2)] + + return [(carla.LaneMarkingType.NONE, carla.LaneMarkingColor.Other, [])] + + def draw_lane(surface, lane, color): + """Renders a single lane in a surface and with a specified color""" + for side in lane: + lane_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in side] + lane_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in side] + + polygon = lane_left_side + [x for x in reversed(lane_right_side)] + polygon = [world_to_pixel(x) for x in polygon] + + if len(polygon) > 2: + pygame.draw.polygon(surface, color, polygon, 5) + pygame.draw.polygon(surface, color, polygon) + + def draw_lane_marking(surface, waypoints): + """Draws the left and right side of lane markings""" + # Left Side + draw_lane_marking_single_side(surface, waypoints[0], -1) + + # Right Side + draw_lane_marking_single_side(surface, waypoints[1], 1) + + def draw_lane_marking_single_side(surface, waypoints, sign): + """Draws the lane marking given a set of waypoints and decides whether drawing the right or left side of + the waypoint based on the sign parameter""" + 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 + + markings_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: + # Get the list of lane markings to draw + markings = get_lane_markings( + previous_marking_type, + lane_marking_color_to_tango(previous_marking_color), + temp_waypoints, + sign) + current_lane_marking = marking_type + + # Append each lane marking in the list + for marking in markings: + markings_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: + markings_list.append(marking) + + # Once the lane markings have been simplified to Solid or Broken lines, we draw them + for markings in markings_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): + """ Draws an arrow with a specified color given a transform""" + 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 + + # Draw lines + 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): + """Draw stop traffic signs and its bounding box if enabled""" + 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 of the stop trigger + 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 draw_crosswalk(surface, transform=None, color=COLOR_ALUMINIUM_2): + # """Given two points A and B, draw white parallel lines from A to B""" + # a = carla.Location(0.0, 0.0, 0.0) + # b = carla.Location(10.0, 10.0, 0.0) + + # ab = b - a + # length_ab = math.sqrt(ab.x**2 + ab.y**2) + # unit_ab = ab / length_ab + # unit_perp_ab = carla.Location(-unit_ab.y, unit_ab.x, 0.0) + + # # Crosswalk lines params + # space_between_lines = 0.5 + # line_width = 0.7 + # line_height = 2 + + # current_length = 0 + # while current_length < length_ab: + + # center = a + unit_ab * current_length + + # width_offset = unit_ab * line_width + # height_offset = unit_perp_ab * line_height + # list_point = [center - width_offset - height_offset, + # center + width_offset - height_offset, + # center + width_offset + height_offset, + # center - width_offset + height_offset] + + # list_point = [world_to_pixel(p) for p in list_point] + # pygame.draw.polygon(surface, color, list_point) + # current_length += (line_width + space_between_lines) * 2 + + def lateral_shift(transform, shift): + """Makes a lateral shift of the forward vector of a transform""" + transform.rotation.yaw += 90 + return transform.location + shift * transform.get_forward_vector() + + def draw_topology(carla_topology, index): + """ Draws traffic signs and the roads network with sidewalks, parking and shoulders by generating waypoints""" + topology = [x[index] for x in carla_topology] + topology = sorted(topology, key=lambda w: w.transform.location.z) + set_waypoints = [] + for waypoint in topology: + waypoints = [waypoint] + + # Generate waypoints of a road id. Stop when road id differs + 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 + set_waypoints.append(waypoints) + + # Draw Shoulders, Parkings and Sidewalks + PARKING_COLOR = COLOR_ALUMINIUM_4_5 + SHOULDER_COLOR = COLOR_ALUMINIUM_5 + SIDEWALK_COLOR = COLOR_ALUMINIUM_3 + + shoulder = [[], []] + parking = [[], []] + sidewalk = [[], []] + + for w in waypoints: + # Classify lane types until there are no waypoints by going left + l = w.get_left_lane() + while l and l.lane_type != carla.LaneType.Driving: + + if l.lane_type == carla.LaneType.Shoulder: + shoulder[0].append(l) + + if l.lane_type == carla.LaneType.Parking: + parking[0].append(l) + + if l.lane_type == carla.LaneType.Sidewalk: + sidewalk[0].append(l) + + l = l.get_left_lane() + + # Classify lane types until there are no waypoints by going right + r = w.get_right_lane() + while r and r.lane_type != carla.LaneType.Driving: + + if r.lane_type == carla.LaneType.Shoulder: + shoulder[1].append(r) + + if r.lane_type == carla.LaneType.Parking: + parking[1].append(r) + + if r.lane_type == carla.LaneType.Sidewalk: + sidewalk[1].append(r) + + r = r.get_right_lane() + + # Draw classified lane types + draw_lane(map_surface, shoulder, SHOULDER_COLOR) + draw_lane(map_surface, parking, PARKING_COLOR) + draw_lane(map_surface, sidewalk, SIDEWALK_COLOR) + + # Draw Roads + for waypoints in set_waypoints: + waypoint = waypoints[0] + 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 Lane Markings and Arrows + if not waypoint.is_junction: + draw_lane_marking(map_surface, [waypoints, waypoints]) + 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) + + 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 + + def to_pixel(wp): return world_to_pixel(wp.transform.location) + for wp in carla_map.generate_waypoints(dist): + col = (0, 255, 255) if wp.is_junction 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() + + # Find and Draw Traffic Signs: Stops and Yields + 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)): + """Converts the world coordinates to pixel coordinates""" + 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): + """Converts the world units to pixel units""" + return int(self.scale * self._pixels_per_meter * width) + + def scale_map(self, scale): + """Scales the map surface""" + 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 World(object): + """Class that contains all the information of a carla world that is running on the server side""" + + 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 = [] + + self._hud = None + self._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): + """Retrieves the data from the server side""" + 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, hud, input_control): + """Build the map image, stores the needed modules and prepares rendering in Hero Mode""" + self.world, self.town_map = self._get_data_from_carla() + + settings = self.world.get_settings() + settings.no_rendering_mode = self.args.no_rendering + self.world.apply_settings(settings) + + # 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) + + self._hud = hud + self._input = input_control + + self.original_surface_size = min(self._hud.dim[0], self._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._hud.dim, pygame.SRCALPHA).convert() + self.border_round_surface.set_colorkey(COLOR_WHITE) + self.border_round_surface.fill(COLOR_BLACK) + + # Used for Hero Mode, draws the map contained in a circle with white border + center_offset = (int(self._hud.dim[0] / 2), int(self._hud.dim[1] / 2)) + pygame.draw.circle(self.border_round_surface, COLOR_ALUMINIUM_1, center_offset, int(self._hud.dim[1] / 2)) + pygame.draw.circle(self.border_round_surface, COLOR_WHITE, center_offset, int((self._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._input.wheel_offset = HERO_DEFAULT_SCALE + self._input.control = carla.VehicleControl() + + # Register event for receiving server tick + weak_self = weakref.ref(self) + self.world.on_tick(lambda timestamp: World.on_world_tick(weak_self, timestamp)) + + def select_hero_actor(self): + """Selects only one hero actor if there are more than one. If there are not any, it will spawn one.""" + 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): + """Spawns the hero actor when the script runs""" + # 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): + """Retrieves the actors for Hero and Map modes and updates de HUD based on that""" + actors = self.world.get_actors() + + # We store the transforms also so that we avoid having transforms of + # previous tick and current tick when rendering them. + 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): + """Updates the HUD info regarding simulation, hero mode and whether there is a traffic light affecting the hero actor""" + + 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() + if math.isnan(affected_speed_limit_text): + affected_speed_limit_text = 0.0 + 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) + 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, + ] + + self._hud.add_info(self.name, info_text) + self._hud.add_info('HERO', hero_mode_text) + + @staticmethod + def on_world_tick(weak_self, timestamp): + """Updates the server tick""" + 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 _show_nearby_vehicles(self, vehicles): + """Shows nearby vehicles of the hero actor""" + 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)) + self._hud.add_info('NEARBY VEHICLES', info_text) + + def _split_actors(self): + """Splits the retrieved actors by type id""" + 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.pedestrian' in actor.type_id: + walkers.append(actor_with_transform) + + return (vehicles, traffic_lights, speed_limits, walkers) + + def _render_traffic_lights(self, surface, list_tl, world_to_pixel): + """Renders the traffic lights and shows its triggers and bounding boxes if flags are enabled""" + 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): + """Renders the speed limits by drawing two concentric circles (outer is red and inner white) and a speed limit text""" + + 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 concentric circles + 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: + # In hero mode, 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: + # In map mode, there is no need to rotate the text of the speed limit + surface.blit(font_surface, (x - radius / 2, y - radius / 2)) + + def _render_walkers(self, surface, list_w, world_to_pixel): + """Renders the walkers' bounding boxes""" + 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): + """Renders the vehicles' bounding boxes""" + 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): + """Renders all the actors""" + # 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): + """Used to improve perfomance. Clips the surfaces in order to render only the part of the surfaces that are going to be visible""" + 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): + """Based on the mouse wheel and mouse position, it will compute the scale and move the map so that it is zoomed in or out based on mouse position""" + m = self._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): + """Renders the map and all the actors in hero and map mode""" + if self.actors_with_transforms is None: + return + self.result_surface.fill(COLOR_BLACK) + + # Split the actors by vehicle type id + vehicles, traffic_lights, speed_limits, walkers = self._split_actors() + + # Zoom in and out + scale_factor = self._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._hud.render_vehicles_ids(self.vehicle_id_surface, vehicles, + self.map_image.world_to_pixel, self.hero_actor, self.hero_transform) + # Show nearby actors from hero mode + self._show_nearby_vehicles(vehicles) + + # 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 Mode + 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: + # Map Mode + # Translation offset + translation_offset = (self._input.mouse_offset[0] * scale_factor + self.scale_offset[0], + self._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._hud.dim[0], self._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): + """Destroy the hero actor when class instance is destroyed""" + if self.spawned_hero is not None: + self.spawned_hero.destroy() + +# ============================================================================== +# -- Input ----------------------------------------------------------- +# ============================================================================== + + +class InputControl(object): + """Class that handles input received such as keyboard and mouse.""" + + def __init__(self, name): + """Initializes input member variables when instance is created.""" + 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 + + # Modules that input will depend on + self._hud = None + self._world = None + + def start(self, hud, world): + """Assigns other initialized modules that input module needs.""" + self._hud = hud + self._world = world + + self._hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def render(self, display): + """Does nothing. Input module does not need render anything.""" + + def tick(self, clock): + """Executed each frame. Calls method for parsing input.""" + self.parse_input(clock) + + def _parse_events(self): + """Parses input events. These events are executed only once when pressing a key.""" + 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): + self._hud.help.toggle() + elif event.key == K_TAB: + # Toggle between hero and map mode + if self._world.hero_actor is None: + self._world.select_hero_actor() + self.wheel_offset = HERO_DEFAULT_SCALE + self.control = carla.VehicleControl() + self._hud.notification('Hero Mode') + else: + self.wheel_offset = MAP_DEFAULT_SCALE + self.mouse_offset = [0, 0] + self.mouse_pos = [0, 0] + self._world.scale_offset = [0, 0] + self._world.hero_actor = None + self._hud.notification('Map Mode') + elif event.key == K_F1: + self._hud.show_info = not self._hud.show_info + elif event.key == K_i: + self._hud.show_actor_ids = not self._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 + self.control.gear = self._world.hero_actor.get_control().gear + self._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: + # Toggle autopilot + if self._world.hero_actor is not None: + self._autopilot_enabled = not self._autopilot_enabled + self._world.hero_actor.set_autopilot(self._autopilot_enabled) + self._hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.type == pygame.MOUSEBUTTONDOWN: + # Handle mouse wheel for zooming in and out + 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): + """Parses keyboard input when keys are pressed""" + 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): + """Parses mouse input""" + 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): + """Parses the input, which is classified in keyboard events and mouse""" + 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 + if (self._world.hero_actor is not None): + self._world.hero_actor.apply_control(self.control) + + @staticmethod + def _is_quit_shortcut(key): + """Returns True if one of the specified keys are pressed""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + """Initialized, Starts and runs all the needed modules for No Rendering Mode""" + try: + # Init Pygame + pygame.init() + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + # Place a title to game window + pygame.display.set_caption(args.description) + + # Show loading screen + 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 + input_control = InputControl(TITLE_INPUT) + hud = HUD(TITLE_HUD, args.width, args.height) + world = World(TITLE_WORLD, args, timeout=2.0) + + # For each module, assign other modules that are going to be used inside that module + input_control.start(hud, world) + hud.start() + world.start(hud, input_control) + + # Game loop + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + + # Tick all modules + world.tick(clock) + hud.tick(clock) + input_control.tick(clock) + + # Render all modules + display.fill(COLOR_ALUMINIUM_4) + world.render(display) + hud.render(display) + input_control.render(display) + + pygame.display.flip() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + finally: + if world is not None: + world.destroy() + + +def exit_game(): + """Shuts down program and PyGame""" + pygame.quit() + sys.exit() + +# ============================================================================== +# -- Main -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + """Parses the arguments received from commandline and runs the game loop""" + + # Define arguments that will be received and parsed + 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', + 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') + + # Parse arguments + args = argparser.parse_args() + args.description = argparser.description + args.width, args.height = [int(x) for x in args.res.split('x')] + + # Print server information + 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__) + + # Run game loop + game_loop(args) + + +if __name__ == '__main__': + main() diff --git a/carla_examples/open3d_lidar.py b/carla_examples/open3d_lidar.py new file mode 100644 index 0000000..7dcfb9b --- /dev/null +++ b/carla_examples/open3d_lidar.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 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 . + +"""Open3D Lidar visualization example for CARLA""" + +import sys +import argparse +import time +from datetime import datetime +import random +import numpy as np +from matplotlib import cm +import open3d as o3d + +import carla + +VIRIDIS = np.array(cm._colormaps.get_cmap('plasma').colors) +VID_RANGE = np.linspace(0.0, 1.0, VIRIDIS.shape[0]) +LABEL_COLORS = np.array([ + (0, 0, 0), # 0: None + # cityscape labels + (128, 64, 128), # 1: Roads + (244, 35, 232), # 2: Sidewalks + (70, 70, 70), # 3: Buildings + (102, 102, 156), # 4: Walls + (190, 153, 153), # 5: Fences + (153, 153, 153), # 6: Poles + (250, 170, 30), # 7: TrafficLight + (220, 220, 0), # 8: TrafficSigns + (107, 142, 35), # 9: Vegetation + (145, 170, 100), # 10: Terrain + (70, 130, 180), # 11: Sky + (220, 20, 60), # 12: Pedestrians + (255, 0, 0), # 13: Rider + (0, 0, 142), # 14: Car + (0, 0, 70), # 15: Truck + (0, 60, 100), # 16: Bus + (0, 80, 100), # 17: Train + (0, 0, 230), # 18: Motorcycle + (119, 11, 32), # 19: Bicycle + # custom labels + (110, 190, 160), # 20: Static (custom, teal) + (170, 120, 50), # 21: Dynamic (custom, brown) + (55, 90, 80), # 22: Other (custom, dark teal) + (45, 60, 150), # 23: Water (custom, blue) + (157, 234, 50), # 24: RoadLines (custom, bright green) + (81, 0, 81), # 25: Ground (custom, purple) + (150, 100, 100), # 26: Bridge (custom, muted red) + (230, 150, 140), # 27: RailTrack (custom, pink) + (180, 165, 180), # 28: GuardRail (custom, light purple) +]) / 255.0 # normalize to [0, 1] for Open3D + +def lidar_callback(point_cloud, point_list): + """Prepares a point cloud with intensity + colors ready to be consumed by Open3D""" + data = np.copy(np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4'))) + data = np.reshape(data, (int(data.shape[0] / 4), 4)) + + # Isolate the intensity and compute a color for it + intensity = data[:, -1] + intensity_col = 1.0 - np.log(intensity) / np.log(np.exp(-0.004 * 100)) + int_color = np.c_[ + np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 0]), + np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 1]), + np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 2])] + + # Isolate the 3D data + points = data[:, :-1] + + # We're negating the y to correclty visualize a world that matches + # what we see in Unreal since Open3D uses a right-handed coordinate system + points[:, :1] = -points[:, :1] + + # # An example of converting points from sensor to vehicle space if we had + # # a carla.Transform variable named "tran": + # points = np.append(points, np.ones((points.shape[0], 1)), axis=1) + # points = np.dot(tran.get_matrix(), points.T).T + # points = points[:, :-1] + + point_list.points = o3d.utility.Vector3dVector(points) + point_list.colors = o3d.utility.Vector3dVector(int_color) + + +def semantic_lidar_callback(point_cloud, point_list): + """Prepares a point cloud with semantic segmentation + colors ready to be consumed by Open3D""" + data = np.frombuffer(point_cloud.raw_data, dtype=np.dtype([ + ('x', np.float32), ('y', np.float32), ('z', np.float32), + ('CosAngle', np.float32), ('ObjIdx', np.uint32), ('ObjTag', np.uint32)])) + + # We're negating the y to correclty visualize a world that matches + # what we see in Unreal since Open3D uses a right-handed coordinate system + points = np.array([data['x'], -data['y'], data['z']]).T + + # # An example of adding some noise to our data if needed: + # points += np.random.uniform(-0.05, 0.05, size=points.shape) + + # Colorize the pointcloud based on the CityScapes color palette + labels = np.array(data['ObjTag']) + int_color = LABEL_COLORS[labels] + + # # In case you want to make the color intensity depending + # # of the incident ray angle, you can use: + # int_color *= np.array(data['CosAngle'])[:, None] + + point_list.points = o3d.utility.Vector3dVector(points) + point_list.colors = o3d.utility.Vector3dVector(int_color) + + +def generate_lidar_bp(arg, world, blueprint_library, delta): + """Generates a CARLA blueprint based on the script parameters""" + if arg.semantic: + lidar_bp = world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') + else: + lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') + if arg.no_noise: + lidar_bp.set_attribute('dropoff_general_rate', '0.0') + lidar_bp.set_attribute('dropoff_intensity_limit', '1.0') + lidar_bp.set_attribute('dropoff_zero_intensity', '0.0') + else: + lidar_bp.set_attribute('noise_stddev', '0.2') + + lidar_bp.set_attribute('upper_fov', str(arg.upper_fov)) + lidar_bp.set_attribute('lower_fov', str(arg.lower_fov)) + lidar_bp.set_attribute('channels', str(arg.channels)) + lidar_bp.set_attribute('range', str(arg.range)) + lidar_bp.set_attribute('rotation_frequency', str(1.0 / delta)) + lidar_bp.set_attribute('points_per_second', str(arg.points_per_second)) + return lidar_bp + + +def add_open3d_axis(vis): + """Add a small 3D axis on Open3D Visualizer""" + axis = o3d.geometry.LineSet() + axis.points = o3d.utility.Vector3dVector(np.array([ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0]])) + axis.lines = o3d.utility.Vector2iVector(np.array([ + [0, 1], + [0, 2], + [0, 3]])) + axis.colors = o3d.utility.Vector3dVector(np.array([ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0]])) + vis.add_geometry(axis) + + +def main(arg): + """Main function of the script""" + client = carla.Client(arg.host, arg.port) + client.set_timeout(2.0) + world = client.get_world() + + try: + original_settings = world.get_settings() + settings = world.get_settings() + traffic_manager = client.get_trafficmanager(8000) + traffic_manager.set_synchronous_mode(True) + + delta = 0.05 + + settings.fixed_delta_seconds = delta + settings.synchronous_mode = True + settings.no_rendering_mode = arg.no_rendering + world.apply_settings(settings) + + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.filter(arg.filter)[0] + vehicle_transform = random.choice(world.get_map().get_spawn_points()) + vehicle = world.spawn_actor(vehicle_bp, vehicle_transform) + vehicle.set_autopilot(arg.no_autopilot) + + lidar_bp = generate_lidar_bp(arg, world, blueprint_library, delta) + + user_offset = carla.Location(arg.x, arg.y, arg.z) + lidar_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) + + lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) + + point_list = o3d.geometry.PointCloud() + if arg.semantic: + lidar.listen(lambda data: semantic_lidar_callback(data, point_list)) + else: + lidar.listen(lambda data: lidar_callback(data, point_list)) + + vis = o3d.visualization.Visualizer() + vis.create_window( + window_name='Carla Lidar', + width=960, + height=540, + left=480, + top=270) + vis.get_render_option().background_color = [0.05, 0.05, 0.05] + vis.get_render_option().point_size = 1 + vis.get_render_option().show_coordinate_frame = True + + if arg.show_axis: + add_open3d_axis(vis) + + frame = 0 + dt0 = datetime.now() + while True: + if frame == 2: + vis.add_geometry(point_list) + vis.update_geometry(point_list) + + vis.poll_events() + vis.update_renderer() + # # This can fix Open3D jittering issues: + time.sleep(0.005) + world.tick() + + process_time = datetime.now() - dt0 + sys.stdout.write('\r' + 'FPS: ' + str(1.0 / process_time.total_seconds())) + sys.stdout.flush() + dt0 = datetime.now() + frame += 1 + + finally: + world.apply_settings(original_settings) + traffic_manager.set_synchronous_mode(False) + + vehicle.destroy() + lidar.destroy() + vis.destroy_window() + + +if __name__ == "__main__": + argparser = argparse.ArgumentParser( + description=__doc__) + argparser.add_argument( + '--host', + metavar='H', + default='localhost', + help='IP of the host CARLA Simulator (default: localhost)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port of CARLA Simulator (default: 2000)') + argparser.add_argument( + '--no-rendering', + action='store_true', + help='use the no-rendering mode which will provide some extra' + ' performance but you will lose the articulated objects in the' + ' lidar, such as pedestrians') + argparser.add_argument( + '--semantic', + action='store_true', + help='use the semantic lidar instead, which provides ground truth' + ' information') + argparser.add_argument( + '--no-noise', + action='store_true', + help='remove the drop off and noise from the normal (non-semantic) lidar') + argparser.add_argument( + '--no-autopilot', + action='store_false', + help='disables the autopilot so the vehicle will remain stopped') + argparser.add_argument( + '--show-axis', + action='store_true', + help='show the cartesian coordinates axis') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='model3', + help='actor filter (default: "vehicle.*")') + argparser.add_argument( + '--upper-fov', + default=15.0, + type=float, + help='lidar\'s upper field of view in degrees (default: 15.0)') + argparser.add_argument( + '--lower-fov', + default=-25.0, + type=float, + help='lidar\'s lower field of view in degrees (default: -25.0)') + argparser.add_argument( + '--channels', + default=64.0, + type=float, + help='lidar\'s channel count (default: 64)') + argparser.add_argument( + '--range', + default=100.0, + type=float, + help='lidar\'s maximum range in meters (default: 100.0)') + argparser.add_argument( + '--points-per-second', + default=500000, + type=int, + help='lidar\'s points per second (default: 500000)') + argparser.add_argument( + '-x', + default=0.0, + type=float, + help='offset in the sensor position in the X-axis in meters (default: 0.0)') + argparser.add_argument( + '-y', + default=0.0, + type=float, + help='offset in the sensor position in the Y-axis in meters (default: 0.0)') + argparser.add_argument( + '-z', + default=0.0, + type=float, + help='offset in the sensor position in the Z-axis in meters (default: 0.0)') + args = argparser.parse_args() + + try: + main(args) + except KeyboardInterrupt: + print(' - Exited by user.') diff --git a/carla_examples/requirements.txt b/carla_examples/requirements.txt new file mode 100644 index 0000000..711528c --- /dev/null +++ b/carla_examples/requirements.txt @@ -0,0 +1,9 @@ +invertedai@git+https://github.com/inverted-ai/invertedai.git@develop ; python_version >= "3.10" +future +numpy<2.0.0 +networkx +Shapely +pygame +matplotlib +open3d +Pillow diff --git a/carla_examples/sensor_synchronization.py b/carla_examples/sensor_synchronization.py new file mode 100644 index 0000000..9cb2c77 --- /dev/null +++ b/carla_examples/sensor_synchronization.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 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 . + +""" +Sensor synchronization example for CARLA + +The communication model for the syncronous mode in CARLA sends the snapshot +of the world and the sensors streams in parallel. +We provide this script as an example of how to syncrononize the sensor +data gathering in the client. +To to this, we create a queue that is being filled by every sensor when the +client receives its data and the main loop is blocked until all the sensors +have received its data. +This suppose that all the sensors gather information at every tick. It this is +not the case, the clients needs to take in account at each frame how many +sensors are going to tick at each frame. +""" + +from queue import Queue +from queue import Empty + +import carla + + +# Sensor callback. +# This is where you receive the sensor data and +# process it as you liked and the important part is that, +# at the end, it should include an element into the sensor queue. +def sensor_callback(sensor_data, sensor_queue, sensor_name): + # Do stuff with the sensor_data data like save it to disk + # Then you just need to add to the queue + sensor_queue.put((sensor_data.frame, sensor_name)) + + +def main(): + # We start creating the client + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + world = client.get_world() + + try: + # We need to save the settings to be able to recover them at the end + # of the script to leave the server in the same state that we found it. + original_settings = world.get_settings() + settings = world.get_settings() + + # We set CARLA syncronous mode + settings.fixed_delta_seconds = 0.2 + settings.synchronous_mode = True + world.apply_settings(settings) + + # We create the sensor queue in which we keep track of the information + # already received. This structure is thread safe and can be + # accessed by all the sensors callback concurrently without problem. + sensor_queue = Queue() + + # Bluepints for the sensors + blueprint_library = world.get_blueprint_library() + cam_bp = blueprint_library.find('sensor.camera.rgb') + lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') + radar_bp = blueprint_library.find('sensor.other.radar') + + # We create all the sensors and keep them in a list for convenience. + sensor_list = [] + + cam01 = world.spawn_actor(cam_bp, carla.Transform()) + cam01.listen(lambda data: sensor_callback(data, sensor_queue, "camera01")) + sensor_list.append(cam01) + + cam02 = world.spawn_actor(cam_bp, carla.Transform()) + cam02.listen(lambda data: sensor_callback(data, sensor_queue, "camera02")) + sensor_list.append(cam02) + + cam03 = world.spawn_actor(cam_bp, carla.Transform()) + cam03.listen(lambda data: sensor_callback(data, sensor_queue, "camera03")) + sensor_list.append(cam03) + + lidar_bp.set_attribute('points_per_second', '100000') + lidar01 = world.spawn_actor(lidar_bp, carla.Transform()) + lidar01.listen(lambda data: sensor_callback(data, sensor_queue, "lidar01")) + sensor_list.append(lidar01) + + lidar_bp.set_attribute('points_per_second', '1000000') + lidar02 = world.spawn_actor(lidar_bp, carla.Transform()) + lidar02.listen(lambda data: sensor_callback(data, sensor_queue, "lidar02")) + sensor_list.append(lidar02) + + radar01 = world.spawn_actor(radar_bp, carla.Transform()) + radar01.listen(lambda data: sensor_callback(data, sensor_queue, "radar01")) + sensor_list.append(radar01) + + radar02 = world.spawn_actor(radar_bp, carla.Transform()) + radar02.listen(lambda data: sensor_callback(data, sensor_queue, "radar02")) + sensor_list.append(radar02) + + # Main loop + while True: + # Tick the server + world.tick() + w_frame = world.get_snapshot().frame + print("\nWorld's frame: %d" % w_frame) + + # Now, we wait to the sensors data to be received. + # As the queue is blocking, we will wait in the queue.get() methods + # until all the information is processed and we continue with the next frame. + # We include a timeout of 1.0 s (in the get method) and if some information is + # not received in this time we continue. + try: + for _ in range(len(sensor_list)): + s_frame = sensor_queue.get(True, 1.0) + print(" Frame: %d Sensor: %s" % (s_frame[0], s_frame[1])) + + except Empty: + print(" Some of the sensor information is missed") + + finally: + world.apply_settings(original_settings) + for sensor in sensor_list: + sensor.destroy() + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print(' - Exited by user.') diff --git a/carla_examples/show_recorder_actors_blocked.py b/carla_examples/show_recorder_actors_blocked.py new file mode 100644 index 0000000..06e251e --- /dev/null +++ b/carla_examples/show_recorder_actors_blocked.py @@ -0,0 +1,66 @@ +#!/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 . + +import carla + +import argparse + + +def main(): + + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-f', '--recorder_filename', + metavar='F', + default="test1.rec", + help='recorder filename (test1.rec)') + argparser.add_argument( + '-t', '--time', + metavar='T', + default="30", + type=float, + help='minimum time to consider it is blocked') + argparser.add_argument( + '-d', '--distance', + metavar='D', + default="100", + type=float, + help='minimum distance to consider it is not moving (in cm)') + args = argparser.parse_args() + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(60.0) + + print(client.show_recorder_actors_blocked(args.recorder_filename, args.time, args.distance)) + + finally: + pass + + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/show_recorder_collisions.py b/carla_examples/show_recorder_collisions.py new file mode 100644 index 0000000..8e31044 --- /dev/null +++ b/carla_examples/show_recorder_collisions.py @@ -0,0 +1,64 @@ +#!/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 . + +import carla + +import argparse + + +def main(): + + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-f', '--recorder_filename', + metavar='F', + default="test1.rec", + help='recorder filename (test1.rec)') + argparser.add_argument( + '-t', '--types', + metavar='T', + default="aa", + help='pair of types (a=any, h=hero, v=vehicle, w=walkers, t=trafficLight, o=others') + args = argparser.parse_args() + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(60.0) + + # types pattern samples: + # -t aa == any to any == show every collision (the default) + # -t vv == vehicle to vehicle == show every collision between vehicles only + # -t vt == vehicle to traffic light == show every collision between a vehicle and a traffic light + # -t hh == hero to hero == show collision between a hero and another hero + print(client.show_recorder_collisions(args.recorder_filename, args.types[0], args.types[1])) + + finally: + pass + + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/show_recorder_file_info.py b/carla_examples/show_recorder_file_info.py new file mode 100644 index 0000000..85ff08b --- /dev/null +++ b/carla_examples/show_recorder_file_info.py @@ -0,0 +1,68 @@ +#!/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 . + +import carla + +import argparse + + +def main(): + + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-f', '--recorder_filename', + metavar='F', + default="test1.rec", + help='recorder filename (test1.rec)') + argparser.add_argument( + '-a', '--show_all', + action='store_true', + help='show detailed info about all frames content') + argparser.add_argument( + '-s', '--save_to_file', + metavar='S', + help='save result to file (specify name and extension)') + + args = argparser.parse_args() + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(60.0) + if args.save_to_file: + doc = open(args.save_to_file, "w+") + doc.write(client.show_recorder_file_info(args.recorder_filename, args.show_all)) + doc.close() + else: + print(client.show_recorder_file_info(args.recorder_filename, args.show_all)) + + + finally: + pass + + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/start_recording.py b/carla_examples/start_recording.py new file mode 100644 index 0000000..e6ad3b2 --- /dev/null +++ b/carla_examples/start_recording.py @@ -0,0 +1,141 @@ +#!/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 . + +import carla + +import argparse +import random +import time +import logging +from carla.command import SpawnActor, SetAutopilot, FutureActor, DestroyActor + +def main(): + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-n', '--number-of-vehicles', + metavar='N', + default=10, + type=int, + help='number of vehicles (default: 10)') + argparser.add_argument( + '-d', '--delay', + metavar='D', + default=2.0, + type=float, + help='delay in seconds between spawns (default: 2.0)') + argparser.add_argument( + '--safe', + action='store_true', + help='avoid spawning vehicles prone to accidents') + argparser.add_argument( + '-f', '--recorder_filename', + metavar='F', + default="test1.log", + help='recorder filename (test1.log)') + argparser.add_argument( + '-t', '--recorder_time', + metavar='T', + default=0, + type=int, + help='recorder duration (auto-stop)') + args = argparser.parse_args() + + actor_list = [] + logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + world = client.get_world() + blueprints = world.get_blueprint_library().filter('vehicle.*') + + spawn_points = world.get_map().get_spawn_points() + random.shuffle(spawn_points) + + print('found %d spawn points.' % len(spawn_points)) + + count = args.number_of_vehicles + + print("Recording on file: %s" % client.start_recorder(args.recorder_filename)) + + if args.safe: + blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] + blueprints = [x for x in blueprints if not x.id.endswith('microlino')] + blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] + blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] + blueprints = [x for x in blueprints if not x.id.endswith('t2')] + blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] + blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] + blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] + + spawn_points = world.get_map().get_spawn_points() + number_of_spawn_points = len(spawn_points) + + if count < number_of_spawn_points: + random.shuffle(spawn_points) + elif count > number_of_spawn_points: + msg = 'requested %d vehicles, but could only find %d spawn points' + logging.warning(msg, count, number_of_spawn_points) + count = number_of_spawn_points + + batch = [] + for n, transform in enumerate(spawn_points): + if n >= count: + break + blueprint = random.choice(blueprints) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + blueprint.set_attribute('role_name', 'autopilot') + batch.append(SpawnActor(blueprint, transform).then(SetAutopilot(FutureActor, True))) + + for response in client.apply_batch_sync(batch): + if response.error: + logging.error(response.error) + else: + actor_list.append(response.actor_id) + + print('spawned %d vehicles, press Ctrl+C to exit.' % len(actor_list)) + + if (args.recorder_time > 0): + time.sleep(args.recorder_time) + else: + while True: + world.wait_for_tick() + # time.sleep(0.1) + + finally: + + print('\ndestroying %d actors' % len(actor_list)) + client.apply_batch_sync([DestroyActor(x) for x in actor_list]) + + print("Stop recording") + client.stop_recorder() + + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/start_replaying.py b/carla_examples/start_replaying.py new file mode 100644 index 0000000..4823477 --- /dev/null +++ b/carla_examples/start_replaying.py @@ -0,0 +1,109 @@ +#!/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 . + +import carla + +import argparse + + +def main(): + + argparser = argparse.ArgumentParser( + description=__doc__) + 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( + '-s', '--start', + metavar='S', + default=0.0, + type=float, + help='starting time (default: 0.0)') + argparser.add_argument( + '-d', '--duration', + metavar='D', + default=0.0, + type=float, + help='duration (default: 0.0)') + argparser.add_argument( + '-f', '--recorder-filename', + metavar='F', + default="test1.log", + help='recorder filename (test1.log)') + argparser.add_argument( + '-c', '--camera', + metavar='C', + default=0, + type=int, + help='camera follows an actor (ex: 82)') + argparser.add_argument( + '-x', '--time-factor', + metavar='X', + default=1.0, + type=float, + help='time factor (default 1.0)') + argparser.add_argument( + '-i', '--ignore-hero', + action='store_true', + help='ignore hero vehicles') + argparser.add_argument( + '--move-spectator', + action='store_true', + help='move spectator camera') + argparser.add_argument( + '--top-view', + action='store_true', + help='enable top-down camera view when following an actor') + argparser.add_argument( + '--spawn-sensors', + action='store_true', + help='spawn sensors in the replayed world') + args = argparser.parse_args() + + try: + + client = carla.Client(args.host, args.port) + client.set_timeout(60.0) + + # set the time factor for the replayer + client.set_replayer_time_factor(args.time_factor) + + # set to ignore the hero vehicles or not + client.set_replayer_ignore_hero(args.ignore_hero) + + # set to ignore the spectator camera or not + client.set_replayer_ignore_spectator(not args.move_spectator) + + # set desired offset + offset = carla.Transform(carla.Location(-10, 0, 5), carla.Rotation(-25, 0, 0)) + if args.top_view: + offset = carla.Transform(carla.Location(0, 0, 40), carla.Rotation(-90, 0, 0)) + + # replay the session + print(client.replay_file(args.recorder_filename, args.start, args.duration, args.camera, args.spawn_sensors, offset)) + + finally: + pass + + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/carla_examples/synchronous_mode.py b/carla_examples/synchronous_mode.py new file mode 100644 index 0000000..0b72a46 --- /dev/null +++ b/carla_examples/synchronous_mode.py @@ -0,0 +1,195 @@ +#!/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 . + +import carla + +import random + +try: + import pygame +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') + +try: + import queue +except ImportError: + import Queue as queue + + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick() + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False): + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + display = pygame.display.set_mode( + (800, 600), + pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + start_pose = random.choice(m.get_spawn_points()) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + vehicle = world.spawn_actor( + random.choice(blueprint_library.filter('vehicle.*')), + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(False) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: + while True: + if should_quit(): + return + clock.tick() + + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=2.0) + + # Choose the next waypoint and update the car location. + waypoint = random.choice(waypoint.next(1.5)) + vehicle.set_transform(waypoint.transform) + + image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + draw_image(display, image_semseg, blend=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + pygame.display.flip() + + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') diff --git a/carla_examples/test_addsecondvx.py b/carla_examples/test_addsecondvx.py new file mode 100644 index 0000000..b1b8e22 --- /dev/null +++ b/carla_examples/test_addsecondvx.py @@ -0,0 +1,83 @@ +import carla +import random +import weakref + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + +class V2XSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + world = self._parent.get_world() + #bp = world.get_blueprint_library().find('sensor.other.v2x_custom') + bp = world.get_blueprint_library().find('sensor.other.v2x') + 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: V2XSensor._V2X_callback(weak_self, sensor_data)) + + def destroy(self): + self.sensor.stop() + self.sensor.destroy() + + @staticmethod + def _V2X_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + for data in sensor_data: + msg = data.get() + # stationId = msg["Header"]["Station ID"] + power = data.power + print(msg) + # print('Cam message received from %s ' % stationId) + print('Cam message received with power %f ' % power) + +client = carla.Client("localhost",2000) +client.set_timeout(2000.0) + +world = client.get_world() +smap = world.get_map() +# acl = world.get_actor(28) +# acl.send("test") + + +spawn_points = smap.get_spawn_points() +spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() +blueprint = random.choice(get_actor_blueprints(world, "vehicle.*", "2")) +blueprint.set_attribute('role_name', "test") +player = world.try_spawn_actor(blueprint, spawn_point) +v2x_sensor = V2XSensor(player) + +world.wait_for_tick() +try: + while True: + world.wait_for_tick() +finally: + v2x_sensor.destroy() + player.destroy() \ No newline at end of file diff --git a/carla_examples/tutorial.py b/carla_examples/tutorial.py new file mode 100644 index 0000000..1c6f6d4 --- /dev/null +++ b/carla_examples/tutorial.py @@ -0,0 +1,116 @@ +#!/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 . + +import carla + +import random +import time + + +def main(): + actor_list = [] + + # In this tutorial script, we are going to add a vehicle to the simulation + # and let it drive in autopilot. We will also create a camera attached to + # that vehicle, and save all the images generated by the camera to disk. + + try: + # 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. + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + + # Once we have a client we can retrieve the world that is currently + # running. + world = client.get_world() + + # The world contains the list blueprints that we can use for adding new + # actors into the simulation. + blueprint_library = world.get_blueprint_library() + + # Now let's filter all the blueprints of type 'vehicle' and choose one + # at random. + bp = random.choice(blueprint_library.filter('vehicle')) + + # A blueprint contains the list of attributes that define a vehicle's + # instance, we can read them and modify some of them. For instance, + # let's randomize its color. + if bp.has_attribute('color'): + color = random.choice(bp.get_attribute('color').recommended_values) + bp.set_attribute('color', color) + + # Now we need to give an initial transform to the vehicle. We choose a + # random transform from the list of recommended spawn points of the map. + transform = random.choice(world.get_map().get_spawn_points()) + + # So let's tell the world to spawn the vehicle. + vehicle = world.spawn_actor(bp, transform) + + # It is important to note that the actors we create won't be destroyed + # unless we call their "destroy" function. If we fail to call "destroy" + # they will stay in the simulation even after we quit the Python script. + # For that reason, we are storing all the actors we create so we can + # destroy them afterwards. + actor_list.append(vehicle) + print('created %s' % vehicle.type_id) + + # Let's put the vehicle to drive around. + vehicle.set_autopilot(True) + + # Let's add now a "depth" camera attached to the vehicle. Note that the + # transform we give here is now relative to the vehicle. + camera_bp = blueprint_library.find('sensor.camera.depth') + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + actor_list.append(camera) + print('created %s' % camera.type_id) + + # Now we register the function that will be called each time the sensor + # receives an image. In this example we are saving the image to disk + # converting the pixels to gray-scale. + cc = carla.ColorConverter.LogarithmicDepth + camera.listen(lambda image: image.save_to_disk('_out/%06d.png' % image.frame, cc)) + + # Oh wait, I don't like the location we gave to the vehicle, I'm going + # to move it a bit forward. + location = vehicle.get_location() + location.x += 40 + vehicle.set_location(location) + print('moved vehicle to %s' % location) + + # But the city now is probably quite empty, let's add a few more + # vehicles. + transform.location += carla.Location(x=40, y=-3.2) + transform.rotation.yaw = -180.0 + for _ in range(0, 10): + transform.location.x += 8.0 + + bp = random.choice(blueprint_library.filter('vehicle')) + + # This time we are using try_spawn_actor. If the spot is already + # occupied by another object, the function will return None. + npc = world.try_spawn_actor(bp, transform) + if npc is not None: + actor_list.append(npc) + npc.set_autopilot(True) + print('created %s' % npc.type_id) + + time.sleep(5) + + finally: + + print('destroying actors') + camera.destroy() + client.apply_batch([carla.command.DestroyActor(x) for x in actor_list]) + print('done.') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/tutorial_gbuffer.py b/carla_examples/tutorial_gbuffer.py new file mode 100644 index 0000000..b7dac06 --- /dev/null +++ b/carla_examples/tutorial_gbuffer.py @@ -0,0 +1,116 @@ +#!/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 . + +import carla + +import random +import time + + + +def main(): + actor_list = [] + + # In this tutorial script, we are going to add a vehicle to the simulation + # and let it drive in autopilot. We will also create a camera attached to + # that vehicle, and save all the images generated by the camera to disk. + # Additionally, we will save all of the gbuffer textures for each frame. + + try: + # 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. + client = carla.Client('127.0.0.1', 2000) + client.set_timeout(2.0) + + # Once we have a client we can retrieve the world that is currently + # running. + world = client.get_world() + + # The world contains the list blueprints that we can use for adding new + # actors into the simulation. + blueprint_library = world.get_blueprint_library() + + # Now let's filter all the blueprints of type 'vehicle' and choose one + # at random. + bp = random.choice(blueprint_library.filter('vehicle')) + + # A blueprint contains the list of attributes that define a vehicle's + # instance, we can read them and modify some of them. For instance, + # let's randomize its color. + if bp.has_attribute('color'): + color = random.choice(bp.get_attribute('color').recommended_values) + bp.set_attribute('color', color) + + # Now we need to give an initial transform to the vehicle. We choose a + # random transform from the list of recommended spawn points of the map. + transform = world.get_map().get_spawn_points()[0] + + # So let's tell the world to spawn the vehicle. + vehicle = world.spawn_actor(bp, transform) + + # It is important to note that the actors we create won't be destroyed + # unless we call their "destroy" function. If we fail to call "destroy" + # they will stay in the simulation even after we quit the Python script. + # For that reason, we are storing all the actors we create so we can + # destroy them afterwards. + actor_list.append(vehicle) + print('created %s' % vehicle.type_id) + + # Let's put the vehicle to drive around. + vehicle.set_autopilot(True) + + # Let's add now a "rgb" camera attached to the vehicle. Note that the + # transform we give here is now relative to the vehicle. + camera_bp = blueprint_library.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '1920') + camera_bp.set_attribute('image_size_y', '1080') + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + actor_list.append(camera) + print('created %s' % camera.type_id) + + # Register a callback for whenever a new frame is available. This step is + # currently required to correctly receive the gbuffer textures, as it is + # used to determine whether the sensor is active. + camera.listen(lambda image: image.save_to_disk('_out/FinalColor-%06d.png' % image.frame)) + + # Here we will register the callbacks for each gbuffer texture. + # The function "listen_to_gbuffer" behaves like the regular listen function, + # but you must first pass it the ID of the desired gbuffer texture. + camera.enable_gbuffers(True) + camera.listen_to_gbuffer(carla.GBufferTextureID.SceneColor, lambda image: image.save_to_disk('_out/GBuffer-SceneColor-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.SceneDepth, lambda image: image.save_to_disk('_out/GBuffer-SceneDepth-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.SceneStencil, lambda image: image.save_to_disk('_out/GBuffer-SceneStencil-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.GBufferA, lambda image: image.save_to_disk('_out/GBuffer-A-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.GBufferB, lambda image: image.save_to_disk('_out/GBuffer-B-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.GBufferC, lambda image: image.save_to_disk('_out/GBuffer-C-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.GBufferD, lambda image: image.save_to_disk('_out/GBuffer-D-%06d.png' % image.frame)) + # Note that some gbuffer textures may not be available for a particular scene. + # For example, the textures E and F are likely unavailable in this example, + # which will result in them being sent as black images. + camera.listen_to_gbuffer(carla.GBufferTextureID.GBufferE, lambda image: image.save_to_disk('_out/GBuffer-E-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.GBufferF, lambda image: image.save_to_disk('_out/GBuffer-F-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.Velocity, lambda image: image.save_to_disk('_out/GBuffer-Velocity-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.SSAO, lambda image: image.save_to_disk('_out/GBuffer-SSAO-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.CustomDepth, lambda image: image.save_to_disk('_out/GBuffer-CustomDepth-%06d.png' % image.frame)) + camera.listen_to_gbuffer(carla.GBufferTextureID.CustomStencil, lambda image: image.save_to_disk('_out/GBuffer-CustomStencil-%06d.png' % image.frame)) + + time.sleep(10) + + finally: + + print('destroying actors') + camera.destroy() + client.apply_batch([carla.command.DestroyActor(x) for x in actor_list]) + print('done.') + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/vehicle_gallery.py b/carla_examples/vehicle_gallery.py new file mode 100644 index 0000000..c739664 --- /dev/null +++ b/carla_examples/vehicle_gallery.py @@ -0,0 +1,51 @@ +#!/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 . + +import carla + +import math +import random + + +def get_transform(vehicle_location, angle, d=6.4): + a = math.radians(angle) + location = carla.Location(d * math.cos(a), d * math.sin(a), 2.0) + vehicle_location + return carla.Transform(location, carla.Rotation(yaw=180 + angle, pitch=-15)) + + +def main(): + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + world = client.get_world() + spectator = world.get_spectator() + vehicle_blueprints = world.get_blueprint_library().filter('vehicle') + + location = random.choice(world.get_map().get_spawn_points()).location + + for blueprint in vehicle_blueprints: + transform = carla.Transform(location, carla.Rotation(yaw=-45.0)) + vehicle = world.spawn_actor(blueprint, transform) + + try: + + print(vehicle.type_id) + + angle = 0 + while angle < 356: + timestamp = world.wait_for_tick().timestamp + angle += timestamp.delta_seconds * 60.0 + spectator.set_transform(get_transform(vehicle.get_location(), angle - 90)) + + finally: + + vehicle.destroy() + + +if __name__ == '__main__': + + main() diff --git a/carla_examples/vehicle_physics.py b/carla_examples/vehicle_physics.py new file mode 100644 index 0000000..1ea47b2 --- /dev/null +++ b/carla_examples/vehicle_physics.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 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 . + +""" +Vehicle physics example for CARLA +Small example that shows the effect of different impulse and force application +methods to a vehicle. +""" + +import argparse + +import carla + + +def print_step_info(world, vehicle): + snapshot = world.get_snapshot() + print("%d %06.03f %+8.03f %+8.03f %+8.03f %+8.03f %+8.03f %+8.03f %+8.03f %+8.03f %+8.03f" % + (snapshot.frame, snapshot.timestamp.elapsed_seconds, \ + vehicle.get_acceleration().x, vehicle.get_acceleration().y, vehicle.get_acceleration().z, \ + vehicle.get_velocity().x, vehicle.get_velocity().y, vehicle.get_velocity().z, \ + vehicle.get_location().x, vehicle.get_location().y, vehicle.get_location().z)) + + +def wait(world, frames=100): + for i in range(0, frames): + world.tick() + + +def main(arg): + """Main function of the script""" + client = carla.Client(arg.host, arg.port) + client.set_timeout(2.0) + world = client.get_world() + + try: + # Setting the world and the spawn properties + original_settings = world.get_settings() + settings = world.get_settings() + + delta = 0.1 + settings.fixed_delta_seconds = delta + settings.synchronous_mode = True + world.apply_settings(settings) + + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.filter(arg.filter)[0] + + vehicle_transform = world.get_map().get_spawn_points()[0] + vehicle_transform.location.z += 3 + vehicle = world.spawn_actor(vehicle_bp, vehicle_transform) + + physics_vehicle = vehicle.get_physics_control() + car_mass = physics_vehicle.mass + + spectator_transform = carla.Transform(vehicle_transform.location, vehicle_transform.rotation) + spectator_transform.location += vehicle_transform.get_forward_vector() * 20 + spectator_transform.rotation.yaw += 180 + spectator = world.get_spectator() + spectator.set_transform(spectator_transform) + + + # We let the vehicle stabilize and save the transform to reset it after each test. + wait(world) + vehicle.set_target_velocity(carla.Vector3D(0, 0, 0)) + vehicle_transform = vehicle.get_transform() + wait(world) + + + # Impulse/Force at the center of mass of the object + impulse = 10 * car_mass + + print("# Adding an Impulse of %f N s" % impulse) + vehicle.add_impulse(carla.Vector3D(0, 0, impulse)) + + wait(world) + vehicle.set_transform(vehicle_transform) + vehicle.set_target_velocity(carla.Vector3D(0, 0, 0)) + wait(world) + + print("# Adding a Force of %f N" % (impulse / delta)) + # The add_force method should not be use for instantaneous forces like this one, + # it is more useful for constant or variable forces acting in a finite amount of time. + # In this script it is done with the proper scaling to show the equivalence + # between the add_impulse and add_force methods. + # As in this case the force is going to be applied during the whole step dt=delta + # a force more or less equivalent is impulse / delta. + vehicle.add_force(carla.Vector3D(0, 0, impulse / delta)) + + wait(world) + vehicle.set_transform(vehicle_transform) + vehicle.set_target_velocity(carla.Vector3D(0, 0, 0)) + wait(world) + + wait(world, 500) + + + finally: + world.apply_settings(original_settings) + + vehicle.destroy() + + +if __name__ == "__main__": + + argparser = argparse.ArgumentParser( + description=__doc__) + argparser.add_argument( + '--host', + metavar='H', + default='localhost', + help='IP of the host CARLA Simulator (default: localhost)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port of CARLA Simulator (default: 2000)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='model3', + help='actor filter (default: "vehicle.*")') + args = argparser.parse_args() + + try: + main(args) + except KeyboardInterrupt: + print(' - Exited by user.') diff --git a/carla_examples/visualize_multiple_sensors.py b/carla_examples/visualize_multiple_sensors.py new file mode 100644 index 0000000..5e5b573 --- /dev/null +++ b/carla_examples/visualize_multiple_sensors.py @@ -0,0 +1,375 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 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 . + +""" +Script that render multiple sensors in the same pygame window + +By default, it renders four cameras, one LiDAR and one Semantic LiDAR. +It can easily be configure for any different number of sensors. +To do that, check lines 290-308. +""" + +import carla +import argparse +import random +import time +import numpy as np + + +try: + import pygame + from pygame.locals import K_ESCAPE + from pygame.locals import K_q +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +class CustomTimer: + def __init__(self): + try: + self.timer = time.perf_counter + except AttributeError: + self.timer = time.time + + def time(self): + return self.timer() + +class DisplayManager: + def __init__(self, grid_size, window_size): + pygame.init() + pygame.font.init() + self.display = pygame.display.set_mode(window_size, pygame.HWSURFACE | pygame.DOUBLEBUF) + + self.grid_size = grid_size + self.window_size = window_size + self.sensor_list = [] + + def get_window_size(self): + return [int(self.window_size[0]), int(self.window_size[1])] + + def get_display_size(self): + return [int(self.window_size[0]/self.grid_size[1]), int(self.window_size[1]/self.grid_size[0])] + + def get_display_offset(self, gridPos): + dis_size = self.get_display_size() + return [int(gridPos[1] * dis_size[0]), int(gridPos[0] * dis_size[1])] + + def add_sensor(self, sensor): + self.sensor_list.append(sensor) + + def get_sensor_list(self): + return self.sensor_list + + def render(self): + if not self.render_enabled(): + return + + for s in self.sensor_list: + s.render() + + pygame.display.flip() + + def destroy(self): + for s in self.sensor_list: + s.destroy() + + def render_enabled(self): + return self.display != None + +class SensorManager: + def __init__(self, world, display_man, sensor_type, transform, attached, sensor_options, display_pos): + self.surface = None + self.world = world + self.display_man = display_man + self.display_pos = display_pos + self.sensor = self.init_sensor(sensor_type, transform, attached, sensor_options) + self.sensor_options = sensor_options + self.timer = CustomTimer() + + self.time_processing = 0.0 + self.tics_processing = 0 + + self.display_man.add_sensor(self) + + def init_sensor(self, sensor_type, transform, attached, sensor_options): + if sensor_type == 'RGBCamera': + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + disp_size = self.display_man.get_display_size() + camera_bp.set_attribute('image_size_x', str(disp_size[0])) + camera_bp.set_attribute('image_size_y', str(disp_size[1])) + + for key in sensor_options: + camera_bp.set_attribute(key, sensor_options[key]) + + camera = self.world.spawn_actor(camera_bp, transform, attach_to=attached) + camera.listen(self.save_rgb_image) + + return camera + + elif sensor_type == 'LiDAR': + lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast') + lidar_bp.set_attribute('range', '100') + lidar_bp.set_attribute('dropoff_general_rate', lidar_bp.get_attribute('dropoff_general_rate').recommended_values[0]) + lidar_bp.set_attribute('dropoff_intensity_limit', lidar_bp.get_attribute('dropoff_intensity_limit').recommended_values[0]) + lidar_bp.set_attribute('dropoff_zero_intensity', lidar_bp.get_attribute('dropoff_zero_intensity').recommended_values[0]) + + for key in sensor_options: + lidar_bp.set_attribute(key, sensor_options[key]) + + lidar = self.world.spawn_actor(lidar_bp, transform, attach_to=attached) + + lidar.listen(self.save_lidar_image) + + return lidar + + elif sensor_type == 'SemanticLiDAR': + lidar_bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast_semantic') + lidar_bp.set_attribute('range', '100') + + for key in sensor_options: + lidar_bp.set_attribute(key, sensor_options[key]) + + lidar = self.world.spawn_actor(lidar_bp, transform, attach_to=attached) + + lidar.listen(self.save_semanticlidar_image) + + return lidar + + elif sensor_type == "Radar": + radar_bp = self.world.get_blueprint_library().find('sensor.other.radar') + for key in sensor_options: + radar_bp.set_attribute(key, sensor_options[key]) + + radar = self.world.spawn_actor(radar_bp, transform, attach_to=attached) + radar.listen(self.save_radar_image) + + return radar + + else: + return None + + def get_sensor(self): + return self.sensor + + def save_rgb_image(self, image): + t_start = self.timer.time() + + image.convert(carla.ColorConverter.Raw) + 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] + + if self.display_man.render_enabled(): + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + t_end = self.timer.time() + self.time_processing += (t_end-t_start) + self.tics_processing += 1 + + def save_lidar_image(self, image): + t_start = self.timer.time() + + disp_size = self.display_man.get_display_size() + lidar_range = 2.0*float(self.sensor_options['range']) + + 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(disp_size) / lidar_range + lidar_data += (0.5 * disp_size[0], 0.5 * disp_size[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 = (disp_size[0], disp_size[1], 3) + lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) + + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + + if self.display_man.render_enabled(): + self.surface = pygame.surfarray.make_surface(lidar_img) + + t_end = self.timer.time() + self.time_processing += (t_end-t_start) + self.tics_processing += 1 + + def save_semanticlidar_image(self, image): + t_start = self.timer.time() + + disp_size = self.display_man.get_display_size() + lidar_range = 2.0*float(self.sensor_options['range']) + + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 6), 6)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(disp_size) / lidar_range + lidar_data += (0.5 * disp_size[0], 0.5 * disp_size[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 = (disp_size[0], disp_size[1], 3) + lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) + + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + + if self.display_man.render_enabled(): + self.surface = pygame.surfarray.make_surface(lidar_img) + + t_end = self.timer.time() + self.time_processing += (t_end-t_start) + self.tics_processing += 1 + + def save_radar_image(self, radar_data): + t_start = self.timer.time() + points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (len(radar_data), 4)) + + t_end = self.timer.time() + self.time_processing += (t_end-t_start) + self.tics_processing += 1 + + def render(self): + if self.surface is not None: + offset = self.display_man.get_display_offset(self.display_pos) + self.display_man.display.blit(self.surface, offset) + + def destroy(self): + self.sensor.destroy() + +def run_simulation(args, client): + """This function performed one test run using the args parameters + and connecting to the carla client passed. + """ + + display_manager = None + vehicle = None + vehicle_list = [] + timer = CustomTimer() + + try: + + # Getting the world and + world = client.get_world() + original_settings = world.get_settings() + + if args.sync: + traffic_manager = client.get_trafficmanager(8000) + settings = world.get_settings() + traffic_manager.set_synchronous_mode(True) + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + + # Instanciating the vehicle to which we attached the sensors + bp = world.get_blueprint_library().filter('charger_2020')[0] + vehicle = world.spawn_actor(bp, random.choice(world.get_map().get_spawn_points())) + vehicle_list.append(vehicle) + vehicle.set_autopilot(True) + + # Display Manager organize all the sensors an its display in a window + # If can easily configure the grid and the total window size + display_manager = DisplayManager(grid_size=[2, 3], window_size=[args.width, args.height]) + + # Then, SensorManager can be used to spawn RGBCamera, LiDARs and SemanticLiDARs as needed + # and assign each of them to a grid position, + SensorManager(world, display_manager, 'RGBCamera', carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=-90)), + vehicle, {}, display_pos=[0, 0]) + SensorManager(world, display_manager, 'RGBCamera', carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+00)), + vehicle, {}, display_pos=[0, 1]) + SensorManager(world, display_manager, 'RGBCamera', carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+90)), + vehicle, {}, display_pos=[0, 2]) + SensorManager(world, display_manager, 'RGBCamera', carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=180)), + vehicle, {}, display_pos=[1, 1]) + + SensorManager(world, display_manager, 'LiDAR', carla.Transform(carla.Location(x=0, z=2.4)), + vehicle, {'channels' : '64', 'range' : '100', 'points_per_second': '250000', 'rotation_frequency': '20'}, display_pos=[1, 0]) + SensorManager(world, display_manager, 'SemanticLiDAR', carla.Transform(carla.Location(x=0, z=2.4)), + vehicle, {'channels' : '64', 'range' : '100', 'points_per_second': '100000', 'rotation_frequency': '20'}, display_pos=[1, 2]) + + + #Simulation loop + call_exit = False + time_init_sim = timer.time() + while True: + # Carla Tick + if args.sync: + world.tick() + else: + world.wait_for_tick() + + # Render received data + display_manager.render() + + for event in pygame.event.get(): + if event.type == pygame.QUIT: + call_exit = True + elif event.type == pygame.KEYDOWN: + if event.key == K_ESCAPE or event.key == K_q: + call_exit = True + break + + if call_exit: + break + + finally: + if display_manager: + display_manager.destroy() + + client.apply_batch([carla.command.DestroyActor(x) for x in vehicle_list]) + + world.apply_settings(original_settings) + + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Sensor tutorial') + 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( + '--sync', + action='store_true', + help='Synchronous mode execution') + argparser.add_argument( + '--async', + dest='sync', + action='store_false', + help='Asynchronous mode execution') + argparser.set_defaults(sync=True) + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(5.0) + + run_simulation(args, client) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/intel/old_implement.md b/intel/old_implement.md new file mode 100644 index 0000000..360d0bc --- /dev/null +++ b/intel/old_implement.md @@ -0,0 +1,472 @@ +# Shenron-Style Radar Integration Plan (CARLA → Foxglove Pipeline) + +## Objective + +Implement a **C-Shenron-inspired radar synthesis module** within the existing CARLA ADAS pipeline to generate a **high-fidelity radar point cloud and heatmap**, while **retaining CARLA’s native radar** for benchmarking and calibration. + +--- + +# 1. Why Shenron Radar (Core Improvements Over CARLA Radar) + +## Limitations of CARLA Radar + +* Sparse ray-cast sampling (low spatial density) +* No Radar Cross Section (RCS) modeling +* No intensity / energy modeling +* No angular resolution control +* No beam pattern or antenna simulation +* No realistic clustering or spread +* Outputs discrete points → not field-based representation + +--- + +## Shenron Key Ideas (What Makes It Better) + +These are the **core principles you must implement**: + +### 1. Dense Geometry-Based Sampling + +* Uses **LiDAR / depth data** instead of sparse rays +* Captures full object surfaces → better shape fidelity + +--- + +### 2. Radar Cross Section (RCS) Approximation + +* Assign reflectivity based on object type +* Vehicles ≫ pedestrians ≫ static clutter + +--- + +### 3. Energy-Based Modeling (Not Binary Hits) + +* Each point contributes **intensity** +* Follows simplified radar equation: + +``` +I ∝ σ / R⁴ +``` + +--- + +### 4. Range-Azimuth Grid Representation + +* Converts scene into **radar image space** +* Aggregates energy into bins instead of discrete hits + +--- + +### 5. Angular Resolution Simulation + +* Models antenna array limitations: + +``` +Δθ ≈ 2 / N_antennas +``` + +* Applies angular blur → realistic radar spread + +--- + +### 6. Doppler Field (Not Per-Ray Velocity) + +* Computes **radial velocity per cluster/bin** +* Produces realistic motion patterns + +--- + +### 7. Multi-Sensor Fusion Capability + +* Supports multiple radar views (F, B, L, R) +* Improves situational awareness + +--- + +### 8. Field Representation Instead of Hit Detection + +* CARLA: “Ray hit object” +* Shenron: “Energy returned from direction” + +--- + +# 2. Integration Strategy (Your Pipeline) + +## Current Pipeline + +``` +CARLA → SensorManager → Recorder → Dataset → MCAP → Foxglove +``` + +--- + +## Updated Pipeline + +``` +CARLA + ↓ +SensorManager (Camera + LiDAR + Native Radar) + ↓ +RadarSynthesizer (NEW) + ↓ +Recorder + ├── /radar_carla (existing) + ├── /radar_shenron (NEW) + ↓ +MCAP → Foxglove +``` + +--- + +## Key Design Decision + +* DO NOT remove CARLA radar +* Add **parallel synthetic radar** +* Enables: + + * A/B comparison + * Parameter tuning + * Real-world calibration + +--- + +# 3. Implementation Plan (Step-by-Step) + +--- + +## STEP 1 — Create RadarSynthesizer Module + +**File:** + +``` +src/radar_synthesizer.py +``` + +**Interface:** + +```python +class RadarSynthesizer: + def __init__(self, config): + pass + + def generate(self, lidar_points, ego_state, gt_objects): + return radar_points +``` + +--- + +## STEP 2 — Input Data Sources + +From your pipeline: + +* LiDAR → `[N, 4] (x, y, z, intensity)` +* Ground Truth → object labels, velocity, bounding boxes +* Ego state → pose + velocity + +--- + +## STEP 3 — Coordinate Transformation + +Convert LiDAR points to radar space: + +``` +R = sqrt(x² + y² + z²) +θ = atan2(y, x) +φ = asin(z / R) +``` + +--- + +## STEP 4 — Object Association + +For each LiDAR point: + +* Assign it to nearest bounding box +* Extract: + + * object class + * velocity + +Output: + +``` +point → {x, y, z, object_type, velocity} +``` + +--- + +## STEP 5 — RCS Modeling + +Define: + +```python +RCS_MAP = { + "vehicle": 10.0, + "walker": 1.0, + "static": 0.3 +} +``` + +--- + +## STEP 6 — Intensity Calculation + +Apply simplified radar equation: + +``` +I = σ / R⁴ +``` + +Optional extensions: + +* Angle attenuation +* Surface normal weighting + +--- + +## STEP 7 — Range-Azimuth Grid Formation + +Discretize: + +```python +range_bins = 256 +angle_bins = 128 +``` + +Accumulate: + +```python +grid[r_bin][theta_bin] += intensity +``` + +--- + +## STEP 8 — Angular Resolution Simulation + +Define antenna count: + +```python +N_antennas = 64 +``` + +Apply blur: + +```python +sigma_theta = k / N_antennas +``` + +Use Gaussian smoothing across angle axis. + +--- + +## STEP 9 — Doppler Modeling + +Compute radial velocity: + +``` +v_r = v_rel · r_hat +``` + +Assign: + +* Per grid cell +* Or per cluster + +--- + +## STEP 10 — Thresholding & Point Cloud Generation + +Convert grid back: + +```python +if grid[r][θ] > threshold: + x = r * cos(θ) + y = r * sin(θ) + z = 0 +``` + +Attach velocity. + +--- + +## STEP 11 — Recorder Integration + +Modify `recorder.py`: + +Add new topic: + +``` +/radar_shenron +``` + +Format: + +``` +[N, 4] → x, y, z, velocity +``` + +--- + +## STEP 12 — MCAP Integration + +Update `data_to_mcap.py`: + +Add: + +| Topic | Schema | +| ---------------- | ---------- | +| `/radar_shenron` | PointCloud | + +--- + +## STEP 13 — Foxglove Visualization + +Display: + +* `/radar_carla` +* `/radar_shenron` + +Side-by-side comparison: + +* Density +* Spread +* Stability + +--- + +# 4. Development Phases + +--- + +## Phase 1 — Offline Prototype + +* Use saved LiDAR logs +* Generate radar output +* Validate visually + +--- + +## Phase 2 — Online Integration + +* Plug into SensorManager +* Generate per frame + +--- + +## Phase 3 — Calibration Mode + +* Compare: + + * CARLA radar + * Shenron radar +* Tune: + + * RCS values + * noise + * thresholds + +--- + +## Phase 4 — Real-World Matching (Future) + +* Match with real radar logs +* Fit: + + * noise distribution + * angular spread + * detection density + +--- + +# 5. Performance Considerations + +* Use NumPy vectorization +* Avoid Python loops +* Keep grid size moderate +* Consider async processing if needed + +--- + +# 6. Optional Enhancements + +--- + +## 6.1 Noise Model + +``` +range_noise ~ N(0, σ_r) +velocity_noise ~ N(0, σ_v) +``` + +--- + +## 6.2 Clutter Simulation + +* Ground reflections +* Random scatter + +--- + +## 6.3 Occlusion Modeling + +* Use LiDAR density +* Drop points behind objects + +--- + +## 6.4 Temporal Persistence + +* Retain previous detections +* Apply decay + +--- + +# 7. Key Engineering Insight + +> The biggest shift is: + +**From:** + +* Discrete ray-hit detection + +**To:** + +* Continuous energy field representation + +--- + +# 8. Expected Outcomes + +Compared to CARLA radar: + +| Feature | CARLA | Shenron-style | +| --------------- | ------- | ------------- | +| Density | Low | High | +| Clustering | Poor | Realistic | +| Angular spread | None | Modeled | +| Physics realism | Low | Medium | +| ML usefulness | Limited | High | + +--- + +# 9. Final Recommendation + +Start with a **minimal MVP**: + +1. LiDAR → spherical +2. Add RCS scaling +3. Bin into grid +4. Convert to point cloud + +Then iterate. + +--- + +# 10. Next Step + +* Implement `radar_synthesizer.py` +* Validate offline +* Integrate into pipeline + +--- + +**End of Document** diff --git a/intel/shenron_implementation_guide.md b/intel/shenron_implementation_guide.md new file mode 100644 index 0000000..6c6cdda --- /dev/null +++ b/intel/shenron_implementation_guide.md @@ -0,0 +1,123 @@ +# Shenron Radar Integration Guide (Fox Pipeline) + +This guide details the "Model-Based Development" (MBD) integration of the physics-based Shenron radar simulation into the Fox CARLA ADAS pipeline. + +--- + +## 1. Architecture Overview (MBD Standard) + +In this approach, the Shenron simulator is treated as a **strictly isolated model block** (black box). Your main pipeline (recording/visualization) never touches the internal math or physics files. Instead, it interacts with a single **Shenron Model API**. + +**The MBD Interface:** +- **Model Location**: `ISOLATE/` +- **Public API**: `ShenronRadarModel` (class) +- **Inputs**: Canonical Semantic LiDAR array `[N, 7]`. +- **Outputs**: Radar Point Cloud `[N, 4]`. + +--- + +## 2. File System Setup (MBD Style) + +## 2. File System Setup + +To isolate the logic, we use the `ISOLATE/` directory. Copy this entire folder into the root of your `Fox/` repository. + +**Required Directory Structure:** +```text +Fox/ +├── ISOLATE/ <-- The Shenron Core +│ ├── e2e_agent_sem_lidar2shenron_package/ +│ │ ├── shenron/ (Sceneset.py, heatmap_gen_fast.py) +│ │ ├── ConfigureRadar.py (Hardware settings) +│ │ └── lidar.py (Main conversion logic) +│ └── sim_radar_utils/ (FFT and BEV Processing) +├── src/ +│ └── sensors.py (Update to Semantic LiDAR) +├── scripts/ +│ ├── generate_shenron.py <-- NEW: Batch processor +│ └── data_to_mcap.py (Update to include Shenron topic) +└── data/ (Target dataset) +``` + +--- + +## 3. Step-by-Step Implementation + +### Step 1: Update LiDAR Sensor +In `src/sensors.py`, you must change the LiDAR blueprint to the **semantic** version. Shenron requires semantic tags (Road, Vehicle, Building) to apply radar reflection coefficients via Fresnel equations. + +**File:** [`src/sensors.py`](file:///d:/Work/Repo/C-Shenron/src/sensors.py) +```python +# Change this: +# 'type': 'sensor.lidar.ray_cast' +# To this: +'type': 'sensor.lidar.ray_cast_semantic' +``` + +### Step 2: Implement the Batch Processor +Create `scripts/generate_shenron.py`. This script acts as a wrapper for the `ISOLATE` package. It iterates through your `data/` directory, looks for `lidar/` folders, and generates a corresponding `shenron_radar/` folder. + +> [!TIP] +> Use the **`heatmap_gen_fast.py`** logic within the processor to leverage CUDA/GPU acceleration for the FMCW signal generation. + +### Step 3: Update the MCAP Writer +Modify `scripts/data_to_mcap.py` to recognize the new `shenron_radar/` directory. + +**File:** [`scripts/data_to_mcap.py`](file:///d:/Work/Repo/C-Shenron/scripts/data_to_mcap.py) +```python +# Add a check for the shenron folder +shenron_path = session_dir / "shenron_radar" +if shenron_path.exists(): + # Write to Foxglove topic: "/radar/shenron" +``` + +--- + +## 4. Operational Workflow + +To generate a dataset with high-accuracy radar, follow this sequence: + +1. **Run Simulation**: + ```powershell + run.bat braking --frames 200 + ``` +2. **Generate Shenron Radar** (Post-Simulation): + ```powershell + python scripts/generate_shenron.py + ``` +3. **Convert to MCAP**: + ```powershell + python scripts/data_to_mcap.py + ``` + +--- + +## 5. Troubleshooting & Dependencies + +### Hardware Requirements +- **GPU**: NVIDIA T1000 with 8GB VRAM is sufficient. The model uses PyTorch tensors to parallelize the FMCW chirp calculations across the GPU cores. +- **CPU/RAM**: Standard multi-core CPU; 16GB+ RAM recommended for handling large point cloud buffers. + +### Installation +Execute these commands in your `carla312` environment: +```powershell +pip install torch +pip install open3d +``` + +### Data Interface Adaptation +The most critical "surgical" modification occurs in **`ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py`**. + +Your CARLA 0.9.16 Semantic LiDAR data typically contains 7 columns: +`[x, y, z, intensity, cos_inc_angle, object_idx, semantic_tag]` + +The `map_carla_semantic_lidar_latest` function in `lidar.py` must be updated to index your specific columns: +```python +def map_carla_semantic_lidar_latest(carla_sem_lidar_data): + # Ensure indices match your [N, 7] array + # e.g., if semantic_tag is index 6: + carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 6)] + ... +``` + +The script `scripts/generate_shenron.py` will handle loading your `.npy` files and feeding them into this function. diff --git a/intel/shenron_integration.md b/intel/shenron_integration.md new file mode 100644 index 0000000..a980705 --- /dev/null +++ b/intel/shenron_integration.md @@ -0,0 +1,64 @@ +# [Plan] Shenron Model-Based Development (MBD) Integration + +This plan shifts the integration to a **Model-Based Development** architecture. We will treat the isolated Shenron logic as a "strictly defined black box" (similar to a Simulink model) and enhance it to output "Rich" radar data. + +## User Review Required + +> [!IMPORTANT] +> **Rich Point Cloud**: We will modify the extraction logic to output more than just geometry. The new format will be: `[x, y, z, velocity, SNR, RCS_estimate]`. +> +> **Source of Data**: +> - **SNR**: Extracted from the peak magnitude in the Range-Doppler heatmaps. +> - **RCS**: Back-calculated from the return power logic in `Sceneset`. +> - **Noise**: Sampled from the simulated noise floor in `ConfigureRadar`. + +--- + +## Phase 1: Environment & Dependency Setup +... (Previously defined) + +--- + +## Phase 2: The "Black Box" Wrapper & Rich Extraction + +### [MODIFY] `ISOLATE/sim_radar_utils/radar_processor.py` +Modify `convert_to_pcd()` to preserve the magnitude of the detected peaks. +- **Current**: Returns `[x, y]`. +- **New**: Returns `[x, y, z, velocity, magnitude]`. + +### [NEW] `ISOLATE/model_wrapper.py` +The wrapper will now handle the "Rich" data synthesis: +1. Call `Sceneset` to get the ground-truth physical power (RCS). +2. Call `HeatmapGen` to generate the noisy ADC stream. +3. Call `RadarProcessor` to extract detected peaks. +4. Merge the detected peaks with the underlying RCS data to provide a complete "Semantic Radar" point cloud. + +--- + +## Phase 3: Modular Pipeline Integration +... (Previously defined) + +--- + +## Verification Plan + +### Automated Verification +1. **API Integrity**: Run a script that initializes the `ShenronRadarModel` and passes a dummy [N, 7] array to ensure it executes and returns the correct [N, 4] shape. +2. **Batch Run**: Execute `generate_shenron.py` on an existing dataset. + +### Manual Verification +1. Compare the MCAP output with vs. without the Shenron layer to ensure topic alignment. + +--- + +## Verification Plan + +### Automated Verification +1. **Pilot Run**: Record a 100-frame scenario with semantic LiDAR. +2. **Post-Process**: Run `python scripts/generate_shenron.py`. +3. **Check Output**: Verify `data//shenron_radar/` contains 100 `.npy` files. + +### Manual Verification +1. Run `python scripts/data_to_mcap.py`. +2. Open the MCAP in Foxglove and verify that the `/radar/shenron` topic perfectly overlays with the `/lidar` data but shows radar-typical noise and sparsity. +