1420 lines
61 KiB
Python
1420 lines
61 KiB
Python
#!/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 <https://opensource.org/licenses/MIT>.
|
|
|
|
# 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
|
|
|
|
|
|
# ==============================================================================
|
|
# -- find carla module ---------------------------------------------------------
|
|
# ==============================================================================
|
|
|
|
|
|
import glob
|
|
import os
|
|
import sys
|
|
|
|
try:
|
|
sys.path.append(glob.glob('../carla/dist/carla-0.9.15-py*%d.%d-%s.egg' % (
|
|
sys.version_info.major,
|
|
sys.version_info.minor,
|
|
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
|
|
except IndexError:
|
|
pass
|
|
|
|
|
|
# ==============================================================================
|
|
# -- imports -------------------------------------------------------------------
|
|
# ==============================================================================
|
|
|
|
|
|
import carla
|
|
|
|
from carla import ColorConverter as cc
|
|
|
|
import argparse
|
|
import collections
|
|
import datetime
|
|
import logging
|
|
import math
|
|
import random
|
|
import re
|
|
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.rec")
|
|
world.recording_enabled = True
|
|
world.hud.notification("Recorder is ON")
|
|
elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL):
|
|
# stop recorder
|
|
client.stop_recorder()
|
|
world.recording_enabled = False
|
|
# work around to fix camera at start of replaying
|
|
current_index = world.camera_manager.index
|
|
world.destroy_sensors()
|
|
# disable autopilot
|
|
self._autopilot_enabled = False
|
|
world.player.set_autopilot(self._autopilot_enabled)
|
|
world.hud.notification("Replaying file 'manual_recording.rec'")
|
|
# replayer
|
|
client.replay_file("manual_recording.rec", world.recording_start, 0, 0)
|
|
world.camera_manager.set_sensor(current_index)
|
|
elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL):
|
|
if pygame.key.get_mods() & KMOD_SHIFT:
|
|
world.recording_start -= 10
|
|
else:
|
|
world.recording_start -= 1
|
|
world.hud.notification("Recording start time is %d" % (world.recording_start))
|
|
elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL):
|
|
if pygame.key.get_mods() & KMOD_SHIFT:
|
|
world.recording_start += 10
|
|
else:
|
|
world.recording_start += 1
|
|
world.hud.notification("Recording start time is %d" % (world.recording_start))
|
|
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()
|