Exemplo n.º 1
0
 def alignWithBouy(self):
     #align x
     if(self.lastKnownBuoyLoc.center_x > 50):
         strafe(-.1)
     elif(self.lastKnownBuoyLoc.center_x < -50):
         strafe(.1)
     else:
         strafe(0)
Exemplo n.º 2
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'
Exemplo n.º 3
0
 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)
Exemplo n.º 4
0
 def centerX(self, msg):
     if abs(msg.center_x) > 30:
         strafe(-(msg.center_x/300))
         return False
     strafe(0)
     return True