p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Adding the planner

This commit is contained in:
felipecode 2017-12-13 19:20:59 +01:00
parent f36bd466c2
commit 35b7229b90
8 changed files with 385 additions and 351 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
PythonClient/carla/benchmarks/agent.py
View File

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

12
PythonClient/carla/benchmarks/benchmark.py
View File

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

74
PythonClient/carla/planner/city_track.py
View File

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

206
PythonClient/carla/planner/graph.py
View File

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

150
PythonClient/carla/planner/grid.py
View File

@ -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
return adjacent

84
PythonClient/carla/planner/map.py
View File

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

122
PythonClient/carla/planner/planner.py
View File

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

78
PythonClient/carla/planner/topology.py Normal file
View File

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