def alignWithBouy(self):
#align x
if(self.lastKnownBuoyLoc.center_x > 50):
strafe(-.1)
elif(self.lastKnownBuoyLoc.center_x < -50):
strafe(.1)
else:
strafe(0)
def execute(self, userdata):
self.reset()
self.buoy_sub = rospy.Subscriber('img_rec/buoys/red', ImgRecObject, self.buoy_cb)
self.depth_sub = rospy.Subscriber('Sub_Depth', Float32, self.depth_cb)
while True:
if self.preempt_requested():
self.service_preempt()
self.unsub()
return 'preempted'
delta = time.time() - self.last_time
if self.last_buoy and delta < self._timeout:
if abs(self.last_buoy.center_x) < 50:
self.good_x_count += 1
strafe(self.last_buoy.center_x * self._gain_x)
else:
self.good_x_count = 0
strafe(0)
if abs(self.last_buoy.center_y) < 50:
self.good_y_count += 1
dive(self.last_buoy.center_y * self._gain_y)
else:
self.good_y_count = 0
if self.last_depth:
dive(self.last_depth - self.last_buoy.center_y * self._gain_y)
if self.good_x_count >= 10 and self.good_y_count >= 10:
self.good_y_count += 1
if self.last_depth:
dive(self.last_depth)
self.last_buoy = None
elif delta > self._timeout:
self.unsub()
return 'lost_buoy'
rospy.sleep(0.5)
self.buoy_sub.unregister()
self.depth_sub.unregister()
return 'succeeded'
def scanForBuoy(self):
# we don't know where it is so let's try right
# we'll go into a circle til we time out or find the buoy
dive(0)
strafe(0)
turn(.1)
def centerX(self, msg):
if abs(msg.center_x) > 30:
strafe(-(msg.center_x/300))
return False
strafe(0)
return True