diff --git a/mil_common/drivers/electrical_protocol/CMakeLists.txt b/mil_common/drivers/electrical_protocol/CMakeLists.txt index 3424e2cb3..9a00e3384 100644 --- a/mil_common/drivers/electrical_protocol/CMakeLists.txt +++ b/mil_common/drivers/electrical_protocol/CMakeLists.txt @@ -3,7 +3,9 @@ project(electrical_protocol) find_package(catkin REQUIRED COMPONENTS rospy serial + axros ) + catkin_python_setup() catkin_package() diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/Async_readme.txt b/mil_common/drivers/electrical_protocol/electrical_protocol/Async_readme.txt new file mode 100644 index 000000000..1180c480f --- /dev/null +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/Async_readme.txt @@ -0,0 +1,26 @@ +Everything currently labelled async is unifinished +and not fully working. + +The last issue I had was the simulated device not +being able to connect to the test, all async files are +commented out. + +You can run the async test with the following command: +"rostest --text electrical_protocol async_simulated.test" + +If you have any questions or confusion about the mess +I've made here feel free to reach out: marcinplaza@ufl.edu + +ps: when starting you will most likely have to make +async_test_simulated_basic.py / async_driver.py executable +example: chmod +x async_test_simulated_basic.py (makes it executable) + +some helpful sites: +axros examples +https://mil.ufl.edu/docs/reference/axros/examples.html + +axros reference +https://mil.ufl.edu/docs/reference/axros/api.html#service + +asyncio reference +https://docs.python.org/3/library/asyncio.html diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/__init__.py b/mil_common/drivers/electrical_protocol/electrical_protocol/__init__.py index 0bec1b8c3..6a0e54793 100644 --- a/mil_common/drivers/electrical_protocol/electrical_protocol/__init__.py +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/__init__.py @@ -11,5 +11,6 @@ class can be used, which supports packets built through this library. """ +from .async_driver import AsyncSerialDevice from .driver import ROSSerialDevice from .packet import AckPacket, ChecksumException, NackPacket, Packet diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/async_driver.py b/mil_common/drivers/electrical_protocol/electrical_protocol/async_driver.py index 107ea362f..b9e49eeb0 100644 --- a/mil_common/drivers/electrical_protocol/electrical_protocol/async_driver.py +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/async_driver.py @@ -1,3 +1,4 @@ +''' from __future__ import annotations import abc @@ -6,7 +7,8 @@ from typing import Any, Generic, TypeVar, Union, cast, get_args, get_origin import axros -import rospy + +# import rospy import serial import serial_asyncio @@ -23,13 +25,17 @@ def __init__(self, device: AsyncSerialDevice): self.byte_count = 0 self.buffer = b"" self.expected_count = None + print("in init?") def connection_made(self, transport: asyncio.BaseTransport) -> None: self.transport = transport + + print("port opened", transport) self.transport.serial.reset_input_buffer() # type: ignore self.transport.serial.reset_output_buffer() # type: ignore def data_received(self, data: bytes) -> None: + print("data received?") # Either wait to hear the next start byte, or keep accumulating bytes # for the next packet if self.expected_count is None: @@ -114,7 +120,7 @@ def __init_subclass__(cls) -> None: cls._send_T = get_args(cls.__orig_bases__[0])[0] # type: ignore cls._recv_T = get_args(cls.__orig_bases__[0])[1] # type: ignore - async def connect(self, port: str, baudrate: int) -> None: + async def connect(self, port: str, baudrate: int, loop) -> None: """ Connects to the port with the given baudrate. If the device is already connected, the input and output buffers will be flushed. @@ -123,34 +129,40 @@ async def connect(self, port: str, baudrate: int) -> None: port (str): The serial port to connect to. baudrate (int): The baudrate to connect with. """ + print("3") self.port = port self.baudrate = baudrate - if self.transport: - raise RuntimeError("Device is already connected.") + + # if self.transport: + # raise RuntimeError("Device is already connected.") + print("the serial connection runs after this?") self.transport, self.protocol = await serial_asyncio.create_serial_connection( - asyncio.get_event_loop(), + loop, DeviceProtocol, port, baudrate=baudrate, ) - def close(self) -> None: + print("the serial connection creator finished?") + + async def close(self) -> None: """ Closes the serial device. """ - rospy.loginfo("Closing serial device...") + print("Closing serial device...") if not self.transport: raise RuntimeError("Device is not connected.") else: with contextlib.suppress(OSError): if self.transport.serial.in_waiting: # type: ignore - rospy.logwarn( + print( "Shutting down device, but packets were left in buffer...", ) self.transport.close() + self.transport = None - def write(self, data: bytes) -> None: + async def write(self, data: bytes) -> None: """ Writes a series of raw bytes to the device. This method should rarely be used; using :meth:`~.send_packet` is preferred because of the guarantees @@ -164,21 +176,22 @@ def write(self, data: bytes) -> None: self.transport.write(data) def send_packet(self, packet: SendPackets) -> None: + print("4") """ Sends a given packet to the device. Arguments: packet (:class:`~.Packet`): The packet to send. """ - self.write(bytes(packet)) + asyncio.create_task(self.write(bytes(packet))) - def _read_from_stream(self) -> bytes: + async def _read_from_stream(self) -> bytes: # Read until SOF is encourntered in case buffer contains the end of a previous packet - if not self.device: + if not self.transport: raise RuntimeError("Device is not connected.") sof = None for _ in range(10): - sof = self.device.read(1) + sof = self.transport.serial.read(1) if not len(sof): continue sof_int = int.from_bytes(sof, byteorder="big") @@ -188,13 +201,13 @@ def _read_from_stream(self) -> bytes: raise TimeoutError("No SOF received in one second.") sof_int = int.from_bytes(sof, byteorder="big") if sof_int != SYNC_CHAR_1: - rospy.logerr("Where da start char at?") + print("Where da start char at?") data = sof # Read sync char 2, msg ID, subclass ID - data += self.device.read(3) - length = self.device.read(2) # read payload length + data += self.transport.serial.read(3) + length = self.transport.serial.read(2) # read payload length data += length - data += self.device.read( + data += self.transport.serial.read( int.from_bytes(length, byteorder="little") + 2, ) # read data and checksum return data @@ -208,14 +221,14 @@ def _correct_type(self, provided: Any) -> bool: else: return isinstance(provided, self._recv_T) - def _read_packet(self) -> bool: + async def _read_packet(self) -> bool: if not self.device: raise RuntimeError("Device is not connected.") try: if not self.is_open() or self.device.in_waiting == 0: return False if self.device.in_waiting > 200: - rospy.logwarn_throttle( + print( 0.5, "Packets are coming in much quicker than expected, upping rate...", ) @@ -224,33 +237,33 @@ def _read_packet(self) -> bool: assert isinstance(packed_packet, bytes) packet = Packet.from_bytes(packed_packet) except serial.SerialException as e: - rospy.logerr(f"Error reading packet: {e}") + print(f"Error reading packet: {e}") return False except OSError: - rospy.logerr_throttle(1, "Cannot read from serial device.") + print(1, "Cannot read from serial device.") return False if not self._correct_type(packet): - rospy.logerr( + print( f"Received unexpected packet: {packet} (expected: {self._recv_T})", ) return False packet = cast(RecvPackets, packet) - self.on_packet_received(packet) + await self.on_packet_received(packet) return True - def _process_buffer(self, _: rospy.timer.TimerEvent) -> None: + async def _process_buffer(self, _: rospy.timer.TimerEvent) -> None: if not self.is_open(): return try: - self._read_packet() + await self._read_packet() except Exception as e: - rospy.logerr(f"Error reading recent packet: {e}") + print(f"Error reading recent packet: {e}") import traceback traceback.print_exc() @abc.abstractmethod - def on_packet_received(self, packet: RecvPackets) -> None: + async def on_packet_received(self, packet: RecvPackets) -> None: """ Abstract method to be implemented by subclasses for handling packets sent by the physical electrical board. @@ -259,3 +272,4 @@ def on_packet_received(self, packet: RecvPackets) -> None: packet (:class:`~.Packet`): The packet that is received. """ pass +''' diff --git a/mil_common/drivers/electrical_protocol/electrical_protocol/driver.py b/mil_common/drivers/electrical_protocol/electrical_protocol/driver.py index 5e7c669ae..01cc371ba 100644 --- a/mil_common/drivers/electrical_protocol/electrical_protocol/driver.py +++ b/mil_common/drivers/electrical_protocol/electrical_protocol/driver.py @@ -75,6 +75,7 @@ def connect(self, port: str, baudrate: int) -> None: port (str): The serial port to connect to. baudrate (int): The baudrate to connect with. """ + rospy.loginfo("3") self.port = port self.baudrate = baudrate self.device = serial.Serial(port, baudrate, timeout=0.1) @@ -113,6 +114,7 @@ def write(self, data: bytes) -> None: self.device.write(data) def send_packet(self, packet: SendPackets) -> None: + rospy.loginfo("4") """ Sends a given packet to the device. diff --git a/mil_common/drivers/electrical_protocol/package.xml b/mil_common/drivers/electrical_protocol/package.xml index 07cf05e65..86c327c5a 100644 --- a/mil_common/drivers/electrical_protocol/package.xml +++ b/mil_common/drivers/electrical_protocol/package.xml @@ -6,6 +6,7 @@ cameron.brown MIT catkin + axros rospy serial diff --git a/mil_common/drivers/electrical_protocol/test/async_calculator_device.py b/mil_common/drivers/electrical_protocol/test/async_calculator_device.py new file mode 100755 index 000000000..9515f6f64 --- /dev/null +++ b/mil_common/drivers/electrical_protocol/test/async_calculator_device.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +""" +from __future__ import annotations + +import asyncio +from dataclasses import dataclass + +# from threading import Event, Thread +from typing import Union + +import uvloop +from axros import NodeHandle +from electrical_protocol import AsyncSerialDevice, Packet +from std_msgs.msg import Float32, String +from std_srvs.srv import Empty, EmptyRequest, EmptyResponse + + +@dataclass +class RequestAddPacket(Packet, class_id=0x37, subclass_id=0x00, payload_format=" EmptyResponse: + if self.transport == None: + print("transport is not with us") + print("4") + self.num_one, self.num_two = self.i, 1000 - self.i + self.i += 1 + self.send_packet( + RequestAddPacket(number_one=self.num_one, number_two=self.num_two), + ) + return EmptyResponse() + + def on_packet_received(self, packet) -> None: + self.answer_topic.publish(Float32(data=packet.result)) + self.next_packet.set() + + +async def main(): + nh, remaining_args = NodeHandle.from_argv_with_remaining( + "async_calculator_device", anonymous=True, + ) + async with nh: + print("Starting AsyncCalculatorDevice...") + device = AsyncCalculatorDevice(nh) + + await device.setup() + + +if __name__ == "__main__": + uvloop.install() + asyncio.run(main()) +""" diff --git a/mil_common/drivers/electrical_protocol/test/async_simulated.test b/mil_common/drivers/electrical_protocol/test/async_simulated.test new file mode 100644 index 000000000..87745fa54 --- /dev/null +++ b/mil_common/drivers/electrical_protocol/test/async_simulated.test @@ -0,0 +1,7 @@ + + diff --git a/mil_common/drivers/electrical_protocol/test/async_test_simulated_basic.py b/mil_common/drivers/electrical_protocol/test/async_test_simulated_basic.py new file mode 100755 index 000000000..cbbe9d0d5 --- /dev/null +++ b/mil_common/drivers/electrical_protocol/test/async_test_simulated_basic.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +""" +import os +import pty +import unittest +from dataclasses import dataclass + +import rospy +import rostest +from electrical_protocol import Packet +from std_msgs.msg import Float32, String +from std_srvs.srv import Empty + + +@dataclass +class RequestAddPacket(Packet, class_id=0x37, subclass_id=0x00, payload_format="