From 538b4e04d6a65e2d5c195569cbb72a8f35b313f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomasz=20Su=C5=82kowski?= Date: Thu, 2 May 2019 16:01:47 +0200 Subject: [PATCH] Client-side bounding boxes (#1383) * BasicSynchronousClient for client side bounding boxes done * client-side bounding boxes done * Updated CHANGELOG.md and client_bounding_boxes with appropriate imports for development build * moved client_bounding_boxes.py to examples/ * attempt to fix strange pylint error * fix attempt for pylint2 * Disabled pylint E1126 as it is getting confused with numpy matrix slice --- CHANGELOG.md | 1 + PythonAPI/.pylintrc | 2 +- PythonAPI/examples/client_bounding_boxes.py | 399 ++++++++++++++++++++ 3 files changed, 401 insertions(+), 1 deletion(-) create mode 100755 PythonAPI/examples/client_bounding_boxes.py diff --git a/CHANGELOG.md b/CHANGELOG.md index bb6984951..84aba56f8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ ## CARLA 0.9.5 + * Added `client_bounding_boxes.py` to show bounding boxes client-side * New Town07, rural environment with narrow roads * Reworked OpenDRIVE parser and waypoints API - Fixed several situations in which the XODR was incorrectly parsed diff --git a/PythonAPI/.pylintrc b/PythonAPI/.pylintrc index b69ea1655..a7983ff4d 100644 --- a/PythonAPI/.pylintrc +++ b/PythonAPI/.pylintrc @@ -1,6 +1,6 @@ [MESSAGES CONTROL] max-line-length=120 [MASTER] -disable=I0011,I0013,E1121 +disable=I0011,I0013,E1121,E1126 [TYPECHECK] ignored-modules=carla,carla.command,libcarla,pygame,numpy,configparser,ConfigParser diff --git a/PythonAPI/examples/client_bounding_boxes.py b/PythonAPI/examples/client_bounding_boxes.py new file mode 100755 index 000000000..3df29c2c2 --- /dev/null +++ b/PythonAPI/examples/client_bounding_boxes.py @@ -0,0 +1,399 @@ +#!/usr/bin/env python + +# Copyright (c) 2019 Aptiv +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +An example of client-side bounding boxes with basic car controls. + +Controls: + + W : throttle + S : brake + AD : steer + Space : hand-brake + + ESC : quit +""" + +# ============================================================================== +# -- 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 + +import weakref +import random + +try: + import pygame + from pygame.locals import K_ESCAPE + from pygame.locals import K_SPACE + from pygame.locals import K_a + from pygame.locals import K_d + 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') + +VIEW_WIDTH = 1920//2 +VIEW_HEIGHT = 1080//2 +VIEW_FOV = 90 + +BB_COLOR = (248, 64, 24) + +# ============================================================================== +# -- ClientSideBoundingBoxes --------------------------------------------------- +# ============================================================================== + + +class ClientSideBoundingBoxes(object): + """ + This is a module responsible for creating 3D bounding boxes and drawing them + client-side on pygame surface. + """ + + @staticmethod + def get_bounding_boxes(vehicles, camera): + """ + Creates 3D bounding boxes based on carla vehicle list and camera. + """ + + bounding_boxes = [ClientSideBoundingBoxes.get_bounding_box(vehicle, camera) for vehicle in vehicles] + # filter objects behind camera + bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)] + return bounding_boxes + + @staticmethod + def draw_bounding_boxes(display, bounding_boxes): + """ + Draws bounding boxes on pygame display. + """ + + bb_surface = pygame.Surface((VIEW_WIDTH, VIEW_HEIGHT)) + bb_surface.set_colorkey((0, 0, 0)) + for bbox in bounding_boxes: + points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)] + # draw lines + # base + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1]) + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[1]) + pygame.draw.line(bb_surface, BB_COLOR, points[1], points[2]) + pygame.draw.line(bb_surface, BB_COLOR, points[2], points[3]) + pygame.draw.line(bb_surface, BB_COLOR, points[3], points[0]) + # top + pygame.draw.line(bb_surface, BB_COLOR, points[4], points[5]) + pygame.draw.line(bb_surface, BB_COLOR, points[5], points[6]) + pygame.draw.line(bb_surface, BB_COLOR, points[6], points[7]) + pygame.draw.line(bb_surface, BB_COLOR, points[7], points[4]) + # base-top + pygame.draw.line(bb_surface, BB_COLOR, points[0], points[4]) + pygame.draw.line(bb_surface, BB_COLOR, points[1], points[5]) + pygame.draw.line(bb_surface, BB_COLOR, points[2], points[6]) + pygame.draw.line(bb_surface, BB_COLOR, points[3], points[7]) + display.blit(bb_surface, (0, 0)) + + @staticmethod + def get_bounding_box(vehicle, camera): + """ + Returns 3D bounding box for a vehicle based on camera view. + """ + + bb_cords = ClientSideBoundingBoxes._create_bb_points(vehicle) + cords_x_y_z = ClientSideBoundingBoxes._vehicle_to_sensor(bb_cords, vehicle, camera)[:3, :] + cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]]) + bbox = np.transpose(np.dot(camera.calibration, cords_y_minus_z_x)) + camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1) + return camera_bbox + + @staticmethod + def _create_bb_points(vehicle): + """ + Returns 3D bounding box for a vehicle. + """ + + cords = np.zeros((8, 4)) + extent = vehicle.bounding_box.extent + cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1]) + cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1]) + cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1]) + cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1]) + cords[4, :] = np.array([extent.x, extent.y, extent.z, 1]) + cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1]) + cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1]) + cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1]) + return cords + + @staticmethod + def _vehicle_to_sensor(cords, vehicle, sensor): + """ + Transforms coordinates of a vehicle bounding box to sensor. + """ + + world_cord = ClientSideBoundingBoxes._vehicle_to_world(cords, vehicle) + sensor_cord = ClientSideBoundingBoxes._world_to_sensor(world_cord, sensor) + return sensor_cord + + @staticmethod + def _vehicle_to_world(cords, vehicle): + """ + Transforms coordinates of a vehicle bounding box to world. + """ + + bb_transform = carla.Transform(vehicle.bounding_box.location) + bb_vehicle_matrix = ClientSideBoundingBoxes.get_matrix(bb_transform) + vehicle_world_matrix = ClientSideBoundingBoxes.get_matrix(vehicle.get_transform()) + bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix) + world_cords = np.dot(bb_world_matrix, np.transpose(cords)) + return world_cords + + @staticmethod + def _world_to_sensor(cords, sensor): + """ + Transforms world coordinates to sensor. + """ + + sensor_world_matrix = ClientSideBoundingBoxes.get_matrix(sensor.get_transform()) + world_sensor_matrix = np.linalg.inv(sensor_world_matrix) + sensor_cords = np.dot(world_sensor_matrix, cords) + return sensor_cords + + @staticmethod + def get_matrix(transform): + """ + Creates matrix from carla transform. + """ + + rotation = transform.rotation + location = transform.location + c_y = np.cos(np.radians(rotation.yaw)) + s_y = np.sin(np.radians(rotation.yaw)) + c_r = np.cos(np.radians(rotation.roll)) + s_r = np.sin(np.radians(rotation.roll)) + c_p = np.cos(np.radians(rotation.pitch)) + s_p = np.sin(np.radians(rotation.pitch)) + matrix = np.matrix(np.identity(4)) + matrix[0, 3] = location.x + matrix[1, 3] = location.y + matrix[2, 3] = location.z + matrix[0, 0] = c_p * c_y + matrix[0, 1] = c_y * s_p * s_r - s_y * c_r + matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r + matrix[1, 0] = s_y * c_p + matrix[1, 1] = s_y * s_p * s_r + c_y * c_r + matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r + matrix[2, 0] = s_p + matrix[2, 1] = -c_p * s_r + matrix[2, 2] = c_p * c_r + return matrix + + +# ============================================================================== +# -- BasicSynchronousClient ---------------------------------------------------- +# ============================================================================== + + +class BasicSynchronousClient(object): + """ + Basic implementation of a synchronous client. + """ + + def __init__(self): + self.client = None + self.world = None + self.camera = None + self.car = None + + self.display = None + self.image = None + self.capture = True + + def camera_blueprint(self): + """ + Returns camera blueprint. + """ + + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(VIEW_WIDTH)) + camera_bp.set_attribute('image_size_y', str(VIEW_HEIGHT)) + camera_bp.set_attribute('fov', str(VIEW_FOV)) + return camera_bp + + def set_synchronous_mode(self, synchronous_mode): + """ + Sets synchronous mode. + """ + + settings = self.world.get_settings() + settings.synchronous_mode = synchronous_mode + self.world.apply_settings(settings) + + def setup_car(self): + """ + Spawns actor-vehicle to be controled. + """ + + car_bp = self.world.get_blueprint_library().filter('vehicle.*')[0] + location = random.choice(self.world.get_map().get_spawn_points()) + self.car = self.world.spawn_actor(car_bp, location) + + def setup_camera(self): + """ + Spawns actor-camera to be used to render view. + Sets calibration for client-side boxes rendering. + """ + + camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)) + self.camera = self.world.spawn_actor(self.camera_blueprint(), camera_transform, attach_to=self.car) + weak_self = weakref.ref(self) + self.camera.listen(lambda image: weak_self().set_image(weak_self, image)) + + calibration = np.identity(3) + calibration[0, 2] = VIEW_WIDTH / 2.0 + calibration[1, 2] = VIEW_HEIGHT / 2.0 + calibration[0, 0] = calibration[1, 1] = VIEW_WIDTH / (2.0 * np.tan(VIEW_FOV * np.pi / 360.0)) + self.camera.calibration = calibration + + def control(self, car): + """ + Applies control to main car based on pygame pressed keys. + Will return True If ESCAPE is hit, otherwise False to end main loop. + """ + + keys = pygame.key.get_pressed() + if keys[K_ESCAPE]: + return True + + control = car.get_control() + control.throttle = 0 + if keys[K_w]: + control.throttle = 1 + control.reverse = False + elif keys[K_s]: + control.throttle = 1 + control.reverse = True + if keys[K_a]: + control.steer = max(-1., min(control.steer - 0.05, 0)) + elif keys[K_d]: + control.steer = min(1., max(control.steer + 0.05, 0)) + else: + control.steer = 0 + control.hand_brake = keys[K_SPACE] + + car.apply_control(control) + return False + + @staticmethod + def set_image(weak_self, img): + """ + Sets image coming from camera sensor. + The self.capture flag is a mean of synchronization - once the flag is + set, next coming image will be stored. + """ + + self = weak_self() + if self.capture: + self.image = img + self.capture = False + + def render(self, display): + """ + Transforms image from camera sensor and blits it to main pygame display. + """ + + if self.image is not None: + array = np.frombuffer(self.image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (self.image.height, self.image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + display.blit(surface, (0, 0)) + + def game_loop(self): + """ + Main program loop. + """ + + try: + pygame.init() + + self.client = carla.Client('127.0.0.1', 2000) + self.client.set_timeout(2.0) + self.world = self.client.get_world() + + self.setup_car() + self.setup_camera() + + self.display = pygame.display.set_mode((VIEW_WIDTH, VIEW_HEIGHT), pygame.HWSURFACE | pygame.DOUBLEBUF) + pygame_clock = pygame.time.Clock() + + self.set_synchronous_mode(True) + vehicles = self.world.get_actors().filter('vehicle.*') + + while True: + self.world.tick() + + self.capture = True + pygame_clock.tick_busy_loop(20) + + self.render(self.display) + bounding_boxes = ClientSideBoundingBoxes.get_bounding_boxes(vehicles, self.camera) + ClientSideBoundingBoxes.draw_bounding_boxes(self.display, bounding_boxes) + + pygame.display.flip() + + pygame.event.pump() + if self.control(self.car): + return + + finally: + self.set_synchronous_mode(False) + self.camera.destroy() + self.car.destroy() + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + """ + Initializes the client-side bounding box demo. + """ + + try: + client = BasicSynchronousClient() + client.game_loop() + finally: + print('EXIT') + + +if __name__ == '__main__': + main()