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 step(self, vision_data):
buoys = []
if vision_data is not None:
buoys = vision_data['buoy'].buoys
if len(buoys) > 0:
looking_for = 'red'
if self.bumped == 1:
looking_for = 'green'
selected = None
for buoy in buoys:
if buoy.color == looking_for:
selected = buoy
if selected is not None:
sw3.nav.do(
sw3.CompoundRoutine(
sw3.SetDepth(7),
sw3.RelativeYaw(selected.theta),
sw3.Forward(.2, 6),
))
self.bumped += 1
sw3.nav.do(sw3.Forward(-.2, 6))
if self.bumped == 2:
self.finish_mission()
else:
sw3.nav.do(sw3.Forward(.1, 1))
def init(self):
self.process_manager.start_process(entities.BuoyHoughEntity,
"buoy",
"forward",
debug=True)
sw3.nav.do(sw3.SetDepth(6))
self.bumped = 0
def seek(self, bins):
# print "seek"
# print bins
if bins and self.orientdata:
sw3.nav.do(sw3.Forward(0, 1))
# print bins
pos_x = math.atan2(bins[0].theta, bins[0].phi) * (180 / math.pi)
print pos_x
pos_rho = math.sqrt(bins[0].theta**2 + bins[0].phi**2)
# sw3.nav.do(sw3.Forward(0,0))
print "center"
center = sw3.CompoundRoutine(sw3.RelativeYaw(pos_x),
sw3.Forward(.2),
sw3.SetDepth(BIN_DEPTH),
timeout=3)
sw3.nav.do(center)
# print pos_x
# sw3.nav.do(sw3.Forward(FORWARD_SPEED, CENTER_TIME))
# print "centering"
#self.orientdata *= 180/math.pi
if pos_x <= CENTER_THRESH:
# print "centered"
#orient_angle = self.orientdata
sw3.nav.do(sw3.Forward(0, 1))
# sw3.nav.do(sw3.RelativeYaw(20))
# sw3.nav.do(sw3.RelativeYaw(self.orientdata))
# sw3.nav.do(sw3.RelativeDepth(BIN_DEPTH))
self.nextState()
def init(self):
# set depth to bouy level
sw3.nav.do(sw3.SetDepth(DEPTH_BUMP))
time.sleep(DELAY)
# startup buoy vision code
self.process_manager.start_process(entities.BuoyHoughEntity,
"buoy",
"forward",
debug=True)
# Go forward
sw3.nav.do(
sw3.CompoundRoutine(
sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw(),
))
# Captures yaw heading for ???
self.start_angle = sw.var.get("YawPID.Heading")
# starts a watchdog timer for the entire mission. After 15 seconds,
# the mission will terminate, regardless of what is happening.
self.set_timer("mission_timeout", 15, self.finish_mission)
self.tracking_buoy_id = None # State variable for bump_buoy()
def init(self):
# Set depth to hedge hight, but keep heading at same time
sw3.nav.do(sw3.CompoundRoutine(
sw3.Forward(0),
sw3.HoldYaw(),
sw3.SetDepth(HEDGE_DEPTH),
))
# give some time for the dive to complete
time.sleep(4)
"""
current_depth = sw3.data.depth()
while current_depth <= HEDGE_DEPTH - 3:
print "changing depth ({}) to {}".format(current_depth, HEDGE_DEPTH)
current_depth = sw3.data.depth()
"""
# start vision
self.process_manager.start_process(entities.GateEntity, "gate", "forward", debug=True)
# go forward
sw3.nav.do(sw3.CompoundRoutine(
#sw3.HoldYaw(),
sw3.Forward(FORWARD_SPEED),
))
def aim(self, target):
# line up with target, see all four corners
# change depth to specified color
sw3.nav.do(sw3.SetDepth(TARGET_DEPTH))
# go forward until only see color
self.aimed += 1
self.nextState()
def init(self):
sw3.nav.do(
sw3.SetDepth(DEPTH),
)
time.sleep(DELAY)
self.process_manager.start_process(entities.GateEntity, "gate", "forward", debug=True)
sw3.nav.do(sw3.CompoundRoutine(
sw3.HoldYaw(),
sw3.Forward(FORWARD_SPEED),
))
def init(self):
self.process_manager.start_process(entities.GateEntity,
"gate",
"forward",
debug=True)
sw3.nav.do(
sw3.CompoundRoutine(
sw3.Strafe(STRAFE_SPEED),
sw3.SetDepth(2),
sw3.HoldYaw(),
))
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 init(self):
sw3.nav.do(sw3.SetDepth(DEPTH_BUMP))
time.sleep(DELAY)
self.process_manager.start_process(entities.BuoyHoughEntity, "buoy", "forward", debug=True)
sw3.nav.do(sw3.CompoundRoutine(
sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw(),
))
self.start_angle = sw.var.get("YawPID.Heading")
self.set_timer("mission_timeout", 15, self.finish_mission)
self.tracking_buoy_id = None # State variable for bump_buoy()
def init(self):
sw3.nav.do(sw3.SetDepth(6))
self.process_manager.start_process(entities.PizzaCornerEntity,
"pizzabox",
"down",
debug=True)
self.reference_angle = sw3.data.imu.yaw()
self.highest_id = None
self.states = [
"findpizza",
"pickup",
]
self.state_num = 0
self.state = self.states[self.state_num]
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 init(self):
'''runs at start of mission '''
if self.double:
self.process_manager.start_process(entities.DoublePathEntity,
"path",
"down",
debug=True)
else:
self.process_manager.start_process(entities.PathEntity,
"path",
"down",
debug=True)
sw3.nav.do(
sw3.CompoundRoutine([
sw3.Forward(FORWARD_SPEED),
sw3.RelativeYaw(0),
sw3.SetDepth(4)
]))
self.reference_angle = sw.var.get("YawPID.Heading") * (pi / 180) % (2 *
pi)
self.state = "centering"
self.set_timer("path_timeout", MISSION_TIMEOUT, self.fail_mission)
def setup(self):
sw3.SetDepth(-1).start()
sw3.Forward(.8).start()
self.state = SearchState()
return
def setup(self): sw3.SetDepth(-1).start() sw3.Forward(.4).start() self.state = YawState() return
def setup(self):
self.time = time.time()
sw3.SetDepth(-1).start()
sw3.Forward(.3).start()
self.centers = []
return
def setup(self):
sw3.SetDepth(DEPTH).start()
sw3.Forward(STARTSPEED).start()
self.state = SearchState()
return
def init(self):
'''runs at start of mission '''
self.set_timer("mission_timeout", MISSION_TIMEOUT,
self.mission_timeout)
# stop, and go down
sw3.nav.do(
sw3.CompoundRoutine(
sw3.Forward(0),
sw3.SetDepth(START_DEPTH),
))
# adjust depth before starting mission
"""
current_depth = sw3.data.depth()
while current_depth <= START_DEPTH - 3:
print "Adjusting depth ({}) to go to {}.".format(current_depth, START_DEPTH)
current_depth = sw3.data.depth()
"""
#
# start vision process
self.process_manager.start_process(entities.BuoyHoughEntity,
"buoy",
"forward",
debug=True)
# ease forwards in order get a better view
sw3.nav.do(sw3.Forward(INITIAL_RECON_SPEED))
# capture orientation at this point
self.reference_angle = sw3.data.imu.yaw()
self.tracking_id = None
self.depth_seen = None
self.states = [
"first_approach",
"bump",
#"second_approach", #ommited. we only want to bump the first buoy we see.
#"bump",
#"center_approach",
"setup_pull_down",
"wait",
"pull_down",
"wait",
#"find_path",
"disconnect",
"wait",
"find_path",
"wait",
]
self.state_num = 0
self.state = self.states[self.state_num]
self.set_timer("buoy_timeout", 120, self.fail_mission)
# debug parameters
self.i = 0
# waiting variables
self.substate = ''
self.wait_stop_mark = -1
self.hidden = False
#import acoustics
from mission_controller import MissionController
BUOY_DEPTH = 4
#
# Ordered list of tasks. Can be one of the following types:
# * Mission Class - Found in ``missions.<name>Mission``. Instantiated with no
# arguments.
# * Nav routine - Found in ``sw3.nav.<name>``.
# * Tuple - First item must be a mission class. The rest of the tuple is
# passed in as arguments to the ``mission.__init__``.
MISSION_ORDER = [
missions.GateMission325, # 01: gate
#sw3.Forward(0.5, 3),
#sw3.Forward(0.0,2),
sw3.SetDepth(3, 2),
#sw3.Forward(0.0, 1),
missions.PathMission,
#sw3.SetDepth(1),
#sw3.Forward(0),
#sw3.Forward(0.9,30),
#sw3.RelativeYaw(0),
#sw3.SetDepth(2),
#sw3.Forward(0.9,90),
#sw3.Forward(0.9,15),
#sw3.RelativeYaw(30,1),
#sw3.Forward(0.9,10),
#sw3.Forward(q,1),
#sw3.RelativeYaw(0),
def pickup(self):
sw3.nav.do(
sw3.CompoundRoutine(sw3.SetDepth(9), sw3.Forward(0, 0),
sw3.SetDepth(0)))
self.nextState()
