diff --git a/SubjuGator/command/subjugator_launch/launch/sub8.launch b/SubjuGator/command/subjugator_launch/launch/sub8.launch
index 247fe1e62..4b90a9c49 100644
--- a/SubjuGator/command/subjugator_launch/launch/sub8.launch
+++ b/SubjuGator/command/subjugator_launch/launch/sub8.launch
@@ -29,6 +29,10 @@
+
+
+
+
diff --git a/SubjuGator/command/subjugator_missions/config/strategy.yaml b/SubjuGator/command/subjugator_missions/config/strategy.yaml
new file mode 100644
index 000000000..2e37e9c09
--- /dev/null
+++ b/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
diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py
index ae0fabb87..54fc51a13 100644
--- a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py
+++ b/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
diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/autonomous_2022.py b/SubjuGator/command/subjugator_missions/subjugator_missions/autonomous_2022.py
index af7322305..48cb717ef 100644
--- a/SubjuGator/command/subjugator_missions/subjugator_missions/autonomous_2022.py
+++ b/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")
diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2022.py b/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2024.py
similarity index 52%
rename from SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2022.py
rename to SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2024.py
index 6b1f06093..3ea38fff5 100644
--- a/SubjuGator/command/subjugator_missions/subjugator_missions/start_gate_2022.py
+++ b/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