updated automatic_control to make it compatible with the manual control code
This commit is contained in:
parent
90fdb53a38
commit
4afcbf9e63
|
@ -151,7 +151,7 @@ class RoamingAgent(object):
|
|||
for traffic_light in lights_list:
|
||||
loc = traffic_light.get_location()
|
||||
magnitude, angle = compute_magnitude_angle(loc, ego_vehicle_location, self._vehicle.get_transform().rotation.yaw)
|
||||
if magnitude < 80.0 and angle < 25.0 and angle < min_angle:
|
||||
if magnitude < 80.0 and angle < min(25.0, min_angle):
|
||||
sel_magnitude = magnitude
|
||||
sel_traffic_light = traffic_light
|
||||
min_angle = angle
|
||||
|
|
|
@ -13,16 +13,49 @@
|
|||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import collections
|
||||
import datetime
|
||||
import glob
|
||||
import logging
|
||||
import math
|
||||
import os
|
||||
import random
|
||||
import re
|
||||
import sys
|
||||
import time
|
||||
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_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')
|
||||
raise RuntimeError('cannot import pygame, make sure pygame package is installed')
|
||||
|
||||
try:
|
||||
import numpy as np
|
||||
|
@ -30,62 +63,90 @@ except ImportError:
|
|||
raise RuntimeError(
|
||||
'cannot import numpy, make sure numpy package is installed')
|
||||
|
||||
# ==============================================================================
|
||||
# -- find carla module ---------------------------------------------------------
|
||||
# ==============================================================================
|
||||
try:
|
||||
sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % (
|
||||
sys.version_info.major,
|
||||
sys.version_info.minor,
|
||||
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
|
||||
except IndexError:
|
||||
pass
|
||||
|
||||
import carla
|
||||
from carla import ColorConverter as cc
|
||||
from agents.navigation.roaming_agent import *
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
# -- 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 ---------------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
|
||||
def 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]
|
||||
|
||||
|
||||
class World(object):
|
||||
def __init__(self, carla_world, hud):
|
||||
self.world = carla_world
|
||||
self.hud = hud
|
||||
blueprints = self.world.get_blueprint_library().filter('vehicle')
|
||||
blueprint = [e for i, e in enumerate(blueprints) if e.id == 'vehicle.lincoln.mkz2017'][0]
|
||||
|
||||
spawn_points = self.world.get_map().get_spawn_points()
|
||||
# random.choice(spawn_points) if spawn_points else carla.Transform()
|
||||
spawn_point = spawn_points[1]
|
||||
self.vehicle = self.world.spawn_actor(blueprint, spawn_point)
|
||||
self.collision_sensor = CollisionSensor(self.vehicle, self.hud)
|
||||
self.camera_manager = CameraManager(self.vehicle, self.hud)
|
||||
self.camera_manager.set_sensor(0, notify=False)
|
||||
self.controller = None
|
||||
self.vehicle = None
|
||||
self.collision_sensor = None
|
||||
self.lane_invasion_sensor = None
|
||||
self.camera_manager = None
|
||||
self._weather_presets = find_weather_presets()
|
||||
self._weather_index = 0
|
||||
self.restart()
|
||||
self.world.on_tick(hud.on_world_tick)
|
||||
|
||||
def restart(self):
|
||||
cam_index = self.camera_manager._index
|
||||
cam_pos_index = self.camera_manager._transform_index
|
||||
start_pose = self.vehicle.get_transform()
|
||||
start_pose.location.z += 2.0
|
||||
start_pose.rotation.roll = 0.0
|
||||
start_pose.rotation.pitch = 0.0
|
||||
blueprint = self._get_random_blueprint()
|
||||
self.destroy()
|
||||
self.vehicle = self.world.spawn_actor(blueprint, start_pose)
|
||||
# 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
|
||||
|
||||
blueprint = self.world.get_blueprint_library().find('vehicle.lincoln.mkz2017')
|
||||
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 vehicle.
|
||||
if self.vehicle is not None:
|
||||
spawn_point = self.vehicle.get_transform()
|
||||
spawn_point.location.z += 2.0
|
||||
spawn_point.rotation.roll = 0.0
|
||||
spawn_point.rotation.pitch = 0.0
|
||||
self.destroy()
|
||||
|
||||
spawn_points = self.world.get_map().get_spawn_points()
|
||||
spawn_point = spawn_points[1]
|
||||
self.vehicle = self.world.spawn_actor(blueprint, spawn_point)
|
||||
|
||||
while self.vehicle is None:
|
||||
spawn_points = self.world.get_map().get_spawn_points()
|
||||
spawn_point = spawn_points[1]
|
||||
self.vehicle = self.world.spawn_actor(blueprint, spawn_point)
|
||||
|
||||
# Set up the sensors.
|
||||
self.collision_sensor = CollisionSensor(self.vehicle, self.hud)
|
||||
self.lane_invasion_sensor = LaneInvasionSensor(self.vehicle, self.hud)
|
||||
self.camera_manager = CameraManager(self.vehicle, self.hud)
|
||||
self.camera_manager._transform_index = cam_pos_index
|
||||
self.camera_manager.set_sensor(cam_index, notify=False)
|
||||
actor_type = ' '.join(
|
||||
self.vehicle.type_id.replace(
|
||||
'_', '.').title().split('.')[
|
||||
1:])
|
||||
actor_type = get_actor_display_name(self.vehicle)
|
||||
self.hud.notification(actor_type)
|
||||
|
||||
def next_weather(self, reverse=False):
|
||||
|
@ -103,17 +164,89 @@ class World(object):
|
|||
self.hud.render(display)
|
||||
|
||||
def destroy(self):
|
||||
for actor in [self.camera_manager.sensor,
|
||||
self.collision_sensor.sensor, self.vehicle]:
|
||||
actors = [
|
||||
self.camera_manager.sensor,
|
||||
self.collision_sensor.sensor,
|
||||
self.lane_invasion_sensor.sensor,
|
||||
self.vehicle]
|
||||
for actor in actors:
|
||||
if actor is not None:
|
||||
actor.destroy()
|
||||
|
||||
def _get_random_blueprint(self):
|
||||
bp = random.choice(self.world.get_blueprint_library().filter('tesla'))
|
||||
if bp.has_attribute('color'):
|
||||
color = random.choice(bp.get_attribute('color').recommended_values)
|
||||
bp.set_attribute('color', color)
|
||||
return bp
|
||||
|
||||
# ==============================================================================
|
||||
# -- KeyboardControl -----------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
class KeyboardControl(object):
|
||||
def __init__(self, world, start_in_autopilot):
|
||||
self._autopilot_enabled = start_in_autopilot
|
||||
self._control = carla.VehicleControl()
|
||||
self._steer_cache = 0.0
|
||||
world.vehicle.set_autopilot(self._autopilot_enabled)
|
||||
world.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
|
||||
|
||||
def parse_events(self, world, clock):
|
||||
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:
|
||||
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()
|
||||
elif event.key == K_q:
|
||||
self._control.gear = 1 if self._control.reverse else -1
|
||||
elif event.key == K_m:
|
||||
self._control.manual_gear_shift = not self._control.manual_gear_shift
|
||||
self._control.gear = world.vehicle.get_vehicle_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.vehicle.set_autopilot(self._autopilot_enabled)
|
||||
world.hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off'))
|
||||
if not self._autopilot_enabled:
|
||||
self._parse_keys(pygame.key.get_pressed(), clock.get_time())
|
||||
self._control.reverse = self._control.gear < 0
|
||||
|
||||
def _parse_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]
|
||||
|
||||
@staticmethod
|
||||
def _is_quit_shortcut(key):
|
||||
return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
|
@ -125,16 +258,80 @@ class HUD(object):
|
|||
def __init__(self, width, height):
|
||||
self.dim = (width, height)
|
||||
font = pygame.font.Font(pygame.font.get_default_font(), 20)
|
||||
# hope for the best...
|
||||
mono = next(x for x in pygame.font.get_fonts() if 'mono' in x)
|
||||
mono = pygame.font.match_font(mono, bold=True)
|
||||
fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
|
||||
default_font = 'ubuntumono'
|
||||
mono = default_font if default_font in fonts else fonts[0]
|
||||
mono = pygame.font.match_font(mono)
|
||||
self._font_mono = pygame.font.Font(mono, 14)
|
||||
self._notifications = FadingText(font, (width, 40), (0, height - 40))
|
||||
self.help = HelpText(pygame.font.Font(mono, 24), width, height)
|
||||
self.server_fps = 0
|
||||
self.frame_number = 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_number = timestamp.frame_count
|
||||
self.simulation_time = timestamp.elapsed_seconds
|
||||
|
||||
def tick(self, world, clock):
|
||||
if not self._show_info:
|
||||
return
|
||||
t = world.vehicle.get_transform()
|
||||
v = world.vehicle.get_velocity()
|
||||
c = world.vehicle.get_vehicle_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_number - 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: % 16d FPS' % self.server_fps,
|
||||
'',
|
||||
'Vehicle: % 20s' % get_actor_display_name(world.vehicle, truncate=20),
|
||||
'Map: % 20s' % world.world.map_name,
|
||||
'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)),
|
||||
'',
|
||||
'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)),
|
||||
u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading),
|
||||
'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)),
|
||||
'Height: % 18.0f m' % t.location.z,
|
||||
'',
|
||||
('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),
|
||||
'',
|
||||
'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.vehicle.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))
|
||||
self._notifications.tick(world, clock)
|
||||
|
||||
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)
|
||||
|
||||
|
@ -142,15 +339,48 @@ class HUD(object):
|
|||
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 ----------------------------------------------------------
|
||||
# -- FadingText ----------------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
|
||||
class FadingText(object):
|
||||
def __init__(self, font, dim, pos):
|
||||
self.font = font
|
||||
|
@ -174,9 +404,8 @@ class FadingText(object):
|
|||
def render(self, display):
|
||||
display.blit(self.surface, self.pos)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
# -- HelpText ------------------------------------------------------------
|
||||
# -- HelpText ------------------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
|
||||
|
@ -185,15 +414,7 @@ class HelpText(object):
|
|||
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.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))
|
||||
|
@ -210,44 +431,74 @@ class HelpText(object):
|
|||
if self._render:
|
||||
display.blit(self.surface, self.pos)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
# -- CollisionSensor -----------------------------------------------------
|
||||
# -- 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)
|
||||
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))
|
||||
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 = ' '.join(
|
||||
event.other_actor.type_id.replace(
|
||||
'_', '.').title().split('.')[
|
||||
1:])
|
||||
self._hud.notification('Collision with %r' % actor_type)
|
||||
actor_type = get_actor_display_name(event.other_actor)
|
||||
self._hud.notification('Collision with %r, id = %d' % (actor_type, event.other_actor.id))
|
||||
impulse = event.normal_impulse
|
||||
intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2)
|
||||
self._history.append((event.frame_number, intensity))
|
||||
if len(self._history) > 4000:
|
||||
self._history.pop(0)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
# -- CameraManager -------------------------------------------------------
|
||||
# -- 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_detector')
|
||||
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
|
||||
text = ['%r' % str(x).split()[-1] for x in set(event.crossed_lane_markings)]
|
||||
self._hud.notification('Crossed line %s' % ' and '.join(text))
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
# -- CameraManager -------------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
class CameraManager(object):
|
||||
def __init__(self, parent_actor, hud):
|
||||
|
@ -257,15 +508,18 @@ class CameraManager(object):
|
|||
self._hud = hud
|
||||
self._recording = False
|
||||
self._camera_transforms = [
|
||||
carla.Transform(carla.Location(x=1.6, z=1.7)),
|
||||
carla.Transform(
|
||||
carla.Location(
|
||||
x=24, z=28.0), carla.Rotation(
|
||||
roll=-90, pitch=-90)),
|
||||
carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15))]
|
||||
self._transform_index = 2
|
||||
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.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:
|
||||
|
@ -275,7 +529,10 @@ class CameraManager(object):
|
|||
bp.set_attribute('image_size_y', str(hud.dim[1]))
|
||||
item.append(bp)
|
||||
self._index = None
|
||||
self._server_clock = pygame.time.Clock()
|
||||
|
||||
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)
|
||||
|
@ -292,13 +549,18 @@ class CameraManager(object):
|
|||
# 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))
|
||||
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))
|
||||
|
@ -308,14 +570,28 @@ class CameraManager(object):
|
|||
self = weak_self()
|
||||
if not self:
|
||||
return
|
||||
self._server_clock.tick()
|
||||
self._hud.server_fps = self._server_clock.get_fps()
|
||||
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._sensors[self._index][0].startswith('sensor.lidar'):
|
||||
points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
|
||||
points = np.reshape(points, (int(points.shape[0] / 3), 3))
|
||||
lidar_data = np.array(points[:, :2])
|
||||
lidar_data *= min(self._hud.dim) / 100.0
|
||||
lidar_data += (0.5 * self._hud.dim[0], 0.5 * self._hud.dim[1])
|
||||
lidar_data = np.fabs(lidar_data)
|
||||
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_number)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
|
@ -337,14 +613,20 @@ def game_loop(args):
|
|||
|
||||
hud = HUD(args.width, args.height)
|
||||
world = World(client.get_world(), hud)
|
||||
controller = KeyboardControl(world, False)
|
||||
|
||||
agent = RoamingAgent(world.vehicle)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
clock = pygame.time.Clock()
|
||||
while True:
|
||||
if controller.parse_events(world, clock):
|
||||
return
|
||||
|
||||
# as soon as the server is ready continue!
|
||||
if not world.world.wait_for_tick(10.0):
|
||||
continue
|
||||
|
||||
world.tick(clock)
|
||||
world.render(display)
|
||||
pygame.display.flip()
|
||||
control = agent.run_step()
|
||||
|
|
Loading…
Reference in New Issue