agents code updated
This commit is contained in:
germanros1987
nsubiron
parent
1bb2ccff10
commit
b85b006ad5
|
|
@ -1,3 +1,5 @@
|
|||
[MESSAGES CONTROL]
|
||||
max-line-length=120
|
||||
[MASTER]
|
||||
disable=I0011,E1121
|
||||
[TYPECHECK]
|
||||
|
|
|
|||
|
|
@ -35,6 +35,8 @@ class Agent(object):
|
|||
:param vehicle: actor to apply to local planner logic onto
|
||||
"""
|
||||
self._vehicle = vehicle
|
||||
self._proximity_threshold = 10.0 # meters
|
||||
self._local_planner = None
|
||||
self._world = self._vehicle.get_world()
|
||||
self._map = self._vehicle.get_world().get_map()
|
||||
self._last_traffic_light = None
|
||||
|
|
@ -46,6 +48,8 @@ class Agent(object):
|
|||
:return: control
|
||||
"""
|
||||
control = carla.VehicleControl()
|
||||
|
||||
if debug:
|
||||
control.steer = 0.0
|
||||
control.throttle = 0.0
|
||||
control.brake = 0.0
|
||||
|
|
@ -96,7 +100,7 @@ class Agent(object):
|
|||
if is_within_distance_ahead(loc, ego_vehicle_location,
|
||||
self._vehicle.get_transform().rotation.yaw,
|
||||
self._proximity_threshold):
|
||||
if traffic_light.state == carla.libcarla.TrafficLightState.Red:
|
||||
if traffic_light.state == carla.TrafficLightState.Red:
|
||||
return (True, traffic_light)
|
||||
|
||||
return (False, None)
|
||||
|
|
@ -119,9 +123,8 @@ class Agent(object):
|
|||
# It is too late. Do not block the intersection! Keep going!
|
||||
return (False, None)
|
||||
|
||||
if self._local_planner._target_waypoint is not None:
|
||||
if self._local_planner._target_waypoint.is_intersection:
|
||||
potential_lights = []
|
||||
if self._local_planner.target_waypoint is not None:
|
||||
if self._local_planner.target_waypoint.is_intersection:
|
||||
min_angle = 180.0
|
||||
sel_magnitude = 0.0
|
||||
sel_traffic_light = None
|
||||
|
|
@ -142,7 +145,7 @@ class Agent(object):
|
|||
if self._last_traffic_light is None:
|
||||
self._last_traffic_light = sel_traffic_light
|
||||
|
||||
if self._last_traffic_light.state == carla.libcarla.TrafficLightState.Red:
|
||||
if self._last_traffic_light.state == carla.TrafficLightState.Red:
|
||||
return (True, self._last_traffic_light)
|
||||
else:
|
||||
self._last_traffic_light = None
|
||||
|
|
|
|||
|
|
@ -15,9 +15,9 @@ import math
|
|||
import numpy as np
|
||||
|
||||
import carla
|
||||
from agents.navigation.agent import *
|
||||
from agents.navigation.agent import Agent, AgentState
|
||||
from agents.navigation.local_planner import LocalPlanner
|
||||
from agents.navigation.local_planner import compute_connection, RoadOption
|
||||
from agents.navigation.local_planner import RoadOption
|
||||
from agents.navigation.global_route_planner import GlobalRoutePlanner
|
||||
from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
|
||||
from agents.tools.misc import vector
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ import math
|
|||
import numpy as np
|
||||
|
||||
import carla
|
||||
from agents.tools.misc import distance_vehicle, get_speed
|
||||
from agents.tools.misc import get_speed
|
||||
|
||||
class VehiclePIDController():
|
||||
"""
|
||||
|
|
@ -22,9 +22,7 @@ class VehiclePIDController():
|
|||
low level control a vehicle from client side
|
||||
"""
|
||||
|
||||
def __init__(self, vehicle,
|
||||
args_lateral={'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0},
|
||||
args_longitudinal={'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}):
|
||||
def __init__(self, vehicle, args_lateral=None, args_longitudinal=None):
|
||||
"""
|
||||
:param vehicle: actor to apply to local planner logic onto
|
||||
:param args_lateral: dictionary of arguments to set the lateral PID controller using the following semantics:
|
||||
|
|
@ -37,12 +35,15 @@ class VehiclePIDController():
|
|||
K_D -- Differential term
|
||||
K_I -- Integral term
|
||||
"""
|
||||
if not args_lateral:
|
||||
args_lateral = {'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}
|
||||
if not args_longitudinal:
|
||||
args_longitudinal = {'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}
|
||||
|
||||
self._vehicle = vehicle
|
||||
self._world = self._vehicle.get_world()
|
||||
self._lon_controller = PIDLongitudinalController(
|
||||
self._vehicle, **args_longitudinal)
|
||||
self._lat_controller = PIDLateralController(
|
||||
self._vehicle, **args_lateral)
|
||||
self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal)
|
||||
self._lat_controller = PIDLateralController(self._vehicle, **args_lateral)
|
||||
|
||||
def run_step(self, target_speed, waypoint):
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ class LocalPlanner(object):
|
|||
# total distance)
|
||||
MIN_DISTANCE_PERCENTAGE = 0.9
|
||||
|
||||
def __init__(self, vehicle, opt_dict={}):
|
||||
def __init__(self, vehicle, opt_dict=None):
|
||||
"""
|
||||
:param vehicle: actor to apply to local planner logic onto
|
||||
:param opt_dict: dictionary of arguments with the following semantics:
|
||||
|
|
@ -60,20 +60,20 @@ class LocalPlanner(object):
|
|||
self._vehicle = vehicle
|
||||
self._map = self._vehicle.get_world().get_map()
|
||||
|
||||
self._dt = None
|
||||
self._target_speed = None
|
||||
self._sampling_radius = None
|
||||
self._min_distance = None
|
||||
self._current_waypoint = None
|
||||
self._target_road_option = None
|
||||
self._next_waypoints = None
|
||||
self._target_waypoint = None
|
||||
self._vehicle_controller = None
|
||||
self._global_plan = None
|
||||
# queue with tuples of (waypoint, RoadOption)
|
||||
self._waypoints_queue = deque(maxlen=600)
|
||||
self._buffer_size = 5
|
||||
self._waypoint_buffer = deque(maxlen=self._buffer_size)
|
||||
self.dt = None
|
||||
self.target_speed = None
|
||||
self.sampling_radius = None
|
||||
self.min_distance = None
|
||||
self.current_waypoint = None
|
||||
self.target_road_option = None
|
||||
self.next_waypoints = None
|
||||
self.target_waypoint = None
|
||||
self.vehicle_controller = None
|
||||
self.global_plan = None
|
||||
# queue with tuples of (waypoint, RoadOption)
|
||||
self.waypoints_queue = deque(maxlen=600)
|
||||
self.waypoint_buffer = deque(maxlen=self._buffer_size)
|
||||
|
||||
# initializing controller
|
||||
self.init_controller(opt_dict)
|
||||
|
|
@ -96,45 +96,46 @@ class LocalPlanner(object):
|
|||
:return:
|
||||
"""
|
||||
# default params
|
||||
self._dt = 1.0 / 20.0
|
||||
self._target_speed = 20.0 # Km/h
|
||||
self._sampling_radius = self._target_speed * 0.5 / 3.6 # 0.5 seconds horizon
|
||||
self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE
|
||||
self.dt = 1.0 / 20.0
|
||||
self.target_speed = 20.0 # Km/h
|
||||
self.sampling_radius = self.target_speed * 0.5 / 3.6 # 0.5 seconds horizon
|
||||
self.min_distance = self.sampling_radius * self.MIN_DISTANCE_PERCENTAGE
|
||||
args_lateral_dict = {
|
||||
'K_P': 1.95,
|
||||
'K_D': 0.01,
|
||||
'K_I': 1.4,
|
||||
'dt': self._dt}
|
||||
'dt': self.dt}
|
||||
args_longitudinal_dict = {
|
||||
'K_P': 1.0,
|
||||
'K_D': 0,
|
||||
'K_I': 1,
|
||||
'dt': self._dt}
|
||||
'dt': self.dt}
|
||||
|
||||
# parameters overload
|
||||
if opt_dict:
|
||||
if 'dt' in opt_dict:
|
||||
self._dt = opt_dict['dt']
|
||||
self.dt = opt_dict['dt']
|
||||
if 'target_speed' in opt_dict:
|
||||
self._target_speed = opt_dict['target_speed']
|
||||
self.target_speed = opt_dict['target_speed']
|
||||
if 'sampling_radius' in opt_dict:
|
||||
self._sampling_radius = self._target_speed * \
|
||||
self.sampling_radius = self.target_speed * \
|
||||
opt_dict['sampling_radius'] / 3.6
|
||||
if 'lateral_control_dict' in opt_dict:
|
||||
args_lateral_dict = opt_dict['lateral_control_dict']
|
||||
if 'longitudinal_control_dict' in opt_dict:
|
||||
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
|
||||
|
||||
self._current_waypoint = self._map.get_waypoint(
|
||||
self._vehicle.get_location())
|
||||
self._vehicle_controller = VehiclePIDController(self._vehicle,
|
||||
self.current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
|
||||
self.vehicle_controller = VehiclePIDController(self._vehicle,
|
||||
args_lateral=args_lateral_dict,
|
||||
args_longitudinal=args_longitudinal_dict)
|
||||
|
||||
self._global_plan = False
|
||||
self.global_plan = False
|
||||
|
||||
# compute initial waypoints
|
||||
self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW))
|
||||
self._target_road_option = RoadOption.LANEFOLLOW
|
||||
self.waypoints_queue.append( (self.current_waypoint.next(self.sampling_radius)[0], RoadOption.LANEFOLLOW))
|
||||
|
||||
self.target_road_option = RoadOption.LANEFOLLOW
|
||||
# fill waypoint trajectory queue
|
||||
self._compute_next_waypoints(k=200)
|
||||
|
||||
|
|
@ -146,7 +147,7 @@ class LocalPlanner(object):
|
|||
:param speed: new target speed in Km/h
|
||||
:return:
|
||||
"""
|
||||
self._target_speed = speed
|
||||
self.target_speed = speed
|
||||
|
||||
def _compute_next_waypoints(self, k=1):
|
||||
"""
|
||||
|
|
@ -156,12 +157,12 @@ class LocalPlanner(object):
|
|||
:return:
|
||||
"""
|
||||
# check we do not overflow the queue
|
||||
available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue)
|
||||
available_entries = self.waypoints_queue.maxlen - len(self.waypoints_queue)
|
||||
k = min(available_entries, k)
|
||||
|
||||
for _ in range(k):
|
||||
last_waypoint = self._waypoints_queue[-1][0]
|
||||
next_waypoints = list(last_waypoint.next(self._sampling_radius))
|
||||
last_waypoint = self.waypoints_queue[-1][0]
|
||||
next_waypoints = list(last_waypoint.next(self.sampling_radius))
|
||||
|
||||
if len(next_waypoints) == 1:
|
||||
# only one option available ==> lanefollowing
|
||||
|
|
@ -175,15 +176,15 @@ class LocalPlanner(object):
|
|||
next_waypoint = next_waypoints[road_options_list.index(
|
||||
road_option)]
|
||||
|
||||
self._waypoints_queue.append((next_waypoint, road_option))
|
||||
self.waypoints_queue.append((next_waypoint, road_option))
|
||||
|
||||
|
||||
def set_global_plan(self, current_plan):
|
||||
self._waypoints_queue.clear()
|
||||
self.waypoints_queue.clear()
|
||||
for elem in current_plan:
|
||||
self._waypoints_queue.append(elem)
|
||||
self._target_road_option = RoadOption.LANEFOLLOW
|
||||
self._global_plan = True
|
||||
self.waypoints_queue.append(elem)
|
||||
self.target_road_option = RoadOption.LANEFOLLOW
|
||||
self.global_plan = True
|
||||
|
||||
def run_step(self, debug=True):
|
||||
"""
|
||||
|
|
@ -195,11 +196,11 @@ class LocalPlanner(object):
|
|||
"""
|
||||
|
||||
# not enough waypoints in the horizon? => add more!
|
||||
if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
|
||||
if not self._global_plan:
|
||||
if len(self.waypoints_queue) < int(self.waypoints_queue.maxlen * 0.5):
|
||||
if not self.global_plan:
|
||||
self._compute_next_waypoints(k=100)
|
||||
|
||||
if len(self._waypoints_queue) == 0:
|
||||
if len(self.waypoints_queue) == 0:
|
||||
control = carla.VehicleControl()
|
||||
control.steer = 0.0
|
||||
control.throttle = 0.0
|
||||
|
|
@ -210,35 +211,35 @@ class LocalPlanner(object):
|
|||
return control
|
||||
|
||||
# Buffering the waypoints
|
||||
if not self._waypoint_buffer:
|
||||
if not self.waypoint_buffer:
|
||||
for i in range(self._buffer_size):
|
||||
if self._waypoints_queue:
|
||||
self._waypoint_buffer.append(
|
||||
self._waypoints_queue.popleft())
|
||||
if self.waypoints_queue:
|
||||
self.waypoint_buffer.append(
|
||||
self.waypoints_queue.popleft())
|
||||
else:
|
||||
break
|
||||
|
||||
# current vehicle waypoint
|
||||
self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
|
||||
self.current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
|
||||
# target waypoint
|
||||
self._target_waypoint, self._target_road_option = self._waypoint_buffer[0]
|
||||
self.target_waypoint, self.target_road_option = self.waypoint_buffer[0]
|
||||
# move using PID controllers
|
||||
control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint)
|
||||
control = self.vehicle_controller.run_step(self.target_speed, self.target_waypoint)
|
||||
|
||||
# purge the queue of obsolete waypoints
|
||||
vehicle_transform = self._vehicle.get_transform()
|
||||
max_index = -1
|
||||
|
||||
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
|
||||
for i, (waypoint, _) in enumerate(self.waypoint_buffer):
|
||||
if distance_vehicle(
|
||||
waypoint, vehicle_transform) < self._min_distance:
|
||||
waypoint, vehicle_transform) < self.min_distance:
|
||||
max_index = i
|
||||
if max_index >= 0:
|
||||
for i in range(max_index + 1):
|
||||
self._waypoint_buffer.popleft()
|
||||
self.waypoint_buffer.popleft()
|
||||
|
||||
if debug:
|
||||
draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0)
|
||||
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0)
|
||||
|
||||
return control
|
||||
|
||||
|
|
|
|||
|
|
@ -9,10 +9,7 @@
|
|||
""" This module implements an agent that roams around a track following random waypoints and avoiding other vehicles.
|
||||
The agent also responds to traffic lights. """
|
||||
|
||||
from enum import Enum
|
||||
|
||||
import carla
|
||||
from agents.navigation.agent import *
|
||||
from agents.navigation.agent import Agent, AgentState
|
||||
from agents.navigation.local_planner import LocalPlanner
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -92,7 +92,7 @@ def distance_vehicle(waypoint, vehicle_transform):
|
|||
def vector(location_1, location_2):
|
||||
"""
|
||||
Returns the unit vector from location_1 to location_2
|
||||
location_1, location_2 : carla.Location objects
|
||||
location_1, location_2: carla.Location objects
|
||||
"""
|
||||
x = location_2.x - location_1.x
|
||||
y = location_2.y - location_1.y
|
||||
|
|
|
|||
|
|
@ -53,6 +53,8 @@ try:
|
|||
from pygame.locals import K_r
|
||||
from pygame.locals import K_s
|
||||
from pygame.locals import K_w
|
||||
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')
|
||||
|
||||
|
|
@ -83,8 +85,8 @@ except IndexError:
|
|||
|
||||
import carla
|
||||
from carla import ColorConverter as cc
|
||||
from agents.navigation.roaming_agent import *
|
||||
from agents.navigation.basic_agent import *
|
||||
from agents.navigation.roaming_agent import RoamingAgent
|
||||
from agents.navigation.basic_agent import BasicAgent
|
||||
|
||||
|
||||
|
||||
|
|
@ -109,54 +111,53 @@ def get_actor_display_name(actor, truncate=250):
|
|||
# ==============================================================================
|
||||
|
||||
class World(object):
|
||||
def __init__(self, carla_world, hud):
|
||||
def __init__(self, carla_world, hud, actor_filter):
|
||||
self.world = carla_world
|
||||
self.map = self.world.get_map()
|
||||
self.hud = hud
|
||||
self.vehicle = None
|
||||
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)
|
||||
self.recording_enabled = False
|
||||
self.recording_start = 0
|
||||
|
||||
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
|
||||
|
||||
blueprint = self.world.get_blueprint_library().find('vehicle.lincoln.mkz2017')
|
||||
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 vehicle.
|
||||
if self.vehicle is not None:
|
||||
spawn_point = self.vehicle.get_transform()
|
||||
# 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.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.map.get_spawn_points()
|
||||
spawn_point = spawn_points[1]
|
||||
self.vehicle = self.world.spawn_actor(blueprint, spawn_point)
|
||||
|
||||
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.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.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.vehicle)
|
||||
actor_type = get_actor_display_name(self.player)
|
||||
self.hud.notification(actor_type)
|
||||
|
||||
def next_weather(self, reverse=False):
|
||||
|
|
@ -164,7 +165,7 @@ class World(object):
|
|||
self._weather_index %= len(self._weather_presets)
|
||||
preset = self._weather_presets[self._weather_index]
|
||||
self.hud.notification('Weather: %s' % preset[1])
|
||||
self.vehicle.get_world().set_weather(preset[0])
|
||||
self.player.get_world().set_weather(preset[0])
|
||||
|
||||
def tick(self, clock):
|
||||
self.hud.tick(self, clock)
|
||||
|
|
@ -173,30 +174,43 @@ class World(object):
|
|||
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):
|
||||
actors = [
|
||||
self.camera_manager.sensor,
|
||||
self.collision_sensor.sensor,
|
||||
self.lane_invasion_sensor.sensor,
|
||||
self.vehicle]
|
||||
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, 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.vehicle.set_autopilot(self._autopilot_enabled)
|
||||
world.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
|
||||
|
||||
def parse_events(self, world, clock):
|
||||
def parse_events(self, client, world, clock):
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
return True
|
||||
|
|
@ -219,28 +233,72 @@ class KeyboardControl(object):
|
|||
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:
|
||||
elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL):
|
||||
world.camera_manager.toggle_recording()
|
||||
elif event.key == K_q:
|
||||
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
|
||||
currentIndex = 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(currentIndex)
|
||||
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.vehicle.get_control().gear
|
||||
world.hud.notification(
|
||||
'%s Transmission' % ('Manual' if self._control.manual_gear_shift else 'Automatic'))
|
||||
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:
|
||||
elif event.key == K_p and not (pygame.key.get_mods() & KMOD_CTRL):
|
||||
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'))
|
||||
world.player.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())
|
||||
if isinstance(self._control, carla.VehicleControl):
|
||||
keys = pygame.key.get_pressed()
|
||||
if sum(keys) > 0:
|
||||
self._parse_vehicle_keys(keys, clock.get_time())
|
||||
self._control.reverse = self._control.gear < 0
|
||||
world.player.apply_control(self._control)
|
||||
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_keys(self, keys, milliseconds):
|
||||
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]:
|
||||
|
|
@ -254,13 +312,28 @@ class KeyboardControl(object):
|
|||
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_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 -----------------------------------------------------------------
|
||||
# -- HUD -----------------------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
|
||||
|
|
@ -289,11 +362,12 @@ class HUD(object):
|
|||
self.simulation_time = timestamp.elapsed_seconds
|
||||
|
||||
def tick(self, world, clock):
|
||||
self._notifications.tick(world, clock)
|
||||
if not self._show_info:
|
||||
return
|
||||
t = world.vehicle.get_transform()
|
||||
v = world.vehicle.get_velocity()
|
||||
c = world.vehicle.get_control()
|
||||
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 ''
|
||||
|
|
@ -304,40 +378,48 @@ class HUD(object):
|
|||
collision = [x / max_col for x in collision]
|
||||
vehicles = world.world.get_actors().filter('vehicle.*')
|
||||
self._info_text = [
|
||||
'Server: % 16d FPS' % self.server_fps,
|
||||
'Server: % 16.0f FPS' % self.server_fps,
|
||||
'Client: % 16.0f FPS' % clock.get_fps(),
|
||||
'',
|
||||
'Vehicle: % 20s' % get_actor_display_name(world.vehicle, truncate=20),
|
||||
'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20),
|
||||
'Map: % 20s' % 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)),
|
||||
'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),
|
||||
'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)
|
||||
]
|
||||
'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]
|
||||
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))
|
||||
self._notifications.tick(world, clock)
|
||||
|
||||
def toggle_info(self):
|
||||
self._show_info = not self._show_info
|
||||
|
|
@ -386,11 +468,11 @@ class HUD(object):
|
|||
self._notifications.render(display)
|
||||
self.help.render(display)
|
||||
|
||||
|
||||
# ==============================================================================
|
||||
# -- FadingText ----------------------------------------------------------------
|
||||
# ==============================================================================
|
||||
|
||||
|
||||
class FadingText(object):
|
||||
def __init__(self, font, dim, pos):
|
||||
self.font = font
|
||||
|
|
@ -449,9 +531,9 @@ class HelpText(object):
|
|||
class CollisionSensor(object):
|
||||
def __init__(self, parent_actor, hud):
|
||||
self.sensor = None
|
||||
self._history = []
|
||||
self.history = []
|
||||
self._parent = parent_actor
|
||||
self._hud = hud
|
||||
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)
|
||||
|
|
@ -462,7 +544,7 @@ class CollisionSensor(object):
|
|||
|
||||
def get_collision_history(self):
|
||||
history = collections.defaultdict(int)
|
||||
for frame, intensity in self._history:
|
||||
for frame, intensity in self.history:
|
||||
history[frame] += intensity
|
||||
return history
|
||||
|
||||
|
|
@ -472,23 +554,23 @@ class CollisionSensor(object):
|
|||
if not self:
|
||||
return
|
||||
actor_type = get_actor_display_name(event.other_actor)
|
||||
self._hud.notification('Collision with %r, id = %d' % (actor_type, event.other_actor.id))
|
||||
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_number, intensity))
|
||||
if len(self._history) > 4000:
|
||||
self._history.pop(0)
|
||||
|
||||
self.history.append((event.frame_number, 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
|
||||
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)
|
||||
|
|
@ -503,25 +585,53 @@ class LaneInvasionSensor(object):
|
|||
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))
|
||||
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.surface = None
|
||||
self._parent = parent_actor
|
||||
self._hud = hud
|
||||
self._recording = False
|
||||
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 = [
|
||||
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)'],
|
||||
|
|
@ -532,75 +642,77 @@ class CameraManager(object):
|
|||
['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']]
|
||||
world = self._parent.get_world()
|
||||
bp_library = world.get_blueprint_library()
|
||||
for item in self._sensors:
|
||||
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', '5000')
|
||||
item.append(bp)
|
||||
self._index = None
|
||||
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])
|
||||
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]
|
||||
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.surface = None
|
||||
self.sensor = self._parent.get_world().spawn_actor(
|
||||
self._sensors[index][-1],
|
||||
self._camera_transforms[self._transform_index],
|
||||
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
|
||||
self.hud.notification(self.sensors[index][2])
|
||||
self.index = index
|
||||
|
||||
def next_sensor(self):
|
||||
self.set_sensor(self._index + 1)
|
||||
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'))
|
||||
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))
|
||||
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'):
|
||||
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 *= 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_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)
|
||||
self.surface = pygame.surfarray.make_surface(lidar_img)
|
||||
else:
|
||||
image.convert(self._sensors[self._index][1])
|
||||
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:
|
||||
self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
|
||||
if self.recording:
|
||||
image.save_to_disk('_out/%08d' % image.frame_number)
|
||||
|
||||
|
||||
|
|
@ -622,13 +734,13 @@ def game_loop(args):
|
|||
pygame.HWSURFACE | pygame.DOUBLEBUF)
|
||||
|
||||
hud = HUD(args.width, args.height)
|
||||
world = World(client.get_world(), hud)
|
||||
world = World(client.get_world(), hud, args.filter)
|
||||
controller = KeyboardControl(world, False)
|
||||
|
||||
if args.agent == "Roaming":
|
||||
agent = RoamingAgent(world.vehicle)
|
||||
agent = RoamingAgent(world.player)
|
||||
else:
|
||||
agent = BasicAgent(world.vehicle)
|
||||
agent = BasicAgent(world.player)
|
||||
spawn_point = world.map.get_spawn_points()[0]
|
||||
agent.set_destination((spawn_point.location.x,
|
||||
spawn_point.location.y,
|
||||
|
|
@ -636,7 +748,7 @@ def game_loop(args):
|
|||
|
||||
clock = pygame.time.Clock()
|
||||
while True:
|
||||
if controller.parse_events(world, clock):
|
||||
if controller.parse_events(client, world, clock):
|
||||
return
|
||||
|
||||
# as soon as the server is ready continue!
|
||||
|
|
@ -647,7 +759,8 @@ def game_loop(args):
|
|||
world.render(display)
|
||||
pygame.display.flip()
|
||||
control = agent.run_step()
|
||||
world.vehicle.apply_control(control)
|
||||
control.manual_gear_shift = False
|
||||
world.player.apply_control(control)
|
||||
|
||||
finally:
|
||||
if world is not None:
|
||||
|
|
@ -685,7 +798,11 @@ def main():
|
|||
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("-a", "--agent", type=str,
|
||||
choices=["Roaming", "Basic"],
|
||||
help="select which agent to run",
|
||||
|
|
@ -707,10 +824,7 @@ def main():
|
|||
|
||||
except KeyboardInterrupt:
|
||||
print('\nCancelled by user. Bye!')
|
||||
except Exception as error:
|
||||
logging.exception(error)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
main()
|
||||
|
|
|
|||
Loading…
Reference in New Issue