Merge remote-tracking branch 'origin/master' into 1299-alexj-boat-dro…

…ne-comms

.github/workflows/ci.yaml

@@ -33,6 +33,32 @@ jobs:
       - id: check_skip
         uses: fkirc/skip-duplicate-actions@v5
 
+  ubuntu-22-test:
+    name: Ensure Ubuntu 22 is prohibited
+    runs-on: ubuntu-22.04
+    needs: avoid-duplicate-ci
+    if: needs.avoid-duplicate-ci.outputs.should_skip != 'true'
+    steps:
+      - name: Check out code from GitHub
+        uses: actions/checkout@v4
+      - name: Ensure install script fails
+        run: |
+          cd $GITHUB_WORKSPACE
+          ./scripts/install.sh && exit 1 || exit 0
+
+  ubuntu-24-test:
+    name: Ensure Ubuntu 24 is prohibited
+    runs-on: ubuntu-24.04
+    needs: avoid-duplicate-ci
+    if: needs.avoid-duplicate-ci.outputs.should_skip != 'true'
+    steps:
+      - name: Check out code from GitHub
+        uses: actions/checkout@v4
+      - name: Ensure install script fails
+        run: |
+          cd $GITHUB_WORKSPACE
+          ./scripts/install.sh && exit 1 || exit 0
+
   super-ci:
     name: Run tests and build docs
     needs: avoid-duplicate-ci

.github/workflows/docs.yaml

@@ -38,7 +38,7 @@ jobs:
           mkdir -p ${{ github.workspace }}/pr-docs/
 
       - name: Download artifact
-        uses: dawidd6/action-download-artifact@v2
+        uses: dawidd6/action-download-artifact@v6
         if: github.event.action != 'closed'
         with:
           github_token: ${{ secrets.INVESTIGATOR_BOT_TOKEN }}

.pre-commit-config.yaml

@@ -51,7 +51,7 @@ repos:
     hooks:
       - id: codespell
         args:
-          - --ignore-words-list=fpr,ser,wan,te,wan,ba,ned,ans,hist,nd,wronly,Voight,assertIn
+          - --ignore-words-list=fpr,ser,wan,te,wan,ba,ned,ans,hist,nd,wronly,Voight,assertIn,flor
           - --skip="./.*,*.csv,*.json"
           - --quiet-level=2
         exclude_types: [csv, json]

NaviGator/mission_control/navigator_launch/config/bounds_sarasota_a.yaml

@@ -0,0 +1,42 @@
+---
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  frame: ecef
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      frame: ecef
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      state: true
+      type: ''
+      x1: 744450.0
+      x2: 744336.0
+      x3: 744342.0
+      x4: 744456.0
+      y1: -5618775.0
+      y2: -5618790.0
+      y3: -5618835.0
+      y4: -5618820.0
+      z1: 2915236.0
+      z2: 2915236.0
+      z3: 2915150.0
+      z4: 2915150.0
+    state: []
+  x1: 744450.0
+  x2: 744336.0
+  x3: 744342.0
+  x4: 744456.0
+  y1: -5618775.0
+  y2: -5618790.0
+  y3: -5618835.0
+  y4: -5618820.0
+  z1: 2915236.0
+  z2: 2915236.0
+  z3: 2915150.0
+  z4: 2915150.0
+state: []

NaviGator/mission_control/navigator_launch/config/bounds_sarasota_b.yaml

@@ -0,0 +1,42 @@
+---
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  frame: ecef
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      frame: ecef
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      state: true
+      type: ''
+      x1: 744456.0
+      x2: 744342.0
+      x3: 744348.0
+      x4: 744463.0
+      y1: -5618820.0
+      y2: -5618835.0
+      y3: -5618885.0
+      y4: -5618870.0
+      z1: 2915149.0
+      z2: 2915149.0
+      z3: 2915052.0
+      z4: 2915052.0
+    state: []
+  x1: 744456.0
+  x2: 744342.0
+  x3: 744348.0
+  x4: 744463.0
+  y1: -5618820.0
+  y2: -5618835.0
+  y3: -5618885.0
+  y4: -5618870.0
+  z1: 2915149.0
+  z2: 2915149.0
+  z3: 2915052.0
+  z4: 2915052.0
+state: []

NaviGator/mission_control/navigator_launch/config/bounds_sarasota_c.yaml

@@ -0,0 +1,42 @@
+---
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  frame: ecef
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      frame: ecef
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      state: true
+      type: ''
+      x1: 744463.0
+      x2: 744349.0
+      x3: 744354.0
+      x4: 744468.0
+      y1: -5618871.0
+      y2: -5618886.0
+      y3: -5618926.0
+      y4: -5618911.0
+      z1: 2915050.0
+      z2: 2915050.0
+      z3: 2914972.0
+      z4: 2914972.0
+    state: []
+  x1: 744463.0
+  x2: 744349.0
+  x3: 744354.0
+  x4: 744468.0
+  y1: -5618871.0
+  y2: -5618886.0
+  y3: -5618926.0
+  y4: -5618911.0
+  z1: 2915050.0
+  z2: 2915050.0
+  z3: 2914972.0
+  z4: 2914972.0
+state: []

NaviGator/mission_control/navigator_launch/config/pcodar.yaml

@@ -21,5 +21,9 @@ ogrid_width_meters: 500
 ogrid_resolution_meters_per_cell: 0.3
 ogrid_inflation_meters: 0.8
 
+# Intensity filter
+intensity_filter_min_intensity: 10
+intensity_filter_max_intensity: 100
+
 # yamllint disable-line rule:line-length
-classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "UNKNOWN"]
+classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy", "UNKNOWN"]

NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml

