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