Merge pull request #95 from carla-simulator/benchmark_branch
Benchmark branch
This commit is contained in:
Néstor Subirón
GitHub
commit
ed092ce7ec
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,2 @@
|
|||
include carla/planner/*.txt
|
||||
include carla/planner/*.png
|
||||
|
|
@ -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
|
||||
"""
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 27 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 10 KiB |
|
|
@ -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))
|
||||
|
|
@ -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]
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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,6 +58,14 @@ 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:
|
||||
|
|
@ -88,60 +78,75 @@ class CarlaMap(object):
|
|||
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])
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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')
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -2,3 +2,5 @@ Pillow
|
|||
numpy
|
||||
protobuf
|
||||
pygame
|
||||
matplotlib
|
||||
future
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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
|
||||
----
|
||||
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
Loading…
Reference in New Issue