def main_middle(sx, sy, syaw, mx, my, myaw, other_sx, other_sy, other_syaw, other_gx, other_gy, other_gyaw, other_other_x=None, other_other_y=None, other_other_yaw=None): # set obstacles global ox, oy oox = ox[:] ooy = oy[:] tox, toy = ox[:], oy[:] kdtree = planner.KDTree(np.vstack((tox, toy)).T) if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw], kdtree) == False: return None, None, None vehicle_x, vehicle_y = planner.vehicle.plot_trailer( mx, my, myaw, 0.0, True) oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy) path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw, other_gx, other_gy, other_gyaw, oox_add, ooy_add, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path2 == []: print 'The middle state is invalid(2)' return None, None, None # path generation path1 = planner.calc_hybrid_astar_path(sx, sy, syaw, mx, my, myaw, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path1 == []: print 'Cannot find path.(1)' return None, None, None vehicle.plot_trailer(other_sx, other_sy, other_syaw, 0) show_animation(path1, ox, oy, other_sx, other_sy, other_syaw, other_other_x, other_other_y, other_other_yaw) show_animation(path2, ox, oy, mx, my, myaw, other_other_x, other_other_y, other_other_yaw) plt.pause(0.01) return mx, my, myaw
def main(): s = [-14, 7, 0] g = [0, 0, math.pi / 2] ox, oy = make_ob() path = planner.calc_hybrid_astar_path(s[0], s[1], s[2], g[0], g[1], g[2], ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) fig, ax = plt.subplots() plt.axis("equal") plt.subplots_adjust(bottom=0.35) ax_x = plt.axes([0.1, 0.25, 0.8, 0.03], facecolor='lightgoldenrodyellow') ax_y = plt.axes([0.1, 0.20, 0.8, 0.03], facecolor='lightgoldenrodyellow') ax_yaw = plt.axes([0.1, 0.15, 0.8, 0.03], facecolor='lightgoldenrodyellow') ax_index = plt.axes([0.1, 0.10, 0.8, 0.03], facecolor='lightgoldenrodyellow') s_x = Slider(ax_x, 'x', -20.0, 20.0, valinit=s[0]) s_y = Slider(ax_y, 'y', 5.0, 10.0, valinit=s[1]) s_yaw = Slider(ax_yaw, 'yaw', -math.pi, math.pi, valinit=s[2]) s_index = Slider(ax_index, 'index', 0, 1.0, valinit=0) ax.plot(ox, oy, 'ko') plot_vehicle(ax, s[0], s[1], s[2]) plot_vehicle(ax, g[0], g[1], g[2]) def update_index(val): global path ax.cla() ax.plot(ox, oy, 'ko') x = s_x.val y = s_y.val yaw = s_yaw.val index = s_index.val plot_vehicle(ax, x, y, yaw) plot_vehicle(ax, g[0], g[1], g[2]) _idx = max(min(int(len(path.x) * index), len(path.x) - 1), 0) plot_vehicle(ax, path.x[_idx], path.y[_idx], path.yaw[_idx]) ax.plot(path.x, path.y, 'r-') fig.canvas.draw_idle() def update(val): global path ax.cla() x = s_x.val y = s_y.val yaw = s_yaw.val index = s_index.val ax.plot(ox, oy, 'ko') plot_vehicle(ax, x, y, yaw) plot_vehicle(ax, g[0], g[1], g[2]) path = planner.calc_hybrid_astar_path(x, y, yaw, g[0], g[1], g[2], ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) ax.plot(path.x, path.y, 'r-') fig.canvas.draw_idle() s_x.on_changed(update) s_y.on_changed(update) s_yaw.on_changed(update) s_index.on_changed(update_index) plt.show()
def update(val): global path ax.cla() x = s_x.val y = s_y.val yaw = s_yaw.val index = s_index.val ax.plot(ox, oy, 'ko') plot_vehicle(ax, x, y, yaw) plot_vehicle(ax, g[0], g[1], g[2]) path = planner.calc_hybrid_astar_path(x, y, yaw, g[0], g[1], g[2], ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) ax.plot(path.x, path.y, 'r-') fig.canvas.draw_idle()
def main(x, y, yaw): sx = SX # [m] sy = SY # [m] syaw0 = SYAW # goal state gx = GX # [m] gy = GY # [m] gyaw0 = GYAW other_sx = OTHER_SX # [m] other_sy = OTHER_SY # [m] other_syaw0 = OTHER_SYAW other_gx = OTHER_GX # [m] other_gy = OTHER_GY # [m] other_gyaw0 = OTHER_GYAW mx = x # [m] my = y # [m] myaw0 = yaw # set obstacles global ox, oy oox = ox[:] ooy = oy[:] tox, toy = ox[:], oy[:] kdtree = planner.KDTree(np.vstack((tox, toy)).T) if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw0], kdtree) == False: return # path generation vehicle_x, vehicle_y = planner.vehicle.plot_trailer( mx, my, myaw0, 0.0, True) oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy) path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw0, other_gx, other_gy, other_gyaw0, oox_add, ooy_add, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path2 == []: print('The middle state is invalid(2)') return path1 = planner.calc_hybrid_astar_path(sx, sy, syaw0, mx, my, myaw0, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path1 == []: print('Cannot find path.(1)') return path3 = planner.calc_hybrid_astar_path(mx, my, myaw0, gx, gy, gyaw0, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path3 == []: print('Cannot find path.(3)') return # global total_cost # global final_path1 # global final_path2 # global final_path3,final_mx,final_my,final_myaw0 # # if path1.cost + path2.cost + path3.cost < total_cost: # final_path1 = path1 # final_path2 = path2 # final_path3 = path3 # final_mx = mx # final_my = my # final_myaw0 = myaw0 plt.grid(True) plt.axis("equal") plt.plot(ox, oy, '.k') plt.pause(5) show_animation(path1, ox, oy, other_sx, other_sy, other_syaw0) show_animation(path2, ox, oy, mx, my, myaw0) show_animation(path3, ox, oy, other_gx, other_gy, other_gyaw0)
print len(yield_x_2) for mx, my, myaw in zip(yield_x_2, yield_y_2, yield_yaw_2): oox, ooy = ox[:], oy[:] states = States(final_mx_1, final_my_1, final_myaw_1, mx, my, np.deg2rad(myaw), ox, oy) states_set_2[s_id] = states s_id = s_id + 1 for i in range(len(states_set_2)): s_id = min( states_set_2, key=lambda o: states_set_2[o].estimated_cost) states = states_set_2[s_id] del states_set_2[s_id] print 'mx:', states.mx, 'my:', states.my, 'myaw:', states.myaw, 'cost:', states.estimated_cost final_mx_2, final_my_2, final_myaw_2 = main_middle(final_mx_1, final_my_1, final_myaw_1, states.mx, states.my, states.myaw, OTHER_OTHER_SX, OTHER_OTHER_SY, OTHER_OTHER_SYAW, OTHER_OTHER_GX, OTHER_OTHER_GY, OTHER_OTHER_GYAW, OTHER_GX, OTHER_GY, OTHER_GYAW) if final_mx_2 != None and final_my_2 != None and final_myaw_2 != None: break oox = ox[:] ooy = oy[:] path3 = planner.calc_hybrid_astar_path(final_mx_2, final_my_2, final_myaw_2, GX, GY, GYAW, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) show_animation(path3, ox, oy, OTHER_OTHER_GX, OTHER_OTHER_GY, OTHER_OTHER_GYAW, OTHER_GX, OTHER_GY, OTHER_GYAW)
def main(x, y, yaw): sx = SX # [m] sy = SY # [m] syaw0 = SYAW # goal state gx = GX # [m] gy = GY # [m] gyaw0 = GYAW other_sx = OTHER_SX # [m] other_sy = OTHER_SY # [m] other_syaw0 = OTHER_SYAW other_gx = OTHER_GX # [m] other_gy = OTHER_GY # [m] other_gyaw0 = OTHER_GYAW mx = x # [m] my = y # [m] myaw0 = np.deg2rad(yaw) # set obstacles ox = [] oy = [] # for i in range(-8, 8): # ox.append(i) # oy.append(12.0) for i in range(-25, -8): ox.append(i) oy.append(9.0) for i in range(8, 25): ox.append(i) oy.append(9.0) for i in range(-25, -8): ox.append(i) oy.append(4.0) for i in range(-3, 5): ox.append(-8.0) oy.append(i) for i in range(-3, 5): ox.append(8.0) oy.append(i) for i in range(8, 25): ox.append(i) oy.append(4.0) # for i in range(-8, 8): # ox.append(i) # oy.append(1.0) for i in range(-8, 8): ox.append(i) oy.append(12.0) for i in range(4, 9): ox.append(-25) oy.append(i) for i in range(4, 9): ox.append(25) oy.append(i) oox = ox[:] ooy = oy[:] tox, toy = ox[:], oy[:] kdtree = planner.KDTree(np.vstack((tox, toy)).T) # plt.plot(ox,oy,'.k') # plt.show() if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw0], kdtree) == False: return # path generation path1 = planner.calc_hybrid_astar_path(sx, sy, syaw0, mx, my, myaw0, ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path1 == []: print 'Cannot find path.(1)' return vehicle_x, vehicle_y = planner.vehicle.plot_trailer( mx, my, myaw0, 0.0, True) oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy) path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw0, other_gx, other_gy, other_gyaw0, oox_add, ooy_add, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path2 == []: print 'The middle state is invalid(2)' return path3 = planner.calc_hybrid_astar_path(mx, my, myaw0, gx, gy, gyaw0, ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path3 == []: print 'Cannot find path.(3)' return # global total_cost # global final_path1 # global final_path2 # global final_path3,final_mx,final_my,final_myaw0 # # if path1.cost + path2.cost + path3.cost < total_cost: # final_path1 = path1 # final_path2 = path2 # final_path3 = path3 # final_mx = mx # final_my = my # final_myaw0 = myaw0 vehicle.plot_trailer(other_sx, other_sy, other_syaw0, 0) show_animation(path1, oox, ooy, other_sx, other_sy, other_syaw0) show_animation(path2, oox, ooy, mx, my, myaw0) show_animation(path3, oox, ooy, other_gx, other_gy, other_gyaw0) plt.show()
for i in range(61): ox.append(i) oy.append(60.0) for i in range(61): ox.append(0.0) oy.append(i) for i in range(20): ox.append(i) oy.append(30.0) for i in range(45): ox.append(i) oy.append(40.0) plt.grid(True) plt.axis("equal") plt.plot(ox, oy, '.k') vehicle.plot_trailer(SX, SY, SYAW, 0) path1 = planner.calc_hybrid_astar_path(SX, SY, SYAW, GX, GY, GYAW, ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) #plt.scatter(path1.x, path1.y, "-r", label="Hybrid A* path") show_animation(path1, ox, oy) # #plt.scatter(x_path,y_path) plt.axis([-40, 40, -40, 40]) plt.grid(True) plt.axis("equal") # plt.plot(ox, oy, '.k') plt.show()