def run(self):
configvar = self.config[self.key]
shared_var = shm._eval(configvar['var'])
messages = []
success = True
warning = False
for test in configvar.sections:
testparams = configvar[test]
testparams['var'] = shared_var
rval = eval(test)(**testparams)
if type(rval) == type(" "): #A returned string indicates failure (or warning)
if "warn:" in rval.lower():
warning = True
else:
success = False
messages.append(rval)
else: #If a boolean was returned, meaning is obvious
success &= rval
#create results dictionary
results = {"key" : self.key,
"pass" : success,
"warn" : warning,
"messages" : messages}
with self.c:
self.results_list.append(results)
self.c.notify()
def process(self, mat):
start_time = time.time()
self.mat = mat
shrunk = resize_shrink(mat, 1 / self.options['scale_factor'])
for name, match in zip(['tentacle', 'squid'], self.matcher.match(shrunk, self.options['debug'])):
results_g = shm._eval('torpedoes_{}'.format(name))
results = results_g.get()
results.visible = False
drawn_mat = mat
if match is not None:
orig_scale_pts = self.scale_points(match.points_map, self.options['scale_factor'])
if self.options['debug']:
drawn_mat = self.draw_board(mat, orig_scale_pts)
self.post('{} matches'.format(name), match.matches_mat)
results.visible = True
top_right = np.array(orig_scale_pts['top_right_corner'])
top_left = np.array(orig_scale_pts['top_left_corner'])
bottom_left = np.array(orig_scale_pts['bottom_left_corner'])
bottom_right = np.array(orig_scale_pts['bottom_right_corner'])
left_height = np.linalg.norm(top_left - bottom_left)
right_height = np.linalg.norm(top_right - bottom_right)
avg_height = (left_height + right_height) / 2
results.height = self.normalized_size(avg_height)
top_width = np.linalg.norm(top_left - top_right)
bottom_width = np.linalg.norm(bottom_left - bottom_right)
avg_width = (top_width + bottom_width) / 2
results.width = self.normalized_size(avg_width)
skew = (right_height - left_height) / avg_height
results.skew = skew if results.height > 0.3 else 0
center = (top_right + top_left + bottom_left + bottom_right) / 4
results.x, results.y = self.normalized(center)
small = orig_scale_pts['small_cutout']
large = orig_scale_pts['large_cutout']
results.small_cutout_x, results.small_cutout_y = self.normalized(small)
results.large_cutout_x, results.large_cutout_y = self.normalized(large)
self.post('{} board'.format(name), drawn_mat)
results_g.set(results)
shm.torpedoes_vision.clock.set(not shm.torpedoes_vision.clock.get())
runtime = time.time() - start_time
min_runtime = 1 / self.options['max_fps']
if min_runtime > runtime:
time.sleep(min_runtime - runtime)
runtime = min_runtime
print('FPS: {}'.format(1 / (runtime)))
def parse_args():
if len(sys.argv) < 4:
raise ValueError('Usage: auv-shm-slider <group> <var> <increment>')
shm_path = '{}.{}'.format(*sys.argv[1:3])
try:
v = shm._eval(shm_path)
except:
raise ValueError('Could not evaluate shm path "{}"'.format(shm_path))
if isinstance(v.get(), int):
try:
inc = int(sys.argv[3])
except ValueError:
raise ValueError('Increment "{}" is not an int'.format(sys.argv[3]))
elif isinstance(v.get(), float):
try:
inc = float(sys.argv[3])
except ValueError:
raise ValueError('Increment "{}" is not a float'.format(sys.argv[3]))
else:
raise ValueError('Unknown increment type')
return Args(shm_path, v, inc)
def process(self, *mats):
self.mats = mats
scale = shm.vision_debug.scale.get()
thickness = shm.vision_debug.thickness.get()
color_r = shm.vision_debug.color_r.get()
color_g = shm.vision_debug.color_g.get()
color_b = shm.vision_debug.color_b.get()
color = (color_b, color_g, color_r)
for i in range(10):
obj = shm._eval('vision_debug{}'.format(i)).get()
for mat in mats:
cv2.putText(
mat,
obj.text.decode('utf8'),
self.denormalized((obj.x, obj.y), mat=mat, round=True),
cv2.FONT_HERSHEY_PLAIN,
scale,
color,
thickness=thickness,
)
for i, mat in enumerate(mats):
self.post(i, mat)
def run(self):
configvar = self.config[self.key]
shared_var = shm._eval(configvar['var'])
messages = []
success = True
warning = False
for test in configvar.sections:
testparams = configvar[test]
testparams['var'] = shared_var
rval = eval(test)(**testparams)
if type(rval) == type(" "): #A returned string indicates failure (or warning)
if "warn:" in rval.lower():
warning = True
else:
success = False
messages.append(rval)
else: #If a boolean was returned, meaning is obvious
success &= rval
#create results dictionary
results = {"key" : self.key,
"pass" : success,
"warn" : warning,
"messages" : messages}
with self.c:
self.results_list.append(results)
self.c.notify()
def onComboSelect(self, event):
group = self.pulldown.GetValue()
newvariables = getvariables(self.preferences, group)
if not newvariables:
print("Error: group not found... Aborted")
return 0
for var in self.plot.variables[:]:
self.removeVar(var)
for var in newvariables:
self.addVar(shm._eval(var), var)
self.plot.SetFocus() # ready to analyze
def onComboSelect(self, event):
group = self.pulldown.GetValue()
newvariables = getvariables(self.preferences, group)
if not newvariables:
print("Error: group not found... Aborted")
return 0
for var in self.plot.variables[:]:
self.removeVar(var)
for var in newvariables:
self.addVar(shm._eval(var), var)
self.plot.SetFocus() # ready to analyze
def debug(self, objs, offset):
for i, obj in enumerate(objs):
abs_index = i + offset
debug_g = shm._eval('vision_debug{}'.format(abs_index))
debug = debug_g.get()
if obj.obs is None:
debug.x, debug.y = 0, 0
debug.text = bytes(str(''), 'utf8')
else:
debug.x, debug.y = obj.obs.x, obj.obs.y
if obj.id is None:
debug.text = bytes(str(abs_index), 'utf8')
else:
debug.text = bytes(constants.colors[obj.id].name, 'utf8')
debug_g.set(debug)
def process(self, img):
print("asdf")
self.img = img
img = img[::2, ::2, :]
h, w, _ = img.shape
shm.camera.forward_height.set(h)
shm.camera.forward_width.set(w)
self.post("Original", img)
set_shared_globals(is_forward=True,
options=self.options,
post=self.post,
img=img)
preprocessed_image = preprocess(img)
threshed = threshold(preprocessed_image)
contours = find_contours(threshed)
funnels = find_funnels(contours)
final = img.copy()
for name, funnel in funnels.items():
shm_group = shm._eval("recovery_vision_forward_{}".format(name))
output = shm_group.get()
output.area = funnel.area
output.center_x = funnel.x
output.center_y = funnel.y
output.probability = funnel.probability
shm_group.set(output)
cv2.circle(final, (int(funnel.x), int(funnel.y)),
int(math.sqrt(funnel.area)), COLORS["BLUE"], 5)
cv2.putText(final, name, (int(funnel.x), int(funnel.y) - 20),
cv2.FONT_HERSHEY_SIMPLEX, 1, COLORS["BLUE"], 2)
self.post("Final", final)
def pull(self):
shm_bins = [
shm.bins_bin0.get(),
shm.bins_bin1.get(),
]
heading = shm.kalman.heading.get()
observations = [self.BinObs(sbin, heading) for sbin in shm_bins if sbin.visible]
self.bins = self.bins_matcher.match(observations)
# Debug locations
for i, bin in enumerate(self.bins):
debug_info_g = shm._eval('vision_debug{}'.format(i))
debug_info = debug_info_g.get()
if bin.obs is None:
debug_info.text = bytes('', 'utf8')
else:
if bin.id is not None:
debug_info.text = bytes('Target bin' if bin.id == self.TARGET_BIN else 'Other bin', 'utf8')
else:
debug_info.text = bytes('Bin {}'.format(i), 'utf8')
debug_info.x, debug_info.y = bin.obs.x, bin.obs.y
debug_info_g.set(debug_info)
# *******************************************************
def print_header(x):
print "-" * len(x)
print x
print "-" * len(x)
print_header("Automated Bollard Pull Data Collection and Curve Fit Utility")
#excessively complex string comparison functional code to deteremine which thruster to control
ban_list = ["__", "watch", "ctype", "auv"]
thruster_input = raw_input("Which thruster do you want to test?\r\n> ")
thruster_list = filter(lambda z: not (reduce(lambda x,y: x or y, map(lambda x: x in z, ban_list))), dir(shm.motor_desires))
matches = map(lambda x: difflib.SequenceMatcher(a=x.lower(), b=thruster_input.lower()).ratio(), thruster_list)
motor_str = thruster_list[matches.index(max(matches))]
print ">> Interpreting your response as", motor_str.upper()
motor = shm._eval("motor_desires." + motor_str) #motor is the shared var we want to control
data = {} #data collection dictionary
RANGE_MIN = -255
RANGE_MAX = 255
exit_list = ["done", "finish", "exit", "bye"]
print_header("Begin data collection phase.")
RISE_DELAY = 10
SAMPLES = 5
SAMPLE_INTERVAL = 0.5
TIME_BETWEEN_SAMPLES = 5
#!/usr/bin/env python3
import argparse
from subprocess import PIPE, run
from tempfile import NamedTemporaryFile
from time import time, sleep
from curses import wrapper
import shm
parser = argparse.ArgumentParser(description='Graph some SHM')
parser.add_argument('variables', metavar='GROUP.VAR', type=str, nargs='+',
help='Variables to graph')
args = parser.parse_args()
tracks = { name: (shm._eval(name), NamedTemporaryFile()) for name in args.variables }
plotfile = NamedTemporaryFile()
with open(plotfile.name, "w") as f:
f.write('set xlabel "Time"\n')
f.write('set ylabel "Value"\n')
f.write('set term dumb\n')
plots = []
for name, (var, filename) in tracks.items():
plots.append('"{}" title "{}" with linespoint'.format(filename.name, name))
f.write("plot ")
f.write(",".join(plots))
f.write("\n")
with open(PREFFILENAME, "r") as f:
preferences = f.readlines()
varnames = getvariables(preferences, args.variables[0])
else:
varnames = args.variables[:]
if varnames is None: # getvariables returned None because group was not found
print "Invalid group %s" % args.variables[0]
quit()
variables = []
for varname in varnames[:]:
try:
var = shm._eval(varname)
except shm.ShmEvalError:
print "Invalid shm variable %s" % varname
varnames.remove(varname)
else:
variables.append(var)
print "Plotting %s" % ' '.join(varnames)
if args.data: # pick up data left by popped plotter
filename = os.path.join(DIRECTORY, args.data[0])
with open(os.path.join(filename), "r") as f:
p = pickle.Unpickler(f)
data = p.load()
with open(PREFFILENAME, "r") as f:
preferences = f.readlines()
varnames = getvariables(preferences, args.variables[0])
else:
varnames = args.variables[:]
if varnames is None: # getvariables returned None because group was not found
print "Invalid group %s" % args.variables[0]
quit()
variables = []
for varname in varnames[:]:
try:
var = shm._eval(varname)
except shm.ShmEvalError:
print "Invalid shm variable %s" % varname
varnames.remove(varname)
else:
variables.append(var)
print "Plotting %s" % ' '.join(varnames)
if args.data: # pick up data left by popped plotter
filename = os.path.join(DIRECTORY, args.data[0])
with open(os.path.join(filename), "r") as f:
p = pickle.Unpickler(f)
data = p.load()
#!/usr/bin/python
import shm
ACTS = 14
for act in range(1, ACTS+1):
shm._eval('actuator_desires.trigger_{0:02d}'.format(act)).set(0)
def __init__(self):
enable_var = shm._eval("vision_modules." + self.buoy_color[0].upper() + self.buoy_color[1:] + "Buoy")
group = shm._eval(self.buoy_color + "_buoy_results")
MissionElement.__init__(self, enable_var, group)
def read_shm_objs(self, name, n):
return [
shm._eval('recovery_{}{}'.format(name, i)).get() for i in range(n)
]
#Shared memory interface for webserver
import shm
shmvars = {}
file = open("vars.txt").readlines()
for line in file:
if "," in line:
k, v = line.split(",")
evalS = v.strip()
shmvars[k.strip()] = shm._eval(evalS)
def v_get(name):
if name in shmvars:
value = shmvars[name].get()
return value
else:
return None
def v_set(name, val):
if (val != ""):
try:
shmvars[name].set(val)
except:
shmvars[name].set(eval(val))
#Shared memory interface for webserver
import shm
shmvars={}
file = open("vars.txt").readlines()
for line in file:
if "," in line:
k,v=line.split(",")
evalS = v.strip()
shmvars[k.strip()]=shm._eval(evalS)
def v_get(name):
if name in shmvars:
value = shmvars[name].get()
return value
else:
return None
def v_set(name, val):
if(val!=""):
try:
shmvars[name].set(val)
except:
shmvars[name].set(eval(val))
def __init__(self, filename, verbose=False, parse_file_end=True):
self.verbose = verbose
self.f = open(filename, 'rb', buffering=4096)
first_line = self.f.readline()
magic_num = first_line[:4]
self.info = first_line[4:]
self.warnings = []
if magic_num != MAGIC_NUMBER:
raise LogParseException("Given file was not an AUV shm log file")
return
#variable dictionary
self.svars = {}
#Parse file shared variable listing
while True:
x, = struct.unpack("=H", self.f.read(2))
if x == GROUP: break #End of table
t, = struct.unpack("=c", self.f.read(1))
s = self.__read_old_string()
try:
v = shm._eval(s)
except ShmEvalError:
self.warnings.append("WARNING: " + s +
" does not match current shared memory")
v = None
typ = type_lookup[ord(t)]
if not v is None:
typv = type(v.get())
if not typ == typv:
if not (set([typ, typv]) == set([str, old_str])):
v = None
self.warnings.append(
"WARNING: " + s + " is of type " + typv.__name__ +
" in local shared memory, but type " +
typ.__name__ + " in log")
self.svars[x] = (v, s, typ)
if verbose:
print("read variable " + str(x) + " named " + s)
if verbose:
print("Done reading variables")
print(str(len(self.svars)) + " variables read")
print("Final dictionary:")
print(str(self.svars))
if len(self.warnings) > 0 and verbose:
print('\n'.join(self.warnings))
self.f.read(2) #Read past time flag which must exist
curpos = self.f.tell()
#read the start time
self.start_time = self.__read_time()
self.end_time = self.start_time #temporary value
self.snapshot_table = []
#Only parse the end of file if desired
#If the logfile is corrupt, we might not want to
#This feature is used primarily for logfile recovery.
if parse_file_end:
self.f.seek(-16, 2) #seek 16 bytes from the end
#read out the snapshot table
while True:
snap_pos, = struct.unpack("=Q", self.f.read(8))
snap_time = self.__read_time()
#read 8 bytes of time
if snap_pos == END_STBL: #end of snapshot table
break
#Check for incomplete logfile condition
#Helpful hint: run auv-shmlog-rebuild to rebuild incomplete files
def incomplete_except():
raise LogParseException(
"Logfile is incomplete.\nUse " + bcolors.FAIL +
"auv-shmlog-rebuild [incomplete log] [new log]" +
bcolors.ENDC + " to rebuild incomplete log files.")
if snap_time < (self.start_time + timedelta(seconds=-10)):
incomplete_except()
self.snapshot_table.append((snap_time, snap_pos))
try:
self.f.seek(-32, 1)
except IOError:
incomplete_except()
#read end time
self.f.seek(-(16 + 8), 1)
self.end_time = self.__read_time()
#return to proper file position for initial playback (first snapshot)
self.f.seek(curpos)
self.done = False
def __init__(self, filename, verbose=False, parse_file_end=True):
self.verbose = verbose
self.f = open(filename, 'rb', buffering=4096)
first_line = self.f.readline()
magic_num = first_line[:4]
self.info = first_line[4:]
self.warnings = []
if magic_num != MAGIC_NUMBER:
raise LogParseException("Given file was not an AUV shm log file")
return
#variable dictionary
self.svars = {}
#Parse file shared variable listing
while True:
x, = struct.unpack("=H", self.f.read(2))
if x == GROUP: break #End of table
t, = struct.unpack("=c", self.f.read(1))
s = self.__read_old_string()
try:
v = shm._eval(s)
except ShmEvalError:
self.warnings.append("WARNING: " + s + " does not match current shared memory")
v = None
typ = type_lookup[ord(t)]
if not v is None:
typv = type(v.get())
if not typ == typv:
if not (set([typ, typv]) == set([str, old_str])):
v = None
self.warnings.append("WARNING: " + s + " is of type " + typv.__name__ +
" in local shared memory, but type " + typ.__name__ + " in log")
self.svars[x] = (v, s, typ)
if verbose:
print("read variable " + str(x) + " named " + s)
if verbose:
print("Done reading variables")
print(str(len(self.svars)) + " variables read")
print("Final dictionary:")
print(str(self.svars))
if len(self.warnings) > 0 and verbose:
print('\n'.join(self.warnings))
self.f.read(2) #Read past time flag which must exist
curpos = self.f.tell()
#read the start time
self.start_time = self.__read_time()
self.end_time = self.start_time #temporary value
self.snapshot_table = []
#Only parse the end of file if desired
#If the logfile is corrupt, we might not want to
#This feature is used primarily for logfile recovery.
if parse_file_end:
self.f.seek(-16,2) #seek 16 bytes from the end
#read out the snapshot table
while True:
snap_pos, = struct.unpack("=Q", self.f.read(8))
snap_time = self.__read_time(); #read 8 bytes of time
if snap_pos == END_STBL: #end of snapshot table
break
#Check for incomplete logfile condition
#Helpful hint: run auv-shmlog-rebuild to rebuild incomplete files
def incomplete_except():
raise LogParseException("Logfile is incomplete.\nUse " + bcolors.FAIL +
"auv-shmlog-rebuild [incomplete log] [new log]" + bcolors.ENDC + " to rebuild incomplete log files.")
if snap_time < (self.start_time + timedelta(seconds=-10)):
incomplete_except()
self.snapshot_table.append((snap_time,snap_pos))
try:
self.f.seek(-32, 1)
except IOError:
incomplete_except()
#read end time
self.f.seek(-(16+8),1)
self.end_time = self.__read_time()
#return to proper file position for initial playback (first snapshot)
self.f.seek(curpos)
self.done = False
def process(self, *mats):
for i, im in enumerate(mats):
for dir in self.directions:
var = shm._eval('poster_status.{}_counter'.format(dir))
var.set(var.get() + 1)
self.post(str(i), im)
def __init__(self,
yaw,
pitch,
position,
min_neg_pwm,
min_pos_pwm,
drag=1.0,
link=None,
name="",
reversed_polarity=False,
broken=False,
vector=False):
"""
Yaw, and pitch are counter-clockwise angles (deg) in a right hand
coordinate system where +z is down (yaw axis) and
+y is starboard (pitch axis)
A yaw of 0 indicates forward thrust is in -x direction
(propels sub forward), a yaw of 90 indicates propulsion to the left
a pitch of 90 indicates upward propulsion
Roll is irrelevant (axis of prop)
position is a 3 element tuple giving the position of the thruster
in the cartesian coordinate system of the sub in the order x, y, z.
+x is forward, +y is starboard, +z is down
The origin is considered to be the center of rotation.
Drag is a property of how well the thruster can deliver power.
e.g. an obstructed thruster might have a drag of 0.5 (1/2 power)
link is a string indicating the name of the shared memory variable
holding the thruster's PWM inside the desires group
reversed_polarity=True essentially indicates that the thruster's
wiring has been flipped, i.e. a negative PWM drives it forwards
vector is non False iff the thruster is "vectored".
A vectored thruster is specified by a 4-tuple that denotes the axis
of the thruster's vectorization and the range, as well as its shm
bindings:
(axis, theta, shm_current_value, shm_desired_value)
The axis should be orthogonal to the thruster's axis of thrust and
theta is the range of rotation CCW from start_vector about axis.
"""
assert type(self) != GenericThruster
assert -90.0 <= pitch <= 90
assert 0 <= drag <= 1.0
self.reversed_polarity = reversed_polarity
self.broken = broken
self.pos = np.array((position))
self.drag = drag
# Link to shared memory variable
self.set = desires.__getattribute__(link).set
self.get = desires.__getattribute__(link).get
self.name = name
self.link = link
#self.set_models()
self.q = quat.Quaternion(hpr=(yaw % 360, pitch, 0.0))
self.calculate_force_and_torque_hat(self.q)
self.vectored = vector is not False
if self.vectored:
self.thrust_vectoring_data = \
VectoringThrusterData(self.pos, vector[0], self.force_hat, vector[1])
self.vector_angle = shm._eval(vector[2])
self.vector_desire = shm._eval(vector[3])
self._thrust_memo = {} # pwm -> thrust mappings
# is this even worth it?, 32Kb of data...
assert (min_neg_pwm < 0 and min_pos_pwm > 0)
self.min_pos_pwm = min_pos_pwm
self.min_neg_pwm = min_neg_pwm
self.max_thrust = self.pwm_to_thrust(self.max_pwm)
self.max_neg_thrust = self.pwm_to_thrust(-self.max_pwm)
self.min_thrust = self.pwm_to_thrust(self.min_pos_pwm)
self.min_neg_thrust = self.pwm_to_thrust(self.min_neg_pwm)
# Variables used for quadratic equation in thrust_to_pwm
self._qvars = [
self.get_qvars(self.curve_reverse),
self.get_qvars(self.curve_forward)
]
def __init__(self):
enable_var = shm._eval("vision_modules." + self.buoy_color[0].upper() +
self.buoy_color[1:] + "Buoy")
group = shm._eval(self.buoy_color + "_buoy_results")
MissionElement.__init__(self, enable_var, group)
def ellipses(self, mat, lab, ycrcb, table_contour, table_thresh, bgr_sp,
morph_kernel):
results = [
shm._eval('recovery_ellipse{}'.format(i)).get()
for i in range(NUM_ELLIPSES)
]
for result in results:
result.visible = False
infos = []
if table_contour is not None:
table_mask = np.zeros(mat.shape[:2], dtype=np.uint8)
cv2.drawContours(table_mask, [table_contour],
-1,
255,
thickness=-1)
table_masked = cv2.bitwise_and(bgr_sp[0], table_mask)
# levelled_blue = cv2.bitwise_or(outer_mat, table_masked)
self.post('levelled image', table_masked)
thresh0 = cv2.adaptiveThreshold(
bgr_sp[1],
255,
cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY_INV,
self.options['block_size'],
self.options['e_c0'],
)
thresh1 = cv2.adaptiveThreshold(
bgr_sp[0],
255,
cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY_INV,
self.options['block_size'],
self.options['e_c1'],
)
self.post('e thresh0', thresh0)
self.post('e thresh1', thresh1)
threshed = cv2.bitwise_or(
thresh0,
thresh1,
mask=table_mask if constants.detect_table else None,
)
# morphed = cv2.morphologyEx(threshed, cv2.MORPH_CLOSE, morph_kernel)
morphed = cv2.erode(threshed, morph_kernel)
self.post('total ellipses morphed', morphed)
_, contours, _ = cv2.findContours(
morphed.copy(),
cv2.RETR_EXTERNAL if constants.detect_table else cv2.RETR_LIST,
cv2.CHAIN_APPROX_SIMPLE,
)
for contour in contours:
info = {'contour': contour}
if is_clipping(mat, contour):
continue
if len(contour) < 5:
continue
if len(info['contour']) < 5:
continue
info['ellipse'] = cv2.fitEllipse(contour)
info['width'], info['len'] = self.normalized_size(
sorted(info['ellipse'][1]))
if info['width'] < self.options['min_ellipse_width']:
continue
ellipse_area = math.pi * np.prod(info['ellipse'][1]) / 4
info['area'] = cv2.contourArea(contour)
ellipticality = ellipse_area / info['area']
if ellipticality > 1:
ellipticality = 1 / ellipticality
if ellipticality < self.options['min_ellipticality']:
continue
minor, major = sorted(info['ellipse'][1])
if major / minor < self.options['min_ellipse_aspect_ratio']:
continue
infos.append(info)
if self.options['debug']:
ellipses_mat = mat.copy()
for info in infos:
cv2.ellipse(ellipses_mat, info['ellipse'], (0, 127, 255), 4)
self.post('ellipses', ellipses_mat)
for result, info in zip(results, sorted(infos,
key=lambda x: -x['area'])):
result.visible = True
result.x, result.y = self.normalized(info['ellipse'][0])
result.angle = get_angle_from_ellipse(info['ellipse'])
result.length = info['len']
color = self.avg_color(info['contour'], lab, ycrcb)
result.lab_a, result.lab_b, result.ycrcb_cr, result.ycrcb_cb = color
for i, result in enumerate(results):
shm._eval('recovery_ellipse{}'.format(i)).set(result)
def tubes(self, mat, lab, ycrcb, lab_sp, hsv_sp):
results = [
shm._eval('recovery_tube{}'.format(i)).get()
for i in range(NUM_TUBES)
]
for result in results:
result.visible = False
thresh0 = cv2.adaptiveThreshold(
lab_sp[1],
255,
cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY,
self.options['block_size'],
self.options['c0'],
)
thresh1 = cv2.adaptiveThreshold(
lab_sp[1],
255,
cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY_INV,
self.options['block_size'],
self.options['c1'],
)
thresh2 = cv2.adaptiveThreshold(
lab_sp[2],
255,
cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY_INV,
self.options['block_size'],
self.options['c2'],
)
self.post('tube thresh0', thresh0)
self.post('tube thresh1', thresh1)
self.post('tube thresh2', thresh2)
# Find contours on single binary image to prevent redundant contours
threshed = cv2.bitwise_or(thresh0, cv2.bitwise_or(thresh1, thresh2))
morph_kernel = cv2.getStructuringElement(
cv2.MORPH_ELLIPSE, (self.options['tube_morph'], ) * 2)
morphed = cv2.morphologyEx(threshed, cv2.MORPH_OPEN, morph_kernel)
self.post('tubes morphed', morphed)
_, contours, _ = cv2.findContours(
morphed.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE,
)
infos = []
for contour in contours:
info = {'contour': contour}
if is_clipping(mat, contour):
continue
info['min_rect'] = cv2.minAreaRect(contour)
info['width'], info['len'] = self.normalized_size(
sorted(info['min_rect'][1]))
if info['len'] < self.options['min_tube_len']:
continue
if info['width'] < self.options['min_tube_width']:
continue
aspect_ratio = info['len'] / info['width']
if aspect_ratio < self.options['min_tube_aspect_ratio']:
continue
info['rect_area'] = np.prod(info['min_rect'][1])
infos.append(info)
if self.options['debug']:
contours_mat = mat.copy()
self.draw_contours(contours_mat, *[i['contour'] for i in infos])
self.post('tube contours', contours_mat)
for result, info in zip(results,
sorted(infos, key=lambda x: -x['rect_area'])):
result.visible = True
result.x, result.y = self.normalized(info['min_rect'][0])
result.angle = get_angle_from_rotated_rect(info['min_rect'])
result.length = info['len']
color = self.avg_color(info['contour'], lab, ycrcb)
result.lab_a, result.lab_b, result.ycrcb_cr, result.ycrcb_cb = color
for i, result in enumerate(results):
shm._eval('recovery_tube{}'.format(i)).set(result)
def toggle_module(self, module_name):
module = module_name.split("_")[0]
print("Toggling module {}".format(module))
module_var = shm._eval("vision_modules.{}".format(module))
module_var.set(not module_var.get())
def __init__(self, yaw, pitch, position, min_neg_pwm, min_pos_pwm, drag=1.0, link=None, name="", reversed_polarity=False, broken=False, vector=False):
"""
Yaw, and pitch are counter-clockwise angles (deg) in a right hand
coordinate system where +z is down (yaw axis) and
+y is starboard (pitch axis)
A yaw of 0 indicates forward thrust is in -x direction
(propels sub forward), a yaw of 90 indicates propulsion to the left
a pitch of 90 indicates upward propulsion
Roll is irrelevant (axis of prop)
position is a 3 element tuple giving the position of the thruster
in the cartesian coordinate system of the sub in the order x, y, z.
+x is forward, +y is starboard, +z is down
The origin is considered to be the center of rotation.
Drag is a property of how well the thruster can deliver power.
e.g. an obstructed thruster might have a drag of 0.5 (1/2 power)
link is a string indicating the name of the shared memory variable
holding the thruster's PWM inside the desires group
reversed_polarity=True essentially indicates that the thruster's
wiring has been flipped, i.e. a negative PWM drives it forwards
vector is non False iff the thruster is "vectored".
A vectored thruster is specified by a 4-tuple that denotes the axis
of the thruster's vectorization and the range, as well as its shm
bindings:
(axis, theta, shm_current_value, shm_desired_value)
The axis should be orthogonal to the thruster's axis of thrust and
theta is the range of rotation CCW from start_vector about axis.
"""
assert type(self) != GenericThruster
assert -90.0 <= pitch <= 90
assert 0 <= drag <= 1.0
self.reversed_polarity = reversed_polarity
self.broken = broken
self.pos = np.array((position))
self.drag = drag
# Link to shared memory variable
self.set = desires.__getattribute__(link).set
self.get = desires.__getattribute__(link).get
self.name = name
self.link = link
#self.set_models()
self.q = quat.Quaternion(hpr=(yaw % 360, pitch, 0.0))
self.calculate_force_and_torque_hat(self.q)
self.vectored = vector is not False
if self.vectored:
self.thrust_vectoring_data = \
VectoringThrusterData(self.pos, vector[0], self.force_hat, vector[1])
self.vector_angle = shm._eval(vector[2])
self.vector_desire = shm._eval(vector[3])
self._thrust_memo = {} # pwm -> thrust mappings
# is this even worth it?, 32Kb of data...
assert(min_neg_pwm < 0 and min_pos_pwm > 0)
self.min_pos_pwm = min_pos_pwm
self.min_neg_pwm = min_neg_pwm
self.max_thrust = self.pwm_to_thrust(self.max_pwm)
self.max_neg_thrust = self.pwm_to_thrust(-self.max_pwm)
self.min_thrust = self.pwm_to_thrust(self.min_pos_pwm)
self.min_neg_thrust = self.pwm_to_thrust(self.min_neg_pwm)
# Variables used for quadratic equation in thrust_to_pwm
self._qvars = [self.get_qvars(self.curve_reverse),
self.get_qvars(self.curve_forward)]
