Merge pull request #95 from carla-simulator/benchmark_branch

Benchmark branch

.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
+

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. <br>
+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

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)
   * [Troubleshooting](troubleshooting.md)
 

PythonClient/MANIFEST.in

@@ -0,0 +1,2 @@
+include carla/planner/*.txt
+include carla/planner/*.png

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 <https://opensource.org/licenses/MIT>.
+# @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
+        """

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 <https://opensource.org/licenses/MIT>.
+
+
+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
+
+

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 <https://opensource.org/licenses/MIT>.
+
+# 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

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

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 <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]
+
+
+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

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))

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]
+

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

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)

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

PythonClient/carla/planner/map.py

@@ -19,54 +19,36 @@ try:
 except ImportError:
     raise RuntimeError('cannot import PIL, make sure pillow package is installed')
 
-
+from carla.planner.graph import Graph
-def string_to_node(string):
+from carla.planner.graph import sldist
-    vec = string.split(',')
+from carla.planner.grid import Grid
-    return (int(vec[0]), int(vec[1]))
+from carla.planner.converter import Converter
 
 
-def string_to_floats(string):
+def color_to_angle(color):
-    vec = string.split(',')
+    return ((float(color) / 255.0)) * 2 * math.pi
-    return (float(vec[0]), float(vec[1]), float(vec[2]))
 
 
 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()
+        self._pixel_density = pixel_density
-            # The offset of the world from the zero coordinates ( The
+        self._grid = Grid(self._graph)
-            # coordinate we consider zero)
+        # The number of game units per pixel. For now this is fixed.
-            self.worldoffset = string_to_floats(linewordloffset)
 
-            lineworldangles = file_object.readline()
+        self._converter = Converter(city_file, pixel_density, node_density)
-            self.angles = string_to_floats(lineworldangles)
 
-            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):
+    def get_map_lanes(self, size=None):
-        # if size is not None:
+        if size is not None:
-        #     img = Image.fromarray(self.map_image_lanes.astype(np.uint8))
+            img = Image.fromarray(self.map_image_lanes.astype(np.uint8))
-        #     img = img.resize((size[1], size[0]), Image.ANTIALIAS)
+            img = img.resize((size[1], size[0]), Image.ANTIALIAS)
-        #     img.load()
+            img.load()
-        #     return np.fliplr(np.asarray(img, dtype="int32"))
+            return np.fliplr(np.asarray(img, dtype="int32"))
-        # return np.fliplr(self.map_image_lanes)
+        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_lane_orientation(self, world):
         """Get the lane orientation of a certain world position."""
-        relative_location = []
+        pixel = self.convert_to_pixel(world)
-        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)))
 
         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])

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

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('<L', len(message))
         try:
             self._socket.sendall(header + message)
-        except OSError as exception:
+        except socket.error as exception:
             self._reraise_exception_as_tcp_error('failed to write data', exception)
 
     def read(self):
@@ -85,7 +85,7 @@ class TCPClient(object):
         while length > 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')

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

PythonClient/requirements.txt

@@ -2,3 +2,5 @@ Pillow
 numpy
 protobuf
 pygame
+matplotlib
+future

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 <https://opensource.org/licenses/MIT>.
+
+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)

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
+)

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
 ----
 

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'
   - 'Troubleshooting': 'troubleshooting.md'
 - Building from source: