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()