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