MISSIONS: Add buoy circler - tested/working

boat_launch/src/boat_launch/mission/circle_bouy.py

@@ -0,0 +1,112 @@
+#! /usr/bin/env python
+
+from __future__ import division
+from std_msgs.msg import Float64, Header
+from txros import util
+import rospy, numpy
+import boat_scripting
+from uf_common.orientation_helpers import xyz_array, xyzw_array, quat_to_rotvec
+import sensor_msgs.point_cloud2 as pc2
+import math
+from visualization_msgs.msg import MarkerArray, Marker
+from geometry_msgs.msg import Vector3
+from uf_common.orientation_helpers import *
+
+BOAT_LENGTH_OFFSET = 2
+FIVE_DEG = .0425 #maybe? # FIVE_DEG - .0425
+PIXEL_TOLERANCE = 15
+
+
+@util.cancellableInlineCallbacks
+def center_buoy(boat):
+    # While the boat is not locked on to the yield boat.move.left(-distance_to_move)target's orientation
+    while True:
+        # Get new target location - 
+        # 0 is dead center
+        # positive values mean right
+        # negative values mean left
+
+        bouys = yield boat.get_bouys()
+        color = bouys[0].color
+        msg_x = bouys[0].x_coord
+
+        # Throws out bad readings
+
+        if msg_x == 0: pass
+            #yield boat.move.forward(.5).go()
+            #print "No shape found, moving forward"
+        if msg_x != 0:
+
+            # If the pixel location is within our range of error
+            if msg_x < 500 + PIXEL_TOLERANCE or msg_x < 500 - PIXEL_TOLERANCE:
+                print color + shape + " in Center at location: " + str(msg_x) + " --- Locking Target"
+                break
+                # Break the loop and continue
+
+            angle_move = FIVE_DEG
+
+            # If the target is right of the center 
+            if msg_x > 500 + PIXEL_TOLERANCE :
+                # turn the number of degrees right between the center and the target, minus an offset
+                boat.move.turn_left(-angle_move).go()
+                print "Turning right", abs(angle_move)
+
+            # If the target is left of the center
+            if msg_x < 500 - PIXEL_TOLERANCE:
+                # turn the number of degrees left between the center and the target, minus an offset
+                boat.move.turn_left(angle_move).go()
+                print "Turning left", abs(angle_move)
+
+            print color + " bouy at pixel location: ", abs(msg_x)
+
+
+@util.cancellableInlineCallbacks
+def main(nh):
+
+    boat = yield boat_scripting.get_boat(nh, False, False)
+    distance = 100
+    distances = []
+
+    #yield center_buoy(boat)
+
+    print "Bouy is now in center, moving towards it"
+
+    while distance > 3:
+
+        # get distance from boat
+        distances = yield boat.get_distance_from_object(.05)
+
+        # Average distance between boat and bouy
+        distance = distances[0] - BOAT_LENGTH_OFFSET
+        print "Distance from target", distance
+        # move forward uuntil we are less than three meters from bouy
+        boat.move.forward(1).go()
+
+    print "Turning right"
+    yield boat.move.turn_right_deg(45).go()
+    yield util.sleep(3)
+
+    print "Starting square"
+
+    for i in xrange(4):
+        print 'Side', i
+        yield boat.move.forward(5).go()
+        yield boat.move.turn_left_deg(90).go()
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

boat_scripting/src/boat_scripting/boat_class.py

@@ -45,6 +45,7 @@
 from azi_drive.srv import trajectory_mode, trajectory_modeRequest
 from camera_docking.msg import Circle, Triangle, Cross
 from azi_drive.srv import AziFloat, AziFloatRequest
+from vision_sandbox.msg import Buoy, Buoys
 
 
 class _PoseProxy(object):
@@ -110,6 +111,8 @@ def _init(self, need_trajectory=True, need_odom=True):
 
         self.float_srv = self._node_handle.get_service_client('/float_mode', AziFloat)
 
+        self_bouy_subsriber = self._node_handle.subscribe('topic', Buoys)
+
 
         # Make sure trajectory topic is publishing 
         if(need_trajectory == True):
@@ -141,6 +144,11 @@ def odom(self):
     def move(self):
         return _PoseProxy(self, self.pose)
 
+    def get_bouys(self):
+        msg = yield self._buoy_sub.get_next_message()
+        defer.returnValue(msg)
+
+
     def pan_lidar(self, freq = 0.5, min_angle = 2.7, max_angle = 3.4):
         self._set_lidar_mode(lidar_servo_modeRequest(
                     mode = lidar_servo_modeRequest.PAN,