true_path_cost = []
est_path_cost = []
est_path_var = []
for ii in range(NUM_SAMPLES):
t0 = time.time()
best_cost = -1
for tx in test_gridx:
for ty in test_gridy:
if not check_row(X, [tx,ty]):
current_value = 0
for td in delta_costs:
stdY = math.sqrt(varYfull[tx*GPg.width+ty])
cost_update =fm_graphtools.polynomial_cost_modifier(GPg, tx, ty, 15, td*stdY)
fbFM.update(cost_update)
current_value += fbFM.updated_min_path_cost
if best_cost == -1 or (current_value < best_cost):
best_cost = current_value
bestX = [tx,ty]
print "Point selected: ({0}, {1}). Estimation time = {2} sec.".format(bestX[0], bestX[1], time.time()-t0)
search_time[ii] = time.time()-t0
# update GP with best observation
X = np.append(X, [bestX], axis=0)
Y = np.append(Y, [[explore_cost_function(bestX[0], bestX[1]) + random.normalvariate(0, 0.5)]], axis=0)
m.set_XY(X, Y-mean_value)
Yfull, varYfull = m.predict(Xfull)
fm_best_cost = -1
for tx in test_gridx:
for ty in test_gridy:
# Max var
if not check_row(maxvar_sampling_explorer.X, [tx,ty]):
temp_var = maxvar_sampling_explorer.varYfull[ty*gridsize[0]+tx]
if temp_var > max_var:
max_var = temp_var
maxvar_bestX = [tx,ty]
if not check_row(fm_sampling_explorer.X, [tx,ty]):
current_value = 0
for td in delta_costs:
stdY = math.sqrt(fm_sampling_explorer.varYfull[ty*gridsize[0]+tx])
cost_update =fm_graphtools.polynomial_cost_modifier(fm_sampling_explorer.GP_cost_graph, tx, ty, 15, td*stdY)
current_value += fm_sampling_explorer.cost_update(cost_update)
if fm_best_cost == -1 or (current_value < fm_best_cost):
fm_best_cost = current_value
fm_bestX = [tx,ty]
maxvar_sampling_explorer.add_observation(maxvar_bestX, sample_cost_fun(explore_cost_function, maxvar_bestX))
true_path_cost[1,ii] = calc_true_path_cost(explore_cost_function, maxvar_sampling_explorer.fbFM.path)
est_path_cost[1,ii],est_path_var[1,ii] = calc_est_path_cost(maxvar_sampling_explorer.GP_model, mean_value, maxvar_sampling_explorer.fbFM.path)
fm_sampling_explorer.add_observation(fm_bestX, sample_cost_fun(explore_cost_function, fm_bestX))
true_path_cost[2,ii] = calc_true_path_cost(explore_cost_function, fm_sampling_explorer.fbFM.path)
est_path_cost[2,ii],est_path_var[2,ii] = calc_est_path_cost(fm_sampling_explorer.GP_model, mean_value, fm_sampling_explorer.fbFM.path)
video_frames.append(plot_updates(ax1, [random_sampling_explorer, maxvar_sampling_explorer, fm_sampling_explorer]))
print "Iteration {0} path costs - Random: {1}, MaxVar: {2}, FM: {3}".format(ii, true_path_cost[0,ii], true_path_cost[1,ii], true_path_cost[2,ii])
def pose_callback(self, msg):
print "Waypoint {0} reached.".format(self.num_visited)
self.cgeopose_ = msg
self.cpose_ = msg.position
self.cquat_ = msg.orientation
pp = geodesy.utm.fromMsg(self.cpose_)
self.num_visited += 1
if self.num_visited <= 1:
print "Arrived at first waypoint, creating fast march explorer."
self.zero_utm = pp
self.test_gridx = range(2, self.gridsize[0], 10)
self.test_gridy = range(2, self.gridsize[1], 10)
self.true_g = fm_graphtools.CostmapGrid(self.gridsize[0], self.gridsize[1], explore_cost_function)
explorer_cost = bfm_explorer.mat_cost_function(self.true_g, explore_cost_function)
self.true_g.cost_fun = explorer_cost.calc_cost
start_node = (3, 3)
end_node = (self.gridsize[0] - 3, self.gridsize[1] - 3)
# Search over true field
tFM = fast_marcher.FullBiFastMarcher(self.true_g)
tFM.set_start(start_node)
tFM.set_goal(end_node)
tFM.search()
tFM.pull_path()
self.best_path = tFM.path
self.best_path_cost = calc_true_path_cost(explore_cost_function, self.best_path)
# Initial sample set
X = np.array([self.get_local_coords(pp)])
Y = np.zeros((1, 1))
Y[0] = sample_cost_fun(explore_cost_function, X[0, :])
self.fm_sampling_explorer = bfm_explorer.fast_marching_explorer(
self.gridsize, start_node, end_node, X, Y, MEAN_VALUE, self.true_g.obstacles
)
elif self.num_visited == self.total_waypoints:
print "Arrived at final waypoint, saving data."
fh = open("lutra_fastmarchlog_" + self.nowstr + ".p", "wb")
pickle.dump(self.fm_sampling_explorer.X, fh)
pickle.dump(self.fm_sampling_explorer.Y, fh)
fh.close()
self.plot_current_path(self.get_local_coords(pp))
# ani1 = animation.ArtistAnimation(self.fig, self.video_frames, interval=1000, repeat_delay=0)
# ani1.save('fm_explorer_'+self.nowstr+'.mp4', writer = 'avconv', fps=1, bitrate=1500)
return
else:
clocalpos = self.get_local_coords(pp)
self.fm_sampling_explorer.add_observation(clocalpos, sample_cost_fun(explore_cost_function, clocalpos))
# Find next sample point
fm_best_cost = -1
for tx in self.test_gridx:
for ty in self.test_gridy:
if (tx, ty) in self.true_g.obstacles:
continue
if not self.previously_sampled([tx, ty]):
current_value = 0
for td in self.delta_costs:
stdY = math.sqrt(self.fm_sampling_explorer.varYfull[ty * self.gridsize[0] + tx])
cost_update = fm_graphtools.polynomial_cost_modifier(
self.fm_sampling_explorer.GP_cost_graph, tx, ty, 15, td * stdY
)
current_value += self.fm_sampling_explorer.cost_update(cost_update)
if fm_best_cost == -1 or (current_value < fm_best_cost):
fm_best_cost = current_value
fm_bestX = [tx, ty]
self.plot_current_path(fm_bestX)
target_utm = self.get_utm_coords(fm_bestX)
print "Next target point selected: E = {0}m, N = {1}m.".format(fm_bestX[0], fm_bestX[1])
self.pub_point(target_utm)