auto mission hi

uf-mil · Nov 8, 2024 · 025bd02 · 025bd02
1 parent 1fa8c65
commit 025bd02
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 11 deletions.
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py
@@ -6,6 +6,7 @@
 
 from .docking import Docking
 from .entrance_gate2 import EntranceGate2
+from .go_to_poi import GoToPOI
 from .navigation import Navigation
 from .navigator import NaviGatorMission
 from .scan_the_code import ScanTheCodeMission
@@ -17,28 +18,75 @@ class Autonomous2024(NaviGatorMission):
     # timeout (in secs)
     TIMEOUT = 180
 
-    async def run_mission(self, mission_cls: type[NaviGatorMission], name: str):
+    async def run_mission(
+        self,
+        mission_cls: type[NaviGatorMission],
+        name: str,
+        *args,
+        str_arg: str = "",
+        **kwargs,
+    ):
         rospy.loginfo(f"[autonomous] beginning {name}...")
         try:
-            await asyncio.wait_for(mission_cls().run(""), self.TIMEOUT)
+            await asyncio.wait_for(
+                mission_cls().run(str_arg, *args, **kwargs), self.TIMEOUT,
+            )
         except asyncio.TimeoutError:
             rospy.logwarn(f"[autonomous] ran out of time on {name}!")
 
     async def run(self, args: str):
         # Step 1: Entrance and exit gates
-        await self.run_mission(EntranceGate2, "entrance gate")
+        await self.send_feedback("[AUTONOMOUS] GOING TO ENTRANCE GATE")
+        await self.run_mission(GoToPOI, "entrance_gate", str_arg="entrance_gate")
+        await self.send_feedback("[AUTONOMOUS] STARTING AUTONOMOUS MISSION")
+        await self.run_mission(EntranceGate2, "entrance gate", scan_code=True)
+
+        # Step 1.5: Launch the drone
+        # FILL IN HERE
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-45 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-35 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-25 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-15 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-5 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(5)
 
         # Step 2: Scan the Code
+        await self.send_feedback("[AUTONOMOUS] GOING TO SCAN THE CODE")
+        await self.run_mission(GoToPOI, "scan_the_code poi", str_arg="scan_the_code")
         await self.run_mission(ScanTheCodeMission, "scan the code")
 
         # Step 3: Wildlife Mission
+        await self.send_feedback("[AUTONOMOUS] GOING TO WILDLIFE")
+        await self.run_mission(GoToPOI, "wildlife poi", str_arg="wildlife")
         await self.run_mission(Wildlife, "wildlife")
 
         # Step 4: Navigation Mission
+        await self.send_feedback("[AUTONOMOUS] GOING TO NAVIGATION")
+        await self.run_mission(GoToPOI, "navigation poi", str_arg="navigation")
         await self.run_mission(Navigation, "navigation")
 
+        # Step 4.5: Receive the drone
+        # FILL IN HERE
+
         # Step 5: Dock Mission
+        await self.send_feedback("[AUTONOMOUS] GOING TO DOCKING")
+        await self.run_mission(GoToPOI, "docking poi", str_arg="docking")
         await self.run_mission(Docking, "docking")
 
-        # Step 6: UAV Mission
-        pass
+        # Step 6: Leave the course
+        await self.send_feedback("[AUTONOMOUS] LEAVING COURSE")
+        await self.run_mission(GoToPOI, "exit_gate poi", str_arg="exit_gate")
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py b/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py
@@ -3,23 +3,21 @@
 
 import numpy as np
 from mil_tools import quaternion_matrix
-from std_srvs.srv import SetBoolRequest
 
 from .navigator import NaviGatorMission
 
 
 class EntranceGate2(NaviGatorMission):
-    async def run(self, args):
+    async def run(self, args, *, scan_code=False):
         # Parameters:
-        scan_code = False
         return_to_start = True
         circle_radius = 10
         circle_direction = "cw"
         yaw_offset = 1.57
         self.traversal_distance = 3
 
-        #await self.set_classifier_enabled.wait_for_service()
-        #await self.set_classifier_enabled(SetBoolRequest(data=True))
+        # await self.set_classifier_enabled.wait_for_service()
+        # await self.set_classifier_enabled(SetBoolRequest(data=True))
 
         # Inspect Gates
         await self.change_wrench("/wrench/autonomous")

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py
@@ -150,7 +150,7 @@ async def explore_closest_until(self, is_done, filter_and_sort) -> dict:
                 if ret is not None
                 else []
             )
-            self.send_feedback(f"Analyzing objects: {labels}")
+            # self.send_feedback(f"Analyzing objects: {labels}")
             if ret is not None:
                 if move_id_tuple is not None:
                     self.send_feedback("Condition met. Canceling investigation")