def find_path(data, grid_start, grid_goal, drone_altitude, safety_distance): # This is now the routine using Voronoi grid, edges = create_grid_and_edges(data, drone_altitude, safety_distance) # equivalent to # plt.imshow(np.flip(grid, 0)) # plt.imshow(grid, origin='lower', cmap='Greys') # # for e in edges: # p1 = e[0] # p2 = e[1] # plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-') # # plt.plot(start_ne[1], start_ne[0], 'rx') # plt.plot(goal_ne[1], goal_ne[0], 'rx') # # plt.xlabel('EAST') # plt.ylabel('NORTH') # plt.show() G = nx.Graph() for p1, p2 in edges: dist = LA.norm(np.array(p2) - np.array(p1)) G.add_edge(p1, p2, weight=dist) start_closest = closest_point(G, grid_start) print(grid_start, start_closest) goal_closest = closest_point(G, grid_goal) print(grid_goal, goal_closest) # 2. Compute the path from start to goal using A* algorithm path, cost = a_star(G, heuristic, start_closest, goal_closest) print("path :", len(path), "cost: ", cost) # insert actual start and goal positions if len(path) > 0: path.insert(0, grid_start) path.append(grid_goal) return grid, path, cost
# In[171]: start_ne = (25, 100) goal_ne = (750., 370.) # In[172]: # Static drone altitude (metres) drone_altitude = 5 safety_distance = 3 # In[173]: # This is now the routine using Voronoi grid, edges = create_grid_and_edges(data, drone_altitude, safety_distance) # print(len(edges)) # Plot the edges on top of the grid along with start and goal locations. # In[174]: # equivalent to # plt.imshow(np.flip(grid, 0)) plt.imshow(grid, origin='lower', cmap='Greys') for e in edges: p1 = e[0] p2 = e[1] plt.plot([p1[1], p2[1]], [p1[0], p2[0]], 'b-')
close_goal_dist = 9999 start_ne = (25, 100) #goal_ne = (750., 370.) goal_ne = (750, 370) close_start_pt = (9999,9999) close_goal_pt = (9999,9999) dist_s = 9999 dist_g = 9999 # Static drone altitude (metres) drone_altitude = 5 # This is now the routine using Voronoi grid, edges = create_grid_and_edges(data, drone_altitude) print(len(edges)) # equivalent to # plt.imshow(np.flip(grid, 0)) plt.imshow(grid, origin='lower', cmap='Greys') G = nx.Graph() def heuristic(n1, n2): # TODO: define a heuristic return distance(n1,n2) for e in edges: p1 = (int(e[0][0]), int(e[0][1])) p2 = (int(e[1][0]), int(e[1][1]))
data = np.loadtxt('vrhovi_new.csv', delimiter=',', dtype='Float64', skiprows=2) print(data) new_data = [] for row in data: if np.absolute(row[0]) > 4000 or np.absolute(row[1]) > 2000: continue else: new_data.append(row) small_data = np.array(new_data) / 10 print(small_data) #s_data = np.array(data)/10 grid, edges = create_grid_and_edges(small_data, 1, 0.5) grid_pickle_out = open("grid_p.pickel", mode='wb') pickle.dump(grid, grid_pickle_out) grid_pickle_out.close() outfile = open("edges_p.pickel", mode='wb') pickle.dump(edges, outfile) outfile.close() in_grid = open("grid_p.pickel", mode='rb') grid = pickle.load(in_grid) in_grid.close() in_edges = open("edges_p.pickel", mode='rb') edges = pickle.load(in_edges)