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="