def test():
the_map = Map()
tic = time.clock()
the_map.discretize_map()
print time.clock() - tic
#obstacle = Polygon([(40,15), (45,15), (45,20), (40,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 10
dT = 1
h = 0.05
N = int(tend/h) + 1
N2 = int(tend/dT) + 1
x0 = np.array([10, 10, 0.0, 3.0, 0, 0])
xg = np.array([50, 50, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0, xg, h, dT, N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
world.update_world(0,0)
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
xlim=(-10, 160), ylim=(-10, 160))
world.visualize(ax, 0, 0)
plt.show()
def __init__(
self,
nb_taps: int = 5,
demand_uniqueness: bool = True,
do_filter=True,
save_data=False,
):
self.do_filter = do_filter
self.save_data = save_data
self.demand_uniqueness = demand_uniqueness
self.M = M = nb_taps
self.p = p = int((M - 1) / 2)
self.q = p + 1
# These vectors hold the time/values being added to the stash.
self.x = deque([], maxlen=1000)
self.t = deque([], maxlen=1000)
# These variables are the filtered version of t/x; cannot sample from these vectors...
self.t_ = deque([], maxlen=1000)
self.x_ = deque([], maxlen=1000)
self.x_prev = 0
# These variables are the filtered version from which we sample. We
# have two versions because, depending on how quickly we're sampling
# from the the object, we may exhaust the data needed for the moving
# average filter.
self.t_filtered = deque([], maxlen=1000)
self.x_filtered = deque([], maxlen=1000)
if self.save_data:
datestring = datetime.now().strftime("%Y.%m.%d.%H.%M")
self.store = Vessel(f"data/{datestring}.dat")
self.store.t = []
self.store.x = []
def classify_tiles(self):
"""Classify all the tiles."""
scores = {}
for scientific_name in tqdm(self.target_species):
cnn = CNN(scientific_name, do_load_model=True)
scores[scientific_name] = cnn.model.predict(self.X)
self.scores = scores
cm = Vessel("confusion_matrix.dat")
cm.scores = scores
cm.save()
def extract_balanced_tiles(
path_to_annotations, target_species, tile_size=128, tiles_per_class=2000
):
"""Extract equal numbers of target species tiles and tiles from randomly selected species."""
print("> Extracting tiles.")
maps = Vessel("data/label_maps.dat")
data = Vessel(path_to_annotations)
images = data.annotated_images
nb_images = len(images)
target_images = find_target_images(target_species, images)
nb_target_images = len(target_images)
y_target = np.ones(tiles_per_class)
y_other = np.zeros(tiles_per_class)
X_target = np.array([])
X_other = np.array([])
# Grab data from the target set.
task_complete = False
while not task_complete:
image_number = np.random.choice(nb_target_images)
image = imread(target_images[image_number]["local_location"])[0]
annotations = target_images[image_number]["annotations"]
tiles = extract_tiles_from_image(
image,
annotations,
tile_size=tile_size,
target_species=target_species,
max_nb_tiles=tiles_per_class,
)
X_target = add_to_array(X_target, tiles, tiles_per_class)
task_complete = X_target.shape[0] == tiles_per_class
# Grab data from random "other" species.
task_complete = False
while not task_complete:
image_number = np.random.choice(nb_images)
image = imread(images[image_number]["local_location"])[0]
annotations = images[image_number]["annotations"]
tiles = extract_tiles_from_image(
image,
annotations,
tile_size=tile_size,
target_species=target_species,
include_target=False,
max_nb_tiles=tiles_per_class,
)
X_other = add_to_array(X_other, tiles, tiles_per_class)
task_complete = X_other.shape[0] == tiles_per_class
idx = np.arange(tiles_per_class * 2)
np.random.shuffle(idx)
X = np.vstack((X_target, X_other))
y = np.hstack((y_target, y_other))
print("> Extraction complete.")
return X[idx, :], y[idx].astype(int)
def find_all_images(tree, imgs, truth_locations, truths):
"""Find all images that contain each ground truth location."""
v = Vessel("ground_truth.dat")
for idx, truth in tqdm(enumerate(truths)):
loc = [truth_locations[idx, :]]
truth["images"] = []
_, candidates = tree.query(loc, k=100)
for candidate in candidates[0]:
if in_image(loc[0], imgs[candidate]["image_loc"]):
truth["images"].append(imgs[candidate]["_id"])
else:
v.truths = truths
v.save()
break
def simple_scenario_with_plot():
the_map = Map('s1')
#obstacle = Polygon([(30,15), (35,15), (35,20), (30,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 50
dT = 1
h = 0.05
N = int(tend / h) + 1
N2 = int(tend / dT) + 1
x0 = np.array([0, 0, 0.5, 3.0, 0, 0])
xg = np.array([120, 120, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0,
xg,
h,
dT,
N, [myDynWnd],
is_main_vessel=True,
vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
n = 0
for t in np.linspace(0, tend, N):
world.update_world(t, n, dT)
n += 1
fig = plt.figure()
ax = fig.add_subplot(111,
aspect='equal',
autoscale_on=False,
xlim=(-10, 10),
ylim=(-10, 10))
#fig, ax = plt.subplots()
ax.grid()
world.draw(ax, N)
print "N, N2, n: ", N, N2, n
myDynWnd.draw(ax, N2)
plt.show()
def test():
the_map = Map()
tic = time.clock()
the_map.discretize_map()
print time.clock() - tic
#obstacle = Polygon([(40,15), (45,15), (45,20), (40,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 10
dT = 1
h = 0.05
N = int(tend / h) + 1
N2 = int(tend / dT) + 1
x0 = np.array([10, 10, 0.0, 3.0, 0, 0])
xg = np.array([50, 50, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0,
xg,
h,
dT,
N, [myDynWnd],
is_main_vessel=True,
vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
world.update_world(0, 0)
fig = plt.figure()
ax = fig.add_subplot(111,
aspect='equal',
autoscale_on=False,
xlim=(-10, 160),
ylim=(-10, 160))
world.visualize(ax, 0, 0)
plt.show()
def simple_scenario_with_plot():
the_map = Map('s1')
#obstacle = Polygon([(30,15), (35,15), (35,20), (30,20)], safety_region_length=4.0)
#the_map.add_obstacles([obstacle])
tend = 50
dT = 1
h = 0.05
N = int(tend/h) + 1
N2 = int(tend/dT) + 1
x0 = np.array([0, 0, 0.5, 3.0, 0, 0])
xg = np.array([120, 120, 0])
myDynWnd = DynamicWindow(dT, N2)
v = Vessel(x0, xg, h, dT, N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v.current_goal = np.array([50, 50])
world = World([v], the_map)
myDynWnd.the_world = world
n = 0
for t in np.linspace(0, tend, N):
world.update_world(t, n, dT)
n += 1
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
xlim=(-10, 10), ylim=(-10, 10))
#fig, ax = plt.subplots()
ax.grid()
world.draw(ax, N)
print "N, N2, n: ", N, N2, n
myDynWnd.draw(ax, N2)
plt.show()
def evaluate(self):
"""Load results, or calculate them if not available."""
results = Vessel("predictions.dat")
if "y_" not in results.keys or results.scientific_name != self.scientific_name:
self.y_ = self.cnn.model.predict(self.X)
results.y = self.y
results.y_ = self.y_
results.scientific_name = self.scientific_name
results.save()
self.y_ = results.y_
def find_target_images(target_species, images):
"""Find the target species in the annotated image collection."""
maps = Vessel("data/label_maps.dat")
label_map_inverse = maps.label_map_inverse
target_images = []
for img in images:
codes = [
label_map_inverse[a["plant"]]
for a in img["annotations"]
if "plant" in a.keys()
]
if target_species in codes:
target_images.append(img)
return target_images
def __init__(self, scientific_name):
"""Generate a ROC curve for the specified target."""
# If no valid batch is present, let's create one before training.
invalid_batch = True
batch = Vessel("batch.dat")
if "scientific_name" in batch.keys:
if batch.scientific_name == scientific_name:
invalid_batch = False
if invalid_batch:
batch = create_batch(scientific_name)
self.X, self.y = batch.X, batch.y
self.scientific_name = scientific_name
self.cnn = CNN(scientific_name, do_load_model=True)
self.evaluate()
def __init__(self, target_species=TARGET_SPECIES, load_vessel=False):
"""Specific the list of scientific names of interest."""
# Specify the target species for the confusion matrix.
self.target_species = target_species
# Load data for each target species.
if load_vessel:
v = Vessel("confusion_matrix.dat")
try:
self.X = np.array(v.X)
self.y = np.array(v.y)
self.scores = v.scores
except:
self.load_targets()
else:
self.load_targets()
def add_outer_layers(self,
temp_corebarrel,
temp_downcomer,
temp_vessel,
hasShield,
temp_shield=None):
self.Corebarrel = Corebarrel(temp_corebarrel)
self.Downcomer = Downcomer(temp_downcomer)
self.Vessel = Vessel(temp_vessel)
if hasShield:
# outer radius taken from Tommy's input Mark1.txt
or_OR = 162
or_shield = 164.7
or_barrel = 168
or_downcomer = 170
or_vessel = 175
# shield
self.Shield = Shield(
temp_Vessel
) # assume it has the same temperature as vessel, need to be separated in the future
self.define_Shield(self.Shield.temp, self.Shield.name, or_OR,
or_shield)
self.define_Corebarrel(self.Corebarrel.temp, self.Corebarrel.name,
or_shield, or_barrel)
self.comp_dict['Shield'] = self.Shield
else:
or_OR = 165
or_barrel = 167.2
or_downcomer = 170
or_vessel = 175
self.define_Corebarrel(self.Corebarrel.temp, self.Corebarrel.name,
or_OR, or_barrel)
self.define_Downcomer(self.Downcomer.temp, self.Downcomer.name,
or_barrel, or_downcomer)
self.define_Vessel(self.Vessel.temp, self.Vessel.name, or_downcomer,
or_vessel)
self.comp_dict.update({
'Downcomer': self.Downcomer,
'Corebarrel': self.Corebarrel,
'Vessel': self.Vessel
})
def create_label_maps(path_to_annotations, path_to_maps):
"""Find species to integer (and inverse) maps."""
# Find all unique species.
v = Vessel(path_to_annotations)
unique_species = set({})
for img in v.annotated_images:
for annotation in img["annotations"]:
if "plant" not in annotation.keys():
continue
unique_species.add(annotation["plant"])
unique_species = sorted(list(unique_species))
# Build label maps.
label_map = {}
label_map_inverse = {}
for itr, plant_name in enumerate(unique_species):
label_map_inverse[plant_name] = itr
label_map[itr] = plant_name
maps = Vessel(path_to_maps)
maps.plant_to_id = label_map_inverse
maps.id_to_plant = label_map
maps.save()
def load_targets(self):
"""Load example targets."""
y = []
X = []
print("> Assembling the data.")
for scientific_name in tqdm(self.target_species):
annotations = get_specified_target(scientific_name,
nb_annotations=100)
for annotation in tqdm(annotations):
X_ = extract_tiles_from_annotation(annotation, 10)
X.extend(X_)
y_ = [scientific_name] * len(X_)
y.extend(y_)
self.X = X
self.y = y
print("> Assembly complete.")
c = Vessel("confusion_matrix.dat")
c.X = X
c.y = y
c.save()
from vessel import Vessel
from cv2 import resize
from glob import glob
from ipdb import set_trace as debug
import numpy as np
from pylab import imread, ion, imshow, close, subplot
from tqdm import tqdm
# Load all training data into the memory.
v = Vessel("targets.dat")
nb_classes = len(v.target_names)
def one_hot(class_number, total_number_of_classes):
"""Convert integer class number to the total number of classes."""
vector = np.zeros(total_number_of_classes)
vector[class_number] = 1
return vector
def resize_image(image, target_size=128):
"""Resize an image to specified width, preserving aspect ratio."""
height, width = image.shape
h2 = np.int(height / 2)
w2 = np.int(width / 2)
max_size = np.min((height, width))
half_max = np.int(max_size / 2)
square_image = image[h2 - half_max:h2 + half_max,
w2 - half_max:w2 + half_max]
return resize(square_image, (target_size, target_size))
from vessel import Vessel
from vessel import FileAccessWrapper
INPUT_FILE = '/home/kelvin/Documents/NMEA_Logs/de_guingand_bowl_20170513/log.txt'
OUTPUT_FILE = '/home/kelvin/Documents/NMEA_Logs/de_guingand_bowl_20170513/log.csv'
polar_file = FileAccessWrapper('modules/process/inter_polar.csv')
vessel = Vessel(polar_file)
timestamp = '0'
with open(OUTPUT_FILE, 'w') as w:
for a in dir(vessel):
if not a.startswith('__') and not a.startswith('Last') and not \
a.startswith('polar_file') and not \
a.startswith('line_offset') and not \
a.startswith('boat_speed_list') and not callable(getattr(vessel, a)):
w.write(a + ',')
w.write('\n')
with open(INPUT_FILE, 'r') as f:
for line in f:
vessel.NMEAInput(line)
if timestamp != str(vessel.time):
for a in dir(vessel):
if not a.startswith('__') and not a.startswith('Last') and\
not \
a.startswith('polar_file') and not \
a.startswith('line_offset') and not \
a.startswith('boat_speed_list') and not callable(getattr(vessel, a)):
w.write(str(getattr(vessel, a)) + ',')
timestamp = str(vessel.time)
import numpy as np
import plotly.plotly as py
import plotly.tools as tls
import plotly.graph_objs as go
import datetime
import time
from vessel import Vessel
INPUT_FILE = '/home/kelvin/Documents/NMEA_Logs/nab_challenge_20170319/log.txt'
vessel = Vessel()
stream_ids = tls.get_credentials_file()['stream_ids']
#print(stream_ids)
def GPSTrack_setup():
stream_id = stream_ids[0]
stream_1 = go.Stream(
token=stream_id, # link stream id to 'token' key
maxpoints=200)
trace1 = go.Scatter(
x=[],
y=[],
mode='markers',
stream=stream_1 # (!) embed stream id, 1 per trace
)
data = go.Data([trace1])
layout = go.Layout(title='GPS Track')
print('Change: ' + str(change))
return (center[0], center[1], center[2])
if __name__ == '__main__':
#parser = OptionParser()
#parser.add_option('-f', '--filename', dest='filename', help='File to read data from.')
#(options, args) = parser.parse_args()
# x, y = generate_circle(200)
# print(find_center(x, y))
# center = descent(x, y)
# print('Done! Center: ' + str(center))
v = Vessel('edges.vsl')
image = random.choice(v.edges)
center = descent(image)
plt.imshow(image)
plt.plot([center[0]], [center[1]], marker='o', color='red')
plt.show()
print(center)
# run_single_image(random.choice(v.edges))
'''
&= -(p(Y = Yes|X = Fast)log_{2}p(Y = Yes|X = Fast) + p(Y = No|X = Fast)log_{2}p(Y = No|X = Fast)) \begin{comment} +
p(Y = Yes|X = Slow)log_{2}p(Y = Yes|X = Slow) + p(Y = No|X = Slow)log_{2}p(Y = No|X = Slow)) \end{comment}\\
'''
def generate_benchmark():
"""
function to generate benchmarks ABCDEFG
:return: None
"""
# set A
counter = 1
for n in range(10, 41, 5):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=10, c=200, f=0.5, d=1.0, g=0.0, n=n, loc="uni")
qu = Quay(2, t=1, ready_time=0)
instance = Instance(safety_margin=1, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_A_{}.json".format(counter))
counter += 1
# set B
counter = 1
for n in range(45, 71, 5):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=15, c=400, f=0.5, d=1.0, g=0.0, n=n, loc="uni")
qu = Quay(4, t=1, ready_time=0)
instance = Instance(safety_margin=1, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_B_{}.json".format(counter))
counter += 1
# set C
counter = 1
for n in range(75, 101, 5):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=20, c=600, f=0.5, d=1.0, g=0.0, n=n, loc="uni")
qu = Quay(6, t=1, ready_time=0)
instance = Instance(safety_margin=1, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_C_{}.json".format(counter))
counter += 1
# set D
counter = 1
for f, loc in itertools.product([0.2, 0.8], ["cl1", "cl2", "uni"]):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=10, c=400, f=f, d=1.0, g=0.0, n=50, loc=loc)
qu = Quay(4, t=1, ready_time=0)
instance = Instance(safety_margin=1, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_D_{}.json".format(counter))
counter += 1
# set E
counter = 1
for d in [0.80, 0.85, 0.90, 0.95, 1.0]:
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=15, c=400, f=0.5, d=d, g=0.0, n=50, loc="uni")
qu = Quay(4, t=1, ready_time=0)
instance = Instance(safety_margin=1, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_E_{}.json".format(counter))
counter += 1
# set F
counter = 1
for q in range(2, 7):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=15, c=400, f=0.5, d=1, g=0.0, n=50, loc="uni")
qu = Quay(q, t=1, ready_time=0)
instance = Instance(safety_margin=1, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_F_{}.json".format(counter))
counter += 1
# set G
counter = 1
for s in range(0, 5):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=15, c=400, f=0.5, d=1, g=0.0, n=50, loc="uni")
qu = Quay(4, t=1, ready_time=0)
instance = Instance(safety_margin=s, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_G_{}.json".format(counter))
counter += 1
from filters import *
from vessel import Vessel
from glob import glob
import numpy as np
import pylab as plt
import seaborn as sns
# List all collected data files.
datafiles = glob("data/*.dat")
datafiles = sorted(datafiles)
# Locate and load the latest datafile.
datafile = datafiles[-1] # grab the latest
v = Vessel(datafile)
t, x = v.t, v.x
t = np.array(t)
x = np.array(x)
t -= t[0]
# Filter the data.
x_, _ = lowpass(t, x, freq_cutoff=3)
plt.ion()
plt.close('all')
plt.figure(100)
plt.plot(t[100:], x_[100:])
counter = 1
for s in range(0, 5):
for j in range(1, 11):
Instance.seed(j)
v = Vessel(b=15, c=400, f=0.5, d=1, g=0.0, n=50, loc="uni")
qu = Quay(4, t=1, ready_time=0)
instance = Instance(safety_margin=s, vessel=v, quay=qu)
instance.generate(style="json",
name="QCSP_Set_G_{}.json".format(counter))
counter += 1
if __name__ == "__main__":
try:
Instance.seed()
v = Vessel(b=20, c=600, f=0.5, d=1.0, g=0.0, n=20, loc="uni")
q = Quay(6, t=1, ready_time=[0, 2, 4, 6, 8, 0])
instance = Instance(safety_margin=1, vessel=v, quay=q)
instance.generate(style="json", name="QCSP.json")
# Instance.seed(123)
# v = Vessel(b=10, c=200, f=0.5, d=1.0, g=0.0, n=20, loc="uni")
# qu = Quay(2, t=1, ready_time=0)
# instance = Instance(safety_margin=1, vessel=v, quay=qu)
# instance.generate(style="json", name="test.json")
# v = Vessel(b=15, c=400, f=0.5, d=1, g=0.0, loc='uni', n=50)
# import copy
# bay_size = 20
# vessel_size = 100
# Instance.seed("hello")
# v_0 = Vessel(b=bay_size, c=600, f=0.5, d=1.0, g=0.0, n=200, loc="uni")
if __name__ == "__main__":
a_map = Map('s1', gridsize=0.5)
tend = 1.0
dT = 0.5
h = 0.05
N = int(np.around(tend/h)) + 1
N2 = int(np.around(tend/dT)) + 1
x0 = np.array([5,5,0, 2.0,0,0])
xg = np.array([120, 120, 0])
potfield = PotentialFields(a_map, N2)
vobj = Vessel(x0, xg, h, dT, N, [potfield], is_main_vessel=True, vesseltype='viknes')
potfield.update(vobj)
fig = plt.figure(1)
ax = fig.add_subplot(111, autoscale_on=False)
ax.axis('scaled')
ax.set_xlim((-10, 160))
ax.set_ylim((-10, 160))
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.grid()
a_map.draw(ax)
potfield.draw(ax)
class Stash(object):
"""Store data and filter it."""
def __init__(
self,
nb_taps: int = 5,
demand_uniqueness: bool = True,
do_filter=True,
save_data=False,
):
self.do_filter = do_filter
self.save_data = save_data
self.demand_uniqueness = demand_uniqueness
self.M = M = nb_taps
self.p = p = int((M - 1) / 2)
self.q = p + 1
# These vectors hold the time/values being added to the stash.
self.x = deque([], maxlen=1000)
self.t = deque([], maxlen=1000)
# These variables are the filtered version of t/x; cannot sample from these vectors...
self.t_ = deque([], maxlen=1000)
self.x_ = deque([], maxlen=1000)
self.x_prev = 0
# These variables are the filtered version from which we sample. We
# have two versions because, depending on how quickly we're sampling
# from the the object, we may exhaust the data needed for the moving
# average filter.
self.t_filtered = deque([], maxlen=1000)
self.x_filtered = deque([], maxlen=1000)
if self.save_data:
datestring = datetime.now().strftime("%Y.%m.%d.%H.%M")
self.store = Vessel(f"data/{datestring}.dat")
self.store.t = []
self.store.x = []
def add(self, t, x):
"""Add new point."""
if self.demand_uniqueness:
# Cannot add two successive identical values.
if len(self.x) > 0:
if self.x[-1] != x:
self.t.append(t)
self.x.append(x)
self.save_to_store(t, x)
else:
self.t.append(t)
self.x.append(x)
self.save_to_store(t, x)
else:
self.t.append(t)
self.x.append(x)
self.save_to_store(t, x)
if len(self.x) >= self.M and self.do_filter:
self.filter()
def save_to_store(self, t, x):
if self.save_data:
self.store.t.append(t)
self.store.x.append(x)
if np.mod(len(self.store.t), 1000) == 0:
# Save every 1000 samples.
self.store.save()
def filter(self):
"""Super efficient moving average filter."""
M, p, q = self.M, self.p, self.q
x = self.x
idx = len(self.x) - (p + 1)
x_ = self.x_prev + (x[idx + p] - x[idx - q]) / M
self.t_.append(self.t[idx])
self.t_filtered.append(self.t[idx])
self.x_.append(x_)
self.x_filtered.append(x_)
self.x_prev = x_
@property
def sample(self):
"""Return first observed pair (t, x), still in queue."""
if self.do_filter:
if len(self.t_filtered) > 0:
yield self.t_filtered.popleft(), self.x_filtered.popleft()
else:
yield None, None
else: # let's not filter
if len(self.t) > 0:
yield self.t.popleft(), self.x.popleft()
else:
yield None, None
from ipdb import set_trace as debug
from tqdm import tqdm
if __name__ == "__main__":
# Update all images to have annotations and ground truth.
images = list(image_collection.find({}))
for image in tqdm(images):
image['ground_truth'] = []
image['annotations'] = []
image_collection.update_one({'_id': image['_id']}, {'$set': image},
upsert=False)
# Load the ground truth data from the vessel.
gt = Vessel("ground_truth.dat")
# For each point, find the image in which it appears, if any.
for truth in tqdm(gt.truths):
# What plant are we talking about here?
plant_data = plant_collection.find_one(
{"species_codes": truth["species_code"]})
# If this is not a plant, we don't care.
if not plant_data:
continue
for image_id in truth["images"]:
# Load the image.
import os
import sys
import shutil
import numpy as np
import pylab as plt
from tqdm import tqdm
from glob import glob
from vessel import Vessel
from utils import load_image_num
FOLDER_NAME = sys.argv[1:][0]
if __name__ == "__main__":
# Load the previously calculated velocity field data.
data_kalman = Vessel("fields_kalman.dat")
image_location_kalman = "./videos/kalman"
data_smoothed = Vessel("fields_smoothed_kalman.dat")
image_location_smoothed = "./videos/smoothed_kalman"
MAX_NUMBER_FRAMES = 100
# Clear previous frames.
if os.path.isdir(image_location_kalman):
shutil.rmtree(image_location_kalman)
if os.path.isdir(image_location_smoothed):
shutil.rmtree(image_location_smoothed)
os.mkdir(image_location_kalman)
os.mkdir(image_location_smoothed)
plt.ioff()
for it in tqdm(np.arange(1, MAX_NUMBER_FRAMES)):
import colors
import pygame
import math
import time
from risk_assessment_model import vessel as vessel_dict, assess_risk
from utils import ang_dis_to_coo
from vessel import Vessel
from target import Target
from utils import rand
""" This module draws the radar screen """
dis_thresh = 350
vessel = Vessel(vessel_dict)
def draw_arrow(screen, colour, start, end, arrow_size=10):
pygame.draw.line(screen, colour, start, end, arrow_size // 2)
rotation = math.degrees(math.atan2(start[1] - end[1],
end[0] - start[0])) + 90
pygame.draw.polygon(
screen, colour,
((end[0] + arrow_size * math.sin(math.radians(rotation)),
end[1] + arrow_size * math.cos(math.radians(rotation))),
(end[0] + arrow_size * math.sin(math.radians(rotation - 120)),
end[1] + arrow_size * math.cos(math.radians(rotation - 120))),
(end[0] + arrow_size * math.sin(math.radians(rotation + 120)),
end[1] + arrow_size * math.cos(math.radians(rotation + 120)))))
def draw_target(radarDisplay, target, x, y, fontRenderer):
pygame.draw.circle(radarDisplay, target.color, (x, y), 10)
rel_x = math.cos(math.radians(target.direction)) * target.velocity
import json
import time
import paho.mqtt.client as mqtt
from constants import SENSOR_HEIGHT
from garden_state import State
from sensors import moisture, environment, ultrasonic, light
from vessel import Vessel
if __name__ == "__main__":
client = mqtt.Client("home")
client.connect(host="localhost")
moisture_obj = moisture.Moisture()
six_quart = Vessel(17.78, 21.59, 21.59) # 7"x8.5"x8.5"
distance_obj = ultrasonic.Distance(six_quart, SENSOR_HEIGHT)
environ_obj = environment.Environment()
light_obj = light.Light()
state = State(moisture_obj, distance_obj, environ_obj, light_obj)
while True:
data = state.read()
print(data)
client.publish("garden/sensors", json.dumps(data))
time.sleep(1)
def __init__(self, mapname, ctrlnames, scenname, name='s1'):
self.name = scenname + "-" + "-".join(ctrlnames)
self.tend = 150 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
if scenname == "s1":
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
elif scenname == "s2":
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
elif scenname == "s3":
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 3.14/2])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
ppf = PurePursuit(mode='pursuit')
else:
# Vessel 1 (Main vessel)
x01 = np.array([10.0, 10.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([100, 125, 3.14/2])
the_map = Map(mapname, gridsize=1., safety_region_length=4.0)
controllers = []
for name in ctrlnames:
if name == "dwa":
controllers.append(DynamicWindow(self.dT,
N2,
the_map))
elif name == "potfield":
controllers.append(PotentialFields(the_map, N2))
if len(controllers) > 1:
controllers[-1].d_max = 20.
elif name == "astar":
controllers.append(AStar(x01, xg1, the_map))
controllers.append(LOSGuidance(switch_criterion="progress"))
elif name == "hastar":
controllers.append(HybridAStar(x01, xg1, the_map))
controllers.append(LOSGuidance(switch_criterion="progress"))
v0 = Vessel(x01,
xg1,
self.h,
self.dT,
self.N,
controllers,
is_main_vessel=True,
vesseltype='viknes')
vessels = [v0]
if scenname == "s3":
ppf.cGoal = v0.x
vf = Vessel(x0f,
xgf,
self.h,
self.dT,
self.N,
[ppf],
is_main_vessel=False,
vesseltype='viknes')
vf.u_d = 2.5
vessels.append(vf)
self.world = World(vessels, the_map)
return
if name == 's1':
the_map = Map('s1', gridsize=2.0, safety_region_length=4.0)
self.tend = 150
self.h = 0.05
self.dT = 0.5
self.N = int(np.around(self.tend / self.h)) +1
# Vessel 1 (Main vessel)
x01 = np.array([0, 0, 0, 1.0, 0, 0])
xg1 = np.array([150, 150, np.pi/4])
myastar = AStar(x01, xg1, the_map)
mypp = PurePursuit()
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myastar, mypp], is_main_vessel=True, vesseltype='viknes')
self.world = World([v1], the_map)
elif name == 'collision':
the_map = Map('s1')
self.tend = 100
self.h = 0.05
self.dT = 0.5
self.N = int(np.around(self.tend / self.h)) + 1
self.h = 0.05
# Vessel 1 (Main vessel)
x01 = np.array([0, 0, 0, 3.0, 0, 0])
xg1 = np.array([120, 120, np.pi/4])
myLOS1 = LOSGuidance()
myAstar = HybridAStar(x01, xg1, the_map)
v1 = Vessel(x01, xg1,self.h, self.dT, self.N, [myLOS1], is_main_vessel=True, vesseltype='viknes')
v1.waypoints = np.array([[0, 0], [50, 60], [70, 60], [120, 10], [120, 120]])
#v1.waypoints = np.array([[0, 0], [140, 0], [120, 120]])
# Vessel 2
x02 = np.array([0, 120, 0, 3.0, 0, 0])
xg2 = np.array([120, 0, 0])
myLOS2 = LOSGuidance()
v2 = Vessel(x02, xg2,self.h, self.dT, self.N, [myLOS2], is_main_vessel=False, vesseltype='viknes')
v2.waypoints = np.array([[0, 120], [120, 120], [120, 0]])
# Vessel 3
x03 = np.array([0, 50, np.pi/2, 3.0, 0, 0])
xg3 = np.array([140, 0, 0])
myLOS3 = LOSGuidance()
v3 = Vessel(x03, xg3, self.h, self.dT, self.N, [myLOS3], is_main_vessel=False, vesseltype='viknes')
v3.waypoints = np.array([[0, 50], [0, 120], [120, 120]])
self.world = World([v1, v2], the_map)
elif name == 's1-dynwnd':
the_map = Map('s1', gridsize=0.5,safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v1.goal = np.array([140, 140, 0])
self.world = World([v1], the_map)
elif name == 's1-potfield':
the_map = Map('s1', gridsize=0.5, safety_region_length=4.0)
self.tend = 140.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0, 2.5, 0, 0])
xg = np.array([140, 140, 0])
potfield = PotentialFields(the_map, N2)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [potfield], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s1-hybridastar':
the_map = Map('s1', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, 0.0])
hastar = HybridAStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s1-astar':
the_map = Map('s1', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, np.pi/4])
astar = AStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [astar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s2-potfield':
the_map = Map('s2', gridsize=1.0, safety_region_length=4.0)
self.tend = 150.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0, 2.5, 0, 0])
xg = np.array([140, 140, 0])
potfield = PotentialFields(the_map, N2)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [potfield], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name == 's2-dynwnd':
the_map = Map('s2', gridsize=0.5,safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v1.goal = np.array([140, 140, 0])
self.world = World([v1], the_map)
elif name=='s2-astar':
the_map = Map('s2', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, np.pi/4])
astar = AStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [astar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s2-hybridastar':
the_map = Map('s2', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, 0.0])
hastar = HybridAStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s3-potfield':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 120 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([60.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
potfield = PotentialFields(the_map, N2)
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [potfield], is_main_vessel=True, vesseltype='viknes')
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 's3-dynwnd':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
myDynWnd = DynamicWindow(self.dT, N2, the_map)
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
#v1.goal = np.array([140, 140, 0])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 's3-astar':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
astar = AStar(x01,xg1,the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [astar, pp], is_main_vessel=True, vesseltype='viknes')
#v1.goal = np.array([140, 140, 0])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 's3-hybridastar':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 300 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 3.14/2])
hastar = HybridAStar(x01, xg1, the_map)
los = LOSGuidance(switch_criterion="progress")
dwa = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
dwa.alpha = .5
dwa.beta = .4
dwa.gamma = .4
pot = PotentialFields(the_map, N2)
pot.d_max = 40
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [hastar, los, dwa], is_main_vessel=True, vesseltype='viknes')
#v1.goal = np.array([140, 140, 0])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 'hastar+dynwnd':
the_map = Map('s2', gridsize=1.0, safety_region_length=4.0)
self.tend = 100 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
hastar = HybridAStar(x01, xg1, the_map)
pp = PurePursuit(mode='goal-switcher')
los = LOSGuidance(switch_criterion="progress")
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
myDynWnd.alpha = .5
myDynWnd.beta = .4
myDynWnd.gamma = .4
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [hastar, los, myDynWnd], is_main_vessel=True, vesseltype='viknes')
xo0 = np.array([50.,130,5*np.pi/4,0.0,0,0])
xog = np.array([250,110,0])
vobj2 = Vessel(xo0, xog, self.h, self.dT, self.N, [], is_main_vessel=False, vesseltype='hurtigruta')
self.world = World([vobj, vobj2], the_map)
myDynWnd.world = self.world
elif name == 'cb-test':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([60.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
pp = PurePursuit()
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [pp], is_main_vessel=True, vesseltype='viknes')
# Other vessel
xo0 = np.array([40.,60,-np.pi/4,1.5,0,0])
xog = np.array([250,110,0])
cb = ConstantBearing(vobj.x)
vobj2 = Vessel(xo0, xog, self.h, self.dT, self.N, [cb], is_main_vessel=False, vesseltype='hurtigruta')
self.world = World([vobj, vobj2], the_map)
elif name == 'dwacollision':
the_map = Map(gridsize=0.5,safety_region_length=4.5)
self.tend = 60 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([5, 5, np.pi/4, 3.0, 0, 0])
xg1 = np.array([120, 120, np.pi/4])
myLOS1 = LOSGuidance()
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1)
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v1.waypoints = np.array([[0, 0], [120, 120]])
#v1.waypoints = np.array([[0, 0], [140, 0], [120, 120]])
# Vessel 2
x02 = np.array([80, 80, -3*np.pi/4, 3.0, 0, 0])
xg2 = np.array([0, 0, -3*np.pi/4])
myLOS2 = LOSGuidance(u_d = 2.0)
v2 = Vessel(x02, xg2, self.h, self.dT, self.N, [myLOS2], is_main_vessel=False, vesseltype='viknes')
v2.waypoints = np.array([[120, 120], [0, 0]])
self.world = World([v1, v2], the_map)
myDynWnd.the_world = self.world
elif name=='hybridastar':
the_map = Map('s1',gridsize=1.0, safety_region_length=6.0)
self.tend = 140.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([130, 130, 8.0])
hastar = HybridAStar(x0, xg, the_map)
pp = PurePursuit(R2=50, mode="goal-switcher")
dynwnd = DynamicWindow(self.dT, N2, the_map)
dynwnd.alpha = 0.8
dynwnd.beta = 0.1
dynwnd.gamma = 0.1
ptf = PotentialFields(the_map, N2)
ptf.mu = 10
ptf.d_max = 30
ptf.k = 10.
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, pp, dynwnd], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
dynwnd.the_world = self.world
ptf.world = self.world
elif name=='minima':
the_map = Map('minima', gridsize=0.5, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.1
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([30,30,0, 2.0,0,0])
xg = np.array([140, 140, 3*np.pi/4])
hastar = HybridAStar(x0, xg, the_map)
los = LOSGuidance()
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, los], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='pptest':
the_map = Map('', gridsize=0.5, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.1
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0,0,0, 2.0,0,0])
xg = np.array([140, 140, 3*np.pi/4])
pp = PurePursuit()
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [pp], is_main_vessel=True, vesseltype='viknes')
vobj.waypoints = np.array([(50.,50.), (50., 0.), (100., 100.)])
self.world = World([vobj], the_map)
else:
print("You might have spelled the scenario name wrong...")
self.tend = 0
self.dT = 0.1
self.h = 0.05
self.N = 0
self.world = None
def process_images(tile_size=100,
step_size=100,
max_number_images=20,
dt=DELTA_TIME):
"""Process multiple images."""
crs = []
vfs = []
weights = []
image_numbers = []
print("Processing Images.")
for image_number in tqdm(np.arange(1, max_number_images)):
img1 = load_image_num(image_number, FOLDER_NAME)
img2 = load_image_num(image_number + FRAME_INTERVAL, FOLDER_NAME)
cr, vf, sims = estimate_velocity_field(img1, img2, tile_size,
step_size, dt)
crs.append(cr)
vfs.append(vf)
weights.append(sims)
image_numbers.append(image_number)
# Kalman filter data for each tile and get computed smoothed kalman filtered data.
crs = np.array(crs)
vfs = np.array(vfs)
kalman_vfs = np.zeros_like(vfs)
smoothed_kalman_vfs = np.zeros_like(vfs)
print("Applying filter.")
n_tiles = len(crs[0])
for index in tqdm(np.arange(n_tiles)):
tile_field_series = [vf[index] for vf in vfs]
tile_weight_series = [w[index] for w in weights]
k_vx, k_vy = kalman(tile_field_series)
new_k = np.matrix(tuple(zip(k_vy, k_vx)))
new_u = smooth_kalman(k_vx, k_vy, tile_weight_series)
for seq in range(len(kalman_vfs)):
kalman_vfs[seq][index] = new_k[seq]
smoothed_kalman_vfs[seq][index] = new_u[seq]
# Save this data.
v = Vessel("fields_kalman.dat")
v.crs = crs
v.vfs = kalman_vfs
v.image_numbers = image_numbers
v.save()
v = Vessel("fields_smoothed_kalman.dat")
v.crs = crs
v.vfs = smoothed_kalman_vfs
v.image_numbers = image_numbers
v.save()
(xy[0] + 1, xy[1] + 1), (xy[0] - 1, xy[1] + 1),
(xy[0] - 1, xy[1] - 1), (xy[0] + 1, xy[1] - 1)]
results = filter(self.in_bounds, results)
results = filter(self.passable, results)
return results
if __name__ == "__main__":
mymap = Map("s2", gridsize=1.0, safety_region_length=5.0)
x0 = np.array([10, 10, np.pi/4, 3.0, 0.0, 0])
xg = np.array([140, 140, 5*np.pi/4])
myvessel = Vessel(x0, xg, 0.05, 0.5, 1, [], True, 'viknes')
myastar = AStar(x0, xg, mymap)
myastar.update(myvessel)
fig = plt.figure()
ax = fig.add_subplot(111, autoscale_on=False)
ax.plot(myvessel.waypoints[:,0],
myvessel.waypoints[:,1],
'-')
ax.plot(x0[0], x0[1], 'bo')
ax.plot(xg[0], xg[1], 'ro')
myvessel.draw_patch(ax, myvessel.x, fcolor='b')
def __init__(self, mapname, ctrlnames, scenname, name='s1'):
self.name = scenname + "-" + "-".join(ctrlnames)
self.tend = 150 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
if scenname == "s1":
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
elif scenname == "s2":
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
elif scenname == "s3":
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 3.14/2])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
ppf = PurePursuit(mode='pursuit')
else:
# Vessel 1 (Main vessel)
x01 = np.array([10.0, 10.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([100, 125, 3.14/2])
the_map = Map(mapname, gridsize=1., safety_region_length=4.0)
controllers = []
for name in ctrlnames:
if name == "dwa":
controllers.append(DynamicWindow(self.dT,
N2,
the_map))
elif name == "potfield":
controllers.append(PotentialFields(the_map, N2))
if len(controllers) > 1:
controllers[-1].d_max = 20.
elif name == "astar":
controllers.append(AStar(x01, xg1, the_map))
controllers.append(LOSGuidance(switch_criterion="progress"))
elif name == "hastar":
controllers.append(HybridAStar(x01, xg1, the_map))
controllers.append(LOSGuidance(switch_criterion="progress"))
v0 = Vessel(x01,
xg1,
self.h,
self.dT,
self.N,
controllers,
is_main_vessel=True,
vesseltype='viknes')
vessels = [v0]
if scenname == "s3":
ppf.cGoal = v0.x
vf = Vessel(x0f,
xgf,
self.h,
self.dT,
self.N,
[ppf],
is_main_vessel=False,
vesseltype='viknes')
vf.u_d = 2.5
vessels.append(vf)
self.world = World(vessels, the_map)
return
if name == 's1':
the_map = Map('s1', gridsize=2.0, safety_region_length=4.0)
self.tend = 150
self.h = 0.05
self.dT = 0.5
self.N = int(np.around(self.tend / self.h)) +1
# Vessel 1 (Main vessel)
x01 = np.array([0, 0, 0, 1.0, 0, 0])
xg1 = np.array([150, 150, np.pi/4])
myastar = AStar(x01, xg1, the_map)
mypp = PurePursuit()
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myastar, mypp], is_main_vessel=True, vesseltype='viknes')
self.world = World([v1], the_map)
elif name == 'collision':
the_map = Map('s1')
self.tend = 100
self.h = 0.05
self.dT = 0.5
self.N = int(np.around(self.tend / self.h)) + 1
self.h = 0.05
# Vessel 1 (Main vessel)
x01 = np.array([0, 0, 0, 3.0, 0, 0])
xg1 = np.array([120, 120, np.pi/4])
myLOS1 = LOSGuidance()
myAstar = HybridAStar(x01, xg1, the_map)
v1 = Vessel(x01, xg1,self.h, self.dT, self.N, [myLOS1], is_main_vessel=True, vesseltype='viknes')
v1.waypoints = np.array([[0, 0], [50, 60], [70, 60], [120, 10], [120, 120]])
#v1.waypoints = np.array([[0, 0], [140, 0], [120, 120]])
# Vessel 2
x02 = np.array([0, 120, 0, 3.0, 0, 0])
xg2 = np.array([120, 0, 0])
myLOS2 = LOSGuidance()
v2 = Vessel(x02, xg2,self.h, self.dT, self.N, [myLOS2], is_main_vessel=False, vesseltype='viknes')
v2.waypoints = np.array([[0, 120], [120, 120], [120, 0]])
# Vessel 3
x03 = np.array([0, 50, np.pi/2, 3.0, 0, 0])
xg3 = np.array([140, 0, 0])
myLOS3 = LOSGuidance()
v3 = Vessel(x03, xg3, self.h, self.dT, self.N, [myLOS3], is_main_vessel=False, vesseltype='viknes')
v3.waypoints = np.array([[0, 50], [0, 120], [120, 120]])
self.world = World([v1, v2], the_map)
elif name == 's1-dynwnd':
the_map = Map('s1', gridsize=0.5,safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v1.goal = np.array([140, 140, 0])
self.world = World([v1], the_map)
elif name == 's1-potfield':
the_map = Map('s1', gridsize=0.5, safety_region_length=4.0)
self.tend = 140.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0, 2.5, 0, 0])
xg = np.array([140, 140, 0])
potfield = PotentialFields(the_map, N2)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [potfield], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s1-hybridastar':
the_map = Map('s1', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, 0.0])
hastar = HybridAStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s1-astar':
the_map = Map('s1', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, np.pi/4])
astar = AStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [astar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s2-potfield':
the_map = Map('s2', gridsize=1.0, safety_region_length=4.0)
self.tend = 150.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0, 2.5, 0, 0])
xg = np.array([140, 140, 0])
potfield = PotentialFields(the_map, N2)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [potfield], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name == 's2-dynwnd':
the_map = Map('s2', gridsize=0.5,safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 0.0, 2.5, 0, 0])
xg1 = np.array([140, 140, 0])
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v1.goal = np.array([140, 140, 0])
self.world = World([v1], the_map)
elif name=='s2-astar':
the_map = Map('s2', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, np.pi/4])
astar = AStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [astar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s2-hybridastar':
the_map = Map('s2', gridsize=2.0, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([140, 140, 0.0])
hastar = HybridAStar(x0, xg, the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, pp], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='s3-potfield':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 120 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([60.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
potfield = PotentialFields(the_map, N2)
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [potfield], is_main_vessel=True, vesseltype='viknes')
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 's3-dynwnd':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
myDynWnd = DynamicWindow(self.dT, N2, the_map)
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
#v1.goal = np.array([140, 140, 0])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 's3-astar':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
astar = AStar(x01,xg1,the_map)
pp = PurePursuit(R2=50)
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [astar, pp], is_main_vessel=True, vesseltype='viknes')
#v1.goal = np.array([140, 140, 0])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 's3-hybridastar':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 300 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
N2 = int(self.tend/self.dT) + 1
# Vessel 1 (Main vessel)
x01 = np.array([100.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 3.14/2])
hastar = HybridAStar(x01, xg1, the_map)
los = LOSGuidance(switch_criterion="progress")
dwa = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
dwa.alpha = .5
dwa.beta = .4
dwa.gamma = .4
pot = PotentialFields(the_map, N2)
pot.d_max = 40
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [hastar, los, dwa], is_main_vessel=True, vesseltype='viknes')
#v1.goal = np.array([140, 140, 0])
# Follower
x0f = np.array([120.,110,-np.pi,1.5,0,0])
xgf = np.array([250,110,0])
pp = PurePursuit(mode='pursuit')
pp.cGoal = vobj.x
vobj3 = Vessel(x0f, xgf, self.h, self.dT, self.N, [pp], is_main_vessel=False, vesseltype='viknes')
vobj3.u_d = 2.5
self.world = World([vobj, vobj3], the_map)
elif name == 'hastar+dynwnd':
the_map = Map('s2', gridsize=1.0, safety_region_length=4.0)
self.tend = 100 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([0.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
hastar = HybridAStar(x01, xg1, the_map)
pp = PurePursuit(mode='goal-switcher')
los = LOSGuidance(switch_criterion="progress")
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1, the_map)
myDynWnd.alpha = .5
myDynWnd.beta = .4
myDynWnd.gamma = .4
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [hastar, los, myDynWnd], is_main_vessel=True, vesseltype='viknes')
xo0 = np.array([50.,130,5*np.pi/4,0.0,0,0])
xog = np.array([250,110,0])
vobj2 = Vessel(xo0, xog, self.h, self.dT, self.N, [], is_main_vessel=False, vesseltype='hurtigruta')
self.world = World([vobj, vobj2], the_map)
myDynWnd.world = self.world
elif name == 'cb-test':
the_map = Map('', gridsize=1.0, safety_region_length=4.0)
self.tend = 80 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([60.0, 0.0, 3.14/4, 2.5, 0, 0])
xg1 = np.array([80, 145, 0])
pp = PurePursuit()
vobj = Vessel(x01, xg1, self.h, self.dT, self.N, [pp], is_main_vessel=True, vesseltype='viknes')
# Other vessel
xo0 = np.array([40.,60,-np.pi/4,1.5,0,0])
xog = np.array([250,110,0])
cb = ConstantBearing(vobj.x)
vobj2 = Vessel(xo0, xog, self.h, self.dT, self.N, [cb], is_main_vessel=False, vesseltype='hurtigruta')
self.world = World([vobj, vobj2], the_map)
elif name == 'dwacollision':
the_map = Map(gridsize=0.5,safety_region_length=4.5)
self.tend = 60 # Simulation time (seconds)
self.h = 0.05 # Integrator time step
self.dT = 0.5 # Controller time step
self.N = int(np.around(self.tend / self.h)) + 1
# Vessel 1 (Main vessel)
x01 = np.array([5, 5, np.pi/4, 3.0, 0, 0])
xg1 = np.array([120, 120, np.pi/4])
myLOS1 = LOSGuidance()
myDynWnd = DynamicWindow(self.dT, int(self.tend/self.dT) + 1)
v1 = Vessel(x01, xg1, self.h, self.dT, self.N, [myDynWnd], is_main_vessel=True, vesseltype='viknes')
v1.waypoints = np.array([[0, 0], [120, 120]])
#v1.waypoints = np.array([[0, 0], [140, 0], [120, 120]])
# Vessel 2
x02 = np.array([80, 80, -3*np.pi/4, 3.0, 0, 0])
xg2 = np.array([0, 0, -3*np.pi/4])
myLOS2 = LOSGuidance(u_d = 2.0)
v2 = Vessel(x02, xg2, self.h, self.dT, self.N, [myLOS2], is_main_vessel=False, vesseltype='viknes')
v2.waypoints = np.array([[120, 120], [0, 0]])
self.world = World([v1, v2], the_map)
myDynWnd.the_world = self.world
elif name=='hybridastar':
the_map = Map('s1',gridsize=1.0, safety_region_length=6.0)
self.tend = 140.0
self.dT = 0.5
self.h = 0.05
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0, 0, 0.0, 2.5, 0.0, 0])
xg = np.array([130, 130, 8.0])
hastar = HybridAStar(x0, xg, the_map)
pp = PurePursuit(R2=50, mode="goal-switcher")
dynwnd = DynamicWindow(self.dT, N2, the_map)
dynwnd.alpha = 0.8
dynwnd.beta = 0.1
dynwnd.gamma = 0.1
ptf = PotentialFields(the_map, N2)
ptf.mu = 10
ptf.d_max = 30
ptf.k = 10.
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, pp, dynwnd], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
dynwnd.the_world = self.world
ptf.world = self.world
elif name=='minima':
the_map = Map('minima', gridsize=0.5, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.1
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([30,30,0, 2.0,0,0])
xg = np.array([140, 140, 3*np.pi/4])
hastar = HybridAStar(x0, xg, the_map)
los = LOSGuidance()
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [hastar, los], is_main_vessel=True, vesseltype='viknes')
self.world = World([vobj], the_map)
elif name=='pptest':
the_map = Map('', gridsize=0.5, safety_region_length=4.0)
self.tend = 120.0
self.dT = 0.5
self.h = 0.1
self.N = int(np.around(self.tend/self.h)) + 1
N2 = int(np.around(self.tend/self.dT)) + 1
x0 = np.array([0,0,0, 2.0,0,0])
xg = np.array([140, 140, 3*np.pi/4])
pp = PurePursuit()
vobj = Vessel(x0, xg, self.h, self.dT, self.N, [pp], is_main_vessel=True, vesseltype='viknes')
vobj.waypoints = np.array([(50.,50.), (50., 0.), (100., 100.)])
self.world = World([vobj], the_map)
else:
print "You might have spelled the scenario name wrong..."
self.tend = 0
self.dT = 0.1
self.h = 0.05
self.N = 0
self.world = None
(xy[0], xy[1] - 1), (xy[0] + 1, xy[1] + 1),
(xy[0] - 1, xy[1] + 1), (xy[0] - 1, xy[1] - 1),
(xy[0] + 1, xy[1] - 1)]
results = filter(self.in_bounds, results)
results = filter(self.passable, results)
return results
if __name__ == "__main__":
mymap = Map("s2", gridsize=1.0, safety_region_length=5.0)
x0 = np.array([10, 10, np.pi / 4, 3.0, 0.0, 0])
xg = np.array([140, 140, 5 * np.pi / 4])
myvessel = Vessel(x0, xg, 0.05, 0.5, 1, [], True, 'viknes')
myastar = AStar(x0, xg, mymap)
myastar.update(myvessel)
fig = plt.figure()
ax = fig.add_subplot(111, autoscale_on=False)
ax.plot(myvessel.waypoints[:, 0], myvessel.waypoints[:, 1], '-')
ax.plot(x0[0], x0[1], 'bo')
ax.plot(xg[0], xg[1], 'ro')
myvessel.draw_patch(ax, myvessel.x, fcolor='b')
#nonpass = np.array(nonpassable)
close,
plot,
imshow,
imread,
xlabel,
ylabel,
xlim,
ylim,
subplot,
figure,
)
if __name__ == "__main__":
# Load the example data.
v = Vessel("batch.dat")
X, y = v.X, v.y
targets = np.nonzero(y == 1)[0]
confusors = np.nonzero(y == 0)[0]
# Load the example data.
ion()
close("all")
max_confusors = 600
figure()
subplot("221")
idx = targets[np.random.randint(1000)]
imshow(X[idx, :])
subplot("222")
