def main():
arr_in = np.array(list(path_parser.read_points(ruta)))
ax, ay = arr_in.T
min_x = np.min(ax)
min_y = np.min(ay)
max_x = np.max(ax)
max_y = np.max(ay)
print 'Minimo en x', min_x
print 'Minimo en y', min_y
print 'Maximo en x', max_x
print 'Maximo en y', max_y
if (min_x > 0 and min_y > 0):
offsetg = 0
else:
if min_x > min_y:
offsetg = min_x
else:
offsetg = min_y
scale_x = 1
scale_y = 1
scale = np.array([scale_x, scale_y])
xy = np.multiply(scale, arr_in) + np.array([0.0, 0.0])
x, y = xy.T
fig = plt.figure(figsize=(7, 7), facecolor='w')
fig.canvas.set_window_title('Trayectoria')
plt.plot(x, y)
tree = KDTree(xy)
plt.plot(x, y, ':o', markersize=4)
plt.tight_layout(
) #Ajusta los titulos de subplots para evitar que salgan de la figura.
print('please wait ...')
X = np.arange(0, map_size_x / 100, resolution / 100)
Y = np.arange(0, map_size_y / 100, resolution / 100)
X, Y = np.meshgrid(X, Y)
lim_x = int(map_size_x / resolution)
lim_y = int(map_size_y / resolution)
print lim_x, lim_y
fig = plt.figure(figsize=(7, 7), facecolor='w')
fig.canvas.set_window_title('Puntos de prueba')
ax = plt.axes()
for xi in range(0, lim_x):
print float(xi) / lim_x * 100
for yi in range(0, lim_y):
#show_nearest((x,y))
near((xi * resolution / 100, yi * resolution / 100), xi, yi, tree,
xy, ax)
Z = matrix_dist
fig = plt.figure(figsize=(7, 7), facecolor='w')
ax = fig.gca(projection='3d')
fig.canvas.set_window_title('Distancias')
surf = ax.plot_surface(X,
Y,
Z,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
plt.show()
np.save('TrayD1.npy', matrix)
print('matrixForce is saved.')
def read_map(map_file):
"""
Read the file with the map data and return a Trajectory.
:return: List of Pose elements
"""
points = []
for x, y in read_points(map_file):
xy_point = Point(x, y, 0)
xy_pose = Pose(position=xy_point)
points.append(xy_pose)
return points
def read_map(map_file):
"""
Read the file with the map data and return a Trajectory.
:return: List of Pose elements
"""
points = []
for x, y in read_points(map_file):
xy_point = Point(x, y, 0)
xy_pose = Pose(position=xy_point)
points.append(xy_pose)
return points
def main(map_file):
xy = np.multiply(np.array([0.833, 0.75]),
np.array(list(path_parser.read_points(map_file))))
print(xy)
x, y = xy.T
plt.plot(x, y)
tree = KDTree(xy)
fig = plt.figure(figsize=(12, 10), facecolor='w')
plt.plot(x, y, ':o', markersize=2)
plt.tight_layout(
) #Ajusta los titulos de subplots para evitar que salgan de la figura.
def show_nearest(target):
dist, index = tree.query(
target) #Obtiene los puntos mas cercanos al camino
global lookahead_offset
lookahead_offset = np.int(2 + (5 / (5 * dist + 1)))
lookahead_target = xy[(index + lookahead_offset) % len(xy)]
x1, y1 = target
x3, y3 = lookahead_target
plt.scatter(*target, color='r')
plt.scatter(*xy[index], color='g')
ax = plt.axes()
ax.arrow(x1,
y1, (x3 - x1) / 5, (y3 - y1) / 5,
head_width=0.01,
head_length=0.01,
fc='k',
ec='k')
plt.scatter(*lookahead_target, color='m')
plt.show(block=False)
global matrix
x_index = np.int(x1 * 10)
y_index = np.int(y1 * 10)
matrix[x_index, y_index, 0] = x3 - x1
matrix[x_index, y_index, 1] = y3 - y1
print('please wait ...')
for x in range(0, map_size_x / resolution):
for y in range(0, map_size_y / resolution):
show_nearest((x * 5 * resolution / map_size_x,
y * 4 * resolution / map_size_y))
#np.save('matrixDynamic_tt_1.npy', matrix)
#print('matrix is saved.')
plt.show()
def main(map_file):
xy = np.array(list(path_parser.read_points(map_file)))
x, y = xy.T
tree = KDTree(xy)
fig = plt.figure(figsize=(12, 10), facecolor='w')
plt.plot(x, y, ':o', markersize=2)
plt.tight_layout()
def show_nearest(target):
dist, index = tree.query(target)
global lookahead_offset
lookahead_offset = 20 #np.int(2 + (8/(8*dist+1)))
lookahead_target = xy[(index + lookahead_offset) % len(xy)]
x1, y1 = target
x3, y3 = lookahead_target
plt.scatter(*target, color='r')
plt.scatter(*xy[index], color='g')
ax = plt.axes()
ax.arrow(x1,
y1, (x3 - x1) / 5, (y3 - y1) / 5,
head_width=0.01,
head_length=0.01,
fc='k',
ec='k')
plt.scatter(*lookahead_target, color='m')
plt.show(block=False)
global matrix
x_index = np.int(x1 * 10)
y_index = np.int(y1 * 10)
matrix[x_index, y_index, 0] = x3 - x1
matrix[x_index, y_index, 1] = y3 - y1
print('please wait ...')
for x in range(0, map_size_x / resolution):
for y in range(0, map_size_y / resolution):
show_nearest((0.1 * x, 0.1 * y))
np.save('matrix100cm_lane2.npy', matrix)
print('matrix is saved.')
plt.show()
def main(map_file):
xy = np.array(list(path_parser.read_points(map_file)))
x, y = xy.T
tree = KDTree(xy)
fig = plt.figure(figsize=(12, 10), facecolor='w')
plt.plot(x, y, ':o', markersize=2)
plt.tight_layout()
def show_nearest(target):
dist, index = tree.query(target)
global lookahead_offset
lookahead_offset = np.int(2 + (5/(5*dist+1)))
lookahead_target = xy[(index + lookahead_offset) % len(xy)]
x1, y1 = target
x3, y3 = lookahead_target
plt.scatter(*target, color='r')
plt.scatter(*xy[index], color='g')
ax = plt.axes()
ax.arrow(x1, y1, (x3-x1)/5, (y3-y1)/5 , head_width=0.01, head_length=0.01, fc='k', ec='k')
plt.scatter(*lookahead_target, color='m')
plt.show(block=False)
global matrix
x_index=np.int(x1*10)
y_index=np.int(y1*10)
matrix[x_index,y_index,0]=x3-x1
matrix[x_index,y_index,1]=y3-y1
print('please wait ...')
for x in range(0, map_size_x/resolution):
for y in range(0, map_size_y/resolution):
show_nearest((0.1*x, 0.1*y))
np.save('matrixDynamic_lane2.npy', matrix)
print('matrix is saved.')
plt.show()
def main():
arr_in = np.array(list(path_parser.read_points(ruta)))
ld = 0.75
min_dist = 0.025
ax, ay = arr_in.T
min_x = np.min(ax)
min_y = np.min(ay)
max_x = np.max(ax)
max_y = np.max(ay)
print 'Minimo en x', min_x
print 'Minimo en y', min_y
print 'Maximo en x', max_x
print 'Maximo en y', max_y
lenx = (max_x - min_x)
leny = (max_y - min_y)
offsetx = -(max_x - lenx / 2)
offsety = -(max_y - leny / 2)
arr_in = arr_in + np.array([offsetx, offsety])
#Escalar
ax, ay = arr_in.T
min_x = np.min(ax)
min_y = np.min(ay)
max_x = np.max(ax)
max_y = np.max(ay)
print 'Minimo en x', min_x
print 'Minimo en y', min_y
print 'Maximo en x', max_x
print 'Maximo en y', max_y
lenx = (max_x - min_x)
leny = (max_y - min_y)
scale_x = 2 * ld / (lenx)
scale_y = 2 * ld / (leny)
scale = np.array([scale_x, scale_y])
xy = np.multiply(scale, arr_in) + np.array([(map_size_x / 2) / 100,
(map_size_y / 2) / 100])
print xy
x, y = xy.T
l = len(x)
xo = x[0]
yo = y[0]
xl = [xo]
yl = [yo]
for i in range(l):
xf = x[i]
yf = y[i]
dist = np.sqrt((xf - xo)**2 + (yf - yo)**2)
if dist > min_dist:
xo = xf
yo = yf
xl.append(xf)
yl.append(yf)
x1 = np.array(xl)
y1 = np.array(yl)
xy = np.array([x1, y1]).T
print xy
fig = plt.figure(figsize=(7, 7), facecolor='w')
fig.canvas.set_window_title('Trayectoria')
plt.plot(x1 - (map_size_x / 2) / 100, y1 - (map_size_y / 2) / 100)
plt.plot(x1 - (map_size_x / 2) / 100,
y1 - (map_size_y / 2) / 100,
':o',
markersize=4)
plt.tight_layout(
) #Ajusta los titulos de subplots para evitar que salgan de la figura. np.array([(map_size_x/2)/100,(map_size_y/2)/100])
plt.axis([-1, 1, -1, 1])
plt.show()
tree = KDTree(xy)
print('please wait ...')
X = np.arange(0, map_size_x / 100, resolution / 100)
Y = np.arange(0, map_size_y / 100, resolution / 100)
X, Y = np.meshgrid(X, Y)
lim_x = int(map_size_x / resolution)
lim_y = int(map_size_y / resolution)
print lim_x, lim_y
fig = plt.figure(figsize=(7, 7), facecolor='w')
fig.canvas.set_window_title('Puntos de prueba')
ax = plt.axes()
for xi in range(0, lim_x):
print float(xi) / lim_x * 100
for yi in range(0, lim_y):
#show_nearest((x,y))
near((xi * resolution / 100, yi * resolution / 100), xi, yi, tree,
xy, ax)
Z = matrix_dist
fig = plt.figure(figsize=(7, 7), facecolor='w')
ax = fig.gca(projection='3d')
fig.canvas.set_window_title('Distancias')
surf = ax.plot_surface(X,
Y,
Z,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
plt.show()
np.save(nombre, matrix)
print('matrixForce is saved.')
def main(map_file):
xy = np.array(list(path_parser.read_points(map_file)))
x, y = xy.T
fig = plt.figure(figsize=(12, 10), facecolor='w')
plt.plot(x, y, ':o', markersize=2)
global matrix
matrix = np.load('matrix1.npy')
plt.gca().set_aspect(1, 'datalim') # keep circles as circles
plt.tight_layout()
def show_nearest(target):
yaw = 0 #np.pi/2
car_length = 0.3
global matrix
x1, y1 = target
print(x1, y1)
x_index = np.int(x1 * resolution)
y_index = np.int(y1 * resolution)
print(x_index, y_index)
x3, y3 = matrix[x_index, y_index, :]
print(x3, y3)
f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3
f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3
steering = np.arctan2(f_y, f_x)
if (steering > (np.pi) / 4):
steering = (np.pi) / 4
if (steering < -(np.pi) / 4):
steering = -(np.pi) / 4
print(steering)
r = car_length * np.abs(np.tan((np.pi) / 2 - steering))
if (r > 10):
r = 10
print(r)
if (steering < 0.0):
r = -r
xc = x1 - np.sin(yaw) * r
yc = y1 + np.cos(yaw) * r
ax = plt.axes()
ax.arrow(x1,
y1,
np.cos(yaw) * car_length,
np.sin(yaw) * car_length,
head_width=0.05,
head_length=0.1,
fc='r',
ec='r')
plt.scatter(*target, color='r')
plt.scatter(*(x1 + x3, y1 + y3), color='g')
circ = plt.Circle((xc, yc), r, color='r', fill=False)
plt.gcf().gca().add_artist(circ)
plt.show(block=False)
def onclick(event):
show_nearest((event.xdata, event.ydata))
fig.canvas.mpl_connect('button_press_event', onclick)
plt.show()
def main(map_file):
xy = np.array(list(path_parser.read_points(map_file)))
x, y = xy.T
fig = plt.figure(figsize=(12, 10), facecolor='w')
plt.plot(x, y, ':o', markersize=2)
global matrix
matrix = np.load('matrixDynamic_lane2.npy')
plt.gca().set_aspect(1, 'datalim') # keep circles as circles
plt.tight_layout()
def show_nearest(target):
#global yaw
#yaw+=np.pi/3 #np.pi/2
car_length = 0.3
global matrix
x1, y1, yaw = target
print(x1, y1)
x_index = np.int(x1 * resolution)
y_index = np.int(y1 * resolution)
print(x_index, y_index)
x3, y3 = matrix[x_index, y_index, :]
f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3
f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3
Kp = 4
steering = Kp * np.arctan(f_y / (4.0 * f_x))
if (f_x > 0):
speed = -150
else:
speed = 150
if (f_y > 0):
steering = -np.pi / 2
if (f_y < 0):
steering = np.pi / 2
if (steering > (np.pi) / 4):
steering = (np.pi) / 4
if (steering < -(np.pi) / 4):
steering = -(np.pi) / 4
print(steering)
r = car_length * np.abs(np.tan((np.pi) / 2 - steering))
if (r > 10):
r = 10
print(r)
if (steering < 0.0):
r = -r
xc = x1 - np.sin(yaw) * r
yc = y1 + np.cos(yaw) * r
ax = plt.axes()
ax.arrow(x1,
y1,
car_length * np.cos(yaw),
car_length * np.sin(yaw),
width=car_length,
head_width=car_length,
head_length=0.09,
fc='b',
ec='b')
ax = plt.axes()
ax.arrow(x1,
y1,
f_x * np.cos(yaw),
f_x * np.sin(yaw),
head_width=0.01,
head_length=0.01,
fc='r',
ec='r')
ax = plt.axes()
ax.arrow(x1,
y1,
-f_y * np.sin(yaw),
f_y * np.cos(yaw),
head_width=0.01,
head_length=0.01,
fc='r',
ec='r')
ax = plt.axes()
ax.arrow(x1,
y1,
x3,
y3,
head_width=0.01,
head_length=0.01,
fc='g',
ec='g')
plt.scatter(*target, color='r')
plt.scatter(*(x1 + x3, y1 + y3), color='g')
circ = plt.Circle((xc, yc), r, color='r', fill=False)
plt.gcf().gca().add_artist(circ)
plt.show(block=False)
print "x,y: ", (x1, y1)
print "YAW: ", rad_to_deg(yaw)
print "steering: ", rad_to_deg(steering)
print "Fx: ", f_x
print "Fy: ", f_y
print ""
#plot for three diffrent locations:
show_nearest((3.0824503810121322, 3.4432021842017404, -3.428323751982624))
show_nearest(
(2.4863752403013173, 2.5317883165130555, -0.71667885419037469))
show_nearest((3.9662325573582558, 0.6006136280417349, -2.8916016620543497))
plt.show()
def main(map_file):
xy = np.array(list(path_parser.read_points(map_file)))
x, y = xy.T
fig = plt.figure(figsize=(12, 10), facecolor='w')
plt.plot(x, y, ':o', markersize=2)
global matrix
matrix = np.load('matrixDynamic.npy')
plt.gca().set_aspect(1, 'datalim') # keep circles as circles
plt.tight_layout()
def show_nearest(target):
global yaw
yaw+=np.pi/3 #np.pi/2
car_length=0.3
global matrix
x1, y1 = target
print(x1,y1)
x_index=np.int(x1*resolution)
y_index=np.int(y1*resolution)
print(x_index,y_index)
x3, y3 = matrix[x_index,y_index,:]
f_x=np.cos(yaw)*x3 + np.sin(yaw)*y3
f_y=-np.sin(yaw)*x3 + np.cos(yaw)*y3
Kp=4
steering=Kp*np.arctan(f_y/(4.0*f_x))
if (f_x>0):
speed = -150
else:
speed = 150
if (f_y>0):
steering = -np.pi/2
if (f_y<0):
steering = np.pi/2
if (steering>(np.pi)/4):
steering = (np.pi)/4
if (steering<-(np.pi)/4):
steering = -(np.pi)/4
print(steering)
r = car_length * np.abs(np.tan((np.pi)/2-steering))
if (r>10):
r = 10
print(r)
if (steering<0.0):
r=-r
xc = x1 - np.sin(yaw) * r
yc = y1 + np.cos(yaw) * r
ax = plt.axes()
ax.arrow(x1, y1, car_length*np.cos(yaw), car_length*np.sin(yaw), width=car_length, head_width=car_length, head_length=0.09, fc='b', ec='b')
ax = plt.axes()
ax.arrow(x1, y1, f_x*np.cos(yaw), f_x*np.sin(yaw), head_width=0.01, head_length=0.01, fc='r', ec='r')
ax = plt.axes()
ax.arrow(x1, y1, -f_y*np.sin(yaw), f_y*np.cos(yaw), head_width=0.01, head_length=0.01, fc='r', ec='r')
ax = plt.axes()
ax.arrow(x1, y1, x3, y3, head_width=0.01, head_length=0.01, fc='g', ec='g')
plt.scatter(*target, color='r')
plt.scatter(*(x1 + x3, y1 + y3), color='g')
circ = plt.Circle((xc, yc), r, color='r', fill=False)
plt.gcf().gca().add_artist(circ)
plt.show(block=False)
def onclick(event):
show_nearest((event.xdata, event.ydata))
fig.canvas.mpl_connect('button_press_event', onclick)
plt.show()
def AjustarDatos():
ruta = os.path.dirname(
os.path.abspath(__file__)
) + '/Generacion_de_puntos_de_trayectoria/Archivos_de_puntos/' # Obtener la ruta del archivo.
arr_in = np.array(list(
path_parser.read_points(ruta +
puntos))) # Guarda los puntos en un arreglo.
ax, ay = arr_in.T # Separa los puntos en 2 vectores (x,y).
min_x, min_y, max_x, max_y = minymax(
ax, ay
) # Calcula los mínimos y maximos de las coordenadas en "x" y en "y".
# Imprime los valores.
Imprimir_valores(min_x, min_y, max_x, max_y) # Imprime los valores.
lenx, leny = [(max_x - min_x), (max_y - min_y)
] # Calcula la longitud (maximo-minimo) en "x" y en "y".
offsetx, offsety = [-(max_x - lenx / 2), -(max_y - leny / 2)
] # Distancia para centrar en "x" y en "y".
arr_in = arr_in + np.array([offsetx, offsety
]) # Centra la trayectoria en (0,0).
# Escalar
ax, ay = arr_in.T
min_x, min_y, max_x, max_y = minymax(
ax, ay
) # Calcula los mínimos y maximos de las coordenadas en "x" y en "y".
Imprimir_valores(min_x, min_y, max_x, max_y) # Imprime los valores.
lenx, leny = [(max_x - min_x), (max_y - min_y)
] # Calcula la longitud (maximo-minimo) en "x" y en "y".
scale_x, scale_y = [
(map_size_x - 2 * margen) / (100 * lenx),
(map_size_y - 2 * margen) / (100 * leny)
] # Calcula los valores de escalamiento para ajustarse al mapa.
scale = np.array([scale_x, scale_y]) # Valor de escala.
xy = np.multiply(scale, arr_in) + np.array([
(map_size_x / 2) / 100, (map_size_y / 2) / 100
]) #Crea los puntos de Trayectoria escalados al tamaño del mapa.
x, y = xy.T # Obtiene los valores de "x" y "y" ya ajustados al tamaño del mapa.
l, xo, yo = [
len(x), x[0], y[0]
] # Obtiene los valores iniciales de los puntos de trayectoria y la cantidad de puntos.
# Crea una lista de puntos para ir guardando los puntos que cumplan con la distancia mínima.
xl = [xo]
yl = [yo]
for i in range(l):
xf, yf = [
x[i], y[i]
] # Propone como punto final el punto seleccionado por el ciclo.
dist = np.sqrt(
(xf - xo)**2 +
(yf - yo)**2) # Calcula la distancia entre los 2 puntos.
if dist > min_dist: # Si la distancia es mayor a la mínima la guarda el arreglo de puntos de trayectoria a seguir.
xo, yo = [xf, yf] # Actualiza el punto inicial.
# Guarda el punto en la lista.
xl.append(xf)
yl.append(yf)
dist = np.sqrt(
(x[0] - xf)**2 + (y[0] - yf)**2
) # Calcula la distancia entre el punto inicial del arreglo y el ultimo seleccionado.
if dist < min_dist: # Si la distancia es menor a la mínima lo elimina de la lista.
xl.pop() # Elimina de la lista el punto en x
yl.pop() # Elimina de la lista el punto en y
x1, y1 = [np.array(xl), np.array(yl)
] # Convierte y guarda en vectores los puntos a seguir.
xy = np.array(
[x1, y1]
).T # Guarda los puntos en un solo arreglo para posteriormente obtneer su árbol kd.
xyA = np.array([
x1 - (map_size_x / 2) / 100, y1 - (map_size_y / 2) / 100
]).T # Obtiene los puntos sin el offset para guardarlos en un archivo.
ruta = os.path.dirname(
os.path.abspath(__file__)
) + '/Archivos_de_Puntos_Ajustados/' # Obtener la ruta para guardar los puntos.
np.save(ruta + nombrep, xyA) # Guarda los puntos a seguir.
print('Puntos de ruta salvados')
fig = plt.figure(figsize=(7, 7),
facecolor='w') # Crea la figura para el plot.
fig.canvas.set_window_title(
'Trayectoria') # Coloca el título de Trayectoria.
# Grafica los puntos.
plt.plot(map_size_x / 2 * (np.concatenate((x1, x1)) - (map_size_x / 200)),
map_size_y / 2 * (np.concatenate((y1, y1)) - (map_size_y / 200)))
plt.plot(map_size_x / 2 * (x1 - (map_size_x / 200)),
map_size_y / 2 * (y1 - (map_size_y / 200)),
':o',
markersize=4)
plt.tight_layout(
) # Ajusta los titulos de subplots para evitar que salgan de la figura.
plt.axis(np.array([-map_size_x, map_size_x, -map_size_y, map_size_y]) / 2)
ruta = os.path.dirname(
os.path.abspath(__file__)
) + '/Graficas_de_las_Trayectorias/' #Obtener la ruta para guardar las gráficas.
plt.savefig(ruta + nombreGrafica1,
bbox_inches='tight') # Guarda la imagen.
lp = splitall(os.path.abspath(__file__))[
1:-4] # Obtiene la ruta del paquete en tipo lista.
ruta = '/' # Inicializa la ruta.
for i in range(len(lp)):
ruta = ruta + str(
lp[i]) + '/' # Reconstruye la ruta en una cadena de texto.
ruta = ruta + 'gui_lider_seguidor/resources/images/ty' + num_archivo + '.png' # Obtiene la ruta total.
plt.savefig(
ruta,
bbox_inches='tight') # Guarda la imagen para ser leida en la gui.
plt.show(block=False)
plt.show() # Muestra la gráfica.
tree2 = Arbol(xyA, 30)
return xy
def main():
ruta=os.path.dirname(os.path.abspath(__file__))
arr_in=np.array(list(path_parser.read_points(ruta+puntos)))
ax,ay=arr_in.T
min_x=np.min(ax)
min_y=np.min(ay)
max_x=np.max(ax)
max_y=np.max(ay)
print 'Minimo en x',min_x
print 'Minimo en y',min_y
print 'Maximo en x',max_x
print 'Maximo en y',max_y
lenx=(max_x-min_x)
leny=(max_y-min_y)
offsetx=-(max_x-lenx/2)
offsety=-(max_y-leny/2)
arr_in = arr_in+np.array([offsetx,offsety])
#Escalar
ax,ay=arr_in.T
min_x=np.min(ax)
min_y=np.min(ay)
max_x=np.max(ax)
max_y=np.max(ay)
print 'Minimo en x',min_x
print 'Minimo en y',min_y
print 'Maximo en x',max_x
print 'Maximo en y',max_y
lenx=(max_x-min_x)
leny=(max_y-min_y)
scale_x=2*ld/(lenx)
scale_y=2*ld/(leny)
scale=np.array([scale_x,scale_y])
xy = np.multiply(scale,arr_in)+np.array([(map_size_x/2)/100,(map_size_y/2)/100])
print xy
x,y = xy.T
l=len(x)
xo=x[0]
yo=y[0]
xl=[xo]
yl=[yo]
for i in range(l):
xf=x[i]
yf=y[i]
dist=np.sqrt((xf-xo)**2+(yf-yo)**2)
if dist>min_dist:
xo=xf
yo=yf
xl.append(xf)
yl.append(yf)
dist=np.sqrt((x[0]-xf)**2+(y[0]-yf)**2)
print dist
if dist<min_dist:
xl.pop()
yl.pop()
x1=np.array(xl)
y1=np.array(yl)
xy=np.array([x1,y1]).T
xyA=np.array([x1-(map_size_x/2)/100,y1-(map_size_y/2)/100]).T
np.save(ruta+nombrep, xyA)
print('points are saved')
print xy
fig = plt.figure(figsize=(7,7), facecolor='w')
fig.canvas.set_window_title('Trayectoria')
plt.plot(x1-(map_size_x/2)/100,y1-(map_size_y/2)/100)
l=len(x1)-1
plt.plot(np.array([x1[0],x1[l]])-(map_size_x/2)/100,np.array([y1[0],y1[l]])-(map_size_y/2)/100)
plt.plot(x1-(map_size_x/2)/100,y1-(map_size_y/2)/100, ':o', markersize=4)
plt.tight_layout()#Ajusta los titulos de subplots para evitar que salgan de la figura. np.array([(map_size_x/2)/100,(map_size_y/2)/100])
plt.axis([-1,1,-1,1])
plt.show()
tree = KDTree(xy)
tree2= Arbol(xyA,30)
def main():
arr_in = np.array(list(path_parser.read_points(ruta)))
print arr_in
