From ebfb62ea4eb4760cc226946cee0d0ce3ee838b81 Mon Sep 17 00:00:00 2001 From: jderobot Date: Wed, 16 Nov 2022 16:03:55 +0100 Subject: [PATCH 01/36] Added current scripts for running CARLA --- PlayingWithCARLA/README.md | 2 + .../carla_0_9_13/bird_eye_example.py | 55 + PlayingWithCARLA/carla_0_9_13/gradcam.py | 95 ++ .../carla_0_9_13/launch_carla_0_9_13.py | 1387 +++++++++++++++++ .../carla_0_9_13/launch_carla_0_9_13_car.py | 20 + .../launch_carla_0_9_13_simple.py | 1311 ++++++++++++++++ .../launch_carla_0_9_13_synchronous.py | 224 +++ .../launch_carla_0_9_13_synchronous_basic.py | 206 +++ ...ynchronous_and_move_tensorflow_bird_eye.py | 404 +++++ ...s_and_move_tensorflow_bird_eye_velocity.py | 414 +++++ ...ensorflow_bird_eye_velocity_and_command.py | 395 +++++ ...chronous_and_move_tensorflow_full_image.py | 315 ++++ .../launch_synchronous_and_record_dataset.py | 272 ++++ .../move_vehicle_tensorflow_bird_eye.py | 171 ++ .../carla_0_9_13/record_dataset.py | 92 ++ .../carla_0_9_13/record_dataset_bird_eye.py | 157 ++ .../remove_traffic_lights_and_autopilot.py | 163 ++ .../carla_0_9_13/review_images.py | 23 + .../carla_0_9_13/synchronous_mode_test.py | 212 +++ PlayingWithCARLA/carla_0_9_13/test.py | 174 +++ PlayingWithCARLA/carla_agent.py | 12 + PlayingWithCARLA/carla_tutorial.py | 55 + PlayingWithCARLA/gradcam.py | 95 ++ PlayingWithCARLA/launch_carla.py | 653 ++++++++ PlayingWithCARLA/manual_control_modified.py | 743 +++++++++ PlayingWithCARLA/move_autopilot.py | 36 + PlayingWithCARLA/move_vehicle.py | 71 + PlayingWithCARLA/move_vehicle_tensorflow.py | 154 ++ PlayingWithCARLA/plot_car_camera.py | 43 + PlayingWithCARLA/print_camera_image.py | 76 + PlayingWithCARLA/record_dataset.py | 69 + PlayingWithCARLA/remove_traffic_lights.py | 86 + .../remove_traffic_lights_and_autopilot.py | 109 ++ .../set_autopilot_and_print_speed.py | 26 + ...utopilot_and_show_tensorflow_estimation.py | 87 ++ PlayingWithCARLA/show_camera_image.py | 77 + PlayingWithCARLA/show_camera_opencv.py | 68 + PlayingWithCARLA/show_crop_camera_image.py | 76 + PlayingWithCARLA/show_dataset_image.py | 7 + PlayingWithCARLA/show_environment_objects.py | 39 + PlayingWithCARLA/show_grad_camera.py | 122 ++ PlayingWithCARLA/tutorial.py | 127 ++ 42 files changed, 8923 insertions(+) create mode 100644 PlayingWithCARLA/README.md create mode 100644 PlayingWithCARLA/carla_0_9_13/bird_eye_example.py create mode 100644 PlayingWithCARLA/carla_0_9_13/gradcam.py create mode 100755 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py create mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py create mode 100644 PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py create mode 100644 PlayingWithCARLA/carla_0_9_13/record_dataset.py create mode 100644 PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py create mode 100644 PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py create mode 100644 PlayingWithCARLA/carla_0_9_13/review_images.py create mode 100644 PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py create mode 100644 PlayingWithCARLA/carla_0_9_13/test.py create mode 100644 PlayingWithCARLA/carla_agent.py create mode 100644 PlayingWithCARLA/carla_tutorial.py create mode 100644 PlayingWithCARLA/gradcam.py create mode 100644 PlayingWithCARLA/launch_carla.py create mode 100755 PlayingWithCARLA/manual_control_modified.py create mode 100644 PlayingWithCARLA/move_autopilot.py create mode 100644 PlayingWithCARLA/move_vehicle.py create mode 100644 PlayingWithCARLA/move_vehicle_tensorflow.py create mode 100644 PlayingWithCARLA/plot_car_camera.py create mode 100644 PlayingWithCARLA/print_camera_image.py create mode 100644 PlayingWithCARLA/record_dataset.py create mode 100644 PlayingWithCARLA/remove_traffic_lights.py create mode 100644 PlayingWithCARLA/remove_traffic_lights_and_autopilot.py create mode 100644 PlayingWithCARLA/set_autopilot_and_print_speed.py create mode 100644 PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py create mode 100644 PlayingWithCARLA/show_camera_image.py create mode 100644 PlayingWithCARLA/show_camera_opencv.py create mode 100644 PlayingWithCARLA/show_crop_camera_image.py create mode 100644 PlayingWithCARLA/show_dataset_image.py create mode 100644 PlayingWithCARLA/show_environment_objects.py create mode 100644 PlayingWithCARLA/show_grad_camera.py create mode 100755 PlayingWithCARLA/tutorial.py diff --git a/PlayingWithCARLA/README.md b/PlayingWithCARLA/README.md new file mode 100644 index 00000000..319a2b92 --- /dev/null +++ b/PlayingWithCARLA/README.md @@ -0,0 +1,2 @@ +# PlayingWithCARLA +Scripts for the initial steps with CARLA simulator diff --git a/PlayingWithCARLA/carla_0_9_13/bird_eye_example.py b/PlayingWithCARLA/carla_0_9_13/bird_eye_example.py new file mode 100644 index 00000000..02a5ffcf --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/bird_eye_example.py @@ -0,0 +1,55 @@ +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import carla + +client = carla.Client('localhost', 2000) +client.set_timeout(2.0) + +birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=150, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AND_REAR_AREA +) + +birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AND_REAR_AREA +) + +world = client.get_world() +print(world.get_actors()) +car = world.get_actors().filter('vehicle.*')[0] +# Input for your model - call it every simulation step +# returned result is np.ndarray with ones and zeros of shape (8, height, width) +birdview = birdview_producer.produce( + agent_vehicle=car # carla.Actor (spawned vehicle) +) + +import matplotlib.pyplot as plt +import cv2 +import time +import numpy as np + +while True: + # Use only if you want to visualize + # produces np.ndarray of shape (height, width, 3) + rgb = BirdViewProducer.as_rgb(birdview) + #print(rgb) + ''' + for x, im in enumerate(rgb): + for y, im_2 in enumerate(rgb[x]): + #print(rgb[x][y]) + #print(np.array([0,0,0])) + #print((rgb[x][y] == np.array([0,0,0])).all()) + if (rgb[x][y] == np.array([0,0,0])).all() != True: + print(rgb[x][y]) + + ''' + plt.imshow(rgb) + plt.show() + + print(rgb.shape) + + #time.sleep(1) diff --git a/PlayingWithCARLA/carla_0_9_13/gradcam.py b/PlayingWithCARLA/carla_0_9_13/gradcam.py new file mode 100644 index 00000000..55dffa10 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/gradcam.py @@ -0,0 +1,95 @@ +from tensorflow.keras.models import Model +from tensorflow.keras.layers import Conv2D +import tensorflow as tf +import numpy as np +import cv2 + + +class GradCAM: + def __init__(self, model, class_idx, layer_name=None): + # store the model, the class index used to measure the class + # activation map, and the layer to be used when visualizing + # the class activation map + self.model = model + self.class_idx = class_idx + self.layerName = layer_name + # if the layer name is None, attempt to automatically find + # the target output layer + if self.layerName is None: + self.layerName = self.find_target_layer() + + def find_target_layer(self): + # attempt to find the final convolutional layer in the network + # by looping over the layers of the network in reverse order + for layer in reversed(self.model.layers): + # check to see if the layer has a 4D output + #if i > 6 and len(layer.output_shape) == 4: + if len(layer.output_shape) == 4 and type(layer) == Conv2D: + print(layer.name) + return layer.name + # otherwise, we could not find a 4D layer so the GradCAM + # algorithm cannot be applied + raise ValueError("Could not find 4D layer. Cannot apply GradCAM.") + + def compute_heatmap(self, image, eps=1e-8): + # construct our gradient model by supplying (1) the inputs + # to our pre-trained model, (2) the output of the (presumably) + # final 4D layer in the network, and (3) the output of the + # softmax activations from the model + grad_model = Model( + inputs=[self.model.inputs], + outputs=[self.model.get_layer(self.layerName).output, + self.model.output]) + + # record operations for automatic differentiation + with tf.GradientTape() as tape: + # cast the image tensor to a float-32 data type, pass the + # image through the gradient model, and grab the loss + # associated with the specific class index + inputs = tf.cast(image, tf.float32) + (conv_outputs, predictions) = grad_model(inputs) + #loss = predictions[:, self.class_idx] + loss = predictions[:, 1] + # use automatic differentiation to compute the gradients + grads = tape.gradient(loss, conv_outputs) + + # compute the guided gradients + cast_conv_outputs = tf.cast(conv_outputs > 0, "float32") + cast_grads = tf.cast(grads > 0, "float32") + guided_grads = cast_conv_outputs * cast_grads * grads + # the convolution and guided gradients have a batch dimension + # (which we don't need) so let's grab the volume itself and + # discard the batch + conv_outputs = conv_outputs[0] + guided_grads = guided_grads[0] + + # compute the average of the gradient values, and using them + # as weights, compute the ponderation of the filters with + # respect to the weights + weights = tf.reduce_mean(guided_grads, axis=(0, 1)) + cam = tf.reduce_sum(tf.multiply(weights, conv_outputs), axis=-1) + + # grab the spatial dimensions of the input image and resize + # the output class activation map to match the input image + # dimensions + (w, h) = (image.shape[2], image.shape[1]) + heatmap = cv2.resize(cam.numpy(), (w, h)) + # normalize the heatmap such that all values lie in the range + # [0, 1], scale the resulting values to the range [0, 255], + # and then convert to an unsigned 8-bit integer + numer = heatmap - np.min(heatmap) + denom = (heatmap.max() - heatmap.min()) + eps + heatmap = numer / denom + heatmap = (heatmap * 255).astype("uint8") + # return the resulting heatmap to the calling function + return heatmap + + def overlay_heatmap(self, heatmap, image, alpha=0.5, + colormap=cv2.COLORMAP_VIRIDIS): + # apply the supplied color map to the heatmap and then + # overlay the heatmap on the input image + heatmap = cv2.applyColorMap(heatmap, colormap) + output = cv2.addWeighted(image, alpha, heatmap, 1 - alpha, 0) + # return a 2-tuple of the color mapped heatmap and the output, + # overlaid image + return heatmap, output \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py new file mode 100755 index 00000000..8da76e9d --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py @@ -0,0 +1,1387 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + A/D : steer left/right + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + O : open/close all doors of vehicle + T : toggle vehicle's telemetry + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + R : toggle recording images to disk + + CTRL + R : toggle recording of simulation (replacing any previous) + CTRL + P : start replaying last recorded simulation + CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) + CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import logging +import math +import random +import re +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + from pygame.locals import K_COMMA + from pygame.locals import K_DOWN + from pygame.locals import K_ESCAPE + from pygame.locals import K_F1 + from pygame.locals import K_LEFT + from pygame.locals import K_PERIOD + from pygame.locals import K_RIGHT + from pygame.locals import K_SLASH + from pygame.locals import K_SPACE + from pygame.locals import K_TAB + from pygame.locals import K_UP + from pygame.locals import K_a + from pygame.locals import K_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_o + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + from pygame.locals import K_MINUS + from pygame.locals import K_EQUALS +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.sync = args.sync + self.actor_role_name = args.rolename + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.imu_sensor = None + self.radar_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._actor_generation = args.generation + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.show_vehicle_telemetry = False + self.doors_are_open = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + self.player_max_speed = 1.589 + self.player_max_speed_fast = 3.713 + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Get a random blueprint. + blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) + blueprint = self.world.get_blueprint_library().filter('vehicle')[0] + print(blueprint) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + # Draw the spawn point locations as numbers in the map + for i, spawn_point in enumerate(spawn_points): + print(i, spawn_point) + self.world.debug.draw_string(spawn_point.location, str(i), life_time=10000) + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + print('*****************************************************') + #spawn_point = self.world.get_map().get_spawn_points()[0] + # Town 2 + #spawn_point = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # Town 1 + # clockwise + #spawn_point = carla.Transform(carla.Location(x=2.0, y=70, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + # anticlockwise + spawn_point = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + + #spawn_point = carla.Transform(carla.Location(x=10.0, y=331, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=5.0, y=330, z=1.37), carla.Rotation(pitch=0, yaw=30, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=150.0, y=331, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=-2.0, y=13, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=2, y=50, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + #spawn_point = carla.Transform(carla.Location(x=20, y=330, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=330, y=330, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + + #spawn_point = carla.Transform(carla.Location(x=228, y=-2, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) + + + #spawn_point = carla.Transform(carla.Location(x=-0.5, y=40, z=1.37), carla.Rotation(pitch=0, yaw=30, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=396, y=30, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 3 + # anticlockwise + #spawn_point = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + # clockwise + #spawn_point = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) + + # Town 4 + #spawn_point = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=16.6, y=152.4, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # anticlockwise + #spawn_point = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + #Town 05 + # clockwise + #spawn_point = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + # anticlockwise + #spawn_point = carla.Transform(carla.Location(x=115, y=-207.3, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) + + # Town 06 + #spawn_point = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 7 + # anticlockwise + #spawn_point = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) + # clockwise + #spawn_point = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #print(spawn_point) + + + + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.imu_sensor = IMUSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + if self.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + sensors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.imu_sensor.sensor] + for sensor in sensors: + if sensor is not None: + sensor.stop() + sensor.destroy() + if self.player is not None: + self.player.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + """Class that handles keyboard input.""" + def __init__(self, world, start_in_autopilot): + self._autopilot_enabled = start_in_autopilot + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock, sync_mode): + if isinstance(self._control, carla.VehicleControl): + current_lights = self._lights + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: + world.next_weather(reverse=True) + elif event.key == K_c: + world.next_weather() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key == K_o: + try: + if world.doors_are_open: + world.hud.notification("Closing Doors") + world.doors_are_open = False + world.player.close_door(carla.VehicleDoor.All) + else: + world.hud.notification("Opening doors") + world.doors_are_open = True + world.player.open_door(carla.VehicleDoor.All) + except Exception: + pass + elif event.key == K_t: + if world.show_vehicle_telemetry: + world.player.show_debug_telemetry(False) + world.show_vehicle_telemetry = False + world.hud.notification("Disabled Vehicle Telemetry") + else: + try: + world.player.show_debug_telemetry(True) + world.show_vehicle_telemetry = True + world.hud.notification("Enabled Vehicle Telemetry") + except Exception: + pass + elif event.key > K_0 and event.key <= K_9: + index_ctrl = 0 + if pygame.key.get_mods() & KMOD_CTRL: + index_ctrl = 9 + world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) + elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): + world.camera_manager.toggle_recording() + elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): + if (world.recording_enabled): + client.stop_recorder() + world.recording_enabled = False + world.hud.notification("Recorder is OFF") + else: + client.start_recorder("manual_recording.rec") + world.recording_enabled = True + world.hud.notification("Recorder is ON") + elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): + # stop recorder + client.stop_recorder() + world.recording_enabled = False + # work around to fix camera at start of replaying + current_index = world.camera_manager.index + world.destroy_sensors() + # disable autopilot + self._autopilot_enabled = False + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification("Replaying file 'manual_recording.rec'") + # replayer + client.replay_file("manual_recording.rec", world.recording_start, 0, 0) + world.camera_manager.set_sensor(current_index) + elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start -= 10 + else: + world.recording_start -= 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start += 10 + else: + world.recording_start += 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_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: + if not self._autopilot_enabled and not sync_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification( + 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: + current_lights ^= carla.VehicleLightState.Special1 + elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: + current_lights ^= carla.VehicleLightState.HighBeam + elif event.key == K_l: + # Use 'L' key to switch between lights: + # closed -> position -> low beam -> fog + if not self._lights & carla.VehicleLightState.Position: + world.hud.notification("Position lights") + current_lights |= carla.VehicleLightState.Position + else: + world.hud.notification("Low beam lights") + current_lights |= carla.VehicleLightState.LowBeam + if self._lights & carla.VehicleLightState.LowBeam: + world.hud.notification("Fog lights") + current_lights |= carla.VehicleLightState.Fog + if self._lights & carla.VehicleLightState.Fog: + world.hud.notification("Lights off") + current_lights ^= carla.VehicleLightState.Position + current_lights ^= carla.VehicleLightState.LowBeam + current_lights ^= carla.VehicleLightState.Fog + elif event.key == K_i: + current_lights ^= carla.VehicleLightState.Interior + elif event.key == K_z: + current_lights ^= carla.VehicleLightState.LeftBlinker + elif event.key == K_x: + current_lights ^= carla.VehicleLightState.RightBlinker + + if not self._autopilot_enabled: + if isinstance(self._control, carla.VehicleControl): + self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) + self._control.reverse = self._control.gear < 0 + # Set automatic control-related vehicle lights + if self._control.brake: + current_lights |= carla.VehicleLightState.Brake + else: # Remove the Brake flag + current_lights &= ~carla.VehicleLightState.Brake + if self._control.reverse: + current_lights |= carla.VehicleLightState.Reverse + else: # Remove the Reverse flag + current_lights &= ~carla.VehicleLightState.Reverse + if current_lights != self._lights: # Change the light state only if necessary + self._lights = current_lights + world.player.set_light_state(carla.VehicleLightState(self._lights)) + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + if keys[K_UP] or keys[K_w]: + self._control.throttle = min(self._control.throttle + 0.01, 1.00) + else: + self._control.throttle = 0.0 + + if keys[K_DOWN] or keys[K_s]: + self._control.brake = min(self._control.brake + 0.2, 1) + else: + self._control.brake = 0 + + steer_increment = 5e-4 * milliseconds + if keys[K_LEFT] or keys[K_a]: + if self._steer_cache > 0: + self._steer_cache = 0 + else: + self._steer_cache -= steer_increment + elif keys[K_RIGHT] or keys[K_d]: + if self._steer_cache < 0: + self._steer_cache = 0 + else: + self._steer_cache += steer_increment + else: + self._steer_cache = 0.0 + self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) + self._control.steer = round(self._steer_cache, 1) + self._control.hand_brake = keys[K_SPACE] + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 16), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + if not self._show_info: + return + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() + compass = world.imu_sensor.compass + heading = 'N' if compass > 270.5 or compass < 89.5 else '' + heading += 'S' if 90.5 < compass < 269.5 else '' + heading += 'E' if 0.5 < compass < 179.5 else '' + heading += 'W' if 180.5 < compass < 359.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name.split('/')[-1], + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), + u'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), + 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), + 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % t.location.z, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) + vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] + for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]): + if d > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (d, vehicle_type)) + + def toggle_info(self): + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """Helper class to handle text output using pygame""" + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.line_space = 18 + self.dim = (780, len(lines) * self.line_space + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * self.line_space)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + + # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor + if parent_actor.type_id.startswith("vehicle."): + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + + +# ============================================================================== +# -- GnssSensor ---------------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + + +# ============================================================================== +# -- IMUSensor ----------------------------------------------------------------- +# ============================================================================== + + +class IMUSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.accelerometer = (0.0, 0.0, 0.0) + self.gyroscope = (0.0, 0.0, 0.0) + self.compass = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.imu') + self.sensor = world.spawn_actor( + bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) + + @staticmethod + def _IMU_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + limits = (-99.9, 99.9) + self.accelerometer = ( + max(limits[0], min(limits[1], sensor_data.accelerometer.x)), + max(limits[0], min(limits[1], sensor_data.accelerometer.y)), + max(limits[0], min(limits[1], sensor_data.accelerometer.z))) + self.gyroscope = ( + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) + self.compass = math.degrees(sensor_data.compass) + + +# ============================================================================== +# -- RadarSensor --------------------------------------------------------------- +# ============================================================================== + + +class RadarSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + bound_x = 0.5 + self._parent.bounding_box.extent.x + bound_y = 0.5 + self._parent.bounding_box.extent.y + bound_z = 0.5 + self._parent.bounding_box.extent.z + + self.velocity_range = 7.5 # m/s + world = self._parent.get_world() + self.debug = world.debug + bp = world.get_blueprint_library().find('sensor.other.radar') + bp.set_attribute('horizontal_fov', str(35)) + bp.set_attribute('vertical_fov', str(20)) + self.sensor = world.spawn_actor( + bp, + carla.Transform( + carla.Location(x=bound_x + 0.05, z=bound_z+0.05), + carla.Rotation(pitch=5)), + attach_to=self._parent) + # We need a weak reference to self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) + + @staticmethod + def _Radar_callback(weak_self, radar_data): + self = weak_self() + if not self: + return + # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: + # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) + # points = np.reshape(points, (len(radar_data), 4)) + + current_rot = radar_data.transform.rotation + for detect in radar_data: + azi = math.degrees(detect.azimuth) + alt = math.degrees(detect.altitude) + # The 0.25 adjusts a bit the distance so the dots can + # be properly seen + fw_vec = carla.Vector3D(x=detect.depth - 0.25) + carla.Transform( + carla.Location(), + carla.Rotation( + pitch=current_rot.pitch + alt, + yaw=current_rot.yaw + azi, + roll=current_rot.roll)).transform(fw_vec) + + def clamp(min_v, max_v, value): + return max(min_v, min(value, max_v)) + + norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] + r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) + g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) + b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) + self.debug.draw_point( + radar_data.transform.location + fw_vec, + size=0.075, + life_time=0.06, + persistent_lines=False, + color=carla.Color(r, g, b)) + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + def __init__(self, parent_actor, hud, gamma_correction): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_x = 0.5 + self._parent.bounding_box.extent.x + bound_y = 0.5 + self._parent.bounding_box.extent.y + bound_z = 0.5 + self._parent.bounding_box.extent.z + Attachment = carla.AttachmentType + + if not self._parent.type_id.startswith("walker.pedestrian"): + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), + (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] + else: + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] + + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}], + ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], + ] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + item.append(bp) + self.index = None + + def toggle_camera(self): + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + index = index % len(self.sensors) + needs_respawn = True if self.index is None else \ + (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1]) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): + # Example of converting the raw_data from a carla.DVSEventArray + # sensor into a NumPy array and using it as an image + dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ + ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) + dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) + # Blue is positive, red is negative + dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 + self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) + elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): + image = image.get_color_coded_flow() + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + original_settings = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(20.0) + + sim_world = client.get_world() + if args.sync: + original_settings = sim_world.get_settings() + settings = sim_world.get_settings() + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + + if args.autopilot and not sim_world.get_settings().synchronous_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + display.fill((0,0,0)) + pygame.display.flip() + + hud = HUD(args.width, args.height) + world = World(sim_world, hud, args) + controller = KeyboardControl(world, args.autopilot) + + if args.sync: + sim_world.tick() + else: + sim_world.wait_for_tick() + + clock = pygame.time.Clock() + while True: + if args.sync: + sim_world.tick() + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock, args.sync): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if original_settings: + sim_world.apply_settings(original_settings) + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='actor filter (default: "vehicle.*")') + argparser.add_argument( + '--generation', + metavar='G', + default='2', + help='restrict to certain actor generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '--sync', + action='store_true', + help='Activate synchronous mode execution') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py new file mode 100644 index 00000000..effff6a1 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py @@ -0,0 +1,20 @@ +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import pandas + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +blueprint = world.get_blueprint_library().filter('vehicle')[0] +print(blueprint) +spawn_point = world.get_map().get_spawn_points()[0] +print(spawn_point) +player = world.try_spawn_actor(blueprint, spawn_point) \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py new file mode 100644 index 00000000..6fa986a4 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py @@ -0,0 +1,1311 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + A/D : steer left/right + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + CTRL + W : toggle constant velocity mode at 60 km/h + + L : toggle next light type + SHIFT + L : toggle high beam + Z/X : toggle right/left blinker + I : toggle interior light + + TAB : change sensor position + ` or N : next sensor + [1-9] : change to sensor [1-9] + G : toggle radar visualization + C : change weather (Shift+C reverse) + Backspace : change vehicle + + O : open/close all doors of vehicle + T : toggle vehicle's telemetry + + V : Select next map layer (Shift+V reverse) + B : Load current selected map layer (Shift+B to unload) + + R : toggle recording images to disk + + CTRL + R : toggle recording of simulation (replacing any previous) + CTRL + P : start replaying last recorded simulation + CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) + CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import logging +import math +import random +import re +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + from pygame.locals import K_COMMA + from pygame.locals import K_DOWN + from pygame.locals import K_ESCAPE + from pygame.locals import K_F1 + from pygame.locals import K_LEFT + from pygame.locals import K_PERIOD + from pygame.locals import K_RIGHT + from pygame.locals import K_SLASH + from pygame.locals import K_SPACE + from pygame.locals import K_TAB + from pygame.locals import K_UP + from pygame.locals import K_a + from pygame.locals import K_b + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_g + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_l + from pygame.locals import K_m + from pygame.locals import K_n + from pygame.locals import K_o + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_t + from pygame.locals import K_v + from pygame.locals import K_w + from pygame.locals import K_x + from pygame.locals import K_z + from pygame.locals import K_MINUS + from pygame.locals import K_EQUALS +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud, args): + self.world = carla_world + self.sync = args.sync + self.actor_role_name = args.rolename + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.imu_sensor = None + self.radar_sensor = None + self.camera_manager = None + self._actor_filter = args.filter + self._actor_generation = args.generation + self._gamma = args.gamma + self.restart() + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + self.constant_velocity_enabled = False + self.show_vehicle_telemetry = False + self.doors_are_open = False + self.current_map_layer = 0 + self.map_layer_names = [ + carla.MapLayer.NONE, + carla.MapLayer.Buildings, + carla.MapLayer.Decals, + carla.MapLayer.Foliage, + carla.MapLayer.Ground, + carla.MapLayer.ParkedVehicles, + carla.MapLayer.Particles, + carla.MapLayer.Props, + carla.MapLayer.StreetLights, + carla.MapLayer.Walls, + carla.MapLayer.All + ] + + def restart(self): + self.player_max_speed = 1.589 + self.player_max_speed_fast = 3.713 + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Get a random blueprint. + blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) + blueprint = self.world.get_blueprint_library().filter('vehicle')[0] + print(blueprint) + blueprint.set_attribute('role_name', self.actor_role_name) + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute('is_invincible'): + blueprint.set_attribute('is_invincible', 'true') + # set the max speed + if blueprint.has_attribute('speed'): + self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) + self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) + + # Spawn the player. + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + print('*****************************************************') + spawn_point = self.world.get_map().get_spawn_points()[0] + #spawn_point = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + print(spawn_point) + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + self.show_vehicle_telemetry = False + self.modify_vehicle_physics(self.player) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.imu_sensor = IMUSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_index + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + if self.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + def next_map_layer(self, reverse=False): + self.current_map_layer += -1 if reverse else 1 + self.current_map_layer %= len(self.map_layer_names) + selected = self.map_layer_names[self.current_map_layer] + self.hud.notification('LayerMap selected: %s' % selected) + + def load_map_layer(self, unload=False): + selected = self.map_layer_names[self.current_map_layer] + if unload: + self.hud.notification('Unloading map layer: %s' % selected) + self.world.unload_map_layer(selected) + else: + self.hud.notification('Loading map layer: %s' % selected) + self.world.load_map_layer(selected) + + def toggle_radar(self): + if self.radar_sensor is None: + self.radar_sensor = RadarSensor(self.player) + elif self.radar_sensor.sensor is not None: + self.radar_sensor.sensor.destroy() + self.radar_sensor = None + + def modify_vehicle_physics(self, actor): + #If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + if self.radar_sensor is not None: + self.toggle_radar() + sensors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.imu_sensor.sensor] + for sensor in sensors: + if sensor is not None: + sensor.stop() + sensor.destroy() + if self.player is not None: + self.player.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + """Class that handles keyboard input.""" + def __init__(self, world, start_in_autopilot): + self._autopilot_enabled = start_in_autopilot + if isinstance(world.player, carla.Vehicle): + self._control = carla.VehicleControl() + self._lights = carla.VehicleLightState.NONE + world.player.set_autopilot(self._autopilot_enabled) + world.player.set_light_state(self._lights) + elif isinstance(world.player, carla.Walker): + self._control = carla.WalkerControl() + self._autopilot_enabled = False + self._rotation = world.player.get_transform().rotation + else: + raise NotImplementedError("Actor type not supported") + self._steer_cache = 0.0 + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self, client, world, clock, sync_mode): + if isinstance(self._control, carla.VehicleControl): + current_lights = self._lights + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_BACKSPACE: + if self._autopilot_enabled: + world.player.set_autopilot(False) + world.restart() + world.player.set_autopilot(True) + else: + world.restart() + elif event.key == K_F1: + world.hud.toggle_info() + elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: + world.next_map_layer(reverse=True) + elif event.key == K_v: + world.next_map_layer() + elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: + world.load_map_layer(unload=True) + elif event.key == K_b: + world.load_map_layer() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + world.hud.help.toggle() + elif event.key == K_TAB: + world.camera_manager.toggle_camera() + elif event.key == K_g: + world.toggle_radar() + elif event.key == K_BACKQUOTE: + world.camera_manager.next_sensor() + elif event.key == K_n: + world.camera_manager.next_sensor() + elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): + if world.constant_velocity_enabled: + world.player.disable_constant_velocity() + world.constant_velocity_enabled = False + world.hud.notification("Disabled Constant Velocity Mode") + else: + world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) + world.constant_velocity_enabled = True + world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") + elif event.key == K_o: + try: + if world.doors_are_open: + world.hud.notification("Closing Doors") + world.doors_are_open = False + world.player.close_door(carla.VehicleDoor.All) + else: + world.hud.notification("Opening doors") + world.doors_are_open = True + world.player.open_door(carla.VehicleDoor.All) + except Exception: + pass + elif event.key == K_t: + if world.show_vehicle_telemetry: + world.player.show_debug_telemetry(False) + world.show_vehicle_telemetry = False + world.hud.notification("Disabled Vehicle Telemetry") + else: + try: + world.player.show_debug_telemetry(True) + world.show_vehicle_telemetry = True + world.hud.notification("Enabled Vehicle Telemetry") + except Exception: + pass + elif event.key > K_0 and event.key <= K_9: + index_ctrl = 0 + if pygame.key.get_mods() & KMOD_CTRL: + index_ctrl = 9 + world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) + elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): + world.camera_manager.toggle_recording() + elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): + if (world.recording_enabled): + client.stop_recorder() + world.recording_enabled = False + world.hud.notification("Recorder is OFF") + else: + client.start_recorder("manual_recording.rec") + world.recording_enabled = True + world.hud.notification("Recorder is ON") + elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): + # stop recorder + client.stop_recorder() + world.recording_enabled = False + # work around to fix camera at start of replaying + current_index = world.camera_manager.index + world.destroy_sensors() + # disable autopilot + self._autopilot_enabled = False + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification("Replaying file 'manual_recording.rec'") + # replayer + client.replay_file("manual_recording.rec", world.recording_start, 0, 0) + world.camera_manager.set_sensor(current_index) + elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start -= 10 + else: + world.recording_start -= 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): + if pygame.key.get_mods() & KMOD_SHIFT: + world.recording_start += 10 + else: + world.recording_start += 1 + world.hud.notification("Recording start time is %d" % (world.recording_start)) + if isinstance(self._control, carla.VehicleControl): + if event.key == K_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: + if not self._autopilot_enabled and not sync_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + self._autopilot_enabled = not self._autopilot_enabled + world.player.set_autopilot(self._autopilot_enabled) + world.hud.notification( + 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: + current_lights ^= carla.VehicleLightState.Special1 + elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: + current_lights ^= carla.VehicleLightState.HighBeam + elif event.key == K_l: + # Use 'L' key to switch between lights: + # closed -> position -> low beam -> fog + if not self._lights & carla.VehicleLightState.Position: + world.hud.notification("Position lights") + current_lights |= carla.VehicleLightState.Position + else: + world.hud.notification("Low beam lights") + current_lights |= carla.VehicleLightState.LowBeam + if self._lights & carla.VehicleLightState.LowBeam: + world.hud.notification("Fog lights") + current_lights |= carla.VehicleLightState.Fog + if self._lights & carla.VehicleLightState.Fog: + world.hud.notification("Lights off") + current_lights ^= carla.VehicleLightState.Position + current_lights ^= carla.VehicleLightState.LowBeam + current_lights ^= carla.VehicleLightState.Fog + elif event.key == K_i: + current_lights ^= carla.VehicleLightState.Interior + elif event.key == K_z: + current_lights ^= carla.VehicleLightState.LeftBlinker + elif event.key == K_x: + current_lights ^= carla.VehicleLightState.RightBlinker + + if not self._autopilot_enabled: + if isinstance(self._control, carla.VehicleControl): + self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) + self._control.reverse = self._control.gear < 0 + # Set automatic control-related vehicle lights + if self._control.brake: + current_lights |= carla.VehicleLightState.Brake + else: # Remove the Brake flag + current_lights &= ~carla.VehicleLightState.Brake + if self._control.reverse: + current_lights |= carla.VehicleLightState.Reverse + else: # Remove the Reverse flag + current_lights &= ~carla.VehicleLightState.Reverse + if current_lights != self._lights: # Change the light state only if necessary + self._lights = current_lights + world.player.set_light_state(carla.VehicleLightState(self._lights)) + elif isinstance(self._control, carla.WalkerControl): + self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) + world.player.apply_control(self._control) + + def _parse_vehicle_keys(self, keys, milliseconds): + if keys[K_UP] or keys[K_w]: + self._control.throttle = min(self._control.throttle + 0.01, 1.00) + else: + self._control.throttle = 0.0 + + if keys[K_DOWN] or keys[K_s]: + self._control.brake = min(self._control.brake + 0.2, 1) + else: + self._control.brake = 0 + + steer_increment = 5e-4 * milliseconds + if keys[K_LEFT] or keys[K_a]: + if self._steer_cache > 0: + self._steer_cache = 0 + else: + self._steer_cache -= steer_increment + elif keys[K_RIGHT] or keys[K_d]: + if self._steer_cache < 0: + self._steer_cache = 0 + else: + self._steer_cache += steer_increment + else: + self._steer_cache = 0.0 + self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) + self._control.steer = round(self._steer_cache, 1) + self._control.hand_brake = keys[K_SPACE] + + def _parse_walker_keys(self, keys, milliseconds, world): + self._control.speed = 0.0 + if keys[K_DOWN] or keys[K_s]: + self._control.speed = 0.0 + if keys[K_LEFT] or keys[K_a]: + self._control.speed = .01 + self._rotation.yaw -= 0.08 * milliseconds + if keys[K_RIGHT] or keys[K_d]: + self._control.speed = .01 + self._rotation.yaw += 0.08 * milliseconds + if keys[K_UP] or keys[K_w]: + self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed + self._control.jump = keys[K_SPACE] + self._rotation.yaw = round(self._rotation.yaw, 1) + self._control.direction = self._rotation.get_forward_vector() + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 16), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + self._notifications.tick(world, clock) + if not self._show_info: + return + t = world.player.get_transform() + v = world.player.get_velocity() + c = world.player.get_control() + compass = world.imu_sensor.compass + heading = 'N' if compass > 270.5 or compass < 89.5 else '' + heading += 'S' if 90.5 < compass < 269.5 else '' + heading += 'E' if 0.5 < compass < 179.5 else '' + heading += 'W' if 180.5 < compass < 359.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name.split('/')[-1], + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), + u'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), + 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), + 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % t.location.z, + ''] + if isinstance(c, carla.VehicleControl): + self._info_text += [ + ('Throttle:', c.throttle, 0.0, 1.0), + ('Steer:', c.steer, -1.0, 1.0), + ('Brake:', c.brake, 0.0, 1.0), + ('Reverse:', c.reverse), + ('Hand brake:', c.hand_brake), + ('Manual:', c.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] + elif isinstance(c, carla.WalkerControl): + self._info_text += [ + ('Speed:', c.speed, 0.0, 5.556), + ('Jump:', c.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) + vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] + for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]): + if d > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (d, vehicle_type)) + + def toggle_info(self): + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """Helper class to handle text output using pygame""" + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.line_space = 18 + self.dim = (780, len(lines) * self.line_space + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * self.line_space)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + + # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor + if parent_actor.type_id.startswith("vehicle."): + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + + +# ============================================================================== +# -- GnssSensor ---------------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + + +# ============================================================================== +# -- IMUSensor ----------------------------------------------------------------- +# ============================================================================== + + +class IMUSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.accelerometer = (0.0, 0.0, 0.0) + self.gyroscope = (0.0, 0.0, 0.0) + self.compass = 0.0 + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.imu') + self.sensor = world.spawn_actor( + bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) + + @staticmethod + def _IMU_callback(weak_self, sensor_data): + self = weak_self() + if not self: + return + limits = (-99.9, 99.9) + self.accelerometer = ( + max(limits[0], min(limits[1], sensor_data.accelerometer.x)), + max(limits[0], min(limits[1], sensor_data.accelerometer.y)), + max(limits[0], min(limits[1], sensor_data.accelerometer.z))) + self.gyroscope = ( + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), + max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) + self.compass = math.degrees(sensor_data.compass) + + +# ============================================================================== +# -- RadarSensor --------------------------------------------------------------- +# ============================================================================== + + +class RadarSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + bound_x = 0.5 + self._parent.bounding_box.extent.x + bound_y = 0.5 + self._parent.bounding_box.extent.y + bound_z = 0.5 + self._parent.bounding_box.extent.z + + self.velocity_range = 7.5 # m/s + world = self._parent.get_world() + self.debug = world.debug + bp = world.get_blueprint_library().find('sensor.other.radar') + bp.set_attribute('horizontal_fov', str(35)) + bp.set_attribute('vertical_fov', str(20)) + self.sensor = world.spawn_actor( + bp, + carla.Transform( + carla.Location(x=bound_x + 0.05, z=bound_z+0.05), + carla.Rotation(pitch=5)), + attach_to=self._parent) + # We need a weak reference to self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) + + @staticmethod + def _Radar_callback(weak_self, radar_data): + self = weak_self() + if not self: + return + # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: + # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) + # points = np.reshape(points, (len(radar_data), 4)) + + current_rot = radar_data.transform.rotation + for detect in radar_data: + azi = math.degrees(detect.azimuth) + alt = math.degrees(detect.altitude) + # The 0.25 adjusts a bit the distance so the dots can + # be properly seen + fw_vec = carla.Vector3D(x=detect.depth - 0.25) + carla.Transform( + carla.Location(), + carla.Rotation( + pitch=current_rot.pitch + alt, + yaw=current_rot.yaw + azi, + roll=current_rot.roll)).transform(fw_vec) + + def clamp(min_v, max_v, value): + return max(min_v, min(value, max_v)) + + norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] + r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) + g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) + b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) + self.debug.draw_point( + radar_data.transform.location + fw_vec, + size=0.075, + life_time=0.06, + persistent_lines=False, + color=carla.Color(r, g, b)) + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + def __init__(self, parent_actor, hud, gamma_correction): + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_x = 0.5 + self._parent.bounding_box.extent.x + bound_y = 0.5 + self._parent.bounding_box.extent.y + bound_z = 0.5 + self._parent.bounding_box.extent.z + Attachment = carla.AttachmentType + + if not self._parent.type_id.startswith("walker.pedestrian"): + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), + (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] + else: + self._camera_transforms = [ + (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), + (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), + (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] + + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], + ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], + ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], + ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', + {'lens_circle_multiplier': '3.0', + 'lens_circle_falloff': '3.0', + 'chromatic_aberration_intensity': '0.5', + 'chromatic_aberration_offset': '0'}], + ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], + ] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + if bp.has_attribute('gamma'): + bp.set_attribute('gamma', str(gamma_correction)) + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + elif item[0].startswith('sensor.lidar'): + self.lidar_range = 50 + + for attr_name, attr_value in item[3].items(): + bp.set_attribute(attr_name, attr_value) + if attr_name == 'range': + self.lidar_range = float(attr_value) + + item.append(bp) + self.index = None + + def toggle_camera(self): + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + index = index % len(self.sensors) + needs_respawn = True if self.index is None else \ + (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1]) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + self.set_sensor(self.index + 1) + + def toggle_recording(self): + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): + # Example of converting the raw_data from a carla.DVSEventArray + # sensor into a NumPy array and using it as an image + dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ + ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) + dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) + # Blue is positive, red is negative + dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 + self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) + elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): + image = image.get_color_coded_flow() + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('_out/%08d' % image.frame) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + original_settings = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(20.0) + + sim_world = client.get_world() + if args.sync: + original_settings = sim_world.get_settings() + settings = sim_world.get_settings() + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + + if args.autopilot and not sim_world.get_settings().synchronous_mode: + print("WARNING: You are currently in asynchronous mode and could " + "experience some issues with the traffic simulation") + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + display.fill((0,0,0)) + pygame.display.flip() + + hud = HUD(args.width, args.height) + world = World(sim_world, hud, args) + controller = KeyboardControl(world, args.autopilot) + + if args.sync: + sim_world.tick() + else: + sim_world.wait_for_tick() + + clock = pygame.time.Clock() + while True: + if args.sync: + sim_world.tick() + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock, args.sync): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if original_settings: + sim_world.apply_settings(original_settings) + + if (world and world.recording_enabled): + client.stop_recorder() + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='actor filter (default: "vehicle.*")') + argparser.add_argument( + '--generation', + metavar='G', + default='2', + help='restrict to certain actor generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--rolename', + metavar='NAME', + default='hero', + help='actor role name (default: "hero")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '--sync', + action='store_true', + help='Activate synchronous mode execution') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py new file mode 100644 index 00000000..120f90b0 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 + +import random + +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False): + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + display = pygame.display.set_mode( + (800, 600), + pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + #start_pose = random.choice(m.get_spawn_points()) + start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: + while True: + if should_quit(): + return + + print('----------------') + print(vehicle.get_transform()) + #vehicle.set_target_velocity(carla.Vector3D(10, 10, 10)) + #vehicle.set_autopilot(True) + #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=1.0, brake=float(0.0))) + #vehicle.apply_control(carla.VehicleControl(0.75)) + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) + + vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) + # Choose the next waypoint and update the car location. + #print(waypoint) + + print(vehicle.get_transform()) + print(vehicle.get_control()) + print('----------------') + #waypoint = waypoint.next(1.5) + #print(waypoint) + #waypoint = random.choice(waypoint.next(1.5)) + #vehicle.set_transform(waypoint.transform) + #vehicle.set_transform(vehicle.get_transform()) + + #image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + pygame.display.flip() + + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py new file mode 100644 index 00000000..ccbbe6d3 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 + +import random + +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False): + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + display = pygame.display.set_mode( + (800, 600), + pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + #start_pose = random.choice(m.get_spawn_points()) + start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: + while True: + if should_quit(): + return + + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) + vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) + + #image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + pygame.display.flip() + + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py new file mode 100644 index 00000000..faa43ac7 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py @@ -0,0 +1,404 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 +import time +import random +import math +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +import cv2 +from albumentations import ( + Compose, Normalize +) +from gradcam import GradCAM + +mean_step_time = [] + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False, location=(0,0), is_black_space=False): + try: + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + except: + if is_black_space: + array = image + else: + array = image + array = cv2.resize(array, (200, 600)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, location) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + #display = pygame.display.set_mode((800*2+200, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + display = pygame.display.set_mode((800+400, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + start_pose = random.choice(m.get_spawn_points()) + + # Town 1 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=280, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=2.0, y=200, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 2 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=-4, y=240, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 3 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) + + # Town 04 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 05 + #start_pose = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + + # Town 06 + #start_pose = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 7 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) + #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) + start_pose = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + + + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + #birdview_producer = BirdViewProducer( + # client, # carla.Client + # target_size=PixelDimensions(width=100, height=300), + # pixels_per_meter=10, + # crop_type=BirdViewCropType.FRONT_AND_REAR_AREA + #) + + birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY + ) + PRETRAINED_MODELS = "../../../" + #model = "20221017-144828_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_cp.h5" + model = "20221019-140458_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_both_directions_cp.h5" + #model = "20221019-155549_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_both_directions_more_cp.h5" + model = "20221021-095950_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_cp.h5" + #model = "20221021-114545_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_extreme_cp.h5" + model = "20221021-140015_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_cp.h5" + model = "20221021-143934_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_cp.h5" + model = "20221021-150901_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_cp.h5" + model = "20221021-154936_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_cp.h5" # BEST MODEL! + + + model = "20221031-093726_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_towns_cp.h5" + model = "20221031-171738_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_towns_3_5_cp.h5" + + model = "20221031-175645_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_towns_3_5_7_cp.h5" + + net = tf.keras.models.load_model(PRETRAINED_MODELS + model) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=10) as sync_mode: + while True: + if should_quit(): + return + start = time.time() + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) + + birdview = birdview_producer.produce( + agent_vehicle=vehicle # carla.Actor (spawned vehicle) + ) + + image = BirdViewProducer.as_rgb(birdview) + image_shape=(50, 150) + img_base = cv2.resize(image, image_shape) + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img_base) + img = image["image"] + + + img = np.expand_dims(img, axis=0) + prediction = net.predict(img) + throttle = prediction[0][0] + steer = prediction[0][1] * (1 - (-1)) + (-1) + break_command = prediction[0][2] + speed = vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + acceleration = vehicle.get_acceleration() + vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) + vehicle_location = vehicle.get_location() + #print(prediction) + + #vehicle.set_autopilot(True) + + ''' + if vehicle_speed > 20: + #print('SLOW DOWN!') + vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=steer, brake=float(1.0))) + else: + if vehicle_speed > 20 and break_command > float(0.01): + #print('1') + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) + else: + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + ''' + if vehicle_speed > 20: + #print('SLOW DOWN!') + vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=steer, brake=float(1.0))) + else: + if vehicle_speed < 5: + vehicle.apply_control(carla.VehicleControl(throttle=float(1.0), steer=steer, brake=float(0.0))) + elif vehicle_speed > 20 and break_command > float(0.01): + #print('1') + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) + else: + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + + + #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) + #print(throttle, steer, break_command) + #print(vehicle.get_control()) + + i = np.argmax(prediction[0]) + cam = GradCAM(net, i) + heatmap = cam.compute_heatmap(img) + heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) + + #print(original_image.shape) + #print(resized_image.shape) + #print(heatmap.shape) + (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) + + image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True, location=(800,0)) + #draw_image(display, img_base, blend=False, location=(1600,0)) + draw_image(display, img_base, blend=False, location=(800,0)) + draw_image(display, output, blend=False, location=(1000,0)) + draw_image(display, np.zeros((160,300, 3)), blend=False, location=(0,0), is_black_space=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + + if vehicle_speed > 20: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 0, 0)), + (22, 46)) + else: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), + (22, 46)) + if mean_step_time != []: + display.blit( + font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), + (22, 64)) + if vehicle_acceleration > 10.0: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), + (22, 82)) + else: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), + (22, 82)) + + display.blit( + font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), + (22, 100)) + + display.blit( + font.render('Position: X=' + str(round(vehicle_location.x, 2)) + ' Y= ' + str(round(vehicle_location.y, 2)), True, (255, 255, 255)), + (22, 118)) + + display.blit( + font.render('World: ' + str(m.name), True, (255, 255, 255)), + (22, 136)) + + pygame.display.flip() + end = time.time() + mean_step_time.append(end - start) + #print(sum(mean_step_time) / len(mean_step_time)) + #print(end - start) + #time.sleep(0.5) + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py new file mode 100644 index 00000000..ec18d73a --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py @@ -0,0 +1,414 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 +import time +import random +import math +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +import cv2 +from albumentations import ( + Compose, Normalize +) +from gradcam import GradCAM + +mean_step_time = [] + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False, location=(0,0), is_black_space=False): + try: + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + except: + if is_black_space: + array = image + else: + array = image + array = cv2.resize(array, (200, 600)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, location) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + #display = pygame.display.set_mode((800*2+200, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + display = pygame.display.set_mode((800+400, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + + # Town 1 + #start_pose = random.choice(m.get_spawn_points()) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=280, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + start_pose = carla.Transform(carla.Location(x=2.0, y=40, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + + # Town 2 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=-4, y=240, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 3 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) + + # Town 4 + # clockwise + #start_pose = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 05 + #start_pose = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + + # Town 06 + #start_pose = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 7 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=10, roll=0)) + + + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + #birdview_producer = BirdViewProducer( + # client, # carla.Client + # target_size=PixelDimensions(width=100, height=300), + # pixels_per_meter=10, + # crop_type=BirdViewCropType.FRONT_AND_REAR_AREA + #) + + birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY + ) + PRETRAINED_MODELS = "../../../" + model = "20221026-133123_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_cp.h5" + model = "20221031-125557_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_cp.h5" + model = "20221031-182622_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_cp.h5" # BEST + #model = "20221104-101558_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_more_cp.h5" + model = "20221104-112038_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_extreme_cp.h5" + model = "20221104-120121_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_only_extreme_cp.h5" + model = "20221104-120359_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_only_clockwise_cp.h5" + model = "20221104-121137_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_only_clockwise_cp.h5" + model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp_3.h5" # BEST! + model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp.h5" # BEST! + #model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp_2.h5" + model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp.h5" # WORKS!!! + model = "20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5" # WORKS! + + #model = "20221104-151236_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_MAX_cp.h5" + + #model = "20221107-111832_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_extreme_vel_MAX_cp.h5" # Works for town01 + + #model = "20221107-143505_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_input_output_velocity_all_towns_and_extreme_vel_MAX_cp.h5" + + #model = "20221108-155441_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_input_output_velocity_town_01_and_extreme_vel_MAX_cp.h5" + model = "20221108-165842_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_prev_velocity_town_01_and_extreme_vel_MAX_cp.h5" + + net = tf.keras.models.load_model(PRETRAINED_MODELS + model) + + + previous_speed = 0 + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=15) as sync_mode: + while True: + if should_quit(): + return + start = time.time() + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) + + birdview = birdview_producer.produce( + agent_vehicle=vehicle # carla.Actor (spawned vehicle) + ) + + image = BirdViewProducer.as_rgb(birdview) + image_shape=(50, 150) + img_base = cv2.resize(image, image_shape) + + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img_base) + img = image["image"] + + #velocity_dim = np.full((150, 50), 0.5) + velocity_dim = np.full((150, 50), previous_speed/30) + #print(velocity_dim) + new_img_vel = np.dstack((img, velocity_dim)) + img = new_img_vel + + img = np.expand_dims(img, axis=0) + print(img.shape) + prediction = net.predict(img) + throttle = prediction[0][0] + steer = prediction[0][1] * (1 - (-1)) + (-1) + break_command = prediction[0][2] + speed = vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + previous_speed = vehicle_speed + acceleration = vehicle.get_acceleration() + vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) + vehicle_location = vehicle.get_location() + print(prediction) + + #vehicle.set_autopilot(True) + + if vehicle_speed > 200: + #print('SLOW DOWN!') + vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=(steer), brake=float(1.0))) + else: + if vehicle_speed < 2: + vehicle.apply_control(carla.VehicleControl(throttle=float(1.0), steer=0.0, brake=float(0.0))) + elif vehicle_speed > 5 and break_command > float(0.001): + print('1') + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + else: + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + + #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) + #print(throttle, steer, break_command) + #print(vehicle.get_control()) + + i = np.argmax(prediction[0]) + cam = GradCAM(net, i) + heatmap = cam.compute_heatmap(img) + heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) + + #print(original_image.shape) + #print(resized_image.shape) + #print(heatmap.shape) + (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) + + image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True, location=(800,0)) + #draw_image(display, img_base, blend=False, location=(1600,0)) + draw_image(display, img_base, blend=False, location=(800,0)) + draw_image(display, output, blend=False, location=(1000,0)) + draw_image(display, np.zeros((160,300, 3)), blend=False, location=(0,0), is_black_space=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + + if vehicle_speed > 30: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/S', True, (255, 0, 0)), + (22, 46)) + else: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), + (22, 46)) + if mean_step_time != []: + display.blit( + font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), + (22, 64)) + if vehicle_acceleration > 10.0: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), + (22, 82)) + else: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), + (22, 82)) + + display.blit( + font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), + (22, 100)) + + display.blit( + font.render('Position: X=' + str(round(vehicle_location.x, 2)) + ' Y= ' + str(round(vehicle_location.y, 2)), True, (255, 255, 255)), + (22, 118)) + + display.blit( + font.render('World: ' + str(m.name), True, (255, 255, 255)), + (22, 136)) + + #display.blit( + # font.render('Predicted previous speed: ' + str(prediction[0][3]*88), True, (255, 255, 255)), + # (22, 150)) + + pygame.display.flip() + end = time.time() + mean_step_time.append(end - start) + #print(sum(mean_step_time) / len(mean_step_time)) + #print(end - start) + #time.sleep(0.5) + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py new file mode 100644 index 00000000..c3620915 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py @@ -0,0 +1,395 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 +import time +import random +import math +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +import cv2 +from albumentations import ( + Compose, Normalize +) +from gradcam import GradCAM + +mean_step_time = [] + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False, location=(0,0), is_black_space=False): + try: + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + except: + if is_black_space: + array = image + else: + array = image + array = cv2.resize(array, (200, 600)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, location) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + #display = pygame.display.set_mode((800*2+200, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + display = pygame.display.set_mode((800+400, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + + # Town 1 + #start_pose = random.choice(m.get_spawn_points()) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=280, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + start_pose = carla.Transform(carla.Location(x=2.0, y=40, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + + # Town 2 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=-4, y=240, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 3 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) + + # Town 4 + # clockwise + #start_pose = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 05 + #start_pose = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + + # Town 06 + #start_pose = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + + # Town 7 + # anticlockwise + #start_pose = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) + # clockwise + #start_pose = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=10, roll=0)) + + + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + #birdview_producer = BirdViewProducer( + # client, # carla.Client + # target_size=PixelDimensions(width=100, height=300), + # pixels_per_meter=10, + # crop_type=BirdViewCropType.FRONT_AND_REAR_AREA + #) + + birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY + ) + PRETRAINED_MODELS = "../../../" + + model = "20221109-111804_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_prev_velocity_and_command_town_01_cp.h5" + + net = tf.keras.models.load_model(PRETRAINED_MODELS + model) + + + previous_speed = 0 + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=15) as sync_mode: + while True: + if should_quit(): + return + start = time.time() + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) + + birdview = birdview_producer.produce( + agent_vehicle=vehicle # carla.Actor (spawned vehicle) + ) + + image = BirdViewProducer.as_rgb(birdview) + image_shape=(50, 150) + img_base = cv2.resize(image, image_shape) + + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img_base) + img = image["image"] + + velocity_dim = np.full((150, 50), previous_speed/30) + # Straight command! + command_dim = np.full((150, 50), 1) + new_img_vel = np.dstack((img, velocity_dim)) + new_img_vel_cmd = np.dstack((new_img_vel, command_dim)) + img = new_img_vel_cmd + + img = np.expand_dims(img, axis=0) + print(img.shape) + prediction = net.predict(img) + throttle = prediction[0][0] + steer = prediction[0][1] * (1 - (-1)) + (-1) + break_command = prediction[0][2] + speed = vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + previous_speed = vehicle_speed + acceleration = vehicle.get_acceleration() + vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) + vehicle_location = vehicle.get_location() + print(prediction) + + #vehicle.set_autopilot(True) + + if vehicle_speed > 200: + #print('SLOW DOWN!') + vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=(steer), brake=float(1.0))) + else: + if vehicle_speed < 2: + vehicle.apply_control(carla.VehicleControl(throttle=float(1.0), steer=0.0, brake=float(0.0))) + elif vehicle_speed > 5 and break_command > float(0.001): + print('1') + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + else: + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + + #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) + #print(throttle, steer, break_command) + #print(vehicle.get_control()) + + i = np.argmax(prediction[0]) + cam = GradCAM(net, i) + heatmap = cam.compute_heatmap(img) + heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) + + #print(original_image.shape) + #print(resized_image.shape) + #print(heatmap.shape) + (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) + + image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True, location=(800,0)) + #draw_image(display, img_base, blend=False, location=(1600,0)) + draw_image(display, img_base, blend=False, location=(800,0)) + draw_image(display, output, blend=False, location=(1000,0)) + draw_image(display, np.zeros((160,300, 3)), blend=False, location=(0,0), is_black_space=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + + if vehicle_speed > 30: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/S', True, (255, 0, 0)), + (22, 46)) + else: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), + (22, 46)) + if mean_step_time != []: + display.blit( + font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), + (22, 64)) + if vehicle_acceleration > 10.0: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), + (22, 82)) + else: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), + (22, 82)) + + display.blit( + font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), + (22, 100)) + + display.blit( + font.render('Position: X=' + str(round(vehicle_location.x, 2)) + ' Y= ' + str(round(vehicle_location.y, 2)), True, (255, 255, 255)), + (22, 118)) + + display.blit( + font.render('World: ' + str(m.name), True, (255, 255, 255)), + (22, 136)) + + #display.blit( + # font.render('Predicted previous speed: ' + str(prediction[0][3]*88), True, (255, 255, 255)), + # (22, 150)) + + pygame.display.flip() + end = time.time() + mean_step_time.append(end - start) + #print(sum(mean_step_time) / len(mean_step_time)) + #print(end - start) + #time.sleep(0.5) + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py new file mode 100644 index 00000000..b9933526 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py @@ -0,0 +1,315 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 +import time +import random +import math +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +import cv2 +from albumentations import ( + Compose, Normalize +) +from gradcam import GradCAM + +mean_step_time = [] + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False, location=(0,0)): + try: + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + except: + array = image + array = cv2.resize(array, (800, 266)) + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, location) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + display = pygame.display.set_mode((800*2, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + start_pose = random.choice(m.get_spawn_points()) + # Town 2 + #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + # Town 1 + #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + start_pose = carla.Transform(carla.Location(x=-2.0, y=285, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + #start_pose = carla.Transform(carla.Location(x=2.0, y=40, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=1.5, z=2.4)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + PRETRAINED_MODELS = "../../../" + model = "20221024-110444_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_no_sync_cp.h5" + model = "20221024-114401_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_cp.h5" + model = "20221024-123807_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_cp.h5" + model = "20221024-132531_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_extreme_cp.h5" #BEST! + #model = "20221026-161920_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_extreme_cp.h5" + model = "20221026-163028_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_extreme_cp.h5" # Weather! + + net = tf.keras.models.load_model(PRETRAINED_MODELS + model) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=5) as sync_mode: + while True: + if should_quit(): + return + start = time.time() + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) + + array = np.frombuffer(image_rgb.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image_rgb.height, image_rgb.width, 4)) + array = array[:,:,:3] + + image = array + image_shape=(150, 50) + image = image[300:600, 0:800] + #image = image[320:600, 0:800] + img_base = cv2.resize(image, image_shape) + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img_base) + img = image["image"] + + img = np.expand_dims(img, axis=0) + prediction = net.predict(img) + throttle = prediction[0][0] + steer = prediction[0][1] * (1 - (-1)) + (-1) + break_command = prediction[0][2] + speed = vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + acceleration = vehicle.get_acceleration() + vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) + print(prediction) + + if vehicle_speed > 20: + print('SLOW DOWN!') + vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=steer, brake=float(1.0))) + else: + if vehicle_speed > 20 and break_command > float(0.01): + #print('1') + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) + elif vehicle_speed < 5: + print('entra') + vehicle.apply_control(carla.VehicleControl(throttle=float(0.5), steer=steer, brake=float(0.0))) + else: + vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + + i = np.argmax(prediction[0]) + cam = GradCAM(net, i) + heatmap = cam.compute_heatmap(img) + heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) + + #print(original_image.shape) + #print(resized_image.shape) + #print(heatmap.shape) + (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) + + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + draw_image(display, img_base, blend=False, location=(800,0)) + draw_image(display, output, blend=False, location=(800,266)) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + + if vehicle_speed > 20: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/S', True, (255, 0, 0)), + (22, 46)) + else: + display.blit( + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), + (22, 46)) + if mean_step_time != []: + display.blit( + font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), + (22, 64)) + if vehicle_acceleration > 10.0: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), + (22, 82)) + else: + display.blit( + font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), + (22, 82)) + + display.blit( + font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), + (22, 100)) + + pygame.display.flip() + end = time.time() + mean_step_time.append(end - start) + #print(sum(mean_step_time) / len(mean_step_time)) + #print(end - start) + #time.sleep(0.5) + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py new file mode 100644 index 00000000..c9d2ea4d --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py @@ -0,0 +1,272 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 + +import random + +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue +import csv +import math +import time + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick(10) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False): + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + display = pygame.display.set_mode( + (800, 600), + pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + f = open('../../carla_dataset_test_24_10_anticlockwise_full_image_2/dataset.csv', 'a') + # create the csv writer + writer = csv.writer(f) + try: + df = pandas.read_csv('../../carla_dataset_test_24_10_anticlockwise_full_image_2/dataset.csv') + batch_number = int(df.iloc[-1]['batch']) + 1 + except Exception as ex: + #fkjfbdjkhfsd + batch_number = 0 + header = ['batch', 'image_id', 'timestamp', 'throttle', 'steer', 'brake', 'location_x', 'location_y'] + writer.writerow(header) + + print(batch_number) + frame_number = 0 + start = time.time() + + + try: + m = world.get_map() + #start_pose = random.choice(m.get_spawn_points()) + start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=0.1), carla.Rotation(pitch=0, yaw=90, roll=0)) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + blueprint = world.get_blueprint_library().filter('vehicle')[0] + + + vehicle = world.spawn_actor( + blueprint, + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(True) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=1.5, z=2.4)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + # Remove traffic lights and traffic limits + traffic_lights = world.get_actors().filter('traffic.traffic_light') + traffic_speed_limits = world.get_actors().filter('traffic.speed_limit*') + print(traffic_speed_limits) + for traffic_light in traffic_lights: + #success = traffic_light.destroy() + traffic_light.set_green_time(20000) + traffic_light.set_state(carla.TrafficLightState.Green) + #print(success) + + for speed_limit in traffic_speed_limits: + success = speed_limit.destroy() + print(success) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, fps=30) as sync_mode: + while True: + if should_quit(): + return + + #print('----------------') + #print(vehicle.get_transform()) + traffic_manager = client.get_trafficmanager() + vehicle.set_autopilot(True) + route = ["Straight", "Straight", "Straight", "Straight", "Straight", + "Straight", "Straight", "Straight", "Straight", "Straight", + "Straight", "Straight", "Straight", "Straight", "Straight", + "Straight", "Straight", "Straight", "Straight", "Straight", + "Straight", "Straight", "Straight", "Straight", "Straight", + "Straight", "Straight", "Straight", "Straight", "Straight"] + traffic_manager.set_route(vehicle, route) + clock.tick() + # Advance the simulation and wait for the data. + snapshot, image_rgb = sync_mode.tick(timeout=10.0) + + #print(vehicle.get_transform()) + #print(vehicle.get_control()) + #print('----------------') + + ############################################################ + vehicle_control = vehicle.get_control() + vehicle_location = vehicle.get_location() + speed = vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + + end = time.time() + + if (vehicle_speed > 0.0 and vehicle.get_location().z < 0.01 and (end - start) > 6): + print('entra') + #print(vehicle_speed) + #print(vehicle.get_transform()) + #image = image_queue.get() + image = image_rgb + cc = carla.ColorConverter.Raw + image.save_to_disk('../../carla_dataset_test_24_10_anticlockwise_full_image_2/' + str(batch_number) + '_' + str(frame_number) + '.png', cc) + # write a row to the csv file + row = [batch_number, str(batch_number) + '_' + str(frame_number) + '.png', time.time(), vehicle_control.throttle, vehicle_control.steer, vehicle_control.brake, vehicle_location.x, vehicle_location.y] + writer.writerow(row) + frame_number += 1 + #print(vehicle_control) + else: + #pass + print('nop') + ############################################################3 + + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + pygame.display.flip() + + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py b/PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py new file mode 100644 index 00000000..b9bd5564 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py @@ -0,0 +1,171 @@ +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import pandas +import tensorflow as tf +import numpy as np + +from albumentations import ( + Compose, Normalize +) + + +#import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + +birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=150, height=300), + pixels_per_meter=4, + crop_type=BirdViewCropType.FRONT_AND_REAR_AREA +) + +birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AND_REAR_AREA +) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + +PRETRAINED_MODELS = "../../../" +#model = "20220920-175541_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_cp.h5" +#model = "20220921-130758_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_only_extreme.h5" +#model = "20220921-154633_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_only_extreme_cp.h5" +#model = "20220921-173038_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_only_extreme_only_extreme_cp.h5" +#model = "20220928-144619_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_random_start_point.h5" +#model = "20220928-162449_pilotnet_CARLA_extreme_cases_28_09_dataset_bird_eye_random_start_point_300_epochs_cp.h5" +#model = "20220929-164843_pilotnet_CARLA_extreme_cases_29_09_dataset_bird_eye_random_start_point_retrained_cp.h5" +#model = "20220930-105720_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_cp.h5" +#model = "20220930-130349_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_retrained.h5" +#model = "20220930-153914_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_cp.h5" +#model = "20221003-131905_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_cp.h5" +''' +model = "20221003-160817_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_cp.h5" +model = "20221004-180428_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_data_cp.h5" +model = "20221005-091607_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_data_cp.h5" +model_w = "20221005-105821_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_more_data_cp.h5" +model = "20221005-120932_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_2_output_more_data_cp.h5" + +model_v = "20221005-150027_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_V_more_data_cp.h5" + +model = "20221005-184041_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_more_data_cp.h5" + +model = "20221007-175421_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_more_data_extreme_cases_cp.h5" +''' +model = "20221007-182407_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_more_data_extreme_cases_cp.h5" +#model = "20221010-090523_xception_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_outputs_more_more_data_cp.h5" + +model = "20221017-110327_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_cp.h5" +model = "20221017-111655_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_cp.h5" +model = "20221017-113220_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_cp.h5" +model = "20221017-134410_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_extreme_cases_cp.h5" +model = "20221017-144828_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_cp.h5" +model = "20221021-154936_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_cp.h5" + + +#model_b = "20221010-102610_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_B_more_more_data_extreme_cases_cp.h5" +print('***********************************************************************************************') +#net_v = tf.keras.models.load_model(PRETRAINED_MODELS + model_v) +#net = tf.keras.models.load_model(PRETRAINED_MODELS + model_w) +#net_b = tf.keras.models.load_model(PRETRAINED_MODELS + model_b) + + +net = tf.keras.models.load_model(PRETRAINED_MODELS + model) +print('***********************************************************************************************') + +try: + while True: + #start = time.time() + + # Input for your model - call it every simulation step + # returned result is np.ndarray with ones and zeros of shape (8, height, width) + birdview = birdview_producer.produce( + agent_vehicle=car # carla.Actor (spawned vehicle) + ) + + image = BirdViewProducer.as_rgb(birdview) + #image_shape=(66, 200) + #image_shape=(200, 66) + image_shape=(50, 150) + #image_shape=(71, 150) + img = cv2.resize(image, image_shape) + + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img) + img = image["image"] + + + img = np.expand_dims(img, axis=0) + prediction = net.predict(img) + #print(prediction) + #prediction_v = net_v.predict(img) + #prediction_b = net_b.predict(img) + #print(prediction_v, prediction) + ''' + prediction_w = prediction[0][1] * (1 - (-1)) + (-1) + throttle = prediction[0][0] + steer = prediction_w + print(throttle, steer) + ''' + prediction_w = prediction[0][1] * (1 - (-1)) + (-1) + throttle = prediction[0][0] + break_command = prediction[0][2] + #if throttle < 0.3: + # throttle = 0.5 + #break_command = prediction[0][2] + steer = prediction_w + #Sprint(throttle, steer, break_command) + #print(throttle, steer) + #print(car.get_velocity().steer) + vehicle_control = car.get_control() + #print(vehicle_control.steer) + speed = car.get_velocity() + + #print(speed) + + if (abs(speed.x) > 6 or abs(speed.y) > 6): + #print('SLOW DOWN!') + car.apply_control(carla.VehicleControl(brake=float(1.0))) + else: + if (speed.x > 0.01 or speed.y > 0.01) and break_command > float(0.1): + #print('1') + car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) + else: + car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + + #car.apply_control(carla.VehicleControl(throttle=0.5, steer=steer, brake=float(0.0))) + #print(steer) + #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) + #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer)) + #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) + ##car.apply_control(carla.VehicleControl(throttle=1.0, steer=steer, brake=float(0.01))) + + #end = time.time() + #print(end) + #print(end - start) +except KeyboardInterrupt: + pass + +# close the file +f.close() + + diff --git a/PlayingWithCARLA/carla_0_9_13/record_dataset.py b/PlayingWithCARLA/carla_0_9_13/record_dataset.py new file mode 100644 index 00000000..e95031b0 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/record_dataset.py @@ -0,0 +1,92 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import pandas +import math + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +#camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +#image_queue = queue.Queue() +#camera.listen(image_queue.put) +last_image = '' +def listener(image): + global last_image + last_image = image +camera.listen(listener) + +#rgb camera +#image = image_queue.get() +# open the file in the write mode +f = open('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/dataset.csv', 'a') + +# create the csv writer +writer = csv.writer(f) + +#mypath = '../carla_dataset_14_09/' +#onlyfiles = [f for f in listdir(mypath) if isfile(join(mypath, f))] +#onlyfiles = sorted(onlyfiles) +#print(onlyfiles) +#print(onlyfiles[len(onlyfiles)-2]) +#print(onlyfiles[len(onlyfiles)-2][0]) +#print(type(onlyfiles[len(onlyfiles)-2][0])) + +try: + df = pandas.read_csv('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/dataset.csv') + batch_number = int(df.iloc[-1]['batch']) + 1 +except Exception as ex: + #ddddd + batch_number = 0 + header = ['batch', 'image_id', 'timestamp', 'throttle', 'steer', 'brake', 'location_x', 'location_y'] + writer.writerow(header) + +print(batch_number) +frame_number = 0 +try: + while True: + vehicle_control = car.get_control() + vehicle_location = car.get_location() + speed = car.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + ''' + print(type(image_queue)) + print(image_queue) + print(image_queue.qsize()) + ''' + if (vehicle_speed > 0.0 and last_image != ''): + #image = image_queue.get() + image = last_image + cc_raw = carla.ColorConverter.Raw + cc_seg = carla.ColorConverter.CityScapesPalette + image.save_to_disk('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/raw/' + str(batch_number) + '_' + str(frame_number) + '.png', cc_raw) + image.save_to_disk('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/mask/' + str(batch_number) + '_' + str(frame_number) + '.png', cc_seg) + # write a row to the csv file + row = [batch_number, str(batch_number) + '_' + str(frame_number) + '.png', time.time(), vehicle_control.throttle, vehicle_control.steer, vehicle_control.brake, vehicle_location.x, vehicle_location.y] + writer.writerow(row) + frame_number += 1 + print(vehicle_control) + else: + pass + #print('nop') +except KeyboardInterrupt: + pass + +# close the file +f.close() + + diff --git a/PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py b/PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py new file mode 100644 index 00000000..99e760dd --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py @@ -0,0 +1,157 @@ +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import pandas +import time +import math + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + + +''' +birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AND_REAR_AREA +) +''' +birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY +) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + + + +# open the file in the write mode +f = open('../../carla_dataset_control/carla_dataset_test_09_11_anticlockwise_town_01/dataset.csv', 'a') +# create the csv writer +writer = csv.writer(f) + +try: + df = pandas.read_csv('../../carla_dataset_control/carla_dataset_test_09_11_anticlockwise_town_01/dataset.csv') + batch_number = int(df.iloc[-1]['batch']) + 1 +except Exception as ex: + #fkjfbdjkhfsd + batch_number = 0 + header = ['batch', 'image_id', 'timestamp', 'throttle', 'steer', 'brake', 'location_x', 'location_y', 'previous_velocity', 'current_velocity', 'control_command'] + writer.writerow(header) + +print(batch_number) + +''' +car.apply_control(carla.VehicleControl(throttle=float(1.0), steer=0.0, brake=float(0.0))) +time.sleep(2) +car.apply_control(carla.VehicleControl(throttle=float(0.0), steer=0.0, brake=float(1.0))) +time.sleep(5) +car.apply_control(carla.VehicleControl(throttle=float(0.5), steer=0.1, brake=float(0.0))) +time.sleep(3) +car.set_autopilot(True) +''' +frame_number = 0 +previous_speed = 0 +#previous_image = 0 + +route = ["Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight"] +''' +route = ["Left", "Straight", "Right", "Right", "Straight", "Left", "Left", +"Right", "Right", "Left", "Straight", "Left"] +''' + +iterator = 0 +previous_waypoint_no_junction = True + +#import numpy as np +try: + while world.wait_for_tick(): + vehicle_control = car.get_control() + vehicle_location = car.get_location() + #print(car.get_location()) + #print(vehicle_control.steer) + + #if (vehicle_control.throttle > 0.1 and vehicle_control.throttle < 0.8 ) or (vehicle_control.brake > 0.2): + #if (vehicle_control.steer < -0.25 or vehicle_control.steer > 0.25): + if (vehicle_control.throttle > 0.0 or vehicle_control.steer > 0.0 or vehicle_control.brake > 0.0): + #print(frame_number) + #print(frame_number) + # Input for your model - call it every simulation step + # returned result is np.ndarray with ones and zeros of shape (8, height, width) + birdview = birdview_producer.produce( + agent_vehicle=car # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + #if np.array_equal(previous_image, image): + # print(True) + #previous_image = image + #print(image.shape) + #print(type(image)) + cv2.imwrite('../../carla_dataset_control/carla_dataset_test_09_11_anticlockwise_town_01/' + str(batch_number) + '_' + str(frame_number) + '.png', image) + #image.save_to_disk('../../carla_dataset_21_09/' + str(batch_number) + '_' + str(frame_number) + '.png', cc) + #vehicle_control = car.get_vehicle_control() + #vehicle_control = car.get_control() + + + #print(vehicle_control) + + + # write a row to the csv file + speed = car.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + + waypoint = world.get_map().get_waypoint(car.get_location(),project_to_road=True, lane_type=(carla.LaneType.Driving | carla.LaneType.Shoulder | carla.LaneType.Sidewalk)) + if waypoint.is_junction and previous_waypoint_no_junction: + # first point in junction + previous_waypoint_no_junction = False + #print('---------------------') + #print('ENTRAMOS EN JUNCTION') + #print(iterator) + #print('Accion -> ', route[iterator]) + #print() + elif not waypoint.is_junction and not previous_waypoint_no_junction: + # last point in junction + previous_waypoint_no_junction = True + iterator += 1 + #print('SALIMOS DE JUNCTION') + #print(iterator) + #print('Siguiente junction -> ', route[iterator]) + #print('---------------------') + #print() + #print('Siguiente junction -> ', route[iterator]) + row = [batch_number, str(batch_number) + '_' + str(frame_number) + '.png', time.time(), vehicle_control.throttle, vehicle_control.steer, vehicle_control.brake, vehicle_location.x, vehicle_location.y, previous_speed, vehicle_speed, route[iterator]] + writer.writerow(row) + previous_speed = vehicle_speed + #print('----') + #print(previous_speed) + #print('----') + frame_number += 1 + else: + pass + #print('nop') + +except KeyboardInterrupt: + pass + +# close the file +f.close() + + diff --git a/PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py b/PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py new file mode 100644 index 00000000..3d6cd937 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py @@ -0,0 +1,163 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +print(carla.__file__) + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) +time.sleep(2) + +traffic_lights = world.get_actors().filter('traffic.traffic_light') +traffic_speed_limits = world.get_actors().filter('traffic.speed_limit*') +print(traffic_speed_limits) +for traffic_light in traffic_lights: + #success = traffic_light.destroy() + traffic_light.set_green_time(20000) + traffic_light.set_state(carla.TrafficLightState.Green) + #print(success) + +for speed_limit in traffic_speed_limits: + success = speed_limit.destroy() + print(success) + +print(world.get_actors().filter('vehicle.*')) +car = world.get_actors().filter('vehicle.*')[0] + + +#settings = world.get_settings() +#settings.synchronous_mode = True # Enables synchronous mode +#world.apply_settings(settings) +traffic_manager = client.get_trafficmanager() +#random.seed(0) +#car.set_autopilot(True) +car.set_autopilot(True) + +''' +# ROUTE 0 +route = ["Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight"] + +# ROUTE 1 +route = ["Left", "Straight", "Right", "Right", "Straight", "Left", "Left", +"Right", "Right", "Left", "Straight", "Left"] +''' +# ROUTE 2 +route = ["Left", "Right", "Straight", "Right", "Right", "Straight", "Straight", "Right", "Left", "Left", "Right", "Right"] + +traffic_manager.set_route(car, route) +iterator = 0 +previous_waypoint_no_junction = True + +while True: + #traffic_manager.set_route(car, route) + #print(traffic_manager.get_next_action(car)) + #print(traffic_manager.get_all_actions(car)) + + waypoint = world.get_map().get_waypoint(car.get_location(),project_to_road=True, lane_type=(carla.LaneType.Driving | carla.LaneType.Shoulder | carla.LaneType.Sidewalk)) + #print(waypoint.is_junction) + if waypoint.is_junction and previous_waypoint_no_junction: + # first point in junction + previous_waypoint_no_junction = False + print('---------------------') + print('ENTRAMOS EN JUNCTION') + print(iterator) + print('Accion -> ', route[iterator]) + print() + elif not waypoint.is_junction and not previous_waypoint_no_junction: + # last point in junction + previous_waypoint_no_junction = True + iterator += 1 + print('SALIMOS DE JUNCTION') + print(iterator) + print('Siguiente junction -> ', route[iterator]) + print('---------------------') + print() + + #print(iterator) + +''' + print("Current lane type: " + str(waypoint.lane_type)) + # Check current lane change allowed + print("Current Lane change: " + str(waypoint.lane_change)) + # Left and Right lane markings + print("L lane marking type: " + str(waypoint.left_lane_marking.type)) + print("L lane marking change: " + str(waypoint.left_lane_marking.lane_change)) + print("R lane marking type: " + str(waypoint.right_lane_marking.type)) + print("R lane marking change: " + str(waypoint.right_lane_marking.lane_change)) + #location = car.get_location() + #print(type(location)) + #print(location.is_junction()) +''' + +''' +import math +while True: + speed = car.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + if (abs(vehicle_speed) > 5): + print('SLOW DOWN!') + car.apply_control(carla.VehicleControl(throttle=float(0), steer=float(0), brake=float(1.0))) + + +''' +''' +# Set up the TM in synchronous mode +traffic_manager = client.get_trafficmanager() +#traffic_manager.set_synchronous_mode(True) + +# Set a seed so behaviour can be repeated if necessary +#traffic_manager.set_random_device_seed(0) +#random.seed(0) +#car.set_autopilot(True) +car.set_autopilot(True) +route = ["Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight"] +traffic_manager.set_route(car, route) +#time.sleep(3) +#car.set_autopilot(False) +#print('autopilot false!') +''' +''' +car.apply_control(carla.VehicleControl(throttle=float(0.75), steer=-0.1, brake=float(0.0))) +time.sleep(3) +car.set_autopilot(True) +#traffic_manager.set_route(car, route) +''' + + +''' +while True: + print('entra') + world.tick() + + #traffic_manager.update_vehicle_lights(car, True) + #traffic_manager.random_left_lanechange_percentage(car, 0) + #traffic_manager.random_right_lanechange_percentage(car, 0) + #traffic_manager.auto_lane_change(car, False) + + world_map =world.get_map() + spawn_points = world_map.get_spawn_points() + + # Create route 1 from the chosen spawn points + route_1_indices = [129, 28, 124, 33, 97, 119, 58, 154, 147] + route_1 = [] + for ind in route_1_indices: + route_1.append(spawn_points[ind].location) + print(route_1) + traffic_manager.set_path(car, route_1) +''' + + diff --git a/PlayingWithCARLA/carla_0_9_13/review_images.py b/PlayingWithCARLA/carla_0_9_13/review_images.py new file mode 100644 index 00000000..903555a2 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/review_images.py @@ -0,0 +1,23 @@ +import glob +import os +import cv2 + +DIR_carla_dataset_name_images = '../../carla_dataset_28_09/' +carla_dataset_images = glob.glob(DIR_carla_dataset_name_images + '*') +array_imgs = [] +#print(carla_dataset_images) + +previous_image = 0 + +for iterator, filename in enumerate(carla_dataset_images): + try: + img = cv2.imread(filename) + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + #print((previous_image==img).all()) + if (previous_image==img).all() == True: + print(iterator) + previous_image = img + array_imgs.append(img) + except: + print('error') + diff --git a/PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py b/PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py new file mode 100644 index 00000000..1f02c947 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/dist/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 + +import random + +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + +try: + import queue +except ImportError: + import Queue as queue + + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get('fps', 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings(carla.WorldSettings( + no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds)) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + self.frame = self.world.tick() + print(self.frame) + #print(self.frame.shape) + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +def draw_image(surface, image, blend=False): + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def get_font(): + fonts = [x for x in pygame.font.get_fonts()] + default_font = 'ubuntumono' + font = default_font if default_font in fonts else fonts[0] + font = pygame.font.match_font(font) + return pygame.font.Font(font, 14) + + +def should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + elif event.type == pygame.KEYUP: + if event.key == pygame.K_ESCAPE: + return True + return False + + +def main(): + actor_list = [] + pygame.init() + + display = pygame.display.set_mode( + (800, 600), + pygame.HWSURFACE | pygame.DOUBLEBUF) + font = get_font() + clock = pygame.time.Clock() + + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + try: + m = world.get_map() + start_pose = random.choice(m.get_spawn_points()) + start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=0), carla.Rotation(pitch=0, yaw=90, roll=0)) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + vehicle = world.spawn_actor( + world.get_blueprint_library().filter('vehicle')[0], + start_pose) + actor_list.append(vehicle) + vehicle.set_simulate_physics(False) + + camera_rgb = world.spawn_actor( + blueprint_library.find('sensor.camera.rgb'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find('sensor.camera.semantic_segmentation'), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle) + actor_list.append(camera_semseg) + + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: + while True: + if should_quit(): + return + clock.tick() + + # Advance the simulation and wait for the data. + #vehicle.set_autopilot(True) + vehicle.apply_control(carla.VehicleControl(throttle=10, steer=0.0)) + print(vehicle.get_velocity()) + world.tick() + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=2.0) + + # Choose the next waypoint and update the car location. + waypoint = random.choice(waypoint.next(1.5)) + vehicle.set_transform(waypoint.transform) + + image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + #draw_image(display, image_semseg, blend=True) + display.blit( + font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), + (8, 10)) + display.blit( + font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), + (8, 28)) + pygame.display.flip() + + finally: + + print('destroying actors.') + for actor in actor_list: + actor.destroy() + + pygame.quit() + print('done.') + + +if __name__ == '__main__': + + try: + + main() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/test.py b/PlayingWithCARLA/carla_0_9_13/test.py new file mode 100644 index 00000000..c55e37ea --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/test.py @@ -0,0 +1,174 @@ + +import carla +import numpy as np +from carla.agents.navigation.basic_agent import BasicAgent + + +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') +try: + import queue +except ImportError: + import Queue as queue + + + +def draw_image(surface, image, blend=False): + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if blend: + image_surface.set_alpha(100) + surface.blit(image_surface, (0, 0)) + + +def main(): + # all the actors in the world. For destroying later. + actor_list = [] + pygame.init() + + # create client + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + # access world from client + world = client.get_world() + + # Enable synchronous mode + print('Enabling synchronous mode') + settings = world.get_settings() + settings.synchronous_mode = True + world.apply_settings(settings) + + try: + # set weather conditions + world.set_weather(carla.WeatherParameters.ClearNoon) + + # define location + map = world.get_map() + spawn_points = map.get_spawn_points() + hero_transform = spawn_points[97] + + # Get the van to spawn in front of the hero + # Get the waypoint of the hero, since the spawn points are only Transforms + hero_waypoint = map.get_waypoint(hero_transform.location) + + # Get the waypoint 15 meters in front of it + van_waypoint = hero_waypoint.next(15.0) + van_transform = van_waypoint[0].transform + + # spawn higher or it will get stuck + van_transform.location.z += 0.5 + + # get all the blueprints in this world + blueprint_library = world.get_blueprint_library() + # define the blueprint of hero vehicle + prius_bp = blueprint_library.find('vehicle.toyota.prius') + white = '255,255,255' + prius_bp.set_attribute('color', white) + + # blueprint colavan + colavan_bp = blueprint_library.find('vehicle.carlamotors.carlacola') + + # spawn our hero + hero = world.spawn_actor(prius_bp, hero_transform) + # add actor to the list for destruction, otherwise vehicle is stuck in there forever + actor_list.append(hero) + + # spawn van + colavan = world.spawn_actor(colavan_bp, van_transform) + actor_list.append(colavan) + + # add a camera + camera_class = blueprint_library.find('sensor.camera.rgb') + camera_class.set_attribute('image_size_x', '600') + camera_class.set_attribute('image_size_y', '600') + camera_class.set_attribute('fov', '90') + camera_class.set_attribute('sensor_tick', '0.1') + cam_transform1 = carla.Transform(carla.Location(x=1.8, z=1.3)) + # cam_transform2 = cam_transform1 + carla.Location(y=0.54) + + # # spawn camera to hero + camera1 = world.spawn_actor(camera_class, cam_transform1, attach_to=hero) + actor_list.append(camera1) + # camera2 = world.spawn_actor(camera_class, cam_transform2, attach_to=hero) + # actor_list.append(camera2) + + # Makes a sync queue for the sensor data + image_queue1 = queue.Queue() + camera1.listen(image_queue1.put) + frame = None + + # image_queue2 = queue.Queue() + # camera2.listen(image_queue2.put) + + # Note its going to drive at consistent speed 15 km/h + # PID controller gets unstable after 15 km/h + #roaming_prius = BasicAgent(hero, target_speed=15) + #destiny = spawn_points[96].location + #roaming_prius.set_destination((destiny.x, destiny.y, destiny.z)) + + roaming_van = BasicAgent(colavan, target_speed=15) + #roaming_van.set_destination((destiny.x, destiny.y, destiny.z)) + + # If you want the hero to drive around in autopilot + hero.set_autopilot(True) + + display = pygame.display.set_mode((600, 600), pygame.HWSURFACE | pygame.DOUBLEBUF) + + # tracks time and frame rate management class. + clock = pygame.time.Clock() + + while True: + clock.tick() + world.tick() + ts = world.tick() + + # Get control commands + #control_hero = roaming_prius.run_step() + #hero.apply_control(control_hero) + + #control_van = roaming_van.run_step() + #colavan.apply_control(control_van) + + if frame is not None: + if ts != frame + 1: + print('frame skip!') + print("frame skip!") + print(ts) + frame = ts + + while True: + image1 = image_queue1.get() + print(image1) + # as long as the image number == frame count you are fine and this loop is not necessary + print("image1.frame_number: {} % ts: {}".format(image1.frame_number, ts)) + if image1.frame_number == ts: + break + print( + 'wrong image time-stampstamp: frame=%d, image.frame=%d', + ts, + image1.frame_number) + + draw_image(display, image1) + + # reset display + pygame.display.flip() + + finally: + print('Disabling synchronous mode') + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + + print('destroying actors') + for actor in actor_list: + actor.destroy() + pygame.quit() + print("pygame quit, done") + +main() \ No newline at end of file diff --git a/PlayingWithCARLA/carla_agent.py b/PlayingWithCARLA/carla_agent.py new file mode 100644 index 00000000..08352814 --- /dev/null +++ b/PlayingWithCARLA/carla_agent.py @@ -0,0 +1,12 @@ +from carla.agent.agent import Agent +from carla.client import VehicleControl + +class ForwardAgent(Agent): + + def run_step(self, measurements, sensor_data, directions, target): + """ + Function to run a control step in the CARLA vehicle. + """ + control = VehicleControl() + control.throttle = 0.9 + return control diff --git a/PlayingWithCARLA/carla_tutorial.py b/PlayingWithCARLA/carla_tutorial.py new file mode 100644 index 00000000..534905bc --- /dev/null +++ b/PlayingWithCARLA/carla_tutorial.py @@ -0,0 +1,55 @@ +import glob +import os +import sys + +try: + sys.path.append(glob.glob('PythonAPI/carla/dist/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 +import random +import cv2 +import skimage.measure as measure + +#in synchronous mode, sensor data must be added to a queue +import queue + +client = carla.Client('localhost', 2000) +client.set_timeout(11.0) + +#print(client.get_available_maps()) +#world = client.load_world('Town03') +#settings = world.get_settings() +#settings.fixed_delta_seconds = 0.05 #must be less than 0.1, or else physics will be noisy +#must use fixed delta seconds and synchronous mode for python api controlled sim, or else +#camera and sensor data may not match simulation properly and will be noisy +#settings.synchronous_mode = True +#world.apply_settings(settings) + +actor_list = [] + +blueprint_library = client.get_world().get_blueprint_library() +bp = random.choice(blueprint_library.filter('vehicle')) # lets choose a vehicle at random + +# lets choose a random spawn point +transform = random.choice(client.get_world().get_map().get_spawn_points()) + +#spawn a vehicle +vehicle = client.get_world().try_spawn_actor(bp, transform) +actor_list.append(vehicle) + +vehicle.set_autopilot(True) + + + +print() + + + + + + diff --git a/PlayingWithCARLA/gradcam.py b/PlayingWithCARLA/gradcam.py new file mode 100644 index 00000000..55dffa10 --- /dev/null +++ b/PlayingWithCARLA/gradcam.py @@ -0,0 +1,95 @@ +from tensorflow.keras.models import Model +from tensorflow.keras.layers import Conv2D +import tensorflow as tf +import numpy as np +import cv2 + + +class GradCAM: + def __init__(self, model, class_idx, layer_name=None): + # store the model, the class index used to measure the class + # activation map, and the layer to be used when visualizing + # the class activation map + self.model = model + self.class_idx = class_idx + self.layerName = layer_name + # if the layer name is None, attempt to automatically find + # the target output layer + if self.layerName is None: + self.layerName = self.find_target_layer() + + def find_target_layer(self): + # attempt to find the final convolutional layer in the network + # by looping over the layers of the network in reverse order + for layer in reversed(self.model.layers): + # check to see if the layer has a 4D output + #if i > 6 and len(layer.output_shape) == 4: + if len(layer.output_shape) == 4 and type(layer) == Conv2D: + print(layer.name) + return layer.name + # otherwise, we could not find a 4D layer so the GradCAM + # algorithm cannot be applied + raise ValueError("Could not find 4D layer. Cannot apply GradCAM.") + + def compute_heatmap(self, image, eps=1e-8): + # construct our gradient model by supplying (1) the inputs + # to our pre-trained model, (2) the output of the (presumably) + # final 4D layer in the network, and (3) the output of the + # softmax activations from the model + grad_model = Model( + inputs=[self.model.inputs], + outputs=[self.model.get_layer(self.layerName).output, + self.model.output]) + + # record operations for automatic differentiation + with tf.GradientTape() as tape: + # cast the image tensor to a float-32 data type, pass the + # image through the gradient model, and grab the loss + # associated with the specific class index + inputs = tf.cast(image, tf.float32) + (conv_outputs, predictions) = grad_model(inputs) + #loss = predictions[:, self.class_idx] + loss = predictions[:, 1] + # use automatic differentiation to compute the gradients + grads = tape.gradient(loss, conv_outputs) + + # compute the guided gradients + cast_conv_outputs = tf.cast(conv_outputs > 0, "float32") + cast_grads = tf.cast(grads > 0, "float32") + guided_grads = cast_conv_outputs * cast_grads * grads + # the convolution and guided gradients have a batch dimension + # (which we don't need) so let's grab the volume itself and + # discard the batch + conv_outputs = conv_outputs[0] + guided_grads = guided_grads[0] + + # compute the average of the gradient values, and using them + # as weights, compute the ponderation of the filters with + # respect to the weights + weights = tf.reduce_mean(guided_grads, axis=(0, 1)) + cam = tf.reduce_sum(tf.multiply(weights, conv_outputs), axis=-1) + + # grab the spatial dimensions of the input image and resize + # the output class activation map to match the input image + # dimensions + (w, h) = (image.shape[2], image.shape[1]) + heatmap = cv2.resize(cam.numpy(), (w, h)) + # normalize the heatmap such that all values lie in the range + # [0, 1], scale the resulting values to the range [0, 255], + # and then convert to an unsigned 8-bit integer + numer = heatmap - np.min(heatmap) + denom = (heatmap.max() - heatmap.min()) + eps + heatmap = numer / denom + heatmap = (heatmap * 255).astype("uint8") + # return the resulting heatmap to the calling function + return heatmap + + def overlay_heatmap(self, heatmap, image, alpha=0.5, + colormap=cv2.COLORMAP_VIRIDIS): + # apply the supplied color map to the heatmap and then + # overlay the heatmap on the input image + heatmap = cv2.applyColorMap(heatmap, colormap) + output = cv2.addWeighted(image, alpha, heatmap, 1 - alpha, 0) + # return a 2-tuple of the color mapped heatmap and the output, + # overlaid image + return heatmap, output \ No newline at end of file diff --git a/PlayingWithCARLA/launch_carla.py b/PlayingWithCARLA/launch_carla.py new file mode 100644 index 00000000..ef3dc9a8 --- /dev/null +++ b/PlayingWithCARLA/launch_carla.py @@ -0,0 +1,653 @@ +#!/usr/bin/env python + +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + AD : steer + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + + TAB : change sensor position + ` : next sensor + [1-9] : change to sensor [1-9] + C : change weather (Shift+C reverse) + Backspace : change vehicle + + R : toggle recording images to disk + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import logging +import math +import random +import re +import weakref + +try: + import pygame +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate-1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud): + self.world = carla_world + self.hud = hud + 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): + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager._index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager._transform_index if self.camera_manager is not None else 0 + # Get a random vehicle blueprint. + print('*****************************************************') + print(self.world.get_blueprint_library().filter('vehicle')) + print('*****************************************************') + print(self.world.get_blueprint_library().filter('vehicle')[0]) + print('*****************************************************') + blueprint = random.choice(self.world.get_blueprint_library().filter('vehicle')) + + blueprint = self.world.get_blueprint_library().filter('vehicle')[0] + + 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() + self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) + while self.vehicle is None: + print('************************ENTRA************************') + print(self.world.get_map().get_spawn_points()) + print('*****************************************************') + spawn_points = self.world.get_map().get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + print('*****************************************************') + spawn_point = self.world.get_map().get_spawn_points()[0] + print(spawn_point) + spawn_point = carla.Transform(carla.Location(x=193.6, y=181.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=181.3, z=1.37), carla.Rotation(pitch=0, yaw=-120, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=181.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) + + # position 2 + #spawn_point = carla.Transform(carla.Location(x=193.6, y=170.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=170.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) + + + # position 3 + #spawn_point = carla.Transform(carla.Location(x=193.6, y=150.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=150.3, z=1.37), carla.Rotation(pitch=0, yaw=-30, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=150.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) + + + # position 4 + #spawn_point = carla.Transform(carla.Location(x=193.6, y=130.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=130.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) + #spawn_point = carla.Transform(carla.Location(x=193.6, y=130.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) + + print(spawn_point) + print('*****************************************************') + self.vehicle = 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.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.vehicle) + self.hud.notification(actor_type) + + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + #self.hud.render(display) + + def destroy(self): + 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() + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + 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._show_info = False + 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, + 'Client: % 16d FPS' % clock.get_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) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + #self._notifications.render(display) + #self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._history = [] + self._parent = parent_actor + self._hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self._history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self._hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self._history.append((event.frame_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 + 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): + self.sensor = None + self._surface = None + self._parent = parent_actor + self._hud = hud + self._recording = False + self._camera_transforms = [ + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + carla.Transform(carla.Location(x=1.6, z=1.7))] + self._transform_index = 1 + self._sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self._sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + elif item[0].startswith('sensor.lidar'): + bp.set_attribute('range', '5000') + item.append(bp) + self._index = None + + def toggle_camera(self): + self._transform_index = (self._transform_index + 1) % len(self._camera_transforms) + self.sensor.set_transform(self._camera_transforms[self._transform_index]) + + def set_sensor(self, index, notify=True): + index = index % len(self._sensors) + needs_respawn = True if self._index is None \ + else self._sensors[index][0] != self._sensors[self._index][0] + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self._surface = None + self.sensor = self._parent.get_world().spawn_actor( + self._sensors[index][-1], + self._camera_transforms[self._transform_index], + attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self._hud.notification(self._sensors[index][2]) + self._index = index + + def next_sensor(self): + self.set_sensor(self._index + 1) + + def toggle_recording(self): + self._recording = not self._recording + self._hud.notification('Recording %s' % ('On' if self._recording else 'Off')) + + def render(self, display): + if self._surface is not None: + display.blit(self._surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self._sensors[self._index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0]/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) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + #hud = HUD(100, 100) + print('----------------------------------------------------------------') + print(client.get_world()) + print('----------------------------------------------------------------') + #client.load_world('Town01') + world = World(client.get_world(), hud) + #controller = KeyboardControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + #if controller.parse_events(world, clock): + # return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/PlayingWithCARLA/manual_control_modified.py b/PlayingWithCARLA/manual_control_modified.py new file mode 100755 index 00000000..d29ce204 --- /dev/null +++ b/PlayingWithCARLA/manual_control_modified.py @@ -0,0 +1,743 @@ +#!/usr/bin/env python + +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# Allows controlling a vehicle with a keyboard. For a simpler and more +# documented example, please take a look at tutorial.py. + +""" +Welcome to CARLA manual control. + +Use ARROWS or WASD keys for control. + + W : throttle + S : brake + AD : steer + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + + TAB : change sensor position + ` : next sensor + [1-9] : change to sensor [1-9] + C : change weather (Shift+C reverse) + Backspace : change vehicle + + R : toggle recording images to disk + + F1 : toggle HUD + H/? : toggle help + ESC : quit +""" + +from __future__ import print_function + + +# ============================================================================== +# -- find carla module --------------------------------------------------------- +# ============================================================================== + + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from carla import ColorConverter as cc + +import argparse +import collections +import datetime +import logging +import math +import random +import re +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + from pygame.locals import K_0 + from pygame.locals import K_9 + from pygame.locals import K_BACKQUOTE + from pygame.locals import K_BACKSPACE + from pygame.locals import K_COMMA + from pygame.locals import K_DOWN + from pygame.locals import K_ESCAPE + from pygame.locals import K_F1 + from pygame.locals import K_LEFT + from pygame.locals import K_PERIOD + from pygame.locals import K_RIGHT + from pygame.locals import K_SLASH + from pygame.locals import K_SPACE + from pygame.locals import K_TAB + from pygame.locals import K_UP + from pygame.locals import K_a + from pygame.locals import K_c + from pygame.locals import K_d + from pygame.locals import K_h + from pygame.locals import K_m + from pygame.locals import K_p + from pygame.locals import K_q + from pygame.locals import K_r + from pygame.locals import K_s + from pygame.locals import K_w +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError('cannot import numpy, make sure numpy package is installed') + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate-1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class World(object): + def __init__(self, carla_world, hud): + self.world = carla_world + self.hud = hud + 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): + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager._index if self.camera_manager is not None else 0 + cam_pos_index = self.camera_manager._transform_index if self.camera_manager is not None else 0 + # Get a random vehicle blueprint. + print('*****************************************************') + print(self.world.get_blueprint_library().filter('vehicle')) + print('*****************************************************') + print(self.world.get_blueprint_library().filter('vehicle')[0]) + print('*****************************************************') + blueprint = random.choice(self.world.get_blueprint_library().filter('vehicle')) + + blueprint = self.world.get_blueprint_library().filter('vehicle')[0] + + 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() + self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) + while self.vehicle is None: + print('************************ENTRA************************') + #print(self.world.get_map().get_spawn_points()) + print('*****************************************************') + spawn_points = self.world.get_map().get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + print('*****************************************************') + spawn_point = self.world.get_map().get_spawn_points()[0] + print(spawn_point) + print('*****************************************************') + self.vehicle = 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.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.vehicle) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.vehicle.get_world().set_weather(preset[0]) + + def tick(self, clock): + self.hud.tick(self, clock) + + def render(self, display): + self.camera_manager.render(display) + self.hud.render(display) + + def destroy(self): + 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() + + +# ============================================================================== +# -- 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 + world.vehicle.apply_control(self._control) + + 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) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + def __init__(self, width, height): + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + 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, + 'Client: % 16d FPS' % clock.get_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) + + def error(self, text): + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + f = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + def __init__(self, font, dim, pos): + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + def __init__(self, font, width, height): + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, n * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + self._render = not self._render + + def render(self, display): + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._history = [] + self._parent = parent_actor + self._hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + history = collections.defaultdict(int) + for frame, intensity in self._history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self._hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self._history.append((event.frame_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 + 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): + self.sensor = None + self._surface = None + self._parent = parent_actor + self._hud = hud + self._recording = False + self._camera_transforms = [ + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + carla.Transform(carla.Location(x=1.6, z=1.7))] + self._transform_index = 1 + self._sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self._sensors: + bp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + bp.set_attribute('image_size_x', str(hud.dim[0])) + bp.set_attribute('image_size_y', str(hud.dim[1])) + elif item[0].startswith('sensor.lidar'): + bp.set_attribute('range', '5000') + item.append(bp) + self._index = None + + def toggle_camera(self): + self._transform_index = (self._transform_index + 1) % len(self._camera_transforms) + self.sensor.set_transform(self._camera_transforms[self._transform_index]) + + def set_sensor(self, index, notify=True): + index = index % len(self._sensors) + needs_respawn = True if self._index is None \ + else self._sensors[index][0] != self._sensors[self._index][0] + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self._surface = None + self.sensor = self._parent.get_world().spawn_actor( + self._sensors[index][-1], + self._camera_transforms[self._transform_index], + attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid + # circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self._hud.notification(self._sensors[index][2]) + self._index = index + + def next_sensor(self): + self.set_sensor(self._index + 1) + + def toggle_recording(self): + self._recording = not self._recording + self._hud.notification('Recording %s' % ('On' if self._recording else 'Off')) + + def render(self, display): + if self._surface is not None: + display.blit(self._surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self._sensors[self._index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0]/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) + + +# ============================================================================== +# -- game_loop() --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + pygame.init() + pygame.font.init() + world = None + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(2.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud) + controller = KeyboardControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(world, clock): + return + world.tick(clock) + world.render(display) + pygame.display.flip() + + finally: + + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + argparser = argparse.ArgumentParser( + description='CARLA Manual Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-a', '--autopilot', + action='store_true', + help='enable autopilot') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='window resolution (default: 1280x720)') + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + + main() diff --git a/PlayingWithCARLA/move_autopilot.py b/PlayingWithCARLA/move_autopilot.py new file mode 100644 index 00000000..5e226b23 --- /dev/null +++ b/PlayingWithCARLA/move_autopilot.py @@ -0,0 +1,36 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) +time.sleep(2) +car = world.get_actors().filter('vehicle.*')[0] +car.set_autopilot(True) +print(car.is_at_traffic_light()) + +''' +if vehicle_actor.is_at_traffic_light(): + traffic_light = vehicle_actor.get_traffic_light() + if traffic_light.get_state() == carla.TrafficLightState.Red: + # world.hud.notification("Traffic light changed! Good to go!") + traffic_light.set_state(carla.TrafficLightState.Green) +''' + +''' +my_vehicles = [car] + +tm = client.get_trafficmanager(port) +tm_port = tm.get_port() +for v in my_vehicles: + v.set_autopilot(True,tm_port) +danger_car = my_vehicles[0] +tm.ignore_lights_percentage(danger_car,100) +tm.distance_to_leading_vehicle(danger_car,0) +tm.vehicle_percentage_speed_difference(danger_car,-20) + +''' diff --git a/PlayingWithCARLA/move_vehicle.py b/PlayingWithCARLA/move_vehicle.py new file mode 100644 index 00000000..1789630f --- /dev/null +++ b/PlayingWithCARLA/move_vehicle.py @@ -0,0 +1,71 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +client = carla.Client('localhost', 2000) + +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) + +time.sleep(2) + +world.get_actors() + +car = world.get_actors().filter('vehicle.*')[0] +print(car) +print(car.get_velocity()) +print(car.get_vehicle_control()) +#vehicle_control =car.get_vehicle_control() +#vehicle_control.steer = 10000 + + +car.apply_control(carla.VehicleControl(throttle = 1, brake = 0)) +print(car.get_velocity()) +print(car.get_vehicle_control()) +time.sleep(5) +print(car.get_velocity()) +print(car.get_vehicle_control()) +time.sleep(5) +print(car.get_velocity()) +print(car.get_vehicle_control()) +car.apply_control(carla.VehicleControl(throttle = 0, brake = 0)) +time.sleep(5) +print(car.get_velocity()) +print(car.get_vehicle_control()) +''' +vehicle_control = carla.VehicleControl() +vehicle_control.steer = 10000 + +vehicle_control.manual_gear_shift =True +car.apply_control(vehicle_control) +time.sleep(0.5) +print(car.get_velocity()) +car.apply_control(vehicle_control) +time.sleep(0.5) +print(car.get_velocity()) +car.apply_control(vehicle_control) +time.sleep(0.5) +print(car.get_velocity()) +car.apply_control(vehicle_control) +time.sleep(0.5) +print(car.get_velocity()) +car.apply_control(vehicle_control) +time.sleep(0.5) +car.apply_control(vehicle_control) +time.sleep(0.5) +car.apply_control(vehicle_control) +time.sleep(0.5) +car.apply_control(vehicle_control) +time.sleep(0.5) +car.apply_control(vehicle_control) +time.sleep(0.5) +car.apply_control(vehicle_control) +time.sleep(0.5) +car.apply_control(vehicle_control) +time.sleep(0.5) + +print(car.get_velocity()) +''' diff --git a/PlayingWithCARLA/move_vehicle_tensorflow.py b/PlayingWithCARLA/move_vehicle_tensorflow.py new file mode 100644 index 00000000..8d55458b --- /dev/null +++ b/PlayingWithCARLA/move_vehicle_tensorflow.py @@ -0,0 +1,154 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +client = carla.Client('localhost', 2000) + +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) + +time.sleep(2) + +world.get_actors() + +car = world.get_actors().filter('vehicle.*')[0] +print(car) +print(car.get_velocity()) +#print(car.get_vehicle_control()) +#vehicle_control =car.get_vehicle_control() +#vehicle_control.steer = 10000 + + +import tensorflow as tf +import numpy as np + +from albumentations import ( + Compose, Normalize +) + +# LOAD CAMERA +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +#camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) +#rgb camera +image = image_queue.get() + +# LOAD TF MODEL +PRETRAINED_MODELS = "/home/jderobot/Documents/Projects/" +#model = "20220905-164806_pilotnet_CARLA_cp.h5" +#model = "20220906-114029_pilotnet_CARLA_extreme_cases_cp.h5" +#model = "20220906-165331_pilotnet_CARLA_extreme_cases_cp.h5" +#model = "20220907-105158_pilotnet_CARLA_extreme_cases_07_09_cp.h5" +#model = "20220907-140021_pilotnet_CARLA_extreme_cases_07_09_cp.h5" +#model = "20220914-124554_pilotnet_CARLA_extreme_cases_14_09_dataset_cp.h5" +#model = "20220914-130834_pilotnet_CARLA_extreme_cases_14_09_dataset_cp.h5" +#model = "20220914-134708_pilotnet_CARLA_extreme_cases_14_09_dataset_new_crop_cp.h5" +#model = "20220914-140016_pilotnet_CARLA_extreme_cases_14_09_dataset_new_crop_cp.h5" +#model = "20220916-140706_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_cp.h5" +#model = "20220916-154943_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_cp.h5" +#model = "20220916-164609_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases.h5" +#model = "20220919-172247_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases_simplified_images_cp.h5" +#model = "20220919-184652_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases_simplified_images_cp.h5" +model = "20220929-181325_pilotnet_CARLA_extreme_cases_29_09_dataset_bird_eye_random_start_point_retrained_double_cp.h5" + + +print('***********************************************************************************************') +net = tf.keras.models.load_model(PRETRAINED_MODELS + model) +#net = tf.saved_model.load(PRETRAINED_MODELS + model) +print('***********************************************************************************************') + + +while True: + # Predict + image = image_queue.get() + ''' + print(image) + print(image.raw_data) + ''' + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) # RGBA format + image = array[:, :, :3] # Take only RGB + image = image[:, :, ::-1] # BGR + + #print(image.shape) + + image = image[325:600, 0:800] + #plt.imshow(image) + #plt.show() + + img = cv2.resize(image, (200, 66)) + frame = img + hsv = cv2.cvtColor(frame, cv2.COLOR_RGB2HSV) + # Threshold of blue in HSV space + lower_blue = np.array([10, 100, 20]) + upper_blue = np.array([25, 255, 255]) + + + # preparing the mask to overlay + mask = cv2.inRange(hsv, lower_blue, upper_blue) + + # The black region in the mask has the value of 0, + # so when multiplied with original image removes all non-blue regions + result = cv2.bitwise_and(frame, frame, mask = mask) + #new_images_carla_dataset.append(result) + + img = result + + + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img) + img = image["image"] + + + img = np.expand_dims(img, axis=0) + prediction = net.predict(img) + prediction_w = prediction[0][1] * (1 - (-1)) + (-1) + ''' + print(prediction) + print(prediction_w) + ''' + + throttle = prediction[0][0] + steer = prediction_w + ''' + print('----') + print(throttle) + print(type(throttle)) + print(float(throttle)) + print(steer) + print(float(steer)) + print(type(steer)) + print('----') + ''' + print(float(throttle), steer) + + #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer)) + car.apply_control(carla.VehicleControl(throttle=0.2, steer=steer)) + #print(car.get_velocity()) + #print(car.get_vehicle_control()) + #time.sleep(5) + + +''' +car.apply_control(carla.VehicleControl(throttle = 1, brake = 0)) +print(car.get_velocity()) +print(car.get_vehicle_control()) +time.sleep(5) +print(car.get_velocity()) +print(car.get_vehicle_control()) +time.sleep(5) +print(car.get_velocity()) +print(car.get_vehicle_control()) +car.apply_control(carla.VehicleControl(throttle = 0, brake = 0)) +time.sleep(5) +print(car.get_velocity()) +print(car.get_vehicle_control()) +''' diff --git a/PlayingWithCARLA/plot_car_camera.py b/PlayingWithCARLA/plot_car_camera.py new file mode 100644 index 00000000..134af2ad --- /dev/null +++ b/PlayingWithCARLA/plot_car_camera.py @@ -0,0 +1,43 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +client = carla.Client('localhost', 2000) + +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) + +time.sleep(2) + +world.get_actors() + +car = world.get_actors().filter('vehicle.*')[0] +print(car) +print(car.get_velocity()) + + + +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) + +#rgb camera +image = image_queue.get() +image = image_queue.get() +cc = carla.ColorConverter.Raw +image.save_to_disk('_out/1.png', cc) + + +img = cv2.imread("_out/1.png", cv2.IMREAD_UNCHANGED) +img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) +img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) +print(car.get_velocity()) +plt.imshow(img) +plt.show() + + diff --git a/PlayingWithCARLA/print_camera_image.py b/PlayingWithCARLA/print_camera_image.py new file mode 100644 index 00000000..cf0a4e97 --- /dev/null +++ b/PlayingWithCARLA/print_camera_image.py @@ -0,0 +1,76 @@ +import carla +import time + +client = carla.Client('localhost', 2000) + +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) + +world.get_actors() + +time.sleep(2) + +car = world.get_actors().filter('vehicle.*')[0] +print(car) +print(car.get_velocity()) + + +# Find the blueprint of the sensor. +blueprint = world.get_blueprint_library().find('sensor.camera.rgb') + +# Modify the attributes of the blueprint to set image resolution and field of view. +blueprint.set_attribute('image_size_x', '1920') +blueprint.set_attribute('image_size_y', '1080') +blueprint.set_attribute('fov', '110') + +# Provide the position of the sensor relative to the vehicle. +transform = carla.Transform(carla.Location(x=0.8, z=1.7)) + +# Tell the world to spawn the sensor, don't forget to attach it to your vehicle actor. +sensor = world.spawn_actor(blueprint, transform, attach_to=car) + +# Subscribe to the sensor stream by providing a callback function, this function is +# called each time a new image is generated by the sensor. +cc = carla.ColorConverter.LogarithmicDepth +cc = carla.ColorConverter.Raw +cc = carla.ColorConverter.CityScapesPalette +cc = carla.ColorConverter.Depth +sensor.listen(lambda image: image.save_to_disk('_out/%06d.png' % image.frame_number, cc)) + + + +import queue +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) + +#rgb camera +image = image_queue.get() +image = image_queue.get() +cc = carla.ColorConverter.Raw +image.save_to_disk('_out/1.png', cc) + +import matplotlib.pyplot as plt +import cv2 +img = cv2.imread("_out/1.png", cv2.IMREAD_UNCHANGED) +img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) +img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) +plt.imshow(img) +plt.show() + + +#rgb camera +image = image_queue.get() +cc = carla.ColorConverter.Raw +image.save_to_disk('_out/%06d.png' % image.frame_number, cc) + +import matplotlib.pyplot as plt +import cv2 +img = cv2.imread("_out/341519.png", cv2.IMREAD_UNCHANGED) +img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) +img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) +plt.imshow(img) +plt.show() diff --git a/PlayingWithCARLA/record_dataset.py b/PlayingWithCARLA/record_dataset.py new file mode 100644 index 00000000..9ad3ec76 --- /dev/null +++ b/PlayingWithCARLA/record_dataset.py @@ -0,0 +1,69 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import pandas + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +#camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) + +#rgb camera +image = image_queue.get() +# open the file in the write mode +f = open('../carla_dataset_20_09/dataset.csv', 'a') + +# create the csv writer +writer = csv.writer(f) + +#mypath = '../carla_dataset_14_09/' +#onlyfiles = [f for f in listdir(mypath) if isfile(join(mypath, f))] +#onlyfiles = sorted(onlyfiles) +#print(onlyfiles) +#print(onlyfiles[len(onlyfiles)-2]) +#print(onlyfiles[len(onlyfiles)-2][0]) +#print(type(onlyfiles[len(onlyfiles)-2][0])) + +try: + df = pandas.read_csv('../carla_dataset_20_09/dataset.csv') + batch_number = df.iloc[-1]['batch'] + 1 +except: + batch_number = 0 + header = ['batch', 'image_id', 'throttle', 'steer'] + writer.writerow(header) + +print(batch_number) + +try: + while True: + image = image_queue.get() + cc = carla.ColorConverter.Raw + image.save_to_disk('../carla_dataset_20_09/' + str(batch_number) + '_' + str(image.frame_number) + '.png', cc) + vehicle_control = car.get_vehicle_control() + # write a row to the csv file + row = [batch_number, str(batch_number) + '_' + str(image.frame_number) + '.png', vehicle_control.throttle, vehicle_control.steer] + writer.writerow(row) + +except KeyboardInterrupt: + pass + +# close the file +f.close() + + diff --git a/PlayingWithCARLA/remove_traffic_lights.py b/PlayingWithCARLA/remove_traffic_lights.py new file mode 100644 index 00000000..473dc818 --- /dev/null +++ b/PlayingWithCARLA/remove_traffic_lights.py @@ -0,0 +1,86 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +print(carla.__file__) + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) +time.sleep(2) + +#print(world.get_actors()) + +#traffic_lights = world.get_actors().filter('traffic.traffic_light') + +#for traffic_light in traffic_lights: +# traffic_light.destroy() + +#print('*****************') +#print(world.get_actors()) + +################################################################################################### +#print(world.get_actors()) + +#actors = world.get_actors() +#for actor in actors: +# print(actor) + +#print('*****************') +#print(world.get_actors()) + + +meshes = world.get_actors().filter('static.prop.mesh') +print(meshes) +for mesh in meshes: + mesh.destroy() +print(world.get_actors()) + + +traffic_lights = world.get_actors().filter('traffic.traffic_light') + +for traffic_light in traffic_lights: + traffic_light.destroy() +print(world.get_actors()) + + +speed_limits = world.get_actors().filter('traffic.speed_limit.30') +print(speed_limits) +for speed_limit in speed_limits: + speed_limit.destroy() +print(world.get_actors()) + +speed_limits = world.get_actors().filter('traffic.speed_limit.60') +print(speed_limits) +for speed_limit in speed_limits: + speed_limit.destroy() +print(world.get_actors()) + +speed_limits = world.get_actors().filter('traffic.speed_limit.90') +print(speed_limits) +for speed_limit in speed_limits: + speed_limit.destroy() +print(world.get_actors()) + + + +''' +print(traffic_lights[0].state) +print(type(traffic_lights[0])) +print(type(traffic_lights[0].state)) +print('*****************') +print(dir(traffic_lights[0])) +print('*****************') +print(dir(traffic_lights[0].state)) + +carla.TrafficLightState('Red') + +traffic_lights[0].set_state('Green') +traffic_lights[0].state = 'Green' +print(traffic_lights[0].state) +''' + + diff --git a/PlayingWithCARLA/remove_traffic_lights_and_autopilot.py b/PlayingWithCARLA/remove_traffic_lights_and_autopilot.py new file mode 100644 index 00000000..7023d461 --- /dev/null +++ b/PlayingWithCARLA/remove_traffic_lights_and_autopilot.py @@ -0,0 +1,109 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +print(carla.__file__) + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) +time.sleep(2) + +traffic_lights = world.get_actors().filter('traffic.traffic_light') +traffic_speed_limits = world.get_actors().filter('traffic.speed_limit*') +print(traffic_speed_limits) +for traffic_light in traffic_lights: + #success = traffic_light.destroy() + traffic_light.set_green_time(20000) + traffic_light.set_state(carla.TrafficLightState.Green) + #print(success) + +#for speed_limit in traffic_speed_limits: +# success = speed_limit.destroy() +# print(success) + +print(world.get_actors().filter('vehicle.*')) +car = world.get_actors().filter('vehicle.*')[0] + + +#settings = world.get_settings() +#settings.synchronous_mode = True # Enables synchronous mode +#world.apply_settings(settings) +traffic_manager = client.get_trafficmanager() +#random.seed(0) +#car.set_autopilot(True) +car.set_autopilot(True) +route = ["Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight"] +traffic_manager.set_route(car, route) + +''' +import math +while True: + speed = car.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + if (abs(vehicle_speed) > 5): + print('SLOW DOWN!') + car.apply_control(carla.VehicleControl(throttle=float(0), steer=float(0), brake=float(1.0))) + + +''' +''' +# Set up the TM in synchronous mode +traffic_manager = client.get_trafficmanager() +#traffic_manager.set_synchronous_mode(True) + +# Set a seed so behaviour can be repeated if necessary +#traffic_manager.set_random_device_seed(0) +#random.seed(0) +#car.set_autopilot(True) +car.set_autopilot(True) +route = ["Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight", +"Straight", "Straight", "Straight", "Straight", "Straight"] +traffic_manager.set_route(car, route) +#time.sleep(3) +#car.set_autopilot(False) +#print('autopilot false!') +''' +''' +car.apply_control(carla.VehicleControl(throttle=float(0.75), steer=-0.1, brake=float(0.0))) +time.sleep(3) +car.set_autopilot(True) +#traffic_manager.set_route(car, route) +''' + + +''' +while True: + print('entra') + world.tick() + + #traffic_manager.update_vehicle_lights(car, True) + #traffic_manager.random_left_lanechange_percentage(car, 0) + #traffic_manager.random_right_lanechange_percentage(car, 0) + #traffic_manager.auto_lane_change(car, False) + + world_map =world.get_map() + spawn_points = world_map.get_spawn_points() + + # Create route 1 from the chosen spawn points + route_1_indices = [129, 28, 124, 33, 97, 119, 58, 154, 147] + route_1 = [] + for ind in route_1_indices: + route_1.append(spawn_points[ind].location) + print(route_1) + traffic_manager.set_path(car, route_1) +''' + + diff --git a/PlayingWithCARLA/set_autopilot_and_print_speed.py b/PlayingWithCARLA/set_autopilot_and_print_speed.py new file mode 100644 index 00000000..3c64210b --- /dev/null +++ b/PlayingWithCARLA/set_autopilot_and_print_speed.py @@ -0,0 +1,26 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +client = carla.Client('localhost', 2000) + +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) + +time.sleep(2) + +world.get_actors() + +car = world.get_actors().filter('vehicle.*')[0] +print(car) +print(car.get_velocity()) +print(car.get_vehicle_control()) + + +car.set_autopilot(True) + +while True: + print(car.get_vehicle_control()) diff --git a/PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py b/PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py new file mode 100644 index 00000000..89f4dd9c --- /dev/null +++ b/PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py @@ -0,0 +1,87 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import tensorflow as tf +import numpy as np + +from albumentations import ( + Compose, Normalize +) + + +client = carla.Client('localhost', 2000) + +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) + +time.sleep(2) + +world.get_actors() + +car = world.get_actors().filter('vehicle.*')[0] +print(car) +print(car.get_velocity()) +print(car.get_vehicle_control()) + + + +# LOAD CAMERA +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +#camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) +#rgb camera +image = image_queue.get() + +# LOAD TF MODEL +PRETRAINED_MODELS = "/home/jderobot/Documents/Projects/" +model = "20220907-140021_pilotnet_CARLA_extreme_cases_07_09_cp.h5" + +print('***********************************************************************************************') +net = tf.keras.models.load_model(PRETRAINED_MODELS + model) +#net = tf.saved_model.load(PRETRAINED_MODELS + model) +print('***********************************************************************************************') + + +car.set_autopilot(True) + +while True: + # Predict + image = image_queue.get() + + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) # RGBA format + image = array[:, :, :3] # Take only RGB + image = image[:, :, ::-1] # BGR + + image = image[300:600, 0:800] + + img = cv2.resize(image, (200, 66)) + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img) + img = image["image"] + + + img = np.expand_dims(img, axis=0) + prediction = net.predict(img) + prediction_w = prediction[0][1] * (1 - (-1)) + (-1) + + throttle = prediction[0][0] + steer = prediction_w + + print(abs(steer)-abs(car.get_vehicle_control().steer)) + + #print(float(throttle), steer) + #print(car.get_vehicle_control()) + + + + + diff --git a/PlayingWithCARLA/show_camera_image.py b/PlayingWithCARLA/show_camera_image.py new file mode 100644 index 00000000..ac5cc07b --- /dev/null +++ b/PlayingWithCARLA/show_camera_image.py @@ -0,0 +1,77 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import numpy as np +import queue +import pygame + + +h,w=800,800 + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) + +#rgb camera +image = image_queue.get() + +pygame.init() +screen = pygame.display.set_mode((w, h)) +pygame.display.set_caption("Serious Work - not games") +done = False +clock = pygame.time.Clock() + +# Get a font for rendering the frame number +basicfont = pygame.font.SysFont(None, 32) + +array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) +array = np.reshape(array, (image.height, image.width, 4)) # RGBA format +image = array[:, :, :3] # Take only RGB +image = image[:, :, ::-1] # BGR + +try: + while True: + image = image_queue.get() + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) # RGBA format + image = array[:, :, :3] # Take only RGB + image = image[:, :, ::-1] # BGR + image = image[:, ::-1, :] # Mirror + + image = image[325:600, 0:800] + #plt.imshow(image) + #plt.show() + + img = cv2.resize(image, (200, 66)) + + # Clear screen to white before drawing + screen.fill((255, 255, 255)) + + # Convert to a surface and splat onto screen offset by border width and height + surface = pygame.surfarray.make_surface(image) + screen.blit(surface, (0, 0)) + screen.blit(pygame.transform.rotate(screen, 270), (0, 0)) + pygame.display.flip() + + clock.tick(60) + + +except KeyboardInterrupt: + pass + diff --git a/PlayingWithCARLA/show_camera_opencv.py b/PlayingWithCARLA/show_camera_opencv.py new file mode 100644 index 00000000..9e9e0861 --- /dev/null +++ b/PlayingWithCARLA/show_camera_opencv.py @@ -0,0 +1,68 @@ +import carla + +import random +import time +import numpy as np +import cv2 + +IM_WIDTH = 640 +IM_HEIGHT = 480 + + +def process_img(image): + i = np.array(image.raw_data) + i2 = i.reshape((IM_HEIGHT, IM_WIDTH, 4)) + i3 = i2[:, :, :3] + cv2.imshow("", i3) + cv2.waitKey(1) + return i3/255.0 + + +actor_list = [] +try: + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + world = client.get_world() + + blueprint_library = world.get_blueprint_library() + + bp = blueprint_library.filter('model3')[0] + print(bp) + + spawn_point = random.choice(world.get_map().get_spawn_points()) + + vehicle = world.spawn_actor(bp, spawn_point) + vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) + # vehicle.set_autopilot(True) # if you just wanted some NPCs to drive. + + actor_list.append(vehicle) + + # https://carla.readthedocs.io/en/latest/cameras_and_sensors + # get the blueprint for this sensor + blueprint = blueprint_library.find('sensor.camera.rgb') + # change the dimensions of the image + blueprint.set_attribute('image_size_x', f'{IM_WIDTH}') + blueprint.set_attribute('image_size_y', f'{IM_HEIGHT}') + blueprint.set_attribute('fov', '110') + + # Adjust sensor relative to vehicle + spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7)) + spawn_point = carla.Transform(carla.Location(x=1.5, z=2.4)) + + # spawn the sensor and attach to vehicle. + sensor = world.spawn_actor(blueprint, spawn_point, attach_to=vehicle) + + # add sensor to list of actors + actor_list.append(sensor) + + # do something with this sensor + sensor.listen(lambda data: process_img(data)) + + time.sleep(5) + +finally: + print('destroying actors') + for actor in actor_list: + actor.destroy() + print('done.') \ No newline at end of file diff --git a/PlayingWithCARLA/show_crop_camera_image.py b/PlayingWithCARLA/show_crop_camera_image.py new file mode 100644 index 00000000..ed996c20 --- /dev/null +++ b/PlayingWithCARLA/show_crop_camera_image.py @@ -0,0 +1,76 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import numpy as np +import queue +import pygame + + +h,w=200,200 + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) + +#rgb camera +image = image_queue.get() + +pygame.init() +screen = pygame.display.set_mode((w, h)) +pygame.display.set_caption("Serious Work - not games") +done = False +clock = pygame.time.Clock() + +# Get a font for rendering the frame number +basicfont = pygame.font.SysFont(None, 32) + +array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) +array = np.reshape(array, (image.height, image.width, 4)) # RGBA format +image = array[:, :, :3] # Take only RGB +image = image[:, :, ::-1] # BGR + + + +try: + while True: + image = image_queue.get() + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) # RGBA format + image = array[:, :, :3] # Take only RGB + image = image[:, :, ::-1] # BGR + image = image[:, ::-1, :] # Mirror + image = image[300:600, 0:800] + image= cv2.resize(image, (200, 66)) + + # Clear screen to white before drawing + + screen.fill((255, 255, 255)) + + # Convert to a surface and splat onto screen offset by border width and height + surface = pygame.surfarray.make_surface(image) + screen.blit(surface, (0, 0)) + screen.blit(pygame.transform.rotate(screen, 270), (0, 0)) + pygame.display.flip() + + clock.tick(60) + + +except KeyboardInterrupt: + pass + diff --git a/PlayingWithCARLA/show_dataset_image.py b/PlayingWithCARLA/show_dataset_image.py new file mode 100644 index 00000000..07cfea76 --- /dev/null +++ b/PlayingWithCARLA/show_dataset_image.py @@ -0,0 +1,7 @@ +import matplotlib.pyplot as plt +import cv2 +img = cv2.imread("../_out/187221.png", cv2.IMREAD_UNCHANGED) +img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) +img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) +plt.imshow(img) +plt.show() diff --git a/PlayingWithCARLA/show_environment_objects.py b/PlayingWithCARLA/show_environment_objects.py new file mode 100644 index 00000000..d17352ca --- /dev/null +++ b/PlayingWithCARLA/show_environment_objects.py @@ -0,0 +1,39 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +print(carla.__file__) + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) +time.sleep(2) + + +#print(world.get_environment_objects()) +print(len(world.get_environment_objects())) + +environment_objects = world.get_environment_objects() + +print(type(environment_objects)) +print(environment_objects[0].id) + +environment_object_ids = [] + +for env_obj in environment_objects: + environment_object_ids.append(env_obj.id) + + + +environment_object_ids = set(environment_object_ids) +print(type(environment_object_ids)) + +world.enable_environment_objects(environment_object_ids, False) +#for env_obj in environment_objects: + + + + diff --git a/PlayingWithCARLA/show_grad_camera.py b/PlayingWithCARLA/show_grad_camera.py new file mode 100644 index 00000000..9dc4896d --- /dev/null +++ b/PlayingWithCARLA/show_grad_camera.py @@ -0,0 +1,122 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time +import csv +from os import listdir +from os.path import isfile, join +import numpy as np +import queue +import pygame + + +h,w=800,800 +h,w=200, 200 + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() + +time.sleep(2) + +world.get_actors() +car = world.get_actors().filter('vehicle.*')[0] + +camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') +camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) +camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) +image_queue = queue.Queue() +camera.listen(image_queue.put) + +#rgb camera +image = image_queue.get() + +pygame.init() +screen = pygame.display.set_mode((w, h)) +pygame.display.set_caption("Serious Work - not games") +done = False +clock = pygame.time.Clock() + +# Get a font for rendering the frame number +basicfont = pygame.font.SysFont(None, 32) + +array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) +array = np.reshape(array, (image.height, image.width, 4)) # RGBA format +image = array[:, :, :3] # Take only RGB +image = image[:, :, ::-1] # BGR + +import tensorflow as tf +import numpy as np +from gradcam import GradCAM + + +from albumentations import ( + Compose, Normalize +) + +# LOAD TF MODEL +PRETRAINED_MODELS = "/home/jderobot/Documents/Projects/" +model = "20220916-164609_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases.h5" +net = tf.keras.models.load_model(PRETRAINED_MODELS + model) +print(net.summary()) + +try: + while True: + image = image_queue.get() + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) # RGBA format + image = array[:, :, :3] # Take only RGB + image = image[:, :, ::-1] # BGR + image = image[:, ::-1, :] # Mirror + + original_image = image[325:600, 0:800] + #plt.imshow(image) + #plt.show() + + resized_image = cv2.resize(original_image, (200, 66)) + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=resized_image) + img = image["image"] + + + img = np.expand_dims(img, axis=0) + prediction = net.predict(img) + + prediction_w = prediction[0][1] * (1 - (-1)) + (-1) + throttle = prediction[0][0] + steer = prediction_w + print(float(throttle), steer) + + #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer)) + car.apply_control(carla.VehicleControl(throttle=0.2, steer=steer)) + + i = np.argmax(prediction[0]) + cam = GradCAM(net, i) + heatmap = cam.compute_heatmap(img) + heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) + + print(original_image.shape) + print(resized_image.shape) + print(heatmap.shape) + (heatmap, output) = cam.overlay_heatmap(heatmap, resized_image, alpha=0.5) + print(output.shape) + + # Clear screen to white before drawing + screen.fill((255, 255, 255)) + + # Convert to a surface and splat onto screen offset by border width and height + #surface = pygame.surfarray.make_surface(original_image) + surface = pygame.surfarray.make_surface(output) + screen.blit(surface, (0, 0)) + screen.blit(pygame.transform.rotate(screen, 270), (0, 0)) + pygame.display.flip() + + clock.tick(60) + + +except KeyboardInterrupt: + pass + diff --git a/PlayingWithCARLA/tutorial.py b/PlayingWithCARLA/tutorial.py new file mode 100755 index 00000000..311df317 --- /dev/null +++ b/PlayingWithCARLA/tutorial.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python + +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('**/*%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 + +import random +import time + + +def main(): + actor_list = [] + + # In this tutorial script, we are going to add a vehicle to the simulation + # and let it drive in autopilot. We will also create a camera attached to + # that vehicle, and save all the images generated by the camera to disk. + + try: + # First of all, we need to create the client that will send the requests + # to the simulator. Here we'll assume the simulator is accepting + # requests in the localhost at port 2000. + client = carla.Client('localhost', 2000) + client.set_timeout(2.0) + + # Once we have a client we can retrieve the world that is currently + # running. + world = client.get_world() + + # The world contains the list blueprints that we can use for adding new + # actors into the simulation. + blueprint_library = world.get_blueprint_library() + + # Now let's filter all the blueprints of type 'vehicle' and choose one + # at random. + bp = random.choice(blueprint_library.filter('vehicle')) + + # A blueprint contains the list of attributes that define a vehicle + # instance, we can read them and modify some of them. For instance, + # let's randomize its color. + #color = random.choice(bp.get_attribute('color').recommended_values) + #bp.set_attribute('color', color) + + # Now we need to give an initial transform to the vehicle. We choose a + # random transform from the list of recommended spawn points of the map. + transform = random.choice(world.get_map().get_spawn_points()) + + # So let's tell the world to spawn the vehicle. + vehicle = world.spawn_actor(bp, transform) + + # It is important to note that the actors we create won't be destroyed + # unless we call their "destroy" function. If we fail to call "destroy" + # they will stay in the simulation even after we quit the Python script. + # For that reason, we are storing all the actors we create so we can + # destroy them afterwards. + actor_list.append(vehicle) + print('created %s' % vehicle.type_id) + + # Let's put the vehicle to drive around. + vehicle.set_autopilot(True) + + # Let's add now a "depth" camera attached to the vehicle. Note that the + # transform we give here is now relative to the vehicle. + camera_bp = blueprint_library.find('sensor.camera.depth') + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + actor_list.append(camera) + print('created %s' % camera.type_id) + + # Now we register the function that will be called each time the sensor + # receives an image. In this example we are saving the image to disk + # converting the pixels to gray-scale. + cc = carla.ColorConverter.LogarithmicDepth + camera.listen(lambda image: image.save_to_disk('_out/%06d.png' % image.frame_number, cc)) + + # Oh wait, I don't like the location we gave to the vehicle, I'm going + # to move it a bit forward. + location = vehicle.get_location() + location.x += 40 + vehicle.set_location(location) + print('moved vehicle to %s' % location) + + # But the city now is probably quite empty, let's add a few more + # vehicles. + transform.location += carla.Location(x=40, y=-3.2) + transform.rotation.yaw = -180.0 + for x in range(0, 10): + transform.location.x += 8.0 + + bp = random.choice(blueprint_library.filter('vehicle')) + + # This time we are using try_spawn_actor. If the spot is already + # occupied by another object, the function will return None. + npc = world.try_spawn_actor(bp, transform) + if npc is not None: + actor_list.append(npc) + npc.set_autopilot() + print('created %s' % npc.type_id) + + time.sleep(5) + + finally: + + print('destroying actors') + for actor in actor_list: + actor.destroy() + print('done.') + + +if __name__ == '__main__': + + main() From 6ac7cc3de8a1fc9eb3bdaa6ec343779607e77d0c Mon Sep 17 00:00:00 2001 From: jderobot Date: Wed, 16 Nov 2022 17:34:45 +0100 Subject: [PATCH 02/36] Save total distance and time for simulation --- ...s_and_move_tensorflow_bird_eye_velocity.py | 37 ++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py index ec18d73a..74329a69 100644 --- a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py +++ b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py @@ -233,7 +233,7 @@ def main(): pixels_per_meter=10, crop_type=BirdViewCropType.FRONT_AREA_ONLY ) - PRETRAINED_MODELS = "../../../" + PRETRAINED_MODELS = "../models/" model = "20221026-133123_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_cp.h5" model = "20221031-125557_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_cp.h5" model = "20221031-182622_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_cp.h5" # BEST @@ -255,12 +255,18 @@ def main(): #model = "20221107-143505_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_input_output_velocity_all_towns_and_extreme_vel_MAX_cp.h5" #model = "20221108-155441_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_input_output_velocity_town_01_and_extreme_vel_MAX_cp.h5" - model = "20221108-165842_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_prev_velocity_town_01_and_extreme_vel_MAX_cp.h5" + #model = "20221108-165842_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_prev_velocity_town_01_and_extreme_vel_MAX_cp.h5" net = tf.keras.models.load_model(PRETRAINED_MODELS + model) previous_speed = 0 + previous_location_x = None + previous_location_y = None + total_distance = 0 + + previous_simulated_time = None + start_simulation_time = world.get_snapshot().timestamp.elapsed_seconds # Create a synchronous mode context. with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=15) as sync_mode: @@ -323,6 +329,15 @@ def main(): #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) #print(throttle, steer, break_command) #print(vehicle.get_control()) + + if previous_location_x is not None and previous_location_y is not None: + dist = math.sqrt( (round(vehicle_location.x, 2) - previous_location_x)**2 + (round(vehicle_location.y, 2) - previous_location_y)**2 ) + total_distance += dist + previous_location_x = round(vehicle_location.x, 2) + previous_location_y = round(vehicle_location.y, 2) + + + print(world.get_snapshot().timestamp.elapsed_seconds-start_simulation_time) i = np.argmax(prediction[0]) cam = GradCAM(net, i) @@ -343,7 +358,7 @@ def main(): #draw_image(display, img_base, blend=False, location=(1600,0)) draw_image(display, img_base, blend=False, location=(800,0)) draw_image(display, output, blend=False, location=(1000,0)) - draw_image(display, np.zeros((160,300, 3)), blend=False, location=(0,0), is_black_space=True) + draw_image(display, np.zeros((200,300, 3)), blend=False, location=(0,0), is_black_space=True) display.blit( font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), (8, 10)) @@ -353,11 +368,11 @@ def main(): if vehicle_speed > 30: display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/S', True, (255, 0, 0)), + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' km/h', True, (255, 0, 0)), (22, 46)) else: display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), + font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' km/h', True, (255, 255, 255)), (22, 46)) if mean_step_time != []: display.blit( @@ -384,9 +399,13 @@ def main(): font.render('World: ' + str(m.name), True, (255, 255, 255)), (22, 136)) - #display.blit( - # font.render('Predicted previous speed: ' + str(prediction[0][3]*88), True, (255, 255, 255)), - # (22, 150)) + display.blit( + font.render('Total distance: ' + str(round(total_distance, 2)) + ' m', True, (255, 255, 255)), + (22, 154)) + + display.blit( + font.render('Total simulated time: ' + str(round(world.get_snapshot().timestamp.elapsed_seconds-start_simulation_time, 2)) + ' s', True, (255, 255, 255)), + (22, 172)) pygame.display.flip() end = time.time() @@ -411,4 +430,4 @@ def main(): main() except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file + print('\nCancelled by user. Bye!') From 763c45daf671ede0a2526841fa4f02bf38c1ffac Mon Sep 17 00:00:00 2001 From: jderobot Date: Thu, 17 Nov 2022 17:28:12 +0100 Subject: [PATCH 03/36] Connection between CARLA and ROS using ROS BRIDGE toy example! --- .../carla_0_9_13/get_vehicles_locations.py | 212 ++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100644 PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py diff --git a/PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py b/PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py new file mode 100644 index 00000000..a0631496 --- /dev/null +++ b/PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py @@ -0,0 +1,212 @@ +import carla +import queue +import matplotlib.pyplot as plt +import cv2 +import time + +print(carla.__file__) + +client = carla.Client('localhost', 2000) +client.set_timeout(10.0) # seconds +world = client.get_world() +print(world) +time.sleep(2) + +print(world.get_actors().filter('vehicle.*')) +print(world.get_actors().filter('vehicle.*')[0].get_location()) + + + +#from robot.interfaces.pose3d import ListenerPose3d + + +import rospy +from nav_msgs.msg import Odometry +import threading +from math import asin, atan2, pi + + +def quat2Yaw(qw, qx, qy, qz): + ''' + Translates from Quaternion to Yaw. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Yaw value translated from Quaternion + + ''' + rotateZa0 = 2.0 * (qx * qy + qw * qz) + rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz + rotateZ = 0.0 + if(rotateZa0 != 0.0 and rotateZa1 != 0.0): + rotateZ = atan2(rotateZa0, rotateZa1) + return rotateZ + + +def quat2Pitch(qw, qx, qy, qz): + ''' + Translates from Quaternion to Pitch. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Pitch value translated from Quaternion + + ''' + + rotateYa0 = -2.0 * (qx * qz - qw * qy) + rotateY = 0.0 + if(rotateYa0 >= 1.0): + rotateY = pi/2.0 + elif(rotateYa0 <= -1.0): + rotateY = -pi/2.0 + else: + rotateY = asin(rotateYa0) + + return rotateY + + +def quat2Roll(qw, qx, qy, qz): + ''' + Translates from Quaternion to Roll. + + @param qw,qx,qy,qz: Quaternion values + + @type qw,qx,qy,qz: float + + @return Roll value translated from Quaternion + + ''' + rotateXa0 = 2.0 * (qy * qz + qw * qx) + rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz + rotateX = 0.0 + + if(rotateXa0 != 0.0 and rotateXa1 != 0.0): + rotateX = atan2(rotateXa0, rotateXa1) + return rotateX + + +def odometry2Pose3D(odom): + ''' + Translates from ROS Odometry to JderobotTypes Pose3d. + + @param odom: ROS Odometry to translate + + @type odom: Odometry + + @return a Pose3d translated from odom + + ''' + pose = Pose3d() + ori = odom.pose.pose.orientation + + pose.x = odom.pose.pose.position.x + pose.y = odom.pose.pose.position.y + pose.z = odom.pose.pose.position.z + # pose.h = odom.pose.pose.position.h + pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z) + pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z) + pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z) + pose.q = [ori.w, ori.x, ori.y, ori.z] + pose.timeStamp = odom.header.stamp.secs + (odom.header.stamp.nsecs * 1e-9) + + return pose + + +class Pose3d (): + + def __init__(self): + + self.x = 0 # X coord [meters] + self.y = 0 # Y coord [meters] + self.z = 0 # Z coord [meters] + self.h = 1 # H param + self.yaw = 0 # Yaw angle[rads] + self.pitch = 0 # Pitch angle[rads] + self.roll = 0 # Roll angle[rads] + self.q = [0, 0, 0, 0] # Quaternion + self.timeStamp = 0 # Time stamp [s] + + def __str__(self): + s = "Pose3D: {\n x: " + str(self.x) + "\n Y: " + str(self.y) + s = s + "\n Z: " + str(self.z) + "\n H: " + str(self.h) + s = s + "\n Yaw: " + str(self.yaw) + "\n Pitch: " + str(self.pitch) + "\n Roll: " + str(self.roll) + s = s + "\n quaternion: " + str(self.q) + "\n timeStamp: " + str(self.timeStamp) + "\n}" + return s + + +class ListenerPose3d: + ''' + ROS Pose3D Subscriber. Pose3D Client to Receive pose3d from ROS nodes. + ''' + def __init__(self, topic): + ''' + ListenerPose3d Constructor. + + @param topic: ROS topic to subscribe + @type topic: String + + ''' + self.topic = topic + self.data = Pose3d() + self.sub = None + self.lock = threading.Lock() + self.start() + + def __callback(self, odom): + ''' + Callback function to receive and save Pose3d. + + @param odom: ROS Odometry received + + @type odom: Odometry + + ''' + pose = odometry2Pose3D(odom) + + self.lock.acquire() + self.data = pose + self.lock.release() + + def stop(self): + ''' + Stops (Unregisters) the client. + + ''' + self.sub.unregister() + + def start(self): + ''' + Starts (Subscribes) the client. + + ''' + print(self.topic) + print(Odometry) + print(self.__callback) + self.sub = rospy.Subscriber(self.topic, Odometry, self.__callback) + + def getPose3d(self): + ''' + Returns last Pose3d. + + @return last JdeRobotTypes Pose3d saved + + ''' + self.lock.acquire() + pose = self.data + self.lock.release() + + return pose + + +rospy.init_node('listener', anonymous=True) +sensor = ListenerPose3d('/carla/ego_vehicle/odometry') + +i=0 + +while i<100: + print(sensor.getPose3d()) + i+=1 \ No newline at end of file From 10ae989ae954b4ad85f20b023ed760a80156a4b8 Mon Sep 17 00:00:00 2001 From: jderobot Date: Thu, 17 Nov 2022 18:08:16 +0100 Subject: [PATCH 04/36] [WIP] Launch both CARLA and Behavior Metrics GUI --- behavior_metrics/driver_carla.py | 124 +++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 behavior_metrics/driver_carla.py diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py new file mode 100644 index 00000000..6614c93c --- /dev/null +++ b/behavior_metrics/driver_carla.py @@ -0,0 +1,124 @@ +import argparse +import os +import sys +import threading + +from pilot import Pilot +from ui.tui.main_view import TUI +from utils import environment, script_manager +from utils.colors import Colors +from utils.configuration import Config +from utils.controller import Controller +from utils.logger import logger +from utils.tmp_world_generator import tmp_world_generator + + +def check_args(argv): + """Function that handles argument checking and parsing. + + Arguments: + argv {list} -- list of arguments from command line. + + Returns: + dict -- dictionary with the detected configuration. + """ + parser = argparse.ArgumentParser(description='Neural Behaviors Suite', + epilog='Enjoy the program! :)') + + parser.add_argument('-c', + '--config', + type=str, + action='append', + required=True, + help='{}Path to the configuration file in YML format.{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-g', + '--gui', + action='store_true', + help='{}Load the GUI (Graphic User Interface). Requires PyQt5 installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-t', + '--tui', + action='store_true', + help='{}Load the TUI (Terminal User Interface). Requires npyscreen installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-s', + '--script', + action='store_true', + help='{}Run Behavior Metrics as script{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-r', + '--random', + action='store_true', + help='{}Run Behavior Metrics F1 with random spawning{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + args = parser.parse_args() + + config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False} + if args.config: + config_data['config'] = [] + for config_file in args.config: + if not os.path.isfile(config_file): + parser.error('{}No such file {} {}'.format(Colors.FAIL, config_file, Colors.ENDC)) + + config_data['config'] = args.config + + if args.gui: + config_data['gui'] = args.gui + + if args.tui: + config_data['tui'] = args.tui + + if args.script: + config_data['script'] = args.script + + if args.random: + config_data['random'] = args.random + + return config_data + +def main_win(configuration, controller): + """shows the Qt main window of the application + + Arguments: + configuration {Config} -- configuration instance for the application + controller {Controller} -- controller part of the MVC model of the application + """ + try: + from PyQt5.QtWidgets import QApplication + from ui.gui.views_controller import ParentWindow, ViewsController + + app = QApplication(sys.argv) + main_window = ParentWindow() + + views_controller = ViewsController(main_window, configuration, controller) + views_controller.show_main_view(True) + + main_window.show() + + app.exec_() + except Exception as e: + logger.error(e) + + + +current_world = 'carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch' + +#environment.launch_env(app_configuration.current_world) +environment.launch_env(current_world) + +config_data = check_args(sys.argv) +app_configuration = Config(config_data['config'][0]) + +print(app_configuration.current_world) + + +controller = Controller() +main_win(app_configuration, controller) + From a9cdc0c94720d57297e95858de0170fbefb0aa5d Mon Sep 17 00:00:00 2001 From: jderobot Date: Fri, 18 Nov 2022 16:18:24 +0100 Subject: [PATCH 05/36] Launch CARLA and run first brain from config file --- .../carla_0_9_13/launch_carla_0_9_13.py | 1 + .../brains/CARLA/brain_carla_autopilot.py | 49 +++ behavior_metrics/brains/brains_handler.py | 4 + behavior_metrics/configs/default_carla.yml | 49 +++ behavior_metrics/driver_carla.py | 17 +- behavior_metrics/robot/actuators.py | 11 +- behavior_metrics/ui/gui/resources/worlds.json | 7 + behavior_metrics/ui/gui/views/toolbar.py | 12 +- behavior_metrics/utils/CARLAController.py | 298 ++++++++++++++++++ 9 files changed, 437 insertions(+), 11 deletions(-) create mode 100644 behavior_metrics/brains/CARLA/brain_carla_autopilot.py create mode 100644 behavior_metrics/configs/default_carla.yml create mode 100644 behavior_metrics/utils/CARLAController.py diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py index 8da76e9d..8ff4b7d5 100755 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py @@ -290,6 +290,7 @@ def restart(self): #spawn_point = carla.Transform(carla.Location(x=2.0, y=70, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) # anticlockwise spawn_point = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + spawn_point = carla.Transform(carla.Location(x=2.0, y=20, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) #spawn_point = carla.Transform(carla.Location(x=10.0, y=331, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) #spawn_point = carla.Transform(carla.Location(x=5.0, y=330, z=1.37), carla.Rotation(pitch=0, yaw=30, roll=0)) diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py new file mode 100644 index 00000000..cc47d012 --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -0,0 +1,49 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + + + +GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR + + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + time.sleep(2) + + + def execute(self): + print('EXECUTE') + #self.motors.sendW(w) + self.motors.sendV(0.1) diff --git a/behavior_metrics/brains/brains_handler.py b/behavior_metrics/brains/brains_handler.py index 3aaaf09e..461f251b 100755 --- a/behavior_metrics/brains/brains_handler.py +++ b/behavior_metrics/brains/brains_handler.py @@ -41,6 +41,10 @@ def load_brain(self, path, model=None): from utils import environment environment.close_gazebo() exec(open(self.brain_path).read()) + elif robot_type == 'CARLA': + module = importlib.import_module(import_name) + Brain = getattr(module, 'Brain') + self.active_brain = Brain(self.sensors, self.actuators, handler=self, config=self.config) else: if import_name in sys.modules: # for reloading sake del sys.modules[import_name] diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml new file mode 100644 index 00000000..d87917f7 --- /dev/null +++ b/behavior_metrics/configs/default_carla.yml @@ -0,0 +1,49 @@ +Behaviors: + Robot: + Sensors: + Cameras: + Camera_0: + Name: 'camera_0' + Topic: '/F1ROS/cameraL/image_raw' + Pose3D: + Pose3D_0: + Name: 'pose3d_0' + Topic: '/F1ROS/odom' + Actuators: + CARLA_Motors: + Motors_0: + Name: 'motors_0' + Topic: '/carla/ego_vehicle/vehicle_control_cmd' + MaxV: 3 + MaxW: 0.3 + + BrainPath: 'brains/CARLA/brain_carla_autopilot.py' + PilotTimeCycle: 50 + Parameters: + ImageTranform: '' + Type: 'CARLA' + Simulation: + World: /home/jderobot/carla-ros-bridge/catkin_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch + Dataset: + In: '/tmp/my_bag.bag' + Out: '' + Stats: + Out: './' + PerfectLap: './perfect_bags/lap-simple-circuit.bag' + Layout: + Frame_0: + Name: frame_0 + Geometry: [1, 1, 2, 2] + Data: rgbimage + Frame_1: + Name: frame_1 + Geometry: [0, 1, 1, 1] + Data: rgbimage + Frame_2: + Name: frame_2 + Geometry: [0, 2, 1, 1] + Data: rgbimage + Frame_3: + Name: frame_3 + Geometry: [0, 3, 3, 1] + Data: rgbimage diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 6614c93c..87a3b936 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -9,6 +9,7 @@ from utils.colors import Colors from utils.configuration import Config from utils.controller import Controller +from utils.CARLAController import CARLAController from utils.logger import logger from utils.tmp_world_generator import tmp_world_generator @@ -108,17 +109,23 @@ def main_win(configuration, controller): -current_world = 'carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch' -#environment.launch_env(app_configuration.current_world) -environment.launch_env(current_world) config_data = check_args(sys.argv) app_configuration = Config(config_data['config'][0]) -print(app_configuration.current_world) +environment.launch_env(app_configuration.current_world) + + +controller = CARLAController() + + +# Launch control +pilot = Pilot(app_configuration, controller, app_configuration.brain_path) +pilot.daemon = True +pilot.start() +logger.info('Executing app') -controller = Controller() main_win(app_configuration, controller) diff --git a/behavior_metrics/robot/actuators.py b/behavior_metrics/robot/actuators.py index 03f1473b..53137b8f 100644 --- a/behavior_metrics/robot/actuators.py +++ b/behavior_metrics/robot/actuators.py @@ -12,7 +12,7 @@ this program. If not, see . """ -from .interfaces.motors import PublisherMotors +from .interfaces.motors import PublisherMotors, PublisherCARLAMotors __author__ = 'fqez' __contributors__ = [] @@ -36,9 +36,12 @@ def __init__(self, actuators_config): # Load motors motors_conf = actuators_config.get('Motors', None) + carla_motors_conf = actuators_config.get('CARLA_Motors', None) self.motors = None if motors_conf: self.motors = self.__create_actuator(motors_conf, 'motor') + elif carla_motors_conf: + self.motors = self.__create_actuator(carla_motors_conf, 'carla_motor') def __create_actuator(self, actuator_config, actuator_type): """Fill the motors dictionary with instances of the motors to control the robot""" @@ -49,14 +52,16 @@ def __create_actuator(self, actuator_config, actuator_type): topic = actuator_config[elem]['Topic'] vmax = actuator_config[elem]['MaxV'] wmax = actuator_config[elem]['MaxW'] - + if 'RL' in actuator_config[elem]: if actuator_config[elem]['RL'] == False: if actuator_type == 'motor': actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) else: if actuator_type == 'motor': - actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) + actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) + elif actuator_type == 'carla_motor': + actuator_dict[name] = PublisherCARLAMotors(topic, vmax, wmax, 0, 0) return actuator_dict def __get_actuator(self, actuator_name, actuator_type): diff --git a/behavior_metrics/ui/gui/resources/worlds.json b/behavior_metrics/ui/gui/resources/worlds.json index 08aa3b43..ad7c0eb3 100644 --- a/behavior_metrics/ui/gui/resources/worlds.json +++ b/behavior_metrics/ui/gui/resources/worlds.json @@ -270,5 +270,12 @@ "world": "/opt/jderobot/share/jderobot/gazebo/launch/roomba_1_house.launch", "description": "Description" } + ], + "CARLA": [ + { + "name": "Town01", + "world": "/home/jderobot/carla-ros-bridge/catkin_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch", + "description": "Description" + } ] } diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py index 0d365826..e9dff78b 100644 --- a/behavior_metrics/ui/gui/views/toolbar.py +++ b/behavior_metrics/ui/gui/views/toolbar.py @@ -28,7 +28,7 @@ from ui.gui.views.stats_window import StatsWindow from ui.gui.views.logo import Logo from ui.gui.views.social import SocialMedia -from utils import constants, environment +from utils import constants, environment, CARLAController __author__ = 'fqez' __contributors__ = [] @@ -664,7 +664,10 @@ def pause_simulation(self): self.confirm_brain.setStyleSheet('color: white') self.controller.pause_pilot() - self.controller.pause_gazebo_simulation() + if type(self.controller) == CARLAController.CARLAController: + print(type(self.controller)) + else: + self.controller.pause_gazebo_simulation() def resume_simulation(self): """Callback that handles simulation resuming""" @@ -684,7 +687,10 @@ def resume_simulation(self): # save to configuration self.configuration.brain_path = brains_path + self.configuration.robot_type + '/' + brain - self.controller.unpause_gazebo_simulation() + if type(self.controller) == CARLAController.CARLAController: + print(type(self.controller)) + else: + self.controller.unpause_gazebo_simulation() def load_brain(self): """Callback that handles brain reloading""" diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py new file mode 100644 index 00000000..d4c3cd1e --- /dev/null +++ b/behavior_metrics/utils/CARLAController.py @@ -0,0 +1,298 @@ +#!/usr/bin/env python + +"""This module contains the controller of the application. + +This application is based on a type of software architecture called Model View Controller. This is the controlling part +of this architecture (controller), which communicates the logical part (model) with the user interface (view). + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import shlex +import subprocess +import threading +import cv2 +import rospy +import os +import time +import rosbag +import json + +from std_srvs.srv import Empty +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from datetime import datetime +from utils.logger import logger +from utils.constants import CIRCUITS_TIMEOUTS +from std_msgs.msg import String +from utils import metrics + +__author__ = 'sergiopaniego' +__contributors__ = [] +__license__ = 'GPLv3' + + +class CARLAController: + """This class defines the controller of the architecture, responsible of the communication between the logic (model) + and the user interface (view). + + Attributes: + data {dict} -- Data to be sent to the view. The key is a frame_if of the view and the value is the data to be + displayed. Depending on the type of data the frame handles (images, laser, etc) + pose3D_data -- Pose data to be sent to the view + recording {bool} -- Flag to determine if a rosbag is being recorded + """ + + def __init__(self): + """ Constructor of the class. """ + pass + self.__data_loc = threading.Lock() + self.__pose_loc = threading.Lock() + self.data = {} + self.pose3D_data = None + self.recording = False + self.cvbridge = CvBridge() + + # GUI update + def update_frame(self, frame_id, data): + """Update the data to be retrieved by the view. + + This function is called by the logic to update the data obtained by the robot to a specific frame in GUI. + + Arguments: + frame_id {str} -- Identifier of the frame that will show the data + data {dict} -- Data to be shown + """ + try: + with self.__data_loc: + self.data[frame_id] = data + except Exception as e: + logger.info(e) + + def get_data(self, frame_id): + """Function to collect data retrieved by the robot for an specific frame of the GUI + + This function is called by the view to get the last updated data to be shown in the GUI. + + Arguments: + frame_id {str} -- Identifier of the frame. + + Returns: + data -- Depending on the caller frame could be image data, laser data, etc. + """ + try: + with self.__data_loc: + data = self.data.get(frame_id, None) + except Exception: + pass + + return data + + def update_pose3d(self, data): + """Update the pose3D data retrieved from the robot + + Arguments: + data {pose3d} -- 3D position of the robot in the environment + """ + try: + with self.__pose_loc: + self.pose3D_data = data + except Exception: + pass + + def get_pose3D(self): + """Function to collect the pose3D data updated in `update_pose3d` function. + + This method is called from the view to collect the pose data and display it in GUI. + + Returns: + pose3d -- 3D position of the robot in the environment + """ + return self.pose3D_data + + # Simulation and dataset + ''' + def reset_gazebo_simulation(self): + logger.info("Restarting simulation") + reset_physics = rospy.ServiceProxy('/gazebo/reset_world', Empty) + reset_physics() + + def pause_gazebo_simulation(self): + logger.info("Pausing simulation") + pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty) + try: + pause_physics() + except Exception as ex: + print(ex) + self.pilot.stop_event.set() + + def unpause_gazebo_simulation(self): + logger.info("Resuming simulation") + unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) + try: + unpause_physics() + except Exception as ex: + print(ex) + self.pilot.stop_event.clear() + + def record_rosbag(self, topics, dataset_name): + """Start the recording process of the dataset using rosbags + + Arguments: + topics {list} -- List of topics to be recorde + dataset_name {str} -- Path of the resulting bag file + """ + + if not self.recording: + logger.info("Recording bag at: {}".format(dataset_name)) + self.recording = True + command = "rosbag record -O " + dataset_name + " " + " ".join(topics) + " __name:=behav_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + self.rosbag_proc = subprocess.Popen(command, stdout=out, stderr=err) + else: + logger.info("Rosbag already recording") + self.stop_record() + + def stop_record(self): + """Stop the rosbag recording process.""" + if self.rosbag_proc and self.recording: + logger.info("Stopping bag recording") + self.recording = False + command = "rosnode kill /behav_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + subprocess.Popen(command, stdout=out, stderr=err) + else: + logger.info("No bag recording") + + def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None): + logger.info("Recording metrics bag at: {}".format(metrics_record_dir_path)) + self.start_time = datetime.now() + current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world) + if brain_counter is not None: + current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path[brain_counter]) + else: + current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path) + self.experiment_metadata = { + 'world': current_world_tail, + 'brain_path': current_brain_tail, + 'robot_type': self.pilot.configuration.robot_type + } + if hasattr(self.pilot.configuration, 'experiment_model'): + if brain_counter is not None: + self.experiment_metadata['experiment_model'] = self.pilot.configuration.experiment_model[brain_counter] + else: + self.experiment_metadata['experiment_model'] = self.pilot.configuration.experiment_model + if hasattr(self.pilot.configuration, 'experiment_name'): + self.experiment_metadata['experiment_name'] = self.pilot.configuration.experiment_name + self.experiment_metadata['experiment_description'] = self.pilot.configuration.experiment_description + if hasattr(self.pilot.configuration, 'experiment_timeouts'): + self.experiment_metadata['experiment_timeout'] = self.pilot.configuration.experiment_timeouts[world_counter] + else: + self.experiment_metadata['experiment_timeout'] = CIRCUITS_TIMEOUTS[os.path.basename(self.experiment_metadata['world'])] * 1.1 + self.experiment_metadata['experiment_repetition'] = repetition_counter + + self.perfect_lap_filename = perfect_lap_filename + self.metrics_record_dir_path = metrics_record_dir_path + time_str = time.strftime("%Y%m%d-%H%M%S") + self.experiment_metrics_filename = time_str + '.bag' + topics = ['/F1ROS/odom', '/clock'] + command = "rosbag record -O " + self.experiment_metrics_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + self.proc = subprocess.Popen(command, stdout=out, stderr=err) + + def stop_recording_metrics(self, pitch_error=False): + logger.info("Stopping metrics bag recording") + end_time = time.time() + + command = "rosnode kill /behav_metrics_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + subprocess.Popen(command, stdout=out, stderr=err) + + # Wait for rosbag file to be closed. Otherwise it causes error + while os.path.isfile(self.experiment_metrics_filename + '.active'): + pass + + perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(self.perfect_lap_filename) + if not pitch_error: + self.experiment_metrics = metrics.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) + self.experiment_metrics, first_image = self.pilot.calculate_metrics(self.experiment_metrics) + + try: + self.save_metrics(first_image) + except rosbag.bag.ROSBagException: + logger.info("Bag was empty, Try Again") + + + else: + self.experiment_metrics = {'percentage_completed': 0, 'average_speed': 0, 'lap_seconds': 0, + 'circuit_diameter': 0, 'position_deviation_mae': 0, + 'position_deviation_total_err': 0, 'experiment_total_simulated_time': 0, + 'completed_distance': 0} + logger.info("* Experiment total real time -> " + str(end_time - self.pilot.pilot_start_time)) + self.experiment_metrics['experiment_total_real_time'] = end_time - self.pilot.pilot_start_time + + time_str = time.strftime("%Y%m%d-%H%M%S") + + with open(time_str + '.json', 'w') as f: + json.dump(self.experiment_metrics, f) + logger.info("Metrics stored in JSON file") + + logger.info("Stopping metrics bag recording") + + def save_metrics(self, first_image): + experiment_metadata_str = json.dumps(self.experiment_metadata) + experiment_metrics_str = json.dumps(self.experiment_metrics) + with rosbag.Bag(self.experiment_metrics_filename, 'a') as bag: + experiment_metadata_msg = String(data=experiment_metadata_str) + experiment_metrics_msg = String(data=experiment_metrics_str) + bag.write('/metadata', experiment_metadata_msg, rospy.Time(bag.get_end_time())) + bag.write('/experiment_metrics', experiment_metrics_msg, rospy.Time(bag.get_end_time())) + if first_image is not None and first_image.shape == (480, 640, 3): + rospy.loginfo('Image received and sent to /first_image') + bag.write('/first_image', self.cvbridge.cv2_to_imgmsg(first_image), rospy.Time(bag.get_end_time())) + else: + rospy.loginfo('Error: Image Broken and /first_image Skipped: {}'.format(first_image)) + bag.close() + ''' + def reload_brain(self, brain, model=None): + """Helper function to reload the current brain from the GUI. + + Arguments: + brain {srt} -- Brain to be reloadaed. + """ + logger.info("Reloading brain... {}".format(brain)) + + self.pause_pilot() + self.pilot.reload_brain(brain, model) + + # Helper functions (connection with logic) + + def set_pilot(self, pilot): + self.pilot = pilot + + def stop_pilot(self): + self.pilot.kill_event.set() + + def pause_pilot(self): + self.pilot.stop_event.set() + + def resume_pilot(self): + self.start_time = datetime.now() + self.pilot.start_time = datetime.now() + self.pilot.stop_event.clear() + + def initialize_robot(self): + self.pause_pilot() + self.pilot.initialize_robot() From ac31025120ba2d906df46006ecae78a1cd7a4248 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 18 Nov 2022 16:18:24 +0100 Subject: [PATCH 06/36] Launch CARLA and run first brain from config file --- .../carla_0_9_13/launch_carla_0_9_13.py | 1 + .../brains/CARLA/brain_carla_autopilot.py | 49 +++ behavior_metrics/brains/brains_handler.py | 4 + behavior_metrics/configs/default_carla.yml | 49 +++ behavior_metrics/driver_carla.py | 17 +- behavior_metrics/robot/actuators.py | 11 +- behavior_metrics/ui/gui/resources/worlds.json | 7 + behavior_metrics/ui/gui/views/toolbar.py | 12 +- behavior_metrics/utils/CARLAController.py | 298 ++++++++++++++++++ 9 files changed, 437 insertions(+), 11 deletions(-) create mode 100644 behavior_metrics/brains/CARLA/brain_carla_autopilot.py create mode 100644 behavior_metrics/configs/default_carla.yml create mode 100644 behavior_metrics/utils/CARLAController.py diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py index 8da76e9d..8ff4b7d5 100755 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py +++ b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py @@ -290,6 +290,7 @@ def restart(self): #spawn_point = carla.Transform(carla.Location(x=2.0, y=70, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) # anticlockwise spawn_point = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) + spawn_point = carla.Transform(carla.Location(x=2.0, y=20, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) #spawn_point = carla.Transform(carla.Location(x=10.0, y=331, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) #spawn_point = carla.Transform(carla.Location(x=5.0, y=330, z=1.37), carla.Rotation(pitch=0, yaw=30, roll=0)) diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py new file mode 100644 index 00000000..cc47d012 --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -0,0 +1,49 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + + + +GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR + + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + time.sleep(2) + + + def execute(self): + print('EXECUTE') + #self.motors.sendW(w) + self.motors.sendV(0.1) diff --git a/behavior_metrics/brains/brains_handler.py b/behavior_metrics/brains/brains_handler.py index 3aaaf09e..461f251b 100755 --- a/behavior_metrics/brains/brains_handler.py +++ b/behavior_metrics/brains/brains_handler.py @@ -41,6 +41,10 @@ def load_brain(self, path, model=None): from utils import environment environment.close_gazebo() exec(open(self.brain_path).read()) + elif robot_type == 'CARLA': + module = importlib.import_module(import_name) + Brain = getattr(module, 'Brain') + self.active_brain = Brain(self.sensors, self.actuators, handler=self, config=self.config) else: if import_name in sys.modules: # for reloading sake del sys.modules[import_name] diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml new file mode 100644 index 00000000..d87917f7 --- /dev/null +++ b/behavior_metrics/configs/default_carla.yml @@ -0,0 +1,49 @@ +Behaviors: + Robot: + Sensors: + Cameras: + Camera_0: + Name: 'camera_0' + Topic: '/F1ROS/cameraL/image_raw' + Pose3D: + Pose3D_0: + Name: 'pose3d_0' + Topic: '/F1ROS/odom' + Actuators: + CARLA_Motors: + Motors_0: + Name: 'motors_0' + Topic: '/carla/ego_vehicle/vehicle_control_cmd' + MaxV: 3 + MaxW: 0.3 + + BrainPath: 'brains/CARLA/brain_carla_autopilot.py' + PilotTimeCycle: 50 + Parameters: + ImageTranform: '' + Type: 'CARLA' + Simulation: + World: /home/jderobot/carla-ros-bridge/catkin_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch + Dataset: + In: '/tmp/my_bag.bag' + Out: '' + Stats: + Out: './' + PerfectLap: './perfect_bags/lap-simple-circuit.bag' + Layout: + Frame_0: + Name: frame_0 + Geometry: [1, 1, 2, 2] + Data: rgbimage + Frame_1: + Name: frame_1 + Geometry: [0, 1, 1, 1] + Data: rgbimage + Frame_2: + Name: frame_2 + Geometry: [0, 2, 1, 1] + Data: rgbimage + Frame_3: + Name: frame_3 + Geometry: [0, 3, 3, 1] + Data: rgbimage diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 6614c93c..87a3b936 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -9,6 +9,7 @@ from utils.colors import Colors from utils.configuration import Config from utils.controller import Controller +from utils.CARLAController import CARLAController from utils.logger import logger from utils.tmp_world_generator import tmp_world_generator @@ -108,17 +109,23 @@ def main_win(configuration, controller): -current_world = 'carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch' -#environment.launch_env(app_configuration.current_world) -environment.launch_env(current_world) config_data = check_args(sys.argv) app_configuration = Config(config_data['config'][0]) -print(app_configuration.current_world) +environment.launch_env(app_configuration.current_world) + + +controller = CARLAController() + + +# Launch control +pilot = Pilot(app_configuration, controller, app_configuration.brain_path) +pilot.daemon = True +pilot.start() +logger.info('Executing app') -controller = Controller() main_win(app_configuration, controller) diff --git a/behavior_metrics/robot/actuators.py b/behavior_metrics/robot/actuators.py index 03f1473b..53137b8f 100644 --- a/behavior_metrics/robot/actuators.py +++ b/behavior_metrics/robot/actuators.py @@ -12,7 +12,7 @@ this program. If not, see . """ -from .interfaces.motors import PublisherMotors +from .interfaces.motors import PublisherMotors, PublisherCARLAMotors __author__ = 'fqez' __contributors__ = [] @@ -36,9 +36,12 @@ def __init__(self, actuators_config): # Load motors motors_conf = actuators_config.get('Motors', None) + carla_motors_conf = actuators_config.get('CARLA_Motors', None) self.motors = None if motors_conf: self.motors = self.__create_actuator(motors_conf, 'motor') + elif carla_motors_conf: + self.motors = self.__create_actuator(carla_motors_conf, 'carla_motor') def __create_actuator(self, actuator_config, actuator_type): """Fill the motors dictionary with instances of the motors to control the robot""" @@ -49,14 +52,16 @@ def __create_actuator(self, actuator_config, actuator_type): topic = actuator_config[elem]['Topic'] vmax = actuator_config[elem]['MaxV'] wmax = actuator_config[elem]['MaxW'] - + if 'RL' in actuator_config[elem]: if actuator_config[elem]['RL'] == False: if actuator_type == 'motor': actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) else: if actuator_type == 'motor': - actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) + actuator_dict[name] = PublisherMotors(topic, vmax, wmax, 0, 0) + elif actuator_type == 'carla_motor': + actuator_dict[name] = PublisherCARLAMotors(topic, vmax, wmax, 0, 0) return actuator_dict def __get_actuator(self, actuator_name, actuator_type): diff --git a/behavior_metrics/ui/gui/resources/worlds.json b/behavior_metrics/ui/gui/resources/worlds.json index 08aa3b43..ad7c0eb3 100644 --- a/behavior_metrics/ui/gui/resources/worlds.json +++ b/behavior_metrics/ui/gui/resources/worlds.json @@ -270,5 +270,12 @@ "world": "/opt/jderobot/share/jderobot/gazebo/launch/roomba_1_house.launch", "description": "Description" } + ], + "CARLA": [ + { + "name": "Town01", + "world": "/home/jderobot/carla-ros-bridge/catkin_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch", + "description": "Description" + } ] } diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py index 0d365826..e9dff78b 100644 --- a/behavior_metrics/ui/gui/views/toolbar.py +++ b/behavior_metrics/ui/gui/views/toolbar.py @@ -28,7 +28,7 @@ from ui.gui.views.stats_window import StatsWindow from ui.gui.views.logo import Logo from ui.gui.views.social import SocialMedia -from utils import constants, environment +from utils import constants, environment, CARLAController __author__ = 'fqez' __contributors__ = [] @@ -664,7 +664,10 @@ def pause_simulation(self): self.confirm_brain.setStyleSheet('color: white') self.controller.pause_pilot() - self.controller.pause_gazebo_simulation() + if type(self.controller) == CARLAController.CARLAController: + print(type(self.controller)) + else: + self.controller.pause_gazebo_simulation() def resume_simulation(self): """Callback that handles simulation resuming""" @@ -684,7 +687,10 @@ def resume_simulation(self): # save to configuration self.configuration.brain_path = brains_path + self.configuration.robot_type + '/' + brain - self.controller.unpause_gazebo_simulation() + if type(self.controller) == CARLAController.CARLAController: + print(type(self.controller)) + else: + self.controller.unpause_gazebo_simulation() def load_brain(self): """Callback that handles brain reloading""" diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py new file mode 100644 index 00000000..d4c3cd1e --- /dev/null +++ b/behavior_metrics/utils/CARLAController.py @@ -0,0 +1,298 @@ +#!/usr/bin/env python + +"""This module contains the controller of the application. + +This application is based on a type of software architecture called Model View Controller. This is the controlling part +of this architecture (controller), which communicates the logical part (model) with the user interface (view). + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import shlex +import subprocess +import threading +import cv2 +import rospy +import os +import time +import rosbag +import json + +from std_srvs.srv import Empty +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from datetime import datetime +from utils.logger import logger +from utils.constants import CIRCUITS_TIMEOUTS +from std_msgs.msg import String +from utils import metrics + +__author__ = 'sergiopaniego' +__contributors__ = [] +__license__ = 'GPLv3' + + +class CARLAController: + """This class defines the controller of the architecture, responsible of the communication between the logic (model) + and the user interface (view). + + Attributes: + data {dict} -- Data to be sent to the view. The key is a frame_if of the view and the value is the data to be + displayed. Depending on the type of data the frame handles (images, laser, etc) + pose3D_data -- Pose data to be sent to the view + recording {bool} -- Flag to determine if a rosbag is being recorded + """ + + def __init__(self): + """ Constructor of the class. """ + pass + self.__data_loc = threading.Lock() + self.__pose_loc = threading.Lock() + self.data = {} + self.pose3D_data = None + self.recording = False + self.cvbridge = CvBridge() + + # GUI update + def update_frame(self, frame_id, data): + """Update the data to be retrieved by the view. + + This function is called by the logic to update the data obtained by the robot to a specific frame in GUI. + + Arguments: + frame_id {str} -- Identifier of the frame that will show the data + data {dict} -- Data to be shown + """ + try: + with self.__data_loc: + self.data[frame_id] = data + except Exception as e: + logger.info(e) + + def get_data(self, frame_id): + """Function to collect data retrieved by the robot for an specific frame of the GUI + + This function is called by the view to get the last updated data to be shown in the GUI. + + Arguments: + frame_id {str} -- Identifier of the frame. + + Returns: + data -- Depending on the caller frame could be image data, laser data, etc. + """ + try: + with self.__data_loc: + data = self.data.get(frame_id, None) + except Exception: + pass + + return data + + def update_pose3d(self, data): + """Update the pose3D data retrieved from the robot + + Arguments: + data {pose3d} -- 3D position of the robot in the environment + """ + try: + with self.__pose_loc: + self.pose3D_data = data + except Exception: + pass + + def get_pose3D(self): + """Function to collect the pose3D data updated in `update_pose3d` function. + + This method is called from the view to collect the pose data and display it in GUI. + + Returns: + pose3d -- 3D position of the robot in the environment + """ + return self.pose3D_data + + # Simulation and dataset + ''' + def reset_gazebo_simulation(self): + logger.info("Restarting simulation") + reset_physics = rospy.ServiceProxy('/gazebo/reset_world', Empty) + reset_physics() + + def pause_gazebo_simulation(self): + logger.info("Pausing simulation") + pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty) + try: + pause_physics() + except Exception as ex: + print(ex) + self.pilot.stop_event.set() + + def unpause_gazebo_simulation(self): + logger.info("Resuming simulation") + unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) + try: + unpause_physics() + except Exception as ex: + print(ex) + self.pilot.stop_event.clear() + + def record_rosbag(self, topics, dataset_name): + """Start the recording process of the dataset using rosbags + + Arguments: + topics {list} -- List of topics to be recorde + dataset_name {str} -- Path of the resulting bag file + """ + + if not self.recording: + logger.info("Recording bag at: {}".format(dataset_name)) + self.recording = True + command = "rosbag record -O " + dataset_name + " " + " ".join(topics) + " __name:=behav_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + self.rosbag_proc = subprocess.Popen(command, stdout=out, stderr=err) + else: + logger.info("Rosbag already recording") + self.stop_record() + + def stop_record(self): + """Stop the rosbag recording process.""" + if self.rosbag_proc and self.recording: + logger.info("Stopping bag recording") + self.recording = False + command = "rosnode kill /behav_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + subprocess.Popen(command, stdout=out, stderr=err) + else: + logger.info("No bag recording") + + def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None): + logger.info("Recording metrics bag at: {}".format(metrics_record_dir_path)) + self.start_time = datetime.now() + current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world) + if brain_counter is not None: + current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path[brain_counter]) + else: + current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path) + self.experiment_metadata = { + 'world': current_world_tail, + 'brain_path': current_brain_tail, + 'robot_type': self.pilot.configuration.robot_type + } + if hasattr(self.pilot.configuration, 'experiment_model'): + if brain_counter is not None: + self.experiment_metadata['experiment_model'] = self.pilot.configuration.experiment_model[brain_counter] + else: + self.experiment_metadata['experiment_model'] = self.pilot.configuration.experiment_model + if hasattr(self.pilot.configuration, 'experiment_name'): + self.experiment_metadata['experiment_name'] = self.pilot.configuration.experiment_name + self.experiment_metadata['experiment_description'] = self.pilot.configuration.experiment_description + if hasattr(self.pilot.configuration, 'experiment_timeouts'): + self.experiment_metadata['experiment_timeout'] = self.pilot.configuration.experiment_timeouts[world_counter] + else: + self.experiment_metadata['experiment_timeout'] = CIRCUITS_TIMEOUTS[os.path.basename(self.experiment_metadata['world'])] * 1.1 + self.experiment_metadata['experiment_repetition'] = repetition_counter + + self.perfect_lap_filename = perfect_lap_filename + self.metrics_record_dir_path = metrics_record_dir_path + time_str = time.strftime("%Y%m%d-%H%M%S") + self.experiment_metrics_filename = time_str + '.bag' + topics = ['/F1ROS/odom', '/clock'] + command = "rosbag record -O " + self.experiment_metrics_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + self.proc = subprocess.Popen(command, stdout=out, stderr=err) + + def stop_recording_metrics(self, pitch_error=False): + logger.info("Stopping metrics bag recording") + end_time = time.time() + + command = "rosnode kill /behav_metrics_bag" + command = shlex.split(command) + with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: + subprocess.Popen(command, stdout=out, stderr=err) + + # Wait for rosbag file to be closed. Otherwise it causes error + while os.path.isfile(self.experiment_metrics_filename + '.active'): + pass + + perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(self.perfect_lap_filename) + if not pitch_error: + self.experiment_metrics = metrics.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) + self.experiment_metrics, first_image = self.pilot.calculate_metrics(self.experiment_metrics) + + try: + self.save_metrics(first_image) + except rosbag.bag.ROSBagException: + logger.info("Bag was empty, Try Again") + + + else: + self.experiment_metrics = {'percentage_completed': 0, 'average_speed': 0, 'lap_seconds': 0, + 'circuit_diameter': 0, 'position_deviation_mae': 0, + 'position_deviation_total_err': 0, 'experiment_total_simulated_time': 0, + 'completed_distance': 0} + logger.info("* Experiment total real time -> " + str(end_time - self.pilot.pilot_start_time)) + self.experiment_metrics['experiment_total_real_time'] = end_time - self.pilot.pilot_start_time + + time_str = time.strftime("%Y%m%d-%H%M%S") + + with open(time_str + '.json', 'w') as f: + json.dump(self.experiment_metrics, f) + logger.info("Metrics stored in JSON file") + + logger.info("Stopping metrics bag recording") + + def save_metrics(self, first_image): + experiment_metadata_str = json.dumps(self.experiment_metadata) + experiment_metrics_str = json.dumps(self.experiment_metrics) + with rosbag.Bag(self.experiment_metrics_filename, 'a') as bag: + experiment_metadata_msg = String(data=experiment_metadata_str) + experiment_metrics_msg = String(data=experiment_metrics_str) + bag.write('/metadata', experiment_metadata_msg, rospy.Time(bag.get_end_time())) + bag.write('/experiment_metrics', experiment_metrics_msg, rospy.Time(bag.get_end_time())) + if first_image is not None and first_image.shape == (480, 640, 3): + rospy.loginfo('Image received and sent to /first_image') + bag.write('/first_image', self.cvbridge.cv2_to_imgmsg(first_image), rospy.Time(bag.get_end_time())) + else: + rospy.loginfo('Error: Image Broken and /first_image Skipped: {}'.format(first_image)) + bag.close() + ''' + def reload_brain(self, brain, model=None): + """Helper function to reload the current brain from the GUI. + + Arguments: + brain {srt} -- Brain to be reloadaed. + """ + logger.info("Reloading brain... {}".format(brain)) + + self.pause_pilot() + self.pilot.reload_brain(brain, model) + + # Helper functions (connection with logic) + + def set_pilot(self, pilot): + self.pilot = pilot + + def stop_pilot(self): + self.pilot.kill_event.set() + + def pause_pilot(self): + self.pilot.stop_event.set() + + def resume_pilot(self): + self.start_time = datetime.now() + self.pilot.start_time = datetime.now() + self.pilot.stop_event.clear() + + def initialize_robot(self): + self.pause_pilot() + self.pilot.initialize_robot() From 84c09402a73e438859b706fc2bb31def17fccac4 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 18 Nov 2022 16:45:42 +0100 Subject: [PATCH 07/36] Close all processes on finish --- .../brains/CARLA/brain_carla_autopilot.py | 2 +- behavior_metrics/driver_carla.py | 3 + behavior_metrics/robot/interfaces/motors.py | 90 +++++++++++++++++++ 3 files changed, 94 insertions(+), 1 deletion(-) diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py index cc47d012..132f01fd 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -46,4 +46,4 @@ def __init__(self, sensors, actuators, handler, config=None): def execute(self): print('EXECUTE') #self.motors.sendW(w) - self.motors.sendV(0.1) + self.motors.sendV(1) diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 87a3b936..a2e694f9 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -129,3 +129,6 @@ def main_win(configuration, controller): main_win(app_configuration, controller) +logger.info('closing all processes...') +pilot.kill_event.set() +environment.close_gazebo() diff --git a/behavior_metrics/robot/interfaces/motors.py b/behavior_metrics/robot/interfaces/motors.py index 457e9a82..803647a1 100644 --- a/behavior_metrics/robot/interfaces/motors.py +++ b/behavior_metrics/robot/interfaces/motors.py @@ -3,6 +3,8 @@ import threading from .threadPublisher import ThreadPublisher +from carla_msgs.msg import CarlaEgoVehicleControl + def cmdvel2Twist(vel): @@ -17,6 +19,13 @@ def cmdvel2Twist(vel): return tw +def cmdvel2CarlaEgoVehicleControl(vel): + vehicle_control = CarlaEgoVehicleControl() + vehicle_control.throttle = vel.vx + return vehicle_control + + + class CMDVel (): def __init__(self): @@ -118,3 +127,84 @@ def sendAZ(self, az): self.lock.acquire() self.data.az = az self.lock.release() + + +class PublisherCARLAMotors: + + def __init__(self, topic, maxV, maxW, v, w): + + self.maxW = maxW + self.maxV = maxV + self.v = v + self.w = w + self.topic = topic + self.data = CMDVel() + self.pub = rospy.Publisher(self.topic, CarlaEgoVehicleControl, queue_size=1) + rospy.init_node("CARLAMotors") + self.lock = threading.Lock() + self.kill_event = threading.Event() + self.thread = ThreadPublisher(self, self.kill_event) + self.thread.daemon = True + self.start() + + def publish(self): + self.lock.acquire() + vehicle_control = cmdvel2CarlaEgoVehicleControl(self.data) + self.lock.release() + self.pub.publish(vehicle_control) + + def stop(self): + self.kill_event.set() + self.pub.unregister() + + def start(self): + + self.kill_event.clear() + self.thread.start() + + def getTopic(self): + return self.topic + + def getMaxW(self): + return self.maxW + + def getMaxV(self): + return self.maxV + + def sendVelocities(self, vel): + + self.lock.acquire() + self.data = vel + self.lock.release() + + def sendV(self, v): + + self.sendVX(v) + self.v = v + + def sendL(self, l): + + self.sendVY(l) + + def sendW(self, w): + + self.sendAZ(w) + self.w = w + + def sendVX(self, vx): + + self.lock.acquire() + self.data.vx = vx + self.lock.release() + + def sendVY(self, vy): + + self.lock.acquire() + self.data.vy = vy + self.lock.release() + + def sendAZ(self, az): + + self.lock.acquire() + self.data.az = az + self.lock.release() From fadf52db92832883e715bb702fc9b74a7f732f85 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 18 Nov 2022 17:46:13 +0100 Subject: [PATCH 08/36] Cameras working --- .../brains/CARLA/brain_carla_autopilot.py | 23 ++++++++++++++++++- behavior_metrics/configs/default_carla.yml | 15 +++++++++--- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py index 132f01fd..b47a0793 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -20,6 +20,10 @@ class Brain: def __init__(self, sensors, actuators, handler, config=None): self.camera = sensors.get_camera('camera_0') + self.camera_1 = sensors.get_camera('camera_1') + self.camera_2 = sensors.get_camera('camera_2') + self.camera_3 = sensors.get_camera('camera_3') + self.motors = actuators.get_motor('motors_0') self.handler = handler self.config = config @@ -42,8 +46,25 @@ def __init__(self, sensors, actuators, handler, config=None): time.sleep(2) + def update_frame(self, frame_id, data): + """Update the information to be shown in one of the GUI's frames. + + Arguments: + frame_id {str} -- Id of the frame that will represent the data + data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) + """ + self.handler.update_frame(frame_id, data) def execute(self): - print('EXECUTE') + image = self.camera.getImage().data + image_1 = self.camera_1.getImage().data + image_2 = self.camera_2.getImage().data + image_3 = self.camera_3.getImage().data + #self.motors.sendW(w) self.motors.sendV(1) + self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index d87917f7..876e59b7 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -4,7 +4,16 @@ Behaviors: Cameras: Camera_0: Name: 'camera_0' - Topic: '/F1ROS/cameraL/image_raw' + Topic: '/carla/ego_vehicle/rgb_front/image' + Camera_1: + Name: 'camera_1' + Topic: '/carla/ego_vehicle/rgb_view/image' + Camera_2: + Name: 'camera_2' + Topic: '/carla/ego_vehicle/semantic_segmentation_front/image' + Camera_3: + Name: 'camera_3' + Topic: '/carla/ego_vehicle/dvs_front/image' Pose3D: Pose3D_0: Name: 'pose3d_0' @@ -33,7 +42,7 @@ Behaviors: Layout: Frame_0: Name: frame_0 - Geometry: [1, 1, 2, 2] + Geometry: [1, 1, 1, 1] Data: rgbimage Frame_1: Name: frame_1 @@ -45,5 +54,5 @@ Behaviors: Data: rgbimage Frame_3: Name: frame_3 - Geometry: [0, 3, 3, 1] + Geometry: [1, 2, 1, 1] Data: rgbimage From 8e3b9af44e4ba0dd97ad96deda3b935e6eeeaa59 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 18 Nov 2022 17:59:48 +0100 Subject: [PATCH 09/36] Odometry working for CARLA --- behavior_metrics/brains/CARLA/brain_carla_autopilot.py | 7 +++++++ behavior_metrics/configs/default_carla.yml | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py index b47a0793..cb0a7552 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -24,6 +24,8 @@ def __init__(self, sensors, actuators, handler, config=None): self.camera_2 = sensors.get_camera('camera_2') self.camera_3 = sensors.get_camera('camera_3') + self.pose = sensors.get_pose3d('pose3d_0') + self.motors = actuators.get_motor('motors_0') self.handler = handler self.config = config @@ -55,6 +57,9 @@ def update_frame(self, frame_id, data): """ self.handler.update_frame(frame_id, data) + def update_pose(self, pose_data): + self.handler.update_pose3d(pose_data) + def execute(self): image = self.camera.getImage().data image_1 = self.camera_1.getImage().data @@ -67,4 +72,6 @@ def execute(self): self.update_frame('frame_1', image_1) self.update_frame('frame_2', image_2) self.update_frame('frame_3', image_3) + self.update_pose(self.pose.getPose3d()) + #print(self.pose.getPose3d()) diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index 876e59b7..cd026246 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -17,7 +17,7 @@ Behaviors: Pose3D: Pose3D_0: Name: 'pose3d_0' - Topic: '/F1ROS/odom' + Topic: '/carla/ego_vehicle/odometry' Actuators: CARLA_Motors: Motors_0: From e6a52bf44fe8a47326fac30dfae7a723dd8befbf Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 18 Nov 2022 18:36:08 +0100 Subject: [PATCH 10/36] Steering added to motors --- .../brains/CARLA/brain_carla_slow_and_turn.py | 76 +++++++++++++++++++ behavior_metrics/robot/interfaces/motors.py | 7 ++ 2 files changed, 83 insertions(+) create mode 100644 behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py diff --git a/behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py b/behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py new file mode 100644 index 00000000..88e685d4 --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_slow_and_turn.py @@ -0,0 +1,76 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + + + +GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR + + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.camera_1 = sensors.get_camera('camera_1') + self.camera_2 = sensors.get_camera('camera_2') + self.camera_3 = sensors.get_camera('camera_3') + + self.pose = sensors.get_pose3d('pose3d_0') + + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + time.sleep(2) + + def update_frame(self, frame_id, data): + """Update the information to be shown in one of the GUI's frames. + + Arguments: + frame_id {str} -- Id of the frame that will represent the data + data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) + """ + self.handler.update_frame(frame_id, data) + + def update_pose(self, pose_data): + self.handler.update_pose3d(pose_data) + + def execute(self): + image = self.camera.getImage().data + image_1 = self.camera_1.getImage().data + image_2 = self.camera_2.getImage().data + image_3 = self.camera_3.getImage().data + + self.motors.sendW(1) + self.motors.sendV(0.5) + self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + self.update_pose(self.pose.getPose3d()) + #print(self.pose.getPose3d()) \ No newline at end of file diff --git a/behavior_metrics/robot/interfaces/motors.py b/behavior_metrics/robot/interfaces/motors.py index 803647a1..373b2e51 100644 --- a/behavior_metrics/robot/interfaces/motors.py +++ b/behavior_metrics/robot/interfaces/motors.py @@ -22,6 +22,13 @@ def cmdvel2Twist(vel): def cmdvel2CarlaEgoVehicleControl(vel): vehicle_control = CarlaEgoVehicleControl() vehicle_control.throttle = vel.vx + vehicle_control.steer = vel.az + vehicle_control.brake = 0.0 + vehicle_control.hand_brake = False + vehicle_control.reverse = False + vehicle_control.gear = 0 + vehicle_control.manual_gear_shift = False + return vehicle_control From c059ff17ce14fbed11a3a9091b73772f1eae6de6 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 21 Nov 2022 11:59:21 +0100 Subject: [PATCH 11/36] Bird Eye View support added --- .../brains/CARLA/brain_carla_autopilot.py | 26 ++++++++++++++++--- behavior_metrics/configs/default_carla.yml | 4 +++ .../robot/interfaces/birdeyeview.py | 21 +++++++++++++++ behavior_metrics/robot/sensors.py | 22 ++++++++++++++++ 4 files changed, 69 insertions(+), 4 deletions(-) create mode 100644 behavior_metrics/robot/interfaces/birdeyeview.py diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py index cb0a7552..7a744b5c 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -6,6 +6,7 @@ import numpy as np import threading import time +import carla from albumentations import ( Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare ) @@ -26,6 +27,8 @@ def __init__(self, sensors, actuators, handler, config=None): self.pose = sensors.get_pose3d('pose3d_0') + self.bird_eye_view = sensors.get_bird_eye_view('bird_eye_view_0') + self.motors = actuators.get_motor('motors_0') self.handler = handler self.config = config @@ -46,6 +49,12 @@ def __init__(self, sensors, actuators, handler, config=None): self.previous_w_normalized = None self.suddenness_distance = [] + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) # seconds + world = client.get_world() + time.sleep(3) + self.vehicle = world.get_actors().filter('vehicle.*')[0] + time.sleep(2) def update_frame(self, frame_id, data): @@ -66,12 +75,21 @@ def execute(self): image_2 = self.camera_2.getImage().data image_3 = self.camera_3.getImage().data + bird_eye_view_1 = self.bird_eye_view.getImage(self.vehicle) + + #print(self.bird_eye_view.getImage(self.vehicle)) + #self.motors.sendW(w) self.motors.sendV(1) - self.update_frame('frame_0', image) - self.update_frame('frame_1', image_1) - self.update_frame('frame_2', image_2) - self.update_frame('frame_3', image_3) + #self.update_frame('frame_0', image) + #self.update_frame('frame_1', image_1) + #self.update_frame('frame_2', image_2) + #self.update_frame('frame_3', image_3) + + self.update_frame('frame_0', bird_eye_view_1) + self.update_frame('frame_1', bird_eye_view_1) + self.update_frame('frame_2', bird_eye_view_1) + self.update_pose(self.pose.getPose3d()) #print(self.pose.getPose3d()) diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index cd026246..bb28a659 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -18,6 +18,10 @@ Behaviors: Pose3D_0: Name: 'pose3d_0' Topic: '/carla/ego_vehicle/odometry' + BirdEyeView: + BirdEyeView_0: + Name: 'bird_eye_view_0' + Topic: '' Actuators: CARLA_Motors: Motors_0: diff --git a/behavior_metrics/robot/interfaces/birdeyeview.py b/behavior_metrics/robot/interfaces/birdeyeview.py new file mode 100644 index 00000000..9112e26b --- /dev/null +++ b/behavior_metrics/robot/interfaces/birdeyeview.py @@ -0,0 +1,21 @@ +import carla +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions + +class BirdEyeView: + + def __init__(self): + client = carla.Client('localhost', 2000) + client.set_timeout(5.0) + self.birdview_producer = BirdViewProducer( + client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY + ) + + def getImage(self, vehicle): + birdview = self.birdview_producer.produce( + agent_vehicle=vehicle # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + return image diff --git a/behavior_metrics/robot/sensors.py b/behavior_metrics/robot/sensors.py index 702e6821..08a5aeb9 100644 --- a/behavior_metrics/robot/sensors.py +++ b/behavior_metrics/robot/sensors.py @@ -15,6 +15,7 @@ from robot.interfaces.camera import ListenerCamera from robot.interfaces.laser import ListenerLaser from robot.interfaces.pose3d import ListenerPose3d +from robot.interfaces.birdeyeview import BirdEyeView __author__ = 'fqez' __contributors__ = [] @@ -54,6 +55,12 @@ def __init__(self, sensors_config): if pose3d_conf: self.pose3d = self.__create_sensor(pose3d_conf, 'pose3d') + # Load BirdEyeView + bird_eye_view_conf = sensors_config.get('BirdEyeView', None) + self.bird_eye_view = None + if bird_eye_view_conf: + self.bird_eye_view = self.__create_sensor(bird_eye_view_conf, 'bird_eye_view') + def __create_sensor(self, sensor_config, sensor_type): """Fill the sensor dictionary with instances of the sensor_type and sensor_config""" sensor_dict = {} @@ -66,6 +73,8 @@ def __create_sensor(self, sensor_config, sensor_type): sensor_dict[name] = ListenerLaser(topic) elif sensor_type == 'pose3d': sensor_dict[name] = ListenerPose3d(topic) + elif sensor_type == 'bird_eye_view': + sensor_dict[name] = BirdEyeView() return sensor_dict @@ -80,6 +89,8 @@ def __get_sensor(self, sensor_name, sensor_type): sensor = self.lasers[sensor_name] elif sensor_type == 'pose3d': sensor = self.pose3d[sensor_name] + elif sensor_type == 'bird_eye_view': + sensor = self.bird_eye_view[sensor_name] except KeyError: return "[ERROR] No existing camera with {} name.".format(sensor_name) @@ -118,6 +129,17 @@ def get_pose3d(self, pose_name): """ return self.__get_sensor(pose_name, 'pose3d') + def get_bird_eye_view(self, bird_eye_view_name): + """Retrieve an specific existing bird eye view + + Arguments: + bird_eye_view_name {str} -- Name of the birdeyeview to be retrieved + + Returns: + robot.interfaces.birdeyeview.BirdEyeView instance -- birdeyeview instance + """ + return self.__get_sensor(bird_eye_view_name, 'bird_eye_view') + def kill(self): """Destroy all the running sensors""" if self.cameras: From 3fdc42b569eeae65581122173ab315bfadb7322b Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 21 Nov 2022 14:05:04 +0100 Subject: [PATCH 12/36] Added initial metrics to CARLA --- behavior_metrics/ui/gui/views/stats_window.py | 20 +++ behavior_metrics/ui/gui/views/toolbar.py | 7 +- behavior_metrics/utils/CARLAController.py | 98 ++++++------- behavior_metrics/utils/CARLAmetrics.py | 134 ++++++++++++++++++ 4 files changed, 202 insertions(+), 57 deletions(-) create mode 100644 behavior_metrics/utils/CARLAmetrics.py diff --git a/behavior_metrics/ui/gui/views/stats_window.py b/behavior_metrics/ui/gui/views/stats_window.py index afba9bd5..187b7927 100644 --- a/behavior_metrics/ui/gui/views/stats_window.py +++ b/behavior_metrics/ui/gui/views/stats_window.py @@ -62,3 +62,23 @@ def __init__(self, parent=None, controller=None): self.layout.addWidget(self.circuit_diameter_label) wid.setLayout(self.layout) + + +class CARLAStatsWindow(QMainWindow): + def __init__(self, parent=None, controller=None): + super(CARLAStatsWindow, self).__init__(parent) + + self.controller = controller + self.setWindowTitle("Metrics") + wid = QWidget(self) + self.setCentralWidget(wid) + + self.layout = QVBoxLayout() + self.completed_distance_label = QLabel("Completed distance -> " + str(self.controller.experiment_metrics['completed_distance']) + "m") + self.layout.addWidget(self.completed_distance_label) + self.average_speed_label = QLabel("Average speed -> " + str(self.controller.experiment_metrics['average_speed']) + "km/h") + self.layout.addWidget(self.average_speed_label) + self.experiment_total_real_time_label = QLabel("Experiment total real time -> " + str(self.controller.experiment_metrics['experiment_total_real_time']) + 's') + self.layout.addWidget(self.experiment_total_real_time_label) + + wid.setLayout(self.layout) \ No newline at end of file diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py index e9dff78b..b8c27ffd 100644 --- a/behavior_metrics/ui/gui/views/toolbar.py +++ b/behavior_metrics/ui/gui/views/toolbar.py @@ -25,7 +25,7 @@ QPushButton, QScrollArea, QSpacerItem, QVBoxLayout, QWidget) -from ui.gui.views.stats_window import StatsWindow +from ui.gui.views.stats_window import StatsWindow, CARLAStatsWindow from ui.gui.views.logo import Logo from ui.gui.views.social import SocialMedia from utils import constants, environment, CARLAController @@ -599,7 +599,10 @@ def stop_recording_stats(self): self.recording_stats_label.hide() self.controller.stop_recording_metrics() - dialog = StatsWindow(self, self.controller) + if type(self.controller) == CARLAController.CARLAController: + dialog = CARLAStatsWindow(self, self.controller) + else: + dialog = StatsWindow(self, self.controller) dialog.show() def selection_change_brain(self, i): diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py index d4c3cd1e..c63aea63 100644 --- a/behavior_metrics/utils/CARLAController.py +++ b/behavior_metrics/utils/CARLAController.py @@ -33,7 +33,7 @@ from utils.logger import logger from utils.constants import CIRCUITS_TIMEOUTS from std_msgs.msg import String -from utils import metrics +from utils import CARLAmetrics __author__ = 'sergiopaniego' __contributors__ = [] @@ -142,7 +142,7 @@ def unpause_gazebo_simulation(self): except Exception as ex: print(ex) self.pilot.stop_event.clear() - + ''' def record_rosbag(self, topics, dataset_name): """Start the recording process of the dataset using rosbags @@ -174,6 +174,38 @@ def stop_record(self): else: logger.info("No bag recording") + def reload_brain(self, brain, model=None): + """Helper function to reload the current brain from the GUI. + + Arguments: + brain {srt} -- Brain to be reloadaed. + """ + logger.info("Reloading brain... {}".format(brain)) + + self.pause_pilot() + self.pilot.reload_brain(brain, model) + + # Helper functions (connection with logic) + + def set_pilot(self, pilot): + self.pilot = pilot + + def stop_pilot(self): + self.pilot.kill_event.set() + + def pause_pilot(self): + self.pilot.stop_event.set() + + def resume_pilot(self): + self.start_time = datetime.now() + self.pilot.start_time = datetime.now() + self.pilot.stop_event.clear() + + def initialize_robot(self): + self.pause_pilot() + self.pilot.initialize_robot() + + def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None): logger.info("Recording metrics bag at: {}".format(metrics_record_dir_path)) self.start_time = datetime.now() @@ -201,17 +233,16 @@ def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_co self.experiment_metadata['experiment_timeout'] = CIRCUITS_TIMEOUTS[os.path.basename(self.experiment_metadata['world'])] * 1.1 self.experiment_metadata['experiment_repetition'] = repetition_counter - self.perfect_lap_filename = perfect_lap_filename self.metrics_record_dir_path = metrics_record_dir_path time_str = time.strftime("%Y%m%d-%H%M%S") self.experiment_metrics_filename = time_str + '.bag' - topics = ['/F1ROS/odom', '/clock'] + topics = ['/carla/ego_vehicle/odometry', '/clock'] command = "rosbag record -O " + self.experiment_metrics_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag" command = shlex.split(command) with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: self.proc = subprocess.Popen(command, stdout=out, stderr=err) - def stop_recording_metrics(self, pitch_error=False): + def stop_recording_metrics(self): logger.info("Stopping metrics bag recording") end_time = time.time() @@ -224,22 +255,13 @@ def stop_recording_metrics(self, pitch_error=False): while os.path.isfile(self.experiment_metrics_filename + '.active'): pass - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(self.perfect_lap_filename) - if not pitch_error: - self.experiment_metrics = metrics.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) - self.experiment_metrics, first_image = self.pilot.calculate_metrics(self.experiment_metrics) - - try: - self.save_metrics(first_image) - except rosbag.bag.ROSBagException: - logger.info("Bag was empty, Try Again") + self.experiment_metrics = CARLAmetrics.get_metrics(self.experiment_metrics_filename) + try: + self.save_metrics() + except rosbag.bag.ROSBagException: + logger.info("Bag was empty, Try Again") - else: - self.experiment_metrics = {'percentage_completed': 0, 'average_speed': 0, 'lap_seconds': 0, - 'circuit_diameter': 0, 'position_deviation_mae': 0, - 'position_deviation_total_err': 0, 'experiment_total_simulated_time': 0, - 'completed_distance': 0} logger.info("* Experiment total real time -> " + str(end_time - self.pilot.pilot_start_time)) self.experiment_metrics['experiment_total_real_time'] = end_time - self.pilot.pilot_start_time @@ -251,7 +273,8 @@ def stop_recording_metrics(self, pitch_error=False): logger.info("Stopping metrics bag recording") - def save_metrics(self, first_image): + + def save_metrics(self): experiment_metadata_str = json.dumps(self.experiment_metadata) experiment_metrics_str = json.dumps(self.experiment_metrics) with rosbag.Bag(self.experiment_metrics_filename, 'a') as bag: @@ -259,40 +282,5 @@ def save_metrics(self, first_image): experiment_metrics_msg = String(data=experiment_metrics_str) bag.write('/metadata', experiment_metadata_msg, rospy.Time(bag.get_end_time())) bag.write('/experiment_metrics', experiment_metrics_msg, rospy.Time(bag.get_end_time())) - if first_image is not None and first_image.shape == (480, 640, 3): - rospy.loginfo('Image received and sent to /first_image') - bag.write('/first_image', self.cvbridge.cv2_to_imgmsg(first_image), rospy.Time(bag.get_end_time())) - else: - rospy.loginfo('Error: Image Broken and /first_image Skipped: {}'.format(first_image)) bag.close() - ''' - def reload_brain(self, brain, model=None): - """Helper function to reload the current brain from the GUI. - - Arguments: - brain {srt} -- Brain to be reloadaed. - """ - logger.info("Reloading brain... {}".format(brain)) - - self.pause_pilot() - self.pilot.reload_brain(brain, model) - - # Helper functions (connection with logic) - def set_pilot(self, pilot): - self.pilot = pilot - - def stop_pilot(self): - self.pilot.kill_event.set() - - def pause_pilot(self): - self.pilot.stop_event.set() - - def resume_pilot(self): - self.start_time = datetime.now() - self.pilot.start_time = datetime.now() - self.pilot.stop_event.clear() - - def initialize_robot(self): - self.pause_pilot() - self.pilot.initialize_robot() diff --git a/behavior_metrics/utils/CARLAmetrics.py b/behavior_metrics/utils/CARLAmetrics.py new file mode 100644 index 00000000..d792895b --- /dev/null +++ b/behavior_metrics/utils/CARLAmetrics.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python + +"""This module contains the metrics manager. +This module is in charge of generating metrics for a brain execution. +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import pandas as pd +import numpy as np +import shutil +import time +import os +import rosbag + +from datetime import datetime +from bagpy import bagreader +from utils.logger import logger + +from scipy.optimize import fmin, dual_annealing +from scipy.interpolate import CubicSpline + +MIN_COMPLETED_DISTANCE_EXPERIMENT = 10 +MIN_PERCENTAGE_COMPLETED_EXPERIMENT = 0 +MIN_EXPERIMENT_TIME = 25 +LAP_COMPLETED_PERCENTAGE = 100 + + +def is_finish_line(point, start_point): + try: + current_point = np.array([point['pose.pose.position.x'], point['pose.pose.position.y']]) + except IndexError: + current_point = point + try: + start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) + except IndexError: + start_point = start_point + dist = (start_point - current_point) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist <= 1.0: + return True + return False + + +def circuit_distance_completed(checkpoints, lap_point): + previous_point = [] + diameter = 0 + for i, point in enumerate(checkpoints): + current_point = np.array([point['pose.pose.position.x'], point['pose.pose.position.y']]) + if i != 0: + dist = (previous_point - current_point) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + diameter += dist + if point is lap_point: + break + previous_point = current_point + return diameter + + +def get_metrics(stats_filename): + empty_metrics = { + "completed_distance": 0, + "average_speed": 0, + "experiment_total_simulated_time": 0 + } + experiment_metrics = {} + + time_counter = 5 + while not os.path.exists(stats_filename): + time.sleep(1) + time_counter -= 1 + if time_counter <= 0: + ValueError(f"{stats_filename} isn't a file!") + return empty_metrics + + try: + bag_reader = bagreader(stats_filename) + except rosbag.bag.ROSBagException: + return empty_metrics + + csv_files = [] + for topic in bag_reader.topics: + data = bag_reader.message_by_topic(topic) + csv_files.append(data) + + data_file = stats_filename.split('.bag')[0] + '/carla-ego_vehicle-odometry.csv' + dataframe_pose = pd.read_csv(data_file) + checkpoints = [] + for index, row in dataframe_pose.iterrows(): + checkpoints.append(row) + + data_file = stats_filename.split('.bag')[0] + '/clock.csv' + dataframe_pose = pd.read_csv(data_file) + clock_points = [] + for index, row in dataframe_pose.iterrows(): + clock_points.append(row) + start_clock = clock_points[0] + seconds_start = start_clock['clock.secs'] + seconds_end = clock_points[len(clock_points) - 1]['clock.secs'] + + if len(checkpoints) > 1: + experiment_metrics = get_distance_completed(experiment_metrics, checkpoints) + experiment_metrics = get_average_speed(experiment_metrics, seconds_start, seconds_end) + experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start + logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time'])) + shutil.rmtree(stats_filename.split('.bag')[0]) + return experiment_metrics + else: + return empty_metrics + + +def get_distance_completed(experiment_metrics, checkpoints): + end_point = checkpoints[len(checkpoints) - 1] + experiment_metrics['completed_distance'] = circuit_distance_completed(checkpoints, end_point) + logger.info('* Completed distance ---> ' + str(experiment_metrics['completed_distance'])) + return experiment_metrics + + +def get_average_speed(experiment_metrics, seconds_start, seconds_end): + if (seconds_end - seconds_start): + experiment_metrics['average_speed'] = experiment_metrics['completed_distance'] / (seconds_end - seconds_start) + else: + experiment_metrics['average_speed'] = 0 + logger.info('* Average speed ---> ' + str(experiment_metrics['average_speed'])) + return experiment_metrics From e423df49a2eac80476a8daaee860b2fd6b556a0f Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 21 Nov 2022 15:33:55 +0100 Subject: [PATCH 13/36] Added example launch file and updated config --- .../brains/CARLA/brain_carla_autopilot.py | 15 +++--- .../CARLA_launch_files/test_town_01.launch | 46 +++++++++++++++++++ behavior_metrics/configs/default_carla.yml | 2 +- 3 files changed, 54 insertions(+), 9 deletions(-) create mode 100644 behavior_metrics/configs/CARLA_launch_files/test_town_01.launch diff --git a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py index 7a744b5c..bf2fd5a8 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_autopilot.py +++ b/behavior_metrics/brains/CARLA/brain_carla_autopilot.py @@ -81,14 +81,13 @@ def execute(self): #self.motors.sendW(w) self.motors.sendV(1) - #self.update_frame('frame_0', image) - #self.update_frame('frame_1', image_1) - #self.update_frame('frame_2', image_2) - #self.update_frame('frame_3', image_3) - - self.update_frame('frame_0', bird_eye_view_1) - self.update_frame('frame_1', bird_eye_view_1) - self.update_frame('frame_2', bird_eye_view_1) + self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + + #self.update_frame('frame_0', bird_eye_view_1) + self.update_pose(self.pose.getPose3d()) #print(self.pose.getPose3d()) diff --git a/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch b/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch new file mode 100644 index 00000000..a1c8e9d3 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index bb28a659..d46c34c9 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -36,7 +36,7 @@ Behaviors: ImageTranform: '' Type: 'CARLA' Simulation: - World: /home/jderobot/carla-ros-bridge/catkin_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch + World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch Dataset: In: '/tmp/my_bag.bag' Out: '' From 125b1ee39e258d34ade3f1f8794e382dbaa35ae0 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 21 Nov 2022 16:23:32 +0100 Subject: [PATCH 14/36] Added CARLA brain with neural network --- .../brain_carla_bird_eye_deep_learning.py | 134 ++++++++++++++++++ .../CARLA_launch_files/test_town_01.launch | 3 +- behavior_metrics/configs/default_carla.yml | 2 +- 3 files changed, 137 insertions(+), 2 deletions(-) create mode 100644 behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py new file mode 100644 index 00000000..818eae7f --- /dev/null +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -0,0 +1,134 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- +import csv +import cv2 +import math +import numpy as np +import threading +import time +import carla +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) +from utils.constants import DATASETS_DIR, ROOT_PATH + +import tensorflow as tf +gpus = tf.config.experimental.list_physical_devices('GPU') +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + +PRETRAINED_MODELS = "../models/" + +class Brain: + + def __init__(self, sensors, actuators, handler, config=None): + self.camera = sensors.get_camera('camera_0') + self.camera_1 = sensors.get_camera('camera_1') + self.camera_2 = sensors.get_camera('camera_2') + self.camera_3 = sensors.get_camera('camera_3') + + self.pose = sensors.get_pose3d('pose3d_0') + + self.bird_eye_view = sensors.get_bird_eye_view('bird_eye_view_0') + + self.motors = actuators.get_motor('motors_0') + self.handler = handler + self.config = config + + self.threshold_image = np.zeros((640, 360, 3), np.uint8) + self.color_image = np.zeros((640, 360, 3), np.uint8) + self.lock = threading.Lock() + self.threshold_image_lock = threading.Lock() + self.color_image_lock = threading.Lock() + self.cont = 0 + self.iteration = 0 + + # self.previous_timestamp = 0 + # self.previous_image = 0 + + self.previous_v = None + self.previous_w = None + self.previous_w_normalized = None + self.suddenness_distance = [] + + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) # seconds + world = client.get_world() + time.sleep(3) + self.vehicle = world.get_actors().filter('vehicle.*')[0] + + model = "20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5" # WORKS! + self.net = tf.keras.models.load_model('/home/jderobot/Documents/Projects/BehaviorMetrics/PlayingWithCARLA/models/20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5') + self.previous_speed = 0 + + time.sleep(2) + + def update_frame(self, frame_id, data): + """Update the information to be shown in one of the GUI's frames. + + Arguments: + frame_id {str} -- Id of the frame that will represent the data + data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) + """ + self.handler.update_frame(frame_id, data) + + def update_pose(self, pose_data): + self.handler.update_pose3d(pose_data) + + def execute(self): + image = self.camera.getImage().data + image_1 = self.camera_1.getImage().data + image_2 = self.camera_2.getImage().data + image_3 = self.camera_3.getImage().data + + bird_eye_view_1 = self.bird_eye_view.getImage(self.vehicle) + + #print(self.bird_eye_view.getImage(self.vehicle)) + + #self.update_frame('frame_0', image) + self.update_frame('frame_1', image_1) + self.update_frame('frame_2', image_2) + self.update_frame('frame_3', image_3) + + self.update_frame('frame_0', bird_eye_view_1) + + + self.update_pose(self.pose.getPose3d()) + + image_shape=(50, 150) + img_base = cv2.resize(bird_eye_view_1, image_shape) + + AUGMENTATIONS_TEST = Compose([ + Normalize() + ]) + image = AUGMENTATIONS_TEST(image=img_base) + img = image["image"] + + #velocity_dim = np.full((150, 50), 0.5) + velocity_dim = np.full((150, 50), self.previous_speed/30) + new_img_vel = np.dstack((img, velocity_dim)) + img = new_img_vel + + img = np.expand_dims(img, axis=0) + prediction = self.net.predict(img) + throttle = prediction[0][0] + steer = prediction[0][1] * (1 - (-1)) + (-1) + break_command = prediction[0][2] + speed = self.vehicle.get_velocity() + vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) + previous_speed = vehicle_speed + print(prediction) + + if vehicle_speed > 200: + self.motors.sendV(0) + self.motors.sendW(steer) + else: + if vehicle_speed < 2: + self.motors.sendV(1.0) + self.motors.sendW(0.0) + else: + self.motors.sendV(throttle) + self.motors.sendW(steer) + + + diff --git a/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch b/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch index a1c8e9d3..6cf923c8 100644 --- a/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch +++ b/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch @@ -8,7 +8,8 @@ - + + diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index d46c34c9..edde0135 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -30,7 +30,7 @@ Behaviors: MaxV: 3 MaxW: 0.3 - BrainPath: 'brains/CARLA/brain_carla_autopilot.py' + BrainPath: 'brains/CARLA/brain_carla_bird_eye_deep_learning.py' PilotTimeCycle: 50 Parameters: ImageTranform: '' From 38736a407fa73f18b30d0eba7d57847d610ac244 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 28 Nov 2022 15:27:06 +0100 Subject: [PATCH 15/36] Updated parameter in motors for CARLA --- .../brain_carla_bird_eye_deep_learning.py | 12 ++-- behavior_metrics/robot/interfaces/motors.py | 58 ++++++++++--------- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index 818eae7f..79744bc4 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -120,15 +120,15 @@ def execute(self): print(prediction) if vehicle_speed > 200: - self.motors.sendV(0) - self.motors.sendW(steer) + self.motors.sendThrottle(0) + self.motors.sendSteer(steer) else: if vehicle_speed < 2: - self.motors.sendV(1.0) - self.motors.sendW(0.0) + self.motors.sendThrottle(1.0) + self.motors.sendSteer(0.0) else: - self.motors.sendV(throttle) - self.motors.sendW(steer) + self.motors.sendThrottle(throttle) + self.motors.sendSteer(steer) diff --git a/behavior_metrics/robot/interfaces/motors.py b/behavior_metrics/robot/interfaces/motors.py index 373b2e51..75f55524 100644 --- a/behavior_metrics/robot/interfaces/motors.py +++ b/behavior_metrics/robot/interfaces/motors.py @@ -21,8 +21,8 @@ def cmdvel2Twist(vel): def cmdvel2CarlaEgoVehicleControl(vel): vehicle_control = CarlaEgoVehicleControl() - vehicle_control.throttle = vel.vx - vehicle_control.steer = vel.az + vehicle_control.throttle = vel.throttle + vehicle_control.steer = vel.steer vehicle_control.brake = 0.0 vehicle_control.hand_brake = False vehicle_control.reverse = False @@ -33,7 +33,7 @@ def cmdvel2CarlaEgoVehicleControl(vel): -class CMDVel (): +class CMDVel(): def __init__(self): @@ -54,6 +54,26 @@ def __str__(self): s = s + "\n timeStamp: " + str(self.timeStamp) + "\n}" return s +class CARLAVel(): + + def __init__(self): + + self.throttle = 0.0 + self.steer = 0.0 + self.brake = 0.0 + self.hand_brake = False + self.reverse = False + self.gear = 0 + self.manual_gear_shift = False + + def __str__(self): + s = "CARLAVel: {\n throttle: " + str(self.throttle) + "\n steer: " + str(self.steer) + s = s + "\n brake: " + str(self.brake) + "\n hand_brake: " + str(self.hand_brake) + s = s + "\n reverse: " + str(self.reverse) + "\n gear: " + str(self.gear) + s = s + "\n manual_gear_shift: " + str(self.manual_gear_shift) + "\n}" + return s + + class PublisherMotors: @@ -145,7 +165,7 @@ def __init__(self, topic, maxV, maxW, v, w): self.v = v self.w = w self.topic = topic - self.data = CMDVel() + self.data = CARLAVel() self.pub = rospy.Publisher(self.topic, CarlaEgoVehicleControl, queue_size=1) rospy.init_node("CARLAMotors") self.lock = threading.Lock() @@ -184,34 +204,16 @@ def sendVelocities(self, vel): self.data = vel self.lock.release() - def sendV(self, v): - - self.sendVX(v) - self.v = v - - def sendL(self, l): - - self.sendVY(l) - - def sendW(self, w): - - self.sendAZ(w) - self.w = w - - def sendVX(self, vx): - - self.lock.acquire() - self.data.vx = vx - self.lock.release() - - def sendVY(self, vy): + def sendThrottle(self, throttle): self.lock.acquire() - self.data.vy = vy + self.data.throttle = throttle self.lock.release() + self.throttle = throttle - def sendAZ(self, az): + def sendSteer(self, steer): self.lock.acquire() - self.data.az = az + self.data.steer = steer self.lock.release() + self.steer = steer From ca8b734678aa8337340713dae7ea6f8dbff525a1 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 30 Nov 2022 10:35:07 +0100 Subject: [PATCH 16/36] Driver for CARLA working --- .../brain_carla_bird_eye_deep_learning.py | 12 +- .../CARLA_launch_files/test_town_01.launch | 6 +- behavior_metrics/driver_carla.py | 4 +- behavior_metrics/pilot.py | 8 - behavior_metrics/pilot_carla.py | 299 ++++++++++++++++++ 5 files changed, 310 insertions(+), 19 deletions(-) create mode 100644 behavior_metrics/pilot_carla.py diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index 79744bc4..90c667cb 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -57,8 +57,8 @@ def __init__(self, sensors, actuators, handler, config=None): time.sleep(3) self.vehicle = world.get_actors().filter('vehicle.*')[0] - model = "20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5" # WORKS! - self.net = tf.keras.models.load_model('/home/jderobot/Documents/Projects/BehaviorMetrics/PlayingWithCARLA/models/20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5') + model = '/home/jderobot/Documents/Projects/BehaviorMetrics/PlayingWithCARLA/models/20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5' + self.net = tf.keras.models.load_model(model) self.previous_speed = 0 time.sleep(2) @@ -92,7 +92,6 @@ def execute(self): self.update_frame('frame_0', bird_eye_view_1) - self.update_pose(self.pose.getPose3d()) image_shape=(50, 150) @@ -116,14 +115,13 @@ def execute(self): break_command = prediction[0][2] speed = self.vehicle.get_velocity() vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - previous_speed = vehicle_speed - print(prediction) + self.previous_speed = vehicle_speed - if vehicle_speed > 200: + if vehicle_speed > 30: self.motors.sendThrottle(0) self.motors.sendSteer(steer) else: - if vehicle_speed < 2: + if vehicle_speed < 2: self.motors.sendThrottle(1.0) self.motors.sendSteer(0.0) else: diff --git a/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch b/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch index 6cf923c8..2889db83 100644 --- a/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch +++ b/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch @@ -9,8 +9,10 @@ - - + + + + diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index a2e694f9..4060a750 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -3,7 +3,7 @@ import sys import threading -from pilot import Pilot +from pilot_carla import PilotCarla from ui.tui.main_view import TUI from utils import environment, script_manager from utils.colors import Colors @@ -121,7 +121,7 @@ def main_win(configuration, controller): # Launch control -pilot = Pilot(app_configuration, controller, app_configuration.brain_path) +pilot = PilotCarla(app_configuration, controller, app_configuration.brain_path) pilot.daemon = True pilot.start() logger.info('Executing app') diff --git a/behavior_metrics/pilot.py b/behavior_metrics/pilot.py index d6d5c862..8686b2a5 100644 --- a/behavior_metrics/pilot.py +++ b/behavior_metrics/pilot.py @@ -93,16 +93,8 @@ def __init__(self, configuration, controller, brain_path): def __wait_gazebo(self): """Wait for gazebo to be initialized""" - # gazebo_ready = False self.stop_event.set() - # while not gazebo_ready: - # try: - # self.controller.pause_gazebo_simulation() - # gazebo_ready = True - # self.stop_event.clear() - # except Exception as ex: - # print(ex) def initialize_robot(self): """Initialize robot interfaces (sensors and actuators) and its brain from configuration""" diff --git a/behavior_metrics/pilot_carla.py b/behavior_metrics/pilot_carla.py new file mode 100644 index 00000000..93df3bbd --- /dev/null +++ b/behavior_metrics/pilot_carla.py @@ -0,0 +1,299 @@ +#!/usr/bin/env python +""" This module is responsible for handling the logic of the robot and its current brain. + +This program is free software: you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) any later +version. +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +You should have received a copy of the GNU General Public License along with +this program. If not, see . +""" + +import threading +import time +import rospy +import subprocess + +from datetime import datetime +from brains.brains_handler import Brains +from robot.actuators import Actuators +from robot.sensors import Sensors +from utils.logger import logger +from utils.constants import MIN_EXPERIMENT_PERCENTAGE_COMPLETED +from rosgraph_msgs.msg import Clock +from carla_msgs.msg import CarlaControl + +import numpy as np + +__author__ = 'fqez' +__contributors__ = [] +__license__ = 'GPLv3' + + +class PilotCarla(threading.Thread): + """This class handles the robot and its brain. + + This class called PilotCarla that handles the initialization of the robot sensors and actuators and the + brain that will control the robot. The main logic consists of an infinite loop called every 60 milliseconds that + invoke an action from the brain. + + Attributes: + controller {utils.controller.Controller} -- Controller instance of the MVC of the application + configuration {utils.configuration.Config} -- Configuration instance of the application + sensors {robot.sensors.Sensors} -- Sensors instance of the robot + actuators {robot.actuators.Actuators} -- Actuators instance of the robot + brains {brains.brains_handler.Brains} -- Brains controller instance + """ + + def __init__(self, configuration, controller, brain_path): + """Constructor of the pilot class + + Arguments: + configuration {utils.configuration.Config} -- Configuration instance of the application + controller {utils.controller.Controller} -- Controller instance of the MVC of the application + """ + + self.controller = controller + self.controller.set_pilot(self) + self.configuration = configuration + self.stop_event = threading.Event() + self.kill_event = threading.Event() + threading.Thread.__init__(self, args=self.stop_event) + self.brain_path = brain_path + self.robot_type = self.brain_path.split("/")[-2] + self.sensors = None + self.actuators = None + self.brains = None + self.initialize_robot() + if self.robot_type == 'drone': + self.pose3d = self.brains.active_brain.getPose3d() + self.start_pose = np.array([self.pose3d[0], self.pose3d[1]]) + else: + self.pose3d = self.sensors.get_pose3d('pose3d_0') + self.start_pose = np.array([self.pose3d.getPose3d().x, self.pose3d.getPose3d().y]) + self.previous = datetime.now() + self.checkpoints = [] + self.metrics = {} + self.checkpoint_save = False + self.max_distance = 0.5 + self.execution_completed = False + self.stats_thread = threading.Thread(target=self.track_stats) + self.stats_thread.start() + self.ros_clock_time = 0 + self.real_time_factor = 0 + self.brain_iterations_real_time = [] + self.brain_iterations_simulated_time = [] + self.real_time_factors = [] + self.real_time_update_rate = 1000 + self.pilot_start_time = 0 + self.time_cycle = self.configuration.pilot_time_cycle + + def __wait_carla(self): + """Wait for simulator to be initialized""" + + self.stop_event.set() + + def initialize_robot(self): + """Initialize robot interfaces (sensors and actuators) and its brain from configuration""" + self.stop_interfaces() + if self.robot_type != 'drone': + self.actuators = Actuators(self.configuration.actuators) + self.sensors = Sensors(self.configuration.sensors) + if hasattr(self.configuration, 'experiment_model') and type(self.configuration.experiment_model) != list: + self.brains = Brains(self.sensors, self.actuators, self.brain_path, self.controller, + self.configuration.experiment_model, self.configuration.brain_kwargs) + else: + self.brains = Brains(self.sensors, self.actuators, self.brain_path, self.controller, + config=self.configuration.brain_kwargs) + self.__wait_carla() + + def stop_interfaces(self): + """Function that kill the current interfaces of the robot. For reloading purposes.""" + if self.sensors: + self.sensors.kill() + if self.actuators: + self.actuators.kill() + pass + + def run(self): + """Main loop of the class. Calls a brain action every self.time_cycle""" + "TODO: cleanup measure of ips" + it = 0 + ss = time.time() + self.brain_iterations_real_time = [] + self.brain_iterations_simulated_time = [] + self.real_time_factors = [] + self.sensors.get_camera('camera_0').total_frames = 0 + self.pilot_start_time = time.time() + + control_pub = rospy.Publisher('/carla/control', CarlaControl, queue_size=1) + control_command = CarlaControl() + control_command.command = 1 + control_pub.publish(control_command) + + while not self.kill_event.is_set(): + if not self.stop_event.is_set(): + control_pub = rospy.Publisher('/carla/control', CarlaControl, queue_size=1) + control_command = CarlaControl() + control_command.command = 2 + control_pub.publish(control_command) + + start_time = datetime.now() + start_time_ros = self.ros_clock_time + self.execution_completed = False + try: + self.brains.active_brain.execute() + except AttributeError as e: + logger.warning('No Brain selected') + logger.error(e) + + dt = datetime.now() - start_time + ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0 + self.brain_iterations_real_time.append(ms / 1000) + elapsed = time.time() - ss + if elapsed < 1: + it += 1 + else: + ss = time.time() + it = 0 + if ms < self.time_cycle: + time.sleep((self.time_cycle - ms) / 1000.0) + self.real_time_factors.append(self.real_time_factor) + self.brain_iterations_simulated_time.append(self.ros_clock_time - start_time_ros) + self.execution_completed = True + self.clock_subscriber.unregister() + self.stats_process.terminate() + poll = self.stats_process.poll() + while poll is None: + time.sleep(1) + poll = self.stats_process.poll() + self.kill() + logger.info('Pilot: pilot killed.') + + def stop(self): + """Pause the main loop""" + + self.stop_event.set() + + def play(self): + """Resume the main loop.""" + + if self.is_alive(): + self.stop_event.clear() + else: + self.start() + + def kill(self): + """Destroy the main loop. For exiting""" + self.stop_interfaces() + self.actuators.kill() + self.kill_event.set() + + def reload_brain(self, brain_path, model=None): + """Reload a brain specified by brain_path + + This function is useful if one wants to change the environment of the robot (simulated world). + + Arguments: + brain_path {str} -- Path to the brain module to load. + """ + self.brains.load_brain(brain_path, model=model) + + def finish_line(self): + pose = self.pose3d.getPose3d() + current_point = np.array([pose.x, pose.y]) + + dist = (self.start_pose - current_point) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist < self.max_distance: + return True + return False + + def calculate_metrics(self, experiment_metrics): + if hasattr(self.brains.active_brain, 'inference_times'): + self.brains.active_brain.inference_times = self.brains.active_brain.inference_times[10:-10] + mean_inference_time = sum(self.brains.active_brain.inference_times) / len(self.brains.active_brain.inference_times) + frame_rate = self.sensors.get_camera('camera_0').total_frames / experiment_metrics['experiment_total_simulated_time'] + gpu_inference = self.brains.active_brain.gpu_inference + real_time_update_rate = self.real_time_update_rate + first_image = self.brains.active_brain.first_image + logger.info('* Mean network inference time ---> ' + str(mean_inference_time) + 's') + logger.info('* Frame rate ---> ' + str(frame_rate) + 'fps') + else: + mean_inference_time = 0 + frame_rate = 0 + gpu_inference = False + real_time_update_rate = self.real_time_update_rate + first_image = None + logger.info('No deep learning based brain') + if self.brain_iterations_real_time and self.brain_iterations_simulated_time and self.brain_iterations_simulated_time: + mean_brain_iterations_simulated_time = sum(self.brain_iterations_simulated_time) / len(self.brain_iterations_simulated_time) + real_time_factor = sum(self.real_time_factors) / len(self.real_time_factors) + brain_iterations_frequency_simulated_time = 1 / mean_brain_iterations_simulated_time + target_brain_iterations_simulated_time = 1000 / self.time_cycle / round(real_time_factor, 2) + mean_brain_iterations_real_time = sum(self.brain_iterations_real_time) / len(self.brain_iterations_real_time) + brain_iterations_frequency_real_time = 1 / mean_brain_iterations_real_time + target_brain_iterations_real_time = 1 / (self.time_cycle / 1000) + suddenness_distance = sum(self.brains.active_brain.suddenness_distance) / len(self.brains.active_brain.suddenness_distance) + else: + mean_brain_iterations_real_time = 0 + mean_brain_iterations_simulated_time = 0 + real_time_factor = 0 + brain_iterations_frequency_simulated_time = 0 + target_brain_iterations_simulated_time = 0 + brain_iterations_frequency_real_time = 0 + target_brain_iterations_real_time = 0 + suddenness_distance = 0 + logger.info('* Brain iterations frequency simulated time ---> ' + str(brain_iterations_frequency_simulated_time) + 'it/s') + logger.info('* Target brain iterations simulated time -> ' + str(target_brain_iterations_simulated_time) + 'it/s') + logger.info('* Mean brain iterations real time ---> ' + str(mean_brain_iterations_real_time) + 's') + logger.info('* Brain iterations frequency real time ---> ' + str(brain_iterations_frequency_real_time) + 'it/s') + logger.info('* Target brain iterations real time -> ' + str(target_brain_iterations_real_time) + 'it/s') + logger.info('* Mean brain iterations simulated time ---> ' + str(mean_brain_iterations_simulated_time) + 's') + logger.info('* Mean brain iterations simulated time ---> ' + str(real_time_factor)) + logger.info('* Real time update rate ---> ' + str(real_time_update_rate)) + logger.info('* GPU inference ---> ' + str(gpu_inference)) + logger.info('* Suddenness distance ---> ' + str(suddenness_distance)) + logger.info('* Saving experiment ---> ' + str(hasattr(self.controller, 'experiment_metrics_filename'))) + experiment_metrics['brain_iterations_frequency_simulated_time'] = brain_iterations_frequency_simulated_time + experiment_metrics['target_brain_iterations_simulated_time'] = target_brain_iterations_simulated_time + experiment_metrics['mean_brain_iterations_real_time'] = mean_brain_iterations_real_time + experiment_metrics['brain_iterations_frequency_real_time'] = brain_iterations_frequency_real_time + experiment_metrics['target_brain_iterations_real_time'] = target_brain_iterations_real_time + experiment_metrics['mean_inference_time'] = mean_inference_time + experiment_metrics['frame_rate'] = frame_rate + experiment_metrics['gpu_inference'] = gpu_inference + experiment_metrics['mean_brain_iterations_simulated_time'] = mean_brain_iterations_simulated_time + experiment_metrics['real_time_factor'] = real_time_factor + experiment_metrics['real_time_update_rate'] = real_time_update_rate + experiment_metrics['suddenness_distance'] = suddenness_distance + logger.info('Saving metrics to ROS bag') + return experiment_metrics, first_image + + def clock_callback(self, clock_data): + self.ros_clock_time = clock_data.clock.to_sec() + + def track_stats(self): + args = ["gz", "stats", "-p"] + # Prints gz statistics. "-p": Output comma-separated values containing- + # real-time factor (percent), simtime (sec), realtime (sec), paused (T or F) + self.stats_process = subprocess.Popen(args, stdout=subprocess.PIPE) + # bufsize=1 enables line-bufferred mode (the input buffer is flushed + # automatically on newlines if you would write to process.stdin ) + poll = self.stats_process.poll() + while poll is not None: + time.sleep(1) + poll = self.stats_process.poll() + + self.clock_subscriber = rospy.Subscriber("/clock", Clock, self.clock_callback) + with self.stats_process.stdout: + for line in iter(self.stats_process.stdout.readline, b''): + stats_list = [x.strip() for x in line.split(b',')] + try: + self.real_time_factor = float(stats_list[0].decode("utf-8")) + except Exception as ex: + self.real_time_factor = 0 From e08523f71968ddaf223a82ae17908a6705698537 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 30 Nov 2022 11:38:48 +0100 Subject: [PATCH 17/36] Add padding to visualizations --- .../CARLA/brain_carla_bird_eye_deep_learning.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index 90c667cb..3bfb7734 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -70,6 +70,19 @@ def update_frame(self, frame_id, data): frame_id {str} -- Id of the frame that will represent the data data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc) """ + if data.shape[0] != data.shape[1]: + if data.shape[0] > data.shape[1]: + difference = data.shape[0] - data.shape[1] + extra_left, extra_right = int(difference/2), int(difference/2) + extra_top, extra_bottom = 0, 0 + else: + difference = data.shape[1] - data.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference/2), int(difference/2) + + + data = np.pad(data, ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), mode='constant', constant_values=0) + self.handler.update_frame(frame_id, data) def update_pose(self, pose_data): From 736fd3e22b1093a4ae775b9161c203db1ac4f67a Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 30 Nov 2022 12:35:37 +0100 Subject: [PATCH 18/36] Added callbacks for lane invasion and collisions --- behavior_metrics/utils/CARLAController.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py index c63aea63..deecb205 100644 --- a/behavior_metrics/utils/CARLAController.py +++ b/behavior_metrics/utils/CARLAController.py @@ -25,6 +25,7 @@ import time import rosbag import json +import math from std_srvs.srv import Empty from sensor_msgs.msg import Image @@ -34,6 +35,8 @@ from utils.constants import CIRCUITS_TIMEOUTS from std_msgs.msg import String from utils import CARLAmetrics +from carla_msgs.msg import CarlaLaneInvasionEvent +from carla_msgs.msg import CarlaCollisionEvent __author__ = 'sergiopaniego' __contributors__ = [] @@ -60,6 +63,26 @@ def __init__(self): self.pose3D_data = None self.recording = False self.cvbridge = CvBridge() + self.collision_sub = rospy.Subscriber('/carla/ego_vehicle/collision', CarlaCollisionEvent, self.__collision_callback) + self.collision_sub = rospy.Subscriber('/carla/ego_vehicle/lane_invasion', CarlaLaneInvasionEvent, self.__lane_invasion_callback) + + + def __collision_callback(self, data): + intensity = math.sqrt(data.normal_impulse.x**2 + data.normal_impulse.y**2 + data.normal_impulse.z**2) + print('Collision with {} (impulse {})'.format(data.other_actor_id, intensity)) + + def __lane_invasion_callback(self, data): + text = [] + for marking in data.crossed_lane_markings: + if marking is CarlaLaneInvasionEvent.LANE_MARKING_OTHER: + text.append("Other") + elif marking is CarlaLaneInvasionEvent.LANE_MARKING_BROKEN: + text.append("Broken") + elif marking is CarlaLaneInvasionEvent.LANE_MARKING_SOLID: + text.append("Solid") + else: + text.append("Unknown ") + print('Crossed line %s' % ' and '.join(text)) # GUI update def update_frame(self, frame_id, data): From e592507e811cf498ce4060799cd8157cb9bbac84 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 30 Nov 2022 13:54:48 +0100 Subject: [PATCH 19/36] Metrics for lane invasion and collisions --- behavior_metrics/ui/gui/views/stats_window.py | 4 +++ behavior_metrics/utils/CARLAController.py | 6 ++--- behavior_metrics/utils/CARLAmetrics.py | 26 +++++++++++++++++++ 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/behavior_metrics/ui/gui/views/stats_window.py b/behavior_metrics/ui/gui/views/stats_window.py index 187b7927..16817511 100644 --- a/behavior_metrics/ui/gui/views/stats_window.py +++ b/behavior_metrics/ui/gui/views/stats_window.py @@ -80,5 +80,9 @@ def __init__(self, parent=None, controller=None): self.layout.addWidget(self.average_speed_label) self.experiment_total_real_time_label = QLabel("Experiment total real time -> " + str(self.controller.experiment_metrics['experiment_total_real_time']) + 's') self.layout.addWidget(self.experiment_total_real_time_label) + self.collisions_label = QLabel("Collisions -> " + str(self.controller.experiment_metrics['collisions'])) + self.layout.addWidget(self.collisions_label) + self.lane_invasions_label = QLabel("Lane invasions -> " + str(self.controller.experiment_metrics['lane_invasions'])) + self.layout.addWidget(self.lane_invasions_label) wid.setLayout(self.layout) \ No newline at end of file diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py index deecb205..f31a3ed5 100644 --- a/behavior_metrics/utils/CARLAController.py +++ b/behavior_metrics/utils/CARLAController.py @@ -69,7 +69,7 @@ def __init__(self): def __collision_callback(self, data): intensity = math.sqrt(data.normal_impulse.x**2 + data.normal_impulse.y**2 + data.normal_impulse.z**2) - print('Collision with {} (impulse {})'.format(data.other_actor_id, intensity)) + logger.info('Collision with {} (impulse {})'.format(data.other_actor_id, intensity)) def __lane_invasion_callback(self, data): text = [] @@ -82,7 +82,7 @@ def __lane_invasion_callback(self, data): text.append("Solid") else: text.append("Unknown ") - print('Crossed line %s' % ' and '.join(text)) + logger.info('Crossed line %s' % ' and '.join(text)) # GUI update def update_frame(self, frame_id, data): @@ -259,7 +259,7 @@ def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_co self.metrics_record_dir_path = metrics_record_dir_path time_str = time.strftime("%Y%m%d-%H%M%S") self.experiment_metrics_filename = time_str + '.bag' - topics = ['/carla/ego_vehicle/odometry', '/clock'] + topics = ['/carla/ego_vehicle/odometry', '/carla/ego_vehicle/collision', '/carla/ego_vehicle/lane_invasion', '/clock'] command = "rosbag record -O " + self.experiment_metrics_filename + " " + " ".join(topics) + " __name:=behav_metrics_bag" command = shlex.split(command) with open("logs/.roslaunch_stdout.log", "w") as out, open("logs/.roslaunch_stderr.log", "w") as err: diff --git a/behavior_metrics/utils/CARLAmetrics.py b/behavior_metrics/utils/CARLAmetrics.py index d792895b..36eb3973 100644 --- a/behavior_metrics/utils/CARLAmetrics.py +++ b/behavior_metrics/utils/CARLAmetrics.py @@ -107,9 +107,25 @@ def get_metrics(stats_filename): seconds_start = start_clock['clock.secs'] seconds_end = clock_points[len(clock_points) - 1]['clock.secs'] + collision_points = [] + if '/carla/ego_vehicle/collision' in bag_reader.topics: + data_file = stats_filename.split('.bag')[0] + '/carla-ego_vehicle-collision.csv' + dataframe_collision = pd.read_csv(data_file) + for index, row in dataframe_collision.iterrows(): + collision_points.append(row) + + lane_invasion_points = [] + if '/carla/ego_vehicle/lane_invasion' in bag_reader.topics: + data_file = stats_filename.split('.bag')[0] + '/carla-ego_vehicle-lane_invasion.csv' + dataframe_lane_invasion = pd.read_csv(data_file, on_bad_lines='skip') + for index, row in dataframe_lane_invasion.iterrows(): + lane_invasion_points.append(row) + if len(checkpoints) > 1: experiment_metrics = get_distance_completed(experiment_metrics, checkpoints) experiment_metrics = get_average_speed(experiment_metrics, seconds_start, seconds_end) + experiment_metrics = get_collisions(experiment_metrics, collision_points) + experiment_metrics = get_lane_invasions(experiment_metrics, lane_invasion_points) experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time'])) shutil.rmtree(stats_filename.split('.bag')[0]) @@ -132,3 +148,13 @@ def get_average_speed(experiment_metrics, seconds_start, seconds_end): experiment_metrics['average_speed'] = 0 logger.info('* Average speed ---> ' + str(experiment_metrics['average_speed'])) return experiment_metrics + +def get_collisions(experiment_metrics, collision_points): + experiment_metrics['collisions'] = len(collision_points) + logger.info('* Collisions ---> ' + str(experiment_metrics['collisions'])) + return experiment_metrics + +def get_lane_invasions(experiment_metrics, lane_invasion_points): + experiment_metrics['lane_invasions'] = len(lane_invasion_points) + logger.info('* Lane invasions ---> ' + str(experiment_metrics['lane_invasions'])) + return experiment_metrics From 5b5a26599d4bbc3a717868568e1c01883291646b Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 30 Nov 2022 14:01:28 +0100 Subject: [PATCH 20/36] Added brake command for CARLA motors --- .../brains/CARLA/brain_carla_bird_eye_deep_learning.py | 8 +++++--- behavior_metrics/robot/interfaces/motors.py | 9 ++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index 3bfb7734..3607fc25 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -61,7 +61,6 @@ def __init__(self, sensors, actuators, handler, config=None): self.net = tf.keras.models.load_model(model) self.previous_speed = 0 - time.sleep(2) def update_frame(self, frame_id, data): """Update the information to be shown in one of the GUI's frames. @@ -122,7 +121,7 @@ def execute(self): img = new_img_vel img = np.expand_dims(img, axis=0) - prediction = self.net.predict(img) + prediction = self.net.predict(img, verbose=0) throttle = prediction[0][0] steer = prediction[0][1] * (1 - (-1)) + (-1) break_command = prediction[0][2] @@ -130,16 +129,19 @@ def execute(self): vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) self.previous_speed = vehicle_speed - if vehicle_speed > 30: + if vehicle_speed > 300: self.motors.sendThrottle(0) self.motors.sendSteer(steer) + self.motors.sendBrake(0) else: if vehicle_speed < 2: self.motors.sendThrottle(1.0) self.motors.sendSteer(0.0) + self.motors.sendBrake(0) else: self.motors.sendThrottle(throttle) self.motors.sendSteer(steer) + self.motors.sendBrake(0) diff --git a/behavior_metrics/robot/interfaces/motors.py b/behavior_metrics/robot/interfaces/motors.py index 75f55524..f5ec6dcb 100644 --- a/behavior_metrics/robot/interfaces/motors.py +++ b/behavior_metrics/robot/interfaces/motors.py @@ -23,7 +23,7 @@ def cmdvel2CarlaEgoVehicleControl(vel): vehicle_control = CarlaEgoVehicleControl() vehicle_control.throttle = vel.throttle vehicle_control.steer = vel.steer - vehicle_control.brake = 0.0 + vehicle_control.brake = vel.brake vehicle_control.hand_brake = False vehicle_control.reverse = False vehicle_control.gear = 0 @@ -217,3 +217,10 @@ def sendSteer(self, steer): self.data.steer = steer self.lock.release() self.steer = steer + + def sendBrake(self, brake): + + self.lock.acquire() + self.data.brake = brake + self.lock.release() + self.brake = brake From 5ed6077d1683e20483c47dd07a4d664a80ac1c9e Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 30 Nov 2022 16:18:50 +0100 Subject: [PATCH 21/36] Play/pause and CARLA logo --- behavior_metrics/pilot_carla.py | 1 + .../ui/gui/resources/assets/carla_black.png | Bin 0 -> 26983 bytes .../ui/gui/resources/assets/carla_light.png | Bin 0 -> 11318 bytes behavior_metrics/ui/gui/views/toolbar.py | 26 ++++++++++++++---- behavior_metrics/utils/CARLAController.py | 21 +++----------- 5 files changed, 25 insertions(+), 23 deletions(-) create mode 100644 behavior_metrics/ui/gui/resources/assets/carla_black.png create mode 100644 behavior_metrics/ui/gui/resources/assets/carla_light.png diff --git a/behavior_metrics/pilot_carla.py b/behavior_metrics/pilot_carla.py index 93df3bbd..44e8a3eb 100644 --- a/behavior_metrics/pilot_carla.py +++ b/behavior_metrics/pilot_carla.py @@ -129,6 +129,7 @@ def run(self): self.sensors.get_camera('camera_0').total_frames = 0 self.pilot_start_time = time.time() + control_pub = rospy.Publisher('/carla/control', CarlaControl, queue_size=1) control_command = CarlaControl() control_command.command = 1 diff --git a/behavior_metrics/ui/gui/resources/assets/carla_black.png b/behavior_metrics/ui/gui/resources/assets/carla_black.png new file mode 100644 index 0000000000000000000000000000000000000000..2a800a013cf1a1f1f9d0f8927a6339680e8840d2 GIT binary patch literal 26983 zcmeI52{@G9|L|{P-*-u5%TkOnc0-nsB|C*E#u!YN5hHuXK9;0XiAY+IJwjwDvV;_6 z5825s`}WS%^LToe-|zKb-u2=tPUrqE=X}ohe9v;brbt75OSyWo1(}oO1NG`j1-x3xsp0cTX4Ax-C{>}s;X`@ax>fl?W$a!8f4%N386@t?5i1df1T0gnGTPKGdO*Ag|#jF%Wu}wm;fIr z3qQHOpaX`E#jd>`7Xc0WwC5_&?$r4woS@ak&bY>h@tjcEJ(bQ;%Pt;r^_glP(Nh?z zQ$-PYa&Z=-a=c-iR`_`5lBz#o-@5ECJ_blO1dxh*Vls&?X~T>iDonsxcaYlUNhCKE zk0N0lcJ9h_%(n%fJ$fV-w@Y4aTiN9bbI-YC;rDGzGIzYGum3s&*4$V0ysDh(_`r#n z@s1IqOz-&}bW9}mjzB%hhIp17Wy8qyJ^w2&kk4kF2~HR9ap_PD+$VLL4ds5to8M<-t%%82*oxIL~iid`eXK zlA^nvJBm8BRvrAC^rlW?aD*23%5mkVovh$5ghG({njrRw>?L$9>1}}Q-pZK-5^jg zamYU@*&+6H+&tZ#e>BYw0YN$=U68IA54;}qANBA^_4M{s|IwX`%Rg#*U^K4ad+-~P zf7J9a^>IT&jFBE_Pj>`T;|jhd{QuN=4~#ML54ig?5i%Ihc zo1t9o&|V(L6(N6B`XhAyu!mHKV~|RCT{*D0Bv@S96d%`;5-_NQh&U7`F8+&=pOSyE zp@*MFc5n>*PgW!(Ora7mX$iccU#l zkPtUl2SvyqmA^>-h+mkts|N<|ia=_sE8#1|P$)Z?3|wA9(%x1QEDN=j14~It$%Adh z<>kS0;u3HqLc&g39QqT7p5CwOe>1I)MtBmY5Z?69Y-5K;;LY!a5=>SaiIkFo!@&}E z2w5-`ij)J($;(K9=AOZwn%BX?4FB%F!MK8S||^E3Cnp4&@f1f3t*0R~495}p!*5b8)fMaZ9O|7g~p z=5K1KzYpjB5d5_Ni~7Iz;pK>Q{dW`Pr>P%O|JsNL+8*NtcSou^;3H&zv-qdoKP&## zfg*&kL^z}V7Yv05{K|iAAmEY+89Nz-3|QV?TpBDdEhP<>lYvTtrJ+c1e5^~!%Rps+ zAL#$wKuF^g6--k0_gwRTav=VQWP~H!)d6Xz1o^#hzgPUfG7g zMoP(nZS5uO@ZY472wQt3QW7rpd;k99VHZ%{g7{Pm z$8T$q?jB0^?r0YtxSN|Z3IQi1RLB)qyWjSOd+QJn2F>%!V)=8{{9@yOW1QL{-BJIM zEb~h%5x-;N`G2g>|Mk2IhuTX^Ny_5SDY8(oEJDg2Y%61L3zoB!utmzt%gNYFAr9u| z|D9{%{`Q(br{sSkul{}i{r|<9_;Z)@=N;$o8|A<6Pzh%o!VdapZaGcJh=fhj|9nG= z&%^lRBJ3CLzaJ6yyBz*S{ZEJ_@$XfCef(F|KQzy}qA*Glzj^yh^)Fh4 z>w=$5P4HJ_NF{v4NJ=Y0{#yN44MUVS(%D=cg+Hiz5Dr`t(lWm({q@N|YFhkD&0n7U zrulQ${&Jo5BQAc{;xEha_jHiGi@IMzX77mi-(UEnOaJ}0_beVb*+a3P!U3-RP}-9@ zz_o{BKZOHa`=PWabAW3P#eND0xb{P7Pv!vE9*X@G4sh*<(w@u#u00g{DIDP152Zbs z16+G3_ER{(wI51*G6%T!Q0%90fNMXL_GAul?V;FD;Q-fuDDBA{;Mzm6pTYsI{ZQJI zIl#4tVn2lgT>GK4Cv$*n55;~82e|e_X;0<=*B*-f6b^9hhti(R0j@n1`zajY+7G2Y znFCyVDE3o0z_lMrdol;O_E7AnaDZz+l=fr}aP6VkPvHR9ekkq99N^kRv7f>LuKiHj zlR3b(hhjg416=!|v?p_bYY)YK3J19MLupUu0M{Oh{S^K!E~-C1`U2^S|GWz?{0Cd0 zw7?Mjhhp#`w2butz*hhOupt1jzKj1k0RYAn01>M>0Ko17z#(+!dG0s>IOeCVu4?Mt zGo9>J%KCP_Uipw1C=E+-HYtJDeyA`f~grlQuGsr)0mG>6}{ z3f{VY=_^l;8>^Fy?Cf}tkG#L|J|+32f@SMwK#GrCUvje2K=R_9;@DU^7Ag&=v?Q6J zQx8c}4Ta+Y7OF5N?5T%_AVXmm9e^tA6s5{TLt!Fe7AgTC?5B`U%u2SzB`2r$4zZ)> zfDMoV%7lg5B66YxdFKX`3gcywd^&N%q%>(tl@cIldwGB?fxAmB>5=q95p2g>m+FqO z)%0rP>XIJOGLWGux244`ZX2UP5*M}$L=#l0((}i}73WLxT29zx%UsU|SU$ha?HJiT z5-4bA`J-g(E1Cy0tza`Oc@Tsj7>7w_ym zlexu&Wl~ud?@JuvYLHI7R+8foamnL2kuZ@db%`bOR-R|d9P;EY%fu7CrzEK)hP2Lb zz1hiXTi%7!77P0A_9h@hPzKq>BEc0X?a+2t)GC)dI0Fv`Bn+i17F?b|p5CbQs*yK! z1)C|s7Winf;QULyS5+0uJL>8#=J;_SF$7IJ>-8_FR0+1f9MBo(b2%pf2w;aE(Rz0# zmR9MF(4#bq)2Gc;AB7Yhi3j3=#n%@))L)6#_ZU}CIfNnPxt4O_ero~6S4saCSJF&^cZkWHg-Fsrdr{@Bp@+w?mPVq5zbQ5|> zNte(~)~^15ez2Ro%j$5aQwhP=vHDjS>Bh4oS;gxUQ)eCdXOsv7XO|!5k}IO>Lr;@^ zt_*eP1d-!)nTTr>Q#oE*+V8g0&dv_MIC2{((oCumcH&Dum*$Q}-S&g!XIy#0gylPW zB{j<_*S^_*8JZPN8|iSYNLd`TrqnVp`R#=bB}FPYtFgPI<7ov4J({>FVIYENT$8gh zC&sD*98yYZ&1Bxp$p_*DtfmrHpW(_N)g{&G)e*Sb0R>#cT=du+oSjY)wH-)0C_Qt| z3zX4etj0)Q>^D0;rD)C6#azmCdEjJ7uGjqXDb7Ri8qT&X6kmU^H+`09haMGuo^#9r=c)!Fmf7h%@vaKywmWL3ICINvue-hvdI>V=x|0m>l^#CWJiP_`Iv7Y)Ky1EJ(cXhuhIi?% z{m5t*AChuA{D#BHkB7fR511$9D$x#ww=#tB5n}Dl(B|0uvwK8J9MEhFPZ}~;9Tk#Z zLTIuuD#*yES`*1Qz6Q5RRL|%YcXtz_TI0=2J({884KpfP2ktATqIk%o`f8YYuhFOaS9(R0otX3bKV}%oOm!iY zd@5PRAbD7SEugtLIn>dFkn|qMhGDPU(iy%8W#(2^&)pd+S|THON7?c$Jnro$zw_bc z{fU^-*@R>Hm4u1QF`;Hgo^15>>NEI4N`E8|;9l1*}kuHKCzhiGnp=4h^%E#x)+9Kwr z1nF85DQ_`SR$AXXtKQKieA?oHDd+=Og=xC!emJbgjioy(!!^p0(>0cTA|t|!@p!pc zbI^zbZgW<$7KvBZvz* zH^)`*OD7wT*u~47Ru(=pCxi8`tE!5qj^E1Bat<6U!FUq8Ml>~^)(^C!xp|SWELF4S zPCYYoFuW5GcWd3IUlyc%LdB9+RT0pmjGt4e6>;lPcS}WB2RObzL&(yvXfiZS#OiyR zk7J}7v*pNG^S}7=l+ddVgh>Wfn7rTQtXC>Ohc}zb7#=QK`0WC8*&vgqBB<)~dSZ%t z8J&W^t)}{&>fD;Z<{;bir8PH5uHeyGV#O70=*l_Z9DH_*s#NRfNbi;cgT6e8h#2N1 z-NGZbt%1UPcU}Q}G@lN=Y)4C4Pi6{UUkD%US94`}DwNTB17{TqR&e7?o4!SsqG89% z0S`t+M`{RRFM*}sEqr!g51zhfyrU^Gp1H9>xiadPbh}nAv1;i-thb0#%-H9ILqr#8 zAN#54xRl;RRJ`wG#kvwXHzVmH)5!GC6R&QlD3vf?ojV#1ANw%l(#8zph}I%?ZByqm zpnJT0f-i0QNCd0{$wioP9J#eECvxVPXGlR2_Jxd>&CHsKISt3DlQh_g??;2vfrQXr6WR5w@HG562KjB7AZQh}sAmJk~hbpT# zm6!X9LxRwp@3xN+{6uh)y%R`T;=Dxn<t=k>7K6*d`dg{bKir9Rxoki9B}Z{}u7brR z=nQ2lPw92%79ZNQEZ^BG&H;soB+Sa}u625zYireh{EEe^;t}q#>DaNiw}GgI<-j_R z6v%nhz;Nl^fQrytRcw_SpS&KqB`&|GsjayQv6?V^kBA4(Uk6-0*x8eOn&+NYrzle$ zx$8YdUSmYJ{9>D|GD7*~HYxEq7@mb)$BRY)GOt^nK7epKJlT}7wa#Z;$@nmm7Z%Oc zGsz*c6L7S5*EH(U=S&hqVCxH9#d+p<-r!w-bM>ul5xI4zuZg&_RiOX*5cgarJsW?| zw-4nyK#nRf;K818-S1tN!C#5Pf&y-qzc|GXtPrfhP2U z_q%U-(8{ajCDkKdU+6e(-Uc+9Sbx`K!KS^!?-5Mz0~h&^sNGy?+ci;lmZecfJ#4h5 z);wJ_kbJI>S#dcs`p`Hs9(Z;J5VNh?#M_duUkaU*zi(_NE#-b)i2-VJzMR9VPwhdr zu6Aw6ZJ_OSH)FE$XPQ@!*N`zM=On?fb*GsM^N`|#9LWo_C>ll8$~!6XfP3wc$LL|S z63k_-CAA>9^)Ofw-`gPEihqAp`NN2v?mU`aKmgcXyP;N({>sa3`7Gzg-P9MD#a#$z zTWa#@Iv(mnqM=&CGACMJ>j0nmu^CRO23oEjdCJKrDy@01Y}L$X>u>Fbp>U(A;)+A- z;bR@R#lH3vBpm=}9hJ-6TI(uN#w_$D; zH=e|2|M}T8>blilMS=H;zovW7&7Ogo`ecaxlLe!SEG@^UuyQ)7?tO^ZQ zDcZ8%ZO=t1_;l*Z2b}e17eNv*Tb>C<5RquPg=EpvP0U->Tjh7GK zQb{Oyf7Jd(8&S;42WxUeo1`H84vtY^`!2@1TR*+#{b?Bzy7z8;U$hXDHF9$DHSX4( zshq`GE1#J=Pzh=l;rNaE1AvXUCwlr4|cz(?)=G2t>c!(&*c+LOH}D3{g^-V{8-pBB_3JuAoHD63lzXgf`trTA|z8EyS| zsGVtE-Ll=_$A+n;oV4A(r~p^W6XaCT0a$skkDCH5Q-ySIduK#=grDxBhRP+fR+1JQ zK(~*EQoEjX+14+65(S&I1|?WhruN|$&6fs3C+Q2XR!yidRbGa;Y0%AVfF{qka~)k= zmA&yzh5O8RdPbpaIJ=hnI)ea~6H0H(TFiJ;Ea`6H${^Za7vO3LxN=dhy@u_?>>X6` zi%XV?bsKC(363|k+1X4$!VtF9f|ujk4QCAt6PjpFF_?4=NOe6(4{B1(wz)ZUKfaCY zRB0fV&QJ2T3TDgeqx>Z-c!tszVGc}Mm%(2SFlB*j9M zDg6~odi{jXQrtNH4`3|LN)9`eseyup8C^!{AT}#4ojVR)vcp#0r%$l(=4$d z{wXSlOK-l<LwWctJNtbCKVsXCp3}*@Qv;4fm^4BXmm1N(Iw(U zx~DK?{tVTo0@$fEhuWn{%JvfS`p(02srMdx&7ULFE!oH|i72`m!z(~AAA>me=KJ{b ztF07~?IK~m0W00 zDHp0By@%SKWn>!fWtsZiThmt3JOO{P+8m0~2>4os$G>U}ZP`w)n4?#4(?w+VM@La- z$bG6Rez8%fc3!(Kj!7kG!LxSBhxw`UTizUq$+O-MY=T&yNkyyVY2KV;2rbIg>IAk0 z#YxHvuolahiJO&X7VM)uEAC8RGrMR|mgr6&u@#{A&CB?j5aSO4YhICzS{8PZSA11Jp<~j zM9UsCJZbttb}$XIk-td=y}wTTb!R|nIlsQVS!a%%_i~3oK$UMElUUpcwXB`tb$*VK z-Q36}-bPkW*J$B-*6=~6-GOKBOV5Kz+%iAI&G?@8OY*^Bne?SO*+q0t40E4p1Y2p^ zplV9o?t}NjJle(2RMq6jIe2HZZ!(UWGhW6TZ|*t@U~QSJ7lY_H+14(FhCN=L3}6hp zK{xoCY(f8+f^2WSpL6E}Ub7U7_|5lZqjF!ON>auPj3J+n(bby=_MW9oEhTY_{%mAs z5m*IHdxTyng5*9axfC+f7(=5yXyOt4Wg7ma16TMlmPYLna>Y{cz2(sjr#YN^7wGh+ zMc^(^bRnAncCD*#>hVHQ#XK)#^2ysHAZbhU$qVf~{cjH&WYEj zTapMOo)v*by6f#hibi+%p{Bkrv^%vIka%bPsE6d&D~ z+`v6oWLoSW!m)lm{|b!1eG$S|$zA2bZxr7=@U9GZ#yqIFroXWp)_$a@9hQ>!b%S@2 z_u4xT=aALwB|{Y%BykX;?86hKw>fHO!YQsxO|2>W2)8rT#@Wq?KffWmkiBP5ci5;|Hs6vmGe*_{C@7@ZVf&~?=;RLs)#0@mr}3YMtKk*}s@ zm%hHi$*gCwuJRcKxJRkIJa<`F#2YMUMaiQ8l54uOCS+UbS6su>Rcj(Ym){oCCa6#l zCb&Dp=&~RwGAoo)j0|B2$~!&kD`lTJetu3VnxA6;oO{OVy`XQerMNs&ScL~jDchdYi_n{aw5|h3 zi#Rr>Ls1n`S~EG?GWi-eb|)Eju~d3HOW=!$)1h+^vXGIscI)zsa_SThHtkqFF3`0QtX$; zyg2rlWbg*SMsdc}n022otx%sI1Opqi>L zD_1G2oF-%WDRP%@mdP)I+2BMz%|vv%ld1q#zl(SWZAz{>Q(I4I#!Y97s=adY28(Y8 zdhy(2;x)anjb)y)MAmP)Bp(!sy25$v>km2U!=Ex*7j+7W6-{m07*VEz zMQ(j@aVZEbV^i0)|J-bWb|N#n<0$L**_wQ3=P+N8oP3xfJ$<%>ebV;AOUgEU=y5`5 zMd3YHlx&WAjC}~FP}ftlo3G~|Yu<6HT;V>};D=a}F5YIl7~!b=G)UoIyKX1{nD}gA zOnvK+Jx!-B4y_t*FtNl+Hs?`r^;A%-rS!0V^dhezwd_-?*nos~%<{K2X!Px|{?)5P z-*$w;vTlzak#N0!r7-x}XjK)Hiqvc&$Mn#BvcPcYd{&clE-*(ns3^frifo2)NE|9 zz5pEIdDgug;~aBZk>Sa;vj|Rm*F=8p9_|-~@xUAS+>zC$^`>IInAf*I@A$K-ku1OJ z@sFg?nNf2`*Ds5+L)&~V+gLb72r=Fm70oAEUfx(T&(we;%PvyW8G#a*g$!eyl#is> z@dV9CDrG3f)XkRFClG(~puOj6Pd84+@3mIZLg}esn_Q}#cC`G}df8l1*SD(3y8hes zyk@Z$R48zC`F#6H(!*zpzYi1Z03Ip+t%BU1AB6juLP)MOJoInkeJ9&mD?DzhJ~eMr zJJ!#xF78k~you(Oq3aa9@Q4e(?3uhZcQ{Q1MD}6wl#+XP5eHFU+_1JwUyinEgUOm5 z&k)DV>X_7y=YO#JMZvgspscnMMOkD))Mu0!_t!rJeVVH zTgGR{uCOj;oa4ui5+|3Oqa>evQ+kHFhl)4A^Qx@6q*%MG82scWNnNXJjylk3Hqqqm z*ZdkrEo;)96VBMsqISR7pWeKv*0cZpv!-4j1{FVjE|>DblwjO-G1Q6%QkA}zV(cU? z%iRp4Is0iKW4)FEJ+o?B)59qUKccUw5U(5jE9L)dbmNb~sah~KKY`=Q;*{-$DNhKPg9N*RQY?IdSc?Wd;tm+yse-Se8~ee&&O zAh!XDdRH7Lb}l4){08$#38Uwfm*UZ5E9&>-X*6O=oGF|`cSHI-7;a62>G8*>;gae` zDKEa+;7Od)t^>|Gr~v1((}VAUX z0256dR;io_OIV7^GBFGjXTB_7jH3(T9Dl;30?+5S1wWl)4!qA>q0}?#OOcVloW=jN z!b^~gJkjT?*wWjo3+3jm0E=sHh#Dia@c;zHRR_rOzZX~fD0UWPC>1fsJJx<0r2MqY z_7gfGZ>t3t!GnLb%7oSYuAv)qO?)?3xAF@4fVv&y(bIkLlXou?F4onWL)b+wkQ%9+ zughj9<2S}t&dbj$*@h0EtsPl8)9^3_|2md3)rOr!G`4RPVzV>A{c`-lk(aA;vFzz< zbRX@u$QJ15S@|TzkKW*3W4|Gkkkj%E|1Kfb&@>b%Lz#%MC$ zTs!8fb>9T4^CmFp*`C!opoU>id;Jm5xu8`(F6yGzubsl-*I2b@hkTP*-g!!Qo394E zLEO8GY4+$0*4>F_F8*>vo!#F>pm4A}UD5D?v?b_)dB5nee>|GG636bEgPfE{h*lCgZbG%qd%{^wYCphDp2xp3;U|leY@ta-o=V}i%XLY7tynFIyB1)!^@ZWIi-965v~>pN z?U$%%Mt)I06Vk6a+ddvy$~&oI6`ALhJ7s0a+Wu9UJU5DQ*JO~aR{M-s`;}WkE|K)5Pjxl z(gMP~h*bpw6Og{}#@l8xu4qhyGoyBLvnk7)f?acZS*LI{DrBZv^*n9;O{=zCvzSsL zqH!tjt4H13W4^adCzZZ2vL*_A;`u?c4nD`OE;jNdZrp?+PcN^wL0M9WL&KEJm+GZ} zBUvvW-SJjk?+LSzEUm?Y*!0@;T4A$~ciJQ^ae&pbR$W3)k9R~RXDlxL5Y-H+^HvFS zWtCv}iEgXIx13`&tUPDVKG!aQMe$sL`UM`+)ljb*5H==_Dg|lj=vbhLdYXny8+sqw zTOqTegSbq3F2*0eym?i0`F1jf)X=sF`90PkJIL`lZH#aeiH+KbdLQYqwgGhkRe-e| z`8^$=5n6?+=cVK73*uy>ZGPFTEcIGQkjRPO%sMBSx7w*e8qHFvlKZS&gvb=lU3w09(XAXnZF&p&9#|o+_TL!@bSbxevib0~m6!tk$CW(S!x@MWByi@b?X{k&| zK^G2p!NLSYSvrr7E;E(+xJA#*IulS`Veb>@RkxmkzH?3UN!&_3E~s89cA<*w+-a%7 z#Wy*nPC9fXDfM~+W!FGd<&SY6aY`!ah*HUBc8erXa>Xbwxf{JWQ|ZZJ|3Km`@?Ecz z5}s$3m)*j;##=IQ~v4Kc>m9;+q-SQfzOe*XK?e zSCZn+T>Y5Y9uoZ9%R!l1b4QLfp?tOARlmod!e@@_mtHZr9GuWp(ENoOM%Rm|(|dHi z=W-QUD~W~?*@jR)s~caM-W^QlMTM9BvT&GJ;;3)J&JIXW->T0=(A2yv1WTSIy3~TR z?_VQBR+dC!H2F)8GiL^V=%j}Fer!Gm=W`E}J2Zs;D7(Sv`aJ9CQ=)X<#FTap-I3A_ z`de{@@k+SmabNEHbnPA8(;8fyUME9xJw?VVig006%{8%s+;4XL?Y#l6<`jw6*iPdSgnI*TKEt*ZxhW;g8^@GA_iz9SKEa<|IgcB`*nD{ygPfO>4n^9}AzSsVM1GqD6!6DqT{N@|Sqy$Kn9S+X@4gi1{(0Zgacu4>NaE_@#@WKsPUqE*FFC4O#)dd)3zK8 z4o7GZ-_!r$hm}FITh%rzdM^RgPW2*91NgP<9@d)mVqSfhfQ8h-uqMi<<9ChZ{49Z< z)bTB}P$nMVl0P@cD7;l~$H95@Z} zNsSf6WmKK0xC_cTC3e0C?En;^qI&70lO&=j9?boY&hepAPw{kXvWR1@Su?5Ggp`c{ zz_vZVM5|HouYce~%Bc&q6|_5prxnr`0o--!r~kOcOejNg`(x<_*j4y666!OhARhbi zA25x4y1pRBXi^;DF%q7=830IV$y;yjf_fn5XCA+N;j>sv>=Ud^4!l>e0X|kNpP}`>|p6 zT{6>_N_OTRM~u=~Z2f^oe2qj-{Dy_U*z1vFf9_-zRZ740NbT{;S|x5H3Q(nGG_w+_ zy?q=U#-+>o4&y|(Onri($D1Sl_n#I9Xm5u17m+8=(%Bn%(y^cL?rH9kWMDP=#6l&e z)c$SYi<6*XJ70NH`QLvmg*|kJ?Y`3>wWPL3^7MoFzG#TH3WNFe`d|rfKMfB|4^TC- zgCI-)H;*EDr<(JwBL0yalrUQBq7p8KI??7g<~0sj8EbhUmYN*|zqWnb#RSX)IcngQ z+NYi4-QJvak>6|#7yLaS8hW?puOZXat@RE;}mtj2O1SOEQpbvc)q3wskA zkf0@pntOsZJ0~Y2Z(-{&;(7@pKpZMc<@D>2fpa@dDrC7v79Uzr6eUx|)%% zX)KW!+fa*iAuY$jN(JLBK0;?(*E#-KId<_zewVdl^I3LdrA`53nLk#4Q*x^KC=*5uRXQi?BmATX=(l6Dyd1vVMcfje zWud<(s~6zshqQGeq7d~E(`h;tSCd+Fw`NN#Petg)hS`-6%`b7;g>Cb?6np+!=Gqq$ zF&G{M5(SpMUl9%I|FRL$dv3kW+TXg>97Ugdp;DCiY-T{e$@+)yY~L87U)#a{AWHKHgh_;MUmr%TigzmZ%3W3G^4MY6&Idb&rx`Y&kI;Ag+42@+i;#}&3ruJL42rS zyiwbJ3PBVwZ7uq?s3ETGxSMtbfph5j;1;^M0>^k+t~;=-$|K6C@;GTwulF;Kvm%iS z&_fYwU59yBRwGDcEXrn*vgRSjSp;r!5F=^kHhzJWjTp{$wAsaIlq1KK{NwS9k9c;} zs0a?|!#qhoPiS^d=(F&ia{>{En{gX3RKjtN+>`VE?vnTX6+YGc-ARUm7 z_!n$X&&T&!aV?i*3-q4e%cQIltjpPJg{T7Aj=!ofJv1Qt^#M-;{OsJx`8r)>O;~p^ny}W34YEiE1RPOaA?~FwgX$|6DB&83 zJhkIBlaD7l*|0W1H>sJ5yTKoMjofB_C7doOIR=vb z5P72Sf>dFTVpO>DpO(^?cVN3s+fLa#h(YE%as_nE}5(?YYOY83w+?$&FyI0*&HZedO>4aFU zkbTBPq*launZL3MJg5S!s*I~;!Qo>2>m+iXFXBRiUBt+i5veSl(6I^EmRth2DQ+x_ zVw;0@E-SC$ZIKOWzR5n~itg}?UJj^rt?X6TMd<7AtY34BsNQ`?JZmS3YB$@|lvtc4 zPVju6o^%~(;5uz9?hRYn<50vdiB9B>ykgJ5X5Um>(?VEG=tb09Djqo8*MP1xbu#ol zu;@dTE!}fNQ6_&KVUK_+P}ggmLe_S?D;UOz9edTCahA|&bRSUO>vhq}>SG9Lvv|;c z^_jBgnW3H=2-{~s^zX-}a+`2KM+uS*v7d=0HAht8OpvX6*6nUSM)KGpU5Y;%7KGw& zAgp&YKhqfnEzrDNHuXY;3`c82tfQxrKSW01gf<0zsUDNdc!bzGr`Ed@CW~Ne8-0Xq zNfF-G%Xb|sZGqG*2&tD!-EvN=VHcco!6uRa5tg4QNaxg3|AU1rcdC;gdg8FlNsB}s zG1h;EbxhKqyAaN^ZP$+zOIGvAN&gSUn<$2Fc4X~i86Dl}yvYjsXxm%shE@&oEz&0`$rE*t(w7SE-P?`0(RQt(r?~C{f=J$6K zLRaNVqYc=9$3rm!rcS~u1hm~Pc`sHqZ zN_+J7Xv68ti{BGA$q4mmfOgJ**IU)34a?RS<<%(JJ|fK;qC?+RcG(A_=yg-VRtrc7 z6XQ4KG&@4V?3tuv4Z?H5$aL<|*+`D`0%LX>f1?V9Q5Fd`TgEE#Yd+lVB?gIdywpMd zNwwx#?hqCIa@f#&#w7oGIM=}W;}9uwogNL6>Lqq?N$ZIWRP z?zDssE@Ar0cc=A8kLOj(QV-)TWRh*$pWgH)fph=FQHp(~Br4j|)M9ow-a&vd>cgTrKt zhL6|pi$zUG)#f54%0>QBgZc~zQAW;>`1}?apUTP3ny|V_&yOEV z*q(Az@|K?zbY)kkK55n!w6mv!9E}Q~$iK2kpFcqhU(s2@XJ4|Fe-)d<^j9cP$=dTu zYt*BQp7?<3nz#50(jc}Tl2Oy9`;tEy35`RhB`+g#aV<~?lU<29zn#9886|j@!=8VW z{*U>l`2%^fk@Tq+e_u0hT#VZ*td01HVdpQja`s5^!%u8_nhmAm+dG&1gs51TLJ;(%=&K0 z^XY5rh;u{_yqiy}VyOA6B3YJ5elX09{;Lo<-W|WO>_lCB-h#)_y@-M2kM-H8B~jg-%WB zTdHrjNqTsJwx#i<++Ym`pzTJS!*YN+{RNPrLRP zdQYw=so~Ez`sr`>w}Ku&ATx?6C`?Da?k5z2^7) z3o+rfCk~_sM$CTfJIY2CDgLRk%!v)PHhcXk8u2JFvWHEr-wM~P1->HN#c0HG0LI-c zwPX7=dUXoeGjS59h6Un!B8>)ye^f`ZfFQbtPs@`G07Sv0o?lKKx&7+9Pir%qH;+B#T6$f)3(a9YBa} zexLS5-x|w=O)t4}CX=YbF8@>Z#~=c!X$ujIoVXj6nr`4H(LNJVdkQ>0aVo>N@A#i; z=X2g+W%`{h4zc|TVm$jIpTV5X!x&1ITqJXb5(zB+e&gAJrP4EQ;_yM|BPU@pri|y0 zh>tj4TYrW4xfaUM`IkJN6S8hUlzeYL;ds!hUM9Z$S%BJHd`@^zbXfaKbIs?1I!q78 zGqunClPpzBl~*U_j)U_LTYUbsms#kJ=%}?A0e$ZYh^;dOQWl&#uTlEq7T-77X|0jp zn9@aU+cx$Dr)r1DKj6Gr^LEeM4Vcc_e!xH9|H;+IU2@X+5%l8K3E@;eqBP6SFZ0FY z)hRwxa7lraqhcp(WJiHgAXw{jQgf#HP41@5tE$NXgYwmqj4*km>k2XyUV5hE7gByQ ztLXexv%V@9LRNx9RSx1R!4=`@Qb?b(;m82ic8))f7r*RyLu<@pFFsCjl)I9j@tKAM z`m^%9KkK$ec?>S?Xzw4FwVgGG;wQT;_|@f=s|hdeONbF`&MDvcrG&tUz~#d$ccKFl zxrd|f*}_40YbT7a>{Hh6me#SiqJqTvHmY$)H#NP_gWfbBR4?n-pNJ4k0we6ghlKz% z%A;;lp=T@jzZecjyk`hHE1|BLmBU>LufVVc^x^Lx+COLHYFN)fw_=vCis zU2R{~T6&Si2YYoD4XYAC@;f3>N6fR}JgdMk#)ja=wdDrVD7wW1Wn8uz88mtmhgPkW z0yc7J$a8$P0rEi#Qj$0QU)~cwvi4{6b>NC~`ujXm-D2-tUR2zSody1rs5WWZ2V4G? z|1P|IMn-R^ZulXj{nEu`#``frE04t{V|@^1gNmP}M|~!EmVZ=J)Rl1N#gMyfza;$K zG<9N(7S8o1jsTX{JgJ=aHQfdmz#YH$n5(5?{CTjyF`MNzZAC<7)BT54e9165QbY=vA2WxovvHH(KkrXE(dS)%{%R>@NIku zHqAbo`5Wk~4Z&8oxM@kS#@3i8h5mdFTsqDFuyicCo~3Nm$;Kk8EijR~Fu_@!N;FL{ z_J4rMQ%M7M;sglm|~XRQ=vlpkPOMy-b|Q3TX5 z=!6!CvA%AoC~fU**}J%WJ1DXv$U~a@zei<2r;)*MZCY|rcIVgXL^<3OeykypS+I`O?8;2!*E8p2W_Hr~h01hYu)8eF&M zR&E>Km6dt!CXrajIHhUhjq<1PP_y(I^}>3cO^RsEIL(MNNoe-G7q1V3I}Uo!dr8kr z);wVEAElj3R=$tel{m_9G}Y4RE-CdTw=)0?>Pk8<*#4uLZ8$)2?gE5srgh5Ni_2dq z?ovFACa3NMO#Qq2*k%+uy}NRUd%9tHf6!j^s?aA*2gr?;XWRbw_rshdp`)=3{<+s6Q#r5?G9qXL6gvp+>GaHX9whK)| z#}6H4R1c}9c)@0HG~p70yl3^kl4k`h|Aid#~&$s{%9 z)LJqTw!%<`rvyYg?loUw2)j6|y?NZvoos85KS&sAlUiHBOAobB zOn0MxV0HP{p;5J7uyOmW_tUMqObVvDV1@lgXIGI*s>n|hY>)4?`q!g0gN#|c!y$pKcaKF!}eGRzRRJN^1Q_Kt= zxXY{^VmTB$+L2%SP_lz&US>a2$L0~g`^4`P&J8<+N?H$}V%6gOh1n8}jj5s+)(^b5 zr-)e$z;Sy0q`@4r(e|sP8j1G{adgAruz%;rEEyJtmzf+4@$g z_+WrxAt6!klw(H$qE@Url~oMhM5n>Zvj6nRdi=v9cze~Yg2-8!%|jVv*Iy)hr|^s- zlJzF?f$G`f)cqk>7a50zx$D&dchU<67zUXClKxN=uwzHD(*|IJRs5c0mf2QpqJmPy zx{m{#-G;7uR;{*Cw?fM;CdGKxR>I)_m=hEikw$<{jwuCPajbu_97XHq`+wx0b#n_T z(U-P~#~T?O_?tv_Dmu^~TD>~OJYZTK;xk-TzPsvw+h@&{i&K*eiBmieIqoc9=3fOG zt$cn0r+(0?ey{3OZwu+|8+!%xY=;fCN5vG#veHk}6W(^ESbTB` z=+d5{Zm@U6z70uE#&UPOjAv17M+}%lX^YI3_N6*%jAymG@((MFPi0Z)s~n8pZ1l71 zaH(;8*sLC8<>ipRuOadC-EAb>$};fu2g>xpa|4;bpHYwuY4R;`$`}Wf!0O$&qMwSs zQ0|mpCf4JeIB>oVHe?EyGQx+oCq$~-Z98IC4;o2(8olgm!hCHqBxO|#%v(KcxI#(X+bBDc%LKn za13EPPDvyEQt%$mdKZq~-(48vV%JmNd{x2r#6ou38^r^$^&*}xS8aq3^uVQ7$Y|^( zP$yIs@ZAriqQATJ1^GG;?aF*vl}z$!8PfaX7k6V*8nnc;vl{%J?FpN4$|{-F`2>$i z2{JG+G$Lt|J)~0V!+bG8d!~QUDsY~|%-Ephia8bVvtu1I@gTGr7Mhe6&DH-9n0VUv ztm|`=b|qv2@d2m6hLT>~`Wa=)kkU|!p{}unMK^u>oMQ2czFR)!$f#QYC+V(w*N)8T zPx_sBgyEK@!1J$8swo%SWY#YeijWMlcDt=Yk9?J?4fJs6L6yJtrEw40W4SifLrY>?{i6<()$@k(WMxN}sm?wV}P zY7mpD)Ex8Sr{J<-vDv2K+Kak-jlh?sk~4y4I=wH(-RtVuFL2A4lnx~&U|Pk>n8dHYHVY$Uvl$=c5C@0ZczJ~#FGS5^juw*%)? zpJ6#!3*~f-Z68muf5OF!GZOxp49D(C+}}h6U12468T<&9?mq5g_fJcJk?uDw4Y$Yn z6~po}6}rZ_XEgD%RX*q}Mg=NunW|-eR@&mnJXZ9KMw>5>TX@fGR&XCT)I=0&D645a zeJ<&s7J+ZH57Ule^W!}INlmw-O6b4LG*Xc^d|NVeUcYzLRC{|tkUR!yR*~hsl>Wr7 zPLyAgkzjhGUzYBh6pT1HOAKj?h8e3Y;CpNz_u~aEJXld_Z(mjMU@J z7~JAqxO~){X_z$Kn-4@X8>lxU-G06FgK7e-F>OgL>oa}HXGwn1YWH(vPx9@1hAx(X zjXF{jan!L-NtFEKcHJ_JUZp6B-wHJA9(ouBi;6lDm9`uxW)<`J)kDN8vpN5&GF1su z1!~8?W&L|4H4^Y*lS6i<-f|z~bTe91s>MJk#4#GJ17hx`W|Ht&x?(N|;h|6Uh)Xv# z)`QSG>Kc`IK6~~OY7DzX9eTafEy}HE$l*j^DCePJ;5?**W3`KIq8s8G+vL8)42n48hVg`P$sDcWZ?ZZy^ zyNQxfz5OgKr=U zbXPv~V(uSdMF&@*#krU|y19bZwm+6%_(Ot-DejHvf~kN2u`#BBq0&14g|Qe^6k7Jk z_&pV5j4$%1LXbpSx>?mnKC$}EFTp1hgy(*YjRtyv<6rt23`eVr*E8rMC^_v!yQV(I zC#d|mS)VAuo#2kwhf*C<^JIZG{PO9M`;a{;KTHv3@C}cLTZ8JMEykAgm+h%Mws+w_ z=rNo3%ZIwle*chR_-j@=r>tkrR;SnJU{dRIqd$-UAI@TJTa`XWCm}IR38>*?r6s)Q z>Eac}Cy%H=hJ~fE=o>ivn!QA&r+B%}Xh9v}zxF^6pcWA>f9qPx6J`ld`(Aw?BF`Xa zqV!bY9??Pm*b&8Np2d#)p==XH15jv|MldTBK(7E49-3;Y3xKVi%Js*ea6YrP# z2|7QVf;xB!M?0W^p!(EZ>78)Mgqr^GkRabQ?-89a{5|m#v8i8$4cO!D$Ky zzogI4kFZ;JwoFWN3W*JYyovGGZjbqU-+|dhD3}5XhMQ8{@tz}?!2(O}U ztZ6T)_`pYz>e0uCeUCCU-W;**$R|!*e*nZ$qI#B_nc^NNg*9eHww+3Vj0LP|GYbtP z?DE*9F!Imq1h!qbUi*0eQJFggJBpwf0G6RKG&D5X2a!J)@d`)Z#wMJXcrkgrx@>}*(XY0m3+1umSp{6rKdjmQ`S{w+>(8&~Ycl$ggX<+1$}|EP z|C!A2J-yrW{X47X##o2`m(UTz8Ld;`?VrjXu|3QO>UZ52l z!qiR$oP%#zo#77_GfY!#m>KI8Ya~!6UNE$OvgrYpEA(1v_I6i!_OZSA7;(L)Ip98V z4wO-GMU&9E54)Z$YZIp!Uj2hKkUSa%?0hgbJjAe~^ZCgmLD8{i*S3A-ZS!A6o3{0$l-Z?MIR$H)fokY||2hCXyklAACNSSw2&@gA0mxsma0_ zIiQmWFGrKIFQhSqos_|W$t#|l=f&^a=^u`ra#P8J$CGItbJ zUlK*CWNWW}t6WAu+azFDzrBh%+)-+m-OgfcpXX8M%5r;B4;!V?S2iTE!n4F*i%mPD z#gmdh3&GF$wna5dI(xWR_ePDV`Pz15q2jrl84sr}c(dWoRsJoM)|)oEsDi3#DR9l7 z3u-pm%P?h&yXKVQT_;WbK1Re{;dUASL8_>u-kv>x*XpA4mF1%IHiV zfDWE@(EsD%qiOji>y{h;A$Kc~q#3Ztn&)81jVq<(g;iIPhe4g`0cX;7_}ZGB37U&L z7!R_*nd4Nx3qkbmK@2FDfR%9F6(=E*8;2Ez(xFqt7mn>t8F6?-UNmTqF#mKcr(x0{ ziv0rOP(c1lGp}4K9FACwtxUjc8XkB;>~mz+(FP&)!8tWSMhT2 z7UYR_jel{xYzPPG#AGkS&xcyBXwOcueS_WFOGQ@6RdjD42qqB6BZFFHXRqks&# z!QwG7lom1DR)ZZ>I3)OB(M#xrw@KjCs3P#l;!XLn4u`kcSDdLe*G3f~zMgZ6aN4z2 z-T+7ouAL`M7n6qu?1l~`TEwMjHraL$n!~#>@f!BH)aI z^SZ2r$(n=|2dT5juy+uw0|F7GVssEk!LV8O5RtgT>c!iGyh%s8#!jTHm|zZBzw`5=_HF3w7;nrJOmNN#7*B{q-C z4ndiBt8dNm7~T=PsHpCoCS((d+U1c0-jYgOHLr77k~mA7yoj9x!@i}}FQ6K4pJOt4h4#zWI0p7&3Z#In%_{mfz9NGSJPqO2z6_8+Z zQ)JseRN!W`DzI4@e?%8UB`83h2%wI*1!7B>%*eVH;Z*8istM8MMhJ13a69e9N-Bzg0>_y45>34_IKu z&S@LMvY!OPXE$kn3c^9HEnX2NFf2-$&PPTg5}%Ll)SZ_HxtsEsjo{763oLgX^A z)-cxO*SZLt$cp@X@p_FLuL*yX%Te6E@S|@`!qO*sM*Te{uM&9f!Qm=zm!?D ztla1&wOX2s&0S*!@kUL7ke;f`%%qN{NwWykzi~f!5PZ74B{b}5xa~|7XNG`6^Pulq z9LL>Y$$nM$23v|UJl7rb;+JB%9VMfWb9v!2>5g6wr?gSGW6Ru|(T`QjwH9dO5ydWO zcapUt9z<=q+-^pl65cd_s*IU7Cyv@ypB_=UJb!rcEfhYS(Ey}K7%V3I6ZUTcznBo1 z#bb`3_=wn1j Date: Wed, 30 Nov 2022 16:46:21 +0100 Subject: [PATCH 22/36] Remove some Gazebo logs appearing in CARLA env --- behavior_metrics/brains/brains_handler.py | 2 +- behavior_metrics/driver.py | 4 +- behavior_metrics/driver_carla.py | 2 +- behavior_metrics/pilot_carla.py | 30 +----------- behavior_metrics/utils/environment.py | 48 +++++++++---------- behavior_metrics/utils/tmp_world_generator.py | 10 ++-- 6 files changed, 34 insertions(+), 62 deletions(-) diff --git a/behavior_metrics/brains/brains_handler.py b/behavior_metrics/brains/brains_handler.py index 461f251b..743b78a6 100755 --- a/behavior_metrics/brains/brains_handler.py +++ b/behavior_metrics/brains/brains_handler.py @@ -39,7 +39,7 @@ def load_brain(self, path, model=None): if robot_type == 'f1rl': from utils import environment - environment.close_gazebo() + environment.close_ros_and_simulators() exec(open(self.brain_path).read()) elif robot_type == 'CARLA': module = importlib.import_module(import_name) diff --git a/behavior_metrics/driver.py b/behavior_metrics/driver.py index fbe4703c..e8f47968 100644 --- a/behavior_metrics/driver.py +++ b/behavior_metrics/driver.py @@ -208,11 +208,11 @@ def main(): # When window is closed or keypress for quit is detected, quit gracefully. logger.info('closing all processes...') pilot.kill_event.set() - environment.close_gazebo() + environment.close_ros_and_simulators() else: script_manager.run_brains_worlds(app_configuration, controller, randomize=config_data['random']) logger.info('closing all processes...') - environment.close_gazebo() + environment.close_ros_and_simulators() logger.info('DONE! Bye, bye :)') diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 4060a750..9ee8779d 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -131,4 +131,4 @@ def main_win(configuration, controller): logger.info('closing all processes...') pilot.kill_event.set() -environment.close_gazebo() +environment.close_ros_and_simulators() diff --git a/behavior_metrics/pilot_carla.py b/behavior_metrics/pilot_carla.py index 44e8a3eb..b5e95be4 100644 --- a/behavior_metrics/pilot_carla.py +++ b/behavior_metrics/pilot_carla.py @@ -80,8 +80,6 @@ def __init__(self, configuration, controller, brain_path): self.checkpoint_save = False self.max_distance = 0.5 self.execution_completed = False - self.stats_thread = threading.Thread(target=self.track_stats) - self.stats_thread.start() self.ros_clock_time = 0 self.real_time_factor = 0 self.brain_iterations_real_time = [] @@ -129,7 +127,7 @@ def run(self): self.sensors.get_camera('camera_0').total_frames = 0 self.pilot_start_time = time.time() - + control_pub = rospy.Publisher('/carla/control', CarlaControl, queue_size=1) control_command = CarlaControl() control_command.command = 1 @@ -165,12 +163,6 @@ def run(self): self.real_time_factors.append(self.real_time_factor) self.brain_iterations_simulated_time.append(self.ros_clock_time - start_time_ros) self.execution_completed = True - self.clock_subscriber.unregister() - self.stats_process.terminate() - poll = self.stats_process.poll() - while poll is None: - time.sleep(1) - poll = self.stats_process.poll() self.kill() logger.info('Pilot: pilot killed.') @@ -278,23 +270,3 @@ def calculate_metrics(self, experiment_metrics): def clock_callback(self, clock_data): self.ros_clock_time = clock_data.clock.to_sec() - def track_stats(self): - args = ["gz", "stats", "-p"] - # Prints gz statistics. "-p": Output comma-separated values containing- - # real-time factor (percent), simtime (sec), realtime (sec), paused (T or F) - self.stats_process = subprocess.Popen(args, stdout=subprocess.PIPE) - # bufsize=1 enables line-bufferred mode (the input buffer is flushed - # automatically on newlines if you would write to process.stdin ) - poll = self.stats_process.poll() - while poll is not None: - time.sleep(1) - poll = self.stats_process.poll() - - self.clock_subscriber = rospy.Subscriber("/clock", Clock, self.clock_callback) - with self.stats_process.stdout: - for line in iter(self.stats_process.stdout.readline, b''): - stats_list = [x.strip() for x in line.split(b',')] - try: - self.real_time_factor = float(stats_list[0].decode("utf-8")) - except Exception as ex: - self.real_time_factor = 0 diff --git a/behavior_metrics/utils/environment.py b/behavior_metrics/utils/environment.py index 5e53518e..8fd4cb9c 100644 --- a/behavior_metrics/utils/environment.py +++ b/behavior_metrics/utils/environment.py @@ -35,63 +35,63 @@ def launch_env(launch_file): launch_file {str} -- path of the launch file to be executed """ - # close previous instances of gazebo if hanged. - close_gazebo() + # close previous instances of ROS and simulators if hanged. + close_ros_and_simulators() try: with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) - logger.info("GazeboEnv: launching gzserver.") + logger.info("SimulatorEnv: launching simulator server.") except OSError as oe: - logger.error("GazeboEnv: exception raised launching gzserver. {}".format(oe)) - close_gazebo() + logger.error("SimulatorEnv: exception raised launching simulator server. {}".format(oe)) + close_ros_and_simulators() sys.exit(-1) - # give gazebo some time to initialize + # give simulator some time to initialize time.sleep(5) -def close_gazebo(): - """Kill all the gazebo and ROS processes.""" +def close_ros_and_simulators(): + """Kill all the simulators and ROS processes.""" try: ps_output = subprocess.check_output(["ps", "-Af"]).decode('utf-8').strip("\n") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing ps command {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing ps command {}".format(ce)) sys.exit(-1) if ps_output.count('gzclient') > 0: try: subprocess.check_call(["killall", "-9", "gzclient"]) - logger.debug("GazeboEnv: gzclient killed.") + logger.debug("SimulatorEnv: gzclient killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for gzclient {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for gzclient {}".format(ce)) if ps_output.count('gzserver') > 0: try: subprocess.check_call(["killall", "-9", "gzserver"]) - logger.debug("GazeboEnv: gzserver killed.") + logger.debug("SimulatorEnv: gzserver killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for gzserver {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for gzserver {}".format(ce)) if ps_output.count('rosmaster') > 0: try: subprocess.check_call(["killall", "-9", "rosmaster"]) - logger.debug("GazeboEnv: rosmaster killed.") + logger.debug("SimulatorEnv: rosmaster killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for rosmaster {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for rosmaster {}".format(ce)) if ps_output.count('roscore') > 0: try: subprocess.check_call(["killall", "-9", "roscore"]) - logger.debug("GazeboEnv: roscore killed.") + logger.debug("SimulatorEnv: roscore killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for roscore {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for roscore {}".format(ce)) if ps_output.count('px4') > 0: try: subprocess.check_call(["killall", "-9", "px4"]) - logger.debug("GazeboEnv: px4 killed.") + logger.debug("SimulatorEnv: px4 killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for px4 {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for px4 {}".format(ce)) def is_gzclient_open(): @@ -104,7 +104,7 @@ def is_gzclient_open(): try: ps_output = subprocess.check_output(["ps", "-Af"], encoding='utf8').strip("\n") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing ps command {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing ps command {}".format(ce)) sys.exit(-1) return ps_output.count('gzclient') > 0 @@ -116,9 +116,9 @@ def close_gzclient(): if is_gzclient_open(): try: subprocess.check_call(["killall", "-9", "gzclient"]) - logger.debug("GazeboEnv: gzclient killed.") + logger.debug("SimulatorEnv: gzclient killed.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing killall command for gzclient {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for gzclient {}".format(ce)) def open_gzclient(): @@ -128,6 +128,6 @@ def open_gzclient(): try: with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: subprocess.Popen(["gzclient"], stdout=out, stderr=err) - logger.debug("GazeboEnv: gzclient started.") + logger.debug("SimulatorEnv: gzclient started.") except subprocess.CalledProcessError as ce: - logger.error("GazeboEnv: exception raised executing gzclient {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing gzclient {}".format(ce)) diff --git a/behavior_metrics/utils/tmp_world_generator.py b/behavior_metrics/utils/tmp_world_generator.py index 95b81450..7b1a82b0 100644 --- a/behavior_metrics/utils/tmp_world_generator.py +++ b/behavior_metrics/utils/tmp_world_generator.py @@ -35,7 +35,7 @@ def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, randomize=False, gui=False, launch=False): - environment.close_gazebo() + environment.close_ros_and_simulators() tree = ET.parse(current_world) root = tree.getroot() for child in root[0]: @@ -103,11 +103,11 @@ def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, try: with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: subprocess.Popen(["roslaunch", 'tmp_circuit.launch'], stdout=out, stderr=err) - logger.info("GazeboEnv: launching gzserver.") + logger.info("SimulatorEnv: launching gzserver.") except OSError as oe: - logger.error("GazeboEnv: exception raised launching gzserver. {}".format(oe)) - environment.close_gazebo() + logger.error("SimulatorEnv: exception raised launching gzserver. {}".format(oe)) + environment.close_ros_and_simulators() sys.exit(-1) - # give gazebo some time to initialize + # give simulator some time to initialize time.sleep(5) From 2b631acb37c64ef3754630c19f6f171041a75153 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 2 Dec 2022 19:00:33 +0100 Subject: [PATCH 23/36] Launch CARLA server and client directly --- behavior_metrics/driver_carla.py | 3 +-- behavior_metrics/utils/environment.py | 22 +++++++++++++++++++++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 9ee8779d..c6a012a2 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -113,8 +113,7 @@ def main_win(configuration, controller): config_data = check_args(sys.argv) app_configuration = Config(config_data['config'][0]) - -environment.launch_env(app_configuration.current_world) +environment.launch_env(app_configuration.current_world, carla_simulator=True) controller = CARLAController() diff --git a/behavior_metrics/utils/environment.py b/behavior_metrics/utils/environment.py index 8fd4cb9c..cdc49aa0 100644 --- a/behavior_metrics/utils/environment.py +++ b/behavior_metrics/utils/environment.py @@ -28,7 +28,7 @@ __license__ = 'GPLv3' -def launch_env(launch_file): +def launch_env(launch_file, carla_simulator=False): """Launch the environmet specified by the launch_file given in command line at launch time. Arguments: @@ -38,6 +38,11 @@ def launch_env(launch_file): # close previous instances of ROS and simulators if hanged. close_ros_and_simulators() try: + if carla_simulator: + with open("/tmp/.carlalaunch_stdout.log", "w") as out, open("/tmp/.carlalaunch_stderr.log", "w") as err: + subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen", "-quality-level=Low"], stdout=out, stderr=err) + logger.info("SimulatorEnv: launching simulator server.") + time.sleep(5) with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) logger.info("SimulatorEnv: launching simulator server.") @@ -92,6 +97,21 @@ def close_ros_and_simulators(): logger.debug("SimulatorEnv: px4 killed.") except subprocess.CalledProcessError as ce: logger.error("SimulatorEnv: exception raised executing killall command for px4 {}".format(ce)) + + if ps_output.count('CarlaUE4.sh') > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4.sh"]) + logger.debug("SimulatorEnv: CARLA SERVER killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for roscore {}".format(ce)) + + if ps_output.count('CarlaUE4-Linux-Shipping') > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-Shipping"]) + logger.debug("SimulatorEnv: CARLA SERVER killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for roscore {}".format(ce)) + def is_gzclient_open(): From d49f9d6f03741dd0be4a0f83787e5260f40ab3fc Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 5 Dec 2022 12:26:31 +0100 Subject: [PATCH 24/36] Launch files for different CARLA towns --- .../brain_carla_bird_eye_deep_learning.py | 3 +- .../{test_town_01.launch => town_01.launch} | 0 .../configs/CARLA_launch_files/town_02.launch | 48 ++++++++++++++++++ .../configs/CARLA_launch_files/town_04.launch | 49 +++++++++++++++++++ .../configs/CARLA_launch_files/town_05.launch | 49 +++++++++++++++++++ .../configs/CARLA_launch_files/town_06.launch | 49 +++++++++++++++++++ .../configs/CARLA_launch_files/town_07.launch | 48 ++++++++++++++++++ behavior_metrics/configs/default_carla.yml | 2 +- .../robot/interfaces/birdeyeview.py | 2 +- 9 files changed, 247 insertions(+), 3 deletions(-) rename behavior_metrics/configs/CARLA_launch_files/{test_town_01.launch => town_01.launch} (100%) create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_02.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_04.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_05.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_06.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_07.launch diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index 3607fc25..f0d8a395 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -54,7 +54,8 @@ def __init__(self, sensors, actuators, handler, config=None): client = carla.Client('localhost', 2000) client.set_timeout(10.0) # seconds world = client.get_world() - time.sleep(3) + time.sleep(10) + print(world.get_actors()) self.vehicle = world.get_actors().filter('vehicle.*')[0] model = '/home/jderobot/Documents/Projects/BehaviorMetrics/PlayingWithCARLA/models/20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5' diff --git a/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch b/behavior_metrics/configs/CARLA_launch_files/town_01.launch similarity index 100% rename from behavior_metrics/configs/CARLA_launch_files/test_town_01.launch rename to behavior_metrics/configs/CARLA_launch_files/town_01.launch diff --git a/behavior_metrics/configs/CARLA_launch_files/town_02.launch b/behavior_metrics/configs/CARLA_launch_files/town_02.launch new file mode 100644 index 00000000..d7cabeab --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_02.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_04.launch b/behavior_metrics/configs/CARLA_launch_files/town_04.launch new file mode 100644 index 00000000..a2c7cc87 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_04.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_05.launch b/behavior_metrics/configs/CARLA_launch_files/town_05.launch new file mode 100644 index 00000000..f29b5fd6 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_05.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_06.launch b/behavior_metrics/configs/CARLA_launch_files/town_06.launch new file mode 100644 index 00000000..0191f223 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_06.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_07.launch b/behavior_metrics/configs/CARLA_launch_files/town_07.launch new file mode 100644 index 00000000..01a09cf4 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_07.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index edde0135..61e34afa 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -36,7 +36,7 @@ Behaviors: ImageTranform: '' Type: 'CARLA' Simulation: - World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/test_town_01.launch + World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_07.launch Dataset: In: '/tmp/my_bag.bag' Out: '' diff --git a/behavior_metrics/robot/interfaces/birdeyeview.py b/behavior_metrics/robot/interfaces/birdeyeview.py index 9112e26b..88ab9cb7 100644 --- a/behavior_metrics/robot/interfaces/birdeyeview.py +++ b/behavior_metrics/robot/interfaces/birdeyeview.py @@ -5,7 +5,7 @@ class BirdEyeView: def __init__(self): client = carla.Client('localhost', 2000) - client.set_timeout(5.0) + client.set_timeout(10.0) self.birdview_producer = BirdViewProducer( client, # carla.Client target_size=PixelDimensions(width=100, height=300), From 056ee2738577815594f4d68b0ba9f6de9b577326 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 5 Dec 2022 13:32:46 +0100 Subject: [PATCH 25/36] Added config for Town03 launch --- .../brain_carla_bird_eye_deep_learning.py | 3 -- .../configs/CARLA_launch_files/town_03.launch | 48 +++++++++++++++++++ behavior_metrics/configs/default_carla.yml | 2 +- behavior_metrics/utils/environment.py | 3 +- 4 files changed, 51 insertions(+), 5 deletions(-) create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_03.launch diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index f0d8a395..d2fb6f17 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -7,9 +7,6 @@ import threading import time import carla -from albumentations import ( - Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare -) from utils.constants import DATASETS_DIR, ROOT_PATH import tensorflow as tf diff --git a/behavior_metrics/configs/CARLA_launch_files/town_03.launch b/behavior_metrics/configs/CARLA_launch_files/town_03.launch new file mode 100644 index 00000000..8e98014b --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_03.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/default_carla.yml b/behavior_metrics/configs/default_carla.yml index 61e34afa..928d55be 100644 --- a/behavior_metrics/configs/default_carla.yml +++ b/behavior_metrics/configs/default_carla.yml @@ -36,7 +36,7 @@ Behaviors: ImageTranform: '' Type: 'CARLA' Simulation: - World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_07.launch + World: /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_03.launch Dataset: In: '/tmp/my_bag.bag' Out: '' diff --git a/behavior_metrics/utils/environment.py b/behavior_metrics/utils/environment.py index cdc49aa0..16a1a049 100644 --- a/behavior_metrics/utils/environment.py +++ b/behavior_metrics/utils/environment.py @@ -40,7 +40,8 @@ def launch_env(launch_file, carla_simulator=False): try: if carla_simulator: with open("/tmp/.carlalaunch_stdout.log", "w") as out, open("/tmp/.carlalaunch_stderr.log", "w") as err: - subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen", "-quality-level=Low"], stdout=out, stderr=err) + subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen"], stdout=out, stderr=err) + #subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen", "-quality-level=Low"], stdout=out, stderr=err) logger.info("SimulatorEnv: launching simulator server.") time.sleep(5) with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: From 92cf3847ed0dd53ea58d9fdeee1e5f2be0bf5f8d Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 5 Dec 2022 14:06:14 +0100 Subject: [PATCH 26/36] Added map launchs without gui for headless launch --- .../CARLA_launch_files/town_01_no_gui.launch | 45 +++++++++++++++++++ .../CARLA_launch_files/town_02_no_gui.launch | 44 ++++++++++++++++++ .../CARLA_launch_files/town_03_no_gui.launch | 44 ++++++++++++++++++ .../CARLA_launch_files/town_04_no_gui.launch | 45 +++++++++++++++++++ .../CARLA_launch_files/town_05_no_gui.launch | 45 +++++++++++++++++++ .../CARLA_launch_files/town_06_no_gui.launch | 45 +++++++++++++++++++ .../CARLA_launch_files/town_07_no_gui.launch | 44 ++++++++++++++++++ 7 files changed, 312 insertions(+) create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch create mode 100644 behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch diff --git a/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch new file mode 100644 index 00000000..04d4cc90 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch new file mode 100644 index 00000000..46ccb37c --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_02_no_gui.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch new file mode 100644 index 00000000..783a0da6 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_03_no_gui.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch new file mode 100644 index 00000000..dd063c87 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_04_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch new file mode 100644 index 00000000..44911f9f --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_05_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch new file mode 100644 index 00000000..3338db1c --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_06_no_gui.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch b/behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch new file mode 100644 index 00000000..4e4ac5e0 --- /dev/null +++ b/behavior_metrics/configs/CARLA_launch_files/town_07_no_gui.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 497121891f6ef76d36b50d59feb55686235b1aa8 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 5 Dec 2022 16:51:01 +0100 Subject: [PATCH 27/36] First approach for multiple and headless CARLA experiments --- .../brain_carla_bird_eye_deep_learning.py | 6 +- .../configs/default_carla_multiple.yml | 70 ++++++++++++++++++ behavior_metrics/driver_carla.py | 71 +++++++++++++------ 3 files changed, 123 insertions(+), 24 deletions(-) create mode 100644 behavior_metrics/configs/default_carla_multiple.yml diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index d2fb6f17..f8829fe7 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -7,6 +7,9 @@ import threading import time import carla +from albumentations import ( + Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare +) from utils.constants import DATASETS_DIR, ROOT_PATH import tensorflow as tf @@ -52,7 +55,7 @@ def __init__(self, sensors, actuators, handler, config=None): client.set_timeout(10.0) # seconds world = client.get_world() time.sleep(10) - print(world.get_actors()) + #print(world.get_actors()) self.vehicle = world.get_actors().filter('vehicle.*')[0] model = '/home/jderobot/Documents/Projects/BehaviorMetrics/PlayingWithCARLA/models/20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5' @@ -86,6 +89,7 @@ def update_pose(self, pose_data): self.handler.update_pose3d(pose_data) def execute(self): + #print(self.vehicle.get_location()) image = self.camera.getImage().data image_1 = self.camera_1.getImage().data image_2 = self.camera_2.getImage().data diff --git a/behavior_metrics/configs/default_carla_multiple.yml b/behavior_metrics/configs/default_carla_multiple.yml new file mode 100644 index 00000000..3b804281 --- /dev/null +++ b/behavior_metrics/configs/default_carla_multiple.yml @@ -0,0 +1,70 @@ +Behaviors: + Robot: + Sensors: + Cameras: + Camera_0: + Name: 'camera_0' + Topic: '/carla/ego_vehicle/rgb_front/image' + Camera_1: + Name: 'camera_1' + Topic: '/carla/ego_vehicle/rgb_view/image' + Camera_2: + Name: 'camera_2' + Topic: '/carla/ego_vehicle/semantic_segmentation_front/image' + Camera_3: + Name: 'camera_3' + Topic: '/carla/ego_vehicle/dvs_front/image' + Pose3D: + Pose3D_0: + Name: 'pose3d_0' + Topic: '/carla/ego_vehicle/odometry' + BirdEyeView: + BirdEyeView_0: + Name: 'bird_eye_view_0' + Topic: '' + Actuators: + CARLA_Motors: + Motors_0: + Name: 'motors_0' + Topic: '/carla/ego_vehicle/vehicle_control_cmd' + MaxV: 3 + MaxW: 0.3 + + BrainPath: ['brains/CARLA/brain_carla_bird_eye_deep_learning.py', 'brains/CARLA/brain_carla_bird_eye_deep_learning.py'] + PilotTimeCycle: 50 + Parameters: + ImageTranform: '' + Type: 'CARLA' + Experiment: + Name: "Experiment name" + Description: "Experiment description" + Timeout: [10, 10] # for each world! + Repetitions: 1 + Simulation: + World: [ + /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch, + /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch + ] + Dataset: + In: '/tmp/my_bag.bag' + Out: '' + Stats: + Out: './' + PerfectLap: './perfect_bags/lap-simple-circuit.bag' + Layout: + Frame_0: + Name: frame_0 + Geometry: [1, 1, 1, 1] + Data: rgbimage + Frame_1: + Name: frame_1 + Geometry: [0, 1, 1, 1] + Data: rgbimage + Frame_2: + Name: frame_2 + Geometry: [0, 2, 1, 1] + Data: rgbimage + Frame_3: + Name: frame_3 + Geometry: [1, 2, 1, 1] + Data: rgbimage diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index c6a012a2..83026032 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -108,26 +108,51 @@ def main_win(configuration, controller): logger.error(e) - - - -config_data = check_args(sys.argv) -app_configuration = Config(config_data['config'][0]) -environment.launch_env(app_configuration.current_world, carla_simulator=True) - - -controller = CARLAController() - - -# Launch control -pilot = PilotCarla(app_configuration, controller, app_configuration.brain_path) -pilot.daemon = True -pilot.start() -logger.info('Executing app') - - -main_win(app_configuration, controller) - -logger.info('closing all processes...') -pilot.kill_event.set() -environment.close_ros_and_simulators() +def main(): + """Main function for the app. Handles creation and destruction of every element of the application.""" + + config_data = check_args(sys.argv) + app_configuration = Config(config_data['config'][0]) + + + if not config_data['script']: + environment.launch_env(app_configuration.current_world, carla_simulator=True) + controller = CARLAController() + + # Launch control + pilot = PilotCarla(app_configuration, controller, app_configuration.brain_path) + pilot.daemon = True + pilot.start() + logger.info('Executing app') + main_win(app_configuration, controller) + else: + for world_counter, world in enumerate(app_configuration.current_world): + for brain_counter, brain in enumerate(app_configuration.brain_path): + for repetition in range(app_configuration.experiment_repetitions): + environment.launch_env(world, carla_simulator=True) + controller = CARLAController() + + # Launch control + pilot = PilotCarla(app_configuration, controller, brain) + pilot.daemon = True + pilot.start() + logger.info('Executing app') + controller.reload_brain(brain) + controller.resume_pilot() + controller.unpause_carla_simulation() + + import time + time.sleep(app_configuration.experiment_timeouts[world_counter]) + logger.info('closing all processes...') + pilot.kill_event.set() + environment.close_ros_and_simulators() + time.sleep(5) + + + logger.info('closing all processes...') + pilot.kill_event.set() + environment.close_ros_and_simulators() + +if __name__ == '__main__': + main() + sys.exit(0) From f13183afa9a45fa52e219fdde96fafc6fc3c51c9 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 5 Dec 2022 19:19:07 +0100 Subject: [PATCH 28/36] CARLA multiple experiments without GUI --- behavior_metrics/configs/default_carla_multiple.yml | 2 +- behavior_metrics/driver_carla.py | 13 ++++++++++++- behavior_metrics/ui/gui/views/toolbar.py | 5 ++++- behavior_metrics/utils/CARLAController.py | 9 ++++++--- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/behavior_metrics/configs/default_carla_multiple.yml b/behavior_metrics/configs/default_carla_multiple.yml index 3b804281..27564420 100644 --- a/behavior_metrics/configs/default_carla_multiple.yml +++ b/behavior_metrics/configs/default_carla_multiple.yml @@ -39,7 +39,7 @@ Behaviors: Name: "Experiment name" Description: "Experiment description" Timeout: [10, 10] # for each world! - Repetitions: 1 + Repetitions: 2 Simulation: World: [ /home/jderobot/Documents/Projects/BehaviorMetrics/behavior_metrics/configs/CARLA_launch_files/town_01_no_gui.launch, diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 83026032..f2ad91fb 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -128,7 +128,8 @@ def main(): else: for world_counter, world in enumerate(app_configuration.current_world): for brain_counter, brain in enumerate(app_configuration.brain_path): - for repetition in range(app_configuration.experiment_repetitions): + for repetition_counter in range(app_configuration.experiment_repetitions): + print('******************************************************************************************************************>>>>>>>>>>>>>>>>>>>>>>>>> ' + str(world_counter) + ' ' + str(brain_counter) + ' ' + str(repetition_counter)) environment.launch_env(world, carla_simulator=True) controller = CARLAController() @@ -140,13 +141,23 @@ def main(): controller.reload_brain(brain) controller.resume_pilot() controller.unpause_carla_simulation() + controller.record_metrics(app_configuration.stats_out, world_counter=world_counter, brain_counter=brain_counter, repetition_counter=repetition_counter) import time time.sleep(app_configuration.experiment_timeouts[world_counter]) + + controller.stop_recording_metrics() + + + controller.stop_pilot() + controller.pause_carla_simulation() + logger.info('closing all processes...') pilot.kill_event.set() environment.close_ros_and_simulators() time.sleep(5) + while not controller.pilot.execution_completed: + time.sleep(1) logger.info('closing all processes...') diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py index f85aa434..fe0af468 100644 --- a/behavior_metrics/ui/gui/views/toolbar.py +++ b/behavior_metrics/ui/gui/views/toolbar.py @@ -596,7 +596,10 @@ def start_recording_stats(self): self.recording_stats_animation_label.start_animation() self.recording_stats_label.show() self.recording_stats_animation_label.show() - self.controller.record_metrics(filename, dirname) + if type(self.controller) == CARLAController.CARLAController: + self.controller.record_metrics(dirname) + else: + self.controller.record_metrics(filename, dirname) else: self.stats_hint_label.setText('Select a directory to save stats first!') self.stats_hint_label.show() diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py index 541daf64..99715716 100644 --- a/behavior_metrics/utils/CARLAController.py +++ b/behavior_metrics/utils/CARLAController.py @@ -216,10 +216,13 @@ def initialize_robot(self): self.pilot.initialize_robot() - def record_metrics(self, perfect_lap_filename, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None): + def record_metrics(self, metrics_record_dir_path, world_counter=None, brain_counter=None, repetition_counter=None): logger.info("Recording metrics bag at: {}".format(metrics_record_dir_path)) - self.start_time = datetime.now() - current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world) + self.start_time = datetime.now() + if world_counter is not None: + current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world[world_counter]) + else: + current_world_head, current_world_tail = os.path.split(self.pilot.configuration.current_world) if brain_counter is not None: current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path[brain_counter]) else: From db60cc62de3cdb5a9b01b1d64b9c2b31f9515464 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 9 Dec 2022 19:05:15 +0100 Subject: [PATCH 29/36] [WIP] Launch multifiles for CARLA as separate python processes --- .../brain_carla_bird_eye_deep_learning.py | 2 + behavior_metrics/driver_carla.py | 46 ++---- behavior_metrics/pilot_carla.py | 5 + behavior_metrics/script_manager_carla.py | 145 ++++++++++++++++++ behavior_metrics/utils/CARLAController.py | 4 +- behavior_metrics/utils/environment.py | 56 +++++-- 6 files changed, 209 insertions(+), 49 deletions(-) create mode 100644 behavior_metrics/script_manager_carla.py diff --git a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py index f8829fe7..1d75f9eb 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py +++ b/behavior_metrics/brains/CARLA/brain_carla_bird_eye_deep_learning.py @@ -37,9 +37,11 @@ def __init__(self, sensors, actuators, handler, config=None): self.threshold_image = np.zeros((640, 360, 3), np.uint8) self.color_image = np.zeros((640, 360, 3), np.uint8) + ''' self.lock = threading.Lock() self.threshold_image_lock = threading.Lock() self.color_image_lock = threading.Lock() + ''' self.cont = 0 self.iteration = 0 diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index f2ad91fb..65889113 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -2,6 +2,8 @@ import os import sys import threading +import time +import rospy from pilot_carla import PilotCarla from ui.tui.main_view import TUI @@ -107,14 +109,11 @@ def main_win(configuration, controller): except Exception as e: logger.error(e) - def main(): """Main function for the app. Handles creation and destruction of every element of the application.""" config_data = check_args(sys.argv) app_configuration = Config(config_data['config'][0]) - - if not config_data['script']: environment.launch_env(app_configuration.current_world, carla_simulator=True) controller = CARLAController() @@ -125,44 +124,19 @@ def main(): pilot.start() logger.info('Executing app') main_win(app_configuration, controller) + logger.info('closing all processes...') + pilot.kill_event.set() + environment.close_ros_and_simulators() else: for world_counter, world in enumerate(app_configuration.current_world): for brain_counter, brain in enumerate(app_configuration.brain_path): for repetition_counter in range(app_configuration.experiment_repetitions): - print('******************************************************************************************************************>>>>>>>>>>>>>>>>>>>>>>>>> ' + str(world_counter) + ' ' + str(brain_counter) + ' ' + str(repetition_counter)) - environment.launch_env(world, carla_simulator=True) - controller = CARLAController() - - # Launch control - pilot = PilotCarla(app_configuration, controller, brain) - pilot.daemon = True - pilot.start() - logger.info('Executing app') - controller.reload_brain(brain) - controller.resume_pilot() - controller.unpause_carla_simulation() - controller.record_metrics(app_configuration.stats_out, world_counter=world_counter, brain_counter=brain_counter, repetition_counter=repetition_counter) - - import time - time.sleep(app_configuration.experiment_timeouts[world_counter]) - - controller.stop_recording_metrics() - + print("python3 script_driver_carla.py -c configs/default_carla_multiple.yml -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter)) + os.system("python3 script_manager_carla.py -c configs/default_carla_multiple.yml -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter)) + print("thread finished...exiting") + # TODO Check when processes are finished instead of sleep. + time.sleep(30) - controller.stop_pilot() - controller.pause_carla_simulation() - - logger.info('closing all processes...') - pilot.kill_event.set() - environment.close_ros_and_simulators() - time.sleep(5) - while not controller.pilot.execution_completed: - time.sleep(1) - - - logger.info('closing all processes...') - pilot.kill_event.set() - environment.close_ros_and_simulators() if __name__ == '__main__': main() diff --git a/behavior_metrics/pilot_carla.py b/behavior_metrics/pilot_carla.py index b5e95be4..94df00b9 100644 --- a/behavior_metrics/pilot_carla.py +++ b/behavior_metrics/pilot_carla.py @@ -80,6 +80,8 @@ def __init__(self, configuration, controller, brain_path): self.checkpoint_save = False self.max_distance = 0.5 self.execution_completed = False + self.stats_thread = threading.Thread(target=self.track_stats) + self.stats_thread.start() self.ros_clock_time = 0 self.real_time_factor = 0 self.brain_iterations_real_time = [] @@ -268,5 +270,8 @@ def calculate_metrics(self, experiment_metrics): return experiment_metrics, first_image def clock_callback(self, clock_data): + #(clock_data.clock.to_sec()) self.ros_clock_time = clock_data.clock.to_sec() + def track_stats(self): + self.clock_subscriber = rospy.Subscriber("/clock", Clock, self.clock_callback) diff --git a/behavior_metrics/script_manager_carla.py b/behavior_metrics/script_manager_carla.py new file mode 100644 index 00000000..46366fb1 --- /dev/null +++ b/behavior_metrics/script_manager_carla.py @@ -0,0 +1,145 @@ +import argparse +import os +import sys +import threading +import time +import rospy + +from pilot_carla import PilotCarla +from ui.tui.main_view import TUI +from utils import environment, script_manager +from utils.colors import Colors +from utils.configuration import Config +from utils.controller import Controller +from utils.CARLAController import CARLAController +from utils.logger import logger +from utils.tmp_world_generator import tmp_world_generator + +def check_args(argv): + parser = argparse.ArgumentParser(description='Neural Behaviors Suite', + epilog='Enjoy the program! :)') + + parser.add_argument('-c', + '--config', + type=str, + action='append', + required=True, + help='{}Path to the configuration file in YML format.{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-g', + '--gui', + action='store_true', + help='{}Load the GUI (Graphic User Interface). Requires PyQt5 installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-t', + '--tui', + action='store_true', + help='{}Load the TUI (Terminal User Interface). Requires npyscreen installed{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + group.add_argument('-s', + '--script', + action='store_true', + help='{}Run Behavior Metrics as script{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-r', + '--random', + action='store_true', + help='{}Run Behavior Metrics F1 with random spawning{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-world_counter', + type=str, + action='append', + help='{}World counter{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-brain_counter', + type=str, + action='append', + help='{}Brain counter{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + parser.add_argument('-repetition_counter', + type=str, + action='append', + help='{}Repetition counter{}'.format( + Colors.OKBLUE, Colors.ENDC)) + + args = parser.parse_args() + + config_data = {'config': None, 'gui': None, 'tui': None, 'script': None, 'random': False, 'world_counter': 0, 'brain_counter': 0, 'repetition_counter': 0} + if args.config: + config_data['config'] = [] + for config_file in args.config: + if not os.path.isfile(config_file): + parser.error('{}No such file {} {}'.format(Colors.FAIL, config_file, Colors.ENDC)) + + config_data['config'] = args.config + + if args.gui: + config_data['gui'] = args.gui + + if args.tui: + config_data['tui'] = args.tui + + if args.script: + config_data['script'] = args.script + + if args.random: + config_data['random'] = args.random + + if args.world_counter: + config_data['world_counter'] = args.world_counter + + if args.brain_counter: + config_data['brain_counter'] = args.brain_counter + + if args.repetition_counter: + config_data['repetition_counter'] = args.repetition_counter + + return config_data + +def main(): + config_data = check_args(sys.argv) + app_configuration = Config(config_data['config'][0]) + + world_counter = 0 + brain_counter = 0 + repetition_counter = 0 + + world = app_configuration.current_world[world_counter] + brain = app_configuration.brain_path[brain_counter] + environment.launch_env(world, carla_simulator=True) + controller = CARLAController() + + # Launch control + pilot = PilotCarla(app_configuration, controller, brain) + pilot.daemon = True + pilot.start() + logger.info('Executing app') + controller.reload_brain(brain) + controller.resume_pilot() + controller.unpause_carla_simulation() + controller.record_metrics(app_configuration.stats_out, world_counter=world_counter, brain_counter=brain_counter, repetition_counter=repetition_counter) + + rospy.sleep(app_configuration.experiment_timeouts[world_counter]) + controller.stop_recording_metrics() + controller.pilot.stop() + controller.stop_pilot() + controller.pause_carla_simulation() + + logger.info('closing all processes...') + controller.pilot.kill() + environment.close_ros_and_simulators() + while not controller.pilot.execution_completed: + time.sleep(1) + + +if __name__ == '__main__': + main() + sys.exit(0) \ No newline at end of file diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/CARLAController.py index 99715716..321ad457 100644 --- a/behavior_metrics/utils/CARLAController.py +++ b/behavior_metrics/utils/CARLAController.py @@ -63,8 +63,8 @@ def __init__(self): self.pose3D_data = None self.recording = False self.cvbridge = CvBridge() - self.collision_sub = rospy.Subscriber('/carla/ego_vehicle/collision', CarlaCollisionEvent, self.__collision_callback) - self.collision_sub = rospy.Subscriber('/carla/ego_vehicle/lane_invasion', CarlaLaneInvasionEvent, self.__lane_invasion_callback) + #self.collision_sub = rospy.Subscriber('/carla/ego_vehicle/collision', CarlaCollisionEvent, self.__collision_callback) + #self.lane_invasion_sub = rospy.Subscriber('/carla/ego_vehicle/lane_invasion', CarlaLaneInvasionEvent, self.__lane_invasion_callback) def __collision_callback(self, data): diff --git a/behavior_metrics/utils/environment.py b/behavior_metrics/utils/environment.py index 16a1a049..ecb78fd9 100644 --- a/behavior_metrics/utils/environment.py +++ b/behavior_metrics/utils/environment.py @@ -18,6 +18,7 @@ import subprocess import sys import time +import os from utils.logger import logger @@ -45,7 +46,7 @@ def launch_env(launch_file, carla_simulator=False): logger.info("SimulatorEnv: launching simulator server.") time.sleep(5) with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: - subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) + child = subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) logger.info("SimulatorEnv: launching simulator server.") except OSError as oe: logger.error("SimulatorEnv: exception raised launching simulator server. {}".format(oe)) @@ -78,6 +79,40 @@ def close_ros_and_simulators(): except subprocess.CalledProcessError as ce: logger.error("SimulatorEnv: exception raised executing killall command for gzserver {}".format(ce)) + if ps_output.count('CarlaUE4.sh') > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4.sh"]) + logger.debug("SimulatorEnv: CARLA server killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for CARLA server {}".format(ce)) + + if ps_output.count('CarlaUE4-Linux-Shipping') > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-Shipping"]) + logger.debug("SimulatorEnv: CarlaUE4-Linux-Shipping killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for CarlaUE4-Linux-Shipping {}".format(ce)) + + if ps_output.count('rosout') > 0: + try: + import rosnode + for node in rosnode.get_node_names(): + if node != '/carla_ros_bridge': + subprocess.check_call(["rosnode", "kill", node]) + + logger.debug("SimulatorEnv:rosout killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for rosout {}".format(ce)) + + if ps_output.count('bridge.py') > 0: + try: + os.system("ps -ef | grep 'bridge.py' | awk '{print $2}' | xargs kill -9") + logger.debug("SimulatorEnv:bridge.py killed.") + except subprocess.CalledProcessError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for bridge.py {}".format(ce)) + except FileNotFoundError as ce: + logger.error("SimulatorEnv: exception raised executing killall command for bridge.py {}".format(ce)) + if ps_output.count('rosmaster') > 0: try: subprocess.check_call(["killall", "-9", "rosmaster"]) @@ -98,21 +133,20 @@ def close_ros_and_simulators(): logger.debug("SimulatorEnv: px4 killed.") except subprocess.CalledProcessError as ce: logger.error("SimulatorEnv: exception raised executing killall command for px4 {}".format(ce)) - - if ps_output.count('CarlaUE4.sh') > 0: + + if ps_output.count('roslaunch') > 0: try: - subprocess.check_call(["killall", "-9", "CarlaUE4.sh"]) - logger.debug("SimulatorEnv: CARLA SERVER killed.") + subprocess.check_call(["killall", "-9", "roslaunch"]) + logger.debug("SimulatorEnv: roslaunch killed.") except subprocess.CalledProcessError as ce: - logger.error("SimulatorEnv: exception raised executing killall command for roscore {}".format(ce)) + logger.error("SimulatorEnv: exception raised executing killall command for roslaunch {}".format(ce)) - if ps_output.count('CarlaUE4-Linux-Shipping') > 0: + if ps_output.count('rosout') > 0: try: - subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-Shipping"]) - logger.debug("SimulatorEnv: CARLA SERVER killed.") + subprocess.check_call(["killall", "-9", "rosout"]) + logger.debug("SimulatorEnv:rosout killed.") except subprocess.CalledProcessError as ce: - logger.error("SimulatorEnv: exception raised executing killall command for roscore {}".format(ce)) - + logger.error("SimulatorEnv: exception raised executing killall command for rosout {}".format(ce)) def is_gzclient_open(): From 40038c3617edffe15ee6440fc97b07bc95df0b5e Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 9 Dec 2022 19:47:44 +0100 Subject: [PATCH 30/36] Launch CARLA multiple times without GUI --- behavior_metrics/script_manager_carla.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/behavior_metrics/script_manager_carla.py b/behavior_metrics/script_manager_carla.py index 46366fb1..2c56cc4e 100644 --- a/behavior_metrics/script_manager_carla.py +++ b/behavior_metrics/script_manager_carla.py @@ -108,9 +108,11 @@ def main(): config_data = check_args(sys.argv) app_configuration = Config(config_data['config'][0]) - world_counter = 0 - brain_counter = 0 - repetition_counter = 0 + world_counter = int(config_data['world_counter'][0]) + brain_counter = int(config_data['brain_counter'][0]) + repetition_counter = int(config_data['repetition_counter'][0]) + + print(world_counter, brain_counter, repetition_counter) world = app_configuration.current_world[world_counter] brain = app_configuration.brain_path[brain_counter] From 5145f4765f3fae4dc481f785026f8a53c77238e9 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 12 Dec 2022 11:49:01 +0100 Subject: [PATCH 31/36] Renamed classes and files for Gazebo and Carla --- behavior_metrics/driver_carla.py | 9 +++++---- behavior_metrics/{driver.py => driver_gazebo.py} | 16 ++++++++-------- behavior_metrics/{pilot.py => pilot_gazebo.py} | 2 +- behavior_metrics/script_manager_carla.py | 7 +++---- behavior_metrics/show_plots.py | 4 ++-- behavior_metrics/ui/gui/views/toolbar.py | 16 ++++++++-------- .../{CARLAController.py => controller_carla.py} | 6 +++--- .../{controller.py => controller_gazebo.py} | 8 ++++---- .../utils/{CARLAmetrics.py => metrics_carla.py} | 0 .../utils/{metrics.py => metrics_gazebo.py} | 0 ...cript_manager.py => script_manager_gazebo.py} | 10 +++++----- behavior_metrics/utils/tmp_world_generator.py | 4 ++-- 12 files changed, 41 insertions(+), 41 deletions(-) rename behavior_metrics/{driver.py => driver_gazebo.py} (90%) rename behavior_metrics/{pilot.py => pilot_gazebo.py} (97%) rename behavior_metrics/utils/{CARLAController.py => controller_carla.py} (98%) rename behavior_metrics/utils/{controller.py => controller_gazebo.py} (97%) rename behavior_metrics/utils/{CARLAmetrics.py => metrics_carla.py} (100%) rename behavior_metrics/utils/{metrics.py => metrics_gazebo.py} (100%) rename behavior_metrics/utils/{script_manager.py => script_manager_gazebo.py} (95%) diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 65889113..400ec176 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -7,11 +7,10 @@ from pilot_carla import PilotCarla from ui.tui.main_view import TUI -from utils import environment, script_manager +from utils import environment from utils.colors import Colors from utils.configuration import Config -from utils.controller import Controller -from utils.CARLAController import CARLAController +from utils.controller_carla import ControllerCarla from utils.logger import logger from utils.tmp_world_generator import tmp_world_generator @@ -116,7 +115,7 @@ def main(): app_configuration = Config(config_data['config'][0]) if not config_data['script']: environment.launch_env(app_configuration.current_world, carla_simulator=True) - controller = CARLAController() + controller = ControllerCarla() # Launch control pilot = PilotCarla(app_configuration, controller, app_configuration.brain_path) @@ -136,6 +135,8 @@ def main(): print("thread finished...exiting") # TODO Check when processes are finished instead of sleep. time.sleep(30) + + logger.info('DONE! Bye, bye :)') if __name__ == '__main__': diff --git a/behavior_metrics/driver.py b/behavior_metrics/driver_gazebo.py similarity index 90% rename from behavior_metrics/driver.py rename to behavior_metrics/driver_gazebo.py index e8f47968..f5a81b38 100644 --- a/behavior_metrics/driver.py +++ b/behavior_metrics/driver_gazebo.py @@ -19,12 +19,12 @@ import sys import threading -from pilot import Pilot +from pilot_gazebo import PilotGazebo from ui.tui.main_view import TUI -from utils import environment, script_manager +from utils import environment, script_manager_gazebo from utils.colors import Colors from utils.configuration import Config -from utils.controller import Controller +from utils.controller_gazebo import ControllerGazebo from utils.logger import logger from utils.tmp_world_generator import tmp_world_generator @@ -112,7 +112,7 @@ def conf_window(configuration): configuration {Config} -- configuration instance for the application Keyword Arguments: - controller {Controller} -- controller part of the MVC of the application (default: {None}) + controller {ControllerGazebo} -- controller part of the MVC of the application (default: {None}) """ try: @@ -137,7 +137,7 @@ def main_win(configuration, controller): Arguments: configuration {Config} -- configuration instance for the application - controller {Controller} -- controller part of the MVC model of the application + controller {ControllerGazebo} -- controller part of the MVC model of the application """ try: from PyQt5.QtWidgets import QApplication @@ -165,7 +165,7 @@ def main(): app_configuration = Config(config) # Create controller of model-view - controller = Controller() + controller = ControllerGazebo() # If there's no config, configure the app through the GUI if app_configuration.empty and config_data['gui']: @@ -194,7 +194,7 @@ def main(): if not config_data['script']: # Launch control - pilot = Pilot(app_configuration, controller, app_configuration.brain_path) + pilot = PilotGazebo(app_configuration, controller, app_configuration.brain_path) pilot.daemon = True pilot.start() logger.info('Executing app') @@ -210,7 +210,7 @@ def main(): pilot.kill_event.set() environment.close_ros_and_simulators() else: - script_manager.run_brains_worlds(app_configuration, controller, randomize=config_data['random']) + script_manager_gazebo.run_brains_worlds(app_configuration, controller, randomize=config_data['random']) logger.info('closing all processes...') environment.close_ros_and_simulators() diff --git a/behavior_metrics/pilot.py b/behavior_metrics/pilot_gazebo.py similarity index 97% rename from behavior_metrics/pilot.py rename to behavior_metrics/pilot_gazebo.py index 8686b2a5..fde7cf7a 100644 --- a/behavior_metrics/pilot.py +++ b/behavior_metrics/pilot_gazebo.py @@ -32,7 +32,7 @@ __license__ = 'GPLv3' -class Pilot(threading.Thread): +class PilotGazebo(threading.Thread): """This class handles the robot and its brain. This class called Pilot that handles the initialization of the robot sensors and actuators and the diff --git a/behavior_metrics/script_manager_carla.py b/behavior_metrics/script_manager_carla.py index 2c56cc4e..7674cacb 100644 --- a/behavior_metrics/script_manager_carla.py +++ b/behavior_metrics/script_manager_carla.py @@ -7,11 +7,10 @@ from pilot_carla import PilotCarla from ui.tui.main_view import TUI -from utils import environment, script_manager +from utils import environment from utils.colors import Colors from utils.configuration import Config -from utils.controller import Controller -from utils.CARLAController import CARLAController +from utils.controller_carla import ControllerCarla from utils.logger import logger from utils.tmp_world_generator import tmp_world_generator @@ -117,7 +116,7 @@ def main(): world = app_configuration.current_world[world_counter] brain = app_configuration.brain_path[brain_counter] environment.launch_env(world, carla_simulator=True) - controller = CARLAController() + controller = ControllerCarla() # Launch control pilot = PilotCarla(app_configuration, controller, brain) diff --git a/behavior_metrics/show_plots.py b/behavior_metrics/show_plots.py index ac8c66b6..4c5662ef 100644 --- a/behavior_metrics/show_plots.py +++ b/behavior_metrics/show_plots.py @@ -7,7 +7,7 @@ from cv_bridge import CvBridge import numpy as np -from utils import metrics +from utils import metrics_gazebo from matplotlib.backends.qt_compat import QtWidgets from matplotlib.backends.backend_qt5agg import (FigureCanvas, NavigationToolbar2QT as NavigationToolbar) from PyQt5.QtWidgets import QApplication, QWidget, QLabel, QVBoxLayout, QPushButton, QGridLayout @@ -222,7 +222,7 @@ def show_metrics(bags, bags_checkpoints, bags_metadata, bags_experiment_data, fi elif bags_metadata[x]['world'] == 'montmelo_line.launch': perfect_lap_path = 'perfect_bags/lap-montmelo.bag' - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(perfect_lap_path) + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag(perfect_lap_path) experiment_metrics = bags_experiment_data[x] experiment_metrics['experiment_metrics'] = experiment_metrics experiments_metrics.append(experiment_metrics) diff --git a/behavior_metrics/ui/gui/views/toolbar.py b/behavior_metrics/ui/gui/views/toolbar.py index fe0af468..afd5328a 100644 --- a/behavior_metrics/ui/gui/views/toolbar.py +++ b/behavior_metrics/ui/gui/views/toolbar.py @@ -28,7 +28,7 @@ from ui.gui.views.stats_window import StatsWindow, CARLAStatsWindow from ui.gui.views.logo import Logo from ui.gui.views.social import SocialMedia -from utils import constants, environment, CARLAController +from utils import constants, environment, controller_carla __author__ = 'fqez' __contributors__ = [] @@ -534,14 +534,14 @@ def create_simulation_gb(self): start_pause_simulation_label.setToolTip('Start/Pause the simulation') reset_simulation = ClickableLabel('reset', 40, QPixmap(':/assets/reload.png'), parent=self) reset_simulation.setToolTip('Reset the simulation') - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: carla_image = ClickableLabel('carlacli', 40, QPixmap(':/assets/carla_light.png'), parent=self) carla_image.setToolTip('Open/Close simulator window') else: show_gzclient = ClickableLabel('gzcli', 40, QPixmap(':/assets/gazebo_light.png'), parent=self) show_gzclient.setToolTip('Open/Close simulator window') pause_reset_layout = QHBoxLayout() - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: pause_reset_layout.addWidget(carla_image, alignment=Qt.AlignRight) else: pause_reset_layout.addWidget(show_gzclient, alignment=Qt.AlignRight) @@ -596,7 +596,7 @@ def start_recording_stats(self): self.recording_stats_animation_label.start_animation() self.recording_stats_label.show() self.recording_stats_animation_label.show() - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: self.controller.record_metrics(dirname) else: self.controller.record_metrics(filename, dirname) @@ -613,7 +613,7 @@ def stop_recording_stats(self): self.recording_stats_label.hide() self.controller.stop_recording_metrics() - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: dialog = CARLAStatsWindow(self, self.controller) else: dialog = StatsWindow(self, self.controller) @@ -667,7 +667,7 @@ def select_directory_dialog(self): def reset_simulation(self): """Callback that handles simulation resetting""" - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: self.controller.reset_carla_simulation() else: self.controller.reset_gazebo_simulation() @@ -684,7 +684,7 @@ def pause_simulation(self): self.confirm_brain.setStyleSheet('color: white') self.controller.pause_pilot() - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: self.controller.pause_carla_simulation() else: self.controller.pause_gazebo_simulation() @@ -707,7 +707,7 @@ def resume_simulation(self): # save to configuration self.configuration.brain_path = brains_path + self.configuration.robot_type + '/' + brain - if type(self.controller) == CARLAController.CARLAController: + if type(self.controller) == controller_carla.ControllerCarla: self.controller.unpause_carla_simulation() else: self.controller.unpause_gazebo_simulation() diff --git a/behavior_metrics/utils/CARLAController.py b/behavior_metrics/utils/controller_carla.py similarity index 98% rename from behavior_metrics/utils/CARLAController.py rename to behavior_metrics/utils/controller_carla.py index 321ad457..98f14ffa 100644 --- a/behavior_metrics/utils/CARLAController.py +++ b/behavior_metrics/utils/controller_carla.py @@ -34,7 +34,7 @@ from utils.logger import logger from utils.constants import CIRCUITS_TIMEOUTS from std_msgs.msg import String -from utils import CARLAmetrics +from utils import metrics_carla from carla_msgs.msg import CarlaLaneInvasionEvent from carla_msgs.msg import CarlaCollisionEvent @@ -43,7 +43,7 @@ __license__ = 'GPLv3' -class CARLAController: +class ControllerCarla: """This class defines the controller of the architecture, responsible of the communication between the logic (model) and the user interface (view). @@ -268,7 +268,7 @@ def stop_recording_metrics(self): while os.path.isfile(self.experiment_metrics_filename + '.active'): pass - self.experiment_metrics = CARLAmetrics.get_metrics(self.experiment_metrics_filename) + self.experiment_metrics = metrics_carla.get_metrics(self.experiment_metrics_filename) try: self.save_metrics() diff --git a/behavior_metrics/utils/controller.py b/behavior_metrics/utils/controller_gazebo.py similarity index 97% rename from behavior_metrics/utils/controller.py rename to behavior_metrics/utils/controller_gazebo.py index 36a513db..2217df10 100644 --- a/behavior_metrics/utils/controller.py +++ b/behavior_metrics/utils/controller_gazebo.py @@ -33,14 +33,14 @@ from utils.logger import logger from utils.constants import CIRCUITS_TIMEOUTS from std_msgs.msg import String -from utils import metrics +from utils import metrics_gazebo __author__ = 'fqez' __contributors__ = [] __license__ = 'GPLv3' -class Controller: +class ControllerGazebo: """This class defines the controller of the architecture, responsible of the communication between the logic (model) and the user interface (view). @@ -224,9 +224,9 @@ def stop_recording_metrics(self, pitch_error=False): while os.path.isfile(self.experiment_metrics_filename + '.active'): pass - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(self.perfect_lap_filename) + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag(self.perfect_lap_filename) if not pitch_error: - self.experiment_metrics = metrics.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) + self.experiment_metrics = metrics_gazebo.get_metrics(self.experiment_metrics_filename, perfect_lap_checkpoints, circuit_diameter) self.experiment_metrics, first_image = self.pilot.calculate_metrics(self.experiment_metrics) try: diff --git a/behavior_metrics/utils/CARLAmetrics.py b/behavior_metrics/utils/metrics_carla.py similarity index 100% rename from behavior_metrics/utils/CARLAmetrics.py rename to behavior_metrics/utils/metrics_carla.py diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics_gazebo.py similarity index 100% rename from behavior_metrics/utils/metrics.py rename to behavior_metrics/utils/metrics_gazebo.py diff --git a/behavior_metrics/utils/script_manager.py b/behavior_metrics/utils/script_manager_gazebo.py similarity index 95% rename from behavior_metrics/utils/script_manager.py rename to behavior_metrics/utils/script_manager_gazebo.py index 322a3c94..32d0ab4a 100644 --- a/behavior_metrics/utils/script_manager.py +++ b/behavior_metrics/utils/script_manager_gazebo.py @@ -25,10 +25,10 @@ import numpy as np -from utils import metrics +from utils import metrics_gazebo from utils.logger import logger from utils.constants import MIN_EXPERIMENT_PERCENTAGE_COMPLETED, CIRCUITS_TIMEOUTS -from pilot import Pilot +from pilot_gazebo import PilotGazebo from utils.tmp_world_generator import tmp_world_generator from rosgraph_msgs.msg import Clock @@ -42,7 +42,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): tmp_world_generator(world, app_configuration.stats_perfect_lap[world_counter], app_configuration.real_time_update_rate, randomize=randomize, gui=False, launch=True) - pilot = Pilot(app_configuration, controller, app_configuration.brain_path[brain_counter]) + pilot = PilotGazebo(app_configuration, controller, app_configuration.brain_path[brain_counter]) pilot.daemon = True pilot.real_time_update_rate = app_configuration.real_time_update_rate controller.pilot.start() @@ -63,7 +63,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): world_counter=world_counter, brain_counter=brain_counter, repetition_counter=repetition_counter) - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag( + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag( app_configuration.stats_perfect_lap[world_counter]) new_point = np.array([controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().x, controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().y]) @@ -84,7 +84,7 @@ def run_brains_worlds(app_configuration, controller, randomize=False): controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().y]) if is_trapped(old_point, new_point): is_finished = True - elif metrics.is_finish_line(new_point, start_point): + elif metrics_gazebo.is_finish_line(new_point, start_point): is_finished = True elif previous_pitch != 0 and abs(controller.pilot.sensors.get_pose3d('pose3d_0').getPose3d().pitch - previous_pitch) > 0.2: diff --git a/behavior_metrics/utils/tmp_world_generator.py b/behavior_metrics/utils/tmp_world_generator.py index 7b1a82b0..e2e395ec 100644 --- a/behavior_metrics/utils/tmp_world_generator.py +++ b/behavior_metrics/utils/tmp_world_generator.py @@ -28,7 +28,7 @@ import numpy as np -from utils import metrics +from utils import metrics_gazebo from utils import environment from utils.logger import logger @@ -52,7 +52,7 @@ def tmp_world_generator(current_world, stats_perfect_lap, real_time_update_rate, tree = ET.parse(os.path.dirname(os.path.dirname(current_world)) + '/worlds/' + world_name) root = tree.getroot() - perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(stats_perfect_lap) + perfect_lap_checkpoints, circuit_diameter = metrics_gazebo.read_perfect_lap_rosbag(stats_perfect_lap) if randomize: random_index = random.randint(0, int(len(perfect_lap_checkpoints))) From 55b800f9af08279b051b2585dd7938fbdd374a36 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 12 Dec 2022 12:20:01 +0100 Subject: [PATCH 32/36] Reviewed metrics calculation for Carla --- behavior_metrics/ui/gui/views/stats_window.py | 8 +++-- behavior_metrics/utils/controller_carla.py | 2 +- behavior_metrics/utils/metrics_carla.py | 33 +++---------------- 3 files changed, 10 insertions(+), 33 deletions(-) diff --git a/behavior_metrics/ui/gui/views/stats_window.py b/behavior_metrics/ui/gui/views/stats_window.py index 16817511..82ef693a 100644 --- a/behavior_metrics/ui/gui/views/stats_window.py +++ b/behavior_metrics/ui/gui/views/stats_window.py @@ -74,12 +74,14 @@ def __init__(self, parent=None, controller=None): self.setCentralWidget(wid) self.layout = QVBoxLayout() - self.completed_distance_label = QLabel("Completed distance -> " + str(self.controller.experiment_metrics['completed_distance']) + "m") + self.completed_distance_label = QLabel("Completed distance -> " + str(self.controller.experiment_metrics['completed_distance']) + " m") self.layout.addWidget(self.completed_distance_label) - self.average_speed_label = QLabel("Average speed -> " + str(self.controller.experiment_metrics['average_speed']) + "km/h") + self.average_speed_label = QLabel("Average speed -> " + str(self.controller.experiment_metrics['average_speed']) + " km/h") self.layout.addWidget(self.average_speed_label) - self.experiment_total_real_time_label = QLabel("Experiment total real time -> " + str(self.controller.experiment_metrics['experiment_total_real_time']) + 's') + self.experiment_total_real_time_label = QLabel("Experiment total real time -> " + str(self.controller.experiment_metrics['experiment_total_real_time']) + ' s') self.layout.addWidget(self.experiment_total_real_time_label) + self.experiment_total_simulated_time_label = QLabel("Experiment total simulated time -> " + str(self.controller.experiment_metrics['experiment_total_simulated_time']) + ' s') + self.layout.addWidget(self.experiment_total_simulated_time_label) self.collisions_label = QLabel("Collisions -> " + str(self.controller.experiment_metrics['collisions'])) self.layout.addWidget(self.collisions_label) self.lane_invasions_label = QLabel("Lane invasions -> " + str(self.controller.experiment_metrics['lane_invasions'])) diff --git a/behavior_metrics/utils/controller_carla.py b/behavior_metrics/utils/controller_carla.py index 98f14ffa..68449e43 100644 --- a/behavior_metrics/utils/controller_carla.py +++ b/behavior_metrics/utils/controller_carla.py @@ -275,7 +275,7 @@ def stop_recording_metrics(self): except rosbag.bag.ROSBagException: logger.info("Bag was empty, Try Again") - logger.info("* Experiment total real time -> " + str(end_time - self.pilot.pilot_start_time)) + logger.info("* Experiment total real time -> " + str(end_time - self.pilot.pilot_start_time) + ' s') self.experiment_metrics['experiment_total_real_time'] = end_time - self.pilot.pilot_start_time time_str = time.strftime("%Y%m%d-%H%M%S") diff --git a/behavior_metrics/utils/metrics_carla.py b/behavior_metrics/utils/metrics_carla.py index 36eb3973..a813922f 100644 --- a/behavior_metrics/utils/metrics_carla.py +++ b/behavior_metrics/utils/metrics_carla.py @@ -24,31 +24,6 @@ from bagpy import bagreader from utils.logger import logger -from scipy.optimize import fmin, dual_annealing -from scipy.interpolate import CubicSpline - -MIN_COMPLETED_DISTANCE_EXPERIMENT = 10 -MIN_PERCENTAGE_COMPLETED_EXPERIMENT = 0 -MIN_EXPERIMENT_TIME = 25 -LAP_COMPLETED_PERCENTAGE = 100 - - -def is_finish_line(point, start_point): - try: - current_point = np.array([point['pose.pose.position.x'], point['pose.pose.position.y']]) - except IndexError: - current_point = point - try: - start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) - except IndexError: - start_point = start_point - dist = (start_point - current_point) ** 2 - dist = np.sum(dist, axis=0) - dist = np.sqrt(dist) - if dist <= 1.0: - return True - return False - def circuit_distance_completed(checkpoints, lap_point): previous_point = [] @@ -127,7 +102,7 @@ def get_metrics(stats_filename): experiment_metrics = get_collisions(experiment_metrics, collision_points) experiment_metrics = get_lane_invasions(experiment_metrics, lane_invasion_points) experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start - logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time'])) + logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time']) + ' s') shutil.rmtree(stats_filename.split('.bag')[0]) return experiment_metrics else: @@ -137,16 +112,16 @@ def get_metrics(stats_filename): def get_distance_completed(experiment_metrics, checkpoints): end_point = checkpoints[len(checkpoints) - 1] experiment_metrics['completed_distance'] = circuit_distance_completed(checkpoints, end_point) - logger.info('* Completed distance ---> ' + str(experiment_metrics['completed_distance'])) + logger.info('* Completed distance ---> ' + str(experiment_metrics['completed_distance']) + ' m') return experiment_metrics def get_average_speed(experiment_metrics, seconds_start, seconds_end): if (seconds_end - seconds_start): - experiment_metrics['average_speed'] = experiment_metrics['completed_distance'] / (seconds_end - seconds_start) + experiment_metrics['average_speed'] = (experiment_metrics['completed_distance'] / (seconds_end - seconds_start))* 3.6 else: experiment_metrics['average_speed'] = 0 - logger.info('* Average speed ---> ' + str(experiment_metrics['average_speed'])) + logger.info('* Average speed ---> ' + str(experiment_metrics['average_speed']) + ' km/h') return experiment_metrics def get_collisions(experiment_metrics, collision_points): From 9b6bbc52bd93c5c449f11c5cd3f2591f7b77e947 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 12 Dec 2022 13:54:27 +0100 Subject: [PATCH 33/36] Driver carla without sleep --- behavior_metrics/driver_carla.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/behavior_metrics/driver_carla.py b/behavior_metrics/driver_carla.py index 400ec176..cf473baf 100644 --- a/behavior_metrics/driver_carla.py +++ b/behavior_metrics/driver_carla.py @@ -133,8 +133,6 @@ def main(): print("python3 script_driver_carla.py -c configs/default_carla_multiple.yml -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter)) os.system("python3 script_manager_carla.py -c configs/default_carla_multiple.yml -s -world_counter " + str(world_counter) + " -brain_counter " + str(brain_counter) + " -repetition_counter " + str(repetition_counter)) print("thread finished...exiting") - # TODO Check when processes are finished instead of sleep. - time.sleep(30) logger.info('DONE! Bye, bye :)') From 39c21c28f0906fa1fb9d7c8f1b14df3d4547ecfb Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 12 Dec 2022 19:03:03 +0100 Subject: [PATCH 34/36] Requirements for CARLA support --- requirements.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/requirements.txt b/requirements.txt index 056549d8..9bec4ac8 100644 --- a/requirements.txt +++ b/requirements.txt @@ -34,3 +34,5 @@ tensorboard-plugin-wit==1.8.0 tensorflow-gpu==2.7.2 albumentations==1.0.3 protobuf==3.20 +carla_birdeye_view==1.1.1 +transforms3d==0.4.1 \ No newline at end of file From eceb5f920af4ac037bfbfdea9041164ae2314bd0 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 12 Dec 2022 19:10:01 +0100 Subject: [PATCH 35/36] Updated requirements --- requirements.txt | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/requirements.txt b/requirements.txt index c9f443b7..38f7a718 100644 --- a/requirements.txt +++ b/requirements.txt @@ -33,10 +33,6 @@ tensorboard-data-server==0.6.1 tensorboard-plugin-wit==1.8.0 tensorflow-gpu==2.7.2 albumentations==1.0.3 -<<<<<<< HEAD -protobuf==3.20 -carla_birdeye_view==1.1.1 -transforms3d==0.4.1 -======= protobuf==3.20.2 ->>>>>>> 56a852d12d5348da04c99ac79499485ac5d9c7a0 +carla_birdeye_view==1.1.1 +transforms3d==0.4.1 \ No newline at end of file From 2e1a0859749b12ca6f1c490238297dd9075b64ee Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 12 Dec 2022 19:15:01 +0100 Subject: [PATCH 36/36] Removed unneeded folder --- PlayingWithCARLA/README.md | 2 - .../carla_0_9_13/bird_eye_example.py | 55 - .../carla_0_9_13/get_vehicles_locations.py | 212 --- PlayingWithCARLA/carla_0_9_13/gradcam.py | 95 -- .../carla_0_9_13/launch_carla_0_9_13.py | 1388 ----------------- .../carla_0_9_13/launch_carla_0_9_13_car.py | 20 - .../launch_carla_0_9_13_simple.py | 1311 ---------------- .../launch_carla_0_9_13_synchronous.py | 224 --- .../launch_carla_0_9_13_synchronous_basic.py | 206 --- ...ynchronous_and_move_tensorflow_bird_eye.py | 404 ----- ...s_and_move_tensorflow_bird_eye_velocity.py | 433 ----- ...ensorflow_bird_eye_velocity_and_command.py | 395 ----- ...chronous_and_move_tensorflow_full_image.py | 315 ---- .../launch_synchronous_and_record_dataset.py | 272 ---- .../move_vehicle_tensorflow_bird_eye.py | 171 -- .../carla_0_9_13/record_dataset.py | 92 -- .../carla_0_9_13/record_dataset_bird_eye.py | 157 -- .../remove_traffic_lights_and_autopilot.py | 163 -- .../carla_0_9_13/review_images.py | 23 - .../carla_0_9_13/synchronous_mode_test.py | 212 --- PlayingWithCARLA/carla_0_9_13/test.py | 174 --- PlayingWithCARLA/carla_agent.py | 12 - PlayingWithCARLA/carla_tutorial.py | 55 - PlayingWithCARLA/gradcam.py | 95 -- PlayingWithCARLA/launch_carla.py | 653 -------- PlayingWithCARLA/manual_control_modified.py | 743 --------- PlayingWithCARLA/move_autopilot.py | 36 - PlayingWithCARLA/move_vehicle.py | 71 - PlayingWithCARLA/move_vehicle_tensorflow.py | 154 -- PlayingWithCARLA/plot_car_camera.py | 43 - PlayingWithCARLA/print_camera_image.py | 76 - PlayingWithCARLA/record_dataset.py | 69 - PlayingWithCARLA/remove_traffic_lights.py | 86 - .../remove_traffic_lights_and_autopilot.py | 109 -- .../set_autopilot_and_print_speed.py | 26 - ...utopilot_and_show_tensorflow_estimation.py | 87 -- PlayingWithCARLA/show_camera_image.py | 77 - PlayingWithCARLA/show_camera_opencv.py | 68 - PlayingWithCARLA/show_crop_camera_image.py | 76 - PlayingWithCARLA/show_dataset_image.py | 7 - PlayingWithCARLA/show_environment_objects.py | 39 - PlayingWithCARLA/show_grad_camera.py | 122 -- PlayingWithCARLA/tutorial.py | 127 -- 43 files changed, 9155 deletions(-) delete mode 100644 PlayingWithCARLA/README.md delete mode 100644 PlayingWithCARLA/carla_0_9_13/bird_eye_example.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/gradcam.py delete mode 100755 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/record_dataset.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/review_images.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py delete mode 100644 PlayingWithCARLA/carla_0_9_13/test.py delete mode 100644 PlayingWithCARLA/carla_agent.py delete mode 100644 PlayingWithCARLA/carla_tutorial.py delete mode 100644 PlayingWithCARLA/gradcam.py delete mode 100644 PlayingWithCARLA/launch_carla.py delete mode 100755 PlayingWithCARLA/manual_control_modified.py delete mode 100644 PlayingWithCARLA/move_autopilot.py delete mode 100644 PlayingWithCARLA/move_vehicle.py delete mode 100644 PlayingWithCARLA/move_vehicle_tensorflow.py delete mode 100644 PlayingWithCARLA/plot_car_camera.py delete mode 100644 PlayingWithCARLA/print_camera_image.py delete mode 100644 PlayingWithCARLA/record_dataset.py delete mode 100644 PlayingWithCARLA/remove_traffic_lights.py delete mode 100644 PlayingWithCARLA/remove_traffic_lights_and_autopilot.py delete mode 100644 PlayingWithCARLA/set_autopilot_and_print_speed.py delete mode 100644 PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py delete mode 100644 PlayingWithCARLA/show_camera_image.py delete mode 100644 PlayingWithCARLA/show_camera_opencv.py delete mode 100644 PlayingWithCARLA/show_crop_camera_image.py delete mode 100644 PlayingWithCARLA/show_dataset_image.py delete mode 100644 PlayingWithCARLA/show_environment_objects.py delete mode 100644 PlayingWithCARLA/show_grad_camera.py delete mode 100755 PlayingWithCARLA/tutorial.py diff --git a/PlayingWithCARLA/README.md b/PlayingWithCARLA/README.md deleted file mode 100644 index 319a2b92..00000000 --- a/PlayingWithCARLA/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# PlayingWithCARLA -Scripts for the initial steps with CARLA simulator diff --git a/PlayingWithCARLA/carla_0_9_13/bird_eye_example.py b/PlayingWithCARLA/carla_0_9_13/bird_eye_example.py deleted file mode 100644 index 02a5ffcf..00000000 --- a/PlayingWithCARLA/carla_0_9_13/bird_eye_example.py +++ /dev/null @@ -1,55 +0,0 @@ -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import carla - -client = carla.Client('localhost', 2000) -client.set_timeout(2.0) - -birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=150, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AND_REAR_AREA -) - -birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AND_REAR_AREA -) - -world = client.get_world() -print(world.get_actors()) -car = world.get_actors().filter('vehicle.*')[0] -# Input for your model - call it every simulation step -# returned result is np.ndarray with ones and zeros of shape (8, height, width) -birdview = birdview_producer.produce( - agent_vehicle=car # carla.Actor (spawned vehicle) -) - -import matplotlib.pyplot as plt -import cv2 -import time -import numpy as np - -while True: - # Use only if you want to visualize - # produces np.ndarray of shape (height, width, 3) - rgb = BirdViewProducer.as_rgb(birdview) - #print(rgb) - ''' - for x, im in enumerate(rgb): - for y, im_2 in enumerate(rgb[x]): - #print(rgb[x][y]) - #print(np.array([0,0,0])) - #print((rgb[x][y] == np.array([0,0,0])).all()) - if (rgb[x][y] == np.array([0,0,0])).all() != True: - print(rgb[x][y]) - - ''' - plt.imshow(rgb) - plt.show() - - print(rgb.shape) - - #time.sleep(1) diff --git a/PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py b/PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py deleted file mode 100644 index a0631496..00000000 --- a/PlayingWithCARLA/carla_0_9_13/get_vehicles_locations.py +++ /dev/null @@ -1,212 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -print(carla.__file__) - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) -time.sleep(2) - -print(world.get_actors().filter('vehicle.*')) -print(world.get_actors().filter('vehicle.*')[0].get_location()) - - - -#from robot.interfaces.pose3d import ListenerPose3d - - -import rospy -from nav_msgs.msg import Odometry -import threading -from math import asin, atan2, pi - - -def quat2Yaw(qw, qx, qy, qz): - ''' - Translates from Quaternion to Yaw. - - @param qw,qx,qy,qz: Quaternion values - - @type qw,qx,qy,qz: float - - @return Yaw value translated from Quaternion - - ''' - rotateZa0 = 2.0 * (qx * qy + qw * qz) - rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz - rotateZ = 0.0 - if(rotateZa0 != 0.0 and rotateZa1 != 0.0): - rotateZ = atan2(rotateZa0, rotateZa1) - return rotateZ - - -def quat2Pitch(qw, qx, qy, qz): - ''' - Translates from Quaternion to Pitch. - - @param qw,qx,qy,qz: Quaternion values - - @type qw,qx,qy,qz: float - - @return Pitch value translated from Quaternion - - ''' - - rotateYa0 = -2.0 * (qx * qz - qw * qy) - rotateY = 0.0 - if(rotateYa0 >= 1.0): - rotateY = pi/2.0 - elif(rotateYa0 <= -1.0): - rotateY = -pi/2.0 - else: - rotateY = asin(rotateYa0) - - return rotateY - - -def quat2Roll(qw, qx, qy, qz): - ''' - Translates from Quaternion to Roll. - - @param qw,qx,qy,qz: Quaternion values - - @type qw,qx,qy,qz: float - - @return Roll value translated from Quaternion - - ''' - rotateXa0 = 2.0 * (qy * qz + qw * qx) - rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz - rotateX = 0.0 - - if(rotateXa0 != 0.0 and rotateXa1 != 0.0): - rotateX = atan2(rotateXa0, rotateXa1) - return rotateX - - -def odometry2Pose3D(odom): - ''' - Translates from ROS Odometry to JderobotTypes Pose3d. - - @param odom: ROS Odometry to translate - - @type odom: Odometry - - @return a Pose3d translated from odom - - ''' - pose = Pose3d() - ori = odom.pose.pose.orientation - - pose.x = odom.pose.pose.position.x - pose.y = odom.pose.pose.position.y - pose.z = odom.pose.pose.position.z - # pose.h = odom.pose.pose.position.h - pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z) - pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z) - pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z) - pose.q = [ori.w, ori.x, ori.y, ori.z] - pose.timeStamp = odom.header.stamp.secs + (odom.header.stamp.nsecs * 1e-9) - - return pose - - -class Pose3d (): - - def __init__(self): - - self.x = 0 # X coord [meters] - self.y = 0 # Y coord [meters] - self.z = 0 # Z coord [meters] - self.h = 1 # H param - self.yaw = 0 # Yaw angle[rads] - self.pitch = 0 # Pitch angle[rads] - self.roll = 0 # Roll angle[rads] - self.q = [0, 0, 0, 0] # Quaternion - self.timeStamp = 0 # Time stamp [s] - - def __str__(self): - s = "Pose3D: {\n x: " + str(self.x) + "\n Y: " + str(self.y) - s = s + "\n Z: " + str(self.z) + "\n H: " + str(self.h) - s = s + "\n Yaw: " + str(self.yaw) + "\n Pitch: " + str(self.pitch) + "\n Roll: " + str(self.roll) - s = s + "\n quaternion: " + str(self.q) + "\n timeStamp: " + str(self.timeStamp) + "\n}" - return s - - -class ListenerPose3d: - ''' - ROS Pose3D Subscriber. Pose3D Client to Receive pose3d from ROS nodes. - ''' - def __init__(self, topic): - ''' - ListenerPose3d Constructor. - - @param topic: ROS topic to subscribe - @type topic: String - - ''' - self.topic = topic - self.data = Pose3d() - self.sub = None - self.lock = threading.Lock() - self.start() - - def __callback(self, odom): - ''' - Callback function to receive and save Pose3d. - - @param odom: ROS Odometry received - - @type odom: Odometry - - ''' - pose = odometry2Pose3D(odom) - - self.lock.acquire() - self.data = pose - self.lock.release() - - def stop(self): - ''' - Stops (Unregisters) the client. - - ''' - self.sub.unregister() - - def start(self): - ''' - Starts (Subscribes) the client. - - ''' - print(self.topic) - print(Odometry) - print(self.__callback) - self.sub = rospy.Subscriber(self.topic, Odometry, self.__callback) - - def getPose3d(self): - ''' - Returns last Pose3d. - - @return last JdeRobotTypes Pose3d saved - - ''' - self.lock.acquire() - pose = self.data - self.lock.release() - - return pose - - -rospy.init_node('listener', anonymous=True) -sensor = ListenerPose3d('/carla/ego_vehicle/odometry') - -i=0 - -while i<100: - print(sensor.getPose3d()) - i+=1 \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/gradcam.py b/PlayingWithCARLA/carla_0_9_13/gradcam.py deleted file mode 100644 index 55dffa10..00000000 --- a/PlayingWithCARLA/carla_0_9_13/gradcam.py +++ /dev/null @@ -1,95 +0,0 @@ -from tensorflow.keras.models import Model -from tensorflow.keras.layers import Conv2D -import tensorflow as tf -import numpy as np -import cv2 - - -class GradCAM: - def __init__(self, model, class_idx, layer_name=None): - # store the model, the class index used to measure the class - # activation map, and the layer to be used when visualizing - # the class activation map - self.model = model - self.class_idx = class_idx - self.layerName = layer_name - # if the layer name is None, attempt to automatically find - # the target output layer - if self.layerName is None: - self.layerName = self.find_target_layer() - - def find_target_layer(self): - # attempt to find the final convolutional layer in the network - # by looping over the layers of the network in reverse order - for layer in reversed(self.model.layers): - # check to see if the layer has a 4D output - #if i > 6 and len(layer.output_shape) == 4: - if len(layer.output_shape) == 4 and type(layer) == Conv2D: - print(layer.name) - return layer.name - # otherwise, we could not find a 4D layer so the GradCAM - # algorithm cannot be applied - raise ValueError("Could not find 4D layer. Cannot apply GradCAM.") - - def compute_heatmap(self, image, eps=1e-8): - # construct our gradient model by supplying (1) the inputs - # to our pre-trained model, (2) the output of the (presumably) - # final 4D layer in the network, and (3) the output of the - # softmax activations from the model - grad_model = Model( - inputs=[self.model.inputs], - outputs=[self.model.get_layer(self.layerName).output, - self.model.output]) - - # record operations for automatic differentiation - with tf.GradientTape() as tape: - # cast the image tensor to a float-32 data type, pass the - # image through the gradient model, and grab the loss - # associated with the specific class index - inputs = tf.cast(image, tf.float32) - (conv_outputs, predictions) = grad_model(inputs) - #loss = predictions[:, self.class_idx] - loss = predictions[:, 1] - # use automatic differentiation to compute the gradients - grads = tape.gradient(loss, conv_outputs) - - # compute the guided gradients - cast_conv_outputs = tf.cast(conv_outputs > 0, "float32") - cast_grads = tf.cast(grads > 0, "float32") - guided_grads = cast_conv_outputs * cast_grads * grads - # the convolution and guided gradients have a batch dimension - # (which we don't need) so let's grab the volume itself and - # discard the batch - conv_outputs = conv_outputs[0] - guided_grads = guided_grads[0] - - # compute the average of the gradient values, and using them - # as weights, compute the ponderation of the filters with - # respect to the weights - weights = tf.reduce_mean(guided_grads, axis=(0, 1)) - cam = tf.reduce_sum(tf.multiply(weights, conv_outputs), axis=-1) - - # grab the spatial dimensions of the input image and resize - # the output class activation map to match the input image - # dimensions - (w, h) = (image.shape[2], image.shape[1]) - heatmap = cv2.resize(cam.numpy(), (w, h)) - # normalize the heatmap such that all values lie in the range - # [0, 1], scale the resulting values to the range [0, 255], - # and then convert to an unsigned 8-bit integer - numer = heatmap - np.min(heatmap) - denom = (heatmap.max() - heatmap.min()) + eps - heatmap = numer / denom - heatmap = (heatmap * 255).astype("uint8") - # return the resulting heatmap to the calling function - return heatmap - - def overlay_heatmap(self, heatmap, image, alpha=0.5, - colormap=cv2.COLORMAP_VIRIDIS): - # apply the supplied color map to the heatmap and then - # overlay the heatmap on the input image - heatmap = cv2.applyColorMap(heatmap, colormap) - output = cv2.addWeighted(image, alpha, heatmap, 1 - alpha, 0) - # return a 2-tuple of the color mapped heatmap and the output, - # overlaid image - return heatmap, output \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py deleted file mode 100755 index 8ff4b7d5..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13.py +++ /dev/null @@ -1,1388 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -# Allows controlling a vehicle with a keyboard. For a simpler and more -# documented example, please take a look at tutorial.py. - -""" -Welcome to CARLA manual control. - -Use ARROWS or WASD keys for control. - - W : throttle - S : brake - A/D : steer left/right - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - CTRL + W : toggle constant velocity mode at 60 km/h - - L : toggle next light type - SHIFT + L : toggle high beam - Z/X : toggle right/left blinker - I : toggle interior light - - TAB : change sensor position - ` or N : next sensor - [1-9] : change to sensor [1-9] - G : toggle radar visualization - C : change weather (Shift+C reverse) - Backspace : change vehicle - - O : open/close all doors of vehicle - T : toggle vehicle's telemetry - - V : Select next map layer (Shift+V reverse) - B : Load current selected map layer (Shift+B to unload) - - R : toggle recording images to disk - - CTRL + R : toggle recording of simulation (replacing any previous) - CTRL + P : start replaying last recorded simulation - CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) - CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) - - F1 : toggle HUD - H/? : toggle help - ESC : quit -""" - -from __future__ import print_function - - -# ============================================================================== -# -- find carla module --------------------------------------------------------- -# ============================================================================== - - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - - -# ============================================================================== -# -- imports ------------------------------------------------------------------- -# ============================================================================== - - -import carla - -from carla import ColorConverter as cc - -import argparse -import collections -import datetime -import logging -import math -import random -import re -import weakref - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import KMOD_SHIFT - from pygame.locals import K_0 - from pygame.locals import K_9 - from pygame.locals import K_BACKQUOTE - from pygame.locals import K_BACKSPACE - from pygame.locals import K_COMMA - from pygame.locals import K_DOWN - from pygame.locals import K_ESCAPE - from pygame.locals import K_F1 - from pygame.locals import K_LEFT - from pygame.locals import K_PERIOD - from pygame.locals import K_RIGHT - from pygame.locals import K_SLASH - from pygame.locals import K_SPACE - from pygame.locals import K_TAB - from pygame.locals import K_UP - from pygame.locals import K_a - from pygame.locals import K_b - from pygame.locals import K_c - from pygame.locals import K_d - from pygame.locals import K_g - from pygame.locals import K_h - from pygame.locals import K_i - from pygame.locals import K_l - from pygame.locals import K_m - from pygame.locals import K_n - from pygame.locals import K_o - from pygame.locals import K_p - from pygame.locals import K_q - from pygame.locals import K_r - from pygame.locals import K_s - from pygame.locals import K_t - from pygame.locals import K_v - from pygame.locals import K_w - from pygame.locals import K_x - from pygame.locals import K_z - from pygame.locals import K_MINUS - from pygame.locals import K_EQUALS -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def find_weather_presets(): - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] - - -def get_actor_display_name(actor, truncate=250): - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - -def get_actor_blueprints(world, filter, generation): - bps = world.get_blueprint_library().filter(filter) - - if generation.lower() == "all": - return bps - - # If the filter returns only one bp, we assume that this one needed - # and therefore, we ignore the generation - if len(bps) == 1: - return bps - - try: - int_generation = int(generation) - # Check if generation is in available generations - if int_generation in [1, 2]: - bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] - return bps - else: - print(" Warning! Actor Generation is not valid. No actor will be spawned.") - return [] - except: - print(" Warning! Actor Generation is not valid. No actor will be spawned.") - return [] - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== - - -class World(object): - def __init__(self, carla_world, hud, args): - self.world = carla_world - self.sync = args.sync - self.actor_role_name = args.rolename - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.imu_sensor = None - self.radar_sensor = None - self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = args.filter - self._actor_generation = args.generation - self._gamma = args.gamma - self.restart() - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - self.constant_velocity_enabled = False - self.show_vehicle_telemetry = False - self.doors_are_open = False - self.current_map_layer = 0 - self.map_layer_names = [ - carla.MapLayer.NONE, - carla.MapLayer.Buildings, - carla.MapLayer.Decals, - carla.MapLayer.Foliage, - carla.MapLayer.Ground, - carla.MapLayer.ParkedVehicles, - carla.MapLayer.Particles, - carla.MapLayer.Props, - carla.MapLayer.StreetLights, - carla.MapLayer.Walls, - carla.MapLayer.All - ] - - def restart(self): - self.player_max_speed = 1.589 - self.player_max_speed_fast = 3.713 - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Get a random blueprint. - blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) - blueprint = self.world.get_blueprint_library().filter('vehicle')[0] - print(blueprint) - blueprint.set_attribute('role_name', self.actor_role_name) - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) - if blueprint.has_attribute('driver_id'): - driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) - blueprint.set_attribute('driver_id', driver_id) - if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'true') - # set the max speed - if blueprint.has_attribute('speed'): - self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) - self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) - - # Spawn the player. - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - self.show_vehicle_telemetry = False - self.modify_vehicle_physics(self.player) - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - # Draw the spawn point locations as numbers in the map - for i, spawn_point in enumerate(spawn_points): - print(i, spawn_point) - self.world.debug.draw_string(spawn_point.location, str(i), life_time=10000) - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - print('*****************************************************') - #spawn_point = self.world.get_map().get_spawn_points()[0] - # Town 2 - #spawn_point = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # Town 1 - # clockwise - #spawn_point = carla.Transform(carla.Location(x=2.0, y=70, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - # anticlockwise - spawn_point = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - spawn_point = carla.Transform(carla.Location(x=2.0, y=20, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - #spawn_point = carla.Transform(carla.Location(x=10.0, y=331, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=5.0, y=330, z=1.37), carla.Rotation(pitch=0, yaw=30, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=150.0, y=331, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=-2.0, y=13, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=2, y=50, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - #spawn_point = carla.Transform(carla.Location(x=20, y=330, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=330, y=330, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - - #spawn_point = carla.Transform(carla.Location(x=228, y=-2, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) - - - #spawn_point = carla.Transform(carla.Location(x=-0.5, y=40, z=1.37), carla.Rotation(pitch=0, yaw=30, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=396, y=30, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 3 - # anticlockwise - #spawn_point = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - # clockwise - #spawn_point = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) - - # Town 4 - #spawn_point = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=16.6, y=152.4, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # anticlockwise - #spawn_point = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - #Town 05 - # clockwise - #spawn_point = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - # anticlockwise - #spawn_point = carla.Transform(carla.Location(x=115, y=-207.3, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) - - # Town 06 - #spawn_point = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 7 - # anticlockwise - #spawn_point = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) - # clockwise - #spawn_point = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #print(spawn_point) - - - - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - self.show_vehicle_telemetry = False - self.modify_vehicle_physics(self.player) - # Set up the sensors. - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) - self.gnss_sensor = GnssSensor(self.player) - self.imu_sensor = IMUSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) - self.camera_manager.transform_index = cam_pos_index - self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) - - if self.sync: - self.world.tick() - else: - self.world.wait_for_tick() - - def next_weather(self, reverse=False): - self._weather_index += -1 if reverse else 1 - self._weather_index %= len(self._weather_presets) - preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) - - def next_map_layer(self, reverse=False): - self.current_map_layer += -1 if reverse else 1 - self.current_map_layer %= len(self.map_layer_names) - selected = self.map_layer_names[self.current_map_layer] - self.hud.notification('LayerMap selected: %s' % selected) - - def load_map_layer(self, unload=False): - selected = self.map_layer_names[self.current_map_layer] - if unload: - self.hud.notification('Unloading map layer: %s' % selected) - self.world.unload_map_layer(selected) - else: - self.hud.notification('Loading map layer: %s' % selected) - self.world.load_map_layer(selected) - - def toggle_radar(self): - if self.radar_sensor is None: - self.radar_sensor = RadarSensor(self.player) - elif self.radar_sensor.sensor is not None: - self.radar_sensor.sensor.destroy() - self.radar_sensor = None - - def modify_vehicle_physics(self, actor): - #If actor is not a vehicle, we cannot use the physics control - try: - physics_control = actor.get_physics_control() - physics_control.use_sweep_wheel_collision = True - actor.apply_physics_control(physics_control) - except Exception: - pass - - def tick(self, clock): - self.hud.tick(self, clock) - - def render(self, display): - self.camera_manager.render(display) - self.hud.render(display) - - def destroy_sensors(self): - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None - - def destroy(self): - if self.radar_sensor is not None: - self.toggle_radar() - sensors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.imu_sensor.sensor] - for sensor in sensors: - if sensor is not None: - sensor.stop() - sensor.destroy() - if self.player is not None: - self.player.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - """Class that handles keyboard input.""" - def __init__(self, world, start_in_autopilot): - self._autopilot_enabled = start_in_autopilot - if isinstance(world.player, carla.Vehicle): - self._control = carla.VehicleControl() - self._lights = carla.VehicleLightState.NONE - world.player.set_autopilot(self._autopilot_enabled) - world.player.set_light_state(self._lights) - elif isinstance(world.player, carla.Walker): - self._control = carla.WalkerControl() - self._autopilot_enabled = False - self._rotation = world.player.get_transform().rotation - else: - raise NotImplementedError("Actor type not supported") - self._steer_cache = 0.0 - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - - def parse_events(self, client, world, clock, sync_mode): - if isinstance(self._control, carla.VehicleControl): - current_lights = self._lights - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if self._is_quit_shortcut(event.key): - return True - elif event.key == K_BACKSPACE: - if self._autopilot_enabled: - world.player.set_autopilot(False) - world.restart() - world.player.set_autopilot(True) - else: - world.restart() - elif event.key == K_F1: - world.hud.toggle_info() - elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: - world.next_map_layer(reverse=True) - elif event.key == K_v: - world.next_map_layer() - elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: - world.load_map_layer(unload=True) - elif event.key == K_b: - world.load_map_layer() - elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): - world.hud.help.toggle() - elif event.key == K_TAB: - world.camera_manager.toggle_camera() - elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: - world.next_weather(reverse=True) - elif event.key == K_c: - world.next_weather() - elif event.key == K_g: - world.toggle_radar() - elif event.key == K_BACKQUOTE: - world.camera_manager.next_sensor() - elif event.key == K_n: - world.camera_manager.next_sensor() - elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): - if world.constant_velocity_enabled: - world.player.disable_constant_velocity() - world.constant_velocity_enabled = False - world.hud.notification("Disabled Constant Velocity Mode") - else: - world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) - world.constant_velocity_enabled = True - world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") - elif event.key == K_o: - try: - if world.doors_are_open: - world.hud.notification("Closing Doors") - world.doors_are_open = False - world.player.close_door(carla.VehicleDoor.All) - else: - world.hud.notification("Opening doors") - world.doors_are_open = True - world.player.open_door(carla.VehicleDoor.All) - except Exception: - pass - elif event.key == K_t: - if world.show_vehicle_telemetry: - world.player.show_debug_telemetry(False) - world.show_vehicle_telemetry = False - world.hud.notification("Disabled Vehicle Telemetry") - else: - try: - world.player.show_debug_telemetry(True) - world.show_vehicle_telemetry = True - world.hud.notification("Enabled Vehicle Telemetry") - except Exception: - pass - elif event.key > K_0 and event.key <= K_9: - index_ctrl = 0 - if pygame.key.get_mods() & KMOD_CTRL: - index_ctrl = 9 - world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) - elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): - world.camera_manager.toggle_recording() - elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): - if (world.recording_enabled): - client.stop_recorder() - world.recording_enabled = False - world.hud.notification("Recorder is OFF") - else: - client.start_recorder("manual_recording.rec") - world.recording_enabled = True - world.hud.notification("Recorder is ON") - elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): - # stop recorder - client.stop_recorder() - world.recording_enabled = False - # work around to fix camera at start of replaying - current_index = world.camera_manager.index - world.destroy_sensors() - # disable autopilot - self._autopilot_enabled = False - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification("Replaying file 'manual_recording.rec'") - # replayer - client.replay_file("manual_recording.rec", world.recording_start, 0, 0) - world.camera_manager.set_sensor(current_index) - elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start -= 10 - else: - world.recording_start -= 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start += 10 - else: - world.recording_start += 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - if isinstance(self._control, carla.VehicleControl): - if event.key == K_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: - if not self._autopilot_enabled and not sync_mode: - print("WARNING: You are currently in asynchronous mode and could " - "experience some issues with the traffic simulation") - self._autopilot_enabled = not self._autopilot_enabled - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification( - 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) - elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: - current_lights ^= carla.VehicleLightState.Special1 - elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: - current_lights ^= carla.VehicleLightState.HighBeam - elif event.key == K_l: - # Use 'L' key to switch between lights: - # closed -> position -> low beam -> fog - if not self._lights & carla.VehicleLightState.Position: - world.hud.notification("Position lights") - current_lights |= carla.VehicleLightState.Position - else: - world.hud.notification("Low beam lights") - current_lights |= carla.VehicleLightState.LowBeam - if self._lights & carla.VehicleLightState.LowBeam: - world.hud.notification("Fog lights") - current_lights |= carla.VehicleLightState.Fog - if self._lights & carla.VehicleLightState.Fog: - world.hud.notification("Lights off") - current_lights ^= carla.VehicleLightState.Position - current_lights ^= carla.VehicleLightState.LowBeam - current_lights ^= carla.VehicleLightState.Fog - elif event.key == K_i: - current_lights ^= carla.VehicleLightState.Interior - elif event.key == K_z: - current_lights ^= carla.VehicleLightState.LeftBlinker - elif event.key == K_x: - current_lights ^= carla.VehicleLightState.RightBlinker - - if not self._autopilot_enabled: - if isinstance(self._control, carla.VehicleControl): - self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) - self._control.reverse = self._control.gear < 0 - # Set automatic control-related vehicle lights - if self._control.brake: - current_lights |= carla.VehicleLightState.Brake - else: # Remove the Brake flag - current_lights &= ~carla.VehicleLightState.Brake - if self._control.reverse: - current_lights |= carla.VehicleLightState.Reverse - else: # Remove the Reverse flag - current_lights &= ~carla.VehicleLightState.Reverse - if current_lights != self._lights: # Change the light state only if necessary - self._lights = current_lights - world.player.set_light_state(carla.VehicleLightState(self._lights)) - elif isinstance(self._control, carla.WalkerControl): - self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) - world.player.apply_control(self._control) - - def _parse_vehicle_keys(self, keys, milliseconds): - if keys[K_UP] or keys[K_w]: - self._control.throttle = min(self._control.throttle + 0.01, 1.00) - else: - self._control.throttle = 0.0 - - if keys[K_DOWN] or keys[K_s]: - self._control.brake = min(self._control.brake + 0.2, 1) - else: - self._control.brake = 0 - - steer_increment = 5e-4 * milliseconds - if keys[K_LEFT] or keys[K_a]: - if self._steer_cache > 0: - self._steer_cache = 0 - else: - self._steer_cache -= steer_increment - elif keys[K_RIGHT] or keys[K_d]: - if self._steer_cache < 0: - self._steer_cache = 0 - else: - self._steer_cache += steer_increment - else: - self._steer_cache = 0.0 - self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) - self._control.steer = round(self._steer_cache, 1) - self._control.hand_brake = keys[K_SPACE] - - def _parse_walker_keys(self, keys, milliseconds, world): - self._control.speed = 0.0 - if keys[K_DOWN] or keys[K_s]: - self._control.speed = 0.0 - if keys[K_LEFT] or keys[K_a]: - self._control.speed = .01 - self._rotation.yaw -= 0.08 * milliseconds - if keys[K_RIGHT] or keys[K_d]: - self._control.speed = .01 - self._rotation.yaw += 0.08 * milliseconds - if keys[K_UP] or keys[K_w]: - self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed - self._control.jump = keys[K_SPACE] - self._rotation.yaw = round(self._rotation.yaw, 1) - self._control.direction = self._rotation.get_forward_vector() - - @staticmethod - def _is_quit_shortcut(key): - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - def __init__(self, width, height): - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - font_name = 'courier' if os.name == 'nt' else 'mono' - fonts = [x for x in pygame.font.get_fonts() if font_name in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) - self._notifications = FadingText(font, (width, 40), (0, height - 40)) - self.help = HelpText(pygame.font.Font(mono, 16), width, height) - self.server_fps = 0 - self.frame = 0 - self.simulation_time = 0 - self._show_info = True - self._info_text = [] - self._server_clock = pygame.time.Clock() - - def on_world_tick(self, timestamp): - self._server_clock.tick() - self.server_fps = self._server_clock.get_fps() - self.frame = timestamp.frame - self.simulation_time = timestamp.elapsed_seconds - - def tick(self, world, clock): - self._notifications.tick(world, clock) - if not self._show_info: - return - t = world.player.get_transform() - v = world.player.get_velocity() - c = world.player.get_control() - compass = world.imu_sensor.compass - heading = 'N' if compass > 270.5 or compass < 89.5 else '' - heading += 'S' if 90.5 < compass < 269.5 else '' - heading += 'E' if 0.5 < compass < 179.5 else '' - heading += 'W' if 180.5 < compass < 359.5 else '' - colhist = world.collision_sensor.get_collision_history() - collision = [colhist[x + self.frame - 200] for x in range(0, 200)] - max_col = max(1.0, max(collision)) - collision = [x / max_col for x in collision] - vehicles = world.world.get_actors().filter('vehicle.*') - self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % world.map.name.split('/')[-1], - 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), - '', - 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), - u'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), - 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), - 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), - 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), - 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), - 'Height: % 18.0f m' % t.location.z, - ''] - if isinstance(c, carla.VehicleControl): - self._info_text += [ - ('Throttle:', c.throttle, 0.0, 1.0), - ('Steer:', c.steer, -1.0, 1.0), - ('Brake:', c.brake, 0.0, 1.0), - ('Reverse:', c.reverse), - ('Hand brake:', c.hand_brake), - ('Manual:', c.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] - elif isinstance(c, carla.WalkerControl): - self._info_text += [ - ('Speed:', c.speed, 0.0, 5.556), - ('Jump:', c.jump)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - if len(vehicles) > 1: - self._info_text += ['Nearby vehicles:'] - distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) - vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] - for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]): - if d > 200.0: - break - vehicle_type = get_actor_display_name(vehicle, truncate=22) - self._info_text.append('% 4dm %s' % (d, vehicle_type)) - - def toggle_info(self): - self._show_info = not self._show_info - - def notification(self, text, seconds=2.0): - self._notifications.set_text(text, seconds=seconds) - - def error(self, text): - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - f = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - def __init__(self, font, dim, pos): - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - """Helper class to handle text output using pygame""" - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.line_space = 18 - self.dim = (780, len(lines) * self.line_space + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, n * self.line_space)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - self._render = not self._render - - def render(self, display): - if self._render: - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - self.history = [] - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - history = collections.defaultdict(int) - for frame, intensity in self.history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self.hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) - self.history.append((event.frame, intensity)) - if len(self.history) > 4000: - self.history.pop(0) - - -# ============================================================================== -# -- LaneInvasionSensor -------------------------------------------------------- -# ============================================================================== - - -class LaneInvasionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - - # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor - if parent_actor.type_id.startswith("vehicle."): - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_invasion') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - - @staticmethod - def _on_invasion(weak_self, event): - self = weak_self() - if not self: - return - lane_types = set(x.type for x in event.crossed_lane_markings) - text = ['%r' % str(x).split()[-1] for x in lane_types] - self.hud.notification('Crossed line %s' % ' and '.join(text)) - - -# ============================================================================== -# -- GnssSensor ---------------------------------------------------------------- -# ============================================================================== - - -class GnssSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - - @staticmethod - def _on_gnss_event(weak_self, event): - self = weak_self() - if not self: - return - self.lat = event.latitude - self.lon = event.longitude - - -# ============================================================================== -# -- IMUSensor ----------------------------------------------------------------- -# ============================================================================== - - -class IMUSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.accelerometer = (0.0, 0.0, 0.0) - self.gyroscope = (0.0, 0.0, 0.0) - self.compass = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.imu') - self.sensor = world.spawn_actor( - bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen( - lambda sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) - - @staticmethod - def _IMU_callback(weak_self, sensor_data): - self = weak_self() - if not self: - return - limits = (-99.9, 99.9) - self.accelerometer = ( - max(limits[0], min(limits[1], sensor_data.accelerometer.x)), - max(limits[0], min(limits[1], sensor_data.accelerometer.y)), - max(limits[0], min(limits[1], sensor_data.accelerometer.z))) - self.gyroscope = ( - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) - self.compass = math.degrees(sensor_data.compass) - - -# ============================================================================== -# -- RadarSensor --------------------------------------------------------------- -# ============================================================================== - - -class RadarSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - bound_x = 0.5 + self._parent.bounding_box.extent.x - bound_y = 0.5 + self._parent.bounding_box.extent.y - bound_z = 0.5 + self._parent.bounding_box.extent.z - - self.velocity_range = 7.5 # m/s - world = self._parent.get_world() - self.debug = world.debug - bp = world.get_blueprint_library().find('sensor.other.radar') - bp.set_attribute('horizontal_fov', str(35)) - bp.set_attribute('vertical_fov', str(20)) - self.sensor = world.spawn_actor( - bp, - carla.Transform( - carla.Location(x=bound_x + 0.05, z=bound_z+0.05), - carla.Rotation(pitch=5)), - attach_to=self._parent) - # We need a weak reference to self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen( - lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) - - @staticmethod - def _Radar_callback(weak_self, radar_data): - self = weak_self() - if not self: - return - # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: - # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) - # points = np.reshape(points, (len(radar_data), 4)) - - current_rot = radar_data.transform.rotation - for detect in radar_data: - azi = math.degrees(detect.azimuth) - alt = math.degrees(detect.altitude) - # The 0.25 adjusts a bit the distance so the dots can - # be properly seen - fw_vec = carla.Vector3D(x=detect.depth - 0.25) - carla.Transform( - carla.Location(), - carla.Rotation( - pitch=current_rot.pitch + alt, - yaw=current_rot.yaw + azi, - roll=current_rot.roll)).transform(fw_vec) - - def clamp(min_v, max_v, value): - return max(min_v, min(value, max_v)) - - norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] - r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) - g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) - b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) - self.debug.draw_point( - radar_data.transform.location + fw_vec, - size=0.075, - life_time=0.06, - persistent_lines=False, - color=carla.Color(r, g, b)) - -# ============================================================================== -# -- CameraManager ------------------------------------------------------------- -# ============================================================================== - - -class CameraManager(object): - def __init__(self, parent_actor, hud, gamma_correction): - self.sensor = None - self.surface = None - self._parent = parent_actor - self.hud = hud - self.recording = False - bound_x = 0.5 + self._parent.bounding_box.extent.x - bound_y = 0.5 + self._parent.bounding_box.extent.y - bound_z = 0.5 + self._parent.bounding_box.extent.z - Attachment = carla.AttachmentType - - if not self._parent.type_id.startswith("walker.pedestrian"): - self._camera_transforms = [ - (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), - (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] - else: - self._camera_transforms = [ - (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), - (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] - - self.transform_index = 1 - self.sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], - ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], - ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], - ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], - ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', - {'lens_circle_multiplier': '3.0', - 'lens_circle_falloff': '3.0', - 'chromatic_aberration_intensity': '0.5', - 'chromatic_aberration_offset': '0'}], - ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], - ] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - bp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - bp.set_attribute('image_size_x', str(hud.dim[0])) - bp.set_attribute('image_size_y', str(hud.dim[1])) - if bp.has_attribute('gamma'): - bp.set_attribute('gamma', str(gamma_correction)) - for attr_name, attr_value in item[3].items(): - bp.set_attribute(attr_name, attr_value) - elif item[0].startswith('sensor.lidar'): - self.lidar_range = 50 - - for attr_name, attr_value in item[3].items(): - bp.set_attribute(attr_name, attr_value) - if attr_name == 'range': - self.lidar_range = float(attr_value) - - item.append(bp) - self.index = None - - def toggle_camera(self): - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - index = index % len(self.sensors) - needs_respawn = True if self.index is None else \ - (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self.surface = None - self.sensor = self._parent.get_world().spawn_actor( - self.sensors[index][-1], - self._camera_transforms[self.transform_index][0], - attach_to=self._parent, - attachment_type=self._camera_transforms[self.transform_index][1]) - # We need to pass the lambda a weak reference to self to avoid - # circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - self.hud.notification(self.sensors[index][2]) - self.index = index - - def next_sensor(self): - self.set_sensor(self.index + 1) - - def toggle_recording(self): - self.recording = not self.recording - self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) - - def render(self, display): - if self.surface is not None: - display.blit(self.surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self.sensors[self.index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0] / 4), 4)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) - lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) - lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) - lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): - # Example of converting the raw_data from a carla.DVSEventArray - # sensor into a NumPy array and using it as an image - dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ - ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) - dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) - # Blue is positive, red is negative - dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 - self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) - elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): - image = image.get_color_coded_flow() - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - else: - image.convert(self.sensors[self.index][1]) - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if self.recording: - image.save_to_disk('_out/%08d' % image.frame) - - -# ============================================================================== -# -- game_loop() --------------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - pygame.init() - pygame.font.init() - world = None - original_settings = None - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(20.0) - - sim_world = client.get_world() - if args.sync: - original_settings = sim_world.get_settings() - settings = sim_world.get_settings() - if not settings.synchronous_mode: - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - sim_world.apply_settings(settings) - - traffic_manager = client.get_trafficmanager() - traffic_manager.set_synchronous_mode(True) - - if args.autopilot and not sim_world.get_settings().synchronous_mode: - print("WARNING: You are currently in asynchronous mode and could " - "experience some issues with the traffic simulation") - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - display.fill((0,0,0)) - pygame.display.flip() - - hud = HUD(args.width, args.height) - world = World(sim_world, hud, args) - controller = KeyboardControl(world, args.autopilot) - - if args.sync: - sim_world.tick() - else: - sim_world.wait_for_tick() - - clock = pygame.time.Clock() - while True: - if args.sync: - sim_world.tick() - clock.tick_busy_loop(60) - if controller.parse_events(client, world, clock, args.sync): - return - world.tick(clock) - world.render(display) - pygame.display.flip() - - finally: - - if original_settings: - sim_world.apply_settings(original_settings) - - if (world and world.recording_enabled): - client.stop_recorder() - - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------------- -# ============================================================================== - - -def main(): - argparser = argparse.ArgumentParser( - description='CARLA Manual Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '-a', '--autopilot', - action='store_true', - help='enable autopilot') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='window resolution (default: 1280x720)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='actor filter (default: "vehicle.*")') - argparser.add_argument( - '--generation', - metavar='G', - default='2', - help='restrict to certain actor generation (values: "1","2","All" - default: "2")') - argparser.add_argument( - '--rolename', - metavar='NAME', - default='hero', - help='actor role name (default: "hero")') - argparser.add_argument( - '--gamma', - default=2.2, - type=float, - help='Gamma correction of the camera (default: 2.2)') - argparser.add_argument( - '--sync', - action='store_true', - help='Activate synchronous mode execution') - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - - main() diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py deleted file mode 100644 index effff6a1..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_car.py +++ /dev/null @@ -1,20 +0,0 @@ -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import pandas - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -blueprint = world.get_blueprint_library().filter('vehicle')[0] -print(blueprint) -spawn_point = world.get_map().get_spawn_points()[0] -print(spawn_point) -player = world.try_spawn_actor(blueprint, spawn_point) \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py deleted file mode 100644 index 6fa986a4..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_simple.py +++ /dev/null @@ -1,1311 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -# Allows controlling a vehicle with a keyboard. For a simpler and more -# documented example, please take a look at tutorial.py. - -""" -Welcome to CARLA manual control. - -Use ARROWS or WASD keys for control. - - W : throttle - S : brake - A/D : steer left/right - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - CTRL + W : toggle constant velocity mode at 60 km/h - - L : toggle next light type - SHIFT + L : toggle high beam - Z/X : toggle right/left blinker - I : toggle interior light - - TAB : change sensor position - ` or N : next sensor - [1-9] : change to sensor [1-9] - G : toggle radar visualization - C : change weather (Shift+C reverse) - Backspace : change vehicle - - O : open/close all doors of vehicle - T : toggle vehicle's telemetry - - V : Select next map layer (Shift+V reverse) - B : Load current selected map layer (Shift+B to unload) - - R : toggle recording images to disk - - CTRL + R : toggle recording of simulation (replacing any previous) - CTRL + P : start replaying last recorded simulation - CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) - CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) - - F1 : toggle HUD - H/? : toggle help - ESC : quit -""" - -from __future__ import print_function - - -# ============================================================================== -# -- find carla module --------------------------------------------------------- -# ============================================================================== - - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - - -# ============================================================================== -# -- imports ------------------------------------------------------------------- -# ============================================================================== - - -import carla - -from carla import ColorConverter as cc - -import argparse -import collections -import datetime -import logging -import math -import random -import re -import weakref - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import KMOD_SHIFT - from pygame.locals import K_0 - from pygame.locals import K_9 - from pygame.locals import K_BACKQUOTE - from pygame.locals import K_BACKSPACE - from pygame.locals import K_COMMA - from pygame.locals import K_DOWN - from pygame.locals import K_ESCAPE - from pygame.locals import K_F1 - from pygame.locals import K_LEFT - from pygame.locals import K_PERIOD - from pygame.locals import K_RIGHT - from pygame.locals import K_SLASH - from pygame.locals import K_SPACE - from pygame.locals import K_TAB - from pygame.locals import K_UP - from pygame.locals import K_a - from pygame.locals import K_b - from pygame.locals import K_c - from pygame.locals import K_d - from pygame.locals import K_g - from pygame.locals import K_h - from pygame.locals import K_i - from pygame.locals import K_l - from pygame.locals import K_m - from pygame.locals import K_n - from pygame.locals import K_o - from pygame.locals import K_p - from pygame.locals import K_q - from pygame.locals import K_r - from pygame.locals import K_s - from pygame.locals import K_t - from pygame.locals import K_v - from pygame.locals import K_w - from pygame.locals import K_x - from pygame.locals import K_z - from pygame.locals import K_MINUS - from pygame.locals import K_EQUALS -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def get_actor_display_name(actor, truncate=250): - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - -def get_actor_blueprints(world, filter, generation): - bps = world.get_blueprint_library().filter(filter) - - if generation.lower() == "all": - return bps - - # If the filter returns only one bp, we assume that this one needed - # and therefore, we ignore the generation - if len(bps) == 1: - return bps - - try: - int_generation = int(generation) - # Check if generation is in available generations - if int_generation in [1, 2]: - bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] - return bps - else: - print(" Warning! Actor Generation is not valid. No actor will be spawned.") - return [] - except: - print(" Warning! Actor Generation is not valid. No actor will be spawned.") - return [] - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== - - -class World(object): - def __init__(self, carla_world, hud, args): - self.world = carla_world - self.sync = args.sync - self.actor_role_name = args.rolename - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.imu_sensor = None - self.radar_sensor = None - self.camera_manager = None - self._actor_filter = args.filter - self._actor_generation = args.generation - self._gamma = args.gamma - self.restart() - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - self.constant_velocity_enabled = False - self.show_vehicle_telemetry = False - self.doors_are_open = False - self.current_map_layer = 0 - self.map_layer_names = [ - carla.MapLayer.NONE, - carla.MapLayer.Buildings, - carla.MapLayer.Decals, - carla.MapLayer.Foliage, - carla.MapLayer.Ground, - carla.MapLayer.ParkedVehicles, - carla.MapLayer.Particles, - carla.MapLayer.Props, - carla.MapLayer.StreetLights, - carla.MapLayer.Walls, - carla.MapLayer.All - ] - - def restart(self): - self.player_max_speed = 1.589 - self.player_max_speed_fast = 3.713 - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Get a random blueprint. - blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) - blueprint = self.world.get_blueprint_library().filter('vehicle')[0] - print(blueprint) - blueprint.set_attribute('role_name', self.actor_role_name) - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) - if blueprint.has_attribute('driver_id'): - driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) - blueprint.set_attribute('driver_id', driver_id) - if blueprint.has_attribute('is_invincible'): - blueprint.set_attribute('is_invincible', 'true') - # set the max speed - if blueprint.has_attribute('speed'): - self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) - self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) - - # Spawn the player. - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - self.show_vehicle_telemetry = False - self.modify_vehicle_physics(self.player) - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - print('*****************************************************') - spawn_point = self.world.get_map().get_spawn_points()[0] - #spawn_point = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - print(spawn_point) - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - self.show_vehicle_telemetry = False - self.modify_vehicle_physics(self.player) - # Set up the sensors. - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) - self.gnss_sensor = GnssSensor(self.player) - self.imu_sensor = IMUSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) - self.camera_manager.transform_index = cam_pos_index - self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) - - if self.sync: - self.world.tick() - else: - self.world.wait_for_tick() - - def next_map_layer(self, reverse=False): - self.current_map_layer += -1 if reverse else 1 - self.current_map_layer %= len(self.map_layer_names) - selected = self.map_layer_names[self.current_map_layer] - self.hud.notification('LayerMap selected: %s' % selected) - - def load_map_layer(self, unload=False): - selected = self.map_layer_names[self.current_map_layer] - if unload: - self.hud.notification('Unloading map layer: %s' % selected) - self.world.unload_map_layer(selected) - else: - self.hud.notification('Loading map layer: %s' % selected) - self.world.load_map_layer(selected) - - def toggle_radar(self): - if self.radar_sensor is None: - self.radar_sensor = RadarSensor(self.player) - elif self.radar_sensor.sensor is not None: - self.radar_sensor.sensor.destroy() - self.radar_sensor = None - - def modify_vehicle_physics(self, actor): - #If actor is not a vehicle, we cannot use the physics control - try: - physics_control = actor.get_physics_control() - physics_control.use_sweep_wheel_collision = True - actor.apply_physics_control(physics_control) - except Exception: - pass - - def tick(self, clock): - self.hud.tick(self, clock) - - def render(self, display): - self.camera_manager.render(display) - self.hud.render(display) - - def destroy_sensors(self): - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None - - def destroy(self): - if self.radar_sensor is not None: - self.toggle_radar() - sensors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.imu_sensor.sensor] - for sensor in sensors: - if sensor is not None: - sensor.stop() - sensor.destroy() - if self.player is not None: - self.player.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - """Class that handles keyboard input.""" - def __init__(self, world, start_in_autopilot): - self._autopilot_enabled = start_in_autopilot - if isinstance(world.player, carla.Vehicle): - self._control = carla.VehicleControl() - self._lights = carla.VehicleLightState.NONE - world.player.set_autopilot(self._autopilot_enabled) - world.player.set_light_state(self._lights) - elif isinstance(world.player, carla.Walker): - self._control = carla.WalkerControl() - self._autopilot_enabled = False - self._rotation = world.player.get_transform().rotation - else: - raise NotImplementedError("Actor type not supported") - self._steer_cache = 0.0 - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - - def parse_events(self, client, world, clock, sync_mode): - if isinstance(self._control, carla.VehicleControl): - current_lights = self._lights - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if self._is_quit_shortcut(event.key): - return True - elif event.key == K_BACKSPACE: - if self._autopilot_enabled: - world.player.set_autopilot(False) - world.restart() - world.player.set_autopilot(True) - else: - world.restart() - elif event.key == K_F1: - world.hud.toggle_info() - elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: - world.next_map_layer(reverse=True) - elif event.key == K_v: - world.next_map_layer() - elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: - world.load_map_layer(unload=True) - elif event.key == K_b: - world.load_map_layer() - elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): - world.hud.help.toggle() - elif event.key == K_TAB: - world.camera_manager.toggle_camera() - elif event.key == K_g: - world.toggle_radar() - elif event.key == K_BACKQUOTE: - world.camera_manager.next_sensor() - elif event.key == K_n: - world.camera_manager.next_sensor() - elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): - if world.constant_velocity_enabled: - world.player.disable_constant_velocity() - world.constant_velocity_enabled = False - world.hud.notification("Disabled Constant Velocity Mode") - else: - world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) - world.constant_velocity_enabled = True - world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") - elif event.key == K_o: - try: - if world.doors_are_open: - world.hud.notification("Closing Doors") - world.doors_are_open = False - world.player.close_door(carla.VehicleDoor.All) - else: - world.hud.notification("Opening doors") - world.doors_are_open = True - world.player.open_door(carla.VehicleDoor.All) - except Exception: - pass - elif event.key == K_t: - if world.show_vehicle_telemetry: - world.player.show_debug_telemetry(False) - world.show_vehicle_telemetry = False - world.hud.notification("Disabled Vehicle Telemetry") - else: - try: - world.player.show_debug_telemetry(True) - world.show_vehicle_telemetry = True - world.hud.notification("Enabled Vehicle Telemetry") - except Exception: - pass - elif event.key > K_0 and event.key <= K_9: - index_ctrl = 0 - if pygame.key.get_mods() & KMOD_CTRL: - index_ctrl = 9 - world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) - elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): - world.camera_manager.toggle_recording() - elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): - if (world.recording_enabled): - client.stop_recorder() - world.recording_enabled = False - world.hud.notification("Recorder is OFF") - else: - client.start_recorder("manual_recording.rec") - world.recording_enabled = True - world.hud.notification("Recorder is ON") - elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): - # stop recorder - client.stop_recorder() - world.recording_enabled = False - # work around to fix camera at start of replaying - current_index = world.camera_manager.index - world.destroy_sensors() - # disable autopilot - self._autopilot_enabled = False - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification("Replaying file 'manual_recording.rec'") - # replayer - client.replay_file("manual_recording.rec", world.recording_start, 0, 0) - world.camera_manager.set_sensor(current_index) - elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start -= 10 - else: - world.recording_start -= 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): - if pygame.key.get_mods() & KMOD_SHIFT: - world.recording_start += 10 - else: - world.recording_start += 1 - world.hud.notification("Recording start time is %d" % (world.recording_start)) - if isinstance(self._control, carla.VehicleControl): - if event.key == K_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: - if not self._autopilot_enabled and not sync_mode: - print("WARNING: You are currently in asynchronous mode and could " - "experience some issues with the traffic simulation") - self._autopilot_enabled = not self._autopilot_enabled - world.player.set_autopilot(self._autopilot_enabled) - world.hud.notification( - 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) - elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: - current_lights ^= carla.VehicleLightState.Special1 - elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: - current_lights ^= carla.VehicleLightState.HighBeam - elif event.key == K_l: - # Use 'L' key to switch between lights: - # closed -> position -> low beam -> fog - if not self._lights & carla.VehicleLightState.Position: - world.hud.notification("Position lights") - current_lights |= carla.VehicleLightState.Position - else: - world.hud.notification("Low beam lights") - current_lights |= carla.VehicleLightState.LowBeam - if self._lights & carla.VehicleLightState.LowBeam: - world.hud.notification("Fog lights") - current_lights |= carla.VehicleLightState.Fog - if self._lights & carla.VehicleLightState.Fog: - world.hud.notification("Lights off") - current_lights ^= carla.VehicleLightState.Position - current_lights ^= carla.VehicleLightState.LowBeam - current_lights ^= carla.VehicleLightState.Fog - elif event.key == K_i: - current_lights ^= carla.VehicleLightState.Interior - elif event.key == K_z: - current_lights ^= carla.VehicleLightState.LeftBlinker - elif event.key == K_x: - current_lights ^= carla.VehicleLightState.RightBlinker - - if not self._autopilot_enabled: - if isinstance(self._control, carla.VehicleControl): - self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) - self._control.reverse = self._control.gear < 0 - # Set automatic control-related vehicle lights - if self._control.brake: - current_lights |= carla.VehicleLightState.Brake - else: # Remove the Brake flag - current_lights &= ~carla.VehicleLightState.Brake - if self._control.reverse: - current_lights |= carla.VehicleLightState.Reverse - else: # Remove the Reverse flag - current_lights &= ~carla.VehicleLightState.Reverse - if current_lights != self._lights: # Change the light state only if necessary - self._lights = current_lights - world.player.set_light_state(carla.VehicleLightState(self._lights)) - elif isinstance(self._control, carla.WalkerControl): - self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) - world.player.apply_control(self._control) - - def _parse_vehicle_keys(self, keys, milliseconds): - if keys[K_UP] or keys[K_w]: - self._control.throttle = min(self._control.throttle + 0.01, 1.00) - else: - self._control.throttle = 0.0 - - if keys[K_DOWN] or keys[K_s]: - self._control.brake = min(self._control.brake + 0.2, 1) - else: - self._control.brake = 0 - - steer_increment = 5e-4 * milliseconds - if keys[K_LEFT] or keys[K_a]: - if self._steer_cache > 0: - self._steer_cache = 0 - else: - self._steer_cache -= steer_increment - elif keys[K_RIGHT] or keys[K_d]: - if self._steer_cache < 0: - self._steer_cache = 0 - else: - self._steer_cache += steer_increment - else: - self._steer_cache = 0.0 - self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) - self._control.steer = round(self._steer_cache, 1) - self._control.hand_brake = keys[K_SPACE] - - def _parse_walker_keys(self, keys, milliseconds, world): - self._control.speed = 0.0 - if keys[K_DOWN] or keys[K_s]: - self._control.speed = 0.0 - if keys[K_LEFT] or keys[K_a]: - self._control.speed = .01 - self._rotation.yaw -= 0.08 * milliseconds - if keys[K_RIGHT] or keys[K_d]: - self._control.speed = .01 - self._rotation.yaw += 0.08 * milliseconds - if keys[K_UP] or keys[K_w]: - self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed - self._control.jump = keys[K_SPACE] - self._rotation.yaw = round(self._rotation.yaw, 1) - self._control.direction = self._rotation.get_forward_vector() - - @staticmethod - def _is_quit_shortcut(key): - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - def __init__(self, width, height): - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - font_name = 'courier' if os.name == 'nt' else 'mono' - fonts = [x for x in pygame.font.get_fonts() if font_name in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) - self._notifications = FadingText(font, (width, 40), (0, height - 40)) - self.help = HelpText(pygame.font.Font(mono, 16), width, height) - self.server_fps = 0 - self.frame = 0 - self.simulation_time = 0 - self._show_info = True - self._info_text = [] - self._server_clock = pygame.time.Clock() - - def on_world_tick(self, timestamp): - self._server_clock.tick() - self.server_fps = self._server_clock.get_fps() - self.frame = timestamp.frame - self.simulation_time = timestamp.elapsed_seconds - - def tick(self, world, clock): - self._notifications.tick(world, clock) - if not self._show_info: - return - t = world.player.get_transform() - v = world.player.get_velocity() - c = world.player.get_control() - compass = world.imu_sensor.compass - heading = 'N' if compass > 270.5 or compass < 89.5 else '' - heading += 'S' if 90.5 < compass < 269.5 else '' - heading += 'E' if 0.5 < compass < 179.5 else '' - heading += 'W' if 180.5 < compass < 359.5 else '' - colhist = world.collision_sensor.get_collision_history() - collision = [colhist[x + self.frame - 200] for x in range(0, 200)] - max_col = max(1.0, max(collision)) - collision = [x / max_col for x in collision] - vehicles = world.world.get_actors().filter('vehicle.*') - self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % world.map.name.split('/')[-1], - 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), - '', - 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), - u'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), - 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), - 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), - 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), - 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), - 'Height: % 18.0f m' % t.location.z, - ''] - if isinstance(c, carla.VehicleControl): - self._info_text += [ - ('Throttle:', c.throttle, 0.0, 1.0), - ('Steer:', c.steer, -1.0, 1.0), - ('Brake:', c.brake, 0.0, 1.0), - ('Reverse:', c.reverse), - ('Hand brake:', c.hand_brake), - ('Manual:', c.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] - elif isinstance(c, carla.WalkerControl): - self._info_text += [ - ('Speed:', c.speed, 0.0, 5.556), - ('Jump:', c.jump)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - if len(vehicles) > 1: - self._info_text += ['Nearby vehicles:'] - distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) - vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] - for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]): - if d > 200.0: - break - vehicle_type = get_actor_display_name(vehicle, truncate=22) - self._info_text.append('% 4dm %s' % (d, vehicle_type)) - - def toggle_info(self): - self._show_info = not self._show_info - - def notification(self, text, seconds=2.0): - self._notifications.set_text(text, seconds=seconds) - - def error(self, text): - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - f = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - def __init__(self, font, dim, pos): - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - """Helper class to handle text output using pygame""" - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.line_space = 18 - self.dim = (780, len(lines) * self.line_space + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, n * self.line_space)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - self._render = not self._render - - def render(self, display): - if self._render: - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - self.history = [] - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - history = collections.defaultdict(int) - for frame, intensity in self.history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self.hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) - self.history.append((event.frame, intensity)) - if len(self.history) > 4000: - self.history.pop(0) - - -# ============================================================================== -# -- LaneInvasionSensor -------------------------------------------------------- -# ============================================================================== - - -class LaneInvasionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - - # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor - if parent_actor.type_id.startswith("vehicle."): - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_invasion') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - - @staticmethod - def _on_invasion(weak_self, event): - self = weak_self() - if not self: - return - lane_types = set(x.type for x in event.crossed_lane_markings) - text = ['%r' % str(x).split()[-1] for x in lane_types] - self.hud.notification('Crossed line %s' % ' and '.join(text)) - - -# ============================================================================== -# -- GnssSensor ---------------------------------------------------------------- -# ============================================================================== - - -class GnssSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - - @staticmethod - def _on_gnss_event(weak_self, event): - self = weak_self() - if not self: - return - self.lat = event.latitude - self.lon = event.longitude - - -# ============================================================================== -# -- IMUSensor ----------------------------------------------------------------- -# ============================================================================== - - -class IMUSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - self.accelerometer = (0.0, 0.0, 0.0) - self.gyroscope = (0.0, 0.0, 0.0) - self.compass = 0.0 - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.imu') - self.sensor = world.spawn_actor( - bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen( - lambda sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) - - @staticmethod - def _IMU_callback(weak_self, sensor_data): - self = weak_self() - if not self: - return - limits = (-99.9, 99.9) - self.accelerometer = ( - max(limits[0], min(limits[1], sensor_data.accelerometer.x)), - max(limits[0], min(limits[1], sensor_data.accelerometer.y)), - max(limits[0], min(limits[1], sensor_data.accelerometer.z))) - self.gyroscope = ( - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), - max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) - self.compass = math.degrees(sensor_data.compass) - - -# ============================================================================== -# -- RadarSensor --------------------------------------------------------------- -# ============================================================================== - - -class RadarSensor(object): - def __init__(self, parent_actor): - self.sensor = None - self._parent = parent_actor - bound_x = 0.5 + self._parent.bounding_box.extent.x - bound_y = 0.5 + self._parent.bounding_box.extent.y - bound_z = 0.5 + self._parent.bounding_box.extent.z - - self.velocity_range = 7.5 # m/s - world = self._parent.get_world() - self.debug = world.debug - bp = world.get_blueprint_library().find('sensor.other.radar') - bp.set_attribute('horizontal_fov', str(35)) - bp.set_attribute('vertical_fov', str(20)) - self.sensor = world.spawn_actor( - bp, - carla.Transform( - carla.Location(x=bound_x + 0.05, z=bound_z+0.05), - carla.Rotation(pitch=5)), - attach_to=self._parent) - # We need a weak reference to self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen( - lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) - - @staticmethod - def _Radar_callback(weak_self, radar_data): - self = weak_self() - if not self: - return - # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: - # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) - # points = np.reshape(points, (len(radar_data), 4)) - - current_rot = radar_data.transform.rotation - for detect in radar_data: - azi = math.degrees(detect.azimuth) - alt = math.degrees(detect.altitude) - # The 0.25 adjusts a bit the distance so the dots can - # be properly seen - fw_vec = carla.Vector3D(x=detect.depth - 0.25) - carla.Transform( - carla.Location(), - carla.Rotation( - pitch=current_rot.pitch + alt, - yaw=current_rot.yaw + azi, - roll=current_rot.roll)).transform(fw_vec) - - def clamp(min_v, max_v, value): - return max(min_v, min(value, max_v)) - - norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] - r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) - g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) - b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) - self.debug.draw_point( - radar_data.transform.location + fw_vec, - size=0.075, - life_time=0.06, - persistent_lines=False, - color=carla.Color(r, g, b)) - -# ============================================================================== -# -- CameraManager ------------------------------------------------------------- -# ============================================================================== - - -class CameraManager(object): - def __init__(self, parent_actor, hud, gamma_correction): - self.sensor = None - self.surface = None - self._parent = parent_actor - self.hud = hud - self.recording = False - bound_x = 0.5 + self._parent.bounding_box.extent.x - bound_y = 0.5 + self._parent.bounding_box.extent.y - bound_z = 0.5 + self._parent.bounding_box.extent.z - Attachment = carla.AttachmentType - - if not self._parent.type_id.startswith("walker.pedestrian"): - self._camera_transforms = [ - (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), - (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] - else: - self._camera_transforms = [ - (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), - (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), - (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] - - self.transform_index = 1 - self.sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)', {}], - ['sensor.camera.instance_segmentation', cc.CityScapesPalette, 'Camera Instance Segmentation (CityScapes Palette)', {}], - ['sensor.camera.instance_segmentation', cc.Raw, 'Camera Instance Segmentation (Raw)', {}], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], - ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], - ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', - {'lens_circle_multiplier': '3.0', - 'lens_circle_falloff': '3.0', - 'chromatic_aberration_intensity': '0.5', - 'chromatic_aberration_offset': '0'}], - ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], - ] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - bp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - bp.set_attribute('image_size_x', str(hud.dim[0])) - bp.set_attribute('image_size_y', str(hud.dim[1])) - if bp.has_attribute('gamma'): - bp.set_attribute('gamma', str(gamma_correction)) - for attr_name, attr_value in item[3].items(): - bp.set_attribute(attr_name, attr_value) - elif item[0].startswith('sensor.lidar'): - self.lidar_range = 50 - - for attr_name, attr_value in item[3].items(): - bp.set_attribute(attr_name, attr_value) - if attr_name == 'range': - self.lidar_range = float(attr_value) - - item.append(bp) - self.index = None - - def toggle_camera(self): - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - index = index % len(self.sensors) - needs_respawn = True if self.index is None else \ - (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self.surface = None - self.sensor = self._parent.get_world().spawn_actor( - self.sensors[index][-1], - self._camera_transforms[self.transform_index][0], - attach_to=self._parent, - attachment_type=self._camera_transforms[self.transform_index][1]) - # We need to pass the lambda a weak reference to self to avoid - # circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - self.hud.notification(self.sensors[index][2]) - self.index = index - - def next_sensor(self): - self.set_sensor(self.index + 1) - - def toggle_recording(self): - self.recording = not self.recording - self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) - - def render(self, display): - if self.surface is not None: - display.blit(self.surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self.sensors[self.index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0] / 4), 4)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) - lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) - lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) - lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): - # Example of converting the raw_data from a carla.DVSEventArray - # sensor into a NumPy array and using it as an image - dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ - ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) - dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) - # Blue is positive, red is negative - dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 - self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) - elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): - image = image.get_color_coded_flow() - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - else: - image.convert(self.sensors[self.index][1]) - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if self.recording: - image.save_to_disk('_out/%08d' % image.frame) - - -# ============================================================================== -# -- game_loop() --------------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - pygame.init() - pygame.font.init() - world = None - original_settings = None - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(20.0) - - sim_world = client.get_world() - if args.sync: - original_settings = sim_world.get_settings() - settings = sim_world.get_settings() - if not settings.synchronous_mode: - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - sim_world.apply_settings(settings) - - traffic_manager = client.get_trafficmanager() - traffic_manager.set_synchronous_mode(True) - - if args.autopilot and not sim_world.get_settings().synchronous_mode: - print("WARNING: You are currently in asynchronous mode and could " - "experience some issues with the traffic simulation") - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - display.fill((0,0,0)) - pygame.display.flip() - - hud = HUD(args.width, args.height) - world = World(sim_world, hud, args) - controller = KeyboardControl(world, args.autopilot) - - if args.sync: - sim_world.tick() - else: - sim_world.wait_for_tick() - - clock = pygame.time.Clock() - while True: - if args.sync: - sim_world.tick() - clock.tick_busy_loop(60) - if controller.parse_events(client, world, clock, args.sync): - return - world.tick(clock) - world.render(display) - pygame.display.flip() - - finally: - - if original_settings: - sim_world.apply_settings(original_settings) - - if (world and world.recording_enabled): - client.stop_recorder() - - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------------- -# ============================================================================== - - -def main(): - argparser = argparse.ArgumentParser( - description='CARLA Manual Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '-a', '--autopilot', - action='store_true', - help='enable autopilot') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='window resolution (default: 1280x720)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='actor filter (default: "vehicle.*")') - argparser.add_argument( - '--generation', - metavar='G', - default='2', - help='restrict to certain actor generation (values: "1","2","All" - default: "2")') - argparser.add_argument( - '--rolename', - metavar='NAME', - default='hero', - help='actor role name (default: "hero")') - argparser.add_argument( - '--gamma', - default=2.2, - type=float, - help='Gamma correction of the camera (default: 2.2)') - argparser.add_argument( - '--sync', - action='store_true', - help='Activate synchronous mode execution') - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - - main() diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py deleted file mode 100644 index 120f90b0..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous.py +++ /dev/null @@ -1,224 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - -import random - -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False): - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, (0, 0)) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - display = pygame.display.set_mode( - (800, 600), - pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - #start_pose = random.choice(m.get_spawn_points()) - start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: - while True: - if should_quit(): - return - - print('----------------') - print(vehicle.get_transform()) - #vehicle.set_target_velocity(carla.Vector3D(10, 10, 10)) - #vehicle.set_autopilot(True) - #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=1.0, brake=float(0.0))) - #vehicle.apply_control(carla.VehicleControl(0.75)) - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) - - vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) - # Choose the next waypoint and update the car location. - #print(waypoint) - - print(vehicle.get_transform()) - print(vehicle.get_control()) - print('----------------') - #waypoint = waypoint.next(1.5) - #print(waypoint) - #waypoint = random.choice(waypoint.next(1.5)) - #vehicle.set_transform(waypoint.transform) - #vehicle.set_transform(vehicle.get_transform()) - - #image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - pygame.display.flip() - - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py b/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py deleted file mode 100644 index ccbbe6d3..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_carla_0_9_13_synchronous_basic.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - -import random - -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False): - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, (0, 0)) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - display = pygame.display.set_mode( - (800, 600), - pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - #start_pose = random.choice(m.get_spawn_points()) - start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: - while True: - if should_quit(): - return - - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) - vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) - - #image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - pygame.display.flip() - - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py deleted file mode 100644 index faa43ac7..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye.py +++ /dev/null @@ -1,404 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 -import time -import random -import math -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import tensorflow as tf -gpus = tf.config.experimental.list_physical_devices('GPU') -for gpu in gpus: - tf.config.experimental.set_memory_growth(gpu, True) - -import cv2 -from albumentations import ( - Compose, Normalize -) -from gradcam import GradCAM - -mean_step_time = [] - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False, location=(0,0), is_black_space=False): - try: - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - except: - if is_black_space: - array = image - else: - array = image - array = cv2.resize(array, (200, 600)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, location) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - #display = pygame.display.set_mode((800*2+200, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - display = pygame.display.set_mode((800+400, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - start_pose = random.choice(m.get_spawn_points()) - - # Town 1 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=280, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=2.0, y=200, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 2 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=-4, y=240, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 3 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) - - # Town 04 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 05 - #start_pose = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - - # Town 06 - #start_pose = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 7 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) - #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) - start_pose = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - - - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - #birdview_producer = BirdViewProducer( - # client, # carla.Client - # target_size=PixelDimensions(width=100, height=300), - # pixels_per_meter=10, - # crop_type=BirdViewCropType.FRONT_AND_REAR_AREA - #) - - birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AREA_ONLY - ) - PRETRAINED_MODELS = "../../../" - #model = "20221017-144828_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_cp.h5" - model = "20221019-140458_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_both_directions_cp.h5" - #model = "20221019-155549_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_both_directions_more_cp.h5" - model = "20221021-095950_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_cp.h5" - #model = "20221021-114545_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_extreme_cp.h5" - model = "20221021-140015_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_cp.h5" - model = "20221021-143934_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_cp.h5" - model = "20221021-150901_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_cp.h5" - model = "20221021-154936_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_cp.h5" # BEST MODEL! - - - model = "20221031-093726_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_towns_cp.h5" - model = "20221031-171738_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_towns_3_5_cp.h5" - - model = "20221031-175645_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_towns_3_5_7_cp.h5" - - net = tf.keras.models.load_model(PRETRAINED_MODELS + model) - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=10) as sync_mode: - while True: - if should_quit(): - return - start = time.time() - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) - - birdview = birdview_producer.produce( - agent_vehicle=vehicle # carla.Actor (spawned vehicle) - ) - - image = BirdViewProducer.as_rgb(birdview) - image_shape=(50, 150) - img_base = cv2.resize(image, image_shape) - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img_base) - img = image["image"] - - - img = np.expand_dims(img, axis=0) - prediction = net.predict(img) - throttle = prediction[0][0] - steer = prediction[0][1] * (1 - (-1)) + (-1) - break_command = prediction[0][2] - speed = vehicle.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - acceleration = vehicle.get_acceleration() - vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) - vehicle_location = vehicle.get_location() - #print(prediction) - - #vehicle.set_autopilot(True) - - ''' - if vehicle_speed > 20: - #print('SLOW DOWN!') - vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=steer, brake=float(1.0))) - else: - if vehicle_speed > 20 and break_command > float(0.01): - #print('1') - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) - else: - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - ''' - if vehicle_speed > 20: - #print('SLOW DOWN!') - vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=steer, brake=float(1.0))) - else: - if vehicle_speed < 5: - vehicle.apply_control(carla.VehicleControl(throttle=float(1.0), steer=steer, brake=float(0.0))) - elif vehicle_speed > 20 and break_command > float(0.01): - #print('1') - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) - else: - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - - - #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) - #print(throttle, steer, break_command) - #print(vehicle.get_control()) - - i = np.argmax(prediction[0]) - cam = GradCAM(net, i) - heatmap = cam.compute_heatmap(img) - heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) - - #print(original_image.shape) - #print(resized_image.shape) - #print(heatmap.shape) - (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) - - image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True, location=(800,0)) - #draw_image(display, img_base, blend=False, location=(1600,0)) - draw_image(display, img_base, blend=False, location=(800,0)) - draw_image(display, output, blend=False, location=(1000,0)) - draw_image(display, np.zeros((160,300, 3)), blend=False, location=(0,0), is_black_space=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - - if vehicle_speed > 20: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 0, 0)), - (22, 46)) - else: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), - (22, 46)) - if mean_step_time != []: - display.blit( - font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), - (22, 64)) - if vehicle_acceleration > 10.0: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), - (22, 82)) - else: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), - (22, 82)) - - display.blit( - font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), - (22, 100)) - - display.blit( - font.render('Position: X=' + str(round(vehicle_location.x, 2)) + ' Y= ' + str(round(vehicle_location.y, 2)), True, (255, 255, 255)), - (22, 118)) - - display.blit( - font.render('World: ' + str(m.name), True, (255, 255, 255)), - (22, 136)) - - pygame.display.flip() - end = time.time() - mean_step_time.append(end - start) - #print(sum(mean_step_time) / len(mean_step_time)) - #print(end - start) - #time.sleep(0.5) - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py deleted file mode 100644 index 74329a69..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity.py +++ /dev/null @@ -1,433 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 -import time -import random -import math -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import tensorflow as tf -gpus = tf.config.experimental.list_physical_devices('GPU') -for gpu in gpus: - tf.config.experimental.set_memory_growth(gpu, True) - -import cv2 -from albumentations import ( - Compose, Normalize -) -from gradcam import GradCAM - -mean_step_time = [] - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False, location=(0,0), is_black_space=False): - try: - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - except: - if is_black_space: - array = image - else: - array = image - array = cv2.resize(array, (200, 600)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, location) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - #display = pygame.display.set_mode((800*2+200, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - display = pygame.display.set_mode((800+400, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - - # Town 1 - #start_pose = random.choice(m.get_spawn_points()) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=280, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - start_pose = carla.Transform(carla.Location(x=2.0, y=40, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - - # Town 2 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=-4, y=240, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 3 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) - - # Town 4 - # clockwise - #start_pose = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 05 - #start_pose = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - - # Town 06 - #start_pose = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 7 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=10, roll=0)) - - - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - #birdview_producer = BirdViewProducer( - # client, # carla.Client - # target_size=PixelDimensions(width=100, height=300), - # pixels_per_meter=10, - # crop_type=BirdViewCropType.FRONT_AND_REAR_AREA - #) - - birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AREA_ONLY - ) - PRETRAINED_MODELS = "../models/" - model = "20221026-133123_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_cp.h5" - model = "20221031-125557_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_cp.h5" - model = "20221031-182622_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_cp.h5" # BEST - #model = "20221104-101558_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_more_cp.h5" - model = "20221104-112038_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_extreme_cp.h5" - model = "20221104-120121_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_only_extreme_cp.h5" - model = "20221104-120359_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_only_clockwise_cp.h5" - model = "20221104-121137_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_only_clockwise_cp.h5" - model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp_3.h5" # BEST! - model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp.h5" # BEST! - #model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp_2.h5" - model = "20221104-124737_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01__cp.h5" # WORKS!!! - model = "20221104-143528_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_30_cp.h5" # WORKS! - - #model = "20221104-151236_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_all_towns_vel_MAX_cp.h5" - - #model = "20221107-111832_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_velocity_town_01_extreme_vel_MAX_cp.h5" # Works for town01 - - #model = "20221107-143505_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_input_output_velocity_all_towns_and_extreme_vel_MAX_cp.h5" - - #model = "20221108-155441_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_input_output_velocity_town_01_and_extreme_vel_MAX_cp.h5" - #model = "20221108-165842_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_prev_velocity_town_01_and_extreme_vel_MAX_cp.h5" - - net = tf.keras.models.load_model(PRETRAINED_MODELS + model) - - - previous_speed = 0 - previous_location_x = None - previous_location_y = None - total_distance = 0 - - previous_simulated_time = None - start_simulation_time = world.get_snapshot().timestamp.elapsed_seconds - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=15) as sync_mode: - while True: - if should_quit(): - return - start = time.time() - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) - - birdview = birdview_producer.produce( - agent_vehicle=vehicle # carla.Actor (spawned vehicle) - ) - - image = BirdViewProducer.as_rgb(birdview) - image_shape=(50, 150) - img_base = cv2.resize(image, image_shape) - - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img_base) - img = image["image"] - - #velocity_dim = np.full((150, 50), 0.5) - velocity_dim = np.full((150, 50), previous_speed/30) - #print(velocity_dim) - new_img_vel = np.dstack((img, velocity_dim)) - img = new_img_vel - - img = np.expand_dims(img, axis=0) - print(img.shape) - prediction = net.predict(img) - throttle = prediction[0][0] - steer = prediction[0][1] * (1 - (-1)) + (-1) - break_command = prediction[0][2] - speed = vehicle.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - previous_speed = vehicle_speed - acceleration = vehicle.get_acceleration() - vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) - vehicle_location = vehicle.get_location() - print(prediction) - - #vehicle.set_autopilot(True) - - if vehicle_speed > 200: - #print('SLOW DOWN!') - vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=(steer), brake=float(1.0))) - else: - if vehicle_speed < 2: - vehicle.apply_control(carla.VehicleControl(throttle=float(1.0), steer=0.0, brake=float(0.0))) - elif vehicle_speed > 5 and break_command > float(0.001): - print('1') - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - else: - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - - #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) - #print(throttle, steer, break_command) - #print(vehicle.get_control()) - - if previous_location_x is not None and previous_location_y is not None: - dist = math.sqrt( (round(vehicle_location.x, 2) - previous_location_x)**2 + (round(vehicle_location.y, 2) - previous_location_y)**2 ) - total_distance += dist - previous_location_x = round(vehicle_location.x, 2) - previous_location_y = round(vehicle_location.y, 2) - - - print(world.get_snapshot().timestamp.elapsed_seconds-start_simulation_time) - - i = np.argmax(prediction[0]) - cam = GradCAM(net, i) - heatmap = cam.compute_heatmap(img) - heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) - - #print(original_image.shape) - #print(resized_image.shape) - #print(heatmap.shape) - (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) - - image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True, location=(800,0)) - #draw_image(display, img_base, blend=False, location=(1600,0)) - draw_image(display, img_base, blend=False, location=(800,0)) - draw_image(display, output, blend=False, location=(1000,0)) - draw_image(display, np.zeros((200,300, 3)), blend=False, location=(0,0), is_black_space=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - - if vehicle_speed > 30: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' km/h', True, (255, 0, 0)), - (22, 46)) - else: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' km/h', True, (255, 255, 255)), - (22, 46)) - if mean_step_time != []: - display.blit( - font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), - (22, 64)) - if vehicle_acceleration > 10.0: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), - (22, 82)) - else: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), - (22, 82)) - - display.blit( - font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), - (22, 100)) - - display.blit( - font.render('Position: X=' + str(round(vehicle_location.x, 2)) + ' Y= ' + str(round(vehicle_location.y, 2)), True, (255, 255, 255)), - (22, 118)) - - display.blit( - font.render('World: ' + str(m.name), True, (255, 255, 255)), - (22, 136)) - - display.blit( - font.render('Total distance: ' + str(round(total_distance, 2)) + ' m', True, (255, 255, 255)), - (22, 154)) - - display.blit( - font.render('Total simulated time: ' + str(round(world.get_snapshot().timestamp.elapsed_seconds-start_simulation_time, 2)) + ' s', True, (255, 255, 255)), - (22, 172)) - - pygame.display.flip() - end = time.time() - mean_step_time.append(end - start) - #print(sum(mean_step_time) / len(mean_step_time)) - #print(end - start) - #time.sleep(0.5) - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py deleted file mode 100644 index c3620915..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_bird_eye_velocity_and_command.py +++ /dev/null @@ -1,395 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 -import time -import random -import math -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import tensorflow as tf -gpus = tf.config.experimental.list_physical_devices('GPU') -for gpu in gpus: - tf.config.experimental.set_memory_growth(gpu, True) - -import cv2 -from albumentations import ( - Compose, Normalize -) -from gradcam import GradCAM - -mean_step_time = [] - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False, location=(0,0), is_black_space=False): - try: - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - except: - if is_black_space: - array = image - else: - array = image - array = cv2.resize(array, (200, 600)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, location) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - #display = pygame.display.set_mode((800*2+200, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - display = pygame.display.set_mode((800+400, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - - # Town 1 - #start_pose = random.choice(m.get_spawn_points()) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=280, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - start_pose = carla.Transform(carla.Location(x=2.0, y=40, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - - # Town 2 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=-4, y=240, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 3 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=13.2, y=208, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=13.2, y=194, z=1.37), carla.Rotation(pitch=0, yaw=180, roll=0)) - - # Town 4 - # clockwise - #start_pose = carla.Transform(carla.Location(x=16.6, y=195.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=14.5, y=-209.4, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 05 - #start_pose = carla.Transform(carla.Location(x=51.2, y=-186.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - - # Town 06 - #start_pose = carla.Transform(carla.Location(x=672.4, y=112.6, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - - # Town 7 - # anticlockwise - #start_pose = carla.Transform(carla.Location(x=14.0, y=63.0, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - #start_pose = carla.Transform(carla.Location(x=72.3, y=-7.2, z=1.37), carla.Rotation(pitch=0, yaw=-60, roll=0)) - # clockwise - #start_pose = carla.Transform(carla.Location(x=14.3, y=-237.3, z=1.37), carla.Rotation(pitch=0, yaw=10, roll=0)) - - - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - #birdview_producer = BirdViewProducer( - # client, # carla.Client - # target_size=PixelDimensions(width=100, height=300), - # pixels_per_meter=10, - # crop_type=BirdViewCropType.FRONT_AND_REAR_AREA - #) - - birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AREA_ONLY - ) - PRETRAINED_MODELS = "../../../" - - model = "20221109-111804_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_prev_velocity_and_command_town_01_cp.h5" - - net = tf.keras.models.load_model(PRETRAINED_MODELS + model) - - - previous_speed = 0 - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=15) as sync_mode: - while True: - if should_quit(): - return - start = time.time() - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) - - birdview = birdview_producer.produce( - agent_vehicle=vehicle # carla.Actor (spawned vehicle) - ) - - image = BirdViewProducer.as_rgb(birdview) - image_shape=(50, 150) - img_base = cv2.resize(image, image_shape) - - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img_base) - img = image["image"] - - velocity_dim = np.full((150, 50), previous_speed/30) - # Straight command! - command_dim = np.full((150, 50), 1) - new_img_vel = np.dstack((img, velocity_dim)) - new_img_vel_cmd = np.dstack((new_img_vel, command_dim)) - img = new_img_vel_cmd - - img = np.expand_dims(img, axis=0) - print(img.shape) - prediction = net.predict(img) - throttle = prediction[0][0] - steer = prediction[0][1] * (1 - (-1)) + (-1) - break_command = prediction[0][2] - speed = vehicle.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - previous_speed = vehicle_speed - acceleration = vehicle.get_acceleration() - vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) - vehicle_location = vehicle.get_location() - print(prediction) - - #vehicle.set_autopilot(True) - - if vehicle_speed > 200: - #print('SLOW DOWN!') - vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=(steer), brake=float(1.0))) - else: - if vehicle_speed < 2: - vehicle.apply_control(carla.VehicleControl(throttle=float(1.0), steer=0.0, brake=float(0.0))) - elif vehicle_speed > 5 and break_command > float(0.001): - print('1') - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - else: - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - - #vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0, brake=float(0.0))) - #print(throttle, steer, break_command) - #print(vehicle.get_control()) - - i = np.argmax(prediction[0]) - cam = GradCAM(net, i) - heatmap = cam.compute_heatmap(img) - heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) - - #print(original_image.shape) - #print(resized_image.shape) - #print(heatmap.shape) - (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) - - image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True, location=(800,0)) - #draw_image(display, img_base, blend=False, location=(1600,0)) - draw_image(display, img_base, blend=False, location=(800,0)) - draw_image(display, output, blend=False, location=(1000,0)) - draw_image(display, np.zeros((160,300, 3)), blend=False, location=(0,0), is_black_space=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - - if vehicle_speed > 30: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/S', True, (255, 0, 0)), - (22, 46)) - else: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), - (22, 46)) - if mean_step_time != []: - display.blit( - font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), - (22, 64)) - if vehicle_acceleration > 10.0: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), - (22, 82)) - else: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), - (22, 82)) - - display.blit( - font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), - (22, 100)) - - display.blit( - font.render('Position: X=' + str(round(vehicle_location.x, 2)) + ' Y= ' + str(round(vehicle_location.y, 2)), True, (255, 255, 255)), - (22, 118)) - - display.blit( - font.render('World: ' + str(m.name), True, (255, 255, 255)), - (22, 136)) - - #display.blit( - # font.render('Predicted previous speed: ' + str(prediction[0][3]*88), True, (255, 255, 255)), - # (22, 150)) - - pygame.display.flip() - end = time.time() - mean_step_time.append(end - start) - #print(sum(mean_step_time) / len(mean_step_time)) - #print(end - start) - #time.sleep(0.5) - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py deleted file mode 100644 index b9933526..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_move_tensorflow_full_image.py +++ /dev/null @@ -1,315 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 -import time -import random -import math -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import tensorflow as tf -gpus = tf.config.experimental.list_physical_devices('GPU') -for gpu in gpus: - tf.config.experimental.set_memory_growth(gpu, True) - -import cv2 -from albumentations import ( - Compose, Normalize -) -from gradcam import GradCAM - -mean_step_time = [] - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False, location=(0,0)): - try: - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - except: - array = image - array = cv2.resize(array, (800, 266)) - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, location) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - display = pygame.display.set_mode((800*2, 600),pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - start_pose = random.choice(m.get_spawn_points()) - # Town 2 - #start_pose = carla.Transform(carla.Location(x=-7.5, y=200, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - # Town 1 - #start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - start_pose = carla.Transform(carla.Location(x=-2.0, y=285, z=1.37), carla.Rotation(pitch=0, yaw=90, roll=0)) - #start_pose = carla.Transform(carla.Location(x=2.0, y=40, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=1.5, z=2.4)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - PRETRAINED_MODELS = "../../../" - model = "20221024-110444_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_no_sync_cp.h5" - model = "20221024-114401_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_cp.h5" - model = "20221024-123807_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_cp.h5" - model = "20221024-132531_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_extreme_cp.h5" #BEST! - #model = "20221026-161920_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_extreme_cp.h5" - model = "20221026-163028_pilotnet_CARLA_24_10_dataset_full_image_300_epochs_no_flip_3_output_more_data_no_sync_extreme_cp.h5" # Weather! - - net = tf.keras.models.load_model(PRETRAINED_MODELS + model) - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=5) as sync_mode: - while True: - if should_quit(): - return - start = time.time() - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=10.0) - - array = np.frombuffer(image_rgb.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image_rgb.height, image_rgb.width, 4)) - array = array[:,:,:3] - - image = array - image_shape=(150, 50) - image = image[300:600, 0:800] - #image = image[320:600, 0:800] - img_base = cv2.resize(image, image_shape) - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img_base) - img = image["image"] - - img = np.expand_dims(img, axis=0) - prediction = net.predict(img) - throttle = prediction[0][0] - steer = prediction[0][1] * (1 - (-1)) + (-1) - break_command = prediction[0][2] - speed = vehicle.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - acceleration = vehicle.get_acceleration() - vehicle_acceleration = 3.6 * math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2) - print(prediction) - - if vehicle_speed > 20: - print('SLOW DOWN!') - vehicle.apply_control(carla.VehicleControl(throttle=float(0), steer=steer, brake=float(1.0))) - else: - if vehicle_speed > 20 and break_command > float(0.01): - #print('1') - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) - elif vehicle_speed < 5: - print('entra') - vehicle.apply_control(carla.VehicleControl(throttle=float(0.5), steer=steer, brake=float(0.0))) - else: - vehicle.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - - i = np.argmax(prediction[0]) - cam = GradCAM(net, i) - heatmap = cam.compute_heatmap(img) - heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) - - #print(original_image.shape) - #print(resized_image.shape) - #print(heatmap.shape) - (heatmap, output) = cam.overlay_heatmap(heatmap, img_base, alpha=0.5) - - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - draw_image(display, img_base, blend=False, location=(800,0)) - draw_image(display, output, blend=False, location=(800,266)) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - - if vehicle_speed > 20: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/S', True, (255, 0, 0)), - (22, 46)) - else: - display.blit( - font.render('Speed: ' + str(round(vehicle_speed, 2)) + ' m/s', True, (255, 255, 255)), - (22, 46)) - if mean_step_time != []: - display.blit( - font.render('Mean step time: ' + str(round(sum(mean_step_time) / len(mean_step_time), 3)), True, (255, 255, 255)), - (22, 64)) - if vehicle_acceleration > 10.0: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 0, 0)), - (22, 82)) - else: - display.blit( - font.render('Acceleration: ' + str(round(vehicle_acceleration, 2)) + ' m/s^2', True, (255, 255, 255)), - (22, 82)) - - display.blit( - font.render('Throttle: ' + str(round(throttle, 2)) + ' Steer: ' + str(round(steer, 2)) + ' Break: ' + str(round(break_command, 2)), True, (255, 255, 255)), - (22, 100)) - - pygame.display.flip() - end = time.time() - mean_step_time.append(end - start) - #print(sum(mean_step_time) / len(mean_step_time)) - #print(end - start) - #time.sleep(0.5) - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py b/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py deleted file mode 100644 index c9d2ea4d..00000000 --- a/PlayingWithCARLA/carla_0_9_13/launch_synchronous_and_record_dataset.py +++ /dev/null @@ -1,272 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - -import random - -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue -import csv -import math -import time - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick(10) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False): - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, (0, 0)) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - display = pygame.display.set_mode( - (800, 600), - pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - f = open('../../carla_dataset_test_24_10_anticlockwise_full_image_2/dataset.csv', 'a') - # create the csv writer - writer = csv.writer(f) - try: - df = pandas.read_csv('../../carla_dataset_test_24_10_anticlockwise_full_image_2/dataset.csv') - batch_number = int(df.iloc[-1]['batch']) + 1 - except Exception as ex: - #fkjfbdjkhfsd - batch_number = 0 - header = ['batch', 'image_id', 'timestamp', 'throttle', 'steer', 'brake', 'location_x', 'location_y'] - writer.writerow(header) - - print(batch_number) - frame_number = 0 - start = time.time() - - - try: - m = world.get_map() - #start_pose = random.choice(m.get_spawn_points()) - start_pose = carla.Transform(carla.Location(x=-2.0, y=307, z=0.1), carla.Rotation(pitch=0, yaw=90, roll=0)) - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - blueprint = world.get_blueprint_library().filter('vehicle')[0] - - - vehicle = world.spawn_actor( - blueprint, - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(True) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=1.5, z=2.4)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - # Remove traffic lights and traffic limits - traffic_lights = world.get_actors().filter('traffic.traffic_light') - traffic_speed_limits = world.get_actors().filter('traffic.speed_limit*') - print(traffic_speed_limits) - for traffic_light in traffic_lights: - #success = traffic_light.destroy() - traffic_light.set_green_time(20000) - traffic_light.set_state(carla.TrafficLightState.Green) - #print(success) - - for speed_limit in traffic_speed_limits: - success = speed_limit.destroy() - print(success) - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, fps=30) as sync_mode: - while True: - if should_quit(): - return - - #print('----------------') - #print(vehicle.get_transform()) - traffic_manager = client.get_trafficmanager() - vehicle.set_autopilot(True) - route = ["Straight", "Straight", "Straight", "Straight", "Straight", - "Straight", "Straight", "Straight", "Straight", "Straight", - "Straight", "Straight", "Straight", "Straight", "Straight", - "Straight", "Straight", "Straight", "Straight", "Straight", - "Straight", "Straight", "Straight", "Straight", "Straight", - "Straight", "Straight", "Straight", "Straight", "Straight"] - traffic_manager.set_route(vehicle, route) - clock.tick() - # Advance the simulation and wait for the data. - snapshot, image_rgb = sync_mode.tick(timeout=10.0) - - #print(vehicle.get_transform()) - #print(vehicle.get_control()) - #print('----------------') - - ############################################################ - vehicle_control = vehicle.get_control() - vehicle_location = vehicle.get_location() - speed = vehicle.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - - end = time.time() - - if (vehicle_speed > 0.0 and vehicle.get_location().z < 0.01 and (end - start) > 6): - print('entra') - #print(vehicle_speed) - #print(vehicle.get_transform()) - #image = image_queue.get() - image = image_rgb - cc = carla.ColorConverter.Raw - image.save_to_disk('../../carla_dataset_test_24_10_anticlockwise_full_image_2/' + str(batch_number) + '_' + str(frame_number) + '.png', cc) - # write a row to the csv file - row = [batch_number, str(batch_number) + '_' + str(frame_number) + '.png', time.time(), vehicle_control.throttle, vehicle_control.steer, vehicle_control.brake, vehicle_location.x, vehicle_location.y] - writer.writerow(row) - frame_number += 1 - #print(vehicle_control) - else: - #pass - print('nop') - ############################################################3 - - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - pygame.display.flip() - - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py b/PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py deleted file mode 100644 index b9bd5564..00000000 --- a/PlayingWithCARLA/carla_0_9_13/move_vehicle_tensorflow_bird_eye.py +++ /dev/null @@ -1,171 +0,0 @@ -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import pandas -import tensorflow as tf -import numpy as np - -from albumentations import ( - Compose, Normalize -) - - -#import tensorflow as tf -gpus = tf.config.experimental.list_physical_devices('GPU') -for gpu in gpus: - tf.config.experimental.set_memory_growth(gpu, True) - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - -birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=150, height=300), - pixels_per_meter=4, - crop_type=BirdViewCropType.FRONT_AND_REAR_AREA -) - -birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AND_REAR_AREA -) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - -PRETRAINED_MODELS = "../../../" -#model = "20220920-175541_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_cp.h5" -#model = "20220921-130758_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_only_extreme.h5" -#model = "20220921-154633_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_only_extreme_cp.h5" -#model = "20220921-173038_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_only_extreme_only_extreme_cp.h5" -#model = "20220928-144619_pilotnet_CARLA_extreme_cases_20_09_dataset_bird_eye_random_start_point.h5" -#model = "20220928-162449_pilotnet_CARLA_extreme_cases_28_09_dataset_bird_eye_random_start_point_300_epochs_cp.h5" -#model = "20220929-164843_pilotnet_CARLA_extreme_cases_29_09_dataset_bird_eye_random_start_point_retrained_cp.h5" -#model = "20220930-105720_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_cp.h5" -#model = "20220930-130349_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_retrained.h5" -#model = "20220930-153914_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_cp.h5" -#model = "20221003-131905_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_cp.h5" -''' -model = "20221003-160817_pilotnet_CARLA_28_09_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_cp.h5" -model = "20221004-180428_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_data_cp.h5" -model = "20221005-091607_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_data_cp.h5" -model_w = "20221005-105821_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_more_data_cp.h5" -model = "20221005-120932_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_2_output_more_data_cp.h5" - -model_v = "20221005-150027_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_V_more_data_cp.h5" - -model = "20221005-184041_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_more_data_cp.h5" - -model = "20221007-175421_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_more_data_extreme_cases_cp.h5" -''' -model = "20221007-182407_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_output_more_more_data_extreme_cases_cp.h5" -#model = "20221010-090523_xception_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_3_outputs_more_more_data_cp.h5" - -model = "20221017-110327_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_cp.h5" -model = "20221017-111655_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_cp.h5" -model = "20221017-113220_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_cp.h5" -model = "20221017-134410_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_extreme_cases_cp.h5" -model = "20221017-144828_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_more_extreme_cases_cp.h5" -model = "20221021-154936_pilotnet_CARLA_17_10_dataset_bird_eye_300_epochs_no_flip_3_output_both_directions_new_bird_eye_view_new_extreme_more_more_cp.h5" - - -#model_b = "20221010-102610_pilotnet_CARLA_04_10_dataset_bird_eye_random_start_point_300_epochs_no_flip_1_output_B_more_more_data_extreme_cases_cp.h5" -print('***********************************************************************************************') -#net_v = tf.keras.models.load_model(PRETRAINED_MODELS + model_v) -#net = tf.keras.models.load_model(PRETRAINED_MODELS + model_w) -#net_b = tf.keras.models.load_model(PRETRAINED_MODELS + model_b) - - -net = tf.keras.models.load_model(PRETRAINED_MODELS + model) -print('***********************************************************************************************') - -try: - while True: - #start = time.time() - - # Input for your model - call it every simulation step - # returned result is np.ndarray with ones and zeros of shape (8, height, width) - birdview = birdview_producer.produce( - agent_vehicle=car # carla.Actor (spawned vehicle) - ) - - image = BirdViewProducer.as_rgb(birdview) - #image_shape=(66, 200) - #image_shape=(200, 66) - image_shape=(50, 150) - #image_shape=(71, 150) - img = cv2.resize(image, image_shape) - - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img) - img = image["image"] - - - img = np.expand_dims(img, axis=0) - prediction = net.predict(img) - #print(prediction) - #prediction_v = net_v.predict(img) - #prediction_b = net_b.predict(img) - #print(prediction_v, prediction) - ''' - prediction_w = prediction[0][1] * (1 - (-1)) + (-1) - throttle = prediction[0][0] - steer = prediction_w - print(throttle, steer) - ''' - prediction_w = prediction[0][1] * (1 - (-1)) + (-1) - throttle = prediction[0][0] - break_command = prediction[0][2] - #if throttle < 0.3: - # throttle = 0.5 - #break_command = prediction[0][2] - steer = prediction_w - #Sprint(throttle, steer, break_command) - #print(throttle, steer) - #print(car.get_velocity().steer) - vehicle_control = car.get_control() - #print(vehicle_control.steer) - speed = car.get_velocity() - - #print(speed) - - if (abs(speed.x) > 6 or abs(speed.y) > 6): - #print('SLOW DOWN!') - car.apply_control(carla.VehicleControl(brake=float(1.0))) - else: - if (speed.x > 0.01 or speed.y > 0.01) and break_command > float(0.1): - #print('1') - car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) - else: - car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - - #car.apply_control(carla.VehicleControl(throttle=0.5, steer=steer, brake=float(0.0))) - #print(steer) - #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(break_command))) - #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer)) - #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer, brake=float(0.0))) - ##car.apply_control(carla.VehicleControl(throttle=1.0, steer=steer, brake=float(0.01))) - - #end = time.time() - #print(end) - #print(end - start) -except KeyboardInterrupt: - pass - -# close the file -f.close() - - diff --git a/PlayingWithCARLA/carla_0_9_13/record_dataset.py b/PlayingWithCARLA/carla_0_9_13/record_dataset.py deleted file mode 100644 index e95031b0..00000000 --- a/PlayingWithCARLA/carla_0_9_13/record_dataset.py +++ /dev/null @@ -1,92 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import pandas -import math - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -#camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -#image_queue = queue.Queue() -#camera.listen(image_queue.put) -last_image = '' -def listener(image): - global last_image - last_image = image -camera.listen(listener) - -#rgb camera -#image = image_queue.get() -# open the file in the write mode -f = open('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/dataset.csv', 'a') - -# create the csv writer -writer = csv.writer(f) - -#mypath = '../carla_dataset_14_09/' -#onlyfiles = [f for f in listdir(mypath) if isfile(join(mypath, f))] -#onlyfiles = sorted(onlyfiles) -#print(onlyfiles) -#print(onlyfiles[len(onlyfiles)-2]) -#print(onlyfiles[len(onlyfiles)-2][0]) -#print(type(onlyfiles[len(onlyfiles)-2][0])) - -try: - df = pandas.read_csv('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/dataset.csv') - batch_number = int(df.iloc[-1]['batch']) + 1 -except Exception as ex: - #ddddd - batch_number = 0 - header = ['batch', 'image_id', 'timestamp', 'throttle', 'steer', 'brake', 'location_x', 'location_y'] - writer.writerow(header) - -print(batch_number) -frame_number = 0 -try: - while True: - vehicle_control = car.get_control() - vehicle_location = car.get_location() - speed = car.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - ''' - print(type(image_queue)) - print(image_queue) - print(image_queue.qsize()) - ''' - if (vehicle_speed > 0.0 and last_image != ''): - #image = image_queue.get() - image = last_image - cc_raw = carla.ColorConverter.Raw - cc_seg = carla.ColorConverter.CityScapesPalette - image.save_to_disk('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/raw/' + str(batch_number) + '_' + str(frame_number) + '.png', cc_raw) - image.save_to_disk('../../carla_dataset_test_26_10_anticlockwise_full_image_segmentation/mask/' + str(batch_number) + '_' + str(frame_number) + '.png', cc_seg) - # write a row to the csv file - row = [batch_number, str(batch_number) + '_' + str(frame_number) + '.png', time.time(), vehicle_control.throttle, vehicle_control.steer, vehicle_control.brake, vehicle_location.x, vehicle_location.y] - writer.writerow(row) - frame_number += 1 - print(vehicle_control) - else: - pass - #print('nop') -except KeyboardInterrupt: - pass - -# close the file -f.close() - - diff --git a/PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py b/PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py deleted file mode 100644 index 99e760dd..00000000 --- a/PlayingWithCARLA/carla_0_9_13/record_dataset_bird_eye.py +++ /dev/null @@ -1,157 +0,0 @@ -from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import pandas -import time -import math - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - - -''' -birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AND_REAR_AREA -) -''' -birdview_producer = BirdViewProducer( - client, # carla.Client - target_size=PixelDimensions(width=100, height=300), - pixels_per_meter=10, - crop_type=BirdViewCropType.FRONT_AREA_ONLY -) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - - - -# open the file in the write mode -f = open('../../carla_dataset_control/carla_dataset_test_09_11_anticlockwise_town_01/dataset.csv', 'a') -# create the csv writer -writer = csv.writer(f) - -try: - df = pandas.read_csv('../../carla_dataset_control/carla_dataset_test_09_11_anticlockwise_town_01/dataset.csv') - batch_number = int(df.iloc[-1]['batch']) + 1 -except Exception as ex: - #fkjfbdjkhfsd - batch_number = 0 - header = ['batch', 'image_id', 'timestamp', 'throttle', 'steer', 'brake', 'location_x', 'location_y', 'previous_velocity', 'current_velocity', 'control_command'] - writer.writerow(header) - -print(batch_number) - -''' -car.apply_control(carla.VehicleControl(throttle=float(1.0), steer=0.0, brake=float(0.0))) -time.sleep(2) -car.apply_control(carla.VehicleControl(throttle=float(0.0), steer=0.0, brake=float(1.0))) -time.sleep(5) -car.apply_control(carla.VehicleControl(throttle=float(0.5), steer=0.1, brake=float(0.0))) -time.sleep(3) -car.set_autopilot(True) -''' -frame_number = 0 -previous_speed = 0 -#previous_image = 0 - -route = ["Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight"] -''' -route = ["Left", "Straight", "Right", "Right", "Straight", "Left", "Left", -"Right", "Right", "Left", "Straight", "Left"] -''' - -iterator = 0 -previous_waypoint_no_junction = True - -#import numpy as np -try: - while world.wait_for_tick(): - vehicle_control = car.get_control() - vehicle_location = car.get_location() - #print(car.get_location()) - #print(vehicle_control.steer) - - #if (vehicle_control.throttle > 0.1 and vehicle_control.throttle < 0.8 ) or (vehicle_control.brake > 0.2): - #if (vehicle_control.steer < -0.25 or vehicle_control.steer > 0.25): - if (vehicle_control.throttle > 0.0 or vehicle_control.steer > 0.0 or vehicle_control.brake > 0.0): - #print(frame_number) - #print(frame_number) - # Input for your model - call it every simulation step - # returned result is np.ndarray with ones and zeros of shape (8, height, width) - birdview = birdview_producer.produce( - agent_vehicle=car # carla.Actor (spawned vehicle) - ) - image = BirdViewProducer.as_rgb(birdview) - #if np.array_equal(previous_image, image): - # print(True) - #previous_image = image - #print(image.shape) - #print(type(image)) - cv2.imwrite('../../carla_dataset_control/carla_dataset_test_09_11_anticlockwise_town_01/' + str(batch_number) + '_' + str(frame_number) + '.png', image) - #image.save_to_disk('../../carla_dataset_21_09/' + str(batch_number) + '_' + str(frame_number) + '.png', cc) - #vehicle_control = car.get_vehicle_control() - #vehicle_control = car.get_control() - - - #print(vehicle_control) - - - # write a row to the csv file - speed = car.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - - waypoint = world.get_map().get_waypoint(car.get_location(),project_to_road=True, lane_type=(carla.LaneType.Driving | carla.LaneType.Shoulder | carla.LaneType.Sidewalk)) - if waypoint.is_junction and previous_waypoint_no_junction: - # first point in junction - previous_waypoint_no_junction = False - #print('---------------------') - #print('ENTRAMOS EN JUNCTION') - #print(iterator) - #print('Accion -> ', route[iterator]) - #print() - elif not waypoint.is_junction and not previous_waypoint_no_junction: - # last point in junction - previous_waypoint_no_junction = True - iterator += 1 - #print('SALIMOS DE JUNCTION') - #print(iterator) - #print('Siguiente junction -> ', route[iterator]) - #print('---------------------') - #print() - #print('Siguiente junction -> ', route[iterator]) - row = [batch_number, str(batch_number) + '_' + str(frame_number) + '.png', time.time(), vehicle_control.throttle, vehicle_control.steer, vehicle_control.brake, vehicle_location.x, vehicle_location.y, previous_speed, vehicle_speed, route[iterator]] - writer.writerow(row) - previous_speed = vehicle_speed - #print('----') - #print(previous_speed) - #print('----') - frame_number += 1 - else: - pass - #print('nop') - -except KeyboardInterrupt: - pass - -# close the file -f.close() - - diff --git a/PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py b/PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py deleted file mode 100644 index 3d6cd937..00000000 --- a/PlayingWithCARLA/carla_0_9_13/remove_traffic_lights_and_autopilot.py +++ /dev/null @@ -1,163 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -print(carla.__file__) - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) -time.sleep(2) - -traffic_lights = world.get_actors().filter('traffic.traffic_light') -traffic_speed_limits = world.get_actors().filter('traffic.speed_limit*') -print(traffic_speed_limits) -for traffic_light in traffic_lights: - #success = traffic_light.destroy() - traffic_light.set_green_time(20000) - traffic_light.set_state(carla.TrafficLightState.Green) - #print(success) - -for speed_limit in traffic_speed_limits: - success = speed_limit.destroy() - print(success) - -print(world.get_actors().filter('vehicle.*')) -car = world.get_actors().filter('vehicle.*')[0] - - -#settings = world.get_settings() -#settings.synchronous_mode = True # Enables synchronous mode -#world.apply_settings(settings) -traffic_manager = client.get_trafficmanager() -#random.seed(0) -#car.set_autopilot(True) -car.set_autopilot(True) - -''' -# ROUTE 0 -route = ["Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight"] - -# ROUTE 1 -route = ["Left", "Straight", "Right", "Right", "Straight", "Left", "Left", -"Right", "Right", "Left", "Straight", "Left"] -''' -# ROUTE 2 -route = ["Left", "Right", "Straight", "Right", "Right", "Straight", "Straight", "Right", "Left", "Left", "Right", "Right"] - -traffic_manager.set_route(car, route) -iterator = 0 -previous_waypoint_no_junction = True - -while True: - #traffic_manager.set_route(car, route) - #print(traffic_manager.get_next_action(car)) - #print(traffic_manager.get_all_actions(car)) - - waypoint = world.get_map().get_waypoint(car.get_location(),project_to_road=True, lane_type=(carla.LaneType.Driving | carla.LaneType.Shoulder | carla.LaneType.Sidewalk)) - #print(waypoint.is_junction) - if waypoint.is_junction and previous_waypoint_no_junction: - # first point in junction - previous_waypoint_no_junction = False - print('---------------------') - print('ENTRAMOS EN JUNCTION') - print(iterator) - print('Accion -> ', route[iterator]) - print() - elif not waypoint.is_junction and not previous_waypoint_no_junction: - # last point in junction - previous_waypoint_no_junction = True - iterator += 1 - print('SALIMOS DE JUNCTION') - print(iterator) - print('Siguiente junction -> ', route[iterator]) - print('---------------------') - print() - - #print(iterator) - -''' - print("Current lane type: " + str(waypoint.lane_type)) - # Check current lane change allowed - print("Current Lane change: " + str(waypoint.lane_change)) - # Left and Right lane markings - print("L lane marking type: " + str(waypoint.left_lane_marking.type)) - print("L lane marking change: " + str(waypoint.left_lane_marking.lane_change)) - print("R lane marking type: " + str(waypoint.right_lane_marking.type)) - print("R lane marking change: " + str(waypoint.right_lane_marking.lane_change)) - #location = car.get_location() - #print(type(location)) - #print(location.is_junction()) -''' - -''' -import math -while True: - speed = car.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - if (abs(vehicle_speed) > 5): - print('SLOW DOWN!') - car.apply_control(carla.VehicleControl(throttle=float(0), steer=float(0), brake=float(1.0))) - - -''' -''' -# Set up the TM in synchronous mode -traffic_manager = client.get_trafficmanager() -#traffic_manager.set_synchronous_mode(True) - -# Set a seed so behaviour can be repeated if necessary -#traffic_manager.set_random_device_seed(0) -#random.seed(0) -#car.set_autopilot(True) -car.set_autopilot(True) -route = ["Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight"] -traffic_manager.set_route(car, route) -#time.sleep(3) -#car.set_autopilot(False) -#print('autopilot false!') -''' -''' -car.apply_control(carla.VehicleControl(throttle=float(0.75), steer=-0.1, brake=float(0.0))) -time.sleep(3) -car.set_autopilot(True) -#traffic_manager.set_route(car, route) -''' - - -''' -while True: - print('entra') - world.tick() - - #traffic_manager.update_vehicle_lights(car, True) - #traffic_manager.random_left_lanechange_percentage(car, 0) - #traffic_manager.random_right_lanechange_percentage(car, 0) - #traffic_manager.auto_lane_change(car, False) - - world_map =world.get_map() - spawn_points = world_map.get_spawn_points() - - # Create route 1 from the chosen spawn points - route_1_indices = [129, 28, 124, 33, 97, 119, 58, 154, 147] - route_1 = [] - for ind in route_1_indices: - route_1.append(spawn_points[ind].location) - print(route_1) - traffic_manager.set_path(car, route_1) -''' - - diff --git a/PlayingWithCARLA/carla_0_9_13/review_images.py b/PlayingWithCARLA/carla_0_9_13/review_images.py deleted file mode 100644 index 903555a2..00000000 --- a/PlayingWithCARLA/carla_0_9_13/review_images.py +++ /dev/null @@ -1,23 +0,0 @@ -import glob -import os -import cv2 - -DIR_carla_dataset_name_images = '../../carla_dataset_28_09/' -carla_dataset_images = glob.glob(DIR_carla_dataset_name_images + '*') -array_imgs = [] -#print(carla_dataset_images) - -previous_image = 0 - -for iterator, filename in enumerate(carla_dataset_images): - try: - img = cv2.imread(filename) - img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) - #print((previous_image==img).all()) - if (previous_image==img).all() == True: - print(iterator) - previous_image = img - array_imgs.append(img) - except: - print('error') - diff --git a/PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py b/PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py deleted file mode 100644 index 1f02c947..00000000 --- a/PlayingWithCARLA/carla_0_9_13/synchronous_mode_test.py +++ /dev/null @@ -1,212 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('../carla/dist/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 - -import random - -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - -try: - import queue -except ImportError: - import Queue as queue - - -class CarlaSyncMode(object): - """ - Context manager to synchronize output from different sensors. Synchronous - mode is enabled as long as we are inside this context - with CarlaSyncMode(world, sensors) as sync_mode: - while True: - data = sync_mode.tick(timeout=1.0) - """ - - def __init__(self, world, *sensors, **kwargs): - self.world = world - self.sensors = sensors - self.frame = None - self.delta_seconds = 1.0 / kwargs.get('fps', 20) - self._queues = [] - self._settings = None - - def __enter__(self): - self._settings = self.world.get_settings() - self.frame = self.world.apply_settings(carla.WorldSettings( - no_rendering_mode=False, - synchronous_mode=True, - fixed_delta_seconds=self.delta_seconds)) - - def make_queue(register_event): - q = queue.Queue() - register_event(q.put) - self._queues.append(q) - - make_queue(self.world.on_tick) - for sensor in self.sensors: - make_queue(sensor.listen) - return self - - def tick(self, timeout): - self.frame = self.world.tick() - print(self.frame) - #print(self.frame.shape) - data = [self._retrieve_data(q, timeout) for q in self._queues] - assert all(x.frame == self.frame for x in data) - return data - - def __exit__(self, *args, **kwargs): - self.world.apply_settings(self._settings) - - def _retrieve_data(self, sensor_queue, timeout): - while True: - data = sensor_queue.get(timeout=timeout) - if data.frame == self.frame: - return data - - -def draw_image(surface, image, blend=False): - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, (0, 0)) - - -def get_font(): - fonts = [x for x in pygame.font.get_fonts()] - default_font = 'ubuntumono' - font = default_font if default_font in fonts else fonts[0] - font = pygame.font.match_font(font) - return pygame.font.Font(font, 14) - - -def should_quit(): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - elif event.type == pygame.KEYUP: - if event.key == pygame.K_ESCAPE: - return True - return False - - -def main(): - actor_list = [] - pygame.init() - - display = pygame.display.set_mode( - (800, 600), - pygame.HWSURFACE | pygame.DOUBLEBUF) - font = get_font() - clock = pygame.time.Clock() - - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - try: - m = world.get_map() - start_pose = random.choice(m.get_spawn_points()) - start_pose = carla.Transform(carla.Location(x=-2.0, y=5, z=0), carla.Rotation(pitch=0, yaw=90, roll=0)) - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - vehicle = world.spawn_actor( - world.get_blueprint_library().filter('vehicle')[0], - start_pose) - actor_list.append(vehicle) - vehicle.set_simulate_physics(False) - - camera_rgb = world.spawn_actor( - blueprint_library.find('sensor.camera.rgb'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find('sensor.camera.semantic_segmentation'), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle) - actor_list.append(camera_semseg) - - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: - while True: - if should_quit(): - return - clock.tick() - - # Advance the simulation and wait for the data. - #vehicle.set_autopilot(True) - vehicle.apply_control(carla.VehicleControl(throttle=10, steer=0.0)) - print(vehicle.get_velocity()) - world.tick() - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=2.0) - - # Choose the next waypoint and update the car location. - waypoint = random.choice(waypoint.next(1.5)) - vehicle.set_transform(waypoint.transform) - - image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - #draw_image(display, image_semseg, blend=True) - display.blit( - font.render('% 5d FPS (real)' % clock.get_fps(), True, (255, 255, 255)), - (8, 10)) - display.blit( - font.render('% 5d FPS (simulated)' % fps, True, (255, 255, 255)), - (8, 28)) - pygame.display.flip() - - finally: - - print('destroying actors.') - for actor in actor_list: - actor.destroy() - - pygame.quit() - print('done.') - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') \ No newline at end of file diff --git a/PlayingWithCARLA/carla_0_9_13/test.py b/PlayingWithCARLA/carla_0_9_13/test.py deleted file mode 100644 index c55e37ea..00000000 --- a/PlayingWithCARLA/carla_0_9_13/test.py +++ /dev/null @@ -1,174 +0,0 @@ - -import carla -import numpy as np -from carla.agents.navigation.basic_agent import BasicAgent - - -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') -try: - import queue -except ImportError: - import Queue as queue - - - -def draw_image(surface, image, blend=False): - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if blend: - image_surface.set_alpha(100) - surface.blit(image_surface, (0, 0)) - - -def main(): - # all the actors in the world. For destroying later. - actor_list = [] - pygame.init() - - # create client - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - # access world from client - world = client.get_world() - - # Enable synchronous mode - print('Enabling synchronous mode') - settings = world.get_settings() - settings.synchronous_mode = True - world.apply_settings(settings) - - try: - # set weather conditions - world.set_weather(carla.WeatherParameters.ClearNoon) - - # define location - map = world.get_map() - spawn_points = map.get_spawn_points() - hero_transform = spawn_points[97] - - # Get the van to spawn in front of the hero - # Get the waypoint of the hero, since the spawn points are only Transforms - hero_waypoint = map.get_waypoint(hero_transform.location) - - # Get the waypoint 15 meters in front of it - van_waypoint = hero_waypoint.next(15.0) - van_transform = van_waypoint[0].transform - - # spawn higher or it will get stuck - van_transform.location.z += 0.5 - - # get all the blueprints in this world - blueprint_library = world.get_blueprint_library() - # define the blueprint of hero vehicle - prius_bp = blueprint_library.find('vehicle.toyota.prius') - white = '255,255,255' - prius_bp.set_attribute('color', white) - - # blueprint colavan - colavan_bp = blueprint_library.find('vehicle.carlamotors.carlacola') - - # spawn our hero - hero = world.spawn_actor(prius_bp, hero_transform) - # add actor to the list for destruction, otherwise vehicle is stuck in there forever - actor_list.append(hero) - - # spawn van - colavan = world.spawn_actor(colavan_bp, van_transform) - actor_list.append(colavan) - - # add a camera - camera_class = blueprint_library.find('sensor.camera.rgb') - camera_class.set_attribute('image_size_x', '600') - camera_class.set_attribute('image_size_y', '600') - camera_class.set_attribute('fov', '90') - camera_class.set_attribute('sensor_tick', '0.1') - cam_transform1 = carla.Transform(carla.Location(x=1.8, z=1.3)) - # cam_transform2 = cam_transform1 + carla.Location(y=0.54) - - # # spawn camera to hero - camera1 = world.spawn_actor(camera_class, cam_transform1, attach_to=hero) - actor_list.append(camera1) - # camera2 = world.spawn_actor(camera_class, cam_transform2, attach_to=hero) - # actor_list.append(camera2) - - # Makes a sync queue for the sensor data - image_queue1 = queue.Queue() - camera1.listen(image_queue1.put) - frame = None - - # image_queue2 = queue.Queue() - # camera2.listen(image_queue2.put) - - # Note its going to drive at consistent speed 15 km/h - # PID controller gets unstable after 15 km/h - #roaming_prius = BasicAgent(hero, target_speed=15) - #destiny = spawn_points[96].location - #roaming_prius.set_destination((destiny.x, destiny.y, destiny.z)) - - roaming_van = BasicAgent(colavan, target_speed=15) - #roaming_van.set_destination((destiny.x, destiny.y, destiny.z)) - - # If you want the hero to drive around in autopilot - hero.set_autopilot(True) - - display = pygame.display.set_mode((600, 600), pygame.HWSURFACE | pygame.DOUBLEBUF) - - # tracks time and frame rate management class. - clock = pygame.time.Clock() - - while True: - clock.tick() - world.tick() - ts = world.tick() - - # Get control commands - #control_hero = roaming_prius.run_step() - #hero.apply_control(control_hero) - - #control_van = roaming_van.run_step() - #colavan.apply_control(control_van) - - if frame is not None: - if ts != frame + 1: - print('frame skip!') - print("frame skip!") - print(ts) - frame = ts - - while True: - image1 = image_queue1.get() - print(image1) - # as long as the image number == frame count you are fine and this loop is not necessary - print("image1.frame_number: {} % ts: {}".format(image1.frame_number, ts)) - if image1.frame_number == ts: - break - print( - 'wrong image time-stampstamp: frame=%d, image.frame=%d', - ts, - image1.frame_number) - - draw_image(display, image1) - - # reset display - pygame.display.flip() - - finally: - print('Disabling synchronous mode') - settings = world.get_settings() - settings.synchronous_mode = False - world.apply_settings(settings) - - print('destroying actors') - for actor in actor_list: - actor.destroy() - pygame.quit() - print("pygame quit, done") - -main() \ No newline at end of file diff --git a/PlayingWithCARLA/carla_agent.py b/PlayingWithCARLA/carla_agent.py deleted file mode 100644 index 08352814..00000000 --- a/PlayingWithCARLA/carla_agent.py +++ /dev/null @@ -1,12 +0,0 @@ -from carla.agent.agent import Agent -from carla.client import VehicleControl - -class ForwardAgent(Agent): - - def run_step(self, measurements, sensor_data, directions, target): - """ - Function to run a control step in the CARLA vehicle. - """ - control = VehicleControl() - control.throttle = 0.9 - return control diff --git a/PlayingWithCARLA/carla_tutorial.py b/PlayingWithCARLA/carla_tutorial.py deleted file mode 100644 index 534905bc..00000000 --- a/PlayingWithCARLA/carla_tutorial.py +++ /dev/null @@ -1,55 +0,0 @@ -import glob -import os -import sys - -try: - sys.path.append(glob.glob('PythonAPI/carla/dist/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 -import random -import cv2 -import skimage.measure as measure - -#in synchronous mode, sensor data must be added to a queue -import queue - -client = carla.Client('localhost', 2000) -client.set_timeout(11.0) - -#print(client.get_available_maps()) -#world = client.load_world('Town03') -#settings = world.get_settings() -#settings.fixed_delta_seconds = 0.05 #must be less than 0.1, or else physics will be noisy -#must use fixed delta seconds and synchronous mode for python api controlled sim, or else -#camera and sensor data may not match simulation properly and will be noisy -#settings.synchronous_mode = True -#world.apply_settings(settings) - -actor_list = [] - -blueprint_library = client.get_world().get_blueprint_library() -bp = random.choice(blueprint_library.filter('vehicle')) # lets choose a vehicle at random - -# lets choose a random spawn point -transform = random.choice(client.get_world().get_map().get_spawn_points()) - -#spawn a vehicle -vehicle = client.get_world().try_spawn_actor(bp, transform) -actor_list.append(vehicle) - -vehicle.set_autopilot(True) - - - -print() - - - - - - diff --git a/PlayingWithCARLA/gradcam.py b/PlayingWithCARLA/gradcam.py deleted file mode 100644 index 55dffa10..00000000 --- a/PlayingWithCARLA/gradcam.py +++ /dev/null @@ -1,95 +0,0 @@ -from tensorflow.keras.models import Model -from tensorflow.keras.layers import Conv2D -import tensorflow as tf -import numpy as np -import cv2 - - -class GradCAM: - def __init__(self, model, class_idx, layer_name=None): - # store the model, the class index used to measure the class - # activation map, and the layer to be used when visualizing - # the class activation map - self.model = model - self.class_idx = class_idx - self.layerName = layer_name - # if the layer name is None, attempt to automatically find - # the target output layer - if self.layerName is None: - self.layerName = self.find_target_layer() - - def find_target_layer(self): - # attempt to find the final convolutional layer in the network - # by looping over the layers of the network in reverse order - for layer in reversed(self.model.layers): - # check to see if the layer has a 4D output - #if i > 6 and len(layer.output_shape) == 4: - if len(layer.output_shape) == 4 and type(layer) == Conv2D: - print(layer.name) - return layer.name - # otherwise, we could not find a 4D layer so the GradCAM - # algorithm cannot be applied - raise ValueError("Could not find 4D layer. Cannot apply GradCAM.") - - def compute_heatmap(self, image, eps=1e-8): - # construct our gradient model by supplying (1) the inputs - # to our pre-trained model, (2) the output of the (presumably) - # final 4D layer in the network, and (3) the output of the - # softmax activations from the model - grad_model = Model( - inputs=[self.model.inputs], - outputs=[self.model.get_layer(self.layerName).output, - self.model.output]) - - # record operations for automatic differentiation - with tf.GradientTape() as tape: - # cast the image tensor to a float-32 data type, pass the - # image through the gradient model, and grab the loss - # associated with the specific class index - inputs = tf.cast(image, tf.float32) - (conv_outputs, predictions) = grad_model(inputs) - #loss = predictions[:, self.class_idx] - loss = predictions[:, 1] - # use automatic differentiation to compute the gradients - grads = tape.gradient(loss, conv_outputs) - - # compute the guided gradients - cast_conv_outputs = tf.cast(conv_outputs > 0, "float32") - cast_grads = tf.cast(grads > 0, "float32") - guided_grads = cast_conv_outputs * cast_grads * grads - # the convolution and guided gradients have a batch dimension - # (which we don't need) so let's grab the volume itself and - # discard the batch - conv_outputs = conv_outputs[0] - guided_grads = guided_grads[0] - - # compute the average of the gradient values, and using them - # as weights, compute the ponderation of the filters with - # respect to the weights - weights = tf.reduce_mean(guided_grads, axis=(0, 1)) - cam = tf.reduce_sum(tf.multiply(weights, conv_outputs), axis=-1) - - # grab the spatial dimensions of the input image and resize - # the output class activation map to match the input image - # dimensions - (w, h) = (image.shape[2], image.shape[1]) - heatmap = cv2.resize(cam.numpy(), (w, h)) - # normalize the heatmap such that all values lie in the range - # [0, 1], scale the resulting values to the range [0, 255], - # and then convert to an unsigned 8-bit integer - numer = heatmap - np.min(heatmap) - denom = (heatmap.max() - heatmap.min()) + eps - heatmap = numer / denom - heatmap = (heatmap * 255).astype("uint8") - # return the resulting heatmap to the calling function - return heatmap - - def overlay_heatmap(self, heatmap, image, alpha=0.5, - colormap=cv2.COLORMAP_VIRIDIS): - # apply the supplied color map to the heatmap and then - # overlay the heatmap on the input image - heatmap = cv2.applyColorMap(heatmap, colormap) - output = cv2.addWeighted(image, alpha, heatmap, 1 - alpha, 0) - # return a 2-tuple of the color mapped heatmap and the output, - # overlaid image - return heatmap, output \ No newline at end of file diff --git a/PlayingWithCARLA/launch_carla.py b/PlayingWithCARLA/launch_carla.py deleted file mode 100644 index ef3dc9a8..00000000 --- a/PlayingWithCARLA/launch_carla.py +++ /dev/null @@ -1,653 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -# Allows controlling a vehicle with a keyboard. For a simpler and more -# documented example, please take a look at tutorial.py. - -""" -Welcome to CARLA manual control. - -Use ARROWS or WASD keys for control. - - W : throttle - S : brake - AD : steer - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - - TAB : change sensor position - ` : next sensor - [1-9] : change to sensor [1-9] - C : change weather (Shift+C reverse) - Backspace : change vehicle - - R : toggle recording images to disk - - F1 : toggle HUD - H/? : toggle help - ESC : quit -""" - -from __future__ import print_function - - -# ============================================================================== -# -- find carla module --------------------------------------------------------- -# ============================================================================== - - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) -except IndexError: - pass - - -# ============================================================================== -# -- imports ------------------------------------------------------------------- -# ============================================================================== - - -import carla - -from carla import ColorConverter as cc - -import argparse -import collections -import datetime -import logging -import math -import random -import re -import weakref - -try: - import pygame -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - -def get_actor_display_name(actor, truncate=250): - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate-1] + u'\u2026') if len(name) > truncate else name - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== - - -class World(object): - def __init__(self, carla_world, hud): - self.world = carla_world - self.hud = hud - 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): - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager._index if self.camera_manager is not None else 0 - cam_pos_index = self.camera_manager._transform_index if self.camera_manager is not None else 0 - # Get a random vehicle blueprint. - print('*****************************************************') - print(self.world.get_blueprint_library().filter('vehicle')) - print('*****************************************************') - print(self.world.get_blueprint_library().filter('vehicle')[0]) - print('*****************************************************') - blueprint = random.choice(self.world.get_blueprint_library().filter('vehicle')) - - blueprint = self.world.get_blueprint_library().filter('vehicle')[0] - - 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() - self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) - while self.vehicle is None: - print('************************ENTRA************************') - print(self.world.get_map().get_spawn_points()) - print('*****************************************************') - spawn_points = self.world.get_map().get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - print('*****************************************************') - spawn_point = self.world.get_map().get_spawn_points()[0] - print(spawn_point) - spawn_point = carla.Transform(carla.Location(x=193.6, y=181.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=181.3, z=1.37), carla.Rotation(pitch=0, yaw=-120, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=181.3, z=1.37), carla.Rotation(pitch=0, yaw=0, roll=0)) - - # position 2 - #spawn_point = carla.Transform(carla.Location(x=193.6, y=170.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=170.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) - - - # position 3 - #spawn_point = carla.Transform(carla.Location(x=193.6, y=150.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=150.3, z=1.37), carla.Rotation(pitch=0, yaw=-30, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=150.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) - - - # position 4 - #spawn_point = carla.Transform(carla.Location(x=193.6, y=130.3, z=1.37), carla.Rotation(pitch=0, yaw=-90, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=130.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) - #spawn_point = carla.Transform(carla.Location(x=193.6, y=130.3, z=1.37), carla.Rotation(pitch=0, yaw=-150, roll=0)) - - print(spawn_point) - print('*****************************************************') - self.vehicle = 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.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.vehicle) - self.hud.notification(actor_type) - - - def tick(self, clock): - self.hud.tick(self, clock) - - def render(self, display): - self.camera_manager.render(display) - #self.hud.render(display) - - def destroy(self): - 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() - - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - def __init__(self, width, height): - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - 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._show_info = False - 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, - 'Client: % 16d FPS' % clock.get_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) - - def error(self, text): - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - f = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - #self._notifications.render(display) - #self.help.render(display) - - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - def __init__(self, font, dim, pos): - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.dim = (680, len(lines) * 22 + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, n * 22)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - self._render = not self._render - - def render(self, display): - if self._render: - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - self._history = [] - self._parent = parent_actor - self._hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - history = collections.defaultdict(int) - for frame, intensity in self._history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self._hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) - self._history.append((event.frame_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 - 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): - self.sensor = None - self._surface = None - self._parent = parent_actor - self._hud = hud - self._recording = False - self._camera_transforms = [ - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - carla.Transform(carla.Location(x=1.6, z=1.7))] - self._transform_index = 1 - self._sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self._sensors: - bp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - bp.set_attribute('image_size_x', str(hud.dim[0])) - bp.set_attribute('image_size_y', str(hud.dim[1])) - elif item[0].startswith('sensor.lidar'): - bp.set_attribute('range', '5000') - item.append(bp) - self._index = None - - def toggle_camera(self): - self._transform_index = (self._transform_index + 1) % len(self._camera_transforms) - self.sensor.set_transform(self._camera_transforms[self._transform_index]) - - def set_sensor(self, index, notify=True): - index = index % len(self._sensors) - needs_respawn = True if self._index is None \ - else self._sensors[index][0] != self._sensors[self._index][0] - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self._surface = None - self.sensor = self._parent.get_world().spawn_actor( - self._sensors[index][-1], - self._camera_transforms[self._transform_index], - attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid - # circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - self._hud.notification(self._sensors[index][2]) - self._index = index - - def next_sensor(self): - self.set_sensor(self._index + 1) - - def toggle_recording(self): - self._recording = not self._recording - self._hud.notification('Recording %s' % ('On' if self._recording else 'Off')) - - def render(self, display): - if self._surface is not None: - display.blit(self._surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self._sensors[self._index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0]/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) - - -# ============================================================================== -# -- game_loop() --------------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - pygame.init() - pygame.font.init() - world = None - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(2.0) - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - - hud = HUD(args.width, args.height) - #hud = HUD(100, 100) - print('----------------------------------------------------------------') - print(client.get_world()) - print('----------------------------------------------------------------') - #client.load_world('Town01') - world = World(client.get_world(), hud) - #controller = KeyboardControl(world, args.autopilot) - - clock = pygame.time.Clock() - while True: - clock.tick_busy_loop(60) - #if controller.parse_events(world, clock): - # return - world.tick(clock) - world.render(display) - pygame.display.flip() - - finally: - - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------------- -# ============================================================================== - - -def main(): - argparser = argparse.ArgumentParser( - description='CARLA Manual Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '-a', '--autopilot', - action='store_true', - help='enable autopilot') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='window resolution (default: 1280x720)') - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - - main() diff --git a/PlayingWithCARLA/manual_control_modified.py b/PlayingWithCARLA/manual_control_modified.py deleted file mode 100755 index d29ce204..00000000 --- a/PlayingWithCARLA/manual_control_modified.py +++ /dev/null @@ -1,743 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -# Allows controlling a vehicle with a keyboard. For a simpler and more -# documented example, please take a look at tutorial.py. - -""" -Welcome to CARLA manual control. - -Use ARROWS or WASD keys for control. - - W : throttle - S : brake - AD : steer - Q : toggle reverse - Space : hand-brake - P : toggle autopilot - M : toggle manual transmission - ,/. : gear up/down - - TAB : change sensor position - ` : next sensor - [1-9] : change to sensor [1-9] - C : change weather (Shift+C reverse) - Backspace : change vehicle - - R : toggle recording images to disk - - F1 : toggle HUD - H/? : toggle help - ESC : quit -""" - -from __future__ import print_function - - -# ============================================================================== -# -- find carla module --------------------------------------------------------- -# ============================================================================== - - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('**/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) -except IndexError: - pass - - -# ============================================================================== -# -- imports ------------------------------------------------------------------- -# ============================================================================== - - -import carla - -from carla import ColorConverter as cc - -import argparse -import collections -import datetime -import logging -import math -import random -import re -import weakref - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import KMOD_SHIFT - from pygame.locals import K_0 - from pygame.locals import K_9 - from pygame.locals import K_BACKQUOTE - from pygame.locals import K_BACKSPACE - from pygame.locals import K_COMMA - from pygame.locals import K_DOWN - from pygame.locals import K_ESCAPE - from pygame.locals import K_F1 - from pygame.locals import K_LEFT - from pygame.locals import K_PERIOD - from pygame.locals import K_RIGHT - from pygame.locals import K_SLASH - from pygame.locals import K_SPACE - from pygame.locals import K_TAB - from pygame.locals import K_UP - from pygame.locals import K_a - from pygame.locals import K_c - from pygame.locals import K_d - from pygame.locals import K_h - from pygame.locals import K_m - from pygame.locals import K_p - from pygame.locals import K_q - from pygame.locals import K_r - from pygame.locals import K_s - from pygame.locals import K_w -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError('cannot import numpy, make sure numpy package is installed') - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def find_weather_presets(): - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] - - -def get_actor_display_name(actor, truncate=250): - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate-1] + u'\u2026') if len(name) > truncate else name - - -# ============================================================================== -# -- World --------------------------------------------------------------------- -# ============================================================================== - - -class World(object): - def __init__(self, carla_world, hud): - self.world = carla_world - self.hud = hud - 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): - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager._index if self.camera_manager is not None else 0 - cam_pos_index = self.camera_manager._transform_index if self.camera_manager is not None else 0 - # Get a random vehicle blueprint. - print('*****************************************************') - print(self.world.get_blueprint_library().filter('vehicle')) - print('*****************************************************') - print(self.world.get_blueprint_library().filter('vehicle')[0]) - print('*****************************************************') - blueprint = random.choice(self.world.get_blueprint_library().filter('vehicle')) - - blueprint = self.world.get_blueprint_library().filter('vehicle')[0] - - 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() - self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) - while self.vehicle is None: - print('************************ENTRA************************') - #print(self.world.get_map().get_spawn_points()) - print('*****************************************************') - spawn_points = self.world.get_map().get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - print('*****************************************************') - spawn_point = self.world.get_map().get_spawn_points()[0] - print(spawn_point) - print('*****************************************************') - self.vehicle = 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.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.vehicle) - self.hud.notification(actor_type) - - def next_weather(self, reverse=False): - self._weather_index += -1 if reverse else 1 - self._weather_index %= len(self._weather_presets) - preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.vehicle.get_world().set_weather(preset[0]) - - def tick(self, clock): - self.hud.tick(self, clock) - - def render(self, display): - self.camera_manager.render(display) - self.hud.render(display) - - def destroy(self): - 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() - - -# ============================================================================== -# -- 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 - world.vehicle.apply_control(self._control) - - 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) - - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - def __init__(self, width, height): - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - 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, - 'Client: % 16d FPS' % clock.get_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) - - def error(self, text): - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - f = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - def __init__(self, font, dim, pos): - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - def __init__(self, font, width, height): - lines = __doc__.split('\n') - self.font = font - self.dim = (680, len(lines) * 22 + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for n, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, n * 22)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - self._render = not self._render - - def render(self, display): - if self._render: - display.blit(self.surface, self.pos) - - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - def __init__(self, parent_actor, hud): - self.sensor = None - self._history = [] - self._parent = parent_actor - self._hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - history = collections.defaultdict(int) - for frame, intensity in self._history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self._hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) - self._history.append((event.frame_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 - 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): - self.sensor = None - self._surface = None - self._parent = parent_actor - self._hud = hud - self._recording = False - self._camera_transforms = [ - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - carla.Transform(carla.Location(x=1.6, z=1.7))] - self._transform_index = 1 - self._sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self._sensors: - bp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - bp.set_attribute('image_size_x', str(hud.dim[0])) - bp.set_attribute('image_size_y', str(hud.dim[1])) - elif item[0].startswith('sensor.lidar'): - bp.set_attribute('range', '5000') - item.append(bp) - self._index = None - - def toggle_camera(self): - self._transform_index = (self._transform_index + 1) % len(self._camera_transforms) - self.sensor.set_transform(self._camera_transforms[self._transform_index]) - - def set_sensor(self, index, notify=True): - index = index % len(self._sensors) - needs_respawn = True if self._index is None \ - else self._sensors[index][0] != self._sensors[self._index][0] - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self._surface = None - self.sensor = self._parent.get_world().spawn_actor( - self._sensors[index][-1], - self._camera_transforms[self._transform_index], - attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid - # circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - self._hud.notification(self._sensors[index][2]) - self._index = index - - def next_sensor(self): - self.set_sensor(self._index + 1) - - def toggle_recording(self): - self._recording = not self._recording - self._hud.notification('Recording %s' % ('On' if self._recording else 'Off')) - - def render(self, display): - if self._surface is not None: - display.blit(self._surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self._sensors[self._index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0]/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) - - -# ============================================================================== -# -- game_loop() --------------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - pygame.init() - pygame.font.init() - world = None - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(2.0) - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - - hud = HUD(args.width, args.height) - world = World(client.get_world(), hud) - controller = KeyboardControl(world, args.autopilot) - - clock = pygame.time.Clock() - while True: - clock.tick_busy_loop(60) - if controller.parse_events(world, clock): - return - world.tick(clock) - world.render(display) - pygame.display.flip() - - finally: - - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------------- -# ============================================================================== - - -def main(): - argparser = argparse.ArgumentParser( - description='CARLA Manual Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '-a', '--autopilot', - action='store_true', - help='enable autopilot') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='window resolution (default: 1280x720)') - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - - main() diff --git a/PlayingWithCARLA/move_autopilot.py b/PlayingWithCARLA/move_autopilot.py deleted file mode 100644 index 5e226b23..00000000 --- a/PlayingWithCARLA/move_autopilot.py +++ /dev/null @@ -1,36 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) -time.sleep(2) -car = world.get_actors().filter('vehicle.*')[0] -car.set_autopilot(True) -print(car.is_at_traffic_light()) - -''' -if vehicle_actor.is_at_traffic_light(): - traffic_light = vehicle_actor.get_traffic_light() - if traffic_light.get_state() == carla.TrafficLightState.Red: - # world.hud.notification("Traffic light changed! Good to go!") - traffic_light.set_state(carla.TrafficLightState.Green) -''' - -''' -my_vehicles = [car] - -tm = client.get_trafficmanager(port) -tm_port = tm.get_port() -for v in my_vehicles: - v.set_autopilot(True,tm_port) -danger_car = my_vehicles[0] -tm.ignore_lights_percentage(danger_car,100) -tm.distance_to_leading_vehicle(danger_car,0) -tm.vehicle_percentage_speed_difference(danger_car,-20) - -''' diff --git a/PlayingWithCARLA/move_vehicle.py b/PlayingWithCARLA/move_vehicle.py deleted file mode 100644 index 1789630f..00000000 --- a/PlayingWithCARLA/move_vehicle.py +++ /dev/null @@ -1,71 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -client = carla.Client('localhost', 2000) - -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) - -time.sleep(2) - -world.get_actors() - -car = world.get_actors().filter('vehicle.*')[0] -print(car) -print(car.get_velocity()) -print(car.get_vehicle_control()) -#vehicle_control =car.get_vehicle_control() -#vehicle_control.steer = 10000 - - -car.apply_control(carla.VehicleControl(throttle = 1, brake = 0)) -print(car.get_velocity()) -print(car.get_vehicle_control()) -time.sleep(5) -print(car.get_velocity()) -print(car.get_vehicle_control()) -time.sleep(5) -print(car.get_velocity()) -print(car.get_vehicle_control()) -car.apply_control(carla.VehicleControl(throttle = 0, brake = 0)) -time.sleep(5) -print(car.get_velocity()) -print(car.get_vehicle_control()) -''' -vehicle_control = carla.VehicleControl() -vehicle_control.steer = 10000 - -vehicle_control.manual_gear_shift =True -car.apply_control(vehicle_control) -time.sleep(0.5) -print(car.get_velocity()) -car.apply_control(vehicle_control) -time.sleep(0.5) -print(car.get_velocity()) -car.apply_control(vehicle_control) -time.sleep(0.5) -print(car.get_velocity()) -car.apply_control(vehicle_control) -time.sleep(0.5) -print(car.get_velocity()) -car.apply_control(vehicle_control) -time.sleep(0.5) -car.apply_control(vehicle_control) -time.sleep(0.5) -car.apply_control(vehicle_control) -time.sleep(0.5) -car.apply_control(vehicle_control) -time.sleep(0.5) -car.apply_control(vehicle_control) -time.sleep(0.5) -car.apply_control(vehicle_control) -time.sleep(0.5) -car.apply_control(vehicle_control) -time.sleep(0.5) - -print(car.get_velocity()) -''' diff --git a/PlayingWithCARLA/move_vehicle_tensorflow.py b/PlayingWithCARLA/move_vehicle_tensorflow.py deleted file mode 100644 index 8d55458b..00000000 --- a/PlayingWithCARLA/move_vehicle_tensorflow.py +++ /dev/null @@ -1,154 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -client = carla.Client('localhost', 2000) - -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) - -time.sleep(2) - -world.get_actors() - -car = world.get_actors().filter('vehicle.*')[0] -print(car) -print(car.get_velocity()) -#print(car.get_vehicle_control()) -#vehicle_control =car.get_vehicle_control() -#vehicle_control.steer = 10000 - - -import tensorflow as tf -import numpy as np - -from albumentations import ( - Compose, Normalize -) - -# LOAD CAMERA -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -#camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) -#rgb camera -image = image_queue.get() - -# LOAD TF MODEL -PRETRAINED_MODELS = "/home/jderobot/Documents/Projects/" -#model = "20220905-164806_pilotnet_CARLA_cp.h5" -#model = "20220906-114029_pilotnet_CARLA_extreme_cases_cp.h5" -#model = "20220906-165331_pilotnet_CARLA_extreme_cases_cp.h5" -#model = "20220907-105158_pilotnet_CARLA_extreme_cases_07_09_cp.h5" -#model = "20220907-140021_pilotnet_CARLA_extreme_cases_07_09_cp.h5" -#model = "20220914-124554_pilotnet_CARLA_extreme_cases_14_09_dataset_cp.h5" -#model = "20220914-130834_pilotnet_CARLA_extreme_cases_14_09_dataset_cp.h5" -#model = "20220914-134708_pilotnet_CARLA_extreme_cases_14_09_dataset_new_crop_cp.h5" -#model = "20220914-140016_pilotnet_CARLA_extreme_cases_14_09_dataset_new_crop_cp.h5" -#model = "20220916-140706_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_cp.h5" -#model = "20220916-154943_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_cp.h5" -#model = "20220916-164609_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases.h5" -#model = "20220919-172247_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases_simplified_images_cp.h5" -#model = "20220919-184652_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases_simplified_images_cp.h5" -model = "20220929-181325_pilotnet_CARLA_extreme_cases_29_09_dataset_bird_eye_random_start_point_retrained_double_cp.h5" - - -print('***********************************************************************************************') -net = tf.keras.models.load_model(PRETRAINED_MODELS + model) -#net = tf.saved_model.load(PRETRAINED_MODELS + model) -print('***********************************************************************************************') - - -while True: - # Predict - image = image_queue.get() - ''' - print(image) - print(image.raw_data) - ''' - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) # RGBA format - image = array[:, :, :3] # Take only RGB - image = image[:, :, ::-1] # BGR - - #print(image.shape) - - image = image[325:600, 0:800] - #plt.imshow(image) - #plt.show() - - img = cv2.resize(image, (200, 66)) - frame = img - hsv = cv2.cvtColor(frame, cv2.COLOR_RGB2HSV) - # Threshold of blue in HSV space - lower_blue = np.array([10, 100, 20]) - upper_blue = np.array([25, 255, 255]) - - - # preparing the mask to overlay - mask = cv2.inRange(hsv, lower_blue, upper_blue) - - # The black region in the mask has the value of 0, - # so when multiplied with original image removes all non-blue regions - result = cv2.bitwise_and(frame, frame, mask = mask) - #new_images_carla_dataset.append(result) - - img = result - - - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img) - img = image["image"] - - - img = np.expand_dims(img, axis=0) - prediction = net.predict(img) - prediction_w = prediction[0][1] * (1 - (-1)) + (-1) - ''' - print(prediction) - print(prediction_w) - ''' - - throttle = prediction[0][0] - steer = prediction_w - ''' - print('----') - print(throttle) - print(type(throttle)) - print(float(throttle)) - print(steer) - print(float(steer)) - print(type(steer)) - print('----') - ''' - print(float(throttle), steer) - - #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer)) - car.apply_control(carla.VehicleControl(throttle=0.2, steer=steer)) - #print(car.get_velocity()) - #print(car.get_vehicle_control()) - #time.sleep(5) - - -''' -car.apply_control(carla.VehicleControl(throttle = 1, brake = 0)) -print(car.get_velocity()) -print(car.get_vehicle_control()) -time.sleep(5) -print(car.get_velocity()) -print(car.get_vehicle_control()) -time.sleep(5) -print(car.get_velocity()) -print(car.get_vehicle_control()) -car.apply_control(carla.VehicleControl(throttle = 0, brake = 0)) -time.sleep(5) -print(car.get_velocity()) -print(car.get_vehicle_control()) -''' diff --git a/PlayingWithCARLA/plot_car_camera.py b/PlayingWithCARLA/plot_car_camera.py deleted file mode 100644 index 134af2ad..00000000 --- a/PlayingWithCARLA/plot_car_camera.py +++ /dev/null @@ -1,43 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -client = carla.Client('localhost', 2000) - -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) - -time.sleep(2) - -world.get_actors() - -car = world.get_actors().filter('vehicle.*')[0] -print(car) -print(car.get_velocity()) - - - -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) - -#rgb camera -image = image_queue.get() -image = image_queue.get() -cc = carla.ColorConverter.Raw -image.save_to_disk('_out/1.png', cc) - - -img = cv2.imread("_out/1.png", cv2.IMREAD_UNCHANGED) -img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) -img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) -print(car.get_velocity()) -plt.imshow(img) -plt.show() - - diff --git a/PlayingWithCARLA/print_camera_image.py b/PlayingWithCARLA/print_camera_image.py deleted file mode 100644 index cf0a4e97..00000000 --- a/PlayingWithCARLA/print_camera_image.py +++ /dev/null @@ -1,76 +0,0 @@ -import carla -import time - -client = carla.Client('localhost', 2000) - -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) - -world.get_actors() - -time.sleep(2) - -car = world.get_actors().filter('vehicle.*')[0] -print(car) -print(car.get_velocity()) - - -# Find the blueprint of the sensor. -blueprint = world.get_blueprint_library().find('sensor.camera.rgb') - -# Modify the attributes of the blueprint to set image resolution and field of view. -blueprint.set_attribute('image_size_x', '1920') -blueprint.set_attribute('image_size_y', '1080') -blueprint.set_attribute('fov', '110') - -# Provide the position of the sensor relative to the vehicle. -transform = carla.Transform(carla.Location(x=0.8, z=1.7)) - -# Tell the world to spawn the sensor, don't forget to attach it to your vehicle actor. -sensor = world.spawn_actor(blueprint, transform, attach_to=car) - -# Subscribe to the sensor stream by providing a callback function, this function is -# called each time a new image is generated by the sensor. -cc = carla.ColorConverter.LogarithmicDepth -cc = carla.ColorConverter.Raw -cc = carla.ColorConverter.CityScapesPalette -cc = carla.ColorConverter.Depth -sensor.listen(lambda image: image.save_to_disk('_out/%06d.png' % image.frame_number, cc)) - - - -import queue -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) - -#rgb camera -image = image_queue.get() -image = image_queue.get() -cc = carla.ColorConverter.Raw -image.save_to_disk('_out/1.png', cc) - -import matplotlib.pyplot as plt -import cv2 -img = cv2.imread("_out/1.png", cv2.IMREAD_UNCHANGED) -img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) -img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) -plt.imshow(img) -plt.show() - - -#rgb camera -image = image_queue.get() -cc = carla.ColorConverter.Raw -image.save_to_disk('_out/%06d.png' % image.frame_number, cc) - -import matplotlib.pyplot as plt -import cv2 -img = cv2.imread("_out/341519.png", cv2.IMREAD_UNCHANGED) -img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) -img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) -plt.imshow(img) -plt.show() diff --git a/PlayingWithCARLA/record_dataset.py b/PlayingWithCARLA/record_dataset.py deleted file mode 100644 index 9ad3ec76..00000000 --- a/PlayingWithCARLA/record_dataset.py +++ /dev/null @@ -1,69 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import pandas - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -#camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) - -#rgb camera -image = image_queue.get() -# open the file in the write mode -f = open('../carla_dataset_20_09/dataset.csv', 'a') - -# create the csv writer -writer = csv.writer(f) - -#mypath = '../carla_dataset_14_09/' -#onlyfiles = [f for f in listdir(mypath) if isfile(join(mypath, f))] -#onlyfiles = sorted(onlyfiles) -#print(onlyfiles) -#print(onlyfiles[len(onlyfiles)-2]) -#print(onlyfiles[len(onlyfiles)-2][0]) -#print(type(onlyfiles[len(onlyfiles)-2][0])) - -try: - df = pandas.read_csv('../carla_dataset_20_09/dataset.csv') - batch_number = df.iloc[-1]['batch'] + 1 -except: - batch_number = 0 - header = ['batch', 'image_id', 'throttle', 'steer'] - writer.writerow(header) - -print(batch_number) - -try: - while True: - image = image_queue.get() - cc = carla.ColorConverter.Raw - image.save_to_disk('../carla_dataset_20_09/' + str(batch_number) + '_' + str(image.frame_number) + '.png', cc) - vehicle_control = car.get_vehicle_control() - # write a row to the csv file - row = [batch_number, str(batch_number) + '_' + str(image.frame_number) + '.png', vehicle_control.throttle, vehicle_control.steer] - writer.writerow(row) - -except KeyboardInterrupt: - pass - -# close the file -f.close() - - diff --git a/PlayingWithCARLA/remove_traffic_lights.py b/PlayingWithCARLA/remove_traffic_lights.py deleted file mode 100644 index 473dc818..00000000 --- a/PlayingWithCARLA/remove_traffic_lights.py +++ /dev/null @@ -1,86 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -print(carla.__file__) - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) -time.sleep(2) - -#print(world.get_actors()) - -#traffic_lights = world.get_actors().filter('traffic.traffic_light') - -#for traffic_light in traffic_lights: -# traffic_light.destroy() - -#print('*****************') -#print(world.get_actors()) - -################################################################################################### -#print(world.get_actors()) - -#actors = world.get_actors() -#for actor in actors: -# print(actor) - -#print('*****************') -#print(world.get_actors()) - - -meshes = world.get_actors().filter('static.prop.mesh') -print(meshes) -for mesh in meshes: - mesh.destroy() -print(world.get_actors()) - - -traffic_lights = world.get_actors().filter('traffic.traffic_light') - -for traffic_light in traffic_lights: - traffic_light.destroy() -print(world.get_actors()) - - -speed_limits = world.get_actors().filter('traffic.speed_limit.30') -print(speed_limits) -for speed_limit in speed_limits: - speed_limit.destroy() -print(world.get_actors()) - -speed_limits = world.get_actors().filter('traffic.speed_limit.60') -print(speed_limits) -for speed_limit in speed_limits: - speed_limit.destroy() -print(world.get_actors()) - -speed_limits = world.get_actors().filter('traffic.speed_limit.90') -print(speed_limits) -for speed_limit in speed_limits: - speed_limit.destroy() -print(world.get_actors()) - - - -''' -print(traffic_lights[0].state) -print(type(traffic_lights[0])) -print(type(traffic_lights[0].state)) -print('*****************') -print(dir(traffic_lights[0])) -print('*****************') -print(dir(traffic_lights[0].state)) - -carla.TrafficLightState('Red') - -traffic_lights[0].set_state('Green') -traffic_lights[0].state = 'Green' -print(traffic_lights[0].state) -''' - - diff --git a/PlayingWithCARLA/remove_traffic_lights_and_autopilot.py b/PlayingWithCARLA/remove_traffic_lights_and_autopilot.py deleted file mode 100644 index 7023d461..00000000 --- a/PlayingWithCARLA/remove_traffic_lights_and_autopilot.py +++ /dev/null @@ -1,109 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -print(carla.__file__) - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) -time.sleep(2) - -traffic_lights = world.get_actors().filter('traffic.traffic_light') -traffic_speed_limits = world.get_actors().filter('traffic.speed_limit*') -print(traffic_speed_limits) -for traffic_light in traffic_lights: - #success = traffic_light.destroy() - traffic_light.set_green_time(20000) - traffic_light.set_state(carla.TrafficLightState.Green) - #print(success) - -#for speed_limit in traffic_speed_limits: -# success = speed_limit.destroy() -# print(success) - -print(world.get_actors().filter('vehicle.*')) -car = world.get_actors().filter('vehicle.*')[0] - - -#settings = world.get_settings() -#settings.synchronous_mode = True # Enables synchronous mode -#world.apply_settings(settings) -traffic_manager = client.get_trafficmanager() -#random.seed(0) -#car.set_autopilot(True) -car.set_autopilot(True) -route = ["Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight"] -traffic_manager.set_route(car, route) - -''' -import math -while True: - speed = car.get_velocity() - vehicle_speed = 3.6 * math.sqrt(speed.x**2 + speed.y**2 + speed.z**2) - if (abs(vehicle_speed) > 5): - print('SLOW DOWN!') - car.apply_control(carla.VehicleControl(throttle=float(0), steer=float(0), brake=float(1.0))) - - -''' -''' -# Set up the TM in synchronous mode -traffic_manager = client.get_trafficmanager() -#traffic_manager.set_synchronous_mode(True) - -# Set a seed so behaviour can be repeated if necessary -#traffic_manager.set_random_device_seed(0) -#random.seed(0) -#car.set_autopilot(True) -car.set_autopilot(True) -route = ["Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight", -"Straight", "Straight", "Straight", "Straight", "Straight"] -traffic_manager.set_route(car, route) -#time.sleep(3) -#car.set_autopilot(False) -#print('autopilot false!') -''' -''' -car.apply_control(carla.VehicleControl(throttle=float(0.75), steer=-0.1, brake=float(0.0))) -time.sleep(3) -car.set_autopilot(True) -#traffic_manager.set_route(car, route) -''' - - -''' -while True: - print('entra') - world.tick() - - #traffic_manager.update_vehicle_lights(car, True) - #traffic_manager.random_left_lanechange_percentage(car, 0) - #traffic_manager.random_right_lanechange_percentage(car, 0) - #traffic_manager.auto_lane_change(car, False) - - world_map =world.get_map() - spawn_points = world_map.get_spawn_points() - - # Create route 1 from the chosen spawn points - route_1_indices = [129, 28, 124, 33, 97, 119, 58, 154, 147] - route_1 = [] - for ind in route_1_indices: - route_1.append(spawn_points[ind].location) - print(route_1) - traffic_manager.set_path(car, route_1) -''' - - diff --git a/PlayingWithCARLA/set_autopilot_and_print_speed.py b/PlayingWithCARLA/set_autopilot_and_print_speed.py deleted file mode 100644 index 3c64210b..00000000 --- a/PlayingWithCARLA/set_autopilot_and_print_speed.py +++ /dev/null @@ -1,26 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -client = carla.Client('localhost', 2000) - -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) - -time.sleep(2) - -world.get_actors() - -car = world.get_actors().filter('vehicle.*')[0] -print(car) -print(car.get_velocity()) -print(car.get_vehicle_control()) - - -car.set_autopilot(True) - -while True: - print(car.get_vehicle_control()) diff --git a/PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py b/PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py deleted file mode 100644 index 89f4dd9c..00000000 --- a/PlayingWithCARLA/set_autopilot_and_show_tensorflow_estimation.py +++ /dev/null @@ -1,87 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import tensorflow as tf -import numpy as np - -from albumentations import ( - Compose, Normalize -) - - -client = carla.Client('localhost', 2000) - -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) - -time.sleep(2) - -world.get_actors() - -car = world.get_actors().filter('vehicle.*')[0] -print(car) -print(car.get_velocity()) -print(car.get_vehicle_control()) - - - -# LOAD CAMERA -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -#camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -camera_transform = carla.Transform(carla.Location(x=0, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) -#rgb camera -image = image_queue.get() - -# LOAD TF MODEL -PRETRAINED_MODELS = "/home/jderobot/Documents/Projects/" -model = "20220907-140021_pilotnet_CARLA_extreme_cases_07_09_cp.h5" - -print('***********************************************************************************************') -net = tf.keras.models.load_model(PRETRAINED_MODELS + model) -#net = tf.saved_model.load(PRETRAINED_MODELS + model) -print('***********************************************************************************************') - - -car.set_autopilot(True) - -while True: - # Predict - image = image_queue.get() - - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) # RGBA format - image = array[:, :, :3] # Take only RGB - image = image[:, :, ::-1] # BGR - - image = image[300:600, 0:800] - - img = cv2.resize(image, (200, 66)) - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=img) - img = image["image"] - - - img = np.expand_dims(img, axis=0) - prediction = net.predict(img) - prediction_w = prediction[0][1] * (1 - (-1)) + (-1) - - throttle = prediction[0][0] - steer = prediction_w - - print(abs(steer)-abs(car.get_vehicle_control().steer)) - - #print(float(throttle), steer) - #print(car.get_vehicle_control()) - - - - - diff --git a/PlayingWithCARLA/show_camera_image.py b/PlayingWithCARLA/show_camera_image.py deleted file mode 100644 index ac5cc07b..00000000 --- a/PlayingWithCARLA/show_camera_image.py +++ /dev/null @@ -1,77 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import numpy as np -import queue -import pygame - - -h,w=800,800 - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) - -#rgb camera -image = image_queue.get() - -pygame.init() -screen = pygame.display.set_mode((w, h)) -pygame.display.set_caption("Serious Work - not games") -done = False -clock = pygame.time.Clock() - -# Get a font for rendering the frame number -basicfont = pygame.font.SysFont(None, 32) - -array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) -array = np.reshape(array, (image.height, image.width, 4)) # RGBA format -image = array[:, :, :3] # Take only RGB -image = image[:, :, ::-1] # BGR - -try: - while True: - image = image_queue.get() - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) # RGBA format - image = array[:, :, :3] # Take only RGB - image = image[:, :, ::-1] # BGR - image = image[:, ::-1, :] # Mirror - - image = image[325:600, 0:800] - #plt.imshow(image) - #plt.show() - - img = cv2.resize(image, (200, 66)) - - # Clear screen to white before drawing - screen.fill((255, 255, 255)) - - # Convert to a surface and splat onto screen offset by border width and height - surface = pygame.surfarray.make_surface(image) - screen.blit(surface, (0, 0)) - screen.blit(pygame.transform.rotate(screen, 270), (0, 0)) - pygame.display.flip() - - clock.tick(60) - - -except KeyboardInterrupt: - pass - diff --git a/PlayingWithCARLA/show_camera_opencv.py b/PlayingWithCARLA/show_camera_opencv.py deleted file mode 100644 index 9e9e0861..00000000 --- a/PlayingWithCARLA/show_camera_opencv.py +++ /dev/null @@ -1,68 +0,0 @@ -import carla - -import random -import time -import numpy as np -import cv2 - -IM_WIDTH = 640 -IM_HEIGHT = 480 - - -def process_img(image): - i = np.array(image.raw_data) - i2 = i.reshape((IM_HEIGHT, IM_WIDTH, 4)) - i3 = i2[:, :, :3] - cv2.imshow("", i3) - cv2.waitKey(1) - return i3/255.0 - - -actor_list = [] -try: - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - world = client.get_world() - - blueprint_library = world.get_blueprint_library() - - bp = blueprint_library.filter('model3')[0] - print(bp) - - spawn_point = random.choice(world.get_map().get_spawn_points()) - - vehicle = world.spawn_actor(bp, spawn_point) - vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=0.0)) - # vehicle.set_autopilot(True) # if you just wanted some NPCs to drive. - - actor_list.append(vehicle) - - # https://carla.readthedocs.io/en/latest/cameras_and_sensors - # get the blueprint for this sensor - blueprint = blueprint_library.find('sensor.camera.rgb') - # change the dimensions of the image - blueprint.set_attribute('image_size_x', f'{IM_WIDTH}') - blueprint.set_attribute('image_size_y', f'{IM_HEIGHT}') - blueprint.set_attribute('fov', '110') - - # Adjust sensor relative to vehicle - spawn_point = carla.Transform(carla.Location(x=2.5, z=0.7)) - spawn_point = carla.Transform(carla.Location(x=1.5, z=2.4)) - - # spawn the sensor and attach to vehicle. - sensor = world.spawn_actor(blueprint, spawn_point, attach_to=vehicle) - - # add sensor to list of actors - actor_list.append(sensor) - - # do something with this sensor - sensor.listen(lambda data: process_img(data)) - - time.sleep(5) - -finally: - print('destroying actors') - for actor in actor_list: - actor.destroy() - print('done.') \ No newline at end of file diff --git a/PlayingWithCARLA/show_crop_camera_image.py b/PlayingWithCARLA/show_crop_camera_image.py deleted file mode 100644 index ed996c20..00000000 --- a/PlayingWithCARLA/show_crop_camera_image.py +++ /dev/null @@ -1,76 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import numpy as np -import queue -import pygame - - -h,w=200,200 - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) - -#rgb camera -image = image_queue.get() - -pygame.init() -screen = pygame.display.set_mode((w, h)) -pygame.display.set_caption("Serious Work - not games") -done = False -clock = pygame.time.Clock() - -# Get a font for rendering the frame number -basicfont = pygame.font.SysFont(None, 32) - -array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) -array = np.reshape(array, (image.height, image.width, 4)) # RGBA format -image = array[:, :, :3] # Take only RGB -image = image[:, :, ::-1] # BGR - - - -try: - while True: - image = image_queue.get() - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) # RGBA format - image = array[:, :, :3] # Take only RGB - image = image[:, :, ::-1] # BGR - image = image[:, ::-1, :] # Mirror - image = image[300:600, 0:800] - image= cv2.resize(image, (200, 66)) - - # Clear screen to white before drawing - - screen.fill((255, 255, 255)) - - # Convert to a surface and splat onto screen offset by border width and height - surface = pygame.surfarray.make_surface(image) - screen.blit(surface, (0, 0)) - screen.blit(pygame.transform.rotate(screen, 270), (0, 0)) - pygame.display.flip() - - clock.tick(60) - - -except KeyboardInterrupt: - pass - diff --git a/PlayingWithCARLA/show_dataset_image.py b/PlayingWithCARLA/show_dataset_image.py deleted file mode 100644 index 07cfea76..00000000 --- a/PlayingWithCARLA/show_dataset_image.py +++ /dev/null @@ -1,7 +0,0 @@ -import matplotlib.pyplot as plt -import cv2 -img = cv2.imread("../_out/187221.png", cv2.IMREAD_UNCHANGED) -img = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR) -img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) -plt.imshow(img) -plt.show() diff --git a/PlayingWithCARLA/show_environment_objects.py b/PlayingWithCARLA/show_environment_objects.py deleted file mode 100644 index d17352ca..00000000 --- a/PlayingWithCARLA/show_environment_objects.py +++ /dev/null @@ -1,39 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time - -print(carla.__file__) - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() -print(world) -time.sleep(2) - - -#print(world.get_environment_objects()) -print(len(world.get_environment_objects())) - -environment_objects = world.get_environment_objects() - -print(type(environment_objects)) -print(environment_objects[0].id) - -environment_object_ids = [] - -for env_obj in environment_objects: - environment_object_ids.append(env_obj.id) - - - -environment_object_ids = set(environment_object_ids) -print(type(environment_object_ids)) - -world.enable_environment_objects(environment_object_ids, False) -#for env_obj in environment_objects: - - - - diff --git a/PlayingWithCARLA/show_grad_camera.py b/PlayingWithCARLA/show_grad_camera.py deleted file mode 100644 index 9dc4896d..00000000 --- a/PlayingWithCARLA/show_grad_camera.py +++ /dev/null @@ -1,122 +0,0 @@ -import carla -import queue -import matplotlib.pyplot as plt -import cv2 -import time -import csv -from os import listdir -from os.path import isfile, join -import numpy as np -import queue -import pygame - - -h,w=800,800 -h,w=200, 200 - -client = carla.Client('localhost', 2000) -client.set_timeout(10.0) # seconds -world = client.get_world() - -time.sleep(2) - -world.get_actors() -car = world.get_actors().filter('vehicle.*')[0] - -camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') -camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) -camera = world.spawn_actor(camera_bp, camera_transform, attach_to=car) -image_queue = queue.Queue() -camera.listen(image_queue.put) - -#rgb camera -image = image_queue.get() - -pygame.init() -screen = pygame.display.set_mode((w, h)) -pygame.display.set_caption("Serious Work - not games") -done = False -clock = pygame.time.Clock() - -# Get a font for rendering the frame number -basicfont = pygame.font.SysFont(None, 32) - -array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) -array = np.reshape(array, (image.height, image.width, 4)) # RGBA format -image = array[:, :, :3] # Take only RGB -image = image[:, :, ::-1] # BGR - -import tensorflow as tf -import numpy as np -from gradcam import GradCAM - - -from albumentations import ( - Compose, Normalize -) - -# LOAD TF MODEL -PRETRAINED_MODELS = "/home/jderobot/Documents/Projects/" -model = "20220916-164609_pilotnet_CARLA_extreme_cases_16_09_dataset_new_crop_extreme_cases.h5" -net = tf.keras.models.load_model(PRETRAINED_MODELS + model) -print(net.summary()) - -try: - while True: - image = image_queue.get() - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) # RGBA format - image = array[:, :, :3] # Take only RGB - image = image[:, :, ::-1] # BGR - image = image[:, ::-1, :] # Mirror - - original_image = image[325:600, 0:800] - #plt.imshow(image) - #plt.show() - - resized_image = cv2.resize(original_image, (200, 66)) - AUGMENTATIONS_TEST = Compose([ - Normalize() - ]) - image = AUGMENTATIONS_TEST(image=resized_image) - img = image["image"] - - - img = np.expand_dims(img, axis=0) - prediction = net.predict(img) - - prediction_w = prediction[0][1] * (1 - (-1)) + (-1) - throttle = prediction[0][0] - steer = prediction_w - print(float(throttle), steer) - - #car.apply_control(carla.VehicleControl(throttle=float(throttle), steer=steer)) - car.apply_control(carla.VehicleControl(throttle=0.2, steer=steer)) - - i = np.argmax(prediction[0]) - cam = GradCAM(net, i) - heatmap = cam.compute_heatmap(img) - heatmap = cv2.resize(heatmap, (heatmap.shape[1], heatmap.shape[0])) - - print(original_image.shape) - print(resized_image.shape) - print(heatmap.shape) - (heatmap, output) = cam.overlay_heatmap(heatmap, resized_image, alpha=0.5) - print(output.shape) - - # Clear screen to white before drawing - screen.fill((255, 255, 255)) - - # Convert to a surface and splat onto screen offset by border width and height - #surface = pygame.surfarray.make_surface(original_image) - surface = pygame.surfarray.make_surface(output) - screen.blit(surface, (0, 0)) - screen.blit(pygame.transform.rotate(screen, 270), (0, 0)) - pygame.display.flip() - - clock.tick(60) - - -except KeyboardInterrupt: - pass - diff --git a/PlayingWithCARLA/tutorial.py b/PlayingWithCARLA/tutorial.py deleted file mode 100755 index 311df317..00000000 --- a/PlayingWithCARLA/tutorial.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('**/*%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 - -import random -import time - - -def main(): - actor_list = [] - - # In this tutorial script, we are going to add a vehicle to the simulation - # and let it drive in autopilot. We will also create a camera attached to - # that vehicle, and save all the images generated by the camera to disk. - - try: - # First of all, we need to create the client that will send the requests - # to the simulator. Here we'll assume the simulator is accepting - # requests in the localhost at port 2000. - client = carla.Client('localhost', 2000) - client.set_timeout(2.0) - - # Once we have a client we can retrieve the world that is currently - # running. - world = client.get_world() - - # The world contains the list blueprints that we can use for adding new - # actors into the simulation. - blueprint_library = world.get_blueprint_library() - - # Now let's filter all the blueprints of type 'vehicle' and choose one - # at random. - bp = random.choice(blueprint_library.filter('vehicle')) - - # A blueprint contains the list of attributes that define a vehicle - # instance, we can read them and modify some of them. For instance, - # let's randomize its color. - #color = random.choice(bp.get_attribute('color').recommended_values) - #bp.set_attribute('color', color) - - # Now we need to give an initial transform to the vehicle. We choose a - # random transform from the list of recommended spawn points of the map. - transform = random.choice(world.get_map().get_spawn_points()) - - # So let's tell the world to spawn the vehicle. - vehicle = world.spawn_actor(bp, transform) - - # It is important to note that the actors we create won't be destroyed - # unless we call their "destroy" function. If we fail to call "destroy" - # they will stay in the simulation even after we quit the Python script. - # For that reason, we are storing all the actors we create so we can - # destroy them afterwards. - actor_list.append(vehicle) - print('created %s' % vehicle.type_id) - - # Let's put the vehicle to drive around. - vehicle.set_autopilot(True) - - # Let's add now a "depth" camera attached to the vehicle. Note that the - # transform we give here is now relative to the vehicle. - camera_bp = blueprint_library.find('sensor.camera.depth') - camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) - camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) - actor_list.append(camera) - print('created %s' % camera.type_id) - - # Now we register the function that will be called each time the sensor - # receives an image. In this example we are saving the image to disk - # converting the pixels to gray-scale. - cc = carla.ColorConverter.LogarithmicDepth - camera.listen(lambda image: image.save_to_disk('_out/%06d.png' % image.frame_number, cc)) - - # Oh wait, I don't like the location we gave to the vehicle, I'm going - # to move it a bit forward. - location = vehicle.get_location() - location.x += 40 - vehicle.set_location(location) - print('moved vehicle to %s' % location) - - # But the city now is probably quite empty, let's add a few more - # vehicles. - transform.location += carla.Location(x=40, y=-3.2) - transform.rotation.yaw = -180.0 - for x in range(0, 10): - transform.location.x += 8.0 - - bp = random.choice(blueprint_library.filter('vehicle')) - - # This time we are using try_spawn_actor. If the spot is already - # occupied by another object, the function will return None. - npc = world.try_spawn_actor(bp, transform) - if npc is not None: - actor_list.append(npc) - npc.set_autopilot() - print('created %s' % npc.type_id) - - time.sleep(5) - - finally: - - print('destroying actors') - for actor in actor_list: - actor.destroy() - print('done.') - - -if __name__ == '__main__': - - main()