Ivc taskplanning cleanup

docker/Dockerfile

@@ -1,9 +1,10 @@
+
 # Base Docker image for ROS2 Jazzy that supports linux/amd64 and linux/arm64
 FROM ros:jazzy
 
 # Dummy argument to force Docker to rebuild the image by invalidating the cache when the argument value changes
 # Cache invalidation forces Docker to rerun the `apt update` command, ensuring that the latest `apt` package index is used when building the image
-ARG CACHE_BUSTER
+# ARG CACHE_BUSTER
 
 # *********************************************************************************************************************************************************************************
 # All commands below are run as root
@@ -28,6 +29,7 @@ RUN apt install -y --no-install-recommends python3-colcon-common-extensions
 RUN apt install -y --no-install-recommends python3-opencv
 RUN apt install -y --no-install-recommends python3-pip
 RUN apt install -y --no-install-recommends python3-venv
+RUN apt install -y --no-install-recommends python3-aiofiles
 RUN apt install -y --no-install-recommends ros-jazzy-cv-bridge
 RUN apt install -y --no-install-recommends ros-jazzy-foxglove-bridge
 RUN apt install -y --no-install-recommends ros-jazzy-robot-localization
@@ -209,4 +211,4 @@ RUN --mount=type=secret,id=foxgloverc \
     fi
 
 # Run entrypoint script via tini to ensure SIGTERM is properly handled and zombie processes are reaped
-ENTRYPOINT ["/usr/bin/tini", "--", "/usr/local/bin/entrypoint.sh"]
+ENTRYPOINT ["/usr/bin/tini", "--", "/usr/local/bin/entrypoint.sh"]

docker/ros_bashrc.sh

@@ -31,6 +31,12 @@ fi
 
 export _colcon_cd_root=/opt/ros/jazzy
 
+alias launch-robot="ros2 launch execute robot.launch.py"
+
+alias tp-run="ros2 run task_planning task_runner"
+
+alias tp-run-auto="ros2 run task_planning task_runner --ros-args autonomous:=True"
+
 # Alias to start foxglove bridge
 alias fg-ws="ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=28765"
 
@@ -61,4 +67,4 @@ nvm use lts/jod > /dev/null  # Suppress stdout, only show stderr
 source /home/ubuntu/robosub-ros2/venv.sh activate
 
 # Get seconds since epoch
-alias secs="date +%s"
+alias secs="date +%s"

onboard/src/controls/config/crush.yaml

@@ -1,37 +1,37 @@
 pid:
   position:
     x:
-      Kp: 0.85
+      Kp: 0.80000000000000004
       Ki: 0
-      Kd: 0.40
+      Kd: 0.41999999999999998
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     y:
-      Kp: 0.80
+      Kp: 0.84999999999999998
       Ki: 0
-      Kd: 0.05
+      Kd: 0.34999999999999998
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     z:
-      Kp: 1.43
+      Kp: 1.4299999999999999
       Ki: 0
-      Kd: 0.5
+      Kd: 0.84999999999999998
       Ff: 0
       control_effort:
         min: -1
         max: 1
       derivative_type: 1
       error_ramp_rate: 1000
     roll:
-      Kp: 0.05
+      Kp: 0.050000000000000003
       Ki: 0
       Kd: 0
       Ff: 0
@@ -41,7 +41,7 @@ pid:
       derivative_type: 1
       error_ramp_rate: 1000
     pitch:
-      Kp: 0.05
+      Kp: 0.050000000000000003
       Ki: 0
       Kd: 0
       Ff: 0
@@ -51,9 +51,9 @@ pid:
       derivative_type: 1
       error_ramp_rate: 1000
     yaw:
-      Kp: 0.09
+      Kp: 0.16
       Ki: 0
-      Kd: 0.02
+      Kd: 0.12
       Ff: 0
       control_effort:
         min: -1
@@ -135,7 +135,7 @@ pid:
     y:
       Kp: 1
       Ki: 0.02
-      Kd: 0.05
+      Kd: 0.050000000000000003
       Ff: 0
       control_effort:
         min: -1
