Ivc taskplanning cleanup

docker/Dockerfile

@@ -27,6 +27,7 @@ RUN apt install -y --no-install-recommends openssh-client
 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-aiofiles
 RUN apt install -y --no-install-recommends python3-venv
 RUN apt install -y --no-install-recommends ros-jazzy-cv-bridge
 RUN apt install -y --no-install-recommends ros-jazzy-foxglove-bridge

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

@@ -1,17 +1,15 @@
 from dataclasses import dataclass
-from datetime import datetime
 from enum import Enum
 from pathlib import Path
-
+from datetime import datetime
 import pytz
 from custom_msgs.msg import ModemStatus, StringWithHeader
 from custom_msgs.srv import SendModemMessage
 from rclpy.logging import get_logger
 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):
@@ -53,29 +51,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)."""

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

@@ -22,7 +22,7 @@ async def main(self: Task) -> Task[None, None, None]:
         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),
-        ivc_tasks.crush_ivc_spam(msg_to_send=IVCMessageType.CRUSH_OCTAGON, parent=self),
+        ivc_tasks.ivc_send(msg=IVCMessageType.CRUSH_OCTAGON, parent=self),
 
         ######## Unused competition tasks ########
         ## Gate

onboard/src/task_planning/task_planning/tasks/ivc_tasks.py

@@ -1,46 +1,20 @@
 from datetime import datetime
 from pathlib import Path
 from typing import TYPE_CHECKING, cast
-
+import aiofiles
 import pytz
 from rclpy.duration import Duration
 from rclpy.logging import get_logger
 from task_planning.interface.ivc import IVC, IVCMessageType
 from task_planning.task import Task, task
 from task_planning.tasks import util_tasks
+from task_planning.utils.other_utils import ros_timestamp_to_pacific_time
 
 if TYPE_CHECKING:
     from custom_msgs.srv import SendModemMessage
 
-# TODO: rewrite/reorganize/clean up the comp ivc_tasks
-# TODO: add docstrings for all tasks
-
 logger = get_logger('ivc_tasks')
 
-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.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')
-
-
 @task
 async def wait_for_modem_status(self: Task[None, None, None], timeout: float = 10) -> bool:
     """
