class FirstPipeGroupFirst(Task):
# Checks whether the first pipe group in shm is the first pipe we should follow.
# Succeeds if the first pipe group is consistently the right one, fails otherwise
def on_first_run(self, bend_right):
self.angle_1_checker = ConsistencyCheck(6, 8)
self.angle_2_checker = ConsistencyCheck(6, 8)
def on_run(self, bend_right):
angle_1 = shm.path_results.angle_1.get()
angle_2 = shm.path_results.angle_2.get()
v1 = np.complex128([np.exp(1j * angle_1)]).view(np.float64)
v2 = np.complex128([np.exp(1j * angle_2)]).view(np.float64)
cp = np.cross(v1, v2)
cond = (cp > 0) ^ bend_right
if self.angle_1_checker.check(cond):
self.finish()
if self.angle_2_checker.check(not cond):
self.finish(success=False)
return
diff = math.atan(
math.sin((angle_2 - angle_1)) / math.cos((angle_2 - angle_1)))
# TODO this might not be working
print(angle_1, angle_2, diff)
if self.angle_1_checker.check(
diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish()
if self.angle_2_checker.check(
diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish(success=False)
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 __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):
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 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.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)
class DualConsistency(Task):
"""edited from will_common"""
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)
def on_run(self, test_success, test_fail, count, total, invert):
test_result_success = call_if_function(test_success)
test_result_fail = call_if_function(test_fail)
if self.checker_success.check(not test_result_success if invert else test_result_success):
self.finish(success=True)
elif self.checker_fail.check(not test_result_fail if invert else test_result_fail):
self.finish(success=False)
class IdentifyGate(Task):
"""
Finish when we can see a good enough amount of the gate
"""
min_bbar_width = 0.15
def on_first_run(self, vision):
self.seen_cons_check = ConsistencyCheck(4, 5)
def on_run(self, vision):
self.seen_cons_check.add(vision.bottom is not None and \
# (vision.left is not None or vision.right is not None) and \
bbar_width_ratio(vision) >= self.min_bbar_width)
if self.seen_cons_check.check():
self.finish()
class IdentifyGate(Task):
"""
Finish when we can see a good enough amount of the gate
"""
min_bbar_width = 0.3
def on_first_run(self, vision):
self.seen_cons_check = ConsistencyCheck(4, 5)
def on_run(self, vision):
self.seen_cons_check.add(vision.bottom is not None and \
# (vision.left is not None or vision.right is not None) and \
bbar_width_ratio(vision) >= self.min_bbar_width)
if self.seen_cons_check.check():
self.finish()
class center(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
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)
#self.logi("Beginning to center on the pipe")
def on_run(self):
self.update_data()
self.center()
#self.logi("Results Y: {}".format(str(self.pipe_results.center_y)))
#self.logi("Center: {}".format(str(get_downward_camera_center()[1])))
if not check_seen():
self.finish(success=False)
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
class AltitudeUntilStop(Task):
"""
Attempt to move to the target altitude until the sub gets stuck
"""
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_run(self, altitude, *args, **kwargs):
if not self.altitude_task.has_ever_finished:
self.altitude_task()
current_altitude = shm.dvl.savg_altitude.get()
self.readings.append((current_altitude, time.time()))
if len(self.readings) > self.deque_size:
self.readings.popleft()
if abs(current_altitude - self.initial_altitude) >= self.min_delta and \
len(self.readings) >= self.deque_size:
delta_altitude = self.readings[-1][0] - self.readings[0][0]
delta_time = self.readings[-1][1] - self.readings[0][1]
speed = abs(delta_altitude / delta_time)
if self.stop_cons_check.check(speed < self.min_speed):
self.logi('Stopped changing altitude, finishing')
self.finish()
else:
self.loge('Bounding altitude reached')
self.finish(success=False)
class center(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
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_run(self):
self.update_data()
self.center()
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
class first_center(Task):
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_run(self, results):
self.update_data(results)
self.center()
# if not check_seen(results):
# self.finish(success=False)
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
class align(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
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, self.pipe_results.center_y),
target=lambda self=self: get_camera_center(self.pipe_results),
deadband=(20,20), px=0.003, py=0.003, dx=0.001, dy=0.001,
valid=pipe_found)
self.logi("Beginning to align to the pipe's heading")
def on_run(self):
self.update_data()
self.align()
self.center()
if self.alignment_checker.check(self.align.finished):
self.finish()
class align(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
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)
#self.logi("Beginning to align to the pipe's heading")
def on_run(self):
self.update_data()
self.align()
self.center()
if not check_seen():
self.finish(success=False)
if self.alignment_checker.check(self.align.finished):
self.finish()
class IdentifyBins(Task):
"""Identifies which bin to drop markers into, centers over it
Start: Both bins visible in downward cam
Finish: Centered over chosen bin
"""
def on_first_run(self, bin_group, heading=None, *args, **kwargs):
self.logi("Centering over bins...")
self.logv("Starting IdentifyBins task")
self.task = DownwardTarget(px=0.0025, py=0.0025)
self.align_checker = ConsistencyCheck(6, 6)
# TODO start alignment task.
self.init_time = self.this_run_time
self.bin_group = bin_group
if bin1.covered == FAST_RUN:
self.target_bin = bin1
else:
self.target_bin = bin2
def on_run(self, bin_group, heading=None):
self.bin1_results = self.bin_group.get()
target = get_camera_center(self.bin1_results)
# TODO Increase deadband as time increases.
self.task((self.bin1_results.x, self.bin1_results.y), target=target, deadband=(25, 25), valid=lambda: self.bin1_results.probability > 0.0)
if self.task.finished:
if heading is None:
target_heading = shm.kalman.heading.get() + self.bin1_results.angle
else:
target_heading = heading()
align_task = Heading(target_heading, deadband=0.5)
align_task()
if self.align_checker.check(align_task.finished):
VelocityX(0)()
VelocityY(0)()
self.finish()
else:
self.align_checker.clear()
def on_finish(self):
self.logi("Centered!")
self.logv('IdentifyBins task finished in {} seconds!'.format(
self.this_run_time - self.init_time))
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)
class Consistent(Task):
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 on_run(self, test, count, total, invert, result):
test_result = call_if_function(test)
if self.checker.check(not test_result if invert else test_result):
self.finish(success=result)
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
class SearchFor(Task):
def on_first_run(self, search_task, visible, consistent_frames=(3, 5)):
self.found_checker = ConsistencyCheck(*consistent_frames)
def on_run(self, search_task, visible, consistent_frames=(3, 5)):
visible = call_if_function(visible)
if self.found_checker.check(visible):
self.finish()
else:
search_task()
class SearchFor(Task):
def on_first_run(self, search_task, visible, consistent_frames=(3, 5)):
self.found_checker = ConsistencyCheck(*consistent_frames)
def on_run(self, search_task, visible, consistent_frames=(3, 5)):
visible = call_if_function(visible)
if self.found_checker.check(visible):
self.finish()
else:
search_task()
class CheckBinsInSight(Task):
""" Checks if the desired bin is in sight of the camera
Used in SearchBinsTask as MasterConcurrent's end condition"""
def on_first_run(self, *args, **kwargs):
self.logv("Checking if bins in sight")
self.init_time = self.this_run_time
self.seen_checker1 = ConsistencyCheck(6, 8)
self.seen_checker2 = ConsistencyCheck(6, 8)
def on_run(self):
cover_results = cover.get()
yellow_results = yellow1.get()
if self.seen_checker1.check(cover_results.probability > 0.0) or \
self.seen_checker2.check(yellow_results.probability > 0.0):
self.finish()
def on_finish(self):
self.logv('CheckBinsInSight task finished in {} seconds!'.format(
self.this_run_time - self.init_time))
class Buoy(Task):
"""
Wrapper around the BuoyRam class that will specifically ram a red or green
buoy
"""
def __init__(self, buoy, *args, **kwargs):
super().__init__(*args, **kwargs)
# Instantiate the BuoyRam task
self.buoy = buoy
self.align_task = AlignTarget(self.location_validator,
AssumeBuoy(self.location_validator),
(self.buoy.center_x.get, self.buoy.center_y.get), self.buoy)
self.ram_task = BuoyRam(self.location_validator,
(self.buoy.center_x.get, self.buoy.center_y.get), self.buoy,
self.collision_validator, self.align_task)
self.seen_frames_checker = ConsistencyCheck(5, 5)
self.last_percent_frame = 0
self.PERCENT_FRAME_THRESHOLD = 10
self.PERCENT_FRAME_DELTA_THRESHOLD = 10
self.TIMEOUT = 100
def on_first_run(self):
self.logv("Starting {} task".format(self.__class__.__name__))
def on_run(self):
# Perform BuoyRam task
if self.this_run_time - self.first_run_time > self.TIMEOUT:
self.finish()
self.loge("Buoy ({}) timed out!".format(self.buoy))
return
self.ram_task()
if self.ram_task.finished:
self.finish()
def on_finish(self):
self.logv("Buoy ({}) task finished in {} seconds!".format(
self.buoy, self.this_run_time - self.first_run_time))
Zero()()
def location_validator(self):
# TODO even more robust location validator
return self.seen_frames_checker.check(self.buoy.probability.get() != 0)
def collision_validator(self):
# TODO even more robust collision validator
if not shm.gpio.wall_1.get():
self.logi("Detected buoy ram using touch sensor!")
return True
current = self.buoy.percent_frame.get()
if current >= self.PERCENT_FRAME_THRESHOLD:
if abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD:
return True
self.last_percent_frame = current
return False
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.locator_task = LocateBuoy(self.location_validator)
self.locator_task = AssumeBuoy(self.location_validator)
self.align_task = AlignTarget(self.location_validator,
self.locator_task, (yellow_buoy_results.top_x.get,
yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)
self.concurrent_align_task = AlignTarget(self.location_validator,
self.locator_task, (yellow_buoy_results.top_x.get,
yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)
self.MAX_RAM_SPEED = 0.6
self.ram_speed = self.MAX_RAM_SPEED
self.ram_task = RamTarget(self.location_validator,
self.collision_validator, self.locator_task,
self.concurrent_align_task, ram_speed=lambda: self.ram_speed)
self.ready_to_ram_checker = ConsistencyCheck(40, 40)
self.seen_frames_checker = ConsistencyCheck(5, 5)
SCUTTLE_TIME = 2.0
BUOY_MOUNTING_DISTANCE = 0.55
self.scuttle = Sequential(Log("Rising to prepare for scuttle"),
RelativeToInitialDepth(-0.2, error=0.02),
Log("Moving on top of Yellow Buoy"),
MoveX(BUOY_MOUNTING_DISTANCE),
Log("Beginning %0.2f second scuttle!" % SCUTTLE_TIME),
MasterConcurrent(Timer(SCUTTLE_TIME), VelocityX(1.0), RelativeToInitialDepth(3.0, error=0.03)),
Log("Scuttle complete. Returning to targeting position."),
Depth(1.0)
# GoToPosition(lambda: self.target_position[0], lambda: self.target_position[1], depth=lambda: self.target_depth)
)
self.tasks = Sequential(self.locator_task, self.align_task,
self.ram_task, self.scuttle)
#self.last_percent_frame = 0
#self.PERCENT_FRAME_DELTA_THRESHOLD = 10
self.PERCENT_FRAME_THRESHOLD = 2
self.PERCENT_FRAME_SLOWDOWN_THRESHOLD = 0.4
self.TIMEOUT = 100
class _TrackPinger(Task):
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 update(self):
self.shmval = shm.hydrophones_results_track.tracked_ping_heading.get()
# print('last: ' + str(self.last_shmval) + ', new: ' + str(self.shmval))
if self.last_shmval is None or self.shmval != self.last_shmval:
lltarget_heading = self.last_target_heading if self.last_target_heading is not None else self.shmval
self.last_shmval = self.shmval
self.last_target_heading = self.shmval
self.last_target_elevation = shm.hydrophones_results_track.tracked_ping_elevation.get(
)
self.log('heading: ' + str(self.last_target_heading), level='info')
self.log('elevation: ' + str(self.last_target_elevation),
level='info')
return self.checker.check(
self.angle_diff(self.get_target_heading(), lltarget_heading) >
90)
return False
def get_target_heading(self):
return self.last_target_heading
def get_target_elevation(self):
return self.last_target_elevation
def angle_diff(self, a, b):
return math.degrees(
abs(
math.atan2(math.sin(math.radians(a) - math.radians(b)),
math.cos(math.radians(a) - math.radians(b)))))
def calc_speed(self, speed):
diff = self.angle_diff(self.get_target_heading(),
shm.kalman.heading.get())
print("diff: " + str(diff))
if diff < 45:
print(min((45 - diff) / 20, speed))
return min((45 - diff) / 20, speed)
else:
print("0")
return 0
def on_run(self, speed):
if self.update():
VelocityX(0)()
self.finish()
return
Heading(self.get_target_heading())()
VelocityX(self.calc_speed(speed))()
class ConsistentTask(Task):
"""
Finishes when a non-finite task is consistently finished
"""
def on_first_run(self, task, success=18, total=20, *args, **kwargs):
self.cons_check = ConsistencyCheck(success, total)
def on_run(self, task, *args, **kwargs):
task()
if self.cons_check.check(task.finished):
self.finish()
def on_first_run(self, altitude, *args, **kwargs):
self.min_speed = 0.008
self.min_delta = 0.1
self.deque_size = 20
self.success = False
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
class ConsistentTask(Task):
"""
Checks to make sure a non-finite task consistently is finished
"""
def on_first_run(self, task, success=18, total=20, *args, **kwargs):
self.cons_check = ConsistencyCheck(success, total)
def on_run(self, task, *args, **kwargs):
task()
if self.cons_check.check(task.finished):
self.finish()
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, self.pipe_results.center_y),
target=lambda self=self: get_camera_center(self.pipe_results),
deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001,
valid=pipe_found)
self.logi("Beginning to center on the pipe")
class align(Task):
def on_first_run(self):
self.align = PIDLoop(output_function=RelativeToCurrentHeading(), target=0, input_value=lambda: shm.pipe_results.angle.get(), negate=True, deadband=1)
self.alignment_checker = ConsistencyCheck(19, 20)
self.logi("Beginning to align to the pipe's heading")
def on_run(self):
self.align()
if self.alignment_checker.check(self.align.finished):
self.finish()
def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None):
self.logi("Centering over bins...")
self.logv("Starting IdentifyBins task")
self.center_valid = False
self.center_coords = (0, 0)
self.task = DownwardTarget(px=0.0025, py=0.0025,
point=lambda: self.center_coords,
valid=lambda: self.center_valid)
self.center_checker = ConsistencyCheck(15, 17)
self.align_checker = ConsistencyCheck(6, 6)
# TODO start alignment task.
self.init_time = self.this_run_time
self.uncovered_bin_vector = None
self.seen_two = False
cam = get_downward_camera()
self.cover_tracker = Tracker(cam['width'] * 0.15)
self.yellow_tracker = Tracker(cam['width'] * 0.15)
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, bin_group, heading=None, *args, **kwargs):
self.logi("Centering over bins...")
self.logv("Starting IdentifyBins task")
self.task = DownwardTarget(px=0.0025, py=0.0025)
self.align_checker = ConsistencyCheck(6, 6)
# TODO start alignment task.
self.init_time = self.this_run_time
self.bin_group = bin_group
if bin1.covered == FAST_RUN:
self.target_bin = bin1
else:
self.target_bin = bin2
class FirstPipeGroupFirst(Task):
# Checks whether the first pipe group in shm is the first pipe we should follow.
# Succeeds if the first pipe group is consistently the right one, fails otherwise
def on_first_run(self, bend_right):
self.angle_1_checker = ConsistencyCheck(6, 8)
self.angle_2_checker = ConsistencyCheck(6, 8)
def on_run(self, bend_right):
angle_1 = shm.path_results.angle_1.get()
angle_2 = shm.path_results.angle_2.get()
diff = math.atan(
math.sin((angle_2 - angle_1)) / math.cos((angle_2 - angle_1)))
# TODO this might not be working
print(angle_1, angle_2, diff)
if self.angle_1_checker.check(
diff > 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish()
if self.angle_2_checker.check(
diff < 0 ^ (angle_1 < angle_2) ^ (not bend_right)):
self.finish(success=False)
def __init__(self, buoy, *args, **kwargs):
super().__init__(*args, **kwargs)
# Instantiate the BuoyRam task
self.buoy = buoy
self.align_task = AlignTarget(self.location_validator,
AssumeBuoy(self.location_validator),
(self.buoy.center_x.get, self.buoy.center_y.get), self.buoy)
self.ram_task = BuoyRam(self.location_validator,
(self.buoy.center_x.get, self.buoy.center_y.get), self.buoy,
self.collision_validator, self.align_task)
self.seen_frames_checker = ConsistencyCheck(5, 5)
self.last_percent_frame = 0
self.PERCENT_FRAME_THRESHOLD = 10
self.PERCENT_FRAME_DELTA_THRESHOLD = 10
self.TIMEOUT = 100
class AlignTarget(Task):
"""
Aligns using ForwardTarget on a target coordinate, while ensuring that the
target is visible
"""
def __init__(self, validator, locator_task, target_coords, vision_group, forward_target_p=0.003, *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
"""
super().__init__(*args, **kwargs)
self.validator = validator
self.locator_task = locator_task
def get_center():
return get_forward_camera_center()
self.target_task = ForwardTarget(target_coords, target=get_center,
px=forward_target_p, dx=forward_target_p/3,
py=forward_target_p, dy=forward_target_p/3, deadband=(30, 30))
self.target_checker = ConsistencyCheck(10, 10)
self.TIMEOUT = 60
def on_first_run(self):
self.logv("Starting {} task".format(self.__class__.__name__))
def on_run(self):
if self.this_run_time - self.first_run_time > self.TIMEOUT:
self.finish()
self.loge("{} timed out!".format(self.__class__.__name__))
return
self.logv("Running {}".format(self.__class__.__name__))
if self.validator():
self.target_task()
else:
self.locator_task()
if self.target_checker.check(self.target_task.finished):
self.finish()
def on_finish(self):
self.logv('{} task finished in {} seconds!'.format(
self.__class__.__name__,
self.this_run_time - self.first_run_time))
def __init__(self, validator, locator_task, target_coords, vision_group, forward_target_p=0.003, *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
"""
super().__init__(*args, **kwargs)
self.validator = validator
self.locator_task = locator_task
def get_center():
return get_forward_camera_center()
self.target_task = ForwardTarget(target_coords, target=get_center,
px=forward_target_p, dx=forward_target_p/3,
py=forward_target_p, dy=forward_target_p/3, deadband=(30, 30))
self.target_checker = ConsistencyCheck(10, 10)
self.TIMEOUT = 60
class ConsistentTask(Task):
"""
Finishes when a non-finite task is consistently finished
"""
def on_first_run(self, task, success=18, total=20, *args, **kwargs):
self.cons_check = ConsistencyCheck(success, total)
def on_run(self, task, debug=False, *args, **kwargs):
task()
if self.cons_check.check(task.finished):
self.finish()
if debug:
success_char = ["x", "^"][self.cons_check.results[-1] == 1]
dropped_char = ["x", "^"][self.cons_check.results[0] == 1]
self.logd("{}/{}/{}/{}/{}".format(
success_char,
sum(x == 1 for x in self.cons_check.results),
self.cons_check.count,
self.cons_check.total,
dropped_char,
))
class AltitudeUntilStop(Task):
"""
Attempt to move to the target altitude until the sub gets stuck
"""
def on_first_run(self, altitude, *args, **kwargs):
self.min_speed = 0.008
self.min_delta = 0.1
self.deque_size = 20
self.success = False
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_run(self, altitude, *args, **kwargs):
if not self.altitude_task.has_ever_finished:
self.altitude_task()
current_altitude = shm.dvl.savg_altitude.get()
self.readings.append((current_altitude, time.time()))
if len(self.readings) > self.deque_size:
self.readings.popleft()
if abs(current_altitude - self.initial_altitude) >= self.min_delta and \
len(self.readings) >= self.deque_size:
delta_altitude = self.readings[-1][0] - self.readings[0][0]
delta_time = self.readings[-1][1] - self.readings[0][1]
speed = abs(delta_altitude / delta_time)
if self.stop_cons_check.check(speed < self.min_speed):
self.logi('Stopped changing altitude, finishing')
self.success = True
self.finish()
else:
self.loge('Bounding altitude reached')
self.finish()
class center(Task):
def update_data(self):
self.pipe_results = shm.pipe_results.get()
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, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(30,30), px=0.003, py=0.003, dx=0.001, dy=0.001,
valid=pipe_found)
self.logi("Beginning to center on the pipe")
def on_run(self):
self.update_data()
self.center()
if self.centered_checker.check(self.center.finished):
self.center.stop()
self.finish()
class ConsistentObject:
"""
Consistency-check an object's existence
"""
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 map(self, obj):
"""
Call with a valid object to count as "seeing the object", or with None
to count as "not seeing the object"
"""
if obj is not None:
self.last_obj = obj
if self.tracking:
self.unseen_cons_check.add(False)
else:
self.seen_cons_check.add(True)
if self.seen_cons_check.check():
self.tracking = True
self.seen_cons_check.clear()
self.unseen_cons_check.clear()
else:
if self.tracking:
self.unseen_cons_check.add(True)
if self.unseen_cons_check.check():
self.tracking = False
self.seen_cons_check.clear()
self.unseen_cons_check.clear()
self.last_obj = None
else:
self.seen_cons_check.add(False)
self.last_obj = None
if self.tracking:
return self.last_obj
else:
return None
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 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 __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
class Buoy(Task):
"""
Wrapper around the BuoyRam class that will specifically ram a red or green
buoy
"""
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):
self.logv("Starting {} task".format(self.__class__.__name__))
def on_run(self):
# Perform BuoyRam task
if self.this_run_time - self.first_run_time > self.TIMEOUT:
self.finish()
self.loge("Buoy ({}) timed out!".format(self.buoy))
return
self.ram_task()
if self.ram_task.finished:
self.finish()
def on_finish(self):
self.logv("Buoy ({}) task finished in {} seconds!".format(
self.buoy, self.this_run_time - self.first_run_time))
Zero()()
def location_validator(self):
# TODO even more robust location validator
return self.seen_frames_checker.check(self.buoy.probability.get() != 0)
def collision_validator(self):
# TODO even more robust collision validator, susceptible to false
# positives
# if not shm.gpio.wall_1.get():
# self.logi("Detected buoy ram using touch sensor!")
# return True
current = self.buoy.percent_frame.get()
# self.logv("Buoy Percent Frame : {}".format(current))
if current >= self.PERCENT_FRAME_THRESHOLD:
# if self.collision_checker.check(abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD):
# self.logv("Returned true!")
return True
# self.last_percent_frame = current
return False
class FindPinger(StatefulTask):
def __init__(self,
method,
ontop_of_pinger=ontop_of_pinger_elevation,
*args,
**kwargs):
super().__init__(*args, **kwargs)
self.track_method = method
self.ontop_of_pinger = ontop_of_pinger
def on_first_run(self):
assert isinstance(self.track_method, TrackMethod)
self.follow_task = None
self.heading_to_pinger = None
set_gain()
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(
TRACK_COOLDOWN_SAMPLES)
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(7, 10)
ontop_success = None
if STOP_OVER_PINGER:
ontop_success = 2
else:
ontop_success = 6
self.ontop_checker = ConsistencyCheck(ontop_success, 5)
self.pings = []
def generate_states(self):
return "listen", {
"listen": {
"enter": self.enter_listen,
"exit": self.exit_listen,
"tick": self.listen
},
"follow": {
"enter": self.enter_follow,
"tick": self.follow
}
}
# Check for new pings, if there is one return its heading and elevation and add it to the
# pings list
def update_pings(self):
self.silencer()
# TODO Better way to filter out bad pings when thrusters are running.
# If the watcher has not changed, there is no new ping
if not self.hydro_watcher.has_changed():
# TODO Do something here if too much time has passed since last ping.
return None
# There is a new ping!
self.time_since_last_ping = self.this_run_time
# TODO Will this be long after the watcher fired?
# Need to ensure that there is little delay.
results = shm.hydrophones_results_track.get()
kalman = shm.kalman.get()
phases = (results.diff_phase_x, results.diff_phase_y)
if not self.localizer.is_valid(phases[0], phases[1]):
self.logi("Warning: Measured phases (%0.3f %0.3f) are not possible given "
"physical parameters" % \
(phases[0], phases[1]))
head, elev = self.localizer.get_heading_elevation(*phases)
abs_head = (head + shm.kalman.heading.get()) % 360
self.logi("Ping: Phases: (%0.3f %0.3f), Relative Heading: %0.5f, Absolute "
"Heading: %0.3f, Elevation: %0.1f" % \
(phases[0], phases[1], head, abs_head, elev))
in_silence = self.silencer.in_silence()
# TODO Better way to detect ping period.
this_ping_time = \
results.daemon_start_time + results.tracked_ping_time
self.silencer.schedule_silence(this_ping_time + PINGER_PERIOD - 0.2,
3.0)
self.logi("This time: %0.3f, This ping time %0.3f, Diff: %0.3f" %
(self.this_run_time, this_ping_time,
self.this_run_time - this_ping_time))
# If the thrusters are not in silence, then we may have heard thruster
# noise and thought it was a ping; ignore it
if not in_silence:
self.logi("Heard ping but thrusters are not silenced, ignoring")
return None
# Record ping data
sub_pos = get_sub_position(kalman)
sub_quat = get_sub_quaternion(kalman)
ping_data = PingData(phases, head, elev, sub_pos, sub_quat)
self.pings.append(ping_data)
return ping_data
def enter_listen(self):
self.logi("Stopping to listen for pings")
kalman = shm.kalman.get()
shm.navigation_settings.position_controls.set(1)
desires = shm.navigation_desires.get()
desires.north = kalman.north
desires.east = kalman.east
shm.navigation_desires.set(desires)
# Clear previous pings
self.pings = []
def exit_listen(self):
self.logi("Exiting listen state")
shm.navigation_settings.position_controls.set(0)
def listen(self):
self.update_pings()
# If we haven't even gotten MIN_CONSISTENT_PINGS in total, there won't be
# enough consistent ones, no point in continuing
if len(self.pings) < MIN_CONSISTENT_PINGS:
return
# We want to make sure pings are consistent before following them. To do
# this, first cluster the pings by heading
headings, elevations = zip(*[
self.localizer.get_heading_elevation(*ping.phases)
for ping in self.pings
])
data = get_clusterable(headings)
clusters = fclusterdata(data, 8, criterion="distance")
# The best cluster is the one with the most pings in it
counted = collections.Counter(clusters)
best_cluster, n_best = max(counted.items(), key=lambda item: item[1])
# To follow a heading, we require that at least three pings are in the
# cluster which contains it
if n_best >= MIN_CONSISTENT_PINGS:
# Compute average phase for best cluster
good_pings = [self.pings[i] for i, cluster_num in enumerate(clusters) \
if cluster_num == best_cluster]
x_phases = [ping.phases[0] for ping in good_pings]
y_phases = [ping.phases[1] for ping in good_pings]
avg_phase_x = np.mean(x_phases)
avg_phase_y = np.mean(y_phases)
self.logi("Average phases (%f, %f) for best cluster %s" % \
(avg_phase_x, avg_phase_y, str(self.pings)))
if self.track_method == TrackMethod.position:
# Localize pinger to a position
for ping in good_pings:
self.localizer.add_observation(ping.phases, ping.sub_pos,
ping.sub_quat)
est_pinger_pos = self.localizer.compute_position()
self.pinger_positions.append(est_pinger_pos)
self.logi("Localized pinger to: %s" % str(est_pinger_pos))
self.logi("All estimated positions: %s" %
str(self.pinger_positions))
self.follow_position = est_pinger_pos
elif self.track_method == TrackMethod.heading:
# Update the heading and elevation of the pinger
pinger_head, elev = self.localizer.get_heading_elevation(
avg_phase_x, avg_phase_y)
sub_head = shm.kalman.heading.get()
self.heading_to_pinger = heading = (pinger_head +
sub_head) % 360
self.follow_elevation = elev
return "follow"
def elevation_to_distance(self, elevation):
return HYDROPHONES_PINGER_DEPTH * math.tan(math.radians(elevation))
def enter_follow(self):
self.logi("Following a heading of %0.3f" % self.heading_to_pinger)
self.follow_change_heading = Heading()
self.follow_inital_heading = Heading(self.heading_to_pinger +
heading_offset)
self.follow_vel_x = VelocityX()
self.follow_vel_y = VelocityY()
distance_to_pinger = self.elevation_to_distance(self.follow_elevation)
self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger))
self.follow_vel_y(0.0)
# Proportional control to slow down near the pinger
def get_follow_speed(self, distance):
# Start slowing down at SLOW_DOWN_DISTANCE
speed = distance / SLOW_DOWN_DISTANCE
# Don't do nothing dumb
if speed > MAX_FOLLOW_SPEED:
speed = MAX_FOLLOW_SPEED
if speed < MIN_FOLLOW_SPEED:
speed = MIN_FOLLOW_SPEED
return speed
def follow_heading(self, new_ping):
distance_to_pinger = self.elevation_to_distance(new_ping.elevation)
speed = self.get_follow_speed(distance_to_pinger)
# Check if the ping suggests the pinger is in a heading deviating from
# the current follow heading
new_ping_heading = (new_ping.heading + shm.kalman.heading.get()) % 360
heading_deviation = abs(new_ping_heading - self.heading_to_pinger)
# When we are close to the pinger, headings will vary more, allow for
# deviation
# Note, this is fairly broken, because it assumes we get good pings
# while moving! Realistically, we should just move forward for a set time,
# and then get new heading. BUT, since the consistency check will really
# just wait for the requisite number of pings (since its a safe assumption
# that we're just getting garbage), we can essentially turn this into
# a makeshift timer by changing the window size on consistency check (:139)
if new_ping.elevation > MIN_DEVIATING_PING_ELEVATION:
deviating_ping = heading_deviation > MAX_FOLLOW_HEADING_DEVIATION
if deviating_ping:
self.logi(
"Ping heading deviated from follow heading by %0.3f, more "
"than maximum allowed" % heading_deviation)
else:
self.heading_to_pinger = new_ping_heading
if self.ping_deviating_checker.check(deviating_ping):
self.logi(
"Consistently getting deviating pings! Stopping to listen."
)
self.ping_deviating_checker.clear()
return "listen"
self.logi("Going straight for the pinger!")
self.follow_vel_x(x_dir * speed)
self.follow_vel_y(0.0)
self.follow_change_heading(self.heading_to_pinger + heading_offset)
else:
velocity = rotate([speed, 0], new_ping.heading)
self.logi("We are close! Translating to pinger: Velocity (%0.3f, "
"%0.3f)" % (velocity[0], velocity[1]))
self.follow_vel_x(velocity[0])
self.follow_vel_y(velocity[1])
return None
def follow_position(self):
return None
def follow(self):
# Turn to face the pinger
if not self.follow_inital_heading.finished:
self.follow_inital_heading()
return
new_ping = self.update_pings()
if new_ping is not None:
# Check whether we are on top of the pinger
ontop_of_pinger = self.ontop_of_pinger(new_ping)
if (ontop_of_pinger):
self.logi(
"Ontop of pinger validator returned true! Waiting for "
"consitent positives to terminate following.")
if self.ontop_checker.check(ontop_of_pinger):
self.finish()
if self.track_method == TrackMethod.heading:
return self.follow_heading(new_ping)
elif self.track_method == TrackMethod.position:
return self.follow_position(new_ping)
class AlignTarget(Task):
"""
Aligns using ForwardTarget on a target coordinate, while ensuring that the
target is visible
"""
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.logv("Starting {} task".format(self.__class__.__name__))
def on_run(self):
if self.this_run_time - self.first_run_time > self.TIMEOUT:
self.finish()
# self.loge("{} timed out!".format(self.__class__.__name__))
return
# self.logv("Running {}".format(self.__class__.__name__))
# if shm.kalman.depth.get() < .4:
# self.giveup_task()
# self.finish()
if self.validator():
# if abs(shm.kalman.depth.get() - BUOY_SEARCH_DEPTH) >= BUOY_DEPTH_VARIANCE:
# self.depthless_target()
# else:
self.target_task()
else:
# self.heading_task()
# HeadingRestore()
self.logv('lost buoy? searching and restoring depth')
Depth(constants.BUOY_SEARCH_DEPTH)
self.locator_task()
if self.target_checker.check(self.target_task.finished):
self.finish()
def on_first_run(self, *args, **kwargs):
self.logv("Checking if bins in sight")
self.init_time = self.this_run_time
self.seen_checker1 = ConsistencyCheck(6, 8)
self.seen_checker2 = ConsistencyCheck(6, 8)
def on_first_run(self, task, success=18, total=20, *args, **kwargs):
self.cons_check = ConsistencyCheck(success, total)
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)
class IdentifyBins(Task):
""" Identifies which bin to drop markers into, centers over it """
def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None):
self.logi("Centering over bins...")
self.logv("Starting IdentifyBins task")
self.center_valid = False
self.center_coords = (0, 0)
self.task = DownwardTarget(px=0.0025, py=0.0025,
point=lambda: self.center_coords,
valid=lambda: self.center_valid)
self.center_checker = ConsistencyCheck(15, 17)
self.align_checker = ConsistencyCheck(6, 6)
# TODO start alignment task.
self.init_time = self.this_run_time
self.uncovered_bin_vector = None
self.seen_two = False
cam = get_downward_camera()
self.cover_tracker = Tracker(cam['width'] * 0.15)
self.yellow_tracker = Tracker(cam['width'] * 0.15)
def on_run(self, run_type, heading=None, uncovered_bin_vector=None):
yellows = [TrackedBin(b.center_x, b.center_y) if b.probability > 0.0 else None for b in [yellow1.get(), yellow2.get()] ]
cover_g = cover.get()
covert = TrackedBin(cover_g.center_x, cover_g.center_y) if cover_g.probability > 0.0 else None
self.consistent_bins = self.yellow_tracker.track(*yellows)
self.consistent_cover = self.cover_tracker.track(covert, None)
def calculate_bin_vector(bin1, bin2):
body_frame = [(bin1.x, bin1.y), (bin2.x, bin2.y)]
world_frame = [np.array(rotate(body, -shm.kalman.heading.get())) for body in body_frame]
bin2bin = world_frame[1] - world_frame[0]
return bin2bin / np.linalg.norm(bin2bin)
if any(self.consistent_cover) and any(self.consistent_bins):
if run_type == "cover":
good_cover = self.consistent_cover[0]
if good_cover is None:
good_cover = self.consistent_cover[1]
good_yellow = self.consistent_bins[0]
if good_yellow is None:
good_yellow = self.consistent_bins[1]
bin2cover_hat = calculate_bin_vector(good_yellow, good_cover)
if self.uncovered_bin_vector is None:
# TODO Take average here.
self.uncovered_bin_vector = bin2cover_hat
self.logi("Discovered cover to bin world vector %0.2f %0.2f" % \
(self.uncovered_bin_vector[0], self.uncovered_bin_vector[1]))
if run_type == "cover":
cands = self.consistent_cover + self.consistent_bins
else:
if all(self.consistent_bins) and uncovered_bin_vector is not None:
bin2bin = calculate_bin_vector(self.consistent_bins[0], self.consistent_bins[1])
if bin2bin.dot(uncovered_bin_vector()) > 0:
index = 1
else:
index = 0
if not self.seen_two:
self.seen_two = True
self.uncovered_ind = index
self.logi("Chose bin with index %d: current coords %d %d" % \
(self.uncovered_ind, self.consistent_bins[self.uncovered_ind].x, self.consistent_bins[self.uncovered_ind].y))
else:
if self.uncovered_ind == index:
self.logv("Confirmed uncovered bin has index %d" % index)
else:
self.logi("WARNING: Detected new uncovered bin index %d!" % index)
if not self.seen_two:
self.logv("Did not find two yellow!")
cands = self.consistent_bins + self.consistent_cover
else:
cands = [self.consistent_bins[self.uncovered_ind], self.consistent_bins[1 - self.uncovered_ind]] + self.consistent_cover
for i, cand in enumerate(cands):
if cand is not None:
self.logv("Found good bin of index %d" % i)
self.center_valid = True
self.center_coords = cand.x, cand.y
break
else:
self.logv("No good bins found to center on!")
self.center_valid = False
# Assumes cover and contours from same camera
target = get_downward_camera_center()
# TODO Increase deadband as time increases.
self.task(target=target, deadband=(25, 25))
if self.center_checker.check(self.task.finished):
if run_type == "cover" or heading is None:
if heading is None:
target_heading = shm.kalman.heading.get() + cover_g.angle
else:
target_heading = heading()
align_task = Heading(target_heading, deadband=0.5)
align_task()
if self.align_checker.check(align_task.finished):
VelocityX(0)()
VelocityY(0)()
self.finish()
else:
self.finish()
else:
self.align_checker.clear()
def on_finish(self):
self.logi("Centered!")
self.logv('IdentifyBins task finished in {} seconds!'.format(
self.this_run_time - self.init_time))
def on_first_run(self, task, success=18, total=20, *args, **kwargs):
self.cons_check = ConsistencyCheck(success, total)
def on_first_run(self, vision):
self.seen_cons_check = ConsistencyCheck(4, 5)
class ScuttleYellowBuoy(Task):
"""
Locates and scuttles a yellow buoy by dragging it down.
Precondition: The yellow buoy is located at a position in front of the
position of the submarine.
Postcondition: The submarine will have dragged down the yellow buoy, and the
submarine will be positioned above the yellow buoy.
"""
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.locator_task = LocateBuoy(self.location_validator)
self.locator_task = AssumeBuoy(self.location_validator)
self.align_task = AlignTarget(self.location_validator,
self.locator_task, (yellow_buoy_results.top_x.get,
yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)
self.concurrent_align_task = AlignTarget(self.location_validator,
self.locator_task, (yellow_buoy_results.top_x.get,
yellow_buoy_results.top_y.get), yellow_buoy_results, forward_target_p=0.002)
self.MAX_RAM_SPEED = 0.6
self.ram_speed = self.MAX_RAM_SPEED
self.ram_task = RamTarget(self.location_validator,
self.collision_validator, self.locator_task,
self.concurrent_align_task, ram_speed=lambda: self.ram_speed)
self.ready_to_ram_checker = ConsistencyCheck(40, 40)
self.seen_frames_checker = ConsistencyCheck(5, 5)
SCUTTLE_TIME = 2.0
BUOY_MOUNTING_DISTANCE = 0.55
self.scuttle = Sequential(Log("Rising to prepare for scuttle"),
RelativeToInitialDepth(-0.2, error=0.02),
Log("Moving on top of Yellow Buoy"),
MoveX(BUOY_MOUNTING_DISTANCE),
Log("Beginning %0.2f second scuttle!" % SCUTTLE_TIME),
MasterConcurrent(Timer(SCUTTLE_TIME), VelocityX(1.0), RelativeToInitialDepth(3.0, error=0.03)),
Log("Scuttle complete. Returning to targeting position."),
Depth(1.0)
# GoToPosition(lambda: self.target_position[0], lambda: self.target_position[1], depth=lambda: self.target_depth)
)
self.tasks = Sequential(self.locator_task, self.align_task,
self.ram_task, self.scuttle)
#self.last_percent_frame = 0
#self.PERCENT_FRAME_DELTA_THRESHOLD = 10
self.PERCENT_FRAME_THRESHOLD = 2
self.PERCENT_FRAME_SLOWDOWN_THRESHOLD = 0.4
self.TIMEOUT = 100
def on_first_run(self):
self.logv("Starting {} task".format(self.__class__.__name__))
def on_run(self):
# Locate the yellow buoy
# Align with the yellow buoy
# Move forward until collision with the buoy (using RamTarget)
# Descend to drag buoy downwards
if self.this_run_time - self.first_run_time > self.TIMEOUT:
self.finish()
self.loge("{} timed out!".format(self.__class__.__name__))
return
self.tasks()
if self.tasks.finished:
self.finish()
def on_finish(self):
self.logv('{} task finished in {} seconds!'.format(
self.__class__.__name__,
self.this_run_time - self.first_run_time))
Zero()()
def location_validator(self):
# TODO even more robust location validator
return self.seen_frames_checker.check(yellow_buoy_results.probability.get() != 0)
def collision_validator(self):
# TODO even more robust collision validator
current = yellow_buoy_results.percent_frame.get()
aligned = self.concurrent_align_task.finished
max_ram_speed = self.MAX_RAM_SPEED
if not aligned:
max_ram_speed /= 3
self.ram_speed = scaled_speed(final_value=self.PERCENT_FRAME_THRESHOLD + 1,
initial_value=self.PERCENT_FRAME_SLOWDOWN_THRESHOLD,
final_speed=0.0, initial_speed=max_ram_speed,
current_value=current)
if self.ready_to_ram_checker.check(current >= self.PERCENT_FRAME_THRESHOLD and \
aligned):
self.target_position = get_sub_position()
self.target_depth = shm.kalman.depth.get()
self.logi("Close enough to yellow buoy to scuttle!")
return True
#if yellow_buoy_results.center_y.get() > (shm.camera.forward_height.get() - 10) \
# and abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD:
# return True
#self.last_percent_frame = current
return False