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