diff --git a/PythonAPI/.pylintrc b/PythonAPI/.pylintrc index 56862e7de..1c870e1b9 100644 --- a/PythonAPI/.pylintrc +++ b/PythonAPI/.pylintrc @@ -1,3 +1,5 @@ +[MESSAGES CONTROL] +max-line-length=120 [MASTER] disable=I0011,E1121 [TYPECHECK] diff --git a/PythonAPI/agents/navigation/agent.py b/PythonAPI/agents/navigation/agent.py index c34d3d57a..510e97908 100644 --- a/PythonAPI/agents/navigation/agent.py +++ b/PythonAPI/agents/navigation/agent.py @@ -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,11 +48,13 @@ class Agent(object): :return: control """ control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 0.0 - control.hand_brake = False - control.manual_gear_shift = False + + if debug: + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 0.0 + control.hand_brake = False + control.manual_gear_shift = False return control @@ -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 diff --git a/PythonAPI/agents/navigation/basic_agent.py b/PythonAPI/agents/navigation/basic_agent.py index 818295868..fb1b7e143 100644 --- a/PythonAPI/agents/navigation/basic_agent.py +++ b/PythonAPI/agents/navigation/basic_agent.py @@ -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 diff --git a/PythonAPI/agents/navigation/controller.py b/PythonAPI/agents/navigation/controller.py index eefe5aa47..69ec9d6b8 100644 --- a/PythonAPI/agents/navigation/controller.py +++ b/PythonAPI/agents/navigation/controller.py @@ -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): """ diff --git a/PythonAPI/agents/navigation/local_planner.py b/PythonAPI/agents/navigation/local_planner.py index bf49650a9..7505cd1a9 100644 --- a/PythonAPI/agents/navigation/local_planner.py +++ b/PythonAPI/agents/navigation/local_planner.py @@ -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 'dt' in opt_dict: - self._dt = opt_dict['dt'] - if 'target_speed' in opt_dict: - self._target_speed = opt_dict['target_speed'] - if 'sampling_radius' in opt_dict: - 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'] + if opt_dict: + if 'dt' in opt_dict: + self.dt = opt_dict['dt'] + if 'target_speed' in opt_dict: + self.target_speed = opt_dict['target_speed'] + if 'sampling_radius' in opt_dict: + 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 diff --git a/PythonAPI/agents/navigation/roaming_agent.py b/PythonAPI/agents/navigation/roaming_agent.py index f01685128..579afa759 100644 --- a/PythonAPI/agents/navigation/roaming_agent.py +++ b/PythonAPI/agents/navigation/roaming_agent.py @@ -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 diff --git a/PythonAPI/agents/tools/misc.py b/PythonAPI/agents/tools/misc.py index b0ab1eb50..83f07c900 100644 --- a/PythonAPI/agents/tools/misc.py +++ b/PythonAPI/agents/tools/misc.py @@ -90,13 +90,13 @@ def distance_vehicle(waypoint, vehicle_transform): return math.sqrt(dx * dx + dy * dy) def vector(location_1, location_2): - """ - Returns the unit vector from location_1 to location_2 - location_1, location_2 : carla.Location objects - """ - x = location_2.x - location_1.x - y = location_2.y - location_1.y - z = location_2.z - location_1.z - norm = np.linalg.norm([x, y, z]) + """ + Returns the unit vector from location_1 to location_2 + location_1, location_2: carla.Location objects + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) - return [x/norm, y/norm, z/norm] + return [x/norm, y/norm, z/norm] diff --git a/PythonAPI/automatic_control.py b/PythonAPI/automatic_control.py index 88c584770..626f98672 100755 --- a/PythonAPI/automatic_control.py +++ b/PythonAPI/automatic_control.py @@ -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 - self._control = carla.VehicleControl() + 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: - 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')) - 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')) + 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.player.get_control().gear + world.hud.notification('%s Transmission' % ( + 'Manual' if self._control.manual_gear_shift else 'Automatic')) + elif self._control.manual_gear_shift and event.key == K_COMMA: + self._control.gear = max(-1, self._control.gear - 1) + elif self._control.manual_gear_shift and event.key == K_PERIOD: + self._control.gear = self._control.gear + 1 + elif event.key == K_p and not (pygame.key.get_mods() & KMOD_CTRL): + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification( + 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) if not self._autopilot_enabled: - self._parse_keys(pygame.key.get_pressed(), clock.get_time()) - self._control.reverse = self._control.gear < 0 + 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, - '', - ('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 isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ '', 'Collision:', collision, '', - 'Number of vehicles: % 8d' % len(vehicles) - ] + '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 @@ -379,18 +461,18 @@ class HUD(object): 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. + 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 @@ -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()