start_node = (start_node[0]+1, start_node[1])
while end_node in true_g.obstacles:
end_node = (end_node[0]-1, end_node[1])
X = np.array([[3,3],[80,95], [55,45], [25,30], [38,60], [52,30],[65,70],[37,45],[14,41],[80,30],[83,85],[97,63]])
Xshape = X.shape
Y = np.zeros((Xshape[0], 1))
for ii in range(Xshape[0]):
Y[ii] = sample_cost_fun(explore_cost_function, X[ii,:], cblobs)
fm_sampling_explorer = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value=mean_value, obs=true_g.obstacles,
GP_l=GP_l, GP_sv=GP_sv, GP_sn=GP_sn)
if MAKE_VIDEO:
t_frames = []
fig_sg, ax_sg = fm_plottools.init_fig(graph=true_g, figsize=(6,5))
fm_sampling_explorer.set_plots(t_frames, ax_sg)
fm_sampling_explorer.set_plot_costs(0, 15)
fm_sampling_explorer.search()
if MAKE_PICTURES:
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_start(start_node)
tFM.set_goal(end_node)
tFM.search()
tFM.pull_path()
fig1,ax1 = fm_plottools.init_fig(graph=true_g, figsize=(6,5))
tempframe,barlims = fm_plottools.draw_grid(ax1, true_g, tFM.path,min_cost=mean_value-0.5)
cbar = fig1.colorbar(tempframe[0], shrink=0.7)
fig1.savefig(VID_DIR+'true_map.png', bbox_inches='tight', transparent=True)
fm_plottools.clear_content(tempframe)
if "GP_model" not in locals():
V_Ireland = np.array([[0, 0], [-43, -38], [-70, -94], [-60, -150], [0, -180], [54, -152], [85, -70], [0, 0]])
start_node = (0, -2)
goal_node = (-30, -150)
model_file = os.path.expanduser("~") + "/catkin_ws/src/ros_lutra/data/IrelandLnModel.pkl"
fh = open(model_file, "rb")
GP_model = pickle.load(fh)
mean_depth = pickle.load(fh)
fh.close()
op_region = OperatingRegion(V_Ireland, start_node, goal_node)
true_g = fm_graphtools.CostmapGridFixedObs(
op_region.width, op_region.height, obstacles=op_region.obstacles, bl_corner=[op_region.left, op_region.bottom]
)
explorer_cost = bfm_explorer.mat_cost_function_GP(true_g, GP_cost_function, max_depth=4.5, mean_depth=mean_depth)
true_g.cost_fun = explorer_cost.calc_cost
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_goal(goal_node)
tFM.set_start(start_node)
tFM.search()
tFM.pull_path()
f0, a0 = fm_plottools.init_fig()
f1, a1 = fm_plottools.init_fig()
f2, a2 = fm_plottools.init_fig()
fm_plottools.draw_grid(a0, true_g, tFM.path)
fm_plottools.draw_costmap(a1, true_g, tFM.FastMarcherSG.cost_to_come, tFM.path)
fm_plottools.draw_fbfmcost(a2, true_g, tFM.path_cost, tFM.path)
plt.show()
fbFM = fast_marcher.FullBiFastMarcher(GPg)
fbFM.set_start(start_node)
fbFM.set_goal(end_node)
# Initial search
fbFM.search()
fbFM.pull_path()
#figfbFM, axfbFM = fm_plottools.init_fig()
fbFM_frames = []
tempframe, tempbar = fm_plottools.draw_fbfmcost(ax1[1][1], GPg, fbFM.path_cost, fbFM.path, 1000, 1400)
tempframe.extend(graph_frame)
fbFM_frames.append(tempframe)
if PLOT_UPDATES:
sg_frames = []
fig_sg, ax_sg = fm_plottools.init_fig()
fbFM.FastMarcherSG.set_plots(sg_frames, ax_sg)
fbFM.FastMarcherSG.set_plot_costs(0, 20)
gs_frames = []
fig_gs, ax_gs = fm_plottools.init_fig()
fbFM.FastMarcherGS.set_plots(gs_frames, ax_gs)
fbFM.FastMarcherGS.set_plot_costs(0, 20)
up_frames = []
fig_up, ax_up = fm_plottools.init_fig()
fbFM.set_plots(up_frames, ax_up)
fbFM.set_plot_costs(0, 20)
## UPDATES!
#cost_update = square_cost_modifier(g, 60, 80, 10, 30, -3)
test_gridx = range(2, 100, 12); lx = len(test_gridx)
test_gridy = range(2, 100, 12); ly = len(test_gridy)
# 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
FM.find_corridor()
t_corridorFM = time.time()-t0-t_searchFM
print "Done. Search took {0}s, extracting corridor took {1}s".format(t_searchFM, t_corridorFM)
FM.pull_path()
f0,a0 = fm_plottools.init_fig()
fm_plottools.draw_grid(a0,g,path=FM.path)
print "Performing biFM search..."
bFM = fast_marcher.BiFastMarcher(g)
bFM.set_start(start_node)
bFM.set_goal(end_node)
t0 = time.time()
bFM.search()
t_searchbFM = time.time()-t0
bFM.find_corridor()
t_corridorbFM = time.time()-t0-t_searchbFM
print "Done. Search took {0}s, extracting corridor took {1}s".format(t_searchbFM, t_corridorbFM)