@@ -206,8 +206,6 @@ static_power_global:
   y: 0
   z: -0.206
 power_scale_factor: 1
-# all x positions assume the corner link is 10.41527MM +x than it actually is
-# see README.md for more details
 thrusters:
   - name: front_right
     type: T200
@@ -242,4 +240,4 @@ thrusters:
     pos: [0.001895, 0.09596, -0.003175]
     rpy: [0, 0, 135]
 wrench_matrix_file_path: data/crush_wrench.csv
-wrench_matrix_pinv_file_path: data/crush_wrench_pinv.csv
+wrench_matrix_pinv_file_path: data/crush_wrench_pinv.csv

onboard/src/offboard_comms/config/crush.yaml

@@ -54,23 +54,23 @@ thrusters:
     cw: false
     standard_esc: true
   - name: middle_right_first
-    cw: false
+    cw: true
     standard_esc: true
   - name: middle_right_second
-    cw: true
-    standard_esc: false
+    cw: false
+    standard_esc: true
   - name: back_right
     cw: false
-    standard_esc: false
+    standard_esc: true
   - name: back_left
     cw: true
-    standard_esc: false
+    standard_esc: true
   - name: middle_left_first
     cw: false
     standard_esc: true
   - name: middle_left_second
     cw: true
-    standard_esc: false
+    standard_esc: true
   - name: front_left
     cw: true
     standard_esc: true

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

@@ -1,7 +1,7 @@
 from dataclasses import dataclass
-from datetime import datetime
 from enum import Enum
 from pathlib import Path
+from datetime import datetime
 from typing import Self
 
 import pytz
@@ -11,8 +11,7 @@
 from rclpy.node import Node
 from rclpy.task import Future
 from rclpy.time import Time
-from task_planning.utils.other_utils import singleton
-
+from task_planning.utils.other_utils import singleton, ros_timestamp_to_pacific_time
 logger = get_logger('ivc_interface')
 
 class IVCMessageType(Enum):
