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)