@@ -21,7 +21,11 @@ ogrid_width_meters: 500
 ogrid_resolution_meters_per_cell: 0.3
 ogrid_inflation_meters: 0.8
 
+# Intensity filter
+intensity_filter_min_intensity: 0
+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", "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.yaml

@@ -1,5 +1,6 @@
 ---
 global_frame: enu
 initial_pois:
-  start_gate: [0, 0, 0]
-  entrance_gate: [0, 0, 0]
+  start_gate: [0, 0, 0, 0.0, 0.0, 0.0, 0.0]
+  entrance_gate: [0, 0, 0, 0.0, 0.0, 0.0, 0.0]
+  wildlife: [0, 0, 0, 0.0, 0.0, 0.0, 0.0]

NaviGator/mission_control/navigator_launch/config/poi_sim.yaml

@@ -1,10 +1,8 @@
 ---
 global_frame: enu
 initial_pois:
-  circle_totems: [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_launch/launch/robotx_comms.launch

@@ -2,11 +2,11 @@
 
     <!-- The IP address of the tournament director server. -->
     <!-- Currently, this is just set to a random IP. -->
-    <arg name="td_ip" default="192.168.37.90" />
+    <arg name="td_ip" default="robot.server" />
     <!-- The port to connect to for the tournament director server. -->
-    <arg name="td_port" default="1337" />
+    <arg name="td_port" default="12345" />
     <!-- The team ID given to us by the tournament director. -->
-    <arg name="team_id" default="GATR" />
+    <arg name="team_id" default="FLOR" />
     <arg name="simulation" default="False"/>
 
     <!-- start robotx_comms node -->

NaviGator/mission_control/navigator_launch/launch/shore.launch

@@ -1,9 +1,14 @@
 <launch>
-    <!-- Run Joystick -->
-    <include file="$(find navigator_launch)/launch/shore/xbox_controller.launch" />
+  <arg name="wireless" default="False" />
+  <arg name="device_input" default="/dev/input/js0" unless="$(arg wireless)"/>
 
-    <node name="network_heartbeat" pkg="mil_tools" type="network_broadcaster.py" output="screen" />
+  <!-- Run Joystick -->
+  <include file="$(find navigator_launch)/launch/shore/xbox_controller.launch">
+      <arg name="device_input" value="$(arg device_input)"/>
+  </include>
 
-    <!-- Host Monitoring -->
-    <node name="host_monitor" pkg="navigator_gui" type="host_monitor.py"/>
+  <node name="network_heartbeat" pkg="mil_tools" type="network_broadcaster.py" output="screen" />
+
+  <!-- Host Monitoring -->
+  <node name="host_monitor" pkg="navigator_gui" type="host_monitor.py"/>
 </launch>

NaviGator/mission_control/navigator_launch/launch/shore/xbox_controller.launch

@@ -1,6 +1,6 @@
 <launch>
   <arg name="wireless" default="False" />
-  <arg name="device_input" default="/dev/input/js0" unless="$(arg wireless)"/>
+  <arg name="device_input" default="/dev/input/js1" unless="$(arg wireless)"/>
   <arg name="device_input" default="/dev/input/by-id/usb-045e_0291-joystick" if="$(arg wireless)"/>
   <!-- Connect to MIL's Xbox Game controller to publish /joy messages -->
   <node pkg="joy" type="joy_node" name="joy_node">

NaviGator/mission_control/navigator_launch/launch/simulation.launch

@@ -7,6 +7,9 @@
     <arg name="world" default="example_course" />
     <arg name="extra_gazebo_args" default=""/>
     <arg name="simulation" default="True"/>
+    <!-- Adding an option to disable the classifier/YOLO model which often runs
+    very slow on laptops -->
+    <arg name="classifier" default="True"/>
     <param name="/use_sim_time" type="bool" value="true" />
     <param name="/is_simulation" type="bool" value="True" />
     <arg name="online-bagger" default="True" />
@@ -27,7 +30,7 @@
     </include>
 
     <!-- Simulation Relevant Code -->
-    <include file="$(find navigator_launch)/launch/perception/classifier.launch" >
+    <include if="$(arg classifier)" file="$(find navigator_launch)/launch/perception/classifier.launch" >
         <arg name="main_image_topic" value="/wamv/sensors/camera/front_left_cam/image_raw"/>
         <arg name="use_yolo_model1" value="True"/>
         <arg name="use_yolo_model2" value="False"/>

NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py

@@ -1,6 +1,7 @@
 import mil_missions_core
 
 from . import pose_editor
+from .autonomous_2024 import Autonomous2024
 from .back_and_forth import BackAndForth
 from .circle import Circle
 from .circle_tower import CircleTower
@@ -40,6 +41,9 @@
 from .teleop import Teleop
 from .wildlife import Wildlife
 
+# RobotX 2024 Missions
+from .wildlife_2024 import Wildlife2024
+
 # Currently breaks mission server, TODO: fix or delete
 # from .track_target import TrackTarget
 

NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py

@@ -0,0 +1,82 @@
+from __future__ import annotations
+
+import asyncio
+import datetime
+
+from .entrance_gate2 import EntranceGate2
+from .go_to_poi import GoToPOI
+from .navigator import NaviGatorMission
+from .wildlife import Wildlife
+
+
+class Autonomous2024(NaviGatorMission):
+
+    # timeout (in secs)
+    TIMEOUT = 180
+
+    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(str_arg, *args, **kwargs),
+                self.TIMEOUT,
+            )
+        except asyncio.TimeoutError:
+            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 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")
+
+        # 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")
+
+        # Step 5: Dock Mission
+        # await self.run_mission(Docking, "docking")
+
+        # Step 6: Exit through the same gate