true_g = fm_graphtools.CostmapGridFixedObs(gridsize[0], gridsize[1], cost_fun=explore_cost_function, obstacles=[])
if OBSTACLES_ON:
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]):
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)
