def init_fig():
fign=plt.figure()
axn=fign.add_subplot(111)
axn.set_aspect('equal', 'datalim')
axn.tick_params(labelbottom='on',labeltop='off')
axn.set_xlabel('x')
axn.set_ylabel('y')
return fign, axn
print "Generating map..."
gridsize = [130, 100]
random.seed(2)
g = fm_graphtools.CostmapGrid(gridsize[0], gridsize[1], fm_graphtools.blob_cost_function)
g.obstacles = fm_plottools.generate_obstacles(gridsize[0], gridsize[1], 250, 10)
start_node = (1,1)
end_node = (127,97) #'''
# FM search
print "Performing FM search..."
figFM, axFM = init_fig()
FM = fast_marcher.FastMarcher(g)
FM.set_start(start_node)
FM.set_goal(end_node)
FM.set_plots([], axFM)
t0 = time.time()
FM.search()
t_searchFM = time.time()-t0
NUM_SAMPLES = 1
OBSTACLES_ON = True
num_obstacles = 40
obstacle_size = 10
delta_costs = [-1, 0.5]
random.seed(2)
num_blobs = 30 # Number of blobs in each field
peak_range = [-3.0,8.0] # Low and high peak values for map blobs
spread_range = [5,12] # Low and high spread distances for map blobs
VID_DIR = '../vid/'
true_g = fm_graphtools.CostmapGridFixedObs(gridsize[0], gridsize[1], cost_fun=explore_cost_function, obstacles=[])
if OBSTACLES_ON:
true_g.update_obstacles(fm_plottools.generate_obstacles(true_g, num_obstacles, obstacle_size))
start_node = (3,3)
while start_node in true_g.obstacles:
start_node = (start_node[0]+1, start_node[1])
end_node = (gridsize[0]-3, gridsize[1]-3)
while end_node in true_g.obstacles:
end_node = (end_node[0]-1, end_node[1])
cblobs = []
for ii in range(num_blobs):
cblobs.append([random.uniform(-10,gridsize[0]+10), random.uniform(-10,gridsize[1]+10),
random.uniform(spread_range[0], spread_range[1]), random.uniform(peak_range[0], peak_range[1])])
explorer_cost = bfm_explorer.mat_cost_function(true_g, explore_cost_function, cblobs)
true_g.cost_fun = explorer_cost.calc_cost
poly_cost_obj = fm_graphtools.polynomial_precompute_cost_modifier(true_g, 13, min_val=0.001)
dlabels = [METHOD_NAMES[i] for i in selector]
ax.boxplot(data)
ax.set_ylim(lims[0], lims[1])
ax.set_yscale('log')
ax.grid(True, which='major', axis='y')
ax.set_ylabel(ylabel)
ax.set_xticklabels(dlabels)
if title != None:
ax.set_title(title)
return ax
print "Generating map..."
gridsize = [100, 100]
g = fm_graphtools.CostmapGridFixedObs(gridsize[0], gridsize[1], fm_graphtools.blob_cost_function)
g.obstacles = fm_plottools.generate_obstacles(g, 20, 20)
g.rebuild_neighbours()
start_node = (1,1)
end_node = (97,97) #'''
while end_node in g.obstacles:
end_node = (end_node[0]-1, end_node[1])
# FM search
print "Performing FM search..."
FM = fast_marcher.FastMarcher(g)
FM.set_start(start_node)
FM.set_goal(end_node)
t0 = time.time()
FM.search()
t_searchFM = time.time()-t0
