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)