Merge branch 'master' into manishthani/load_props_definition
|
@ -0,0 +1,14 @@
|
||||||
|
# Read the Docs configuration file
|
||||||
|
# See https://docs.readthedocs.io/en/stable/config-file/v2.html for details
|
||||||
|
|
||||||
|
version: 2
|
||||||
|
|
||||||
|
mkdocs:
|
||||||
|
configuration: mkdocs.yml
|
||||||
|
|
||||||
|
formats: all
|
||||||
|
|
||||||
|
python:
|
||||||
|
version: 3.7
|
||||||
|
install:
|
||||||
|
- requirements: Docs/requirements.txt
|
|
@ -13,9 +13,10 @@ matrix:
|
||||||
apt:
|
apt:
|
||||||
sources:
|
sources:
|
||||||
- ubuntu-toolchain-r-test
|
- ubuntu-toolchain-r-test
|
||||||
|
- llvm-toolchain-xenial-7
|
||||||
packages:
|
packages:
|
||||||
- g++-7 # we need this one for the libstdc++.
|
- g++-7 # we need this one for the libstdc++.
|
||||||
- clang-6.0
|
- clang-7
|
||||||
- ninja-build
|
- ninja-build
|
||||||
- python
|
- python
|
||||||
- python-pip
|
- python-pip
|
||||||
|
|
|
@ -1,5 +1,6 @@
|
||||||
## Latest
|
## Latest
|
||||||
|
|
||||||
|
* Upgraded to Unreal Engine 4.22
|
||||||
* Recorder fixes:
|
* Recorder fixes:
|
||||||
- Actors at start of playback could interpolate positions from its current position instead than the recorded position, making some fast sliding effect during 1 frame.
|
- Actors at start of playback could interpolate positions from its current position instead than the recorded position, making some fast sliding effect during 1 frame.
|
||||||
- Camera following in playback was not working if a new map was needed to load.
|
- Camera following in playback was not working if a new map was needed to load.
|
||||||
|
@ -7,6 +8,7 @@
|
||||||
- Script 'start_recording.py' now properly saves destruction of actors at stop.
|
- Script 'start_recording.py' now properly saves destruction of actors at stop.
|
||||||
* API extension: waypoint's `junction_id` that returns de OpenDrive identifier of the current junction
|
* API extension: waypoint's `junction_id` that returns de OpenDrive identifier of the current junction
|
||||||
* API change: deprecated waypoint's `is_intersection`, now is `is_junction`
|
* API change: deprecated waypoint's `is_intersection`, now is `is_junction`
|
||||||
|
* Removed deprecated code and content
|
||||||
* New recorder features:
|
* New recorder features:
|
||||||
- Added optional parameter to show more details about a recorder file (related to `show_recorder_file_info.py`)
|
- Added optional parameter to show more details about a recorder file (related to `show_recorder_file_info.py`)
|
||||||
- Added playback speed (slow/fast motion) for the replayer
|
- Added playback speed (slow/fast motion) for the replayer
|
||||||
|
@ -15,11 +17,13 @@
|
||||||
- Wheels of vehicles are animated (steering, throttle, handbrake), also bikes and cycles
|
- Wheels of vehicles are animated (steering, throttle, handbrake), also bikes and cycles
|
||||||
- Walkers animation is simulated in playback (through speed of walker), so they walk properly.
|
- Walkers animation is simulated in playback (through speed of walker), so they walk properly.
|
||||||
* Fixed Lidar effectiveness bug in manual_control.py
|
* Fixed Lidar effectiveness bug in manual_control.py
|
||||||
|
* Fixed dead-lock when loading a new map in synchronous mode
|
||||||
* Added C++ client example using LibCarla
|
* Added C++ client example using LibCarla
|
||||||
* Updated OpenDriveActor to use the new Waypoint API
|
* Updated OpenDriveActor to use the new Waypoint API
|
||||||
* Fixed wrong units in VehiclePhysicsControl's center of mass
|
* Fixed wrong units in VehiclePhysicsControl's center of mass
|
||||||
* Several optimizations to the RPC server, now supports a bigger load of async messages
|
* Several optimizations to the RPC server, now supports a bigger load of async messages
|
||||||
* Register user props in fbx format, make them available in Carla Blueprint Library and spawnable.
|
* Register user props in fbx format, make them available in Carla Blueprint Library and spawnable.
|
||||||
|
* Exposed 'is_invincible' for pedestrians
|
||||||
|
|
||||||
## CARLA 0.9.5
|
## CARLA 0.9.5
|
||||||
|
|
||||||
|
@ -74,6 +78,7 @@
|
||||||
* Fixed obstacle detector not working
|
* Fixed obstacle detector not working
|
||||||
* Fixed small float bug in misc.py
|
* Fixed small float bug in misc.py
|
||||||
|
|
||||||
|
|
||||||
## CARLA 0.9.4
|
## CARLA 0.9.4
|
||||||
|
|
||||||
* Added recording and playback functionality
|
* Added recording and playback functionality
|
||||||
|
|
|
@ -1,4 +0,0 @@
|
||||||
[TYPECHECK]
|
|
||||||
ignore=carla_server_pb2.py
|
|
||||||
ignored-modules=ConfigParser,numpy,numpy.random,pygame,shutil
|
|
||||||
ignored-classes=_socketobject,EpisodeReady,SceneDescription,Sensor
|
|
|
@ -1,2 +0,0 @@
|
||||||
include carla/planner/*.txt
|
|
||||||
include carla/planner/*.png
|
|
|
@ -1,2 +0,0 @@
|
||||||
from .forward_agent import ForwardAgent
|
|
||||||
from .agent import Agent
|
|
|
@ -1,24 +0,0 @@
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
# @author: german,felipecode
|
|
||||||
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
import abc
|
|
||||||
|
|
||||||
|
|
||||||
class Agent(object):
|
|
||||||
def __init__(self):
|
|
||||||
self.__metaclass__ = abc.ABCMeta
|
|
||||||
|
|
||||||
@abc.abstractmethod
|
|
||||||
def run_step(self, measurements, sensor_data, directions, target):
|
|
||||||
"""
|
|
||||||
Function to be redefined by an agent.
|
|
||||||
:param The measurements like speed, the image data and a target
|
|
||||||
:returns A carla Control object, with the steering/gas/brake for the agent
|
|
||||||
"""
|
|
|
@ -1,15 +0,0 @@
|
||||||
|
|
||||||
from carla.agent.agent import Agent
|
|
||||||
from carla.client import VehicleControl
|
|
||||||
|
|
||||||
|
|
||||||
class ForwardAgent(Agent):
|
|
||||||
"""
|
|
||||||
Simple derivation of Agent Class,
|
|
||||||
A trivial agent agent that goes straight
|
|
||||||
"""
|
|
||||||
def run_step(self, measurements, sensor_data, directions, target):
|
|
||||||
control = VehicleControl()
|
|
||||||
control.throttle = 0.9
|
|
||||||
|
|
||||||
return control
|
|
|
@ -1,232 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""CARLA Client."""
|
|
||||||
|
|
||||||
import logging
|
|
||||||
import struct
|
|
||||||
|
|
||||||
from contextlib import contextmanager
|
|
||||||
|
|
||||||
from . import sensor
|
|
||||||
from . import tcp
|
|
||||||
from . import util
|
|
||||||
|
|
||||||
try:
|
|
||||||
from . import carla_server_pb2 as carla_protocol
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file')
|
|
||||||
|
|
||||||
try:
|
|
||||||
import numpy
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import numpy, make sure numpy package is installed.')
|
|
||||||
|
|
||||||
|
|
||||||
VehicleControl = carla_protocol.Control
|
|
||||||
|
|
||||||
|
|
||||||
@contextmanager
|
|
||||||
def make_carla_client(host, world_port, timeout=15):
|
|
||||||
"""Context manager for creating and connecting a CarlaClient."""
|
|
||||||
with util.make_connection(CarlaClient, host, world_port, timeout) as client:
|
|
||||||
yield client
|
|
||||||
|
|
||||||
|
|
||||||
class CarlaClient(object):
|
|
||||||
"""The CARLA client. Manages communications with the CARLA server."""
|
|
||||||
|
|
||||||
def __init__(self, host, world_port, timeout=15):
|
|
||||||
self._world_client = tcp.TCPClient(host, world_port, timeout)
|
|
||||||
self._stream_client = tcp.TCPClient(host, world_port + 1, timeout)
|
|
||||||
self._control_client = tcp.TCPClient(host, world_port + 2, timeout)
|
|
||||||
self._current_settings = None
|
|
||||||
self._is_episode_requested = False
|
|
||||||
self._sensors = {}
|
|
||||||
|
|
||||||
def connect(self, connection_attempts=10):
|
|
||||||
"""
|
|
||||||
Try to establish a connection to a CARLA server at the given host:port.
|
|
||||||
"""
|
|
||||||
self._world_client.connect(connection_attempts)
|
|
||||||
|
|
||||||
def disconnect(self):
|
|
||||||
"""Disconnect from server."""
|
|
||||||
self._control_client.disconnect()
|
|
||||||
self._stream_client.disconnect()
|
|
||||||
self._world_client.disconnect()
|
|
||||||
|
|
||||||
def connected(self):
|
|
||||||
"""Return whether there is an active connection."""
|
|
||||||
return self._world_client.connected()
|
|
||||||
|
|
||||||
def load_settings(self, carla_settings):
|
|
||||||
"""
|
|
||||||
Load new settings and request a new episode based on these settings.
|
|
||||||
carla_settings object must be convertible to a str holding the contents
|
|
||||||
of a CarlaSettings.ini file.
|
|
||||||
|
|
||||||
Return a protobuf object holding the scene description.
|
|
||||||
"""
|
|
||||||
self._current_settings = carla_settings
|
|
||||||
return self._request_new_episode(carla_settings)
|
|
||||||
|
|
||||||
def start_episode(self, player_start_index):
|
|
||||||
"""
|
|
||||||
Start the new episode at the player start given by the
|
|
||||||
player_start_index. The list of player starts is retrieved by
|
|
||||||
"load_settings".
|
|
||||||
|
|
||||||
The new episode is started based on the last settings loaded by
|
|
||||||
"load_settings".
|
|
||||||
|
|
||||||
This function waits until the server answers with an EpisodeReady.
|
|
||||||
"""
|
|
||||||
if self._current_settings is None:
|
|
||||||
raise RuntimeError('no settings loaded, cannot start episode')
|
|
||||||
|
|
||||||
# if no new settings are loaded, request new episode with previous
|
|
||||||
if not self._is_episode_requested:
|
|
||||||
self._request_new_episode(self._current_settings)
|
|
||||||
|
|
||||||
try:
|
|
||||||
pb_message = carla_protocol.EpisodeStart()
|
|
||||||
pb_message.player_start_spot_index = player_start_index
|
|
||||||
self._world_client.write(pb_message.SerializeToString())
|
|
||||||
# Wait for EpisodeReady.
|
|
||||||
data = self._world_client.read()
|
|
||||||
if not data:
|
|
||||||
raise RuntimeError('failed to read data from server')
|
|
||||||
pb_message = carla_protocol.EpisodeReady()
|
|
||||||
pb_message.ParseFromString(data)
|
|
||||||
if not pb_message.ready:
|
|
||||||
raise RuntimeError('cannot start episode: server failed to start episode')
|
|
||||||
# We can start the agent clients now.
|
|
||||||
self._stream_client.connect()
|
|
||||||
self._control_client.connect()
|
|
||||||
# Set again the status for no episode requested
|
|
||||||
finally:
|
|
||||||
self._is_episode_requested = False
|
|
||||||
|
|
||||||
def read_data(self):
|
|
||||||
"""
|
|
||||||
Read the data sent from the server this frame. The episode must be
|
|
||||||
started. Return a pair containing the protobuf object containing the
|
|
||||||
measurements followed by the raw data of the sensors.
|
|
||||||
"""
|
|
||||||
# Read measurements.
|
|
||||||
data = self._stream_client.read()
|
|
||||||
if not data:
|
|
||||||
raise RuntimeError('failed to read data from server')
|
|
||||||
pb_message = carla_protocol.Measurements()
|
|
||||||
pb_message.ParseFromString(data)
|
|
||||||
# Read sensor data.
|
|
||||||
return pb_message, dict(x for x in self._read_sensor_data())
|
|
||||||
|
|
||||||
def send_control(self, *args, **kwargs):
|
|
||||||
"""
|
|
||||||
Send the VehicleControl to be applied this frame.
|
|
||||||
|
|
||||||
If synchronous mode was requested, the server will pause the simulation
|
|
||||||
until this message is received.
|
|
||||||
"""
|
|
||||||
if isinstance(args[0] if args else None, carla_protocol.Control):
|
|
||||||
pb_message = args[0]
|
|
||||||
else:
|
|
||||||
pb_message = carla_protocol.Control()
|
|
||||||
pb_message.steer = kwargs.get('steer', 0.0)
|
|
||||||
pb_message.throttle = kwargs.get('throttle', 0.0)
|
|
||||||
pb_message.brake = kwargs.get('brake', 0.0)
|
|
||||||
pb_message.hand_brake = kwargs.get('hand_brake', False)
|
|
||||||
pb_message.reverse = kwargs.get('reverse', False)
|
|
||||||
self._control_client.write(pb_message.SerializeToString())
|
|
||||||
|
|
||||||
def _request_new_episode(self, carla_settings):
|
|
||||||
"""
|
|
||||||
Internal function to request a new episode. Prepare the client for a new
|
|
||||||
episode by disconnecting agent clients.
|
|
||||||
"""
|
|
||||||
# Disconnect agent clients.
|
|
||||||
self._stream_client.disconnect()
|
|
||||||
self._control_client.disconnect()
|
|
||||||
# Send new episode request.
|
|
||||||
pb_message = carla_protocol.RequestNewEpisode()
|
|
||||||
pb_message.ini_file = str(carla_settings)
|
|
||||||
self._world_client.write(pb_message.SerializeToString())
|
|
||||||
# Read scene description.
|
|
||||||
data = self._world_client.read()
|
|
||||||
if not data:
|
|
||||||
raise RuntimeError('failed to read data from server')
|
|
||||||
pb_message = carla_protocol.SceneDescription()
|
|
||||||
pb_message.ParseFromString(data)
|
|
||||||
self._sensors = dict((sensor.id, sensor) \
|
|
||||||
for sensor in _make_sensor_parsers(pb_message.sensors))
|
|
||||||
self._is_episode_requested = True
|
|
||||||
return pb_message
|
|
||||||
|
|
||||||
def _read_sensor_data(self):
|
|
||||||
while True:
|
|
||||||
data = self._stream_client.read()
|
|
||||||
if not data:
|
|
||||||
raise StopIteration
|
|
||||||
yield self._parse_sensor_data(data)
|
|
||||||
|
|
||||||
def _parse_sensor_data(self, data):
|
|
||||||
sensor_id = struct.unpack('<L', data[0:4])[0]
|
|
||||||
parser = self._sensors[sensor_id]
|
|
||||||
return parser.name, parser.parse_raw_data(data[4:])
|
|
||||||
|
|
||||||
|
|
||||||
def _make_sensor_parsers(sensors):
|
|
||||||
image_types = ['None', 'SceneFinal', 'Depth', 'SemanticSegmentation']
|
|
||||||
getimgtype = lambda id: image_types[id] if len(image_types) > id else 'Unknown'
|
|
||||||
getint32 = lambda data, index: struct.unpack('<L', data[index*4:index*4+4])[0]
|
|
||||||
getint64 = lambda data, index: struct.unpack('<Q', data[index*4:index*4+8])[0]
|
|
||||||
getfloat = lambda data, index: struct.unpack('<f', data[index*4:index*4+4])[0]
|
|
||||||
|
|
||||||
def parse_image(data):
|
|
||||||
frame_number = getint64(data, 0)
|
|
||||||
width = getint32(data, 2)
|
|
||||||
height = getint32(data, 3)
|
|
||||||
image_type = getimgtype(getint32(data, 4))
|
|
||||||
fov = getfloat(data, 5)
|
|
||||||
return sensor.Image(frame_number, width, height, image_type, fov, data[24:])
|
|
||||||
|
|
||||||
def parse_lidar(data):
|
|
||||||
frame_number = getint64(data, 0)
|
|
||||||
horizontal_angle = getfloat(data, 2)
|
|
||||||
channels = getint32(data, 3)
|
|
||||||
header_size = 16
|
|
||||||
point_count_by_channel = numpy.frombuffer(
|
|
||||||
data[header_size:header_size+channels*4],
|
|
||||||
dtype=numpy.dtype('uint32'))
|
|
||||||
points = numpy.frombuffer(
|
|
||||||
data[header_size+channels*4:],
|
|
||||||
dtype=numpy.dtype('f4'))
|
|
||||||
points = numpy.reshape(points, (int(points.shape[0]/3), 3))
|
|
||||||
return sensor.LidarMeasurement(
|
|
||||||
frame_number,
|
|
||||||
horizontal_angle,
|
|
||||||
channels,
|
|
||||||
point_count_by_channel,
|
|
||||||
sensor.PointCloud(frame_number, points))
|
|
||||||
|
|
||||||
class SensorDefinition(object):
|
|
||||||
def __init__(self, s):
|
|
||||||
self.id = s.id
|
|
||||||
self.name = s.name
|
|
||||||
self.type = s.type
|
|
||||||
self.parse_raw_data = lambda x: x
|
|
||||||
|
|
||||||
for s in sensors:
|
|
||||||
sensor_def = SensorDefinition(s)
|
|
||||||
if sensor_def.type == carla_protocol.Sensor.CAMERA:
|
|
||||||
sensor_def.parse_raw_data = parse_image
|
|
||||||
elif sensor_def.type == carla_protocol.Sensor.LIDAR_RAY_CAST:
|
|
||||||
sensor_def.parse_raw_data = parse_lidar
|
|
||||||
else:
|
|
||||||
logging.error('unknown sensor type %s', sensor_def.type)
|
|
||||||
yield sensor_def
|
|
|
@ -1 +0,0 @@
|
||||||
from .driving_benchmark import run_driving_benchmark
|
|
|
@ -1,317 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
|
|
||||||
import abc
|
|
||||||
import logging
|
|
||||||
import math
|
|
||||||
import time
|
|
||||||
|
|
||||||
from carla.client import VehicleControl
|
|
||||||
from carla.client import make_carla_client
|
|
||||||
from carla.driving_benchmark.metrics import Metrics
|
|
||||||
from carla.planner.planner import Planner
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
|
|
||||||
from . import results_printer
|
|
||||||
from .recording import Recording
|
|
||||||
|
|
||||||
|
|
||||||
def sldist(c1, c2):
|
|
||||||
return math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1]) ** 2)
|
|
||||||
|
|
||||||
|
|
||||||
class DrivingBenchmark(object):
|
|
||||||
"""
|
|
||||||
The Benchmark class, controls the execution of the benchmark interfacing
|
|
||||||
an Agent class with a set Suite.
|
|
||||||
|
|
||||||
|
|
||||||
The benchmark class must be inherited by a class that defines the
|
|
||||||
all the experiments to be run by the agent
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(
|
|
||||||
self,
|
|
||||||
city_name='Town01',
|
|
||||||
name_to_save='Test',
|
|
||||||
continue_experiment=False,
|
|
||||||
save_images=False,
|
|
||||||
distance_for_success=2.0
|
|
||||||
):
|
|
||||||
|
|
||||||
self.__metaclass__ = abc.ABCMeta
|
|
||||||
|
|
||||||
self._city_name = city_name
|
|
||||||
self._base_name = name_to_save
|
|
||||||
# The minimum distance for arriving into the goal point in
|
|
||||||
# order to consider ir a success
|
|
||||||
self._distance_for_success = distance_for_success
|
|
||||||
# The object used to record the benchmark and to able to continue after
|
|
||||||
self._recording = Recording(name_to_save=name_to_save,
|
|
||||||
continue_experiment=continue_experiment,
|
|
||||||
save_images=save_images
|
|
||||||
)
|
|
||||||
|
|
||||||
# We have a default planner instantiated that produces high level commands
|
|
||||||
self._planner = Planner(city_name)
|
|
||||||
|
|
||||||
def benchmark_agent(self, experiment_suite, agent, client):
|
|
||||||
"""
|
|
||||||
Function to benchmark the agent.
|
|
||||||
It first checks the log file of this benchmark.
|
|
||||||
if it exists, it continues from the experiment where it stopped.
|
|
||||||
|
|
||||||
|
|
||||||
Args:
|
|
||||||
experiment_suite
|
|
||||||
agent: an agent object with the run step class implemented.
|
|
||||||
client:
|
|
||||||
|
|
||||||
|
|
||||||
Return:
|
|
||||||
A dictionary with all the metrics computed from the
|
|
||||||
agent running the set of experiments.
|
|
||||||
"""
|
|
||||||
|
|
||||||
# Instantiate a metric object that will be used to compute the metrics for
|
|
||||||
# the benchmark afterwards.
|
|
||||||
metrics_object = Metrics(experiment_suite.metrics_parameters,
|
|
||||||
experiment_suite.dynamic_tasks)
|
|
||||||
|
|
||||||
# Function returns the current pose and task for this benchmark.
|
|
||||||
start_pose, start_experiment = self._recording.get_pose_and_experiment(
|
|
||||||
experiment_suite.get_number_of_poses_task())
|
|
||||||
|
|
||||||
logging.info('START')
|
|
||||||
|
|
||||||
for experiment in experiment_suite.get_experiments()[int(start_experiment):]:
|
|
||||||
|
|
||||||
positions = client.load_settings(
|
|
||||||
experiment.conditions).player_start_spots
|
|
||||||
|
|
||||||
self._recording.log_start(experiment.task)
|
|
||||||
|
|
||||||
for pose in experiment.poses[start_pose:]:
|
|
||||||
for rep in range(experiment.repetitions):
|
|
||||||
|
|
||||||
start_index = pose[0]
|
|
||||||
end_index = pose[1]
|
|
||||||
|
|
||||||
client.start_episode(start_index)
|
|
||||||
# Print information on
|
|
||||||
logging.info('======== !!!! ==========')
|
|
||||||
logging.info(' Start Position %d End Position %d ',
|
|
||||||
start_index, end_index)
|
|
||||||
|
|
||||||
self._recording.log_poses(start_index, end_index,
|
|
||||||
experiment.Conditions.WeatherId)
|
|
||||||
|
|
||||||
# Calculate the initial distance for this episode
|
|
||||||
initial_distance = \
|
|
||||||
sldist(
|
|
||||||
[positions[start_index].location.x, positions[start_index].location.y],
|
|
||||||
[positions[end_index].location.x, positions[end_index].location.y])
|
|
||||||
|
|
||||||
time_out = experiment_suite.calculate_time_out(
|
|
||||||
self._get_shortest_path(positions[start_index], positions[end_index]))
|
|
||||||
|
|
||||||
# running the agent
|
|
||||||
(result, reward_vec, control_vec, final_time, remaining_distance) = \
|
|
||||||
self._run_navigation_episode(
|
|
||||||
agent, client, time_out, positions[end_index],
|
|
||||||
str(experiment.Conditions.WeatherId) + '_'
|
|
||||||
+ str(experiment.task) + '_' + str(start_index)
|
|
||||||
+ '.' + str(end_index))
|
|
||||||
|
|
||||||
# Write the general status of the just ran episode
|
|
||||||
self._recording.write_summary_results(
|
|
||||||
experiment, pose, rep, initial_distance,
|
|
||||||
remaining_distance, final_time, time_out, result)
|
|
||||||
|
|
||||||
# Write the details of this episode.
|
|
||||||
self._recording.write_measurements_results(experiment, rep, pose, reward_vec,
|
|
||||||
control_vec)
|
|
||||||
if result > 0:
|
|
||||||
logging.info('+++++ Target achieved in %f seconds! +++++',
|
|
||||||
final_time)
|
|
||||||
else:
|
|
||||||
logging.info('----- Timeout! -----')
|
|
||||||
|
|
||||||
start_pose = 0
|
|
||||||
|
|
||||||
self._recording.log_end()
|
|
||||||
|
|
||||||
return metrics_object.compute(self._recording.path)
|
|
||||||
|
|
||||||
def get_path(self):
|
|
||||||
"""
|
|
||||||
Returns the path where the log was saved.
|
|
||||||
"""
|
|
||||||
return self._recording.path
|
|
||||||
|
|
||||||
def _get_directions(self, current_point, end_point):
|
|
||||||
"""
|
|
||||||
Class that should return the directions to reach a certain goal
|
|
||||||
"""
|
|
||||||
|
|
||||||
directions = self._planner.get_next_command(
|
|
||||||
(current_point.location.x,
|
|
||||||
current_point.location.y, 0.22),
|
|
||||||
(current_point.orientation.x,
|
|
||||||
current_point.orientation.y,
|
|
||||||
current_point.orientation.z),
|
|
||||||
(end_point.location.x, end_point.location.y, 0.22),
|
|
||||||
(end_point.orientation.x, end_point.orientation.y, end_point.orientation.z))
|
|
||||||
return directions
|
|
||||||
|
|
||||||
def _get_shortest_path(self, start_point, end_point):
|
|
||||||
"""
|
|
||||||
Calculates the shortest path between two points considering the road network
|
|
||||||
"""
|
|
||||||
|
|
||||||
return self._planner.get_shortest_path_distance(
|
|
||||||
[
|
|
||||||
start_point.location.x, start_point.location.y, 0.22], [
|
|
||||||
start_point.orientation.x, start_point.orientation.y, 0.22], [
|
|
||||||
end_point.location.x, end_point.location.y, end_point.location.z], [
|
|
||||||
end_point.orientation.x, end_point.orientation.y, end_point.orientation.z])
|
|
||||||
|
|
||||||
def _run_navigation_episode(
|
|
||||||
self,
|
|
||||||
agent,
|
|
||||||
client,
|
|
||||||
time_out,
|
|
||||||
target,
|
|
||||||
episode_name):
|
|
||||||
"""
|
|
||||||
Runs one episode of the benchmark (Pose) for a certain agent.
|
|
||||||
|
|
||||||
|
|
||||||
Args:
|
|
||||||
agent: the agent object
|
|
||||||
client: an object of the carla client to communicate
|
|
||||||
with the CARLA simulator
|
|
||||||
time_out: the time limit to complete this episode
|
|
||||||
target: the target to reach
|
|
||||||
episode_name: The name for saving images of this episode
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
# Send an initial command.
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
client.send_control(VehicleControl())
|
|
||||||
|
|
||||||
initial_timestamp = measurements.game_timestamp
|
|
||||||
current_timestamp = initial_timestamp
|
|
||||||
|
|
||||||
# The vector containing all measurements produced on this episode
|
|
||||||
measurement_vec = []
|
|
||||||
# The vector containing all controls produced on this episode
|
|
||||||
control_vec = []
|
|
||||||
frame = 0
|
|
||||||
distance = 10000
|
|
||||||
success = False
|
|
||||||
|
|
||||||
while (current_timestamp - initial_timestamp) < (time_out * 1000) and not success:
|
|
||||||
|
|
||||||
# Read data from server with the client
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
# The directions to reach the goal are calculated.
|
|
||||||
directions = self._get_directions(measurements.player_measurements.transform, target)
|
|
||||||
# The agent processes the data.
|
|
||||||
control = agent.run_step(measurements, sensor_data, directions, target)
|
|
||||||
# Send the control commands to the vehicle
|
|
||||||
client.send_control(control)
|
|
||||||
|
|
||||||
# save images if the flag is activated
|
|
||||||
self._recording.save_images(sensor_data, episode_name, frame)
|
|
||||||
|
|
||||||
current_x = measurements.player_measurements.transform.location.x
|
|
||||||
current_y = measurements.player_measurements.transform.location.y
|
|
||||||
|
|
||||||
logging.info("Controller is Inputting:")
|
|
||||||
logging.info('Steer = %f Throttle = %f Brake = %f ',
|
|
||||||
control.steer, control.throttle, control.brake)
|
|
||||||
|
|
||||||
current_timestamp = measurements.game_timestamp
|
|
||||||
# Get the distance travelled until now
|
|
||||||
distance = sldist([current_x, current_y],
|
|
||||||
[target.location.x, target.location.y])
|
|
||||||
# Write status of the run on verbose mode
|
|
||||||
logging.info('Status:')
|
|
||||||
logging.info(
|
|
||||||
'[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f',
|
|
||||||
float(distance), current_x, current_y, target.location.x,
|
|
||||||
target.location.y)
|
|
||||||
# Check if reach the target
|
|
||||||
if distance < self._distance_for_success:
|
|
||||||
success = True
|
|
||||||
|
|
||||||
# Increment the vectors and append the measurements and controls.
|
|
||||||
frame += 1
|
|
||||||
measurement_vec.append(measurements.player_measurements)
|
|
||||||
control_vec.append(control)
|
|
||||||
|
|
||||||
if success:
|
|
||||||
return 1, measurement_vec, control_vec, float(
|
|
||||||
current_timestamp - initial_timestamp) / 1000.0, distance
|
|
||||||
return 0, measurement_vec, control_vec, time_out, distance
|
|
||||||
|
|
||||||
|
|
||||||
def run_driving_benchmark(agent,
|
|
||||||
experiment_suite,
|
|
||||||
city_name='Town01',
|
|
||||||
log_name='Test',
|
|
||||||
continue_experiment=False,
|
|
||||||
host='127.0.0.1',
|
|
||||||
port=2000
|
|
||||||
):
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
with make_carla_client(host, port) as client:
|
|
||||||
# Hack to fix for the issue 310, we force a reset, so it does not get
|
|
||||||
# the positions on first server reset.
|
|
||||||
client.load_settings(CarlaSettings())
|
|
||||||
client.start_episode(0)
|
|
||||||
|
|
||||||
# We instantiate the driving benchmark, that is the engine used to
|
|
||||||
# benchmark an agent. The instantiation starts the log process, sets
|
|
||||||
|
|
||||||
benchmark = DrivingBenchmark(city_name=city_name,
|
|
||||||
name_to_save=log_name + '_'
|
|
||||||
+ type(experiment_suite).__name__
|
|
||||||
+ '_' + city_name,
|
|
||||||
continue_experiment=continue_experiment)
|
|
||||||
# This function performs the benchmark. It returns a dictionary summarizing
|
|
||||||
# the entire execution.
|
|
||||||
|
|
||||||
benchmark_summary = benchmark.benchmark_agent(experiment_suite, agent, client)
|
|
||||||
|
|
||||||
print("")
|
|
||||||
print("")
|
|
||||||
print("----- Printing results for training weathers (Seen in Training) -----")
|
|
||||||
print("")
|
|
||||||
print("")
|
|
||||||
results_printer.print_summary(benchmark_summary, experiment_suite.train_weathers,
|
|
||||||
benchmark.get_path())
|
|
||||||
|
|
||||||
print("")
|
|
||||||
print("")
|
|
||||||
print("----- Printing results for test weathers (Unseen in Training) -----")
|
|
||||||
print("")
|
|
||||||
print("")
|
|
||||||
|
|
||||||
results_printer.print_summary(benchmark_summary, experiment_suite.test_weathers,
|
|
||||||
benchmark.get_path())
|
|
||||||
|
|
||||||
break
|
|
||||||
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
|
@ -1,53 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
|
|
||||||
|
|
||||||
class Experiment(object):
|
|
||||||
"""
|
|
||||||
Experiment defines a certain task, under conditions
|
|
||||||
A task is associated with a set of poses, containing start and end pose.
|
|
||||||
|
|
||||||
Conditions are associated with a carla Settings and describe the following:
|
|
||||||
|
|
||||||
Number Of Vehicles
|
|
||||||
Number Of Pedestrians
|
|
||||||
Weather
|
|
||||||
Random Seed of the agents, describing their behaviour.
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
self.Task = 0
|
|
||||||
self.Conditions = CarlaSettings()
|
|
||||||
self.Poses = [[]]
|
|
||||||
self.Repetitions = 1
|
|
||||||
|
|
||||||
def set(self, **kwargs):
|
|
||||||
for key, value in kwargs.items():
|
|
||||||
if not hasattr(self, key):
|
|
||||||
raise ValueError('Experiment: no key named %r' % key)
|
|
||||||
setattr(self, key, value)
|
|
||||||
|
|
||||||
if self.Repetitions != 1:
|
|
||||||
raise NotImplementedError()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def task(self):
|
|
||||||
return self.Task
|
|
||||||
|
|
||||||
@property
|
|
||||||
def conditions(self):
|
|
||||||
return self.Conditions
|
|
||||||
|
|
||||||
@property
|
|
||||||
def poses(self):
|
|
||||||
return self.Poses
|
|
||||||
|
|
||||||
@property
|
|
||||||
def repetitions(self):
|
|
||||||
return self.Repetitions
|
|
|
@ -1,2 +0,0 @@
|
||||||
from .basic_experiment_suite import BasicExperimentSuite
|
|
||||||
from .corl_2017 import CoRL2017
|
|
|
@ -1,83 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
from carla.sensor import Camera
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
|
|
||||||
from .experiment_suite import ExperimentSuite
|
|
||||||
|
|
||||||
|
|
||||||
class BasicExperimentSuite(ExperimentSuite):
|
|
||||||
|
|
||||||
@property
|
|
||||||
def train_weathers(self):
|
|
||||||
return [1]
|
|
||||||
|
|
||||||
@property
|
|
||||||
def test_weathers(self):
|
|
||||||
return [1]
|
|
||||||
|
|
||||||
def build_experiments(self):
|
|
||||||
"""
|
|
||||||
Creates the whole set of experiment objects,
|
|
||||||
The experiments created depends on the selected Town.
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
# We check the town, based on that we define the town related parameters
|
|
||||||
# The size of the vector is related to the number of tasks, inside each
|
|
||||||
# task there is also multiple poses ( start end, positions )
|
|
||||||
if self._city_name == 'Town01':
|
|
||||||
poses_tasks = [[[7, 3]], [[138, 17]], [[140, 134]], [[140, 134]]]
|
|
||||||
vehicles_tasks = [0, 0, 0, 20]
|
|
||||||
pedestrians_tasks = [0, 0, 0, 50]
|
|
||||||
else:
|
|
||||||
poses_tasks = [[[4, 2]], [[37, 76]], [[19, 66]], [[19, 66]]]
|
|
||||||
vehicles_tasks = [0, 0, 0, 15]
|
|
||||||
pedestrians_tasks = [0, 0, 0, 50]
|
|
||||||
|
|
||||||
# We set the camera
|
|
||||||
# This single RGB camera is used on every experiment
|
|
||||||
|
|
||||||
camera = Camera('CameraRGB')
|
|
||||||
camera.set(FOV=100)
|
|
||||||
camera.set_image_size(800, 600)
|
|
||||||
camera.set_position(2.0, 0.0, 1.4)
|
|
||||||
camera.set_rotation(-15.0, 0, 0)
|
|
||||||
|
|
||||||
# Based on the parameters, creates a vector with experiment objects.
|
|
||||||
experiments_vector = []
|
|
||||||
for weather in self.weathers:
|
|
||||||
|
|
||||||
for iteration in range(len(poses_tasks)):
|
|
||||||
poses = poses_tasks[iteration]
|
|
||||||
vehicles = vehicles_tasks[iteration]
|
|
||||||
pedestrians = pedestrians_tasks[iteration]
|
|
||||||
|
|
||||||
conditions = CarlaSettings()
|
|
||||||
conditions.set(
|
|
||||||
SendNonPlayerAgentsInfo=True,
|
|
||||||
NumberOfVehicles=vehicles,
|
|
||||||
NumberOfPedestrians=pedestrians,
|
|
||||||
WeatherId=weather
|
|
||||||
|
|
||||||
)
|
|
||||||
# Add all the cameras that were set for this experiments
|
|
||||||
conditions.add_sensor(camera)
|
|
||||||
experiment = Experiment()
|
|
||||||
experiment.set(
|
|
||||||
Conditions=conditions,
|
|
||||||
Poses=poses,
|
|
||||||
Task=iteration,
|
|
||||||
Repetitions=1
|
|
||||||
)
|
|
||||||
experiments_vector.append(experiment)
|
|
||||||
|
|
||||||
return experiments_vector
|
|
|
@ -1,144 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
# CORL experiment set.
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
from carla.sensor import Camera
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.driving_benchmark.experiment_suites.experiment_suite import ExperimentSuite
|
|
||||||
|
|
||||||
|
|
||||||
class CoRL2017(ExperimentSuite):
|
|
||||||
|
|
||||||
@property
|
|
||||||
def train_weathers(self):
|
|
||||||
return [1, 3, 6, 8]
|
|
||||||
|
|
||||||
@property
|
|
||||||
def test_weathers(self):
|
|
||||||
return [4, 14]
|
|
||||||
|
|
||||||
def _poses_town01(self):
|
|
||||||
"""
|
|
||||||
Each matrix is a new task. We have all the four tasks
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
def _poses_straight():
|
|
||||||
return [[36, 40], [39, 35], [110, 114], [7, 3], [0, 4],
|
|
||||||
[68, 50], [61, 59], [47, 64], [147, 90], [33, 87],
|
|
||||||
[26, 19], [80, 76], [45, 49], [55, 44], [29, 107],
|
|
||||||
[95, 104], [84, 34], [53, 67], [22, 17], [91, 148],
|
|
||||||
[20, 107], [78, 70], [95, 102], [68, 44], [45, 69]]
|
|
||||||
|
|
||||||
def _poses_one_curve():
|
|
||||||
return [[138, 17], [47, 16], [26, 9], [42, 49], [140, 124],
|
|
||||||
[85, 98], [65, 133], [137, 51], [76, 66], [46, 39],
|
|
||||||
[40, 60], [0, 29], [4, 129], [121, 140], [2, 129],
|
|
||||||
[78, 44], [68, 85], [41, 102], [95, 70], [68, 129],
|
|
||||||
[84, 69], [47, 79], [110, 15], [130, 17], [0, 17]]
|
|
||||||
|
|
||||||
def _poses_navigation():
|
|
||||||
return [[105, 29], [27, 130], [102, 87], [132, 27], [24, 44],
|
|
||||||
[96, 26], [34, 67], [28, 1], [140, 134], [105, 9],
|
|
||||||
[148, 129], [65, 18], [21, 16], [147, 97], [42, 51],
|
|
||||||
[30, 41], [18, 107], [69, 45], [102, 95], [18, 145],
|
|
||||||
[111, 64], [79, 45], [84, 69], [73, 31], [37, 81]]
|
|
||||||
|
|
||||||
return [_poses_straight(),
|
|
||||||
_poses_one_curve(),
|
|
||||||
_poses_navigation(),
|
|
||||||
_poses_navigation()]
|
|
||||||
|
|
||||||
def _poses_town02(self):
|
|
||||||
|
|
||||||
def _poses_straight():
|
|
||||||
return [[38, 34], [4, 2], [12, 10], [62, 55], [43, 47],
|
|
||||||
[64, 66], [78, 76], [59, 57], [61, 18], [35, 39],
|
|
||||||
[12, 8], [0, 18], [75, 68], [54, 60], [45, 49],
|
|
||||||
[46, 42], [53, 46], [80, 29], [65, 63], [0, 81],
|
|
||||||
[54, 63], [51, 42], [16, 19], [17, 26], [77, 68]]
|
|
||||||
|
|
||||||
def _poses_one_curve():
|
|
||||||
return [[37, 76], [8, 24], [60, 69], [38, 10], [21, 1],
|
|
||||||
[58, 71], [74, 32], [44, 0], [71, 16], [14, 24],
|
|
||||||
[34, 11], [43, 14], [75, 16], [80, 21], [3, 23],
|
|
||||||
[75, 59], [50, 47], [11, 19], [77, 34], [79, 25],
|
|
||||||
[40, 63], [58, 76], [79, 55], [16, 61], [27, 11]]
|
|
||||||
|
|
||||||
def _poses_navigation():
|
|
||||||
return [[19, 66], [79, 14], [19, 57], [23, 1],
|
|
||||||
[53, 76], [42, 13], [31, 71], [33, 5],
|
|
||||||
[54, 30], [10, 61], [66, 3], [27, 12],
|
|
||||||
[79, 19], [2, 29], [16, 14], [5, 57],
|
|
||||||
[70, 73], [46, 67], [57, 50], [61, 49], [21, 12],
|
|
||||||
[51, 81], [77, 68], [56, 65], [43, 54]]
|
|
||||||
|
|
||||||
return [_poses_straight(),
|
|
||||||
_poses_one_curve(),
|
|
||||||
_poses_navigation(),
|
|
||||||
_poses_navigation()
|
|
||||||
]
|
|
||||||
|
|
||||||
def build_experiments(self):
|
|
||||||
"""
|
|
||||||
Creates the whole set of experiment objects,
|
|
||||||
The experiments created depend on the selected Town.
|
|
||||||
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
# We set the camera
|
|
||||||
# This single RGB camera is used on every experiment
|
|
||||||
|
|
||||||
camera = Camera('CameraRGB')
|
|
||||||
camera.set(FOV=100)
|
|
||||||
camera.set_image_size(800, 600)
|
|
||||||
camera.set_position(2.0, 0.0, 1.4)
|
|
||||||
camera.set_rotation(-15.0, 0, 0)
|
|
||||||
|
|
||||||
if self._city_name == 'Town01':
|
|
||||||
poses_tasks = self._poses_town01()
|
|
||||||
vehicles_tasks = [0, 0, 0, 20]
|
|
||||||
pedestrians_tasks = [0, 0, 0, 50]
|
|
||||||
else:
|
|
||||||
poses_tasks = self._poses_town02()
|
|
||||||
vehicles_tasks = [0, 0, 0, 15]
|
|
||||||
pedestrians_tasks = [0, 0, 0, 50]
|
|
||||||
|
|
||||||
experiments_vector = []
|
|
||||||
|
|
||||||
for weather in self.weathers:
|
|
||||||
|
|
||||||
for iteration in range(len(poses_tasks)):
|
|
||||||
poses = poses_tasks[iteration]
|
|
||||||
vehicles = vehicles_tasks[iteration]
|
|
||||||
pedestrians = pedestrians_tasks[iteration]
|
|
||||||
|
|
||||||
conditions = CarlaSettings()
|
|
||||||
conditions.set(
|
|
||||||
SendNonPlayerAgentsInfo=True,
|
|
||||||
NumberOfVehicles=vehicles,
|
|
||||||
NumberOfPedestrians=pedestrians,
|
|
||||||
WeatherId=weather
|
|
||||||
)
|
|
||||||
# Add all the cameras that were set for this experiments
|
|
||||||
|
|
||||||
conditions.add_sensor(camera)
|
|
||||||
|
|
||||||
experiment = Experiment()
|
|
||||||
experiment.set(
|
|
||||||
Conditions=conditions,
|
|
||||||
Poses=poses,
|
|
||||||
Task=iteration,
|
|
||||||
Repetitions=1
|
|
||||||
)
|
|
||||||
experiments_vector.append(experiment)
|
|
||||||
|
|
||||||
return experiments_vector
|
|
|
@ -1,102 +0,0 @@
|
||||||
# To be redefined on subclasses on how to calculate timeout for an episode
|
|
||||||
import abc
|
|
||||||
|
|
||||||
|
|
||||||
class ExperimentSuite(object):
|
|
||||||
|
|
||||||
def __init__(self, city_name):
|
|
||||||
|
|
||||||
self._city_name = city_name
|
|
||||||
self._experiments = self.build_experiments()
|
|
||||||
|
|
||||||
def calculate_time_out(self, path_distance):
|
|
||||||
"""
|
|
||||||
Function to return the timeout ,in milliseconds,
|
|
||||||
that is calculated based on distance to goal.
|
|
||||||
This is the same timeout as used on the CoRL paper.
|
|
||||||
"""
|
|
||||||
return ((path_distance / 1000.0) / 10.0) * 3600.0 + 10.0
|
|
||||||
|
|
||||||
def get_number_of_poses_task(self):
|
|
||||||
"""
|
|
||||||
Get the number of poses a task have for this benchmark
|
|
||||||
"""
|
|
||||||
|
|
||||||
# Warning: assumes that all tasks have the same size
|
|
||||||
|
|
||||||
return len(self._experiments[0].poses)
|
|
||||||
|
|
||||||
def get_experiments(self):
|
|
||||||
"""
|
|
||||||
Getter for the experiment set.
|
|
||||||
"""
|
|
||||||
return self._experiments
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dynamic_tasks(self):
|
|
||||||
"""
|
|
||||||
Returns the episodes that contain dynamic obstacles
|
|
||||||
"""
|
|
||||||
dynamic_tasks = set()
|
|
||||||
for exp in self._experiments:
|
|
||||||
if exp.conditions.NumberOfVehicles > 0 or exp.conditions.NumberOfPedestrians > 0:
|
|
||||||
dynamic_tasks.add(exp.task)
|
|
||||||
|
|
||||||
return list(dynamic_tasks)
|
|
||||||
|
|
||||||
@property
|
|
||||||
def metrics_parameters(self):
|
|
||||||
"""
|
|
||||||
Property to return the parameters for the metric module
|
|
||||||
Could be redefined depending on the needs of the user.
|
|
||||||
"""
|
|
||||||
return {
|
|
||||||
|
|
||||||
'intersection_offroad': {'frames_skip': 10,
|
|
||||||
'frames_recount': 20,
|
|
||||||
'threshold': 0.3
|
|
||||||
},
|
|
||||||
'intersection_otherlane': {'frames_skip': 10,
|
|
||||||
'frames_recount': 20,
|
|
||||||
'threshold': 0.4
|
|
||||||
},
|
|
||||||
'collision_other': {'frames_skip': 10,
|
|
||||||
'frames_recount': 20,
|
|
||||||
'threshold': 400
|
|
||||||
},
|
|
||||||
'collision_vehicles': {'frames_skip': 10,
|
|
||||||
'frames_recount': 30,
|
|
||||||
'threshold': 400
|
|
||||||
},
|
|
||||||
'collision_pedestrians': {'frames_skip': 5,
|
|
||||||
'frames_recount': 100,
|
|
||||||
'threshold': 300
|
|
||||||
},
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
@property
|
|
||||||
def weathers(self):
|
|
||||||
weathers = set(self.train_weathers)
|
|
||||||
weathers.update(self.test_weathers)
|
|
||||||
return weathers
|
|
||||||
|
|
||||||
@abc.abstractmethod
|
|
||||||
def build_experiments(self):
|
|
||||||
"""
|
|
||||||
Returns a set of experiments to be evaluated
|
|
||||||
Must be redefined in an inherited class.
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
@abc.abstractproperty
|
|
||||||
def train_weathers(self):
|
|
||||||
"""
|
|
||||||
Return the weathers that are considered as training conditions
|
|
||||||
"""
|
|
||||||
|
|
||||||
@abc.abstractproperty
|
|
||||||
def test_weathers(self):
|
|
||||||
"""
|
|
||||||
Return the weathers that are considered as testing conditions
|
|
||||||
"""
|
|
|
@ -1,335 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import math
|
|
||||||
import os
|
|
||||||
|
|
||||||
sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1]) ** 2)
|
|
||||||
flatten = lambda l: [item for sublist in l for item in sublist]
|
|
||||||
|
|
||||||
|
|
||||||
class Metrics(object):
|
|
||||||
"""
|
|
||||||
The metrics class is made to take the driving measurements
|
|
||||||
and calculate some specific performance metrics.
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, parameters, dynamic_tasks):
|
|
||||||
"""
|
|
||||||
Args
|
|
||||||
parameters: A dictionary with the used parameters for checking how to count infractions
|
|
||||||
dynamic_tasks: A list of the all dynamic tasks (That contain dynamic objects)
|
|
||||||
"""
|
|
||||||
|
|
||||||
self._parameters = parameters
|
|
||||||
self._parameters['dynamic_tasks'] = dynamic_tasks
|
|
||||||
|
|
||||||
def _divide_by_episodes(self, measurements_matrix, header):
|
|
||||||
|
|
||||||
"""
|
|
||||||
Divides the measurements matrix on different episodes.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
measurements_matrix: The full measurements matrix
|
|
||||||
header: The header from the measurements matrix
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
# Read previous for position zero
|
|
||||||
prev_start = measurements_matrix[0, header.index('start_point')]
|
|
||||||
prev_end = measurements_matrix[0, header.index('end_point')]
|
|
||||||
prev_exp_id = measurements_matrix[0, header.index('exp_id')]
|
|
||||||
|
|
||||||
# Start at the position 1.
|
|
||||||
i = 1
|
|
||||||
prev_i_position = 0
|
|
||||||
episode_matrix_metrics = []
|
|
||||||
|
|
||||||
while i < measurements_matrix.shape[0]:
|
|
||||||
|
|
||||||
current_start = measurements_matrix[i, header.index('start_point')]
|
|
||||||
current_end = measurements_matrix[i, header.index('end_point')]
|
|
||||||
current_exp_id = measurements_matrix[i, header.index('exp_id')]
|
|
||||||
|
|
||||||
# If there is a change in the position it means it is a new episode for sure.
|
|
||||||
if (current_start != prev_start and current_end != prev_end) \
|
|
||||||
or current_exp_id != prev_exp_id:
|
|
||||||
episode_matrix_metrics.append(measurements_matrix[prev_i_position:i, :])
|
|
||||||
prev_i_position = i
|
|
||||||
|
|
||||||
prev_start = current_start
|
|
||||||
prev_end = current_end
|
|
||||||
prev_exp_id = current_exp_id
|
|
||||||
|
|
||||||
i += 1
|
|
||||||
|
|
||||||
episode_matrix_metrics.append(measurements_matrix[prev_i_position:-1, :])
|
|
||||||
|
|
||||||
return episode_matrix_metrics
|
|
||||||
|
|
||||||
def _get_collisions(self, selected_matrix, header):
|
|
||||||
"""
|
|
||||||
Get the number of collisions for pedestrians, vehicles or other
|
|
||||||
Args:
|
|
||||||
selected_matrix: The matrix with all the experiments summary
|
|
||||||
header: The header , to know the positions of details
|
|
||||||
|
|
||||||
|
|
||||||
"""
|
|
||||||
count_collisions_general = 0
|
|
||||||
count_collisions_pedestrian = 0
|
|
||||||
count_collisions_vehicle = 0
|
|
||||||
i = 1
|
|
||||||
# Computing general collisions
|
|
||||||
while i < selected_matrix.shape[0]:
|
|
||||||
if (selected_matrix[i, header.index('collision_other')]
|
|
||||||
- selected_matrix[
|
|
||||||
(i - self._parameters['collision_other']['frames_skip']), header.index(
|
|
||||||
'collision_other')]) > \
|
|
||||||
self._parameters['collision_other']['threshold']:
|
|
||||||
count_collisions_general += 1
|
|
||||||
i += self._parameters['collision_other']['frames_recount']
|
|
||||||
i += 1
|
|
||||||
|
|
||||||
i = 1
|
|
||||||
# Computing collisions for vehicles
|
|
||||||
while i < selected_matrix.shape[0]:
|
|
||||||
if (selected_matrix[i, header.index('collision_vehicles')]
|
|
||||||
- selected_matrix[
|
|
||||||
(i - self._parameters['collision_vehicles']['frames_skip']), header.index(
|
|
||||||
'collision_vehicles')]) > \
|
|
||||||
self._parameters['collision_vehicles']['threshold']:
|
|
||||||
count_collisions_vehicle += 1
|
|
||||||
i += self._parameters['collision_vehicles']['frames_recount']
|
|
||||||
i += 1
|
|
||||||
|
|
||||||
i = 1
|
|
||||||
|
|
||||||
# Computing the collisions for pedestrians
|
|
||||||
while i < selected_matrix.shape[0]:
|
|
||||||
if (selected_matrix[i, header.index('collision_pedestrians')]
|
|
||||||
- selected_matrix[i - self._parameters['collision_pedestrians']['frames_skip'],
|
|
||||||
header.index('collision_pedestrians')]) > \
|
|
||||||
self._parameters['collision_pedestrians']['threshold']:
|
|
||||||
count_collisions_pedestrian += 1
|
|
||||||
i += self._parameters['collision_pedestrians']['frames_recount']
|
|
||||||
i += 1
|
|
||||||
|
|
||||||
return count_collisions_general, count_collisions_vehicle, count_collisions_pedestrian
|
|
||||||
|
|
||||||
def _get_distance_traveled(self, selected_matrix, header):
|
|
||||||
"""
|
|
||||||
Compute the total distance travelled
|
|
||||||
Args:
|
|
||||||
selected_matrix: The matrix with all the experiments summary
|
|
||||||
header: The header , to know the positions of details
|
|
||||||
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
prev_x = selected_matrix[0, header.index('pos_x')]
|
|
||||||
prev_y = selected_matrix[0, header.index('pos_y')]
|
|
||||||
|
|
||||||
i = 1
|
|
||||||
acummulated_distance = 0
|
|
||||||
|
|
||||||
while i < selected_matrix.shape[0]:
|
|
||||||
x = selected_matrix[i, header.index('pos_x')]
|
|
||||||
y = selected_matrix[i, header.index('pos_y')]
|
|
||||||
|
|
||||||
acummulated_distance += sldist((x, y), (prev_x, prev_y))
|
|
||||||
|
|
||||||
prev_x = x
|
|
||||||
prev_y = y
|
|
||||||
|
|
||||||
i += 1
|
|
||||||
|
|
||||||
return acummulated_distance / (1000.0)
|
|
||||||
|
|
||||||
def _get_out_of_road_lane(self, selected_matrix, header):
|
|
||||||
|
|
||||||
"""
|
|
||||||
Check for the situations were the agent goes out of the road.
|
|
||||||
Args:
|
|
||||||
selected_matrix: The matrix with all the experiments summary
|
|
||||||
header: The header , to know the positions of details
|
|
||||||
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
count_sidewalk_intersect = 0
|
|
||||||
count_lane_intersect = 0
|
|
||||||
|
|
||||||
i = 0
|
|
||||||
|
|
||||||
while i < selected_matrix.shape[0]:
|
|
||||||
|
|
||||||
if (selected_matrix[i, header.index('intersection_offroad')]
|
|
||||||
- selected_matrix[(i - self._parameters['intersection_offroad']['frames_skip']),
|
|
||||||
header.index('intersection_offroad')]) \
|
|
||||||
> self._parameters['intersection_offroad']['threshold']:
|
|
||||||
count_sidewalk_intersect += 1
|
|
||||||
i += self._parameters['intersection_offroad']['frames_recount']
|
|
||||||
if i >= selected_matrix.shape[0]:
|
|
||||||
break
|
|
||||||
|
|
||||||
if (selected_matrix[i, header.index('intersection_otherlane')]
|
|
||||||
- selected_matrix[(i - self._parameters['intersection_otherlane']['frames_skip']),
|
|
||||||
header.index('intersection_otherlane')]) \
|
|
||||||
> self._parameters['intersection_otherlane']['threshold']:
|
|
||||||
count_lane_intersect += 1
|
|
||||||
i += self._parameters['intersection_otherlane']['frames_recount']
|
|
||||||
|
|
||||||
i += 1
|
|
||||||
|
|
||||||
return count_lane_intersect, count_sidewalk_intersect
|
|
||||||
|
|
||||||
def compute(self, path):
|
|
||||||
|
|
||||||
"""
|
|
||||||
Compute a dictionary containing the following metrics
|
|
||||||
|
|
||||||
* Off Road Intersection: The number of times the agent goes out of the road.
|
|
||||||
The intersection is only counted if the area of the vehicle outside
|
|
||||||
of the road is bigger than a *threshold*.
|
|
||||||
|
|
||||||
* Other Lane Intersection: The number of times the agent goes to the other
|
|
||||||
lane. The intersection is only counted if the area of the vehicle on the
|
|
||||||
other lane is bigger than a *threshold*.
|
|
||||||
|
|
||||||
* Vehicle Collisions: The number of collisions with vehicles that have
|
|
||||||
an impact bigger than a *threshold*.
|
|
||||||
|
|
||||||
* Pedestrian Collisions: The number of collisions with pedestrians
|
|
||||||
that have an impact bigger than a threshold.
|
|
||||||
|
|
||||||
* General Collisions: The number of collisions with all other
|
|
||||||
objects.
|
|
||||||
|
|
||||||
|
|
||||||
Args:
|
|
||||||
path: Path where the log files are.
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
with open(os.path.join(path, 'summary.csv'), "rU") as f:
|
|
||||||
header = f.readline()
|
|
||||||
header = header.split(',')
|
|
||||||
header[-1] = header[-1][:-1]
|
|
||||||
|
|
||||||
with open(os.path.join(path, 'measurements.csv'), "rU") as f:
|
|
||||||
|
|
||||||
header_metrics = f.readline()
|
|
||||||
header_metrics = header_metrics.split(',')
|
|
||||||
header_metrics[-1] = header_metrics[-1][:-1]
|
|
||||||
|
|
||||||
result_matrix = np.loadtxt(os.path.join(path, 'summary.csv'), delimiter=",", skiprows=1)
|
|
||||||
|
|
||||||
# Corner Case: The presented test just had one episode
|
|
||||||
if result_matrix.ndim == 1:
|
|
||||||
result_matrix = np.expand_dims(result_matrix, axis=0)
|
|
||||||
|
|
||||||
tasks = np.unique(result_matrix[:, header.index('exp_id')])
|
|
||||||
|
|
||||||
all_weathers = np.unique(result_matrix[:, header.index('weather')])
|
|
||||||
|
|
||||||
measurements_matrix = np.loadtxt(os.path.join(path, 'measurements.csv'), delimiter=",",
|
|
||||||
skiprows=1)
|
|
||||||
|
|
||||||
metrics_dictionary = {'episodes_completion': {w: [0] * len(tasks) for w in all_weathers},
|
|
||||||
'intersection_offroad': {w: [[] for i in range(len(tasks))] for w in
|
|
||||||
all_weathers},
|
|
||||||
'intersection_otherlane': {w: [[] for i in range(len(tasks))] for w in
|
|
||||||
all_weathers},
|
|
||||||
'collision_pedestrians': {w: [[] for i in range(len(tasks))] for w in
|
|
||||||
all_weathers},
|
|
||||||
'collision_vehicles': {w: [[] for i in range(len(tasks))] for w in
|
|
||||||
all_weathers},
|
|
||||||
'collision_other': {w: [[] for i in range(len(tasks))] for w in
|
|
||||||
all_weathers},
|
|
||||||
'episodes_fully_completed': {w: [0] * len(tasks) for w in
|
|
||||||
all_weathers},
|
|
||||||
'average_speed': {w: [0] * len(tasks) for w in all_weathers},
|
|
||||||
'driven_kilometers': {w: [0] * len(tasks) for w in all_weathers}
|
|
||||||
}
|
|
||||||
|
|
||||||
for t in range(len(tasks)):
|
|
||||||
experiment_results_matrix = result_matrix[
|
|
||||||
result_matrix[:, header.index('exp_id')] == tasks[t]]
|
|
||||||
|
|
||||||
weathers = np.unique(experiment_results_matrix[:, header.index('weather')])
|
|
||||||
|
|
||||||
for w in weathers:
|
|
||||||
|
|
||||||
experiment_results_matrix = result_matrix[
|
|
||||||
np.logical_and(result_matrix[:, header.index(
|
|
||||||
'exp_id')] == tasks[t], result_matrix[:, header.index('weather')] == w)]
|
|
||||||
|
|
||||||
experiment_metrics_matrix = measurements_matrix[
|
|
||||||
np.logical_and(measurements_matrix[:, header_metrics.index(
|
|
||||||
'exp_id')] == float(tasks[t]),
|
|
||||||
measurements_matrix[:, header_metrics.index('weather')] == float(
|
|
||||||
w))]
|
|
||||||
|
|
||||||
metrics_dictionary['episodes_fully_completed'][w][t] = \
|
|
||||||
experiment_results_matrix[:, header.index('result')].tolist()
|
|
||||||
|
|
||||||
metrics_dictionary['episodes_completion'][w][t] = \
|
|
||||||
((experiment_results_matrix[:, header.index('initial_distance')]
|
|
||||||
- experiment_results_matrix[:, header.index('final_distance')])
|
|
||||||
/ experiment_results_matrix[:, header.index('initial_distance')]).tolist()
|
|
||||||
|
|
||||||
# Now we divide the experiment metrics matrix
|
|
||||||
|
|
||||||
episode_experiment_metrics_matrix = self._divide_by_episodes(
|
|
||||||
experiment_metrics_matrix, header_metrics)
|
|
||||||
|
|
||||||
count = 0
|
|
||||||
|
|
||||||
for episode_experiment_metrics in episode_experiment_metrics_matrix:
|
|
||||||
|
|
||||||
km_run_episodes = self._get_distance_traveled(
|
|
||||||
episode_experiment_metrics, header_metrics)
|
|
||||||
metrics_dictionary['driven_kilometers'][w][t] += km_run_episodes
|
|
||||||
metrics_dictionary['average_speed'][w][t] = \
|
|
||||||
km_run_episodes / (experiment_results_matrix[count,
|
|
||||||
header.index(
|
|
||||||
'final_time')] / 3600.0)
|
|
||||||
count += 1
|
|
||||||
|
|
||||||
lane_road = self._get_out_of_road_lane(
|
|
||||||
episode_experiment_metrics, header_metrics)
|
|
||||||
|
|
||||||
metrics_dictionary['intersection_otherlane'][
|
|
||||||
w][t].append(lane_road[0])
|
|
||||||
metrics_dictionary['intersection_offroad'][
|
|
||||||
w][t].append(lane_road[1])
|
|
||||||
|
|
||||||
if tasks[t] in set(self._parameters['dynamic_tasks']):
|
|
||||||
|
|
||||||
collisions = self._get_collisions(episode_experiment_metrics,
|
|
||||||
header_metrics)
|
|
||||||
|
|
||||||
metrics_dictionary['collision_pedestrians'][
|
|
||||||
w][t].append(collisions[2])
|
|
||||||
metrics_dictionary['collision_vehicles'][
|
|
||||||
w][t].append(collisions[1])
|
|
||||||
metrics_dictionary['collision_other'][
|
|
||||||
w][t].append(collisions[0])
|
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
metrics_dictionary['collision_pedestrians'][
|
|
||||||
w][t].append(0)
|
|
||||||
metrics_dictionary['collision_vehicles'][
|
|
||||||
w][t].append(0)
|
|
||||||
metrics_dictionary['collision_other'][
|
|
||||||
w][t].append(0)
|
|
||||||
|
|
||||||
return metrics_dictionary
|
|
|
@ -1,276 +0,0 @@
|
||||||
import csv
|
|
||||||
import datetime
|
|
||||||
import os
|
|
||||||
|
|
||||||
|
|
||||||
class Recording(object):
|
|
||||||
"""
|
|
||||||
Class to record all the logs for the benchmark. It records individual episodes measurements
|
|
||||||
and also summary for each episodes.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, name_to_save, continue_experiment, save_images):
|
|
||||||
"""
|
|
||||||
Recorder constructors
|
|
||||||
Args:
|
|
||||||
name_to_save: Name of the log
|
|
||||||
continue_experiment: If you want to continue a previous experiment with the same names
|
|
||||||
save_images: If you want to save images to the disk.
|
|
||||||
"""
|
|
||||||
|
|
||||||
self._dict_summary = {'exp_id': -1,
|
|
||||||
'rep': -1,
|
|
||||||
'weather': -1,
|
|
||||||
'start_point': -1,
|
|
||||||
'end_point': -1,
|
|
||||||
'result': -1,
|
|
||||||
'initial_distance': -1,
|
|
||||||
'final_distance': -1,
|
|
||||||
'final_time': -1,
|
|
||||||
'time_out': -1
|
|
||||||
}
|
|
||||||
self._dict_measurements = {'exp_id': -1,
|
|
||||||
'rep': -1,
|
|
||||||
'weather': -1,
|
|
||||||
'start_point': -1,
|
|
||||||
'end_point': -1,
|
|
||||||
'collision_other': -1,
|
|
||||||
'collision_pedestrians': -1,
|
|
||||||
'collision_vehicles': -1,
|
|
||||||
'intersection_otherlane': -1,
|
|
||||||
'intersection_offroad': -1,
|
|
||||||
'pos_x': -1,
|
|
||||||
'pos_y': -1,
|
|
||||||
'steer': -1,
|
|
||||||
'throttle': -1,
|
|
||||||
'brake': -1
|
|
||||||
}
|
|
||||||
|
|
||||||
# Just in the case is the first time and there is no benchmark results folder
|
|
||||||
if not os.path.exists('_benchmarks_results'):
|
|
||||||
os.mkdir('_benchmarks_results')
|
|
||||||
|
|
||||||
# Generate the full path for the log files
|
|
||||||
self._path = os.path.join('_benchmarks_results', name_to_save)
|
|
||||||
|
|
||||||
# Check for continuation of experiment, also returns the last line, used for test purposes
|
|
||||||
# If you don't want to continue it will create a new path name with a number.
|
|
||||||
# Also returns the fieldnames for both measurements and summary, so you can keep the
|
|
||||||
# previous order
|
|
||||||
self._path, _, self._summary_fieldnames, self._measurements_fieldnames \
|
|
||||||
= self._continue_experiment(continue_experiment)
|
|
||||||
|
|
||||||
self._create_log_files()
|
|
||||||
|
|
||||||
# A log with a date file: to show when was the last access and log what was tested,
|
|
||||||
now = datetime.datetime.now()
|
|
||||||
self._internal_log_name = os.path.join(self._path, 'log_' + now.strftime("%Y%m%d%H%M"))
|
|
||||||
open(self._internal_log_name, 'w').close()
|
|
||||||
|
|
||||||
# store the save images flag, and already store the format for image saving
|
|
||||||
self._save_images = save_images
|
|
||||||
self._image_filename_format = os.path.join(
|
|
||||||
self._path, '_images/episode_{:s}/{:s}/image_{:0>5d}.jpg')
|
|
||||||
|
|
||||||
@property
|
|
||||||
def path(self):
|
|
||||||
return self._path
|
|
||||||
|
|
||||||
def log_poses(self, start_index, end_index, weather_id):
|
|
||||||
"""
|
|
||||||
Log tested poses by the benchmark
|
|
||||||
"""
|
|
||||||
with open(self._internal_log_name, 'a+') as log:
|
|
||||||
log.write(' Start Poses (%d %d ) on weather %d \n ' %
|
|
||||||
(start_index, end_index, weather_id))
|
|
||||||
|
|
||||||
def log_poses_finish(self):
|
|
||||||
"""
|
|
||||||
Log when a set of poses (task) is finished by the benchmark
|
|
||||||
"""
|
|
||||||
with open(self._internal_log_name, 'a+') as log:
|
|
||||||
log.write('Finished Task')
|
|
||||||
|
|
||||||
def log_start(self, id_experiment):
|
|
||||||
"""
|
|
||||||
Log when a set of poses (task) is started by the benchmark
|
|
||||||
"""
|
|
||||||
with open(self._internal_log_name, 'a+') as log:
|
|
||||||
log.write('Start Task %d \n' % id_experiment)
|
|
||||||
|
|
||||||
def log_end(self):
|
|
||||||
"""
|
|
||||||
Log when the benchmark is finished
|
|
||||||
"""
|
|
||||||
with open(self._internal_log_name, 'a+') as log:
|
|
||||||
log.write('====== Finished Entire Benchmark ======')
|
|
||||||
|
|
||||||
def write_summary_results(self, experiment, pose, rep,
|
|
||||||
path_distance, remaining_distance,
|
|
||||||
final_time, time_out, result):
|
|
||||||
"""
|
|
||||||
Method to record the summary of an episode(pose) execution
|
|
||||||
"""
|
|
||||||
|
|
||||||
self._dict_summary['exp_id'] = experiment.task
|
|
||||||
self._dict_summary['rep'] = rep
|
|
||||||
self._dict_summary['weather'] = experiment.Conditions.WeatherId
|
|
||||||
self._dict_summary['start_point'] = pose[0]
|
|
||||||
self._dict_summary['end_point'] = pose[1]
|
|
||||||
self._dict_summary['result'] = result
|
|
||||||
self._dict_summary['initial_distance'] = path_distance
|
|
||||||
self._dict_summary['final_distance'] = remaining_distance
|
|
||||||
self._dict_summary['final_time'] = final_time
|
|
||||||
self._dict_summary['time_out'] = time_out
|
|
||||||
|
|
||||||
with open(os.path.join(self._path, 'summary.csv'), 'a+') as ofd:
|
|
||||||
w = csv.DictWriter(ofd, self._dict_summary.keys())
|
|
||||||
w.fieldnames = self._summary_fieldnames
|
|
||||||
w.writerow(self._dict_summary)
|
|
||||||
|
|
||||||
def write_measurements_results(self, experiment, rep, pose, reward_vec, control_vec):
|
|
||||||
"""
|
|
||||||
Method to record the measurements, sensors,
|
|
||||||
controls and status of the entire benchmark.
|
|
||||||
"""
|
|
||||||
with open(os.path.join(self._path, 'measurements.csv'), 'a+') as rfd:
|
|
||||||
mw = csv.DictWriter(rfd, self._dict_measurements.keys())
|
|
||||||
mw.fieldnames = self._measurements_fieldnames
|
|
||||||
for i in range(len(reward_vec)):
|
|
||||||
self._dict_measurements['exp_id'] = experiment.task
|
|
||||||
self._dict_measurements['rep'] = rep
|
|
||||||
self._dict_measurements['start_point'] = pose[0]
|
|
||||||
self._dict_measurements['end_point'] = pose[1]
|
|
||||||
self._dict_measurements['weather'] = experiment.Conditions.WeatherId
|
|
||||||
self._dict_measurements['collision_other'] = reward_vec[
|
|
||||||
i].collision_other
|
|
||||||
self._dict_measurements['collision_pedestrians'] = reward_vec[
|
|
||||||
i].collision_pedestrians
|
|
||||||
self._dict_measurements['collision_vehicles'] = reward_vec[
|
|
||||||
i].collision_vehicles
|
|
||||||
self._dict_measurements['intersection_otherlane'] = reward_vec[
|
|
||||||
i].intersection_otherlane
|
|
||||||
self._dict_measurements['intersection_offroad'] = reward_vec[
|
|
||||||
i].intersection_offroad
|
|
||||||
self._dict_measurements['pos_x'] = reward_vec[
|
|
||||||
i].transform.location.x
|
|
||||||
self._dict_measurements['pos_y'] = reward_vec[
|
|
||||||
i].transform.location.y
|
|
||||||
self._dict_measurements['steer'] = control_vec[
|
|
||||||
i].steer
|
|
||||||
self._dict_measurements['throttle'] = control_vec[
|
|
||||||
i].throttle
|
|
||||||
self._dict_measurements['brake'] = control_vec[
|
|
||||||
i].brake
|
|
||||||
|
|
||||||
mw.writerow(self._dict_measurements)
|
|
||||||
|
|
||||||
def _create_log_files(self):
|
|
||||||
"""
|
|
||||||
Just create the log files and add the necessary header for it.
|
|
||||||
"""
|
|
||||||
|
|
||||||
if not self._experiment_exist():
|
|
||||||
os.mkdir(self._path)
|
|
||||||
|
|
||||||
with open(os.path.join(self._path, 'summary.csv'), 'w') as ofd:
|
|
||||||
sw = csv.DictWriter(ofd, self._dict_summary.keys())
|
|
||||||
sw.writeheader()
|
|
||||||
if self._summary_fieldnames is None:
|
|
||||||
self._summary_fieldnames = sw.fieldnames
|
|
||||||
|
|
||||||
with open(os.path.join(self._path, 'measurements.csv'), 'w') as rfd:
|
|
||||||
mw = csv.DictWriter(rfd, self._dict_measurements.keys())
|
|
||||||
mw.writeheader()
|
|
||||||
if self._measurements_fieldnames is None:
|
|
||||||
self._measurements_fieldnames = mw.fieldnames
|
|
||||||
|
|
||||||
def _continue_experiment(self, continue_experiment):
|
|
||||||
"""
|
|
||||||
Get the line on the file for the experiment.
|
|
||||||
If continue_experiment is false and experiment exist, generates a new file path
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
def get_non_existent_path(f_name_path):
|
|
||||||
"""
|
|
||||||
Get the path to a filename which does not exist by incrementing path.
|
|
||||||
"""
|
|
||||||
if not os.path.exists(f_name_path):
|
|
||||||
return f_name_path
|
|
||||||
filename, file_extension = os.path.splitext(f_name_path)
|
|
||||||
i = 1
|
|
||||||
new_f_name = "{}-{}{}".format(filename, i, file_extension)
|
|
||||||
while os.path.exists(new_f_name):
|
|
||||||
i += 1
|
|
||||||
new_f_name = "{}-{}{}".format(filename, i, file_extension)
|
|
||||||
return new_f_name
|
|
||||||
|
|
||||||
# start the new path as the same one as before
|
|
||||||
new_path = self._path
|
|
||||||
summary_fieldnames = None
|
|
||||||
measurements_fieldnames = None
|
|
||||||
|
|
||||||
# if the experiment exist
|
|
||||||
if self._experiment_exist():
|
|
||||||
|
|
||||||
# If you want to continue just get the last position
|
|
||||||
if continue_experiment:
|
|
||||||
line_on_file = self._get_last_position()
|
|
||||||
# Get the previously used fileorder
|
|
||||||
with open(os.path.join(self._path, 'summary.csv'), 'r') as ofd:
|
|
||||||
summary_reader = csv.DictReader(ofd)
|
|
||||||
summary_fieldnames = summary_reader.fieldnames
|
|
||||||
with open(os.path.join(self._path, 'measurements.csv'), 'r') as ofd:
|
|
||||||
measurements_reader = csv.DictReader(ofd)
|
|
||||||
measurements_fieldnames = measurements_reader.fieldnames
|
|
||||||
|
|
||||||
else:
|
|
||||||
# Get a new non_conflicting path name
|
|
||||||
new_path = get_non_existent_path(new_path)
|
|
||||||
line_on_file = 1
|
|
||||||
|
|
||||||
else:
|
|
||||||
line_on_file = 1
|
|
||||||
return new_path, line_on_file, summary_fieldnames, measurements_fieldnames
|
|
||||||
|
|
||||||
def save_images(self, sensor_data, episode_name, frame):
|
|
||||||
"""
|
|
||||||
Save a image during the experiment
|
|
||||||
"""
|
|
||||||
if self._save_images:
|
|
||||||
for name, image in sensor_data.items():
|
|
||||||
image.save_to_disk(self._image_filename_format.format(
|
|
||||||
episode_name, name, frame))
|
|
||||||
|
|
||||||
def get_pose_and_experiment(self, number_poses_task):
|
|
||||||
"""
|
|
||||||
Based on the line in log file, return the current pose and experiment.
|
|
||||||
If the line is zero, create new log files.
|
|
||||||
|
|
||||||
"""
|
|
||||||
# Warning: assumes that all tasks have the same size
|
|
||||||
line_on_file = self._get_last_position() - 1
|
|
||||||
if line_on_file == 0:
|
|
||||||
return 0, 0
|
|
||||||
|
|
||||||
return line_on_file % number_poses_task, line_on_file // number_poses_task
|
|
||||||
|
|
||||||
def _experiment_exist(self):
|
|
||||||
""" Check if the experiment exists"""
|
|
||||||
return os.path.exists(self._path)
|
|
||||||
|
|
||||||
def _get_last_position(self):
|
|
||||||
"""
|
|
||||||
Get the last position on the summary experiment file
|
|
||||||
With this you are able to continue from there
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
int, position:
|
|
||||||
"""
|
|
||||||
# Try to open, if the file is not found
|
|
||||||
try:
|
|
||||||
with open(os.path.join(self._path, 'summary.csv')) as f:
|
|
||||||
return sum(1 for _ in f)
|
|
||||||
except IOError:
|
|
||||||
return 0
|
|
|
@ -1,124 +0,0 @@
|
||||||
import os
|
|
||||||
import numpy as np
|
|
||||||
import json
|
|
||||||
|
|
||||||
|
|
||||||
def print_summary(metrics_summary, weathers, path):
|
|
||||||
"""
|
|
||||||
We plot the summary of the testing for the set selected weathers.
|
|
||||||
|
|
||||||
We take the raw data and print the way it was described on CORL 2017 paper
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
# Improve readability by adding a weather dictionary
|
|
||||||
weather_name_dict = {1: 'Clear Noon', 3: 'After Rain Noon',
|
|
||||||
6: 'Heavy Rain Noon', 8: 'Clear Sunset',
|
|
||||||
4: 'Cloudy After Rain', 14: 'Soft Rain Sunset'}
|
|
||||||
|
|
||||||
# First we write the entire dictionary on the benchmark folder.
|
|
||||||
with open(os.path.join(path, 'metrics.json'), 'w') as fo:
|
|
||||||
fo.write(json.dumps(metrics_summary))
|
|
||||||
|
|
||||||
# Second we plot the metrics that are already ready by averaging
|
|
||||||
|
|
||||||
metrics_to_average = [
|
|
||||||
'episodes_fully_completed',
|
|
||||||
'episodes_completion'
|
|
||||||
|
|
||||||
]
|
|
||||||
# We compute the number of episodes based on size of average completion
|
|
||||||
number_of_episodes = len(list(metrics_summary['episodes_fully_completed'].items())[0][1])
|
|
||||||
|
|
||||||
for metric in metrics_to_average:
|
|
||||||
|
|
||||||
if metric == 'episodes_completion':
|
|
||||||
print ("Average Percentage of Distance to Goal Travelled ")
|
|
||||||
else:
|
|
||||||
print ("Percentage of Successful Episodes")
|
|
||||||
|
|
||||||
print ("")
|
|
||||||
values = metrics_summary[metric]
|
|
||||||
|
|
||||||
metric_sum_values = np.zeros(number_of_episodes)
|
|
||||||
for weather, tasks in values.items():
|
|
||||||
if weather in set(weathers):
|
|
||||||
print(' Weather: ', weather_name_dict[weather])
|
|
||||||
count = 0
|
|
||||||
for t in tasks:
|
|
||||||
# if isinstance(t, np.ndarray) or isinstance(t, list):
|
|
||||||
if t == []:
|
|
||||||
print(' Metric Not Computed')
|
|
||||||
else:
|
|
||||||
print(' Task:', count, ' -> ', float(sum(t)) / float(len(t)))
|
|
||||||
metric_sum_values[count] += (float(sum(t)) / float(len(t))) * 1.0 / float(
|
|
||||||
len(weathers))
|
|
||||||
|
|
||||||
count += 1
|
|
||||||
|
|
||||||
print (' Average Between Weathers')
|
|
||||||
for i in range(len(metric_sum_values)):
|
|
||||||
print(' Task ', i, ' -> ', metric_sum_values[i])
|
|
||||||
print ("")
|
|
||||||
|
|
||||||
infraction_metrics = [
|
|
||||||
'collision_pedestrians',
|
|
||||||
'collision_vehicles',
|
|
||||||
'collision_other',
|
|
||||||
'intersection_offroad',
|
|
||||||
'intersection_otherlane'
|
|
||||||
|
|
||||||
]
|
|
||||||
|
|
||||||
# We need to collect the total number of kilometers for each task
|
|
||||||
|
|
||||||
for metric in infraction_metrics:
|
|
||||||
values_driven = metrics_summary['driven_kilometers']
|
|
||||||
values = metrics_summary[metric]
|
|
||||||
metric_sum_values = np.zeros(number_of_episodes)
|
|
||||||
summed_driven_kilometers = np.zeros(number_of_episodes)
|
|
||||||
|
|
||||||
if metric == 'collision_pedestrians':
|
|
||||||
print ('Avg. Kilometers driven before a collision to a PEDESTRIAN')
|
|
||||||
elif metric == 'collision_vehicles':
|
|
||||||
print('Avg. Kilometers driven before a collision to a VEHICLE')
|
|
||||||
elif metric == 'collision_other':
|
|
||||||
print('Avg. Kilometers driven before a collision to a STATIC OBSTACLE')
|
|
||||||
elif metric == 'intersection_offroad':
|
|
||||||
print('Avg. Kilometers driven before going OUTSIDE OF THE ROAD')
|
|
||||||
else:
|
|
||||||
print('Avg. Kilometers driven before invading the OPPOSITE LANE')
|
|
||||||
|
|
||||||
# print (zip(values.items(), values_driven.items()))
|
|
||||||
for items_metric, items_driven in zip(values.items(), values_driven.items()):
|
|
||||||
weather = items_metric[0]
|
|
||||||
tasks = items_metric[1]
|
|
||||||
tasks_driven = items_driven[1]
|
|
||||||
|
|
||||||
if weather in set(weathers):
|
|
||||||
print(' Weather: ', weather_name_dict[weather])
|
|
||||||
count = 0
|
|
||||||
for t, t_driven in zip(tasks, tasks_driven):
|
|
||||||
# if isinstance(t, np.ndarray) or isinstance(t, list):
|
|
||||||
if t == []:
|
|
||||||
print('Metric Not Computed')
|
|
||||||
else:
|
|
||||||
if sum(t) > 0:
|
|
||||||
print(' Task ', count, ' -> ', t_driven / float(sum(t)))
|
|
||||||
else:
|
|
||||||
print(' Task ', count, ' -> more than', t_driven)
|
|
||||||
|
|
||||||
metric_sum_values[count] += float(sum(t))
|
|
||||||
summed_driven_kilometers[count] += t_driven
|
|
||||||
|
|
||||||
count += 1
|
|
||||||
print (' Average Between Weathers')
|
|
||||||
for i in range(len(metric_sum_values)):
|
|
||||||
if metric_sum_values[i] == 0:
|
|
||||||
print(' Task ', i, ' -> more than ', summed_driven_kilometers[i])
|
|
||||||
else:
|
|
||||||
print(' Task ', i, ' -> ', summed_driven_kilometers[i] / metric_sum_values[i])
|
|
||||||
print ("")
|
|
||||||
|
|
||||||
print("")
|
|
||||||
print("")
|
|
|
@ -1,162 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""
|
|
||||||
Handy conversions for CARLA images.
|
|
||||||
|
|
||||||
The functions here are provided for real-time display, if you want to save the
|
|
||||||
converted images, save the images from Python without conversion and convert
|
|
||||||
them afterwards with the C++ implementation at "Util/ImageConverter" as it
|
|
||||||
provides considerably better performance.
|
|
||||||
"""
|
|
||||||
|
|
||||||
import math
|
|
||||||
|
|
||||||
try:
|
|
||||||
import numpy
|
|
||||||
from numpy.matlib import repmat
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import numpy, make sure numpy package is installed')
|
|
||||||
|
|
||||||
|
|
||||||
from . import sensor
|
|
||||||
|
|
||||||
|
|
||||||
def to_bgra_array(image):
|
|
||||||
"""Convert a CARLA raw image to a BGRA numpy array."""
|
|
||||||
if not isinstance(image, sensor.Image):
|
|
||||||
raise ValueError("Argument must be a carla.sensor.Image")
|
|
||||||
array = numpy.frombuffer(image.raw_data, dtype=numpy.dtype("uint8"))
|
|
||||||
array = numpy.reshape(array, (image.height, image.width, 4))
|
|
||||||
return array
|
|
||||||
|
|
||||||
|
|
||||||
def to_rgb_array(image):
|
|
||||||
"""Convert a CARLA raw image to a RGB numpy array."""
|
|
||||||
array = to_bgra_array(image)
|
|
||||||
# Convert BGRA to RGB.
|
|
||||||
array = array[:, :, :3]
|
|
||||||
array = array[:, :, ::-1]
|
|
||||||
return array
|
|
||||||
|
|
||||||
|
|
||||||
def labels_to_array(image):
|
|
||||||
"""
|
|
||||||
Convert an image containing CARLA semantic segmentation labels to a 2D array
|
|
||||||
containing the label of each pixel.
|
|
||||||
"""
|
|
||||||
return to_bgra_array(image)[:, :, 2]
|
|
||||||
|
|
||||||
|
|
||||||
def labels_to_cityscapes_palette(image):
|
|
||||||
"""
|
|
||||||
Convert an image containing CARLA semantic segmentation labels to
|
|
||||||
Cityscapes palette.
|
|
||||||
"""
|
|
||||||
classes = {
|
|
||||||
0: [0, 0, 0], # None
|
|
||||||
1: [70, 70, 70], # Buildings
|
|
||||||
2: [190, 153, 153], # Fences
|
|
||||||
3: [72, 0, 90], # Other
|
|
||||||
4: [220, 20, 60], # Pedestrians
|
|
||||||
5: [153, 153, 153], # Poles
|
|
||||||
6: [157, 234, 50], # RoadLines
|
|
||||||
7: [128, 64, 128], # Roads
|
|
||||||
8: [244, 35, 232], # Sidewalks
|
|
||||||
9: [107, 142, 35], # Vegetation
|
|
||||||
10: [0, 0, 255], # Vehicles
|
|
||||||
11: [102, 102, 156], # Walls
|
|
||||||
12: [220, 220, 0] # TrafficSigns
|
|
||||||
}
|
|
||||||
array = labels_to_array(image)
|
|
||||||
result = numpy.zeros((array.shape[0], array.shape[1], 3))
|
|
||||||
for key, value in classes.items():
|
|
||||||
result[numpy.where(array == key)] = value
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def depth_to_array(image):
|
|
||||||
"""
|
|
||||||
Convert an image containing CARLA encoded depth-map to a 2D array containing
|
|
||||||
the depth value of each pixel normalized between [0.0, 1.0].
|
|
||||||
"""
|
|
||||||
array = to_bgra_array(image)
|
|
||||||
array = array.astype(numpy.float32)
|
|
||||||
# Apply (R + G * 256 + B * 256 * 256) / (256 * 256 * 256 - 1).
|
|
||||||
normalized_depth = numpy.dot(array[:, :, :3], [65536.0, 256.0, 1.0])
|
|
||||||
normalized_depth /= 16777215.0 # (256.0 * 256.0 * 256.0 - 1.0)
|
|
||||||
return normalized_depth
|
|
||||||
|
|
||||||
|
|
||||||
def depth_to_logarithmic_grayscale(image):
|
|
||||||
"""
|
|
||||||
Convert an image containing CARLA encoded depth-map to a logarithmic
|
|
||||||
grayscale image array.
|
|
||||||
"max_depth" is used to omit the points that are far enough.
|
|
||||||
"""
|
|
||||||
normalized_depth = depth_to_array(image)
|
|
||||||
# Convert to logarithmic depth.
|
|
||||||
logdepth = numpy.ones(normalized_depth.shape) + \
|
|
||||||
(numpy.log(normalized_depth) / 5.70378)
|
|
||||||
logdepth = numpy.clip(logdepth, 0.0, 1.0)
|
|
||||||
logdepth *= 255.0
|
|
||||||
# Expand to three colors.
|
|
||||||
return numpy.repeat(logdepth[:, :, numpy.newaxis], 3, axis=2)
|
|
||||||
|
|
||||||
|
|
||||||
def depth_to_local_point_cloud(image, color=None, max_depth=0.9):
|
|
||||||
"""
|
|
||||||
Convert an image containing CARLA encoded depth-map to a 2D array containing
|
|
||||||
the 3D position (relative to the camera) of each pixel and its corresponding
|
|
||||||
RGB color of an array.
|
|
||||||
"max_depth" is used to omit the points that are far enough.
|
|
||||||
"""
|
|
||||||
far = 1000.0 # max depth in meters.
|
|
||||||
normalized_depth = depth_to_array(image)
|
|
||||||
|
|
||||||
# (Intrinsic) K Matrix
|
|
||||||
k = numpy.identity(3)
|
|
||||||
k[0, 2] = image.width / 2.0
|
|
||||||
k[1, 2] = image.height / 2.0
|
|
||||||
k[0, 0] = k[1, 1] = image.width / \
|
|
||||||
(2.0 * math.tan(image.fov * math.pi / 360.0))
|
|
||||||
|
|
||||||
# 2d pixel coordinates
|
|
||||||
pixel_length = image.width * image.height
|
|
||||||
u_coord = repmat(numpy.r_[image.width-1:-1:-1],
|
|
||||||
image.height, 1).reshape(pixel_length)
|
|
||||||
v_coord = repmat(numpy.c_[image.height-1:-1:-1],
|
|
||||||
1, image.width).reshape(pixel_length)
|
|
||||||
if color is not None:
|
|
||||||
color = color.reshape(pixel_length, 3)
|
|
||||||
normalized_depth = numpy.reshape(normalized_depth, pixel_length)
|
|
||||||
|
|
||||||
# Search for pixels where the depth is greater than max_depth to
|
|
||||||
# delete them
|
|
||||||
max_depth_indexes = numpy.where(normalized_depth > max_depth)
|
|
||||||
normalized_depth = numpy.delete(normalized_depth, max_depth_indexes)
|
|
||||||
u_coord = numpy.delete(u_coord, max_depth_indexes)
|
|
||||||
v_coord = numpy.delete(v_coord, max_depth_indexes)
|
|
||||||
if color is not None:
|
|
||||||
color = numpy.delete(color, max_depth_indexes, axis=0)
|
|
||||||
|
|
||||||
# pd2 = [u,v,1]
|
|
||||||
p2d = numpy.array([u_coord, v_coord, numpy.ones_like(u_coord)])
|
|
||||||
|
|
||||||
# P = [X,Y,Z]
|
|
||||||
p3d = numpy.dot(numpy.linalg.inv(k), p2d)
|
|
||||||
p3d *= normalized_depth * far
|
|
||||||
|
|
||||||
# Formating the output to:
|
|
||||||
# [[X1,Y1,Z1,R1,G1,B1],[X2,Y2,Z2,R2,G2,B2], ... [Xn,Yn,Zn,Rn,Gn,Bn]]
|
|
||||||
if color is not None:
|
|
||||||
# numpy.concatenate((numpy.transpose(p3d), color), axis=1)
|
|
||||||
return sensor.PointCloud(
|
|
||||||
image.frame_number,
|
|
||||||
numpy.transpose(p3d),
|
|
||||||
color_array=color)
|
|
||||||
# [[X1,Y1,Z1],[X2,Y2,Z2], ... [Xn,Yn,Zn]]
|
|
||||||
return sensor.PointCloud(image.frame_number, numpy.transpose(p3d))
|
|
Before Width: | Height: | Size: 534 KiB |
|
@ -1,49 +0,0 @@
|
||||||
0.0,0.0,-0.3811000000
|
|
||||||
0.000000,0.000000,0.0
|
|
||||||
1.000000,1.000000,1.000000
|
|
||||||
-16.43022,-16.43022,0.000
|
|
||||||
49, 41
|
|
||||||
0,0 0,40 40
|
|
||||||
0,40 0,0 40
|
|
||||||
48,40 41,40 7
|
|
||||||
41,40 48,40 7
|
|
||||||
48,0 48,40 40
|
|
||||||
48,40 48,0 40
|
|
||||||
0,0 11,0 11
|
|
||||||
11,0 0,0 11
|
|
||||||
41,0 48,0 7
|
|
||||||
48,0 41,0 7
|
|
||||||
41,40 11,40 30
|
|
||||||
11,40 41,40 30
|
|
||||||
41,0 41,7 7
|
|
||||||
41,7 41,0 7
|
|
||||||
11,40 0,40 11
|
|
||||||
0,40 11,40 11
|
|
||||||
11,0 19,0 8
|
|
||||||
19,0 11,0 8
|
|
||||||
11,40 11,24 16
|
|
||||||
11,24 11,40 16
|
|
||||||
41,24 41,40 16
|
|
||||||
41,40 41,24 16
|
|
||||||
11,24 11,16 8
|
|
||||||
11,16 11,24 8
|
|
||||||
41,24 11,24 30
|
|
||||||
11,24 41,24 30
|
|
||||||
41,16 41,24 8
|
|
||||||
41,24 41,16 8
|
|
||||||
11,16 11,7 9
|
|
||||||
11,7 11,16 9
|
|
||||||
41,16 11,16 30
|
|
||||||
11,16 41,16 30
|
|
||||||
41,7 41,16 9
|
|
||||||
41,16 41,7 9
|
|
||||||
11,7 11,0 7
|
|
||||||
11,0 11,7 7
|
|
||||||
41,7 19,7 22
|
|
||||||
19,7 41,7 22
|
|
||||||
19,0 41,0 22
|
|
||||||
41,0 19,0 22
|
|
||||||
19,7 11,7 8
|
|
||||||
11,7 19,7 8
|
|
||||||
19,0 19,7 7
|
|
||||||
19,7 19,0 7
|
|
Before Width: | Height: | Size: 27 KiB |
Before Width: | Height: | Size: 33 KiB |
Before Width: | Height: | Size: 175 KiB |
|
@ -1,37 +0,0 @@
|
||||||
5.4400,-107.48000,-0.22000000
|
|
||||||
0.000000,0.000000,0.000000
|
|
||||||
1.000000,1.000000,1.000000
|
|
||||||
-16.43022,-16.43022,0.000
|
|
||||||
25, 25
|
|
||||||
0,10 0,24 14
|
|
||||||
0,24 0,10 14
|
|
||||||
24,24 6,24 18
|
|
||||||
6,24 24,24 18
|
|
||||||
24,0 24,10 10
|
|
||||||
24,10 24,0 10
|
|
||||||
0,0 24,0 24
|
|
||||||
24,0 0,0 24
|
|
||||||
0,10 0,0 10
|
|
||||||
0,0 0,10 10
|
|
||||||
24,10 24,16 6
|
|
||||||
24,16 24,10 6
|
|
||||||
0,10 6,10 6
|
|
||||||
6,10 0,10 6
|
|
||||||
6,24 0,24 6
|
|
||||||
0,24 6,24 6
|
|
||||||
6,10 17,10 11
|
|
||||||
17,10 6,10 11
|
|
||||||
6,24 6,16 8
|
|
||||||
6,16 6,24 8
|
|
||||||
24,16 24,24 8
|
|
||||||
24,24 24,16 8
|
|
||||||
6,16 6,10 6
|
|
||||||
6,10 6,16 6
|
|
||||||
24,16 17,16 7
|
|
||||||
17,16 24,16 7
|
|
||||||
17,16 6,16 11
|
|
||||||
6,16 17,16 11
|
|
||||||
17,10 24,10 7
|
|
||||||
24,10 17,10 7
|
|
||||||
17,16 17,10 6
|
|
||||||
17,10 17,16 6
|
|
Before Width: | Height: | Size: 400 KiB |
Before Width: | Height: | Size: 10 KiB |
Before Width: | Height: | Size: 15 KiB |
|
@ -1,156 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import heapq
|
|
||||||
|
|
||||||
|
|
||||||
class Cell(object):
|
|
||||||
def __init__(self, x, y, reachable):
|
|
||||||
"""Initialize new cell.
|
|
||||||
|
|
||||||
@param reachable is cell reachable? not a wall?
|
|
||||||
@param x cell x coordinate
|
|
||||||
@param y cell y coordinate
|
|
||||||
@param g cost to move from the starting cell to this cell.
|
|
||||||
@param h estimation of the cost to move from this cell
|
|
||||||
to the ending cell.
|
|
||||||
@param f f = g + h
|
|
||||||
"""
|
|
||||||
self.reachable = reachable
|
|
||||||
self.x = x
|
|
||||||
self.y = y
|
|
||||||
self.parent = None
|
|
||||||
self.g = 0
|
|
||||||
self.h = 0
|
|
||||||
self.f = 0
|
|
||||||
|
|
||||||
def __lt__(self, other):
|
|
||||||
return self.g < other.g
|
|
||||||
|
|
||||||
|
|
||||||
class AStar(object):
|
|
||||||
def __init__(self):
|
|
||||||
# open list
|
|
||||||
self.opened = []
|
|
||||||
heapq.heapify(self.opened)
|
|
||||||
# visited cells list
|
|
||||||
self.closed = set()
|
|
||||||
# grid cells
|
|
||||||
self.cells = []
|
|
||||||
self.grid_height = None
|
|
||||||
self.grid_width = None
|
|
||||||
self.start = None
|
|
||||||
self.end = None
|
|
||||||
|
|
||||||
def init_grid(self, width, height, walls, start, end):
|
|
||||||
"""Prepare grid cells, walls.
|
|
||||||
|
|
||||||
@param width grid's width.
|
|
||||||
@param height grid's height.
|
|
||||||
@param walls list of wall x,y tuples.
|
|
||||||
@param start grid starting point x,y tuple.
|
|
||||||
@param end grid ending point x,y tuple.
|
|
||||||
"""
|
|
||||||
self.grid_height = height
|
|
||||||
self.grid_width = width
|
|
||||||
for x in range(self.grid_width):
|
|
||||||
for y in range(self.grid_height):
|
|
||||||
if (x, y) in walls:
|
|
||||||
reachable = False
|
|
||||||
else:
|
|
||||||
reachable = True
|
|
||||||
self.cells.append(Cell(x, y, reachable))
|
|
||||||
self.start = self.get_cell(*start)
|
|
||||||
self.end = self.get_cell(*end)
|
|
||||||
|
|
||||||
def get_heuristic(self, cell):
|
|
||||||
"""Compute the heuristic value H for a cell.
|
|
||||||
|
|
||||||
Distance between this cell and the ending cell multiply by 10.
|
|
||||||
|
|
||||||
@returns heuristic value H
|
|
||||||
"""
|
|
||||||
return 10 * (abs(cell.x - self.end.x) + abs(cell.y - self.end.y))
|
|
||||||
|
|
||||||
def get_cell(self, x, y):
|
|
||||||
"""Returns a cell from the cells list.
|
|
||||||
|
|
||||||
@param x cell x coordinate
|
|
||||||
@param y cell y coordinate
|
|
||||||
@returns cell
|
|
||||||
"""
|
|
||||||
return self.cells[x * self.grid_height + y]
|
|
||||||
|
|
||||||
def get_adjacent_cells(self, cell):
|
|
||||||
"""Returns adjacent cells to a cell.
|
|
||||||
|
|
||||||
Clockwise starting from the one on the right.
|
|
||||||
|
|
||||||
@param cell get adjacent cells for this cell
|
|
||||||
@returns adjacent cells list.
|
|
||||||
"""
|
|
||||||
cells = []
|
|
||||||
if cell.x < self.grid_width - 1:
|
|
||||||
cells.append(self.get_cell(cell.x + 1, cell.y))
|
|
||||||
if cell.y > 0:
|
|
||||||
cells.append(self.get_cell(cell.x, cell.y - 1))
|
|
||||||
if cell.x > 0:
|
|
||||||
cells.append(self.get_cell(cell.x - 1, cell.y))
|
|
||||||
if cell.y < self.grid_height - 1:
|
|
||||||
cells.append(self.get_cell(cell.x, cell.y + 1))
|
|
||||||
return cells
|
|
||||||
|
|
||||||
def get_path(self):
|
|
||||||
cell = self.end
|
|
||||||
path = [(cell.x, cell.y)]
|
|
||||||
while cell.parent is not self.start:
|
|
||||||
cell = cell.parent
|
|
||||||
path.append((cell.x, cell.y))
|
|
||||||
|
|
||||||
path.append((self.start.x, self.start.y))
|
|
||||||
path.reverse()
|
|
||||||
return path
|
|
||||||
|
|
||||||
def update_cell(self, adj, cell):
|
|
||||||
"""Update adjacent cell.
|
|
||||||
|
|
||||||
@param adj adjacent cell to current cell
|
|
||||||
@param cell current cell being processed
|
|
||||||
"""
|
|
||||||
adj.g = cell.g + 10
|
|
||||||
adj.h = self.get_heuristic(adj)
|
|
||||||
adj.parent = cell
|
|
||||||
adj.f = adj.h + adj.g
|
|
||||||
|
|
||||||
def solve(self):
|
|
||||||
"""Solve maze, find path to ending cell.
|
|
||||||
|
|
||||||
@returns path or None if not found.
|
|
||||||
"""
|
|
||||||
# add starting cell to open heap queue
|
|
||||||
heapq.heappush(self.opened, (self.start.f, self.start))
|
|
||||||
while len(self.opened):
|
|
||||||
# pop cell from heap queue
|
|
||||||
_, cell = heapq.heappop(self.opened)
|
|
||||||
# add cell to closed list so we don't process it twice
|
|
||||||
self.closed.add(cell)
|
|
||||||
# if ending cell, return found path
|
|
||||||
if cell is self.end:
|
|
||||||
return self.get_path()
|
|
||||||
# get adjacent cells for cell
|
|
||||||
adj_cells = self.get_adjacent_cells(cell)
|
|
||||||
for adj_cell in adj_cells:
|
|
||||||
if adj_cell.reachable and adj_cell not in self.closed:
|
|
||||||
if (adj_cell.f, adj_cell) in self.opened:
|
|
||||||
# if adj cell in open list, check if current path is
|
|
||||||
# better than the one previously found
|
|
||||||
# for this adj cell.
|
|
||||||
if adj_cell.g > cell.g + 10:
|
|
||||||
self.update_cell(adj_cell, cell)
|
|
||||||
else:
|
|
||||||
self.update_cell(adj_cell, cell)
|
|
||||||
# add adj cell to open list
|
|
||||||
heapq.heappush(self.opened, (adj_cell.f, adj_cell))
|
|
|
@ -1,131 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
from carla.planner.graph import sldist
|
|
||||||
|
|
||||||
from carla.planner.astar import AStar
|
|
||||||
from carla.planner.map import CarlaMap
|
|
||||||
|
|
||||||
|
|
||||||
class CityTrack(object):
|
|
||||||
|
|
||||||
def __init__(self, city_name):
|
|
||||||
self._map = CarlaMap(city_name)
|
|
||||||
|
|
||||||
self._astar = AStar()
|
|
||||||
|
|
||||||
# Refers to the start position of the previous route computation
|
|
||||||
self._previous_node = []
|
|
||||||
|
|
||||||
# The current computed route
|
|
||||||
self._route = None
|
|
||||||
|
|
||||||
def project_node(self, position):
|
|
||||||
"""
|
|
||||||
Projecting the graph node into the city road
|
|
||||||
"""
|
|
||||||
|
|
||||||
node = self._map.convert_to_node(position)
|
|
||||||
|
|
||||||
# To change the orientation with respect to the map standards
|
|
||||||
|
|
||||||
node = tuple([int(x) for x in node])
|
|
||||||
|
|
||||||
# Set to zero if it is less than zero.
|
|
||||||
|
|
||||||
node = (max(0, node[0]), max(0, node[1]))
|
|
||||||
node = (min(self._map.get_graph_resolution()[0] - 1, node[0]),
|
|
||||||
min(self._map.get_graph_resolution()[1] - 1, node[1]))
|
|
||||||
|
|
||||||
node = self._map.search_on_grid(node)
|
|
||||||
|
|
||||||
return node
|
|
||||||
|
|
||||||
def get_intersection_nodes(self):
|
|
||||||
return self._map.get_intersection_nodes()
|
|
||||||
|
|
||||||
def get_pixel_density(self):
|
|
||||||
return self._map.get_map_resolution()
|
|
||||||
|
|
||||||
def get_node_density(self):
|
|
||||||
return self._map.get_graph_resolution()
|
|
||||||
|
|
||||||
def is_at_goal(self, source, target):
|
|
||||||
return source == target
|
|
||||||
|
|
||||||
def is_at_new_node(self, current_node):
|
|
||||||
return current_node != self._previous_node
|
|
||||||
|
|
||||||
def is_away_from_intersection(self, current_node):
|
|
||||||
return self._closest_intersection_position(current_node) > 1
|
|
||||||
|
|
||||||
def is_far_away_from_route_intersection(self, current_node):
|
|
||||||
# CHECK FOR THE EMPTY CASE
|
|
||||||
if self._route is None:
|
|
||||||
raise RuntimeError('Impossible to find route'
|
|
||||||
+ ' Current planner is limited'
|
|
||||||
+ ' Try to select start points away from intersections')
|
|
||||||
|
|
||||||
return self._closest_intersection_route_position(current_node,
|
|
||||||
self._route) > 4
|
|
||||||
|
|
||||||
def compute_route(self, node_source, source_ori, node_target, target_ori):
|
|
||||||
|
|
||||||
self._previous_node = node_source
|
|
||||||
|
|
||||||
a_star = AStar()
|
|
||||||
a_star.init_grid(self._map.get_graph_resolution()[0],
|
|
||||||
self._map.get_graph_resolution()[1],
|
|
||||||
self._map.get_walls_directed(node_source, source_ori,
|
|
||||||
node_target, target_ori), node_source,
|
|
||||||
node_target)
|
|
||||||
|
|
||||||
route = a_star.solve()
|
|
||||||
|
|
||||||
# JuSt a Corner Case
|
|
||||||
# Clean this to avoid having to use this function
|
|
||||||
if route is None:
|
|
||||||
a_star = AStar()
|
|
||||||
a_star.init_grid(self._map.get_graph_resolution()[0],
|
|
||||||
self._map.get_graph_resolution()[1], self._map.get_walls(),
|
|
||||||
node_source, node_target)
|
|
||||||
|
|
||||||
route = a_star.solve()
|
|
||||||
|
|
||||||
self._route = route
|
|
||||||
|
|
||||||
return route
|
|
||||||
|
|
||||||
def get_distance_closest_node_route(self, pos, route):
|
|
||||||
distance = []
|
|
||||||
|
|
||||||
for node_iter in route:
|
|
||||||
|
|
||||||
if node_iter in self._map.get_intersection_nodes():
|
|
||||||
distance.append(sldist(node_iter, pos))
|
|
||||||
|
|
||||||
if not distance:
|
|
||||||
return sldist(route[-1], pos)
|
|
||||||
return sorted(distance)[0]
|
|
||||||
|
|
||||||
|
|
||||||
def _closest_intersection_position(self, current_node):
|
|
||||||
|
|
||||||
distance_vector = []
|
|
||||||
for node_iterator in self._map.get_intersection_nodes():
|
|
||||||
distance_vector.append(sldist(node_iterator, current_node))
|
|
||||||
|
|
||||||
return sorted(distance_vector)[0]
|
|
||||||
|
|
||||||
|
|
||||||
def _closest_intersection_route_position(self, current_node, route):
|
|
||||||
|
|
||||||
distance_vector = []
|
|
||||||
for _ in route:
|
|
||||||
for node_iterator in self._map.get_intersection_nodes():
|
|
||||||
distance_vector.append(sldist(node_iterator, current_node))
|
|
||||||
|
|
||||||
return sorted(distance_vector)[0]
|
|
|
@ -1,169 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from carla.planner.graph import string_to_floats
|
|
||||||
|
|
||||||
# Constant definition enumeration
|
|
||||||
|
|
||||||
PIXEL = 0
|
|
||||||
WORLD = 1
|
|
||||||
NODE = 2
|
|
||||||
|
|
||||||
|
|
||||||
class Converter(object):
|
|
||||||
|
|
||||||
def __init__(self, city_file, pixel_density, node_density):
|
|
||||||
|
|
||||||
self._node_density = node_density
|
|
||||||
self._pixel_density = pixel_density
|
|
||||||
with open(city_file, 'r') as f:
|
|
||||||
# The offset of the world from the zero coordinates ( The
|
|
||||||
# coordinate we consider zero)
|
|
||||||
self._worldoffset = string_to_floats(f.readline())
|
|
||||||
|
|
||||||
angles = string_to_floats(f.readline())
|
|
||||||
|
|
||||||
# If there is an rotation between the world and map coordinates.
|
|
||||||
self._worldrotation = np.array([
|
|
||||||
[math.cos(math.radians(angles[2])), -math.sin(math.radians(angles[2])), 0.0],
|
|
||||||
[math.sin(math.radians(angles[2])), math.cos(math.radians(angles[2])), 0.0],
|
|
||||||
[0.0, 0.0, 1.0]])
|
|
||||||
|
|
||||||
# Ignore for now, these are offsets for map coordinates and scale
|
|
||||||
# (not used).
|
|
||||||
_ = f.readline()
|
|
||||||
|
|
||||||
# The offset of the map zero coordinate.
|
|
||||||
self._mapoffset = string_to_floats(f.readline())
|
|
||||||
|
|
||||||
def convert_to_node(self, input_data):
|
|
||||||
"""
|
|
||||||
Receives a data type (Can Be Pixel or World )
|
|
||||||
:param input_data: position in some coordinate
|
|
||||||
:return: A vector representing a node
|
|
||||||
"""
|
|
||||||
|
|
||||||
input_type = self._check_input_type(input_data)
|
|
||||||
if input_type == PIXEL:
|
|
||||||
return self._pixel_to_node(input_data)
|
|
||||||
elif input_type == WORLD:
|
|
||||||
return self._world_to_node(input_data)
|
|
||||||
else:
|
|
||||||
raise ValueError('Invalid node to be converted')
|
|
||||||
|
|
||||||
def convert_to_pixel(self, input_data):
|
|
||||||
|
|
||||||
"""
|
|
||||||
Receives a data type (Can Be Node or World )
|
|
||||||
:param input_data: position in some coordinate
|
|
||||||
:return: A vector with pixel coordinates
|
|
||||||
"""
|
|
||||||
|
|
||||||
input_type = self._check_input_type(input_data)
|
|
||||||
|
|
||||||
if input_type == NODE:
|
|
||||||
return self._node_to_pixel(input_data)
|
|
||||||
elif input_type == WORLD:
|
|
||||||
return self._world_to_pixel(input_data)
|
|
||||||
else:
|
|
||||||
raise ValueError('Invalid node to be converted')
|
|
||||||
|
|
||||||
def convert_to_world(self, input_data):
|
|
||||||
|
|
||||||
"""
|
|
||||||
Receives a data type (Can Be Pixel or Node )
|
|
||||||
:param input_data: position in some coordinate
|
|
||||||
:return: vector with world coordinates
|
|
||||||
"""
|
|
||||||
|
|
||||||
input_type = self._check_input_type(input_data)
|
|
||||||
if input_type == NODE:
|
|
||||||
return self._node_to_world(input_data)
|
|
||||||
elif input_type == PIXEL:
|
|
||||||
return self._pixel_to_world(input_data)
|
|
||||||
else:
|
|
||||||
raise ValueError('Invalid node to be converted')
|
|
||||||
|
|
||||||
def get_map_resolution(self):
|
|
||||||
return self._pixel_density
|
|
||||||
|
|
||||||
def _node_to_pixel(self, node):
|
|
||||||
"""
|
|
||||||
Conversion from node format (graph) to pixel (image)
|
|
||||||
:param node:
|
|
||||||
:return: pixel
|
|
||||||
"""
|
|
||||||
pixel = [((node[0] + 2) * self._node_density)
|
|
||||||
, ((node[1] + 2) * self._node_density)]
|
|
||||||
return pixel
|
|
||||||
|
|
||||||
def _pixel_to_node(self, pixel):
|
|
||||||
"""
|
|
||||||
Conversion from pixel format (image) to node (graph)
|
|
||||||
:param node:
|
|
||||||
:return: pixel
|
|
||||||
"""
|
|
||||||
node = [int(((pixel[0]) / self._node_density) - 2)
|
|
||||||
, int(((pixel[1]) / self._node_density) - 2)]
|
|
||||||
|
|
||||||
return tuple(node)
|
|
||||||
|
|
||||||
def _pixel_to_world(self, pixel):
|
|
||||||
"""
|
|
||||||
Conversion from pixel format (image) to world (3D)
|
|
||||||
:param pixel:
|
|
||||||
:return: world
|
|
||||||
"""
|
|
||||||
|
|
||||||
relative_location = [pixel[0] * self._pixel_density,
|
|
||||||
pixel[1] * self._pixel_density]
|
|
||||||
|
|
||||||
world = [
|
|
||||||
relative_location[0] + self._mapoffset[0] - self._worldoffset[0],
|
|
||||||
relative_location[1] + self._mapoffset[1] - self._worldoffset[1],
|
|
||||||
self._mapoffset[2] - self._worldoffset[2]
|
|
||||||
]
|
|
||||||
|
|
||||||
return world
|
|
||||||
|
|
||||||
def _world_to_pixel(self, world):
|
|
||||||
"""
|
|
||||||
Conversion from world format (3D) to pixel
|
|
||||||
:param world:
|
|
||||||
:return: pixel
|
|
||||||
"""
|
|
||||||
|
|
||||||
rotation = np.array([world[0], world[1], world[2]])
|
|
||||||
rotation = rotation.dot(self._worldrotation)
|
|
||||||
|
|
||||||
relative_location = [rotation[0] + self._worldoffset[0] - self._mapoffset[0],
|
|
||||||
rotation[1] + self._worldoffset[1] - self._mapoffset[1],
|
|
||||||
rotation[2] + self._worldoffset[2] - self._mapoffset[2]]
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
pixel = [math.floor(relative_location[0] / float(self._pixel_density)),
|
|
||||||
math.floor(relative_location[1] / float(self._pixel_density))]
|
|
||||||
|
|
||||||
return pixel
|
|
||||||
|
|
||||||
def _world_to_node(self, world):
|
|
||||||
return self._pixel_to_node(self._world_to_pixel(world))
|
|
||||||
|
|
||||||
def _node_to_world(self, node):
|
|
||||||
|
|
||||||
return self._pixel_to_world(self._node_to_pixel(node))
|
|
||||||
|
|
||||||
def _check_input_type(self, input_data):
|
|
||||||
if len(input_data) > 2:
|
|
||||||
return WORLD
|
|
||||||
elif type(input_data[0]) is int:
|
|
||||||
return NODE
|
|
||||||
else:
|
|
||||||
return PIXEL
|
|
|
@ -1,141 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
def string_to_node(string):
|
|
||||||
vec = string.split(',')
|
|
||||||
return (int(vec[0]), int(vec[1]))
|
|
||||||
|
|
||||||
|
|
||||||
def string_to_floats(string):
|
|
||||||
vec = string.split(',')
|
|
||||||
return (float(vec[0]), float(vec[1]), float(vec[2]))
|
|
||||||
|
|
||||||
|
|
||||||
def sldist(c1, c2):
|
|
||||||
return math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1]) ** 2)
|
|
||||||
|
|
||||||
|
|
||||||
def sldist3(c1, c2):
|
|
||||||
return math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1])
|
|
||||||
** 2 + (c2[2] - c1[2]) ** 2)
|
|
||||||
|
|
||||||
|
|
||||||
class Graph(object):
|
|
||||||
"""
|
|
||||||
A simple directed, weighted graph
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, graph_file=None, node_density=50):
|
|
||||||
|
|
||||||
self._nodes = set()
|
|
||||||
self._angles = {}
|
|
||||||
self._edges = {}
|
|
||||||
self._distances = {}
|
|
||||||
self._node_density = node_density
|
|
||||||
|
|
||||||
if graph_file is not None:
|
|
||||||
with open(graph_file, 'r') as f:
|
|
||||||
# Skipe the first four lines that
|
|
||||||
lines_after_4 = f.readlines()[4:]
|
|
||||||
|
|
||||||
# the graph resolution.
|
|
||||||
linegraphres = lines_after_4[0]
|
|
||||||
self._resolution = string_to_node(linegraphres)
|
|
||||||
for line in lines_after_4[1:]:
|
|
||||||
|
|
||||||
from_node, to_node, d = line.split()
|
|
||||||
from_node = string_to_node(from_node)
|
|
||||||
to_node = string_to_node(to_node)
|
|
||||||
|
|
||||||
if from_node not in self._nodes:
|
|
||||||
self.add_node(from_node)
|
|
||||||
if to_node not in self._nodes:
|
|
||||||
self.add_node(to_node)
|
|
||||||
|
|
||||||
self._edges.setdefault(from_node, [])
|
|
||||||
self._edges[from_node].append(to_node)
|
|
||||||
self._distances[(from_node, to_node)] = float(d)
|
|
||||||
|
|
||||||
def add_node(self, value):
|
|
||||||
self._nodes.add(value)
|
|
||||||
|
|
||||||
def make_orientations(self, node, heading):
|
|
||||||
|
|
||||||
import collections
|
|
||||||
distance_dic = {}
|
|
||||||
for node_iter in self._nodes:
|
|
||||||
if node_iter != node:
|
|
||||||
distance_dic[sldist(node, node_iter)] = node_iter
|
|
||||||
|
|
||||||
distance_dic = collections.OrderedDict(
|
|
||||||
sorted(distance_dic.items()))
|
|
||||||
|
|
||||||
self._angles[node] = heading
|
|
||||||
for _, v in distance_dic.items():
|
|
||||||
start_to_goal = np.array([node[0] - v[0], node[1] - v[1]])
|
|
||||||
|
|
||||||
print(start_to_goal)
|
|
||||||
|
|
||||||
self._angles[v] = start_to_goal / np.linalg.norm(start_to_goal)
|
|
||||||
|
|
||||||
def add_edge(self, from_node, to_node, distance):
|
|
||||||
self._add_edge(from_node, to_node, distance)
|
|
||||||
|
|
||||||
def _add_edge(self, from_node, to_node, distance):
|
|
||||||
self._edges.setdefault(from_node, [])
|
|
||||||
self._edges[from_node].append(to_node)
|
|
||||||
self._distances[(from_node, to_node)] = distance
|
|
||||||
|
|
||||||
def get_resolution(self):
|
|
||||||
return self._resolution
|
|
||||||
def get_edges(self):
|
|
||||||
return self._edges
|
|
||||||
|
|
||||||
def intersection_nodes(self):
|
|
||||||
|
|
||||||
intersect_nodes = []
|
|
||||||
for node in self._nodes:
|
|
||||||
if len(self._edges[node]) > 2:
|
|
||||||
intersect_nodes.append(node)
|
|
||||||
|
|
||||||
return intersect_nodes
|
|
||||||
|
|
||||||
# This contains also the non-intersection turns...
|
|
||||||
|
|
||||||
def turn_nodes(self):
|
|
||||||
|
|
||||||
return self._nodes
|
|
||||||
|
|
||||||
def plot_ori(self, c):
|
|
||||||
from matplotlib import collections as mc
|
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
line_len = 1
|
|
||||||
|
|
||||||
lines = [[(p[0], p[1]), (p[0] + line_len * self._angles[p][0],
|
|
||||||
p[1] + line_len * self._angles[p][1])] for p in self._nodes]
|
|
||||||
lc = mc.LineCollection(lines, linewidth=2, color='green')
|
|
||||||
_, ax = plt.subplots()
|
|
||||||
ax.add_collection(lc)
|
|
||||||
|
|
||||||
ax.autoscale()
|
|
||||||
ax.margins(0.1)
|
|
||||||
|
|
||||||
xs = [p[0] for p in self._nodes]
|
|
||||||
ys = [p[1] for p in self._nodes]
|
|
||||||
|
|
||||||
plt.scatter(xs, ys, color=c)
|
|
||||||
|
|
||||||
def plot(self, c):
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
xs = [p[0] for p in self._nodes]
|
|
||||||
ys = [p[1] for p in self._nodes]
|
|
||||||
|
|
||||||
plt.scatter(xs, ys, color=c)
|
|
|
@ -1,135 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import copy
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
def angle_between(v1, v2):
|
|
||||||
return np.arccos(np.dot(v1, v2) / np.linalg.norm(v1) / np.linalg.norm(v2))
|
|
||||||
|
|
||||||
|
|
||||||
class Grid(object):
|
|
||||||
|
|
||||||
def __init__(self, graph):
|
|
||||||
|
|
||||||
self._graph = graph
|
|
||||||
self._structure = self._make_structure()
|
|
||||||
self._walls = self._make_walls()
|
|
||||||
|
|
||||||
def search_on_grid(self, x, y):
|
|
||||||
visit = [[0, 1], [0, -1], [1, 0], [1, 1],
|
|
||||||
[1, -1], [-1, 0], [-1, 1], [-1, -1]]
|
|
||||||
c_x, c_y = x, y
|
|
||||||
scale = 1
|
|
||||||
while self._structure[c_x, c_y] != 0:
|
|
||||||
for offset in visit:
|
|
||||||
c_x, c_y = x + offset[0] * scale, y + offset[1] * scale
|
|
||||||
|
|
||||||
if c_x >= 0 and c_x < self._graph.get_resolution()[
|
|
||||||
0] and c_y >= 0 and c_y < self._graph.get_resolution()[1]:
|
|
||||||
if self._structure[c_x, c_y] == 0:
|
|
||||||
break
|
|
||||||
else:
|
|
||||||
c_x, c_y = x, y
|
|
||||||
scale += 1
|
|
||||||
|
|
||||||
return c_x, c_y
|
|
||||||
def get_walls(self):
|
|
||||||
return self._walls
|
|
||||||
|
|
||||||
def get_wall_source(self, pos, pos_ori, target):
|
|
||||||
|
|
||||||
free_nodes = self._get_adjacent_free_nodes(pos)
|
|
||||||
# print self._walls
|
|
||||||
final_walls = copy.copy(self._walls)
|
|
||||||
# print final_walls
|
|
||||||
heading_start = np.array([pos_ori[0], pos_ori[1]])
|
|
||||||
for adj in free_nodes:
|
|
||||||
|
|
||||||
start_to_goal = np.array([adj[0] - pos[0], adj[1] - pos[1]])
|
|
||||||
angle = angle_between(heading_start, start_to_goal)
|
|
||||||
if (angle > 1.6 and adj != target):
|
|
||||||
final_walls.add((adj[0], adj[1]))
|
|
||||||
|
|
||||||
return final_walls
|
|
||||||
|
|
||||||
def get_wall_target(self, pos, pos_ori, source):
|
|
||||||
|
|
||||||
free_nodes = self._get_adjacent_free_nodes(pos)
|
|
||||||
final_walls = copy.copy(self._walls)
|
|
||||||
heading_start = np.array([pos_ori[0], pos_ori[1]])
|
|
||||||
for adj in free_nodes:
|
|
||||||
|
|
||||||
start_to_goal = np.array([adj[0] - pos[0], adj[1] - pos[1]])
|
|
||||||
angle = angle_between(heading_start, start_to_goal)
|
|
||||||
|
|
||||||
if (angle < 1.0 and adj != source):
|
|
||||||
final_walls.add((adj[0], adj[1]))
|
|
||||||
|
|
||||||
return final_walls
|
|
||||||
|
|
||||||
def _draw_line(self, grid, xi, yi, xf, yf):
|
|
||||||
|
|
||||||
if xf < xi:
|
|
||||||
aux = xi
|
|
||||||
xi = xf
|
|
||||||
xf = aux
|
|
||||||
|
|
||||||
if yf < yi:
|
|
||||||
aux = yi
|
|
||||||
yi = yf
|
|
||||||
yf = aux
|
|
||||||
|
|
||||||
for i in range(xi, xf + 1):
|
|
||||||
|
|
||||||
for j in range(yi, yf + 1):
|
|
||||||
grid[i, j] = 0.0
|
|
||||||
|
|
||||||
return grid
|
|
||||||
|
|
||||||
def _make_structure(self):
|
|
||||||
structure = np.ones(
|
|
||||||
(self._graph.get_resolution()[0],
|
|
||||||
self._graph.get_resolution()[1]))
|
|
||||||
|
|
||||||
for key, connections in self._graph.get_edges().items():
|
|
||||||
|
|
||||||
# draw a line
|
|
||||||
for con in connections:
|
|
||||||
# print key[0],key[1],con[0],con[1]
|
|
||||||
structure = self._draw_line(
|
|
||||||
structure, key[0], key[1], con[0], con[1])
|
|
||||||
# print grid
|
|
||||||
return structure
|
|
||||||
|
|
||||||
def _make_walls(self):
|
|
||||||
walls = set()
|
|
||||||
|
|
||||||
for i in range(self._structure.shape[0]):
|
|
||||||
|
|
||||||
for j in range(self._structure.shape[1]):
|
|
||||||
if self._structure[i, j] == 1.0:
|
|
||||||
walls.add((i, j))
|
|
||||||
|
|
||||||
return walls
|
|
||||||
|
|
||||||
def _get_adjacent_free_nodes(self, pos):
|
|
||||||
""" Eight nodes in total """
|
|
||||||
visit = [[0, 1], [0, -1], [1, 0], [1, 1],
|
|
||||||
[1, -1], [-1, 0], [-1, 1], [-1, -1]]
|
|
||||||
|
|
||||||
adjacent = set()
|
|
||||||
for offset in visit:
|
|
||||||
node = (pos[0] + offset[0], pos[1] + offset[1])
|
|
||||||
|
|
||||||
if (node[0] >= 0 and node[0] < self._graph.get_resolution()[0]
|
|
||||||
and node[1] >= 0 and node[1] < self._graph.get_resolution()[1]):
|
|
||||||
|
|
||||||
if self._structure[node[0], node[1]] == 0.0:
|
|
||||||
adjacent.add(node)
|
|
||||||
|
|
||||||
return adjacent
|
|
|
@ -1,154 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Class used for operating the city map."""
|
|
||||||
|
|
||||||
import math
|
|
||||||
import os
|
|
||||||
|
|
||||||
try:
|
|
||||||
import numpy as np
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import numpy, make sure numpy package is installed')
|
|
||||||
|
|
||||||
try:
|
|
||||||
from PIL import Image
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import PIL, make sure pillow package is installed')
|
|
||||||
|
|
||||||
from carla.planner.graph import Graph
|
|
||||||
from carla.planner.graph import sldist
|
|
||||||
from carla.planner.grid import Grid
|
|
||||||
from carla.planner.converter import Converter
|
|
||||||
|
|
||||||
|
|
||||||
def color_to_angle(color):
|
|
||||||
return (float(color) / 255.0) * 2 * math.pi
|
|
||||||
|
|
||||||
|
|
||||||
class CarlaMap(object):
|
|
||||||
|
|
||||||
def __init__(self, city, pixel_density=0.1643, node_density=50):
|
|
||||||
dir_path = os.path.dirname(__file__)
|
|
||||||
city_file = os.path.join(dir_path, city + '.txt')
|
|
||||||
|
|
||||||
city_map_file = os.path.join(dir_path, city + '.png')
|
|
||||||
city_map_file_lanes = os.path.join(dir_path, city + 'Lanes.png')
|
|
||||||
city_map_file_center = os.path.join(dir_path, city + 'Central.png')
|
|
||||||
|
|
||||||
# The built graph. This is the exact same graph that unreal builds. This
|
|
||||||
# is a generic structure used for many cases
|
|
||||||
self._graph = Graph(city_file, node_density)
|
|
||||||
|
|
||||||
self._pixel_density = pixel_density
|
|
||||||
self._grid = Grid(self._graph)
|
|
||||||
# The number of game units per pixel. For now this is fixed.
|
|
||||||
|
|
||||||
self._converter = Converter(city_file, pixel_density, node_density)
|
|
||||||
|
|
||||||
# Load the lanes image
|
|
||||||
self.map_image_lanes = Image.open(city_map_file_lanes)
|
|
||||||
self.map_image_lanes.load()
|
|
||||||
self.map_image_lanes = np.asarray(self.map_image_lanes, dtype="int32")
|
|
||||||
# Load the image
|
|
||||||
self.map_image = Image.open(city_map_file)
|
|
||||||
self.map_image.load()
|
|
||||||
self.map_image = np.asarray(self.map_image, dtype="int32")
|
|
||||||
|
|
||||||
# Load the lanes image
|
|
||||||
self.map_image_center = Image.open(city_map_file_center)
|
|
||||||
self.map_image_center.load()
|
|
||||||
self.map_image_center = np.asarray(self.map_image_center, dtype="int32")
|
|
||||||
|
|
||||||
def get_graph_resolution(self):
|
|
||||||
return self._graph.get_resolution()
|
|
||||||
|
|
||||||
def get_map_resolution(self):
|
|
||||||
return self._converter.get_map_resolution()
|
|
||||||
|
|
||||||
def get_map(self, height=None):
|
|
||||||
if height is not None:
|
|
||||||
img = Image.fromarray(self.map_image.astype(np.uint8))
|
|
||||||
|
|
||||||
aspect_ratio = height / float(self.map_image.shape[0])
|
|
||||||
|
|
||||||
img = img.resize((int(aspect_ratio * self.map_image.shape[1]), height), Image.ANTIALIAS)
|
|
||||||
img.load()
|
|
||||||
return np.asarray(img, dtype="int32")
|
|
||||||
return np.fliplr(self.map_image)
|
|
||||||
|
|
||||||
def get_map_lanes(self, size=None):
|
|
||||||
if size is not None:
|
|
||||||
img = Image.fromarray(self.map_image_lanes.astype(np.uint8))
|
|
||||||
img = img.resize((size[1], size[0]), Image.ANTIALIAS)
|
|
||||||
img.load()
|
|
||||||
return np.fliplr(np.asarray(img, dtype="int32"))
|
|
||||||
return np.fliplr(self.map_image_lanes)
|
|
||||||
|
|
||||||
def get_lane_orientation(self, world):
|
|
||||||
"""Get the lane orientation of a certain world position."""
|
|
||||||
pixel = self.convert_to_pixel(world)
|
|
||||||
|
|
||||||
ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 2]
|
|
||||||
ori = color_to_angle(ori)
|
|
||||||
|
|
||||||
return (-math.cos(ori), -math.sin(ori))
|
|
||||||
|
|
||||||
def convert_to_node(self, input_data):
|
|
||||||
"""
|
|
||||||
Receives a data type (Can Be Pixel or World )
|
|
||||||
:param input_data: position in some coordinate
|
|
||||||
:return: A node object
|
|
||||||
"""
|
|
||||||
return self._converter.convert_to_node(input_data)
|
|
||||||
|
|
||||||
def convert_to_pixel(self, input_data):
|
|
||||||
"""
|
|
||||||
Receives a data type (Can Be Node or World )
|
|
||||||
:param input_data: position in some coordinate
|
|
||||||
:return: A node object
|
|
||||||
"""
|
|
||||||
return self._converter.convert_to_pixel(input_data)
|
|
||||||
|
|
||||||
def convert_to_world(self, input_data):
|
|
||||||
"""
|
|
||||||
Receives a data type (Can Be Pixel or Node )
|
|
||||||
:param input_data: position in some coordinate
|
|
||||||
:return: A node object
|
|
||||||
"""
|
|
||||||
return self._converter.convert_to_world(input_data)
|
|
||||||
|
|
||||||
def get_walls_directed(self, node_source, source_ori, node_target, target_ori):
|
|
||||||
"""
|
|
||||||
This is the most hacky function. Instead of planning on two ways,
|
|
||||||
we basically use a one way road and interrupt the other road by adding
|
|
||||||
an artificial wall.
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
final_walls = self._grid.get_wall_source(node_source, source_ori, node_target)
|
|
||||||
|
|
||||||
final_walls = final_walls.union(self._grid.get_wall_target(
|
|
||||||
node_target, target_ori, node_source))
|
|
||||||
return final_walls
|
|
||||||
|
|
||||||
def get_walls(self):
|
|
||||||
|
|
||||||
return self._grid.get_walls()
|
|
||||||
|
|
||||||
def get_distance_closest_node(self, pos):
|
|
||||||
|
|
||||||
distance = []
|
|
||||||
for node_iter in self._graph.intersection_nodes():
|
|
||||||
distance.append(sldist(node_iter, pos))
|
|
||||||
|
|
||||||
return sorted(distance)[0]
|
|
||||||
|
|
||||||
def get_intersection_nodes(self):
|
|
||||||
return self._graph.intersection_nodes()
|
|
||||||
|
|
||||||
def search_on_grid(self,node):
|
|
||||||
return self._grid.search_on_grid(node[0], node[1])
|
|
|
@ -1,175 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import collections
|
|
||||||
import math
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from . import city_track
|
|
||||||
|
|
||||||
|
|
||||||
def compare(x, y):
|
|
||||||
return collections.Counter(x) == collections.Counter(y)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# Constants Used for the high level commands
|
|
||||||
|
|
||||||
|
|
||||||
REACH_GOAL = 0.0
|
|
||||||
GO_STRAIGHT = 5.0
|
|
||||||
TURN_RIGHT = 4.0
|
|
||||||
TURN_LEFT = 3.0
|
|
||||||
LANE_FOLLOW = 2.0
|
|
||||||
|
|
||||||
|
|
||||||
# Auxiliary algebra function
|
|
||||||
def angle_between(v1, v2):
|
|
||||||
return np.arccos(np.dot(v1, v2) / np.linalg.norm(v1) / np.linalg.norm(v2))
|
|
||||||
|
|
||||||
|
|
||||||
def sldist(c1, c2): return math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1]) ** 2)
|
|
||||||
|
|
||||||
|
|
||||||
def signal(v1, v2):
|
|
||||||
return np.cross(v1, v2) / np.linalg.norm(v1) / np.linalg.norm(v2)
|
|
||||||
|
|
||||||
|
|
||||||
class Planner(object):
|
|
||||||
|
|
||||||
def __init__(self, city_name):
|
|
||||||
|
|
||||||
self._city_track = city_track.CityTrack(city_name)
|
|
||||||
|
|
||||||
self._commands = []
|
|
||||||
|
|
||||||
def get_next_command(self, source, source_ori, target, target_ori):
|
|
||||||
"""
|
|
||||||
Computes the full plan and returns the next command,
|
|
||||||
Args
|
|
||||||
source: source position
|
|
||||||
source_ori: source orientation
|
|
||||||
target: target position
|
|
||||||
target_ori: target orientation
|
|
||||||
Returns
|
|
||||||
a command ( Straight,Lane Follow, Left or Right)
|
|
||||||
"""
|
|
||||||
|
|
||||||
track_source = self._city_track.project_node(source)
|
|
||||||
track_target = self._city_track.project_node(target)
|
|
||||||
|
|
||||||
# reach the goal
|
|
||||||
|
|
||||||
if self._city_track.is_at_goal(track_source, track_target):
|
|
||||||
return REACH_GOAL
|
|
||||||
|
|
||||||
if (self._city_track.is_at_new_node(track_source)
|
|
||||||
and self._city_track.is_away_from_intersection(track_source)):
|
|
||||||
|
|
||||||
route = self._city_track.compute_route(track_source, source_ori,
|
|
||||||
track_target, target_ori)
|
|
||||||
if route is None:
|
|
||||||
raise RuntimeError('Impossible to find route')
|
|
||||||
|
|
||||||
self._commands = self._route_to_commands(route)
|
|
||||||
|
|
||||||
if self._city_track.is_far_away_from_route_intersection(
|
|
||||||
track_source):
|
|
||||||
return LANE_FOLLOW
|
|
||||||
else:
|
|
||||||
if self._commands:
|
|
||||||
return self._commands[0]
|
|
||||||
else:
|
|
||||||
return LANE_FOLLOW
|
|
||||||
else:
|
|
||||||
|
|
||||||
if self._city_track.is_far_away_from_route_intersection(
|
|
||||||
track_source):
|
|
||||||
return LANE_FOLLOW
|
|
||||||
|
|
||||||
# If there is computed commands
|
|
||||||
if self._commands:
|
|
||||||
return self._commands[0]
|
|
||||||
else:
|
|
||||||
return LANE_FOLLOW
|
|
||||||
|
|
||||||
def get_shortest_path_distance(
|
|
||||||
self,
|
|
||||||
source,
|
|
||||||
source_ori,
|
|
||||||
target,
|
|
||||||
target_ori):
|
|
||||||
|
|
||||||
distance = 0
|
|
||||||
track_source = self._city_track.project_node(source)
|
|
||||||
track_target = self._city_track.project_node(target)
|
|
||||||
|
|
||||||
current_pos = track_source
|
|
||||||
|
|
||||||
route = self._city_track.compute_route(track_source, source_ori,
|
|
||||||
track_target, target_ori)
|
|
||||||
# No Route, distance is zero
|
|
||||||
if route is None:
|
|
||||||
return 0.0
|
|
||||||
|
|
||||||
for node_iter in route:
|
|
||||||
distance += sldist(node_iter, current_pos)
|
|
||||||
current_pos = node_iter
|
|
||||||
|
|
||||||
# We multiply by these values to convert distance to world coordinates
|
|
||||||
return distance * self._city_track.get_pixel_density() \
|
|
||||||
* self._city_track.get_node_density()
|
|
||||||
|
|
||||||
def is_there_posible_route(self, source, source_ori, target, target_ori):
|
|
||||||
|
|
||||||
track_source = self._city_track.project_node(source)
|
|
||||||
track_target = self._city_track.project_node(target)
|
|
||||||
|
|
||||||
return not self._city_track.compute_route(
|
|
||||||
track_source, source_ori, track_target, target_ori) is None
|
|
||||||
|
|
||||||
def test_position(self, source):
|
|
||||||
|
|
||||||
node_source = self._city_track.project_node(source)
|
|
||||||
|
|
||||||
return self._city_track.is_away_from_intersection(node_source)
|
|
||||||
|
|
||||||
def _route_to_commands(self, route):
|
|
||||||
|
|
||||||
"""
|
|
||||||
from the shortest path graph, transform it into a list of commands
|
|
||||||
|
|
||||||
:param route: the sub graph containing the shortest path
|
|
||||||
:return: list of commands encoded from 0-5
|
|
||||||
"""
|
|
||||||
|
|
||||||
commands_list = []
|
|
||||||
|
|
||||||
for i in range(0, len(route)):
|
|
||||||
if route[i] not in self._city_track.get_intersection_nodes():
|
|
||||||
continue
|
|
||||||
|
|
||||||
current = route[i]
|
|
||||||
past = route[i - 1]
|
|
||||||
future = route[i + 1]
|
|
||||||
|
|
||||||
past_to_current = np.array(
|
|
||||||
[current[0] - past[0], current[1] - past[1]])
|
|
||||||
current_to_future = np.array(
|
|
||||||
[future[0] - current[0], future[1] - current[1]])
|
|
||||||
angle = signal(current_to_future, past_to_current)
|
|
||||||
|
|
||||||
if angle < -0.1:
|
|
||||||
command = TURN_RIGHT
|
|
||||||
elif angle > 0.1:
|
|
||||||
command = TURN_LEFT
|
|
||||||
else:
|
|
||||||
command = GO_STRAIGHT
|
|
||||||
|
|
||||||
commands_list.append(command)
|
|
||||||
|
|
||||||
return commands_list
|
|
|
@ -1,334 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""CARLA sensors."""
|
|
||||||
|
|
||||||
|
|
||||||
import os
|
|
||||||
|
|
||||||
from collections import namedtuple
|
|
||||||
|
|
||||||
try:
|
|
||||||
import numpy
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import numpy, make sure numpy package is installed.')
|
|
||||||
|
|
||||||
from .transform import Transform, Translation, Rotation, Scale
|
|
||||||
|
|
||||||
|
|
||||||
# ==============================================================================
|
|
||||||
# -- Helpers -------------------------------------------------------------------
|
|
||||||
# ==============================================================================
|
|
||||||
|
|
||||||
|
|
||||||
Color = namedtuple('Color', 'r g b')
|
|
||||||
Color.__new__.__defaults__ = (0, 0, 0)
|
|
||||||
|
|
||||||
|
|
||||||
Point = namedtuple('Point', 'x y z color')
|
|
||||||
Point.__new__.__defaults__ = (0.0, 0.0, 0.0, None)
|
|
||||||
|
|
||||||
|
|
||||||
def _append_extension(filename, ext):
|
|
||||||
return filename if filename.lower().endswith(ext.lower()) else filename + ext
|
|
||||||
|
|
||||||
|
|
||||||
# ==============================================================================
|
|
||||||
# -- Sensor --------------------------------------------------------------------
|
|
||||||
# ==============================================================================
|
|
||||||
|
|
||||||
|
|
||||||
class Sensor(object):
|
|
||||||
"""
|
|
||||||
Base class for sensor descriptions. Used to add sensors to CarlaSettings.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, name, sensor_type):
|
|
||||||
self.SensorName = name
|
|
||||||
self.SensorType = sensor_type
|
|
||||||
self.PositionX = 0.2
|
|
||||||
self.PositionY = 0.0
|
|
||||||
self.PositionZ = 1.3
|
|
||||||
self.RotationPitch = 0.0
|
|
||||||
self.RotationRoll = 0.0
|
|
||||||
self.RotationYaw = 0.0
|
|
||||||
|
|
||||||
def set(self, **kwargs):
|
|
||||||
for key, value in kwargs.items():
|
|
||||||
if not hasattr(self, key):
|
|
||||||
raise ValueError('sensor.Sensor: no key named %r' % key)
|
|
||||||
setattr(self, key, value)
|
|
||||||
|
|
||||||
def set_position(self, x, y, z):
|
|
||||||
self.PositionX = x
|
|
||||||
self.PositionY = y
|
|
||||||
self.PositionZ = z
|
|
||||||
|
|
||||||
def set_rotation(self, pitch, yaw, roll):
|
|
||||||
self.RotationPitch = pitch
|
|
||||||
self.RotationYaw = yaw
|
|
||||||
self.RotationRoll = roll
|
|
||||||
|
|
||||||
def get_transform(self):
|
|
||||||
'''
|
|
||||||
Returns the camera to [whatever the camera is attached to]
|
|
||||||
transformation.
|
|
||||||
'''
|
|
||||||
return Transform(
|
|
||||||
Translation(self.PositionX, self.PositionY, self.PositionZ),
|
|
||||||
Rotation(self.RotationPitch, self.RotationYaw, self.RotationRoll))
|
|
||||||
|
|
||||||
def get_unreal_transform(self):
|
|
||||||
'''
|
|
||||||
Returns the camera to [whatever the camera is attached to]
|
|
||||||
transformation with the Unreal necessary corrections applied.
|
|
||||||
|
|
||||||
@todo Do we need to expose this?
|
|
||||||
'''
|
|
||||||
to_unreal_transform = Transform(Rotation(roll=-90, yaw=90), Scale(x=-1))
|
|
||||||
return self.get_transform() * to_unreal_transform
|
|
||||||
|
|
||||||
|
|
||||||
class Camera(Sensor):
|
|
||||||
"""
|
|
||||||
Camera description. This class can be added to a CarlaSettings object to add
|
|
||||||
a camera to the player vehicle.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, name, **kwargs):
|
|
||||||
super(Camera, self).__init__(name, sensor_type="CAMERA")
|
|
||||||
self.PostProcessing = 'SceneFinal'
|
|
||||||
self.ImageSizeX = 720
|
|
||||||
self.ImageSizeY = 512
|
|
||||||
self.FOV = 90.0
|
|
||||||
self.set(**kwargs)
|
|
||||||
|
|
||||||
def set_image_size(self, pixels_x, pixels_y):
|
|
||||||
'''Sets the image size in pixels'''
|
|
||||||
self.ImageSizeX = pixels_x
|
|
||||||
self.ImageSizeY = pixels_y
|
|
||||||
|
|
||||||
|
|
||||||
class Lidar(Sensor):
|
|
||||||
"""
|
|
||||||
Lidar description. This class can be added to a CarlaSettings object to add
|
|
||||||
a Lidar to the player vehicle.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, name, **kwargs):
|
|
||||||
super(Lidar, self).__init__(name, sensor_type="LIDAR_RAY_CAST")
|
|
||||||
self.Channels = 32
|
|
||||||
self.Range = 50.0
|
|
||||||
self.PointsPerSecond = 56000
|
|
||||||
self.RotationFrequency = 10.0
|
|
||||||
self.UpperFovLimit = 10.0
|
|
||||||
self.LowerFovLimit = -30.0
|
|
||||||
self.ShowDebugPoints = False
|
|
||||||
self.set(**kwargs)
|
|
||||||
|
|
||||||
|
|
||||||
# ==============================================================================
|
|
||||||
# -- SensorData ----------------------------------------------------------------
|
|
||||||
# ==============================================================================
|
|
||||||
|
|
||||||
|
|
||||||
class SensorData(object):
|
|
||||||
"""Base class for sensor data returned from the server."""
|
|
||||||
def __init__(self, frame_number):
|
|
||||||
self.frame_number = frame_number
|
|
||||||
|
|
||||||
|
|
||||||
class Image(SensorData):
|
|
||||||
"""Data generated by a Camera."""
|
|
||||||
|
|
||||||
def __init__(self, frame_number, width, height, image_type, fov, raw_data):
|
|
||||||
super(Image, self).__init__(frame_number=frame_number)
|
|
||||||
assert len(raw_data) == 4 * width * height
|
|
||||||
self.width = width
|
|
||||||
self.height = height
|
|
||||||
self.type = image_type
|
|
||||||
self.fov = fov
|
|
||||||
self.raw_data = raw_data
|
|
||||||
self._converted_data = None
|
|
||||||
|
|
||||||
@property
|
|
||||||
def data(self):
|
|
||||||
"""
|
|
||||||
Lazy initialization for data property, stores converted data in its
|
|
||||||
default format.
|
|
||||||
"""
|
|
||||||
if self._converted_data is None:
|
|
||||||
from . import image_converter
|
|
||||||
|
|
||||||
if self.type == 'Depth':
|
|
||||||
self._converted_data = image_converter.depth_to_array(self)
|
|
||||||
elif self.type == 'SemanticSegmentation':
|
|
||||||
self._converted_data = image_converter.labels_to_array(self)
|
|
||||||
else:
|
|
||||||
self._converted_data = image_converter.to_rgb_array(self)
|
|
||||||
return self._converted_data
|
|
||||||
|
|
||||||
def save_to_disk(self, filename):
|
|
||||||
"""Save this image to disk (requires PIL installed)."""
|
|
||||||
filename = _append_extension(filename, '.png')
|
|
||||||
|
|
||||||
try:
|
|
||||||
from PIL import Image as PImage
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError(
|
|
||||||
'cannot import PIL, make sure pillow package is installed')
|
|
||||||
|
|
||||||
image = PImage.frombytes(
|
|
||||||
mode='RGBA',
|
|
||||||
size=(self.width, self.height),
|
|
||||||
data=self.raw_data,
|
|
||||||
decoder_name='raw')
|
|
||||||
color = image.split()
|
|
||||||
image = PImage.merge("RGB", color[2::-1])
|
|
||||||
|
|
||||||
folder = os.path.dirname(filename)
|
|
||||||
if not os.path.isdir(folder):
|
|
||||||
os.makedirs(folder)
|
|
||||||
image.save(filename)
|
|
||||||
|
|
||||||
|
|
||||||
class PointCloud(SensorData):
|
|
||||||
"""A list of points."""
|
|
||||||
|
|
||||||
def __init__(self, frame_number, array, color_array=None):
|
|
||||||
super(PointCloud, self).__init__(frame_number=frame_number)
|
|
||||||
self._array = array
|
|
||||||
self._color_array = color_array
|
|
||||||
self._has_colors = color_array is not None
|
|
||||||
|
|
||||||
@property
|
|
||||||
def array(self):
|
|
||||||
"""The numpy array holding the point-cloud.
|
|
||||||
|
|
||||||
3D points format for n elements:
|
|
||||||
[ [X0,Y0,Z0],
|
|
||||||
...,
|
|
||||||
[Xn,Yn,Zn] ]
|
|
||||||
"""
|
|
||||||
return self._array
|
|
||||||
|
|
||||||
@property
|
|
||||||
def color_array(self):
|
|
||||||
"""The numpy array holding the colors corresponding to each point.
|
|
||||||
It is None if there are no colors.
|
|
||||||
|
|
||||||
Colors format for n elements:
|
|
||||||
[ [R0,G0,B0],
|
|
||||||
...,
|
|
||||||
[Rn,Gn,Bn] ]
|
|
||||||
"""
|
|
||||||
return self._color_array
|
|
||||||
|
|
||||||
def has_colors(self):
|
|
||||||
"""Return whether the points have color."""
|
|
||||||
return self._has_colors
|
|
||||||
|
|
||||||
def apply_transform(self, transformation):
|
|
||||||
"""Modify the PointCloud instance transforming its points"""
|
|
||||||
self._array = transformation.transform_points(self._array)
|
|
||||||
|
|
||||||
def save_to_disk(self, filename):
|
|
||||||
"""Save this point-cloud to disk as PLY format."""
|
|
||||||
filename = _append_extension(filename, '.ply')
|
|
||||||
|
|
||||||
def construct_ply_header():
|
|
||||||
"""Generates a PLY header given a total number of 3D points and
|
|
||||||
coloring property if specified
|
|
||||||
"""
|
|
||||||
points = len(self) # Total point number
|
|
||||||
header = ['ply',
|
|
||||||
'format ascii 1.0',
|
|
||||||
'element vertex {}',
|
|
||||||
'property float32 x',
|
|
||||||
'property float32 y',
|
|
||||||
'property float32 z',
|
|
||||||
'property uchar diffuse_red',
|
|
||||||
'property uchar diffuse_green',
|
|
||||||
'property uchar diffuse_blue',
|
|
||||||
'end_header']
|
|
||||||
if not self._has_colors:
|
|
||||||
return '\n'.join(header[0:6] + [header[-1]]).format(points)
|
|
||||||
return '\n'.join(header).format(points)
|
|
||||||
|
|
||||||
if not self._has_colors:
|
|
||||||
ply = '\n'.join(['{:.2f} {:.2f} {:.2f}'.format(
|
|
||||||
*p) for p in self._array.tolist()])
|
|
||||||
else:
|
|
||||||
points_3d = numpy.concatenate(
|
|
||||||
(self._array, self._color_array), axis=1)
|
|
||||||
ply = '\n'.join(['{:.2f} {:.2f} {:.2f} {:.0f} {:.0f} {:.0f}'
|
|
||||||
.format(*p) for p in points_3d.tolist()])
|
|
||||||
|
|
||||||
# Create folder to save if does not exist.
|
|
||||||
folder = os.path.dirname(filename)
|
|
||||||
if not os.path.isdir(folder):
|
|
||||||
os.makedirs(folder)
|
|
||||||
|
|
||||||
# Open the file and save with the specific PLY format.
|
|
||||||
with open(filename, 'w+') as ply_file:
|
|
||||||
ply_file.write('\n'.join([construct_ply_header(), ply, '']))
|
|
||||||
|
|
||||||
def __len__(self):
|
|
||||||
return len(self.array)
|
|
||||||
|
|
||||||
def __getitem__(self, key):
|
|
||||||
color = None if self._color_array is None else Color(
|
|
||||||
*self._color_array[key])
|
|
||||||
return Point(*self._array[key], color=color)
|
|
||||||
|
|
||||||
def __iter__(self):
|
|
||||||
class PointIterator(object):
|
|
||||||
"""Iterator class for PointCloud"""
|
|
||||||
|
|
||||||
def __init__(self, point_cloud):
|
|
||||||
self.point_cloud = point_cloud
|
|
||||||
self.index = -1
|
|
||||||
|
|
||||||
def __next__(self):
|
|
||||||
self.index += 1
|
|
||||||
if self.index >= len(self.point_cloud):
|
|
||||||
raise StopIteration
|
|
||||||
return self.point_cloud[self.index]
|
|
||||||
|
|
||||||
def next(self):
|
|
||||||
return self.__next__()
|
|
||||||
|
|
||||||
return PointIterator(self)
|
|
||||||
|
|
||||||
def __str__(self):
|
|
||||||
return str(self.array)
|
|
||||||
|
|
||||||
|
|
||||||
class LidarMeasurement(SensorData):
|
|
||||||
"""Data generated by a Lidar."""
|
|
||||||
|
|
||||||
def __init__(self, frame_number, horizontal_angle, channels, point_count_by_channel, point_cloud):
|
|
||||||
super(LidarMeasurement, self).__init__(frame_number=frame_number)
|
|
||||||
assert numpy.sum(point_count_by_channel) == len(point_cloud.array)
|
|
||||||
self.horizontal_angle = horizontal_angle
|
|
||||||
self.channels = channels
|
|
||||||
self.point_count_by_channel = point_count_by_channel
|
|
||||||
self.point_cloud = point_cloud
|
|
||||||
|
|
||||||
@property
|
|
||||||
def data(self):
|
|
||||||
"""The numpy array holding the point-cloud.
|
|
||||||
|
|
||||||
3D points format for n elements:
|
|
||||||
[ [X0,Y0,Z0],
|
|
||||||
...,
|
|
||||||
[Xn,Yn,Zn] ]
|
|
||||||
"""
|
|
||||||
return self.point_cloud.array
|
|
||||||
|
|
||||||
def save_to_disk(self, filename):
|
|
||||||
"""Save point-cloud to disk as PLY format."""
|
|
||||||
self.point_cloud.save_to_disk(filename)
|
|
|
@ -1,123 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""CARLA Settings"""
|
|
||||||
|
|
||||||
import io
|
|
||||||
import random
|
|
||||||
import sys
|
|
||||||
|
|
||||||
|
|
||||||
if sys.version_info >= (3, 0):
|
|
||||||
|
|
||||||
from configparser import ConfigParser
|
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
from ConfigParser import RawConfigParser as ConfigParser
|
|
||||||
|
|
||||||
|
|
||||||
from . import sensor as carla_sensor
|
|
||||||
|
|
||||||
|
|
||||||
MAX_NUMBER_OF_WEATHER_IDS = 14
|
|
||||||
|
|
||||||
|
|
||||||
class CarlaSettings(object):
|
|
||||||
"""
|
|
||||||
The CarlaSettings object controls the settings of an episode. The __str__
|
|
||||||
method retrieves an str with a CarlaSettings.ini file contents.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, **kwargs):
|
|
||||||
# [CARLA/Server]
|
|
||||||
self.SynchronousMode = True
|
|
||||||
self.SendNonPlayerAgentsInfo = False
|
|
||||||
self.DisableRendering = False
|
|
||||||
# [CARLA/QualitySettings]
|
|
||||||
self.QualityLevel = 'Epic'
|
|
||||||
# [CARLA/LevelSettings]
|
|
||||||
self.PlayerVehicle = None
|
|
||||||
self.NumberOfVehicles = 20
|
|
||||||
self.NumberOfPedestrians = 30
|
|
||||||
self.WeatherId = 1
|
|
||||||
self.SeedVehicles = None
|
|
||||||
self.SeedPedestrians = None
|
|
||||||
self.DisableTwoWheeledVehicles = False
|
|
||||||
self.set(**kwargs)
|
|
||||||
self._sensors = []
|
|
||||||
|
|
||||||
def set(self, **kwargs):
|
|
||||||
for key, value in kwargs.items():
|
|
||||||
if not hasattr(self, key):
|
|
||||||
raise ValueError('CarlaSettings: no key named %r' % key)
|
|
||||||
setattr(self, key, value)
|
|
||||||
|
|
||||||
def randomize_seeds(self):
|
|
||||||
"""
|
|
||||||
Randomize the seeds of the new episode's pseudo-random number
|
|
||||||
generators.
|
|
||||||
"""
|
|
||||||
self.SeedVehicles = random.getrandbits(16)
|
|
||||||
self.SeedPedestrians = random.getrandbits(16)
|
|
||||||
|
|
||||||
def randomize_weather(self):
|
|
||||||
"""Randomized the WeatherId."""
|
|
||||||
self.WeatherId = random.randint(0, MAX_NUMBER_OF_WEATHER_IDS)
|
|
||||||
|
|
||||||
def add_sensor(self, sensor):
|
|
||||||
"""Add a sensor to the player vehicle (see sensor.py)."""
|
|
||||||
if not isinstance(sensor, carla_sensor.Sensor):
|
|
||||||
raise ValueError('Sensor not supported')
|
|
||||||
self._sensors.append(sensor)
|
|
||||||
|
|
||||||
def __str__(self):
|
|
||||||
"""Converts this object to an INI formatted string."""
|
|
||||||
ini = ConfigParser()
|
|
||||||
ini.optionxform = str
|
|
||||||
S_SERVER = 'CARLA/Server'
|
|
||||||
S_QUALITY = 'CARLA/QualitySettings'
|
|
||||||
S_LEVEL = 'CARLA/LevelSettings'
|
|
||||||
S_SENSOR = 'CARLA/Sensor'
|
|
||||||
|
|
||||||
def get_attribs(obj):
|
|
||||||
return [a for a in dir(obj) if not a.startswith('_') and not callable(getattr(obj, a))]
|
|
||||||
|
|
||||||
def add_section(section, obj, keys):
|
|
||||||
for key in keys:
|
|
||||||
if hasattr(obj, key) and getattr(obj, key) is not None:
|
|
||||||
if not ini.has_section(section):
|
|
||||||
ini.add_section(section)
|
|
||||||
ini.set(section, key, str(getattr(obj, key)))
|
|
||||||
|
|
||||||
add_section(S_SERVER, self, [
|
|
||||||
'SynchronousMode',
|
|
||||||
'SendNonPlayerAgentsInfo',
|
|
||||||
'DisableRendering'])
|
|
||||||
add_section(S_QUALITY, self, [
|
|
||||||
'QualityLevel'])
|
|
||||||
add_section(S_LEVEL, self, [
|
|
||||||
'NumberOfVehicles',
|
|
||||||
'NumberOfPedestrians',
|
|
||||||
'WeatherId',
|
|
||||||
'SeedVehicles',
|
|
||||||
'SeedPedestrians',
|
|
||||||
'DisableTwoWheeledVehicles'])
|
|
||||||
|
|
||||||
ini.add_section(S_SENSOR)
|
|
||||||
ini.set(S_SENSOR, 'Sensors', ','.join(s.SensorName for s in self._sensors))
|
|
||||||
|
|
||||||
for sensor_def in self._sensors:
|
|
||||||
section = S_SENSOR + '/' + sensor_def.SensorName
|
|
||||||
add_section(section, sensor_def, get_attribs(sensor_def))
|
|
||||||
|
|
||||||
if sys.version_info >= (3, 0):
|
|
||||||
text = io.StringIO()
|
|
||||||
else:
|
|
||||||
text = io.BytesIO()
|
|
||||||
|
|
||||||
ini.write(text)
|
|
||||||
return text.getvalue().replace(' = ', '=')
|
|
|
@ -1,97 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Basic TCP client."""
|
|
||||||
|
|
||||||
import logging
|
|
||||||
import socket
|
|
||||||
import struct
|
|
||||||
import time
|
|
||||||
|
|
||||||
class TCPConnectionError(Exception):
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
class TCPClient(object):
|
|
||||||
"""
|
|
||||||
Basic networking client for TCP connections. Errors occurred during
|
|
||||||
networking operations are raised as TCPConnectionError.
|
|
||||||
|
|
||||||
Received messages are expected to be prepended by a int32 defining the
|
|
||||||
message size. Messages are sent following this convention.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, host, port, timeout):
|
|
||||||
self._host = host
|
|
||||||
self._port = port
|
|
||||||
self._timeout = timeout
|
|
||||||
self._socket = None
|
|
||||||
self._logprefix = '(%s:%s) ' % (self._host, self._port)
|
|
||||||
|
|
||||||
def connect(self, connection_attempts=10):
|
|
||||||
"""Try to establish a connection to the given host:port."""
|
|
||||||
connection_attempts = max(1, connection_attempts)
|
|
||||||
error = None
|
|
||||||
for attempt in range(1, connection_attempts + 1):
|
|
||||||
try:
|
|
||||||
self._socket = socket.create_connection(address=(self._host, self._port), timeout=self._timeout)
|
|
||||||
self._socket.settimeout(self._timeout)
|
|
||||||
logging.debug('%sconnected', self._logprefix)
|
|
||||||
return
|
|
||||||
except socket.error as exception:
|
|
||||||
error = exception
|
|
||||||
logging.debug('%sconnection attempt %d: %s', self._logprefix, attempt, error)
|
|
||||||
time.sleep(1)
|
|
||||||
self._reraise_exception_as_tcp_error('failed to connect', error)
|
|
||||||
|
|
||||||
def disconnect(self):
|
|
||||||
"""Disconnect any active connection."""
|
|
||||||
if self._socket is not None:
|
|
||||||
logging.debug('%sdisconnecting', self._logprefix)
|
|
||||||
self._socket.close()
|
|
||||||
self._socket = None
|
|
||||||
|
|
||||||
def connected(self):
|
|
||||||
"""Return whether there is an active connection."""
|
|
||||||
return self._socket is not None
|
|
||||||
|
|
||||||
def write(self, message):
|
|
||||||
"""Send message to the server."""
|
|
||||||
if self._socket is None:
|
|
||||||
raise TCPConnectionError(self._logprefix + 'not connected')
|
|
||||||
header = struct.pack('<L', len(message))
|
|
||||||
try:
|
|
||||||
self._socket.sendall(header + message)
|
|
||||||
except socket.error as exception:
|
|
||||||
self._reraise_exception_as_tcp_error('failed to write data', exception)
|
|
||||||
|
|
||||||
def read(self):
|
|
||||||
"""Read a message from the server."""
|
|
||||||
header = self._read_n(4)
|
|
||||||
if not header:
|
|
||||||
raise TCPConnectionError(self._logprefix + 'connection closed')
|
|
||||||
length = struct.unpack('<L', header)[0]
|
|
||||||
data = self._read_n(length)
|
|
||||||
return data
|
|
||||||
|
|
||||||
def _read_n(self, length):
|
|
||||||
"""Read n bytes from the socket."""
|
|
||||||
if self._socket is None:
|
|
||||||
raise TCPConnectionError(self._logprefix + 'not connected')
|
|
||||||
buf = bytes()
|
|
||||||
while length > 0:
|
|
||||||
try:
|
|
||||||
data = self._socket.recv(length)
|
|
||||||
except socket.error as exception:
|
|
||||||
self._reraise_exception_as_tcp_error('failed to read data', exception)
|
|
||||||
if not data:
|
|
||||||
raise TCPConnectionError(self._logprefix + 'connection closed')
|
|
||||||
buf += data
|
|
||||||
length -= len(data)
|
|
||||||
return buf
|
|
||||||
|
|
||||||
def _reraise_exception_as_tcp_error(self, message, exception):
|
|
||||||
raise TCPConnectionError('%s%s: %s' % (self._logprefix, message, exception))
|
|
|
@ -1,137 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import math
|
|
||||||
|
|
||||||
from collections import namedtuple
|
|
||||||
|
|
||||||
try:
|
|
||||||
import numpy
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError(
|
|
||||||
'cannot import numpy, make sure numpy package is installed.')
|
|
||||||
|
|
||||||
try:
|
|
||||||
from . import carla_server_pb2 as carla_protocol
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import "carla_server_pb2.py", run '
|
|
||||||
'the protobuf compiler to generate this file')
|
|
||||||
|
|
||||||
|
|
||||||
Translation = namedtuple('Translation', 'x y z')
|
|
||||||
Translation.__new__.__defaults__ = (0.0, 0.0, 0.0)
|
|
||||||
|
|
||||||
Rotation = namedtuple('Rotation', 'pitch yaw roll')
|
|
||||||
Rotation.__new__.__defaults__ = (0.0, 0.0, 0.0)
|
|
||||||
|
|
||||||
Scale = namedtuple('Scale', 'x y z')
|
|
||||||
Scale.__new__.__defaults__ = (1.0, 1.0, 1.0)
|
|
||||||
|
|
||||||
|
|
||||||
class Transform(object):
|
|
||||||
"""A 3D transformation.
|
|
||||||
|
|
||||||
The transformation is applied in the order: scale, rotation, translation.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, *args, **kwargs):
|
|
||||||
if 'matrix' in kwargs:
|
|
||||||
self.matrix = kwargs['matrix']
|
|
||||||
return
|
|
||||||
if isinstance(args[0], carla_protocol.Transform):
|
|
||||||
args = [
|
|
||||||
Translation(
|
|
||||||
args[0].location.x,
|
|
||||||
args[0].location.y,
|
|
||||||
args[0].location.z),
|
|
||||||
Rotation(
|
|
||||||
args[0].rotation.pitch,
|
|
||||||
args[0].rotation.yaw,
|
|
||||||
args[0].rotation.roll)
|
|
||||||
]
|
|
||||||
self.matrix = numpy.matrix(numpy.identity(4))
|
|
||||||
self.set(*args, **kwargs)
|
|
||||||
|
|
||||||
def set(self, *args):
|
|
||||||
"""Builds the transform matrix given a Translate, Rotation
|
|
||||||
and Scale.
|
|
||||||
"""
|
|
||||||
translation = Translation()
|
|
||||||
rotation = Rotation()
|
|
||||||
scale = Scale()
|
|
||||||
|
|
||||||
if len(args) > 3:
|
|
||||||
raise ValueError("'Transform' accepts 3 values as maximum.")
|
|
||||||
|
|
||||||
def get_single_obj_type(obj_type):
|
|
||||||
"""Returns the unique object contained in the
|
|
||||||
arguments lists that is instance of 'obj_type'.
|
|
||||||
"""
|
|
||||||
obj = [x for x in args if isinstance(x, obj_type)]
|
|
||||||
if len(obj) > 1:
|
|
||||||
raise ValueError("Transform only accepts one instances of " +
|
|
||||||
str(obj_type) + " as a parameter")
|
|
||||||
elif not obj:
|
|
||||||
# Create an instance of the type that is 'obj_type'
|
|
||||||
return obj_type()
|
|
||||||
return obj[0]
|
|
||||||
|
|
||||||
translation = get_single_obj_type(Translation)
|
|
||||||
rotation = get_single_obj_type(Rotation)
|
|
||||||
scale = get_single_obj_type(Scale)
|
|
||||||
|
|
||||||
for param in args:
|
|
||||||
if not isinstance(param, Translation) and \
|
|
||||||
not isinstance(param, Rotation) and \
|
|
||||||
not isinstance(param, Scale):
|
|
||||||
raise TypeError(
|
|
||||||
"'" + str(type(param)) + "' type not match with \
|
|
||||||
'Translation', 'Rotation' or 'Scale'")
|
|
||||||
|
|
||||||
# Transformation matrix
|
|
||||||
cy = math.cos(numpy.radians(rotation.yaw))
|
|
||||||
sy = math.sin(numpy.radians(rotation.yaw))
|
|
||||||
cr = math.cos(numpy.radians(rotation.roll))
|
|
||||||
sr = math.sin(numpy.radians(rotation.roll))
|
|
||||||
cp = math.cos(numpy.radians(rotation.pitch))
|
|
||||||
sp = math.sin(numpy.radians(rotation.pitch))
|
|
||||||
self.matrix[0, 3] = translation.x
|
|
||||||
self.matrix[1, 3] = translation.y
|
|
||||||
self.matrix[2, 3] = translation.z
|
|
||||||
self.matrix[0, 0] = scale.x * (cp * cy)
|
|
||||||
self.matrix[0, 1] = scale.y * (cy * sp * sr - sy * cr)
|
|
||||||
self.matrix[0, 2] = -scale.z * (cy * sp * cr + sy * sr)
|
|
||||||
self.matrix[1, 0] = scale.x * (sy * cp)
|
|
||||||
self.matrix[1, 1] = scale.y * (sy * sp * sr + cy * cr)
|
|
||||||
self.matrix[1, 2] = scale.z * (cy * sr - sy * sp * cr)
|
|
||||||
self.matrix[2, 0] = scale.x * (sp)
|
|
||||||
self.matrix[2, 1] = -scale.y * (cp * sr)
|
|
||||||
self.matrix[2, 2] = scale.z * (cp * cr)
|
|
||||||
|
|
||||||
def inverse(self):
|
|
||||||
"""Return the inverse transform."""
|
|
||||||
return Transform(matrix=numpy.linalg.inv(self.matrix))
|
|
||||||
|
|
||||||
def transform_points(self, points):
|
|
||||||
"""
|
|
||||||
Given a 4x4 transformation matrix, transform an array of 3D points.
|
|
||||||
Expected point foramt: [[X0,Y0,Z0],..[Xn,Yn,Zn]]
|
|
||||||
"""
|
|
||||||
# Needed foramt: [[X0,..Xn],[Z0,..Zn],[Z0,..Zn]]. So let's transpose
|
|
||||||
# the point matrix.
|
|
||||||
points = points.transpose()
|
|
||||||
# Add 1s row: [[X0..,Xn],[Y0..,Yn],[Z0..,Zn],[1,..1]]
|
|
||||||
points = numpy.append(points, numpy.ones((1, points.shape[1])), axis=0)
|
|
||||||
# Point transformation
|
|
||||||
points = self.matrix * points
|
|
||||||
# Return all but last row
|
|
||||||
return points[0:3].transpose()
|
|
||||||
|
|
||||||
def __mul__(self, other):
|
|
||||||
return Transform(matrix=numpy.dot(self.matrix, other.matrix))
|
|
||||||
|
|
||||||
def __str__(self):
|
|
||||||
return str(self.matrix)
|
|
|
@ -1,68 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import datetime
|
|
||||||
import sys
|
|
||||||
|
|
||||||
from contextlib import contextmanager
|
|
||||||
|
|
||||||
|
|
||||||
@contextmanager
|
|
||||||
def make_connection(client_type, *args, **kwargs):
|
|
||||||
"""Context manager to create and connect a networking client object."""
|
|
||||||
client = None
|
|
||||||
try:
|
|
||||||
client = client_type(*args, **kwargs)
|
|
||||||
client.connect()
|
|
||||||
yield client
|
|
||||||
finally:
|
|
||||||
if client is not None:
|
|
||||||
client.disconnect()
|
|
||||||
|
|
||||||
|
|
||||||
class StopWatch(object):
|
|
||||||
def __init__(self):
|
|
||||||
self.start = datetime.datetime.now()
|
|
||||||
self.end = None
|
|
||||||
|
|
||||||
def restart(self):
|
|
||||||
self.start = datetime.datetime.now()
|
|
||||||
self.end = None
|
|
||||||
|
|
||||||
def stop(self):
|
|
||||||
self.end = datetime.datetime.now()
|
|
||||||
|
|
||||||
def seconds(self):
|
|
||||||
return (self.end - self.start).total_seconds()
|
|
||||||
|
|
||||||
def milliseconds(self):
|
|
||||||
return 1000.0 * self.seconds()
|
|
||||||
|
|
||||||
|
|
||||||
def to_hex_str(header):
|
|
||||||
return ':'.join('{:02x}'.format(ord(c)) for c in header)
|
|
||||||
|
|
||||||
|
|
||||||
if sys.version_info >= (3, 3):
|
|
||||||
|
|
||||||
import shutil
|
|
||||||
|
|
||||||
def print_over_same_line(text):
|
|
||||||
terminal_width = shutil.get_terminal_size((80, 20)).columns
|
|
||||||
empty_space = max(0, terminal_width - len(text))
|
|
||||||
sys.stdout.write('\r' + text + empty_space * ' ')
|
|
||||||
sys.stdout.flush()
|
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
# Workaround for older Python versions.
|
|
||||||
def print_over_same_line(text):
|
|
||||||
line_length = max(print_over_same_line.last_line_length, len(text))
|
|
||||||
empty_space = max(0, line_length - len(text))
|
|
||||||
sys.stdout.write('\r' + text + empty_space * ' ')
|
|
||||||
sys.stdout.flush()
|
|
||||||
print_over_same_line.last_line_length = line_length
|
|
||||||
print_over_same_line.last_line_length = 0
|
|
|
@ -1,250 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Basic CARLA client example."""
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import random
|
|
||||||
import time
|
|
||||||
|
|
||||||
from carla.client import make_carla_client
|
|
||||||
from carla.sensor import Camera, Lidar
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
from carla.util import print_over_same_line
|
|
||||||
|
|
||||||
|
|
||||||
def run_carla_client(args):
|
|
||||||
# Here we will run 3 episodes with 300 frames each.
|
|
||||||
number_of_episodes = 3
|
|
||||||
frames_per_episode = 300
|
|
||||||
|
|
||||||
# We assume the CARLA server is already waiting for a client to connect at
|
|
||||||
# host:port. To create a connection we can use the `make_carla_client`
|
|
||||||
# context manager, it creates a CARLA client object and starts the
|
|
||||||
# connection. It will throw an exception if something goes wrong. The
|
|
||||||
# context manager makes sure the connection is always cleaned up on exit.
|
|
||||||
with make_carla_client(args.host, args.port) as client:
|
|
||||||
print('CarlaClient connected')
|
|
||||||
|
|
||||||
for episode in range(0, number_of_episodes):
|
|
||||||
# Start a new episode.
|
|
||||||
|
|
||||||
if args.settings_filepath is None:
|
|
||||||
|
|
||||||
# Create a CarlaSettings object. This object is a wrapper around
|
|
||||||
# the CarlaSettings.ini file. Here we set the configuration we
|
|
||||||
# want for the new episode.
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.set(
|
|
||||||
SynchronousMode=True,
|
|
||||||
SendNonPlayerAgentsInfo=True,
|
|
||||||
NumberOfVehicles=20,
|
|
||||||
NumberOfPedestrians=40,
|
|
||||||
WeatherId=random.choice([1, 3, 7, 8, 14]),
|
|
||||||
QualityLevel=args.quality_level)
|
|
||||||
settings.randomize_seeds()
|
|
||||||
|
|
||||||
# Now we want to add a couple of cameras to the player vehicle.
|
|
||||||
# We will collect the images produced by these cameras every
|
|
||||||
# frame.
|
|
||||||
|
|
||||||
# The default camera captures RGB images of the scene.
|
|
||||||
camera0 = Camera('CameraRGB')
|
|
||||||
# Set image resolution in pixels.
|
|
||||||
camera0.set_image_size(800, 600)
|
|
||||||
# Set its position relative to the car in meters.
|
|
||||||
camera0.set_position(0.30, 0, 1.30)
|
|
||||||
settings.add_sensor(camera0)
|
|
||||||
|
|
||||||
# Let's add another camera producing ground-truth depth.
|
|
||||||
camera1 = Camera('CameraDepth', PostProcessing='Depth')
|
|
||||||
camera1.set_image_size(800, 600)
|
|
||||||
camera1.set_position(0.30, 0, 1.30)
|
|
||||||
settings.add_sensor(camera1)
|
|
||||||
|
|
||||||
if args.lidar:
|
|
||||||
lidar = Lidar('Lidar32')
|
|
||||||
lidar.set_position(0, 0, 2.50)
|
|
||||||
lidar.set_rotation(0, 0, 0)
|
|
||||||
lidar.set(
|
|
||||||
Channels=32,
|
|
||||||
Range=50,
|
|
||||||
PointsPerSecond=100000,
|
|
||||||
RotationFrequency=10,
|
|
||||||
UpperFovLimit=10,
|
|
||||||
LowerFovLimit=-30)
|
|
||||||
settings.add_sensor(lidar)
|
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
# Alternatively, we can load these settings from a file.
|
|
||||||
with open(args.settings_filepath, 'r') as fp:
|
|
||||||
settings = fp.read()
|
|
||||||
|
|
||||||
# Now we load these settings into the server. The server replies
|
|
||||||
# with a scene description containing the available start spots for
|
|
||||||
# the player. Here we can provide a CarlaSettings object or a
|
|
||||||
# CarlaSettings.ini file as string.
|
|
||||||
scene = client.load_settings(settings)
|
|
||||||
|
|
||||||
# Choose one player start at random.
|
|
||||||
number_of_player_starts = len(scene.player_start_spots)
|
|
||||||
player_start = random.randint(0, max(0, number_of_player_starts - 1))
|
|
||||||
|
|
||||||
# Notify the server that we want to start the episode at the
|
|
||||||
# player_start index. This function blocks until the server is ready
|
|
||||||
# to start the episode.
|
|
||||||
print('Starting new episode at %r...' % scene.map_name)
|
|
||||||
client.start_episode(player_start)
|
|
||||||
|
|
||||||
# Iterate every frame in the episode.
|
|
||||||
for frame in range(0, frames_per_episode):
|
|
||||||
|
|
||||||
# Read the data produced by the server this frame.
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
|
|
||||||
# Print some of the measurements.
|
|
||||||
print_measurements(measurements)
|
|
||||||
|
|
||||||
# Save the images to disk if requested.
|
|
||||||
if args.save_images_to_disk:
|
|
||||||
for name, measurement in sensor_data.items():
|
|
||||||
filename = args.out_filename_format.format(episode, name, frame)
|
|
||||||
measurement.save_to_disk(filename)
|
|
||||||
|
|
||||||
# We can access the encoded data of a given image as numpy
|
|
||||||
# array using its "data" property. For instance, to get the
|
|
||||||
# depth value (normalized) at pixel X, Y
|
|
||||||
#
|
|
||||||
# depth_array = sensor_data['CameraDepth'].data
|
|
||||||
# value_at_pixel = depth_array[Y, X]
|
|
||||||
#
|
|
||||||
|
|
||||||
# Now we have to send the instructions to control the vehicle.
|
|
||||||
# If we are in synchronous mode the server will pause the
|
|
||||||
# simulation until we send this control.
|
|
||||||
|
|
||||||
if not args.autopilot:
|
|
||||||
|
|
||||||
client.send_control(
|
|
||||||
steer=random.uniform(-1.0, 1.0),
|
|
||||||
throttle=0.5,
|
|
||||||
brake=0.0,
|
|
||||||
hand_brake=False,
|
|
||||||
reverse=False)
|
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
# Together with the measurements, the server has sent the
|
|
||||||
# control that the in-game autopilot would do this frame. We
|
|
||||||
# can enable autopilot by sending back this control to the
|
|
||||||
# server. We can modify it if wanted, here for instance we
|
|
||||||
# will add some noise to the steer.
|
|
||||||
|
|
||||||
control = measurements.player_measurements.autopilot_control
|
|
||||||
control.steer += random.uniform(-0.1, 0.1)
|
|
||||||
client.send_control(control)
|
|
||||||
|
|
||||||
|
|
||||||
def print_measurements(measurements):
|
|
||||||
number_of_agents = len(measurements.non_player_agents)
|
|
||||||
player_measurements = measurements.player_measurements
|
|
||||||
message = 'Vehicle at ({pos_x:.1f}, {pos_y:.1f}), '
|
|
||||||
message += '{speed:.0f} km/h, '
|
|
||||||
message += 'Collision: {{vehicles={col_cars:.0f}, pedestrians={col_ped:.0f}, other={col_other:.0f}}}, '
|
|
||||||
message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road, '
|
|
||||||
message += '({agents_num:d} non-player agents in the scene)'
|
|
||||||
message = message.format(
|
|
||||||
pos_x=player_measurements.transform.location.x,
|
|
||||||
pos_y=player_measurements.transform.location.y,
|
|
||||||
speed=player_measurements.forward_speed * 3.6, # m/s -> km/h
|
|
||||||
col_cars=player_measurements.collision_vehicles,
|
|
||||||
col_ped=player_measurements.collision_pedestrians,
|
|
||||||
col_other=player_measurements.collision_other,
|
|
||||||
other_lane=100 * player_measurements.intersection_otherlane,
|
|
||||||
offroad=100 * player_measurements.intersection_offroad,
|
|
||||||
agents_num=number_of_agents)
|
|
||||||
print_over_same_line(message)
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host',
|
|
||||||
metavar='H',
|
|
||||||
default='localhost',
|
|
||||||
help='IP of the host server (default: localhost)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p', '--port',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-a', '--autopilot',
|
|
||||||
action='store_true',
|
|
||||||
help='enable autopilot')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-l', '--lidar',
|
|
||||||
action='store_true',
|
|
||||||
help='enable Lidar')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-q', '--quality-level',
|
|
||||||
choices=['Low', 'Epic'],
|
|
||||||
type=lambda s: s.title(),
|
|
||||||
default='Epic',
|
|
||||||
help='graphics quality level, a lower level makes the simulation run considerably faster.')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-i', '--images-to-disk',
|
|
||||||
action='store_true',
|
|
||||||
dest='save_images_to_disk',
|
|
||||||
help='save images (and Lidar data if active) to disk')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-c', '--carla-settings',
|
|
||||||
metavar='PATH',
|
|
||||||
dest='settings_filepath',
|
|
||||||
default=None,
|
|
||||||
help='Path to a "CarlaSettings.ini" file')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
args.out_filename_format = '_out/episode_{:0>4d}/{:s}/{:0>6d}'
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
run_carla_client(args)
|
|
||||||
|
|
||||||
print('Done.')
|
|
||||||
return
|
|
||||||
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -1,92 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
|
|
||||||
from carla.driving_benchmark import run_driving_benchmark
|
|
||||||
from carla.driving_benchmark.experiment_suites import CoRL2017
|
|
||||||
from carla.driving_benchmark.experiment_suites import BasicExperimentSuite
|
|
||||||
from carla.agent import ForwardAgent
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='verbose',
|
|
||||||
help='print some extra status information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-db', '--debug',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host',
|
|
||||||
metavar='H',
|
|
||||||
default='localhost',
|
|
||||||
help='IP of the host server (default: localhost)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p', '--port',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-c', '--city-name',
|
|
||||||
metavar='C',
|
|
||||||
default='Town01',
|
|
||||||
help='The town that is going to be used on benchmark'
|
|
||||||
+ '(needs to match active town in server, options: Town01 or Town02)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-n', '--log_name',
|
|
||||||
metavar='T',
|
|
||||||
default='test',
|
|
||||||
help='The name of the log file to be created by the benchmark'
|
|
||||||
)
|
|
||||||
argparser.add_argument(
|
|
||||||
'--corl-2017',
|
|
||||||
action='store_true',
|
|
||||||
help='If you want to benchmark the corl-2017 instead of the Basic one'
|
|
||||||
)
|
|
||||||
argparser.add_argument(
|
|
||||||
'--continue-experiment',
|
|
||||||
action='store_true',
|
|
||||||
help='If you want to continue the experiment with the same name'
|
|
||||||
)
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
if args.debug:
|
|
||||||
log_level = logging.DEBUG
|
|
||||||
elif args.verbose:
|
|
||||||
log_level = logging.INFO
|
|
||||||
else:
|
|
||||||
log_level = logging.WARNING
|
|
||||||
|
|
||||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)
|
|
||||||
logging.info('listening to server %s:%s', args.host, args.port)
|
|
||||||
|
|
||||||
# We instantiate a forward agent, a simple policy that just set
|
|
||||||
# acceleration as 0.9 and steering as zero
|
|
||||||
agent = ForwardAgent()
|
|
||||||
|
|
||||||
# We instantiate an experiment suite. Basically a set of experiments
|
|
||||||
# that are going to be evaluated on this benchmark.
|
|
||||||
if args.corl_2017:
|
|
||||||
experiment_suite = CoRL2017(args.city_name)
|
|
||||||
else:
|
|
||||||
print (' WARNING: running the basic driving benchmark, to run for CoRL 2017'
|
|
||||||
' experiment suites, you should run'
|
|
||||||
' python driving_benchmark_example.py --corl-2017')
|
|
||||||
experiment_suite = BasicExperimentSuite(args.city_name)
|
|
||||||
|
|
||||||
# Now actually run the driving_benchmark
|
|
||||||
run_driving_benchmark(agent, experiment_suite, args.city_name,
|
|
||||||
args.log_name, args.continue_experiment,
|
|
||||||
args.host, args.port)
|
|
|
@ -1,441 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
# Keyboard controlling for CARLA. Please refer to client_example.py for a simpler
|
|
||||||
# and more documented example.
|
|
||||||
|
|
||||||
"""
|
|
||||||
Welcome to CARLA manual control.
|
|
||||||
|
|
||||||
Use ARROWS or WASD keys for control.
|
|
||||||
|
|
||||||
W : throttle
|
|
||||||
S : brake
|
|
||||||
AD : steer
|
|
||||||
Q : toggle reverse
|
|
||||||
Space : hand-brake
|
|
||||||
P : toggle autopilot
|
|
||||||
|
|
||||||
R : restart level
|
|
||||||
|
|
||||||
STARTING in a moment...
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import random
|
|
||||||
import time
|
|
||||||
|
|
||||||
try:
|
|
||||||
import pygame
|
|
||||||
from pygame.locals import K_DOWN
|
|
||||||
from pygame.locals import K_LEFT
|
|
||||||
from pygame.locals import K_RIGHT
|
|
||||||
from pygame.locals import K_SPACE
|
|
||||||
from pygame.locals import K_UP
|
|
||||||
from pygame.locals import K_a
|
|
||||||
from pygame.locals import K_d
|
|
||||||
from pygame.locals import K_p
|
|
||||||
from pygame.locals import K_q
|
|
||||||
from pygame.locals import K_r
|
|
||||||
from pygame.locals import K_s
|
|
||||||
from pygame.locals import K_w
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import pygame, make sure pygame package is installed')
|
|
||||||
|
|
||||||
try:
|
|
||||||
import numpy as np
|
|
||||||
except ImportError:
|
|
||||||
raise RuntimeError('cannot import numpy, make sure numpy package is installed')
|
|
||||||
|
|
||||||
from carla import image_converter
|
|
||||||
from carla import sensor
|
|
||||||
from carla.client import make_carla_client, VehicleControl
|
|
||||||
from carla.planner.map import CarlaMap
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
from carla.util import print_over_same_line
|
|
||||||
|
|
||||||
|
|
||||||
WINDOW_WIDTH = 800
|
|
||||||
WINDOW_HEIGHT = 600
|
|
||||||
MINI_WINDOW_WIDTH = 320
|
|
||||||
MINI_WINDOW_HEIGHT = 180
|
|
||||||
|
|
||||||
|
|
||||||
def make_carla_settings(args):
|
|
||||||
"""Make a CarlaSettings object with the settings we need."""
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.set(
|
|
||||||
SynchronousMode=False,
|
|
||||||
SendNonPlayerAgentsInfo=True,
|
|
||||||
NumberOfVehicles=15,
|
|
||||||
NumberOfPedestrians=30,
|
|
||||||
WeatherId=random.choice([1, 3, 7, 8, 14]),
|
|
||||||
QualityLevel=args.quality_level)
|
|
||||||
settings.randomize_seeds()
|
|
||||||
camera0 = sensor.Camera('CameraRGB')
|
|
||||||
camera0.set_image_size(WINDOW_WIDTH, WINDOW_HEIGHT)
|
|
||||||
camera0.set_position(2.0, 0.0, 1.4)
|
|
||||||
camera0.set_rotation(0.0, 0.0, 0.0)
|
|
||||||
settings.add_sensor(camera0)
|
|
||||||
camera1 = sensor.Camera('CameraDepth', PostProcessing='Depth')
|
|
||||||
camera1.set_image_size(MINI_WINDOW_WIDTH, MINI_WINDOW_HEIGHT)
|
|
||||||
camera1.set_position(2.0, 0.0, 1.4)
|
|
||||||
camera1.set_rotation(0.0, 0.0, 0.0)
|
|
||||||
settings.add_sensor(camera1)
|
|
||||||
camera2 = sensor.Camera('CameraSemSeg', PostProcessing='SemanticSegmentation')
|
|
||||||
camera2.set_image_size(MINI_WINDOW_WIDTH, MINI_WINDOW_HEIGHT)
|
|
||||||
camera2.set_position(2.0, 0.0, 1.4)
|
|
||||||
camera2.set_rotation(0.0, 0.0, 0.0)
|
|
||||||
settings.add_sensor(camera2)
|
|
||||||
if args.lidar:
|
|
||||||
lidar = sensor.Lidar('Lidar32')
|
|
||||||
lidar.set_position(0, 0, 2.5)
|
|
||||||
lidar.set_rotation(0, 0, 0)
|
|
||||||
lidar.set(
|
|
||||||
Channels=32,
|
|
||||||
Range=50,
|
|
||||||
PointsPerSecond=100000,
|
|
||||||
RotationFrequency=10,
|
|
||||||
UpperFovLimit=10,
|
|
||||||
LowerFovLimit=-30)
|
|
||||||
settings.add_sensor(lidar)
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
class Timer(object):
|
|
||||||
def __init__(self):
|
|
||||||
self.step = 0
|
|
||||||
self._lap_step = 0
|
|
||||||
self._lap_time = time.time()
|
|
||||||
|
|
||||||
def tick(self):
|
|
||||||
self.step += 1
|
|
||||||
|
|
||||||
def lap(self):
|
|
||||||
self._lap_step = self.step
|
|
||||||
self._lap_time = time.time()
|
|
||||||
|
|
||||||
def ticks_per_second(self):
|
|
||||||
return float(self.step - self._lap_step) / self.elapsed_seconds_since_lap()
|
|
||||||
|
|
||||||
def elapsed_seconds_since_lap(self):
|
|
||||||
return time.time() - self._lap_time
|
|
||||||
|
|
||||||
|
|
||||||
class CarlaGame(object):
|
|
||||||
def __init__(self, carla_client, args):
|
|
||||||
self.client = carla_client
|
|
||||||
self._carla_settings = make_carla_settings(args)
|
|
||||||
self._timer = None
|
|
||||||
self._display = None
|
|
||||||
self._main_image = None
|
|
||||||
self._mini_view_image1 = None
|
|
||||||
self._mini_view_image2 = None
|
|
||||||
self._enable_autopilot = args.autopilot
|
|
||||||
self._lidar_measurement = None
|
|
||||||
self._map_view = None
|
|
||||||
self._is_on_reverse = False
|
|
||||||
self._display_map = args.map
|
|
||||||
self._city_name = None
|
|
||||||
self._map = None
|
|
||||||
self._map_shape = None
|
|
||||||
self._map_view = None
|
|
||||||
self._position = None
|
|
||||||
self._agent_positions = None
|
|
||||||
|
|
||||||
def execute(self):
|
|
||||||
"""Launch the PyGame."""
|
|
||||||
pygame.init()
|
|
||||||
self._initialize_game()
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
for event in pygame.event.get():
|
|
||||||
if event.type == pygame.QUIT:
|
|
||||||
return
|
|
||||||
self._on_loop()
|
|
||||||
self._on_render()
|
|
||||||
finally:
|
|
||||||
pygame.quit()
|
|
||||||
|
|
||||||
def _initialize_game(self):
|
|
||||||
self._on_new_episode()
|
|
||||||
|
|
||||||
if self._city_name is not None:
|
|
||||||
self._map = CarlaMap(self._city_name)
|
|
||||||
self._map_shape = self._map.map_image.shape
|
|
||||||
self._map_view = self._map.get_map(WINDOW_HEIGHT)
|
|
||||||
|
|
||||||
extra_width = int((WINDOW_HEIGHT/float(self._map_shape[0]))*self._map_shape[1])
|
|
||||||
self._display = pygame.display.set_mode(
|
|
||||||
(WINDOW_WIDTH + extra_width, WINDOW_HEIGHT),
|
|
||||||
pygame.HWSURFACE | pygame.DOUBLEBUF)
|
|
||||||
else:
|
|
||||||
self._display = pygame.display.set_mode(
|
|
||||||
(WINDOW_WIDTH, WINDOW_HEIGHT),
|
|
||||||
pygame.HWSURFACE | pygame.DOUBLEBUF)
|
|
||||||
|
|
||||||
logging.debug('pygame started')
|
|
||||||
|
|
||||||
def _on_new_episode(self):
|
|
||||||
self._carla_settings.randomize_seeds()
|
|
||||||
self._carla_settings.randomize_weather()
|
|
||||||
scene = self.client.load_settings(self._carla_settings)
|
|
||||||
if self._display_map:
|
|
||||||
self._city_name = scene.map_name
|
|
||||||
number_of_player_starts = len(scene.player_start_spots)
|
|
||||||
player_start = np.random.randint(number_of_player_starts)
|
|
||||||
print('Starting new episode...')
|
|
||||||
self.client.start_episode(player_start)
|
|
||||||
self._timer = Timer()
|
|
||||||
self._is_on_reverse = False
|
|
||||||
|
|
||||||
def _on_loop(self):
|
|
||||||
self._timer.tick()
|
|
||||||
|
|
||||||
measurements, sensor_data = self.client.read_data()
|
|
||||||
|
|
||||||
self._main_image = sensor_data.get('CameraRGB', None)
|
|
||||||
self._mini_view_image1 = sensor_data.get('CameraDepth', None)
|
|
||||||
self._mini_view_image2 = sensor_data.get('CameraSemSeg', None)
|
|
||||||
self._lidar_measurement = sensor_data.get('Lidar32', None)
|
|
||||||
|
|
||||||
# Print measurements every second.
|
|
||||||
if self._timer.elapsed_seconds_since_lap() > 1.0:
|
|
||||||
if self._city_name is not None:
|
|
||||||
# Function to get car position on map.
|
|
||||||
map_position = self._map.convert_to_pixel([
|
|
||||||
measurements.player_measurements.transform.location.x,
|
|
||||||
measurements.player_measurements.transform.location.y,
|
|
||||||
measurements.player_measurements.transform.location.z])
|
|
||||||
# Function to get orientation of the road car is in.
|
|
||||||
lane_orientation = self._map.get_lane_orientation([
|
|
||||||
measurements.player_measurements.transform.location.x,
|
|
||||||
measurements.player_measurements.transform.location.y,
|
|
||||||
measurements.player_measurements.transform.location.z])
|
|
||||||
|
|
||||||
self._print_player_measurements_map(
|
|
||||||
measurements.player_measurements,
|
|
||||||
map_position,
|
|
||||||
lane_orientation)
|
|
||||||
else:
|
|
||||||
self._print_player_measurements(measurements.player_measurements)
|
|
||||||
|
|
||||||
# Plot position on the map as well.
|
|
||||||
|
|
||||||
self._timer.lap()
|
|
||||||
|
|
||||||
control = self._get_keyboard_control(pygame.key.get_pressed())
|
|
||||||
# Set the player position
|
|
||||||
if self._city_name is not None:
|
|
||||||
self._position = self._map.convert_to_pixel([
|
|
||||||
measurements.player_measurements.transform.location.x,
|
|
||||||
measurements.player_measurements.transform.location.y,
|
|
||||||
measurements.player_measurements.transform.location.z])
|
|
||||||
self._agent_positions = measurements.non_player_agents
|
|
||||||
|
|
||||||
if control is None:
|
|
||||||
self._on_new_episode()
|
|
||||||
elif self._enable_autopilot:
|
|
||||||
self.client.send_control(measurements.player_measurements.autopilot_control)
|
|
||||||
else:
|
|
||||||
self.client.send_control(control)
|
|
||||||
|
|
||||||
def _get_keyboard_control(self, keys):
|
|
||||||
"""
|
|
||||||
Return a VehicleControl message based on the pressed keys. Return None
|
|
||||||
if a new episode was requested.
|
|
||||||
"""
|
|
||||||
if keys[K_r]:
|
|
||||||
return None
|
|
||||||
control = VehicleControl()
|
|
||||||
if keys[K_LEFT] or keys[K_a]:
|
|
||||||
control.steer = -1.0
|
|
||||||
if keys[K_RIGHT] or keys[K_d]:
|
|
||||||
control.steer = 1.0
|
|
||||||
if keys[K_UP] or keys[K_w]:
|
|
||||||
control.throttle = 1.0
|
|
||||||
if keys[K_DOWN] or keys[K_s]:
|
|
||||||
control.brake = 1.0
|
|
||||||
if keys[K_SPACE]:
|
|
||||||
control.hand_brake = True
|
|
||||||
if keys[K_q]:
|
|
||||||
self._is_on_reverse = not self._is_on_reverse
|
|
||||||
if keys[K_p]:
|
|
||||||
self._enable_autopilot = not self._enable_autopilot
|
|
||||||
control.reverse = self._is_on_reverse
|
|
||||||
return control
|
|
||||||
|
|
||||||
def _print_player_measurements_map(
|
|
||||||
self,
|
|
||||||
player_measurements,
|
|
||||||
map_position,
|
|
||||||
lane_orientation):
|
|
||||||
message = 'Step {step} ({fps:.1f} FPS): '
|
|
||||||
message += 'Map Position ({map_x:.1f},{map_y:.1f}) '
|
|
||||||
message += 'Lane Orientation ({ori_x:.1f},{ori_y:.1f}) '
|
|
||||||
message += '{speed:.2f} km/h, '
|
|
||||||
message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road'
|
|
||||||
message = message.format(
|
|
||||||
map_x=map_position[0],
|
|
||||||
map_y=map_position[1],
|
|
||||||
ori_x=lane_orientation[0],
|
|
||||||
ori_y=lane_orientation[1],
|
|
||||||
step=self._timer.step,
|
|
||||||
fps=self._timer.ticks_per_second(),
|
|
||||||
speed=player_measurements.forward_speed * 3.6,
|
|
||||||
other_lane=100 * player_measurements.intersection_otherlane,
|
|
||||||
offroad=100 * player_measurements.intersection_offroad)
|
|
||||||
print_over_same_line(message)
|
|
||||||
|
|
||||||
def _print_player_measurements(self, player_measurements):
|
|
||||||
message = 'Step {step} ({fps:.1f} FPS): '
|
|
||||||
message += '{speed:.2f} km/h, '
|
|
||||||
message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road'
|
|
||||||
message = message.format(
|
|
||||||
step=self._timer.step,
|
|
||||||
fps=self._timer.ticks_per_second(),
|
|
||||||
speed=player_measurements.forward_speed * 3.6,
|
|
||||||
other_lane=100 * player_measurements.intersection_otherlane,
|
|
||||||
offroad=100 * player_measurements.intersection_offroad)
|
|
||||||
print_over_same_line(message)
|
|
||||||
|
|
||||||
def _on_render(self):
|
|
||||||
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
|
|
||||||
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
|
|
||||||
|
|
||||||
if self._main_image is not None:
|
|
||||||
array = image_converter.to_rgb_array(self._main_image)
|
|
||||||
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
|
|
||||||
self._display.blit(surface, (0, 0))
|
|
||||||
|
|
||||||
if self._mini_view_image1 is not None:
|
|
||||||
array = image_converter.depth_to_logarithmic_grayscale(self._mini_view_image1)
|
|
||||||
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
|
|
||||||
self._display.blit(surface, (gap_x, mini_image_y))
|
|
||||||
|
|
||||||
if self._mini_view_image2 is not None:
|
|
||||||
array = image_converter.labels_to_cityscapes_palette(
|
|
||||||
self._mini_view_image2)
|
|
||||||
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
|
|
||||||
|
|
||||||
self._display.blit(
|
|
||||||
surface, (2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
|
|
||||||
|
|
||||||
if self._lidar_measurement is not None:
|
|
||||||
lidar_data = np.array(self._lidar_measurement.data[:, :2])
|
|
||||||
lidar_data *= 2.0
|
|
||||||
lidar_data += 100.0
|
|
||||||
lidar_data = np.fabs(lidar_data)
|
|
||||||
lidar_data = lidar_data.astype(np.int32)
|
|
||||||
lidar_data = np.reshape(lidar_data, (-1, 2))
|
|
||||||
#draw lidar
|
|
||||||
lidar_img_size = (200, 200, 3)
|
|
||||||
lidar_img = np.zeros(lidar_img_size)
|
|
||||||
lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
|
|
||||||
surface = pygame.surfarray.make_surface(lidar_img)
|
|
||||||
self._display.blit(surface, (10, 10))
|
|
||||||
|
|
||||||
if self._map_view is not None:
|
|
||||||
array = self._map_view
|
|
||||||
array = array[:, :, :3]
|
|
||||||
|
|
||||||
new_window_width = \
|
|
||||||
(float(WINDOW_HEIGHT) / float(self._map_shape[0])) * \
|
|
||||||
float(self._map_shape[1])
|
|
||||||
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
|
|
||||||
|
|
||||||
w_pos = int(self._position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0])))
|
|
||||||
h_pos = int(self._position[1] *(new_window_width/float(self._map_shape[1])))
|
|
||||||
|
|
||||||
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0)
|
|
||||||
for agent in self._agent_positions:
|
|
||||||
if agent.HasField('vehicle'):
|
|
||||||
agent_position = self._map.convert_to_pixel([
|
|
||||||
agent.vehicle.transform.location.x,
|
|
||||||
agent.vehicle.transform.location.y,
|
|
||||||
agent.vehicle.transform.location.z])
|
|
||||||
|
|
||||||
w_pos = int(agent_position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0])))
|
|
||||||
h_pos = int(agent_position[1] *(new_window_width/float(self._map_shape[1])))
|
|
||||||
|
|
||||||
pygame.draw.circle(surface, [255, 0, 255, 255], (w_pos, h_pos), 4, 0)
|
|
||||||
|
|
||||||
self._display.blit(surface, (WINDOW_WIDTH, 0))
|
|
||||||
|
|
||||||
pygame.display.flip()
|
|
||||||
|
|
||||||
|
|
||||||
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='localhost',
|
|
||||||
help='IP of the host server (default: localhost)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p', '--port',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-a', '--autopilot',
|
|
||||||
action='store_true',
|
|
||||||
help='enable autopilot')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-l', '--lidar',
|
|
||||||
action='store_true',
|
|
||||||
help='enable Lidar')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-q', '--quality-level',
|
|
||||||
choices=['Low', 'Epic'],
|
|
||||||
type=lambda s: s.title(),
|
|
||||||
default='Epic',
|
|
||||||
help='graphics quality level, a lower level makes the simulation run considerably faster')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-m', '--map',
|
|
||||||
action='store_true',
|
|
||||||
help='plot the map of the current city')
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
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__)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
with make_carla_client(args.host, args.port) as client:
|
|
||||||
game = CarlaGame(client, args)
|
|
||||||
game.execute()
|
|
||||||
break
|
|
||||||
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -1,191 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB), and the INTEL Visual Computing Lab.
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Basic CARLA client to generate point cloud in PLY format that you
|
|
||||||
can visualize with MeshLab (meshlab.net) for instance. Please
|
|
||||||
refer to client_example.py for a simpler and more documented example."""
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import os
|
|
||||||
import random
|
|
||||||
import time
|
|
||||||
|
|
||||||
from carla.client import make_carla_client
|
|
||||||
from carla.sensor import Camera
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
from carla.util import print_over_same_line, StopWatch
|
|
||||||
from carla.image_converter import depth_to_local_point_cloud, to_rgb_array
|
|
||||||
from carla.transform import Transform
|
|
||||||
|
|
||||||
|
|
||||||
def run_carla_client(host, port, far):
|
|
||||||
# Here we will run a single episode with 300 frames.
|
|
||||||
number_of_frames = 3000
|
|
||||||
frame_step = 100 # Save one image every 100 frames
|
|
||||||
output_folder = '_out'
|
|
||||||
image_size = [800, 600]
|
|
||||||
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
|
|
||||||
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
|
|
||||||
fov = 70
|
|
||||||
|
|
||||||
# Connect with the server
|
|
||||||
with make_carla_client(host, port) as client:
|
|
||||||
print('CarlaClient connected')
|
|
||||||
|
|
||||||
# Here we load the settings.
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.set(
|
|
||||||
SynchronousMode=True,
|
|
||||||
SendNonPlayerAgentsInfo=False,
|
|
||||||
NumberOfVehicles=20,
|
|
||||||
NumberOfPedestrians=40,
|
|
||||||
WeatherId=random.choice([1, 3, 7, 8, 14]))
|
|
||||||
settings.randomize_seeds()
|
|
||||||
|
|
||||||
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
|
|
||||||
camera1.set_image_size(*image_size)
|
|
||||||
camera1.set_position(*camera_local_pos)
|
|
||||||
camera1.set_rotation(*camera_local_rotation)
|
|
||||||
settings.add_sensor(camera1)
|
|
||||||
|
|
||||||
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
|
|
||||||
camera2.set_image_size(*image_size)
|
|
||||||
camera2.set_position(*camera_local_pos)
|
|
||||||
camera2.set_rotation(*camera_local_rotation)
|
|
||||||
settings.add_sensor(camera2)
|
|
||||||
|
|
||||||
client.load_settings(settings)
|
|
||||||
|
|
||||||
# Start at location index id '0'
|
|
||||||
client.start_episode(0)
|
|
||||||
|
|
||||||
# Compute the camera transform matrix
|
|
||||||
camera_to_car_transform = camera2.get_unreal_transform()
|
|
||||||
|
|
||||||
# Iterate every frame in the episode except for the first one.
|
|
||||||
for frame in range(1, number_of_frames):
|
|
||||||
# Read the data produced by the server this frame.
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
|
|
||||||
# Save one image every 'frame_step' frames
|
|
||||||
if not frame % frame_step:
|
|
||||||
# Start transformations time mesure.
|
|
||||||
timer = StopWatch()
|
|
||||||
|
|
||||||
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
|
|
||||||
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
|
|
||||||
|
|
||||||
# 2d to (camera) local 3d
|
|
||||||
# We use the image_RGB to colorize each 3D point, this is optional.
|
|
||||||
# "max_depth" is used to keep only the points that are near to the
|
|
||||||
# camera, meaning 1.0 the farest points (sky)
|
|
||||||
point_cloud = depth_to_local_point_cloud(
|
|
||||||
sensor_data['CameraDepth'],
|
|
||||||
image_RGB,
|
|
||||||
max_depth=far
|
|
||||||
)
|
|
||||||
|
|
||||||
# (Camera) local 3d to world 3d.
|
|
||||||
# Get the transform from the player protobuf transformation.
|
|
||||||
world_transform = Transform(
|
|
||||||
measurements.player_measurements.transform
|
|
||||||
)
|
|
||||||
|
|
||||||
# Compute the final transformation matrix.
|
|
||||||
car_to_world_transform = world_transform * camera_to_car_transform
|
|
||||||
|
|
||||||
# Car to World transformation given the 3D points and the
|
|
||||||
# transformation matrix.
|
|
||||||
point_cloud.apply_transform(car_to_world_transform)
|
|
||||||
|
|
||||||
# End transformations time mesure.
|
|
||||||
timer.stop()
|
|
||||||
|
|
||||||
# Save PLY to disk
|
|
||||||
# This generates the PLY string with the 3D points and the RGB colors
|
|
||||||
# for each row of the file.
|
|
||||||
point_cloud.save_to_disk(os.path.join(
|
|
||||||
output_folder, '{:0>5}.ply'.format(frame))
|
|
||||||
)
|
|
||||||
|
|
||||||
print_message(timer.milliseconds(), len(point_cloud), frame)
|
|
||||||
|
|
||||||
client.send_control(
|
|
||||||
measurements.player_measurements.autopilot_control
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def print_message(elapsed_time, point_n, frame):
|
|
||||||
message = ' '.join([
|
|
||||||
'Transformations took {:>3.0f} ms.',
|
|
||||||
'Saved {:>6} points to "{:0>5}.ply".'
|
|
||||||
]).format(elapsed_time, point_n, frame)
|
|
||||||
print_over_same_line(message)
|
|
||||||
|
|
||||||
|
|
||||||
def check_far(value):
|
|
||||||
fvalue = float(value)
|
|
||||||
if fvalue < 0.0 or fvalue > 1.0:
|
|
||||||
raise argparse.ArgumentTypeError(
|
|
||||||
"{} must be a float between 0.0 and 1.0")
|
|
||||||
return fvalue
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host',
|
|
||||||
metavar='H',
|
|
||||||
default='localhost',
|
|
||||||
help='IP of the host server (default: localhost)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p', '--port',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-f', '--far',
|
|
||||||
default=0.2,
|
|
||||||
type=check_far,
|
|
||||||
help='The maximum save distance of camera-point '
|
|
||||||
'[0.0 (near), 1.0 (far)] (default: 0.2)')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
run_carla_client(host=args.host, port=args.port, far=args.far)
|
|
||||||
print('\nDone!')
|
|
||||||
return
|
|
||||||
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nClient stoped by user.')
|
|
|
@ -1,6 +0,0 @@
|
||||||
Pillow
|
|
||||||
numpy
|
|
||||||
protobuf
|
|
||||||
pygame
|
|
||||||
matplotlib
|
|
||||||
future
|
|
|
@ -1,16 +0,0 @@
|
||||||
from setuptools import setup
|
|
||||||
|
|
||||||
# @todo Dependencies are missing.
|
|
||||||
|
|
||||||
setup(
|
|
||||||
name='carla_client',
|
|
||||||
version='0.8.4',
|
|
||||||
packages=['carla', 'carla.driving_benchmark', 'carla.agent',
|
|
||||||
'carla.driving_benchmark.experiment_suites', 'carla.planner'],
|
|
||||||
license='MIT License',
|
|
||||||
description='Python API for communicating with the CARLA server.',
|
|
||||||
url='https://github.com/carla-simulator/carla',
|
|
||||||
author='The CARLA team',
|
|
||||||
author_email='carla.simulator@gmail.com',
|
|
||||||
include_package_data=True
|
|
||||||
)
|
|
|
@ -1,242 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""A client for benchmarking the CARLA server."""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import os
|
|
||||||
import random
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
|
|
||||||
sys.path.append(os.path.join(os.path.dirname(__file__), '..'))
|
|
||||||
|
|
||||||
import carla
|
|
||||||
|
|
||||||
from carla import sensor
|
|
||||||
from carla.client import make_carla_client
|
|
||||||
from carla.sensor import Camera
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.util import StopWatch
|
|
||||||
|
|
||||||
TEXT = \
|
|
||||||
"""===========================
|
|
||||||
Annotated {count:d} frames.
|
|
||||||
---------------------------
|
|
||||||
average = {avg:.2f} FPS
|
|
||||||
maximum = {max:.2f} FPS
|
|
||||||
minimum = {min:.2f} FPS
|
|
||||||
===========================
|
|
||||||
"""
|
|
||||||
|
|
||||||
def make_base_settings():
|
|
||||||
return CarlaSettings(
|
|
||||||
WeatherId=1,
|
|
||||||
SendNonPlayerAgentsInfo=False,
|
|
||||||
SynchronousMode=False,
|
|
||||||
NumberOfVehicles=20,
|
|
||||||
NumberOfPedestrians=30,
|
|
||||||
SeedVehicles=123456789,
|
|
||||||
SeedPedestrians=123456789,
|
|
||||||
QualityLevel='Epic')
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_001():
|
|
||||||
logging.info('Scenario 001: no sensors')
|
|
||||||
return make_base_settings()
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_002():
|
|
||||||
logging.info('Scenario 002: no sensors, no agents at all')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.set(NumberOfVehicles=0, NumberOfPedestrians=0)
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_003():
|
|
||||||
logging.info('Scenario 003: no sensors, no pedestrians')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.set(NumberOfPedestrians=0)
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_004():
|
|
||||||
logging.info('Scenario 004: no sensors, no vehicles')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.set(NumberOfVehicles=0)
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_005():
|
|
||||||
logging.info('Scenario 005: no sensors, hard rain')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.set(WeatherId=13)
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_006():
|
|
||||||
logging.info('Scenario 006: no sensors, sending agents info')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.set(SendNonPlayerAgentsInfo=True)
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_007():
|
|
||||||
logging.info('Scenario 007: single camera RGB')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.add_sensor(Camera('DefaultRGBCamera'))
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_008():
|
|
||||||
logging.info('Scenario 008: single camera Depth')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.add_sensor(Camera('DefaultDepthCamera', PostProcessing='Depth'))
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_009():
|
|
||||||
logging.info('Scenario 009: single camera SemanticSegmentation')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.add_sensor(Camera('DefaultSemSegCamera', PostProcessing='SemanticSegmentation'))
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
def generate_settings_scenario_010():
|
|
||||||
logging.info('Scenario 010: 3 cameras')
|
|
||||||
settings = make_base_settings()
|
|
||||||
settings.add_sensor(Camera('DefaultRGBCamera'))
|
|
||||||
settings.add_sensor(Camera('DefaultDepthCamera', PostProcessing='Depth'))
|
|
||||||
settings.add_sensor(Camera('DefaultSemSegCamera', PostProcessing='SemanticSegmentation'))
|
|
||||||
return settings
|
|
||||||
|
|
||||||
|
|
||||||
class FPSWatch(object):
|
|
||||||
def __init__(self):
|
|
||||||
self.stop_watch = StopWatch()
|
|
||||||
self.sum = 0.0
|
|
||||||
self.count = 0
|
|
||||||
self.max = 0.0
|
|
||||||
self.min = float("inf")
|
|
||||||
|
|
||||||
def annotate(self):
|
|
||||||
self.stop_watch.stop()
|
|
||||||
fps = 1.0 / self.stop_watch.seconds()
|
|
||||||
self.sum += fps
|
|
||||||
self.count += 1
|
|
||||||
self.max = max(self.max, fps)
|
|
||||||
self.min = min(self.min, fps)
|
|
||||||
self.stop_watch.restart()
|
|
||||||
|
|
||||||
def __str__(self):
|
|
||||||
return TEXT.format(
|
|
||||||
count=self.count,
|
|
||||||
avg=self.sum/self.count,
|
|
||||||
max=self.max,
|
|
||||||
min=self.min)
|
|
||||||
|
|
||||||
|
|
||||||
def run_carla_client(args, settings_generators):
|
|
||||||
with make_carla_client(args.host, args.port, timeout=25) as client:
|
|
||||||
for settings_generator in settings_generators:
|
|
||||||
scene = client.load_settings(settings_generator())
|
|
||||||
client.start_episode(0)
|
|
||||||
watch = FPSWatch()
|
|
||||||
for frame in range(0, 3000):
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
client.send_control(measurements.player_measurements.autopilot_control)
|
|
||||||
watch.annotate()
|
|
||||||
print(str(watch))
|
|
||||||
print('done.')
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--log',
|
|
||||||
metavar='LOG_FILE',
|
|
||||||
default=None,
|
|
||||||
help='print output to file')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host',
|
|
||||||
metavar='H',
|
|
||||||
default='localhost',
|
|
||||||
help='IP of the host server (default: localhost)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p', '--port',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-s', '--scenario',
|
|
||||||
metavar='N',
|
|
||||||
default=-1,
|
|
||||||
type=int,
|
|
||||||
help='benchmark scenario to use')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-l', '--list',
|
|
||||||
action='store_true',
|
|
||||||
help='list available benchmark scenarios')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
logging_config = {
|
|
||||||
'format': '%(levelname)s: %(message)s',
|
|
||||||
'level': logging.DEBUG if args.debug else logging.INFO
|
|
||||||
}
|
|
||||||
if args.log:
|
|
||||||
logging_config['filename'] = args.log
|
|
||||||
logging_config['filemode'] = 'w+'
|
|
||||||
logging.basicConfig(**logging_config)
|
|
||||||
|
|
||||||
self_module = sys.modules[__name__]
|
|
||||||
generators = sorted(x for x in dir(self_module) if x.startswith('generate_settings_scenario_'))
|
|
||||||
|
|
||||||
if args.list:
|
|
||||||
for generator_name in generators:
|
|
||||||
getattr(self_module, generator_name)()
|
|
||||||
return
|
|
||||||
|
|
||||||
if args.scenario == -1:
|
|
||||||
generators_to_use = generators
|
|
||||||
elif args.scenario < 1 or args.scenario > len(generators):
|
|
||||||
logging.error('invalid scenario %d', args.scenario)
|
|
||||||
sys.exit(1)
|
|
||||||
else:
|
|
||||||
generators_to_use = [generators[args.scenario - 1]]
|
|
||||||
|
|
||||||
settings_generators = [getattr(self_module, x) for x in generators_to_use]
|
|
||||||
|
|
||||||
logging.info('listening to server %s:%s', args.host, args.port)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
run_carla_client(args, settings_generators)
|
|
||||||
return
|
|
||||||
|
|
||||||
except AssertionError as assertion:
|
|
||||||
raise assertion
|
|
||||||
except Exception as exception:
|
|
||||||
logging.error('exception: %s', exception)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -1,180 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""CARLA client console"""
|
|
||||||
|
|
||||||
|
|
||||||
import cmd
|
|
||||||
import logging
|
|
||||||
import subprocess
|
|
||||||
import tempfile
|
|
||||||
import threading
|
|
||||||
import time
|
|
||||||
|
|
||||||
|
|
||||||
from carla.client import CarlaClient
|
|
||||||
from carla.sensor import Camera, Image
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
|
|
||||||
|
|
||||||
class _Control(object):
|
|
||||||
def __init__(self):
|
|
||||||
self.c_throttle = 0.0
|
|
||||||
self.c_steer = 0.0
|
|
||||||
self.c_brake = 0.0
|
|
||||||
self.c_hand_brake = False
|
|
||||||
self.c_reverse = False
|
|
||||||
|
|
||||||
def action_list(self):
|
|
||||||
return [x[2:] for x in dir(self) if x.startswith('c_')]
|
|
||||||
|
|
||||||
def kwargs(self):
|
|
||||||
return dict((x[2:], getattr(self, x)) for x in dir(self) if x.startswith('c_'))
|
|
||||||
|
|
||||||
def set(self, line):
|
|
||||||
control, value = line.split(' ')
|
|
||||||
control = 'c_' + control
|
|
||||||
if not hasattr(self, control):
|
|
||||||
raise ValueError('invalid control: %r' % control[2:])
|
|
||||||
setattr(self, control, self._parse(type(getattr(self, control)), value))
|
|
||||||
|
|
||||||
@staticmethod
|
|
||||||
def _parse(atype, value):
|
|
||||||
if atype == bool:
|
|
||||||
false_keys = ['f', 'false', '0', 'n', 'no', 'disable', 'off']
|
|
||||||
return value not in false_keys
|
|
||||||
return atype(value)
|
|
||||||
|
|
||||||
|
|
||||||
def get_default_carla_settings(args):
|
|
||||||
settings = CarlaSettings(
|
|
||||||
SynchronousMode=args.synchronous,
|
|
||||||
SendNonPlayerAgentsInfo=False,
|
|
||||||
NumberOfVehicles=20,
|
|
||||||
NumberOfPedestrians=40,
|
|
||||||
WeatherId=1)
|
|
||||||
settings.add_sensor(Camera('Camera1'))
|
|
||||||
return str(settings)
|
|
||||||
|
|
||||||
|
|
||||||
def edit_text(text):
|
|
||||||
editor = 'vim'
|
|
||||||
with tempfile.NamedTemporaryFile('w+', suffix='.ini') as fp:
|
|
||||||
fp.write(text)
|
|
||||||
fp.flush()
|
|
||||||
try:
|
|
||||||
if 0 != subprocess.run([editor, fp.name]).returncode:
|
|
||||||
print('Cancelled.')
|
|
||||||
return None
|
|
||||||
except FileNotFoundError:
|
|
||||||
logging.error('error opening text editor, is %r installed?', editor)
|
|
||||||
return None
|
|
||||||
fp.seek(0)
|
|
||||||
return fp.read()
|
|
||||||
|
|
||||||
|
|
||||||
class CarlaClientConsole(cmd.Cmd):
|
|
||||||
def __init__(self, args):
|
|
||||||
cmd.Cmd.__init__(self)
|
|
||||||
self.args = args
|
|
||||||
self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)'
|
|
||||||
self.client = CarlaClient(args.host, args.port)
|
|
||||||
self.settings = get_default_carla_settings(args)
|
|
||||||
self.print_measurements = False
|
|
||||||
self.control = _Control()
|
|
||||||
self.thread = threading.Thread(target=self._agent_thread_worker)
|
|
||||||
self.done = False
|
|
||||||
self.thread.start()
|
|
||||||
|
|
||||||
def cleanup(self):
|
|
||||||
self.do_disconnect()
|
|
||||||
self.done = True
|
|
||||||
if self.thread.is_alive():
|
|
||||||
self.thread.join()
|
|
||||||
|
|
||||||
def default(self, line):
|
|
||||||
logging.error('unknown command \'%s\'!', line)
|
|
||||||
|
|
||||||
def emptyline(self):
|
|
||||||
pass
|
|
||||||
|
|
||||||
def precmd(self, line):
|
|
||||||
return line.strip().lower()
|
|
||||||
|
|
||||||
def can_exit(self):
|
|
||||||
return True
|
|
||||||
|
|
||||||
def do_exit(self, line=None):
|
|
||||||
"""Exit the console."""
|
|
||||||
return True
|
|
||||||
|
|
||||||
def do_eof(self, line=None):
|
|
||||||
"""Exit the console."""
|
|
||||||
print('exit')
|
|
||||||
return self.do_exit(line)
|
|
||||||
|
|
||||||
def help_help(self):
|
|
||||||
print('usage: help [topic]')
|
|
||||||
|
|
||||||
def do_disconnect(self, line=None):
|
|
||||||
"""Disconnect from the server."""
|
|
||||||
self.client.disconnect()
|
|
||||||
|
|
||||||
def do_new_episode(self, line=None):
|
|
||||||
"""Request a new episode. Connect to the server if not connected."""
|
|
||||||
try:
|
|
||||||
self.control = _Control()
|
|
||||||
if not self.client.connected():
|
|
||||||
self.client.connect()
|
|
||||||
self.client.load_settings(self.settings)
|
|
||||||
self.client.start_episode(0)
|
|
||||||
logging.info('new episode started')
|
|
||||||
except Exception as exception:
|
|
||||||
logging.error(exception)
|
|
||||||
|
|
||||||
def do_control(self, line=None):
|
|
||||||
"""Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)"""
|
|
||||||
try:
|
|
||||||
if line == 'reset':
|
|
||||||
self.control = _Control()
|
|
||||||
else:
|
|
||||||
self.control.set(line)
|
|
||||||
logging.debug('control: %s', self.control.kwargs())
|
|
||||||
except Exception as exception:
|
|
||||||
logging.error(exception)
|
|
||||||
|
|
||||||
def complete_control(self, text, *args, **kwargs):
|
|
||||||
options = self.control.action_list()
|
|
||||||
options.append('reset')
|
|
||||||
return [x + ' ' for x in options if x.startswith(text)]
|
|
||||||
|
|
||||||
def do_settings(self, line=None):
|
|
||||||
"""Open a text editor to edit CARLA settings."""
|
|
||||||
result = edit_text(self.settings)
|
|
||||||
if result is not None:
|
|
||||||
self.settings = result
|
|
||||||
|
|
||||||
def do_print_measurements(self, line):
|
|
||||||
"""Print received measurements to console.\nusage: print_measurements [t/f]"""
|
|
||||||
self.print_measurements = True if not line else _Control._parse(bool, line)
|
|
||||||
|
|
||||||
def _agent_thread_worker(self):
|
|
||||||
filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png'
|
|
||||||
while not self.done:
|
|
||||||
try:
|
|
||||||
measurements, sensor_data = self.client.read_data()
|
|
||||||
if self.print_measurements:
|
|
||||||
print(measurements)
|
|
||||||
|
|
||||||
if self.args.images_to_disk:
|
|
||||||
images = [x for x in sensor_data.values() if isinstance(x, Image)]
|
|
||||||
for n, image in enumerate(images):
|
|
||||||
path = filename.format(n, measurements.game_timestamp)
|
|
||||||
image.save_to_disk(path)
|
|
||||||
self.client.send_control(**self.control.kwargs())
|
|
||||||
except Exception as exception:
|
|
||||||
# logging.exception(exception)
|
|
||||||
time.sleep(1)
|
|
|
@ -1,130 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
import logging
|
|
||||||
import random
|
|
||||||
|
|
||||||
import suite
|
|
||||||
|
|
||||||
import carla
|
|
||||||
|
|
||||||
from carla.client import CarlaClient
|
|
||||||
from carla.sensor import Camera, Image
|
|
||||||
from carla.sensor import Lidar, LidarMeasurement
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.util import make_connection
|
|
||||||
|
|
||||||
|
|
||||||
class _BasicTestBase(suite.CarlaServerTest):
|
|
||||||
def run_carla_client(self, carla_settings, number_of_episodes, number_of_frames, use_autopilot_control=None):
|
|
||||||
with make_connection(CarlaClient, self.args.host, self.args.port, timeout=15) as client:
|
|
||||||
logging.info('CarlaClient connected, running %d episodes', number_of_episodes)
|
|
||||||
for _ in range(0, number_of_episodes):
|
|
||||||
carla_settings.randomize_seeds()
|
|
||||||
carla_settings.randomize_weather()
|
|
||||||
logging.debug('sending CarlaSettings:\n%s', carla_settings)
|
|
||||||
logging.info('new episode requested')
|
|
||||||
scene = client.load_settings(carla_settings)
|
|
||||||
number_of_player_starts = len(scene.player_start_spots)
|
|
||||||
player_start = random.randint(0, max(0, number_of_player_starts - 1))
|
|
||||||
logging.info(
|
|
||||||
'start episode at %d/%d player start (%d frames)',
|
|
||||||
player_start,
|
|
||||||
number_of_player_starts,
|
|
||||||
number_of_frames)
|
|
||||||
client.start_episode(player_start)
|
|
||||||
if use_autopilot_control is None:
|
|
||||||
use_autopilot_control = (random.random() < 0.5)
|
|
||||||
reverse = (random.random() < 0.2)
|
|
||||||
for _ in range(0, number_of_frames):
|
|
||||||
logging.debug('reading measurements...')
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
images = [x for x in sensor_data.values() if isinstance(x, Image)]
|
|
||||||
number_of_agents = len(measurements.non_player_agents)
|
|
||||||
logging.debug('received data of %d agents', number_of_agents)
|
|
||||||
logging.debug('received %d images', len(images))
|
|
||||||
if len(sensor_data) != len(carla_settings._sensors):
|
|
||||||
raise RuntimeError('received %d, expected %d' % (len(sensor_data), len(carla_settings._sensors)))
|
|
||||||
logging.debug('sending control...')
|
|
||||||
control = measurements.player_measurements.autopilot_control
|
|
||||||
if not use_autopilot_control:
|
|
||||||
control.steer = random.uniform(-1.0, 1.0)
|
|
||||||
control.throttle = 0.3
|
|
||||||
control.hand_brake = False
|
|
||||||
control.reverse = reverse
|
|
||||||
client.send_control(
|
|
||||||
steer=control.steer,
|
|
||||||
throttle=control.throttle,
|
|
||||||
brake=control.brake,
|
|
||||||
hand_brake=control.hand_brake,
|
|
||||||
reverse=control.reverse)
|
|
||||||
|
|
||||||
|
|
||||||
class UseCase(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.add_sensor(Camera('DefaultCamera'))
|
|
||||||
self.run_carla_client(settings, 5, 200)
|
|
||||||
|
|
||||||
|
|
||||||
class NoCamera(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings()
|
|
||||||
self.run_carla_client(settings, 3, 200)
|
|
||||||
|
|
||||||
|
|
||||||
class TwoCameras(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.add_sensor(Camera('DefaultCamera'))
|
|
||||||
camera2 = Camera('Camera2')
|
|
||||||
camera2.set(PostProcessing='Depth', FOV=120)
|
|
||||||
camera2.set_image_size(1924, 1028)
|
|
||||||
settings.add_sensor(camera2)
|
|
||||||
self.run_carla_client(settings, 3, 100)
|
|
||||||
|
|
||||||
|
|
||||||
class SynchronousMode(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings(SynchronousMode=True)
|
|
||||||
settings.add_sensor(Camera('DefaultCamera'))
|
|
||||||
self.run_carla_client(settings, 3, 200)
|
|
||||||
|
|
||||||
|
|
||||||
class GetAgentsInfo(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.set(
|
|
||||||
SynchronousMode=True,
|
|
||||||
SendNonPlayerAgentsInfo=True,
|
|
||||||
NumberOfVehicles=60,
|
|
||||||
NumberOfPedestrians=90)
|
|
||||||
settings.add_sensor(Camera('DefaultCamera'))
|
|
||||||
self.run_carla_client(settings, 3, 100)
|
|
||||||
|
|
||||||
|
|
||||||
class LongEpisode(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.add_sensor(Camera('DefaultCamera'))
|
|
||||||
self.run_carla_client(settings, 1, 2000, use_autopilot_control=True)
|
|
||||||
|
|
||||||
|
|
||||||
class LidarTest(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.add_sensor(Lidar('DefaultLidar'))
|
|
||||||
self.run_carla_client(settings, 3, 100)
|
|
||||||
|
|
||||||
|
|
||||||
class SpeedLowQuality(_BasicTestBase):
|
|
||||||
def run(self):
|
|
||||||
settings = CarlaSettings(QualityLevel='Low')
|
|
||||||
settings.add_sensor(Lidar('DefaultLidar'))
|
|
||||||
settings.add_sensor(Camera('DefaultCamera'))
|
|
||||||
settings.add_sensor(Camera('DefaultDepth', PostProcessing='Depth'))
|
|
||||||
settings.add_sensor(Camera('DefaultSemSeg', PostProcessing='SemanticSegmentation'))
|
|
||||||
self.run_carla_client(settings, 3, 200)
|
|
|
@ -1,10 +0,0 @@
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
|
|
||||||
class CarlaServerTest(object):
|
|
||||||
def __init__(self, args):
|
|
||||||
self.args = args
|
|
|
@ -1,186 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""A CARLA client for testing."""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import os
|
|
||||||
import random
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
|
|
||||||
sys.path.append(os.path.join(os.path.dirname(__file__), '..'))
|
|
||||||
|
|
||||||
import carla
|
|
||||||
|
|
||||||
from carla import sensor
|
|
||||||
from carla.client import CarlaClient
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPClient
|
|
||||||
from carla.util import make_connection
|
|
||||||
|
|
||||||
import console
|
|
||||||
|
|
||||||
|
|
||||||
def run_carla_client(args):
|
|
||||||
with make_connection(CarlaClient, args.host, args.port, timeout=15) as client:
|
|
||||||
logging.info('CarlaClient connected')
|
|
||||||
filename = '_out/test_episode_{:0>4d}/{:s}/{:0>6d}'
|
|
||||||
frames_per_episode = 300
|
|
||||||
episode = 0
|
|
||||||
while True:
|
|
||||||
episode += 1
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.set(SendNonPlayerAgentsInfo=True, SynchronousMode=args.synchronous)
|
|
||||||
settings.randomize_seeds()
|
|
||||||
camera = sensor.Camera('DefaultCamera')
|
|
||||||
camera.set_image_size(300, 200) # Do not change this, hard-coded in test.
|
|
||||||
settings.add_sensor(camera)
|
|
||||||
lidar = sensor.Lidar('DefaultLidar')
|
|
||||||
settings.add_sensor(lidar)
|
|
||||||
|
|
||||||
logging.debug('sending CarlaSettings:\n%s', settings)
|
|
||||||
logging.info('new episode requested')
|
|
||||||
|
|
||||||
scene = client.load_settings(settings)
|
|
||||||
|
|
||||||
number_of_player_starts = len(scene.player_start_spots)
|
|
||||||
player_start = random.randint(0, max(0, number_of_player_starts - 1))
|
|
||||||
logging.info(
|
|
||||||
'start episode at %d/%d player start (%d frames)',
|
|
||||||
player_start,
|
|
||||||
number_of_player_starts,
|
|
||||||
frames_per_episode)
|
|
||||||
|
|
||||||
client.start_episode(player_start)
|
|
||||||
|
|
||||||
use_autopilot_control = (random.random() < 0.5)
|
|
||||||
reverse = (random.random() < 0.2)
|
|
||||||
|
|
||||||
for frame in range(0, frames_per_episode):
|
|
||||||
logging.debug('reading measurements...')
|
|
||||||
measurements, sensor_data = client.read_data()
|
|
||||||
images = [x for x in sensor_data.values() if isinstance(x, sensor.Image)]
|
|
||||||
|
|
||||||
logging.debug('received data of %d agents', len(measurements.non_player_agents))
|
|
||||||
if len(images) > 0:
|
|
||||||
assert (images[0].width, images[0].height) == (camera.ImageSizeX, camera.ImageSizeY)
|
|
||||||
|
|
||||||
if args.images_to_disk:
|
|
||||||
for name, data in sensor_data.items():
|
|
||||||
data.save_to_disk(filename.format(episode, name, frame))
|
|
||||||
|
|
||||||
logging.debug('sending control...')
|
|
||||||
control = measurements.player_measurements.autopilot_control
|
|
||||||
if not use_autopilot_control:
|
|
||||||
control.steer = random.uniform(-1.0, 1.0)
|
|
||||||
control.throttle = 0.3
|
|
||||||
control.hand_brake = False
|
|
||||||
control.reverse = reverse
|
|
||||||
client.send_control(
|
|
||||||
steer=control.steer,
|
|
||||||
throttle=control.throttle,
|
|
||||||
brake=control.brake,
|
|
||||||
hand_brake=control.hand_brake,
|
|
||||||
reverse=control.reverse)
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--log',
|
|
||||||
metavar='LOG_FILE',
|
|
||||||
default=None,
|
|
||||||
help='print output to file')
|
|
||||||
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(
|
|
||||||
'-s', '--synchronous',
|
|
||||||
action='store_true',
|
|
||||||
help='enable synchronous mode')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-i', '--images-to-disk',
|
|
||||||
action='store_true',
|
|
||||||
help='save images to disk')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--echo',
|
|
||||||
action='store_true',
|
|
||||||
help='start a client that just echoes what the server sends')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-c', '--console',
|
|
||||||
action='store_true',
|
|
||||||
help='start the client console')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
name = 'echo_client: ' if args.echo else 'carla_client: '
|
|
||||||
logging_config = {
|
|
||||||
'format': name + '%(levelname)s: %(message)s',
|
|
||||||
'level': logging.DEBUG if args.debug else logging.INFO
|
|
||||||
}
|
|
||||||
if args.log:
|
|
||||||
logging_config['filename'] = args.log
|
|
||||||
logging_config['filemode'] = 'w+'
|
|
||||||
logging.basicConfig(**logging_config)
|
|
||||||
|
|
||||||
logging.info('listening to server %s:%s', args.host, args.port)
|
|
||||||
|
|
||||||
if args.console:
|
|
||||||
cmd = console.CarlaClientConsole(args)
|
|
||||||
try:
|
|
||||||
cmd.cmdloop()
|
|
||||||
finally:
|
|
||||||
cmd.cleanup()
|
|
||||||
return
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
if args.echo:
|
|
||||||
|
|
||||||
with make_connection(TCPClient, args.host, args.port, timeout=15) as client:
|
|
||||||
while True:
|
|
||||||
logging.info('reading...')
|
|
||||||
data = client.read()
|
|
||||||
if not data:
|
|
||||||
raise RuntimeError('failed to read data from server')
|
|
||||||
logging.info('writing...')
|
|
||||||
client.write(data)
|
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
run_carla_client(args)
|
|
||||||
|
|
||||||
except AssertionError as assertion:
|
|
||||||
raise assertion
|
|
||||||
except Exception as exception:
|
|
||||||
logging.error('exception: %s', exception)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -1,168 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB), and the INTEL Visual Computing Lab.
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Client that runs two servers simultaneously to test repeatability."""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import os
|
|
||||||
import random
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
|
|
||||||
sys.path.append(os.path.join(os.path.dirname(__file__), '..'))
|
|
||||||
|
|
||||||
from carla.client import make_carla_client
|
|
||||||
from carla.sensor import Camera, Image
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
|
|
||||||
|
|
||||||
def run_carla_clients(args):
|
|
||||||
filename = '_images_repeatability/server{:d}/{:0>6d}.png'
|
|
||||||
with make_carla_client(args.host1, args.port1) as client1:
|
|
||||||
logging.info('1st client connected')
|
|
||||||
with make_carla_client(args.host2, args.port2) as client2:
|
|
||||||
logging.info('2nd client connected')
|
|
||||||
|
|
||||||
settings = CarlaSettings()
|
|
||||||
settings.set(
|
|
||||||
SynchronousMode=True,
|
|
||||||
SendNonPlayerAgentsInfo=True,
|
|
||||||
NumberOfVehicles=50,
|
|
||||||
NumberOfPedestrians=50,
|
|
||||||
WeatherId=random.choice([1, 3, 7, 8, 14]))
|
|
||||||
settings.randomize_seeds()
|
|
||||||
|
|
||||||
if args.images_to_disk:
|
|
||||||
camera = Camera('DefaultCamera')
|
|
||||||
camera.set_image_size(800, 600)
|
|
||||||
settings.add_sensor(camera)
|
|
||||||
|
|
||||||
scene1 = client1.load_settings(settings)
|
|
||||||
scene2 = client2.load_settings(settings)
|
|
||||||
|
|
||||||
number_of_player_starts = len(scene1.player_start_spots)
|
|
||||||
assert number_of_player_starts == len(scene2.player_start_spots)
|
|
||||||
player_start = random.randint(0, max(0, number_of_player_starts - 1))
|
|
||||||
logging.info(
|
|
||||||
'start episode at %d/%d player start (run forever, press ctrl+c to cancel)',
|
|
||||||
player_start,
|
|
||||||
number_of_player_starts)
|
|
||||||
|
|
||||||
client1.start_episode(player_start)
|
|
||||||
client2.start_episode(player_start)
|
|
||||||
|
|
||||||
frame = 0
|
|
||||||
while True:
|
|
||||||
frame += 1
|
|
||||||
|
|
||||||
meas1, sensor_data1 = client1.read_data()
|
|
||||||
meas2, sensor_data2 = client2.read_data()
|
|
||||||
|
|
||||||
player1 = meas1.player_measurements
|
|
||||||
player2 = meas2.player_measurements
|
|
||||||
|
|
||||||
images1 = [x for x in sensor_data1.values() if isinstance(x, Image)]
|
|
||||||
images2 = [x for x in sensor_data2.values() if isinstance(x, Image)]
|
|
||||||
|
|
||||||
control1 = player1.autopilot_control
|
|
||||||
control2 = player2.autopilot_control
|
|
||||||
|
|
||||||
try:
|
|
||||||
assert len(images1) == len(images2)
|
|
||||||
assert len(meas1.non_player_agents) == len(meas2.non_player_agents)
|
|
||||||
assert player1.transform.location.x == player2.transform.location.x
|
|
||||||
assert player1.transform.location.y == player2.transform.location.y
|
|
||||||
assert player1.transform.location.z == player2.transform.location.z
|
|
||||||
assert control1.steer == control2.steer
|
|
||||||
assert control1.throttle == control2.throttle
|
|
||||||
assert control1.brake == control2.brake
|
|
||||||
assert control1.hand_brake == control2.hand_brake
|
|
||||||
assert control1.reverse == control2.reverse
|
|
||||||
except AssertionError:
|
|
||||||
logging.exception('assertion failed')
|
|
||||||
|
|
||||||
if args.images_to_disk:
|
|
||||||
assert len(images1) == 1
|
|
||||||
images1[0].save_to_disk(filename.format(1, frame))
|
|
||||||
images2[0].save_to_disk(filename.format(2, frame))
|
|
||||||
|
|
||||||
client1.send_control(control1)
|
|
||||||
client2.send_control(control2)
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--log',
|
|
||||||
metavar='LOG_FILE',
|
|
||||||
default=None,
|
|
||||||
help='print output to file')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host1',
|
|
||||||
metavar='H',
|
|
||||||
default='127.0.0.1',
|
|
||||||
help='IP of the first host server (default: 127.0.0.1)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p1', '--port1',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to the first server (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host2',
|
|
||||||
metavar='H',
|
|
||||||
default='127.0.0.1',
|
|
||||||
help='IP of the second host server (default: 127.0.0.1)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p2', '--port2',
|
|
||||||
metavar='P',
|
|
||||||
default=3000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to the second server (default: 3000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-i', '--images-to-disk',
|
|
||||||
action='store_true',
|
|
||||||
help='save images to disk')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
logging_config = {
|
|
||||||
'format': '%(levelname)s: %(message)s',
|
|
||||||
'level': logging.DEBUG if args.debug else logging.INFO
|
|
||||||
}
|
|
||||||
if args.log:
|
|
||||||
logging_config['filename'] = args.log
|
|
||||||
logging_config['filemode'] = 'w+'
|
|
||||||
logging.basicConfig(**logging_config)
|
|
||||||
|
|
||||||
logging.info('listening to 1st server at %s:%s', args.host1, args.port1)
|
|
||||||
logging.info('listening to 2nd server at %s:%s', args.host2, args.port2)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
run_carla_clients(args)
|
|
||||||
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -1,191 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Test suite for testing CARLAUE4."""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import glob
|
|
||||||
import imp
|
|
||||||
import inspect
|
|
||||||
import logging
|
|
||||||
import os
|
|
||||||
import random
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
|
|
||||||
sys.path.append(os.path.join(os.path.dirname(__file__), '..'))
|
|
||||||
sys.path.append(os.path.join(os.path.dirname(__file__), '.'))
|
|
||||||
|
|
||||||
import carla
|
|
||||||
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
from carla.util import StopWatch
|
|
||||||
|
|
||||||
from suite import CarlaServerTest
|
|
||||||
|
|
||||||
# Modified by command-line args.
|
|
||||||
LOGGING_TO_FILE = False
|
|
||||||
|
|
||||||
# Output.
|
|
||||||
GREEN = '\x1b[0;32m%s\x1b[0m'
|
|
||||||
YELLOW = '\x1b[0;33m%s\x1b[0m'
|
|
||||||
RED = '\x1b[0;31m%s\x1b[0m'
|
|
||||||
|
|
||||||
SEP0 = GREEN % '[==========]'
|
|
||||||
SEP1 = GREEN % '[----------]'
|
|
||||||
RUN = GREEN % '[ RUN ]'
|
|
||||||
OK = GREEN % '[ OK ]'
|
|
||||||
PASSED = GREEN % '[ PASSED ]'
|
|
||||||
FAILED = RED % '[EXCEPTION ]'
|
|
||||||
FAILED = RED % '[ FAILED ]'
|
|
||||||
|
|
||||||
|
|
||||||
def log_test(prep, message, *args):
|
|
||||||
message = prep + ' ' + message % args
|
|
||||||
print(message)
|
|
||||||
if LOGGING_TO_FILE:
|
|
||||||
logging.info(message)
|
|
||||||
|
|
||||||
|
|
||||||
class TestProxy(object):
|
|
||||||
def __init__(self, name, declaration, module_name):
|
|
||||||
self.name = module_name + '.' + name
|
|
||||||
self.declaration = declaration
|
|
||||||
self.instance = None
|
|
||||||
|
|
||||||
def instantiate(self, *args, **kwargs):
|
|
||||||
self.instance = self.declaration(*args, **kwargs)
|
|
||||||
|
|
||||||
def run(self, *args, **kwargs):
|
|
||||||
if not hasattr(self.instance, 'run'):
|
|
||||||
logging.error('%s: "run" method should be implemented in derived class', self.name)
|
|
||||||
return False
|
|
||||||
result = self.instance.run(*args, **kwargs)
|
|
||||||
return True if result is None else result
|
|
||||||
|
|
||||||
|
|
||||||
def iterate_tests():
|
|
||||||
interface = CarlaServerTest
|
|
||||||
strip_ext = lambda f: os.path.splitext(os.path.basename(f))[0]
|
|
||||||
is_valid = lambda obj: inspect.isclass(obj) and issubclass(obj, interface)
|
|
||||||
|
|
||||||
folder = os.path.join(os.path.dirname(__file__), 'suite')
|
|
||||||
modules = glob.glob(os.path.join(folder, "*.py"))
|
|
||||||
|
|
||||||
for module_name in set(strip_ext(m) for m in modules if not m.startswith('_')):
|
|
||||||
logging.debug('parsing module %r', module_name)
|
|
||||||
try:
|
|
||||||
module_info = imp.find_module(module_name, [folder])
|
|
||||||
# This do a reload if already imported.
|
|
||||||
module = imp.load_module(module_name, *module_info)
|
|
||||||
for name, declaration in inspect.getmembers(module, is_valid):
|
|
||||||
if not name.startswith('_'):
|
|
||||||
yield TestProxy(name, declaration, module_name)
|
|
||||||
except Exception as exception:
|
|
||||||
logging.error('failed to load module %r: %s', module_name, exception)
|
|
||||||
finally:
|
|
||||||
module_info[0].close()
|
|
||||||
|
|
||||||
|
|
||||||
def run_test(test, args):
|
|
||||||
log_test(SEP1, 'Instantiating %s', test.name)
|
|
||||||
try:
|
|
||||||
test.instantiate(args)
|
|
||||||
except Exception as exception:
|
|
||||||
logging.error('exception instantiating %r: %s', test.name, exception)
|
|
||||||
return False
|
|
||||||
log_test(RUN, test.name)
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
timer = StopWatch()
|
|
||||||
result = test.run()
|
|
||||||
timer.stop()
|
|
||||||
break
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
||||||
except Exception as exception:
|
|
||||||
timer.stop()
|
|
||||||
logging.exception('exception: %s', exception)
|
|
||||||
result = False
|
|
||||||
break
|
|
||||||
log_test(OK if result else FAILED, '%s (%d ms)', test.name, timer.milliseconds())
|
|
||||||
return result
|
|
||||||
|
|
||||||
|
|
||||||
def do_the_tests(args):
|
|
||||||
tests = [t for t in iterate_tests()]
|
|
||||||
random.shuffle(tests)
|
|
||||||
succeeded = []
|
|
||||||
failed = []
|
|
||||||
log_test(SEP0, 'Running %d tests.', len(tests))
|
|
||||||
for test in tests:
|
|
||||||
if run_test(test, args):
|
|
||||||
succeeded.append(test)
|
|
||||||
else:
|
|
||||||
failed.append(test)
|
|
||||||
log_test(SEP0, '%d tests ran.', len(tests))
|
|
||||||
if succeeded:
|
|
||||||
log_test(PASSED, '%d tests.', len(succeeded))
|
|
||||||
if failed:
|
|
||||||
log_test(FAILED, '%d tests.', len(failed))
|
|
||||||
for test in failed:
|
|
||||||
log_test(FAILED, test.name)
|
|
||||||
return True
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--log',
|
|
||||||
metavar='LOG_FILE',
|
|
||||||
default=None,
|
|
||||||
help='print output to file')
|
|
||||||
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)')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
global LOGGING_TO_FILE
|
|
||||||
LOGGING_TO_FILE = args.log is not None
|
|
||||||
|
|
||||||
logging_config = {
|
|
||||||
'format': '%(levelname)s: %(message)s',
|
|
||||||
'level': logging.DEBUG if args.debug else logging.INFO
|
|
||||||
}
|
|
||||||
if args.log:
|
|
||||||
logging_config['filename'] = args.log
|
|
||||||
logging_config['filemode'] = 'w+'
|
|
||||||
logging.basicConfig(**logging_config)
|
|
||||||
|
|
||||||
logging.info('listening to server %s:%s', args.host, args.port)
|
|
||||||
|
|
||||||
print('Running the CARLAUE4 test suite (looks like GTest but it\'s not).')
|
|
||||||
do_the_tests(args)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -1,3 +0,0 @@
|
||||||
weather,time_out,result,final_time,end_point,final_distance,exp_id,rep,start_point,initial_distance
|
|
||||||
3,335314,0,335314,29,171.3219381575824,3,0,105,280.44944447968976
|
|
||||||
3,243.6346,0,243.6346,130,215.56398248559435,3,0,27,174.94691018267446
|
|
|
|
@ -1,26 +0,0 @@
|
||||||
import unittest
|
|
||||||
from carla.driving_benchmark.experiment_suites.experiment_suite import ExperimentSuite
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment_suites.basic_experiment_suite import BasicExperimentSuite
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment_suites.corl_2017 import CoRL2017
|
|
||||||
|
|
||||||
class testExperimentSuite(unittest.TestCase):
|
|
||||||
|
|
||||||
|
|
||||||
def test_init(self):
|
|
||||||
|
|
||||||
base_class = ExperimentSuite('Town01')
|
|
||||||
subclasses_instanciate = [obj('Town01') for obj in ExperimentSuite.__subclasses__()]
|
|
||||||
|
|
||||||
|
|
||||||
def test_properties(self):
|
|
||||||
|
|
||||||
all_classes = [obj('Town01') for obj in ExperimentSuite.__subclasses__()]
|
|
||||||
print (all_classes)
|
|
||||||
for exp_suite in all_classes:
|
|
||||||
print(exp_suite.__class__)
|
|
||||||
print(exp_suite.dynamic_tasks)
|
|
||||||
print(exp_suite.weathers)
|
|
||||||
|
|
||||||
|
|
|
@ -1,190 +0,0 @@
|
||||||
import os
|
|
||||||
import numpy as np
|
|
||||||
import unittest
|
|
||||||
from carla.driving_benchmark.metrics import Metrics
|
|
||||||
from carla.driving_benchmark.recording import Recording
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def sum_matrix(matrix):
|
|
||||||
# Line trick to reduce sum a matrix in one line
|
|
||||||
return sum(sum(matrix, []))
|
|
||||||
|
|
||||||
|
|
||||||
class testMetrics(unittest.TestCase):
|
|
||||||
|
|
||||||
def __init__(self, *args, **kwargs):
|
|
||||||
super(testMetrics, self).__init__(*args, **kwargs)
|
|
||||||
|
|
||||||
self._metrics_parameters = {
|
|
||||||
|
|
||||||
'intersection_offroad': {'frames_skip': 10, # Check intersection always with 10 frames tolerance
|
|
||||||
'frames_recount': 20,
|
|
||||||
'threshold': 0.3
|
|
||||||
},
|
|
||||||
'intersection_otherlane': {'frames_skip': 10, # Check intersection always with 10 frames tolerance
|
|
||||||
'frames_recount': 20,
|
|
||||||
'threshold': 0.4
|
|
||||||
},
|
|
||||||
'collision_other': {'frames_skip': 10,
|
|
||||||
'frames_recount': 20,
|
|
||||||
'threshold': 400
|
|
||||||
},
|
|
||||||
'collision_vehicles': {'frames_skip': 10,
|
|
||||||
'frames_recount': 30,
|
|
||||||
'threshold': 400
|
|
||||||
},
|
|
||||||
'collision_pedestrians': {'frames_skip': 5,
|
|
||||||
'frames_recount': 100,
|
|
||||||
'threshold': 300
|
|
||||||
},
|
|
||||||
'dynamic_episodes': [3]
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def _generate_test_case(self, poses_to_test):
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='TestMetric'
|
|
||||||
, continue_experiment=False, save_images=True
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
from carla.carla_server_pb2 import Measurements
|
|
||||||
from carla.carla_server_pb2 import Control
|
|
||||||
|
|
||||||
|
|
||||||
for pose in poses_to_test:
|
|
||||||
experiment = Experiment()
|
|
||||||
|
|
||||||
recording.write_summary_results(experiment=experiment, pose=pose, rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
|
|
||||||
reward_vec = [Measurements().player_measurements for x in range(25)]
|
|
||||||
control_vec = [Control() for x in range(25)]
|
|
||||||
|
|
||||||
recording.write_measurements_results(experiment=experiment,
|
|
||||||
rep=1, pose=pose, reward_vec=reward_vec,
|
|
||||||
control_vec=control_vec)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
return recording._path
|
|
||||||
|
|
||||||
def test_init(self):
|
|
||||||
|
|
||||||
# Metric should instantiate with parameters
|
|
||||||
Metrics(self._metrics_parameters,[3])
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def test_divide_by_episodes(self):
|
|
||||||
|
|
||||||
|
|
||||||
metrics_obj = Metrics(self._metrics_parameters,[3])
|
|
||||||
|
|
||||||
poses_to_test = [[24, 32], [34, 36], [54, 67]]
|
|
||||||
path = self._generate_test_case(poses_to_test)
|
|
||||||
|
|
||||||
# We start by reading the summary header file and the measurements header file.
|
|
||||||
with open(os.path.join(path, 'summary.csv'), "r") as f:
|
|
||||||
header = f.readline()
|
|
||||||
header = header.split(',')
|
|
||||||
header[-1] = header[-1][:-2]
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
with open(os.path.join(path,'measurements.csv'), "r") as f:
|
|
||||||
|
|
||||||
header_metrics = f.readline()
|
|
||||||
header_metrics = header_metrics.split(',')
|
|
||||||
header_metrics[-1] = header_metrics[-1][:-2]
|
|
||||||
|
|
||||||
|
|
||||||
result_matrix = np.loadtxt(os.path.join(path, 'summary.csv'), delimiter=",", skiprows=1)
|
|
||||||
|
|
||||||
# Corner Case: The presented test just had one episode
|
|
||||||
if result_matrix.ndim == 1:
|
|
||||||
result_matrix = np.expand_dims(result_matrix, axis=0)
|
|
||||||
|
|
||||||
|
|
||||||
tasks = np.unique(result_matrix[:, header.index('exp_id')])
|
|
||||||
|
|
||||||
|
|
||||||
all_weathers = np.unique(result_matrix[:, header.index('weather')])
|
|
||||||
|
|
||||||
measurements_matrix = np.loadtxt(os.path.join(path, 'measurements.csv'), delimiter=",", skiprows=1)
|
|
||||||
|
|
||||||
|
|
||||||
episodes = metrics_obj._divide_by_episodes(measurements_matrix,header_metrics)
|
|
||||||
|
|
||||||
|
|
||||||
self.assertEqual(len(episodes),3)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def test_compute(self):
|
|
||||||
|
|
||||||
# This is is the last one, generate many cases, corner cases, to be tested.
|
|
||||||
|
|
||||||
metrics_obj = Metrics(self._metrics_parameters,[3])
|
|
||||||
|
|
||||||
|
|
||||||
# Lets start testing a general file, not from a real run
|
|
||||||
# The case is basically an empty case
|
|
||||||
poses_to_test = [[24, 32], [34, 36], [54, 67]]
|
|
||||||
path = self._generate_test_case(poses_to_test)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
summary_dict = metrics_obj.compute(path)
|
|
||||||
|
|
||||||
|
|
||||||
number_of_colisions_vehicles = sum_matrix(summary_dict['collision_vehicles'][1.0])
|
|
||||||
number_of_colisions_general = sum_matrix(summary_dict['collision_other'][1.0])
|
|
||||||
number_of_colisions_pedestrians = sum_matrix(summary_dict['collision_pedestrians'][1.0])
|
|
||||||
number_of_intersection_offroad = sum_matrix(summary_dict['intersection_offroad'][1.0])
|
|
||||||
number_of_intersection_otherlane = sum_matrix(summary_dict['intersection_otherlane'][1.0])
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
self.assertEqual(number_of_colisions_vehicles, 0)
|
|
||||||
self.assertEqual(number_of_colisions_general, 0)
|
|
||||||
self.assertEqual(number_of_colisions_pedestrians, 0)
|
|
||||||
self.assertEqual(number_of_intersection_offroad, 0)
|
|
||||||
self.assertEqual(number_of_intersection_otherlane, 0)
|
|
||||||
|
|
||||||
|
|
||||||
# Now lets make a collision test on a premade file
|
|
||||||
|
|
||||||
path = 'test/unit_tests/test_data/testfile_collisions'
|
|
||||||
|
|
||||||
summary_dict = metrics_obj.compute(path)
|
|
||||||
|
|
||||||
number_of_colisions_vehicles = sum_matrix(summary_dict['collision_vehicles'][3.0])
|
|
||||||
number_of_colisions_general = sum_matrix(summary_dict['collision_other'][3.0])
|
|
||||||
number_of_colisions_pedestrians = sum_matrix(summary_dict['collision_pedestrians'][3.0])
|
|
||||||
number_of_intersection_offroad = sum_matrix(summary_dict['intersection_offroad'][3.0])
|
|
||||||
number_of_intersection_otherlane = sum_matrix(summary_dict['intersection_otherlane'][3.0])
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
self.assertEqual(number_of_colisions_vehicles, 2)
|
|
||||||
self.assertEqual(number_of_colisions_general, 9)
|
|
||||||
self.assertEqual(number_of_colisions_pedestrians, 0)
|
|
||||||
self.assertEqual(number_of_intersection_offroad, 1)
|
|
||||||
self.assertEqual(number_of_intersection_otherlane, 3)
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1,254 +0,0 @@
|
||||||
import os
|
|
||||||
import unittest
|
|
||||||
from carla.driving_benchmark.recording import Recording
|
|
||||||
|
|
||||||
|
|
||||||
class testRecording(unittest.TestCase):
|
|
||||||
|
|
||||||
def test_init(self):
|
|
||||||
"""
|
|
||||||
The recording should have a reasonable full name
|
|
||||||
"""
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test1',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
_ = open(os.path.join(recording._path, 'summary.csv'), 'r')
|
|
||||||
_ = open(os.path.join(recording._path, 'measurements.csv'), 'r')
|
|
||||||
|
|
||||||
# There should be three files in any newly created case
|
|
||||||
self.assertEqual(len(os.listdir(recording._path)), 3)
|
|
||||||
|
|
||||||
def test_write_summary_results(self):
|
|
||||||
"""
|
|
||||||
Test writting summary results.
|
|
||||||
"""
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test1',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
with open(os.path.join(recording._path, 'summary.csv'), 'r') as f:
|
|
||||||
header = f.readline().split(',')
|
|
||||||
# Assert if header is header
|
|
||||||
self.assertIn('exp_id', header)
|
|
||||||
|
|
||||||
self.assertEqual(len(header), len(recording._dict_summary))
|
|
||||||
# Assert if there is something writen in the row
|
|
||||||
|
|
||||||
written_row = f.readline().split(',')
|
|
||||||
|
|
||||||
# Assert if the number of collums is correct
|
|
||||||
self.assertEqual(len(written_row), len(recording._dict_summary))
|
|
||||||
|
|
||||||
def test_write_summary_results_good_order(self):
|
|
||||||
"""
|
|
||||||
Test if the summary results are writen in the same order on a new process
|
|
||||||
"""
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test_good_order',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
for _ in range(0, 10):
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test_good_order',
|
|
||||||
continue_experiment=True, save_images=True)
|
|
||||||
|
|
||||||
for _ in range(0, 10):
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test_good_order',
|
|
||||||
continue_experiment=True, save_images=True)
|
|
||||||
|
|
||||||
for _ in range(0, 10):
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test_good_order',
|
|
||||||
continue_experiment=True, save_images=True)
|
|
||||||
|
|
||||||
for _ in range(0, 10):
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
# Check if the the test_good_order summaries have all the same files.
|
|
||||||
|
|
||||||
def test_write_measurements_results(self):
|
|
||||||
"""
|
|
||||||
Test writing a few measurements into the log
|
|
||||||
"""
|
|
||||||
|
|
||||||
import os
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
from carla.carla_server_pb2 import Measurements
|
|
||||||
from carla.carla_server_pb2 import Control
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test1',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
reward_vec = [Measurements().player_measurements for _ in range(20)]
|
|
||||||
control_vec = [Control() for _ in range(25)]
|
|
||||||
|
|
||||||
recording.write_measurements_results(experiment=Experiment(),
|
|
||||||
rep=1, pose=[24, 32], reward_vec=reward_vec,
|
|
||||||
control_vec=control_vec)
|
|
||||||
|
|
||||||
with open(os.path.join(recording._path, 'measurements.csv'), 'r') as f:
|
|
||||||
header = f.readline().split(',')
|
|
||||||
# Assert if header is header
|
|
||||||
self.assertIn('exp_id', header)
|
|
||||||
|
|
||||||
self.assertEqual(len(header), len(recording._dict_measurements))
|
|
||||||
# Assert if there is something writen in the row
|
|
||||||
|
|
||||||
written_row = f.readline().split(',')
|
|
||||||
|
|
||||||
# Assert if the number of collums is correct
|
|
||||||
self.assertEqual(len(written_row), len(recording._dict_measurements))
|
|
||||||
|
|
||||||
def test_continue_experiment(self):
|
|
||||||
"""
|
|
||||||
Test if you are able to continue an experiment after restarting the process
|
|
||||||
"""
|
|
||||||
recording = Recording(name_to_save='Test1',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
# A just started case should return the continue experiment case
|
|
||||||
self.assertEqual(recording._continue_experiment(True)[1], 1)
|
|
||||||
# If you don't want to continue, should return also one
|
|
||||||
self.assertEqual(recording._continue_experiment(False)[1], 1)
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
# After writing two experiments it should return 2, so you could start writing os pos 3
|
|
||||||
self.assertEqual(recording._continue_experiment(True)[1], 3)
|
|
||||||
# If you dont want to continue, should return also one
|
|
||||||
self.assertEqual(recording._continue_experiment(False)[1], 1)
|
|
||||||
|
|
||||||
def test_get_pose_and_experiment(self):
|
|
||||||
"""
|
|
||||||
Test getting the pose and the experiment from a previous executed benchmark
|
|
||||||
"""
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test1',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(25)
|
|
||||||
|
|
||||||
# An starting experiment should return zero zero
|
|
||||||
|
|
||||||
self.assertEqual(pose, 0)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(25)
|
|
||||||
self.assertEqual(pose, 2)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
for _ in range(23):
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(25)
|
|
||||||
self.assertEqual(pose, 0)
|
|
||||||
self.assertEqual(experiment, 1)
|
|
||||||
|
|
||||||
for _ in range(23):
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(25)
|
|
||||||
self.assertEqual(pose, 23)
|
|
||||||
self.assertEqual(experiment, 1)
|
|
||||||
|
|
||||||
def test_get_pose_and_experiment_corner(self):
|
|
||||||
"""
|
|
||||||
Test getting the pose from multiple cases.
|
|
||||||
"""
|
|
||||||
from carla.driving_benchmark.experiment import Experiment
|
|
||||||
|
|
||||||
recording = Recording(name_to_save='Test1',
|
|
||||||
continue_experiment=False, save_images=True)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(1)
|
|
||||||
|
|
||||||
# An starting experiment should return one
|
|
||||||
|
|
||||||
self.assertEqual(pose, 0)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(2)
|
|
||||||
self.assertEqual(pose, 0)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(1)
|
|
||||||
|
|
||||||
print(pose, experiment)
|
|
||||||
self.assertEqual(pose, 0)
|
|
||||||
self.assertEqual(experiment, 1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(2)
|
|
||||||
|
|
||||||
print(pose, experiment)
|
|
||||||
# An starting experiment should return one
|
|
||||||
self.assertEqual(pose, 1)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(3)
|
|
||||||
|
|
||||||
print(pose, experiment)
|
|
||||||
# An starting experiment should return one
|
|
||||||
self.assertEqual(pose, 1)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
recording.write_summary_results(experiment=Experiment(), pose=[24, 32], rep=1,
|
|
||||||
path_distance=200, remaining_distance=0,
|
|
||||||
final_time=0.2, time_out=49, result=1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(2)
|
|
||||||
|
|
||||||
self.assertEqual(pose, 0)
|
|
||||||
self.assertEqual(experiment, 1)
|
|
||||||
|
|
||||||
pose, experiment = recording.get_pose_and_experiment(3)
|
|
||||||
|
|
||||||
self.assertEqual(pose, 2)
|
|
||||||
self.assertEqual(experiment, 0)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
unittest.main()
|
|
|
@ -1,135 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
|
|
||||||
# Barcelona (UAB).
|
|
||||||
#
|
|
||||||
# This work is licensed under the terms of the MIT license.
|
|
||||||
# For a copy, see <https://opensource.org/licenses/MIT>.
|
|
||||||
|
|
||||||
"""Connects with a CARLA simulator and displays the available start positions
|
|
||||||
for the current map."""
|
|
||||||
|
|
||||||
from __future__ import print_function
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import logging
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
|
|
||||||
import matplotlib.image as mpimg
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
|
|
||||||
from matplotlib.patches import Circle
|
|
||||||
|
|
||||||
from carla.client import make_carla_client
|
|
||||||
from carla.planner.map import CarlaMap
|
|
||||||
from carla.settings import CarlaSettings
|
|
||||||
from carla.tcp import TCPConnectionError
|
|
||||||
|
|
||||||
|
|
||||||
def view_start_positions(args):
|
|
||||||
# We assume the CARLA server is already waiting for a client to connect at
|
|
||||||
# host:port. The same way as in the client example.
|
|
||||||
with make_carla_client(args.host, args.port) as client:
|
|
||||||
print('CarlaClient connected')
|
|
||||||
|
|
||||||
# We load the default settings to the client.
|
|
||||||
scene = client.load_settings(CarlaSettings())
|
|
||||||
print("Received the start positions")
|
|
||||||
|
|
||||||
try:
|
|
||||||
image = mpimg.imread('carla/planner/%s.png' % scene.map_name)
|
|
||||||
carla_map = CarlaMap(scene.map_name)
|
|
||||||
except IOError as exception:
|
|
||||||
logging.error(exception)
|
|
||||||
logging.error('Cannot find map "%s"', scene.map_name)
|
|
||||||
sys.exit(1)
|
|
||||||
|
|
||||||
fig, ax = plt.subplots(1)
|
|
||||||
|
|
||||||
ax.imshow(image)
|
|
||||||
|
|
||||||
if args.positions == 'all':
|
|
||||||
positions_to_plot = range(len(scene.player_start_spots))
|
|
||||||
else:
|
|
||||||
positions_to_plot = map(int, args.positions.split(','))
|
|
||||||
|
|
||||||
for position in positions_to_plot:
|
|
||||||
# Check if position is valid
|
|
||||||
if position >= len(scene.player_start_spots):
|
|
||||||
raise RuntimeError('Selected position is invalid')
|
|
||||||
|
|
||||||
# Convert world to pixel coordinates
|
|
||||||
pixel = carla_map.convert_to_pixel([scene.player_start_spots[position].location.x,
|
|
||||||
scene.player_start_spots[position].location.y,
|
|
||||||
scene.player_start_spots[position].location.z])
|
|
||||||
|
|
||||||
circle = Circle((pixel[0], pixel[1]), 12, color='r', label='A point')
|
|
||||||
ax.add_patch(circle)
|
|
||||||
|
|
||||||
if not args.no_labels:
|
|
||||||
plt.text(pixel[0], pixel[1], str(position), size='x-small')
|
|
||||||
|
|
||||||
plt.axis('off')
|
|
||||||
plt.show()
|
|
||||||
|
|
||||||
fig.savefig('town_positions.pdf', orientation='landscape', bbox_inches='tight')
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
argparser = argparse.ArgumentParser(description=__doc__)
|
|
||||||
argparser.add_argument(
|
|
||||||
'-v', '--verbose',
|
|
||||||
action='store_true',
|
|
||||||
dest='debug',
|
|
||||||
help='print debug information')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--host',
|
|
||||||
metavar='H',
|
|
||||||
default='localhost',
|
|
||||||
help='IP of the host server (default: localhost)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-p', '--port',
|
|
||||||
metavar='P',
|
|
||||||
default=2000,
|
|
||||||
type=int,
|
|
||||||
help='TCP port to listen to (default: 2000)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'-pos', '--positions',
|
|
||||||
metavar='P',
|
|
||||||
default='all',
|
|
||||||
help='Indices of the positions that you want to plot on the map. '
|
|
||||||
'The indices must be separated by commas (default = all positions)')
|
|
||||||
argparser.add_argument(
|
|
||||||
'--no-labels',
|
|
||||||
action='store_true',
|
|
||||||
help='do not display position indices')
|
|
||||||
|
|
||||||
args = argparser.parse_args()
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
|
|
||||||
view_start_positions(args)
|
|
||||||
print('Done.')
|
|
||||||
return
|
|
||||||
|
|
||||||
except TCPConnectionError as error:
|
|
||||||
logging.error(error)
|
|
||||||
time.sleep(1)
|
|
||||||
except RuntimeError as error:
|
|
||||||
logging.error(error)
|
|
||||||
break
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try:
|
|
||||||
main()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print('\nCancelled by user. Bye!')
|
|
|
@ -11,7 +11,7 @@ process.
|
||||||
|
|
||||||
![modules](img/modules.png)
|
![modules](img/modules.png)
|
||||||
|
|
||||||
In Linux, we compile CARLA and all the dependencies with clang-6.0 and C++14
|
In Linux, we compile CARLA and all the dependencies with clang-7.0 and C++14
|
||||||
standard. We however link against different runtime C++ libraries depending on
|
standard. We however link against different runtime C++ libraries depending on
|
||||||
where the code going to be used, since all the code that is going to be linked
|
where the code going to be used, since all the code that is going to be linked
|
||||||
with Unreal Engine needs to be compiled using `libc++`.
|
with Unreal Engine needs to be compiled using `libc++`.
|
||||||
|
@ -26,10 +26,10 @@ make setup
|
||||||
|
|
||||||
Get and compile dependencies
|
Get and compile dependencies
|
||||||
|
|
||||||
* llvm-6.0 (libc++ and libc++abi)
|
* llvm-7.1 (libc++ and libc++abi)
|
||||||
* rpclib-2.2.1 (twice, with libstdc++ and libc++)
|
* rpclib-2.2.1 (twice, with libstdc++ and libc++)
|
||||||
* boost-1.69 (headers only)
|
* boost-1.69 (headers only)
|
||||||
* googletest-1.8.0 (with libc++)
|
* googletest-1.8.1 (with libc++)
|
||||||
|
|
||||||
#### LibCarla
|
#### LibCarla
|
||||||
|
|
||||||
|
@ -53,8 +53,8 @@ Two configurations:
|
||||||
|
|
||||||
#### CarlaUE4 and Carla plugin
|
#### CarlaUE4 and Carla plugin
|
||||||
|
|
||||||
Both compiled at the same step with Unreal Engine 4.21 build tool. They require
|
Both compiled at the same step with Unreal Engine build tool. They require the
|
||||||
the `UE4_ROOT` environment variable set.
|
`UE4_ROOT` environment variable set.
|
||||||
|
|
||||||
Command
|
Command
|
||||||
|
|
||||||
|
|
|
@ -27,7 +27,7 @@ C++
|
||||||
* Comments should not exceed 80 columns, code may exceed this limit a bit in
|
* Comments should not exceed 80 columns, code may exceed this limit a bit in
|
||||||
rare occasions if it results in clearer code.
|
rare occasions if it results in clearer code.
|
||||||
* Compilation should not give any error or warning
|
* Compilation should not give any error or warning
|
||||||
(`clang++-6.0 -Wall -Wextra -std=C++14 -Wno-missing-braces`).
|
(`clang++-7 -Wall -Wextra -std=C++14 -Wno-missing-braces`).
|
||||||
* The use of `throw` is forbidden, use `carla::throw_exception` instead.
|
* The use of `throw` is forbidden, use `carla::throw_exception` instead.
|
||||||
* Unreal C++ code (CarlaUE4 and Carla plugin) follow the
|
* Unreal C++ code (CarlaUE4 and Carla plugin) follow the
|
||||||
[Unreal Engine's Coding Standard][ue4link] with the exception of using
|
[Unreal Engine's Coding Standard][ue4link] with the exception of using
|
||||||
|
|
|
@ -7,8 +7,10 @@ Install the build tools and dependencies
|
||||||
|
|
||||||
```
|
```
|
||||||
sudo add-apt-repository ppa:ubuntu-toolchain-r/test
|
sudo add-apt-repository ppa:ubuntu-toolchain-r/test
|
||||||
|
wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add -
|
||||||
|
sudo apt-add-repository "deb http://apt.llvm.org/xenial/ llvm-toolchain-xenial-7 main"
|
||||||
sudo apt-get update
|
sudo apt-get update
|
||||||
sudo apt-get install build-essential clang-6.0 lld-6.0 g++-7 cmake ninja-build python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl wget unzip autoconf libtool
|
sudo apt-get install build-essential clang-7 lld-7 g++-7 cmake ninja-build python python-pip python-dev python3-dev python3-pip libpng16-dev libtiff5-dev libjpeg-dev tzdata sed curl wget unzip autoconf libtool
|
||||||
pip2 install --user setuptools
|
pip2 install --user setuptools
|
||||||
pip3 install --user setuptools
|
pip3 install --user setuptools
|
||||||
```
|
```
|
||||||
|
@ -20,8 +22,8 @@ change your default clang version to compile Unreal Engine and the CARLA
|
||||||
dependencies
|
dependencies
|
||||||
|
|
||||||
```sh
|
```sh
|
||||||
sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-6.0/bin/clang++ 102
|
sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/lib/llvm-7/bin/clang++ 170
|
||||||
sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-6.0/bin/clang 102
|
sudo update-alternatives --install /usr/bin/clang clang /usr/lib/llvm-7/bin/clang 170
|
||||||
```
|
```
|
||||||
|
|
||||||
Build Unreal Engine
|
Build Unreal Engine
|
||||||
|
@ -32,13 +34,13 @@ Build Unreal Engine
|
||||||
need to add your GitHub username when you sign up at
|
need to add your GitHub username when you sign up at
|
||||||
[www.unrealengine.com](https://www.unrealengine.com).
|
[www.unrealengine.com](https://www.unrealengine.com).
|
||||||
|
|
||||||
Download and compile Unreal Engine 4.21. Here we will assume you install it at
|
Download and compile Unreal Engine 4.22. Here we will assume you install it at
|
||||||
`~/UnrealEngine_4.21", but you can install it anywhere, just replace the path
|
`~/UnrealEngine_4.22", but you can install it anywhere, just replace the path
|
||||||
where necessary.
|
where necessary.
|
||||||
|
|
||||||
```sh
|
```sh
|
||||||
git clone --depth=1 -b 4.21 https://github.com/EpicGames/UnrealEngine.git ~/UnrealEngine_4.21
|
git clone --depth=1 -b 4.22 https://github.com/EpicGames/UnrealEngine.git ~/UnrealEngine_4.22
|
||||||
cd ~/UnrealEngine_4.21
|
cd ~/UnrealEngine_4.22
|
||||||
./Setup.sh && ./GenerateProjectFiles.sh && make
|
./Setup.sh && ./GenerateProjectFiles.sh && make
|
||||||
```
|
```
|
||||||
|
|
||||||
|
@ -71,7 +73,7 @@ For CARLA to find your Unreal Engine's installation folder you need to set the
|
||||||
following environment variable
|
following environment variable
|
||||||
|
|
||||||
```sh
|
```sh
|
||||||
export UE4_ROOT=~/UnrealEngine_4.21
|
export UE4_ROOT=~/UnrealEngine_4.22
|
||||||
```
|
```
|
||||||
|
|
||||||
You can also add this variable to your `~/.bashrc` or `~/.profile`.
|
You can also add this variable to your `~/.bashrc` or `~/.profile`.
|
||||||
|
|
|
@ -15,7 +15,7 @@
|
||||||
|
|
||||||
Also:
|
Also:
|
||||||
|
|
||||||
- [Unreal Engine](https://www.unrealengine.com/download) (v4.21.x)
|
- [Unreal Engine](https://www.unrealengine.com/download) (v4.22.x)
|
||||||
- [Visual Studio](https://www.visualstudio.com/downloads/) (2017)
|
- [Visual Studio](https://www.visualstudio.com/downloads/) (2017)
|
||||||
|
|
||||||
<h3>Environment Setup</h3>
|
<h3>Environment Setup</h3>
|
||||||
|
|
|
@ -158,8 +158,8 @@ for the meshes the last one was referencing.
|
||||||
* Spawners must always be oriented as the road dictates. This must be done by
|
* Spawners must always be oriented as the road dictates. This must be done by
|
||||||
hand but we will make some form of automation in the future.
|
hand but we will make some form of automation in the future.
|
||||||
|
|
||||||
* Traffic lights must be placed in every crossing in whitch vehicles might
|
* Traffic lights must be placed in every crossing in which vehicles might
|
||||||
conflict. theese are the childs of "TrafficLightBase" and are stored inside
|
conflict. These are the childs of "TrafficLightBase" and are stored inside
|
||||||
Game/Carla/Static/TrafficSigns/Streetlights_01.
|
Game/Carla/Static/TrafficSigns/Streetlights_01.
|
||||||
|
|
||||||
Again, remember this is just needed to use the server autopilot. We recommend to start using new the client driving stack provided from 0.9.2 version.
|
Again, remember this is just needed to use the server autopilot. We recommend to start using new the client driving stack provided from 0.9.2 version.
|
||||||
|
|
|
@ -10,7 +10,6 @@
|
||||||
* [Getting started](getting_started.md)
|
* [Getting started](getting_started.md)
|
||||||
* [Python API tutorial](python_api_tutorial.md)
|
* [Python API tutorial](python_api_tutorial.md)
|
||||||
* [Configuring the simulation](configuring_the_simulation.md)
|
* [Configuring the simulation](configuring_the_simulation.md)
|
||||||
<!-- * [Measurements](measurements.md) -->
|
|
||||||
* [Cameras and sensors](cameras_and_sensors.md)
|
* [Cameras and sensors](cameras_and_sensors.md)
|
||||||
* [F.A.Q.](faq.md)
|
* [F.A.Q.](faq.md)
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
mkdocs >= 1.0
|
|
@ -4,8 +4,8 @@ BINDIR=$(CURDIR)/bin
|
||||||
INSTALLDIR=$(CURDIR)/libcarla-install
|
INSTALLDIR=$(CURDIR)/libcarla-install
|
||||||
TOOLCHAIN=$(CURDIR)/ToolChain.cmake
|
TOOLCHAIN=$(CURDIR)/ToolChain.cmake
|
||||||
|
|
||||||
CC=/usr/bin/clang-6.0
|
CC=/usr/bin/clang-7
|
||||||
CXX=/usr/bin/clang++-6.0
|
CXX=/usr/bin/clang++-7
|
||||||
CXXFLAGS=-std=c++14 -pthread -fPIC -O3 -DNDEBUG -Werror -Wall -Wextra
|
CXXFLAGS=-std=c++14 -pthread -fPIC -O3 -DNDEBUG -Werror -Wall -Wextra
|
||||||
|
|
||||||
define log
|
define log
|
||||||
|
|
|
@ -24,8 +24,8 @@ want
|
||||||
|
|
||||||
```cmake
|
```cmake
|
||||||
# Example ToolChain.cmake
|
# Example ToolChain.cmake
|
||||||
set(CMAKE_C_COMPILER /usr/bin/clang-6.0)
|
set(CMAKE_C_COMPILER /usr/bin/clang-7)
|
||||||
set(CMAKE_CXX_COMPILER /usr/bin/clang++-6.0)
|
set(CMAKE_CXX_COMPILER /usr/bin/clang++-7)
|
||||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -DNDEBUG" CACHE STRING "" FORCE)
|
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -DNDEBUG" CACHE STRING "" FORCE)
|
||||||
```
|
```
|
||||||
|
|
||||||
|
|
|
@ -2,7 +2,7 @@ pipeline {
|
||||||
agent any
|
agent any
|
||||||
|
|
||||||
environment {
|
environment {
|
||||||
UE4_ROOT = '/var/lib/jenkins/UnrealEngine_4.21'
|
UE4_ROOT = '/var/lib/jenkins/UnrealEngine_4.22'
|
||||||
}
|
}
|
||||||
|
|
||||||
options {
|
options {
|
||||||
|
@ -65,7 +65,7 @@ pipeline {
|
||||||
|
|
||||||
stage('Smoke Tests') {
|
stage('Smoke Tests') {
|
||||||
steps {
|
steps {
|
||||||
sh 'DISPLAY= ./Dist/*/LinuxNoEditor/CarlaUE4.sh --carla-rpc-port=3654 --carla-streaming-port=0 > CarlaUE4.log &'
|
sh 'DISPLAY= ./Dist/*/LinuxNoEditor/CarlaUE4.sh --carla-rpc-port=3654 --carla-streaming-port=0 -nosound > CarlaUE4.log &'
|
||||||
sh 'make smoke_tests ARGS="--xml"'
|
sh 'make smoke_tests ARGS="--xml"'
|
||||||
sh 'make run-examples ARGS="localhost 3654"'
|
sh 'make run-examples ARGS="localhost 3654"'
|
||||||
}
|
}
|
||||||
|
|
|
@ -9,7 +9,7 @@ install(FILES ${libcarla_carla_rpclib} DESTINATION lib)
|
||||||
# Install boost headers (install in system folder to avoid extra warnings).
|
# Install boost headers (install in system folder to avoid extra warnings).
|
||||||
# @todo Remove boost from public interface of LibCarla.client.
|
# @todo Remove boost from public interface of LibCarla.client.
|
||||||
install(DIRECTORY "${BOOST_INCLUDE_PATH}/boost" DESTINATION include/system)
|
install(DIRECTORY "${BOOST_INCLUDE_PATH}/boost" DESTINATION include/system)
|
||||||
file(GLOB boost_libraries "${BOOST_LIB_PATH}/*")
|
file(GLOB boost_libraries "${BOOST_LIB_PATH}/*.*")
|
||||||
install(FILES ${boost_libraries} DESTINATION lib)
|
install(FILES ${boost_libraries} DESTINATION lib)
|
||||||
|
|
||||||
# Windows need libpng alongside with zlib to be installed
|
# Windows need libpng alongside with zlib to be installed
|
||||||
|
|
|
@ -10,6 +10,20 @@
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
|
namespace detail {
|
||||||
|
|
||||||
|
template <typename FunctorT>
|
||||||
|
struct MoveWrapper : FunctorT {
|
||||||
|
MoveWrapper(FunctorT &&f) : FunctorT(std::move(f)) {}
|
||||||
|
|
||||||
|
MoveWrapper(MoveWrapper &&) = default;
|
||||||
|
MoveWrapper& operator=(MoveWrapper &&) = default;
|
||||||
|
|
||||||
|
MoveWrapper(const MoveWrapper &);
|
||||||
|
MoveWrapper& operator=(const MoveWrapper &);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace detail
|
||||||
|
|
||||||
/// Hack to trick asio into accepting move-only handlers, if the handler were
|
/// Hack to trick asio into accepting move-only handlers, if the handler were
|
||||||
/// actually copied it would result in a link error.
|
/// actually copied it would result in a link error.
|
||||||
|
@ -18,14 +32,7 @@ namespace carla {
|
||||||
template <typename FunctorT>
|
template <typename FunctorT>
|
||||||
auto MoveHandler(FunctorT &&func) {
|
auto MoveHandler(FunctorT &&func) {
|
||||||
using F = typename std::decay<FunctorT>::type;
|
using F = typename std::decay<FunctorT>::type;
|
||||||
struct MoveWrapper : F {
|
return detail::MoveWrapper<F>{std::move(func)};
|
||||||
MoveWrapper(F&& f) : F(std::move(f)) {}
|
|
||||||
MoveWrapper(MoveWrapper&&) = default;
|
|
||||||
MoveWrapper& operator=(MoveWrapper&&) = default;
|
|
||||||
MoveWrapper(const MoveWrapper&);
|
|
||||||
MoveWrapper& operator=(const MoveWrapper&);
|
|
||||||
};
|
|
||||||
return MoveWrapper{std::move(func)};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace carla
|
} // namespace carla
|
||||||
|
|
|
@ -11,7 +11,7 @@
|
||||||
#include "carla/ThreadGroup.h"
|
#include "carla/ThreadGroup.h"
|
||||||
#include "carla/Time.h"
|
#include "carla/Time.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
|
|
||||||
#include <future>
|
#include <future>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
|
@ -19,20 +19,20 @@
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
|
|
||||||
/// A thread pool based on Boost.Asio's io service.
|
/// A thread pool based on Boost.Asio's io context.
|
||||||
class ThreadPool : private NonCopyable {
|
class ThreadPool : private NonCopyable {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
ThreadPool() : _work_to_do(_io_service) {}
|
ThreadPool() : _work_to_do(_io_context) {}
|
||||||
|
|
||||||
/// Stops the ThreadPool and joins all its threads.
|
/// Stops the ThreadPool and joins all its threads.
|
||||||
~ThreadPool() {
|
~ThreadPool() {
|
||||||
Stop();
|
Stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Return the underlying io service.
|
/// Return the underlying io_context.
|
||||||
auto &service() {
|
auto &io_context() {
|
||||||
return _io_service;
|
return _io_context;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Post a task to the pool.
|
/// Post a task to the pool.
|
||||||
|
@ -40,7 +40,7 @@ namespace carla {
|
||||||
std::future<ResultT> Post(FunctorT &&functor) {
|
std::future<ResultT> Post(FunctorT &&functor) {
|
||||||
auto task = std::packaged_task<ResultT()>(std::forward<FunctorT>(functor));
|
auto task = std::packaged_task<ResultT()>(std::forward<FunctorT>(functor));
|
||||||
auto future = task.get_future();
|
auto future = task.get_future();
|
||||||
_io_service.post(carla::MoveHandler(task));
|
_io_context.post(carla::MoveHandler(task));
|
||||||
return future;
|
return future;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -60,7 +60,7 @@ namespace carla {
|
||||||
///
|
///
|
||||||
/// @warning This function blocks until the ThreadPool has been stopped.
|
/// @warning This function blocks until the ThreadPool has been stopped.
|
||||||
void Run() {
|
void Run() {
|
||||||
_io_service.run();
|
_io_context.run();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Run tasks in this thread for an specific @a duration.
|
/// Run tasks in this thread for an specific @a duration.
|
||||||
|
@ -68,20 +68,20 @@ namespace carla {
|
||||||
/// @warning This function blocks until the ThreadPool has been stopped, or
|
/// @warning This function blocks until the ThreadPool has been stopped, or
|
||||||
/// until the specified time duration has elapsed.
|
/// until the specified time duration has elapsed.
|
||||||
void RunFor(time_duration duration) {
|
void RunFor(time_duration duration) {
|
||||||
_io_service.run_for(duration.to_chrono());
|
_io_context.run_for(duration.to_chrono());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Stop the ThreadPool and join all its threads.
|
/// Stop the ThreadPool and join all its threads.
|
||||||
void Stop() {
|
void Stop() {
|
||||||
_io_service.stop();
|
_io_context.stop();
|
||||||
_workers.JoinAll();
|
_workers.JoinAll();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
boost::asio::io_service _io_service;
|
boost::asio::io_context _io_context;
|
||||||
|
|
||||||
boost::asio::io_service::work _work_to_do;
|
boost::asio::io_context::work _work_to_do;
|
||||||
|
|
||||||
ThreadGroup _workers;
|
ThreadGroup _workers;
|
||||||
};
|
};
|
||||||
|
|
|
@ -257,23 +257,23 @@ namespace road {
|
||||||
RELEASE_ASSERT(waypoint.lane_id >= lanes.begin()->first);
|
RELEASE_ASSERT(waypoint.lane_id >= lanes.begin()->first);
|
||||||
RELEASE_ASSERT(waypoint.lane_id <= lanes.rbegin()->first);
|
RELEASE_ASSERT(waypoint.lane_id <= lanes.rbegin()->first);
|
||||||
|
|
||||||
double lane_width = 0;
|
float lane_width = 0.0f;
|
||||||
double lane_tangent = 0;
|
float lane_tangent = 0.0f;
|
||||||
if (waypoint.lane_id < 0) {
|
if (waypoint.lane_id < 0) {
|
||||||
// right lane
|
// right lane
|
||||||
const auto side_lanes = MakeListView(
|
const auto side_lanes = MakeListView(
|
||||||
std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
|
std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
|
||||||
const auto computed_width =
|
const auto computed_width =
|
||||||
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
|
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
|
||||||
lane_width = computed_width.first;
|
lane_width = static_cast<float>(computed_width.first);
|
||||||
lane_tangent = computed_width.second;
|
lane_tangent = static_cast<float>(computed_width.second);
|
||||||
} else {
|
} else {
|
||||||
// left lane
|
// left lane
|
||||||
const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end());
|
const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end());
|
||||||
const auto computed_width =
|
const auto computed_width =
|
||||||
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
|
ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id);
|
||||||
lane_width = computed_width.first;
|
lane_width = static_cast<float>(computed_width.first);
|
||||||
lane_tangent = computed_width.second;
|
lane_tangent = static_cast<float>(computed_width.second);
|
||||||
}
|
}
|
||||||
|
|
||||||
// get a directed point in s and apply the computed lateral offet
|
// get a directed point in s and apply the computed lateral offet
|
||||||
|
@ -281,7 +281,7 @@ namespace road {
|
||||||
|
|
||||||
// compute the tangent of the laneOffset
|
// compute the tangent of the laneOffset
|
||||||
const auto lane_offset_info = road.GetInfo<RoadInfoLaneOffset>(waypoint.s);
|
const auto lane_offset_info = road.GetInfo<RoadInfoLaneOffset>(waypoint.s);
|
||||||
const auto lane_offset_tangent = lane_offset_info->GetPolynomial().Tangent(waypoint.s);
|
const auto lane_offset_tangent = static_cast<float>(lane_offset_info->GetPolynomial().Tangent(waypoint.s));
|
||||||
|
|
||||||
lane_tangent -= lane_offset_tangent;
|
lane_tangent -= lane_offset_tangent;
|
||||||
|
|
||||||
|
|
|
@ -184,7 +184,7 @@ namespace road {
|
||||||
const auto geometry = _info.GetInfo<element::RoadInfoGeometry>(clamped_s);
|
const auto geometry = _info.GetInfo<element::RoadInfoGeometry>(clamped_s);
|
||||||
|
|
||||||
const auto lane_offset = _info.GetInfo<element::RoadInfoLaneOffset>(clamped_s);
|
const auto lane_offset = _info.GetInfo<element::RoadInfoLaneOffset>(clamped_s);
|
||||||
const auto offset = lane_offset->GetPolynomial().Evaluate(clamped_s);
|
const auto offset = static_cast<float>(lane_offset->GetPolynomial().Evaluate(clamped_s));
|
||||||
|
|
||||||
// Apply road's lane offset record
|
// Apply road's lane offset record
|
||||||
element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance());
|
element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance());
|
||||||
|
@ -244,7 +244,7 @@ namespace road {
|
||||||
DirectedPoint current_dp = dp_lane_zero;
|
DirectedPoint current_dp = dp_lane_zero;
|
||||||
for (const auto &lane : right_lanes) {
|
for (const auto &lane : right_lanes) {
|
||||||
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
||||||
const auto half_width = lane_width_info->GetPolynomial().Evaluate(s) * 0.5;
|
const auto half_width = static_cast<float>(lane_width_info->GetPolynomial().Evaluate(s)) * 0.5f;
|
||||||
|
|
||||||
current_dp.ApplyLateralOffset(half_width);
|
current_dp.ApplyLateralOffset(half_width);
|
||||||
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
||||||
|
@ -267,7 +267,7 @@ namespace road {
|
||||||
current_dp = dp_lane_zero;
|
current_dp = dp_lane_zero;
|
||||||
for (const auto &lane : left_lanes) {
|
for (const auto &lane : left_lanes) {
|
||||||
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
|
||||||
const auto half_width = -lane_width_info->GetPolynomial().Evaluate(s) * 0.5;
|
const auto half_width = -static_cast<float>(lane_width_info->GetPolynomial().Evaluate(s)) * 0.5f;
|
||||||
|
|
||||||
current_dp.ApplyLateralOffset(half_width);
|
current_dp.ApplyLateralOffset(half_width);
|
||||||
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
const auto current_dist = geom::Math::Distance(current_dp.location, loc);
|
||||||
|
|
|
@ -41,10 +41,10 @@ namespace element {
|
||||||
double tangent = 0.0; // [radians]
|
double tangent = 0.0; // [radians]
|
||||||
double pitch = 0.0; // [radians]
|
double pitch = 0.0; // [radians]
|
||||||
|
|
||||||
void ApplyLateralOffset(double lateral_offset) {
|
void ApplyLateralOffset(float lateral_offset) {
|
||||||
/// @todo Z axis??
|
/// @todo Z axis??
|
||||||
auto normal_x = std::sin(tangent);
|
auto normal_x = std::sin(static_cast<float>(tangent));
|
||||||
auto normal_y = -std::cos(tangent);
|
auto normal_y = -std::cos(static_cast<float>(tangent));
|
||||||
location.x += lateral_offset * normal_x;
|
location.x += lateral_offset * normal_x;
|
||||||
location.y += lateral_offset * normal_y;
|
location.y += lateral_offset * normal_y;
|
||||||
}
|
}
|
||||||
|
@ -117,11 +117,11 @@ namespace element {
|
||||||
: Geometry(GeometryType::LINE, start_offset, length, heading, start_pos) {}
|
: Geometry(GeometryType::LINE, start_offset, length, heading, start_pos) {}
|
||||||
|
|
||||||
DirectedPoint PosFromDist(double dist) const override {
|
DirectedPoint PosFromDist(double dist) const override {
|
||||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
|
||||||
DEBUG_ASSERT(_length > 0.0);
|
DEBUG_ASSERT(_length > 0.0);
|
||||||
|
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||||
DirectedPoint p(_start_position, _heading);
|
DirectedPoint p(_start_position, _heading);
|
||||||
p.location.x += dist * std::cos(p.tangent);
|
p.location.x += static_cast<float>(dist * std::cos(p.tangent));
|
||||||
p.location.y += dist * std::sin(p.tangent);
|
p.location.y += static_cast<float>(dist * std::sin(p.tangent));
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -159,11 +159,11 @@ namespace element {
|
||||||
const double radius = 1.0 / _curvature;
|
const double radius = 1.0 / _curvature;
|
||||||
constexpr double pi_half = geom::Math::Pi<double>() / 2.0;
|
constexpr double pi_half = geom::Math::Pi<double>() / 2.0;
|
||||||
DirectedPoint p(_start_position, _heading);
|
DirectedPoint p(_start_position, _heading);
|
||||||
p.location.x += radius * std::cos(p.tangent + pi_half);
|
p.location.x += static_cast<float>(radius * std::cos(p.tangent + pi_half));
|
||||||
p.location.y += radius * std::sin(p.tangent + pi_half);
|
p.location.y += static_cast<float>(radius * std::sin(p.tangent + pi_half));
|
||||||
p.tangent += dist * _curvature;
|
p.tangent += dist * _curvature;
|
||||||
p.location.x -= radius * std::cos(p.tangent + pi_half);
|
p.location.x -= static_cast<float>(radius * std::cos(p.tangent + pi_half));
|
||||||
p.location.y -= radius * std::sin(p.tangent + pi_half);
|
p.location.y -= static_cast<float>(radius * std::sin(p.tangent + pi_half));
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -228,8 +228,8 @@ namespace element {
|
||||||
DirectedPoint p(_start_position, _heading);
|
DirectedPoint p(_start_position, _heading);
|
||||||
const double cos_a = std::cos(p.tangent);
|
const double cos_a = std::cos(p.tangent);
|
||||||
const double sin_a = std::sin(p.tangent);
|
const double sin_a = std::sin(p.tangent);
|
||||||
p.location.x += C * cos_a - S * sin_a;
|
p.location.x += static_cast<float>(C * cos_a - S * sin_a);
|
||||||
p.location.y += S * cos_a + C * sin_a;
|
p.location.y += static_cast<float>(S * cos_a + C * sin_a);
|
||||||
p.tangent += length * length;
|
p.tangent += length * length;
|
||||||
|
|
||||||
return p;
|
return p;
|
||||||
|
|
|
@ -11,7 +11,7 @@
|
||||||
#include "carla/rpc/Metadata.h"
|
#include "carla/rpc/Metadata.h"
|
||||||
#include "carla/rpc/Response.h"
|
#include "carla/rpc/Response.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
|
|
||||||
#include <rpc/server.h>
|
#include <rpc/server.h>
|
||||||
|
|
||||||
|
@ -50,8 +50,8 @@ namespace rpc {
|
||||||
}
|
}
|
||||||
|
|
||||||
void SyncRunFor(time_duration duration) {
|
void SyncRunFor(time_duration duration) {
|
||||||
_sync_io_service.reset();
|
_sync_io_context.reset();
|
||||||
_sync_io_service.run_for(duration.to_chrono());
|
_sync_io_context.run_for(duration.to_chrono());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @warning does not stop the game thread.
|
/// @warning does not stop the game thread.
|
||||||
|
@ -61,7 +61,7 @@ namespace rpc {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
boost::asio::io_service _sync_io_service;
|
boost::asio::io_context _sync_io_context;
|
||||||
|
|
||||||
::rpc::server _server;
|
::rpc::server _server;
|
||||||
};
|
};
|
||||||
|
@ -92,15 +92,15 @@ namespace detail {
|
||||||
|
|
||||||
/// Wraps @a functor into a function type with equivalent signature. The
|
/// Wraps @a functor into a function type with equivalent signature. The
|
||||||
/// wrap function returned. When called, posts @a functor into the
|
/// wrap function returned. When called, posts @a functor into the
|
||||||
/// io_service; if the client called this method synchronously, waits for
|
/// io_context; if the client called this method synchronously, waits for
|
||||||
/// the posted task to finish, otherwise returns immediately.
|
/// the posted task to finish, otherwise returns immediately.
|
||||||
///
|
///
|
||||||
/// This way, no matter from which thread the wrap function is called, the
|
/// This way, no matter from which thread the wrap function is called, the
|
||||||
/// @a functor provided is always called from the context of the io_service.
|
/// @a functor provided is always called from the context of the io_context.
|
||||||
/// I.e., we can use the io_service to run tasks on a specific thread (e.g.
|
/// I.e., we can use the io_context to run tasks on a specific thread (e.g.
|
||||||
/// game thread).
|
/// game thread).
|
||||||
template <typename FuncT>
|
template <typename FuncT>
|
||||||
static auto WrapSyncCall(boost::asio::io_service &io, FuncT &&functor) {
|
static auto WrapSyncCall(boost::asio::io_context &io, FuncT &&functor) {
|
||||||
return [&io, functor=std::forward<FuncT>(functor)](Metadata metadata, Args... args) -> R {
|
return [&io, functor=std::forward<FuncT>(functor)](Metadata metadata, Args... args) -> R {
|
||||||
auto task = std::packaged_task<R()>([functor=std::move(functor), args...]() {
|
auto task = std::packaged_task<R()>([functor=std::move(functor), args...]() {
|
||||||
return functor(args...);
|
return functor(args...);
|
||||||
|
@ -147,7 +147,7 @@ namespace detail {
|
||||||
using Wrapper = detail::FunctionWrapper<FunctorT>;
|
using Wrapper = detail::FunctionWrapper<FunctorT>;
|
||||||
_server.bind(
|
_server.bind(
|
||||||
name,
|
name,
|
||||||
Wrapper::WrapSyncCall(_sync_io_service, std::forward<FunctorT>(functor)));
|
Wrapper::WrapSyncCall(_sync_io_context, std::forward<FunctorT>(functor)));
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename FunctorT>
|
template <typename FunctorT>
|
||||||
|
|
|
@ -12,7 +12,7 @@
|
||||||
#include "carla/streaming/detail/tcp/Client.h"
|
#include "carla/streaming/detail/tcp/Client.h"
|
||||||
#include "carla/streaming/low_level/Client.h"
|
#include "carla/streaming/low_level/Client.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace streaming {
|
namespace streaming {
|
||||||
|
@ -37,7 +37,7 @@ namespace streaming {
|
||||||
/// MultiStream).
|
/// MultiStream).
|
||||||
template <typename Functor>
|
template <typename Functor>
|
||||||
void Subscribe(const Token &token, Functor &&callback) {
|
void Subscribe(const Token &token, Functor &&callback) {
|
||||||
_client.Subscribe(_service.service(), token, std::forward<Functor>(callback));
|
_client.Subscribe(_service.io_context(), token, std::forward<Functor>(callback));
|
||||||
}
|
}
|
||||||
|
|
||||||
void UnSubscribe(const Token &token) {
|
void UnSubscribe(const Token &token) {
|
||||||
|
|
|
@ -6,7 +6,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
#include <boost/asio/ip/address.hpp>
|
#include <boost/asio/ip/address.hpp>
|
||||||
#include <boost/asio/ip/tcp.hpp>
|
#include <boost/asio/ip/tcp.hpp>
|
||||||
#include <boost/asio/ip/udp.hpp>
|
#include <boost/asio/ip/udp.hpp>
|
||||||
|
@ -73,8 +73,8 @@ namespace detail {
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline auto make_address(const std::string &address) {
|
static inline auto make_address(const std::string &address) {
|
||||||
boost::asio::io_service io_service;
|
boost::asio::io_context io_context;
|
||||||
boost::asio::ip::tcp::resolver resolver(io_service);
|
boost::asio::ip::tcp::resolver resolver(io_context);
|
||||||
boost::asio::ip::tcp::resolver::query query(boost::asio::ip::tcp::v4(), address, "", boost::asio::ip::tcp::resolver::query::canonical_name);
|
boost::asio::ip::tcp::resolver::query query(boost::asio::ip::tcp::v4(), address, "", boost::asio::ip::tcp::resolver::query::canonical_name);
|
||||||
boost::asio::ip::tcp::resolver::iterator iter = resolver.resolve(query);
|
boost::asio::ip::tcp::resolver::iterator iter = resolver.resolve(query);
|
||||||
boost::asio::ip::tcp::resolver::iterator end;
|
boost::asio::ip::tcp::resolver::iterator end;
|
||||||
|
|
|
@ -10,7 +10,7 @@
|
||||||
#include "carla/streaming/detail/tcp/Server.h"
|
#include "carla/streaming/detail/tcp/Server.h"
|
||||||
#include "carla/streaming/low_level/Server.h"
|
#include "carla/streaming/low_level/Server.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace streaming {
|
namespace streaming {
|
||||||
|
@ -23,21 +23,21 @@ namespace streaming {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
explicit Server(uint16_t port)
|
explicit Server(uint16_t port)
|
||||||
: _server(_service.service(), make_endpoint<protocol_type>(port)) {}
|
: _server(_pool.io_context(), make_endpoint<protocol_type>(port)) {}
|
||||||
|
|
||||||
explicit Server(const std::string &address, uint16_t port)
|
explicit Server(const std::string &address, uint16_t port)
|
||||||
: _server(_service.service(), make_endpoint<protocol_type>(address, port)) {}
|
: _server(_pool.io_context(), make_endpoint<protocol_type>(address, port)) {}
|
||||||
|
|
||||||
explicit Server(
|
explicit Server(
|
||||||
const std::string &address, uint16_t port,
|
const std::string &address, uint16_t port,
|
||||||
const std::string &external_address, uint16_t external_port)
|
const std::string &external_address, uint16_t external_port)
|
||||||
: _server(
|
: _server(
|
||||||
_service.service(),
|
_pool.io_context(),
|
||||||
make_endpoint<protocol_type>(address, port),
|
make_endpoint<protocol_type>(address, port),
|
||||||
make_endpoint<protocol_type>(external_address, external_port)) {}
|
make_endpoint<protocol_type>(external_address, external_port)) {}
|
||||||
|
|
||||||
~Server() {
|
~Server() {
|
||||||
_service.Stop();
|
_pool.Stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
auto GetLocalEndpoint() const {
|
auto GetLocalEndpoint() const {
|
||||||
|
@ -57,18 +57,18 @@ namespace streaming {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Run() {
|
void Run() {
|
||||||
_service.Run();
|
_pool.Run();
|
||||||
}
|
}
|
||||||
|
|
||||||
void AsyncRun(size_t worker_threads) {
|
void AsyncRun(size_t worker_threads) {
|
||||||
_service.AsyncRun(worker_threads);
|
_pool.AsyncRun(worker_threads);
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
// The order of these two arguments is very important.
|
// The order of these two arguments is very important.
|
||||||
|
|
||||||
ThreadPool _service;
|
ThreadPool _pool;
|
||||||
|
|
||||||
underlying_server _server;
|
underlying_server _server;
|
||||||
};
|
};
|
||||||
|
|
|
@ -29,7 +29,7 @@ namespace detail {
|
||||||
|
|
||||||
Dispatcher::~Dispatcher() {
|
Dispatcher::~Dispatcher() {
|
||||||
// Disconnect all the sessions from their streams, this should kill any
|
// Disconnect all the sessions from their streams, this should kill any
|
||||||
// session remaining since at this point the io_service should be already
|
// session remaining since at this point the io_context should be already
|
||||||
// stopped.
|
// stopped.
|
||||||
for (auto &pair : _stream_map) {
|
for (auto &pair : _stream_map) {
|
||||||
#ifndef LIBCARLA_NO_EXCEPTIONS
|
#ifndef LIBCARLA_NO_EXCEPTIONS
|
||||||
|
|
|
@ -64,16 +64,16 @@ namespace tcp {
|
||||||
// ===========================================================================
|
// ===========================================================================
|
||||||
|
|
||||||
Client::Client(
|
Client::Client(
|
||||||
boost::asio::io_service &io_service,
|
boost::asio::io_context &io_context,
|
||||||
const token_type &token,
|
const token_type &token,
|
||||||
callback_function_type callback)
|
callback_function_type callback)
|
||||||
: LIBCARLA_INITIALIZE_LIFETIME_PROFILER(
|
: LIBCARLA_INITIALIZE_LIFETIME_PROFILER(
|
||||||
std::string("tcp client ") + std::to_string(token.get_stream_id())),
|
std::string("tcp client ") + std::to_string(token.get_stream_id())),
|
||||||
_token(token),
|
_token(token),
|
||||||
_callback(std::move(callback)),
|
_callback(std::move(callback)),
|
||||||
_socket(io_service),
|
_socket(io_context),
|
||||||
_strand(io_service),
|
_strand(io_context),
|
||||||
_connection_timer(io_service),
|
_connection_timer(io_context),
|
||||||
_buffer_pool(std::make_shared<BufferPool>()) {
|
_buffer_pool(std::make_shared<BufferPool>()) {
|
||||||
if (!_token.protocol_is_tcp()) {
|
if (!_token.protocol_is_tcp()) {
|
||||||
throw_exception(std::invalid_argument("invalid token, only TCP tokens supported"));
|
throw_exception(std::invalid_argument("invalid token, only TCP tokens supported"));
|
||||||
|
@ -173,7 +173,7 @@ namespace tcp {
|
||||||
// Move the buffer to the callback function and start reading the next
|
// Move the buffer to the callback function and start reading the next
|
||||||
// piece of data.
|
// piece of data.
|
||||||
log_debug("streaming client: success reading data, calling the callback");
|
log_debug("streaming client: success reading data, calling the callback");
|
||||||
_socket.get_io_service().post([self, message]() { self->_callback(message->pop()); });
|
_strand.context().post([self, message]() { self->_callback(message->pop()); });
|
||||||
ReadData();
|
ReadData();
|
||||||
} else {
|
} else {
|
||||||
// As usual, if anything fails start over from the very top.
|
// As usual, if anything fails start over from the very top.
|
||||||
|
|
|
@ -13,7 +13,7 @@
|
||||||
#include "carla/streaming/detail/Types.h"
|
#include "carla/streaming/detail/Types.h"
|
||||||
|
|
||||||
#include <boost/asio/deadline_timer.hpp>
|
#include <boost/asio/deadline_timer.hpp>
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
#include <boost/asio/ip/tcp.hpp>
|
#include <boost/asio/ip/tcp.hpp>
|
||||||
#include <boost/asio/strand.hpp>
|
#include <boost/asio/strand.hpp>
|
||||||
|
|
||||||
|
@ -44,7 +44,7 @@ namespace tcp {
|
||||||
using callback_function_type = std::function<void (Buffer)>;
|
using callback_function_type = std::function<void (Buffer)>;
|
||||||
|
|
||||||
Client(
|
Client(
|
||||||
boost::asio::io_service &io_service,
|
boost::asio::io_context &io_context,
|
||||||
const token_type &token,
|
const token_type &token,
|
||||||
callback_function_type callback);
|
callback_function_type callback);
|
||||||
|
|
||||||
|
@ -70,7 +70,7 @@ namespace tcp {
|
||||||
|
|
||||||
boost::asio::ip::tcp::socket _socket;
|
boost::asio::ip::tcp::socket _socket;
|
||||||
|
|
||||||
boost::asio::io_service::strand _strand;
|
boost::asio::io_context::strand _strand;
|
||||||
|
|
||||||
boost::asio::deadline_timer _connection_timer;
|
boost::asio::deadline_timer _connection_timer;
|
||||||
|
|
||||||
|
|
|
@ -15,8 +15,9 @@ namespace streaming {
|
||||||
namespace detail {
|
namespace detail {
|
||||||
namespace tcp {
|
namespace tcp {
|
||||||
|
|
||||||
Server::Server(boost::asio::io_service &io_service, endpoint ep)
|
Server::Server(boost::asio::io_context &io_context, endpoint ep)
|
||||||
: _acceptor(io_service, std::move(ep)),
|
: _io_context(io_context),
|
||||||
|
_acceptor(_io_context, std::move(ep)),
|
||||||
_timeout(time_duration::seconds(10u)) {}
|
_timeout(time_duration::seconds(10u)) {}
|
||||||
|
|
||||||
void Server::OpenSession(
|
void Server::OpenSession(
|
||||||
|
@ -25,7 +26,7 @@ namespace tcp {
|
||||||
ServerSession::callback_function_type on_closed) {
|
ServerSession::callback_function_type on_closed) {
|
||||||
using boost::system::error_code;
|
using boost::system::error_code;
|
||||||
|
|
||||||
auto session = std::make_shared<ServerSession>(_acceptor.get_io_service(), timeout);
|
auto session = std::make_shared<ServerSession>(_io_context, timeout);
|
||||||
|
|
||||||
auto handle_query = [on_opened, on_closed, session](const error_code &ec) {
|
auto handle_query = [on_opened, on_closed, session](const error_code &ec) {
|
||||||
if (!ec) {
|
if (!ec) {
|
||||||
|
@ -37,7 +38,7 @@ namespace tcp {
|
||||||
|
|
||||||
_acceptor.async_accept(session->_socket, [=](error_code ec) {
|
_acceptor.async_accept(session->_socket, [=](error_code ec) {
|
||||||
// Handle query and open a new session immediately.
|
// Handle query and open a new session immediately.
|
||||||
_acceptor.get_io_service().post([=]() { handle_query(ec); });
|
_io_context.post([=]() { handle_query(ec); });
|
||||||
OpenSession(timeout, on_opened, on_closed);
|
OpenSession(timeout, on_opened, on_closed);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
|
@ -10,7 +10,7 @@
|
||||||
#include "carla/Time.h"
|
#include "carla/Time.h"
|
||||||
#include "carla/streaming/detail/tcp/ServerSession.h"
|
#include "carla/streaming/detail/tcp/ServerSession.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
#include <boost/asio/ip/tcp.hpp>
|
#include <boost/asio/ip/tcp.hpp>
|
||||||
|
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
|
@ -20,7 +20,7 @@ namespace streaming {
|
||||||
namespace detail {
|
namespace detail {
|
||||||
namespace tcp {
|
namespace tcp {
|
||||||
|
|
||||||
/// @warning This server cannot be destructed before its @a io_service is
|
/// @warning This server cannot be destructed before its @a io_context is
|
||||||
/// stopped.
|
/// stopped.
|
||||||
class Server : private NonCopyable {
|
class Server : private NonCopyable {
|
||||||
public:
|
public:
|
||||||
|
@ -28,7 +28,7 @@ namespace tcp {
|
||||||
using endpoint = boost::asio::ip::tcp::endpoint;
|
using endpoint = boost::asio::ip::tcp::endpoint;
|
||||||
using protocol_type = endpoint::protocol_type;
|
using protocol_type = endpoint::protocol_type;
|
||||||
|
|
||||||
explicit Server(boost::asio::io_service &io_service, endpoint ep);
|
explicit Server(boost::asio::io_context &io_context, endpoint ep);
|
||||||
|
|
||||||
endpoint GetLocalEndpoint() const {
|
endpoint GetLocalEndpoint() const {
|
||||||
return _acceptor.local_endpoint();
|
return _acceptor.local_endpoint();
|
||||||
|
@ -45,7 +45,7 @@ namespace tcp {
|
||||||
/// is closed.
|
/// is closed.
|
||||||
template <typename FunctorT1, typename FunctorT2>
|
template <typename FunctorT1, typename FunctorT2>
|
||||||
void Listen(FunctorT1 on_session_opened, FunctorT2 on_session_closed) {
|
void Listen(FunctorT1 on_session_opened, FunctorT2 on_session_closed) {
|
||||||
_acceptor.get_io_service().post([=]() {
|
_io_context.post([=]() {
|
||||||
OpenSession(
|
OpenSession(
|
||||||
_timeout,
|
_timeout,
|
||||||
std::move(on_session_opened),
|
std::move(on_session_opened),
|
||||||
|
@ -60,6 +60,8 @@ namespace tcp {
|
||||||
ServerSession::callback_function_type on_session_opened,
|
ServerSession::callback_function_type on_session_opened,
|
||||||
ServerSession::callback_function_type on_session_closed);
|
ServerSession::callback_function_type on_session_closed);
|
||||||
|
|
||||||
|
boost::asio::io_context &_io_context;
|
||||||
|
|
||||||
boost::asio::ip::tcp::acceptor _acceptor;
|
boost::asio::ip::tcp::acceptor _acceptor;
|
||||||
|
|
||||||
std::atomic<time_duration> _timeout;
|
std::atomic<time_duration> _timeout;
|
||||||
|
|
|
@ -22,15 +22,15 @@ namespace tcp {
|
||||||
static std::atomic_size_t SESSION_COUNTER{0u};
|
static std::atomic_size_t SESSION_COUNTER{0u};
|
||||||
|
|
||||||
ServerSession::ServerSession(
|
ServerSession::ServerSession(
|
||||||
boost::asio::io_service &io_service,
|
boost::asio::io_context &io_context,
|
||||||
const time_duration timeout)
|
const time_duration timeout)
|
||||||
: LIBCARLA_INITIALIZE_LIFETIME_PROFILER(
|
: LIBCARLA_INITIALIZE_LIFETIME_PROFILER(
|
||||||
std::string("tcp server session ") + std::to_string(SESSION_COUNTER)),
|
std::string("tcp server session ") + std::to_string(SESSION_COUNTER)),
|
||||||
_session_id(SESSION_COUNTER++),
|
_session_id(SESSION_COUNTER++),
|
||||||
_socket(io_service),
|
_socket(io_context),
|
||||||
_timeout(timeout),
|
_timeout(timeout),
|
||||||
_deadline(io_service),
|
_deadline(io_context),
|
||||||
_strand(io_service) {}
|
_strand(io_context) {}
|
||||||
|
|
||||||
void ServerSession::Open(
|
void ServerSession::Open(
|
||||||
callback_function_type on_opened,
|
callback_function_type on_opened,
|
||||||
|
@ -47,7 +47,7 @@ namespace tcp {
|
||||||
if (!ec) {
|
if (!ec) {
|
||||||
DEBUG_ASSERT_EQ(bytes_received, sizeof(_stream_id));
|
DEBUG_ASSERT_EQ(bytes_received, sizeof(_stream_id));
|
||||||
log_debug("session", _session_id, "for stream", _stream_id, " started");
|
log_debug("session", _session_id, "for stream", _stream_id, " started");
|
||||||
_socket.get_io_service().post([=]() { callback(self); });
|
_strand.context().post([=]() { callback(self); });
|
||||||
} else {
|
} else {
|
||||||
log_error("session", _session_id, ": error retrieving stream id :", ec.message());
|
log_error("session", _session_id, ": error retrieving stream id :", ec.message());
|
||||||
CloseNow();
|
CloseNow();
|
||||||
|
@ -123,7 +123,7 @@ namespace tcp {
|
||||||
if (_socket.is_open()) {
|
if (_socket.is_open()) {
|
||||||
_socket.close();
|
_socket.close();
|
||||||
}
|
}
|
||||||
_socket.get_io_service().post([self=shared_from_this()]() {
|
_strand.context().post([self=shared_from_this()]() {
|
||||||
DEBUG_ASSERT(self->_on_closed);
|
DEBUG_ASSERT(self->_on_closed);
|
||||||
self->_on_closed(self);
|
self->_on_closed(self);
|
||||||
});
|
});
|
||||||
|
|
|
@ -14,7 +14,7 @@
|
||||||
#include "carla/streaming/detail/tcp/Message.h"
|
#include "carla/streaming/detail/tcp/Message.h"
|
||||||
|
|
||||||
#include <boost/asio/deadline_timer.hpp>
|
#include <boost/asio/deadline_timer.hpp>
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
#include <boost/asio/ip/tcp.hpp>
|
#include <boost/asio/ip/tcp.hpp>
|
||||||
#include <boost/asio/strand.hpp>
|
#include <boost/asio/strand.hpp>
|
||||||
|
|
||||||
|
@ -38,7 +38,9 @@ namespace tcp {
|
||||||
using socket_type = boost::asio::ip::tcp::socket;
|
using socket_type = boost::asio::ip::tcp::socket;
|
||||||
using callback_function_type = std::function<void(std::shared_ptr<ServerSession>)>;
|
using callback_function_type = std::function<void(std::shared_ptr<ServerSession>)>;
|
||||||
|
|
||||||
explicit ServerSession(boost::asio::io_service &io_service, time_duration timeout);
|
explicit ServerSession(
|
||||||
|
boost::asio::io_context &io_context,
|
||||||
|
time_duration timeout);
|
||||||
|
|
||||||
/// Starts the session and calls @a on_opened after successfully reading the
|
/// Starts the session and calls @a on_opened after successfully reading the
|
||||||
/// stream id, and @a on_closed once the session is closed.
|
/// stream id, and @a on_closed once the session is closed.
|
||||||
|
@ -90,7 +92,7 @@ namespace tcp {
|
||||||
|
|
||||||
boost::asio::deadline_timer _deadline;
|
boost::asio::deadline_timer _deadline;
|
||||||
|
|
||||||
boost::asio::io_service::strand _strand;
|
boost::asio::io_context::strand _strand;
|
||||||
|
|
||||||
callback_function_type _on_closed;
|
callback_function_type _on_closed;
|
||||||
|
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
#include "carla/streaming/detail/Token.h"
|
#include "carla/streaming/detail/Token.h"
|
||||||
#include "carla/streaming/detail/tcp/Client.h"
|
#include "carla/streaming/detail/tcp/Client.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
|
@ -19,9 +19,9 @@ namespace streaming {
|
||||||
namespace low_level {
|
namespace low_level {
|
||||||
|
|
||||||
/// A client able to subscribe to multiple streams. Accepts an external
|
/// A client able to subscribe to multiple streams. Accepts an external
|
||||||
/// io_service.
|
/// io_context.
|
||||||
///
|
///
|
||||||
/// @warning The client should not be destroyed before the @a io_service is
|
/// @warning The client should not be destroyed before the @a io_context is
|
||||||
/// stopped.
|
/// stopped.
|
||||||
template <typename T>
|
template <typename T>
|
||||||
class Client {
|
class Client {
|
||||||
|
@ -50,7 +50,7 @@ namespace low_level {
|
||||||
/// MultiStream).
|
/// MultiStream).
|
||||||
template <typename Functor>
|
template <typename Functor>
|
||||||
void Subscribe(
|
void Subscribe(
|
||||||
boost::asio::io_service &io_service,
|
boost::asio::io_context &io_context,
|
||||||
token_type token,
|
token_type token,
|
||||||
Functor &&callback) {
|
Functor &&callback) {
|
||||||
DEBUG_ASSERT_EQ(_clients.find(token.get_stream_id()), _clients.end());
|
DEBUG_ASSERT_EQ(_clients.find(token.get_stream_id()), _clients.end());
|
||||||
|
@ -58,7 +58,7 @@ namespace low_level {
|
||||||
token.set_address(_fallback_address);
|
token.set_address(_fallback_address);
|
||||||
}
|
}
|
||||||
auto client = std::make_shared<underlying_client>(
|
auto client = std::make_shared<underlying_client>(
|
||||||
io_service,
|
io_context,
|
||||||
token,
|
token,
|
||||||
std::forward<Functor>(callback));
|
std::forward<Functor>(callback));
|
||||||
client->Connect();
|
client->Connect();
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
#include "carla/streaming/detail/Dispatcher.h"
|
#include "carla/streaming/detail/Dispatcher.h"
|
||||||
#include "carla/streaming/Stream.h"
|
#include "carla/streaming/Stream.h"
|
||||||
|
|
||||||
#include <boost/asio/io_service.hpp>
|
#include <boost/asio/io_context.hpp>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace streaming {
|
namespace streaming {
|
||||||
|
@ -17,9 +17,9 @@ namespace low_level {
|
||||||
|
|
||||||
/// A low-level streaming server. Each new stream has a token associated, this
|
/// A low-level streaming server. Each new stream has a token associated, this
|
||||||
/// token can be used by a client to subscribe to the stream. This server
|
/// token can be used by a client to subscribe to the stream. This server
|
||||||
/// requires an external io_service running.
|
/// requires an external io_context running.
|
||||||
///
|
///
|
||||||
/// @warning This server cannot be destructed before its @a io_service is
|
/// @warning This server cannot be destructed before its @a io_context is
|
||||||
/// stopped.
|
/// stopped.
|
||||||
template <typename T>
|
template <typename T>
|
||||||
class Server {
|
class Server {
|
||||||
|
@ -31,26 +31,26 @@ namespace low_level {
|
||||||
|
|
||||||
template <typename InternalEPType, typename ExternalEPType>
|
template <typename InternalEPType, typename ExternalEPType>
|
||||||
explicit Server(
|
explicit Server(
|
||||||
boost::asio::io_service &io_service,
|
boost::asio::io_context &io_context,
|
||||||
detail::EndPoint<protocol_type, InternalEPType> internal_ep,
|
detail::EndPoint<protocol_type, InternalEPType> internal_ep,
|
||||||
detail::EndPoint<protocol_type, ExternalEPType> external_ep)
|
detail::EndPoint<protocol_type, ExternalEPType> external_ep)
|
||||||
: _server(io_service, std::move(internal_ep)),
|
: _server(io_context, std::move(internal_ep)),
|
||||||
_dispatcher(std::move(external_ep)) {
|
_dispatcher(std::move(external_ep)) {
|
||||||
StartServer();
|
StartServer();
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename InternalEPType>
|
template <typename InternalEPType>
|
||||||
explicit Server(
|
explicit Server(
|
||||||
boost::asio::io_service &io_service,
|
boost::asio::io_context &io_context,
|
||||||
detail::EndPoint<protocol_type, InternalEPType> internal_ep)
|
detail::EndPoint<protocol_type, InternalEPType> internal_ep)
|
||||||
: _server(io_service, std::move(internal_ep)),
|
: _server(io_context, std::move(internal_ep)),
|
||||||
_dispatcher(make_endpoint<protocol_type>(_server.GetLocalEndpoint().port())) {
|
_dispatcher(make_endpoint<protocol_type>(_server.GetLocalEndpoint().port())) {
|
||||||
StartServer();
|
StartServer();
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename... EPArgs>
|
template <typename... EPArgs>
|
||||||
explicit Server(boost::asio::io_service &io_service, EPArgs &&... args)
|
explicit Server(boost::asio::io_context &io_context, EPArgs &&... args)
|
||||||
: Server(io_service, make_endpoint<protocol_type>(std::forward<EPArgs>(args)...)) {}
|
: Server(io_context, make_endpoint<protocol_type>(std::forward<EPArgs>(args)...)) {}
|
||||||
|
|
||||||
typename underlying_server::endpoint GetLocalEndpoint() const {
|
typename underlying_server::endpoint GetLocalEndpoint() const {
|
||||||
return _server.GetLocalEndpoint();
|
return _server.GetLocalEndpoint();
|
||||||
|
|
|
@ -6,16 +6,7 @@
|
||||||
|
|
||||||
#include "Buffer.h"
|
#include "Buffer.h"
|
||||||
|
|
||||||
/// @todo This header uses deprecated functionality, please re-enable
|
#include <boost/random/independent_bits.hpp>
|
||||||
/// pragma-messages after upgrading Boost 1.69 if possible.
|
|
||||||
#if defined(__clang__)
|
|
||||||
# pragma clang diagnostic push
|
|
||||||
# pragma clang diagnostic ignored "-W#pragma-messages"
|
|
||||||
#endif
|
|
||||||
# include <boost/random/independent_bits.hpp>
|
|
||||||
#if defined(__clang__)
|
|
||||||
# pragma clang diagnostic pop
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include <climits>
|
#include <climits>
|
||||||
#include <random>
|
#include <random>
|
||||||
|
|
|
@ -21,23 +21,23 @@ using namespace std::chrono_literals;
|
||||||
|
|
||||||
// This is required for low level to properly stop the threads in case of
|
// This is required for low level to properly stop the threads in case of
|
||||||
// exception/assert.
|
// exception/assert.
|
||||||
class io_service_running {
|
class io_context_running {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
boost::asio::io_service service;
|
boost::asio::io_context service;
|
||||||
|
|
||||||
explicit io_service_running(size_t threads = 2u)
|
explicit io_context_running(size_t threads = 2u)
|
||||||
: _work_to_do(service) {
|
: _work_to_do(service) {
|
||||||
_threads.CreateThreads(threads, [this]() { service.run(); });
|
_threads.CreateThreads(threads, [this]() { service.run(); });
|
||||||
}
|
}
|
||||||
|
|
||||||
~io_service_running() {
|
~io_context_running() {
|
||||||
service.stop();
|
service.stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
boost::asio::io_service::work _work_to_do;
|
boost::asio::io_context::work _work_to_do;
|
||||||
|
|
||||||
carla::ThreadGroup _threads;
|
carla::ThreadGroup _threads;
|
||||||
};
|
};
|
||||||
|
@ -53,7 +53,7 @@ TEST(streaming, low_level_sending_strings) {
|
||||||
|
|
||||||
std::atomic_size_t message_count{0u};
|
std::atomic_size_t message_count{0u};
|
||||||
|
|
||||||
io_service_running io;
|
io_context_running io;
|
||||||
|
|
||||||
Server<tcp::Server> srv(io.service, TESTING_PORT);
|
Server<tcp::Server> srv(io.service, TESTING_PORT);
|
||||||
srv.SetTimeout(1s);
|
srv.SetTimeout(1s);
|
||||||
|
@ -86,7 +86,7 @@ TEST(streaming, low_level_unsubscribing) {
|
||||||
constexpr auto number_of_messages = 50u;
|
constexpr auto number_of_messages = 50u;
|
||||||
const std::string message_text = "Hello client!";
|
const std::string message_text = "Hello client!";
|
||||||
|
|
||||||
io_service_running io;
|
io_context_running io;
|
||||||
|
|
||||||
Server<tcp::Server> srv(io.service, TESTING_PORT);
|
Server<tcp::Server> srv(io.service, TESTING_PORT);
|
||||||
srv.SetTimeout(1s);
|
srv.SetTimeout(1s);
|
||||||
|
@ -124,10 +124,10 @@ TEST(streaming, low_level_tcp_small_message) {
|
||||||
using namespace carla::streaming;
|
using namespace carla::streaming;
|
||||||
using namespace carla::streaming::detail;
|
using namespace carla::streaming::detail;
|
||||||
|
|
||||||
boost::asio::io_service io_service;
|
boost::asio::io_context io_context;
|
||||||
tcp::Server::endpoint ep(boost::asio::ip::tcp::v4(), TESTING_PORT);
|
tcp::Server::endpoint ep(boost::asio::ip::tcp::v4(), TESTING_PORT);
|
||||||
|
|
||||||
tcp::Server srv(io_service, ep);
|
tcp::Server srv(io_context, ep);
|
||||||
srv.SetTimeout(1s);
|
srv.SetTimeout(1s);
|
||||||
std::atomic_bool done{false};
|
std::atomic_bool done{false};
|
||||||
std::atomic_size_t message_count{0u};
|
std::atomic_size_t message_count{0u};
|
||||||
|
@ -145,7 +145,7 @@ TEST(streaming, low_level_tcp_small_message) {
|
||||||
|
|
||||||
Dispatcher dispatcher{make_endpoint<tcp::Client::protocol_type>(srv.GetLocalEndpoint())};
|
Dispatcher dispatcher{make_endpoint<tcp::Client::protocol_type>(srv.GetLocalEndpoint())};
|
||||||
auto stream = dispatcher.MakeStream();
|
auto stream = dispatcher.MakeStream();
|
||||||
auto c = std::make_shared<tcp::Client>(io_service, stream.token(), [&](carla::Buffer message) {
|
auto c = std::make_shared<tcp::Client>(io_context, stream.token(), [&](carla::Buffer message) {
|
||||||
++message_count;
|
++message_count;
|
||||||
ASSERT_FALSE(message.empty());
|
ASSERT_FALSE(message.empty());
|
||||||
ASSERT_EQ(message.size(), 5u);
|
ASSERT_EQ(message.size(), 5u);
|
||||||
|
@ -158,10 +158,10 @@ TEST(streaming, low_level_tcp_small_message) {
|
||||||
carla::ThreadGroup threads;
|
carla::ThreadGroup threads;
|
||||||
threads.CreateThreads(
|
threads.CreateThreads(
|
||||||
std::max(2u, std::thread::hardware_concurrency()),
|
std::max(2u, std::thread::hardware_concurrency()),
|
||||||
[&]() { io_service.run(); });
|
[&]() { io_context.run(); });
|
||||||
|
|
||||||
std::this_thread::sleep_for(2s);
|
std::this_thread::sleep_for(2s);
|
||||||
io_service.stop();
|
io_context.stop();
|
||||||
done = true;
|
done = true;
|
||||||
std::cout << "client received " << message_count << " messages\n";
|
std::cout << "client received " << message_count << " messages\n";
|
||||||
ASSERT_GT(message_count, 10u);
|
ASSERT_GT(message_count, 10u);
|
||||||
|
|
|
@ -111,9 +111,9 @@ private:
|
||||||
|
|
||||||
const carla::Buffer _message;
|
const carla::Buffer _message;
|
||||||
|
|
||||||
boost::asio::io_service _client_callback;
|
boost::asio::io_context _client_callback;
|
||||||
|
|
||||||
boost::asio::io_service::work _work_to_do;
|
boost::asio::io_context::work _work_to_do;
|
||||||
|
|
||||||
const double _success_ratio;
|
const double _success_ratio;
|
||||||
|
|
||||||
|
|
|
@ -40,7 +40,7 @@ def get_libcarla_extensions():
|
||||||
os.path.join(pwd, 'dependencies/lib', pylib)]
|
os.path.join(pwd, 'dependencies/lib', pylib)]
|
||||||
extra_compile_args = [
|
extra_compile_args = [
|
||||||
'-isystem', 'dependencies/include/system', '-fPIC', '-std=c++14',
|
'-isystem', 'dependencies/include/system', '-fPIC', '-std=c++14',
|
||||||
'-Werror', '-Wall', '-Wextra', '-Wpedantic',
|
'-Werror', '-Wall', '-Wextra', '-Wpedantic', '-Wno-self-assign-overloaded',
|
||||||
'-Wdeprecated', '-Wno-shadow', '-Wuninitialized', '-Wunreachable-code',
|
'-Wdeprecated', '-Wno-shadow', '-Wuninitialized', '-Wunreachable-code',
|
||||||
'-Wpessimizing-move', '-Wold-style-cast', '-Wnull-dereference',
|
'-Wpessimizing-move', '-Wold-style-cast', '-Wnull-dereference',
|
||||||
'-Wduplicate-enum', '-Wnon-virtual-dtor', '-Wheader-hygiene',
|
'-Wduplicate-enum', '-Wnon-virtual-dtor', '-Wheader-hygiene',
|
||||||
|
@ -84,6 +84,7 @@ def get_libcarla_extensions():
|
||||||
|
|
||||||
# https://docs.microsoft.com/es-es/cpp/porting/modifying-winver-and-win32-winnt
|
# https://docs.microsoft.com/es-es/cpp/porting/modifying-winver-and-win32-winnt
|
||||||
extra_compile_args = [
|
extra_compile_args = [
|
||||||
|
'/experimental:external', '/external:I', 'dependencies/include/system',
|
||||||
'/DBOOST_ALL_NO_LIB', '/DBOOST_PYTHON_STATIC_LIB',
|
'/DBOOST_ALL_NO_LIB', '/DBOOST_PYTHON_STATIC_LIB',
|
||||||
'/DBOOST_ERROR_CODE_HEADER_ONLY', '/D_WIN32_WINNT=0x0501',
|
'/DBOOST_ERROR_CODE_HEADER_ONLY', '/D_WIN32_WINNT=0x0501',
|
||||||
'/DLIBCARLA_WITH_PYTHON_SUPPORT', '-DLIBCARLA_IMAGE_WITH_PNG_SUPPORT=true']
|
'/DLIBCARLA_WITH_PYTHON_SUPPORT', '-DLIBCARLA_IMAGE_WITH_PNG_SUPPORT=true']
|
||||||
|
|
|
@ -171,6 +171,8 @@ class World(object):
|
||||||
if blueprint.has_attribute('color'):
|
if blueprint.has_attribute('color'):
|
||||||
color = random.choice(blueprint.get_attribute('color').recommended_values)
|
color = random.choice(blueprint.get_attribute('color').recommended_values)
|
||||||
blueprint.set_attribute('color', color)
|
blueprint.set_attribute('color', color)
|
||||||
|
if blueprint.has_attribute('is_invincible'):
|
||||||
|
blueprint.set_attribute('is_invincible', 'true')
|
||||||
# Spawn the player.
|
# Spawn the player.
|
||||||
if self.player is not None:
|
if self.player is not None:
|
||||||
spawn_point = self.player.get_transform()
|
spawn_point = self.player.get_transform()
|
||||||
|
|
|
@ -30,6 +30,14 @@ class TestSynchronousMode(SmokeTest):
|
||||||
self.world = None
|
self.world = None
|
||||||
super(TestSynchronousMode, self).tearDown()
|
super(TestSynchronousMode, self).tearDown()
|
||||||
|
|
||||||
|
def test_reloading_map(self):
|
||||||
|
settings = carla.WorldSettings(
|
||||||
|
no_rendering_mode=False,
|
||||||
|
synchronous_mode=True)
|
||||||
|
for _ in range(0, 4):
|
||||||
|
self.world = self.client.reload_world()
|
||||||
|
self.world.apply_settings(settings)
|
||||||
|
|
||||||
def test_camera_on_synchronous_mode(self):
|
def test_camera_on_synchronous_mode(self):
|
||||||
cam_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
|
cam_bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
|
||||||
t = carla.Transform(carla.Location(z=10))
|
t = carla.Transform(carla.Location(z=10))
|
||||||
|
|