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/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 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..47358ced 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/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') \ 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')