MISSIONS: balldrop, pinger, startgate

command/sub8_launch/launch/subsystems/perception.launch

@@ -1,7 +1,7 @@
 <launch>
-  <arg name="tensorflow" default="True" />
-  <arg name="torp" default="True" />
-
+  <arg name="tensorflow" default="False" />
+  <arg name="torp" default="False" />
+  <arg name="threed" default="True" />
   <!-- roslaunch sub8_launch perception.launch color_calibration:=gazebo_color_calibration.yaml -->
   <arg name="color_calibration" default="default_color_calibration.yaml" />
 
@@ -12,6 +12,8 @@
     <node if="$(arg torp)" pkg="sub8_perception" type="torpedo_target_acq.py" name="arm_torpedos" output="screen">
       <rosparam file="$(find sub8_launch)/config/torpedo_target_acq.yaml" command="load"/>
     </node>
+    <node if="$(arg threed)" pkg="sub8_perception" type="observation_pub.py" name="xyz_points" output="screen">
+    </node>
     <node if="$(arg tensorflow)" pkg="sub8_perception" type="object_detection.py" name="object_detection" output="screen">
       <rosparam file="$(find sub8_launch)/config/path_localizer.yaml" command="load" />
     </node>

command/sub8_missions/sub8_missions/__init__.py

@@ -12,3 +12,6 @@
 from .gripper_test import GripperTest
 from .strip import Strip
 from .ball_drop import BallDrop
+from .arm_torpedos import FireTorpedos
+from .dracula_grab import DraculaGrabber
+from .start_gate_guess import StartGateGuess

command/sub8_missions/sub8_missions/autonomous.py

@@ -10,6 +10,8 @@
 from .pinger import Pinger
 from .surface import Surface
 from .vampire_slayer import VampireSlayer
+from .arm_torpedos import FireTorpedos
+from .ball_drop import BallDrop
 
 
 fprint = text_effects.FprintFactory(title="AUTO_MISSION").fprint
@@ -52,8 +54,7 @@ def do_mission(self):
                         yield self.run_mission(Surface(), 30)
                     elif (yield self.nh.get_param('pinger_where')) == 1:
                         fprint('Shooting Mission')
-                        yield self.move.yaw_right_deg(179).go(speed=0.4)
-                        yield self.move.yaw_right_deg(179).go(speed=0.4)
+                        yield self.run_mission(FireTorpedos(), 400)
 
             # Go to the other pinger mission and do respective mission
             completed = yield self.run_mission(Pinger(), 400)
@@ -67,12 +68,12 @@ def do_mission(self):
 
                     elif (yield self.nh.get_param('pinger_where')) == 1:
                         fprint('Shooting Mission')
-                        yield self.move.yaw_right_deg(179).go(speed=0.4)
-                        yield self.move.yaw_right_deg(179).go(speed=0.4)
+                        yield self.run_mission(FireTorpedos(), 400)
 
             fprint("Vampire Slayer")
             yield self.run_mission(VampireSlayer(), 400)
             fprint("Garlic drop?")
+            yield self.rub_mission(BallDrop(), 400)
 
         except Exception as e:
             fprint("Error in Chain 1 missions!", msg_color="red")

command/sub8_missions/sub8_missions/ball_drop.py

@@ -12,21 +12,31 @@
 from sub8_msgs.srv import GuessRequest, GuessRequestRequest
 
 import mil_ros_tools
+import rospy
 
 fprint = text_effects.FprintFactory(title="BALL_DROP", msg_color="cyan").fprint
 
 SPEED = 0.25
 FAST_SPEED = 1
 
 SEARCH_HEIGHT = 1.5
-HEIGHT_BALL_DROPER = 0.5
-TRAVEL_DEPTH = 0.5  # 2
+HEIGHT_BALL_DROPER = 0.75
+TRAVEL_DEPTH = 0.75  # 2
 
 
 class BallDrop(SubjuGator):
 
     @util.cancellableInlineCallbacks
     def run(self, args):
