cleaned up the planner + controller interface (#238)

* cleaned up the planner + controller interface

* fix CI error from old code

---------

Co-authored-by: frompotenza <83185972+frompotenza@users.noreply.github.com>

catkin_ws/src/planner/src/mission.py

@@ -19,30 +19,17 @@
 
 def endMission(msg):
     print(msg)
-    control.stop_in_place()
+    control.freeze_pose()
 
 def endPlanner(msg="Shutting down mission planner."):
     print(msg)
     control.kill()
 
-def qualiVisionMission():
-    global sm
-    sm = smach.StateMachine(outcomes=['success', 'failure']) 
-    with sm:
-        smach.StateMachine.add('initialization', InitializeForComp(wait_time=wait_time_for_comp), 
-                transitions={'success': 'find_quali'})
-        smach.StateMachine.add('find_quali', LinearSearch(timeout=120, forward_speed=5, target_class="Quali Gate", min_objects=1, control=control, mapping=mapping), 
-                transitions={'success': 'quali', 'failure': 'failure'})
-        smach.StateMachine.add('quali', QualiVision(control=control, mapping=mapping, state=state, quali_gate_width=quali_gate_width), 
-            transitions={'success': 'success', 'failure':'failure'})
-    res = sm.execute()
-    endMission("Finished quali mission. Result {}".format(res))
-
 def gateMission():
     global sm
     sm = smach.StateMachine(outcomes=['success', 'failure']) 
     with sm:
-        smach.StateMachine.add('find_gate', InPlaceSearch(timeout=120, target_class="Gate", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_gate', InPlaceSearch(timeout=120, target_class="Gate", min_objects=1, control=control, mapping=mapping, search_depth=-2), 
             transitions={'success': 'navigate_gate_go_through', 'failure': 'failure'})
         smach.StateMachine.add('navigate_gate_go_through', NavigateGate(control=control, mapping=mapping, state=state, goThrough=True, target_symbol=target_symbol, gate_width=gate_width), 
             transitions={'success': 'success', 'failure':'failure'})
@@ -53,7 +40,7 @@ def buoyMission():
     global sm
     sm = smach.StateMachine(outcomes=['success', 'failure'])
     with sm:
-        smach.StateMachine.add('find_buoy', InPlaceSearch(timeout=120, target_class="Buoy", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_buoy', InPlaceSearch(timeout=120, target_class="Buoy", min_objects=1, control=control, mapping=mapping, search_depth=-2), 
                 transitions={'success': 'navigate_buoy', 'failure': 'failure'})
 
         smach.StateMachine.add('navigate_buoy', NavigateBuoy(control=control, mapping=mapping, state=state, target_symbol=target_symbol, buoy_width=buoy_width, buoy_height=buoy_height), 
@@ -75,7 +62,7 @@ def laneMarkerMission():
     global sm
     sm = smach.StateMachine(outcomes=['success', 'failure'])
     with sm:
-        smach.StateMachine.add('find_lane_marker', BreadthFirstSearch(timeout=120, expansionAmt=0.5, target_class="Lane Marker", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_lane_marker', BreadthFirstSearch(timeout=120, expansionAmt=0.5, target_class="Lane Marker", min_objects=1, control=control, mapping=mapping, search_depth=-1), 
                 transitions={'success': 'navigate_lane_marker', 'failure': 'failure'})
 
         smach.StateMachine.add('navigate_lane_marker', NavigateLaneMarker(origin_class="", control=control, mapping=mapping, state=state), 
@@ -87,10 +74,7 @@ def semiFinals():
     global sm
     sm = smach.StateMachine(outcomes=['success', 'failure']) 
     with sm:
-        smach.StateMachine.add('initialization', InitializeForComp(wait_time=wait_time_for_comp), 
-                transitions={'success': 'find_gate'})
-
-        smach.StateMachine.add('find_gate', InPlaceSearch(timeout=120, target_class="Gate", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_gate', InPlaceSearch(timeout=120, target_class="Gate", min_objects=1, control=control, mapping=mapping, search_depth=-2), 
                 transitions={'success': 'navigate_gate_no_go_through', 'failure': 'failure'})
 
         smach.StateMachine.add('navigate_gate_no_go_through', NavigateGate(control=control, mapping=mapping, state=state, goThrough=False, target_symbol=target_symbol, gate_width=gate_width), 
@@ -102,25 +86,25 @@ def semiFinals():
         smach.StateMachine.add('navigate_gate_go_through', NavigateGate(control=control, mapping=mapping, state=state, goThrough=True, target_symbol=target_symbol, gate_width=gate_width), 
                 transitions={'success': 'find_lane_marker', 'failure': 'failure'})
 
-        smach.StateMachine.add('find_lane_marker', BreadthFirstSearch(timeout=120, expansionAmt=0.5, target_class="Lane Marker", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_lane_marker', BreadthFirstSearch(timeout=120, expansionAmt=0.5, target_class="Lane Marker", min_objects=1, control=control, mapping=mapping, search_depth=-1), 
                 transitions={'success': 'navigate_lane_marker', 'failure': 'failure'})
 
         smach.StateMachine.add('navigate_lane_marker', NavigateLaneMarker(origin_class="Gate", control=control, mapping=mapping, state=state), 
                 transitions={'success': 'find_buoy', 'failure': 'failure'})
 
-        smach.StateMachine.add('find_buoy', LinearSearch(timeout=120, forward_speed=5, target_class="Buoy", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_buoy', LinearSearch(timeout=120, forward_speed=5, target_class="Buoy", min_objects=1, control=control, mapping=mapping, search_depth=-2), 
                 transitions={'success': 'navigate_buoy', 'failure': 'failure'})
 
         smach.StateMachine.add('navigate_buoy', NavigateBuoy(control=control, mapping=mapping, state=state, target_symbol=target_symbol, buoy_width=buoy_width, buoy_height=buoy_height), 
                 transitions={'success': 'find_second_lane_marker', 'failure':'failure'})
 
-        smach.StateMachine.add('find_second_lane_marker', BreadthFirstSearch(timeout=120, expansionAmt=0.5, target_class="Lane Marker", min_objects=2, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_second_lane_marker', BreadthFirstSearch(timeout=120, expansionAmt=0.5, target_class="Lane Marker", min_objects=2, control=control, mapping=mapping, search_depth=-1), 
                 transitions={'success': 'navigate_second_lane_marker', 'failure': 'failure'})
 
         smach.StateMachine.add('navigate_second_lane_marker', NavigateLaneMarker(origin_class="Buoy", control=control, mapping=mapping, state=state), 
                 transitions={'success': 'find_octagon', 'failure': 'failure'})
 
-        smach.StateMachine.add('find_octagon', LinearSearch(timeout=120, forward_speed=5, target_class="Octagon Table", min_objects=1, control=control, mapping=mapping), 
+        smach.StateMachine.add('find_octagon', LinearSearch(timeout=120, forward_speed=5, target_class="Octagon Table", min_objects=1, control=control, mapping=mapping, search_depth=-2), 
                 transitions={'success': 'navigate_octagon', 'failure':'failure'})
 
         smach.StateMachine.add('navigate_octagon', NavigateOctagon(control=control, mapping=mapping, state=state), 
@@ -146,7 +130,7 @@ def semiFinals():
 
 
         # control.move((None,None,-1))
-        # control.moveDelta((0,0,0))
+        # control.freeze_position()
         control.rotateEuler((0,0,None))
         # while True:
         #     control.rotateEuler((0,0,0))

catkin_ws/src/planner/src/substates/breadth_first_search.py

@@ -7,7 +7,7 @@
 
 #search for objects by moving in a growing square (i.e. each side of square grows in size after every rotation)
 class BreadthFirstSearch(smach.State):
-    def __init__(self, expansionAmt, control, mapping, target_class, min_objects, timeout):
+    def __init__(self, expansionAmt, control, mapping, target_class, min_objects, timeout, search_depth):
         super().__init__(outcomes=['success', 'failure'])
         self.control = control
         self.mapping = mapping
@@ -16,37 +16,25 @@ def __init__(self, expansionAmt, control, mapping, target_class, min_objects, ti
         self.expansionAmt = expansionAmt
         self.target_class = target_class
         self.min_objects = min_objects
+        self.search_depth = search_depth
 
     def doBreadthFirstSearch(self):
-        global moving
-        moving = False                
-
-        movement = [0,1,0]
-
+        movement = [0,self.expansionAmt,0]
         while not rospy.is_shutdown():
             #move left
             print("Moving by {}.".format(movement))
-            moving = True
-            # [TODO] FIX
-            self.control.moveDeltaLocal(movement, callback=movementComplete, face_destination=True)
-            #check for object detected while moving
-            while moving and not rospy.is_shutdown():
-                if self.detectedObject: return # stop grid search when object found
+            self.control.moveDeltaLocal(movement, face_destination=True)
+            if self.detectedObject: return # stop grid search when object found
             #move left by the same amount again
             print("Moving by {}.".format(movement))
-            moving = True
-            self.control.moveDeltaLocal(movement, callback=movementComplete, face_destination=True)
-            #check for object detected while moving
-            while moving and not rospy.is_shutdown():
-                if self.detectedObject: return # stop grid search when object found
+            self.control.moveDeltaLocal(movement, face_destination=True)
+            if self.detectedObject: return # stop grid search when object found
             #increase distance to move by
             movement[1] += self.expansionAmt
 
     def execute(self, ud):
         print("Starting breadth-first search.")
-        #MOVE TO MIDDLE OF POOL DEPTH AND FLAT ORIENTATION
-        # [COMP] make sure depth is appropriate here
-        self.control.move((None, None, -0.5))
+        self.control.move((None, None, self.search_depth))
         self.control.rotateEuler((0,0,None))
 
         self.searchThread = threading.Thread(target=self.doBreadthFirstSearch)
@@ -56,14 +44,12 @@ def execute(self, ud):
             if len(self.mapping.getClass(self.target_class)) >= self.min_objects:
                 self.detectedObject = True
                 self.searchThread.join()
-                self.control.stop_in_place()
-                print("Found object! Waiting 10 seconds to get more observations of object.")
-                rospy.sleep(10)
+                self.control.freeze_pose()
+                print("Found object! Waiting 5 seconds to get more observations of object.")
+                rospy.sleep(5)
                 return 'success'
-        self.control.stop_in_place()
+        self.detectedObject = True
+        self.searchThread.join()
+        self.control.freeze_pose()
         print("Breadth-first search timed out.")
-        return 'failure'
-
-def movementComplete(msg1=None, msg2=None): #called when translation is complete
-    global moving
-    moving = False    
+        return 'failure'   

catkin_ws/src/planner/src/substates/in_place_search.py

@@ -7,20 +7,17 @@
 
 #search for objects by moving in a growing square (i.e. each side of square grows in size after every rotation)
 class InPlaceSearch(smach.State):
-    def __init__(self, timeout, control, mapping, target_class, min_objects):
+    def __init__(self, timeout, control, mapping, target_class, min_objects, search_depth):
         super().__init__(outcomes=['success', 'failure'])
         self.control = control
         self.mapping = mapping
         self.timeout = timeout
         self.target_class = target_class
         self.min_objects = min_objects
         self.detectedObject = False
+        self.search_depth = search_depth
 
     def doRotation(self):
-        rotating = False
-        def rotationComplete(arg1, arg2): #called when rotation is complete
-            self.rotating = False
-
         turn_amt = (0,0,-30)
         num_turns = 0
         num_full_turns = 0
@@ -29,22 +26,19 @@ def rotationComplete(arg1, arg2): #called when rotation is complete
             if (num_turns >= 360/abs(turn_amt[2])):
                 num_turns = 0
                 num_full_turns += 1
-                if num_full_turns == 1: self.control.move((None,None,-0.5))
-                elif num_full_turns == 2: self.control.move((None,None,-1.5))
+                if num_full_turns == 1: self.control.move((None,None, self.search_depth + 1))
+                elif num_full_turns == 2: self.control.move((None,None, self.search_depth - 1))
                 else: return
             #move forward
             print("Rotating by {}.".format(turn_amt))
-            self.rotating = True
-            self.control.rotateDeltaEuler(turn_amt, rotationComplete)
-            #check for object detected while rotating
-            while self.rotating and not rospy.is_shutdown():
-                if self.detectedObject: return # stop grid search when object found
+            self.control.rotateDeltaEuler(turn_amt)
+            if self.detectedObject: return # stop grid search when object found
             num_turns += 1
 
     def execute(self, ud):
         print("Starting in-place search.")
         #MOVE TO MIDDLE OF POOL DEPTH AND FLAT ORIENTATION
-        self.control.move((None, None, -1))
+        self.control.move((None, None, self.search_depth))
         self.control.rotateEuler((0,0,None))
 
         self.searchThread = threading.Thread(target=self.doRotation)
@@ -55,10 +49,12 @@ def execute(self, ud):
             if len(self.mapping.getClass(self.target_class)) >= self.min_objects:
                 self.detectedObject = True
                 self.searchThread.join()
-                self.control.stop_in_place()
+                self.control.freeze_pose()
                 print("Found object! Waiting 10 seconds to get more observations of object.")
                 rospy.sleep(10)
                 return 'success'
-        self.control.stop_in_place()
+        self.detectedObject = True
+        self.searchThread.join()
+        self.control.freeze_pose()
         print("In-place search timed out.")
         return 'failure'

catkin_ws/src/planner/src/substates/linear_search.py

@@ -6,35 +6,33 @@
 
 #ASSUMES AUV IS FACING DIRECTION TO SEARCH IN
 class LinearSearch(smach.State):
-    def __init__(self, timeout, forward_speed, control, mapping, target_class, min_objects):
+    def __init__(self, timeout, forward_speed, control, mapping, target_class, min_objects, search_depth):
         super().__init__(outcomes=['success', 'failure'])
         self.control = control
         self.mapping = mapping
         self.detectedObject = False
         self.target_class = target_class
         self.min_objects = min_objects
         self.timeout = timeout
+        self.search_depth = search_depth
         self.forward_speed = forward_speed
 
     def execute(self, ud):
         print("Starting linear search.")
-        #MOVE TO MIDDLE OF POOL DEPTH AND FLAT ORIENTATION
-        #[COMP] set to appropriate search depth
-        self.control.move((None, None, -1))
+        self.control.move((None, None, self.search_depth))
         self.control.rotateEuler((0,0,None))
 
-        # self.control.forceLocal((self.forward_speed, 0))
         startTime = time.time()
         while startTime + self.timeout > time.time() and not rospy.is_shutdown(): 
-            self.control.moveDeltaLocal((0.5,0,0))
+            self.control.moveDeltaLocal((2,0,0))
             if len(self.mapping.getClass(self.target_class)) >= self.min_objects:
                 self.detectedObject = True
-                self.control.stop_in_place()
+                self.control.freeze_pose()
                 print("Found object! Waiting 10 seconds to get more observations of object.")
                 rospy.sleep(10)
                 return 'success'
             rospy.sleep(0.1)
-        self.control.stop_in_place()
+        self.control.freeze_pose()
         print("Linear search timed out.")
         return 'failure'
 

catkin_ws/src/planner/src/substates/navigate_lane_marker.py

@@ -15,7 +15,6 @@ def __init__(self, origin_class, control, state, mapping):
     def execute(self, ud):
         print("Starting lane marker navigation.") 
         #MOVE TO MIDDLE OF POOL DEPTH AND FLAT ORIENTATION
-        #[COMP] make sure depth is appropriate here
         self.control.move((None, None, -2))
         self.control.rotateEuler((0,0,None))
 
@@ -38,15 +37,15 @@ def execute(self, ud):
                 return 'failure'
             rospy.sleep(5.0)
             self.mapping.updateObject(lane_marker_obj)
-            self.control.move((lane_marker_obj[1], lane_marker_obj[2], -2))
+            self.control.move((lane_marker_obj[1], lane_marker_obj[2], -2), face_destination=True)
 
         #Waiting 10 seconds and repeating to make sure it's correct
         print("Waiting 10 seconds to ensure correct measurement of lane marker")
         self.mapping.updateObject(lane_marker_obj)
-        self.control.move((lane_marker_obj[1], lane_marker_obj[2], -2))
+        self.control.move((lane_marker_obj[1], lane_marker_obj[2], -2), face_destination=True)
         rospy.sleep(10)
         self.mapping.updateObject(lane_marker_obj)
-        self.control.move((lane_marker_obj[1], lane_marker_obj[2], -2))
+        self.control.move((lane_marker_obj[1], lane_marker_obj[2], -2), face_destination=True)
 
         heading1 = lane_marker_obj[4]
         heading2 = lane_marker_obj[5]

catkin_ws/src/planner/src/substates/octagon_task.py

@@ -23,7 +23,7 @@ def execute(self, ud):
             return 'failure'
 
         print("Moving to the center of the octagon.")
-        self.control.move((octagon_obj[1], octagon_obj[2], -2))
+        self.control.move((octagon_obj[1], octagon_obj[2], -2), face_destination=True)
         print("Surfacing.")
         self.control.kill()
 

catkin_ws/src/planner/src/substates/trick.py

@@ -12,7 +12,7 @@ def __init__(self, control, trick_type, num_full_spins=1):
 
     def execute(self,ud):
         #STAY IN SAME POSITION AND AT FLAT ORIENTATION
-        self.control.moveDelta((0,0,0))
+        self.control.freeze_position()
         self.control.rotateEuler((0,0,None))
 
         if self.trick_type == "roll":