def test_planned_path_smoothing():
start_pos = [2650, 2650]
goal_pos = [1900, 400]
graph_path = plan_path(start_pos, goal_pos)
path_pos = nx.get_node_attributes(graph_path, 'pos')
#this relies on the fact that nodes are sorted properly
od = OrderedDict(sorted(path_pos.items(), key=lambda t: t[0]))
path_keys = od.keys()
path_list = od.values()
#finally, actual smoothing
smoothed_path = smooth(path_list)
printpaths(path_list, smoothed_path)
smoothed_graph = deepcopy(graph_path)
for i, k in enumerate(path_keys):
smoothed_graph.node[k]['pos'] = smoothed_path[i]
smoothed_pos = nx.get_node_attributes(smoothed_graph, 'pos')
plot_map()
nx.draw(smoothed_graph, smoothed_pos, node_size=5, edge_color='r')
nx.draw(graph_path, path_pos, node_size=5, edge_color='b')
#nx.draw_networkx_labels(graph_path, path_pos)
plt.show()
def test_planned_path_smoothing():
start_pos = [2650, 2650]
goal_pos = [1900, 400]
graph_path = plan_path(start_pos, goal_pos)
path_pos = nx.get_node_attributes(graph_path, 'pos')
#this relies on the fact that nodes are sorted properly
od = OrderedDict(sorted(path_pos.items(), key=lambda t: t[0]))
path_keys = od.keys()
path_list = od.values()
#finally, actual smoothing
smoothed_path = smooth(path_list)
printpaths(path_list, smoothed_path)
smoothed_graph = deepcopy(graph_path)
for i, k in enumerate(path_keys):
smoothed_graph.node[k]['pos'] = smoothed_path[i]
smoothed_pos = nx.get_node_attributes(smoothed_graph, 'pos')
plot_map()
nx.draw(smoothed_graph, smoothed_pos, node_size=5, edge_color='r')
nx.draw(graph_path, path_pos, node_size=5, edge_color='b')
#nx.draw_networkx_labels(graph_path, path_pos)
plt.show()
def main():
plot_map()
thismanager = get_current_fig_manager()
thismanager.window.wm_geometry("+700+0")
plt.gca().set_title("Running...")
plt.ion()
conn1, conn2 = Pipe()
data_stream = NavigateProcess(conn1)
data_stream.start()
#plt.gca().set_xlim([0, 2800])
#plt.gca().set_ylim([0, 2800])
map_box = None
while True:
if not(conn2.poll(0.1)):
if not(data_stream.is_alive()):
break
else:
continue
(sent_time, map_box, estimate, robot_pos, spath) = conn2.recv()
while (time.time() - sent_time) > 1/20:
#we are getting behind by more then a sec
(sent_time, map_box, estimate, robot_pos, spath) = conn2.recv()
if map_box is not None:
(x0, y0, x1, y1) = map_box
plt.gca().set_xlim([x0, x1])
plt.gca().set_ylim([y1, y0])
#new_position = (max_loc[0] + w/2, max_loc[1] + h/2)
plt.scatter( [(x0 + x1)/2],
[(y0 + y1)/2],)
plt.scatter( [robot_pos[0]],
[robot_pos[1]], color='red')
plt.scatter( [estimate[0]],
[estimate[1]], color='green')
plt.plot([spath[0], spath[2]], [spath[1], spath[3]], color = 'green')
#plt.plot([pt[0], new_pt[0]], [pt[1], new_pt[1]], "bs:")
plt.pause(0.001)
map_box = (x0, y0, x1, y1)
plt.gca().set_title("Terminated.")
plt.draw()
plt.show(block=True)
def main():
plot_map()
thismanager = get_current_fig_manager()
thismanager.window.wm_geometry("+700+0")
plt.gca().set_title("Running...")
plt.ion()
conn1, conn2 = Pipe()
data_stream = NavigateProcess(conn1)
data_stream.start()
#plt.gca().set_xlim([0, 2800])
#plt.gca().set_ylim([0, 2800])
map_box = None
while True:
if not (conn2.poll(0.1)):
if not (data_stream.is_alive()):
break
else:
continue
(sent_time, map_box, estimate, robot_pos, spath) = conn2.recv()
while (time.time() - sent_time) > 1 / 20:
#we are getting behind by more then a sec
(sent_time, map_box, estimate, robot_pos, spath) = conn2.recv()
if map_box is not None:
(x0, y0, x1, y1) = map_box
plt.gca().set_xlim([x0, x1])
plt.gca().set_ylim([y1, y0])
#new_position = (max_loc[0] + w/2, max_loc[1] + h/2)
plt.scatter(
[(x0 + x1) / 2],
[(y0 + y1) / 2],
)
plt.scatter([robot_pos[0]], [robot_pos[1]], color='red')
plt.scatter([estimate[0]], [estimate[1]], color='green')
plt.plot([spath[0], spath[2]], [spath[1], spath[3]], color='green')
#plt.plot([pt[0], new_pt[0]], [pt[1], new_pt[1]], "bs:")
plt.pause(0.001)
map_box = (x0, y0, x1, y1)
plt.gca().set_title("Terminated.")
plt.draw()
plt.show(block=True)
def test_smooth_graph():
start_pos = [2650, 2650]
goal_pos = [1900, 400]
graph_path = plan_path(start_pos, goal_pos)
path_pos = nx.get_node_attributes(graph_path, 'pos')
sg = smooth_graph(graph_path, start_pos, goal_pos)
sg_pos = nx.get_node_attributes(sg, 'pos')
plot_map()
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
nx.draw(graph_path, path_pos, node_size=5, edge_color='b')
#nx.draw_networkx_labels(graph_path, path_pos)
plt.show()
def test_smooth_graph():
start_pos = [2650, 2650]
goal_pos = [1900, 400]
graph_path = plan_path(start_pos, goal_pos)
path_pos = nx.get_node_attributes(graph_path, 'pos')
sg = smooth_graph(graph_path, start_pos, goal_pos)
sg_pos = nx.get_node_attributes(sg, 'pos')
plot_map()
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
nx.draw(graph_path, path_pos, node_size=5, edge_color='b')
#nx.draw_networkx_labels(graph_path, path_pos)
plt.show()
def __init__(self):
self.fig, self.ax = plot_map()
#position window properly
thismanager = get_current_fig_manager()
try:
thismanager.window.wm_geometry("+700+0")
except AttributeError:
self.fig.canvas.manager.window.move(700, 0)
self.ax.set_aspect('equal')
self.ax.set_xlim(0, 700)
self.ax.set_ylim(0, 500)
self.ax.hold(True)
self.fig.canvas.draw()
def __init__(self):
self.fig, self.ax = plot_map()
#position window properly
thismanager = get_current_fig_manager()
try:
thismanager.window.wm_geometry("+700+0")
except AttributeError:
self.fig.canvas.manager.window.move(700,0)
self.ax.set_aspect('equal')
self.ax.set_xlim(0,700)
self.ax.set_ylim(0,500)
self.ax.hold(True)
self.fig.canvas.draw()