Adding the planner
This commit is contained in:
felipecode
parent
f36bd466c2
commit
35b7229b90
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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])
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -12,6 +20,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,11 +88,17 @@ 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),
|
||||
self._map.get_walls_directed(node_source,source_ori,
|
||||
node_target,target_ori),node_source,
|
||||
node_target)
|
||||
|
||||
|
||||
|
|
@ -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,9 +133,10 @@ 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_iter in route:
|
||||
for node_iterator in self._map._graph.intersection_nodes():
|
||||
|
||||
distance_vector.append(sldist(node_iterator, current_node))
|
||||
|
|
@ -118,4 +145,19 @@ 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:
|
||||
|
||||
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]
|
||||
|
||||
|
|
|
|||
|
|
@ -3,74 +3,72 @@ 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)
|
||||
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 = {}
|
||||
self.node_density =50
|
||||
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)
|
||||
# Skipe the first four lines that
|
||||
lines_after_4 = file.readlines()[4:]
|
||||
|
||||
# the graph resolution.
|
||||
linegraphres = file.readline()
|
||||
linegraphres = lines_after_4[0]
|
||||
self._resolution = string_to_node(linegraphres)
|
||||
for line in file:
|
||||
|
||||
for line in lines_after_4[1:]:
|
||||
|
||||
from_node, to_node, d = line.split()
|
||||
from_node = string_to_node(from_node)
|
||||
to_node = string_to_node(to_node)
|
||||
|
||||
if from_node not in self.nodes:
|
||||
if from_node not in self._nodes:
|
||||
self.add_node(from_node)
|
||||
if to_node not in self.nodes:
|
||||
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)
|
||||
|
||||
|
||||
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)
|
||||
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)
|
||||
|
||||
|
|
@ -78,16 +76,14 @@ class Graph(object):
|
|||
|
||||
import collections
|
||||
distance_dic = {}
|
||||
for node_iter in self.nodes:
|
||||
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()))
|
||||
|
||||
|
||||
|
||||
distance_dic = collections.OrderedDict(sorted(distance_dic.items()))
|
||||
|
||||
self.angles[node ] = heading
|
||||
self._angles[node] = heading
|
||||
for k, v in distance_dic.iteritems():
|
||||
|
||||
# print k
|
||||
|
|
@ -99,21 +95,19 @@ class Graph(object):
|
|||
|
||||
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
|
||||
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:
|
||||
for node in self._nodes:
|
||||
if len(self._edges[node]) > 2:
|
||||
intersect_nodes.append(node)
|
||||
|
||||
return intersect_nodes
|
||||
|
|
@ -122,40 +116,28 @@ class Graph(object):
|
|||
|
||||
def turn_nodes(self):
|
||||
|
||||
|
||||
return self.nodes
|
||||
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]
|
||||
|
||||
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]
|
||||
|
||||
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]
|
||||
|
||||
xs = [p[0] for p in self._nodes]
|
||||
ys = [p[1] for p in self._nodes]
|
||||
|
||||
plt.scatter(xs, ys, color=c)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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,7 +72,7 @@ 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
|
||||
|
|
@ -75,7 +80,7 @@ class CarlaMap(object):
|
|||
|
||||
self._graph = Graph(city_file)
|
||||
|
||||
self._grid = Grid()
|
||||
self._grid = Grid(self._graph)
|
||||
|
||||
with open(city_file, 'r') as file:
|
||||
|
||||
|
|
@ -166,7 +171,6 @@ class CarlaMap(object):
|
|||
|
||||
|
||||
|
||||
|
||||
def get_position_on_world(self, pixel):
|
||||
"""Get world position of a certain map position."""
|
||||
relative_location = []
|
||||
|
|
@ -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 get_walls_directed(self,node_source,source_ori,node_target,target_ori):
|
||||
"""
|
||||
def make_node(self, worldvertex):
|
||||
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.
|
||||
|
||||
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):
|
||||
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
|
||||
|
||||
# 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]
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
"""
|
||||
|
|
@ -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:
|
||||
Loading…
Reference in New Issue