def on_first_run(self):
assert isinstance(self.track_method, TrackMethod)
self.follow_task = None
self.heading_to_pinger = None
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(795000)
shm.navigation_settings.position_controls.set(1)
shm.navigation_settings.optimize.set(0)
self.localizer = Localizer(PINGER_FREQUENCY)
self.has_made_progress = False
self.hydro_watcher = shm.watchers.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.silencer = ThrusterSilencer()
self.pinger = None
self.ping_deviating_checker = ConsistencyCheck(3, 4)
self.ontop_checker = ConsistencyCheck(3, 4)
self.pings = []
def __init__(self,
validator,
locator_task,
target_coords,
vision_group,
heading_task,
forward_target_p=0.001,
*args,
**kwargs):
"""
validator - a function that returns True when the target is visible and
False otherwise.
locator_task - a task that locates the target
target_coords - the coordinates of the target with which to align
vision_group - the shm group for the buoy
"""
super().__init__(*args, **kwargs)
self.validator = validator
self.locator_task = locator_task
self.heading_task = heading_task
def get_center():
return (0, 0)
# TODO use normalized coordinates instead
self.target_task = ForwardTarget(target_coords,
target=get_center,
px=forward_target_p,
dx=forward_target_p / 2,
py=forward_target_p,
dy=forward_target_p / 2,
deadband=(30, 30))
self.target_checker = ConsistencyCheck(5, 5, strict=True)
self.TIMEOUT = 60
def on_first_run(self):
self.update_data()
self.align = Heading(
lambda: self.pipe_results.angle + shm.kalman.heading.get(),
deadband=0.5)
self.alignment_checker = ConsistencyCheck(19, 20)
pipe_found = self.pipe_results.heuristic_score > 0
self.center = DownwardTarget(lambda self=self:
(self.pipe_results.center_x * 100, self.
pipe_results.center_y * 100),
target=(0, 0),
deadband=(1, 1),
px=0.03,
py=0.03,
dx=0.01,
dy=0.01,
valid=pipe_found)
self.logi("Beginning to align to the pipe's heading")
c_center = "Camera Center: " + str((.5, .5))
self.logi("Pipe center" + str(self.pipe_results.center_x) + " " +
str(self.pipe_results.center_y))
self.logi(c_center)
def on_first_run(self, altitude, *args, **kwargs):
self.min_speed = 0.008
self.min_delta = 0.1
self.deque_size = 20
self.altitude_task = Altitude(altitude, p=0.3)
self.stop_cons_check = ConsistencyCheck(3, 3)
self.readings = deque()
self.initial_altitude = shm.dvl.savg_altitude.get()
self.last_altitude = self.initial_altitude
def on_first_run(self):
self.search = SpiralSearch(optimize_heading=True, min_spin_radius=2.0)
self.recovery_check = ConsistencyCheck(5, 5)
self.bins_check = ConsistencyCheck(5, 5)
self.bins_watcher = shm.watchers.watcher()
self.bins_watcher.watch(shm.bin_cover)
self.bins_watcher.watch(shm.bin_yellow_1)
self.bins_watcher.watch(shm.bin_yellow_2)
self.bins_visible = False
self.recovery_watcher = shm.watchers.watcher()
self.recovery_watcher.watch(shm.recovery_vision)
self.recovery_visible = False
self.bins = Bins()
self.recovery = Recovery()
self.task = None
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(8, 10)
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(30,30), px=0.002, py=0.002, dx=0.002, dy=0.002,
valid=pipe_found)
def on_first_run(self):
self.update_data()
self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5)
self.alignment_checker = ConsistencyCheck(49, 50)
pipe_found = self.pipe_results.heuristic_score > 0
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(10,10), px=0.001, py=0.001, dx=0.002, dy=0.002,
valid=pipe_found)
def on_first_run(self, results):
self.update_data(results)
path_found = self.path_results.visible > 0
self.centered_checker = ConsistencyCheck(8, 10)
self.center = DownwardTarget(
lambda self=self:
(self.path_results.center_x, self.path_results.center_y),
target=(0, 0),
deadband=(.05, .05),
px=0.5,
py=0.5,
dx=0.02,
dy=0.02,
valid=path_found)
def on_first_run(self):
self.update_data()
pipe_found = self.pipe_results.heuristic_score > 0
self.centered_checker = ConsistencyCheck(18, 20)
self.center = DownwardTarget(lambda self=self:
(self.pipe_results.center_x * 100, self.
pipe_results.center_y * 100),
target=(0, 0),
deadband=(4, 4),
px=0.03,
py=0.03,
dx=0.01,
dy=0.01,
valid=pipe_found)
self.logi("Beginning to center on the pipe")
self.logi("Pipe center" + str(self.pipe_results.center_x) + " " +
str(self.pipe_results.center_y))
def on_first_run(self):
self.has_made_progress = True
self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False)
def location_validator(buoy):
return self.seen_frames_checker.check(buoy.probability.get() != 0)
self.depth_task = DepthRestore()
self.heading_task = HeadingRestore()
self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH)
self.dodge_vel = -.4 if yellowRight else .4
self.over_task = Sequential(Timed(VelocityY(self.dodge_vel), 2),
DirectionalSurge(6, .4, compensate=True))
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
self.heading_task,
# Depth(0.9, error=.01)
# SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results),
Buoy(red_buoy_results, first_buoy=True, right=not redRight()),
# self.depth_task,
# SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3),
SeqLog("Looking for green buoy"),
LocateBuoyStrafe(lambda: location_validator(green_buoy_results),
right=not greenRight(),
timeout=3),
Buoy(green_buoy_results, right=not greenRight()),
# self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results),
right=not yellowRight(),
timeout=2),
Buoy(yellow_buoy_results, right=not yellowRight(), yellow=True),
HeadingRestore(),
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going around buoys"),
self.over_task)
def on_first_run(self, task, success=18, total=20, *args, **kwargs):
self.cons_check = ConsistencyCheck(success, total)
def on_first_run(self, bend_right):
self.angle_1_checker = ConsistencyCheck(6, 8)
self.angle_2_checker = ConsistencyCheck(6, 8)
def on_first_run(self, search_task, visible, consistent_frames=(3, 5)):
self.found_checker = ConsistencyCheck(*consistent_frames)
def __init__(self, seen_cons_check=(2, 3), unseen_cons_check=(5, 6)):
self.last_obj = None
self.tracking = False
self.seen_cons_check = ConsistencyCheck(*seen_cons_check)
self.unseen_cons_check = ConsistencyCheck(*unseen_cons_check)
def on_first_run(self, test, count, total, invert, result):
# Multiple by 60 to specify in seconds
self.checker = ConsistencyCheck(count * 60, total * 60, default=False)
def __init__(self,
buoy,
right=True,
first_buoy=False,
yellow=False,
align_only=False,
*args,
**kwargs):
super().__init__(*args, **kwargs)
# Instantiate the BuoyRam task
self.buoy = buoy
self.heading_task = HeadingRestore()
if align_only:
self.align_task = AlignTarget(
self.location_validator,
LocateAdjacentBuoy(self.location_validator, right=right),
(self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy,
self.heading_task)
self.ram_task = BuoyAlign(
self.location_validator,
(self.buoy.r_side_x.get, self.buoy.center_y.get),
self.buoy,
self.collision_validator,
self.align_task,
right=right,
first_buoy=first_buoy,
yellow=yellow)
elif yellow:
self.align_task = AlignTarget(
self.location_validator,
LocateAdjacentBuoy(self.location_validator, right=right),
(self.buoy.r_side_x.get, self.buoy.bottom_y.get), self.buoy,
self.heading_task)
self.ram_task = BuoyRam(
self.location_validator,
(self.buoy.r_side_x.get, self.buoy.bottom_y.get),
self.buoy,
self.collision_validator,
self.align_task,
right=right,
first_buoy=first_buoy,
yellow=yellow)
else:
self.align_task = AlignTarget(
self.location_validator,
LocateAdjacentBuoy(self.location_validator, right=right),
(self.buoy.r_side_x.get, self.buoy.center_y.get), self.buoy,
self.heading_task)
self.ram_task = BuoyRam(
self.location_validator,
(self.buoy.r_side_x.get, self.buoy.center_y.get),
self.buoy,
self.collision_validator,
self.align_task,
right=right,
first_buoy=first_buoy,
yellow=yellow)
self.seen_frames_checker = ConsistencyCheck(3, 3, strict=True)
self.collision_checker = ConsistencyCheck(2, 2, strict=True)
self.last_percent_frame = 0
self.PERCENT_FRAME_THRESHOLD = 2.5
self.PERCENT_FRAME_DELTA_THRESHOLD = 10
self.TIMEOUT = 100
def on_first_run(self, test_success, test_fail, count, total, invert):
# Multiply by 60 to specify values in seconds, not ticks
self.checker_success = ConsistencyCheck(count*60, total*60, default=False)
self.checker_fail = ConsistencyCheck(count*60, total*60)
import shm
BUOY_DEPTH = settings.depth
MAX_DEPTH = settings.max_depth
CAM_DIM = (shm.camera.forward_width.get(), shm.camera.forward_height.get())
CAM_CENTER = (shm.camera.forward_width.get() / 2,
shm.camera.forward_height.get() / 2)
shm_vars = [shm.dice0, shm.dice1]
# True for HeadingTarget, False for ForwardTarget
HEADING_TARGET = False
buoy_pick_checker = ConsistencyCheck(15, 20, default=0)
def __correct_buoy(num):
if shm_vars[1].visible.get():
# We assume that we can see both buoys
coords = [(var.center_x.get(), var.center_y.get()) for var in shm_vars]
required_diff = 0.04
axis = not (abs(coords[1][0] - coords[0][0]) > required_diff)
return num if coords[1][axis] > coords[0][axis] else int(not num)
else:
return 0
def update_correct_buoy(num):
def on_first_run(self, speed=0.4):
self.last_shmval = None
self.last_target_heading = None
self.last_target_elevation = 0
self.checker = ConsistencyCheck(count=3, total=7)
def on_first_run(self):
self.has_made_progress = True
self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False)
def location_validator(buoy):
return self.seen_frames_checker.check(buoy.probability.get() != 0)
self.depth_task = DepthRestore()
self.heading_task = HeadingRestore()
self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH)
self.dodge_vel = -.4 if yellowRight else .4
self.over_task = Timed(VelocityX(.4), 8)
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
Conditional(
MasterConcurrent(
Sequential(
self.heading_task,
# Depth(0.9, error=.01)
# SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results),
Buoy(red_buoy_results,
first_buoy=True,
right=redRight()),
# self.depth_task,
# SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3),
SeqLog("Looking for green buoy"),
LocateBuoyStrafe(
lambda: location_validator(green_buoy_results),
right=greenRight(),
timeout=3),
Buoy(green_buoy_results, right=greenRight()),
# self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(
lambda: location_validator(yellow_buoy_results),
right=yellowRight(),
timeout=2),
Buoy(yellow_buoy_results,
right=yellowRight(),
yellow=True),
Log("re-aligning red buoy"),
LocateBuoyStrafe(
lambda: location_validator(red_buoy_results),
right=secondRedRight(),
timeout=2),
Buoy(red_buoy_results,
right=secondRedRight(),
yellow=False,
align_only=True),
),
PreventSurfacing(),
),
on_success=Sequential(
Zero(),
self.heading_task,
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
),
on_fail=Sequential(
Zero(),
self.heading_task,
SeqLog("Going to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
Timed(VelocityX(.4), 8),
)))
def on_first_run(self, vision):
self.seen_cons_check = ConsistencyCheck(4, 5)