# Create search for true field
tFM = fast_marcher.FullBiFastMarcher(true_g)
tFM.set_start(start_node)
tFM.set_goal(end_node)
t0 = time.time()
tFM.search()
tFM.pull_path()
fig1, ax1 = plt.subplots(2, 2, sharex=True, sharey=True)
ax1[0][0].set_title("True cost field")
ax1[0][1].set_title("Estimated cost field")
ax1[1][0].set_title("True path cost")
ax1[1][1].set_title("Estimated path cost")
fm_plottools.draw_fbfmcost(ax1[1][0], true_g, tFM.path_cost, tFM.path, 1000, 1400)
# Create initial GP
mean_value = 3;
GPg = copy.copy(true_g)
#X = 100.0*np.random.rand(100,2)
if PLOT_UPDATES:
X = np.array([[3, 3], [14, 14], [14, 38], [38, 14], [38, 38], [62, 14], [2, 62], [86, 14], [26, 74], [74, 38]])
else:
X = np.array([[3, 3]])
Xshape = X.shape
Y = np.zeros((Xshape[0], 1))
for ii in range(Xshape[0]):
Y[ii] = explore_cost_function(X[ii,0], X[ii,1]) + random.normalvariate(0, 0.5)
# define kernel
ker = GPy.kern.RBF(2)
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()
