Start gate mission 2024

SubjuGator/command/subjugator_launch/launch/sub8.launch

@@ -29,6 +29,10 @@
   <group ns="course">
       <rosparam file="$(find subjugator_launch)/config/course_geometry.yaml" />
   </group>
+  <!-- Parameters for competition strategy -->
+  <group ns="strategy">
+      <rosparam file="$(find subjugator_missions)/config/strategy.yaml" />
+  </group>
 
   <include if="$(eval environment != 'dynsim')" file="$(find subjugator_launch)/launch/subsystems/cameras.launch">
     <arg name="environment" value="$(arg environment)" />

SubjuGator/command/subjugator_missions/config/strategy.yaml

@@ -0,0 +1,28 @@
+---
+####################################
+# Overall variables
+####################################
+
+# Which team the sub is playing for.
+#   - Two possible values: red, blue.
+team: red
+
+####################################
+# Start Gate task
+####################################
+start_gate:
+  with_style: true
+
+####################################
+# Hydrothermal Vent (red buoy) task
+####################################
+
+####################################
+# Ocean Temperatures (bins) task
+####################################
+
+####################################
+# Mapping (torpedoes) task
+####################################
+torpedoes:
+  color: red

SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py

@@ -11,7 +11,7 @@
 from .prequal_mission import PrequalMission
 from .square import Square
 from .start_gate import StartGate
-from .start_gate_2022 import StartGate2022
+from .start_gate_2024 import StartGate2024
 from .start_gate_guess import StartGateGuess
 from .strip import Strip
 from .sub_singleton import SubjuGatorMission

SubjuGator/command/subjugator_missions/subjugator_missions/autonomous_2022.py

@@ -3,7 +3,7 @@
 from ros_alarms import TxAlarmBroadcaster
 
 # Import missions here
-from .start_gate_2022 import StartGate2022
+from .start_gate_2024 import StartGate2024
 from .sub_singleton import SubjuGatorMission
 from .surface import Surface
 
@@ -31,7 +31,7 @@ async def do_mission(self):
         try:
             # Run start gate mission
             fprint("Running start gate mission", msg_color="green")
-            await self.run_mission(StartGate2022(), 400)
+            await self.run_mission(StartGate2024(), 400)
 
             # Run mission to follow orange marker
             fprint("Following orange marker", msg_color="green")

SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2022.py → SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2024.py

@@ -5,7 +5,6 @@
 from axros import Subscriber
 from mil_misc_tools import text_effects
 from sensor_msgs.msg import MagneticField
-from std_msgs.msg import Bool
 from tf.transformations import *
 
 from .sub_singleton import SubjuGatorMission
@@ -21,7 +20,7 @@
 WAIT_SECONDS = 1
 
 
-class StartGate2022(SubjuGatorMission):
+class StartGate2024(SubjuGatorMission):
     async def current_angle(self):
         imu_sub: Subscriber[MagneticField] = self.nh.subscribe(
             name="/imu/mag",
@@ -38,22 +37,34 @@ async def current_angle(self):
             + declination
         )
 
-    async def is_left(self):
-        side_sub: Subscriber[Bool] = self.nh.subscribe(
-            name="/getside",
-            message_type=Bool,
-        )
-        async with side_sub:
-            result = await side_sub.get_next_message()
-        return result.data
+    async def is_left(self, team) -> bool:
+        assert isinstance(team, str)
+        return await self.identify_side(team) == "left"
+
+    async def identify_side(self, team: str) -> str:
+        """
+        Identifies which side the side the desired team is on using computer
+        vision.
+
+        Args:
+            team (str): Identifies the team to look for. Will either be 'red'
+                or 'blue'.
+
+        Returns:
+            str: Either 'left' or 'right', depending on which side the team is.
+        """
+        # TODO: This needs to be implemented using CV or a similar algo.
+        return "left"
 
     async def run(self, args):
+        ### Start of mission
         fprint("Waiting for odom")
         await self.tx_pose()
 
         fprint(f"Waiting {WAIT_SECONDS} seconds...")
         await self.nh.sleep(WAIT_SECONDS)
 
+        ### Rotate to the start gate
         orientation = await self.nh.get_param("/course/start_gate/orientation")
         fprint(f"Rotating to start gate orientation (of {orientation} degrees)...")
         cur_degree = await self.current_angle()
@@ -67,24 +78,47 @@ async def run(self, args):
             await self.go(self.move().yaw_right_deg(orientation - cur_degree))
         await self.nh.sleep(2)  # Give sub time to turn before moving straight
 
-        fprint("Found odom")
+        ### Move down into the water
         down = self.move().down(1)
         await self.go(down, speed=SPEED)
 
-        fprint("Waiting for side")
-        IS_LEFT = await self.is_left()
-
-        side = "left" if IS_LEFT else "right"
+        ### Choose a side and move to it
+        team = await self.nh.get_param("/strategy/team")
+        fprint(f"Identifying the side of our team ({team})")
+        is_left = await self.is_left(team)
 
-        msg = "Found side: " + side
-        fprint(msg)
-
-        if IS_LEFT:
+        ### Move to the side of the start gate
+        fprint(f"Team was identified. Moving to the {team} side of the gate.")
+        if is_left:
             left = self.move().left(1)
             await self.go(left, speed=SPEED)
         else:
             right = self.move().right(1)
             await self.go(right, speed=SPEED)
 
-        forward = self.move().forward(5)
+        ### Move through the start gate
+        forward = self.move().forward(7)
         await self.go(forward, speed=SPEED)
+
+        ### Complete style if requested
+        style = await self.nh.get_param("/strategy/start_gate/with_style")
+        assert isinstance(style, bool)
+        if style:
+            fprint("Completing style...")
+            #### Rotate left
+            rotations = [90] * 4
+            for rotation in rotations:
+                await self.go(self.move().yaw_left_deg(rotation), speed=SPEED)
+                await self.nh.sleep(0.5)
+            await self.nh.sleep(1)  # Give sub some time to catch up
+
+            #### Rotate right
+            rotations = [90] * 4
+            for rotation in rotations:
+                await self.go(self.move().yaw_right_deg(rotation), speed=SPEED)
+                await self.nh.sleep(0.5)
+
+        else:
+            fprint("Not completing style (not requested).")
+
+        ### End of mission