goes to scan_the_code, timeouts

NaviGator/mission_control/navigator_launch/config/poi_sim.yaml

@@ -4,5 +4,6 @@ initial_pois:
   wildlife: [-1.7053, 44.9246, 0.0]
   docking: [-3.24, 58.64265, 0.0]
   entrance_gate: [22.87, 8.66, 0.0]
+  exit_gate: [22.87, 17.29, 0.0]
   navigation: [46.6878, 23.3085, 0.0]
   scan_the_code: [29.868, 44.1452, 0.0]

NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py

@@ -3,30 +3,31 @@
 import asyncio
 import datetime
 
-from .entrance_gate2 import EntranceGate2
+from navigator_msgs.msg import ScanTheCode
+
 from .go_to_poi import GoToPOI
 from .navigator import NaviGatorMission
 from .wildlife import Wildlife
 
+# from .scan_the_code_2024 import ScanTheCode2024
 
-class Autonomous2024(NaviGatorMission):
 
-    # timeout (in secs)
-    TIMEOUT = 180
+class Autonomous2024(NaviGatorMission):
 
     async def run_mission(
         self,
         mission_cls: type[NaviGatorMission],
         name: str,
         *args,
         str_arg: str = "",
+        timeout: int = 180,
         **kwargs,
     ):
         # self.send_feedback(f"beginning {name}...")
         try:
             await asyncio.wait_for(
                 mission_cls().run(str_arg, *args, **kwargs),
-                self.TIMEOUT,
+                timeout,
             )
         except asyncio.TimeoutError:
             self.send_feedback(f"!!! ran out of time on {name}!")
@@ -51,32 +52,54 @@ def send_feedback(self, msg: str):
 
     async def run(self, args: str):
         # Step 1: Entrance gate
-        self.start_time = datetime.datetime.now()
-        self._last_checkpoint = self.start_time
-        self.send_feedback("proceeding to entrance gate...")
-        await self.go_to_poi("entrance_gate")
-        self.send_feedback("starting entrance gate task...")
-        await self.run_mission(
-            EntranceGate2,
-            "entrance gate",
-            scan_code=True,
-            return_to_start=False,
-            circle=False,
-        )
+        td_feedback_pub = self.nh.advertise("/scan_the_code", ScanTheCode)
+        async with td_feedback_pub:
+            self.start_time = datetime.datetime.now()
+            self._last_checkpoint = self.start_time
+            # self.send_feedback("proceeding to entrance gate...")
+            # await self.go_to_poi("entrance_gate")
+            # self.send_feedback("starting entrance gate task...")
+            # await self.run_mission(
+            #     EntranceGate2,
+            #     "entrance gate",
+            #     scan_code=True,
+            #     return_to_start=False,
+            #     circle=False,
+            # )
 
-        # Step 2: Scan the Code
-        # await self.run_mission(ScanTheCodeMission, "scan the code")
+            # 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")
+            # # 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")
+            # Step 4: Navigation Mission
+            # await self.run_mission(Navigation, "navigation")
 
-        # Step 5: Dock Mission
-        # await self.run_mission(Docking, "docking")
+            # Step 5: Dock Mission
+            # await self.run_mission(Docking, "docking")
 
-        # Step 6: Exit through the same gate
+            # Step 6: Exit through the same gate
+            self.send_feedback("returning through same gate...")
+            await self.go_to_poi("exit_gate")
+            # await self.run_mission(
+            #     EntranceGate2,
+            #     "same gate",
+            #     scan_code=True,
+            #     return_to_start=False,
+            #     circle=False,
+            # )
+            # Go could be safer than attempting to find the best gate, just remember
+            # to send heartbeat
+            await self.move.forward(12).go()

NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py

@@ -1,8 +1,11 @@
 #!/usr/bin/env python3
+import asyncio
 import math
 
+import axros
 import numpy as np
 from mil_tools import quaternion_matrix
+from navigator_msgs.srv import MessageEntranceExitGate, MessageEntranceExitGateRequest
 from std_srvs.srv import SetBoolRequest
 
 from .navigator import NaviGatorMission
@@ -54,6 +57,18 @@ async def run(
         ).go()
         await self.move.set_position(traversal_points[1]).go()
 
+        td_feedback = self.nh.get_service_client(
+            "/entrance_exit_gate_message",
+            MessageEntranceExitGate,
+        )
+        try:
+            await axros.wrap_timeout(td_feedback.wait_for_service(), duration=5)
+            await td_feedback(
+                MessageEntranceExitGateRequest(entrance_gate=3, exit_gate=3),
+            )
+        except asyncio.TimeoutError:
+            self.send_feedback("!!! timed out on td feedback")
+
         if scan_code:
             pass
         elif return_to_start and circle: