navigator_missions: Integrate entrance gate into autonomous mission, …

…allow POIs to be created with PointStamped poses, update possible PCODAR labels in sim

NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml

@@ -28,4 +28,4 @@ intensity_filter_max_intensity: 1
 # yamllint disable-line rule:line-length
 # classifications: ["buoy", "dock", "stc_platform", "red_totem", "green_totem", "blue_totem", "yellow_totem", "black_totem", "surmark46104", "surmark950400", "surmark950410", "UNKNOWN"]
 # yamllint disable-line rule:line-length
-classifications: ["mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange", "stc_platform", "red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy", "UNKNOWN"]
+classifications: ["mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange", "stc_platform", "red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy", "black_cylinder", "white_cylinder", "red_cylinder", "green_cylinder", "UNKNOWN"]

NaviGator/mission_control/navigator_launch/config/poi_sim.yaml

@@ -1,10 +1,8 @@
 ---
 global_frame: enu
 initial_pois:
-  wildlife: [26.09821319580078, 59.91523361206055, 0.0]
-  dock: [108.05, -1.07, 0.0]
-  entrance_gate: [51.28, -48.69, 0.0]
-  obstacle_course: [-32.803, -83.41, 0.0]
-  ring_challenge: [61.271873474121094, 15.894840240478516, 0.0]
-  start_gate: [12.75, 0.433, 0.0]
-  stc: [49.49, -82.65, 0.0]
+  wildlife: [-1.7053, 44.9246, 0.0]
+  docking: [-3.24, 58.64265, 0.0]
+  entrance_gate: [22.87, 8.66, 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

@@ -1,14 +1,11 @@
 from __future__ import annotations
 
 import asyncio
+import datetime
 
-import rospy
-
-from .docking import Docking
 from .entrance_gate2 import EntranceGate2
-from .navigation import Navigation
+from .go_to_poi import GoToPOI
 from .navigator import NaviGatorMission
-from .scan_the_code import ScanTheCodeMission
 from .wildlife import Wildlife
 
 
@@ -17,28 +14,69 @@ class Autonomous2024(NaviGatorMission):
     # timeout (in secs)
     TIMEOUT = 180
 
-    async def run_mission(self, mission_cls: type[NaviGatorMission], name: str):
-        rospy.loginfo(f"[autonomous] beginning {name}...")
+    async def run_mission(
+        self,
+        mission_cls: type[NaviGatorMission],
+        name: str,
+        *args,
+        str_arg: str = "",
+        **kwargs,
+    ):
+        # self.send_feedback(f"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}!")
+            self.send_feedback(f"!!! ran out of time on {name}!")
+
+    async def go_to_poi(self, name: str):
+        await self.run_mission(GoToPOI, f"go to {name} poi", str_arg=name)
+
+    def time_elapsed(self) -> str:
+        last_min, last_sec = divmod(
+            (datetime.datetime.now() - self._last_checkpoint).total_seconds(),
+            60,
+        )
+        min, sec = divmod(
+            (datetime.datetime.now() - self.start_time).total_seconds(),
+            60,
+        )
+        self._last_checkpoint = datetime.datetime.now()
+        return f"{int(min)}m {int(sec)}s (+{int(last_min)}m {int(last_sec)}s)"
+
+    def send_feedback(self, msg: str):
+        super().send_feedback(f"[autonomous, {self.time_elapsed()}] {msg}")
 
     async def run(self, args: str):
-        # Step 1: Entrance and exit gates
-        await self.run_mission(EntranceGate2, "entrance gate")
+        # 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,
+        )
 
         # Step 2: Scan the Code
-        await self.run_mission(ScanTheCodeMission, "scan the code")
+        # await self.run_mission(ScanTheCodeMission, "scan the code")
 
         # 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 4: Navigation Mission
-        await self.run_mission(Navigation, "navigation")
+        # await self.run_mission(Navigation, "navigation")
 
         # Step 5: Dock Mission
-        await self.run_mission(Docking, "docking")
+        # await self.run_mission(Docking, "docking")
 
-        # Step 6: UAV Mission
-        pass
+        # Step 6: Exit through the same gate

NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py

@@ -9,10 +9,14 @@
 
 
 class EntranceGate2(NaviGatorMission):
-    async def run(self, args):
-        # Parameters:
-        scan_code = False
-        return_to_start = True
+    async def run(
+        self,
+        args,
+        *,
+        scan_code: bool = False,
+        return_to_start: bool = True,
+        circle: bool = True,
+    ):
         circle_radius = 5
         circle_direction = "cw"
         yaw_offset = 1.57
@@ -52,7 +56,7 @@ async def run(self, args):
 
         if scan_code:
             pass
-        elif return_to_start:
+        elif return_to_start and circle:
             # Approach buoy we will rotate around
             buoy = await self.get_sorted_objects("black_cylinder", n=1)
             buoy = buoy[1][0]
@@ -78,8 +82,9 @@ async def run(self, args):
                 yaw_offset,
             )
 
+        if return_to_start:
             # Go back through start gate
-            self.send_feedback("Navigating through gate")
+            self.send_feedback("Navigating through original gate")
             await self.move.set_position(traversal_points[1]).look_at(
                 traversal_points[0],
             ).go()

mil_common/utils/mil_poi/mil_poi/server.py

@@ -5,11 +5,12 @@
 
 import rospy
 import tf2_ros
-from geometry_msgs.msg import Point, PointStamped, Pose, Quaternion
+from geometry_msgs.msg import Point, Pose, Quaternion
 from interactive_markers.interactive_marker_server import InteractiveMarkerServer
 from mil_ros_tools.msg_helpers import numpy_to_point
 from mil_tools import thread_lock
 from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+from tf2_geometry_msgs import PointStamped
 from visualization_msgs.msg import (
     InteractiveMarker,
     InteractiveMarkerControl,