def state_approach(self, buoy_to_bump, buoys):
# TODO: include depth routine
if len(buoys) == 3:
buoys.sort(key=lambda x: x.theta)
self.tracking_id = buoys[buoy_to_bump].id
if self.depth_seen is None:
self.depth_seen = sw3.data.depth()
track_buoy = None
for buoy in buoys:
if buoy.id == self.tracking_id:
track_buoy = buoy
if not track_buoy:
return
#self.set_timer("Approach_Timeout", APPROACH_TIMEOUT, self.approach_timeout, sw3.data.imu.yaw())
# print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi
yaw_routine = sw3.RelativeYaw(track_buoy.theta)
forward_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0, 0)
backup_routine = sw3.Forward(BACKWARD_SPEED)
reset_routine = sw3.SetYaw(self.reference_angle)
track_depth_angle = (track_buoy.phi)
if abs(track_depth_angle) > DEPTH_THRESHOLD:
if (track_depth_angle > 0):
depth_routine = sw3.RelativeDepth(-DEPTH_UNIT)
if (track_depth_angle < 0):
depth_routine = sw3.RelativeDepth(DEPTH_UNIT)
else:
depth_routine = sw3.NullRoutine()
centered = False
if abs(track_buoy.theta) <= CENTER_THRESHOLD:
centered = True
if centered:
sw3.nav.do(
sw3.CompoundRoutine(forward_routine, yaw_routine,
depth_routine))
'''
if abs(track_buoy.r) <= DIST_THRESHOLD:
self.delete_timer("Approach_Timeout")
self.next_state()
'''
self.next_state()
else:
sw3.nav.do(sw3.CompoundRoutine(stop_routine, yaw_routine))
def bump_buoy(self, buoys, depth=None):
# Check if we saw all 3 buoys
# found_buoys = filter(lambda x: x.found, buoys)
found_buoys = buoys
if found_buoys:
found_buoys.sort(key=lambda x: x.theta)
if len(found_buoys) == 3:
print "found 3 buoys"
# Assign tracking_buoy_id to middle buoy
self.tracking_buoy_id = found_buoys[1].id
if len(found_buoys) == 1:
print "found 1 buoy"
self.tracking_buoy_id = found_buoys[0].id
"""
elif found_buoys:
for buoy in found_buoys:
absolute_theta = buoy.theta + sw3.data.imu.yaw()
buoy.center_distance = sw3.util.circular_distance(absolute_theta, self.start_angle, 180, -180)
found_buoys.sort(key=lambda x: x.center_distance)
if found_buoys[0].center_distance < BUOY_CENTER_THRESHOLD:
self.tracking_buoy_id = found_buoys[0].id
"""
# what does this conditional do?
if self.tracking_buoy_id is not None:
self.delete_timer("mission_timeout")
# Get tracking buoy from its id
tracking_buoy = None
for buoy in buoys:
if buoy.id == self.tracking_buoy_id:
tracking_buoy = buoy
# Correct if we saw it
if tracking_buoy is not None:
print tracking_buoy
self.last_seen = time.time()
if depth:
depth_routine = sw3.SetDepth(depth)
else:
depth_routine = sw3.RelativeDepth(-(tracking_buoy.phi) / 5)
print "Adjusting depth:", -tracking_buoy.phi / 5
print "id: ", tracking_buoy.id
print "Theta:", tracking_buoy.theta
print "Adjusting Yaw:", DEGREE_PER_PIXEL * tracking_buoy.theta
sw3.nav.do(sw3.CompoundRoutine(
depth_routine,
sw3.Forward(FORWARD_SPEED),
sw3.RelativeYaw(DEGREE_PER_PIXEL * tracking_buoy.theta),
))
# Consider ending the mission if we didn't see it
elif time.time() - self.last_seen > 10:
self.tracking_buoy_id = None
print "Lost buoy, finishing"
return True
def state_setup_pulldown(self):
# start: buoy has dissappeard b/c we're so close to it
# ###
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(-0.7, 1), sw3.Forward(0, 0.1),
sw3.RelativeDepth(-2),
sw3.Forward(0.7, 1 * 1.5),
sw3.Forward(0, 0.1)))
self.wait_stop_mark = 4
self.next_state()
def state_findpath(self):
# TODO: check if the second buoy was center, if so, dont do another approach
riseup_routine = sw3.RelativeDepth(OVER_DEPTH)
runover_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0)
depth_goto = sw3.SetDepth(self.depth_seen)
print "findpath"
sw3.nav.do(
sw3.SequentialRoutine(depth_goto, riseup_routine, runover_routine))
self.finish_mission()
def processFrame(self, frame):
buoys = vision.ProcessFrame(frame)
if buoys.found:
(x, y) = buoys.loc()
h, w, _ = frame.shape
heightError = h / 2 - y
print('modifying depth by: %.3f' % (heightError / PIXTODEPTH))
sw3.RelativeDepth(heightError / PIXTODEPTH).start()
widthError = x - w / 2
print('setting strafe to: %.3f' % (widthError / PIXTODEPTH))
sw3.Strafe(widthError / PIXTODEPTH).start()
return self.runtime > (time.time() - self.time)
def state_findpath(self):
sw3.nav.do(sw3.Forward(0))
# TODO: check if the second buoy was center, if so, dont do another approach
riseup_routine = sw3.RelativeDepth(REL_OVER_DEPTH)
runover_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0)
depth_goto = sw3.SetDepth(self.depth_seen)
print "findpath"
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(-0.6, 0.1), riseup_routine,
sw3.SetYaw(self.reference_angle),
runover_routine))
self.wait_stop_mark = 3
self.next_state()
def state_approach(self, buoy_to_bump, buoys):
print("approach state: {} buoys detected".format(len(buoys)))
# TODO: include depth routine
if len(buoys) == 3:
buoys.sort(key=lambda x: x.theta)
self.tracking_id = buoys[buoy_to_bump].id
if self.depth_seen is None:
self.depth_seen = sw3.data.depth()
# any available buoys represent the one we want to hit, track it
track_buoy = None
for buoy in buoys:
if buoy.id == self.tracking_id:
track_buoy = buoy
# if no hits on finding the target buoy, go back and try again.
# mission will time out if this happens too much.
if not track_buoy:
return
# print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi
# various buoy related routines
yaw_routine = sw3.RelativeYaw(track_buoy.theta)
forward_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0,0)
backup_routine = sw3.Forward(BACKWARD_SPEED)
reset_routine = sw3.SetYaw(self.reference_angle)
#change Depth
track_depth_angle = (track_buoy.phi)
centered = False
if abs(track_depth_angle) > DEPTH_THRESHOLD:
if (track_depth_angle > 0):
depth_routine = sw3.RelativeDepth(-DEPTH_UNIT)
if (track_depth_angle < 0):
depth_routine = sw3.RelativeDepth(DEPTH_UNIT)
else:
depth_routine = sw3.NullRoutine()
centered = True
# check if yaw is centered
if abs(track_buoy.theta) <= CENTER_THRESHOLD:
centered = True*centered
# if yaw and depth are centered, advance to the next state!
if centered:
sw3.nav.do(sw3.CompoundRoutine(
forward_routine,
yaw_routine,
depth_routine
))
'''
if abs(track_buoy.r) <= DIST_THRESHOLD:
self.delete_timer("Approach_Timeout")
self.next_state()
'''
self.next_state()
# otherwise, stop and adjust yaw
else:
sw3.nav.do(sw3.CompoundRoutine(
stop_routine,
yaw_routine
))
def state_approach(self, buoy_to_bump, buoys):
# self.tracking_id
# if vision doesn't return any buoy objects, there's nothing to process.
if not buoys:
return
#
print "buoy to bump: {}".format(buoy_to_bump)
# TODO: include depth routine
if len(buoys) >= 1:
# Note: this is meant to create a pocket of code that only runs at
# the begginning of the approach phase. The assumption is that every
# approach begins with 3 buoys in view. Once the sub moves forward, the
# assumption is that this setup code ran enough times such that the
# correct buoy is being tracked, and the optimal depth for seeing all
# buoys simultaneously has been captured.
# sort buoys from left to right
buoys.sort(key=lambda x: x.theta)
# store buoy-to-bump's ID under self.tracking_id for later recall
self.tracking_id = buoys[buoy_to_bump].id
# update depth parameter
if self.depth_seen is None:
self.depth_seen = sw3.data.depth()
# assert that the tracked/target buoy is still in our field of view
track_buoy = None
for buoy in buoys:
if buoy.id == self.tracking_id:
track_buoy = buoy
# if target buoy is not in our FOV, terminate processing and try again.
# mission will time out if this happens too much.
if not track_buoy:
return
# start/reset the approach timer
self.set_timer("Approach_Timeout", APPROACH_TIMEOUT,
self.approach_timeout, sw3.data.imu.yaw())
# print debug text
print("approach state: {} buoys detected".format(len(buoys)))
# print "State: BuoyBump dist: ",track_buoy.r, " x angle from current: ",track_buoy.theta, " y angle from current: ", track_buoy.phi
# various buoy related routines
yaw_routine = sw3.RelativeYaw(track_buoy.theta)
forward_routine = sw3.Forward(FORWARD_SPEED)
stop_routine = sw3.Forward(0, 0)
backup_routine = sw3.Forward(BACKWARD_SPEED)
reset_routine = sw3.SetYaw(self.reference_angle)
# generate Depth changing parameters
track_depth_angle = (track_buoy.phi)
centered = False
#print (track_depth_angle)
# dynamically assign depth_routine to trim depth by DEPTH_UNITs
if abs(track_depth_angle) > DEPTH_THRESHOLD:
if (track_depth_angle > 0):
depth_routine = sw3.RelativeDepth(-DEPTH_UNIT)
if (track_depth_angle < 0):
depth_routine = sw3.RelativeDepth(DEPTH_UNIT)
else:
depth_routine = sw3.NullRoutine()
centered = True
# check if yaw is centered
#print (track_buoy.theta)
if abs(track_buoy.theta) <= CENTER_THRESHOLD:
centered = True * centered
# if yaw and depth are not centered, stop and adjust yaw/depth
if not centered:
sw3.nav.do(
sw3.CompoundRoutine(stop_routine, yaw_routine, depth_routine))
# else yaw and depth are centered, advance to the next state!
else:
sw3.nav.do(
sw3.CompoundRoutine(forward_routine, yaw_routine,
depth_routine))
self.delete_timer("Approach_Timeout")
self.next_state()
def state_pulldown(self):
sw3.nav.do(
sw3.SequentialRoutine(sw3.Forward(0.7, 0.2), sw3.DepthRate(1, 1),
sw3.RelativeDepth(2), sw3.Forward(0, 0.01)))
self.wait_stop_mark = 3
self.next_state()
