RoboSub 2024

core/catkin_ws/src/custom_msgs/CMakeLists.txt

@@ -22,6 +22,7 @@ add_message_files(
   PIDInfo.msg
   PIDTerms.msg
   PWMAllocs.msg
+  RectInfo.msg
   RemoteLaunchInfo.msg
   RunningNode.msg
   ServoAngleArray.msg
@@ -30,6 +31,7 @@ add_message_files(
   SonarSweepRequest.msg
   SonarSweepResponse.msg
   SystemUsage.msg
+  TaskUpdate.msg
   ThrusterAllocs.msg
 )
 

core/catkin_ws/src/custom_msgs/msg/RectInfo.msg

@@ -0,0 +1,5 @@
+float64 center_x
+float64 center_y
+float64 width
+float64 height
+float64 angle

core/catkin_ws/src/custom_msgs/msg/TaskUpdate.msg

@@ -0,0 +1,19 @@
+std_msgs/Header header
+
+uint64 id
+
+uint64 parent_id
+
+string name
+
+uint8 INITIALIZED = 0
+uint8 PAUSED = 1
+uint8 RESUMED = 2
+uint8 THREW = 3
+uint8 RETURNED = 4
+uint8 CLOSED = 5
+uint8 DELETED = 6
+uint8 ERRORED = 7
+uint8 status
+
+string data

onboard/catkin_ws/src/controls/config/oogway.yaml

@@ -1,59 +1,59 @@
 pid:
   position:
     x:
-      Kp: 4
+      Kp: 1.8
       Ki: 0
-      Kd: 5
+      Kd: 1.2
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     y:
-      Kp: 3.5
+      Kp: 1.8
       Ki: 0
-      Kd: 5
+      Kd: 1.2
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     z:
-      Kp: 1
+      Kp: 1.9
       Ki: 0
-      Kd: 1.5
+      Kd: 1.2
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     roll:
-      Kp: 0.7
+      Kp: 0.2
       Ki: 0
-      Kd: 0.5
+      Kd: 0.4
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     pitch:
-      Kp: 0.7
+      Kp: 0.2
       Ki: 0
-      Kd: 0.5
+      Kd: 0.4
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     yaw:
-      Kp: 0.5
+      Kp: 0.2
       Ki: 0
-      Kd: 1
+      Kd: 0.4
       Ff: 0
       control_effort:
         min: -1
@@ -204,7 +204,7 @@ desired_power_limits:
 static_power_global:
   x: 0
   y: 0
-  z: -0.19
+  z: -0.1625
 power_scale_factor: 1
 thrusters:
   - name: bottom_front_left

onboard/catkin_ws/src/cv/configs/usb_cameras.yaml

@@ -0,0 +1,21 @@
+front:
+  device_path: "/dev/video_front"
+  topic: "front"
+  focal_length: 2.65
+  sensor_size:
+    width: 3.054
+    height: 1.718
+  img_size:
+    width: 640
+    height: 480
+
+bottom:
+  device_path: "/dev/video_bottom"
+  topic: "bottom"
+  focal_length: 2.65
+  sensor_size:
+    width: 3.054
+    height: 1.718
+  img_size:
+    width: 640
+    height: 480

onboard/catkin_ws/src/cv/launch/bin_detector.launch

@@ -0,0 +1,3 @@
+<launch>
+	<node name="bin_detector" pkg="cv" type="bin_detector.py" output="screen"/>
+</launch>

onboard/catkin_ws/src/cv/launch/blue_rectangle_detector.launch

@@ -0,0 +1,3 @@
+<launch>
+	<node name="blue_rectangle_detector" pkg="cv" type="blue_rectangle_detector.py" output="screen"/>
+</launch>

onboard/catkin_ws/src/cv/launch/buoy_detector_contour_matching.launch

@@ -0,0 +1,3 @@
+<launch>
+	<node name="buoy_detector_contour_matching" pkg="cv" type="buoy_detector_contour_matching.py" output="screen"/>
+</launch>

onboard/catkin_ws/src/cv/launch/depthai_mono_detection.launch

@@ -0,0 +1,25 @@
+<launch>
+    <arg name="feed_path" default="/camera/usb/front/compressed" doc='{"type":"str","allowEmpty":false,"help":"Topic path to use as the simulated camera feed."}'/>
+    <arg name="model" default="2024_gate_glyphs" doc='{"options":["yolov7_tiny_2023_main","yolov8n_2023_main","gate_buoy","gate"],"allowEmpty":false,"help":"The DepthAI model to use from depthai_models.yaml"}'/>
+    <arg name="rgb_raw" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to publish the raw rgb stream (no detections)."}'/>
+    <arg name="rgb_detections" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to publish the rgb stream with detections overlaid."}'/>
+    <arg name="depth" default="False" doc='{"type":"bool","allowEmpty":false,"help":"Whether to publish the depth map."}'/>
+    <arg name="sync_nn" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to sync the RGB frames with the detections so the latest published detections correspond to the latest published RGB frame."}'/>
+    <arg name="using_sonar" default="False" doc='{"type":"bool","allowEmpty":false,"help":"Whether to use sonar to estimate depth instead of stereo."}'/>
+    <arg name="show_class_name" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to show the class name."}'/>
+    <arg name="show_confidence" default="True" doc='{"type":"bool","allowEmp
+    ty":false,"help":"Whether to show the confidence value."}'/>
+    <arg name="correct_color" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to apply underwater color correction."}'/>
+    <node name="depthai_mono_detection" pkg="cv" type="depthai_mono_detection.py" output="screen">
+        <param name="feed_path" value="$(arg feed_path)" />
+        <param name="model" value="$(arg model)" />
+        <param name="rgb_raw" value="$(arg rgb_raw)" />
+        <param name="rgb_detections" value="$(arg rgb_detections)" />
+        <param name="depth" value="$(arg depth)" />
+        <param name="sync_nn" value="$(arg sync_nn)" />
+        <param name="using_sonar" value="$(arg using_sonar)" />
+        <param name="show_class_name" value="$(arg show_class_name)" />
+        <param name="show_confidence" value="$(arg show_confidence)" />
+        <param name="correct_color" value="$(arg correct_color)" />
+    </node>
+</launch>

onboard/catkin_ws/src/cv/launch/depthai_simulate_detection.launch

@@ -1,14 +1,16 @@
 <launch>
-	<arg name="feed_path" default="" doc='{"type":"str","allowEmpty":false,"help":"The path to the still image or topic to use as the simulated camera feed."}'/>
-	<arg name="depthai_model_name" default="" doc='{"options":["yolov7_tiny_2023_main","yolov8n_2023_main","gate","gate_buoy"],"allowEmpty":false,"help":"The DepthAI model to use from depthai_models.yaml"}' />
+	<arg name="feed_path" default="/camera/usb/front/compressed" doc='{"type":"str","allowEmpty":false,"help":"The path to the still image or topic to use as the simulated camera feed."}'/>
+	<arg name="depthai_model_name" default="2024_gate_glyphs" doc='{"options":["yolov7_tiny_2023_main","yolov8n_2023_main","gate","gate_buoy"],"allowEmpty":false,"help":"The DepthAI model to use from depthai_models.yaml"}' />
 	<arg name="publishing_topic" default="/cv/depthai_sim/detections" doc='{"type":"str","allowEmpty":false,"help":"The topic to publish detections to."}'/>
 	<arg name="show_class_name" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to show the class name."}'/>
 	<arg name="show_confidence" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to show the confidence value."}'/>
+	<arg name="correct_color" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to apply underwater color correction."}'/>
 	<node name="depthai_simulate_detection" pkg="cv" type="depthai_simulate_detection.py" output="screen">
 		<param name="feed_path" value="$(arg feed_path)" />
 		<param name="depthai_model_name" value="$(arg depthai_model_name)" />
 		<param name="publishing_topic" value="$(arg publishing_topic)" />
 		<param name="show_class_name" value="$(arg show_class_name)" />
 		<param name="show_confidence" value="$(arg show_confidence)" />
+		<param name="correct_color" value="$(arg correct_color)" />
 	</node>
 </launch>

onboard/catkin_ws/src/cv/launch/depthai_spatial_detection.launch

@@ -1,12 +1,13 @@
 <launch>
-    <arg name="model" default="yolov7_tiny_2023_main" doc='{"options":["yolov7_tiny_2023_main","yolov8n_2023_main","gate_buoy","gate"],"allowEmpty":false,"help":"The DepthAI model to use from depthai_models.yaml"}'/>
+    <arg name="model" default="2024_gate_glyphs" doc='{"options":["yolov7_tiny_2023_main","yolov8n_2023_main","gate_buoy","gate"],"allowEmpty":false,"help":"The DepthAI model to use from depthai_models.yaml"}'/>
     <arg name="rgb_raw" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to publish the raw rgb stream (no detections)."}'/>
     <arg name="rgb_detections" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to publish the rgb stream with detections overlaid."}'/>
     <arg name="depth" default="False" doc='{"type":"bool","allowEmpty":false,"help":"Whether to publish the depth map."}'/>
     <arg name="sync_nn" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to sync the RGB frames with the detections so the latest published detections correspond to the latest published RGB frame."}'/>
-    <arg name="using_sonar" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to use sonar to estimate depth instead of stereo."}'/>
+    <arg name="using_sonar" default="False" doc='{"type":"bool","allowEmpty":false,"help":"Whether to use sonar to estimate depth instead of stereo."}'/>
     <arg name="show_class_name" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to show the class name."}'/>
     <arg name="show_confidence" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to show the confidence value."}'/>
+    <arg name="correct_color" default="True" doc='{"type":"bool","allowEmpty":false,"help":"Whether to apply underwater color correction."}'/>
     <node name="depthai_spatial_detection" pkg="cv" type="depthai_spatial_detection.py" output="screen">
         <param name="model" value="$(arg model)" />
         <param name="rgb_raw" value="$(arg rgb_raw)" />
@@ -16,5 +17,6 @@
         <param name="using_sonar" value="$(arg using_sonar)" />
         <param name="show_class_name" value="$(arg show_class_name)" />
         <param name="show_confidence" value="$(arg show_confidence)" />
+        <param name="correct_color" value="$(arg correct_color)" />
     </node>
 </launch>

onboard/catkin_ws/src/cv/launch/path_marker_detector.launch

@@ -0,0 +1,3 @@
+<launch>
+	<node name="path_marker_detector" pkg="cv" type="path_marker_detector.py" output="screen"/>
+</launch>

onboard/catkin_ws/src/cv/launch/pink_bins_detector.launch

@@ -0,0 +1,6 @@
+<launch>
+	<arg name="camera" default="front" />
+	<node name="$(anon pink_bins_detector)" pkg="cv" type="pink_bins_detector.py" output="screen">
+		<param name="camera" value="$(arg camera)" />
+	</node>
+</launch>

onboard/catkin_ws/src/cv/launch/usb_camera.launch

@@ -1,11 +1,11 @@
 <?xml version="1.0"?>
 <launch>
 	<arg name="topic" default="usb_camera" doc='{"type":"str","help":"The feed from the USB camera will be published to /camera/[topic]/image_raw."}'/>
-	<arg name="channel" default="0"  doc='{"type":"int","help":"If the desired USB camera is publishing at the path /dev/video/[channel], this value is [channel]."}' />
+	<arg name="device_path" default="/dev/video_front"  doc='{"type":"int","help":"The path the desired USB camera is publishing at. Ex. /dev/video0."}' />
 	<arg name="framerate" default="-1" doc='{"type":"int","help":"Publishing framerate of USB camera. If this value is -1, the default framerate of the camera will be used."}' />
-	<node name="usb_camera" pkg="cv" type="usb_camera.py" output="screen">
+	<node name="$(anon usb_camera)" pkg="cv" type="usb_camera.py" output="screen">
 		<param name="topic" value="$(arg topic)" />
-		<param name="channel" value="$(arg channel)" />
+		<param name="device_path" value="$(arg device_path)" />
 		<param name="framerate" value="$(arg framerate)" />
 	</node>
 </launch>

onboard/catkin_ws/src/cv/launch/usb_camera_connect.launch

@@ -0,0 +1,3 @@
+<launch>
+	<node name="usb_camera_connect" pkg="cv" type="usb_camera_connect.py" output="screen"/>
+</launch>

onboard/catkin_ws/src/cv/models/depthai_models.yaml

@@ -44,4 +44,46 @@ yolov7_tiny_2023_main:
   anchor_masks: {"side52": [0,1,2], "side26": [3,4,5], "side13": [6,7,8]}
   iou_threshold: 0.5
   confidence_threshold: 0.5
-  colors: ["DC143C", "FFA500", "FFFF00", "00FF00", "000080", "8A2BE2"]
+  colors: ["DC143C", "FFA500", "FFFF00", "00FF00", "000080", "8A2BE2"]
+
+##################
+# COMPETITION MODEL
+2024_gate_glyphs:
+  classes: ["gate_blue_ccw", "gate_red_cw", "gate_tick", "gate_whole"]
+  sizes: {"gate_blue_ccw": [0.24, 0.3], "gate_red_cw": [0.24, 0.3], "gate_tick": [0.051, 0.61], "gate_whole": [3.048, 1.52]}
+  topic: cv/
+  weights: 2024_gate_glyphs_5shave.blob
+  input_size: [416, 416]
+  coordinate_size: 4
+  anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0]
+  anchor_masks: {"side52": [0,1,2], "side26": [3,4,5], "side13": [6,7,8]}
+  iou_threshold: 0.5
+  confidence_threshold: 0.9
+  colors: ["800000", "3C14DC", "00FFFF", "00A5FF"]
+###################
+
+2024_gate_glyphs_5shave:
+  classes: ["gate_blue_ccw", "gate_red_cw", "gate_tick", "gate_whole"]
+  sizes: {"gate_blue_ccw": [0.24, 0.3], "gate_red_cw": [0.24, 0.3], "gate_tick": [0.051, 0.61], "gate_whole": [3.048, 1.52]}
+  topic: cv/
+  weights: 2024_gate_glyphs_5shave.blob
+  input_size: [416, 416]
+  coordinate_size: 4
+  anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0]
+  anchor_masks: {"side52": [0,1,2], "side26": [3,4,5], "side13": [6,7,8]}
+  iou_threshold: 0.5
+  confidence_threshold: 0.9
+  colors: ["800000", "3C14DC", "00FFFF", "00A5FF"]
+
+2024_gate_glyphs_10shave:
+  classes: ["gate_blue_ccw", "gate_red_cw", "gate_tick", "gate_whole"]
+  sizes: {"gate_blue_ccw": [0.24, 0.3], "gate_red_cw": [0.24, 0.3], "gate_tick": [0.051, 0.61], "gate_whole": [3.048, 1.52]}
+  topic: cv/
+  weights: 2024_gate_glyphs_10shave.blob
+  input_size: [416, 416]
+  coordinate_size: 4
+  anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0]
+  anchor_masks: {"side52": [0,1,2], "side26": [3,4,5], "side13": [6,7,8]}
+  iou_threshold: 0.5
+  confidence_threshold: 0.9
+  colors: ["800000", "3C14DC", "00FFFF", "00A5FF"]