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)
