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()
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)
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,
t0 = time.time()
fbFM.search()
t_searchfbFM = time.time()-t0
print "Done. Search took {0}s".format(t_searchfbFM)
eFM = copy.deepcopy(FM)
ubFM = copy.deepcopy(bFM)
#bFM_reset(ubFM, bFM, g)
#ccost =fm_graphtools.square_cost_modifier(g, 25, 37, 88, 97, -0.991526711378)
#ubFM.update(ccost)
poly_cost = fm_graphtools.polynomial_precompute_cost_modifier(g, 12, min_val=0.001)
for ii in range(NUM_TESTS):
xlo = random.randint(5, g.width-5)
ylo = random.randint(5, g.height-5)
modcost = 1.9*random.random()-1
#dx = random.randint(1, 20)
#dy = random.randint(1, 20)
#cost_update = fm_graphtools.square_cost_modifier(g, xlo, xlo+dx, ylo, ylo+dy, modcost)
#print "Square map update {0}, ({1},{2}) to ({3},{4}), delta = {5}".format(ii, xlo, ylo, xlo+dx, ylo+dy, modcost)
cost_update = poly_cost.set_update(xlo,ylo,modcost)
print "Poly map update {0}, ({1},{2}), delta = {3}".format(ii, xlo, ylo, modcost)
print "Method | Time (s) | SNodes | DNodes | Cost |"
ug = g.copy()
ug.add_delta_costs(cost_update)
