From d9624f6616dfc97912a865b5db49feedaf32a809 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 1 Dec 2017 19:27:27 +0100 Subject: [PATCH 01/59] Adding the benchmark to the client, a first big step. --- PythonClient/benchmarks/__init__.py | 0 PythonClient/benchmarks/agent.py | 72 ++++ PythonClient/benchmarks/benchmark.py | 309 +++++++++++++++ PythonClient/benchmarks/corl.py | 137 +++++++ PythonClient/benchmarks/experiment.py | 37 ++ PythonClient/benchmarks/metrics.py | 292 ++++++++++++++ PythonClient/carla/planner/astar.py | 145 +++++++ PythonClient/carla/planner/bezier.py | 37 ++ PythonClient/carla/planner/graph.py | 152 ++++++++ PythonClient/carla/planner/planner.py | 541 ++++++++++++++++++++++++++ PythonClient/run_benchmark.py | 77 ++++ 11 files changed, 1799 insertions(+) create mode 100644 PythonClient/benchmarks/__init__.py create mode 100644 PythonClient/benchmarks/agent.py create mode 100644 PythonClient/benchmarks/benchmark.py create mode 100644 PythonClient/benchmarks/corl.py create mode 100644 PythonClient/benchmarks/experiment.py create mode 100644 PythonClient/benchmarks/metrics.py create mode 100644 PythonClient/carla/planner/astar.py create mode 100644 PythonClient/carla/planner/bezier.py create mode 100644 PythonClient/carla/planner/graph.py create mode 100644 PythonClient/carla/planner/planner.py create mode 100644 PythonClient/run_benchmark.py diff --git a/PythonClient/benchmarks/__init__.py b/PythonClient/benchmarks/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/PythonClient/benchmarks/agent.py b/PythonClient/benchmarks/agent.py new file mode 100644 index 000000000..a0d450307 --- /dev/null +++ b/PythonClient/benchmarks/agent.py @@ -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 +""" diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py new file mode 100644 index 000000000..11b13d1f6 --- /dev/null +++ b/PythonClient/benchmarks/benchmark.py @@ -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 = [] + +""" diff --git a/PythonClient/benchmarks/corl.py b/PythonClient/benchmarks/corl.py new file mode 100644 index 000000000..c7b032493 --- /dev/null +++ b/PythonClient/benchmarks/corl.py @@ -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 diff --git a/PythonClient/benchmarks/experiment.py b/PythonClient/benchmarks/experiment.py new file mode 100644 index 000000000..3d26052c3 --- /dev/null +++ b/PythonClient/benchmarks/experiment.py @@ -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 + diff --git a/PythonClient/benchmarks/metrics.py b/PythonClient/benchmarks/metrics.py new file mode 100644 index 000000000..3e8bf2859 --- /dev/null +++ b/PythonClient/benchmarks/metrics.py @@ -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 + + + + diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py new file mode 100644 index 000000000..e239247ad --- /dev/null +++ b/PythonClient/carla/planner/astar.py @@ -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)) diff --git a/PythonClient/carla/planner/bezier.py b/PythonClient/carla/planner/bezier.py new file mode 100644 index 000000000..04e46d150 --- /dev/null +++ b/PythonClient/carla/planner/bezier.py @@ -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 diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py new file mode 100644 index 000000000..a3a4d7d4b --- /dev/null +++ b/PythonClient/carla/planner/graph.py @@ -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) + + + + + + diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py new file mode 100644 index 000000000..04c72cd12 --- /dev/null +++ b/PythonClient/carla/planner/planner.py @@ -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 = 0 and c_y = 0 and node[0] = 0 and node[1] 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 + diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py new file mode 100644 index 000000000..0d43c12bf --- /dev/null +++ b/PythonClient/run_benchmark.py @@ -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 \ No newline at end of file From 85d693032ec769da4a8e5719139dbde24fd64ad3 Mon Sep 17 00:00:00 2001 From: felipecode Date: Tue, 5 Dec 2017 14:22:37 +0100 Subject: [PATCH 02/59] benchmark seems to be working a first version. The planner needs further work " ; --- PythonClient/benchmarks/agent.py | 16 +-- PythonClient/benchmarks/benchmark.py | 129 +++++++++---------- PythonClient/benchmarks/corl.py | 13 +- PythonClient/benchmarks/experiment.py | 2 +- PythonClient/carla/client.py | 2 +- PythonClient/carla/planner/Town01Central.png | Bin 0 -> 27332 bytes PythonClient/carla/planner/Town02Central.png | Bin 0 -> 10624 bytes PythonClient/carla/planner/planner.py | 2 +- PythonClient/run_benchmark.py | 19 ++- 9 files changed, 91 insertions(+), 92 deletions(-) create mode 100644 PythonClient/carla/planner/Town01Central.png create mode 100644 PythonClient/carla/planner/Town02Central.png diff --git a/PythonClient/benchmarks/agent.py b/PythonClient/benchmarks/agent.py index a0d450307..7918c74d7 100644 --- a/PythonClient/benchmarks/agent.py +++ b/PythonClient/benchmarks/agent.py @@ -21,21 +21,21 @@ from carla.carla_server_pb2 import Control class Agent(object, ): - def __init__(self, **kwargs): + def __init__(self,city_name, **kwargs): import os dir_path = os.path.dirname(__file__) - planner = Planner(dir_path+'/../planner/' + city_name + '.txt',\ - dir_path+'/../planner/' + city_name + '.png') + self._planner = Planner(dir_path+'/../carla/planner/' + city_name + '.txt',\ + dir_path+'/../carla/planner/' + city_name + '.png') - def get_initial_distance(self): + def get_distance(self,start_point,end_point): - _,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)) + _,path_distance=self._planner.get_next_command([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],(1,0,0)) # We calculate the timout based on the distance return path_distance diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py index 11b13d1f6..7424452b9 100644 --- a/PythonClient/benchmarks/benchmark.py +++ b/PythonClient/benchmarks/benchmark.py @@ -1,10 +1,19 @@ from .metrics import plot_summary +try: + from carla import carla_server_pb2 as carla_protocol +except ImportError: + raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') + + +import json, csv, time + class Benchmark(object,): # Param @name to be used for saving purposes - def __init__(self,name): + def __init__(self,city_name,name): + self._city_name = city_name # The name of the city that is going to be used. 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, @@ -19,10 +28,6 @@ class Benchmark(object,): } - with open(self._base_name + self._suffix_name , 'wb') as ofd: - - w = csv.DictWriter(ofd, self._dict_stats.keys()) - w.writeheader() self._dict_rewards = { @@ -47,7 +52,7 @@ class Benchmark(object,): prev_x = -1 prev_y = -1 measurements,sensor_data= carla.read_data() - carla.send_control(Control()) + carla.send_control(carla_protocol.Control()) t0 = measurements.game_timestamp t1=t0 success = False @@ -59,9 +64,7 @@ class Benchmark(object,): 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) @@ -78,17 +81,12 @@ class Benchmark(object,): 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 + # The distance is based on graph but quite not exact. + distance = agent.get_distance(measurements.player_measurements.transform,target) + 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): @@ -107,51 +105,46 @@ class Benchmark(object,): 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 + self._suffix_name = self._get_experiments_names(experiments[starting_position:]) # The fixed name considering all the experiments being run + with open(self._base_name + self._suffix_name , 'wb') as ofd: + + w = csv.DictWriter(ofd, self._dict_stats.keys()) + w.writeheader() + + + with open('rewards_' + self._base_name + self._suffix_name , 'wb') as rfd: + + rw = csv.DictWriter(rfd, self._dict_rewards.keys()) + rw.writeheader() + + + + self.write_experiment() # write the experiment 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: + for rep in range(experiment.repetitions): - trajectory = experiment.poses - #ped = pedestrians_exp[i] - #vehic = vehicles_exp[i] - start_point = trajectory[0] - end_point = trajectory[1] + start_point = pose[0] + end_point = pose[1] carla.start_episode(start_point) print('======== !!!! ==========') - path_distance = agent.get_initial_distance(positions[start_point],positions[end_point]) + path_distance = agent.get_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]) + (result, reward_vec, final_time, remaining_distance) = self.run_navigation_episode(agent,carla,time_out,positions[end_point]) @@ -160,18 +153,11 @@ class Benchmark(object,): 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) + self.write_reward_results(experiment,rep,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: @@ -182,8 +168,11 @@ class Benchmark(object,): - def write_details(self): - pass + def write_experiment(self): + + with open(self._get_details() , 'wb') as ofd: + pass + def write_summary_results(self,experiment,pose,rep,path_distance,remaining_distance,final_time,time_out,result): @@ -205,31 +194,31 @@ class Benchmark(object,): w = csv.DictWriter(ofd, self._dict_stats.keys()) - w.writerow(dict_stats) + w.writerow(self._dict_stats) def write_reward_results(self,experiment,rep,reward_vec): - with open('rewards_' + self._base_name + self._suffix_name , 'a+') as ofd: + with open('rewards_' + self._base_name + self._suffix_name , 'a+') as rfd: - rw = csv.DictWriter(rfd, dict_rewards.keys()) + rw = csv.DictWriter(rfd, self._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 + 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(dict_rewards) + rw.writerow(self._dict_rewards) @@ -240,7 +229,7 @@ class Benchmark(object,): summary_weathers = {'train_weather': [1,3,6,8]} - summary = plot_summary(self._base_name + self._suffix_name,summary_weathers, ) + summary = plot_summary(self._base_name + self._suffix_name,summary_weathers) def plot_summary_train(self): @@ -257,11 +246,11 @@ class Benchmark(object,): return 0 # To be redefined on subclasses - def build_experiments(self): + def _build_experiments(self): pass - def _get_experiments(self,experiments): + def _get_experiments_names(self,experiments): name_cat ='_t' for experiment in experiments: diff --git a/PythonClient/benchmarks/corl.py b/PythonClient/benchmarks/corl.py index c7b032493..17fe61d21 100644 --- a/PythonClient/benchmarks/corl.py +++ b/PythonClient/benchmarks/corl.py @@ -1,7 +1,8 @@ from .benchmark import Benchmark from .experiment import Experiment - +from carla.sensor import Camera +from carla.settings import CarlaSettings # 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. @@ -10,7 +11,7 @@ from .experiment import Experiment class CoRL(Benchmark): - def _calculate_time_out(self,distance): + def _calculate_time_out(self,path_distance): return ((path_distance/100000.0)/10.0)*3600.0 + 10.0 def _poses_town01(self): @@ -72,7 +73,7 @@ class CoRL(Benchmark): ] - def build_experiments(self,town): + def _build_experiments(self): # We set the camera that is going to be used for all experiments @@ -86,7 +87,7 @@ class CoRL(Benchmark): camera.set_rotation(-15.0,0,0) weathers = [1,3,6,8,4,14] - if town == 'Town01': + if self._city_name == 'Town01': poses_tasks = self._poses_town01() vehicles_tasks =[0,0,0,20] pedestrians_tasks = [0,0,0,50] @@ -130,8 +131,10 @@ class CoRL(Benchmark): Conditions=conditions, Poses= poses, Id = iteration, - Repetions = 1 + 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 'corl_' + self._city_name \ No newline at end of file diff --git a/PythonClient/benchmarks/experiment.py b/PythonClient/benchmarks/experiment.py index 3d26052c3..7e31821ad 100644 --- a/PythonClient/benchmarks/experiment.py +++ b/PythonClient/benchmarks/experiment.py @@ -20,7 +20,7 @@ class Experiment(object): setattr(self, key, value) @property - def name(self): + def id(self): return self.Id @property diff --git a/PythonClient/carla/client.py b/PythonClient/carla/client.py index 748200928..80fde992b 100644 --- a/PythonClient/carla/client.py +++ b/PythonClient/carla/client.py @@ -126,7 +126,7 @@ class CarlaClient(object): def send_control(self, *args, **kwargs): """ Send the VehicleControl to be applied this frame. - + If synchronous mode was requested, the server will pause the simulation until this message is received. """ diff --git a/PythonClient/carla/planner/Town01Central.png b/PythonClient/carla/planner/Town01Central.png new file mode 100644 index 0000000000000000000000000000000000000000..b2d769f94b463364721d7d9e3ffd2e8ba0405441 GIT binary patch literal 27332 zcmeI5ZAep57{{N<-gTr+NXuYls4zoiUwo<1nCLKvMIaKRnP`Co^96|%nj<1JGAKgR zV&D3d)|awOnL#n5=tG1rWiG7n!g1M@feXhi%8k4l(Q{mt)su#ll)XgnUPlc!#R5I2HT>tmDjWo#cTPW zgl-R{u#JjVQ)Q{*vs$YuG;}u&HWKN`WGp#;ZpJ$^uCeQqW#c`6{8wcQ6W;NKBdSHZ z=uKcRLxBQNU@I$NtLyVvmR>)K3mv4ZrDB(&ImxCX z{*+JK#2b3zJNL5Q+Qx>O0DZpv1*vDK<42@>^^5e1*pn}c%}!-_1r?A4Fc8cDW&ksQ z83+c@KnfY)6X6rr!zcF6&dWk$0oeP&x?o*ltN;UG01WVn0R}cS;&|qreW-EeCP?#A z-7ESc?Qqvne8KZcg(~pbNRtR1kS5_W00zJy6o`2kdYF65pEi8p1AKYu*u%!odCt7a z){9lwZiR%F^*MKuwm^155P?*ckO8SGA;TI|RY-wy6(J&50T#to01QF}zyKH!a>6GH zD}YagPYeu|^VBb1J4=^pb(bfKmrhRixhgv>?xe9pL)3VOkq^N(N`?1J4*8oaYjOdj{Zt`T!jJmQdDv3;` z#YYf9C?XI=1iK(R90ag@BrZfk4O<7AUt3gAUt3<*Tsbx!UJXt2oDe* z@TPH-^d6WeYoD^88`#f(BLHFBKb#k5+xK22CCaV3sN-Gsev@K^~P+;(MaSW-r_2$k-US>lc7ROf) z|3y#Z{&A$>2y2?xjs^j<3p_xLqhK@yFhii;O6|teb1~W=nGJU@iaqpwzzSq0So3E0 zA7=rp_hr%t(GF7%aD!+8)elS{+J$ouh++sFl?DaFXt0bX2VgQ7%_XCy1E@F{Efq&A z3t%!Btt3a=4!~qE+Bg_(8~~HSXyahCaR5vPqm6?h-;?}mc+jr=Srj8EHWl3Nu-?&O zV0bUYP(D5W=OT!p7q}nI&>>U-<}YH21F3D$XcPzQ8I>LlmeJ&(Fq#dB%mxo`pOc-# zz{3!{WT4bUd1nhkOHhbdqQ^A&)jp=s51Tv*WK6 zaLE88Xf42K0XjNYJvv7}+Kw3Q7moG|hevbziqXN?Z)diF3$^Xhx0Jw*t5IS!1kghu YSAT|%VlYqiT1$|Or>mdKI;Vst0Q|Fa2><{9 literal 0 HcmV?d00001 diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 04c72cd12..cf898ee20 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -59,7 +59,7 @@ class Planner(object): 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 = Image.open(map_file[:-4] +'Central.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 diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 0d43c12bf..b6cc05a74 100644 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -9,11 +9,16 @@ from benchmarks.agent import Agent from carla.tcp import TCPConnectionError from carla.client import make_carla_client +import time +try: + from carla import carla_server_pb2 as carla_protocol +except ImportError: + raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') class Manual(Agent): - def run_step(self, data,target): - control = Control() + def run_step(self,measurements,sensor_data,target): + control = carla_protocol.Control() control.steer = 0.0 control.throttle = 0.9 control.brake = 0.0 @@ -55,10 +60,12 @@ if(__name__ == '__main__'): while True: try: - with make_carla_client(host, port) as client: - corl= CoRL('test') - agent = Manual() - results = corl.benchmark_agent(agent,carla) + with make_carla_client(args.host, args.port) as client: + corl= CoRL('Town02','test') + agent = Manual('Town02') + results = corl.benchmark_agent(agent,client) + corl.plot_summary_test() + corl.plot_summary_train() break From 5406d5f0ebe74926b7d3360eb992bf9e525bcabb Mon Sep 17 00:00:00 2001 From: felipecode Date: Tue, 5 Dec 2017 17:02:16 +0100 Subject: [PATCH 03/59] changed the used distance function --- PythonClient/benchmarks/benchmark.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py index 7424452b9..48c08fab0 100644 --- a/PythonClient/benchmarks/benchmark.py +++ b/PythonClient/benchmarks/benchmark.py @@ -9,6 +9,8 @@ except ImportError: import json, csv, time +sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) + class Benchmark(object,): # Param @name to be used for saving purposes @@ -85,7 +87,9 @@ class Benchmark(object,): step += 1 # The distance is based on graph but quite not exact. - distance = agent.get_distance(measurements.player_measurements.transform,target) + distance = sldist([curr_x,curr_y],[target.location.x, target.location.y]) + + 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)) @@ -139,6 +143,7 @@ class Benchmark(object,): carla.start_episode(start_point) print('======== !!!! ==========') + print(' Start Position ',start_point,' End Position ',end_point) path_distance = agent.get_distance(positions[start_point],positions[end_point]) From 1b89041375c11dda68e313b9dcf28922fd2235c1 Mon Sep 17 00:00:00 2001 From: felipecode Date: Tue, 5 Dec 2017 17:04:34 +0100 Subject: [PATCH 04/59] changed the used distance function --- PythonClient/benchmarks/benchmark.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py index 48c08fab0..26d4b840c 100644 --- a/PythonClient/benchmarks/benchmark.py +++ b/PythonClient/benchmarks/benchmark.py @@ -6,7 +6,7 @@ except ImportError: raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') -import json, csv, time +import json, csv, time, math sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) From 269979d0c72e05713b2526fcb9c9ae6a17830f10 Mon Sep 17 00:00:00 2001 From: felipecode Date: Tue, 5 Dec 2017 18:28:25 +0100 Subject: [PATCH 05/59] changing spaces. Debugging ploting --- PythonClient/benchmarks/benchmark.py | 19 +-- PythonClient/benchmarks/corl.py | 204 ++++++++++++++------------- PythonClient/benchmarks/metrics.py | 7 +- 3 files changed, 121 insertions(+), 109 deletions(-) diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py index 26d4b840c..82f4554a5 100644 --- a/PythonClient/benchmarks/benchmark.py +++ b/PythonClient/benchmarks/benchmark.py @@ -14,7 +14,7 @@ sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) class Benchmark(object,): # Param @name to be used for saving purposes - def __init__(self,city_name,name): + def __init__(self,city_name,name,starting_position=0): self._city_name = city_name # The name of the city that is going to be used. 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, @@ -45,6 +45,11 @@ class Benchmark(object,): 'pos_y':-1 } + # Returns a experiment class that is build from a benchmark inherited class + self._experiments = self._build_experiments() + + self._suffix_name = self._get_experiments_names(self._experiments) + def run_navigation_episode(self, agent,carla, time_out, target): @@ -104,12 +109,10 @@ class Benchmark(object,): return (0, measurement_vec, time_out,distance) - def benchmark_agent(self,agent,carla,starting_position=0): + def benchmark_agent(self,agent,carla): - + # The fixed name considering all the experiments being run - experiments = self._build_experiments() # Returns a experiment class that is build from a benchmark inherited class - self._suffix_name = self._get_experiments_names(experiments[starting_position:]) # The fixed name considering all the experiments being run with open(self._base_name + self._suffix_name , 'wb') as ofd: w = csv.DictWriter(ofd, self._dict_stats.keys()) @@ -126,7 +129,7 @@ class Benchmark(object,): self.write_experiment() # write the experiment being run - for experiment in experiments[starting_position:]: + for experiment in self._experiments[starting_position:]: @@ -208,7 +211,7 @@ class Benchmark(object,): with open('rewards_' + self._base_name + self._suffix_name , 'a+') as rfd: rw = csv.DictWriter(rfd, self._dict_rewards.keys()) - rw.writeheader() + for i in range(len(reward_vec)): @@ -258,7 +261,7 @@ class Benchmark(object,): def _get_experiments_names(self,experiments): name_cat ='_t' - for experiment in experiments: + for experiment in experiments: name_cat += str(experiment.id) + '.' diff --git a/PythonClient/benchmarks/corl.py b/PythonClient/benchmarks/corl.py index 17fe61d21..ecd522e1b 100644 --- a/PythonClient/benchmarks/corl.py +++ b/PythonClient/benchmarks/corl.py @@ -1,3 +1,11 @@ +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB), and the INTEL Visual Computing Lab. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +# CORL experiment set. + from .benchmark import Benchmark from .experiment import Experiment @@ -11,130 +19,136 @@ from carla.settings import CarlaSettings class CoRL(Benchmark): - def _calculate_time_out(self,path_distance): - return ((path_distance/100000.0)/10.0)*3600.0 + 10.0 + def _calculate_time_out(self,path_distance): + return ((path_distance/100000.0)/10.0)*3600.0 + 10.0 - def _poses_town01(self): + 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]], + 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]], + [[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]], - [[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): + 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]], + 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]], + [[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]], - [[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): + 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) - # 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) - 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] + 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 = [] + experiments_vector = [] - for weather in weathers: + for weather in weathers: - for iteration in range(len(poses_tasks)): + for iteration in range(len(poses_tasks)): - poses = poses_tasks[iteration] - vehicles = vehicles_tasks[iteration] - pedestrians = pedestrians_tasks[iteration] + 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 = 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) + conditions.add_sensor(camera) - experiment = Experiment() - experiment.set( - Conditions=conditions, - Poses= poses, - Id = iteration, - Repetitions = 1 - ) - experiments_vector.append(experiment) + 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 'corl_' + self._city_name \ No newline at end of file + return experiments_vector + + def _get_details(self): + # Function to get automatic information from the experiment for writing purposes + return 'corl_' + self._city_name \ No newline at end of file diff --git a/PythonClient/benchmarks/metrics.py b/PythonClient/benchmarks/metrics.py index 3e8bf2859..bd3856ad5 100644 --- a/PythonClient/benchmarks/metrics.py +++ b/PythonClient/benchmarks/metrics.py @@ -278,12 +278,7 @@ def plot_summary(file,summary_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 From 876d5a091ec9808b559468acb3945bcc29264495 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 6 Dec 2017 15:17:46 +0100 Subject: [PATCH 06/59] Correcting the metrics path and adding a new path definition for the experiments --- PythonClient/benchmarks/benchmark.py | 20 ++++++++++++-------- PythonClient/benchmarks/corl.py | 9 +++++++-- PythonClient/benchmarks/metrics.py | 17 ++++++++--------- 3 files changed, 27 insertions(+), 19 deletions(-) diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py index 82f4554a5..428a036f1 100644 --- a/PythonClient/benchmarks/benchmark.py +++ b/PythonClient/benchmarks/benchmark.py @@ -6,7 +6,7 @@ except ImportError: raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') -import json, csv, time, math +import json, csv, time, math,os sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) @@ -49,7 +49,15 @@ class Benchmark(object,): self._experiments = self._build_experiments() self._suffix_name = self._get_experiments_names(self._experiments) - + + + self._full_name = '_benchmarks_results/' + self._base_name +'_' + self._get_details() +'/' + folder = os.path.dirname(self._full_name) + if not os.path.isdir(folder): + os.makedirs(folder) + + + def run_navigation_episode(self, agent,carla, time_out, target): @@ -174,12 +182,8 @@ class Benchmark(object,): return list_stats - - - def write_experiment(self): - with open(self._get_details() , 'wb') as ofd: - pass + def write_summary_results(self,experiment,pose,rep,path_distance,remaining_distance,final_time,time_out,result): @@ -236,7 +240,7 @@ class Benchmark(object,): summary_weathers = {'train_weather': [1,3,6,8]} - + print self._base_name + self._suffix_name summary = plot_summary(self._base_name + self._suffix_name,summary_weathers) diff --git a/PythonClient/benchmarks/corl.py b/PythonClient/benchmarks/corl.py index ecd522e1b..59872bff6 100644 --- a/PythonClient/benchmarks/corl.py +++ b/PythonClient/benchmarks/corl.py @@ -12,6 +12,9 @@ from .experiment import Experiment from carla.sensor import Camera from carla.settings import CarlaSettings +import datetime + + # 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. @@ -88,7 +91,7 @@ class CoRL(Benchmark): # We set the camera # This single RGB camera is used on every experiment - + camera = Camera('CameraRGB') camera.set(CameraFOV=100) @@ -150,5 +153,7 @@ class CoRL(Benchmark): return experiments_vector def _get_details(self): + + now = datetime.datetime.now() # Function to get automatic information from the experiment for writing purposes - return 'corl_' + self._city_name \ No newline at end of file + return 'corl_' + self._city_name + '_' + now.strftime("%Y%m%d%H%M") \ No newline at end of file diff --git a/PythonClient/benchmarks/metrics.py b/PythonClient/benchmarks/metrics.py index bd3856ad5..5597ed6c9 100644 --- a/PythonClient/benchmarks/metrics.py +++ b/PythonClient/benchmarks/metrics.py @@ -206,30 +206,29 @@ def plot_summary(file,summary_weathers): infractions_vec = [0,0,0,0,0] compute_infractions = True - #,'test_weather':[4,14] - # 'test_weather':[4,14] + # Separate the PATH and the basename + path = os.path.dirname(file) + base_name = os.path.basename(file) + - #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") + + f = open(os.path.join(path,'rewards_' , base_name), "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) + data_matrix = np.loadtxt(open(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) + reward_matrix = np.loadtxt(open(os.path.join(path,'rewards_' , base_name), "rb"), delimiter=",", skiprows=1) for t in tasks: task_data_matrix = data_matrix[data_matrix[:,header.index('exp_id')]== t] From 9a622b5fbef4b906beba66e2bfd92fe1d17b1615 Mon Sep 17 00:00:00 2001 From: felipecode Date: Sat, 9 Dec 2017 18:51:34 +0100 Subject: [PATCH 07/59] A new version of the benchmark, now able to continue were stopped Added a installation script, currently have to install for pip --- PythonClient/MANIFEST.in | 2 + PythonClient/benchmarks/benchmark.py | 310 ------------------ PythonClient/benchmarks/metrics.py | 286 ---------------- .../{ => carla}/benchmarks/__init__.py | 0 PythonClient/{ => carla}/benchmarks/agent.py | 4 +- PythonClient/carla/benchmarks/benchmark.py | 296 +++++++++++++++++ PythonClient/{ => carla}/benchmarks/corl.py | 145 ++++++-- .../{ => carla}/benchmarks/experiment.py | 0 PythonClient/carla/benchmarks/metrics.py | 201 ++++++++++++ PythonClient/run_benchmark.py | 91 ++--- PythonClient/setup.py | 12 + 11 files changed, 673 insertions(+), 674 deletions(-) create mode 100644 PythonClient/MANIFEST.in delete mode 100644 PythonClient/benchmarks/benchmark.py delete mode 100644 PythonClient/benchmarks/metrics.py rename PythonClient/{ => carla}/benchmarks/__init__.py (100%) rename PythonClient/{ => carla}/benchmarks/agent.py (93%) create mode 100644 PythonClient/carla/benchmarks/benchmark.py rename PythonClient/{ => carla}/benchmarks/corl.py (55%) rename PythonClient/{ => carla}/benchmarks/experiment.py (100%) create mode 100644 PythonClient/carla/benchmarks/metrics.py create mode 100644 PythonClient/setup.py diff --git a/PythonClient/MANIFEST.in b/PythonClient/MANIFEST.in new file mode 100644 index 000000000..824b0ad1d --- /dev/null +++ b/PythonClient/MANIFEST.in @@ -0,0 +1,2 @@ +include carla/planner/*.txt +include carla/planner/*.png diff --git a/PythonClient/benchmarks/benchmark.py b/PythonClient/benchmarks/benchmark.py deleted file mode 100644 index 428a036f1..000000000 --- a/PythonClient/benchmarks/benchmark.py +++ /dev/null @@ -1,310 +0,0 @@ - -from .metrics import plot_summary -try: - from carla import carla_server_pb2 as carla_protocol -except ImportError: - raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') - - -import json, csv, time, math,os - - -sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) - -class Benchmark(object,): - - # Param @name to be used for saving purposes - def __init__(self,city_name,name,starting_position=0): - self._city_name = city_name # The name of the city that is going to be used. - 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 - - - } - - - 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 - } - - # Returns a experiment class that is build from a benchmark inherited class - self._experiments = self._build_experiments() - - self._suffix_name = self._get_experiments_names(self._experiments) - - - self._full_name = '_benchmarks_results/' + self._base_name +'_' + self._get_details() +'/' - folder = os.path.dirname(self._full_name) - if not os.path.isdir(folder): - os.makedirs(folder) - - - - - 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(carla_protocol.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() - - 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 - - - step += 1 - # The distance is based on graph but quite not exact. - distance = sldist([curr_x,curr_y],[target.location.x, target.location.y]) - - - - 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): - - # The fixed name considering all the experiments being run - - with open(self._base_name + self._suffix_name , 'wb') as ofd: - - w = csv.DictWriter(ofd, self._dict_stats.keys()) - w.writeheader() - - - with open('rewards_' + self._base_name + self._suffix_name , 'wb') as rfd: - - rw = csv.DictWriter(rfd, self._dict_rewards.keys()) - rw.writeheader() - - - - self.write_experiment() # write the experiment being run - - - for experiment in self._experiments[starting_position:]: - - - - positions = carla.load_settings(experiment.conditions).player_start_spots - - for pose in experiment.poses: - for rep in range(experiment.repetitions): - - - - start_point = pose[0] - end_point = pose[1] - - carla.start_episode(start_point) - - print('======== !!!! ==========') - print(' Start Position ',start_point,' End Position ',end_point) - - path_distance = agent.get_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_navigation_episode(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) - - - - self.write_reward_results(experiment,rep,reward_vec) - - - - if(result > 0): - print('+++++ Target achieved in %f seconds! +++++' % final_time) - else: - print('----- Tmeout! -----') - - return list_stats - - - - - - - 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(self._dict_stats) - - - - def write_reward_results(self,experiment,rep,reward_vec): - - with open('rewards_' + self._base_name + 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 plot_summary_test(self): - - - summary_weathers = {'train_weather': [1,3,6,8]} - print self._base_name + self._suffix_name - 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_names(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 = [] - -""" diff --git a/PythonClient/benchmarks/metrics.py b/PythonClient/benchmarks/metrics.py deleted file mode 100644 index 5597ed6c9..000000000 --- a/PythonClient/benchmarks/metrics.py +++ /dev/null @@ -1,286 +0,0 @@ - -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 - # Separate the PATH and the basename - path = os.path.dirname(file) - base_name = os.path.basename(file) - - - f = open(file, "rb") - header = f.readline() - header= header.split(',') - header[-1] = header[-1][:-2] - f.close() - print header - - f = open(os.path.join(path,'rewards_' , base_name), "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(file, "rb"), delimiter=",", skiprows=1) - - tasks = np.unique(data_matrix[:,header.index('exp_id')]) - - reward_matrix = np.loadtxt(open(os.path.join(path,'rewards_' , base_name), "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']) - - - return summaries - - - - diff --git a/PythonClient/benchmarks/__init__.py b/PythonClient/carla/benchmarks/__init__.py similarity index 100% rename from PythonClient/benchmarks/__init__.py rename to PythonClient/carla/benchmarks/__init__.py diff --git a/PythonClient/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py similarity index 93% rename from PythonClient/benchmarks/agent.py rename to PythonClient/carla/benchmarks/agent.py index 7918c74d7..9bfe3aca9 100644 --- a/PythonClient/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -25,8 +25,8 @@ class Agent(object, ): import os dir_path = os.path.dirname(__file__) - self._planner = Planner(dir_path+'/../carla/planner/' + city_name + '.txt',\ - dir_path+'/../carla/planner/' + city_name + '.png') + self._planner = Planner(dir_path+'/../planner/' + city_name + '.txt',\ + dir_path+'/../planner/' + city_name + '.png') diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py new file mode 100644 index 000000000..4d52d5d6a --- /dev/null +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -0,0 +1,296 @@ + +import os +import math +import time +import csv +import json + +import datetime + + +try: + from carla import carla_server_pb2 as carla_protocol +except ImportError: + raise RuntimeError( + 'cannot import "carla_server_pb2.py' + +', run the protobuf compiler to generate this file') + + +sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) + + +class Benchmark(object,): + + # Param @name to be used for saving purposes + def __init__(self, city_name, name_to_save, continue_exp=False): + # The name of the city that is going to be used. + self._city_name = city_name + # Sends a base name, the rest will be saved with respect to what the + # episode was about + 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 + } + + # If this name was run try to continue + + # Returns a experiment class that is build from a benchmark inherited + # class + + self._experiments = self._build_experiments() + self._suffix_name = self._get_experiments_names(self._experiments) + self._full_name = os.path.join('_benchmarks_results', + self._base_name + '_' + + self._get_details() + '/') + + if self._experiment_exist(): + # If continue_exp was set we continue directly, else we ask for + if continue_exp: + self._line_on_file = self._get_last_position() + + else: + answer = str(raw_input("The experiment was already found in the files" + + ", Do you want to Continue ? \n")) + if answer == 'Yes' or answer == 'y': + self._line_on_file = self._get_last_position() + else: + self._line_on_file = 0 + + else: + self._line_on_file = 0 + + folder = os.path.dirname(self._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() + with open(os.path.join(self._full_name, + now.strftime("%Y%m%d%H%M")), 'wb') as f: + pass + + 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(carla_protocol.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() + + 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 + + step += 1 + # The distance is based on graph but quite not exact. + distance = sldist([curr_x, curr_y], + [target.location.x, target.location.y]) + + 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): + + 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), 'wb') as ofd: + + w = csv.DictWriter(ofd, self._dict_stats.keys()) + w.writeheader() + + with open(os.path.join(self._full_name, + 'rewards_' + self._suffix_name), 'wb') 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) + + print (' 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) + + print('======== !!!! ==========') + print(' Start Position ', start_point, + ' End Position ', end_point) + + path_distance = agent.get_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_navigation_episode( + 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) + + self.write_reward_results(experiment, rep, reward_vec) + + if(result > 0): + print('+++++ Target achieved in %f seconds! +++++' % + final_time) + else: + print('----- 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_reward_results(self, experiment, rep, reward_vec): + + with open(os.path.join(self._full_name, + 'rewards_' + 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) + + + + # 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_names(self, experiments): + + name_cat = 'w' + + for experiment in experiments: + + name_cat += str(experiment.Conditions.WeatherId) + '.' + + return name_cat + + def _experiment_exist(self): + + folder = os.path.dirname(self._full_name) + return os.path.isdir(folder) + + def _get_last_position(self): + + with open(os.path.join(self._full_name, self._suffix_name)) as f: + + # Function to count the lines. + for i, l in enumerate(f): + pass + return i + diff --git a/PythonClient/benchmarks/corl.py b/PythonClient/carla/benchmarks/corl.py similarity index 55% rename from PythonClient/benchmarks/corl.py rename to PythonClient/carla/benchmarks/corl.py index 59872bff6..b9ac67bff 100644 --- a/PythonClient/benchmarks/corl.py +++ b/PythonClient/carla/benchmarks/corl.py @@ -6,87 +6,156 @@ # CORL experiment set. +from __future__ import print_function + +import os +import datetime from .benchmark import Benchmark from .experiment import Experiment from carla.sensor import Camera from carla.settings import CarlaSettings -import datetime - -# 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. +from .metrics import compute_summary class CoRL(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,path_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 ((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],\ + + + def _poses_town01(self): + """ + For each matrix is a new task + + """ + 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],[34,84],[51,67],[22,17],[91,148],\ - [20,107],[78,70],[95,102],[68,44],[45,69]], + [20,107],[78,70],[95,102],[68,44],[45,69]] - [[138,17],[46,16],[26,9],[42,49],[140,26],\ + def _poses_one_curve(): + return [[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]], + [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],\ + 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): - return [ [[38, 34], [4, 2], [12, 10], [62, 55], [43, 47],\ + + 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]], + [54,63],[51,42],[16,19],[17,26],[77,68]] - [[37, 76], [8, 24], [60, 69], [38, 10], [21, 1],\ + 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]], + [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],\ + 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. + """ + Creates the whole set of experiment objects, + The experiments created depend on the selected Town. """ # We set the camera @@ -156,4 +225,14 @@ class CoRL(Benchmark): now = datetime.datetime.now() # Function to get automatic information from the experiment for writing purposes - return 'corl_' + self._city_name + '_' + now.strftime("%Y%m%d%H%M") \ No newline at end of file + return 'corl_' + 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 line_on_file/len(self._experiments),line_on_file%25 + diff --git a/PythonClient/benchmarks/experiment.py b/PythonClient/carla/benchmarks/experiment.py similarity index 100% rename from PythonClient/benchmarks/experiment.py rename to PythonClient/carla/benchmarks/experiment.py diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py new file mode 100644 index 000000000..46e22e316 --- /dev/null +++ b/PythonClient/carla/benchmarks/metrics.py @@ -0,0 +1,201 @@ +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB), and the INTEL Visual Computing Lab. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + + +import numpy as np +import math +import os +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 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(file, dynamic_episodes): + + # Separate the PATH and the basename + path = os.path.dirname(file) + base_name = os.path.basename(file) + + f = open(file, "rb") + header = f.readline() + header = header.split(',') + header[-1] = header[-1][:-2] + f.close() + + f = open(os.path.join(path, 'rewards_' + base_name), "rb") + header_rewards = f.readline() + header_rewards = header_rewards.split(',') + header_rewards[-1] = header_rewards[-1][:-2] + f.close() + + data_matrix = np.loadtxt(open(file, "rb"), delimiter=",", skiprows=1) + + 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, 'rewards_' + 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_rewards.index( + 'exp_id')] == float(t), reward_matrix[:, header_rewards.index('weather')] == float(w))] + + km_run = get_distance_traveled( + task_reward_matrix, header_rewards) + + 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_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] + + + metrics_dictionary['intersection_offroad'][ + w][t] = lane_road[0]/km_run + metrics_dictionary['intersection_otherlane'][ + w][t] = lane_road[1]/km_run + metrics_dictionary['collision_pedestrians'][ + w][t] = colisions[2]/km_run + metrics_dictionary['collision_vehicles'][ + w][t] = colisions[1]/km_run + metrics_dictionary['collision_other'][ + w][t] = colisions[0]/km_run + + + return metrics_dictionary diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index b6cc05a74..ce7824d09 100644 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -1,7 +1,9 @@ +import time +import sys import argparse import logging -import sys + from benchmarks.corl import CoRL @@ -9,7 +11,7 @@ from benchmarks.agent import Agent from carla.tcp import TCPConnectionError from carla.client import make_carla_client -import time + try: from carla import carla_server_pb2 as carla_protocol @@ -31,54 +33,57 @@ class Manual(Agent): 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)') + argparser = argparse.ArgumentParser(description=__doc__) + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='localhost', + help='IP of the host server (default: localhost)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-c', '--city-name', + metavar='C', + default='Town01', + help='plot the map of the current city' + +'(needs to match active map in server, options: Town01 or Town02)') - args = argparser.parse_args() + args = argparser.parse_args() - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + 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(args.host, args.port) as client: - corl= CoRL('Town02','test') - agent = Manual('Town02') - 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) - except Exception as exception: - logging.exception(exception) - sys.exit(1) + 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= CoRL(city_name= args.city_name,name_to_save='test') + agent = Manual(args.map_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) + except Exception as exception: + logging.exception(exception) + sys.exit(1) -# DETECT AN ERROR AND WRITE THE COMPLETE SUMMARY ALREADY \ No newline at end of file diff --git a/PythonClient/setup.py b/PythonClient/setup.py new file mode 100644 index 000000000..d1317032d --- /dev/null +++ b/PythonClient/setup.py @@ -0,0 +1,12 @@ +from setuptools import setup + +setup( + name='PythonClient', + version='0.7', + packages=['carla','carla.planner','carla.benchmarks','carla.planner'], + license='', + long_description=open('README.txt').read(), + include_package_data=True, + package_data={'carla.planner' : ['Town01.txt']} +) + From f36bd466c27697ef90d264667eb4c9bd0ce6889d Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 11 Dec 2017 17:07:29 +0100 Subject: [PATCH 08/59] Planner complete refactoring, pt 1 --- PythonClient/carla/benchmarks/agent.py | 3 +- PythonClient/carla/benchmarks/benchmark.py | 8 +- PythonClient/carla/benchmarks/experiment.py | 49 +- PythonClient/carla/planner/astar.py | 14 +- PythonClient/carla/planner/city_track.py | 121 ++++ PythonClient/carla/planner/graph.py | 15 +- PythonClient/carla/planner/grid.py | 133 +++++ PythonClient/carla/planner/map.py | 230 +++++++- PythonClient/carla/planner/planner.py | 592 +++++--------------- PythonClient/run_benchmark.py | 8 +- 10 files changed, 665 insertions(+), 508 deletions(-) create mode 100644 PythonClient/carla/planner/city_track.py create mode 100644 PythonClient/carla/planner/grid.py diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py index 9bfe3aca9..3a03ea1cb 100644 --- a/PythonClient/carla/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -25,8 +25,7 @@ class Agent(object, ): import os dir_path = os.path.dirname(__file__) - self._planner = Planner(dir_path+'/../planner/' + city_name + '.txt',\ - dir_path+'/../planner/' + city_name + '.png') + self._planner = Planner(city_name) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 4d52d5d6a..fe9214189 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -196,11 +196,11 @@ class Benchmark(object,): # compute stats for the experiment - self.write_summary_results( + self._write_summary_results( experiment, pose, rep, path_distance, remaining_distance, final_time, time_out, result) - self.write_reward_results(experiment, rep, reward_vec) + self._write_reward_results(experiment, rep, reward_vec) if(result > 0): print('+++++ Target achieved in %f seconds! +++++' % @@ -211,7 +211,7 @@ class Benchmark(object,): - def write_summary_results(self, experiment, pose, rep, + def _write_summary_results(self, experiment, pose, rep, path_distance, remaining_distance, final_time, time_out, result): @@ -232,7 +232,7 @@ class Benchmark(object,): w.writerow(self._dict_stats) - def write_reward_results(self, experiment, rep, reward_vec): + def _write_reward_results(self, experiment, rep, reward_vec): with open(os.path.join(self._full_name, 'rewards_' + self._suffix_name), 'a+') as rfd: diff --git a/PythonClient/carla/benchmarks/experiment.py b/PythonClient/carla/benchmarks/experiment.py index 7e31821ad..7c13c50b5 100644 --- a/PythonClient/carla/benchmarks/experiment.py +++ b/PythonClient/carla/benchmarks/experiment.py @@ -2,36 +2,35 @@ 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 + def __init__(self, **kwargs): + self.Id = '' + self.Conditions = CarlaSettings() + self.Poses = [[]] + self.Repetitions = 1 - #self. ,vehicles,pedestrians,weather,cameras + # 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) - 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 id(self): - return self.Id + @property + def conditions(self): + return self.Conditions - @property - def conditions(self): - return self.Conditions - - @property - def poses(self): - return self.Poses - - @property - def repetitions(self): - return self.Repetitions + @property + def poses(self): + return self.Poses + @property + def repetitions(self): + return self.Repetitions diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py index e239247ad..26d1c371e 100644 --- a/PythonClient/carla/planner/astar.py +++ b/PythonClient/carla/planner/astar.py @@ -1,7 +1,9 @@ import heapq +from carla.planner.map import CarlaMap class Cell(object): + def __init__(self, x, y, reachable): """Initialize new cell. @@ -23,7 +25,10 @@ class Cell(object): class AStar(object): + def __init__(self): + + # open list self.opened = [] heapq.heapify(self.opened) @@ -34,8 +39,10 @@ class AStar(object): self.grid_height = None self.grid_width = None - def init_grid(self, width, height, walls, start, end): - """Prepare grid cells, walls. + def init_grid(self, width, height, walls,start, end): + + """ + Prepare grid cells, walls. @param width grid's width. @param height grid's height. @@ -143,3 +150,6 @@ class AStar(object): self.update_cell(adj_cell, cell) # add adj cell to open list heapq.heappush(self.opened, (adj_cell.f, adj_cell)) + + + diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py new file mode 100644 index 000000000..db37285be --- /dev/null +++ b/PythonClient/carla/planner/city_track.py @@ -0,0 +1,121 @@ + +class CityTrack(object) + + + def __init__(self,city_name) + + + self._map = CarlaMap(city_name) + + self._astar = AStar() + + # Refers to the start position of the previous route computation + self._previous_node = [] + + + + + + + def project_node(self,node,node_orientation): + """ + Projecting the graph node into the city road + """ + + # To change the orientation with respect to the map standards + node_orientation = np.array([node_orientation[0], + node_orientation[1],node_orientation[2]]) + node_orientation = source_ori.dot(self.worldrotation) + + 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])) + # is it x or y ? Check to avoid special corner cases + + + if math.fabs(node_orientation[0]) > math.fabs(node_orientation[1]): + node_orientation = (source_ori[0],0.0,0.0) + else: + node_orientation = (0.0,source_ori[1],0.0) + + node = self.map.grid_search(node[0],node[1]) + + + return node + + 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 + return self._closest_intersection_route_position(current_node, + self._previous_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(node_source,self._map.get_graph_resolution()[0], + self._map.get_graph_resolution()[1], + self._map.get_walls_directed(node_target,target_ori,node_source), + node_target) + + + route = a_star.solve() + + + + #print route # JuSt a Corner Case + # REALLY, I want to remove this + if route == None: + a_star =AStar() + a_star.init_grid(node_source,self._map.get_graph_resolution()[0], + self._map.get_graph_resolution()[1],self._map.get_walls(), node_target) + + route = a_star.solve() + + + + return route + + + + + def _closest_intersection_position(self, current_node): + + distance_vector = [] + for node_iterator in self._map._graph.intersection_nodes(): + + distance_vector.append(sldist(node_iterator, current_node)) + + return sorted(distance_vector)[0] + + def _closest_intersection_route_position(self, current_node): + + distance_vector = [] + for node_iterator in self._map._graph.intersection_nodes(): + + distance_vector.append(sldist(node_iterator, current_node)) + + return sorted(distance_vector)[0] + + + + diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index a3a4d7d4b..3b7579358 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -28,8 +28,6 @@ class Graph(object): """ - - def __init__(self,graph_file=None): @@ -37,12 +35,18 @@ class Graph(object): self.angles ={} self.edges = {} self.distances = {} + self.node_density =50 if graph_file != None: with open(graph_file, 'r') as file: - for i in range(5): + for i in range(4): next(file) + + # the graph resolution. + linegraphres = file.readline() + self._resolution = string_to_node(linegraphres) 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) @@ -63,7 +67,12 @@ class Graph(object): def add_node(self, value): self.nodes.add(value) + def project_pixel(self,pixel) + node = [] + node.append((pixel[0])/self.node_density - 2) + node.append((pixel[1])/self.node_density - 2) + return tuple(node) def make_orientations(self,node,heading): diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py new file mode 100644 index 000000000..aecbe2d16 --- /dev/null +++ b/PythonClient/carla/planner/grid.py @@ -0,0 +1,133 @@ + + +class Grid(object): + + def __init__(self,graph): + + self.graph = graph + self.structure = self._make_structure() + self.walls = self._make_walls() + + #np.set_printoptions(linewidth=206, threshold=np.nan) + + + + + + + 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.resolution[0], self.graph.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] + 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.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 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.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 \ No newline at end of file diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index 142fc2a14..c8ba8247f 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -9,6 +9,7 @@ import math import os + try: import numpy as np except ImportError: @@ -19,15 +20,44 @@ 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 graph import string_to_node,string_to_floats -def string_to_floats(string): - vec = string.split(',') - return (float(vec[0]), float(vec[1]), float(vec[2])) + + + +""" + + with open(city_file + '.txt', '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) + +""" + +# The map contains a graph class CarlaMap(object): @@ -37,6 +67,15 @@ class CarlaMap(object): 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 + 'Center.png') + + + # The built graph. This is the exact same graph that unreal builds. This + # is a generic structure used for many cases + + self._graph = Graph(city_file) + + self._grid = Grid() with open(city_file, 'r') as file: @@ -48,6 +87,7 @@ class CarlaMap(object): lineworldangles = file.readline() self.angles = string_to_floats(lineworldangles) + # If tere is an rotation between the world and map coordinates. 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], @@ -55,17 +95,14 @@ class CarlaMap(object): # Ignore for now, these are offsets for map coordinates and scale # (not used). - _ = file.readline() + map_scale = 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) - # The number of game units per pixel. + # The number of game units per pixel. For now this is fixed. self.pixel_density = 16.43 # Load the lanes image self.map_image_lanes = Image.open(city_map_file_lanes) @@ -76,6 +113,15 @@ 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._resolution def get_map(self, height=None): if height is not None: @@ -113,6 +159,14 @@ class CarlaMap(object): return pixel + def get_position_on_graph(self, world): + """Get the position on the map for a certain world position.""" + pixel = self.get_position_on_map(world) + return self._graph.project_pixel(pixel) + + + + def get_position_on_world(self, pixel): """Get world position of a certain map position.""" relative_location = [] @@ -144,3 +198,155 @@ class CarlaMap(object): ori = ((float(ori) / 255.0)) * 2 * math.pi return (-math.cos(ori), -math.sin(ori)) + + + + + + + + def + + + + + + + + + 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_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 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 get_walls_directed(self,node_target,target_ori,node_source): + + # GOes to Grid + #added_walls = self.set_grid_direction(node_source,source_ori,node_target) + + + #print added_walls + # Goes to grid + #added_walls=added_walls.union(self.set_grid_direction_target(node_target,target_ori,node_source)) + #print added_walls + + def get_walls(self): + + return walss + + + + + 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] + diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index cf898ee20..9e835e187 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -1,541 +1,221 @@ -from graph import * -from PIL import Image import math -from astar import * import time import collections +import os + + +from PIL import Image + +from graph import Graph +from astar import Astar 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)) + +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 +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) + def __init__(self, city_name): - #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]]) + self._city_track = CityTrack(city_name) - # Ignore for now - lineworscale = file.readline() + #self.previous_source = (0, 0) + ##self.distance = 0 + #self.complete_distance = 0 - linemapoffset = file.readline() + # print self.map_image + self._commands = [] - # The offset of the map zero coordinate - self.mapoffset = string_to_floats(linemapoffset) + #self.route = [] - # 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" ) + # 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. - import os - dir_path = os.path.dirname(__file__) - self.central_path_map_image = Image.open(map_file[:-4] +'Central.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 get_next_command(self, source, source_ori, target, target_ori): + #self.astar. + track_source = self._city_track.project_node(source,source_ori) + track_target = self._city_track.project_node(target,target_ori) - 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 + # reach the goal + if self._city_track.is_at_goal(track_source,track_target) + return 0,0 + #if node_source == node_target: + # return 0, 0 - for i in range(xi,xf+1): - for j in range(yi,yf+1): + if self._city_track.is_at_new_node(track_source) + and self._city_track.is_away_from_intersection(track_source): - grid[i,j] = 0.0 - return grid - def make_grid(self): # The resolution could be easily increased + # print route + route= self._city_track.compute_route(track_source,source_ori, + track_target,target_ori) + #(node_source,source_ori,node_target,target_ori) - grid = np.ones((self.resolution[0],self.resolution[1])) - for key,connections in self.graph.edges.iteritems(): + # We recompute the distance based on route + #self.distance = self.get_distance_closest_node_route( + # node_source, route) - # draw a line - for con in connections: + #self.complete_distance = self.get_full_distance_route( + # node_source, route)*50.0*16.42 # This is bad - #print key[0],key[1],con[0],con[1] - grid = self._draw_line(grid,key[0],key[1],con[0],con[1]) - #print grid + self._commands = self._route_to_commands(route) - np.set_printoptions( linewidth =206,threshold=np.nan) - return grid + if self._city_track.is_far_away_from_route_intersection(track_source): + return 2.0 + else: + if self.commands: + return self.commands[0] + else: + return 2.0 + else: - def make_walls(self): - walls = set() + if self._city_track.is_far_away_from_route_intersection(track_source): + return 2.0 - 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)) + # If there is computed commands + if self.commands: + return self.commands[0] + else: + return 2.0 - return walls - def init(self,source,target): + def get_shortest_path_distance(self, source, source_ori, target, target_ori): + import collections + distance = 0 + # if self.graph.intersection_nodes() == set(): + current_pos = pos + for node_iter in route: - self.a_star.init_grid(self.resolution[0],self.resolution[1], self.walls,source,target) + distance += sldist(node_iter, current_pos) + current_pos = node_iter + return distance# *50.0*16.42 , maybe this goes to another layer - def solve(self): - return self.a_star.solve() + def is_there_posible_route(self,source,source_ori,target,target_ori): + track_source = self._city_track.project_node(source,source_ori) + track_target = self._city_track.project_node(target,target_ori) - # Convert world position into "Graph World" node positions - def make_node(self,worldvertex): + return len(self._city_track.compute_route( + node_source,source_ori,node_target,target_ori))>0 - pixel = self.make_map_world(worldvertex) - node = [] + # 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 _route_to_commands(self, route): + commands_list = [] - node.append((pixel[0])/self.node_density - 2) - node.append((pixel[1])/self.node_density - 2 ) + for i in range(0, len(route)): + if route[i] not in self.astar.intersection_nodes(): + continue - return tuple(node) + current = route[i] + past = route[i-1] + future = route[i+1] - def make_map_world(self,world): + 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) - relative_location = [] - pixel=[] + command = 0.0 + if angle < -0.1: + command = 4.0 + elif angle > 0.1: + command = 3.0 + else: + command = 5.0 - rotation = np.array([world[0],world[1],world[2]]) - rotation = rotation.dot(self.worldrotation) + commands_list.append(command) + return commands_list +""" +# 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 - 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 self.grid - - pixel.append(math.floor(relative_location[0]/float(self.pixel_density))) - pixel.append(math.floor(relative_location[1]/float(self.pixel_density))) +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) - 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) + 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() - return pixel - def make_world_map(self,pixel): + 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 - relative_location =[] - world_vertex = [] - relative_location.append(pixel[0]*self.pixel_density) - relative_location.append(pixel[1]*self.pixel_density) + elif len(commands) < len(previous_commands): - 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 + return True, compare(commands, previous_commands[1:]) + else: + return True, False - 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 = 0 and c_y = 0 and node[0] = 0 and node[1] 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 - +""" \ No newline at end of file diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index ce7824d09..62a56eb14 100644 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -6,8 +6,8 @@ import logging -from benchmarks.corl import CoRL -from benchmarks.agent import Agent +from carla.benchmarks.corl import CoRL +from carla.benchmarks.agent import Agent from carla.tcp import TCPConnectionError from carla.client import make_carla_client @@ -71,8 +71,8 @@ if(__name__ == '__main__'): while True: try: with make_carla_client(args.host, args.port) as client: - corl= CoRL(city_name= args.city_name,name_to_save='test') - agent = Manual(args.map_name) + corl= CoRL(city_name= args.city_name,name_to_save='test2') + agent = Manual(args.city_name) results = corl.benchmark_agent(agent,client) corl.plot_summary_test() corl.plot_summary_train() From 35b7229b90cfceb987225ba45b84ee0307e4ad65 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 13 Dec 2017 19:20:59 +0100 Subject: [PATCH 09/59] Adding the planner --- PythonClient/carla/benchmarks/agent.py | 10 +- PythonClient/carla/benchmarks/benchmark.py | 12 +- PythonClient/carla/planner/city_track.py | 74 ++++++-- PythonClient/carla/planner/graph.py | 206 ++++++++++----------- PythonClient/carla/planner/grid.py | 150 ++++++++------- PythonClient/carla/planner/map.py | 84 +++------ PythonClient/carla/planner/planner.py | 122 ++++-------- PythonClient/carla/planner/topology.py | 78 ++++++++ 8 files changed, 385 insertions(+), 351 deletions(-) create mode 100644 PythonClient/carla/planner/topology.py diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py index 3a03ea1cb..7f01401e0 100644 --- a/PythonClient/carla/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -20,7 +20,7 @@ from carla.carla_server_pb2 import Control -class Agent(object, ): +class Agent(object): def __init__(self,city_name, **kwargs): import os dir_path = os.path.dirname(__file__) @@ -31,10 +31,10 @@ class Agent(object, ): def get_distance(self,start_point,end_point): - _,path_distance=self._planner.get_next_command([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],(1,0,0)) + 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],(1,0,0)) # We calculate the timout based on the distance return path_distance diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index fe9214189..2d0e1021e 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -54,7 +54,6 @@ class Benchmark(object,): 'pos_y': -1 } - # If this name was run try to continue # Returns a experiment class that is build from a benchmark inherited # class @@ -98,14 +97,15 @@ class Benchmark(object,): curr_y = -1 prev_x = -1 prev_y = -1 + measurements, sensor_data = carla.read_data() carla.send_control(carla_protocol.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): @@ -128,8 +128,8 @@ class Benchmark(object,): t1 = measurements.game_timestamp - step += 1 - # The distance is based on graph but quite not exact. + + distance = sldist([curr_x, curr_y], [target.location.x, target.location.y]) diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index db37285be..7e649eb2e 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -1,8 +1,16 @@ - -class CityTrack(object) +import math - def __init__(self,city_name) +import numpy as np + +from carla.planner.astar import AStar +from carla.planner.map import CarlaMap + + +class CityTrack(object): + + + def __init__(self,city_name): self._map = CarlaMap(city_name) @@ -11,6 +19,10 @@ class CityTrack(object) # Refers to the start position of the previous route computation self._previous_node = [] + + # The current computed rout + + self._route =None @@ -25,7 +37,7 @@ class CityTrack(object) # To change the orientation with respect to the map standards node_orientation = np.array([node_orientation[0], node_orientation[1],node_orientation[2]]) - node_orientation = source_ori.dot(self.worldrotation) + node = tuple([ int(x) for x in node ]) @@ -39,11 +51,11 @@ class CityTrack(object) if math.fabs(node_orientation[0]) > math.fabs(node_orientation[1]): - node_orientation = (source_ori[0],0.0,0.0) + node_orientation = (node_orientation[0],0.0,0.0) else: - node_orientation = (0.0,source_ori[1],0.0) + node_orientation = (0.0,node_orientation[1],0.0) - node = self.map.grid_search(node[0],node[1]) + node = self._map._grid.search_on_grid(node[0],node[1]) return node @@ -60,8 +72,14 @@ class CityTrack(object) def is_far_away_from_route_intersection(self,current_node): # CHECK FOR THE EMPTY CASE + if self._route == None: + raise RuntimeError('Impossible to find route' + + ' Current planner is limited' + + ' Try to select start points away from interesections') + + return self._closest_intersection_route_position(current_node, - self._previous_route) > 4 + self._route) > 4 @@ -70,12 +88,18 @@ class CityTrack(object) self._previous_node = node_source + #print node_source + #print node_target + #print self._map.get_walls_directed(node_source,source_ori, + # node_target,target_ori) + print self._map.get_graph_resolution() a_star =AStar() - a_star.init_grid(node_source,self._map.get_graph_resolution()[0], + a_star.init_grid(self._map.get_graph_resolution()[0], self._map.get_graph_resolution()[1], - self._map.get_walls_directed(node_target,target_ori,node_source), - node_target) + self._map.get_walls_directed(node_source,source_ori, + node_target,target_ori),node_source, + node_target) route = a_star.solve() @@ -86,12 +110,14 @@ class CityTrack(object) # REALLY, I want to remove this if route == None: a_star =AStar() - a_star.init_grid(node_source,self._map.get_graph_resolution()[0], - self._map.get_graph_resolution()[1],self._map.get_walls(), node_target) + 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 @@ -107,15 +133,31 @@ class CityTrack(object) return sorted(distance_vector)[0] - def _closest_intersection_route_position(self, current_node): + def _closest_intersection_route_position(self, current_node,route): distance_vector = [] - for node_iterator in self._map._graph.intersection_nodes(): + for node_iter in route: + for node_iterator in self._map._graph.intersection_nodes(): - distance_vector.append(sldist(node_iterator, current_node)) + distance_vector.append(sldist(node_iterator, current_node)) return sorted(distance_vector)[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] diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index 3b7579358..b8229f737 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -1,161 +1,143 @@ import math import numpy as np -from matplotlib import collections as mc +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_node(string): + vec = string.split(',') + + return (int(vec[0]), int(vec[1])) + def string_to_floats(string): - vec = string.split(',') + vec = string.split(',') - return (float(vec[0]),float(vec[1]),float(vec[2])) + 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) + + + +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) +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 - """ + """ + A simple directed, weighted graph + """ + def __init__(self, graph_file=None): - def __init__(self,graph_file=None): + self._nodes = set() + self._angles = {} + self._edges = {} + self._distances = {} + self._node_density = 50 + if graph_file != None: + with open(graph_file, 'r') as file: + # Skipe the first four lines that + lines_after_4 = file.readlines()[4:] + # the graph resolution. + linegraphres = lines_after_4[0] + self._resolution = string_to_node(linegraphres) + for line in lines_after_4[1:]: - self.nodes = set() - self.angles ={} - self.edges = {} - self.distances = {} - self.node_density =50 - if graph_file != None: - with open(graph_file, 'r') as file: - for i in range(4): - next(file) + from_node, to_node, d = line.split() + from_node = string_to_node(from_node) + to_node = string_to_node(to_node) - # the graph resolution. - linegraphres = file.readline() - self._resolution = string_to_node(linegraphres) - for line in file: + 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) - from_node, to_node, d = line.split() - from_node = string_to_node(from_node) - to_node = string_to_node(to_node) + def add_node(self, value): + self._nodes.add(value) - 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 project_pixel(self,pixel) + def project_pixel(self, pixel): node = [] - node.append((pixel[0])/self.node_density - 2) - node.append((pixel[1])/self.node_density - 2) + node.append((pixel[0])/self._node_density - 2) + node.append((pixel[1])/self._node_density - 2) return tuple(node) - def make_orientations(self,node,heading): + 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 + 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 - distance_dic = collections.OrderedDict(sorted(distance_dic.items())) + start_to_goal = np.array([node[0] - v[0], node[1] - v[1]]) - self.angles[node ] = heading - for k, v in distance_dic.iteritems(): + print start_to_goal - #print k - #print v + self.angles[v] = start_to_goal / np.linalg.norm(start_to_goal) - start_to_goal = np.array([node[0] - v[0], node[1] - v[1]]) + def add_edge(self, from_node, to_node, distance): + self._add_edge(from_node, to_node, distance) - print start_to_goal + 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 - self.angles[v] = start_to_goal / np.linalg.norm(start_to_goal) + def intersection_nodes(self): + intersect_nodes = [] + for node in self._nodes: + if len(self._edges[node]) > 2: + intersect_nodes.append(node) - def add_edge(self, from_node, to_node, distance): - self._add_edge(from_node, to_node, distance) + return intersect_nodes + # This contains also the non-intersection turns... - 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 turn_nodes(self): - def intersection_nodes(self): + return self._nodes - intersect_nodes = [] - for node in self.nodes: - if len(self.edges[node]) > 2: - intersect_nodes.append(node) + def plot_ori(self, c): + line_len = 1 - return intersect_nodes + 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) - # This contains also the non-intersection turns... + ax.autoscale() + ax.margins(0.1) - 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) - - + 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) diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py index aecbe2d16..f3ba5e670 100644 --- a/PythonClient/carla/planner/grid.py +++ b/PythonClient/carla/planner/grid.py @@ -1,18 +1,85 @@ +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() + self._graph = graph + self._structure = self._make_structure() + self._walls = self._make_walls() #np.set_printoptions(linewidth=206, threshold=np.nan) + 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.resolution[0] and c_y >= 0 and c_y < self._graph.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_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): + #self.grid[adj[0], adj[1]] = 1.0 + + final_walls.add((adj[0], adj[1])) + #self.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): + #self.grid[adj[0], adj[1]] = 1.0 + + final_walls.add((adj[0], adj[1])) + #self.walls.add((adj[0], adj[1])) + + return final_walls + + + def _draw_line(self, grid, xi, yi, xf, yf): @@ -36,9 +103,9 @@ class Grid(object): return grid def _make_structure(self): - structure =np.ones((self.graph.resolution[0], self.graph.resolution[1])) + structure =np.ones((self._graph._resolution[0], self._graph._resolution[1])) - for key, connections in self.graph.edges.iteritems(): + for key, connections in self._graph._edges.iteritems(): # draw a line for con in connections: @@ -51,35 +118,14 @@ class Grid(object): def _make_walls(self): walls = set() - for i in range(self.grid.shape[0]): + for i in range(self._structure.shape[0]): - for j in range(self.grid.shape[1]): - if self.grid[i, j] == 1.0: + for j in range(self._structure.shape[1]): + if self._structure[i, j] == 1.0: walls.add((i, j)) return 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.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): + 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]] @@ -88,46 +134,10 @@ class Grid(object): 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 (node[0] >= 0 and node[0] < self._graph._resolution[0] + and node[1] >= 0 and node[1] < self._graph._resolution[1]): - if self.grid[node[0], node[1]] == 0.0: + if self._structure[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 \ No newline at end of file + return adjacent \ No newline at end of file diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index c8ba8247f..962b44c80 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -20,9 +20,14 @@ try: except ImportError: raise RuntimeError('cannot import PIL, make sure pillow package is installed') -from graph import string_to_node,string_to_floats +from carla.planner.graph import Graph +from carla.planner.grid import Grid +from carla.planner.graph import string_to_node,string_to_floats +def color_to_angle(color): + return ((float(color)/255.0)) * 2*math.pi + @@ -67,15 +72,15 @@ class CarlaMap(object): 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 + 'Center.png') + city_map_file_center = os.path.join(dir_path, city + 'Central.png') # The built graph. This is the exact same graph that unreal builds. This # is a generic structure used for many cases self._graph = Graph(city_file) - - self._grid = Grid() + + self._grid = Grid(self._graph) with open(city_file, 'r') as file: @@ -163,7 +168,6 @@ class CarlaMap(object): """Get the position on the map for a certain world position.""" pixel = self.get_position_on_map(world) return self._graph.project_pixel(pixel) - @@ -195,25 +199,12 @@ class CarlaMap(object): pixel.append(math.floor(relative_location[1] / float(self.pixel_density))) ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 2] - ori = ((float(ori) / 255.0)) * 2 * math.pi + ori = color_to_angle(ori) return (-math.cos(ori), -math.sin(ori)) - - - - - def - - - - - - - - def make_map_world(self, world): relative_location = [] @@ -286,42 +277,30 @@ class CarlaMap(object): 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 + ori = color_to_angle(ori) - # 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 make_node(self, worldvertex): - pixel = self.make_map_world(worldvertex) + 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. - node = [] + """ + print self._grid._structure + final_walls = self._grid.get_wall_source(node_source,source_ori,node_target) + print 'Returned final ',final_walls + final_walls = final_walls.union(self._grid.get_wall_target( + node_target,target_ori,node_source)) + return final_walls - node.append((pixel[0])/self.node_density - 2) - node.append((pixel[1])/self.node_density - 2) - - return tuple(node) -""" - def get_walls_directed(self,node_target,target_ori,node_source): - - # GOes to Grid - #added_walls = self.set_grid_direction(node_source,source_ori,node_target) - - - #print added_walls - # Goes to grid - #added_walls=added_walls.union(self.set_grid_direction_target(node_target,target_ori,node_source)) - #print added_walls def get_walls(self): - return walss - + return self._grid.walls @@ -334,19 +313,4 @@ class CarlaMap(object): 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] diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 9e835e187..8ea3ac9bd 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -5,12 +5,20 @@ import os from PIL import Image - -from graph import Graph -from astar import Astar +from city_track import CityTrack compare = lambda x, y: 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 + def angle_between(v1, v2): return np.arccos(np.dot(v1, v2) / np.linalg.norm(v1) / np.linalg.norm(v2)) @@ -18,9 +26,6 @@ def angle_between(v1, 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): @@ -58,7 +63,9 @@ class Planner(object): - #self.astar. + # Take the world position and project it on the road. + # The road is represented in a grid + track_source = self._city_track.project_node(source,source_ori) track_target = self._city_track.project_node(target,target_ori) @@ -66,67 +73,61 @@ class Planner(object): # reach the goal - if self._city_track.is_at_goal(track_source,track_target) - return 0,0 - #if node_source == node_target: - # return 0, 0 + 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): + if (self._city_track.is_at_new_node(track_source) + and self._city_track.is_away_from_intersection(track_source)): - # print route route= self._city_track.compute_route(track_source,source_ori, track_target,target_ori) - - #(node_source,source_ori,node_target,target_ori) - - - # 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 # This is bad + if route == 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 2.0 + return LANE_FOLLOW else: if self.commands: return self.commands[0] else: - return 2.0 + return LANE_FOLLOW else: if self._city_track.is_far_away_from_route_intersection(track_source): - return 2.0 + return LANE_FOLLOW # If there is computed commands if self.commands: return self.commands[0] else: - return 2.0 + return LANE_FOLLOW def get_shortest_path_distance(self, source, source_ori, target, target_ori): - import collections + distance = 0 - # if self.graph.intersection_nodes() == set(): - current_pos = pos + track_source = self._city_track.project_node(source,source_ori) + track_target = self._city_track.project_node(target,target_ori) + + current_pos = track_source + + route = self._city_track.compute_route(track_source,source_ori, + track_target,target_ori) + for node_iter in route: distance += sldist(node_iter, current_pos) current_pos = node_iter - return distance# *50.0*16.42 , maybe this goes to another layer + # We multiply by these values to convert distance to world coordinates + return distance *50.0*16.42 # , maybe this goes to another layer def is_there_posible_route(self,source,source_ori,target,target_ori): @@ -135,8 +136,8 @@ class Planner(object): track_target = self._city_track.project_node(target,target_ori) - return len(self._city_track.compute_route( - node_source,source_ori,node_target,target_ori))>0 + return not self._city_track.compute_route( + node_source,source_ori,node_target,target_ori) == None @@ -166,56 +167,13 @@ class Planner(object): command = 0.0 if angle < -0.1: - command = 4.0 + command = TURN_RIGHT elif angle > 0.1: - command = 3.0 + command = TURN_LEFT else: - command = 5.0 + command = GO_STRAIGHT commands_list.append(command) return commands_list -""" -# 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() - - - 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 - - -""" \ No newline at end of file diff --git a/PythonClient/carla/planner/topology.py b/PythonClient/carla/planner/topology.py new file mode 100644 index 000000000..6d21a5650 --- /dev/null +++ b/PythonClient/carla/planner/topology.py @@ -0,0 +1,78 @@ + + + # Projecting the nodes + 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]) + + def _route_compute(self,node_source,source_ori,node_target,target_ori): + + + # GOes to Grid + #added_walls = self.set_grid_direction(node_source,source_ori,node_target) + #print added_walls + # Goes to 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) + self.route = self.solve() + #print route # JuSt a Corner Case + if self.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) + self.route = self.solve() + + + for i in added_walls: + self.walls.remove(i) + + self.grid[i[0],i[1]] = 0.0 + + + + return self.route + + # This is to avoid computing a new route when inside the route + # distance_node = self.get_distance_closest_node(node_source) + # Planner shouldnt have knowledge about node + + #if (distance_node > 1 \ + # and self.previous_source != node_source) or self.complete_distance == 0: From b1f4db48052c1a95de922e5c9e4b5cebd5b55468 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 14 Dec 2017 00:05:34 +0100 Subject: [PATCH 10/59] printing --- PythonClient/carla/planner/grid.py | 4 ++-- PythonClient/carla/planner/map.py | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py index f3ba5e670..f5df1155e 100644 --- a/PythonClient/carla/planner/grid.py +++ b/PythonClient/carla/planner/grid.py @@ -44,9 +44,9 @@ class Grid(object): def get_wall_source(self, pos, pos_ori, target): free_nodes = self._get_adjacent_free_nodes(pos) - print self._walls + #print self._walls final_walls = copy.copy(self._walls) - print final_walls + #print final_walls heading_start = np.array([pos_ori[0], pos_ori[1]]) for adj in free_nodes: diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index 962b44c80..114eb57d7 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -290,9 +290,13 @@ class CarlaMap(object): an artificial wall. """ + + np.set_printoptions( linewidth =206,threshold=np.nan) + print self._grid._structure + final_walls = self._grid.get_wall_source(node_source,source_ori,node_target) - print 'Returned final ',final_walls + #print 'Returned final ',final_walls final_walls = final_walls.union(self._grid.get_wall_target( node_target,target_ori,node_source)) return final_walls @@ -300,6 +304,7 @@ class CarlaMap(object): def get_walls(self): + return self._grid.walls From 71aa3be5777aa0b544f611004a1d580234c7b24b Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 14 Dec 2017 00:24:54 +0100 Subject: [PATCH 11/59] Some more debugging on map --- PythonClient/carla/planner/city_track.py | 5 +++-- PythonClient/carla/planner/map.py | 8 ++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index 7e649eb2e..aba351c52 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -97,8 +97,9 @@ class CityTrack(object): 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, + self._map.get_walls(),node_source, + #self._map.get_walls_directed(node_source,source_ori, + # node_target,target_ori),node_source, node_target) diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index 114eb57d7..678dc0a1e 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -291,9 +291,9 @@ class CarlaMap(object): """ - np.set_printoptions( linewidth =206,threshold=np.nan) - - print self._grid._structure + #np.set_printoptions( linewidth =206,threshold=np.nan) + + #print self._grid._structure final_walls = self._grid.get_wall_source(node_source,source_ori,node_target) #print 'Returned final ',final_walls @@ -305,7 +305,7 @@ class CarlaMap(object): def get_walls(self): - return self._grid.walls + return self._grid._walls From f31eaa09987a16b1d17eee467383f37d60bfaa6f Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 14 Dec 2017 12:03:50 +0100 Subject: [PATCH 12/59] Planner full redefinition. Now tested on many positons --- PythonClient/carla/benchmarks/agent.py | 4 +-- PythonClient/carla/planner/astar.py | 4 +++ PythonClient/carla/planner/city_track.py | 18 ++++++---- PythonClient/carla/planner/graph.py | 6 ---- PythonClient/carla/planner/grid.py | 2 +- PythonClient/carla/planner/map.py | 34 +++--------------- PythonClient/carla/planner/planner.py | 20 +++++++---- PythonClient/carla/planner/test_planner.py | 41 ++++++++++++++++++++++ 8 files changed, 77 insertions(+), 52 deletions(-) create mode 100644 PythonClient/carla/planner/test_planner.py diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py index 7f01401e0..cb32c185e 100644 --- a/PythonClient/carla/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -9,7 +9,6 @@ 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 @@ -34,7 +33,8 @@ class Agent(object): 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],(1,0,0)) + ,[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 diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py index 26d1c371e..e9b3d4b70 100644 --- a/PythonClient/carla/planner/astar.py +++ b/PythonClient/carla/planner/astar.py @@ -52,6 +52,7 @@ class AStar(object): """ self.grid_height = height self.grid_width = width + #print walls for x in range(self.grid_width): for y in range(self.grid_height): if (x, y) in walls: @@ -59,6 +60,7 @@ class AStar(object): else: reachable = True self.cells.append(Cell(x, y, reachable)) + self.start = self.get_cell(*start) self.end = self.get_cell(*end) @@ -126,6 +128,8 @@ class AStar(object): @returns path or None if not found. """ + if self.start == self.end: + return None # add starting cell to open heap queue heapq.heappush(self.opened, (self.start.f, self.start)) while len(self.opened): diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index aba351c52..f1adbde20 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -3,6 +3,8 @@ import math import numpy as np +from carla.planner.graph import sldist + from carla.planner.astar import AStar from carla.planner.map import CarlaMap @@ -29,14 +31,16 @@ class CityTrack(object): - def project_node(self,node,node_orientation): + def project_node(self,position,node_orientation): """ Projecting the graph node into the city road """ + node =self._map.get_position_on_graph(position) + # To change the orientation with respect to the map standards node_orientation = np.array([node_orientation[0], - node_orientation[1],node_orientation[2]]) + node_orientation[1]]) node = tuple([ int(x) for x in node ]) @@ -92,14 +96,14 @@ class CityTrack(object): #print node_target #print self._map.get_walls_directed(node_source,source_ori, # node_target,target_ori) - print self._map.get_graph_resolution() - + #print self._map.get_graph_resolution() + #print self._map.get_walls() + #print 's ',node_source,'e ',node_target 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, - #self._map.get_walls_directed(node_source,source_ori, - # node_target,target_ori),node_source, + self._map.get_walls_directed(node_source,source_ori, + node_target,target_ori),node_source, node_target) diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index b8229f737..de22b1d43 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -15,12 +15,6 @@ def string_to_floats(string): return (float(vec[0]), float(vec[1]), float(vec[2])) - - - -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( diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py index f5df1155e..12bf8c602 100644 --- a/PythonClient/carla/planner/grid.py +++ b/PythonClient/carla/planner/grid.py @@ -28,7 +28,7 @@ class Grid(object): 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.resolution[0] and c_y >= 0 and c_y < self._graph.resolution[1]: + if c_x >= 0 and c_x < self._graph._resolution[0] and c_y >= 0 and c_y < self._graph._resolution[1]: if self._structure[c_x, c_y] == 0: break else: diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index 678dc0a1e..e71af686e 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -227,10 +227,12 @@ class CarlaMap(object): 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) + pixel.append((node[0] + 2) * self._graph._node_density) + pixel.append((node[1] + 2) * self._graph._node_density) return pixel @@ -254,34 +256,6 @@ class CarlaMap(object): - 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 = color_to_angle(ori) - - - return (-math.cos(ori), -math.sin(ori)) - def get_walls_directed(self,node_source,source_ori,node_target,target_ori): """ diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 8ea3ac9bd..05608cdfc 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -2,9 +2,11 @@ import math import time import collections import os +import numpy as np from PIL import Image + from city_track import CityTrack compare = lambda x, y: collections.Counter(x) == collections.Counter(y) @@ -20,11 +22,14 @@ 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)) sldist = lambda c1, c2: 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): @@ -82,7 +87,7 @@ class Planner(object): - route= self._city_track.compute_route(track_source,source_ori, + route = self._city_track.compute_route(track_source,source_ori, track_target,target_ori) if route == None: raise RuntimeError('Impossible to find route') @@ -94,8 +99,8 @@ class Planner(object): if self._city_track.is_far_away_from_route_intersection(track_source): return LANE_FOLLOW else: - if self.commands: - return self.commands[0] + if self._commands: + return self._commands[0] else: return LANE_FOLLOW else: @@ -104,8 +109,8 @@ class Planner(object): return LANE_FOLLOW # If there is computed commands - if self.commands: - return self.commands[0] + if self._commands: + return self._commands[0] else: return LANE_FOLLOW @@ -120,6 +125,9 @@ class Planner(object): route = self._city_track.compute_route(track_source,source_ori, track_target,target_ori) + # No Route, distance is zero + if route == None: + return 0.0 for node_iter in route: @@ -152,7 +160,7 @@ class Planner(object): commands_list = [] for i in range(0, len(route)): - if route[i] not in self.astar.intersection_nodes(): + if route[i] not in self._city_track._map._graph.intersection_nodes(): continue current = route[i] diff --git a/PythonClient/carla/planner/test_planner.py b/PythonClient/carla/planner/test_planner.py new file mode 100644 index 000000000..667d350e0 --- /dev/null +++ b/PythonClient/carla/planner/test_planner.py @@ -0,0 +1,41 @@ +from carla import sensor +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 + + + + +planner= Planner('Town01') + + + + +resolution = planner._city_track._map.get_graph_resolution() + +for i in range(resolution[0]): + for j in range(resolution[1]): + + world_source = planner._city_track._map.make_world_node((i,j)) + source_ori = planner._city_track._map.get_lane_orientation(world_source) + print ' Making Route from (',i, ',',j,') o (',source_ori[0],',',source_ori[1],')' + + for k in range(resolution[0]): + for l in range(resolution[1]): + world_target = planner._city_track._map.make_world_node((k,l)) + + target_ori = planner._city_track._map.get_lane_orientation(world_target) + print ' To (',k, ',',l,') o (',target_ori[0],',',target_ori[1],')' + + path_distance=planner.get_shortest_path_distance( + world_source,source_ori,world_target,target_ori) + + print 'Distance is ',path_distance + command=planner.get_next_command( + world_source,source_ori,world_target,target_ori) + print 'Command is ',command + print 'Latest Route ',planner._city_track._route \ No newline at end of file From a371b6347c6a31b633eb9cfda65fdbf0321c64ba Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 14 Dec 2017 16:50:01 +0100 Subject: [PATCH 13/59] Fixed some wrong positions on corl benchmark --- PythonClient/carla/benchmarks/corl.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/PythonClient/carla/benchmarks/corl.py b/PythonClient/carla/benchmarks/corl.py index b9ac67bff..379400cdd 100644 --- a/PythonClient/carla/benchmarks/corl.py +++ b/PythonClient/carla/benchmarks/corl.py @@ -88,20 +88,21 @@ class CoRL(Benchmark): def _poses_town01(self): """ - For each matrix is a new task + Each matrix is a new task """ 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],[34,84],[51,67],[22,17],[91,148],\ + [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],[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],\ + 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]] From d2a5a717679c8bbfa83ab9354c23494a7fda4919 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 14 Dec 2017 17:38:08 +0100 Subject: [PATCH 14/59] Planner --- PythonClient/carla/benchmarks/corl.py | 4 ++-- PythonClient/carla/planner/planner.py | 15 ++++++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/PythonClient/carla/benchmarks/corl.py b/PythonClient/carla/benchmarks/corl.py index 379400cdd..102959a96 100644 --- a/PythonClient/carla/benchmarks/corl.py +++ b/PythonClient/carla/benchmarks/corl.py @@ -125,7 +125,7 @@ class CoRL(Benchmark): def _poses_straight(): - return [[38, 34], [4, 2], [12, 10], [62, 55], [43, 47],\ + 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],\ @@ -135,7 +135,7 @@ class CoRL(Benchmark): 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] ,\ + [75,59],[50,47],[11,19],[77,34],[79,25],\ [40,63],[58,76],[79,55],[16,61],[27,11]] def _poses_navigation(): diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 05608cdfc..e8dd83094 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -40,11 +40,7 @@ class Planner(object): self._city_track = CityTrack(city_name) - #self.previous_source = (0, 0) - ##self.distance = 0 - #self.complete_distance = 0 - # print self.map_image self._commands = [] #self.route = [] @@ -135,8 +131,10 @@ class Planner(object): current_pos = node_iter # We multiply by these values to convert distance to world coordinates - return distance *50.0*16.42 # , maybe this goes to another layer + return distance *self._city_track._map.pixel_density \ + *self._city_track._map._graph._node_density + def is_there_posible_route(self,source,source_ori,target,target_ori): @@ -147,6 +145,13 @@ class Planner(object): return not self._city_track.compute_route( node_source,source_ori,node_target,target_ori) == None + def test_position(self,source,source_ori): + + + node_source = self._city_track.project_node(source,source_ori) + + return self.is_away_from_intersection(node_source) + # from the shortest path graph, transform it into a list of commands From f4f238f248f1af41b66cff70d25c606ace5f5938 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 15 Dec 2017 15:06:17 +0100 Subject: [PATCH 15/59] Benchmark small changes --- PythonClient/carla/benchmarks/benchmark.py | 30 ++++++++++++++++----- PythonClient/carla/benchmarks/experiment.py | 1 - PythonClient/run_benchmark.py | 2 +- 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 2d0e1021e..88d0c1435 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -22,7 +22,7 @@ sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) class Benchmark(object,): # Param @name to be used for saving purposes - def __init__(self, city_name, name_to_save, continue_exp=False): + def __init__(self, city_name, name_to_save, continue_exp=False,save_images=False): # The name of the city that is going to be used. self._city_name = city_name # Sends a base name, the rest will be saved with respect to what the @@ -91,7 +91,14 @@ class Benchmark(object,): now.strftime("%Y%m%d%H%M")), 'wb') as f: pass - def run_navigation_episode(self, agent, carla, time_out, target): + + 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): curr_x = -1 curr_y = -1 @@ -108,6 +115,7 @@ class Benchmark(object,): distance = 100000 measurement_vec = [] + frame = 0 while((t1-t0) < (time_out*1000) and not success): capture_time = time.time() measurements, sensor_data = carla.read_data() @@ -118,6 +126,11 @@ class Benchmark(object,): carla.send_control(control) # meassure 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)) + prev_x = curr_x prev_y = curr_y @@ -139,6 +152,8 @@ class Benchmark(object,): if(distance < 200.0): success = True + frame +=1 + if(success): return (1, measurement_vec, float(t1-t0)/1000.0, distance) else: @@ -192,7 +207,10 @@ class Benchmark(object,): # running the agent (result, reward_vec, final_time, remaining_distance) = \ self.run_navigation_episode( - agent, carla, time_out, positions[end_point]) + 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 @@ -200,7 +218,7 @@ class Benchmark(object,): experiment, pose, rep, path_distance, remaining_distance, final_time, time_out, result) - self._write_reward_results(experiment, rep, reward_vec) + self._write_details_results(experiment, rep, reward_vec) if(result > 0): print('+++++ Target achieved in %f seconds! +++++' % @@ -232,10 +250,10 @@ class Benchmark(object,): w.writerow(self._dict_stats) - def _write_reward_results(self, experiment, rep, reward_vec): + def _write_details_results(self, experiment, rep, reward_vec): with open(os.path.join(self._full_name, - 'rewards_' + self._suffix_name), 'a+') as rfd: + 'details_' + self._suffix_name), 'a+') as rfd: rw = csv.DictWriter(rfd, self._dict_rewards.keys()) diff --git a/PythonClient/carla/benchmarks/experiment.py b/PythonClient/carla/benchmarks/experiment.py index 7c13c50b5..7ec9496c1 100644 --- a/PythonClient/carla/benchmarks/experiment.py +++ b/PythonClient/carla/benchmarks/experiment.py @@ -11,7 +11,6 @@ class Experiment(object): self.Poses = [[]] self.Repetitions = 1 - # self. ,vehicles,pedestrians,weather,cameras def set(self, **kwargs): for key, value in kwargs.items(): diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 62a56eb14..8f600b9f6 100644 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -71,7 +71,7 @@ if(__name__ == '__main__'): while True: try: with make_carla_client(args.host, args.port) as client: - corl= CoRL(city_name= args.city_name,name_to_save='test2') + corl= CoRL(city_name= args.city_name,name_to_save='details') agent = Manual(args.city_name) results = corl.benchmark_agent(agent,client) corl.plot_summary_test() From a7dacc22b8571c50309aef33c4928345ca19f65c Mon Sep 17 00:00:00 2001 From: nsubiron Date: Tue, 19 Dec 2017 12:24:22 +0100 Subject: [PATCH 16/59] Make run_benchmark executable and some clean up --- PythonClient/run_benchmark.py | 41 +++++++++++++++-------------------- 1 file changed, 17 insertions(+), 24 deletions(-) mode change 100644 => 100755 PythonClient/run_benchmark.py diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py old mode 100644 new mode 100755 index 8f600b9f6..193320d33 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -1,26 +1,26 @@ -import time -import sys +#!/usr/bin/env python3 + +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB), and the INTEL Visual Computing Lab. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . import argparse import logging +import sys +import time - - -from carla.benchmarks.corl import CoRL from carla.benchmarks.agent import Agent +from carla.benchmarks.corl import CoRL +from carla.client import make_carla_client, VehicleControl from carla.tcp import TCPConnectionError -from carla.client import make_carla_client -try: - from carla import carla_server_pb2 as carla_protocol -except ImportError: - raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') - class Manual(Agent): - def run_step(self,measurements,sensor_data,target): - control = carla_protocol.Control() + def run_step(self, measurements, sensor_data, target): + control = VehicleControl() control.steer = 0.0 control.throttle = 0.9 control.brake = 0.0 @@ -30,8 +30,7 @@ class Manual(Agent): return control -if(__name__ == '__main__'): - +if __name__ == '__main__': argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( @@ -55,9 +54,7 @@ if(__name__ == '__main__'): metavar='C', default='Town01', help='plot the map of the current city' - +'(needs to match active map in server, options: Town01 or Town02)') - - + + '(needs to match active map in server, options: Town01 or Town02)') args = argparser.parse_args() @@ -66,14 +63,12 @@ if(__name__ == '__main__'): 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= CoRL(city_name= args.city_name,name_to_save='details') + corl = CoRL(city_name=args.city_name, name_to_save='details') agent = Manual(args.city_name) - results = corl.benchmark_agent(agent,client) + results = corl.benchmark_agent(agent, client) corl.plot_summary_test() corl.plot_summary_train() @@ -85,5 +80,3 @@ if(__name__ == '__main__'): except Exception as exception: logging.exception(exception) sys.exit(1) - - From fa34e359d013c79708376fd9b79d0723ae9c1086 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Tue, 19 Dec 2017 15:31:28 +0100 Subject: [PATCH 17/59] Some fixes for Python 3 --- .gitignore | 1 + PythonClient/carla/benchmarks/benchmark.py | 111 +++++++++---------- PythonClient/carla/planner/graph.py | 29 ++--- PythonClient/carla/planner/grid.py | 50 ++++----- PythonClient/carla/planner/planner.py | 120 +++++++++------------ 5 files changed, 140 insertions(+), 171 deletions(-) diff --git a/.gitignore b/.gitignore index fa06fd814..2aef97e3e 100644 --- a/.gitignore +++ b/.gitignore @@ -22,5 +22,6 @@ Util/Build .tags* .vs __pycache__ +_benchmarks_results _images core diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 88d0c1435..92dd021e4 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -1,28 +1,27 @@ +from builtins import input -import os -import math -import time import csv -import json - import datetime +import math +import os +import time + +from carla.client import VehicleControl -try: - from carla import carla_server_pb2 as carla_protocol -except ImportError: - raise RuntimeError( - 'cannot import "carla_server_pb2.py' - +', run the protobuf compiler to generate this file') +def sldist(c1, c2): + return math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) -sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) - - -class Benchmark(object,): +class Benchmark(object): # Param @name to be used for saving purposes - def __init__(self, city_name, name_to_save, continue_exp=False,save_images=False): + def __init__( + self, + city_name, + name_to_save, + continue_exp=False, + save_images=False): # The name of the city that is going to be used. self._city_name = city_name # Sends a base name, the rest will be saved with respect to what the @@ -54,15 +53,14 @@ class Benchmark(object,): 'pos_y': -1 } - # Returns a experiment class that is build from a benchmark inherited # class self._experiments = self._build_experiments() self._suffix_name = self._get_experiments_names(self._experiments) self._full_name = os.path.join('_benchmarks_results', - self._base_name + '_' - + self._get_details() + '/') + self._base_name + '_' + + self._get_details() + '/') if self._experiment_exist(): # If continue_exp was set we continue directly, else we ask for @@ -70,8 +68,8 @@ class Benchmark(object,): self._line_on_file = self._get_last_position() else: - answer = str(raw_input("The experiment was already found in the files" - + ", Do you want to Continue ? \n")) + answer = input("The experiment was already found in the files" + + ", Do you want to Continue ? \n") if answer == 'Yes' or answer == 'y': self._line_on_file = self._get_last_position() else: @@ -88,36 +86,37 @@ class Benchmark(object,): # the number of times the experiments were run now = datetime.datetime.now() with open(os.path.join(self._full_name, - now.strftime("%Y%m%d%H%M")), 'wb') as f: + now.strftime("%Y%m%d%H%M")), 'w') as f: pass - self._save_images = save_images - self._image_filename_format = os.path.join(self._full_name, - '_images/episode_{:s}/{:s}/image_{:0>5d}.jpg') + 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): + def run_navigation_episode( + self, + agent, + carla, + time_out, + target, + episode_name): curr_x = -1 curr_y = -1 prev_x = -1 prev_y = -1 - + measurements, sensor_data = carla.read_data() - carla.send_control(carla_protocol.Control()) - + carla.send_control(VehicleControl()) + t0 = measurements.game_timestamp t1 = t0 success = False - distance = 100000 measurement_vec = [] frame = 0 - while((t1-t0) < (time_out*1000) and not success): - capture_time = time.time() + while((t1 - t0) < (time_out * 1000) and not success): measurements, sensor_data = carla.read_data() control = agent.run_step(measurements, sensor_data, target) @@ -131,7 +130,6 @@ class Benchmark(object,): image.save_to_disk(self._image_filename_format.format( episode_name, name, frame)) - prev_x = curr_x prev_y = curr_y curr_x = measurements.player_measurements.transform.location.x @@ -141,47 +139,43 @@ class Benchmark(object,): t1 = measurements.game_timestamp - - distance = sldist([curr_x, curr_y], [target.location.x, target.location.y]) - 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)) + 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 - frame +=1 + frame += 1 - if(success): - return (1, measurement_vec, float(t1-t0)/1000.0, distance) - else: - return (0, measurement_vec, time_out, distance) + 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), 'wb') as ofd: + self._suffix_name), 'w') as ofd: w = csv.DictWriter(ofd, self._dict_stats.keys()) w.writeheader() with open(os.path.join(self._full_name, - 'rewards_' + self._suffix_name), 'wb') as rfd: + 'rewards_' + self._suffix_name), 'w') as rfd: rw = csv.DictWriter(rfd, self._dict_rewards.keys()) rw.writeheader() start_task = 0 - start_pose = 0 + start_pose = 0 else: - (start_task,start_pose) = self._get_pose_and_task(self._line_on_file) - - print (' START ') - + (start_task, start_pose) = self._get_pose_and_task(self._line_on_file) + print(' START ') for experiment in self._experiments[start_task:]: @@ -209,13 +203,13 @@ class Benchmark(object,): self.run_navigation_episode( agent, carla, time_out, positions[end_point], str(experiment.Conditions.WeatherId) + '_' - + str(experiment.id) +'_'+ str(start_point) - + '.' + str(end_point)) + + str(experiment.id) + '_' + str(start_point) + + '.' + str(end_point)) # compute stats for the experiment self._write_summary_results( - experiment, pose, rep, path_distance, + experiment, pose, rep, path_distance, remaining_distance, final_time, time_out, result) self._write_details_results(experiment, rep, reward_vec) @@ -227,11 +221,9 @@ class Benchmark(object,): print('----- Timeout! -----') return self.get_all_statistics() - - def _write_summary_results(self, experiment, pose, rep, - path_distance, remaining_distance, - final_time, time_out, result): + path_distance, remaining_distance, + final_time, time_out, result): self._dict_stats['exp_id'] = experiment.id self._dict_stats['rep'] = rep @@ -278,8 +270,6 @@ class Benchmark(object,): rw.writerow(self._dict_rewards) - - # To be redefined on subclasses on how to calculate timeout for an episode def _calculate_time_out(self, distance): return 0 @@ -311,4 +301,3 @@ class Benchmark(object,): for i, l in enumerate(f): pass return i - diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index de22b1d43..00e1dc47f 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -1,24 +1,27 @@ import math -import numpy as np -from matplotlib import collections as mc import matplotlib.pyplot as plt +import numpy as np + +from matplotlib import collections as mc 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])) -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) +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): @@ -33,7 +36,7 @@ class Graph(object): self._edges = {} self._distances = {} self._node_density = 50 - if graph_file != None: + if graph_file is not None: with open(graph_file, 'r') as file: # Skipe the first four lines that lines_after_4 = file.readlines()[4:] @@ -61,8 +64,8 @@ class Graph(object): def project_pixel(self, pixel): node = [] - node.append((pixel[0])/self._node_density - 2) - node.append((pixel[1])/self._node_density - 2) + node.append((pixel[0]) / self._node_density - 2) + node.append((pixel[1]) / self._node_density - 2) return tuple(node) @@ -85,7 +88,7 @@ class Graph(object): start_to_goal = np.array([node[0] - v[0], node[1] - v[1]]) - print start_to_goal + print(start_to_goal) self.angles[v] = start_to_goal / np.linalg.norm(start_to_goal) @@ -115,8 +118,8 @@ class Graph(object): def plot_ori(self, c): 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] + 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) diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py index 12bf8c602..12e0377e5 100644 --- a/PythonClient/carla/planner/grid.py +++ b/PythonClient/carla/planner/grid.py @@ -1,14 +1,14 @@ - 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): + def __init__(self, graph): self._graph = graph self._structure = self._make_structure() @@ -16,9 +16,6 @@ class Grid(object): #np.set_printoptions(linewidth=206, threshold=np.nan) - - - def search_on_grid(self, x, y): visit = [[0, 1], [0, -1], [1, 0], [1, 1], [1, -1], [-1, 0], [-1, 1], [-1, -1]] @@ -26,9 +23,10 @@ class Grid(object): 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 + c_x, c_y = x + offset[0] * scale, y + offset[1] * scale - if c_x >= 0 and c_x < self._graph._resolution[0] and c_y >= 0 and c_y < self._graph._resolution[1]: + if c_x >= 0 and c_x < self._graph._resolution[ + 0] and c_y >= 0 and c_y < self._graph._resolution[1]: if self._structure[c_x, c_y] == 0: break else: @@ -37,20 +35,16 @@ class Grid(object): return (c_x, c_y) - - - - def get_wall_source(self, pos, pos_ori, target): free_nodes = self._get_adjacent_free_nodes(pos) - #print self._walls + # print self._walls final_walls = copy.copy(self._walls) - #print final_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]]) + 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 @@ -67,7 +61,7 @@ class Grid(object): 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]]) + 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): @@ -78,10 +72,6 @@ class Grid(object): return final_walls - - - - def _draw_line(self, grid, xi, yi, xf, yf): if xf < xi: @@ -94,24 +84,27 @@ class Grid(object): yi = yf yf = aux - for i in range(xi, xf+1): + for i in range(xi, xf + 1): - for j in range(yi, yf+1): + for j in range(yi, yf + 1): grid[i, j] = 0.0 return grid def _make_structure(self): - structure =np.ones((self._graph._resolution[0], self._graph._resolution[1])) + structure = np.ones( + (self._graph._resolution[0], + self._graph._resolution[1])) - for key, connections in self._graph._edges.iteritems(): + for key, connections in self._graph._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]) + structure = self._draw_line( + structure, key[0], key[1], con[0], con[1]) # print grid return structure @@ -125,6 +118,7 @@ class Grid(object): walls.add((i, j)) return walls + def _get_adjacent_free_nodes(self, pos): """ Acht nodes in total """ visit = [[0, 1], [0, -1], [1, 0], [1, 1], @@ -132,12 +126,12 @@ class Grid(object): adjacent = set() for offset in visit: - node = (pos[0] + offset[0], pos[1]+offset[1]) + node = (pos[0] + offset[0], pos[1] + offset[1]) - if (node[0] >= 0 and node[0] < self._graph._resolution[0] - and node[1] >= 0 and node[1] < self._graph._resolution[1]): + if (node[0] >= 0 and node[0] < self._graph._resolution[0] + and node[1] >= 0 and node[1] < self._graph._resolution[1]): if self._structure[node[0], node[1]] == 0.0: adjacent.add(node) - return adjacent \ No newline at end of file + return adjacent diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index e8dd83094..a1bef0cc7 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -1,17 +1,19 @@ -import math -import time import collections -import os +import math import numpy as np - +import os +import time from PIL import Image -from city_track import CityTrack +from . import city_track -compare = lambda x, y: collections.Counter(x) == collections.Counter(y) -""" +def compare(x, y): + return collections.Counter(x) == collections.Counter(y) + + +""" Constants Used for the high level commands """ @@ -19,14 +21,16 @@ REACH_GOAL = 0.0 GO_STRAIGHT = 5.0 TURN_RIGHT = 4.0 TURN_LEFT = 3.0 -LANE_FOLLOW =2.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)) -sldist = lambda c1, c2: math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) + +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) @@ -34,12 +38,9 @@ def signal(v1, v2): class Planner(object): - def __init__(self, city_name): - - self._city_track = CityTrack(city_name) - + self._city_track = city_track.CityTrack(city_name) self._commands = [] @@ -52,47 +53,31 @@ class Planner(object): #self.node_density = 50.0 # This function converts the 2d map into a 3D one in a vector. - - - - - - - - - def get_next_command(self, source, source_ori, target, target_ori): - - + def get_next_command(self, source, source_ori, target, target_ori): # Take the world position and project it on the road. # The road is represented in a grid - track_source = self._city_track.project_node(source,source_ori) - track_target = self._city_track.project_node(target,target_ori) - - + track_source = self._city_track.project_node(source, source_ori) + track_target = self._city_track.project_node(target, target_ori) # reach the goal - if self._city_track.is_at_goal(track_source,track_target): + 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)): + 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 == None: + 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): + if self._city_track.is_far_away_from_route_intersection( + track_source): return LANE_FOLLOW else: if self._commands: @@ -101,7 +86,8 @@ class Planner(object): return LANE_FOLLOW else: - if self._city_track.is_far_away_from_route_intersection(track_source): + if self._city_track.is_far_away_from_route_intersection( + track_source): return LANE_FOLLOW # If there is computed commands @@ -110,19 +96,23 @@ class Planner(object): else: return LANE_FOLLOW - - def get_shortest_path_distance(self, source, source_ori, target, target_ori): + def get_shortest_path_distance( + self, + source, + source_ori, + target, + target_ori): distance = 0 - track_source = self._city_track.project_node(source,source_ori) - track_target = self._city_track.project_node(target,target_ori) + track_source = self._city_track.project_node(source, source_ori) + track_target = self._city_track.project_node(target, target_ori) current_pos = track_source - route = self._city_track.compute_route(track_source,source_ori, - track_target,target_ori) + route = self._city_track.compute_route(track_source, source_ori, + track_target, target_ori) # No Route, distance is zero - if route == None: + if route is None: return 0.0 for node_iter in route: @@ -131,35 +121,28 @@ class Planner(object): current_pos = node_iter # We multiply by these values to convert distance to world coordinates - return distance *self._city_track._map.pixel_density \ - *self._city_track._map._graph._node_density + return distance * self._city_track._map.pixel_density \ + * self._city_track._map._graph._node_density - + def is_there_posible_route(self, source, source_ori, target, target_ori): - def is_there_posible_route(self,source,source_ori,target,target_ori): + track_source = self._city_track.project_node(source, source_ori) + track_target = self._city_track.project_node(target, target_ori) - track_source = self._city_track.project_node(source,source_ori) - track_target = self._city_track.project_node(target,target_ori) + return not self._city_track.compute_route( + node_source, source_ori, node_target, target_ori) is None + def test_position(self, source, source_ori): - return not self._city_track.compute_route( - node_source,source_ori,node_target,target_ori) == None - - def test_position(self,source,source_ori): - - - node_source = self._city_track.project_node(source,source_ori) - - return self.is_away_from_intersection(node_source) - + node_source = self._city_track.project_node(source, source_ori) + return self.is_away_from_intersection(node_source) # 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 _route_to_commands(self, route): commands_list = [] @@ -169,13 +152,13 @@ class Planner(object): continue current = route[i] - past = route[i-1] - future = route[i+1] + past = route[i - 1] + future = route[i + 1] past_to_current = np.array( - [current[0]-past[0], current[1]-past[1]]) + [current[0] - past[0], current[1] - past[1]]) current_to_future = np.array( - [future[0]-current[0], future[1]-current[1]]) + [future[0] - current[0], future[1] - current[1]]) angle = signal(current_to_future, past_to_current) command = 0.0 @@ -189,4 +172,3 @@ class Planner(object): commands_list.append(command) return commands_list - From 463757c21464a9dd22492f73dd39f6ca22a5ca65 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 20 Dec 2017 10:52:31 -0200 Subject: [PATCH 18/59] Cleaning classes, adding abstract class. --- PythonClient/carla/benchmarks/agent.py | 65 ++---- PythonClient/carla/benchmarks/benchmark.py | 187 +++++++++++------- .../benchmarks/{corl.py => corl_2017.py} | 70 ++++--- PythonClient/carla/benchmarks/experiment.py | 1 - PythonClient/carla/benchmarks/metrics.py | 5 +- PythonClient/carla/planner/astar.py | 1 - PythonClient/carla/planner/bezier.py | 37 ---- PythonClient/carla/planner/city_track.py | 20 +- PythonClient/carla/planner/graph.py | 2 - PythonClient/carla/planner/grid.py | 9 +- PythonClient/carla/planner/map.py | 54 +---- PythonClient/carla/planner/planner.py | 42 ++-- PythonClient/carla/planner/test_planner.py | 41 ---- PythonClient/carla/planner/topology.py | 78 -------- PythonClient/run_benchmark.py | 18 +- 15 files changed, 223 insertions(+), 407 deletions(-) rename PythonClient/carla/benchmarks/{corl.py => corl_2017.py} (77%) delete mode 100644 PythonClient/carla/planner/bezier.py delete mode 100644 PythonClient/carla/planner/test_planner.py delete mode 100644 PythonClient/carla/planner/topology.py diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py index cb32c185e..63f4e7b77 100644 --- a/PythonClient/carla/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -1,33 +1,23 @@ #!/usr/bin/env python2 # -*- coding: utf-8 -*- -""" -Created on Sat Jun 10 19:12:37 2017 +# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB), and the INTEL Visual Computing Lab. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +# @author: german,felipecode + -@author: german,felipecode -""" from __future__ import print_function -import time -import math -from carla import sensor -from carla.sensor import Camera -from carla.settings import CarlaSettings -from carla.tcp import TCPConnectionError -from carla.util import print_over_same_line +import abc + from carla.planner.planner import Planner -from carla.carla_server_pb2 import Control - - - class Agent(object): def __init__(self,city_name, **kwargs): - import os - dir_path = os.path.dirname(__file__) - + __metaclass__ = abc.ABCMeta self._planner = Planner(city_name) - - def get_distance(self,start_point,end_point): path_distance=self._planner.get_shortest_path_distance( @@ -39,33 +29,12 @@ class Agent(object): return path_distance - - # Function to be redefined by the AI. - def run_step(self, data): - pass + @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 + """ - -""" - 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 -""" diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 92dd021e4..a895aa125 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -1,3 +1,12 @@ +#!/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 . + + from builtins import input import csv @@ -5,6 +14,8 @@ import datetime import math import os import time +import abc + from carla.client import VehicleControl @@ -15,17 +26,22 @@ def sldist(c1, c2): class Benchmark(object): - # Param @name to be used for saving purposes + """ + 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_exp=False, + continue_experiment=False, save_images=False): - # The name of the city that is going to be used. + + __metaclass__ = abc.ABCMeta + self._city_name = city_name - # Sends a base name, the rest will be saved with respect to what the - # episode was about + self._base_name = name_to_save self._dict_stats = {'exp_id': -1, 'rep': -1, @@ -37,8 +53,6 @@ class Benchmark(object): 'final_distance': -1, 'final_time': -1, 'time_out': -1 - - } self._dict_rewards = {'exp_id': -1, @@ -53,41 +67,12 @@ class Benchmark(object): 'pos_y': -1 } - # Returns a experiment class that is build from a benchmark inherited - # class + # Get the line for the experiment to be continued + self._line_on_file = self._continue_experiment(continue_experiment) self._experiments = self._build_experiments() - self._suffix_name = self._get_experiments_names(self._experiments) - self._full_name = os.path.join('_benchmarks_results', - self._base_name + '_' - + self._get_details() + '/') - - if self._experiment_exist(): - # If continue_exp was set we continue directly, else we ask for - if continue_exp: - self._line_on_file = self._get_last_position() - - else: - answer = input("The experiment was already found in the files" - + ", Do you want to Continue ? \n") - if answer == 'Yes' or answer == 'y': - self._line_on_file = self._get_last_position() - else: - self._line_on_file = 0 - - else: - self._line_on_file = 0 - - folder = os.path.dirname(self._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() - with open(os.path.join(self._full_name, - now.strftime("%Y%m%d%H%M")), 'w') as f: - pass + # Create the log files and get the names + self._suffix_name, self._full_name = self._create_log_record(self._experiments) self._save_images = save_images self._image_filename_format = os.path.join( @@ -101,22 +86,17 @@ class Benchmark(object): target, episode_name): - curr_x = -1 - curr_y = -1 - prev_x = -1 - prev_y = -1 - measurements, sensor_data = carla.read_data() carla.send_control(VehicleControl()) t0 = measurements.game_timestamp t1 = t0 success = False - - distance = 100000 measurement_vec = [] frame = 0 - while((t1 - t0) < (time_out * 1000) and not success): + 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) @@ -124,14 +104,12 @@ class Benchmark(object): control.throttle, 'Brake ', control.brake) carla.send_control(control) - # meassure distance to target + # 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)) - prev_x = curr_x - prev_y = curr_y curr_x = measurements.player_measurements.transform.location.x curr_y = measurements.player_measurements.transform.location.y @@ -146,14 +124,14 @@ class Benchmark(object): '[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): + 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) + return 1, measurement_vec, float(t1 - t0) / 1000.0, distance + return 0, measurement_vec, time_out, distance def benchmark_agent(self, agent, carla): @@ -166,7 +144,7 @@ class Benchmark(object): w.writeheader() with open(os.path.join(self._full_name, - 'rewards_' + self._suffix_name), 'w') as rfd: + 'details_' + self._suffix_name), 'w') as rfd: rw = csv.DictWriter(rfd, self._dict_rewards.keys()) rw.writeheader() @@ -270,23 +248,52 @@ class Benchmark(object): rw.writerow(self._dict_rewards) - # To be redefined on subclasses on how to calculate timeout for an episode - def _calculate_time_out(self, distance): - return 0 + def _create_log_record(self, base_name, experiments): + """ + This function creates the log files for the benchmark. - # To be redefined on subclasses - def _build_experiments(self): - pass + """ + suffix_name = self._get_experiments_names(experiments) + full_name = os.path.join('_benchmarks_results', + base_name + '_' + + self._get_details() + '/') - def _get_experiments_names(self, experiments): + folder = os.path.dirname(full_name) + if not os.path.isdir(folder): + os.makedirs(folder) - name_cat = 'w' + # Make a date file: to show when this was modified, + # the number of times the experiments were run + now = datetime.datetime.now() + with open(os.path.join(full_name, + now.strftime("%Y%m%d%H%M")), 'w') as f: + pass - for experiment in experiments: + 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("The experiment was already found in the files" + + ",Do you want to continue (y/n) ? \n" + + " If not the experiment will be overwritten") + 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 - name_cat += str(experiment.Conditions.WeatherId) + '.' - return name_cat def _experiment_exist(self): @@ -301,3 +308,51 @@ class Benchmark(object): for i, l in enumerate(f): pass return i + + # 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 _build_experiments(self): + """ + Returns a set of experiments to be evaluated + Must be redefined in an inherited class. + + """ + pass + + @abc.abstractmethod + def _get_pose_and_task(self): + """ + Parse the experiment depending on number of poses and tasks + """ + pass + + @abc.abstractmethod + def plot_summary_train(self): + """ + returns the summary for the train weather/task episodes + + """ + + @abc.abstractmethod + def plot_summary_test(self): + """ + returns the summary for the test weather/task episodes + + """ + @staticmethod + def get_experiments_names(experiments): + + name_cat = 'w' + + for experiment in experiments: + + name_cat += str(experiment.Conditions.WeatherId) + '.' + + return name_cat + + diff --git a/PythonClient/carla/benchmarks/corl.py b/PythonClient/carla/benchmarks/corl_2017.py similarity index 77% rename from PythonClient/carla/benchmarks/corl.py rename to PythonClient/carla/benchmarks/corl_2017.py index 102959a96..274ea21e9 100644 --- a/PythonClient/carla/benchmarks/corl.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -21,18 +21,17 @@ from .metrics import compute_summary -class CoRL(Benchmark): +class CoRL2017(Benchmark): def get_all_statistics(self): summary = compute_summary(os.path.join( - self._full_name, self._suffix_name),[3]) + self._full_name, self._suffix_name),[3]) return summary - def plot_summary_train(self): @@ -88,29 +87,29 @@ class CoRL(Benchmark): def _poses_town01(self): """ - Each matrix is a new task + 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],\ + 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],\ + 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],\ + 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]] @@ -125,25 +124,25 @@ class CoRL(Benchmark): 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],\ + 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],\ + 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],\ + 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(), @@ -213,20 +212,19 @@ class CoRL(Benchmark): experiment = Experiment() experiment.set( - Conditions=conditions, - Poses= poses, - Id = iteration, - Repetitions = 1 - ) + Conditions=conditions, + Poses=poses, + Id=iteration, + Repetitions=1 + ) experiments_vector.append(experiment) return experiments_vector def _get_details(self): - now = datetime.datetime.now() # Function to get automatic information from the experiment for writing purposes - return 'corl_' + self._city_name + return 'corl2017_' + self._city_name def _get_pose_and_task(self,line_on_file): diff --git a/PythonClient/carla/benchmarks/experiment.py b/PythonClient/carla/benchmarks/experiment.py index 7ec9496c1..4340d5599 100644 --- a/PythonClient/carla/benchmarks/experiment.py +++ b/PythonClient/carla/benchmarks/experiment.py @@ -11,7 +11,6 @@ class Experiment(object): self.Poses = [[]] self.Repetitions = 1 - def set(self, **kwargs): for key, value in kwargs.items(): if not hasattr(self, key): diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py index 46e22e316..d55064b23 100644 --- a/PythonClient/carla/benchmarks/metrics.py +++ b/PythonClient/carla/benchmarks/metrics.py @@ -8,10 +8,8 @@ import numpy as np import math import os -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] @@ -182,8 +180,7 @@ def compute_summary(file, dynamic_episodes): 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] + metrics_dictionary['intersection_offroad'][ diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py index e9b3d4b70..7ebc90c10 100644 --- a/PythonClient/carla/planner/astar.py +++ b/PythonClient/carla/planner/astar.py @@ -1,6 +1,5 @@ import heapq -from carla.planner.map import CarlaMap class Cell(object): diff --git a/PythonClient/carla/planner/bezier.py b/PythonClient/carla/planner/bezier.py deleted file mode 100644 index 04e46d150..000000000 --- a/PythonClient/carla/planner/bezier.py +++ /dev/null @@ -1,37 +0,0 @@ -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 diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index f1adbde20..949a5b19e 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -22,15 +22,12 @@ class CityTrack(object): # Refers to the start position of the previous route computation self._previous_node = [] - # The current computed rout - + # The current computed route self._route =None - - def project_node(self,position,node_orientation): """ Projecting the graph node into the city road @@ -92,13 +89,7 @@ class CityTrack(object): self._previous_node = node_source - #print node_source - #print node_target - #print self._map.get_walls_directed(node_source,source_ori, - # node_target,target_ori) - #print self._map.get_graph_resolution() - #print self._map.get_walls() - #print 's ',node_source,'e ',node_target + a_star =AStar() a_star.init_grid(self._map.get_graph_resolution()[0], self._map.get_graph_resolution()[1], @@ -111,8 +102,8 @@ class CityTrack(object): - #print route # JuSt a Corner Case - # REALLY, I want to remove this + # JuSt a Corner Case + # TODO: Clean this to avoid having to use this function if route == None: a_star =AStar() a_star.init_grid(self._map.get_graph_resolution()[0], @@ -127,8 +118,6 @@ class CityTrack(object): return route - - def _closest_intersection_position(self, current_node): distance_vector = [] @@ -153,7 +142,6 @@ class CityTrack(object): def get_distance_closest_node_route(self, pos, route): import collections distance = [] - # if self.graph.intersection_nodes() == set(): for node_iter in route: diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index 00e1dc47f..216fc4fa4 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -83,8 +83,6 @@ class Graph(object): 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]]) diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py index 12e0377e5..01d9eb9f1 100644 --- a/PythonClient/carla/planner/grid.py +++ b/PythonClient/carla/planner/grid.py @@ -14,7 +14,6 @@ class Grid(object): self._structure = self._make_structure() self._walls = self._make_walls() - #np.set_printoptions(linewidth=206, threshold=np.nan) def search_on_grid(self, x, y): visit = [[0, 1], [0, -1], [1, 0], [1, 1], @@ -47,10 +46,9 @@ class Grid(object): 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 final_walls.add((adj[0], adj[1])) - #self.walls.add((adj[0], adj[1])) + return final_walls @@ -65,10 +63,9 @@ class Grid(object): angle = angle_between(heading_start, start_to_goal) if (angle < 1.0 and adj != source): - #self.grid[adj[0], adj[1]] = 1.0 final_walls.add((adj[0], adj[1])) - #self.walls.add((adj[0], adj[1])) + return final_walls @@ -120,7 +117,7 @@ class Grid(object): return walls def _get_adjacent_free_nodes(self, pos): - """ Acht nodes in total """ + """ Eight nodes in total """ visit = [[0, 1], [0, -1], [1, 0], [1, 1], [1, -1], [-1, 0], [-1, 1], [-1, -1]] diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index e71af686e..d43479057 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -22,7 +22,7 @@ except ImportError: from carla.planner.graph import Graph from carla.planner.grid import Grid -from carla.planner.graph import string_to_node,string_to_floats +from carla.planner.graph import string_to_node,string_to_floats def color_to_angle(color): @@ -30,41 +30,6 @@ def color_to_angle(color): - -""" - - with open(city_file + '.txt', '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) - -""" - -# The map contains a graph - - class CarlaMap(object): def __init__(self, city): dir_path = os.path.dirname(__file__) @@ -230,9 +195,9 @@ class CarlaMap(object): def make_map_node(self, node): - pixel = [] - pixel.append((node[0] + 2) * self._graph._node_density) - pixel.append((node[1] + 2) * self._graph._node_density) + + pixel = [((node[0] + 2) * self._graph._node_density) + , ((node[1] + 2) * self._graph._node_density)] return pixel @@ -265,26 +230,21 @@ class CarlaMap(object): """ - #np.set_printoptions( linewidth =206,threshold=np.nan) - - #print self._grid._structure - final_walls = self._grid.get_wall_source(node_source,source_ori,node_target) - #print 'Returned final ',final_walls + final_walls = final_walls.union(self._grid.get_wall_target( - node_target,target_ori,node_source)) + node_target, target_ori, node_source)) return final_walls def get_walls(self): - return self._grid._walls def get_distance_closest_node(self, pos): - import collections + distance = [] for node_iter in self.graph.intersection_nodes(): diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index a1bef0cc7..b07f95f50 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -1,10 +1,9 @@ import collections import math -import numpy as np -import os -import time -from PIL import Image + +import numpy as np + from . import city_track @@ -44,19 +43,17 @@ class Planner(object): 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 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) + """ - # Take the world position and project it on the road. - # The road is represented in a grid track_source = self._city_track.project_node(source, source_ori) track_target = self._city_track.project_node(target, target_ori) @@ -130,7 +127,7 @@ class Planner(object): track_target = self._city_track.project_node(target, target_ori) return not self._city_track.compute_route( - node_source, source_ori, node_target, target_ori) is None + track_source, source_ori, track_target, target_ori) is None def test_position(self, source, source_ori): @@ -138,13 +135,18 @@ class Planner(object): return self.is_away_from_intersection(node_source) - # 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 _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)): @@ -161,7 +163,7 @@ class Planner(object): [future[0] - current[0], future[1] - current[1]]) angle = signal(current_to_future, past_to_current) - command = 0.0 + if angle < -0.1: command = TURN_RIGHT elif angle > 0.1: diff --git a/PythonClient/carla/planner/test_planner.py b/PythonClient/carla/planner/test_planner.py deleted file mode 100644 index 667d350e0..000000000 --- a/PythonClient/carla/planner/test_planner.py +++ /dev/null @@ -1,41 +0,0 @@ -from carla import sensor -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 - - - - -planner= Planner('Town01') - - - - -resolution = planner._city_track._map.get_graph_resolution() - -for i in range(resolution[0]): - for j in range(resolution[1]): - - world_source = planner._city_track._map.make_world_node((i,j)) - source_ori = planner._city_track._map.get_lane_orientation(world_source) - print ' Making Route from (',i, ',',j,') o (',source_ori[0],',',source_ori[1],')' - - for k in range(resolution[0]): - for l in range(resolution[1]): - world_target = planner._city_track._map.make_world_node((k,l)) - - target_ori = planner._city_track._map.get_lane_orientation(world_target) - print ' To (',k, ',',l,') o (',target_ori[0],',',target_ori[1],')' - - path_distance=planner.get_shortest_path_distance( - world_source,source_ori,world_target,target_ori) - - print 'Distance is ',path_distance - command=planner.get_next_command( - world_source,source_ori,world_target,target_ori) - print 'Command is ',command - print 'Latest Route ',planner._city_track._route \ No newline at end of file diff --git a/PythonClient/carla/planner/topology.py b/PythonClient/carla/planner/topology.py deleted file mode 100644 index 6d21a5650..000000000 --- a/PythonClient/carla/planner/topology.py +++ /dev/null @@ -1,78 +0,0 @@ - - - # Projecting the nodes - 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]) - - def _route_compute(self,node_source,source_ori,node_target,target_ori): - - - # GOes to Grid - #added_walls = self.set_grid_direction(node_source,source_ori,node_target) - #print added_walls - # Goes to 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) - self.route = self.solve() - #print route # JuSt a Corner Case - if self.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) - self.route = self.solve() - - - for i in added_walls: - self.walls.remove(i) - - self.grid[i[0],i[1]] = 0.0 - - - - return self.route - - # This is to avoid computing a new route when inside the route - # distance_node = self.get_distance_closest_node(node_source) - # Planner shouldnt have knowledge about node - - #if (distance_node > 1 \ - # and self.previous_source != node_source) or self.complete_distance == 0: diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 193320d33..81d65ae53 100755 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -12,13 +12,17 @@ import sys import time from carla.benchmarks.agent import Agent -from carla.benchmarks.corl import CoRL +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.steer = 0.0 @@ -53,8 +57,14 @@ if __name__ == '__main__': '-c', '--city-name', metavar='C', default='Town01', - help='plot the map of the current city' - + '(needs to match active map in server, options: Town01 or Town02)') + 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 scripts' + ) args = argparser.parse_args() @@ -66,7 +76,7 @@ if __name__ == '__main__': while True: try: with make_carla_client(args.host, args.port) as client: - corl = CoRL(city_name=args.city_name, name_to_save='details') + 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() From 209f169ff2ae8c82979b31b6ba983056ffd41ba8 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 20 Dec 2017 11:33:57 -0200 Subject: [PATCH 19/59] Testing a different import --- PythonClient/carla/client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/carla/client.py b/PythonClient/carla/client.py index 80fde992b..8a535d8ae 100644 --- a/PythonClient/carla/client.py +++ b/PythonClient/carla/client.py @@ -16,7 +16,7 @@ from . import tcp from . import util try: - from . import carla_server_pb2 as carla_protocol + from carla.carla_server_pb2 import carla_server_pb2 as carla_protocol except ImportError: raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') From b7cd3438373f67c37d30e10a644885d64255df40 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 20 Dec 2017 11:36:29 -0200 Subject: [PATCH 20/59] Testing a different import --- PythonClient/carla/client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/carla/client.py b/PythonClient/carla/client.py index 8a535d8ae..80fde992b 100644 --- a/PythonClient/carla/client.py +++ b/PythonClient/carla/client.py @@ -16,7 +16,7 @@ from . import tcp from . import util try: - from carla.carla_server_pb2 import carla_server_pb2 as carla_protocol + from . import carla_server_pb2 as carla_protocol except ImportError: raise RuntimeError('cannot import "carla_server_pb2.py", run the protobuf compiler to generate this file') From 02fbb41bdab04e9c545ab4f2e42819ccf09bbe5a Mon Sep 17 00:00:00 2001 From: nsubiron Date: Wed, 20 Dec 2017 18:43:22 +0100 Subject: [PATCH 21/59] Remove trailing whitespace --- PythonClient/carla/benchmarks/corl_2017.py | 18 +++++++++--------- PythonClient/carla/benchmarks/metrics.py | 6 +++--- PythonClient/carla/client.py | 2 +- PythonClient/carla/planner/city_track.py | 8 ++++---- PythonClient/carla/planner/map.py | 2 +- PythonClient/manual_control.py | 2 +- 6 files changed, 19 insertions(+), 19 deletions(-) diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index 274ea21e9..24a483bc4 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -29,7 +29,7 @@ class CoRL2017(Benchmark): summary = compute_summary(os.path.join( self._full_name, self._suffix_name),[3]) - return summary + return summary def plot_summary_train(self): @@ -44,13 +44,13 @@ class CoRL2017(Benchmark): def _plot_summary(self,weathers): - """ - We plot the summary of the testing for the set selected 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]) @@ -61,7 +61,7 @@ class CoRL2017(Benchmark): for weather,tasks in values.items(): if weather in set(weathers): print(' Weather: ',weather) - count =0 + count =0 for t in tasks: print(' Task ',count,' -> ',t) @@ -153,7 +153,7 @@ class CoRL2017(Benchmark): def _build_experiments(self): - """ + """ Creates the whole set of experiment objects, The experiments created depend on the selected Town. """ @@ -208,7 +208,7 @@ class CoRL2017(Benchmark): ) # Add all the cameras that were set for this experiments - conditions.add_sensor(camera) + conditions.add_sensor(camera) experiment = Experiment() experiment.set( @@ -221,7 +221,7 @@ class CoRL2017(Benchmark): return experiments_vector - def _get_details(self): + def _get_details(self): # Function to get automatic information from the experiment for writing purposes return 'corl2017_' + self._city_name @@ -232,6 +232,6 @@ class CoRL2017(Benchmark): 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 + # We assume that the number of poses is constant return line_on_file/len(self._experiments),line_on_file%25 diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py index d55064b23..3f6ced110 100644 --- a/PythonClient/carla/benchmarks/metrics.py +++ b/PythonClient/carla/benchmarks/metrics.py @@ -61,7 +61,7 @@ def get_distance_traveled(selected_matrix, header): # 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 @@ -152,7 +152,7 @@ def compute_summary(file, dynamic_episodes): 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_rewards.index( 'exp_id')] == float(t), reward_matrix[:, header_rewards.index('weather')] == float(w))] @@ -182,7 +182,7 @@ def compute_summary(file, dynamic_episodes): colisions = get_colisions(task_reward_matrix, header_rewards) - + metrics_dictionary['intersection_offroad'][ w][t] = lane_road[0]/km_run metrics_dictionary['intersection_otherlane'][ diff --git a/PythonClient/carla/client.py b/PythonClient/carla/client.py index 80fde992b..748200928 100644 --- a/PythonClient/carla/client.py +++ b/PythonClient/carla/client.py @@ -126,7 +126,7 @@ class CarlaClient(object): def send_control(self, *args, **kwargs): """ Send the VehicleControl to be applied this frame. - + If synchronous mode was requested, the server will pause the simulation until this message is received. """ diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index 949a5b19e..c28913626 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -24,8 +24,8 @@ class CityTrack(object): # The current computed route self._route =None - - + + def project_node(self,position,node_orientation): @@ -94,7 +94,7 @@ class CityTrack(object): 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,target_ori),node_source, node_target) @@ -112,7 +112,7 @@ class CityTrack(object): route = a_star.solve() - + self._route = route return route diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index d43479057..f099a391b 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -223,7 +223,7 @@ class CarlaMap(object): 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. diff --git a/PythonClient/manual_control.py b/PythonClient/manual_control.py index 6933747ce..f869cfd46 100755 --- a/PythonClient/manual_control.py +++ b/PythonClient/manual_control.py @@ -301,7 +301,7 @@ class CarlaGame(object): agent.vehicle.transform.location.y, agent.vehicle.transform.location.z]) w_pos = int(agent_position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0]))) - h_pos =int(agent_position[1] *(new_window_width/float(self._map_shape[1]))) + h_pos =int(agent_position[1] *(new_window_width/float(self._map_shape[1]))) pygame.draw.circle(surface, [255, 0, 255, 255], (w_pos,h_pos), 4, 0) self._display.blit(surface, (WINDOW_WIDTH, 0)) From 92f14af8e9f1b8dc238b03bd12660ba52c45523c Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 21 Dec 2017 18:58:24 -0200 Subject: [PATCH 22/59] Cleaning more, creating the converter --- PythonClient/carla/benchmarks/benchmark.py | 4 +- PythonClient/carla/planner/city_track.py | 14 +- PythonClient/carla/planner/converter.py | 165 +++++++++++++++++++++ PythonClient/carla/planner/graph.py | 12 +- PythonClient/carla/planner/map.py | 150 ++++--------------- PythonClient/carla/planner/planner.py | 5 +- 6 files changed, 213 insertions(+), 137 deletions(-) create mode 100644 PythonClient/carla/planner/converter.py diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index a895aa125..27998271a 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -13,10 +13,12 @@ import csv import datetime import math import os -import time + import abc + + from carla.client import VehicleControl diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index 949a5b19e..25fc8e598 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -8,14 +8,15 @@ 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._map = CarlaMap(city_name, self._pixel_density, self._node_density) self._astar = AStar() @@ -24,7 +25,8 @@ class CityTrack(object): # The current computed route self._route =None - + + @@ -33,7 +35,7 @@ class CityTrack(object): Projecting the graph node into the city road """ - node =self._map.get_position_on_graph(position) + node =self._map.convert_to_node(position) # To change the orientation with respect to the map standards node_orientation = np.array([node_orientation[0], @@ -52,9 +54,9 @@ class CityTrack(object): if math.fabs(node_orientation[0]) > math.fabs(node_orientation[1]): - node_orientation = (node_orientation[0],0.0,0.0) + node_orientation = (node_orientation[0], 0.0, 0.0) else: - node_orientation = (0.0,node_orientation[1],0.0) + node_orientation = (0.0, node_orientation[1], 0.0) node = self._map._grid.search_on_grid(node[0],node[1]) diff --git a/PythonClient/carla/planner/converter.py b/PythonClient/carla/planner/converter.py new file mode 100644 index 000000000..dce2ba71f --- /dev/null +++ b/PythonClient/carla/planner/converter.py @@ -0,0 +1,165 @@ +import math +import numpy as np + +from carla.planner.graph import string_to_node,string_to_floats + + +# Constant definition enumeration + +PIXEL = 0 +WORLD = 1 +NODE = 2 + + +class Converter(object): + + def __init__(self, city_file, node_density, pixel_density): + + self._node_density = node_density + self._pixel_density = pixel_density + with open(city_file, 'r') as f: + + # The offset of the world from the zero coordinates ( The + # coordinate we consider zero) + self._worldoffset = string_to_floats(f.readline()) + + angles = string_to_floats(f.readline()) + + # If there is an rotation between the world and map coordinates. + self._worldrotation = np.array([ + [math.cos(math.radians(angles[2])), -math.sin(math.radians(angles[2])), 0.0], + [math.sin(math.radians(angles[2])), math.cos(math.radians(angles[2])), 0.0], + [0.0, 0.0, 1.0]]) + + # Ignore for now, these are offsets for map coordinates and scale + # (not used). + _ = f.readline() + + # The offset of the map zero coordinate. + self._mapoffset = string_to_floats(f.readline()) + + def convert_to_node(self,input_data): + """ + Receives a data type (Can Be Pixel or World ) + :param input_data: position in some coordinate + :return: A vector representing a node + """ + + + input_type = self._check_input_type(input_data) + if input_type == PIXEL: + return self._pixel_to_node(input_data) + elif input_type == WORLD: + return self._world_to_node(input_data) + else: + raise ValueError('Invalid node to be converted') + + + + def convert_to_pixel(self,input_data): + + """ + Receives a data type (Can Be Node or World ) + :param input_data: position in some coordinate + :return: A vector with pixel coordinates + """ + + input_type = self._check_input_type(input_data) + if input_type == NODE: + return self._node_to_pixel(input_data) + elif input_type == WORLD: + return self._world_to_pixel(input_data) + else: + raise ValueError('Invalid node to be converted') + + + def convert_to_world(self,input_data): + + """ + Receives a data type (Can Be Pixel or Node ) + :param input_data: position in some coordinate + :return: vector with world coordinates + """ + + input_type = self._check_input_type(input_data) + if input_type == NODE: + return self._node_to_world(input_data) + elif input_type == PIXEL: + return self._pixel_to_world(input_data) + else: + raise ValueError('Invalid node to be converted') + + def _node_to_pixel(self, node): + """ + Conversion from node format (graph) to pixel (image) + :param node: + :return: pixel + """ + pixel = [((node[0] + 2) * self._node_density) + , ((node[1] + 2) * self._node_density)] + return pixel + + def _pixel_to_node(self, pixel): + """ + Conversion from pixel format (image) to node (graph) + :param node: + :return: pixel + """ + node = [int(((pixel[0]) / self._node_density) - 2) + , int(((pixel[1]) / self._node_density) - 2)] + + return tuple(node) + def _pixel_to_world(self,pixel): + """ + Conversion from pixel format (image) to world (3D) + :param pixel: + :return: world + """ + + + relative_location = [ pixel[0] * self._pixel_density, + pixel[1] * self._pixel_density] + + world = [ + relative_location[0] + self._mapoffset[0] - self._worldoffset[0], + relative_location[1] + self._mapoffset[1] - self._worldoffset[1], + 22 + ] + + return world + + def _world_to_pixel(self, world): + """ + Conversion from world format (3D) to pixel + :param world: + :return: pixel + """ + + rotation = np.array([world[0], world[1], world[2]]) + rotation = rotation.dot(self._worldrotation) + + relative_location = [rotation[0] + self._worldoffset[0] - self._mapoffset[0], + rotation[1] + self._worldoffset[1] - self._mapoffset[1], + rotation[2] + self._worldoffset[2] - self._mapoffset[2]] + + pixel = [math.floor(relative_location[0] / float(self._pixel_density)), + math.floor(relative_location[1] / float(self._pixel_density))] + + return pixel + + def _world_to_node(self, world): + return self._pixel_to_node(self._world_to_pixel(world)) + + def _node_to_world(self, node): + + return self._pixel_to_world(self._node_to_pixel(node)) + + def _check_input_type(self, input_data): + if len(input_data) > 2: + return WORLD + elif type(input_data[0]) is int: + return NODE + else: + return PIXEL + + diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index 216fc4fa4..1d740561f 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -29,13 +29,14 @@ class Graph(object): A simple directed, weighted graph """ - def __init__(self, graph_file=None): + def __init__(self, graph_file=None,node_density=50): self._nodes = set() self._angles = {} self._edges = {} self._distances = {} - self._node_density = 50 + self._node_density = node_density + if graph_file is not None: with open(graph_file, 'r') as file: # Skipe the first four lines that @@ -62,13 +63,6 @@ class Graph(object): def add_node(self, value): self._nodes.add(value) - def project_pixel(self, pixel): - node = [] - node.append((pixel[0]) / self._node_density - 2) - node.append((pixel[1]) / self._node_density - 2) - - return tuple(node) - def make_orientations(self, node, heading): import collections diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index d43479057..d35f698b2 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -22,16 +22,15 @@ except ImportError: from carla.planner.graph import Graph from carla.planner.grid import Grid -from carla.planner.graph import string_to_node,string_to_floats +from carla.planner.converter import Converter def color_to_angle(color): return ((float(color)/255.0)) * 2*math.pi - - class CarlaMap(object): - def __init__(self, city): + + def __init__(self, city, pixel_density, node_density): dir_path = os.path.dirname(__file__) city_file = os.path.join(dir_path, city + '.txt') @@ -39,41 +38,18 @@ class CarlaMap(object): city_map_file_lanes = os.path.join(dir_path, city + 'Lanes.png') city_map_file_center = os.path.join(dir_path, city + 'Central.png') - # The built graph. This is the exact same graph that unreal builds. This # is a generic structure used for many cases - self._graph = Graph(city_file) - self._grid = Grid(self._graph) - - 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) - - lineworldangles = file.readline() - self.angles = string_to_floats(lineworldangles) - - # If tere is an rotation between the world and map coordinates. - 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). - map_scale = file.readline() - linemapoffset = file.readline() - - # The offset of the map zero coordinate. - self.mapoffset = string_to_floats(linemapoffset) - - + self._pixel_density = pixel_density + self._grid = Grid(self._graph, node_density) # The number of game units per pixel. For now this is fixed. - self.pixel_density = 16.43 + + self._converter = Converter(city_file, pixel_density, node_density) + + + # Load the lanes image self.map_image_lanes = Image.open(city_map_file_lanes) self.map_image_lanes.load() @@ -112,42 +88,7 @@ class CarlaMap(object): return np.fliplr(np.asarray(img, dtype="int32")) return np.fliplr(self.map_image_lanes) - 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_graph(self, world): - """Get the position on the map for a certain world position.""" - pixel = self.get_position_on_map(world) - return self._graph.project_pixel(pixel) - - - - def get_position_on_world(self, pixel): - """Get world position of a certain map position.""" - relative_location = [] - world_vertex = [] - relative_location.append(pixel[0] * self.pixel_density) - relative_location.append(pixel[1] * self.pixel_density) - - world_vertex.append(relative_location[0] + self.mapoffset[0] - self.worldoffset[0]) - world_vertex.append(relative_location[1] + self.mapoffset[1] - self.worldoffset[1]) - world_vertex.append(22) # Z does not matter for now. - - return world_vertex def get_lane_orientation(self, world): """Get the lane orientation of a certain world position.""" @@ -167,58 +108,29 @@ class CarlaMap(object): ori = color_to_angle(ori) return (-math.cos(ori), -math.sin(ori)) + def convert_to_node(self,input): + """ + Receives a data type (Can Be Pixel or World ) + :param input: position in some coordinate + :return: A node object + """ + return self._converter.convert_to_node(input) + def convert_to_pixel(self,input): + """ + Receives a data type (Can Be Pixel or World ) + :param input: position in some coordinate + :return: A node object + """ + return self._converter.convert_to_pixel(input) - - 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 = [((node[0] + 2) * self._graph._node_density) - , ((node[1] + 2) * self._graph._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 convert_to_world(self,input): + """ + Receives a data type (Can Be Pixel or World ) + :param input: position in some coordinate + :return: A node object + """ + return self._converter.convert_to_world(input) diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index b07f95f50..0fd7125b3 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -118,8 +118,9 @@ class Planner(object): current_pos = node_iter # We multiply by these values to convert distance to world coordinates - return distance * self._city_track._map.pixel_density \ - * self._city_track._map._graph._node_density + return distance * self._city_track._map.get_pixel_density() \ + * self._city_track.get_node_density() + def is_there_posible_route(self, source, source_ori, target, target_ori): From 7b7375e3e2b7aa5b437f1ef2d10348fcb57c3fb4 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 21 Dec 2017 20:21:54 -0200 Subject: [PATCH 23/59] Not anymore --- Docs/carla_benchmark.md | 26 ++++++++++++++++++++++++++ PythonClient/carla/planner/graph.py | 8 ++++---- PythonClient/carla/planner/planner.py | 1 - 3 files changed, 30 insertions(+), 5 deletions(-) create mode 100644 Docs/carla_benchmark.md diff --git a/Docs/carla_benchmark.md b/Docs/carla_benchmark.md new file mode 100644 index 000000000..5bdfdd420 --- /dev/null +++ b/Docs/carla_benchmark.md @@ -0,0 +1,26 @@ +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. + +The basic functionality requires only the protobuf module to be installed + $ sudo apt-get install python3 python3-pip + $ sudo pip3 install protobuf +However, other operations as handling images require some extra modules, and the +"manual_control.py" example requires pygame + $ sudo pip3 install numpy Pillow pygame +The script "PythonClient/client_example.py" provides basic functionality for +controlling the vehicle and saving images to disk. Run the help command to see +options available + $ ./client_example.py --help +The script "PythonClient/manual_control.py" launches a PyGame window with +several views and allows to control the vehicle using the WASD keys. + $ ./manual_control.py --help + + diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index 1d740561f..f35d73aae 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -1,9 +1,6 @@ import math -import matplotlib.pyplot as plt import numpy as np -from matplotlib import collections as mc - def string_to_node(string): vec = string.split(',') @@ -108,6 +105,9 @@ class Graph(object): 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], @@ -125,7 +125,7 @@ class Graph(object): 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] diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 0fd7125b3..164eb17e1 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -54,7 +54,6 @@ class Planner(object): :return: a command ( Straight,Lane Follow, Left or Right) """ - track_source = self._city_track.project_node(source, source_ori) track_target = self._city_track.project_node(target, target_ori) From 3329471b1173f46cb2919129595fe58f45fb9fa6 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 21 Dec 2017 21:11:12 -0200 Subject: [PATCH 24/59] Updated the docs for benchmark --- Docs/carla_benchmark.md | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/Docs/carla_benchmark.md b/Docs/carla_benchmark.md index 5bdfdd420..2228401fa 100644 --- a/Docs/carla_benchmark.md +++ b/Docs/carla_benchmark.md @@ -9,18 +9,15 @@ 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. -The basic functionality requires only the protobuf module to be installed - $ sudo apt-get install python3 python3-pip - $ sudo pip3 install protobuf -However, other operations as handling images require some extra modules, and the -"manual_control.py" example requires pygame - $ sudo pip3 install numpy Pillow pygame -The script "PythonClient/client_example.py" provides basic functionality for -controlling the vehicle and saving images to disk. Run the help command to see -options available - $ ./client_example.py --help -The script "PythonClient/manual_control.py" launches a PyGame window with -several views and allows to control the vehicle using the WASD keys. - $ ./manual_control.py --help +! Currently not tested on python 3 + +Besides the requirements of the CARLA client, +the benchmark package also needs the future package + $ sudo pip install future + +Run the help command to see options available + $ ./run_benchmark --help + + From a1ba42c70e1f8c1e05bacf58484d79b53cb8a53d Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 21 Dec 2017 21:15:44 -0200 Subject: [PATCH 25/59] added carla benchmark to the index --- Docs/index.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Docs/index.md b/Docs/index.md index 1666c0670..1eb92cb1a 100644 --- a/Docs/index.md +++ b/Docs/index.md @@ -4,6 +4,7 @@ CARLA Documentation #### Using CARLA * [How to run CARLA server and client](how_to_run.md) + * [Benchmark](carla_benchmark.md) * [CARLA settings](carla_settings.md) * [Measurements](measurements.md) * [Cameras and sensors](cameras_and_sensors.md) From c997732e0b9d7a84333de28b63af8166def66b41 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 21 Dec 2017 21:19:57 -0200 Subject: [PATCH 26/59] A bit more documentation on the benchmark --- Docs/carla_benchmark.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Docs/carla_benchmark.md b/Docs/carla_benchmark.md index 2228401fa..198c46123 100644 --- a/Docs/carla_benchmark.md +++ b/Docs/carla_benchmark.md @@ -15,6 +15,15 @@ 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 +or + $ python run_benchmark + Run the help command to see options available $ ./run_benchmark --help From 3d418f9be4e6d7491e8b5f15edf8421ea14e1ebf Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 22 Dec 2017 11:03:47 -0200 Subject: [PATCH 27/59] Small bug on line position --- PythonClient/carla/benchmarks/benchmark.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 27998271a..c32c688c7 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -69,12 +69,14 @@ class Benchmark(object): 'pos_y': -1 } - # Get the line for the experiment to be continued - self._line_on_file = self._continue_experiment(continue_experiment) self._experiments = self._build_experiments() # Create the log files and get the names self._suffix_name, self._full_name = self._create_log_record(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( From 67289ac7eee7aa1c98ea3624c1e9db25d118b443 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 22 Dec 2017 11:28:13 -0200 Subject: [PATCH 28/59] Small bug class definition --- PythonClient/carla/benchmarks/benchmark.py | 19 +++++++++++++++---- PythonClient/carla/benchmarks/corl_2017.py | 4 ++-- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index c32c688c7..dfdc2cf91 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -72,7 +72,7 @@ class Benchmark(object): self._experiments = self._build_experiments() # Create the log files and get the names - self._suffix_name, self._full_name = self._create_log_record(self._experiments) + 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) @@ -318,7 +318,12 @@ class Benchmark(object): 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): """ @@ -327,9 +332,15 @@ class Benchmark(object): """ pass + @abc.abstractmethod + def get_all_statistics(self): + """ + Get the statistics of the evaluated experiments + :return: + """ @abc.abstractmethod - def _get_pose_and_task(self): + def _get_pose_and_task(self, line_on_file): """ Parse the experiment depending on number of poses and tasks """ @@ -349,7 +360,7 @@ class Benchmark(object): """ @staticmethod - def get_experiments_names(experiments): + def _get_experiments_names(experiments): name_cat = 'w' diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index 24a483bc4..323261507 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -57,8 +57,8 @@ class CoRL2017(Benchmark): for metric,values in metrics_summary.items(): - print('Metric : ',metric) - for weather,tasks in values.items(): + print('Metric : ', metric) + for weather, tasks in values.items(): if weather in set(weathers): print(' Weather: ',weather) count =0 From 9d61a2e8d508e78e0b20c8d1068fdec57410bb93 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 22 Dec 2017 11:35:30 -0200 Subject: [PATCH 29/59] Small corrections on planner --- PythonClient/carla/benchmarks/benchmark.py | 8 ++++---- PythonClient/carla/planner/city_track.py | 20 ++++++++++++-------- PythonClient/carla/planner/graph.py | 2 +- PythonClient/carla/planner/map.py | 4 ++-- PythonClient/carla/planner/planner.py | 2 +- 5 files changed, 20 insertions(+), 16 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index dfdc2cf91..8151f618f 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -285,8 +285,8 @@ class Benchmark(object): else: # Ask question, to avoid mistaken override situations answer = input("The experiment was already found in the files" - + ",Do you want to continue (y/n) ? \n" - + " If not the experiment will be overwritten") + + ", Do you want to continue (y/n) ? \n" + ) if answer == 'Yes' or answer == 'y': line_on_file = self._get_last_position() else: @@ -331,7 +331,7 @@ class Benchmark(object): Must be redefined in an inherited class. """ - pass + @abc.abstractmethod def get_all_statistics(self): """ @@ -344,7 +344,7 @@ class Benchmark(object): """ Parse the experiment depending on number of poses and tasks """ - pass + @abc.abstractmethod def plot_summary_train(self): diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index d19622c52..ad33e7142 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -37,28 +37,32 @@ class CityTrack(object): node_orientation = np.array([node_orientation[0], node_orientation[1]]) - - node = tuple([ int(x) for x in node ]) + 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])) # is it x or y ? Check to avoid special corner cases - if math.fabs(node_orientation[0]) > math.fabs(node_orientation[1]): - node_orientation = (node_orientation[0], 0.0, 0.0) - else: - node_orientation = (0.0, node_orientation[1], 0.0) + #if math.fabs(node_orientation[0]) > math.fabs(node_orientation[1]): + # node_orientation = (node_orientation[0], 0.0, 0.0) + #else: + # node_orientation = (0.0, node_orientation[1], 0.0) - node = self._map._grid.search_on_grid(node[0],node[1]) + node = self._map._grid.search_on_grid(node[0], node[1]) return node + + 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 diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index f35d73aae..92c1933c5 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -26,7 +26,7 @@ class Graph(object): A simple directed, weighted graph """ - def __init__(self, graph_file=None,node_density=50): + def __init__(self, graph_file=None, node_density=50): self._nodes = set() self._angles = {} diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index e220f8a11..67258c31c 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -40,10 +40,10 @@ class CarlaMap(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) + self._graph = Graph(city_file, node_density) self._pixel_density = pixel_density - self._grid = Grid(self._graph, node_density) + self._grid = Grid(self._graph) # The number of game units per pixel. For now this is fixed. self._converter = Converter(city_file, pixel_density, node_density) diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 164eb17e1..907696751 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -117,7 +117,7 @@ class Planner(object): current_pos = node_iter # We multiply by these values to convert distance to world coordinates - return distance * self._city_track._map.get_pixel_density() \ + return distance * self._city_track.get_pixel_density() \ * self._city_track.get_node_density() From 081db8238f9eb367f9fffc44167e84fbbe4b1145 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 22 Dec 2017 13:54:21 -0200 Subject: [PATCH 30/59] Some python2/3 fixes --- PythonClient/carla/benchmarks/benchmark.py | 2 +- PythonClient/carla/planner/astar.py | 24 ++++++-------------- PythonClient/carla/planner/city_track.py | 4 ++-- PythonClient/carla/planner/map.py | 26 ++++++++-------------- 4 files changed, 19 insertions(+), 37 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 8151f618f..a008d4d46 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -285,7 +285,7 @@ class Benchmark(object): else: # Ask question, to avoid mistaken override situations answer = input("The experiment was already found in the files" - + ", Do you want to continue (y/n) ? \n" + + ", Do you want to continue (y/n)? \n" ) if answer == 'Yes' or answer == 'y': line_on_file = self._get_last_position() diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py index 7ebc90c10..76a6626ee 100644 --- a/PythonClient/carla/planner/astar.py +++ b/PythonClient/carla/planner/astar.py @@ -1,8 +1,6 @@ import heapq - class Cell(object): - def __init__(self, x, y, reachable): """Initialize new cell. @@ -23,11 +21,12 @@ class Cell(object): 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) @@ -38,10 +37,8 @@ class AStar(object): self.grid_height = None self.grid_width = None - def init_grid(self, width, height, walls,start, end): - - """ - Prepare grid cells, walls. + def init_grid(self, width, height, walls, start, end): + """Prepare grid cells, walls. @param width grid's width. @param height grid's height. @@ -51,7 +48,6 @@ class AStar(object): """ self.grid_height = height self.grid_width = width - #print walls for x in range(self.grid_width): for y in range(self.grid_height): if (x, y) in walls: @@ -59,7 +55,6 @@ class AStar(object): else: reachable = True self.cells.append(Cell(x, y, reachable)) - self.start = self.get_cell(*start) self.end = self.get_cell(*end) @@ -127,8 +122,6 @@ class AStar(object): @returns path or None if not found. """ - if self.start == self.end: - return None # add starting cell to open heap queue heapq.heappush(self.opened, (self.start.f, self.start)) while len(self.opened): @@ -152,7 +145,4 @@ class AStar(object): else: self.update_cell(adj_cell, cell) # add adj cell to open list - heapq.heappush(self.opened, (adj_cell.f, adj_cell)) - - - + heapq.heappush(self.opened, (adj_cell.f, adj_cell)) \ No newline at end of file diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index ad33e7142..72bec3d26 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -34,8 +34,8 @@ class CityTrack(object): node =self._map.convert_to_node(position) # To change the orientation with respect to the map standards - node_orientation = np.array([node_orientation[0], - node_orientation[1]]) + #node_orientation = np.array([node_orientation[0], + # node_orientation[1]]) node = tuple([ int(x) for x in node ]) diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index 67258c31c..932f5b221 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -80,7 +80,8 @@ class CarlaMap(object): return np.asarray(img, dtype="int32") return np.fliplr(self.map_image) - def get_map_lanes(self, height=None): + def get_map_lanes(self, size=None): + if size is not None: img = Image.fromarray(self.map_image_lanes.astype(np.uint8)) img = img.resize((size[1], size[0]), Image.ANTIALIAS) @@ -92,23 +93,14 @@ class CarlaMap(object): 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 = color_to_angle(ori) return (-math.cos(ori), -math.sin(ori)) - def convert_to_node(self,input): + + def convert_to_node(self, input): """ Receives a data type (Can Be Pixel or World ) :param input: position in some coordinate @@ -116,7 +108,7 @@ class CarlaMap(object): """ return self._converter.convert_to_node(input) - def convert_to_pixel(self,input): + def convert_to_pixel(self, input): """ Receives a data type (Can Be Pixel or World ) :param input: position in some coordinate @@ -124,7 +116,7 @@ class CarlaMap(object): """ return self._converter.convert_to_pixel(input) - def convert_to_world(self,input): + def convert_to_world(self, input): """ Receives a data type (Can Be Pixel or World ) :param input: position in some coordinate @@ -134,7 +126,7 @@ class CarlaMap(object): - def get_walls_directed(self,node_source,source_ori,node_target,target_ori): + 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 @@ -142,7 +134,7 @@ class CarlaMap(object): """ - final_walls = self._grid.get_wall_source(node_source,source_ori,node_target) + 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)) From 8c252af9cb7061437b92dbcbbd924ddb01dfa99f Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 22 Dec 2017 14:04:54 -0200 Subject: [PATCH 31/59] Small fix on conversion --- PythonClient/carla/benchmarks/corl_2017.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index 323261507..fdaaec8a1 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -227,11 +227,11 @@ class CoRL2017(Benchmark): return 'corl2017_' + self._city_name - def _get_pose_and_task(self,line_on_file): + 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 line_on_file/len(self._experiments),line_on_file%25 + return int(line_on_file/len(self._experiments)), line_on_file % 25 From 99ef016f2e63c2c0062c9a8a6df850862c5f6f64 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Fri, 22 Dec 2017 18:33:30 +0100 Subject: [PATCH 32/59] Fix docs format --- Docs/carla_benchmark.md | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/Docs/carla_benchmark.md b/Docs/carla_benchmark.md index 198c46123..b42279752 100644 --- a/Docs/carla_benchmark.md +++ b/Docs/carla_benchmark.md @@ -2,31 +2,31 @@ 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. +--------------------- -! Currently not tested on python 3 +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. + +!!! important + Currently not tested on python 3 + +Besides the requirements of the CARLA client, the benchmark package also needs +the future package -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 -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 or - $ python run_benchmark + + $ python run_benchmark.py Run the help command to see options available - $ ./run_benchmark --help - - - + $ ./run_benchmark.py --help From 5067b9109e32801b6f6e0f95455f4d4174547e5c Mon Sep 17 00:00:00 2001 From: nsubiron Date: Fri, 22 Dec 2017 18:38:10 +0100 Subject: [PATCH 33/59] Add benchmark to documentation --- Docs/{carla_benchmark.md => benchmark.md} | 4 ++++ Docs/index.md | 1 + mkdocs.yml | 1 + 3 files changed, 6 insertions(+) rename Docs/{carla_benchmark.md => benchmark.md} (89%) diff --git a/Docs/carla_benchmark.md b/Docs/benchmark.md similarity index 89% rename from Docs/carla_benchmark.md rename to Docs/benchmark.md index b42279752..64c536607 100644 --- a/Docs/carla_benchmark.md +++ b/Docs/benchmark.md @@ -1,6 +1,10 @@ CARLA Benchmark =============== +!!! important + Benchmark code is currently in beta and can be found only in the + `benchmark_branch`. + Running the Benchmark --------------------- diff --git a/Docs/index.md b/Docs/index.md index 1eb92cb1a..560a4078f 100644 --- a/Docs/index.md +++ b/Docs/index.md @@ -8,6 +8,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) diff --git a/mkdocs.yml b/mkdocs.yml index 20ed2da28..204cff558 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -9,6 +9,7 @@ pages: - 'CARLA Settings': 'carla_settings.md' - 'Measurements': 'measurements.md' - 'Cameras and sensors': 'cameras_and_sensors.md' + - 'Benchmark': 'benchmark.md' - 'F.A.Q.': 'faq.md' - 'Troubleshooting': 'troubleshooting.md' - Building from source: From 50418c1e066cee9fe418b9b86303ba7a2e6e9df2 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Fri, 22 Dec 2017 18:40:48 +0100 Subject: [PATCH 34/59] Remove duplicate link --- Docs/index.md | 1 - 1 file changed, 1 deletion(-) diff --git a/Docs/index.md b/Docs/index.md index 560a4078f..44565e3aa 100644 --- a/Docs/index.md +++ b/Docs/index.md @@ -4,7 +4,6 @@ CARLA Documentation #### Using CARLA * [How to run CARLA server and client](how_to_run.md) - * [Benchmark](carla_benchmark.md) * [CARLA settings](carla_settings.md) * [Measurements](measurements.md) * [Cameras and sensors](cameras_and_sensors.md) From ada138c0395b727eacb48c47693dd83f77df49bf Mon Sep 17 00:00:00 2001 From: nsubiron Date: Fri, 22 Dec 2017 19:14:16 +0100 Subject: [PATCH 35/59] Some improvements to setup.py --- .gitignore | 3 +++ PythonClient/setup.py | 21 ++++++++++++--------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/.gitignore b/.gitignore index 2aef97e3e..6ef363893 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,11 @@ Dist Doxygen +PythonClient/dist Util/Build *.VC.db *.VC.opendb +*.egg-info *.kdev4 *.log *.pb.cc @@ -25,3 +27,4 @@ __pycache__ _benchmarks_results _images core + diff --git a/PythonClient/setup.py b/PythonClient/setup.py index d1317032d..31e26f2de 100644 --- a/PythonClient/setup.py +++ b/PythonClient/setup.py @@ -1,12 +1,15 @@ from setuptools import setup -setup( - name='PythonClient', - version='0.7', - packages=['carla','carla.planner','carla.benchmarks','carla.planner'], - license='', - long_description=open('README.txt').read(), - include_package_data=True, - package_data={'carla.planner' : ['Town01.txt']} -) +# @todo Dependencies are missing. +setup( + name='carla_client', + version='0.7.1', + packages=['carla', 'carla.planner', '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 +) From 45f8ec54fe7e2300d3f0834e5be82403660022ef Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 12 Jan 2018 15:59:00 +0100 Subject: [PATCH 36/59] Fixing some coding issues for travis integration --- PythonClient/carla/benchmarks/agent.py | 22 ++- PythonClient/carla/benchmarks/corl_2017.py | 151 ++++++++------------- PythonClient/carla/benchmarks/metrics.py | 2 +- PythonClient/carla/planner/astar.py | 18 +-- PythonClient/carla/planner/city_track.py | 129 +++++++----------- PythonClient/carla/planner/converter.py | 30 ++-- PythonClient/carla/planner/graph.py | 19 +-- PythonClient/carla/planner/grid.py | 27 ++-- PythonClient/carla/planner/map.py | 48 +++---- PythonClient/carla/planner/planner.py | 33 ++--- 10 files changed, 198 insertions(+), 281 deletions(-) diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py index 63f4e7b77..8a6c65363 100644 --- a/PythonClient/carla/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -11,25 +11,24 @@ from __future__ import print_function import abc -from carla.planner.planner import Planner +from carla.planner.planner import Planner + class Agent(object): - def __init__(self,city_name, **kwargs): - __metaclass__ = abc.ABCMeta + def __init__(self, city_name): + __metaclass__ = abc.ABCMeta # Try to remove 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]) + 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): """ @@ -37,4 +36,3 @@ class Agent(object): :param The measurements like speed, the image data and a target :returns A carla Control object, with the steering/gas/brake for the agent """ - diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index fdaaec8a1..8f61b17cd 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -16,141 +16,118 @@ 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]) + 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]) + self._plot_summary([1.0, 3.0, 6.0, 8.0]) def plot_summary_test(self): + self._plot_summary([4.0, 14.0]) - self._plot_summary([4.0,14.0]) - - - def _plot_summary(self,weathers): + 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]) + self._full_name, self._suffix_name), [3]) - - for metric,values in metrics_summary.items(): + 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 + print(' Weather: ', weather) + count = 0 for t in tasks: - - print(' Task ',count,' -> ',t) + print(' Task ', count, ' -> ', t) count += 1 + print(' AvG -> ', float(sum(tasks)) / float(len(tasks))) - print(' AvG -> ',float(sum(tasks))/float(len(tasks))) - - - - - - def _calculate_time_out(self,path_distance): + def _calculate_time_out(self, path_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 ((path_distance/100000.0)/10.0)*3600.0 + 10.0 - - - + return ((path_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_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]] + 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()] - + 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]] + [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]] + [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() - ] + 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): """ @@ -164,38 +141,30 @@ class CoRL2017(Benchmark): 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) + camera.set_rotation(-15.0, 0, 0) - weathers = [1,3,6,8,4,14] + 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] + 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] - - + 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, @@ -216,7 +185,7 @@ class CoRL2017(Benchmark): Poses=poses, Id=iteration, Repetitions=1 - ) + ) experiments_vector.append(experiment) return experiments_vector @@ -226,12 +195,10 @@ class CoRL2017(Benchmark): # 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 - + return int(line_on_file / len(self._experiments)), line_on_file % 25 diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py index 3f6ced110..b08b1730c 100644 --- a/PythonClient/carla/benchmarks/metrics.py +++ b/PythonClient/carla/benchmarks/metrics.py @@ -68,7 +68,7 @@ def get_distance_traveled(selected_matrix, header): i += 1 - return (float(acummulated_distance)/float(100*1000)) + return float(acummulated_distance)/float(100*1000) diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py index 76a6626ee..d22bbc2d5 100644 --- a/PythonClient/carla/planner/astar.py +++ b/PythonClient/carla/planner/astar.py @@ -1,5 +1,6 @@ import heapq + class Cell(object): def __init__(self, x, y, reachable): """Initialize new cell. @@ -20,7 +21,6 @@ class Cell(object): self.h = 0 self.f = 0 - def __lt__(self, other): return self.g < other.g @@ -36,6 +36,8 @@ class AStar(object): 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. @@ -85,14 +87,14 @@ class AStar(object): @returns adjacent cells list. """ cells = [] - if cell.x < self.grid_width-1: - cells.append(self.get_cell(cell.x+1, cell.y)) + 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)) + 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)) + 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): @@ -145,4 +147,4 @@ class AStar(object): else: self.update_cell(adj_cell, cell) # add adj cell to open list - heapq.heappush(self.opened, (adj_cell.f, adj_cell)) \ No newline at end of file + heapq.heappush(self.opened, (adj_cell.f, adj_cell)) diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index 72bec3d26..db543b44d 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -1,17 +1,12 @@ -import math - - -import numpy as np - 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): + def __init__(self, city_name): self._node_density = 50.0 self._pixel_density = 16.43 @@ -24,134 +19,112 @@ class CityTrack(object): self._previous_node = [] # The current computed route - self._route =None + self._route = None - def project_node(self, position, node_orientation): + def project_node(self, position): """ Projecting the graph node into the city road """ - node =self._map.convert_to_node(position) + node = self._map.convert_to_node(position) # To change the orientation with respect to the map standards - #node_orientation = np.array([node_orientation[0], - # node_orientation[1]]) - node = tuple([ int(x) for x in node ]) + 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])) - # is it x or y ? Check to avoid special corner cases - - - #if math.fabs(node_orientation[0]) > math.fabs(node_orientation[1]): - # node_orientation = (node_orientation[0], 0.0, 0.0) - #else: - # node_orientation = (0.0, node_orientation[1], 0.0) - - node = self._map._grid.search_on_grid(node[0], node[1]) + 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): + def is_at_goal(self, source, target): return source == target - def is_at_new_node(self,current_node): + def is_at_new_node(self, current_node): return current_node != self._previous_node - - def is_away_from_intersection(self,current_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): + def is_far_away_from_route_intersection(self, current_node): # CHECK FOR THE EMPTY CASE - if self._route == None: + if self._route is None: raise RuntimeError('Impossible to find route' - + ' Current planner is limited' - + ' Try to select start points away from interesections') - + + ' 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._route) > 4 + def compute_route(self, node_source, source_ori, node_target, target_ori): self._previous_node = node_source - - a_star =AStar() + 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) - + 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 # TODO: Clean this to avoid having to use this function - if route == None: - a_star =AStar() + 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) + self._map.get_graph_resolution()[1], self._map.get_walls(), + node_source, node_target) route = a_star.solve() - self._route = route return route - - def _closest_intersection_position(self, current_node): - - distance_vector = [] - for node_iterator in self._map._graph.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 node_iter in route: - for node_iterator in self._map._graph.intersection_nodes(): - - distance_vector.append(sldist(node_iterator, current_node)) - - return sorted(distance_vector)[0] - - - def get_distance_closest_node_route(self, pos, route): distance = [] for node_iter in route: - if node_iter in self.graph.intersection_nodes(): - + if node_iter in self._map.get_intersection_nodes(): distance.append(sldist(node_iter, pos)) if not distance: - return sldist(route[-1], pos) return sorted(distance)[0] + + def _closest_intersection_position(self, current_node): + + distance_vector = [] + for node_iterator in self._map.get_intersection_nodes(): + distance_vector.append(sldist(node_iterator, current_node)) + + return sorted(distance_vector)[0] + +""" + def _closest_intersection_route_position(self, current_node, route): + + distance_vector = [] + for _ in route: + for node_iterator in self._map.get_intersection_nodes(): + distance_vector.append(sldist(node_iterator, current_node)) + + return sorted(distance_vector)[0] +""" diff --git a/PythonClient/carla/planner/converter.py b/PythonClient/carla/planner/converter.py index dce2ba71f..f417f7610 100644 --- a/PythonClient/carla/planner/converter.py +++ b/PythonClient/carla/planner/converter.py @@ -1,8 +1,7 @@ import math import numpy as np -from carla.planner.graph import string_to_node,string_to_floats - +from carla.planner.graph import string_to_floats # Constant definition enumeration @@ -18,7 +17,6 @@ class Converter(object): 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()) @@ -38,14 +36,13 @@ class Converter(object): # The offset of the map zero coordinate. self._mapoffset = string_to_floats(f.readline()) - def convert_to_node(self,input_data): + 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) @@ -54,9 +51,7 @@ class Converter(object): else: raise ValueError('Invalid node to be converted') - - - def convert_to_pixel(self,input_data): + def convert_to_pixel(self, input_data): """ Receives a data type (Can Be Node or World ) @@ -72,8 +67,7 @@ class Converter(object): else: raise ValueError('Invalid node to be converted') - - def convert_to_world(self,input_data): + def convert_to_world(self, input_data): """ Receives a data type (Can Be Pixel or Node ) @@ -109,22 +103,22 @@ class Converter(object): , int(((pixel[1]) / self._node_density) - 2)] return tuple(node) - def _pixel_to_world(self,pixel): + + 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, + 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 @@ -139,11 +133,11 @@ class Converter(object): 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]] + 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))] + math.floor(relative_location[1] / float(self._pixel_density))] return pixel @@ -161,5 +155,3 @@ class Converter(object): return NODE else: return PIXEL - - diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index 92c1933c5..54769e859 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -13,12 +13,12 @@ def string_to_floats(string): def sldist(c1, c2): - return math.sqrt((c2[0] - c1[0])**2 + (c2[1] - c1[1])**2) + 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) + return math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1]) + ** 2 + (c2[2] - c1[2]) ** 2) class Graph(object): @@ -72,14 +72,12 @@ class Graph(object): sorted(distance_dic.items())) self._angles[node] = heading - for k, v in distance_dic.iteritems(): - - + for k, 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) + 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) @@ -89,6 +87,11 @@ class Graph(object): 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 = [] @@ -111,7 +114,7 @@ class Graph(object): 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] + 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) diff --git a/PythonClient/carla/planner/grid.py b/PythonClient/carla/planner/grid.py index 01d9eb9f1..3e7d8bcf4 100644 --- a/PythonClient/carla/planner/grid.py +++ b/PythonClient/carla/planner/grid.py @@ -14,25 +14,26 @@ class Grid(object): 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): + 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._resolution[ - 0] and c_y >= 0 and c_y < self._graph._resolution[1]: + 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) + return c_x, c_y + def get_walls(self): + return self._walls def get_wall_source(self, pos, pos_ori, target): @@ -46,10 +47,8 @@ class Grid(object): 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): @@ -63,10 +62,8 @@ class Grid(object): 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): @@ -84,21 +81,19 @@ class Grid(object): 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._resolution[0], - self._graph._resolution[1])) + (self._graph.get_resolution()[0], + self._graph.get_resolution()[1])) - for key, connections in self._graph._edges.items(): + 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]) @@ -125,8 +120,8 @@ class Grid(object): for offset in visit: node = (pos[0] + offset[0], pos[1] + offset[1]) - if (node[0] >= 0 and node[0] < self._graph._resolution[0] - and node[1] >= 0 and node[1] < self._graph._resolution[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) diff --git a/PythonClient/carla/planner/map.py b/PythonClient/carla/planner/map.py index 25806f71b..9c8decb70 100644 --- a/PythonClient/carla/planner/map.py +++ b/PythonClient/carla/planner/map.py @@ -9,7 +9,6 @@ import math import os - try: import numpy as np except ImportError: @@ -21,12 +20,14 @@ except ImportError: raise RuntimeError('cannot import PIL, make sure pillow package is installed') from carla.planner.graph import Graph +from carla.planner.graph import sldist from carla.planner.grid import Grid from carla.planner.converter import Converter def color_to_angle(color): - return ((float(color)/255.0)) * 2*math.pi + return ((float(color) / 255.0)) * 2 * math.pi + class CarlaMap(object): @@ -48,8 +49,6 @@ class CarlaMap(object): self._converter = Converter(city_file, pixel_density, node_density) - - # Load the lanes image self.map_image_lanes = Image.open(city_map_file_lanes) self.map_image_lanes.load() @@ -64,18 +63,17 @@ class CarlaMap(object): 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._resolution + return self._graph.get_resolution() def get_map(self, height=None): if height is not None: img = Image.fromarray(self.map_image.astype(np.uint8)) - aspect_ratio = height/float(self.map_image.shape[0]) + aspect_ratio = height / float(self.map_image.shape[0]) - img = img.resize((int(aspect_ratio*self.map_image.shape[1]),height), Image.ANTIALIAS) + img = img.resize((int(aspect_ratio * self.map_image.shape[1]), height), Image.ANTIALIAS) img.load() return np.asarray(img, dtype="int32") return np.fliplr(self.map_image) @@ -97,31 +95,29 @@ class CarlaMap(object): return (-math.cos(ori), -math.sin(ori)) - def convert_to_node(self, input): + def convert_to_node(self, input_data): """ Receives a data type (Can Be Pixel or World ) - :param input: position in some coordinate + :param input_data: position in some coordinate :return: A node object """ - return self._converter.convert_to_node(input) + return self._converter.convert_to_node(input_data) - def convert_to_pixel(self, input): + def convert_to_pixel(self, input_data): """ Receives a data type (Can Be Pixel or World ) - :param input: position in some coordinate + :param input_data: position in some coordinate :return: A node object """ - return self._converter.convert_to_pixel(input) + return self._converter.convert_to_pixel(input_data) - def convert_to_world(self, input): + def convert_to_world(self, input_data): """ Receives a data type (Can Be Pixel or World ) - :param input: position in some coordinate + :param input_data: position in some coordinate :return: A node object """ - return self._converter.convert_to_world(input) - - + return self._converter.convert_to_world(input_data) def get_walls_directed(self, node_source, source_ori, node_target, target_ori): """ @@ -134,23 +130,23 @@ class CarlaMap(object): 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)) + node_target, target_ori, node_source)) return final_walls - def get_walls(self): - return self._grid._walls - - + return self._grid.get_walls() def get_distance_closest_node(self, pos): distance = [] - for node_iter in self.graph.intersection_nodes(): - + 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]) \ No newline at end of file diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 907696751..45949503b 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -1,10 +1,8 @@ import collections import math - import numpy as np - from . import city_track @@ -28,7 +26,7 @@ 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 sldist(c1, c2): return math.sqrt((c2[0] - c1[0]) ** 2 + (c2[1] - c1[1]) ** 2) def signal(v1, v2): @@ -43,7 +41,6 @@ class Planner(object): self._commands = [] - def get_next_command(self, source, source_ori, target, target_ori): """ Computes the full plan and returns the next command, @@ -54,8 +51,8 @@ class Planner(object): :return: a command ( Straight,Lane Follow, Left or Right) """ - track_source = self._city_track.project_node(source, source_ori) - track_target = self._city_track.project_node(target, target_ori) + track_source = self._city_track.project_node(source) + track_target = self._city_track.project_node(target) # reach the goal @@ -100,8 +97,8 @@ class Planner(object): target_ori): distance = 0 - track_source = self._city_track.project_node(source, source_ori) - track_target = self._city_track.project_node(target, target_ori) + track_source = self._city_track.project_node(source) + track_target = self._city_track.project_node(target) current_pos = track_source @@ -112,30 +109,26 @@ class Planner(object): 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() - + * 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, source_ori) - track_target = self._city_track.project_node(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, source_ori): - - node_source = self._city_track.project_node(source, source_ori) - - return self.is_away_from_intersection(node_source) + 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): @@ -146,11 +139,10 @@ class Planner(object): :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._map._graph.intersection_nodes(): + if route[i] not in self._city_track.get_intersection_nodes(): continue current = route[i] @@ -163,7 +155,6 @@ class Planner(object): [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: From 4083adcba2f09e2802045b7e482f6515f6d97990 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 12 Jan 2018 15:57:53 -0200 Subject: [PATCH 37/59] Adding dependencies and fixing a few bugs --- PythonClient/carla/planner/city_track.py | 4 ++-- PythonClient/manual_control.py | 2 +- PythonClient/requirements.txt | 2 ++ 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index db543b44d..f7adff1b1 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -118,7 +118,7 @@ class CityTrack(object): return sorted(distance_vector)[0] -""" + def _closest_intersection_route_position(self, current_node, route): distance_vector = [] @@ -127,4 +127,4 @@ class CityTrack(object): distance_vector.append(sldist(node_iterator, current_node)) return sorted(distance_vector)[0] -""" + diff --git a/PythonClient/manual_control.py b/PythonClient/manual_control.py index dfca225db..9e3d9d69b 100755 --- a/PythonClient/manual_control.py +++ b/PythonClient/manual_control.py @@ -127,7 +127,7 @@ class CarlaGame(object): self._map_view = None self._is_on_reverse = False self._city_name = city_name - self._map = CarlaMap(city_name) if city_name is not None else None + self._map = CarlaMap(city_name, 16.43, 50.0) if city_name is not None else None self._map_shape = self._map.map_image.shape if city_name is not None else None self._map_view = self._map.get_map(WINDOW_HEIGHT) if city_name is not None else None self._position = None diff --git a/PythonClient/requirements.txt b/PythonClient/requirements.txt index 1a697a692..c6c7a2cfe 100644 --- a/PythonClient/requirements.txt +++ b/PythonClient/requirements.txt @@ -2,3 +2,5 @@ Pillow numpy protobuf pygame +matplotlib +builtins From 676656d20b09f62c4eedfb50536a0349ef7621f3 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 12 Jan 2018 16:06:01 -0200 Subject: [PATCH 38/59] Changing requirements --- PythonClient/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/requirements.txt b/PythonClient/requirements.txt index c6c7a2cfe..f170835dd 100644 --- a/PythonClient/requirements.txt +++ b/PythonClient/requirements.txt @@ -3,4 +3,4 @@ numpy protobuf pygame matplotlib -builtins +future From 6195653942f725d62d8b05a6b9edb354c6ae7fe8 Mon Sep 17 00:00:00 2001 From: felipecode Date: Fri, 12 Jan 2018 16:31:39 -0200 Subject: [PATCH 39/59] Removing travis warnings --- PythonClient/carla/benchmarks/agent.py | 2 +- PythonClient/carla/benchmarks/benchmark.py | 2 +- PythonClient/carla/planner/astar.py | 2 +- PythonClient/carla/planner/city_track.py | 2 +- PythonClient/carla/planner/graph.py | 8 ++--- PythonClient/carla/planner/test_planner.py | 41 ++++++++++++++++++++++ 6 files changed, 49 insertions(+), 8 deletions(-) create mode 100644 PythonClient/carla/planner/test_planner.py diff --git a/PythonClient/carla/benchmarks/agent.py b/PythonClient/carla/benchmarks/agent.py index 8a6c65363..c71f4c1db 100644 --- a/PythonClient/carla/benchmarks/agent.py +++ b/PythonClient/carla/benchmarks/agent.py @@ -16,7 +16,7 @@ from carla.planner.planner import Planner class Agent(object): def __init__(self, city_name): - __metaclass__ = abc.ABCMeta # Try to remove + self.__metaclass__ = abc.ABCMeta self._planner = Planner(city_name) def get_distance(self, start_point, end_point): diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index a008d4d46..c138a7c8b 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -40,7 +40,7 @@ class Benchmark(object): continue_experiment=False, save_images=False): - __metaclass__ = abc.ABCMeta + self.__metaclass__ = abc.ABCMeta self._city_name = city_name diff --git a/PythonClient/carla/planner/astar.py b/PythonClient/carla/planner/astar.py index d22bbc2d5..e322395ec 100644 --- a/PythonClient/carla/planner/astar.py +++ b/PythonClient/carla/planner/astar.py @@ -128,7 +128,7 @@ class AStar(object): heapq.heappush(self.opened, (self.start.f, self.start)) while len(self.opened): # pop cell from heap queue - f, cell = heapq.heappop(self.opened) + _, 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 diff --git a/PythonClient/carla/planner/city_track.py b/PythonClient/carla/planner/city_track.py index f7adff1b1..4223426c8 100644 --- a/PythonClient/carla/planner/city_track.py +++ b/PythonClient/carla/planner/city_track.py @@ -84,7 +84,7 @@ class CityTrack(object): route = a_star.solve() # JuSt a Corner Case - # TODO: Clean this to avoid having to use this function + # 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], diff --git a/PythonClient/carla/planner/graph.py b/PythonClient/carla/planner/graph.py index 54769e859..54913b00f 100644 --- a/PythonClient/carla/planner/graph.py +++ b/PythonClient/carla/planner/graph.py @@ -35,9 +35,9 @@ class Graph(object): self._node_density = node_density if graph_file is not None: - with open(graph_file, 'r') as file: + with open(graph_file, 'r') as f: # Skipe the first four lines that - lines_after_4 = file.readlines()[4:] + lines_after_4 = f.readlines()[4:] # the graph resolution. linegraphres = lines_after_4[0] @@ -72,7 +72,7 @@ class Graph(object): sorted(distance_dic.items())) self._angles[node] = heading - for k, v in distance_dic.items(): + for _, v in distance_dic.items(): start_to_goal = np.array([node[0] - v[0], node[1] - v[1]]) print(start_to_goal) @@ -116,7 +116,7 @@ class Graph(object): 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 = plt.subplots() ax.add_collection(lc) ax.autoscale() diff --git a/PythonClient/carla/planner/test_planner.py b/PythonClient/carla/planner/test_planner.py new file mode 100644 index 000000000..9d977cf44 --- /dev/null +++ b/PythonClient/carla/planner/test_planner.py @@ -0,0 +1,41 @@ +from carla import sensor +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 + + + + +planner= Planner('Town01') + + + + +resolution = planner._city_track._map.get_graph_resolution() + +for i in range(resolution[0]): + for j in range(resolution[1]): + + world_source = planner._city_track._map.convert_to_world((i,j)) + source_ori = planner._city_track._map.get_lane_orientation(world_source) + print (' Making Route from (',i, ',',j,') o (',source_ori[0],',',source_ori[1],')') + + for k in range(resolution[0]): + for l in range(resolution[1]): + world_target = planner._city_track._map.convert_to_world((k,l)) + + target_ori = planner._city_track._map.get_lane_orientation(world_target) + print (' To (',k, ',',l,') o (',target_ori[0],',',target_ori[1],')') + + path_distance=planner.get_shortest_path_distance( + world_source,source_ori,world_target,target_ori) + + #print ('Distance is ',path_distance) + command=planner.get_next_command( + world_source,source_ori,world_target,target_ori) + #print ('Command is ',command) + #print ('Latest Route ',planner._city_track._route) From 388a8060d7a357cd3451d3863c65c5cbf1ef8cd8 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 13:00:33 +0100 Subject: [PATCH 40/59] organizing benchmark code --- PythonClient/carla/benchmarks/benchmark.py | 7 ++-- PythonClient/carla/benchmarks/corl_2017.py | 4 +-- PythonClient/carla/benchmarks/metrics.py | 10 +++--- PythonClient/carla/planner/test_planner.py | 41 ---------------------- PythonClient/run_benchmark.py | 3 -- 5 files changed, 9 insertions(+), 56 deletions(-) delete mode 100644 PythonClient/carla/planner/test_planner.py diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index c138a7c8b..b62dd2114 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -273,7 +273,7 @@ class Benchmark(object): now.strftime("%Y%m%d%H%M")), 'w') as f: pass - return suffix_name,full_name + return suffix_name, full_name def _continue_experiment(self, continue_experiment): @@ -307,11 +307,8 @@ class Benchmark(object): def _get_last_position(self): with open(os.path.join(self._full_name, self._suffix_name)) as f: + return sum(1 for _ in f) - # Function to count the lines. - for i, l in enumerate(f): - pass - return i # To be redefined on subclasses on how to calculate timeout for an episode @abc.abstractmethod diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index 8f61b17cd..85bc15718 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -59,13 +59,13 @@ class CoRL2017(Benchmark): print(' AvG -> ', float(sum(tasks)) / float(len(tasks))) - def _calculate_time_out(self, path_distance): + 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 ((path_distance / 100000.0) / 10.0) * 3600.0 + 10.0 + return ((distance / 100000.0) / 10.0) * 3600.0 + 10.0 def _poses_town01(self): """ diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py index b08b1730c..e9ca1d80f 100644 --- a/PythonClient/carla/benchmarks/metrics.py +++ b/PythonClient/carla/benchmarks/metrics.py @@ -101,13 +101,13 @@ def get_out_of_road_lane(selected_matrix, header): -def compute_summary(file, dynamic_episodes): +def compute_summary(filename, dynamic_episodes): # Separate the PATH and the basename - path = os.path.dirname(file) - base_name = os.path.basename(file) + path = os.path.dirname(filename) + base_name = os.path.basename(filename) - f = open(file, "rb") + f = open(filename, "rb") header = f.readline() header = header.split(',') header[-1] = header[-1][:-2] @@ -119,7 +119,7 @@ def compute_summary(file, dynamic_episodes): header_rewards[-1] = header_rewards[-1][:-2] f.close() - data_matrix = np.loadtxt(open(file, "rb"), delimiter=",", skiprows=1) + data_matrix = np.loadtxt(open(filename, "rb"), delimiter=",", skiprows=1) tasks = np.unique(data_matrix[:, header.index('exp_id')]) diff --git a/PythonClient/carla/planner/test_planner.py b/PythonClient/carla/planner/test_planner.py deleted file mode 100644 index 9d977cf44..000000000 --- a/PythonClient/carla/planner/test_planner.py +++ /dev/null @@ -1,41 +0,0 @@ -from carla import sensor -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 - - - - -planner= Planner('Town01') - - - - -resolution = planner._city_track._map.get_graph_resolution() - -for i in range(resolution[0]): - for j in range(resolution[1]): - - world_source = planner._city_track._map.convert_to_world((i,j)) - source_ori = planner._city_track._map.get_lane_orientation(world_source) - print (' Making Route from (',i, ',',j,') o (',source_ori[0],',',source_ori[1],')') - - for k in range(resolution[0]): - for l in range(resolution[1]): - world_target = planner._city_track._map.convert_to_world((k,l)) - - target_ori = planner._city_track._map.get_lane_orientation(world_target) - print (' To (',k, ',',l,') o (',target_ori[0],',',target_ori[1],')') - - path_distance=planner.get_shortest_path_distance( - world_source,source_ori,world_target,target_ori) - - #print ('Distance is ',path_distance) - command=planner.get_next_command( - world_source,source_ori,world_target,target_ori) - #print ('Command is ',command) - #print ('Latest Route ',planner._city_track._route) diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 81d65ae53..ef3317ac0 100755 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -87,6 +87,3 @@ if __name__ == '__main__': except TCPConnectionError as error: logging.error(error) time.sleep(1) - except Exception as exception: - logging.exception(exception) - sys.exit(1) From bad04d82da4346106c0b43099aef0519e979159b Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 14:06:29 +0100 Subject: [PATCH 41/59] Fixing some format issues --- PythonClient/carla/benchmarks/benchmark.py | 9 ++++----- PythonClient/carla/benchmarks/experiment.py | 3 +-- PythonClient/carla/planner/planner.py | 6 +++--- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index b62dd2114..6696bcc62 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -7,7 +7,7 @@ # For a copy, see . -from builtins import input +from builtins import input_data import csv import datetime @@ -269,12 +269,11 @@ class Benchmark(object): # Make a date file: to show when this was modified, # the number of times the experiments were run now = datetime.datetime.now() - with open(os.path.join(full_name, - now.strftime("%Y%m%d%H%M")), 'w') as f: - pass + open(os.path.join(full_name, now.strftime("%Y%m%d%H%M"))).close() return suffix_name, full_name + def _continue_experiment(self, continue_experiment): if self._experiment_exist(): @@ -284,7 +283,7 @@ class Benchmark(object): else: # Ask question, to avoid mistaken override situations - answer = input("The experiment was already found in the files" + 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': diff --git a/PythonClient/carla/benchmarks/experiment.py b/PythonClient/carla/benchmarks/experiment.py index 4340d5599..8626dd797 100644 --- a/PythonClient/carla/benchmarks/experiment.py +++ b/PythonClient/carla/benchmarks/experiment.py @@ -1,11 +1,10 @@ from carla.settings import CarlaSettings -from carla.sensor import Camera class Experiment(object): - def __init__(self, **kwargs): + def __init__(self): self.Id = '' self.Conditions = CarlaSettings() self.Poses = [[]] diff --git a/PythonClient/carla/planner/planner.py b/PythonClient/carla/planner/planner.py index 45949503b..20f84f981 100644 --- a/PythonClient/carla/planner/planner.py +++ b/PythonClient/carla/planner/planner.py @@ -10,9 +10,9 @@ def compare(x, y): return collections.Counter(x) == collections.Counter(y) -""" -Constants Used for the high level commands -""" + +# Constants Used for the high level commands + REACH_GOAL = 0.0 GO_STRAIGHT = 5.0 From d481a5f3ba5dc8cafa1f778cb4ade932e412c7b5 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 14:12:56 +0100 Subject: [PATCH 42/59] Correcting travis issues --- PythonClient/carla/benchmarks/benchmark.py | 4 ++-- PythonClient/carla/benchmarks/corl_2017.py | 1 - PythonClient/run_benchmark.py | 1 - 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 6696bcc62..a1b113d66 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -7,7 +7,7 @@ # For a copy, see . -from builtins import input_data +from builtins import input as input_data import csv import datetime @@ -269,7 +269,7 @@ class Benchmark(object): # 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"))).close() + open(os.path.join(full_name, now.strftime("%Y%m%d%H%M")),'w').close() return suffix_name, full_name diff --git a/PythonClient/carla/benchmarks/corl_2017.py b/PythonClient/carla/benchmarks/corl_2017.py index 85bc15718..cf2cf11c2 100644 --- a/PythonClient/carla/benchmarks/corl_2017.py +++ b/PythonClient/carla/benchmarks/corl_2017.py @@ -9,7 +9,6 @@ from __future__ import print_function import os -import datetime from .benchmark import Benchmark from .experiment import Experiment diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index ef3317ac0..2aa70e1da 100755 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -8,7 +8,6 @@ import argparse import logging -import sys import time from carla.benchmarks.agent import Agent From 59762f52debd43494a57d4eeb6ac266e02312b3c Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 15:21:52 +0100 Subject: [PATCH 43/59] Adding documentation for the benchmarking --- Docs/benchmark.md | 41 ++++++++++++++++++++++++++++++----- PythonClient/run_benchmark.py | 4 ---- 2 files changed, 36 insertions(+), 9 deletions(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index 64c536607..4eb02d8fa 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -1,9 +1,6 @@ CARLA Benchmark =============== -!!! important - Benchmark code is currently in beta and can be found only in the - `benchmark_branch`. Running the Benchmark --------------------- @@ -13,8 +10,6 @@ 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. -!!! important - Currently not tested on python 3 Besides the requirements of the CARLA client, the benchmark package also needs the future package @@ -34,3 +29,39 @@ or 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 excerpted is executed in the run_benchmark.py example. + +First a benchmark object is defined. This is the object that is used to benchmark a certain Agent. + +On the second line of our sample code. There is an object of a Manual class instanced. This class inherited an Agent base class +that is used on the benchmarked. +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 [measurements](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [target](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [sensor_data](https://github.com/carla-simulator/carla/blob/master/Docs/cameras_and_sensors.m) and [control](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md) types are described at the documentation. + +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. + +Creating your Benchmark +--------------------- +Tutorial to be added diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 2aa70e1da..008347b22 100755 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -24,11 +24,7 @@ class Manual(Agent): """ def run_step(self, measurements, sensor_data, target): control = VehicleControl() - control.steer = 0.0 control.throttle = 0.9 - control.brake = 0.0 - control.hand_brake = False - control.reverse = False return control From 4dc711a20b0db674cc7801622e9d43a940e5ec56 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 15:27:15 +0100 Subject: [PATCH 44/59] Benchmarking added to main README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 74f6d5390..ce1c58288 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ 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 ---- From f901b05a66f835501b8803ccd7386b6a8f1a2de2 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 15:28:47 +0100 Subject: [PATCH 45/59] Benchmarking added to main README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index ce1c58288..82b6d64ab 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ 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 From f139431c628dc8dbb24783c2ce06955025db8eb9 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 15:33:47 +0100 Subject: [PATCH 46/59] More fix on benchmark documentation --- Docs/benchmark.md | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index 4eb02d8fa..b21096032 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -1,6 +1,9 @@ CARLA Benchmark =============== +!!! important + Benchmark code is currently in beta and can be found only in the + `benchmark_branch`. Running the Benchmark --------------------- @@ -38,13 +41,12 @@ The benchmark works by calling three lines of code agent = Manual(args.city_name) results = corl.benchmark_agent(agent, client) -This is excerpted is executed in the run_benchmark.py example. +This is excerpt is executed in the run_benchmark.py example. -First a benchmark object is defined. This is the object that is used to benchmark a certain Agent. - -On the second line of our sample code. There is an object of a Manual class instanced. This class inherited an Agent base class -that is used on the benchmarked. -To be benchmarked an Agent subclass must redefine the *run_step* function as it is done in the following excerpt: +First a *benchmark* object is defined. This is the object that is used to benchmark a certain Agent.
+On the second line of our sample code, there is an object of a Manual class instanced. This class inherited an Agent base class +that is used by the *benchmark* object. +To be benchmarked, an Agent subclass must redefine the *run_step* function as it is done in the following excerpt: def run_step(self, measurements, sensor_data, target): """ @@ -57,10 +59,11 @@ To be benchmarked an Agent subclass must redefine the *run_step* function as it 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](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [target](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [sensor_data](https://github.com/carla-simulator/carla/blob/master/Docs/cameras_and_sensors.m) and [control](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md) types are described at the documentation. -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. + Creating your Benchmark --------------------- From eabec718453ccc552b328baa8704971fbf6f9e09 Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 15:35:36 +0100 Subject: [PATCH 47/59] Update benchmark.md --- Docs/benchmark.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index b21096032..4f61ec581 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -41,7 +41,7 @@ The benchmark works by calling three lines of code agent = Manual(args.city_name) results = corl.benchmark_agent(agent, client) -This is excerpt is executed in the run_benchmark.py example. +This is excerpt is executed in the [run_benchmark.py](https://github.com/carla-simulator/carla/blob/benchmark_branch/PythonClient/run_benchmark.py) example. First a *benchmark* object is defined. This is the object that is used to benchmark a certain Agent.
On the second line of our sample code, there is an object of a Manual class instanced. This class inherited an Agent base class From 3feccb8b22fd2612e5ca496621edc45c9cf51aec Mon Sep 17 00:00:00 2001 From: felipecode Date: Wed, 17 Jan 2018 15:37:38 +0100 Subject: [PATCH 48/59] Update benchmark.md --- Docs/benchmark.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index 4f61ec581..2dc8ff2b9 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -43,7 +43,7 @@ The benchmark works by calling three lines of code This is excerpt is executed in the [run_benchmark.py](https://github.com/carla-simulator/carla/blob/benchmark_branch/PythonClient/run_benchmark.py) example. -First a *benchmark* object is defined. This is the object that is used to benchmark a certain Agent.
+First a *benchmark* object is defined, for this case, a CoRL2017 benchmark. This is object is used to benchmark a certain Agent.
On the second line of our sample code, there is an object of a Manual class instanced. This class inherited an Agent base class that is used by the *benchmark* object. To be benchmarked, an Agent subclass must redefine the *run_step* function as it is done in the following excerpt: @@ -59,7 +59,7 @@ To be benchmarked, an Agent subclass must redefine the *run_step* function as it 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 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](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [target](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [sensor_data](https://github.com/carla-simulator/carla/blob/master/Docs/cameras_and_sensors.m) and [control](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md) types are described at the documentation. From 1e2c3a3ea07dfa35690e83a80fd55a9eb6193ca6 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 18 Jan 2018 12:52:02 +0100 Subject: [PATCH 49/59] metrics, wrong f name, corner case --- PythonClient/carla/benchmarks/metrics.py | 27 +++++++++++++++--------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/PythonClient/carla/benchmarks/metrics.py b/PythonClient/carla/benchmarks/metrics.py index e9ca1d80f..dd47a5f52 100644 --- a/PythonClient/carla/benchmarks/metrics.py +++ b/PythonClient/carla/benchmarks/metrics.py @@ -107,26 +107,33 @@ def compute_summary(filename, dynamic_episodes): 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, 'rewards_' + base_name), "rb") - header_rewards = f.readline() - header_rewards = header_rewards.split(',') - header_rewards[-1] = header_rewards[-1][:-2] + 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, 'rewards_' + base_name), "rb"), delimiter=",", skiprows=1) + 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}, @@ -153,11 +160,11 @@ def compute_summary(filename, dynamic_episodes): 'exp_id')] == t, data_matrix[:, header.index('weather')] == w)] - 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))] + 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_rewards) + task_reward_matrix, header_details) metrics_dictionary['average_fully_completed'][w][t] = sum( task_data_matrix[:, header.index('result')])/task_data_matrix.shape[0] @@ -178,8 +185,8 @@ def compute_summary(filename, dynamic_episodes): if list(tasks).index(t) in set(dynamic_episodes): lane_road = get_out_of_road_lane( - task_reward_matrix, header_rewards) - colisions = get_colisions(task_reward_matrix, header_rewards) + task_reward_matrix, header_details) + colisions = get_colisions(task_reward_matrix, header_details) From cec548c9644b991f24588fe0b8db91933f9aab81 Mon Sep 17 00:00:00 2001 From: felipecode Date: Thu, 18 Jan 2018 14:19:50 +0100 Subject: [PATCH 50/59] Small bug on checking experiment --- PythonClient/carla/benchmarks/benchmark.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index a1b113d66..e5ced2339 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -299,9 +299,7 @@ class Benchmark(object): def _experiment_exist(self): - - folder = os.path.dirname(self._full_name) - return os.path.isdir(folder) + return os.path.isfile(self._full_name) def _get_last_position(self): From f0eccca7ad9f02fbd38bebe29cf901caa31cb95f Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 10:43:12 +0100 Subject: [PATCH 51/59] Fixing exception on tcp, adding verbose --- PythonClient/carla/benchmarks/benchmark.py | 42 +++++++++++++--------- PythonClient/carla/tcp.py | 6 ++-- PythonClient/run_benchmark.py | 17 +++++++-- 3 files changed, 43 insertions(+), 22 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index e5ced2339..12bfd0beb 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -7,21 +7,19 @@ # For a copy, see . -from builtins import input as input_data - 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) @@ -38,11 +36,15 @@ class Benchmark(object): city_name, name_to_save, continue_experiment=False, - save_images=False): + save_images=False, + verbose=False): + self.__metaclass__ = abc.ABCMeta self._city_name = city_name + self._verbose = verbose + self._base_name = name_to_save self._dict_stats = {'exp_id': -1, @@ -104,8 +106,11 @@ class Benchmark(object): measurements, sensor_data = carla.read_data() control = agent.run_step(measurements, sensor_data, target) - print('STEER ', control.steer, 'GAS ', - control.throttle, 'Brake ', control.brake) + + 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 @@ -124,9 +129,11 @@ class Benchmark(object): distance = sldist([curr_x, curr_y], [target.location.x, target.location.y]) - print( + 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)) + (float(distance), curr_x, curr_y, target.location.x, + target.location.y)) if distance < 200.0: success = True @@ -157,7 +164,7 @@ class Benchmark(object): else: (start_task, start_pose) = self._get_pose_and_task(self._line_on_file) - print(' START ') + logging.info(' START ') for experiment in self._experiments[start_task:]: @@ -172,12 +179,15 @@ class Benchmark(object): carla.start_episode(start_point) - print('======== !!!! ==========') - print(' Start Position ', start_point, - ' End Position ', end_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 @@ -191,16 +201,16 @@ class Benchmark(object): # compute stats for the experiment self._write_summary_results( - experiment, pose, rep, path_distance, + experiment, pose, rep, euclidean_distance, remaining_distance, final_time, time_out, result) self._write_details_results(experiment, rep, reward_vec) if(result > 0): - print('+++++ Target achieved in %f seconds! +++++' % + logging.info('+++++ Target achieved in %f seconds! +++++' % final_time) else: - print('----- Timeout! -----') + logging.info('----- Timeout! -----') return self.get_all_statistics() def _write_summary_results(self, experiment, pose, rep, diff --git a/PythonClient/carla/tcp.py b/PythonClient/carla/tcp.py index 5dd9ed9ef..fbf1b17f3 100644 --- a/PythonClient/carla/tcp.py +++ b/PythonClient/carla/tcp.py @@ -41,7 +41,7 @@ class TCPClient(object): self._socket.settimeout(self._timeout) logging.debug('%sconnected', self._logprefix) return - except OSError as exception: + except socket.error as exception: error = exception logging.debug('%sconnection attempt %d: %s', self._logprefix, attempt, error) time.sleep(1) @@ -65,7 +65,7 @@ class TCPClient(object): header = struct.pack(' 0: try: data = self._socket.recv(length) - except OSError as exception: + except socket.error as exception: self._reraise_exception_as_tcp_error('failed to read data', exception) if not data: raise TCPConnectionError(self._logprefix + 'connection closed') diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 008347b22..790882600 100755 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -35,6 +35,11 @@ if __name__ == '__main__': 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( @@ -58,12 +63,17 @@ if __name__ == '__main__': '-n', '--log_name', metavar='T', default='test', - help='The name of the log file to be created by the scripts' + 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 - 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) @@ -71,7 +81,8 @@ if __name__ == '__main__': 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) + corl = CoRL2017(city_name=args.city_name, name_to_save=args.log_name, + verbose=args.verbose) agent = Manual(args.city_name) results = corl.benchmark_agent(agent, client) corl.plot_summary_test() From 78187b83d07bf7e84629fd370bc5574c4a0db7e3 Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 10:53:08 +0100 Subject: [PATCH 52/59] log not lazy fix --- PythonClient/carla/benchmarks/benchmark.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index 12bfd0beb..b4e3dba02 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -108,8 +108,8 @@ class Benchmark(object): 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)) + logging.info('Steer = %f Throttle = %f Brake = %f ', + control.steer, control.throttle, control.brake) carla.send_control(control) @@ -131,9 +131,9 @@ class Benchmark(object): 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)) + '[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 @@ -180,8 +180,8 @@ class Benchmark(object): carla.start_episode(start_point) logging.info('======== !!!! ==========') - logging.info(' Start Position %d End Position %d ' % - (start_point, end_point)) + logging.info(' Start Position %d End Position %d ', + start_point, end_point) path_distance = agent.get_distance( positions[start_point], positions[end_point]) @@ -207,7 +207,7 @@ class Benchmark(object): self._write_details_results(experiment, rep, reward_vec) if(result > 0): - logging.info('+++++ Target achieved in %f seconds! +++++' % + logging.info('+++++ Target achieved in %f seconds! +++++', final_time) else: logging.info('----- Timeout! -----') From 07d2231ac3ab0955bca96320fb27d9abe09020a8 Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 11:01:13 +0100 Subject: [PATCH 53/59] removing verbose label --- PythonClient/carla/benchmarks/benchmark.py | 6 +++--- PythonClient/run_benchmark.py | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/PythonClient/carla/benchmarks/benchmark.py b/PythonClient/carla/benchmarks/benchmark.py index b4e3dba02..672e4f0f7 100644 --- a/PythonClient/carla/benchmarks/benchmark.py +++ b/PythonClient/carla/benchmarks/benchmark.py @@ -36,14 +36,14 @@ class Benchmark(object): city_name, name_to_save, continue_experiment=False, - save_images=False, - verbose=False): + save_images=False + ): self.__metaclass__ = abc.ABCMeta self._city_name = city_name - self._verbose = verbose + self._base_name = name_to_save diff --git a/PythonClient/run_benchmark.py b/PythonClient/run_benchmark.py index 790882600..384885ede 100755 --- a/PythonClient/run_benchmark.py +++ b/PythonClient/run_benchmark.py @@ -81,8 +81,7 @@ if __name__ == '__main__': 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, - verbose=args.verbose) + 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() From 0df68de755adbcaa1c66efff37f6ee2453fc1f89 Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 15:30:54 +0100 Subject: [PATCH 54/59] Fixing links --- Docs/benchmark.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index 2dc8ff2b9..dd4a1ffed 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -61,7 +61,7 @@ To be benchmarked, an Agent subclass must redefine the *run_step* function as it 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](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [target](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md), [sensor_data](https://github.com/carla-simulator/carla/blob/master/Docs/cameras_and_sensors.m) and [control](https://github.com/carla-simulator/carla/blob/master/Docs/measurements.md) types are described at the documentation. +The [measurements](measurements.md), [target](measurements.md), [sensor_data](cameras_and_sensors.m) and [control](measurements.md) types are described at the documentation. From d700a388d6341bd5d76ce429e6b8827654977501 Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 15:39:59 +0100 Subject: [PATCH 55/59] Update benchmark.md --- Docs/benchmark.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index dd4a1ffed..c737beb08 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -41,7 +41,7 @@ The benchmark works by calling three lines of code 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/benchmark_branch/PythonClient/run_benchmark.py) example. +This is excerpt is executed in the [run_benchmark.py](https://github.com/carla-simulator/carla/blob/master/PythonClient/run_benchmark.py) example. First a *benchmark* object is defined, for this case, a CoRL2017 benchmark. This is object is used to benchmark a certain Agent.
On the second line of our sample code, there is an object of a Manual class instanced. This class inherited an Agent base class @@ -61,7 +61,7 @@ To be benchmarked, an Agent subclass must redefine the *run_step* function as it 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.m) and [control](measurements.md) types are described at the documentation. +The [measurements](measurements.md), [target](measurements#player-measurements.md), [sensor_data](cameras_and_sensors.md) and [control](measurements.md) types are described at the documentation. From 16b4679c1002c4344c790061af36e74d5be144de Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 15:40:35 +0100 Subject: [PATCH 56/59] Update benchmark.md --- Docs/benchmark.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index c737beb08..c73503521 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -61,7 +61,7 @@ To be benchmarked, an Agent subclass must redefine the *run_step* function as it 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#player-measurements.md), [sensor_data](cameras_and_sensors.md) and [control](measurements.md) types are described at the documentation. +The [measurements](measurements.md), [target](measurements.md), [sensor_data](cameras_and_sensors.md) and [control](measurements.md) types are described at the documentation. From bb797c8700bba7dc6d210b8a235425e494fbc4b2 Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 15:41:22 +0100 Subject: [PATCH 57/59] Update benchmark.md --- Docs/benchmark.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index c73503521..e27d850cb 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -61,7 +61,7 @@ To be benchmarked, an Agent subclass must redefine the *run_step* function as it 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 at the documentation. +The [measurements](measurements.md), [target](measurements.md), [sensor_data](cameras_and_sensors.md) and [control](measurements.md) types are described on the documentation. From a991e53059050e712c14ff308ed5ae7179bfc36f Mon Sep 17 00:00:00 2001 From: felipecode Date: Mon, 22 Jan 2018 15:41:39 +0100 Subject: [PATCH 58/59] Update benchmark.md --- Docs/benchmark.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Docs/benchmark.md b/Docs/benchmark.md index e27d850cb..b42d6eafa 100644 --- a/Docs/benchmark.md +++ b/Docs/benchmark.md @@ -1,10 +1,6 @@ CARLA Benchmark =============== -!!! important - Benchmark code is currently in beta and can be found only in the - `benchmark_branch`. - Running the Benchmark --------------------- From 438fd3baced8a5cc34e7fc377b9bdb8677081fb5 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Mon, 22 Jan 2018 16:06:28 +0100 Subject: [PATCH 59/59] Remove repeated package --- PythonClient/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/setup.py b/PythonClient/setup.py index 31e26f2de..7969450a6 100644 --- a/PythonClient/setup.py +++ b/PythonClient/setup.py @@ -5,7 +5,7 @@ from setuptools import setup setup( name='carla_client', version='0.7.1', - packages=['carla', 'carla.planner', 'carla.benchmarks', 'carla.planner'], + 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',