def compute_reachable_cache(run_dir, args):
print '%s: calculating reachable cache' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
t_max = args['t_max']
G = store['G']
G.cache_soonest_arrivals()
all_edges = set()
for v_i in G.nodes:
for v_j in G.edge_dict[v_i]:
all_edges.update(G.edge_dict[v_i][v_j])
reachable_cache = {}
for v_i in G.nodes:
print v_i
for v_j in G.nodes:
for b in range(t_max+1): # HACK! assumes integer edge length and start time of zero!
reachable = set()
for e in all_edges:
if (G.soonest_arrival(v_i, e.v_i) + e.length()
+ G.soonest_arrival(e.v_j, v_j)) <= b:
reachable.update(e.samples())
reachable_cache[(v_i, v_j, b)] = reachable
store['reachable_cache'] = reachable_cache
def __init__(self):
gladefile = os.path.join(roslib.packages.get_pkg_dir('pplan'), "glade/pplan_gui.glade")
self.windowname = "main_window"
self.w_tree = gtk.glade.XML(gladefile, self.windowname)
dic = {'on_main_window_destroy' : gtk.main_quit,
'on_get_data_button_clicked' : self.get_data,
'on_make_graph_button_clicked' : self.make_graph,
'on_calc_kernel_button_clicked' : self.calc_kernel,
'on_plan_path_button_clicked' : self.plan_path,
}
self.w_tree.signal_autoconnect(dic)
main_window = self.w_tree.get_widget('main_window')
# setup matplotlib stuff on first notebook page (empty graph)
self.figure = Figure(figsize=(6,4), dpi=72)
self.axis = self.figure.add_subplot(111)
self.axis.set_xlabel('Longitude')
self.axis.set_ylabel('Latitude')
self.axis.set_title('')
self.axis.grid(True)
self.canvas = FigureCanvasGTK(self.figure) # a gtk.DrawingArea
self.canvas.show()
self.graphview = self.w_tree.get_widget('vbox1')
self.graphview.pack_start(self.canvas, True, True)
self.nav_bar = NavigationToolbar(self.canvas, main_window)
self.graphview.pack_start(self.nav_bar, False, True)
run_dir = sys.argv[1]
self.settings = pplan.PPlanSettings(run_dir)
# initialize the data directory, if not done already
self.store = ppas.Store(self.settings.data_dir)
self.plot_graph()
self.plot_path()
def calc_emse(run_dir):
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
P = store['P']
G = store['G']
objective = ppas.objectives.EMSEObjective(store['kmat'], settings.planner_properties['sigma_n'])
start_t = ppas.datetime_to_seconds(settings.roi_properties['starttime'])
samples = ppas.graph.path_samples(store['P'], start_t)
print '%s: EMSE:' % run_dir, objective.f(samples)
def make_ma(run_dir):
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
P = store['P']
G = store['G']
waypoints = []
waypoints.append((G.node_points[P[0].v_i][0], G.node_points[P[0].v_i][1]))
for e in P:
waypoints.append((G.node_points[e.v_j][0], G.node_points[e.v_j][1]))
ppas.glider.make_ma(waypoints, os.path.join(run_dir, 'waypoints.ma'))
def calc_cov(run_dir, args):
print '%s: calculating empirical covariance matrix' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
if settings.training_properties['kernel_type'] == 'weighted_rbf':
kernel = ppas.gp.kernels.WeightedRBF()
elif settings.training_properties['kernel_type'] == 'rbf':
kernel = ppas.gp.kernels.RBF()
else:
diee_unknown_kernel_type___
points = store['points']
kmat = kernel.value(settings.training_properties['hyper_params'], points, points)
store['kmat'] = kmat
def plan_path_bruteforce(run_dir, args):
print '%s: planning path using brute force' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
# create the objective class
objective = ppas.objectives.EMSEObjective(store['kmat'], settings.planner_properties['sigma_n'])
obj_val0 = objective.f(set())
bnb_planner = ppas.bnb.BranchAndBound(
store['G'],
store['Xi_pilot'],
objective,
reachable_cache=None,
use_bnb=False,
use_smart_order=False
)
print 'BF planning path from %d to %d' % (
settings.planner_properties['start_node'],
settings.planner_properties['end_node'])
t0 = time.time()
P, obj_val = bnb_planner.plan_with_horizon(
settings.planner_properties['start_node'],
settings.planner_properties['end_node'],
settings.planner_properties['start_t'],
settings.planner_properties['end_t'],
horizon=args['horizon']
)
runtime = time.time() - t0
result = {
'P' : P,
'obj_val' : obj_val,
'obj_val0' : obj_val0,
'objective_function' : 'emse',
'runtime' : runtime
}
save_result(store, 'bruteforce', args, result)
if P == None:
print '%s: Path planner failed to find a feasible path' % run_dir
else:
print '%s: Objective achieved by path: %f' % (run_dir, obj_val)
print P[0].v_i, P[-1].v_j
def plan_path_bnbmulti(run_dir, args):
print '%s: planning path using bnb multi' % run_dir
print args
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
G = store['G']
reachable_cache = store['reachable_cache']
# create the objective class
objective = ppas.objectives.EMSEObjective(store['kmat'], settings.planner_properties['sigma_n'])
obj_val0 = objective.f(set())
# create the planner
bnb_multi_planner = ppas.bnb_multi.BranchAndBoundMulti(
G,
pilot_samples = set(),
objective = objective,
reachable_cache = reachable_cache,
args = args,
)
t0 = time.time()
planner_result = bnb_multi_planner.plan()
paths = planner_result['paths']
runtime = time.time() - t0
obj_val = objective.f(set.union(*[ppas.graph.path_samples(P) for P in paths]))
result = {
'paths' : paths,
'planner_result' : planner_result,
'obj_val' : obj_val,
'obj_val0' : obj_val0,
'objective_function' : 'emse',
'runtime' : runtime,
'objfunc_evals' : objective.num_evaluations
}
save_result(store, 'bnb_multi', args, result)
if P == None:
print '%s: Path planner failed to find a feasible path' % run_dir
else:
print '%s: Objective achieved by path: %f' % (run_dir, obj_val)
print P[0].v_i, P[-1].v_j
def calc_emp_cov(run_dir, args):
print '%s: calculating empirical covariance matrix' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
training_store = ppas.Store(os.path.join(settings.data_dir, 'training_data'))
points = store['points']
qoi = settings.roi_properties['qoi']
training_dates = sorted(training_store.keys())
training_array = np.zeros((len(points), len(training_dates)))
exp_start = settings.roi_properties['starttime']
exp_midnight = datetime.datetime(exp_start.year, exp_start.month, exp_start.day, 0, 0)
# build array of training data
for date_i in range(len(training_dates)):
date_str = training_dates[date_i]
data = training_store[date_str]
data_year, data_month, data_day = [int(s) for s in date_str.split('-')]
data_midnight = datetime.datetime(data_year, data_month, data_day, 0, 0)
if exp_start.hour < 13:
exp_midnight += datetime.timedelta(days=1)
time_offset = ppas.datetime_to_seconds(exp_midnight) - ppas.datetime_to_seconds(data_midnight)
for p_i in range(len(points)):
p = points[p_i]
t = p[3] - time_offset
val = ppas.roms.get_value(data, p[0], p[1], p[2], t, qoi, interp='linear')
training_array[p_i,date_i] = val
# filter out bad data
if qoi == 'salt':
varmin, varmax = 30., 40.
elif qoi == 'temp':
varmin, varmax = 0., 100.
else:
dieee_unknown_var_type___
good_columns = ((training_array > varmin) & ( training_array < varmax)).all(axis=0)
good_training_data = training_array[:,good_columns]
store['good_training_data'] = good_training_data
# calculate covariance matrix
kmat = np.cov(good_training_data)
store['kmat'] = kmat
def make_graph(run_dir, args):
global pplan
print '%s: making graph' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
graph_type = settings.graph_properties['graph_type']
if graph_type == 'edgetime_equilateral':
import pplan.makegraph_edgetime_equilateral
G, points = pplan.makegraph_edgetime_equilateral.makegraph_edgetime_equilateral(
settings.roi_properties, settings.graph_properties)
elif graph_type == 'static_equilateral':
import pplan.makegraph_static_equilateral
G, points = pplan.makegraph_static_equilateral.makegraph_static_equilateral(
settings.roi_properties, settings.graph_properties)
else:
crash_bad_graph_type_
G.cache_soonest_arrivals(settings.graph_properties['time_list'])
store['G'] = G
store['points'] = points
store['Xi_pilot'] = set()
def plan_path_grg(run_dir, args):
print '%s: planning path using GRG' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
# create the objective class
objective = ppas.objectives.EMSEObjective(store['kmat'], settings.planner_properties['sigma_n'])
obj_val0 = objective.f(set())
# plan path
t0 = time.time()
P, obj_val, obj = ppas.pplan.grg_equilateral(
store['G'], # graph
settings.planner_properties['start_node'], # start node
settings.planner_properties['end_node'], # end node
settings.planner_properties['start_t'], # start time
settings.planner_properties['end_t'], # end time
settings.planner_properties['max_recursions'], # recursion level
objective, # objective function class
store['Xi_pilot'],
settings.planner_properties['max_recursions'])
runtime = time.time() - t0
result = {
'P' : P,
'obj_val' : -(-obj_val0 - obj_val), # convert reduction in EMSE to -EMSE
'obj_val0' : obj_val0,
'objective_function' : 'emse',
'runtime' : runtime
}
save_result(store, 'grg', args, result)
if P == None:
print '%s: Path planner failed to find a feasible path' % run_dir
else:
print '%s: Objective achieved by path: %f' % (run_dir, obj_val)
# save result
store['P'] = P
def get_data(run_dir):
print '%s: getting data' % run_dir
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
rp = settings.roi_properties
dates = settings.training_properties['dates']
for date_i in range(len(dates)):
date = dates[date_i]
try:
# connect to ROMS
dset = ppas.roms.open_dataset(date)
# get the data
data = ppas.roms.get_data(
dset, rp['lat0']-0.03, rp['lat1']+0.03, rp['lon0']-0.03, rp['lon1']+0.03,
rp['depth']-10., rp['depth']+10.)
store['training_data/' + str(date)] = data
except Exception, e:
print 'No ROMS data available for %s' % str(date)
print e
print '%s: Downloaded data for %s (%d/%d)' % (run_dir, str(date), date_i+1, len(dates))
time.sleep(0.1)
import roslib; roslib.load_manifest('pplan')
import sys, os.path
import numpy as np
from matplotlib import pyplot as plt
import ppas, pplan
# load settings
run_dir = sys.argv[1]
result_num = sys.argv[2]
settings = pplan.PPlanSettings(run_dir)
store = ppas.Store(settings.data_dir)
# setup plot
fig = plt.figure()
ax = fig.add_subplot(111)
G = store['G']
if not 'bonelli' in run_dir:
G.node_points = G.node_points[:,::-1]
graph_plotter = ppas.plot.GraphPlotter(G, 'xy')
graph_plotter.plot(ax=plt.gca(), nodenames=True, text_xoff=0.00005, text_yoff=0.00005)
results = store[os.path.join('results/%s/result' % result_num)]
graph_plotter.plot_path(results['P'], ax)
plt.show()
#!/usr/bin/env python
import roslib
roslib.load_manifest('pplan')
import sys, time, os, os.path, multiprocessing, datetime, signal
import numpy as np
from scipy import linalg
from matplotlib import pyplot as plt
import pplan, ppas
settings = pplan.PPlanSettings(sys.argv[1])
store = ppas.Store(settings.data_dir)
G = store['G']
points = store['points']
good_training_data = store['good_training_data']
for sp0 in G.node_points:
point_indices = []
for p_i in range(len(points)):
d = linalg.norm(points[p_i][:2] - sp0)
#print p_i, d
if d < 0.001:
point_indices.append(p_i)
qoi_means = good_training_data.mean(axis=1)
qoi_var = good_training_data.var(axis=1)
times = (points[point_indices, 3] - points[0, 3]) / 3600.
plt.plot(times, qoi_means[point_indices], '-')
#plt.plot(times, qoi_var[point_indices], '-')
plt.show()