+        try:
+          ree = rospy.ServiceProxy('/vision/garlic/enable', SetBool)
+          resp = ree(True)
+          if not resp:
+            print("Error, failed to init neural net.")
+            return
+        except rospy.ServiceException, e:
+          print("Service Call Failed")
+
         fprint('Enabling cam_ray publisher')
 
         yield self.nh.sleep(1)

command/sub8_missions/sub8_missions/pinger.py

@@ -20,7 +20,7 @@
 PINGER_HEIGHT = 0.75  # how high to go above pinger after found
 MOVE_AT_DEPTH = 0.5  # how low to swim and move
 
-POSITION_TOL = 0.075  # how close to pinger before quiting
+POSITION_TOL = 0.09  # how close to pinger before quiting
 Z_POSITION_TOL = -0.53
 
 
@@ -132,7 +132,7 @@ def run(self, args):
                                                            pinger_2_req)
                 fprint('Transformed guess: {}'.format(pinger_guess))
                 # Check if the pinger aligns with guess
-                check, vec = self.check_with_guess(vec, pinger_guess)
+                # check, vec = self.check_with_guess(vec, pinger_guess)
 
             fprint('move to {}'.format(vec))
             yield self.fancy_move(self, vec)

command/sub8_missions/sub8_missions/start_gate_guess.py

@@ -7,38 +7,37 @@
 
 fprint = text_effects.FprintFactory(title="PINGER", msg_color="cyan").fprint
 
-SPEED = 0.2
+SPEED = 0.6
 DOWN_SPEED = 0.1
 
 DOWN = 1.5
 
 
 class StartGateGuess(SubjuGator):
-
     @util.cancellableInlineCallbacks
     def run(self, args):
 
-        fprint('Getting Guess Locations')
+      fprint('Getting Guess Locations')
 
-        sub_start_position, sub_start_orientation = yield self.tx_pose()
+      sub_start_position, sub_start_orientation = yield self.tx_pose()
 
-        gate_txros = yield self.nh.get_service_client('/guess_location',
-                                                      GuessRequest)
-        gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1'))
-        gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2'))
+      gate_txros = yield self.nh.get_service_client('/guess_location',
+                                                   GuessRequest)
+      gate_1_req = yield gate_txros(GuessRequestRequest(item='start_gate1'))
+      gate_2_req = yield gate_txros(GuessRequestRequest(item='start_gate2'))
 
-        gate_1 = mil_ros_tools.rosmsg_to_numpy(
-            gate_1_req.location.pose.position)
-        gate_2 = mil_ros_tools.rosmsg_to_numpy(
-            gate_2_req.location.pose.position)
+      gate_1 = mil_ros_tools.rosmsg_to_numpy(gate_1_req.location.pose.position)
+      gate_2 = mil_ros_tools.rosmsg_to_numpy(gate_2_req.location.pose.position)
 
-        mid = (gate_1 + gate_2) / 2
-        fprint('Found mid {}'.format(mid))
+      #mid = (gate_1 + gate_2) / 2
+      #mid = gate_1
+      fprint('Found mid {}'.format(gate_1))
 
-        fprint('Looking at gate')
-        yield self.move.down(DOWN).set_orientation(sub_start_orientation).go(
-            speed=DOWN_SPEED)
-        yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED)
+      fprint('Looking at gate')
+#      yield self.move.down(DOWN).set_orientation(sub_start_orientation).go(
+          #########speed=DOWN_SPEED)
+#      yield self.move.look_at_without_pitching(mid).go(speed=DOWN_SPEED)
 
-        fprint('Going!')
-        yield self.move.set_position(mid).depth(DOWN).go(speed=SPEED)
+      fprint('Going!')
+      yield self.move.set_position(gate_1).depth(DOWN).go(speed=SPEED)
+      yield self.move.forward(1).go(speed=SPEED)

command/sub8_missions/sub8_missions/sub_singleton.py

@@ -236,8 +236,8 @@ def shoot_torpedo2(self):
 
     @util.cancellableInlineCallbacks
     def drop_marker(self):
-        yield self.pulse(2)
-        yield self.pulse(3)
+        yield self.pulse(2, time=0.75)
+        yield self.pulse(3, time=0.25)
         return
 
 

command/sub8_missions/sub8_missions/vision_proxies.yaml

