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()
