def __init__( self, total_waypoints, n_samples, operating_region, GPm, mean_depth=0, fake_sonar=False, max_depth=20.0, pond_image=None, continuous_sonar=False, swath_samples=False, ): print "Creating Lutra Fast Marching Explorer object" print "Total number of waypoints to be visited: {0}".format(total_waypoints) print "Number of random samples: {0}".format(n_samples) print "Max depth for min graph cost: {0}".format(max_depth) print "Fake sonar activated: {0}".format(fake_sonar) print "Continous sonar measurements: {0}".format(continuous_sonar) print "Swath samples: {0}".format(swath_samples) self.continuous_sonar = continuous_sonar self.swath_samples = swath_samples self.sonar_buffer = collections.deque(maxlen=20) # FM search area self.n_samples = n_samples self.total_waypoints = total_waypoints self.operating_region = operating_region self.start_node = self.operating_region.start_node self.goal_node = self.operating_region.goal_node self.gridsize = [self.operating_region.width, self.operating_region.height] self.observations = np.zeros((n_samples, 6)) # Time, lat, lon, x, y, depth # GP self.GPm = GPm self.mean_depth = mean_depth self.max_depth = max_depth self.delta_costs = [-1] self.num_visited = 0 self.nowstr = time.strftime("%Y_%m_%d-%H_%M") print "Setting up truth FM graph..." self.true_g = fm_graphtools.CostmapGridFixedObs( self.gridsize[0], self.gridsize[1], obstacles=self.operating_region.obstacles, bl_corner=[self.operating_region.left, self.operating_region.bottom], ) print "Building truth cost matrix..." explorer_cost = bfm_explorer.mat_cost_function_GP( self.true_g, cost_fun=bfm_explorer.GP_cost_function, GPm=self.GPm, max_depth=self.max_depth, mean_depth=mean_depth, ) self.true_g.cost_fun = explorer_cost.calc_cost # Search over true field print "Creating full searcher over true field..." tFM = fast_marcher.FullBiFastMarcher(self.true_g) tFM.set_start(self.start_node) tFM.set_goal(self.goal_node) print "Searching over true field..." tFM.search() tFM.pull_path() self.best_path = tFM.path print "Calculating best cost cost over true field..." self.best_path_cost = calc_true_path_cost( bfm_explorer.GP_cost_function, np.array(self.best_path), GPm=self.GPm, max_depth=self.max_depth, mean_depth=self.mean_depth, ) # Plots print "Creating plots..." self.fig, self.ax = plt.subplots(1, 3, sharex=True, sharey=True) self.fig.set_size_inches(15, 5) self.cost_alpha = 1.0 if pond_image: self.pond = scipy.misc.imread(pond_image + ".png") pond_locations = np.genfromtxt(pond_image + ".csv", delimiter=",") pond_origin = GeoPoint(pond_locations[2, 0], pond_locations[2, 1], pond_locations[2, 2]) self.zero_utm = geodesy.utm.fromMsg(pond_origin) self.pond_bl = self.get_local_coords( geodesy.utm.fromMsg(GeoPoint(pond_locations[0, 0], pond_locations[0, 1], pond_locations[0, 2])) ) self.pond_tr = self.get_local_coords( geodesy.utm.fromMsg(GeoPoint(pond_locations[1, 0], pond_locations[1, 1], pond_locations[1, 2])) ) self.cost_alpha = 0.5 for axx in self.ax: axx.set_aspect("equal", "datalim") axx.tick_params(labelbottom="on", labeltop="off") axx.set_xlabel("Easting (m)") axx.set_ylabel("Northing (m)") axx.autoscale(tight=True) if pond_image: axx.imshow(self.pond, extent=(self.pond_bl[0], self.pond_tr[0], self.pond_bl[1], self.pond_tr[1])) axx.plot(self.operating_region.vertices[:, 0], self.operating_region.vertices[:, 1], "k-") axx.set_xlim([self.true_g.left - 20, self.true_g.right + 20]) axx.set_ylim([self.true_g.bottom - 20, self.true_g.top + 20]) self.ax[0].set_title("True cost field") self.ax[1].set_title("Estimated cost - FMEx sampling") self.ax[2].set_title("GP Variance - FMEx Sampling") print "Generating random samples..." self.create_random_samples() self.best_cost_plot = [] self.cost_plot_matrix = [] self.video_frames = [] # ROS pub/subs print "Setting up publishers and subscribers..." self.wp_sub_ = rospy.Subscriber("/crw_waypoint_reached", GeoPose, self.waypoint_reached_callback) self.pos_sub_ = rospy.Subscriber("/crw_geopose_pub", GeoPose, self.pose_callback) self.depth_sub_ = rospy.Subscriber("/crw_sonar_pub", Range, self.sonar_callback) self.wp_pub_ = rospy.Publisher("/crw_waypoint_sub", GeoPose, queue_size=10) self.sonar_time = rospy.Time.now() - rospy.Duration(10) # Fake sonar self.fake_sonar = fake_sonar if self.fake_sonar: print "Setting up simulated sonar..." self.depth_pub_ = rospy.Publisher("/crw_sonar_pub", Range, queue_size=10) self.next_fakesonar = rospy.Time.now() newhead = Header(seq=0, stamp=rospy.Time.now(), frame_id="sonar") self.fake_sonar_msg = Range(header=newhead, min_range=0.5, max_range=100.0) print "Setup complete, waiting for first waypoint." if not pond_image: print "The next waypoint reached will be assumed to " print "represent the local origin of the search area."
if "GP_model" not in locals(): V_Ireland = np.array([[0, 0], [-43, -38], [-70, -94], [-60, -150], [0, -180], [54, -152], [85, -70], [0, 0]]) start_node = (0, -2) goal_node = (-30, -150) model_file = os.path.expanduser("~") + "/catkin_ws/src/ros_lutra/data/IrelandLnModel.pkl" fh = open(model_file, "rb") GP_model = pickle.load(fh) mean_depth = pickle.load(fh) fh.close() op_region = OperatingRegion(V_Ireland, start_node, goal_node) true_g = fm_graphtools.CostmapGridFixedObs( op_region.width, op_region.height, obstacles=op_region.obstacles, bl_corner=[op_region.left, op_region.bottom] ) explorer_cost = bfm_explorer.mat_cost_function_GP(true_g, GP_cost_function, max_depth=4.5, mean_depth=mean_depth) true_g.cost_fun = explorer_cost.calc_cost tFM = fast_marcher.FullBiFastMarcher(true_g) tFM.set_goal(goal_node) tFM.set_start(start_node) tFM.search() tFM.pull_path() f0, a0 = fm_plottools.init_fig() f1, a1 = fm_plottools.init_fig() f2, a2 = fm_plottools.init_fig() fm_plottools.draw_grid(a0, true_g, tFM.path) fm_plottools.draw_costmap(a1, true_g, tFM.FastMarcherSG.cost_to_come, tFM.path) fm_plottools.draw_fbfmcost(a2, true_g, tFM.path_cost, tFM.path) plt.show()