init pacific gate mission

SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py

@@ -18,3 +18,4 @@
 from .surface import Surface
 from .torpedos_test import TorpedosTest
 from .vampire_slayer import VampireSlayer
+from .pacific_gate import PacificGate

SubjuGator/command/subjugator_missions/subjugator_missions/pacific_gate.py

@@ -0,0 +1,117 @@
+#!/usr/bin/env python
+
+import math
+
+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
+
+fprint = text_effects.FprintFactory(title="START_GATE", msg_color="cyan").fprint
+
+SPEED = 0.6
+
+CAREFUL_SPEED = 0.3
+
+# How many meters to pass the gate by
+DIST_AFTER_GATE = 1
+WAIT_SECONDS = 1
+
+
+class PacificGate(SubjuGatorMission):
+
+    async def current_angle(self):
+        imu_sub: Subscriber[MagneticField] = self.nh.subscribe(
+            name="/imu/mag",
+            message_type=MagneticField,
+        )
+        async with imu_sub:
+            reading = await imu_sub.get_next_message()
+        declination = await self.nh.get_param("/course/location/declination")
+        assert isinstance(declination, (float, int))
+        return (
+            math.atan2(reading.magnetic_field.z, reading.magnetic_field.y)
+            * 180
+            / math.pi
+            + 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 run(self, args):
+        fprint("Waiting for odom")
+        await self.tx_pose()
+
+        fprint(f"Waiting {WAIT_SECONDS} seconds...")
+        await self.nh.sleep(WAIT_SECONDS)
+
+        # Turn toward gate from random orientation
+        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()
+        fprint(f"Current degree: {cur_degree} degrees")
+        assert isinstance(orientation, int)
+        if cur_degree > orientation:
+            fprint(f"Yaw lefting: {cur_degree - orientation} degrees")
+            await self.go(self.move().yaw_left_deg(cur_degree - orientation))
+        else:
+            fprint(f"Yaw righting: {orientation - cur_degree} degrees")
+            await self.go(self.move().yaw_right_deg(orientation - cur_degree))
+        await self.nh.sleep(2)  # Give sub time to turn before moving straight
+
+        # Align to gate
+        fprint("Found odom")
+        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"
+
+        msg = "Found side: " + side
+        fprint(msg)
+
+
+
+        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)
+
+        # Style Points
+        yaw_angle = math.radians(720)
+        pitch_angle = math.radians(720)
+        roll_angle = math.radians(720)
+
+        await self.go(
+             self.move().yaw_left(yaw_angle)
+        )
+
+        await self.go(
+            self.move().pitch_up(pitch_angle)
+        )
+
+
+        await self.go(
+            self.move().roll_left(roll_angle)
+        )
+
+        # Pass Gate
+        forward = self.move().forward(5)
+        await self.go(forward, speed=SPEED)
+
+