final changes for 2024

uf-mil · Nov 9, 2024 · ec383e7 · ec383e7
1 parent df1031d
commit ec383e7
Show file tree

Hide file tree

Showing 4 changed files with 481 additions and 19 deletions.
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py b/NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py
@@ -25,6 +25,7 @@
 from .killed import Killed
 from .move import Move
 from .navigation import Navigation
+from .navigation_gatefinder import NavigationGatefinder
 from .navigator import NaviGatorMission
 from .obstacle_avoid import ObstacleAvoid
 from .pinger import PingerMission

diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py
@@ -3,11 +3,13 @@
 import asyncio
 import datetime
 
+import axros
 from navigator_msgs.msg import ScanTheCode
+from navigator_msgs.srv import MessageDetectDock, MessageDetectDockRequest
+from std_srvs.srv import Empty, EmptyRequest, SetBool, SetBoolRequest
 
 from .go_to_poi import GoToPOI
 from .navigator import NaviGatorMission
-from .wildlife import Wildlife
 
 # from .scan_the_code_2024 import ScanTheCode2024
 
@@ -53,7 +55,9 @@ def send_feedback(self, msg: str):
     async def run(self, args: str):
         # Step 1: Entrance gate
         td_feedback_pub = self.nh.advertise("/scan_the_code", ScanTheCode)
-        async with td_feedback_pub:
+        stc_pub = self.nh.advertise("/stc_display", ScanTheCode)
+        async with td_feedback_pub, stc_pub:
+            stc_pub.publish(ScanTheCode(color_pattern="XXX"))
             self.start_time = datetime.datetime.now()
             self._last_checkpoint = self.start_time
             # self.send_feedback("proceeding to entrance gate...")
@@ -68,27 +72,73 @@ async def run(self, args: str):
             # )
 
             # Step 2: Scan the Code
-            self.send_feedback("going to scan_the_code poi...")
-            await self.go_to_poi("scan_the_code")
-            # await self.run_mission(ScanTheCode2024, "scan the code")
-            await asyncio.sleep(5)
-            # fake publishing
-            sequence = "RGB"
-            self.send_feedback("reporting sequence...")
-            td_feedback_pub.publish(ScanTheCode(color_pattern=sequence))
-            await self.nh.set_param("color_sequence", sequence)
-
-            # # Step 3: Wildlife Mission
-            self.send_feedback("going to wildlife poi...")
-            await self.go_to_poi("wildlife")
-            self.send_feedback("running wildlife mission...")
-            await self.run_mission(Wildlife, "wildlife", timeout=250)
+            # self.send_feedback("going to scan_the_code poi...")
+            # await self.go_to_poi("scan_the_code")
+            # # await self.run_mission(ScanTheCode2024, "scan the code")
+            # await asyncio.sleep(5)
+            # # fake publishing
+            # sequence = "RGB"
+            # self.send_feedback("reporting sequence...")
+            # td_feedback_pub.publish(ScanTheCode(color_pattern=sequence))
+            # stc_pub.publish(ScanTheCode(color_pattern=sequence))
+            # await self.nh.set_param("color_sequence", sequence)
+
+            # # # Step 3: Wildlife Mission
+            # self.send_feedback("going to wildlife poi...")
+            # await self.go_to_poi("wildlife")
+            # self.send_feedback("running wildlife mission...")
+            # await self.run_mission(Wildlife, "wildlife", timeout=250)
 
             # Step 4: Navigation Mission
-            # await self.run_mission(Navigation, "navigation")
+            # self.send_feedback("going to navigation poi...")
+            # await self.go_to_poi("navigation")
+            # await self.run_mission(NavigationGatefinder, "navigation", timeout=240)
 
             # Step 5: Dock Mission
+            self.send_feedback("going to docking poi...")
+            await self.go_to_poi("docking")
             # await self.run_mission(Docking, "docking")
+            await self.move.forward(10).go(speed_factor=0.5)
+            spin_srv = self.nh.get_service_client("/ball_launcher/spin", SetBool)
+            drop_srv = self.nh.get_service_client("/ball_launcher/drop_ball", Empty)
+            dock_heartbeat = self.nh.get_service_client(
+                "/detect_dock_message",
+                MessageDetectDock,
+            )
+            try:
+                await axros.wrap_timeout(dock_heartbeat.wait_for_service(), duration=5)
+                await dock_heartbeat(
+                    MessageDetectDockRequest(
+                        color="R",
+                        ams_status=1,
+                        status_of_delivery="S",
+                    ),
+                )
+            except TimeoutError:
+                self.send_feedback("dock heartbeat service not available")
+            try:
+                await axros.wrap_timeout(spin_srv.wait_for_service(), duration=5)
+                await axros.wrap_timeout(drop_srv.wait_for_service(), duration=5)
+                await spin_srv(SetBoolRequest(True))
+                await asyncio.sleep(4)
+                for _ in range(4):
+                    await drop_srv(EmptyRequest())
+                    await asyncio.sleep(1.5)
+                await spin_srv(SetBoolRequest(False))
+            except TimeoutError:
+                pass
+            try:
+                await axros.wrap_timeout(dock_heartbeat.wait_for_service(), duration=5)
+                await dock_heartbeat(
+                    MessageDetectDockRequest(
+                        color="R",
+                        ams_status=1,
+                        status_of_delivery="S",
+                    ),
+                )
+            except TimeoutError:
+                self.send_feedback("dock heartbeat service not available")
+            await self.move.backward(10).go(speed_factor=0.5)
 
             # Step 6: Exit through the same gate
             self.send_feedback("returning through same gate...")