diff --git a/MANIFEST.in b/MANIFEST.in index 1aba38f..3c7a6b8 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -1 +1,3 @@ include LICENSE +include README.md +include carla_real_traffic_scenarios/roundabouts/Town03/data/*.json \ No newline at end of file diff --git a/README.md b/README.md index 4eb1b13..ac6282c 100644 --- a/README.md +++ b/README.md @@ -1,30 +1,53 @@ +![](https://img.shields.io/badge/release-TODO-brightgreen.svg?style=popout-square) +![](https://img.shields.io/badge/CARLA-0.9.6+-blue.svg?style=popout-square) +![](https://img.shields.io/badge/python-3.6%20|%203.7%20|3.8-blue.svg?style=popout-square) +![](https://img.shields.io/badge/license-MIT-orange.svg?style=popout-square) + CARLA real traffic scenarios ======================== ![I80 demo](I80_demo.gif) -### TL:DR +### TL;DR -- We prepared CARLA maps that mimic real-world roads from NGSim dataset (I80 and US101), -- we train and benchmark policies on real-world lane-change manuveurs from NGSim dataset, -- we share code for running NGSim-based scenarios in CARLA. Scenarios interface is similar to open-ai gym interface. +- Custom CARLA maps that mimic real-world roads and human driver behaviors from NGSim dataset (I80 and US101) +- We've trained and benchmarked policies on real-world lane change maneuvers from NGSim dataset +- We provide the source code for running NGSim-based scenarios in CARLA. Scenario interface is similar to [openai gym](https://gym.openai.com/) interface -### How to run examples? +### Prerequisites +1. Download and extract CARLA ([0.9.6 download link](https://github.com/carla-simulator/carla/releases/tag/0.9.6)). Then, add PythonAPI wheel to your `PYTHONPATH`: + ```bash + export CARLA_ROOT=/path/to/your/carla/release-folder + export PYTHONPATH=$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.6-py3.6-linux-x86_64.egg:$PYTHONPATH + ``` +2. You also need to install our asset packages with two new CARLA maps + - Download archives - **TODO LINK** + - Copy archives to `$CARLA_ROOT/Import` + - Import `cd Import && ./ImportAssets.sh` -1) You need to add CARLA python client whl to your pythonpath: -``` -export CARLA_ROOT=/path/to/your/carla/release -export PYTHONPATH=$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.6-py3.6-linux-x86_64.egg:$PYTHONPATH -``` -2) You need to install our asset packs with two new CARLA maps - DOWNLOAD_LINK_TODO +### Quickstart -### Examples: +##### Terminal I +```bash +./CarlaUE4.sh -benchmark -fps=10 +``` -* `example/example_replay_ngsim_in_carla.py` - shows how to run scenario in training loop. -* `example/example_replay_ngsim_in_carla.py` - shows how to replay NGSim dataset in CARLA. It was used to generated GIF in this README file. +##### Terminal II +```bash +# (wait until server loads) +python example/example_roundabout_scenario_usage.py +``` +##### Terminal III +```bash +# (wait until scenario script connects successfully, map rendering may tak a while) +python example/manual_driving.py --res 900x500 +``` +Code tested with CARLA 0.9.6. -Code is tested with CARLA 0.9.6. +### Real-traffic scenarios +* `python example/example_replay_ngsim_in_carla.py` - shows how to run scenario in training loop. +* `python example/example_replay_ngsim_in_carla.py` - shows how to replay NGSim dataset in CARLA. It was used to generated GIF in this README file. ### Credits -* Code for interfacing with NGSim dataset based on https://github.com/Atcold/pytorch-PPUU \ No newline at end of file +Code for interfacing with NGSim dataset was based on https://github.com/Atcold/pytorch-PPUU \ No newline at end of file diff --git a/carla_real_traffic_scenarios/roundabouts/Town03/data/actor-respawn-points.assets.json b/carla_real_traffic_scenarios/roundabouts/Town03/data/actor-respawn-points.assets.json deleted file mode 100644 index 0f959d4..0000000 --- a/carla_real_traffic_scenarios/roundabouts/Town03/data/actor-respawn-points.assets.json +++ /dev/null @@ -1,121 +0,0 @@ -[ - { - "id": 0, - "location": { - "x": 60.46520233154297, - "y": -4.073229789733887, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 178.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 1, - "location": { - "x": 65.46520233154297, - "y": -7.823229789733887, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 178.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 2, - "location": { - "x": -1.5347976684570312, - "y": -109.32322692871094, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 94.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 3, - "location": { - "x": -5.284797668457031, - "y": -109.82322692871094, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 94.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 4, - "location": { - "x": -64.53479766845703, - "y": 0.9267730712890625, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 354.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 5, - "location": { - "x": 1.4652023315429688, - "y": 117.67677307128906, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 630.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 6, - "location": { - "x": 5.715202331542969, - "y": 110.92677307128906, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 630.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - } -] \ No newline at end of file diff --git a/carla_real_traffic_scenarios/roundabouts/Town03/data/populated-inner-ring.assets.json b/carla_real_traffic_scenarios/roundabouts/Town03/data/populated-inner-ring.assets.json deleted file mode 100644 index 5a0d5ca..0000000 --- a/carla_real_traffic_scenarios/roundabouts/Town03/data/populated-inner-ring.assets.json +++ /dev/null @@ -1,376 +0,0 @@ -[ - { - "id": 0, - "location": { - "x": 19.086740493774414, - "y": 0.6575663685798645, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 270.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 1, - "location": { - "x": 18.036739349365234, - "y": -5.525766849517822, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 250.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 2, - "location": { - "x": 16.053403854370117, - "y": -9.842437744140625, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 238.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 3, - "location": { - "x": 13.253400802612305, - "y": -13.225774765014648, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 222.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 4, - "location": { - "x": 9.520063400268555, - "y": -16.142444610595703, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 210.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 5, - "location": { - "x": 4.853391647338867, - "y": -18.009113311767578, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 190.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 6, - "location": { - "x": -0.9799408316612244, - "y": -18.942447662353516, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 178.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 7, - "location": { - "x": -6.3466081619262695, - "y": -18.12578010559082, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 158.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 8, - "location": { - "x": -11.71328067779541, - "y": -15.559110641479492, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 142.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 9, - "location": { - "x": -15.563284873962402, - "y": -11.825773239135742, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 126.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 10, - "location": { - "x": -18.47995376586914, - "y": -7.625768661499023, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 106.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 11, - "location": { - "x": -19.763288497924805, - "y": -2.1424331665039062, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 94.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 12, - "location": { - "x": -19.879955291748047, - "y": 3.6908986568450928, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 82.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 13, - "location": { - "x": -18.129953384399414, - "y": 9.057570457458496, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 58.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 14, - "location": { - "x": -14.863283157348633, - "y": 13.140908241271973, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 38.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 15, - "location": { - "x": -10.42994499206543, - "y": 17.10757827758789, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 22.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 16, - "location": { - "x": -4.479938507080078, - "y": 19.440914154052734, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": 14.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 17, - "location": { - "x": 1.8200602531433105, - "y": 20.024248123168945, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": -6.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 18, - "location": { - "x": 7.770062446594238, - "y": 17.924245834350586, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": -30.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 19, - "location": { - "x": 12.786734580993652, - "y": 14.190908432006836, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": -50.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 20, - "location": { - "x": 16.75340461730957, - "y": 9.757570266723633, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": -66.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - }, - { - "id": 21, - "location": { - "x": 18.27007293701172, - "y": 5.090898513793945, - "z": 0.5 - }, - "rotation": { - "pitch": 0.0, - "yaw": -78.0, - "roll": 0.0 - }, - "yaw_agnostic": false, - "blueprint_patterns": [ - "vehicle.*" - ] - } -] \ No newline at end of file diff --git a/carla_real_traffic_scenarios/roundabouts/Town03/nodes.py b/carla_real_traffic_scenarios/roundabouts/Town03/nodes.py index bd83dd1..dc66e14 100644 --- a/carla_real_traffic_scenarios/roundabouts/Town03/nodes.py +++ b/carla_real_traffic_scenarios/roundabouts/Town03/nodes.py @@ -1,8 +1,5 @@ import carla -from libs.carla_real_traffic_scenarios.carla_real_traffic_scenarios.roundabouts.types import ( - RoundaboutNode, - CircleArea, -) +from carla_real_traffic_scenarios.roundabouts.types import RoundaboutNode, CircleArea node3 = RoundaboutNode( name="Node3 - right", @@ -11,13 +8,13 @@ carla.Rotation(yaw=270), ), entrance_area=CircleArea( - location=carla.Location(x=5.050692, y=21.908991, z=0.500000), radius=7 + center=carla.Location(x=5.050692, y=21.908991, z=0.500000), radius=7 ), next_exit=CircleArea( - location=carla.Location(x=27.760864, y=6.000000, z=0.500000), radius=3 + center=carla.Location(x=27.760864, y=6.000000, z=0.500000), radius=3 ), final_area_for_next_exit=CircleArea( - location=carla.Location(x=54.827374, y=5.625000, z=0.500000), radius=3 + center=carla.Location(x=54.827374, y=5.625000, z=0.500000), radius=3 ), next_node=None, ) @@ -27,13 +24,13 @@ carla.Location(x=-60.9058464050293, y=1.0, z=0.5), carla.Rotation(yaw=358) ), entrance_area=CircleArea( - location=carla.Location(x=-21.922626, y=1.375000, z=0.500000), radius=7.5 + center=carla.Location(x=-21.922626, y=1.375000, z=0.500000), radius=7.5 ), next_exit=CircleArea( - location=carla.Location(x=-9.886812, y=27.106071, z=0.500000), radius=3.5 + center=carla.Location(x=-9.886812, y=27.106071, z=0.500000), radius=3.5 ), final_area_for_next_exit=CircleArea( - location=carla.Location(x=-7.886812, y=69.356071, z=0.500000), radius=3 + center=carla.Location(x=-7.886812, y=69.356071, z=0.500000), radius=3 ), next_node=node3, ) @@ -45,13 +42,13 @@ carla.Rotation(yaw=90), ), entrance_area=CircleArea( - location=carla.Location(x=-3.922626, y=-20.875000, z=0.500000), radius=7.5 + center=carla.Location(x=-3.922626, y=-20.875000, z=0.500000), radius=7.5 ), next_exit=CircleArea( - location=carla.Location(x=-29.239136, y=-6.250000, z=0.500000), radius=3 + center=carla.Location(x=-29.239136, y=-6.250000, z=0.500000), radius=3 ), final_area_for_next_exit=CircleArea( - location=carla.Location(x=-51.834660, y=-2.930555, z=0.500000), radius=3 + center=carla.Location(x=-51.834660, y=-2.930555, z=0.500000), radius=3 ), next_node=node2, ) @@ -61,13 +58,13 @@ carla.Location(x=35.750507, y=-8.345556, z=0.5), carla.Rotation(yaw=180) ), entrance_area=CircleArea( - location=carla.Location(x=20.827374, y=-5.125000, z=0.500000), radius=7.5 + center=carla.Location(x=20.827374, y=-5.125000, z=0.500000), radius=7.5 ), next_exit=CircleArea( - location=carla.Location(x=7.575461, y=-28.649958, z=0.500000), radius=3 + center=carla.Location(x=7.575461, y=-28.649958, z=0.500000), radius=3 ), final_area_for_next_exit=CircleArea( - location=carla.Location(x=7.075461, y=-50.946571, z=0.500000), radius=3 + center=carla.Location(x=7.075461, y=-50.946571, z=0.500000), radius=3 ), next_node=node1, ) diff --git a/carla_real_traffic_scenarios/roundabouts/__init__.py b/carla_real_traffic_scenarios/roundabouts/__init__.py index 5cbccc5..fc580e5 100644 --- a/carla_real_traffic_scenarios/roundabouts/__init__.py +++ b/carla_real_traffic_scenarios/roundabouts/__init__.py @@ -1,7 +1,7 @@ import carla import random from pathlib import Path -from typing import Optional +from typing import Optional, List from carla_real_traffic_scenarios import FPS from carla_real_traffic_scenarios.assets import markings @@ -11,32 +11,62 @@ TOWN03_ROUNDABOUT_NODES, ) from carla_real_traffic_scenarios.roundabouts import route -from carla_real_traffic_scenarios.roundabouts.types import CircleArea, RoundaboutNode +from carla_real_traffic_scenarios.roundabouts.types import ( + CircleArea, + RoundaboutNode, + RouteCheckpoint, +) from carla_real_traffic_scenarios.scenario import ( ScenarioStepResult, Scenario, ChauffeurCommand, ) +from carla_real_traffic_scenarios.utils import geometry MAX_NUM_STEPS_TO_REACH_CHECKPOINT = FPS * 10 +DEBUG = False + + +def debug_draw(area: CircleArea, world: carla.World, **kwargs): + world.debug.draw_point(area.center, **kwargs) + xs, ys = geometry.points_on_ring(radius=area.radius, num_points=10) + for x, y in zip(xs, ys): + center = carla.Location(x=area.center.x + x, y=area.center.y + y, z=0.2) + world.debug.draw_point(center, **kwargs) -class RoundaboutExitingScenario(Scenario): +class RoundaboutScenario(Scenario): """ + Randomly chooses which roundabout exit the ego agent must take. + "Turn right" command will be given just after entering the last checkpoint located on roundabout ring. + Only Town03 roundabout is currently supported, but it's trivial to use with custom maps (just provide new marking files) + + It is advised (but not enforced by this implementation) to use CARLA synchronous mode - calling `world.tick()` + after reset() or step(). Code example can be found in __main__.py (`roundabouts` package) + Checkpoints can be visualized by toggling DEBUG flag. + + Specification: + 1. Ego vehicle must be on a road, otherwise reset() is triggered + (alternatively may want to modify this behavior and just give negative rewards) + 2. Collision with other vehicle instantly triggers reset() + + 3. Sparse reward mode: agent will get reward==1 when it reaches final checkpoint + + 4. Dense reward mode (default): agent will get 1/n reward for each of n checkpoints + and additional 1 for final checkpoint (max reward == 2) """ def __init__(self, client: carla.Client, sparse_reward_mode: bool = False): super().__init__(client) - self._client = client self._sparse_reward_mode = sparse_reward_mode - self._world = client.get_world() - self._map = self._world.get_map() # Saving state between consecutive steps self._next_route_checkpoint_idx: Optional[int] = None self._steps_to_reach_next_checkpoint: Optional[int] = None - self._command: ChauffeurCommand = None + self._command: Optional[ChauffeurCommand] = None + self._collision_sensor: Optional[carla.Actor] = None + self._collided: bool = False # Driving actors vehicle_blueprints = self._world.get_blueprint_library().filter("vehicle.*") @@ -52,7 +82,8 @@ def __init__(self, client: carla.Client, sparse_reward_mode: bool = False): path=data_dir / "on-reset-spawn-points.assets.json" ) self._driving_actors_manager = ActorManager(client) - self._scenario_area = CircleArea(location=carla.Location(0, 0, 0), radius=100) + self._scenario_area = CircleArea(center=carla.Location(0, 0, 0), radius=100) + self._route: Optional[List[RouteCheckpoint]] = None def reset(self, ego_vehicle: carla.Vehicle): # Actors @@ -65,15 +96,31 @@ def reset(self, ego_vehicle: carla.Vehicle): start_node = random.choice(TOWN03_ROUNDABOUT_NODES) start_node.spawn_point.location.z = 0.1 + if self._collision_sensor is not None: + self._collision_sensor.destroy() + self._collision_sensor = None + + blueprint_library = self._world.get_blueprint_library() + collision_sensor_bp = blueprint_library.find("sensor.other.collision") + self._collision_sensor = self._world.spawn_actor( + collision_sensor_bp, ego_vehicle.get_transform(), attach_to=ego_vehicle + ) + + def on_collision(e): + self._collided = True + + self._collision_sensor.listen(on_collision) + self._collided = False + # Physics trick is necessary to prevent vehicle from keeping the velocity ego_vehicle.set_simulate_physics(False) ego_vehicle.set_transform(start_node.spawn_point) ego_vehicle.set_simulate_physics(True) # Route - self._take_nth_exit = random.randrange(1, 5) + take_nth_exit = random.randrange(1, 5) self._route = route.build_roundabout_checkpoint_route( - start_node=start_node, nth_exit_to_take=self._take_nth_exit + start_node=start_node, nth_exit_to_take=take_nth_exit ) # States @@ -90,17 +137,26 @@ def step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult: ego_location = ego_transform.location next_checkpoint = self._route[self._next_route_checkpoint_idx] - # Displaying checkpoints' areas server-side - # for route_checkpoint in self.route: - # color = carla.Color(random.randrange(255), random.randrange(255), random.randrange(255)) - # route_checkpoint.draw(self._world, color=color, life_time=0.1) - # next_checkpoint.draw(self._world, life_time=0.01) + if DEBUG: + lightgreen_color = carla.Color(153, 255, 51) + for route_checkpoint in self._route: + debug_draw( + route_checkpoint.area, + self._world, + color=lightgreen_color, + life_time=1.0 / FPS, + ) + debug_draw(next_checkpoint.area, self._world, life_time=0.01) checkpoint_area = next_checkpoint.area reward = 0 done = False info = {} + if self._collided is True: + reward = 0 + done = True + if ego_location in checkpoint_area: if not self._sparse_reward_mode: num_checkpoints_excluding_final = len(self._route) - 1 @@ -109,12 +165,14 @@ def step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult: self._steps_to_reach_next_checkpoint = MAX_NUM_STEPS_TO_REACH_CHECKPOINT self._next_route_checkpoint_idx += 1 - if self._map.get_waypoint(ego_location, project_to_road=False) is None: + is_ego_offroad = ( + self._world_map.get_waypoint(ego_location, project_to_road=False) is None + ) + if is_ego_offroad: reward = 0 done = True if self._next_route_checkpoint_idx == len(self._route): - # NOTE in default (dense) reward mode, agent will get 2 points for completing whole route reward = 1 done = True @@ -129,88 +187,3 @@ def step(self, ego_vehicle: carla.Vehicle) -> ScenarioStepResult: def close(self): self._driving_actors_manager.clean_up_all() - - -# TODO reset() -# if SPAWN_STATIC: -# self.static_actors_manager.clean_up_all() -# for marking in self.asset_spawn_markings: -# marking.transform.location.z = 0.1 -# self.static_actors_manager.spawn_random_assets_at_markings( -# markings=self.asset_spawn_markings, coverage=1 -# ) -# self.static_actors_manager.apply_physics_settings_to_spawned(enable=False) - -# NOTE Must not tick..., use of .then() is necessary -# SetAutopilot = carla.command.SetAutopilot -# batch = [ -# SetAutopilot(actor_id, True) -# for actor_id in self.driving_actors_manager.spawned -# ] -# responses = self._client.apply_batch_sync(batch, True) -# errors = [r.actor_id for r in responses if r.has_error()] -# print(errors) -# self.driving_actors_manager.apply_physics_settings_to_spawned(enable=False) - -# NOTE step() -# print(len(self.driving_actors_manager.spawned), len(self._world.get_actors())) -# for actor_id in self.driving_actors_manager.spawned: -# actor = self._world.get_actors().find(actor_id) -# if actor is None: -# print("nie ma", actor_id) -# self.driving_actors_manager.spawned.remove(actor_id) -# continue -# actor_trans = actor.get_transform() -# if actor_trans.location not in self._map_area: -# new_actor = self._respawn_randomly(actor) -# if new_actor: -# new_actor.set_autopilot(True) - -# blueprint = random.choice(self._car_blueprints) -# actor.destroy() -# self.driving_actors_manager.spawned.remove(actor_id) -# spawn_point_idx = np.argmin(self.respawn_usage_counter) -# spawn_point = self.driving_actors_respawn_markings[ -# spawn_point_idx -# ].transform -# actor = self.driving_actors_manager.spawn(spawn_point, blueprint) -# if actor is not None: -# self.respawn_usage_counter[spawn_point_idx] += 1 -# actor.set_autopilot(True) -# self.driving_actors_manager.spawned.append(actor.id) -# else: -# if TELEPORTATION_DRIVING: -# nearest_waypoint = self._map.get_waypoint(actor_trans.location) -# next_transform = random.choice(nearest_waypoint.next(0.1)).transform -# actor.set_transform(next_transform) -# elif AUTOPILOT_DRIVING: -# if get_speed(actor) < 3: -# if self.actor_stuck_counter.get(actor_id): -# self.actor_stuck_counter[actor_id] += 1 -# else: -# self.actor_stuck_counter[actor_id] = 1 -# else: -# self.actor_stuck_counter[actor_id] = 0 -# -# if self.actor_stuck_counter[actor_id] > 50: -# new_actor = self._respawn_randomly(actor) -# if new_actor: -# new_actor.set_autopilot(True) -# print( -# f"Steps to reach checkpoint: {self.steps_to_reach_next_checkpoint} | Command: {self.command.name}| Next checkpoint: {next_checkpoint.name} | Take nth exit: {self.take_nth_exit}" -# ) - -# NOTE useful -# def _respawn_randomly(self, actor) -> Optional[carla.Actor]: -# # least_used_idx = np.argmin(self.respawn_usage_counter) -# # spawn_point = self.driving_actors_respawn_markings[least_used_idx].transform -# # actor.set_transform(spawn_point) -# # self.respawn_usage_counter[least_used_idx] += 1 -# -# actor.destroy() -# self.driving_actors_manager.spawned.remove(actor.id) -# -# blueprint = random.choice(self._car_blueprints) -# spawn_point = random.choice(self.driving_actors_respawn_markings).transform -# actor = self.driving_actors_manager.spawn(spawn_point, blueprint) -# return actor diff --git a/carla_real_traffic_scenarios/roundabouts/route.py b/carla_real_traffic_scenarios/roundabouts/route.py index 2a95253..d84a2a4 100644 --- a/carla_real_traffic_scenarios/roundabouts/route.py +++ b/carla_real_traffic_scenarios/roundabouts/route.py @@ -1,14 +1,10 @@ -import carla -import math -from typing import NamedTuple, List +from typing import List -from libs.carla_real_traffic_scenarios.carla_real_traffic_scenarios.roundabouts.types import ( +from carla_real_traffic_scenarios.roundabouts.types import ( RoundaboutNode, RouteCheckpoint, ) -from libs.carla_real_traffic_scenarios.carla_real_traffic_scenarios.scenario import ( - ChauffeurCommand, -) +from carla_real_traffic_scenarios.scenario import ChauffeurCommand def build_roundabout_checkpoint_route( diff --git a/carla_real_traffic_scenarios/roundabouts/types.py b/carla_real_traffic_scenarios/roundabouts/types.py index 8b4e821..c936301 100644 --- a/carla_real_traffic_scenarios/roundabouts/types.py +++ b/carla_real_traffic_scenarios/roundabouts/types.py @@ -1,20 +1,15 @@ import carla from dataclasses import dataclass from typing import NamedTuple - -from libs.carla_real_traffic_scenarios.carla_real_traffic_scenarios.scenario import ChauffeurCommand -from libs.carla_real_traffic_scenarios.carla_real_traffic_scenarios.utils import ( - geometry, -) +from carla_real_traffic_scenarios.scenario import ChauffeurCommand class CircleArea(NamedTuple): - location: carla.Location + center: carla.Location radius: float def __contains__(self, loc: carla.Location) -> bool: - dist = geometry.distance(loc, self.location) - return dist <= self.radius + return loc.distance(self.center) <= self.radius @dataclass @@ -26,17 +21,8 @@ class RoundaboutNode: final_area_for_next_exit: CircleArea next_node: "RoundaboutNode" + class RouteCheckpoint(NamedTuple): name: str area: CircleArea command: ChauffeurCommand - - # FIXME Could be a method of CircleArea instead... - def draw(self, world: carla.World, **kwargs): - world.debug.draw_point(self.area.location, **kwargs) - xs, ys = geometry.points_on_ring(radius=self.area.radius, num_points=10) - for x, y in zip(xs, ys): - center = carla.Location( - x=self.area.location.x + x, y=self.area.location.y + y, z=0.1 - ) - world.debug.draw_point(center, **kwargs) diff --git a/carla_real_traffic_scenarios/utils/geometry.py b/carla_real_traffic_scenarios/utils/geometry.py index 4af9cc9..81aa839 100644 --- a/carla_real_traffic_scenarios/utils/geometry.py +++ b/carla_real_traffic_scenarios/utils/geometry.py @@ -26,12 +26,6 @@ def normalize_angle_npy(angles: np.ndarray) -> np.ndarray: return angles -def distance(a: carla.Location, b: carla.Location) -> float: - dx = a.x - b.x - dy = a.y - b.y - return np.sqrt(dx * dx + dy * dy) - - def points_on_ring(radius: float, num_points: int) -> Tuple[np.array, np.array]: """Generates `n` coordinates lying on a ring with radius `r` and center at (0, 0).""" t = np.linspace(0, 2 * np.pi, num_points) diff --git a/carla_real_traffic_scenarios/roundabouts/__main__.py b/example/example_roundabout_scenario_usage.py similarity index 70% rename from carla_real_traffic_scenarios/roundabouts/__main__.py rename to example/example_roundabout_scenario_usage.py index b941dcd..1858a01 100644 --- a/carla_real_traffic_scenarios/roundabouts/__main__.py +++ b/example/example_roundabout_scenario_usage.py @@ -1,10 +1,9 @@ import carla -from libs.carla_real_traffic_scenarios.carla_real_traffic_scenarios.roundabouts import ( - RoundaboutExitingScenario, -) +from carla_real_traffic_scenarios.carla_maps import CarlaMaps +from carla_real_traffic_scenarios.roundabouts import RoundaboutScenario from carla_real_traffic_scenarios import DT -synch = True +SYNCHRONOUS_MODE = True def set_birds_eye_view_spectator( @@ -20,8 +19,8 @@ def set_birds_eye_view_spectator( client = carla.Client("localhost", 2000) client.set_timeout(3.0) -world = client.get_world() -if synch: +world = client.load_world(CarlaMaps.TOWN03.level_path) +if SYNCHRONOUS_MODE: settings = world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = DT @@ -35,20 +34,26 @@ def set_birds_eye_view_spectator( spectator = world.get_spectator() set_birds_eye_view_spectator(spectator, carla.Location(), above=80) - -scenario = RoundaboutExitingScenario(client) +scenario = RoundaboutScenario(client) scenario.reset(agent_vehicle) -if synch: +if SYNCHRONOUS_MODE: world.tick() +print("Scenario has been loaded") done = False +episode_reward = 0 try: while True: result = scenario.step(agent_vehicle) - if synch: + if SYNCHRONOUS_MODE: world.tick() + episode_reward += result.reward + if result.done: + print("Episode reward:", episode_reward) scenario.reset(agent_vehicle) + episode_reward = 0 + print("Scenario has been reset") finally: settings = world.get_settings() settings.synchronous_mode = False diff --git a/example/manual_driving.py b/example/manual_driving.py new file mode 100755 index 0000000..93a16ed --- /dev/null +++ b/example/manual_driving.py @@ -0,0 +1,1645 @@ +#!/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 +import hashlib + +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 +TITLE_WORLD = 'WORLD' +TITLE_HUD = 'HUD' +TITLE_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): + """Function that renders the all the source surfaces in a destination source""" + for surface in source_surfaces: + destination_surface.blit(surface[0], surface[1], rect, blend_mode) + + @staticmethod + def length(v): + """Returns the length of a vector""" + return math.sqrt(v.x**2 + v.y**2 + v.z**2) + + @staticmethod + def get_bounding_box(actor): + """Gets the bounding box corners of an actor in world space""" + 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 + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + """Renders texts that fades out after some seconds that the user specifies""" + + def __init__(self, font, dim, pos): + """Initializes variables such as text font, dimensions and position""" + 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): + """Sets the text, color and seconds until fade out""" + 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): + """Each frame, it shows the displayed text for some specified seconds, if any""" + 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): + """ Renders the text in its surface and its position""" + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + def __init__(self, font, width, height): + """Renders the help text that shows the controls for using no rendering mode""" + 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): + """Toggles display of help text""" + self._render = not self._render + + def render(self, display): + """Renders the help text, if enabled""" + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------- +# ============================================================================== + + +class HUD (object): + """Class encharged of rendering the HUD that displays information about the world and the hero vehicle""" + + def __init__(self, name, width, height): + """Initializes default HUD params and content data parameters that will be displayed""" + self.name = name + self.dim = (width, height) + self._init_hud_params() + self._init_data_params() + + def start(self): + """Does nothing since it does not need to use other modules""" + + def _init_hud_params(self): + """Initialized visual parameters such as font text and size""" + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 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): + """Initializes the content data structures""" + self.show_info = True + self.show_actor_ids = False + self._info_text = {} + + def notification(self, text, seconds=2.0): + """Shows fading texts for some specified seconds""" + self._notifications.set_text(text, seconds=seconds) + + def tick(self, clock): + """Updated the fading texts each frame""" + self._notifications.tick(clock) + + def add_info(self, title, info): + """Adds a block of information in the left HUD panel of the visualizer""" + self._info_text[title] = info + + def render_vehicles_ids(self, vehicle_id_surface, list_actors, world_to_pixel, hero_actor, hero_transform): + """When flag enabled, it shows the IDs of the vehicles that are spawned in the world. Depending on the vehicle type, + it will render it in different colors""" + + 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 flag enabled, it renders all the information regarding the left panel of the visualizer""" + 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 title, info in self._info_text.items(): + if not info: + continue + surface = self._header_font.render(title, 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 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): + """Draws a traffic light, which is composed of a dark background surface with 3 circles that indicate its color depending on the state""" + 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 + + # Draws the corresponding color if is on, otherwise it will be gray if its off + 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): + """Rotates and scales the traffic light surface""" + for key, surface in self._original_surfaces.items(): + self.surfaces[key] = pygame.transform.rotozoom(surface, angle, scale) + + +# ============================================================================== +# -- World --------------------------------------------------------------------- +# ============================================================================== + + +class MapImage(object): + """Class encharged of rendering a 2D image from top view of a carla world. Please note that a cache system is used, so if the OpenDrive content + of a Carla town has not changed, it will read and use the stored image if it was rendered in a previous execution""" + + def __init__(self, carla_world, carla_map, pixels_per_meter, show_triggers, show_connections, show_spawn_points): + """ Renders the map image generated based on the world, its map and additional flags that provide extra information about the road network""" + 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) + + # Maximum size of a Pygame surface + width_in_pixels = (1 << 14) - 1 + + # Adapt Pixels per meter to make world fit in surface + surface_pixel_per_meter = int(width_in_pixels / self.width) + if surface_pixel_per_meter > PIXELS_PER_METER: + surface_pixel_per_meter = PIXELS_PER_METER + + self._pixels_per_meter = surface_pixel_per_meter + width_in_pixels = int(self._pixels_per_meter * self.width) + + self.big_map_surface = pygame.Surface((width_in_pixels, width_in_pixels)).convert() + + # Load OpenDrive content + opendrive_content = carla_map.to_opendrive() + + # Get hash based on content + hash_func = hashlib.sha1() + hash_func.update(opendrive_content.encode("UTF-8")) + opendrive_hash = str(hash_func.hexdigest()) + + # Build path for saving or loading the cached rendered map + filename = carla_map.name + "_" + opendrive_hash + ".tga" + dirname = os.path.join("cache", "no_rendering_mode") + full_path = str(os.path.join(dirname, filename)) + + if os.path.isfile(full_path): + # Load Image + self.big_map_surface = pygame.image.load(full_path) + else: + # Render map + self.draw_road_map( + self.big_map_surface, + carla_world, + carla_map, + self.world_to_pixel, + self.world_to_pixel_width) + + # If folders path does not exist, create it + if not os.path.exists(dirname): + os.makedirs(dirname) + + # Remove files if selected town had a previous version saved + list_filenames = glob.glob(os.path.join(dirname, carla_map.name) + "*") + for town_filename in list_filenames: + os.remove(town_filename) + + # Save rendered map for next executions of same map + pygame.image.save(self.big_map_surface, full_path) + + self.surface = self.big_map_surface + + def draw_road_map(self, map_surface, carla_world, carla_map, world_to_pixel, world_to_pixel_width): + """Draws all the roads, including lane markings, arrows and traffic signs""" + map_surface.fill(COLOR_ALUMINIUM_4) + precision = 0.05 + + def lane_marking_color_to_tango(lane_marking_color): + """Maps the lane marking color enum specified in PythonAPI to a Tango 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): + """Draws solid lines in a surface given a set of points, width and color""" + if len(points) >= 2: + pygame.draw.lines(surface, color, closed, points, width) + + def draw_broken_line(surface, color, closed, points, width): + """Draws broken lines in a surface given a set of points, width and color""" + # Select which lines are going to be rendered from the set of lines + broken_lines = [x for n, x in enumerate(zip(*(iter(points),) * 20)) if n % 3 == 0] + + # Draw selected lines + 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): + """For multiple lane marking types (SolidSolid, BrokenSolid, SolidBroken and BrokenBroken), it converts them + as a combination of Broken and Solid lines""" + margin = 0.25 + marking_1 = [world_to_pixel(lateral_shift(w.transform, sign * w.lane_width * 0.5)) for w in waypoints] + if lane_marking_type == carla.LaneMarkingType.Broken or (lane_marking_type == carla.LaneMarkingType.Solid): + return [(lane_marking_type, lane_marking_color, marking_1)] + else: + marking_2 = [world_to_pixel(lateral_shift(w.transform, + sign * (w.lane_width * 0.5 + margin * 2))) for w in waypoints] + if lane_marking_type == carla.LaneMarkingType.SolidBroken: + return [(carla.LaneMarkingType.Broken, lane_marking_color, marking_1), + (carla.LaneMarkingType.Solid, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.BrokenSolid: + return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1), + (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.BrokenBroken: + return [(carla.LaneMarkingType.Broken, lane_marking_color, marking_1), + (carla.LaneMarkingType.Broken, lane_marking_color, marking_2)] + elif lane_marking_type == carla.LaneMarkingType.SolidSolid: + return [(carla.LaneMarkingType.Solid, lane_marking_color, marking_1), + (carla.LaneMarkingType.Solid, lane_marking_color, marking_2)] + + return [(carla.LaneMarkingType.NONE, carla.LaneMarkingColor.Other, [])] + + def draw_lane(surface, lane, color): + """Renders a single lane in a surface and with a specified color""" + for side in lane: + lane_left_side = [lateral_shift(w.transform, -w.lane_width * 0.5) for w in side] + lane_right_side = [lateral_shift(w.transform, w.lane_width * 0.5) for w in side] + + polygon = lane_left_side + [x for x in reversed(lane_right_side)] + polygon = [world_to_pixel(x) for x in polygon] + + if len(polygon) > 2: + pygame.draw.polygon(surface, color, polygon, 5) + pygame.draw.polygon(surface, color, polygon) + + def draw_lane_marking(surface, waypoints): + """Draws the left and right side of lane markings""" + # Left Side + draw_lane_marking_single_side(surface, waypoints[0], -1) + + # Right Side + draw_lane_marking_single_side(surface, waypoints[1], 1) + + def draw_lane_marking_single_side(surface, waypoints, sign): + """Draws the lane marking given a set of waypoints and decides whether drawing the right or left side of + the waypoint based on the sign parameter""" + 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 + + markings_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: + # Get the list of lane markings to draw + markings = get_lane_markings( + previous_marking_type, + lane_marking_color_to_tango(previous_marking_color), + temp_waypoints, + sign) + current_lane_marking = marking_type + + # Append each lane marking in the list + for marking in markings: + markings_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: + markings_list.append(marking) + + # Once the lane markings have been simplified to Solid or Broken lines, we draw them + for markings in markings_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): + """ Draws an arrow with a specified color given a transform""" + 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 + + # Draw lines + 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): + """Draw stop traffic signs and its bounding box if enabled""" + 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 of the stop trigger + 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 draw_crosswalk(surface, transform=None, color=COLOR_ALUMINIUM_2): + # """Given two points A and B, draw white parallel lines from A to B""" + # a = carla.Location(0.0, 0.0, 0.0) + # b = carla.Location(10.0, 10.0, 0.0) + + # ab = b - a + # length_ab = math.sqrt(ab.x**2 + ab.y**2) + # unit_ab = ab / length_ab + # unit_perp_ab = carla.Location(-unit_ab.y, unit_ab.x, 0.0) + + # # Crosswalk lines params + # space_between_lines = 0.5 + # line_width = 0.7 + # line_height = 2 + + # current_length = 0 + # while current_length < length_ab: + + # center = a + unit_ab * current_length + + # width_offset = unit_ab * line_width + # height_offset = unit_perp_ab * line_height + # list_point = [center - width_offset - height_offset, + # center + width_offset - height_offset, + # center + width_offset + height_offset, + # center - width_offset + height_offset] + + # list_point = [world_to_pixel(p) for p in list_point] + # pygame.draw.polygon(surface, color, list_point) + # current_length += (line_width + space_between_lines) * 2 + + def lateral_shift(transform, shift): + """Makes a lateral shift of the forward vector of a transform""" + transform.rotation.yaw += 90 + return transform.location + shift * transform.get_forward_vector() + + def draw_topology(carla_topology, index): + """ Draws traffic signs and the roads network with sidewalks, parking and shoulders by generating waypoints""" + topology = [x[index] for x in carla_topology] + topology = sorted(topology, key=lambda w: w.transform.location.z) + set_waypoints = [] + for waypoint in topology: + waypoints = [waypoint] + + # Generate waypoints of a road id. Stop when road id differs + 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 + set_waypoints.append(waypoints) + + # Draw Shoulders, Parkings and Sidewalks + PARKING_COLOR = COLOR_ALUMINIUM_4_5 + SHOULDER_COLOR = COLOR_ALUMINIUM_5 + SIDEWALK_COLOR = COLOR_ALUMINIUM_3 + + shoulder = [[], []] + parking = [[], []] + sidewalk = [[], []] + + for w in waypoints: + # Classify lane types until there are no waypoints by going left + l = w.get_left_lane() + while l and l.lane_type != carla.LaneType.Driving: + + if l.lane_type == carla.LaneType.Shoulder: + shoulder[0].append(l) + + if l.lane_type == carla.LaneType.Parking: + parking[0].append(l) + + if l.lane_type == carla.LaneType.Sidewalk: + sidewalk[0].append(l) + + l = l.get_left_lane() + + # Classify lane types until there are no waypoints by going right + r = w.get_right_lane() + while r and r.lane_type != carla.LaneType.Driving: + + if r.lane_type == carla.LaneType.Shoulder: + shoulder[1].append(r) + + if r.lane_type == carla.LaneType.Parking: + parking[1].append(r) + + if r.lane_type == carla.LaneType.Sidewalk: + sidewalk[1].append(r) + + r = r.get_right_lane() + + # Draw classified lane types + draw_lane(map_surface, shoulder, SHOULDER_COLOR) + draw_lane(map_surface, parking, PARKING_COLOR) + draw_lane(map_surface, sidewalk, SIDEWALK_COLOR) + + # Draw Roads + for waypoints in set_waypoints: + waypoint = waypoints[0] + 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 Lane Markings and Arrows + # if not waypoint.is_junction: + # draw_lane_marking(map_surface, [waypoints, waypoints]) + # for n, wp in enumerate(waypoints): + # if ((n + 1) % 400) == 0: + # draw_arrow(map_surface, wp.transform) + draw_lane_marking(map_surface, [waypoints, waypoints]) + 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) + + 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 + + def to_pixel(wp): return world_to_pixel(wp.transform.location) + for wp in carla_map.generate_waypoints(dist): + col = (0, 255, 255) if wp.is_junction 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() + + # Find and Draw Traffic Signs: Stops and Yields + 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)): + """Converts the world coordinates to pixel coordinates""" + 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): + """Converts the world units to pixel units""" + return int(self.scale * self._pixels_per_meter * width) + + def scale_map(self, scale): + """Scales the map surface""" + 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 World(object): + """Class that contains all the information of a carla world that is running on the server side""" + + 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 = [] + + self._hud = None + self._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): + """Retrieves the data from the server side""" + 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, hud, input_control): + """Build the map image, stores the needed modules and prepares rendering in Hero Mode""" + self.world, self.town_map = self._get_data_from_carla() + + settings = self.world.get_settings() + settings.no_rendering_mode = self.args.no_rendering + self.world.apply_settings(settings) + + # 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) + + self._hud = hud + self._input = input_control + + self.original_surface_size = min(self._hud.dim[0], self._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._hud.dim, pygame.SRCALPHA).convert() + self.border_round_surface.set_colorkey(COLOR_WHITE) + self.border_round_surface.fill(COLOR_BLACK) + + # Used for Hero Mode, draws the map contained in a circle with white border + center_offset = (int(self._hud.dim[0] / 2), int(self._hud.dim[1] / 2)) + pygame.draw.circle(self.border_round_surface, COLOR_ALUMINIUM_1, center_offset, int(self._hud.dim[1] / 2)) + pygame.draw.circle(self.border_round_surface, COLOR_WHITE, center_offset, int((self._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._input.wheel_offset = HERO_DEFAULT_SCALE + self._input.control = carla.VehicleControl() + + # Register event for receiving server tick + weak_self = weakref.ref(self) + self.world.on_tick(lambda timestamp: World.on_world_tick(weak_self, timestamp)) + + def select_hero_actor(self): + """Selects only one hero actor if there are more than one. If there are not any, it will spawn one.""" + 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): + """Spawns the hero actor when the script runs""" + # 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() + spawn_point = carla.Transform() + spawn_point.location.z += 2.0 + 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): + """Retrieves the actors for Hero and Map modes and updates de HUD based on that""" + actors = self.world.get_actors() + + # We store the transforms also so that we avoid having transforms of + # previous tick and current tick when rendering them. + 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): + """Updates the HUD info regarding simulation, hero mode and whether there is a traffic light affecting the hero actor""" + + 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() + if math.isnan(affected_speed_limit_text): + affected_speed_limit_text = 0.0 + 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) + 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, + ] + + self._hud.add_info(self.name, info_text) + self._hud.add_info('HERO', hero_mode_text) + + @staticmethod + def on_world_tick(weak_self, timestamp): + """Updates the server tick""" + 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 _show_nearby_vehicles(self, vehicles): + """Shows nearby vehicles of the hero actor""" + 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)) + self._hud.add_info('NEARBY VEHICLES', info_text) + + def _split_actors(self): + """Splits the retrieved actors by type id""" + 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.pedestrian' in actor.type_id: + walkers.append(actor_with_transform) + + return (vehicles, traffic_lights, speed_limits, walkers) + + def _render_traffic_lights(self, surface, list_tl, world_to_pixel): + """Renders the traffic lights and shows its triggers and bounding boxes if flags are enabled""" + 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): + """Renders the speed limits by drawing two concentric circles (outer is red and inner white) and a speed limit text""" + + 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 concentric circles + 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: + # In hero mode, 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: + # In map mode, there is no need to rotate the text of the speed limit + surface.blit(font_surface, (x - radius / 2, y - radius / 2)) + + def _render_walkers(self, surface, list_w, world_to_pixel): + """Renders the walkers' bounding boxes""" + 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): + """Renders the vehicles' bounding boxes""" + 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): + """Renders all the actors""" + # 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): + """Used to improve perfomance. Clips the surfaces in order to render only the part of the surfaces that are going to be visible""" + 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): + """Based on the mouse wheel and mouse position, it will compute the scale and move the map so that it is zoomed in or out based on mouse position""" + m = self._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): + """Renders the map and all the actors in hero and map mode""" + if self.actors_with_transforms is None: + return + self.result_surface.fill(COLOR_BLACK) + + # Split the actors by vehicle type id + vehicles, traffic_lights, speed_limits, walkers = self._split_actors() + + # Zoom in and out + scale_factor = self._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._hud.render_vehicles_ids(self.vehicle_id_surface, vehicles, + self.map_image.world_to_pixel, self.hero_actor, self.hero_transform) + # Show nearby actors from hero mode + self._show_nearby_vehicles(vehicles) + + # 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 Mode + 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: + # Map Mode + # Translation offset + translation_offset = (self._input.mouse_offset[0] * scale_factor + self.scale_offset[0], + self._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._hud.dim[0], self._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): + """Destroy the hero actor when class instance is destroyed""" + if self.spawned_hero is not None: + self.spawned_hero.destroy() + +# ============================================================================== +# -- Input ----------------------------------------------------------- +# ============================================================================== + + +class InputControl(object): + """Class that handles input received such as keyboard and mouse.""" + + def __init__(self, name): + """Initializes input member variables when instance is created.""" + 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 + + # Modules that input will depend on + self._hud = None + self._world = None + + def start(self, hud, world): + """Assigns other initialized modules that input module needs.""" + self._hud = hud + self._world = world + + self._hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def render(self, display): + """Does nothing. Input module does not need render anything.""" + + def tick(self, clock): + """Executed each frame. Calls method for parsing input.""" + self.parse_input(clock) + + def _parse_events(self): + """Parses input events. These events are executed only once when pressing a key.""" + 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): + self._hud.help.toggle() + elif event.key == K_TAB: + # Toggle between hero and map mode + if self._world.hero_actor is None: + self._world.select_hero_actor() + self.wheel_offset = HERO_DEFAULT_SCALE + self.control = carla.VehicleControl() + self._hud.notification('Hero Mode') + else: + self.wheel_offset = MAP_DEFAULT_SCALE + self.mouse_offset = [0, 0] + self.mouse_pos = [0, 0] + self._world.scale_offset = [0, 0] + self._world.hero_actor = None + self._hud.notification('Map Mode') + elif event.key == K_F1: + self._hud.show_info = not self._hud.show_info + elif event.key == K_i: + self._hud.show_actor_ids = not self._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 + self.control.gear = self._world.hero_actor.get_control().gear + self._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: + # Toggle autopilot + if self._world.hero_actor is not None: + self._autopilot_enabled = not self._autopilot_enabled + self._world.hero_actor.set_autopilot(self._autopilot_enabled) + self._hud.notification('Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) + elif event.type == pygame.MOUSEBUTTONDOWN: + # Handle mouse wheel for zooming in and out + 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): + """Parses keyboard input when keys are pressed""" + 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): + """Parses mouse input""" + 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): + """Parses the input, which is classified in keyboard events and mouse""" + 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 + if (self._world.hero_actor is not None): + self._world.hero_actor.apply_control(self.control) + + @staticmethod + def _is_quit_shortcut(key): + """Returns True if one of the specified keys are pressed""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + """Initialized, Starts and runs all the needed modules for No Rendering Mode""" + try: + # Init Pygame + pygame.init() + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + # Place a title to game window + pygame.display.set_caption(args.description) + + # Show loading screen + 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 + input_control = InputControl(TITLE_INPUT) + hud = HUD(TITLE_HUD, args.width, args.height) + world = World(TITLE_WORLD, args, timeout=2.0) + + # For each module, assign other modules that are going to be used inside that module + input_control.start(hud, world) + hud.start() + world.start(hud, input_control) + + # Game loop + clock = pygame.time.Clock() + while True: + clock.tick_busy_loop(60) + + # Tick all modules + world.tick(clock) + hud.tick(clock) + input_control.tick(clock) + + # Render all modules + display.fill(COLOR_ALUMINIUM_4) + world.render(display) + hud.render(display) + input_control.render(display) + + pygame.display.flip() + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + finally: + if world is not None: + world.destroy() + + +def exit_game(): + """Shuts down program and PyGame""" + pygame.quit() + sys.exit() + +# ============================================================================== +# -- Main -------------------------------------------------------------------- +# ============================================================================== + + +def main(): + """Parses the arguments received from commandline and runs the game loop""" + + # Define arguments that will be received and parsed + 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='TCP 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', + 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') + + # Parse arguments + args = argparser.parse_args() + args.description = argparser.description + args.width, args.height = [int(x) for x in args.res.split('x')] + + # Print server information + 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__) + + # Run game loop + game_loop(args) + + +if __name__ == '__main__': + main() diff --git a/setup.py b/setup.py index eda845c..3092a0c 100755 --- a/setup.py +++ b/setup.py @@ -20,6 +20,7 @@ url='TODO', license=license, install_requires=requirements, - packages=find_packages(exclude=('tests', 'docs')) + packages=find_packages(exclude=('tests', 'docs')), + include_package_data=True )