Prequal 2025

onboard/src/cv/cv/config.py

@@ -29,8 +29,8 @@ class Buoy:
 
 class LaneMarker:
     """Lane marker color constants."""
-    LANE_MARKER_BOT = np.array([100, 150, 50])
-    LANE_MARKER_TOP = np.array([140, 255, 255])
+    LANE_MARKER_BOT = np.array([175, 10, 25])
+    LANE_MARKER_TOP = np.array([255, 100, 70])
 
 class Torpedo:
     """Torpedo dimension and color constants."""

onboard/src/cv/cv/lane_marker_detector.py

@@ -41,9 +41,7 @@ def image_callback(self, data: CompressedImage) -> None:
                 self.angle_pub.publish(angle_msg)
 
             if distance is not None:
-                distance_msg = Point()
-                distance_msg.y = distance
-                self.distance_pub.publish(distance_msg)
+                self.distance_pub.publish(distance)
 
             if bounding_box is not None:
                 self.bounding_box_pub.publish(bounding_box)
@@ -116,17 +114,17 @@ def get_angle_and_distance_of_rectangle(self, frame: np.array) -> tuple[float, i
             # Compute distance between center of bounding box and center of image
             # Here, image x is robot's y, and image y is robot's z
             distance = Point()
-            distance.x = rect_center[0] - frame_center[0]
-            distance.y = frame_center[1] - rect_center[1]
+            distance.x = float(rect_center[0] - frame_center[0])
+            distance.y = float(frame_center[1] - rect_center[1])
 
             # Create CVObject message
             bounding_box = CVObject()
             bounding_box.header.stamp = self.get_clock().now().to_msg()
             bounding_box.coords = Point()
-            bounding_box.coords.x = rect_center[0]
-            bounding_box.coords.y = rect_center[1]
-            bounding_box.width = rect[1][0]
-            bounding_box.height = rect[1][1]
+            bounding_box.coords.x = float(rect_center[0])
+            bounding_box.coords.y = float(rect_center[1])
+            bounding_box.width = int(rect[1][0])
+            bounding_box.height = int(rect[1][1])
             bounding_box.yaw = math.radians(angle_in_degrees)
 
         return math.radians(angle_in_degrees), distance, bounding_box, frame

onboard/src/cv/launch/cv.launch.py

@@ -3,8 +3,9 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
 from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
 
 
@@ -21,6 +22,12 @@ def generate_launch_description() -> LaunchDescription:
 
     ld = LaunchDescription()
 
+    ld.add_action(DeclareLaunchArgument(
+        'prequal',
+        default_value='false',
+        description='Whether this is being launched for a prequal run',
+    ))
+
     if robot_name in ['oogway', 'oogway_shell']:
         ld.add_action(IncludeLaunchDescription(
             XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'depthai_spatial_detection.xml')),
@@ -29,6 +36,9 @@ def generate_launch_description() -> LaunchDescription:
 
     ld.add_action(IncludeLaunchDescription(
         PythonLaunchDescriptionSource(str(pkg_cv / 'launch' / 'usb_camera_detectors.launch.py')),
+        launch_arguments=[
+            ('enable_lane_marker', LaunchConfiguration('prequal')),
+        ],
     ))
 
     return ld

onboard/src/cv/launch/usb_camera_detectors.launch.py

@@ -3,7 +3,9 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
 from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
 
 
@@ -20,6 +22,12 @@ def generate_launch_description() -> LaunchDescription:
 
     ld = LaunchDescription()
 
+    ld.add_action(DeclareLaunchArgument(
+        'enable_lane_marker',
+        default_value='false',
+        description='Whether to launch the lane marker detector node',
+    ))
+
     ld.add_action(IncludeLaunchDescription(
         XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'usb_camera.xml')),
         launch_arguments={'camera': 'front'}.items(),
@@ -42,4 +50,9 @@ def generate_launch_description() -> LaunchDescription:
             XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_bottom.xml')),
         ))
 
+    ld.add_action(IncludeLaunchDescription(
+        XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'lane_marker_detector.xml')),
+        condition=IfCondition(LaunchConfiguration('enable_lane_marker')),
+    ))
+
     return ld

onboard/src/execute/launch/robot.launch.py

