def main(global_map, init, goal, steering_noise, distance_noise, measurement_noise,
weight_data, weight_smooth, p_gain, d_gain):
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, True,
weight_data, weight_smooth)
sg_pos = nx.get_node_attributes(sg, 'pos')
spath = graph_to_path(sg)
global_map.plot()
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
(reached_goal, penalty, num_steps, mg) = run(global_map, start_pos,
goal_pos, spath,
[p_gain, d_gain])
mg_pos = nx.get_node_attributes(mg, 'pos')
nx.draw(mg, mg_pos, node_size=5, edge_color='b')
plt.show()
def __init__(self):
self.start_pos = [2650, 2650]
self.goal_pos = [1900, 400]
self.graph_path = plan_path(self.start_pos, self.goal_pos)
self.global_map = GlobalMap()
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 __init__(self, connec, *args, **kwargs):
self.connec = connec
map_filename = os.path.join(root, 'flash', 'fft2', 'processed',
'aligned_localization_data_map.png')
car_filename = os.path.join(root, 'flash', 'fft2', 'export', 'images',
'445.png')
self.mapper = LocalizeMap(map_filename, car_filename)
filename = os.path.join(root, 'flash', 'fft2', 'processed',
'level1_start.png')
self.c = Capture(filename)
#default starting value
self.start_pos = [2650, 2650]
#level1
#self.goal_pos = [1900, 400]
#leve2
self.goal_pos = [1252, 1476]
#from twiddle
weight_data = 1.1
weight_smooth = 0.2
self.p_gain = .5
self.d_gain = 0.0
self.steering_noise = 5
self.distance_noise = 5
self.measurement_noise = 0.0005
self.speed = 2
#planning
print "planning..."
graph_path = plan_path(self.start_pos, self.goal_pos)
#extract points from graph
path_pos = nx.get_node_attributes(graph_path, 'pos')
#smooth
print "smoothing..."
sg = smooth_graph(graph_path, self.start_pos, self.goal_pos, False,
weight_data, weight_smooth)
#extract points from ad smoothed graph
sg_pos = nx.get_node_attributes(sg, 'pos')
#convert graph to spath
self.spath = graph_to_path(sg)
#plot smoothed path on a graph
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
#self.async_steering = AsyncFactory(press, cb_func)
Process.__init__(self, *args, **kwargs)
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 __init__(self, connec, *args, **kwargs):
self.connec = connec
map_filename = os.path.join(root, 'flash', 'fft2', 'processed', 'aligned_localization_data_map.png')
car_filename = os.path.join(root, 'flash', 'fft2', 'export', 'images', '445.png')
self.mapper = LocalizeMap(map_filename, car_filename)
filename = os.path.join(root, 'flash', 'fft2', 'processed', 'level1_start.png')
self.c = Capture(filename)
#default starting value
self.start_pos = [2650, 2650]
#level1
#self.goal_pos = [1900, 400]
#leve2
self.goal_pos = [1252, 1476]
#from twiddle
weight_data = 1.1
weight_smooth = 0.2
self.p_gain = .5
self.d_gain = 0.0
self.steering_noise = 5
self.distance_noise = 5
self.measurement_noise = 0.0005
self.speed = 2
#planning
print "planning..."
graph_path = plan_path(self.start_pos, self.goal_pos)
#extract points from graph
path_pos = nx.get_node_attributes(graph_path, 'pos')
#smooth
print "smoothing..."
sg = smooth_graph(graph_path, self.start_pos, self.goal_pos, False,
weight_data, weight_smooth)
#extract points from ad smoothed graph
sg_pos = nx.get_node_attributes(sg, 'pos')
#convert graph to spath
self.spath = graph_to_path(sg)
#plot smoothed path on a graph
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
#self.async_steering = AsyncFactory(press, cb_func)
Process.__init__(self, *args, **kwargs)
def __init__(self):
map_filename = os.path.join(root, 'flash', 'fft2', 'processed',
'aligned_localization_data_map.png')
self.mapper = LocalizeMap(map_filename)
filename = os.path.join(root, 'flash', 'fft2', 'processed',
'level1_start.png')
self.c = Capture(filename)
#default starting value
self.start_pos = [2650, 2650]
self.goal_pos = [1900, 400]
#from twiddle
weight_data = 1.1
weight_smooth = 0.2
self.p_gain = 2.0
self.d_gain = 6.0
self.steering_noise = 0.01
self.distance_noise = 0.05
self.measurement_noise = 0.05
self.speed = 2
#planning
print "planning..."
graph_path = plan_path(self.start_pos, self.goal_pos)
#extract points from graph
path_pos = nx.get_node_attributes(graph_path, 'pos')
#smooth
print "smoothing..."
sg = smooth_graph(graph_path, self.start_pos, self.goal_pos, True,
weight_data, weight_smooth)
#extract points from ad smoothed graph
sg_pos = nx.get_node_attributes(sg, 'pos')
#convert graph to spath
self.spath = graph_to_path(sg)
#plot smoothed path on a graph
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
def __init__(self):
map_filename = os.path.join(root, 'flash', 'fft2', 'processed', 'aligned_localization_data_map.png')
self.mapper = LocalizeMap(map_filename)
filename = os.path.join(root, 'flash', 'fft2', 'processed', 'level1_start.png')
self.c = Capture(filename)
#default starting value
self.start_pos = [2650, 2650]
self.goal_pos = [1900, 400]
#from twiddle
weight_data = 1.1
weight_smooth = 0.2
self.p_gain = 2.0
self.d_gain = 6.0
self.steering_noise = 0.01
self.distance_noise = 0.05
self.measurement_noise = 0.05
self.speed = 2
#planning
print "planning..."
graph_path = plan_path(self.start_pos, self.goal_pos)
#extract points from graph
path_pos = nx.get_node_attributes(graph_path, 'pos')
#smooth
print "smoothing..."
sg = smooth_graph(graph_path, self.start_pos, self.goal_pos, True,
weight_data, weight_smooth)
#extract points from ad smoothed graph
sg_pos = nx.get_node_attributes(sg, 'pos')
#convert graph to spath
self.spath = graph_to_path(sg)
#plot smoothed path on a graph
nx.draw(sg, sg_pos, node_size=5, edge_color='r')
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()