Merge pull request #36 from SHC-ASTRA/serial-buffer

Resolves Riley's comments, makes it so UTF-8 characters over 1 byte aren't destroyed

flake.lock

@@ -24,16 +24,16 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1770108954,
+        "lastModified": 1775216071,
-        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
+        "narHash": "sha256-PrPW70Fh1uLx3JxNV/NLeXjUhgfrZmi7ac8LJOlS0q4=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
+        "rev": "197a2b55c4ed24f8b885a5b20b65f426fb6d57ca",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "develop",
+        "ref": "master",
         "repo": "nix-ros-overlay",
         "type": "github"
       }

flake.nix

@@ -2,7 +2,7 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
 
     treefmt-nix = {
@@ -98,7 +98,8 @@
     );
 
   nixConfig = {
-    extra-substituters = [ "https://ros.cachix.org" ];
+    # Cache to pull ros packages from
-    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
+    extra-substituters = [ "https://ros.cachix.org" "https://attic.iid.ciirc.cvut.cz/ros" ];
+    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" "ros:JR95vUYsShSqfA1VTYoFt1Nz6uXasm5QrcOsGry9f6Q=" ];
   };
 }

scripts/reset-repo.bash

@@ -3,7 +3,7 @@
 repo_root="$(git rev-parse --show-toplevel)"
 
 if [[ -z $repo_root ]]; then
-  echo "script must be run from within the rover-ros2 repo" >&2
+  echo "script must be run from within a git repo" >&2
   exit 1
 fi
 

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -22,15 +22,15 @@ class Anchor(Node):
     """
     Publishers:
     * /anchor/from_vic/debug
-        - Every string received from the MCU is published here for debugging (String)
+        - Every string received from the MCU is published here for debugging
     * /anchor/from_vic/core
         - VicCAN messages for Core node
     * /anchor/from_vic/arm
-        - VicCAN messages for Arm node (also receives digit messages)
+        - VicCAN messages for Arm node
     * /anchor/from_vic/bio
-        - VicCAN messages for Bio node (also receives digit messages)
+        - VicCAN messages for Bio node
     * /anchor/to_vic/debug
-        - String copy of all messages sent to the connector
+        - A string copy of the messages published to ./relay are published here
 
     Subscribers:
     * /anchor/from_vic/mock_mcu
@@ -38,9 +38,10 @@ class Anchor(Node):
     * /anchor/to_vic/relay
         - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
     * /anchor/to_vic/relay_string
-        - Send raw strings to connectors. Does not work for CANConnector
+        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
     * /anchor/relay
-        - (Deprecated) Legacy topic. Takes String, converts to VicCAN, then sends to connector
+        - Legacy method for talking to connectors. Takes String as input, but does not send the raw strings to connectors.
+          Instead, it converts them to VicCAN messages first.
     """
 
     connector: Connector
@@ -50,7 +51,7 @@ class Anchor(Node):
 
         logger = self.get_logger()
 
-        # ROS2 parameter setup
+        # ROS2 Parameter Setup
 
         self.declare_parameter(
             "connector",
@@ -85,7 +86,7 @@ class Anchor(Node):
             ),
         )
 
-        # determine which connector to use. options are Mock, Serial, and CAN
+        # Determine which connector to use. Options are Mock, Serial, and CAN
         connector_select = (
             self.get_parameter("connector").get_parameter_value().string_value
         )
@@ -125,9 +126,9 @@ class Anchor(Node):
                 )
                 exit(1)
 
-        # ROS2 topic setup
+        # ROS2 Topic Setup
 
-        # publishers
+        # Publishers
         self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
             String,
             "/anchor/from_vic/debug",
@@ -148,14 +149,14 @@ class Anchor(Node):
             "/anchor/from_vic/bio",
             20,
         )
-        # debug publisher for outgoing messages
+        # Debug publisher
         self.tovic_debug_pub_ = self.create_publisher(
             String,
             "/anchor/to_vic/debug",
             20,
         )
 
