def on_first_run(self): shm.hydrophones_settings.track_frequency_target.set(PINGER_FREQUENCY) shm.hydrophones_settings.track_magnitude_threshold.set(TRACK_MAG_THRESH) shm.hydrophones_settings.track_cooldown_samples.set(150000) shm.navigation_settings.position_controls.set(1) shm.navigation_settings.optimize.set(0) self.localizer = Localizer(PINGER_FREQUENCY) self.hydro_watcher = watcher() self.hydro_watcher.watch(shm.hydrophones_results_track) self.time_since_last_ping = self.this_run_time self.pinger_positions = collections.deque(maxlen=7) self.PINGS_LISTEN = 5 self.CONSISTENT_PINGS = 3 self.listens = 0 self.queued_moves = [] self.pinger_found = False self.silencer = ThrusterSilencer() self.available_depths = set([HYDROPHONES_SEARCH_DEPTH, HYDROPHONES_SEARCH_DEPTH + 0.4, HYDROPHONES_SEARCH_DEPTH - 0.4]) self.last_depth = None self.observe_from(get_sub_position())
def on_first_run(self, *args, **kwargs): self.watcher = watcher() self.watcher.watch(shm.wire_results) self.consistent_objs = {t: ConsistentObject() for t in self.bar_types} self.pull()
def on_first_run(self, *args, **kwargs): self.watcher = watcher() self.watcher.watch(shm.navigate_results) self.consistent_objs = {t: ConsistentObject() for t in self.bar_types} self.pull()
def __init__(self, *args, **kwargs): super().__init__() self.watcher = watcher() self.watcher.watch(shm.recovery_vision) # Initially pattern-match tubes and ellipses by color and size (later by pos/angle) def color_pattern(color_type_func): pattern = [] for i, c in enumerate(constants.colors): camera_coord = HeadingInvCameraCoord(0, 0, 0) angle = HeadingInvAngle(0, 0) color = color_type_func(c) complex_color = ComplexColor(color.lab_a, color.lab_b, color.ycrcb_cr, color.ycrcb_cb) pattern.append( Match( i, self.ColorObs( None, camera_coord, angle, complex_color, c.size, ))) return pattern self.tube_matcher = Matcher(color_pattern(lambda c: c.tube_color)) self.ellipse_matcher = Matcher( color_pattern(lambda c: c.ellipse_color)) self.table_tracker = ConsistentObject() self.pull()
def on_first_run(self, *args, **kwargs): self.watcher = watcher() self.watcher.watch(shm.recovery_vision) self.pull_shm() self.stacks = [None] * 4 tracker = lambda: Tracker(self.cam_width * Vision.TRACKING_REJECT_WIDTH_RATIO) self.red_stack_tracker = tracker() self.green_stack_tracker = tracker() self.mark_mappings = {color: ConsistentObject() for color in Vision.COLORS} self.region_mappings = {color: ConsistentObject() for color in Vision.COLORS} self.pull()
def __init__(self, *args, **kwargs): super().__init__() # Vision object needs to be ready for rest of mission to access, so must # initialize before on_first_run() shm.vision_debug.color_r.set(0) shm.vision_debug.color_g.set(200) shm.vision_debug.color_b.set(255) self.bins_matcher = Matcher( [], # We don't know the bins' relative positions ahead of time num_trackers=2, ) self.watcher = watcher() self.watcher.watch(shm.bins_vision) self.pull()
def daemon(): watcher = watchers.watcher() watcher.watch(switches) last_sk = switches.soft_kill.get() while 1: watcher.wait(new_update=False) set_all(0) sk = switches.soft_kill.get() if sk and not last_sk: blink([LEDS["port"]["red"]], 3) if switches.hard_kill.get(): while switches.hard_kill.get(): LEDS["port"]["red"].set(255) sleep(0.5) LEDS["port"]["red"].set(0) set_group(LEDS["starboard"].values(), 255) sleep(0.5) set_group(LEDS["starboard"].values(), 0) blink([LEDS["port"]["green"], LEDS["starboard"]["green"]], 3) if not sk and last_sk: blink([LEDS["port"]["blue"], LEDS["starboard"]["blue"]], 3) last_sk = sk
def daemon(): watcher = watchers.watcher() watcher.watch(switches) last_sk = switches.soft_kill.get() while 1: watcher.wait(new_update=False) set_all(0) sk = switches.soft_kill.get() if sk and not last_sk: blink([LEDS["port"]["red"]], 3) if switches.hard_kill.get(): while switches.hard_kill.get(): LEDS["port"]["red"].set(255) sleep(0.5) LEDS["port"]["red"].set(0) set_group(LEDS["starboard"].values(), 255) sleep(0.5) set_group(LEDS["starboard"].values(), 0) blink([LEDS["port"]["green"], LEDS["starboard"]["green"]], 3) if not sk and last_sk: blink([LEDS["port"]["blue"], LEDS["starboard"]["blue"]], 3) last_sk = sk
# y_acc_in.get(), 0, 0, 0, depth_in.get(), 0 ]]).reshape(8, 1) # Pass in ftarray, shared memory handle to controller kalman_position = PositionFilter(kalman_xHat) watchers = dict() for var in [ hdg_input_var, rate_var, rate_var_imu, pitch_var, pitch_rate_var, roll_var, roll_rate_var, gx4_q0, gx4_q1, gx4_q2, gx4_q3 ]: watchers[var] = watcher() group = eval(var.__module__) watchers[var].watch(group) def get(var): #if watchers[var].has_changed(): # return var.get() #else: # return None return var.get() start = time.time() Thread(target=calibrate_depth).start() # Do a depth calibration on startup
def on_first_run(self, main_task, *args, **kwargs): self.main_task = main_task self.watcher = watcher() self.watcher.watch(shm.navigate_results) self.pull()
y_vel_in.get(), 0, # y_acc_in.get(), 0, 0, 0, depth_in.get() - depth_offset.get(), #depth_in.get() - 8.64, 0]]).reshape(8,1) # Pass in ftarray, shared memory handle to controller kalman_position = PositionFilter(kalman_xHat) watchers = dict() for var in [hdg_input_var, rate_var, rate_var_imu, pitch_var, pitch_rate_var, roll_var, roll_rate_var, gx4_q0, gx4_q1, gx4_q2, gx4_q3]: watchers[var] = watcher() group = eval(var.__module__) watchers[var].watch(group) def get(var): #if watchers[var].has_changed(): # return var.get() #else: # return None return var.get() start = time.time() iteration = 0 while True: while (iteration*dt < (time.time() - start)): yaw_rate = get(rate_var_imu)
#!/usr/bin/env python2 """ Watch for hydrophones derp, and reset it """ from time import sleep from shm import hydrophones_results as hr from shm import sensor_power as sp from shm import watchers watcher = watchers.watcher() watcher.watch(hr) last_ping_count = hr.ping_count.get() num_bad_pings = 0 while True: watcher.wait(new_update=False) if hr.ping_count.get() != last_ping_count: last_ping_count = hr.ping_count.get() if hr.ping_time.get() < 400: num_bad_pings = num_bad_pings + 1 if num_bad_pings >= 3: print "Resetting hydrophones" sp.enable_12v_6.set(0) sleep(0.1) sp.enable_12v_6.set(1) num_bad_pings = 0 else: num_bad_pings = 0