New yaw to cv object

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

@@ -32,7 +32,7 @@ def generate_launch_description() -> LaunchDescription:
 
     if robot_name in ['oogway', 'oogway_shell']:
         ld.add_action(IncludeLaunchDescription(
-            XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'torpedo_target_detector.xml')),
+            XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'buoy_detector.xml')),
         ))
     elif robot_name in ['crush']:
         ld.add_action(IncludeLaunchDescription(

onboard/src/offboard_comms/sketches/peripheral/Oogway.cpp

@@ -20,7 +20,7 @@ class Oogway : public Robot {
     Oogway(int voltageDelay, int pressureDelay, int tempHumidityDelay, int servoDelay, bool isShell = false)
         : Robot(voltageDelay, pressureDelay, tempHumidityDelay, servoDelay), isShell(isShell) {
 
-        voltage_sensor = new Voltage(VOLTAGE_PIN, 4.055, 0.987, -0.00524, "");
+        voltage_sensor = new Voltage(VOLTAGE_PIN, 4.642, 0.987, -0.00524, "");
         pressure_sensor = new Pressure(MS5837::MS5837_02BA, "");
         temp_humidity_sensor = new TempHumidity(TH_PIN, "");
         servo_marker = new RobotServo(SERVO_MARKER, 500, 1500, 2500, "M");

onboard/src/static_transforms/config/oogway.yaml

@@ -68,7 +68,7 @@ transforms:
     z: -0.2552065
     roll: 0.0
     pitch: 0.0
-    yaw: 0.0
+    yaw: -1.5707963267948966
     frame_id: corner_link
     child_frame_id: dvl
   marker_dropper:

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

@@ -149,8 +149,9 @@ def __init__(self, node: Node, bypass: bool = False) -> None:
             )
 
         # Subscribe to angle topics
-        self._angles: dict[CVObjectType, float] = dict.fromkeys(self.ANGLE_TOPICS, 0)
-        self._angle_queues: dict[CVObjectType, list[float]] = {object_type: [] for object_type in self.ANGLE_TOPICS}
+        self._angles: dict[CVObjectType, float] = dict.fromkeys(self.ANGLE_TOPICS | self.BOUNDING_BOX_TOPICS, 0)
+        self._angle_queues: dict[CVObjectType, list[float]] = {
+            object_type: [] for object_type in self.ANGLE_TOPICS | self.BOUNDING_BOX_TOPICS}
         for object_type, object_topic in self.ANGLE_TOPICS.items():
             node.create_subscription(
                 Float64,
@@ -183,16 +184,13 @@ def lane_marker_data(self) -> dict:
         """The dictionary containing lane marker-specific data."""
         return self._lane_marker_data
 
-    def _on_receive_bounding_box_data(self, cv_data: CVObject, object_type: CVObjectType, filter_len: int = 10) -> None:
+    def _on_receive_bounding_box_data(self, cv_data: CVObject, object_type: CVObjectType) -> None:
         """
         Store the received CV bounding box.
 
         Args:
             cv_data (CVObject): The received CV data.
             object_type (CVObjectType): The name/type of the object.
-            filter_len (int, optional): The maximum number of distance data points to retain
-                for the moving average filter. Defaults to 10.
-
         """
         # Special filtering for TORPEDO_BANNER
         if object_type == CVObjectType.TORPEDO_BANNER:
@@ -221,9 +219,12 @@ def _on_receive_bounding_box_data(self, cv_data: CVObject, object_type: CVObject
             self._lane_marker_data['touching_top'] = cv_data.coords.y - cv_data.height / 2 <= 0
             self._lane_marker_data['touching_bottom'] = cv_data.coords.y + cv_data.height / 2 >= self.FRAME_HEIGHT
 
-        if object_type == CVObjectType.PATH_MARKER:
-            self._angles[object_type] = self.update_moving_average(self._angle_queues[object_type],
-                                                                   cv_data.yaw, filter_len)
+        # self._angles[object_type] = self.update_moving_average(self._angle_queues[object_type],
+        #                                                        cv_data.yaw, filter_len)
+        if object_type not in self._angles:
+            self._angles[object_type] = cv_data.yaw
+        else:
+            self._angles[object_type] = self.update_exponential_moving_average(self._angles[object_type], cv_data.yaw)
 
     def _on_receive_distance_data(self, distance_data: Point, object_type: CVObjectType, filter_len: int = 10) -> None:
         """
@@ -302,6 +303,20 @@ def update_moving_average(self, queue: list[float], new_value: float, filter_len
 
         return sum(queue) / len(queue)
 
+    def update_exponential_moving_average(self, old_value: float, new_value: float, smoothing_k: float = 0.2) -> float:
+        """
+        Update the exponential moving average filter with a new value.
+
+        Args:
+            old_value (lifloatst): The most recent value output.
+            new_value (float): The new data point.
+            smoothing_k (float, optional): The smoothing factor. Higher prioritizes newer values
+
+        Returns:
+            float: The new moving average.
+        """
+        return old_value * (1 - smoothing_k) + new_value * smoothing_k if old_value != 0.0 else new_value
+
     def get_pose(self, name: CVObjectType) -> Pose:
         """
         Get the pose of a detected object.
@@ -383,4 +398,3 @@ def get_sonar_sweep_params(self, name: CVObjectType) -> tuple[float, float, floa
 
         data = self._bounding_boxes[name]
         return (data.sonar_start_angle, data.sonar_end_angle, data.sonar_scan_distance)
-

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

@@ -3,7 +3,7 @@
 from enum import Enum
 from pathlib import Path
 
-import pytz
+# import pytz
 from custom_msgs.msg import ModemStatus, StringWithHeader
 from custom_msgs.srv import SendModemMessage
 from rclpy.logging import get_logger

onboard/src/task_planning/task_planning/robot/crush.py

@@ -3,7 +3,7 @@
 
 import numpy as np
 from task_planning.interface.cv import CVObjectType
-from task_planning.interface.ivc import IVCMessageType
+# from task_planning.interface.ivc import IVCMessageType
 from task_planning.task import Task, task
 from task_planning.tasks import buoyancy_tasks, comp_tasks, ivc_tasks, move_tasks, prequal_tasks, sonar_tasks
 from task_planning.utils import geometry_utils

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

@@ -8,6 +8,7 @@
 from task_planning.tasks import (
     buoyancy_tasks,
     comp_tasks,
+    cv_tasks,
     ivc_tasks,
     move_tasks,
     prequal_tasks,
@@ -22,16 +23,15 @@ async def main(self: Task) -> Task[None, None, None]:
     """Run the tasks to be performed by Oogway."""
     # Constants
     DIRECTION_OF_TORPEDO_BANNER = 1
-    DEPTH = 0.7
+    DEPTH = 0.5
     # CVObjectType.TORPEDO_REEF_SHARK_TARGET or CVObjectType.TORPEDO_SAWFISH_TARGET
-    FIRST_TARGET = CVObjectType.TORPEDO_REEF_SHARK_TARGET
+    FIRST_TARGET = CVObjectType.TORPEDO_BANNER
     tasks = [
         ######## Main competition tasks ########
         # ivc_tasks.delineate_ivc_log(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),
+        cv_tasks.move_to_cv_obj(FIRST_TARGET, target_distance=1, parent=self, search_direction=-1),
         # TODO: task not found???
         # comp_tasks.send_torpedo_ivc(parent=self),
         # comp_tasks.octagon_task(direction=1, parent=self),
@@ -65,6 +65,7 @@ async def main(self: Task) -> Task[None, None, None]:
 
         ## Movement/CV tasks
         # move_tasks.move_with_directions([(1, 0, 0), (0, 1, 0), (-1, 0, 0), (0, -1, 0)], parent=self),
+        # move_tasks.move_with_directions([(4, 0, 0), [-4, 0, 0]], parent=self),
         # move_tasks.move_to_pose_local(
         #     geometry_utils.create_pose(0, 0, 0, 0, 0, 1.6),
         #     parent=self,

onboard/src/task_planning/task_planning/task_runner.py

@@ -9,7 +9,7 @@
 
 from task_planning.interface.controls import Controls
 from task_planning.interface.cv import CV
-from task_planning.interface.ivc import IVC
+# from task_planning.interface.ivc import IVC
 from task_planning.interface.servos import Servos
 from task_planning.interface.sonar import Sonar
 from task_planning.interface.state import State
@@ -45,7 +45,7 @@ def __init__(self) -> None:
         # Initialize interfaces
         Controls(self, bypass=self.bypass)
         CV(self, bypass=self.bypass)
-        IVC(node=self, bypass=self.bypass)
+        # IVC(node=self, bypass=self.bypass)
         Servos(self, bypass=self.bypass)
         Sonar(self, bypass=self.bypass)
         State(self, tf_buffer=tf_buffer, bypass=self.bypass)

onboard/src/task_planning/task_planning/tasks/comp_2025.py

@@ -19,7 +19,7 @@
 from task_planning.interface.state import State
 from task_planning.interface.ivc import IVCMessageType
 from task_planning.task import Task, Yield, task
-from task_planning.tasks import cv_tasks, move_tasks, util_tasks, ivc_tasks
+from task_planning.tasks import cv_tasks, move_tasks, util_tasks#, ivc_tasks
 from task_planning.utils import geometry_utils
 from task_planning.utils.other_utils import get_robot_name, RobotName