@@ -32,10 +31,10 @@ class IVCMessageType(Enum):
     """
     UNKNOWN = ''
     OOGWAY_TEST = 'to'
+    CRUSH_TEST = 'tc'
     OOGWAY_GATE = 'og'
     OOGWAY_ACKNOWLEDGE = 'oa'
     OOGWAY_TORPEDOES = 'fu'
-    CRUSH_TEST = 'tc'
     CRUSH_GATE = 'cg'
     CRUSH_ACKNOWLEDGE = 'ca'
     CRUSH_OCTAGON = 'ky'
@@ -54,29 +53,6 @@ class IVCMessage:
     timestamp: Time
     msg: IVCMessageType
 
-def ros_timestamp_to_pacific_time(sec: int, nanosec: int) -> str:
-    """
-    Convert ROS timestamp (seconds and nanoseconds) to human-readable Pacific time.
-
-    # TODO: move to utils + merge with same function in ivc_tasks.py
-
-    Args:
-        sec (int): Seconds since epoch
-        nanosec (int): Nanoseconds
-
-    Returns:
-        str: Human-readable timestamp in Pacific timezone
-    """
-    # Convert to datetime object
-    pacific_tz = pytz.timezone('US/Pacific')
-    timestamp = datetime.fromtimestamp(sec + nanosec / 1e9, tz=pacific_tz)
-
-    # Convert to Pacific timezone
-    pacific_time = timestamp.astimezone(pacific_tz)
-
-    # Format as human-readable string
-    return pacific_time.strftime('%Y-%m-%d %H:%M:%S %Z')
-
 @singleton
 class IVC:
     """Interface for inter-vehicle communication (IVC)."""
@@ -187,7 +163,7 @@ def _on_receive_modem_message(self, msg: StringWithHeader) -> None:
             seconds,
             nanoseconds,
         )
-        msg = f'Received IVC message: {ivc_message.msg.name} at {timestamp}\n'
+        msg = f'Received IVC message: {ivc_message.msg.name} at {timestamp}. Raw data is {msg}.\n'
 
         # Log to text file
         logger.info(msg)

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

@@ -3,10 +3,11 @@
 
 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, move_tasks, prequal_tasks, sonar_tasks
+from task_planning.tasks import buoyancy_tasks, comp_tasks, move_tasks, prequal_tasks, sonar_tasks, ivc_tasks
 from task_planning.utils import geometry_utils
+import time
 
 
 @task
@@ -15,11 +16,18 @@ async def main(self: Task) -> Task[None, None, None]:
     tasks = [
         ######## Main competition tasks ########
         # ivc_tasks.delineate_ivc_log(parent=self),
-        comp_tasks.initial_submerge(0.5, parent=self),
+        comp_tasks.initial_submerge(0.35, parent=self),
+        # move_tasks.move_with_directions([(1, 0, 0)], parent=self),
+        move_tasks.move_to_pose_local(geometry_utils.create_pose(2, 0, 0, 0, 0, 0),
+                                      pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3),
+                                      parent=self),
+        move_tasks.move_to_pose_local(geometry_utils.create_pose(2, 0, 0, 0, 0, 0),
+                                      pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3),
+                                      parent=self),                                      
         # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=False, timeout=10, parent=self),
         # comp_tasks.coin_flip(parent=self),
         # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self),  # Move through gate via 2,2; right strafe via 1.5  # noqa: E501
-        comp_tasks.gate_style_task(depth_level=0.7, parent=self),  # Spin
+        # comp_tasks.gate_style_task(depth_level=0.7, parent=self),  # Spin
         # comp_tasks.slalom_task_dead_reckoning(depth_level=0.975, parent=self),  # Move through slalom via 2,2,2
         # Move to octagon front via 2,2; left strafe via 0.75
         # comp_tasks.slalom_to_octagon_dead_reckoning(depth_level=0.975, parent=self),
@@ -50,7 +58,7 @@ async def main(self: Task) -> Task[None, None, None]:
         #     msg_to_receive=IVCMessageType.OOGWAY_ACKNOWLEDGE, timeout=90, parent=self),
         # ivc_tasks.crush_ivc_receive(msg_to_receive=IVCMessageType.OOGWAY_GATE,
         #     msg_to_send=IVCMessageType.CRUSH_ACKNOWLEDGE, timeout=90, parent=self),
-
+        #ivc_tasks.test_ivc(IVCMessageType.CRUSH_TEST, parent=self),
         ## Movement/CV tasks
         # move_tasks.yaw_from_local_pose(np.pi / 2, parent=self),
         # move_tasks.move_with_directions([(1, 0, 0)], depth_level=-0.7, correct_depth=True, correct_yaw=True,
@@ -71,6 +79,5 @@ async def main(self: Task) -> Task[None, None, None]:
         ######## Prequal tasks ########
         # prequal_tasks.prequal_task(parent=self),
     ]
-
     for task_to_run in tasks:
         await task_to_run

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

@@ -45,7 +45,7 @@ async def main(self: Task) -> Task[None, None, None]:
 
         ######## Unused competition tasks ########
         ## Coin flip
-        comp_tasks.coin_flip(parent=self),
+        #comp_tasks.coin_flip(parent=self),
 
         ## Gate
         # comp_tasks.gate_task(offset=-0.1, direction=-1, parent=self),
@@ -83,7 +83,7 @@ async def main(self: Task) -> Task[None, None, None]:
 
         ######## Setup/Testing tasks ########
         # sonar_tasks.sonar_test(-45, 45, 5, parent=self),
-        # ivc_tasks.test_ivc(IVCMessageType.OOGWAY_TEST, parent=self),
+        ivc_tasks.test_ivc(IVCMessageType.OOGWAY_TEST, parent=self),
         # buoyancy_tasks.buoyancy_task(-0.5, parent=self),  # Submerge and stabilize buoyancy
 
         ######## Prequal tasks ########

onboard/src/task_planning/task_planning/task_runner.py

@@ -45,9 +45,9 @@ 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)
+        # Sonar(self, bypass=self.bypass)
         State(self, tf_buffer=tf_buffer, bypass=self.bypass)
 
         # Initialize the task update publisher