-        # subscribers
+        # Subscribers
         self.tovic_sub_ = self.create_subscription(
             VicCAN,
             "/anchor/to_vic/relay",

src/anchor_pkg/anchor_pkg/connector.py

@@ -1,4 +1,6 @@
 from abc import ABC, abstractmethod
+from time import monotonic
+from typing import TYPE_CHECKING
 from astra_msgs.msg import VicCAN
 from std_msgs.msg import String
 from rclpy.clock import Clock
@@ -20,8 +22,11 @@ KNOWN_USBS = [
     (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
     (0x1A86, 0x55D3),  # ESP32 S3 Development Board
 ]
+
 BAUD_RATE = 115200
 
+SERIAL_READ_TIMEOUT = 0.5  # seconds
+
 MCU_IDS = [
     "broadcast",
     "core",
@@ -96,6 +101,10 @@ class SerialConnector(Connector):
         ports = self._find_ports()
         mcu_name: str | None = None
 
+        # Serial buffering
+        self._serial_buffer: bytes = b""
+        self._last_read_time = monotonic()
+
         if serial_override:
             logger.warn(
                 f"using serial_override: `{serial_override}`! this will bypass several checks."
@@ -123,7 +132,7 @@ class SerialConnector(Connector):
         self.mcu_name = mcu_name
 
         # if we fail at this point, it should crash because we've already tested the port
-        self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1)
+        self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=0)
 
     def _find_ports(self) -> list[str]:
         """
@@ -182,9 +191,66 @@ class SerialConnector(Connector):
 
         return None
 
+    def _try_readline(self) -> str | None:
+        """Attempts to read a full string from the MCU without blocking.
+
+        When pyserial is used with 'timeout=0', reads are performed non-blocking.
+        When used with interface.readline(), this breaks the assumption that a returned
+        string will be a completed attempt by the MCU to send a string; it may be
+        cut off between the start of the string and the newline, removing information
+        and rendering the string(s) useless; thus, to get around the downside of readline()
+        not waiting for a newline while still not blocking, this function manually
+        implements a serial input buffer and newline timeout.
+
+        If readline() returns a non-empty string, send it if it ends with a newline
+        (readline() will not read past any newline); otherwise, save the read string.
+        This buffered string should be pre-pended to the next readline() result.
+
+        If readline() does not receive a non-empty string after the last non-newline-
+        terminated readline() result within the manual timeout, send the contents of the
+        buffer as if it ended with a newline, and clear the buffer.
+
+        Returns:
+            str: A hopefully-complete string read from the MCU via the serial interface.
+        """
+        if TYPE_CHECKING:
+            assert type(self.serial_interface) == serial.Serial
+
+        # Warn on buffer timeout, as the only scenarios that would trigger this are
+        # a microcontroller output that isn't newline-terminated (bad), or the MCU is
+        # hanging (also bad).
+        if (
+            self._serial_buffer
+            and (monotonic() - self._last_read_time) > SERIAL_READ_TIMEOUT
+        ):
+            self.logger.warn(
+                f"Serial buffer timeout, last received '{self._serial_buffer}'."
+            )
+            result = self._serial_buffer
+            self._serial_buffer = b""
+            self._last_read_time = monotonic()
+            return str(result, "utf8").strip()
+
+        # No try-except here so caller catches it instead.
+        raw = self.serial_interface.readline()
+
+        # Empty or whitespace-only string
+        if not raw or not raw.strip():
+            return None
+
+        # Add to buffer or send finished buffer
+        if not (raw.endswith(b"\n") or raw.endswith(b"\r")):  # unfinished string
+            self._serial_buffer += raw
+            self._last_read_time = monotonic()
+            return None
+        else:
+            result = self._serial_buffer + raw
+            self._serial_buffer = b""
+            return str(result, "utf8").strip()
+
     def read(self) -> tuple[VicCAN | None, str | None]:
         try:
-            raw = str(self.serial_interface.readline(), "utf8")
+            raw = self._try_readline()
 
             if not raw:
                 return (None, None)
@@ -429,7 +495,7 @@ class CANConnector(Connector):
 class MockConnector(Connector):
     def __init__(self, logger: RcutilsLogger, clock: Clock):
         super().__init__(logger, clock)
-        # no hardware interface for MockConnector. publish to `/anchor/from_vic/mock_mcu` instead
+        # No hardware interface for MockConnector. Publish to `/anchor/from_vic/mock_mcu` instead.
 
     def read(self) -> tuple[VicCAN | None, str | None]:
         return (None, None)