From 9981dfdea6c8adfa864d1b7f12d01af94bb3ff4e Mon Sep 17 00:00:00 2001 From: Srinath Iyer Date: Fri, 16 Jan 2026 01:44:59 +0000 Subject: [PATCH 1/3] made some progress --- docker/Dockerfile | 1 + .../task_planning/interface/ivc.py | 29 +---------- .../task_planning/tasks/ivc_tasks.py | 48 ++++--------------- .../task_planning/utils/other_utils.py | 24 +++++++++- 4 files changed, 34 insertions(+), 68 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 2cd1be09..0e9a9f60 100644 --- a/docker/Dockerfile +++ b/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 diff --git a/onboard/src/task_planning/task_planning/interface/ivc.py b/onboard/src/task_planning/task_planning/interface/ivc.py index 2d2752d0..f3a17247 100644 --- a/onboard/src/task_planning/task_planning/interface/ivc.py +++ b/onboard/src/task_planning/task_planning/interface/ivc.py @@ -1,8 +1,7 @@ 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 @@ -10,8 +9,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): @@ -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).""" diff --git a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py index 9f390ef8..db2ca33f 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py @@ -1,13 +1,14 @@ 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 @@ -17,30 +18,6 @@ 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 +76,7 @@ async def wait_for_modem_ready(self: Task[None, None, None], timeout: float = 15 logger.info('Modem is ready.') return True - +# TODO: read through this to see what's unique @task async def test_ivc(self: Task[None, None, None], msg: IVCMessageType) -> None: """Test inter-vehicle communication.""" @@ -153,7 +130,7 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM """Receive IVC message.""" await wait_for_modem_ready(parent=self) - messages_received = len(IVC().messages) + messages_received = len(IVC().messages) # TODO: This is slightly dumb. re-think if possible. sleep_task = util_tasks.sleep(timeout, parent=self) while not (len(IVC().messages) > messages_received): @@ -176,15 +153,6 @@ 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]: @@ -246,12 +214,12 @@ async def ivc_receive_then_send(self: Task[None, None, None], msg: IVCMessageTyp @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') \ No newline at end of file diff --git a/onboard/src/task_planning/task_planning/utils/other_utils.py b/onboard/src/task_planning/task_planning/utils/other_utils.py index 07e40df1..85d63361 100644 --- a/onboard/src/task_planning/task_planning/utils/other_utils.py +++ b/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') From 94271bd5d915bf2a5e5c40f015d9b3c5f9241d8f Mon Sep 17 00:00:00 2001 From: Srinath Iyer Date: Fri, 30 Jan 2026 01:28:09 +0000 Subject: [PATCH 2/3] finished --- .../task_planning/tasks/ivc_tasks.py | 60 ++++--------------- 1 file changed, 11 insertions(+), 49 deletions(-) diff --git a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py index db2ca33f..47358ced 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py @@ -13,9 +13,6 @@ 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') @task @@ -76,7 +73,6 @@ async def wait_for_modem_ready(self: Task[None, None, None], timeout: float = 15 logger.info('Modem is ready.') return True -# TODO: read through this to see what's unique @task async def test_ivc(self: Task[None, None, None], msg: IVCMessageType) -> None: """Test inter-vehicle communication.""" @@ -115,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}') @@ -130,7 +126,7 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM """Receive IVC message.""" await wait_for_modem_ready(parent=self) - messages_received = len(IVC().messages) # TODO: This is slightly dumb. re-think if possible. + messages_received = len(IVC().messages) sleep_task = util_tasks.sleep(timeout, parent=self) while not (len(IVC().messages) > messages_received): @@ -154,61 +150,27 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM return IVCMessageType.UNKNOWN @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 From c0cf852e449f9558f89eaffc0f5ed5102ee5db04 Mon Sep 17 00:00:00 2001 From: Srinath Iyer Date: Fri, 30 Jan 2026 01:33:42 +0000 Subject: [PATCH 3/3] updated oogway and crush.py --- onboard/src/task_planning/task_planning/robot/crush.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index efbe15fa..d5a172a7 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/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