poly_cost_obj = fm_graphtools.polynomial_precompute_cost_modifier(true_g, 13, min_val=0.001)
start_node = (3,3)
end_node = (97, 97)
while start_node in true_g.obstacles:
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))
ax1[1][1].set_title("Estimated cost - FM sampling")
fm_plottools.draw_grid(ax1[0][0], true_g, tFM.path, 19)
#fm_plottools.draw_fbfmcost(ax1[0][0], true_g, tFM.path_cost, tFM.path, 1000, 1400)
print 'Creating fast marching explorers... ',
# Create initial GP
mean_value = 3;
X = np.array([[3, 5]])
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)
# Create BiFM explorer objects for each sampling strategy
random_sampling_explorer = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value, true_g.obstacles)
maxvar_sampling_explorer = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value, true_g.obstacles)
fm_sampling_explorer = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value, true_g.obstacles)
print 'done.'
## UPDATES!
#cost_update = square_cost_modifier(g, 60, 80, 10, 30, -3)
test_gridx = range(2, true_g.width, 12); lx = len(test_gridx)
test_gridy = range(2, true_g.height, 12); ly = len(test_gridy)
delta_costs = [-1, 1]; ld = len(delta_costs)
NUM_TESTS = lx*ly*ld
search_time = np.zeros(NUM_SAMPLES, float)
true_path_cost = np.zeros((3, NUM_SAMPLES), float)
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)
def waypoint_reached_callback(self, msg):
print "Waypoint {0} reached.".format(self.num_visited)
self.cgeopose_ = msg
self.cpose_ = msg.position
self.cquat_ = msg.orientation
self.pp = geodesy.utm.fromMsg(self.cpose_)
if not hasattr(self, "zero_utm"):
print "Waypoint 0: defining local origin."
self.zero_utm = self.pp
self.num_visited += 1
# Wait for valid/recent sonar data
waiter = True
while rospy.Time.now() > self.sonar_time + rospy.Duration(0.5):
if waiter:
waiter = False
print "Waiting for valid sonar data"
time.sleep(0.1)
print "Sonar data recorded, depth = {0}m".format(self.sonar_depth)
# Current position in world frame
self.clocalpos = self.get_local_coords(self.pp)
print "Current local position: {0}, {1}".format(self.clocalpos[0], self.clocalpos[1])
self.observations[self.num_visited - 1, :] = np.array(
[
rospy.Time.now().secs,
self.cpose_.latitude,
self.cpose_.longitude,
self.clocalpos[0],
self.clocalpos[1],
self.sonar_depth,
]
)
tt = time.time()
# Current position in grid frame
# cX = np.array([clocalpos[0]-self.origin_offset[0], clocalpos[1]-self.origin_offset[1]])
if self.num_visited <= 1:
# Initial sample set
X = np.reshape(self.clocalpos, (1, 2))
Y = np.zeros((1, 1))
Y[0] = self.sonar_depth
self.fm_sampling_explorer = bfm_explorer.fast_marching_explorer(
self.gridsize,
self.start_node,
self.goal_node,
X,
Y,
obs=self.true_g.obstacles,
mean_value=self.mean_depth,
GP_l=self.GPm.kern.lengthscale[0] * 1.0,
GP_sv=self.GPm.kern.variance[0] * 1.0,
GP_sn=self.GPm.Gaussian_noise.variance[0] * 5.0,
max_depth=self.max_depth,
mean_depth=self.mean_depth,
bl_corner=[self.operating_region.left, self.operating_region.bottom],
)
self.fm_sampling_explorer.search()
self.poly_cost = fm_graphtools.polynomial_precompute_cost_modifier(self.true_g, 14, min_val=0.001)
print "Initial setup took {0:0.2f}s".format(time.time() - tt)
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)
pickle.dump(self.observations, fh)
fh.close()
# self.plot_current_path(self.get_local_coords(pp))
# try:
# 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)
# except:
# print "Saving video directly failed."
#
# fh = open('lutra_videoframes_'+self.nowstr+'.p', 'wb')
# try:
# pickle.dump(self.fig, fh)
# pickle.dump(self.video_frames)
# except:
# print "Dumping video frames to file failed"
# fh.close()
return
else:
if self.continuous_sonar: # pop off all the recent sonar data
oX = []
oY = []
while True:
try:
cO = self.sonar_buffer.pop()
oX.append([cO[0], cO[1]])
oY.append(cO[2])
except IndexError:
break
oX = np.array(oX)
oY = np.reshape(oY, (len(oY), 1))
else:
oX = [self.clocalpos]
oY = [[self.sonar_depth]]
self.fm_sampling_explorer.add_observation(oX, oY)
print "Adding {1} new sample(s) took {0:0.2f}s".format(time.time() - tt, len(oY))
# Find next sample point
fm_best_cost = -1
tt = time.time()
for ii in range(self.sample_locations.shape[0]):
[tx, ty] = self.sample_locations[ii]
# print self.fm_sampling_explorer.cmodel.var_dict
# if ((tx,ty) not in self.true_g.obstacles) and not self.previously_sampled([tx,ty]):
current_value = 0
for td in self.delta_costs:
stdY = self.fm_sampling_explorer.GP_cost_graph.var_fun(tx, ty) # math.sqrt()
cost_update = self.poly_cost.calc_cost(tx, ty, td * stdY)
current_value += self.fm_sampling_explorer.cost_update_new(cost_update)
# print "Sample {0:2d} at ({1:4d},{2:4d}), std={3:0.3f}, pcost={4:0.3f}".format(ii,tx,ty,stdY,current_value)
if fm_best_cost == -1 or (current_value < fm_best_cost):
fm_best_cost = current_value
fm_bestX = [tx, ty]
fm_bestVar = stdY
fm_besti = ii
print "Finding best sample took {0:0.2f}s.".format(time.time() - tt)
target_utm = self.get_utm_coords(fm_bestX)
print "Next target point selected: E = {0}m, N = {1}m (c={2:0.3f}, std={3:0.3f}, i={4}).".format(
fm_bestX[0], fm_bestX[1], fm_best_cost, fm_bestVar, fm_besti
)
self.pub_point(target_utm)
self.sonar_buffer.clear()
tt = time.time()
self.plot_current_path(fm_bestX)
print "Plotting took {0:0.2f}s.".format(time.time() - tt)
self.create_random_samples()
def fmex_constructor(gridsize, start_node, end_node, X, Y, mean_value, obstacles):
fmex = bfm_explorer.fast_marching_explorer(gridsize, start_node, end_node, X, Y, mean_value=mean_value, obs=obstacles,
GP_l=GP_l, GP_sv=GP_sv, GP_sn=GP_sn)
fmex.search()
return fmex