@@ -99,7 +73,6 @@ async def wait_for_modem_ready(self: Task[None, None, None], timeout: float = 15
     logger.info('Modem is ready.')
     return True
 
-
 @task
 async def test_ivc(self: Task[None, None, None], msg: IVCMessageType) -> None:
     """Test inter-vehicle communication."""
@@ -138,12 +111,12 @@ async def ivc_send(self: Task[None, None, None], msg: IVCMessageType) -> None:
             logger.info(f'Sent IVC message: {msg.name}')
 
             # Log to text file
-            with Path('ivc_log.txt').open('a') as f:  # noqa: ASYNC230 TODO switch to async IO
+            async with aiofiles.open('ivc_log.txt', 'a') as f:  # noqa: ASYNC230
                 timestamp = ros_timestamp_to_pacific_time(
                     IVC().modem_status.header.stamp.sec,
                     IVC().modem_status.header.stamp.nanosec,
                 )
-                f.write(f'Sent IVC message: {msg.name} at {timestamp}\n')
+                await f.write(f'Sent IVC message: {msg.name} at {timestamp}\n')
         else:
             logger.error(f'Modem failed to send message. Response: {service_response.message}')
 
@@ -176,82 +149,39 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM
     logger.warning(f'Received message {IVC().messages[-1].msg.name} is unknown.')
     return IVCMessageType.UNKNOWN
 
-
 @task
-async def crush_ivc_spam(self: Task[None, None, None], msg_to_send: IVCMessageType) -> Task[None, None, None]:
-    """Spam IVC message for Crush."""
-    while True:
-        await ivc_send(msg_to_send, parent=self) # Send crush is done with gate
-        await util_tasks.sleep(20, parent=self)
-
-
-@task
-async def ivc_send_then_receive(self: Task[None, None, None], msg_to_send: IVCMessageType,
-                                msg_to_receive: IVCMessageType, timeout: float = 60) -> Task[None, None, None]:
-    """Send then receive IVC message."""
-    await ivc_send(msg_to_send, parent=self) # Send crush is done with gate
-
-    count = 2
-    # Wait for Oogway to say starting/acknowledge command
+async def ivc_send_blocking(self: Task[None, None, None], msg_to_send: IVCMessageType,
+                                msg_to_receive: IVCMessageType, num_attempts: int, timeout: float = 60) -> Task[None, None, None]:
+    """Send and wait for an IVC message in a blocking manner."""
+    await ivc_send(msg_to_send, parent=self)
+    count = num_attempts
     while count != 0 and await ivc_receive(timeout=timeout, parent=self) != msg_to_receive:
         logger.info(f'Unexpected message received. Remaining attempts: {count}')
         count -= 1
 
 
 @task
-async def crush_ivc_send(self: Task[None, None, None], msg_to_send: IVCMessageType,
-                         msg_to_receive: IVCMessageType, timeout: float = 60) -> Task[None, None, None]:
-    """Send IVC message crush."""
-    await ivc_send(msg_to_send, parent=self) # Send crush is done with gate
-
-    count = 2
-    # Wait for Oogway to say starting/acknowledge command
-    while count != 0 and await ivc_receive(timeout=timeout, parent=self) != msg_to_receive:
-        logger.info(f'Unexpected message or no message received. Sending again. Remaining attempts: {count - 1}')
-        await ivc_send(msg_to_send, parent=self) # Send crush is done with gate
-        count -= 1
-
-    if count == 0:
-        logger.info('No acknowledgement, sending one last time')
-        await ivc_send(msg_to_send, parent=self) # Send crush is done with gate
-
-
-@task
-async def crush_ivc_receive(self: Task[None, None, None], msg_to_receive: IVCMessageType,
-                            msg_to_send: IVCMessageType, timeout: float = 60) -> Task[None, None, None]:
-    """Receive IVC message crush."""
-    count = 2
-    # Wait for Oogway to say starting/acknowledge command
-    while count != 0 and await ivc_receive(timeout=timeout, parent=self) != msg_to_receive:
-        logger.info(f'Unexpected message or no message received. Remaining attempts: {count - 1}')
-        count -= 1
-
-    await ivc_send(msg_to_send, parent=self) # Send crush is done with gate
-
-
-@task
-async def ivc_receive_then_send(self: Task[None, None, None], msg: IVCMessageType,
+async def ivc_receive_then_send(self: Task[None, None, None], check_msg: IVCMessageType, send_msg: IVCMessageType,
                                 timeout: float = 60) -> Task[None, None, None]:
     """Receive then after receipt send IVC message."""
     if timeout <= 0:
         return
 
-    # Wait until Crush is done with gate
-    while await ivc_receive(timeout=timeout, parent=self) != IVCMessageType.CRUSH_GATE:
+    while await ivc_receive(timeout=timeout, parent=self) != check_msg:
         logger.info('Unexpected message received.')
 
-    await ivc_send(msg, parent=self) # Oogway says ok and starting
+    await ivc_send(send_msg, parent=self)
 
 
 @task
 async def delineate_ivc_log(self: Task[None, None, None]) -> Task[None, None, None]:  # noqa: ARG001
     """Append a header to the IVC log file."""
-    with Path('ivc_log.txt').open('a') as f:  # noqa: ASYNC230 TODO eventually use async io
-        f.write('----- NEW RUN STARTED -----\n')
+    async with aiofiles.open('ivc_log.txt', 'a') as f:
+        await f.write('----- NEW RUN STARTED -----\n')
 
 
 @task
 async def add_to_ivc_log(self: Task[None, None, None], message: str) -> Task[None, None, None]:  # noqa: ARG001
     """Add a message to the IVC log file."""
-    with Path('ivc_log.txt').open('a') as f: # noqa: ASYNC230 TODO eventually use async io
-        f.write(f'{message}\n')
+    async with aiofiles.open('ivc_log.txt', 'a') as f:
+        await f.write(f'{message}\n')

onboard/src/task_planning/task_planning/utils/other_utils.py

@@ -2,7 +2,8 @@
 from collections.abc import Callable
 from enum import Enum
 from typing import Any
-
+from datetime import datetime
+import pytz
 
 class RobotName(Enum):
     """Enum for valid robot names."""
@@ -36,3 +37,24 @@ def getinstance(*args: tuple, **kwargs: dict) -> type:
             instances[cls] = cls(*args, **kwargs)
         return instances[cls]
     return getinstance
+
+def ros_timestamp_to_pacific_time(sec: int, nanosec: int) -> str:
+    """
+    Convert ROS timestamp (seconds and nanoseconds) to human-readable Pacific time.
+
+    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')