DepthAI Constants Refactoring (#117)

* bruh

* refactoring draft

* linting whole repository

* additional linting

* more linting

* praise to copilot bruh

Author: ChuChuCodes0414

onboard/src/controls/scripts/compute_wrench_matrix.py

@@ -39,7 +39,7 @@ def get_robot_name() -> str:
     user_robot_name = input(f"Enter the robot name (press enter for default '{default_robot_name}'): ")
 
     # Use the default value if the user input is empty
-    return user_robot_name.strip() if user_robot_name.strip() else default_robot_name
+    return user_robot_name.strip() or default_robot_name
 
 def get_transform(node: Node, tf_buffer: Buffer) -> TransformStamped:
     """

onboard/src/cv/cv/buoy_detector.py

@@ -97,7 +97,7 @@ def image_callback(self, data: CompressedImage) -> None:
 
         # takes contour w/ greatest y-distance
         if similar_size_contours:
-            similar_size_contours.sort(key=lambda x: cv2.contourArea(x))
+            similar_size_contours.sort(key=cv2.contourArea)
             best_cnt = similar_size_contours[0]
             for cnt in similar_size_contours:
                 x, y, w, h = cv2.boundingRect(cnt)

onboard/src/cv/cv/config.py

@@ -42,6 +42,9 @@ class Torpedo:
     HIGH_BOT = np.array([330, 50, 45])
     HIGH_TOP = np.array([360, 95, 95])
 
+    TORPEDO_BANNER_X_SCALE = 1.2
+    TORPEDO_BANNER_Y_SCALE = 0.5
+
 class BlueRect:
     """BlueRect color constants."""
     BLUE_BOT = np.array([100, 150, 50])

onboard/src/cv/cv/depthai_spatial_detection.py

@@ -14,6 +14,7 @@
 from std_msgs.msg import String
 
 from cv import depthai_camera_connect
+from cv.config import Torpedo
 from cv.image_tools import ImageTools
 from cv.utils import DetectionVisualizer, calculate_relative_pose
 
@@ -308,10 +309,17 @@ def detect(self) -> None:
 
             confidence = detection.confidence
 
-            # Calculate relative pose
+            # Calculate relative pose, and pull scalings from config dependent on model
+            scale_x, scale_y, scale_z = None, None, None
+            match label:
+                case 'torpedo_banner':
+                    scale_x = Torpedo.TORPEDO_BANNER_X_SCALE
+                    scale_y = Torpedo.TORPEDO_BANNER_Y_SCALE
+
             det_coords_robot_mm = calculate_relative_pose(bbox, tuple(model['input_size']),
-                                                          tuple(model['sizes'][label]),
-                                                          self.focal_length, self.sensor_size, 2)
+                                                        tuple(model['sizes'][label]),
+                                                        self.focal_length, self.sensor_size, 2,
+                                                        scale_x=scale_x, scale_y=scale_y, scale_z=scale_z)
 
             # Find yaw angle offset
             left_end_compute = self.compute_angle_from_x_offset(detection.xmin * self.camera_pixel_width)
@@ -341,7 +349,7 @@ def detect(self) -> None:
                                            det_coords_robot_mm[2])  # Maintain original z
 
             self.publish_prediction(
-                bbox, det_coords_robot_mm, yaw_offset, label, confidence,
+                bbox, det_coords_robot_mm, -yaw_offset, label, confidence,
                 (self.camera_pixel_height, self.camera_pixel_width), self.using_sonar)
 
     def publish_prediction(self, bbox: tuple, det_coords: tuple, yaw: float, label: str, confidence: float,
@@ -367,21 +375,16 @@ def publish_prediction(self, bbox: tuple, det_coords: tuple, yaw: float, label:
         object_msg.label = label
         object_msg.score = confidence
 
-        if label == 'torpedo_banner':
-            object_msg.coords.x = 1.2 * det_coords[0]
-            object_msg.coords.y = 0.5 * det_coords[1]
-            object_msg.coords.z = det_coords[2]
-        else:
-            object_msg.coords.x = det_coords[0]
-            object_msg.coords.y = det_coords[1]
-            object_msg.coords.z = det_coords[2]
+        object_msg.coords.x = det_coords[0]
+        object_msg.coords.y = det_coords[1]
+        object_msg.coords.z = det_coords[2]
 
         object_msg.xmin = bbox[0]
         object_msg.ymin = bbox[1]
         object_msg.xmax = bbox[2]
         object_msg.ymax = bbox[3]
 
-        object_msg.yaw = -yaw
+        object_msg.yaw = yaw
 
         object_msg.height = shape[0]
         object_msg.width = shape[1]

onboard/src/cv/cv/utils.py

@@ -78,7 +78,8 @@ def compute_angle_from_x_offset(x_offset: float, camera_pixel_width: float) -> f
 
 def calculate_relative_pose(bbox_bounds: list[int | float], input_size: tuple[float, float],
                             label_shape: tuple[float, float], focal_length: float,
-                            sensor_size: tuple[float, float], adjustment_factor: int) -> list[float]:
+                            sensor_size: tuple[float, float], adjustment_factor: int,
+                            scale_x: float = 1.0, scale_y: float = 1.0, scale_z: float = 1.0) -> list[float]:
     """
     Return relative pose, to be used as a part of the CVObject.
 
@@ -89,6 +90,9 @@ def calculate_relative_pose(bbox_bounds: list[int | float], input_size: tuple[fl
         focal_length (float): The distance between the lens and the image sensor when the lens is focused on a subject.
         sensor_size (tuple[float, float]): The physical size of the camera's image sensor.
         adjustment_factor (int): 1 if mono, 2 if depthai.
+        scale_x (float): Multiplicative scalar for the x position.
+        scale_y (float): Multiplicative scalar for the y position.
+        scale_z (float): Multiplicative scalar for the z position.
 
     Returns:
         list[float]: The relative pose of the object.
@@ -108,6 +112,10 @@ def calculate_relative_pose(bbox_bounds: list[int | float], input_size: tuple[fl
     x_meters = cam_dist_with_obj_height(bbox_height, label_shape[1], focal_length, input_size, sensor_size,
                                         adjustment_factor)
 
+    x_meters *= scale_x
+    y_meters *= scale_y
+    z_meters *= scale_z
+
     return [x_meters, y_meters, z_meters]
 
 

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

@@ -34,7 +34,7 @@ def generate_launch_description() -> LaunchDescription:
         ld.add_action(IncludeLaunchDescription(
             XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'torpedo_target_detector.xml')),
         ))
-    elif robot_name in ['crush']:
+    elif robot_name == 'crush':
         ld.add_action(IncludeLaunchDescription(
             XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_front.xml')),
         ))

onboard/src/task_planning/task_planning/interface/ivc.py

@@ -2,6 +2,7 @@
 from datetime import datetime
 from enum import Enum
 from pathlib import Path
+from typing import Self
 
 import pytz
 from custom_msgs.msg import ModemStatus, StringWithHeader
@@ -86,7 +87,7 @@ class IVC:
     MESSAGES_TOPIC = '/sensors/modem/messages'
     SEND_MESSAGE_SERVICE = '/sensors/modem/send_message'
 
-    def __new__(cls, node: Node | None = None, bypass: bool = False) -> 'IVC':  # noqa: ARG004
+    def __new__(cls, node: Node | None = None, bypass: bool = False) -> Self:  # noqa: ARG004
         """Create a new instance of the IVC class or return the existing instance."""
         if cls._instance is None:
             cls._instance = super().__new__(cls)

onboard/src/task_planning/task_planning/interface/state.py

@@ -45,7 +45,7 @@ def __init__(self, node: Node, bypass: bool = False, tf_buffer: Buffer | None =
             tf_buffer: The transform buffer for the robot. Defaults to None.
         """
         self.bypass = bypass
-        self._tf_buffer = tf_buffer if tf_buffer else Buffer()
+        self._tf_buffer = tf_buffer or Buffer()
 
         self._received_state = False
         self._received_depth = False

onboard/src/task_planning/task_planning/utils/coroutine_utils.py

@@ -1,21 +1,21 @@
 from collections.abc import Callable, Coroutine
-from typing import TypeVar
 
 from task_planning.task import Task, Yield
 
-SendType = TypeVar('SendType')
-TransformedSendType = TypeVar('TransformedSendType')
-YieldType = TypeVar('YieldType')
-TransformedYieldType = TypeVar('TransformedYieldType')
-ReturnType = TypeVar('ReturnType')
-TransformedReturnType = TypeVar('TransformedReturnType')
 
-
-async def transform(task: Task[YieldType, TransformedSendType, ReturnType],
-                    send_transformer: Callable[[SendType], TransformedSendType] | None = None,
-                    yield_transformer: Callable[[YieldType], TransformedYieldType] | None = None,
-                    return_transformer: Callable[[ReturnType], TransformedReturnType] | None = None) -> \
-                        Coroutine[TransformedYieldType, SendType, TransformedReturnType]:
+async def transform[
+    SendType,
+    TransformedSendType,
+    YieldType,
+    TransformedYieldType,
+    ReturnType,
+    TransformedReturnType,
+](
+    task: Task[YieldType, TransformedSendType, ReturnType],
+    send_transformer: Callable[[SendType], TransformedSendType] | None = None,
+    yield_transformer: Callable[[YieldType], TransformedYieldType] | None = None,
+    return_transformer: Callable[[ReturnType], TransformedReturnType] | None = None,
+) -> Coroutine[TransformedYieldType, SendType, TransformedReturnType]:
     """
     Transform the input and output of a task.