From e38a2a4cd6500583531fdaa068d6bde88afb9dde Mon Sep 17 00:00:00 2001 From: Yuchao Feng <99387180+fyctime052@users.noreply.github.com> Date: Fri, 26 Jan 2024 14:29:20 +0800 Subject: [PATCH] Add files via upload --- scenario_runner/CARLA_VER | 2 + scenario_runner/Dockerfile | 51 + scenario_runner/Jenkinsfile | 186 ++++ scenario_runner/LICENSE | 21 + scenario_runner/manual_control.py | 230 ++++ scenario_runner/metrics_manager.py | 154 +++ scenario_runner/mkdocs.yml | 29 + scenario_runner/no_rendering_mode.py | 1494 ++++++++++++++++++++++++++ scenario_runner/scenario_runner.py | 586 ++++++++++ 9 files changed, 2753 insertions(+) create mode 100644 scenario_runner/CARLA_VER create mode 100644 scenario_runner/Dockerfile create mode 100644 scenario_runner/Jenkinsfile create mode 100644 scenario_runner/LICENSE create mode 100644 scenario_runner/manual_control.py create mode 100644 scenario_runner/metrics_manager.py create mode 100644 scenario_runner/mkdocs.yml create mode 100644 scenario_runner/no_rendering_mode.py create mode 100644 scenario_runner/scenario_runner.py diff --git a/scenario_runner/CARLA_VER b/scenario_runner/CARLA_VER new file mode 100644 index 0000000..3cb97c6 --- /dev/null +++ b/scenario_runner/CARLA_VER @@ -0,0 +1,2 @@ +HOST = https://carla-releases.s3.eu-west-3.amazonaws.com/Linux +RELEASE=CARLA_0.9.9 diff --git a/scenario_runner/Dockerfile b/scenario_runner/Dockerfile new file mode 100644 index 0000000..aaa4580 --- /dev/null +++ b/scenario_runner/Dockerfile @@ -0,0 +1,51 @@ +from ubuntu:18.04 + +# Install base libs +run apt-get update && apt-get install --no-install-recommends -y libpng16-16=1.6.34-1ubuntu0.18.04.2 \ +libtiff5=4.0.9-5ubuntu0.3 libjpeg8=8c-2ubuntu8 build-essential=12.4ubuntu1 wget=1.19.4-1ubuntu2.2 git=1:2.17.1-1ubuntu0.7 \ + python3.6=3.6.9-1~18.04ubuntu1 python3.6-dev=3.6.9-1~18.04ubuntu1 python3-pip=9.0.1-2.3~ubuntu1.18.04.1 \ + && rm -rf /var/lib/apt/lists/* + +# Install python requirements +run pip3 install --user setuptools==46.3.0 wheel==0.34.2 && pip3 install py_trees==0.8.3 networkx==2.2 pygame==1.9.6 \ + six==1.14.0 numpy==1.18.4 psutil==5.7.0 shapely==1.7.0 xmlschema==1.1.3 ephem==3.7.6.0 tabulate==0.8.7\ +&& mkdir -p /app/scenario_runner + +# Install scenario_runner +copy . /app/scenario_runner + +# setup environment : +# +# CARLA_HOST : uri for carla package without trailing slash. +# For example, "https://carla-releases.s3.eu-west-3.amazonaws.com/Linux". +# If this environment is not passed to docker build, the value +# is taken from CARLA_VER file inside the repository. +# +# CARLA_RELEASE : Name of the package to be used. For example, "CARLA_0.9.9". +# If this environment is not passed to docker build, the value +# is taken from CARLA_VER file inside the repository. +# +# +# It's expected that $(CARLA_HOST)/$(CARLA_RELEASE).tar.gz is a downloadable resource. +# + +env CARLA_HOST "" +env CARLA_RELEASE "" + +# Extract and install python API and resources from CARLA +run export DEFAULT_CARLA_HOST="$(sed -e 's/^\s*HOST\s*=\s*//;t;d' /app/scenario_runner/CARLA_VER)" && \ + echo "$DEFAULT_CARLA_HOST" && \ + export CARLA_HOST="${CARLA_HOST:-$DEFAULT_CARLA_HOST}" && \ + export DEFAULT_CARLA_RELEASE="$(sed -e 's/^\s*RELEASE\s*=\s*//;t;d' /app/scenario_runner/CARLA_VER)" && \ + export CARLA_RELEASE="${CARLA_RELEASE:-$DEFAULT_CARLA_RELEASE}" && \ + echo "$CARLA_HOST/$CARLA_RELEASE.tar.gz" && \ + wget -qO- "$CARLA_HOST/$CARLA_RELEASE.tar.gz" | tar -xzv PythonAPI/carla -C / && \ + mv /PythonAPI/carla /app/ && \ + python3 -m easy_install --no-find-links --no-deps "$(find /app/carla/ -iname '*py3.*.egg' )" + + +# Setup working environment +workdir /app/scenario_runner +env PYTHONPATH "${PYTHONPATH}:/app/carla/agents:/app/carla" +entrypoint ["/bin/sh" ] + diff --git a/scenario_runner/Jenkinsfile b/scenario_runner/Jenkinsfile new file mode 100644 index 0000000..088f794 --- /dev/null +++ b/scenario_runner/Jenkinsfile @@ -0,0 +1,186 @@ +#!/usr/bin/env groovy + + +String CARLA_HOST +String CARLA_RELEASE +String TEST_HOST +String COMMIT +String ECR_REPOSITORY = "456841689987.dkr.ecr.eu-west-3.amazonaws.com/scenario_runner" +boolean CARLA_RUNNING = false +boolean CONCURRENCY = true + +// V3 - include detection of concurrent builds + +pipeline +{ + agent none + + options + { + buildDiscarder(logRotator(numToKeepStr: '3', artifactNumToKeepStr: '3')) + skipDefaultCheckout() + } + + stages + { + stage('setup') + { + agent { label "master" } + steps + { + checkout scm + script + { + jenkinsLib = load("/home/jenkins/scenario_runner.groovy") + TEST_HOST = jenkinsLib.getUbuntuTestNodeHost() + CARLA_HOST= sh( + script: "cat ./CARLA_VER | grep HOST | sed 's/HOST\\s*=\\s*//g'", + returnStdout: true).trim() + CARLA_RELEASE = sh( + script: "cat ./CARLA_VER | grep RELEASE | sed 's/RELEASE\\s*=\\s*//g'", + returnStdout: true).trim() + COMMIT = sh(returnStdout: true, script: "git log -n 1 --pretty=format:'%h'").trim() + } + println "using CARLA version ${CARLA_RELEASE} from ${TEST_HOST}" + } + } + stage('get concurrency status') + { + options + { + lock resource: 'ubuntu_gpu', skipIfLocked: true + } + agent { label "master" } + steps + { + script + { + CONCURRENCY = false + println "no concurrent builds detected." + } + } + } + stage('act on concurrency') + { + agent { label "master" } + steps + { + script + { + if ( CONCURRENCY == true ) + { + println "concurrent builds detected, prebuilding SR image." + stage('prebuild SR docker image') + { + //checkout scm + sh "docker build -t jenkins/scenario_runner:${COMMIT} ." + sh "docker tag jenkins/scenario_runner:${COMMIT} ${ECR_REPOSITORY}:${COMMIT}" + sh '$(aws ecr get-login | sed \'s/ -e none//g\' )' + sh "docker push ${ECR_REPOSITORY}" + sh "docker image rmi -f \"\$(docker images -q ${ECR_REPOSITORY}:${COMMIT})\"" + } + } + } + } + } + stage('lock ubuntu_gpu instance') + { + options + { + lock resource: "ubuntu_gpu" + } + stages + { + stage('start server') + { + agent { label "master" } + steps + { + script + { + jenkinsLib = load("/home/jenkins/scenario_runner.groovy") + jenkinsLib.StartUbuntuTestNode() + } + } + } + stage('deploy') + { + parallel + { + stage('build SR docker image') + { + agent { label "master" } + steps + { + script + { + if ( CONCURRENCY == false ) + { + //checkout scm + sh "docker build -t jenkins/scenario_runner:${COMMIT} ." + sh "docker tag jenkins/scenario_runner:${COMMIT} ${ECR_REPOSITORY}:${COMMIT}" + sh '$(aws ecr get-login | sed \'s/ -e none//g\' )' + sh "docker push ${ECR_REPOSITORY}" + sh "docker image rmi -f \"\$(docker images -q ${ECR_REPOSITORY}:${COMMIT})\"" + } + else + { + println "SR docker image already built due concurrency" + } + } + } + } + stage('deploy CARLA') + { + stages + { + stage('install CARLA') + { + agent { label "secondary && ubuntu && gpu && sr" } + steps + { + println "using CARLA version ${CARLA_RELEASE}" + sh "wget -qO- ${CARLA_HOST}/${CARLA_RELEASE}.tar.gz | tar -xzv -C ." + } + } + } + } + } + } + stage('run test') + { + agent { label "secondary && ubuntu && gpu && sr" } + steps + { + sh 'DISPLAY= ./CarlaUE4.sh -opengl -nosound > CarlaUE4.log&' + sleep 10 + script + { + sh '$(aws ecr get-login | sed \'s/ -e none//g\' )' + sh "docker pull ${ECR_REPOSITORY}:${COMMIT}" + sh "docker container run --rm --network host -e LANG=C.UTF-8 \"${ECR_REPOSITORY}:${COMMIT}\" -c \"python3 scenario_runner.py --scenario FollowLeadingVehicle_1 --debug --output --reloadWorld \"" + deleteDir() + } + } + } + } + post + { + always + { + node('master') + { + script + { + jenkinsLib = load("/home/jenkins/scenario_runner.groovy") + jenkinsLib.StopUbuntuTestNode() + echo 'test node stopped' + sh 'docker system prune --volumes -f' + } + deleteDir() + } + } + } + } + } +} diff --git a/scenario_runner/LICENSE b/scenario_runner/LICENSE new file mode 100644 index 0000000..e1af388 --- /dev/null +++ b/scenario_runner/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 CARLA + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/scenario_runner/manual_control.py b/scenario_runner/manual_control.py new file mode 100644 index 0000000..7e3fcff --- /dev/null +++ b/scenario_runner/manual_control.py @@ -0,0 +1,230 @@ +#!/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 + + 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) + + 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 + +# ============================================================================== +# -- imports ------------------------------------------------------------------- +# ============================================================================== + + +import carla + +from examples.manual_control import (World, HUD, KeyboardControl, CameraManager, + CollisionSensor, LaneInvasionSensor, GnssSensor, IMUSensor) + +import os +import argparse +import logging +import time +import pygame + +# ============================================================================== +# -- 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 WorldSR(World): + + restarted = False + + def restart(self): + + if self.restarted: + return + self.restarted = True + + 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 the ego vehicle + while self.player is None: + print("Waiting for the ego vehicle...") + time.sleep(1) + possible_vehicles = self.world.get_actors().filter('vehicle.*') + for vehicle in possible_vehicles: + if vehicle.attributes['role_name'] == "hero": + print("Ego vehicle found") + self.player = vehicle + break + + self.player_name = self.player.type_id + + # 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) + + def tick(self, clock): + if len(self.world.get_actors().filter(self.player_name)) < 1: + return False + + self.hud.tick(self, clock) + return True + +# ============================================================================== +# -- 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 = WorldSR(client.get_world(), hud, args) + controller = KeyboardControl(world, args.autopilot) + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + if controller.parse_events(client, world, clock): + return + if not world.tick(clock): + return + world.render(display) + pygame.display.flip() + + finally: + + 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='PolarPointBEV 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.rolename = 'hero' # Needed for CARLA version + args.filter = "vehicle.*" # Needed for CARLA version + args.gamma = 2.2 # Needed for CARLA version + 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!') + except Exception as error: + logging.exception(error) + + +if __name__ == '__main__': + + main() diff --git a/scenario_runner/metrics_manager.py b/scenario_runner/metrics_manager.py new file mode 100644 index 0000000..2218182 --- /dev/null +++ b/scenario_runner/metrics_manager.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 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 the execution of user-implemented metrics + +""" +Welcome to the ScenarioRunner's metric module + +This is the main script to be executed when running a metric. +It is responsible of parsing all the information and executing +the metric specified by the user. +""" + +import os +import sys +import importlib +import inspect +import json +import argparse +from argparse import RawTextHelpFormatter + +import carla +from srunner.metrics.tools.metrics_log import MetricsLog + + +class MetricsManager(object): + """ + Main class of the metrics module. Handles the parsing and execution of + the metrics. + """ + + def __init__(self, args): + """ + Initialization of the metrics manager. This creates the client, needed to parse + the information from the recorder, extract the metrics class, and runs it + """ + self._args = args + + # Parse the arguments + recorder_str = self._get_recorder(self._args.log) + criteria_dict = self._get_criteria(self._args.criteria) + + # Get the correct world and load it + map_name = self._get_recorder_map(recorder_str) + world = self._client.load_world(map_name) + town_map = world.get_map() + + # Instanciate the MetricsLog, used to querry the needed information + log = MetricsLog(recorder_str) + + # Read and run the metric class + metric_class = self._get_metric_class(self._args.metric) + metric_class(town_map, log, criteria_dict) + + def _get_recorder(self, log): + """ + Parses the log argument into readable information + """ + + # Get the log information. + self._client = carla.Client(self._args.host, self._args.port) + recorder_file = "{}/{}".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"), log) + + # Check that the file is correct + if recorder_file[-4:] != '.log': + print("ERROR: The log argument has to point to a .log file") + sys.exit(-1) + if not os.path.exists(recorder_file): + print("ERROR: The specified log file does not exist") + sys.exit(-1) + + recorder_str = self._client.show_recorder_file_info(recorder_file, True) + + return recorder_str + + def _get_criteria(self, criteria_file): + """ + Parses the criteria argument into a dictionary + """ + if criteria_file: + with open(criteria_file) as fd: + criteria_dict = json.load(fd) + else: + criteria_dict = None + + return criteria_dict + + def _get_metric_class(self, metric_file): + """ + Function to extract the metrics class from the path given by the metrics + argument. Returns the first class found that is a child of BasicMetric + + Args: + metric_file (str): path to the metric's file. + """ + # Get their module + module_name = os.path.basename(metric_file).split('.')[0] + sys.path.insert(0, os.path.dirname(metric_file)) + metric_module = importlib.import_module(module_name) + + # And their members of type class + for member in inspect.getmembers(metric_module, inspect.isclass): + # Get the first one with parent BasicMetrics + member_parent = member[1].__bases__[0] + if 'BasicMetric' in str(member_parent): + return member[1] + + print("No child class of BasicMetric was found ... Exiting") + sys.exit(-1) + + def _get_recorder_map(self, recorder_str): + """ + Returns the name of the map the simulation took place in + """ + + header = recorder_str.split("\n") + sim_map = header[1][5:] + + return sim_map + + +def main(): + """ + main function + """ + + # pylint: disable=line-too-long + description = ("Scenario Runner's metrics module. Evaluate the execution of a specific scenario by developing your own metric.\n") + + parser = argparse.ArgumentParser(description=description, + formatter_class=RawTextHelpFormatter) + parser.add_argument('--host', default='127.0.0.1', + help='IP of the host server (default: localhost)') + parser.add_argument('--port', '-p', default=2000, + help='PolarPointBEV port to listen to (default: 2000)') + parser.add_argument('--log', required=True, + help='Path to the CARLA recorder .log file (relative to SCENARIO_RUNNER_ROOT).\nThis file is created by the record functionality at ScenarioRunner') + parser.add_argument('--metric', required=True, + help='Path to the .py file defining the used metric.\nSome examples at srunner/metrics') + parser.add_argument('--criteria', default="", + help='Path to the .json file with the criteria information.\nThis file is created by the record functionality at ScenarioRunner') + # pylint: enable=line-too-long + + args = parser.parse_args() + + MetricsManager(args) + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scenario_runner/mkdocs.yml b/scenario_runner/mkdocs.yml new file mode 100644 index 0000000..520d710 --- /dev/null +++ b/scenario_runner/mkdocs.yml @@ -0,0 +1,29 @@ +site_name: CARLA ScenarioRunner +repo_url: https://github.com/carla-simulator/scenario_runner +docs_dir: Docs +edit_uri: 'edit/master/Docs/' +theme: readthedocs +extra_css: [extra.css] + +nav: +- Home: index.md +- Quick start: + - Get ScenarioRunner: getting_scenariorunner.md + - First steps: getting_started.md + - Create a new scenario: creating_new_scenario.md + - Metrics module: metrics_module.md + - FAQ: FAQ.md + - Release Notes: CHANGELOG.md +- References: + - List of scenarios: list_of_scenarios.md + - OpenSCENARIO support: openscenario_support.md +- Contributing: + - Code of conduct: CODE_OF_CONDUCT.md + - Coding standard: coding_standard.md + - Contribution guidelines: CONTRIBUTING.md + +markdown_extensions: + - admonition + - markdown_include.include: + base_path: '.' + diff --git a/scenario_runner/no_rendering_mode.py b/scenario_runner/no_rendering_mode.py new file mode 100644 index 0000000..52f6d95 --- /dev/null +++ b/scenario_runner/no_rendering_mode.py @@ -0,0 +1,1494 @@ +#!/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 visualising a 2D map generated by vehicles. + +""" +Welcome to CARLA No-Rendering Mode Visualizer + + TAB : toggle hero mode + Mouse Wheel : zoom in / zoom out + Mouse Drag : move map (map mode only) + + W : throttle + S : brake + AD : steer + Q : toggle reverse + Space : hand-brake + P : toggle autopilot + M : toggle manual transmission + ,/. : gear up/down + + F1 : toggle HUD + I : toggle actor ids + H/? : toggle help + ESC : quit +""" + +# ============================================================================== +# -- 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 TrafficLightState as tls + +import argparse +import logging +import datetime +import weakref +import math +import random + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import KMOD_SHIFT + 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_d + from pygame.locals import K_h + from pygame.locals import K_i + from pygame.locals import K_m + from pygame.locals import K_p + from pygame.locals import K_q + 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') + +# ============================================================================== +# -- Constants ----------------------------------------------------------------- +# ============================================================================== + +# Colors + +# We will use the color palette used in Tango Desktop Project (Each color is indexed depending on brightness level) +# See: https://en.wikipedia.org/wiki/Tango_Desktop_Project + +COLOR_BUTTER_0 = pygame.Color(252, 233, 79) +COLOR_BUTTER_1 = pygame.Color(237, 212, 0) +COLOR_BUTTER_2 = pygame.Color(196, 160, 0) + +COLOR_ORANGE_0 = pygame.Color(252, 175, 62) +COLOR_ORANGE_1 = pygame.Color(245, 121, 0) +COLOR_ORANGE_2 = pygame.Color(209, 92, 0) + +COLOR_CHOCOLATE_0 = pygame.Color(233, 185, 110) +COLOR_CHOCOLATE_1 = pygame.Color(193, 125, 17) +COLOR_CHOCOLATE_2 = pygame.Color(143, 89, 2) + +COLOR_CHAMELEON_0 = pygame.Color(138, 226, 52) +COLOR_CHAMELEON_1 = pygame.Color(115, 210, 22) +COLOR_CHAMELEON_2 = pygame.Color(78, 154, 6) + +COLOR_SKY_BLUE_0 = pygame.Color(114, 159, 207) +COLOR_SKY_BLUE_1 = pygame.Color(52, 101, 164) +COLOR_SKY_BLUE_2 = pygame.Color(32, 74, 135) + +COLOR_PLUM_0 = pygame.Color(173, 127, 168) +COLOR_PLUM_1 = pygame.Color(117, 80, 123) +COLOR_PLUM_2 = pygame.Color(92, 53, 102) + +COLOR_SCARLET_RED_0 = pygame.Color(239, 41, 41) +COLOR_SCARLET_RED_1 = pygame.Color(204, 0, 0) +COLOR_SCARLET_RED_2 = pygame.Color(164, 0, 0) + +COLOR_ALUMINIUM_0 = pygame.Color(238, 238, 236) +COLOR_ALUMINIUM_1 = pygame.Color(211, 215, 207) +COLOR_ALUMINIUM_2 = pygame.Color(186, 189, 182) +COLOR_ALUMINIUM_3 = pygame.Color(136, 138, 133) +COLOR_ALUMINIUM_4 = pygame.Color(85, 87, 83) +COLOR_ALUMINIUM_4_5 = pygame.Color(66, 62, 64) +COLOR_ALUMINIUM_5 = pygame.Color(46, 52, 54) + + +COLOR_WHITE = pygame.Color(255, 255, 255) +COLOR_BLACK = pygame.Color(0, 0, 0) + + +# Module Defines +MODULE_WORLD = 'WORLD' +MODULE_HUD = 'HUD' +MODULE_INPUT = 'INPUT' + +PIXELS_PER_METER = 12 + +MAP_DEFAULT_SCALE = 0.1 +HERO_DEFAULT_SCALE = 1.0 + +PIXELS_AHEAD_VEHICLE = 150 + +# ============================================================================== +# -- Util ----------------------------------------------------------- +# ============================================================================== + + +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 + + +class Util(object): + + @staticmethod + def blits(destination_surface, source_surfaces, rect=None, blend_mode=0): + for surface in source_surfaces: + destination_surface.blit(surface[0], surface[1], rect, blend_mode) + + @staticmethod + def length(v): + return math.sqrt(v.x**2 + v.y**2 + v.z**2) + + @staticmethod + def get_bounding_box(actor): + bb = actor.trigger_volume.extent + corners = [carla.Location(x=-bb.x, y=-bb.y), + carla.Location(x=bb.x, y=-bb.y), + carla.Location(x=bb.x, y=bb.y), + carla.Location(x=-bb.x, y=bb.y), + carla.Location(x=-bb.x, y=-bb.y)] + corners = [x + actor.trigger_volume.location for x in corners] + t = actor.get_transform() + t.transform(corners) + return corners + +# ============================================================================== +# -- ModuleManager ------------------------------------------------------------- +# ============================================================================== + + +class ModuleManager(object): + def __init__(self): + self.modules = [] + + def register_module(self, module): + self.modules.append(module) + + def clear_modules(self): + del self.modules[:] + + def tick(self, clock): + # Update all the modules + for module in self.modules: + module.tick(clock) + + def render(self, display): + display.fill(COLOR_ALUMINIUM_4) + for module in self.modules: + module.render(display) + + def get_module(self, name): + for module in self.modules: + if module.name == name: + return module + + def start_modules(self): + for module in self.modules: + module.start() + + +# ============================================================================== +# -- 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=COLOR_WHITE, seconds=2.0): + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill(COLOR_BLACK) + 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(COLOR_BLACK) + for n, line in enumerate(lines): + text_texture = self.font.render(line, True, COLOR_WHITE) + 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) + + +# ============================================================================== +# -- ModuleHUD ----------------------------------------------------------------- +# ============================================================================== + + +class ModuleHUD (object): + + def __init__(self, name, width, height): + self.name = name + self.dim = (width, height) + self._init_hud_params() + self._init_data_params() + + def start(self): + pass + + def _init_hud_params(self): + 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._header_font = pygame.font.SysFont('Arial', 14, True) + self.help = HelpText(pygame.font.Font(mono, 24), *self.dim) + self._notifications = FadingText( + pygame.font.Font(pygame.font.get_default_font(), 20), + (self.dim[0], 40), (0, self.dim[1] - 40)) + + def _init_data_params(self): + self.show_info = True + self.show_actor_ids = False + self._info_text = {} + + def notification(self, text, seconds=2.0): + self._notifications.set_text(text, seconds=seconds) + + def tick(self, clock): + self._notifications.tick(clock) + + def add_info(self, module_name, info): + self._info_text[module_name] = info + + def render_vehicles_ids(self, vehicle_id_surface, list_actors, world_to_pixel, hero_actor, hero_transform): + vehicle_id_surface.fill(COLOR_BLACK) + if self.show_actor_ids: + vehicle_id_surface.set_alpha(150) + for actor in list_actors: + x, y = world_to_pixel(actor[1].location) + + angle = 0 + if hero_actor is not None: + angle = -hero_transform.rotation.yaw - 90 + + color = COLOR_SKY_BLUE_0 + if int(actor[0].attributes['number_of_wheels']) == 2: + color = COLOR_CHOCOLATE_0 + if actor[0].attributes['role_name'] == 'hero': + color = COLOR_CHAMELEON_0 + + font_surface = self._header_font.render(str(actor[0].id), True, color) + rotated_font_surface = pygame.transform.rotate(font_surface, angle) + rect = rotated_font_surface.get_rect(center=(x, y)) + vehicle_id_surface.blit(rotated_font_surface, rect) + + return vehicle_id_surface + + def render(self, display): + if self.show_info: + info_surface = pygame.Surface((240, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + i = 0 + for module_name, module_info in self._info_text.items(): + if not module_info: + continue + surface = self._header_font.render(module_name, True, COLOR_ALUMINIUM_0).convert_alpha() + display.blit(surface, (8 + bar_width / 2, 18 * i + v_offset)) + v_offset += 12 + i += 1 + for item in module_info: + 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 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, COLOR_ALUMINIUM_0, 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, COLOR_ALUMINIUM_0, 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, COLOR_ALUMINIUM_0, rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, COLOR_ALUMINIUM_0).convert_alpha() + display.blit(surface, (8, 18 * i + v_offset)) + v_offset += 18 + v_offset += 24 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- TrafficLightSurfaces ------------------------------------------------------ +# ============================================================================== + + +class TrafficLightSurfaces(object): + """Holds the surfaces (scaled and rotated) for painting traffic lights""" + + def __init__(self): + def make_surface(tl): + w = 40 + surface = pygame.Surface((w, 3 * w), pygame.SRCALPHA) + surface.fill(COLOR_ALUMINIUM_5 if tl != 'h' else COLOR_ORANGE_2) + if tl != 'h': + hw = int(w / 2) + off = COLOR_ALUMINIUM_4 + red = COLOR_SCARLET_RED_0 + yellow = COLOR_BUTTER_0 + green = COLOR_CHAMELEON_0 + pygame.draw.circle(surface, red if tl == tls.Red else off, (hw, hw), int(0.4 * w)) + pygame.draw.circle(surface, yellow if tl == tls.Yellow else off, (hw, w + hw), int(0.4 * w)) + pygame.draw.circle(surface, green if tl == tls.Green else off, (hw, 2 * w + hw), int(0.4 * w)) + return pygame.transform.smoothscale(surface, (15, 45) if tl != 'h' else (19, 49)) + self._original_surfaces = { + 'h': make_surface('h'), + tls.Red: make_surface(tls.Red), + tls.Yellow: make_surface(tls.Yellow), + tls.Green: make_surface(tls.Green), + tls.Off: make_surface(tls.Off), + tls.Unknown: make_surface(tls.Unknown) + } + self.surfaces = dict(self._original_surfaces) + + def rotozoom(self, angle, scale): + for key, surface in self._original_surfaces.items(): + self.surfaces[key] = pygame.transform.rotozoom(surface, angle, scale) + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class MapImage(object): + def __init__(self, carla_world, carla_map, pixels_per_meter, show_triggers, show_connections, show_spawn_points): + self._pixels_per_meter = pixels_per_meter + self.scale = 1.0 + self.show_triggers = show_triggers + self.show_connections = show_connections + self.show_spawn_points = show_spawn_points + + waypoints = carla_map.generate_waypoints(2) + margin = 50 + max_x = max(waypoints, key=lambda x: x.transform.location.x).transform.location.x + margin + max_y = max(waypoints, key=lambda x: x.transform.location.y).transform.location.y + margin + min_x = min(waypoints, key=lambda x: x.transform.location.x).transform.location.x - margin + min_y = min(waypoints, key=lambda x: x.transform.location.y).transform.location.y - margin + + self.width = max(max_x - min_x, max_y - min_y) + self._world_offset = (min_x, min_y) + + width_in_pixels = int(self._pixels_per_meter * self.width) + + self.big_map_surface = pygame.Surface((width_in_pixels, width_in_pixels)).convert() + self.draw_road_map(self.big_map_surface, carla_world, carla_map, self.world_to_pixel, self.world_to_pixel_width) + self.surface = self.big_map_surface + + def draw_road_map(self, map_surface, carla_world, carla_map, world_to_pixel, world_to_pixel_width): + map_surface.fill(COLOR_ALUMINIUM_4) + precision = 0.05 + + def lane_marking_color_to_tango(lane_marking_color): + tango_color = COLOR_BLACK + + if lane_marking_color == carla.LaneMarkingColor.White: + tango_color = COLOR_ALUMINIUM_2 + + elif lane_marking_color == carla.LaneMarkingColor.Blue: + tango_color = COLOR_SKY_BLUE_0 + + elif lane_marking_color == carla.LaneMarkingColor.Green: + tango_color = COLOR_CHAMELEON_0 + + elif lane_marking_color == carla.LaneMarkingColor.Red: + tango_color = COLOR_SCARLET_RED_0 + + elif lane_marking_color == carla.LaneMarkingColor.Yellow: + tango_color = COLOR_ORANGE_0 + + return tango_color + + def draw_solid_line(surface, color, closed, points, width): + if len(points) >= 2: + pygame.draw.lines(surface, color, closed, points, width) + + def draw_broken_line(surface, color, closed, points, width): + broken_lines = [x for n, x in enumerate(zip(*(iter(points),) * 20)) if n % 3 == 0] + for line in broken_lines: + pygame.draw.lines(surface, color, closed, line, width) + + def get_lane_markings(lane_marking_type, lane_marking_color, waypoints, sign): + margin = 0.20 + if lane_marking_type == carla.LaneMarkingType.Broken or (lane_marking_type == carla.LaneMarkingType.Solid): + marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints] + return [(lane_marking_type, lane_marking_color, marking_1)] + elif lane_marking_type == carla.LaneMarkingType.SolidBroken or lane_marking_type == carla.LaneMarkingType.BrokenSolid: + marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints] + marking_2 = [world_to_pixel(lateral_shift(w.transform, + sign * (w.lane_width * 0.5 + margin * 2))) for w in waypoints] + return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1), + (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.BrokenBroken: + marking = [world_to_pixel(lateral_shift(w.transform, + sign * (w.lane_width * 0.5 - margin))) for w in waypoints] + return [(carla.LaneMarkingType.Broken, lane_marking_color, marking)] + elif lane_marking_type == carla.LaneMarkingType.SolidSolid: + marking = [world_to_pixel(lateral_shift(w.transform, + sign * ((w.lane_width * 0.5) - margin))) for w in waypoints] + return [(carla.LaneMarkingType.Solid, lane_marking_color, marking)] + + return [(carla.LaneMarkingType.NONE, carla.LaneMarkingColor.Other, [])] + + def draw_lane_marking(surface, waypoints, is_left): + sign = -1 if is_left else 1 + lane_marking = None + + marking_type = carla.LaneMarkingType.NONE + previous_marking_type = carla.LaneMarkingType.NONE + + marking_color = carla.LaneMarkingColor.Other + previous_marking_color = carla.LaneMarkingColor.Other + + waypoints_list = [] + temp_waypoints = [] + current_lane_marking = carla.LaneMarkingType.NONE + for sample in waypoints: + lane_marking = sample.left_lane_marking if sign < 0 else sample.right_lane_marking + + if lane_marking is None: + continue + + marking_type = lane_marking.type + marking_color = lane_marking.color + + if current_lane_marking != marking_type: + markings = get_lane_markings( + previous_marking_type, + lane_marking_color_to_tango(previous_marking_color), + temp_waypoints, + sign) + current_lane_marking = marking_type + + for marking in markings: + waypoints_list.append(marking) + + temp_waypoints = temp_waypoints[-1:] + + else: + temp_waypoints.append((sample)) + previous_marking_type = marking_type + previous_marking_color = marking_color + + # Add last marking + last_markings = get_lane_markings( + previous_marking_type, + lane_marking_color_to_tango(previous_marking_color), + temp_waypoints, + sign) + for marking in last_markings: + waypoints_list.append(marking) + + for markings in waypoints_list: + if markings[0] == carla.LaneMarkingType.Solid: + draw_solid_line(surface, markings[1], False, markings[2], 2) + elif markings[0] == carla.LaneMarkingType.Broken: + draw_broken_line(surface, markings[1], False, markings[2], 2) + + def draw_arrow(surface, transform, color=COLOR_ALUMINIUM_2): + transform.rotation.yaw += 180 + forward = transform.get_forward_vector() + transform.rotation.yaw += 90 + right_dir = transform.get_forward_vector() + end = transform.location + start = end - 2.0 * forward + right = start + 0.8 * forward + 0.4 * right_dir + left = start + 0.8 * forward - 0.4 * right_dir + pygame.draw.lines( + surface, color, False, [ + world_to_pixel(x) for x in [ + start, end]], 4) + pygame.draw.lines( + surface, color, False, [ + world_to_pixel(x) for x in [ + left, start, right]], 4) + + def draw_traffic_signs(surface, font_surface, actor, color=COLOR_ALUMINIUM_2, trigger_color=COLOR_PLUM_0): + transform = actor.get_transform() + waypoint = carla_map.get_waypoint(transform.location) + + angle = -waypoint.transform.rotation.yaw - 90.0 + font_surface = pygame.transform.rotate(font_surface, angle) + pixel_pos = world_to_pixel(waypoint.transform.location) + offset = font_surface.get_rect(center=(pixel_pos[0], pixel_pos[1])) + surface.blit(font_surface, offset) + + # Draw line in front of stop + forward_vector = carla.Location(waypoint.transform.get_forward_vector()) + left_vector = carla.Location(-forward_vector.y, forward_vector.x, + forward_vector.z) * waypoint.lane_width / 2 * 0.7 + + line = [(waypoint.transform.location + (forward_vector * 1.5) + (left_vector)), + (waypoint.transform.location + (forward_vector * 1.5) - (left_vector))] + + line_pixel = [world_to_pixel(p) for p in line] + pygame.draw.lines(surface, color, True, line_pixel, 2) + + # draw bounding box + if self.show_triggers: + corners = Util.get_bounding_box(actor) + corners = [world_to_pixel(p) for p in corners] + pygame.draw.lines(surface, trigger_color, True, corners, 2) + + def lateral_shift(transform, shift): + transform.rotation.yaw += 90 + return transform.location + shift * transform.get_forward_vector() + + def draw_topology(carla_topology, index): + topology = [x[index] for x in carla_topology] + topology = sorted(topology, key=lambda w: w.transform.location.z) + for waypoint in topology: + # if waypoint.road_id == 150 or waypoint.road_id == 16: + waypoints = [waypoint] + + nxt = waypoint.next(precision) + if len(nxt) > 0: + nxt = nxt[0] + while nxt.road_id == waypoint.road_id: + waypoints.append(nxt) + nxt = nxt.next(precision) + if len(nxt) > 0: + nxt = nxt[0] + else: + break + + # Draw Road + road_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in waypoints] + road_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in waypoints] + + polygon = road_left_side + [x for x in reversed(road_right_side)] + polygon = [world_to_pixel(x) for x in polygon] + + if len(polygon) > 2: + pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon, 5) + pygame.draw.polygon(map_surface, COLOR_ALUMINIUM_5, polygon) + + # Draw Shoulders and Parkings + PARKING_COLOR = COLOR_ALUMINIUM_4_5 + SHOULDER_COLOR = COLOR_ALUMINIUM_5 + + final_color = SHOULDER_COLOR + + # Draw Right + shoulder = [] + for w in waypoints: + r = w.get_right_lane() + if r is not None and ( + r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking): + if r.lane_type == carla.LaneType.Parking: + final_color = PARKING_COLOR + shoulder.append(r) + + shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder] + shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder] + + polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)] + polygon = [world_to_pixel(x) for x in polygon] + + if len(polygon) > 2: + pygame.draw.polygon(map_surface, final_color, polygon, 5) + pygame.draw.polygon(map_surface, final_color, polygon) + + draw_lane_marking( + map_surface, + shoulder, + False) + + # Draw Left + shoulder = [] + for w in waypoints: + r = w.get_left_lane() + if r is not None and ( + r.lane_type == carla.LaneType.Shoulder or r.lane_type == carla.LaneType.Parking): + if r.lane_type == carla.LaneType.Parking: + final_color = PARKING_COLOR + shoulder.append(r) + + shoulder_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in shoulder] + shoulder_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in shoulder] + + polygon = shoulder_left_side + [x for x in reversed(shoulder_right_side)] + polygon = [world_to_pixel(x) for x in polygon] + + if len(polygon) > 2: + pygame.draw.polygon(map_surface, final_color, polygon, 5) + pygame.draw.polygon(map_surface, final_color, polygon) + + draw_lane_marking( + map_surface, + shoulder, + True) + + # Draw Lane Markings and Arrows + if not waypoint.is_intersection: + draw_lane_marking( + map_surface, + waypoints, + True) + draw_lane_marking( + map_surface, + waypoints, + False) + for n, wp in enumerate(waypoints): + if ((n + 1) % 400) == 0: + draw_arrow(map_surface, wp.transform) + + topology = carla_map.get_topology() + draw_topology(topology, 0) + draw_topology(topology, 1) + + if self.show_spawn_points: + for sp in carla_map.get_spawn_points(): + draw_arrow(map_surface, sp, color=COLOR_CHOCOLATE_0) + + if self.show_connections: + dist = 1.5 + to_pixel = lambda wp: world_to_pixel(wp.transform.location) + for wp in carla_map.generate_waypoints(dist): + col = (0, 255, 255) if wp.is_intersection else (0, 255, 0) + for nxt in wp.next(dist): + pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(nxt), 2) + if wp.lane_change & carla.LaneChange.Right: + r = wp.get_right_lane() + if r and r.lane_type == carla.LaneType.Driving: + pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(r), 2) + if wp.lane_change & carla.LaneChange.Left: + l = wp.get_left_lane() + if l and l.lane_type == carla.LaneType.Driving: + pygame.draw.line(map_surface, col, to_pixel(wp), to_pixel(l), 2) + + actors = carla_world.get_actors() + + # Draw Traffic Signs + font_size = world_to_pixel_width(1) + font = pygame.font.SysFont('Arial', font_size, True) + + stops = [actor for actor in actors if 'stop' in actor.type_id] + yields = [actor for actor in actors if 'yield' in actor.type_id] + + stop_font_surface = font.render("STOP", False, COLOR_ALUMINIUM_2) + stop_font_surface = pygame.transform.scale( + stop_font_surface, (stop_font_surface.get_width(), stop_font_surface.get_height() * 2)) + + yield_font_surface = font.render("YIELD", False, COLOR_ALUMINIUM_2) + yield_font_surface = pygame.transform.scale( + yield_font_surface, (yield_font_surface.get_width(), yield_font_surface.get_height() * 2)) + + for ts_stop in stops: + draw_traffic_signs(map_surface, stop_font_surface, ts_stop, trigger_color=COLOR_SCARLET_RED_1) + + for ts_yield in yields: + draw_traffic_signs(map_surface, yield_font_surface, ts_yield, trigger_color=COLOR_ORANGE_1) + + def world_to_pixel(self, location, offset=(0, 0)): + x = self.scale * self._pixels_per_meter * (location.x - self._world_offset[0]) + y = self.scale * self._pixels_per_meter * (location.y - self._world_offset[1]) + return [int(x - offset[0]), int(y - offset[1])] + + def world_to_pixel_width(self, width): + return int(self.scale * self._pixels_per_meter * width) + + def scale_map(self, scale): + if scale != self.scale: + self.scale = scale + width = int(self.big_map_surface.get_width() * self.scale) + self.surface = pygame.transform.smoothscale(self.big_map_surface, (width, width)) + + +class ModuleWorld(object): + def __init__(self, name, args, timeout): + self.client = None + self.name = name + self.args = args + self.timeout = timeout + self.server_fps = 0.0 + self.simulation_time = 0 + self.server_clock = pygame.time.Clock() + + # World data + self.world = None + self.town_map = None + self.actors_with_transforms = [] + # Store necessary modules + self.module_hud = None + self.module_input = None + + self.surface_size = [0, 0] + self.prev_scaled_size = 0 + self.scaled_size = 0 + # Hero actor + self.hero_actor = None + self.spawned_hero = None + self.hero_transform = None + + self.scale_offset = [0, 0] + + self.vehicle_id_surface = None + self.result_surface = None + + self.traffic_light_surfaces = TrafficLightSurfaces() + self.affected_traffic_light = None + + # Map info + self.map_image = None + self.border_round_surface = None + self.original_surface_size = None + self.hero_surface = None + self.actors_surface = None + + def _get_data_from_carla(self): + try: + self.client = carla.Client(self.args.host, self.args.port) + self.client.set_timeout(self.timeout) + + if self.args.map is None: + world = self.client.get_world() + else: + world = self.client.load_world(self.args.map) + + town_map = world.get_map() + return (world, town_map) + + except RuntimeError as ex: + logging.error(ex) + exit_game() + + def start(self): + self.world, self.town_map = self._get_data_from_carla() + + # Create Surfaces + self.map_image = MapImage( + carla_world=self.world, + carla_map=self.town_map, + pixels_per_meter=PIXELS_PER_METER, + show_triggers=self.args.show_triggers, + show_connections=self.args.show_connections, + show_spawn_points=self.args.show_spawn_points) + + # Store necessary modules + self.module_hud = module_manager.get_module(MODULE_HUD) + self.module_input = module_manager.get_module(MODULE_INPUT) + + self.original_surface_size = min(self.module_hud.dim[0], self.module_hud.dim[1]) + self.surface_size = self.map_image.big_map_surface.get_width() + + self.scaled_size = int(self.surface_size) + self.prev_scaled_size = int(self.surface_size) + + # Render Actors + self.actors_surface = pygame.Surface((self.map_image.surface.get_width(), self.map_image.surface.get_height())) + self.actors_surface.set_colorkey(COLOR_BLACK) + + self.vehicle_id_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() + self.vehicle_id_surface.set_colorkey(COLOR_BLACK) + + self.border_round_surface = pygame.Surface(self.module_hud.dim, pygame.SRCALPHA).convert() + self.border_round_surface.set_colorkey(COLOR_WHITE) + self.border_round_surface.fill(COLOR_BLACK) + + center_offset = (int(self.module_hud.dim[0] / 2), int(self.module_hud.dim[1] / 2)) + pygame.draw.circle(self.border_round_surface, COLOR_ALUMINIUM_1, center_offset, int(self.module_hud.dim[1] / 2)) + pygame.draw.circle(self.border_round_surface, COLOR_WHITE, center_offset, int((self.module_hud.dim[1] - 8) / 2)) + + scaled_original_size = self.original_surface_size * (1.0 / 0.9) + self.hero_surface = pygame.Surface((scaled_original_size, scaled_original_size)).convert() + + self.result_surface = pygame.Surface((self.surface_size, self.surface_size)).convert() + self.result_surface.set_colorkey(COLOR_BLACK) + + # Start hero mode by default + self.select_hero_actor() + self.hero_actor.set_autopilot(False) + self.module_input.wheel_offset = HERO_DEFAULT_SCALE + self.module_input.control = carla.VehicleControl() + + weak_self = weakref.ref(self) + self.world.on_tick(lambda timestamp: ModuleWorld.on_world_tick(weak_self, timestamp)) + + def select_hero_actor(self): + hero_vehicles = [actor for actor in self.world.get_actors( + ) if 'vehicle' in actor.type_id and actor.attributes['role_name'] == 'hero'] + if len(hero_vehicles) > 0: + self.hero_actor = random.choice(hero_vehicles) + self.hero_transform = self.hero_actor.get_transform() + else: + self._spawn_hero() + + def _spawn_hero(self): + # Get a random blueprint. + blueprint = random.choice(self.world.get_blueprint_library().filter(self.args.filter)) + blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + # Spawn the player. + while self.hero_actor is None: + spawn_points = self.world.get_map().get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + self.hero_actor = self.world.try_spawn_actor(blueprint, spawn_point) + self.hero_transform = self.hero_actor.get_transform() + + # Save it in order to destroy it when closing program + self.spawned_hero = self.hero_actor + + def tick(self, clock): + actors = self.world.get_actors() + self.actors_with_transforms = [(actor, actor.get_transform()) for actor in actors] + if self.hero_actor is not None: + self.hero_transform = self.hero_actor.get_transform() + self.update_hud_info(clock) + + def update_hud_info(self, clock): + hero_mode_text = [] + if self.hero_actor is not None: + hero_speed = self.hero_actor.get_velocity() + hero_speed_text = 3.6 * math.sqrt(hero_speed.x ** 2 + hero_speed.y ** 2 + hero_speed.z ** 2) + + affected_traffic_light_text = 'None' + if self.affected_traffic_light is not None: + state = self.affected_traffic_light.state + if state == carla.TrafficLightState.Green: + affected_traffic_light_text = 'GREEN' + elif state == carla.TrafficLightState.Yellow: + affected_traffic_light_text = 'YELLOW' + else: + affected_traffic_light_text = 'RED' + + affected_speed_limit_text = self.hero_actor.get_speed_limit() + + hero_mode_text = [ + 'Hero Mode: ON', + 'Hero ID: %7d' % self.hero_actor.id, + 'Hero Vehicle: %14s' % get_actor_display_name(self.hero_actor, truncate=14), + 'Hero Speed: %3d km/h' % hero_speed_text, + 'Hero Affected by:', + ' Traffic Light: %12s' % affected_traffic_light_text, + ' Speed Limit: %3d km/h' % affected_speed_limit_text + ] + else: + hero_mode_text = ['Hero Mode: OFF'] + + self.server_fps = self.server_clock.get_fps() + self.server_fps = 'inf' if self.server_fps == float('inf') else round(self.server_fps) + module_info_text = [ + 'Server: % 16s FPS' % self.server_fps, + 'Client: % 16s FPS' % round(clock.get_fps()), + 'Simulation Time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + 'Map Name: %10s' % self.town_map.name, + ] + + module_info_text = module_info_text + module_hud = module_manager.get_module(MODULE_HUD) + module_hud.add_info(self.name, module_info_text) + module_hud.add_info('HERO', hero_mode_text) + + @staticmethod + def on_world_tick(weak_self, timestamp): + self = weak_self() + if not self: + return + + self.server_clock.tick() + self.server_fps = self.server_clock.get_fps() + self.simulation_time = timestamp.elapsed_seconds + + def _split_actors(self): + vehicles = [] + traffic_lights = [] + speed_limits = [] + walkers = [] + + for actor_with_transform in self.actors_with_transforms: + actor = actor_with_transform[0] + if 'vehicle' in actor.type_id: + vehicles.append(actor_with_transform) + elif 'traffic_light' in actor.type_id: + traffic_lights.append(actor_with_transform) + elif 'speed_limit' in actor.type_id: + speed_limits.append(actor_with_transform) + elif 'walker' in actor.type_id: + walkers.append(actor_with_transform) + + info_text = [] + if self.hero_actor is not None and len(vehicles) > 1: + location = self.hero_transform.location + vehicle_list = [x[0] for x in vehicles if x[0].id != self.hero_actor.id] + + def distance(v): return location.distance(v.get_location()) + for n, vehicle in enumerate(sorted(vehicle_list, key=distance)): + if n > 15: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + info_text.append('% 5d %s' % (vehicle.id, vehicle_type)) + module_manager.get_module(MODULE_HUD).add_info( + 'NEARBY VEHICLES', + info_text) + + return (vehicles, traffic_lights, speed_limits, walkers) + + def _render_traffic_lights(self, surface, list_tl, world_to_pixel): + self.affected_traffic_light = None + + for tl in list_tl: + world_pos = tl.get_location() + pos = world_to_pixel(world_pos) + + if self.args.show_triggers: + corners = Util.get_bounding_box(tl) + corners = [world_to_pixel(p) for p in corners] + pygame.draw.lines(surface, COLOR_BUTTER_1, True, corners, 2) + + if self.hero_actor is not None: + corners = Util.get_bounding_box(tl) + corners = [world_to_pixel(p) for p in corners] + tl_t = tl.get_transform() + + transformed_tv = tl_t.transform(tl.trigger_volume.location) + hero_location = self.hero_actor.get_location() + d = hero_location.distance(transformed_tv) + s = Util.length(tl.trigger_volume.extent) + Util.length(self.hero_actor.bounding_box.extent) + if (d <= s): + # Highlight traffic light + self.affected_traffic_light = tl + srf = self.traffic_light_surfaces.surfaces['h'] + surface.blit(srf, srf.get_rect(center=pos)) + + srf = self.traffic_light_surfaces.surfaces[tl.state] + surface.blit(srf, srf.get_rect(center=pos)) + + def _render_speed_limits(self, surface, list_sl, world_to_pixel, world_to_pixel_width): + + font_size = world_to_pixel_width(2) + radius = world_to_pixel_width(2) + font = pygame.font.SysFont('Arial', font_size) + + for sl in list_sl: + + x, y = world_to_pixel(sl.get_location()) + + # Render speed limit + white_circle_radius = int(radius * 0.75) + + pygame.draw.circle(surface, COLOR_SCARLET_RED_1, (x, y), radius) + pygame.draw.circle(surface, COLOR_ALUMINIUM_0, (x, y), white_circle_radius) + + limit = sl.type_id.split('.')[2] + font_surface = font.render(limit, True, COLOR_ALUMINIUM_5) + + if self.args.show_triggers: + corners = Util.get_bounding_box(sl) + corners = [world_to_pixel(p) for p in corners] + pygame.draw.lines(surface, COLOR_PLUM_2, True, corners, 2) + + # Blit + if self.hero_actor is not None: + # Rotate font surface with respect to hero vehicle front + angle = -self.hero_transform.rotation.yaw - 90.0 + font_surface = pygame.transform.rotate(font_surface, angle) + offset = font_surface.get_rect(center=(x, y)) + surface.blit(font_surface, offset) + + else: + surface.blit(font_surface, (x - radius / 2, y - radius / 2)) + + def _render_walkers(self, surface, list_w, world_to_pixel): + for w in list_w: + color = COLOR_PLUM_0 + + # Compute bounding box points + bb = w[0].bounding_box.extent + corners = [ + carla.Location(x=-bb.x, y=-bb.y), + carla.Location(x=bb.x, y=-bb.y), + carla.Location(x=bb.x, y=bb.y), + carla.Location(x=-bb.x, y=bb.y)] + + w[1].transform(corners) + corners = [world_to_pixel(p) for p in corners] + pygame.draw.polygon(surface, color, corners) + + def _render_vehicles(self, surface, list_v, world_to_pixel): + + for v in list_v: + color = COLOR_SKY_BLUE_0 + if int(v[0].attributes['number_of_wheels']) == 2: + color = COLOR_CHOCOLATE_1 + if v[0].attributes['role_name'] == 'hero': + color = COLOR_CHAMELEON_0 + # Compute bounding box points + bb = v[0].bounding_box.extent + corners = [carla.Location(x=-bb.x, y=-bb.y), + carla.Location(x=bb.x - 0.8, y=-bb.y), + carla.Location(x=bb.x, y=0), + carla.Location(x=bb.x - 0.8, y=bb.y), + carla.Location(x=-bb.x, y=bb.y), + carla.Location(x=-bb.x, y=-bb.y) + ] + v[1].transform(corners) + corners = [world_to_pixel(p) for p in corners] + pygame.draw.lines(surface, color, False, corners, int(math.ceil(4.0 * self.map_image.scale))) + + def render_actors(self, surface, vehicles, traffic_lights, speed_limits, walkers): + # Static actors + self._render_traffic_lights(surface, [tl[0] for tl in traffic_lights], self.map_image.world_to_pixel) + self._render_speed_limits(surface, [sl[0] for sl in speed_limits], self.map_image.world_to_pixel, + self.map_image.world_to_pixel_width) + + # Dynamic actors + self._render_vehicles(surface, vehicles, self.map_image.world_to_pixel) + self._render_walkers(surface, walkers, self.map_image.world_to_pixel) + + def clip_surfaces(self, clipping_rect): + self.actors_surface.set_clip(clipping_rect) + self.vehicle_id_surface.set_clip(clipping_rect) + self.result_surface.set_clip(clipping_rect) + + def _compute_scale(self, scale_factor): + m = self.module_input.mouse_pos + + # Percentage of surface where mouse position is actually + px = (m[0] - self.scale_offset[0]) / float(self.prev_scaled_size) + py = (m[1] - self.scale_offset[1]) / float(self.prev_scaled_size) + + # Offset will be the previously accumulated offset added with the + # difference of mouse positions in the old and new scales + diff_between_scales = ((float(self.prev_scaled_size) * px) - (float(self.scaled_size) * px), + (float(self.prev_scaled_size) * py) - (float(self.scaled_size) * py)) + + self.scale_offset = (self.scale_offset[0] + diff_between_scales[0], + self.scale_offset[1] + diff_between_scales[1]) + + # Update previous scale + self.prev_scaled_size = self.scaled_size + + # Scale performed + self.map_image.scale_map(scale_factor) + + def render(self, display): + if self.actors_with_transforms is None: + return + self.result_surface.fill(COLOR_BLACK) + vehicles, traffic_lights, speed_limits, walkers = self._split_actors() + + scale_factor = self.module_input.wheel_offset + self.scaled_size = int(self.map_image.width * scale_factor) + if self.scaled_size != self.prev_scaled_size: + self._compute_scale(scale_factor) + + # Render Actors + + self.actors_surface.fill(COLOR_BLACK) + self.render_actors( + self.actors_surface, + vehicles, + traffic_lights, + speed_limits, + walkers) + + # Render Ids + self.module_hud.render_vehicles_ids(self.vehicle_id_surface, vehicles, + self.map_image.world_to_pixel, self.hero_actor, self.hero_transform) + + # Blit surfaces + surfaces = ((self.map_image.surface, (0, 0)), + (self.actors_surface, (0, 0)), + (self.vehicle_id_surface, (0, 0)), + ) + + angle = 0.0 if self.hero_actor is None else self.hero_transform.rotation.yaw + 90.0 + self.traffic_light_surfaces.rotozoom(-angle, self.map_image.scale) + + center_offset = (0, 0) + if self.hero_actor is not None: + + hero_location_screen = self.map_image.world_to_pixel(self.hero_transform.location) + hero_front = self.hero_transform.get_forward_vector() + translation_offset = ( + hero_location_screen[0] - + self.hero_surface.get_width() / + 2 + + hero_front.x * + PIXELS_AHEAD_VEHICLE, + (hero_location_screen[1] - + self.hero_surface.get_height() / + 2 + + hero_front.y * + PIXELS_AHEAD_VEHICLE)) + + # Apply clipping rect + clipping_rect = pygame.Rect(translation_offset[0], + translation_offset[1], + self.hero_surface.get_width(), + self.hero_surface.get_height()) + self.clip_surfaces(clipping_rect) + + Util.blits(self.result_surface, surfaces) + + self.border_round_surface.set_clip(clipping_rect) + + self.hero_surface.fill(COLOR_ALUMINIUM_4) + self.hero_surface.blit(self.result_surface, (-translation_offset[0], + -translation_offset[1])) + + rotated_result_surface = pygame.transform.rotozoom(self.hero_surface, angle, 0.9).convert() + + center = (display.get_width() / 2, display.get_height() / 2) + rotation_pivot = rotated_result_surface.get_rect(center=center) + display.blit(rotated_result_surface, rotation_pivot) + + display.blit(self.border_round_surface, (0, 0)) + else: + # Translation offset + translation_offset = (self.module_input.mouse_offset[0] * scale_factor + self.scale_offset[0], + self.module_input.mouse_offset[1] * scale_factor + self.scale_offset[1]) + center_offset = (abs(display.get_width() - self.surface_size) / 2 * scale_factor, 0) + + # Apply clipping rect + clipping_rect = pygame.Rect(-translation_offset[0] - center_offset[0], -translation_offset[1], + self.module_hud.dim[0], self.module_hud.dim[1]) + self.clip_surfaces(clipping_rect) + Util.blits(self.result_surface, surfaces) + + display.blit(self.result_surface, (translation_offset[0] + center_offset[0], + translation_offset[1])) + + def destroy(self): + if self.spawned_hero is not None: + self.spawned_hero.destroy() +# ============================================================================== +# -- Input ----------------------------------------------------------- +# ============================================================================== + + +class ModuleInput(object): + def __init__(self, name): + self.name = name + self.mouse_pos = (0, 0) + self.mouse_offset = [0.0, 0.0] + self.wheel_offset = 0.1 + self.wheel_amount = 0.025 + self._steer_cache = 0.0 + self.control = None + self._autopilot_enabled = False + + def start(self): + hud = module_manager.get_module(MODULE_HUD) + hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def render(self, display): + pass + + def tick(self, clock): + self.parse_input(clock) + + def _parse_events(self): + self.mouse_pos = pygame.mouse.get_pos() + for event in pygame.event.get(): + if event.type == pygame.QUIT: + exit_game() + elif event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + exit_game() + elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): + module_hud = module_manager.get_module(MODULE_HUD) + module_hud.help.toggle() + elif event.key == K_TAB: + module_world = module_manager.get_module(MODULE_WORLD) + module_hud = module_manager.get_module(MODULE_HUD) + if module_world.hero_actor is None: + module_world.select_hero_actor() + self.wheel_offset = HERO_DEFAULT_SCALE + self.control = carla.VehicleControl() + module_hud.notification('Hero Mode') + else: + self.wheel_offset = MAP_DEFAULT_SCALE + self.mouse_offset = [0, 0] + self.mouse_pos = [0, 0] + module_world.scale_offset = [0, 0] + module_world.hero_actor = None + module_hud.notification('Map Mode') + elif event.key == K_F1: + module_hud = module_manager.get_module(MODULE_HUD) + module_hud.show_info = not module_hud.show_info + elif event.key == K_i: + module_hud = module_manager.get_module(MODULE_HUD) + module_hud.show_actor_ids = not module_hud.show_actor_ids + elif 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 + world = module_manager.get_module(MODULE_WORLD) + self.control.gear = world.hero_actor.get_control().gear + module_hud = module_manager.get_module(MODULE_HUD) + module_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: + world = module_manager.get_module(MODULE_WORLD) + if world.hero_actor is not None: + self._autopilot_enabled = not self._autopilot_enabled + world.hero_actor.set_autopilot(self._autopilot_enabled) + module_hud = module_manager.get_module(MODULE_HUD) + module_hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.type == pygame.MOUSEBUTTONDOWN: + if event.button == 4: + self.wheel_offset += self.wheel_amount + if self.wheel_offset >= 1.0: + self.wheel_offset = 1.0 + elif event.button == 5: + self.wheel_offset -= self.wheel_amount + if self.wheel_offset <= 0.1: + self.wheel_offset = 0.1 + + def _parse_keys(self, milliseconds): + keys = pygame.key.get_pressed() + 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] + + def _parse_mouse(self): + if pygame.mouse.get_pressed()[0]: + x, y = pygame.mouse.get_pos() + self.mouse_offset[0] += (1.0 / self.wheel_offset) * (x - self.mouse_pos[0]) + self.mouse_offset[1] += (1.0 / self.wheel_offset) * (y - self.mouse_pos[1]) + self.mouse_pos = (x, y) + + def parse_input(self, clock): + self._parse_events() + self._parse_mouse() + if not self._autopilot_enabled: + if isinstance(self.control, carla.VehicleControl): + self._parse_keys(clock.get_time()) + self.control.reverse = self.control.gear < 0 + world = module_manager.get_module(MODULE_WORLD) + if (world.hero_actor is not None): + world.hero_actor.apply_control(self.control) + + @staticmethod + def _is_quit_shortcut(key): + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- Global Objects ------------------------------------------------------------ +# ============================================================================== +module_manager = ModuleManager() + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + try: + # Init Pygame + pygame.init() + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + pygame.display.set_caption(args.description) + + font = pygame.font.Font(pygame.font.get_default_font(), 20) + text_surface = font.render('Rendering map...', True, COLOR_WHITE) + display.blit(text_surface, text_surface.get_rect(center=(args.width / 2, args.height / 2))) + pygame.display.flip() + + # Init modules + input_module = ModuleInput(MODULE_INPUT) + hud_module = ModuleHUD(MODULE_HUD, args.width, args.height) + world_module = ModuleWorld(MODULE_WORLD, args, timeout=2.0) + + # Register Modules + module_manager.register_module(world_module) + module_manager.register_module(hud_module) + module_manager.register_module(input_module) + + module_manager.start_modules() + + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + + module_manager.tick(clock) + module_manager.render(display) + + pygame.display.flip() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + finally: + if world_module is not None: + world_module.destroy() + +def exit_game(): + module_manager.clear_modules() + pygame.quit() + sys.exit() + +# ============================================================================== +# -- Main -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + # Parse arguments + argparser = argparse.ArgumentParser( + description='CARLA No Rendering Mode Visualizer') + 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='PolarPointBEV port to listen to (default: 2000)') + 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( + '--map', + metavar='TOWN', + default=None, + help='start a new episode at the given TOWN') + argparser.add_argument( + '--no-rendering', + action='store_true', + default=True, + help='switch off server rendering') + argparser.add_argument( + '--show-triggers', + action='store_true', + help='show trigger boxes of traffic signs') + argparser.add_argument( + '--show-connections', + action='store_true', + help='show waypoint connections') + argparser.add_argument( + '--show-spawn-points', + action='store_true', + help='show recommended spawn points') + + args = argparser.parse_args() + args.description = argparser.description + 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__) + + game_loop(args) + +if __name__ == '__main__': + main() diff --git a/scenario_runner/scenario_runner.py b/scenario_runner/scenario_runner.py new file mode 100644 index 0000000..a31ca5b --- /dev/null +++ b/scenario_runner/scenario_runner.py @@ -0,0 +1,586 @@ +#!/usr/bin/env python + +# Copyright (c) 2018-2020 Intel Corporation +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +Welcome to CARLA scenario_runner + +This is the main script to be executed when running a scenario. +It loads the scenario configuration, loads the scenario and manager, +and finally triggers the scenario execution. +""" + +from __future__ import print_function + +import glob +import traceback +import argparse +from argparse import RawTextHelpFormatter +from datetime import datetime +from distutils.version import LooseVersion +import importlib +import inspect +import os +import signal +import sys +import time +import json +import pkg_resources + +import carla + +from srunner.scenarioconfigs.openscenario_configuration import OpenScenarioConfiguration +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenario_manager import ScenarioManager +from srunner.scenarios.open_scenario import OpenScenario +from srunner.scenarios.route_scenario import RouteScenario +from srunner.tools.scenario_parser import ScenarioConfigurationParser +from srunner.tools.route_parser import RouteParser + +# Version of scenario_runner +VERSION = '0.9.9' + + +class ScenarioRunner(object): + + """ + This is the core scenario runner module. It is responsible for + running (and repeating) a single scenario or a list of scenarios. + + Usage: + scenario_runner = ScenarioRunner(args) + scenario_runner.run() + del scenario_runner + """ + + ego_vehicles = [] + + # Tunable parameters + client_timeout = 10.0 # in seconds + wait_for_world = 20.0 # in seconds + frame_rate = 20.0 # in Hz + + # CARLA world and scenario handlers + world = None + manager = None + + additional_scenario_module = None + + agent_instance = None + module_agent = None + + def __init__(self, args): + """ + Setup CARLA client and world + Setup ScenarioManager + """ + self._args = args + + if args.timeout: + self.client_timeout = float(args.timeout) + + # 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. + self.client = carla.Client(args.host, int(args.port)) + self.client.set_timeout(self.client_timeout) + + dist = pkg_resources.get_distribution("carla") + if LooseVersion(dist.version) < LooseVersion('0.9.8'): + raise ImportError("CARLA version 0.9.8 or newer required. CARLA version found: {}".format(dist)) + + # Load agent if requested via command line args + # If something goes wrong an exception will be thrown by importlib (ok here) + if self._args.agent is not None: + module_name = os.path.basename(args.agent).split('.')[0] + sys.path.insert(0, os.path.dirname(args.agent)) + self.module_agent = importlib.import_module(module_name) + + # Create the ScenarioManager + self.manager = ScenarioManager(self._args.debug, self._args.sync, self._args.timeout) + + # Create signal handler for SIGINT + self._shutdown_requested = False + if sys.platform != 'win32': + signal.signal(signal.SIGHUP, self._signal_handler) + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + + self._start_wall_time = datetime.now() + + def destroy(self): + """ + Cleanup and delete actors, ScenarioManager and CARLA world + """ + + self._cleanup() + if self.manager is not None: + del self.manager + if self.world is not None: + del self.world + if self.client is not None: + del self.client + + def _signal_handler(self, signum, frame): + """ + Terminate scenario ticking when receiving a signal interrupt + """ + self._shutdown_requested = True + if self.manager: + self.manager.stop_scenario() + self._cleanup() + if not self.manager.get_running_status(): + raise RuntimeError("Timeout occured during scenario execution") + + def _get_scenario_class_or_fail(self, scenario): + """ + Get scenario class by scenario name + If scenario is not supported or not found, exit script + """ + + # Path of all scenario at "srunner/scenarios" folder + the path of the additional scenario argument + scenarios_list = glob.glob("{}/srunner/scenarios/*.py".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) + scenarios_list.append(self._args.additionalScenario) + + for scenario_file in scenarios_list: + + # Get their module + module_name = os.path.basename(scenario_file).split('.')[0] + sys.path.insert(0, os.path.dirname(scenario_file)) + scenario_module = importlib.import_module(module_name) + + # And their members of type class + for member in inspect.getmembers(scenario_module, inspect.isclass): + if scenario in member: + return member[1] + + # Remove unused Python paths + sys.path.pop(0) + + print("Scenario '{}' not supported ... Exiting".format(scenario)) + sys.exit(-1) + + def _cleanup(self): + """ + Remove and destroy all actors + """ + # Simulation still running and in synchronous mode? + if self.manager is not None and self.manager.get_running_status() \ + and self.world is not None and self._args.sync: + # Reset to asynchronous mode + settings = self.world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + + self.manager.cleanup() + + CarlaDataProvider.cleanup() + + for i, _ in enumerate(self.ego_vehicles): + if self.ego_vehicles[i]: + if not self._args.waitForEgo: + print("Destroying ego vehicle {}".format(self.ego_vehicles[i].id)) + self.ego_vehicles[i].destroy() + self.ego_vehicles[i] = None + self.ego_vehicles = [] + + if self.agent_instance: + self.agent_instance.destroy() + self.agent_instance = None + + def _prepare_ego_vehicles(self, ego_vehicles): + """ + Spawn or update the ego vehicles + """ + + if not self._args.waitForEgo: + for vehicle in ego_vehicles: + self.ego_vehicles.append(CarlaDataProvider.request_new_actor(vehicle.model, + vehicle.transform, + vehicle.rolename, + color=vehicle.color, + actor_category=vehicle.category)) + else: + ego_vehicle_missing = True + while ego_vehicle_missing: + self.ego_vehicles = [] + ego_vehicle_missing = False + for ego_vehicle in ego_vehicles: + ego_vehicle_found = False + carla_vehicles = CarlaDataProvider.get_world().get_actors().filter('vehicle.*') + for carla_vehicle in carla_vehicles: + if carla_vehicle.attributes['role_name'] == ego_vehicle.rolename: + ego_vehicle_found = True + self.ego_vehicles.append(carla_vehicle) + break + if not ego_vehicle_found: + ego_vehicle_missing = True + break + + for i, _ in enumerate(self.ego_vehicles): + self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) + CarlaDataProvider.register_actor(self.ego_vehicles[i]) + + # sync state + if CarlaDataProvider.is_sync_mode(): + self.world.tick() + else: + self.world.wait_for_tick() + + def _analyze_scenario(self, config): + """ + Provide feedback about success/failure of a scenario + """ + + # Create the filename + current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) + junit_filename = None + config_name = config.name + if self._args.outputDir != '': + config_name = os.path.join(self._args.outputDir, config_name) + if self._args.junit: + junit_filename = config_name + current_time + ".xml" + filename = None + if self._args.file: + filename = config_name + current_time + ".txt" + + if not self.manager.analyze_scenario(self._args.output, filename, junit_filename): + print("All scenario tests were passed successfully!") + else: + print("Not all scenario tests were successful") + if not (self._args.output or filename or junit_filename): + print("Please run with --output for further information") + + def _record_criteria(self, criteria, name): + """ + Filter the JSON serializable attributes of the criterias and + dumps them into a file. This will be used by the metrics manager, + in case the user wants specific information about the criterias. + """ + file_name = name[:-4] + ".json" + + # Filter the attributes that aren't JSON serializable + with open('temp.json', 'w') as fp: + + criteria_dict = {} + for criterion in criteria: + + criterion_dict = criterion.__dict__ + criteria_dict[criterion.name] = {} + + for key in criterion_dict: + if key != "name": + try: + key_dict = {key: criterion_dict[key]} + json.dump(key_dict, fp, sort_keys=False, indent=4) + criteria_dict[criterion.name].update(key_dict) + except TypeError: + pass + + os.remove('temp.json') + + # Save the criteria dictionary into a .json file + with open(file_name, 'w') as fp: + json.dump(criteria_dict, fp, sort_keys=False, indent=4) + + def _load_and_wait_for_world(self, town, ego_vehicles=None): + """ + Load a new CARLA world and provide data to CarlaDataProvider + """ + + if self._args.reloadWorld: + self.world = self.client.load_world(town) + else: + # if the world should not be reloaded, wait at least until all ego vehicles are ready + ego_vehicle_found = False + if self._args.waitForEgo: + while not ego_vehicle_found and not self._shutdown_requested: + vehicles = self.client.get_world().get_actors().filter('vehicle.*') + for ego_vehicle in ego_vehicles: + ego_vehicle_found = False + for vehicle in vehicles: + if vehicle.attributes['role_name'] == ego_vehicle.rolename: + ego_vehicle_found = True + break + if not ego_vehicle_found: + print("Not all ego vehicles ready. Waiting ... ") + time.sleep(1) + break + + self.world = self.client.get_world() + + if self._args.sync: + settings = self.world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 1.0 / self.frame_rate + self.world.apply_settings(settings) + + CarlaDataProvider.set_client(self.client) + CarlaDataProvider.set_world(self.world) + CarlaDataProvider.set_traffic_manager_port(int(self._args.trafficManagerPort)) + + # Wait for the world to be ready + if CarlaDataProvider.is_sync_mode(): + self.world.tick() + else: + self.world.wait_for_tick() + if CarlaDataProvider.get_map().name != town and CarlaDataProvider.get_map().name != "OpenDriveMap": + print("The CARLA server uses the wrong map: {}".format(CarlaDataProvider.get_map().name)) + print("This scenario requires to use map: {}".format(town)) + return False + + return True + + def _load_and_run_scenario(self, config): + """ + Load and run the scenario given by config + """ + result = False + if not self._load_and_wait_for_world(config.town, config.ego_vehicles): + self._cleanup() + return False + + if self._args.agent: + agent_class_name = self.module_agent.__name__.title().replace('_', '') + try: + self.agent_instance = getattr(self.module_agent, agent_class_name)(self._args.agentConfig) + config.agent = self.agent_instance + except Exception as e: # pylint: disable=broad-except + traceback.print_exc() + print("Could not setup required agent due to {}".format(e)) + self._cleanup() + return False + + # Prepare scenario + print("Preparing scenario: " + config.name) + try: + self._prepare_ego_vehicles(config.ego_vehicles) + if self._args.openscenario: + scenario = OpenScenario(world=self.world, + ego_vehicles=self.ego_vehicles, + config=config, + config_file=self._args.openscenario, + timeout=100000) + elif self._args.route: + scenario = RouteScenario(world=self.world, + config=config, + debug_mode=self._args.debug) + else: + scenario_class = self._get_scenario_class_or_fail(config.type) + scenario = scenario_class(self.world, + self.ego_vehicles, + config, + self._args.randomize, + self._args.debug) + except Exception as exception: # pylint: disable=broad-except + print("The scenario cannot be loaded") + traceback.print_exc() + print(exception) + self._cleanup() + return False + + try: + if self._args.record: + recorder_name = "{}/{}/{}.log".format( + os.getenv('SCENARIO_RUNNER_ROOT', "./"), self._args.record, config.name) + self.client.start_recorder(recorder_name, True) + + # Load scenario and run it + self.manager.load_scenario(scenario, self.agent_instance) + self.manager.run_scenario() + + # Provide outputs if required + self._analyze_scenario(config) + + # Remove all actors, stop the recorder and save all criterias (if needed) + scenario.remove_all_actors() + if self._args.record: + self.client.stop_recorder() + self._record_criteria(self.manager.scenario.get_criteria(), recorder_name) + + result = True + + except Exception as e: # pylint: disable=broad-except + traceback.print_exc() + print(e) + result = False + + self._cleanup() + return result + + def _run_scenarios(self): + """ + Run conventional scenarios (e.g. implemented using the Python API of ScenarioRunner) + """ + result = False + + # Load the scenario configurations provided in the config file + scenario_configurations = ScenarioConfigurationParser.parse_scenario_configuration( + self._args.scenario, + self._args.configFile) + if not scenario_configurations: + print("Configuration for scenario {} cannot be found!".format(self._args.scenario)) + return result + + # Execute each configuration + for config in scenario_configurations: + for _ in range(self._args.repetitions): + result = self._load_and_run_scenario(config) + + self._cleanup() + return result + + def _run_route(self): + """ + Run the route scenario + """ + result = False + + if self._args.route: + routes = self._args.route[0] + scenario_file = self._args.route[1] + single_route = None + if len(self._args.route) > 2: + single_route = self._args.route[2] + + # retrieve routes + route_configurations = RouteParser.parse_routes_file(routes, scenario_file, single_route) + + for config in route_configurations: + for _ in range(self._args.repetitions): + result = self._load_and_run_scenario(config) + + self._cleanup() + return result + + def _run_openscenario(self): + """ + Run a scenario based on OpenSCENARIO + """ + + # Load the scenario configurations provided in the config file + if not os.path.isfile(self._args.openscenario): + print("File does not exist") + self._cleanup() + return False + + config = OpenScenarioConfiguration(self._args.openscenario, self.client) + + result = self._load_and_run_scenario(config) + self._cleanup() + return result + + def run(self): + """ + Run all scenarios according to provided commandline args + """ + result = True + if self._args.openscenario: + result = self._run_openscenario() + elif self._args.route: + result = self._run_route() + else: + result = self._run_scenarios() + + print("No more scenarios .... Exiting") + return result + + +def main(): + """ + main function + """ + description = ("CARLA Scenario Runner: Setup, Run and Evaluate scenarios using CARLA\n" + "Current version: " + VERSION) + + # pylint: disable=line-too-long + parser = argparse.ArgumentParser(description=description, + formatter_class=RawTextHelpFormatter) + parser.add_argument('-v', '--version', action='version', version='%(prog)s ' + VERSION) + parser.add_argument('--host', default='127.0.0.1', + help='IP of the host server (default: localhost)') + parser.add_argument('--port', default='2000', + help='PolarPointBEV port to listen to (default: 2000)') + parser.add_argument('--timeout', default="10.0", + help='Set the CARLA client timeout value in seconds') + parser.add_argument('--trafficManagerPort', default='8000', + help='Port to use for the TrafficManager (default: 8000)') + parser.add_argument('--sync', action='store_true', + help='Forces the simulation to run synchronously') + parser.add_argument('--list', action="store_true", help='List all supported scenarios and exit') + + parser.add_argument( + '--scenario', help='Name of the scenario to be executed. Use the preposition \'group:\' to run all scenarios of one class, e.g. ControlLoss or FollowLeadingVehicle') + parser.add_argument('--openscenario', help='Provide an OpenSCENARIO definition') + parser.add_argument( + '--route', help='Run a route as a scenario (input: (route_file,scenario_file,[route id]))', nargs='+', type=str) + + parser.add_argument( + '--agent', help="Agent used to execute the scenario. Currently only compatible with route-based scenarios.") + parser.add_argument('--agentConfig', type=str, help="Path to Agent's configuration file", default="") + + parser.add_argument('--output', action="store_true", help='Provide results on stdout') + parser.add_argument('--file', action="store_true", help='Write results into a txt file') + parser.add_argument('--junit', action="store_true", help='Write results into a junit file') + parser.add_argument('--outputDir', default='', help='Directory for output files (default: this directory)') + + parser.add_argument('--configFile', default='', help='Provide an additional scenario configuration file (*.xml)') + parser.add_argument('--additionalScenario', default='', help='Provide additional scenario implementations (*.py)') + + parser.add_argument('--debug', action="store_true", help='Run with debug output') + parser.add_argument('--reloadWorld', action="store_true", + help='Reload the CARLA world before starting a scenario (default=True)') + parser.add_argument('--record', type=str, default='', + help='Path were the files will be saved, relative to SCENARIO_RUNNER_ROOT.\nActivates the CARLA recording feature and saves to file all the criteria information.') + parser.add_argument('--randomize', action="store_true", help='Scenario parameters are randomized') + parser.add_argument('--repetitions', default=1, type=int, help='Number of scenario executions') + parser.add_argument('--waitForEgo', action="store_true", help='Connect the scenario to an existing ego vehicle') + + arguments = parser.parse_args() + # pylint: enable=line-too-long + + if arguments.list: + print("Currently the following scenarios are supported:") + print(*ScenarioConfigurationParser.get_list_of_scenarios(arguments.configFile), sep='\n') + return 1 + + if not arguments.scenario and not arguments.openscenario and not arguments.route: + print("Please specify either a scenario or use the route mode\n\n") + parser.print_help(sys.stdout) + return 1 + + if arguments.route and (arguments.openscenario or arguments.scenario): + print("The route mode cannot be used together with a scenario (incl. OpenSCENARIO)'\n\n") + parser.print_help(sys.stdout) + return 1 + + if arguments.agent and (arguments.openscenario or arguments.scenario): + print("Agents are currently only compatible with route scenarios'\n\n") + parser.print_help(sys.stdout) + return 1 + + if arguments.route: + arguments.reloadWorld = True + + if arguments.agent: + arguments.sync = True + + scenario_runner = None + result = True + try: + scenario_runner = ScenarioRunner(arguments) + result = scenario_runner.run() + + finally: + if scenario_runner is not None: + scenario_runner.destroy() + del scenario_runner + return not result + + +if __name__ == "__main__": + sys.exit(main())