Adding the benchmark to the client, a first big step.
This commit is contained in:
felipecode
parent
146bd1948e
commit
d9624f6616
|
|
@ -0,0 +1,72 @@
|
|||
#!/usr/bin/env python2
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Sat Jun 10 19:12:37 2017
|
||||
|
||||
@author: german,felipecode
|
||||
"""
|
||||
from __future__ import print_function
|
||||
import time
|
||||
import math
|
||||
from carla import sensor
|
||||
from carla.client import make_carla_client
|
||||
from carla.sensor import Camera
|
||||
from carla.settings import CarlaSettings
|
||||
from carla.tcp import TCPConnectionError
|
||||
from carla.util import print_over_same_line
|
||||
from carla.planner.planner import Planner
|
||||
|
||||
from carla.carla_server_pb2 import Control
|
||||
|
||||
|
||||
|
||||
class Agent(object, ):
|
||||
def __init__(self, **kwargs):
|
||||
import os
|
||||
dir_path = os.path.dirname(__file__)
|
||||
|
||||
planner = Planner(dir_path+'/../planner/' + city_name + '.txt',\
|
||||
dir_path+'/../planner/' + city_name + '.png')
|
||||
|
||||
|
||||
|
||||
def get_initial_distance(self):
|
||||
|
||||
_,path_distance=planner.get_next_command([positions[start_point].location.x\
|
||||
,positions[start_point].location.y,22],[positions[start_point].orientation.x\
|
||||
,positions[start_point].orientation.y,22],[positions[end_point].location.x\
|
||||
,positions[end_point].location.y,22],(1,0,0))
|
||||
# We calculate the timout based on the distance
|
||||
|
||||
return path_distance
|
||||
|
||||
|
||||
# Function to be redefined by the AI.
|
||||
def run_step(self, data):
|
||||
pass
|
||||
|
||||
|
||||
|
||||
"""
|
||||
def compute_distance(self, curr, prev, target):
|
||||
# no history info
|
||||
if(prev[0] == -1 and prev[1] == -1):
|
||||
distance = math.sqrt((curr[0] - target[0])*(curr[0] - target[0]) + (curr[1] - target[1])*(curr[1] - target[1]))
|
||||
else:
|
||||
# distance point to segment
|
||||
v1 = [target[0]-curr[0], target[1]-curr[1]]
|
||||
v2 = [prev[0]-curr[0], prev[1]-curr[1]]
|
||||
|
||||
w1 = v1[0]*v2[0] + v1[1]*v2[1]
|
||||
w2 = v2[0]*v2[0] + v2[1]*v2[1]
|
||||
t_hat = w1 / (w2 + 1e-4)
|
||||
t_start = min(max(t_hat, 0.0), 1.0)
|
||||
|
||||
s = [0, 0]
|
||||
s[0] = curr[0] + t_start * (prev[0] - curr[0])
|
||||
s[1] = curr[1] + t_start * (prev[1] - curr[1])
|
||||
distance = math.sqrt((s[0] - target[0])*(s[0] - target[0]) + (s[1] - target[1])*(s[1] - target[1]))
|
||||
|
||||
|
||||
return distance
|
||||
"""
|
||||
|
|
@ -0,0 +1,309 @@
|
|||
|
||||
from .metrics import plot_summary
|
||||
|
||||
class Benchmark(object,):
|
||||
|
||||
# Param @name to be used for saving purposes
|
||||
def __init__(self,name):
|
||||
self._base_name = name # Sends a base name, the rest will be saved with respect to what the episode was about
|
||||
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
|
||||
|
||||
|
||||
}
|
||||
with open(self._base_name + self._suffix_name , 'wb') as ofd:
|
||||
|
||||
w = csv.DictWriter(ofd, self._dict_stats.keys())
|
||||
w.writeheader()
|
||||
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
def run_navigation_episode(self, agent,carla, time_out, target):
|
||||
|
||||
|
||||
curr_x = -1
|
||||
curr_y = -1
|
||||
prev_x = -1
|
||||
prev_y = -1
|
||||
measurements,sensor_data= carla.read_data()
|
||||
carla.send_control(Control())
|
||||
t0 = measurements.game_timestamp
|
||||
t1=t0
|
||||
success = False
|
||||
step = 0
|
||||
accum_lane_intersect = 0.0
|
||||
accum_sidewalk_intersect = 0.0
|
||||
distance = 100000
|
||||
measurement_vec=[]
|
||||
while((t1-t0) < (time_out*1000) and not success):
|
||||
capture_time = time.time()
|
||||
measurements,sensor_data = carla.read_data()
|
||||
print (sensor_data)
|
||||
print (measurements)
|
||||
print (time.time()-capture_time)
|
||||
control = agent.run_step(measurements,sensor_data,target)
|
||||
print ('STEER ',control.steer,'GAS ',control.throttle,'Brake ',control.brake)
|
||||
carla.send_control(control)
|
||||
|
||||
|
||||
# meassure distance to target
|
||||
|
||||
prev_x = curr_x
|
||||
prev_y = curr_y
|
||||
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
|
||||
print (t1-t0)
|
||||
|
||||
# accumulate layout related signal
|
||||
# accum_lane_intersect += reward.road_intersect
|
||||
#accum_sidewalk_intersect += reward.sidewalk_intersect
|
||||
step += 1
|
||||
# This distance is wrong
|
||||
|
||||
agent.compute_distance()
|
||||
#distance = self.compute_distance([curr_x, curr_y], [prev_x, prev_y], [target.location.x, target.location.y])
|
||||
# debug
|
||||
print('[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
|
||||
|
||||
|
||||
|
||||
if(success):
|
||||
return (1, measurement_vec, float(t1-t0)/1000.0,distance)
|
||||
else:
|
||||
return (0, measurement_vec, time_out,distance)
|
||||
|
||||
|
||||
def benchmark_agent(self,agent,carla,starting_position=0):
|
||||
|
||||
|
||||
|
||||
experiments = self._build_experiments() # Returns a experiment class that is build from a benchmark inherited class
|
||||
self._suffix_name = self._get_names(experiments[starting_position:]) # The fixed name considering all the experiments being run
|
||||
|
||||
|
||||
for experiment in experiments[starting_position:]:
|
||||
|
||||
|
||||
self.write_experiment(experiment) # write the experiment being run
|
||||
|
||||
|
||||
#poses_exp = start_goal_poses[experiment_id]
|
||||
#repetitions = repetitions_per_experiment[experiment_id]
|
||||
#pedestrians_exp = pedestrians[experiment_id]
|
||||
#ehicles_exp = vehicles[experiment_id]
|
||||
|
||||
#for rep in range(repetitions): # CONTROL REPETITION INSIDE EXPERIMENT ???
|
||||
|
||||
# for the different weathers
|
||||
#for weather_cond in weathers:
|
||||
# let's go through all the starting-goal positions of the experiment
|
||||
|
||||
|
||||
|
||||
|
||||
positions = carla.load_settings(experiment.conditions).player_start_spots
|
||||
|
||||
for pose in experiment.poses:
|
||||
for rep in experiment.repetitions:
|
||||
|
||||
trajectory = experiment.poses
|
||||
|
||||
#ped = pedestrians_exp[i]
|
||||
#vehic = vehicles_exp[i]
|
||||
|
||||
start_point = trajectory[0]
|
||||
end_point = trajectory[1]
|
||||
|
||||
carla.start_episode(start_point)
|
||||
|
||||
print('======== !!!! ==========')
|
||||
|
||||
path_distance = agent.get_initial_distance(positions[start_point],positions[end_point])
|
||||
|
||||
time_out = self._calculate_time_out(path_distance)
|
||||
# running the agent
|
||||
(result, reward_vec, final_time, remaining_distance) = self.run_until(agent,carla,time_out,positions[end_point])
|
||||
|
||||
|
||||
|
||||
# compute stats for the experiment
|
||||
|
||||
self.write_summary_results(experiment,pose,rep,path_distance,remaining_distance,final_time,time_out,result)
|
||||
|
||||
|
||||
#rw.writerow(dict_rewards)
|
||||
|
||||
self.write_reward_results(reward_vec)
|
||||
|
||||
|
||||
|
||||
# save results of the experiment
|
||||
#list_stats.append(dict_stats)
|
||||
#print (dict_stats)
|
||||
#w.writerow(dict_stats)
|
||||
|
||||
|
||||
if(result > 0):
|
||||
print('+++++ Target achieved in %f seconds! +++++' % final_time)
|
||||
else:
|
||||
print('----- Tmeout! -----')
|
||||
|
||||
return list_stats
|
||||
|
||||
|
||||
|
||||
|
||||
def write_details(self):
|
||||
pass
|
||||
|
||||
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(self._base_name + self._suffix_name , 'a+') as ofd:
|
||||
|
||||
w = csv.DictWriter(ofd, self._dict_stats.keys())
|
||||
|
||||
|
||||
w.writerow(dict_stats)
|
||||
|
||||
|
||||
|
||||
def write_reward_results(self,experiment,rep,reward_vec):
|
||||
|
||||
with open('rewards_' + self._base_name + self._suffix_name , 'a+') as ofd:
|
||||
|
||||
rw = csv.DictWriter(rfd, dict_rewards.keys())
|
||||
rw.writeheader()
|
||||
|
||||
|
||||
for i in range(len(reward_vec)):
|
||||
dict_rewards['exp_id'] = experiment.id
|
||||
dict_rewards['rep'] = rep
|
||||
dict_rewards['weather'] = experiment.Conditions.WeatherId
|
||||
dict_rewards['collision_gen'] = reward_vec[i].collision_other
|
||||
dict_rewards['collision_ped'] = reward_vec[i].collision_pedestrians
|
||||
dict_rewards['collision_car'] = reward_vec[i].collision_vehicles
|
||||
dict_rewards['lane_intersect'] = reward_vec[i].intersection_otherlane
|
||||
dict_rewards['sidewalk_intersect'] = reward_vec[i].intersection_offroad
|
||||
dict_rewards['pos_x'] = reward_vec[i].transform.location.x
|
||||
dict_rewards['pos_y'] = reward_vec[i].transform.location.y
|
||||
|
||||
rw.writerow(dict_rewards)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def plot_summary_test(self):
|
||||
|
||||
|
||||
summary_weathers = {'train_weather': [1,3,6,8]}
|
||||
|
||||
summary = plot_summary(self._base_name + self._suffix_name,summary_weathers, )
|
||||
|
||||
|
||||
def plot_summary_train(self):
|
||||
|
||||
|
||||
summary_weathers = {'test_weather': [4,14]}
|
||||
|
||||
summary = plot_summary(self._base_name + self._suffix_name,summary_weathers)
|
||||
|
||||
|
||||
|
||||
# To be redefined on subclasses on how to calculate timeout for an episode
|
||||
def _calculate_time_out(self,distance):
|
||||
return 0
|
||||
|
||||
# To be redefined on subclasses
|
||||
def build_experiments(self):
|
||||
pass
|
||||
|
||||
|
||||
def _get_experiments(self,experiments):
|
||||
|
||||
name_cat ='_t'
|
||||
for experiment in experiments:
|
||||
|
||||
name_cat += str(experiment.id) + '.'
|
||||
|
||||
name_cat ='_w'
|
||||
|
||||
for experiment in experiments:
|
||||
|
||||
name_cat += str(experiment.Conditions.WeatherId) +'.'
|
||||
|
||||
|
||||
return name_cat
|
||||
|
||||
|
||||
|
||||
|
||||
"""
|
||||
w = csv.DictWriter(ofd, dict_stats.keys())
|
||||
w.writeheader()
|
||||
rw = csv.DictWriter(rfd, dict_rewards.keys())
|
||||
rw.writeheader()
|
||||
|
||||
|
||||
|
||||
carla = opt_dict['CARLA']
|
||||
width = opt_dict['WIDTH']
|
||||
height = opt_dict['HEIGHT']
|
||||
host = opt_dict['HOST']
|
||||
port = opt_dict['PORT']
|
||||
|
||||
output_summary = opt_dict['OUTPUT_SUMMARY']
|
||||
runnable = opt_dict['RUNNABLE']
|
||||
experiments_to_run = opt_dict['EXPERIMENTS_TO_RUN']
|
||||
pedestrians = opt_dict['PEDESTRIANS']
|
||||
vehicles = opt_dict['VEHICLES']
|
||||
repetitions_per_experiment = opt_dict['REPETITIONS']
|
||||
weathers = opt_dict['WEATHERS']
|
||||
start_goal_poses = opt_dict['START_GOAL_POSES']
|
||||
cameras = opt_dict['CAMERAS']
|
||||
|
||||
list_stats = []
|
||||
|
||||
"""
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
|
||||
from .benchmark import Benchmark
|
||||
from .experiment import Experiment
|
||||
|
||||
|
||||
# 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.
|
||||
|
||||
|
||||
|
||||
class CoRL(Benchmark):
|
||||
|
||||
def _calculate_time_out(self,distance):
|
||||
return ((path_distance/100000.0)/10.0)*3600.0 + 10.0
|
||||
|
||||
def _poses_town01(self):
|
||||
|
||||
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],[34,84],[51,67],[22,17],[91,148],\
|
||||
[20,107],[78,70],[95,102],[68,44],[45,69]],
|
||||
|
||||
[[138,17],[46,16],[26,9],[42,49],[140,26],\
|
||||
[85,97],[65,133],[137,51],[76,66],[46,39],\
|
||||
[40,60],[1,28],[4,129],[121,107],[2,129],\
|
||||
[78,44],[68,85],[41,102],[95,70],[68,129],\
|
||||
[84,69],[47,79],[110,15],[130,17],[0,17]],
|
||||
|
||||
[[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]],
|
||||
|
||||
[[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]]
|
||||
|
||||
]
|
||||
|
||||
def _poses_town02(self):
|
||||
|
||||
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]],
|
||||
|
||||
[[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]],
|
||||
|
||||
[[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]],
|
||||
|
||||
[[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]]
|
||||
|
||||
]
|
||||
|
||||
|
||||
def build_experiments(self,town):
|
||||
|
||||
|
||||
# We set the camera that is going to be used for all experiments
|
||||
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 town == '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,
|
||||
Repetions = 1
|
||||
)
|
||||
experiments_vector.append(experiment)
|
||||
|
||||
return experiments_vector
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
|
||||
from carla.settings import CarlaSettings
|
||||
from carla.sensor import Camera
|
||||
|
||||
class Experiment(object):
|
||||
|
||||
def __init__(self,**kwargs):
|
||||
self.Id = ''
|
||||
self.Conditions = CarlaSettings()
|
||||
self.Poses = [[]]
|
||||
self.Repetitions = 1
|
||||
|
||||
#self. ,vehicles,pedestrians,weather,cameras
|
||||
|
||||
|
||||
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 name(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,292 @@
|
|||
|
||||
import numpy as np
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
import argparse
|
||||
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 task_complete_percentages(data_matrix):
|
||||
|
||||
complete_per = []
|
||||
pos_ant =0
|
||||
for pos in reset_positions:
|
||||
complete_per.append(sum(data_matrix[pos_ant:pos,1])/len(reset_positions))
|
||||
pos_ant =pos
|
||||
|
||||
return complete_per
|
||||
|
||||
def task_average_time_percentages(data_matrix,reset_positions):
|
||||
|
||||
complete_per = []
|
||||
pos_ant =0
|
||||
for pos in reset_positions:
|
||||
complete_per.append(sum(data_matrix[pos_ant:pos,0])/25.0)
|
||||
pos_ant =pos
|
||||
|
||||
return complete_per
|
||||
|
||||
|
||||
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')]
|
||||
|
||||
|
||||
acummulated_distance += sldist((x,y),(prev_x,prev_y))
|
||||
#print sldist((x,y),(prev_x,prev_y))
|
||||
|
||||
prev_x =x
|
||||
prev_y =y
|
||||
|
||||
i+=1
|
||||
return (float(acummulated_distance)/float(100*1000))
|
||||
|
||||
def get_end_positions(data_matrix):
|
||||
|
||||
|
||||
i=0
|
||||
end_positions_vec = []
|
||||
accumulated_time = 0
|
||||
while i < data_matrix.shape[0]:
|
||||
|
||||
end_positions_vec.append(accumulated_time)
|
||||
accumulated_time += data_matrix[i,2]*10
|
||||
i+=1
|
||||
|
||||
return end_positions_vec
|
||||
|
||||
|
||||
def is_car_static(pos,reward_matrix):
|
||||
|
||||
|
||||
x = reward_matrix[pos,0]
|
||||
y = reward_matrix[pos,1]
|
||||
|
||||
prev_x = reward_matrix[pos,0]
|
||||
prev_y = reward_matrix[pos,1]
|
||||
|
||||
if sldist((x,y),(prev_x,prev_y)) > 100:
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def get_end_positions_state(end_positions,data_matrix, reward_matrix):
|
||||
|
||||
|
||||
vector_of_infractions = [0,0,0,0] # Inf1+inf3 , inf2+inf3 or inf3, , inf1+inf4, timeout
|
||||
|
||||
|
||||
for i in range(len(end_positions)):
|
||||
pos = int(end_positions[i] -20)
|
||||
|
||||
if data_matrix[i,1] == 0: # if it failed, lets find the reason
|
||||
|
||||
if reward_matrix[pos,4] > 30000 and is_car_static(pos,reward_matrix): # If it crashed_general
|
||||
|
||||
if reward_matrix[pos,5] > 0.4: # Check if it is out of road
|
||||
# Case 0
|
||||
vector_of_infractions[0] +=1
|
||||
else: # Check it is out of lane or whaever
|
||||
vector_of_infractions[1] +=1
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
elif reward_matrix[pos,2] > 30000 and is_car_static(pos,reward_matrix):
|
||||
|
||||
|
||||
if reward_matrix[pos,6] > 0.1: # Check if it is out of lane
|
||||
vector_of_infractions[2]+=1
|
||||
|
||||
else: # Likely that the it bumped the car but it didn't bother
|
||||
vector_of_infractions[3]+=1
|
||||
|
||||
else: # TimeOUt
|
||||
vector_of_infractions[3]+=1
|
||||
|
||||
|
||||
return vector_of_infractions
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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 print_infractions(infractions):
|
||||
print ' Killometers Without Pedestrians Colision - > ',1.0/(infractions[4]+0.0001)
|
||||
print ' Average Colision',3.0/(infractions[4]+infractions[3] + infractions[2]+0.0001)
|
||||
print ' Killometers Without Car Colision - > ',1.0/(infractions[3]+0.0001)
|
||||
print ' Killometers Without Other Colision - > ',1.0/(infractions[2]+0.0001)
|
||||
print ' Killometers Without Crossing Lane - > ',1.0/(infractions[0]+0.0001)
|
||||
print ' Killometers Without Going to Sidewalk - > ',1.0/(infractions[1]+0.0001)
|
||||
print ' Average Infraction ',2.0/(infractions[0]+infractions[1]+0.0001)
|
||||
|
||||
|
||||
def plot_summary(file,summary_weathers):
|
||||
|
||||
intervention_acc =[0,0,0,0,0]
|
||||
completions_acc = [0,0,0,0]
|
||||
infractions_vec = [0,0,0,0,0]
|
||||
|
||||
compute_infractions = True
|
||||
#,'test_weather':[4,14]
|
||||
# 'test_weather':[4,14]
|
||||
|
||||
#tasks =[0]
|
||||
#tasks =[0,1,2,3]
|
||||
#weathers = [1,3,6,8,2,14]
|
||||
#for file in files_parsed:
|
||||
f = open(file, "rb")
|
||||
header = f.readline()
|
||||
header= header.split(',')
|
||||
header[-1] = header[-1][:-2]
|
||||
f.close()
|
||||
print header
|
||||
f = open('rewards_' + file, "rb")
|
||||
header_rewards = f.readline()
|
||||
header_rewards= header_rewards.split(',')
|
||||
header_rewards[-1] = header_rewards[-1][:-2]
|
||||
f.close()
|
||||
print header_rewards
|
||||
data_matrix = np.loadtxt(open(path + file, "rb"), delimiter=",", skiprows=1)
|
||||
|
||||
tasks = np.unique(data_matrix[:,header.index('exp_id')])
|
||||
|
||||
reward_matrix = np.loadtxt(open(path + 'rewards_file_' + file, "rb"), delimiter=",", skiprows=1)
|
||||
|
||||
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')])
|
||||
summaries = {}
|
||||
for sw in summary_weathers:
|
||||
summaries[sw] = {'completion': 0., 'infractions': np.zeros(5, dtype=np.float), 'num_weathers': 0}
|
||||
for w in weathers:
|
||||
|
||||
task_data_matrix =data_matrix[np.logical_and(data_matrix[:,header.index('exp_id')]== t, data_matrix[:,header.index('weather')]== w)]
|
||||
if compute_infractions:
|
||||
task_reward_matrix =reward_matrix[np.logical_and(reward_matrix[:,header_rewards.index('exp_id')]== float(t), reward_matrix[:,header_rewards.index('weather')]== float(w))]
|
||||
|
||||
completed_episodes = sum(task_data_matrix[:,header.index('result')])/task_data_matrix.shape[0]
|
||||
print 'Task ',t , 'Weather', w
|
||||
|
||||
print ' Entire Episodes Completed (%) - > ', completed_episodes
|
||||
print ''
|
||||
|
||||
#completions_acc = [sum(x) for x in zip(completions, completions_acc)]
|
||||
|
||||
for sw in summary_weathers:
|
||||
if w in summary_weathers[sw]:
|
||||
summaries[sw]['completion'] += completed_episodes
|
||||
summaries[sw]['num_weathers'] += 1
|
||||
|
||||
if compute_infractions:
|
||||
print ' ==== Infraction Related ====='
|
||||
km_run = get_distance_traveled(task_reward_matrix,header_rewards)
|
||||
print ' Drove (KM) - > ', km_run
|
||||
lane_road = get_out_of_road_lane(task_reward_matrix,header_rewards)
|
||||
colisions = get_colisions(task_reward_matrix,header_rewards)
|
||||
infractions = [lane_road[0]/km_run,lane_road[1]/km_run,colisions[0]/km_run,colisions[1]/km_run,colisions[2]/km_run]
|
||||
print_infractions(infractions)
|
||||
|
||||
for sw in summary_weathers:
|
||||
if w in summary_weathers[sw]:
|
||||
# print summaries[sw]
|
||||
# print infractions
|
||||
summaries[sw]['infractions'] += np.array(infractions)
|
||||
|
||||
print '\n\n >>> Task', t, 'summary <<<\n\n'
|
||||
for sw in summary_weathers:
|
||||
print sw, summary_weathers[sw]
|
||||
print 'Num weathers', summaries[sw]['num_weathers']
|
||||
print 'Avg completion', summaries[sw]['completion']/summaries[sw]['num_weathers']
|
||||
print 'Avg infractions'
|
||||
print_infractions(summaries[sw]['infractions']/summaries[sw]['num_weathers'])
|
||||
#
|
||||
#
|
||||
#infractions_vec = [sum(x) for x in zip(infractions, infractions_vec)]
|
||||
#print 'Non_Colisions/Km', (infractions[1]+ infractions[0])/2.0 ,'Lane Cross/Km ',infractions[0],'Side Cross/Km ',infractions[1],'Col Gen /Km ',infractions[2]\
|
||||
#,'Col Ped /Km ',infractions[3],'Col Ped /Km ',infractions[4], 'Acidents/Km ', (infractions[4] +infractions[2] + infractions[3])/3,\
|
||||
#'total', 1/((infractions[4] +infractions[2] + infractions[3] + infractions[1] + infractions[0])/5.0)
|
||||
|
||||
return summaries
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
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
|
||||
|
||||
|
||||
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
|
||||
|
||||
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
|
||||
f, 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,37 @@
|
|||
import numpy as np
|
||||
from scipy.misc import comb
|
||||
|
||||
def bernstein_poly(i, n, t):
|
||||
"""
|
||||
The Bernstein polynomial of n, i as a function of t
|
||||
"""
|
||||
|
||||
return comb(n, i) * ( t**(n-i) ) * (1 - t)**i
|
||||
|
||||
|
||||
def bezier_curve(points, nTimes=1000):
|
||||
"""
|
||||
Given a set of control points, return the
|
||||
bezier curve defined by the control points.
|
||||
|
||||
points should be a list of lists, or list of tuples
|
||||
such as [ [1,1],
|
||||
[2,3],
|
||||
[4,5], ..[Xn, Yn] ]
|
||||
nTimes is the number of time steps, defaults to 1000
|
||||
|
||||
See http://processingjs.nihongoresources.com/bezierinfo/
|
||||
"""
|
||||
|
||||
nPoints = len(points)
|
||||
xPoints = np.array([p[0] for p in points])
|
||||
yPoints = np.array([p[1] for p in points])
|
||||
|
||||
t = np.linspace(0.0, 1.0, nTimes)
|
||||
|
||||
polynomial_array = np.array([ bernstein_poly(i, nPoints-1, t) for i in range(0, nPoints) ])
|
||||
|
||||
xvals = np.dot(xPoints, polynomial_array)
|
||||
yvals = np.dot(yPoints, polynomial_array)
|
||||
|
||||
return xvals, yvals
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
import math
|
||||
import numpy as np
|
||||
from matplotlib import collections as mc
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
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 angle_between(v1,v2):
|
||||
return np.arccos(np.dot(v1,v2) / np.linalg.norm(v1) / np.linalg.norm(v2))
|
||||
def signal(v1,v2):
|
||||
return np.cross(v1,v2) / np.linalg.norm(v1) / np.linalg.norm(v2)
|
||||
|
||||
sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2)
|
||||
|
||||
sldist3 = lambda c1, c2: 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):
|
||||
|
||||
|
||||
self.nodes = set()
|
||||
self.angles ={}
|
||||
self.edges = {}
|
||||
self.distances = {}
|
||||
if graph_file != None:
|
||||
with open(graph_file, 'r') as file:
|
||||
for i in range(5):
|
||||
next(file)
|
||||
for line in file:
|
||||
|
||||
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 k, v in distance_dic.iteritems():
|
||||
|
||||
#print k
|
||||
#print v
|
||||
|
||||
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 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):
|
||||
line_len = 1
|
||||
print self.angles
|
||||
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')
|
||||
fig, 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):
|
||||
|
||||
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,541 @@
|
|||
from graph import *
|
||||
from PIL import Image
|
||||
import math
|
||||
from astar import *
|
||||
import time
|
||||
import collections
|
||||
|
||||
compare = lambda x, y: collections.Counter(x) == collections.Counter(y)
|
||||
|
||||
def angle_between(v1,v2):
|
||||
return np.arccos(np.dot(v1,v2) / np.linalg.norm(v1) / np.linalg.norm(v2))
|
||||
|
||||
sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2)
|
||||
|
||||
def color_to_angle(color):
|
||||
return ((float(color)/255.0) ) *2*math.pi
|
||||
|
||||
|
||||
|
||||
class Planner(object):
|
||||
|
||||
# The built graph. This is the exact same graph that unreal builds. This is a generic structure used for many cases
|
||||
def __init__(self,city_file,map_file):
|
||||
# read the conversion parameters from the city file
|
||||
with open(city_file, 'r') as file:
|
||||
|
||||
linewordloffset = file.readline()
|
||||
# The offset of the world from the zero coordinates ( The coordinate we consider zero)
|
||||
self.worldoffset = string_to_floats(linewordloffset)
|
||||
|
||||
#WARNING: for now just considering the y angle
|
||||
lineworldangles = file.readline()
|
||||
self.angles = string_to_floats(lineworldangles)
|
||||
#self.worldrotation = np.array([[math.cos(math.radians(self.angles[0])),0,math.sin(math.radians(self.angles[0])) ],[0,1,0],[-math.sin(math.radians(self.angles[0])),0,math.cos(math.radians(self.angles[0]))]])
|
||||
|
||||
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
|
||||
lineworscale = file.readline()
|
||||
|
||||
linemapoffset = file.readline()
|
||||
|
||||
# The offset of the map zero coordinate
|
||||
self.mapoffset = string_to_floats(linemapoffset)
|
||||
|
||||
# the graph resolution.
|
||||
linegraphres = file.readline()
|
||||
self.resolution = string_to_node(linegraphres)
|
||||
|
||||
|
||||
#This is the bmp map that will be associated with the graph.
|
||||
#This map contains the navigable paths drawed
|
||||
self.graph = Graph(city_file)
|
||||
|
||||
self.map_image = Image.open(map_file)
|
||||
self.map_image.load()
|
||||
self.map_image = np.asarray(self.map_image, dtype="int32" )
|
||||
|
||||
|
||||
import os
|
||||
dir_path = os.path.dirname(__file__)
|
||||
self.central_path_map_image = Image.open(map_file[:-4] +'c.png')
|
||||
self.central_path_map_image.load()
|
||||
self.central_path_map_image = np.asarray(self.central_path_map_image, dtype="int32" )
|
||||
self.central_path_map_image =self.central_path_map_image[:,:,0] # Just take the red dimension
|
||||
|
||||
self.grid = self.make_grid()
|
||||
self.walls = self.make_walls()
|
||||
|
||||
|
||||
self.previous_source = (0,0)
|
||||
self.distance = 0
|
||||
self.complete_distance = 0
|
||||
|
||||
#print self.map_image
|
||||
self.commands = []
|
||||
|
||||
self.route =[]
|
||||
|
||||
|
||||
# The number of game units per pixel
|
||||
self.pixel_density = 16.43
|
||||
#A pixel positions with respect to graph node position is: Pixel = Node*50 +2
|
||||
self.node_density = 50.0
|
||||
# This function converts the 2d map into a 3D one in a vector.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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_grid(self): # The resolution could be easily increased
|
||||
|
||||
|
||||
grid = np.ones((self.resolution[0],self.resolution[1]))
|
||||
|
||||
for key,connections in self.graph.edges.iteritems():
|
||||
|
||||
# draw a line
|
||||
for con in connections:
|
||||
|
||||
#print key[0],key[1],con[0],con[1]
|
||||
grid = self._draw_line(grid,key[0],key[1],con[0],con[1])
|
||||
#print grid
|
||||
|
||||
|
||||
np.set_printoptions( linewidth =206,threshold=np.nan)
|
||||
|
||||
|
||||
return grid
|
||||
|
||||
|
||||
def make_walls(self):
|
||||
walls = set()
|
||||
|
||||
for i in range(self.grid.shape[0]):
|
||||
|
||||
for j in range(self.grid.shape[1]):
|
||||
if self.grid[i,j] == 1.0:
|
||||
walls.add((i,j))
|
||||
|
||||
return walls
|
||||
|
||||
def init(self,source,target):
|
||||
|
||||
self.a_star.init_grid(self.resolution[0],self.resolution[1], self.walls,source,target)
|
||||
|
||||
|
||||
def solve(self):
|
||||
return self.a_star.solve()
|
||||
|
||||
|
||||
|
||||
|
||||
# Convert world position into "Graph World" node positions
|
||||
def make_node(self,worldvertex):
|
||||
|
||||
pixel = self.make_map_world(worldvertex)
|
||||
|
||||
|
||||
node = []
|
||||
|
||||
|
||||
|
||||
|
||||
node.append((pixel[0])/self.node_density - 2)
|
||||
node.append((pixel[1])/self.node_density - 2 )
|
||||
|
||||
return tuple(node)
|
||||
|
||||
def make_map_world(self,world):
|
||||
|
||||
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 make_map_node(self,node):
|
||||
pixel = []
|
||||
pixel.append((node[0] +2) *self.node_density)
|
||||
pixel.append((node[1] +2) *self.node_density)
|
||||
|
||||
return pixel
|
||||
|
||||
def make_world_map(self,pixel):
|
||||
|
||||
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)
|
||||
return world_vertex
|
||||
|
||||
|
||||
def make_world_node(self,node):
|
||||
|
||||
return self.make_world_map(self.make_map_node(node))
|
||||
|
||||
|
||||
def get_distance_closest_node(self,pos):
|
||||
import collections
|
||||
distance = []
|
||||
for node_iter in self.graph.intersection_nodes():
|
||||
|
||||
distance.append( sldist(node_iter,pos))
|
||||
|
||||
return sorted(distance)[0]
|
||||
|
||||
def get_distance_closest_node(self,pos):
|
||||
import collections
|
||||
distance = []
|
||||
for node_iter in self.graph.intersection_nodes():
|
||||
|
||||
distance.append( sldist(node_iter,pos))
|
||||
|
||||
return sorted(distance)[0]
|
||||
|
||||
def get_distance_closest_node_route(self,pos,route):
|
||||
import collections
|
||||
distance = []
|
||||
#if self.graph.intersection_nodes() == set():
|
||||
|
||||
for node_iter in route:
|
||||
|
||||
if node_iter in self.graph.intersection_nodes():
|
||||
|
||||
distance.append( sldist(node_iter,pos))
|
||||
|
||||
if not distance:
|
||||
|
||||
return sldist(route[-1],pos)
|
||||
return sorted(distance)[0]
|
||||
|
||||
|
||||
def get_target_ori(self,target_pos):
|
||||
|
||||
relative_location = []
|
||||
pixel=[]
|
||||
rotation = np.array([target_pos[0],target_pos[1],target_pos[2]])
|
||||
rotation = rotation.dot(self.worldrotation)
|
||||
|
||||
#print 'rot ', rotation
|
||||
|
||||
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])
|
||||
#print 'trans ', relative_location
|
||||
|
||||
pixel.append(math.floor(relative_location[0]/float(self.pixel_density)))
|
||||
pixel.append(math.floor(relative_location[1]/float(self.pixel_density)))
|
||||
#print self.map_image.shape
|
||||
ori = self.map_image[int(pixel[1]),int(pixel[0]),2]
|
||||
ori = ((float(ori)/255.0) ) *2*math.pi
|
||||
|
||||
#print self.map_image[int(pixel[1]),int(pixel[0]),:]
|
||||
#print ori
|
||||
#print (math.cos(ori),math.sin(ori))
|
||||
#print exit()
|
||||
|
||||
return (-math.cos(ori),-math.sin(ori))
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def search(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.grid[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.resolution[0] and c_y>= 0 and c_y <self.resolution[1]:
|
||||
if self.grid[c_x,c_y] ==0:
|
||||
break
|
||||
else:
|
||||
c_x,c_y = x,y
|
||||
scale +=1
|
||||
|
||||
return (c_x,c_y)
|
||||
|
||||
def get_adjacent_free_nodes(self,pos):
|
||||
|
||||
""" Acht 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.resolution[0] and node[1]>= 0 and node[1] <self.resolution[1]:
|
||||
|
||||
if self.grid[node[0],node[1]] == 0.0:
|
||||
adjacent.add(node)
|
||||
|
||||
return adjacent
|
||||
|
||||
|
||||
def set_grid_direction(self,pos,pos_ori,target):
|
||||
|
||||
free_nodes = self.get_adjacent_free_nodes(pos)
|
||||
|
||||
|
||||
added_walls =set()
|
||||
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) :
|
||||
self.grid[adj[0],adj[1]] =1.0
|
||||
|
||||
added_walls.add((adj[0],adj[1]))
|
||||
self.walls.add((adj[0],adj[1]))
|
||||
|
||||
return added_walls
|
||||
|
||||
def set_grid_direction_target(self,pos,pos_ori,source):
|
||||
|
||||
free_nodes = self.get_adjacent_free_nodes(pos)
|
||||
|
||||
|
||||
added_walls =set()
|
||||
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) :
|
||||
self.grid[adj[0],adj[1]] =1.0
|
||||
|
||||
added_walls.add((adj[0],adj[1]))
|
||||
self.walls.add((adj[0],adj[1]))
|
||||
|
||||
return added_walls
|
||||
|
||||
|
||||
#from the shortest path graph, transform it into a list of commands
|
||||
# @param the sub graph containing the shortest path
|
||||
# @param Orientation of the car
|
||||
# returns list of commands ( 3 is left, 4 is right, 5 is straight)
|
||||
|
||||
def graph_to_commands(self, route ):
|
||||
|
||||
commands_list = []
|
||||
|
||||
for i in range(0,len(route)):
|
||||
if route[i] not in self.graph.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)
|
||||
|
||||
command = 0.0
|
||||
if angle < -0.1:
|
||||
command = 4.0
|
||||
elif angle > 0.1:
|
||||
command = 3.0
|
||||
else:
|
||||
command =5.0
|
||||
|
||||
commands_list.append(command)
|
||||
|
||||
|
||||
return commands_list
|
||||
|
||||
def check_command_completed(self,commands,previous_commands):
|
||||
if compare(commands,previous_commands):
|
||||
return False,False
|
||||
elif (len(commands) + 1) < len(previous_commands):
|
||||
return True,False
|
||||
|
||||
elif len(commands) < len(previous_commands):
|
||||
|
||||
|
||||
|
||||
return True,compare(commands,previous_commands[1:])
|
||||
else:
|
||||
return True,False
|
||||
|
||||
|
||||
|
||||
def get_full_distance_route(self,pos,route):
|
||||
import collections
|
||||
distance = 0
|
||||
#if self.graph.intersection_nodes() == set():
|
||||
current_pos = pos
|
||||
for node_iter in route:
|
||||
|
||||
|
||||
|
||||
distance += sldist(node_iter,current_pos)
|
||||
current_pos = node_iter
|
||||
|
||||
|
||||
return distance
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def get_next_command(self,source,source_ori, target,target_ori):
|
||||
|
||||
|
||||
node_source = self.make_node(source)
|
||||
node_target = self.make_node(target)
|
||||
|
||||
source_ori = np.array([source_ori[0],source_ori[1],source_ori[2]])
|
||||
source_ori = source_ori.dot(self.worldrotation)
|
||||
|
||||
|
||||
# Trunkate !
|
||||
node_source = tuple([ int(x) for x in node_source ])
|
||||
node_target = tuple([ int(x) for x in node_target ])
|
||||
target_ori = self.get_target_ori(target)
|
||||
# Set to zero if it is less than zero.
|
||||
|
||||
|
||||
|
||||
target_ori = np.array([target_ori[0],target_ori[1],0])
|
||||
target_ori = target_ori.dot(self.worldrotation)
|
||||
|
||||
|
||||
node_source =(max(0,node_source[0]),max(0,node_source[1]))
|
||||
node_source =(min(self.resolution[0]-1,node_source[0]),min(self.resolution[1]-1,node_source[1]))
|
||||
# is it x or y ? Check to avoid special corner cases
|
||||
|
||||
|
||||
if math.fabs(source_ori[0]) > math.fabs(source_ori[1]):
|
||||
source_ori = (source_ori[0],0.0,0.0)
|
||||
else:
|
||||
source_ori = (0.0,source_ori[1],0.0)
|
||||
|
||||
|
||||
|
||||
node_source = self.search(node_source[0],node_source[1])
|
||||
node_target = self.search(node_target[0],node_target[1])
|
||||
#print ''
|
||||
#print node_source
|
||||
#print node_target
|
||||
#print self.grid
|
||||
|
||||
# reach the goal
|
||||
if node_source == node_target:
|
||||
return 0,0
|
||||
|
||||
|
||||
# This is to avoid computing a new route when inside the route
|
||||
distance_node = self.get_distance_closest_node(node_source)
|
||||
|
||||
if (distance_node >1 and self.previous_source != node_source) or self.complete_distance ==0:
|
||||
|
||||
#print node_source
|
||||
#print node_target
|
||||
added_walls = self.set_grid_direction(node_source,source_ori,node_target)
|
||||
#print added_walls
|
||||
added_walls=added_walls.union(self.set_grid_direction_target(node_target,target_ori,node_source))
|
||||
#print added_walls
|
||||
self.previous_source = node_source
|
||||
|
||||
#print self.grid
|
||||
|
||||
self.a_star =AStar()
|
||||
self.init(node_source, node_target)
|
||||
route = self.solve()
|
||||
#print route # JuSt a Corner Case
|
||||
if route == None:
|
||||
for i in added_walls:
|
||||
self.walls.remove(i)
|
||||
|
||||
self.grid[i[0],i[1]] = 0.0
|
||||
added_walls = self.set_grid_direction(node_source,source_ori,node_target)
|
||||
self.a_star =AStar()
|
||||
self.init(node_source, node_target)
|
||||
route = self.solve()
|
||||
|
||||
#print route
|
||||
|
||||
# We recompute the distance based on route
|
||||
self.distance= self.get_distance_closest_node_route(node_source,route)
|
||||
self.complete_distance = self.get_full_distance_route(node_source,route)*50.0*16.42
|
||||
|
||||
for i in added_walls:
|
||||
self.walls.remove(i)
|
||||
|
||||
self.grid[i[0],i[1]] = 0.0
|
||||
|
||||
commands = self.graph_to_commands(route)
|
||||
made_turn,completed_command = self.check_command_completed(commands,self.commands)
|
||||
|
||||
|
||||
self.commands = commands
|
||||
next_node=route[0]
|
||||
for i in route:
|
||||
if i in self.graph.nodes:
|
||||
next_node = i
|
||||
break
|
||||
|
||||
|
||||
|
||||
if self.distance > 4:
|
||||
return 2.0,self.complete_distance
|
||||
else:
|
||||
if self.commands:
|
||||
return self.commands[0],self.complete_distance
|
||||
else:
|
||||
return 2.0,self.complete_distance
|
||||
else:
|
||||
|
||||
if self.distance >4:
|
||||
return 2.0,self.complete_distance
|
||||
|
||||
if self.commands:
|
||||
return self.commands[0],self.complete_distance
|
||||
else:
|
||||
return 2.0,self.complete_distance
|
||||
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
|
||||
import argparse
|
||||
import logging
|
||||
import sys
|
||||
|
||||
|
||||
from benchmarks.corl import CoRL
|
||||
from benchmarks.agent import Agent
|
||||
|
||||
from carla.tcp import TCPConnectionError
|
||||
from carla.client import make_carla_client
|
||||
|
||||
|
||||
class Manual(Agent):
|
||||
def run_step(self, data,target):
|
||||
control = Control()
|
||||
control.steer = 0.0
|
||||
control.throttle = 0.9
|
||||
control.brake = 0.0
|
||||
control.hand_brake = False
|
||||
control.reverse = False
|
||||
|
||||
return control
|
||||
|
||||
|
||||
if(__name__ == '__main__'):
|
||||
|
||||
|
||||
argparser = argparse.ArgumentParser(description=__doc__)
|
||||
argparser.add_argument(
|
||||
'-v', '--verbose',
|
||||
action='store_true',
|
||||
dest='debug',
|
||||
help='print debug information')
|
||||
argparser.add_argument(
|
||||
'--host',
|
||||
metavar='H',
|
||||
default='localhost',
|
||||
help='IP of the host server (default: localhost)')
|
||||
argparser.add_argument(
|
||||
'-p', '--port',
|
||||
metavar='P',
|
||||
default=2000,
|
||||
type=int,
|
||||
help='TCP port to listen to (default: 2000)')
|
||||
|
||||
|
||||
|
||||
args = argparser.parse_args()
|
||||
|
||||
log_level = logging.DEBUG if args.debug else logging.INFO
|
||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)
|
||||
|
||||
logging.info('listening to server %s:%s', args.host, args.port)
|
||||
|
||||
while True:
|
||||
try:
|
||||
with make_carla_client(host, port) as client:
|
||||
corl= CoRL('test')
|
||||
agent = Manual()
|
||||
results = corl.benchmark_agent(agent,carla)
|
||||
|
||||
break
|
||||
|
||||
except TCPConnectionError as error:
|
||||
logging.error(error)
|
||||
time.sleep(1)
|
||||
except Exception as exception:
|
||||
logging.exception(exception)
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# DETECT AN ERROR AND WRITE THE COMPLETE SUMMARY ALREADY
|
||||
Loading…
Reference in New Issue