def test_weights_planning():
plot_map()
start_pos = [ 2650, 2650 ]
L, c = grid_graph(start_pos, dim=10, width=1000)
filename = os.path.join(root, 'flash', 'fft2', 'processed', 'map.png')
img_data = imread(filename)
custom_labels = add_weights(L, img_data)
astar_path = nx.astar_path(L, (5, 5), (0, 4))
H = L.subgraph(astar_path)
h_pos = nx.get_node_attributes(H, 'pos')
pos = nx.get_node_attributes(L,'pos')
nx.draw(L, pos, node_size=5)
edge_weight=dict([((u,v,),int(d['weight'])) for u,v,d in L.edges(data=True)])
nx.draw_networkx_edge_labels(L,pos,edge_labels=edge_weight)
nx.draw_networkx_nodes(L,pos, node_size=0)
nx.draw_networkx_edges(L,pos)
nx.draw_networkx_labels(L,pos, labels=custom_labels)
nx.draw(H,h_pos, node_size=5, edge_color='r')
plt.show()
def test_waypoints(filename):
G = graph_from_waypoints(filename)
plot_map()
pos = nx.get_node_attributes(G, 'pos')
nx.draw(G, pos, node_size=5)
plt.show()
def test_waypoints(filename):
G = graph_from_waypoints(filename)
plot_map()
pos = nx.get_node_attributes(G, 'pos')
nx.draw(G, pos, node_size=5)
plt.show()
def test_stitch():
#make local unconstrained motion graph
start_pos = [2650, 2650]
goal_pos = [1900, 400]
#create unconstrained local graph at the start
start_local_graph, start_center = grid_graph(start_pos, dim=10, width=1000)
filename = os.path.join(root, 'flash', 'fft2', 'processed', 'map.png')
img_data = imread(filename)
add_weights(start_local_graph, img_data)
#create unconstrained local graph at the goal
goal_local_graph, goal_center = grid_graph(goal_pos, dim=10, width=1000)
add_weights(goal_local_graph, img_data)
#make global graph based on waypoints
filename = os.path.join(root, 'flash', 'fft2',
'export', 'binaryData', '910.bin')
global_graph = graph_from_waypoints(filename)
#make a tree from the global graph
pos = nx.get_node_attributes(global_graph, 'pos')
#sorted by keys
d_x = OrderedDict(sorted(pos.items(), key=lambda t: t[0])).values()
c_x = numpy.array(d_x)
global_tree = scipy.spatial.cKDTree(c_x)
#stitch together unconstrained local with global
u_graph = stitch(start_local_graph, global_graph, global_tree, 100, start_center, 'S-')
u_graph = stitch(goal_local_graph, u_graph, global_tree, 100, goal_center, 'G-')
u_pos = nx.get_node_attributes(u_graph, 'pos')
plot_map()
nx.draw(u_graph, u_pos, node_size=5)
astar_path = nx.astar_path(u_graph, 'S-' + str(start_center), 'G-' + str(goal_center))
H = u_graph.subgraph(astar_path)
h_pos = nx.get_node_attributes(H, 'pos')
nx.draw(H, h_pos, node_size=5, edge_color='r')
plt.show()
def test_grid():
plot_map()
start_pos = [ 2650, 2650 ]
L, c = grid_graph(start_pos, dim=10, width=1000)
pos = nx.get_node_attributes(L,'pos')
nx.draw(L, pos, node_size=5)
plt.show()
def test_planner():
start_pos = [2650, 2650]
goal_pos = [1900, 400]
planned_path = plan_path(start_pos, goal_pos)
planned_path_pos = nx.get_node_attributes(planned_path, 'pos')
plot_map()
nx.draw(planned_path, planned_path_pos, node_size=5, edge_color='r')
plt.show()