diff --git a/.gitignore b/.gitignore
index fa06fd814..6ef363893 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,9 +1,11 @@
Dist
Doxygen
+PythonClient/dist
Util/Build
*.VC.db
*.VC.opendb
+*.egg-info
*.kdev4
*.log
*.pb.cc
@@ -22,5 +24,7 @@ Util/Build
.tags*
.vs
__pycache__
+_benchmarks_results
_images
core
+
diff --git a/Docs/benchmark.md b/Docs/benchmark.md
new file mode 100644
index 000000000..b42d6eafa
--- /dev/null
+++ b/Docs/benchmark.md
@@ -0,0 +1,66 @@
+CARLA Benchmark
+===============
+
+Running the Benchmark
+---------------------
+
+The "carla" api provides a basic benchmarking system, that allows making several
+tests on a certain agent. We already provide the same benchmark used in the CoRL
+2017 paper. By running this benchmark you can compare the results of your agent
+to the results obtained by the agents show in the paper.
+
+
+Besides the requirements of the CARLA client, the benchmark package also needs
+the future package
+
+ $ sudo pip install future
+
+By running the benchmark a default agent that just go straight will be tested.
+To run the benchmark you need a server running. For a default localhost server
+on port 2000, to run the benchmark you just need to run
+
+ $ ./run_benchmark.py
+
+or
+
+ $ python run_benchmark.py
+
+Run the help command to see options available
+
+ $ ./run_benchmark.py --help
+
+Benchmarking your Agent
+---------------------
+The benchmark works by calling three lines of code
+
+ corl = CoRL2017(city_name=args.city_name, name_to_save=args.log_name)
+ agent = Manual(args.city_name)
+ results = corl.benchmark_agent(agent, client)
+
+This is excerpt is executed in the [run_benchmark.py](https://github.com/carla-simulator/carla/blob/master/PythonClient/run_benchmark.py) example.
+
+First a *benchmark* object is defined, for this case, a CoRL2017 benchmark. This is object is used to benchmark a certain Agent.
+On the second line of our sample code, there is an object of a Manual class instanced. This class inherited an Agent base class
+that is used by the *benchmark* object.
+To be benchmarked, an Agent subclass must redefine the *run_step* function as it is done in the following excerpt:
+
+ def run_step(self, measurements, sensor_data, target):
+ """
+ Function to run a control step in the CARLA vehicle.
+ :param measurements: object of the Measurements type
+ :param sensor_data: images list object
+ :param target: target position of Transform type
+ :return: an object of the control type.
+ """
+ control = VehicleControl()
+ control.throttle = 0.9
+ return control
+The function receives measurements from the world, sensor data and a target position. With this, the function must return a control to the car, *i.e.* steering value, throttle value, brake value, etc.
+
+The [measurements](measurements.md), [target](measurements.md), [sensor_data](cameras_and_sensors.md) and [control](measurements.md) types are described on the documentation.
+
+
+
+Creating your Benchmark
+---------------------
+Tutorial to be added
diff --git a/Docs/index.md b/Docs/index.md
index 2a264647c..40491ec84 100644
--- a/Docs/index.md
+++ b/Docs/index.md
@@ -7,6 +7,7 @@ CARLA Documentation
* [CARLA settings](carla_settings.md)
* [Measurements](measurements.md)
* [Cameras and sensors](cameras_and_sensors.md)
+ * [Benchmark](benchmark.md)
* [F.A.Q.](faq.md)
#### Building from source
diff --git a/PythonClient/MANIFEST.in b/PythonClient/MANIFEST.in
new file mode 100644
index 000000000..824b0ad1d
--- /dev/null
+++ b/PythonClient/MANIFEST.in
@@ -0,0 +1,2 @@
+include carla/planner/*.txt
+include carla/planner/*.png
diff --git a/PythonClient/carla/benchmarks/__init__.py b/PythonClient/carla/benchmarks/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py
new file mode 100644
index 000000000..c71f4c1db
--- /dev/null
+++ b/PythonClient/carla/benchmarks/agent.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python2
+# -*- coding: utf-8 -*-
+# 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 .
+# @author: german,felipecode
+
+
+from __future__ import print_function
+import abc
+
+from carla.planner.planner import Planner
+
+
+class Agent(object):
+ def __init__(self, city_name):
+ self.__metaclass__ = abc.ABCMeta
+ self._planner = Planner(city_name)
+
+ def get_distance(self, start_point, end_point):
+ path_distance = self._planner.get_shortest_path_distance(
+ [start_point.location.x, start_point.location.y, 22]
+ , [start_point.orientation.x, start_point.orientation.y, 22]
+ , [end_point.location.x, end_point.location.y, 22]
+ , [end_point.orientation.x, end_point.orientation.y, 22])
+ # We calculate the timout based on the distance
+
+ return path_distance
+
+ @abc.abstractmethod
+ def run_step(self, measurements, sensor_data, 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
+ """
diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py
new file mode 100644
index 000000000..672e4f0f7
--- /dev/null
+++ b/PythonClient/carla/benchmarks/benchmark.py
@@ -0,0 +1,377 @@
+#!/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 .
+
+
+import csv
+import datetime
+import math
+import os
+import abc
+import logging
+
+
+from builtins import input as input_data
+
+
+from carla.client import VehicleControl
+
+def sldist(c1, c2):
+ return math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2)
+
+
+class Benchmark(object):
+
+ """
+ The Benchmark class, controls the execution of the benchmark by an
+ Agent class.
+ The benchmark class must be inherited
+ """
+ def __init__(
+ self,
+ city_name,
+ name_to_save,
+ continue_experiment=False,
+ save_images=False
+ ):
+
+
+ self.__metaclass__ = abc.ABCMeta
+
+ self._city_name = city_name
+
+
+
+ self._base_name = name_to_save
+ self._dict_stats = {'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_rewards = {'exp_id': -1,
+ 'rep': -1,
+ 'weather': -1,
+ 'collision_gen': -1,
+ 'collision_ped': -1,
+ 'collision_car': -1,
+ 'lane_intersect': -1,
+ 'sidewalk_intersect': -1,
+ 'pos_x': -1,
+ 'pos_y': -1
+ }
+
+
+ self._experiments = self._build_experiments()
+ # Create the log files and get the names
+ self._suffix_name, self._full_name = self._create_log_record(name_to_save, self._experiments)
+ # Get the line for the experiment to be continued
+ self._line_on_file = self._continue_experiment(continue_experiment)
+
+
+
+ self._save_images = save_images
+ self._image_filename_format = os.path.join(
+ self._full_name, '_images/episode_{:s}/{:s}/image_{:0>5d}.jpg')
+
+ def run_navigation_episode(
+ self,
+ agent,
+ carla,
+ time_out,
+ target,
+ episode_name):
+
+ measurements, sensor_data = carla.read_data()
+ carla.send_control(VehicleControl())
+
+ t0 = measurements.game_timestamp
+ t1 = t0
+ success = False
+ measurement_vec = []
+ frame = 0
+ distance = 10000
+
+ while(t1 - t0) < (time_out * 1000) and not success:
+ measurements, sensor_data = carla.read_data()
+
+ control = agent.run_step(measurements, sensor_data, target)
+
+ logging.info("Controller is Inputting:")
+ logging.info('Steer = %f Throttle = %f Brake = %f ',
+ control.steer, control.throttle, control.brake)
+
+ carla.send_control(control)
+
+ # measure distance to target
+ if self._save_images:
+ for name, image in sensor_data.items():
+ image.save_to_disk(self._image_filename_format.format(
+ episode_name, name, frame))
+
+ curr_x = measurements.player_measurements.transform.location.x
+ curr_y = measurements.player_measurements.transform.location.y
+
+ measurement_vec.append(measurements.player_measurements)
+
+ t1 = measurements.game_timestamp
+
+ distance = sldist([curr_x, curr_y],
+ [target.location.x, target.location.y])
+
+ logging.info('Status:')
+ logging.info(
+ '[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f',
+ float(distance), curr_x, curr_y, target.location.x,
+ target.location.y)
+
+ if distance < 200.0:
+ success = True
+
+ frame += 1
+
+ if success:
+ return 1, measurement_vec, float(t1 - t0) / 1000.0, distance
+ return 0, measurement_vec, time_out, distance
+
+ def benchmark_agent(self, agent, carla):
+
+ if self._line_on_file == 0:
+ # The fixed name considering all the experiments being run
+ with open(os.path.join(self._full_name,
+ self._suffix_name), 'w') as ofd:
+
+ w = csv.DictWriter(ofd, self._dict_stats.keys())
+ w.writeheader()
+
+ with open(os.path.join(self._full_name,
+ 'details_' + self._suffix_name), 'w') as rfd:
+
+ rw = csv.DictWriter(rfd, self._dict_rewards.keys())
+ rw.writeheader()
+ start_task = 0
+ start_pose = 0
+ else:
+ (start_task, start_pose) = self._get_pose_and_task(self._line_on_file)
+
+ logging.info(' START ')
+
+ for experiment in self._experiments[start_task:]:
+
+ positions = carla.load_settings(
+ experiment.conditions).player_start_spots
+
+ for pose in experiment.poses[start_pose:]:
+ for rep in range(experiment.repetitions):
+
+ start_point = pose[0]
+ end_point = pose[1]
+
+ carla.start_episode(start_point)
+
+ logging.info('======== !!!! ==========')
+ logging.info(' Start Position %d End Position %d ',
+ start_point, end_point)
+
+ path_distance = agent.get_distance(
+ positions[start_point], positions[end_point])
+ euclidean_distance = \
+ sldist([positions[start_point].location.x, positions[start_point].location.y],
+ [positions[end_point].location.x, positions[end_point].location.y])
+
+ time_out = self._calculate_time_out(path_distance)
+ # running the agent
+ (result, reward_vec, final_time, remaining_distance) = \
+ self.run_navigation_episode(
+ agent, carla, time_out, positions[end_point],
+ str(experiment.Conditions.WeatherId) + '_'
+ + str(experiment.id) + '_' + str(start_point)
+ + '.' + str(end_point))
+
+ # compute stats for the experiment
+
+ self._write_summary_results(
+ experiment, pose, rep, euclidean_distance,
+ remaining_distance, final_time, time_out, result)
+
+ self._write_details_results(experiment, rep, reward_vec)
+
+ if(result > 0):
+ logging.info('+++++ Target achieved in %f seconds! +++++',
+ final_time)
+ else:
+ logging.info('----- Timeout! -----')
+ return self.get_all_statistics()
+
+ def _write_summary_results(self, experiment, pose, rep,
+ path_distance, remaining_distance,
+ final_time, time_out, result):
+
+ self._dict_stats['exp_id'] = experiment.id
+ self._dict_stats['rep'] = rep
+ self._dict_stats['weather'] = experiment.Conditions.WeatherId
+ self._dict_stats['start_point'] = pose[0]
+ self._dict_stats['end_point'] = pose[1]
+ self._dict_stats['result'] = result
+ self._dict_stats['initial_distance'] = path_distance
+ self._dict_stats['final_distance'] = remaining_distance
+ self._dict_stats['final_time'] = final_time
+ self._dict_stats['time_out'] = time_out
+
+ with open(os.path.join(self._full_name, self._suffix_name), 'a+') as ofd:
+
+ w = csv.DictWriter(ofd, self._dict_stats.keys())
+
+ w.writerow(self._dict_stats)
+
+ def _write_details_results(self, experiment, rep, reward_vec):
+
+ with open(os.path.join(self._full_name,
+ 'details_' + self._suffix_name), 'a+') as rfd:
+
+ rw = csv.DictWriter(rfd, self._dict_rewards.keys())
+
+ for i in range(len(reward_vec)):
+ self._dict_rewards['exp_id'] = experiment.id
+ self._dict_rewards['rep'] = rep
+ self._dict_rewards['weather'] = experiment.Conditions.WeatherId
+ self._dict_rewards['collision_gen'] = reward_vec[
+ i].collision_other
+ self._dict_rewards['collision_ped'] = reward_vec[
+ i].collision_pedestrians
+ self._dict_rewards['collision_car'] = reward_vec[
+ i].collision_vehicles
+ self._dict_rewards['lane_intersect'] = reward_vec[
+ i].intersection_otherlane
+ self._dict_rewards['sidewalk_intersect'] = reward_vec[
+ i].intersection_offroad
+ self._dict_rewards['pos_x'] = reward_vec[
+ i].transform.location.x
+ self._dict_rewards['pos_y'] = reward_vec[
+ i].transform.location.y
+
+ rw.writerow(self._dict_rewards)
+
+ def _create_log_record(self, base_name, experiments):
+ """
+ This function creates the log files for the benchmark.
+
+ """
+ suffix_name = self._get_experiments_names(experiments)
+ full_name = os.path.join('_benchmarks_results',
+ base_name + '_'
+ + self._get_details() + '/')
+
+ folder = os.path.dirname(full_name)
+ if not os.path.isdir(folder):
+ os.makedirs(folder)
+
+ # Make a date file: to show when this was modified,
+ # the number of times the experiments were run
+ now = datetime.datetime.now()
+ open(os.path.join(full_name, now.strftime("%Y%m%d%H%M")),'w').close()
+
+ return suffix_name, full_name
+
+
+ def _continue_experiment(self, continue_experiment):
+
+ if self._experiment_exist():
+
+ if continue_experiment:
+ line_on_file = self._get_last_position()
+
+ else:
+ # Ask question, to avoid mistaken override situations
+ answer = input_data("The experiment was already found in the files"
+ + ", Do you want to continue (y/n)? \n"
+ )
+ if answer == 'Yes' or answer == 'y':
+ line_on_file = self._get_last_position()
+ else:
+ line_on_file = 0
+
+ else:
+ line_on_file = 0
+
+ return line_on_file
+
+
+
+ def _experiment_exist(self):
+ return os.path.isfile(self._full_name)
+
+ def _get_last_position(self):
+
+ with open(os.path.join(self._full_name, self._suffix_name)) as f:
+ return sum(1 for _ in f)
+
+
+ # To be redefined on subclasses on how to calculate timeout for an episode
+ @abc.abstractmethod
+ def _calculate_time_out(self, distance):
+ pass
+
+ @abc.abstractmethod
+ def _get_details(self):
+ """
+ Get details
+ :return: a string with name and town of the subclass
+ """
+ @abc.abstractmethod
+ def _build_experiments(self):
+ """
+ Returns a set of experiments to be evaluated
+ Must be redefined in an inherited class.
+
+ """
+
+ @abc.abstractmethod
+ def get_all_statistics(self):
+ """
+ Get the statistics of the evaluated experiments
+ :return:
+ """
+
+ @abc.abstractmethod
+ def _get_pose_and_task(self, line_on_file):
+ """
+ Parse the experiment depending on number of poses and tasks
+ """
+
+
+ @abc.abstractmethod
+ def plot_summary_train(self):
+ """
+ returns the summary for the train weather/task episodes
+
+ """
+
+ @abc.abstractmethod
+ def plot_summary_test(self):
+ """
+ returns the summary for the test weather/task episodes
+
+ """
+ @staticmethod
+ def _get_experiments_names(experiments):
+
+ name_cat = 'w'
+
+ for experiment in experiments:
+
+ name_cat += str(experiment.Conditions.WeatherId) + '.'
+
+ return name_cat
+
+
diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py
new file mode 100644
index 000000000..cf2cf11c2
--- /dev/null
+++ b/PythonClient/carla/benchmarks/corl_2017.py
@@ -0,0 +1,203 @@
+# 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 .
+
+# CORL experiment set.
+
+from __future__ import print_function
+
+import os
+
+from .benchmark import Benchmark
+from .experiment import Experiment
+from carla.sensor import Camera
+from carla.settings import CarlaSettings
+
+from .metrics import compute_summary
+
+
+class CoRL2017(Benchmark):
+
+ def get_all_statistics(self):
+
+ summary = compute_summary(os.path.join(
+ self._full_name, self._suffix_name), [3])
+
+ return summary
+
+ def plot_summary_train(self):
+
+ self._plot_summary([1.0, 3.0, 6.0, 8.0])
+
+ def plot_summary_test(self):
+
+ self._plot_summary([4.0, 14.0])
+
+ def _plot_summary(self, weathers):
+ """
+ We plot the summary of the testing for the set selected weathers.
+ The test weathers are [4,14]
+
+ """
+
+ metrics_summary = compute_summary(os.path.join(
+ self._full_name, self._suffix_name), [3])
+
+ for metric, values in metrics_summary.items():
+
+ print('Metric : ', metric)
+ for weather, tasks in values.items():
+ if weather in set(weathers):
+ print(' Weather: ', weather)
+ count = 0
+ for t in tasks:
+ print(' Task ', count, ' -> ', t)
+ count += 1
+
+ print(' AvG -> ', float(sum(tasks)) / float(len(tasks)))
+
+ def _calculate_time_out(self, distance):
+ """
+ Function to return the timeout ( in miliseconds) that is calculated based on distance to goal.
+ This is the same timeout as used on the CoRL paper.
+ """
+
+ return ((distance / 100000.0) / 10.0) * 3600.0 + 10.0
+
+ 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(CameraFOV=100)
+
+ camera.set_image_size(800, 600)
+
+ camera.set_position(200, 0, 140)
+ camera.set_rotation(-15.0, 0, 0)
+
+ weathers = [1, 3, 6, 8, 4, 14]
+ 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 weathers:
+
+ for iteration in range(len(poses_tasks)):
+ poses = poses_tasks[iteration]
+ vehicles = vehicles_tasks[iteration]
+ pedestrians = pedestrians_tasks[iteration]
+
+ conditions = CarlaSettings()
+ conditions.set(
+ SynchronousMode=True,
+ SendNonPlayerAgentsInfo=True,
+ NumberOfVehicles=vehicles,
+ NumberOfPedestrians=pedestrians,
+ WeatherId=weather,
+ SeedVehicles=123456789,
+ SeedPedestrians=123456789
+ )
+ # Add all the cameras that were set for this experiments
+
+ conditions.add_sensor(camera)
+
+ experiment = Experiment()
+ experiment.set(
+ Conditions=conditions,
+ Poses=poses,
+ Id=iteration,
+ Repetitions=1
+ )
+ experiments_vector.append(experiment)
+
+ return experiments_vector
+
+ def _get_details(self):
+
+ # Function to get automatic information from the experiment for writing purposes
+ return 'corl2017_' + self._city_name
+
+ def _get_pose_and_task(self, line_on_file):
+ """
+ Returns the pose and task this experiment is, based on the line it was
+ on the log file.
+ """
+ # We assume that the number of poses is constant
+ return int(line_on_file / len(self._experiments)), line_on_file % 25
diff --git a/PythonClient/carla/benchmarks/experiment.py b/PythonClient/carla/benchmarks/experiment.py
new file mode 100644
index 000000000..8626dd797
--- /dev/null
+++ b/PythonClient/carla/benchmarks/experiment.py
@@ -0,0 +1,33 @@
+
+from carla.settings import CarlaSettings
+
+
+class Experiment(object):
+
+ def __init__(self):
+ self.Id = ''
+ 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)
+
+ @property
+ def id(self):
+ return self.Id
+
+ @property
+ def conditions(self):
+ return self.Conditions
+
+ @property
+ def poses(self):
+ return self.Poses
+
+ @property
+ def repetitions(self):
+ return self.Repetitions
diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py
new file mode 100644
index 000000000..dd47a5f52
--- /dev/null
+++ b/PythonClient/carla/benchmarks/metrics.py
@@ -0,0 +1,205 @@
+# 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 .
+
+
+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]
+
+
+def get_colisions(selected_matrix, header):
+
+ count_gen = 0
+ count_ped = 0
+ count_car = 0
+ i = 1
+
+ while i < selected_matrix.shape[0]:
+ if (selected_matrix[i, header.index('collision_gen')]
+ - selected_matrix[(i-10), header.index('collision_gen')]) > 40000:
+ count_gen += 1
+ i += 20
+ i += 1
+
+ i = 1
+ while i < selected_matrix.shape[0]:
+ if (selected_matrix[i, header.index('collision_car')]
+ - selected_matrix[(i-10), header.index('collision_car')]) > 40000:
+ count_car += 1
+ i += 30
+ i += 1
+
+ i = 1
+ while i < selected_matrix.shape[0]:
+ if (selected_matrix[i, header.index('collision_ped')]
+ - selected_matrix[i-5, header.index('collision_ped')]) > 30000:
+ count_ped += 1
+ i += 100
+ i += 1
+
+ return count_gen, count_car, count_ped
+
+
+def get_distance_traveled(selected_matrix, header):
+
+ 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')]
+ # Here we defined a maximun distance in a tick, this case 8 meters or 288km/h
+ if sldist((x, y), (prev_x, prev_y)) < 800:
+ acummulated_distance += sldist((x, y), (prev_x, prev_y))
+
+
+ prev_x = x
+ prev_y = y
+
+ i += 1
+
+ return float(acummulated_distance)/float(100*1000)
+
+
+
+
+
+def get_out_of_road_lane(selected_matrix, header):
+
+ count_road = 0
+ count_lane = 0
+
+ i = 0
+
+ while i < selected_matrix.shape[0]:
+ # print selected_matrix[i,6]
+ if (selected_matrix[i, header.index('sidewalk_intersect')]
+ - selected_matrix[(i-10), header.index('sidewalk_intersect')]) > 0.3:
+ count_road += 1
+ i += 20
+ if i >= selected_matrix.shape[0]:
+ break
+
+ if (selected_matrix[i, header.index('lane_intersect')]
+ - selected_matrix[(i-10), header.index('lane_intersect')]) > 0.4:
+ count_lane += 1
+ i += 20
+
+ i += 1
+
+ return count_lane, count_road
+
+
+
+def compute_summary(filename, dynamic_episodes):
+
+ # Separate the PATH and the basename
+ path = os.path.dirname(filename)
+ base_name = os.path.basename(filename)
+
+
+
+ f = open(filename, "rb")
+ header = f.readline()
+ header = header.split(',')
+ header[-1] = header[-1][:-2]
+ f.close()
+
+ f = open(os.path.join(path, 'details_' + base_name), "rb")
+ header_details = f.readline()
+ header_details = header_details.split(',')
+ header_details[-1] = header_details[-1][:-2]
+ f.close()
+
+ data_matrix = np.loadtxt(open(filename, "rb"), delimiter=",", skiprows=1)
+
+ # Corner Case: The presented test just had one episode
+ if data_matrix.ndim == 1:
+ data_matrix = np.expand_dims(data_matrix, axis=0)
+
+
+ tasks = np.unique(data_matrix[:, header.index('exp_id')])
+
+ all_weathers = np.unique(data_matrix[:, header.index('weather')])
+
+ reward_matrix = np.loadtxt(open(os.path.join(
+ path, 'details_' + base_name), "rb"), delimiter=",", skiprows=1)
+
+ metrics_dictionary = {'average_completion': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'intersection_offroad': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'intersection_otherlane': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'collision_pedestrians': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'collision_vehicles': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'collision_other': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'average_fully_completed': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'average_speed': {w: [0.0]*len(tasks) for w in all_weathers},
+ 'driven_kilometers': {w: [0.0]*len(tasks) for w in all_weathers}
+ }
+
+ for t in tasks:
+ task_data_matrix = data_matrix[
+ data_matrix[:, header.index('exp_id')] == t]
+ weathers = np.unique(task_data_matrix[:, header.index('weather')])
+
+
+
+ for w in weathers:
+ t = int(t)
+
+ task_data_matrix = data_matrix[np.logical_and(data_matrix[:, header.index(
+ 'exp_id')] == t, data_matrix[:, header.index('weather')] == w)]
+
+
+ task_reward_matrix = reward_matrix[np.logical_and(reward_matrix[:, header_details.index(
+ 'exp_id')] == float(t), reward_matrix[:, header_details.index('weather')] == float(w))]
+
+ km_run = get_distance_traveled(
+ task_reward_matrix, header_details)
+
+ metrics_dictionary['average_fully_completed'][w][t] = sum(
+ task_data_matrix[:, header.index('result')])/task_data_matrix.shape[0]
+
+ metrics_dictionary['average_completion'][w][t] = sum(
+ (task_data_matrix[:, header.index('initial_distance')]
+ - task_data_matrix[:, header.index('final_distance')])
+ / task_data_matrix[:, header.index('initial_distance')]) \
+ / len(task_data_matrix[:, header.index('final_distance')])
+
+
+ metrics_dictionary['driven_kilometers'][w][t]= km_run
+ metrics_dictionary['average_speed'][w][t]= km_run/ \
+ ((sum(task_data_matrix[:, header.index('final_time')]))/3600.0)
+
+
+
+ if list(tasks).index(t) in set(dynamic_episodes):
+
+ lane_road = get_out_of_road_lane(
+ task_reward_matrix, header_details)
+ colisions = get_colisions(task_reward_matrix, header_details)
+
+
+
+ metrics_dictionary['intersection_offroad'][
+ w][t] = lane_road[0]/km_run
+ metrics_dictionary['intersection_otherlane'][
+ w][t] = lane_road[1]/km_run
+ metrics_dictionary['collision_pedestrians'][
+ w][t] = colisions[2]/km_run
+ metrics_dictionary['collision_vehicles'][
+ w][t] = colisions[1]/km_run
+ metrics_dictionary['collision_other'][
+ w][t] = colisions[0]/km_run
+
+
+ return metrics_dictionary
diff --git a/PythonClient/carla/planner/Town01Central.png b/PythonClient/carla/planner/Town01Central.png
new file mode 100644
index 000000000..b2d769f94
Binary files /dev/null and b/PythonClient/carla/planner/Town01Central.png differ
diff --git a/PythonClient/carla/planner/Town02Central.png b/PythonClient/carla/planner/Town02Central.png
new file mode 100644
index 000000000..0c3a1ef10
Binary files /dev/null and b/PythonClient/carla/planner/Town02Central.png differ
diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py
new file mode 100644
index 000000000..e322395ec
--- /dev/null
+++ b/PythonClient/carla/planner/astar.py
@@ -0,0 +1,150 @@
+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))
diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py
new file mode 100644
index 000000000..4223426c8
--- /dev/null
+++ b/PythonClient/carla/planner/city_track.py
@@ -0,0 +1,130 @@
+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._node_density = 50.0
+ self._pixel_density = 16.43
+
+ self._map = CarlaMap(city_name, self._pixel_density, self._node_density)
+
+ 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._pixel_density
+
+ def get_node_density(self):
+ return self._node_density
+
+ 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]
+
diff --git a/PythonClient/carla/planner/converter.py b/PythonClient/carla/planner/converter.py
new file mode 100644
index 000000000..f417f7610
--- /dev/null
+++ b/PythonClient/carla/planner/converter.py
@@ -0,0 +1,157 @@
+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, node_density, pixel_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 _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],
+ 22
+ ]
+
+ 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
diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py
new file mode 100644
index 000000000..54913b00f
--- /dev/null
+++ b/PythonClient/carla/planner/graph.py
@@ -0,0 +1,135 @@
+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)
diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py
new file mode 100644
index 000000000..3e7d8bcf4
--- /dev/null
+++ b/PythonClient/carla/planner/grid.py
@@ -0,0 +1,129 @@
+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
diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py
index 8779bb169..f402b2650 100644
--- a/PythonClient/carla/planner/map.py
+++ b/PythonClient/carla/planner/map.py
@@ -19,54 +19,36 @@ try:
except ImportError:
raise RuntimeError('cannot import PIL, make sure pillow package is installed')
-
-def string_to_node(string):
- vec = string.split(',')
- return (int(vec[0]), int(vec[1]))
+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 string_to_floats(string):
- vec = string.split(',')
- return (float(vec[0]), float(vec[1]), float(vec[2]))
+def color_to_angle(color):
+ return ((float(color) / 255.0)) * 2 * math.pi
class CarlaMap(object):
- def __init__(self, city):
+
+ def __init__(self, city, pixel_density, node_density):
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')
- with open(city_file, 'r') as file_object:
+ # 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)
- linewordloffset = file_object.readline()
- # The offset of the world from the zero coordinates ( The
- # coordinate we consider zero)
- self.worldoffset = string_to_floats(linewordloffset)
+ self._pixel_density = pixel_density
+ self._grid = Grid(self._graph)
+ # The number of game units per pixel. For now this is fixed.
- lineworldangles = file_object.readline()
- self.angles = string_to_floats(lineworldangles)
+ self._converter = Converter(city_file, pixel_density, node_density)
- self.worldrotation = np.array([
- [math.cos(math.radians(self.angles[2])), -math.sin(math.radians(self.angles[2])), 0.0],
- [math.sin(math.radians(self.angles[2])), math.cos(math.radians(self.angles[2])), 0.0],
- [0.0, 0.0, 1.0]])
-
- # Ignore for now, these are offsets for map coordinates and scale
- # (not used).
- _ = file_object.readline()
- linemapoffset = file_object.readline()
-
- # The offset of the map zero coordinate.
- self.mapoffset = string_to_floats(linemapoffset)
-
- # the graph resolution.
- linegraphres = file_object.readline()
- self.resolution = string_to_node(linegraphres)
-
- # The number of game units per pixel.
- self.pixel_density = 16.43
# Load the lanes image
self.map_image_lanes = Image.open(city_map_file_lanes)
self.map_image_lanes.load()
@@ -76,72 +58,95 @@ class CarlaMap(object):
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(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])
+ aspect_ratio = height / float(self.map_image.shape[0])
- img = img.resize((int(aspect_ratio*self.map_image.shape[1]),height), Image.ANTIALIAS)
+ 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, height=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)
- raise NotImplementedError
-
- def get_position_on_map(self, world):
- """Get the position on the map for a certain world position."""
- relative_location = []
- pixel = []
-
- rotation = np.array([world[0], world[1], world[2]])
- rotation = rotation.dot(self.worldrotation)
-
- relative_location.append(rotation[0] + self.worldoffset[0] - self.mapoffset[0])
- relative_location.append(rotation[1] + self.worldoffset[1] - self.mapoffset[1])
- relative_location.append(rotation[2] + self.worldoffset[2] - self.mapoffset[2])
-
- pixel.append(math.floor(relative_location[0] / float(self.pixel_density)))
- pixel.append(math.floor(relative_location[1] / float(self.pixel_density)))
-
- return pixel
-
- def get_position_on_world(self, pixel):
- """Get world position of a certain map position."""
- relative_location = []
- world_vertex = []
- relative_location.append(pixel[0] * self.pixel_density)
- relative_location.append(pixel[1] * self.pixel_density)
-
- world_vertex.append(relative_location[0] + self.mapoffset[0] - self.worldoffset[0])
- world_vertex.append(relative_location[1] + self.mapoffset[1] - self.worldoffset[1])
- world_vertex.append(22) # Z does not matter for now.
-
- return world_vertex
+ 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."""
- relative_location = []
- pixel = []
- rotation = np.array([world[0], world[1], world[2]])
- rotation = rotation.dot(self.worldrotation)
-
- relative_location.append(rotation[0] + self.worldoffset[0] - self.mapoffset[0])
- relative_location.append(rotation[1] + self.worldoffset[1] - self.mapoffset[1])
- relative_location.append(rotation[2] + self.worldoffset[2] - self.mapoffset[2])
-
- pixel.append(math.floor(relative_location[0] / float(self.pixel_density)))
- pixel.append(math.floor(relative_location[1] / float(self.pixel_density)))
+ pixel = self.convert_to_pixel(world)
ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 2]
- ori = ((float(ori) / 255.0)) * 2 * math.pi
+ 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 Pixel 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 World )
+ :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])
diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py
new file mode 100644
index 000000000..20f84f981
--- /dev/null
+++ b/PythonClient/carla/planner/planner.py
@@ -0,0 +1,167 @@
+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,
+ :param source: source position
+ :param source_ori: source orientation
+ :param target: target position
+ :param target_ori: target orientation
+ :return: 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
diff --git a/PythonClient/carla/tcp.py b/PythonClient/carla/tcp.py
index 5dd9ed9ef..fbf1b17f3 100644
--- a/PythonClient/carla/tcp.py
+++ b/PythonClient/carla/tcp.py
@@ -41,7 +41,7 @@ class TCPClient(object):
self._socket.settimeout(self._timeout)
logging.debug('%sconnected', self._logprefix)
return
- except OSError as exception:
+ except socket.error as exception:
error = exception
logging.debug('%sconnection attempt %d: %s', self._logprefix, attempt, error)
time.sleep(1)
@@ -65,7 +65,7 @@ class TCPClient(object):
header = struct.pack(' 0:
try:
data = self._socket.recv(length)
- except OSError as exception:
+ 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')
diff --git a/PythonClient/manual_control.py b/PythonClient/manual_control.py
index dfca225db..9e3d9d69b 100755
--- a/PythonClient/manual_control.py
+++ b/PythonClient/manual_control.py
@@ -127,7 +127,7 @@ class CarlaGame(object):
self._map_view = None
self._is_on_reverse = False
self._city_name = city_name
- self._map = CarlaMap(city_name) if city_name is not None else None
+ self._map = CarlaMap(city_name, 16.43, 50.0) if city_name is not None else None
self._map_shape = self._map.map_image.shape if city_name is not None else None
self._map_view = self._map.get_map(WINDOW_HEIGHT) if city_name is not None else None
self._position = None
diff --git a/PythonClient/requirements.txt b/PythonClient/requirements.txt
index 1a697a692..f170835dd 100644
--- a/PythonClient/requirements.txt
+++ b/PythonClient/requirements.txt
@@ -2,3 +2,5 @@ Pillow
numpy
protobuf
pygame
+matplotlib
+future
diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py
new file mode 100755
index 000000000..384885ede
--- /dev/null
+++ b/PythonClient/run_benchmark.py
@@ -0,0 +1,94 @@
+#!/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 .
+
+import argparse
+import logging
+import time
+
+from carla.benchmarks.agent import Agent
+from carla.benchmarks.corl_2017 import CoRL2017
+
+from carla.client import make_carla_client, VehicleControl
+from carla.tcp import TCPConnectionError
+
+
+class Manual(Agent):
+ """
+ Sample redefinition of the Agent,
+ An agent that goes straight
+ """
+ def run_step(self, measurements, sensor_data, target):
+ control = VehicleControl()
+ control.throttle = 0.9
+
+ return control
+
+
+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'
+ )
+
+ 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)
+
+ while True:
+ try:
+ with make_carla_client(args.host, args.port) as client:
+ corl = CoRL2017(city_name=args.city_name, name_to_save=args.log_name)
+ agent = Manual(args.city_name)
+ results = corl.benchmark_agent(agent, client)
+ corl.plot_summary_test()
+ corl.plot_summary_train()
+
+ break
+
+ except TCPConnectionError as error:
+ logging.error(error)
+ time.sleep(1)
diff --git a/PythonClient/setup.py b/PythonClient/setup.py
new file mode 100644
index 000000000..7969450a6
--- /dev/null
+++ b/PythonClient/setup.py
@@ -0,0 +1,15 @@
+from setuptools import setup
+
+# @todo Dependencies are missing.
+
+setup(
+ name='carla_client',
+ version='0.7.1',
+ packages=['carla', 'carla.benchmarks', '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
+)
diff --git a/README.md b/README.md
index 15b914876..f2a803b6d 100644
--- a/README.md
+++ b/README.md
@@ -16,6 +16,8 @@ environmental conditions.
For instructions on how to use and compile CARLA, check out
[CARLA Documentation](http://carla.readthedocs.io).
+If you want to benchmark your model in the same conditions as in our CoRL’17 paper, check out [Benchmarking](https://github.com/carla-simulator/carla/blob/benchmark_branch/Docs/benchmark.md).
+
News
----
diff --git a/mkdocs.yml b/mkdocs.yml
index 79b54f1fb..6144dc8ed 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -9,6 +9,7 @@ pages:
- 'CARLA Settings': 'carla_settings.md'
- 'Measurements': 'measurements.md'
- 'Cameras and sensors': 'cameras_and_sensors.md'
+ - 'Benchmark': 'benchmark.md'
- 'F.A.Q.': 'faq.md'
- Building from source:
- 'How to build on Linux': 'how_to_build_on_linux.md'