@@ -6,8 +6,8 @@ orange_rectangle:
     root: "/vision/orange_rectangle"
 buoy:
     root: "/vision/buoys"
-arm_torpedos:
-    root: "/vision/arm_torpedos"
+xyz_points:
+    root: "/vision/xyz_points"
 the_path:
     root: "/vision/the_path"
 start_gate:

perception/sub8_perception/ml_classifiers/localizer.py

@@ -55,12 +55,12 @@ def __init__(self):
             self.target = 'vamp'
             self.classes = 4
             self.see_sub = mil_tools.Image_Subscriber(
-              topic="/camera/front/right/image_raw", callback=self.img_callback)
+              topic="/camera/front/left/image_raw", callback=self.img_callback)
         else:  
             self.target = 'stake'
             self.classes = 1
             self.see_sub = mil_tools.Image_Subscriber(
-              topic="/camera/front/right/image_raw", callback=self.img_callback)
+              topic="/camera/front/left/image_raw", callback=self.img_callback)
         # Number of frames
         self.num_frames = rospy.get_param('~num_frames', 0)
         # Number of objects we detect
@@ -146,12 +146,13 @@ def img_callback(self, data):
         if len(bbox) > 0:
           pointx = (bbox[0][0] + bbox[1][0]) /2 
           pointy = (bbox[0][1] + bbox[1][1]) /2
-          pointxdist = bbox[0][0] - bbox[1][0]
-          pointydist = bbox[0][1] - bbox[1][1]
-          msg = Point(x=pointx, y=pointy, z=-1)
-          print("Point: ", pointx, "Point: ", pointy)
+          pointxdist = abs(bbox[0][0] - bbox[1][0])
+          pointydist = abs(bbox[0][1] - bbox[1][1])
+          print(pointxdist)
+          msg = Point(x=pointx, y=pointy, z=self.see_sub.last_image_time.to_sec())
+          print("X: ", pointx, "Y: ", pointy, "TIMESTAMP: ", msg.z)
           self.bbox_pub.publish(msg)
-          roi = RegionOfInterest(x_offset=bbox[0][0], y_offset=bbox[0][1], height = pointydist, width=pointxdist)
+          roi = RegionOfInterest(x_offset=int(bbox[0][0]), y_offset=int(bbox[0][1]), height = int(pointydist), width=int(pointxdist))
           self.roi_pub.publish(roi)
 
 

perception/sub8_perception/ml_classifiers/object_detection.py

@@ -12,8 +12,8 @@
 on enable, and close it on disable.
 '''
 
-# Exec names without the .py
-exec_names = ['dice_detection', 'path_localizer']
+# Exec instructions
+exec_names = ['vamp', 'stake', 'garlic']
 
 # Store subprocesses in global
 alive_processes = {}
@@ -37,7 +37,7 @@ def enable_callback(self, srv):
             rospy.loginfo('Enabling {}'.format(self.name))
             # Open up a subprocess under PID
             alive_processes[self.name] = subprocess.Popen(
-                'rosrun sub8_perception {}.py'.format(self.name),
+		'rosrun sub8_perception localizer.py --{}'.format(self.name),
                 shell=True,
                 stdout=subprocess.PIPE,
                 stderr=subprocess.STDOUT,

perception/sub8_perception/nodes/guess_server.py

@@ -11,7 +11,7 @@ class Guess:
     def __init__(self):
         rospy.sleep(1.0)
         self.items = [
-            'pinger_surface', 'pinger_shooter', 'vampire_slayer'
+            'pinger_surface', 'pinger_shooter', 'vampire_slayer', 'start_gate1', 'start_gate2'
         ]
         self.guess_service = rospy.Service('guess_location', GuessRequest,
                                            self.request_location)
@@ -41,7 +41,7 @@ def __init__(self):
         spacer = 0
         for i in self.items:
             self.markers.append(InteractiveMarker())
-            self.markers[spacer].header.frame_id = "map"
+            self.markers[spacer].header.frame_id = "/map"
             self.markers[spacer].name = i
             self.markers[spacer].description = i
             self.markers[spacer].controls.append(box_control)