def findpath(self):
if self.turn_count % 2:
sw3.nav.do(sw3.RelativeYaw(-90))
else:
sw3.nav.do(sw3.RelativeYaw(90))
sw3.nav.do(sw3.Forward(.1, 2))
self.finish_mission()
def pass_with_style(self):
"""the code for turning the robot, and passing through the hedge
with style."""
turn_routine = sw3.RelativeYaw(180)
stop_routine = sw3.Forward(0, 0.1)
backup_routine = sw3.Forward(BACKUP_SPEED, BACKUP_TIME)
# stop
sw3.nav.do(stop_routine)
time.sleep(1)
# 180
sw3.nav.do(
stop_routine,
turn_routine,) # do a 180
time.sleep(10)
sw3.nav.do(backup_routine) # backup
time.sleep(BACKUP_TIME)
time.sleep(1)
sw3.nav.do(stop_routine) # stop
time.sleep(1)
print "doing the 2nd 180"
sw3.nav.do(sw3.RelativeYaw(180)) # do another 180
time.sleep(10)
print "done"
def set_course(self, forward, rel_yaw, get_obj=False):
"""Changes the course of the robot as it's searching
for the pinger.
ARGS:
forward: how fast to go forward
rel_yaw: how much to change the relative yaw heading"""
# change heading and move forward (or backward)
if not get_obj:
sw3.nav.do(sw3.CompoundRoutine(
sw3.RelativeYaw(rel_yaw),
sw3.Forward(forward),
)
)
return
else:
set_course_routine = sw3.CompoundRoutine(
sw3.RelativeYaw(rel_yaw),
sw3.Forward(forward),
)
)
# deliver an object to the caller
return set_course_routine
def findpath(self, holes):
# swim around target?
turn = sw3.CompoundRoutine(sw3.RelativeYaw(TURN_ANGLE),
sw3.Forward(TURN_SPEED),
timeout=TURN_TIMER)
finish = sw3.CompoundRoutine(sw3.RelativeYaw(-TURN_ANGLE),
sw3.Forward(TURN_SPEED),
timeout=TURN_TIMER)
sw3.nav.do(turn)
sw3.nav.do(finish)
self.finish_mission()
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 step(self, vision_data):
self.mission_timeout -= 1
if not vision_data:
return
gate_data = vision_data['gate']
if not gate_data:
return
print gate_data
if gate_data and gate_data.left_pole and gate_data.right_pole:
gate_center = DEGREE_PER_PIXEL * (gate_data.left_pole + gate_data.right_pole) / 2 # degrees
# If both poles are seen, point toward it then go forward.
self.gate_seen += 1
self.gate_lost = 0
if abs(gate_center) < STRAIGHT_TOLERANCE:
sw3.nav.do(sw3.CompoundRoutine([
sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw()
]))
else:
print "Correcting Yaw", gate_center
sw3.nav.do(sw3.CompoundRoutine([
sw3.RelativeYaw(gate_center),
sw3.Forward(0.4)
]))
elif self.gate_seen >= 15:
self.gate_lost += 1
if self.gate_lost > 1 or self.mission_timeout <= 0:
print "Heading Locked"
self.finish_mission()
return
def orient(self, bins):
if bins and self.orientdata:
orient_angle = self.orientdata * (180 / math.pi)
# print "forward"
#center = sw3.Forward(0.5,2)
#centering = sw3.nav.do(center)
# sw3.nav.do(center)
# sw3.nav.do(sw3.Forward(FORWARD_SPEED,1))
# sw3.nav.do(sw3.Forward(0,0))
print "orient"
orient = sw3.CompoundRoutine(sw3.Forward(0, 5),
sw3.RelativeYaw(orient_angle))
# sw3.nav.do(sw3.CompoundRoutine(sw3.Forward(0,0),(sw3.RelativeYaw(self.orientdata),timeout =5)))
sw3.nav.do(orient)
#orient.on_done(lambda z: sw3.nav.do(sw3.Forward(FORWARD_SPEED, 1)))
#center.on_done(lambda b: sw3.nav.do(orient))
# sw3.nav.do(sw3.RelativeDepth(BIN_DEPTH))
# print sw3.data.imu.yaw()
# print self.orientdata
if (abs(abs(sw3.data.imu.yaw()) - orient_angle) <= ORIENT_THRESH):
print "done orienting"
orient.on_done(
lambda z: sw3.nav.do(sw3.Forward(FORWARD_SPEED, 1)))
self.turn_count += 1
self.nextState()
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 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 processFrame(self, frame):
print "turn state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
finding out where the start of the path is. This is the path
end point closest to the center of the camera
"""
#pt1, pt2 = path.endPoints()
pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
self.startPt, self.endPt = sortPoints(self.startPt, pts)
center = (path.cx, path.cy)
angle = turnParallelTo(center, self.endPt)
print "Angle: %d" % angle
if abs(angle) <= MAXANGLEDIF:
sw3.RelativeYaw(0).start()
return OnwardState()
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return OnwardState()
print "ret found"
return self
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
Turning to face the end point of the path.
"""
#pt1, pt2 = path.endPoints()
pts = [[path.p1x, path.p1y], [path.p2x, path.p2y]]
self.startPt, self.endPt = sortPoints(self.startPt, pts)
center = (path.cx, path.cy)
angle = turnParallelTo(center, self.endPt)
print "Angle: %d" % angle
if abs(angle) <= MAXANGLEDIF:
sw3.RelativeYaw(0).start()
return ToEndState(self.startPt, self.endPt)
elif not self.pathLost.timeLeft():
"""if the path has been lost for too long go to path lost state"""
return PathLostState()
print "ret found"
return self
def processFrame(self, frame):
gate = vision.ProcessFrame(frame)
print gate.found
if gate.found:
frame = gate.draw(frame)
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
_, w, _ = frame.shape
center = w / 2.0 - gate.cp
print("got center %d" % center)
self.centers.insert(0, center)
centers = 0
for i in self.centers:
centers += i
center = float(centers) / len(self.centers)
print(center)
sw3.RelativeYaw(center / PIXTODEG).start()
cv2.imshow(self.name, frame)
return self.runtime > (time.time() - self.time)
def change_heading(self, forward, rel_yaw):
if abs(rel_yaw) > YAW_TOLERANCE:
sw3.nav.do(sw3.CompoundRoutine(
sw3.RelativeYaw(-rel_yaw),
sw3.Forward(forward)
)
)
def step(self, vision_data):
self.mission_timeout -= 1
if not vision_data:
return
hedge_data = vision_data['hedge']
if not hedge_data:
return
print hedge_data
if hedge_data:
hedge_center = None
if hedge_data.right_pole and hedge_data.left_pole:
hedge_center = DEGREE_PER_PIXEL * (
hedge_data.left_pole +
hedge_data.right_pole) / 2 # degrees
elif hedge_data.center_pole is not None:
hedge_center = hedge_data.center_pole
self.hedge_seen += 1
self.hedge_lost = 0
if hedge_center is not None:
if abs(gate_center) < STRAIGHT_TOLERANCE:
sw3.nav.do(
sw3.CompoundRoutine(
[sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw()]))
else:
print "Correcting Yaw", hedge_center
sw3.nav.do(
sw3.CompoundRoutine([
sw3.RelativeYaw(hedge_center + 2),
sw3.Forward(FORWARD_SPEED),
]))
elif self.hedge_seen >= 5:
self.hedge_lost += 1
if self.hedge_lost > 1 or self.mission_timeout <= 0:
sw3.nav.do(
sw3.SequentialRoutine(sw3.RelativeYaw(180),
sw3.Forward(-1 * FORWARD_SPEED, 5)))
time.sleep(self.mission_timeout)
self.finish_mission()
def turnToAngle(angle):
angle *= 180.0 / math.pi
print "NEW ANGLE", angle
#gets the angle it needs to turn by
if angle < 0:
angleDif = abs(angle) - 90
elif angle < 90:
angleDif = -90 - angle
else:
angleDif = 270 - angle
sw3.RelativeYaw(angleDif).start()
return angleDif
def processFrame(self, frame):
yaw = sw.var.get("Acoustics.Yaw")
pitch = abs(sw.var.get("Acoustics.Pitch"))
print("Yaw: %4.2f, Pitch %4.2f" % (yaw, pitch))
if yaw != self.yaw or pitch != self.pitch:
self.yaw = yaw
self.pitch = pitch
if pitch >= PITCHCUT:
sw3.RelativeYaw(yaw).start()
else:
return PitchState(pitch)
return self
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 drop(self, bins):
# print "Marker Dropped"
if bins and self.orientdata:
orient_angle = self.orientdata * (180 / math.pi)
print "orient"
orient = sw3.CompoundRoutine(sw3.Forward(0),
sw3.RelativeYaw(orient_angle),
timeout=5)
sw3.nav.do(orient)
self.drop_count += 1
print "State:", self.state
self.state = self.states[0]
self.state_num = 0
def step(self, vision_data):
if not vision_data:
return
if vision_data['path']:
path_data = vision_data['path']
print path_data
theta_x = path_data.x * FIELD_OF_VIEW * math.pi / 180 # path_data.x is percent of fram view . multiplying them gives you theta_x
theta_y = path_data.y * FIELD_OF_VIEW * math.pi / 180 # path_data.y is percent of frame view . multiplying them gives you theta
d = PATH_DEPTH - sw3.data.depth(
) # depth between path and camera
x = d * math.sin(
theta_x
) # g1ves you the x distance from the frame center to path center
y = d * math.sin(
theta_y
) # gives you the y distance from the frame center to path center
print "Status:Step x ", x, " y ", y
if self.state == "centering":
self.state_centering(x, y)
if self.state == "orienting":
return self.state_orienting(path_data)
if vision_data['gate']:
gate_data = vision_data['gate']
print gate_data
if gate_data.left_pole and gate_data.right_pole:
gate_center = DEGREE_PER_PIXEL * (
gate_data.left_pole +
gate_data.right_pole) / 2 # degrees
if abs(gate_center) < STRAIGHT_TOLERANCE:
sw3.nav.do(
sw3.CompoundRoutine(
[sw3.Forward(FORWARD_SPEED),
sw3.HoldYaw()]))
else:
print "Correcting Yaw", gate_center
sw3.nav.do(
sw3.CompoundRoutine([
sw3.RelativeYaw(gate_center),
sw3.Forward(0.4)
]))
def sweep(self, bins):
# print "sweep"
# print self.orientdata
sweep = sw3.Forward(FORWARD_SPEED, 1)
turning = sw3.CompoundRoutine(sw3.Forward(0, TURNING_TIME),
sw3.RelativeYaw(180))
turnaround = lambda: sw3.nav.do(turning)
#self.id_holder = self.highest_id
# iif self.x == 0:
# print "nav.sweep"
# sw3.nav.do(sweep)
#self.x = 1
if bins:
for bincount in bins:
if bincount.id > self.highest_id:
self.highest_id = bincount.id
current_bin = None
if self.orientdata is not None:
# if self.highest_id > self.id_holder:
# this means weve seen a new bini
# print self.highest_id
# print self.id_holder
#self.id_holder = self.highest_id
# sw3.nav.do(sw3.Forward(0,0))
# sw3.nav.do(turning)
print "turning"
self.set_timer("bin_timeout", TURNAROUND_TIMER, turnaround)
turning.on_done(lambda y: sw3.nav.do(sweep))
# print "I turned!"
# sw3.nav.do(sw3.Forward(FORWARD_SPEED))
# print "turning"
self.turn_count += 1
# print self.turn_countF none
for bina in bins:
if bina.id == self.highest_id:
current_bin = bina
print current_bin.shape
#self.highest_id = bina.id
if current_bin:
if current_bin.shape is "A" or current_bin.shape is "C":
if self.dropped is "A" and current_bin.shape is "C":
self.nextState()
if self.dropped is "C" and current_bin.shape is "A":
self.nextState()
if self.dropped is "E":
self.nextState()
self.dropped = current_bin.shape
def state_centering(self, x, y):
position_rho = math.sqrt(
x**2 +
y**2) # hypotenuse-distance from frame center to path center
position_phi = math.atan2(x, y) * (
180 / pi) # angle of center of path from current position
print "State:Centering dist ", position_rho, " angle from current ", position_phi
sw3.nav.do(sw3.Forward(0))
yaw_routine = sw3.RelativeYaw(position_phi)
forward_routine = sw3.Forward(FORWARD_SPEED, CENTER_TIME)
sw3.nav.do(yaw_routine)
yaw_routine.on_done(lambda x: sw3.nav.do(forward_routine))
if position_rho <= CENTER_THRESHOLD:
self.state = "orienting"
def processFrame(self, frame):
print "found state"
path = vision.ProcessFrame(frame)
if path.found:
print "path found"
self.pathLost.restart()
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
print("got direction %d" % path.orientation)
sw3.RelativeYaw(path.orientation).start()
elif not self.pathLost.timeLeft():
"""if the gate has been lost for too long go to gate lost state"""
return PathLostState()
print "ret found"
return self
def step(self, vision_data):
#if not vision_data:
# return
ac_data = self.acoustics.get_data()
if ac_data['error'] == 0:
# grab data
epoch = ac_data['data']['epoch']
rel_yaw = ac_data['data']['heading']['ab']
if epoch > 5:
print "data is stale. will try again later"
return # data is stale
print rel_yaw
if abs(rel_yaw) > YAW_TOLERANCE:
sw3.nav.do(
sw3.CompoundRoutine(sw3.RelativeYaw(-rel_yaw),
sw3.Forward(0)))
time.sleep(6)
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 processFrame(self, frame):
print "found state"
gate = vision.ProcessFrame(frame)
if gate.found:
print "gate found"
self.gateLost.restart()
"""
finding out how many pixels from the center the gate is
the center obj of the gate is the number or pixels over the gate is.
Subtracting the middle pixel index from it returns a pos value if the gate is to left
and pos value if the gate is to the right
"""
_, w, _ = frame.shape
center = w / 2.0 - gate.cp
print("got center %d" % center)
self.centers.insert(0, center)
centers = 0
for i in self.centers:
centers += i
center = float(centers) / len(self.centers)
print(center)
print self.centers
if len(self.centers) > 10:
self.centers.pop()
print self.centers
#if less than set difference ignore it
center = center if center > SIGPIX else 0
sw3.RelativeYaw(center / PIXTODEG).start()
elif not self.gateLost.timeLeft():
"""if the gate has been lost for too long go to gate lost state"""
return GateLostState()
print "ret found"
return self
def step(self, vision_data):
#if not vision_data:
# pick next state
if self.state == 'searching':
self.state_searching()
elif self.state == 'verify':
self.state_verify()
elif self.state == 'accepted':
self.state_accepted()
elif not (self.state in self.states):
raise ValueError("{} is not an acceptable state".format(self.state))
if abs(rel_yaw) > YAW_TOLERANCE:
# change yaw and go forward
sw3.nav.do(
sw3.CompoundRoutine(
sw3.RelativeYaw(-rel_yaw),
sw3.Forward(.3)
)
)
time.sleep(6)
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
))
import vision
import missions
#import acoustics
from mission_controller import MissionController
# 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.GateMission,
missions.PathMission,
sw3.nav.do(sw3.RelativeYaw(-5)),
missions.PathMission,
missions.ReverseHedgeMission,
missions.HedgeMission,
missions.NewBuoyMission,
missions.PathMission,
#missions.PathMission,
#missions.PathMission,
#missions.AcousticsMission,
#missions.HedgeMission,
#(missions.PathMission, True, 1),
#sw3.Forward(.5, 1),
#missions.NewBinsMission,
#(missions.PathMission, True, 1),
#missions.HedgeMission,
#missions.FakePizzaMission
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()
from seawolf/mission_control import sw3 sw3.nav.do(sw3.RelativeYaw(10))
