diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py index ae0fabb87..7a0142573 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py +++ b/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 diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/pacific_gate.py b/SubjuGator/command/subjugator_missions/subjugator_missions/pacific_gate.py new file mode 100644 index 000000000..4399ecf9c --- /dev/null +++ b/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) + + diff --git a/mil_common/axros b/mil_common/axros index 8cdf13186..a4951d333 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit 8cdf131866af08edf0bbe3ac9020e1da47b3e432 +Subproject commit a4951d33356c349856532ddef5c97c6ec6810c28