Merge branch 'master' of https://github.com/uf-mil/mil into daniel-na…

…vigation-mission

.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"]

NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml

@@ -12,15 +12,19 @@ cluster_min_points: 5
 cluster_max_points: 1000
 
 # Associator
-associator_max_distance: 2.0
-associator_forget_unseen: false
+associator_max_distance: 20.0
+associator_forget_unseen: true
 
 # Ogrid
 ogrid_height_meters: 500
 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

NaviGator/mission_control/navigator_launch/launch/robotx_comms.launch

@@ -6,7 +6,7 @@
     <!-- The port to connect to for the tournament director server. -->
     <arg name="td_port" default="1337" />
     <!-- 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

NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py

@@ -0,0 +1,44 @@
+from __future__ import annotations
+
+import asyncio
+
+import rospy
+
+from .docking import Docking
+from .entrance_gate2 import EntranceGate2
+from .navigation import Navigation
+from .navigator import NaviGatorMission
+from .scan_the_code import ScanTheCodeMission
+from .wildlife import Wildlife
+
+
+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}...")
+        try:
+            await asyncio.wait_for(mission_cls().run(""), 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")
+
+        # Step 2: Scan the Code
+        await self.run_mission(ScanTheCodeMission, "scan the code")
+
+        # Step 3: 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: UAV Mission
+        pass

NaviGator/scripts/tmux_start.sh

@@ -0,0 +1,25 @@
+#!/bin/bash
+if tmux has-session -t auto; then
+	tmux a -t auto
+else
+	# First window (panes)
+	tmux new-session -d -s auto
+	tmux send-keys -t auto:0.0 'roslaunch navigator_launch master.launch --screen' Enter
+	tmux split-window -h -t auto
+	tmux split-window -v -t auto
+
+	# Second window (alarms, other panes)
+	sleep 1.5
+	tmux new-window -t auto
+	tmux split-window -h -t auto:1
+	tmux split-window -v -t auto:1
+	tmux split-window -v -t auto:1.0
+	tmux send-keys 'amonitor kill' Enter
+	tmux split-window -h
+	tmux send-keys 'amonitor hw-kill' Enter
+	tmux select-pane -t auto:1.0
+
+	# Return to the first window
+	tmux select-window -t auto:0
+	tmux a -t auto
+fi

NaviGator/utils/navigator_msgs/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   message_generation
   geometry_msgs
+  geographic_msgs
   genmsg
   actionlib_msgs
   actionlib
@@ -57,7 +58,7 @@ add_service_files(
   MessageEntranceExitGate.srv
   MessageFindFling.srv
   MessageFollowPath.srv
-  MessageReactReport.srv
+  MessageWildlifeEncounter.srv
   MessageUAVReplenishment.srv
   MessageUAVSearchReport.srv
   TwoClosestCones.srv
@@ -75,6 +76,7 @@ add_action_files(
 generate_messages(
   DEPENDENCIES
   std_msgs
+  geographic_msgs
   geometry_msgs
   actionlib_msgs
   sensor_msgs
@@ -83,6 +85,7 @@ generate_messages(
 catkin_package(
   CATKIN_DEPENDS
   std_msgs
+  geographic_msgs
   geometry_msgs
   sensor_msgs
 )

NaviGator/utils/navigator_msgs/package.xml

@@ -10,6 +10,7 @@
   <build_depend>actionlib</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>actionlib_msgs</build_depend>
+  <build_depend>geographic_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>message_runtime</build_depend>
@@ -19,6 +20,7 @@
   <run_depend>actionlib</run_depend>
   <run_depend>actionlib_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>geographic_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>actionlib_msgs</run_depend>

NaviGator/utils/navigator_msgs/srv/MessageDetectDock.srv

@@ -1,4 +1,5 @@
-string color  # color of docking bay R, G, B
+string color  # color of docking bay to park in ('R' for example)
 int8 ams_status  # 1=docking, 2=complete
+string status_of_delivery # S = 'scanning', D = 'delivering'
 ---
 string message

NaviGator/utils/navigator_msgs/srv/MessageFollowPath.srv

@@ -1,3 +1,4 @@
+string entry_color # color of the entry buoy ('R' for example)
 int8 finished # 1=in progress 2=completed
 ---
 string message

NaviGator/utils/navigator_msgs/srv/MessageUAVReplenishment.srv

@@ -1,4 +1,5 @@
 int8 uav_status # 1=stowed, 2=deployed, 3=faulted
 int8 item_status # 0=not picked up, 1=picked up, 2=delivered
+string item_color # color of the item ('R' for example)
 ---
 string message

NaviGator/utils/navigator_msgs/srv/MessageUAVSearchReport.srv

@@ -1,13 +1,7 @@
-string object1
-float64 object1_latitude
-string object1_n_s
-float64 object1_longitude
-string object1_e_w
+string object1 # 'R' for the R pad, 'N' for the N pad
+geographic_msgs/GeoPoint object1_location
 string object2
-float64 object2_latitude
-string object2_n_s
-float64 object2_longitude
-string object2_e_w
+geographic_msgs/GeoPoint object2_location
 int8 uav_status # 1=manual, 2=autonomous, 3=faulted
 ---
 string message

NaviGator/utils/navigator_msgs/srv/MessageWildlifeEncounter.srv

@@ -0,0 +1,5 @@
+string circling_wildlife # which animal to circle ('P' for example (python))
+bool clockwise # whether to circle clockwise or counter-clockwise
+int8 number_of_circles # how many circles to do
+---
+string message