@@ -18,7 +18,6 @@ def generate_launch_description() -> LaunchDescription:
     """
     pkg_controls = Path(get_package_share_directory('controls'))
     pkg_cv = Path(get_package_share_directory('cv'))
-    pkg_dvl_pathfinder = Path(get_package_share_directory('dvl_pathfinder'))  # noqa: F841 TODO tf lol
     pkg_dvl_wayfinder = Path(get_package_share_directory('dvl_wayfinder'))
     pkg_offboard_comms = Path(get_package_share_directory('offboard_comms'))
     pkg_sensor_fusion = Path(get_package_share_directory('sensor_fusion'))
@@ -37,12 +36,21 @@ def generate_launch_description() -> LaunchDescription:
         description='Enable or disable recording functionality',
     ))
 
+    ld.add_action(DeclareLaunchArgument(
+        'prequal',
+        default_value='false',
+        description='Whether this is being launched for a prequal run',
+    ))
+
     ld.add_action(IncludeLaunchDescription(
         XMLLaunchDescriptionSource(str(pkg_controls / 'launch' / 'controls.xml')),
     ))
 
     ld.add_action(IncludeLaunchDescription(
         PythonLaunchDescriptionSource(str(pkg_cv / 'launch' / 'cv.launch.py')),
+        launch_arguments=[
+            ('prequal', LaunchConfiguration('prequal')),
+        ],
     ))
 
     if robot_name == 'crush':

onboard/src/task_planning/task_planning/interface/cv.py

@@ -160,7 +160,7 @@ def __init__(self, node: Node, bypass: bool = False) -> None:
             )
 
         # Lane marker-specific data
-        self._lane_marker_data = {}
+        self._lane_marker_data = {'touching_top': False, 'touching_bottom': False, 'height': 0.0}
         self._lane_marker_heights = []
 
     @property

onboard/src/task_planning/task_planning/robot/oogway.py

@@ -1,4 +1,4 @@
-# ruff: noqa: ERA001, F401, N806
+# ruff: noqa: ERA001, F401, N806, F841
 from math import radians
 
 from task_planning.interface.cv import CVObjectType
@@ -28,10 +28,10 @@ async def main(self: Task) -> Task[None, None, None]:
     tasks = [
         ######## Main competition tasks ########
         # ivc_tasks.delineate_ivc_log(parent=self),
-        comp_tasks.initial_submerge(DEPTH, parent=self),
+        # comp_tasks.initial_submerge(DEPTH, parent=self),
         # comp_tasks.gate_task_dead_reckoning(depth_level=-DEPTH, parent=self),
-        comp_tasks.torpedo_task(first_target=FIRST_TARGET, depth_level=DEPTH,
-                                direction=DIRECTION_OF_TORPEDO_BANNER, parent=self),
+        # comp_tasks.torpedo_task(first_target=FIRST_TARGET, depth_level=DEPTH,
+        #                         direction=DIRECTION_OF_TORPEDO_BANNER, parent=self),
         # TODO: task not found???
         # comp_tasks.send_torpedo_ivc(parent=self),
         # comp_tasks.octagon_task(direction=1, parent=self),
@@ -78,7 +78,7 @@ async def main(self: Task) -> Task[None, None, None]:
         # buoyancy_tasks.buoyancy_task(-0.5, parent=self),  # Submerge and stabilize buoyancy
 
         ######## Prequal tasks ########
-        # prequal_tasks.prequal_task(parent=self),
+        prequal_tasks.prequal_task(parent=self),
     ]
 
     for task_to_run in tasks:

onboard/src/task_planning/task_planning/tasks/prequal_tasks.py

@@ -19,7 +19,7 @@
 @task
 async def prequal_task(self: Task) -> Task[None, None, None]:  # noqa: PLR0915
     """Complete the prequalification task by tracking the lane marker."""
-    DEPTH_LEVEL = -0.5
+    DEPTH_LEVEL = -0.7
 
     await move_tasks.move_to_pose_local(
             geometry_utils.create_pose(0, 0, DEPTH_LEVEL, 0, 0, 0),
@@ -76,7 +76,7 @@ async def track_lane_marker(distance: float, forward: bool, step_size: float = 1
             step_size (float): The distance to move the robot in meters each step.
         """
         direction_term = 'forward' if forward else 'backward'
-        logger.info(f'track_lane_marker {distance} {direction_term}')
+        logger.info(f'[track_lane_marker] distance: {distance} {direction_term}')
 
         direction_sign = 1 if forward else -1
 
@@ -140,7 +140,7 @@ async def track_lane_marker(distance: float, forward: bool, step_size: float = 1
             # Y correction so the robot is centered on the lane marker
             dist_pixels = CV().distances[CVObjectType.LANE_MARKER].y
             height_pixels = CV().lane_marker_data['height']
-            dist_meters = dist_pixels * LANE_MARKER_HEIGHT_METERS / height_pixels
+            dist_meters = dist_pixels * LANE_MARKER_HEIGHT_METERS / height_pixels if height_pixels != 0.0 else 0.0
             if abs(dist_meters) > 0:
                 await move_tasks.move_to_pose_local(
                     geometry_utils.create_pose(0, dist_meters, 0, 0, 0, 0),