worth a shot to see if it works

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,18 +1,13 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import ExternalShutdownException
-from std_srvs.srv import Empty
 
 import signal
-import time
 import atexit
 
-import serial
+from connector import Connector, SerialConnector, CANConnector
-import serial.tools.list_ports
-import os
 import sys
 import threading
-import glob
 
 from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
@@ -46,120 +41,13 @@ class Anchor(Node):
         - Publish raw strings to this topic to send directly to the MCU for debugging
     """
 
+    connector: Connector
+
     def __init__(self):
         # Initalize node with name
         super().__init__("anchor_node")  # previously 'serial_publisher'
 
-        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
+        self.connector = SerialConnector(self.get_logger())
-
-        # Serial port override
-        if port_override := os.getenv("PORT_OVERRIDE"):
-            self.serial_port = port_override
-
-        ##################################################
-        # Serial MCU Discovery
-
-        # If there was not a port override, look for a MCU over USB for Serial.
-        if self.serial_port is None:
-            comports = serial.tools.list_ports.comports()
-            real_ports = list(
-                filter(
-                    lambda p: p.vid is not None
-                    and p.pid is not None
-                    and p.device is not None,
-                    comports,
-                )
-            )
-            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
-
-            if len(recog_ports) == 1:  # Found singular recognized MCU
-                found_port = recog_ports[0]
-                self.get_logger().info(
-                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
-                )
-                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
-            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
-                # Kinda jank log message
-                self.get_logger().error(
-                    f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
-                )
-                # Don't set self.serial_port; later if-statement will exit()
-            elif (
-                len(recog_ports) == 0 and len(real_ports) > 0
-            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
-                self.get_logger().error(
-                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
-                )
-                # Don't set self.serial_port; later if-statement will exit()
-            else:  # Found jack shit
-                self.get_logger().error("No valid Serial ports specified or found.")
-                # Don't set self.serial_port; later if-statement will exit()
-
-        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
-        # Loop through all serial devices on the computer to check for the MCU
-        if self.serial_port is None:
-            self.get_logger().warning("Falling back to legacy MCU discovery...")
-            ports = Anchor.list_serial_ports()
-            for _ in range(4):
-                if self.serial_port is not None:
-                    break
-                for port in ports:
-                    try:
-                        # connect and send a ping command
-                        ser = serial.Serial(port, 115200, timeout=1)
-                        # (f"Checking port {port}...")
-                        ser.write(b"ping\n")
-                        response = ser.read_until(bytes("\n", "utf8"))
-
-                        # if pong is in response, then we are talking with the MCU
-                        if b"pong" in response:
-                            self.serial_port = port
-                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
-                            break
-                    except:
-                        pass
-
-        # If port is still None then we ain't finding no mcu
-        if self.serial_port is None:
-            self.get_logger().error("Unable to find MCU. Exiting...")
-            time.sleep(1)
-            sys.exit(1)
-        # Found a Serial port, try to open it; above code has not officially opened a Serial port
-        else:
-            self.get_logger().debug(
-                f"Attempting to open Serial port '{self.serial_port}'..."
-            )
-            try:
-                self.serial_interface = serial.Serial(
-                    self.serial_port, 115200, timeout=1
-                )
-
-                # Attempt to get name of connected MCU
-                self.serial_interface.write(
-                    b"can_relay_mode,on\n"
-                )  # can_relay_ready,[mcu]
-                mcu_name: str = ""
-                for _ in range(4):
-                    response = self.serial_interface.read_until(bytes("\n", "utf8"))
-                    try:
-                        if b"can_relay_ready" in response:
-                            args: list[str] = response.decode("utf8").strip().split(",")
-                            if len(args) == 2:
-                                mcu_name = args[1]
-                            break
-                    except UnicodeDecodeError:
-                        pass  # ignore malformed responses
-                self.get_logger().info(
-                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
-                )
-
-            except serial.SerialException as e:
-                self.get_logger().error(
-                    f"Could not open Serial port '{self.serial_port}' for reason:"
-                )
-                self.get_logger().error(e.strerror)
-                time.sleep(1)
-                sys.exit(1)
 
         # Close serial port on exit
         atexit.register(self.cleanup)
@@ -167,7 +55,7 @@ class Anchor(Node):
         ##################################################
         # ROS2 Topic Setup
 
-        # New pub/sub with VicCAN
+        # Pub/sub with VicCAN
         self.fromvic_debug_pub_ = self.create_publisher(
             String, "/anchor/from_vic/debug", 20
         )
@@ -181,163 +69,32 @@ class Anchor(Node):
             VicCAN, "/anchor/from_vic/bio", 20
         )
 
-        self.mock_mcu_sub_ = self.create_subscription(
-            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
-        )
         self.tovic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+            VicCAN, "/anchor/to_vic/relay", self.connector.write, 20
-        )
-        self.tovic_debug_sub_ = self.create_subscription(
-            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
         )
 
-        # Create publishers
+    def cleanup(self):
-        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
+        self.connector.cleanup()
-        self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
-        self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
-
-        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
-
-        # Create a subscriber
-        self.relay_sub = self.create_subscription(
-            String, "/anchor/relay", self.on_relay_tovic_string, 10
-        )
 
     def read_MCU(self):
         """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
-        try:
+        output = self.connector.read()
-            output = str(self.serial_interface.readline(), "utf8")
+
+        if not output:
+            return
 
-            if output:
         self.relay_fromvic(output)
-                # All output over debug temporarily
-                # self.get_logger().info(f"[MCU] {output}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,core"):
-                    self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,arm") or output.startswith(
-                    "can_relay_fromvic,digit"
-                ):  # digit for voltage readings
-                    self.arm_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,citadel") or output.startswith(
-                    "can_relay_fromvic,digit"
-                ):  # digit for SHT sensor
-                    self.bio_pub.publish(msg)
-                # msg = String()
-                # msg.data = output
-                # self.debug_pub.publish(msg)
-                return
-        except serial.SerialException as e:
-            print(f"SerialException: {e}")
-            print("Closing serial port.")
-            try:
-                if self.serial_interface.is_open:
-                    self.serial_interface.close()
-            except:
-                pass
-            exit(1)
-        except TypeError as e:
-            print(f"TypeError: {e}")
-            print("Closing serial port.")
-            try:
-                if self.serial_interface.is_open:
-                    self.serial_interface.close()
-            except:
-                pass
-            exit(1)
-        except Exception as e:
-            print(f"Exception: {e}")
-            # print("Closing serial port.")
-            # if self.ser.is_open:
-            #     self.ser.close()
-            # exit(1)
 
-    def on_mock_fromvic(self, msg: String):
+    def relay_fromvic(self, msg: VicCAN):
-        """For testing without an actual MCU, publish strings here as if they came from an MCU"""
-        # self.get_logger().info(f"Got command from mock MCU: {msg}")
-        self.relay_fromvic(msg.data)
-
-    def on_relay_tovic_viccan(self, msg: VicCAN):
-        """Relay a VicCAN message to the MCU"""
-        output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
-        for num in msg.data:
-            output += f",{round(num, 7)}"  # limit to 7 decimal places
-        output += "\n"
-        # self.get_logger().info(f"VicCAN relay to MCU: {output}")
-        self.serial_interface.write(bytes(output, "utf8"))
-
-    def relay_fromvic(self, msg: str):
         """Relay a string message from the MCU to the appropriate VicCAN topic"""
-        self.fromvic_debug_pub_.publish(String(data=msg))
+        msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
-        parts = msg.strip().split(",")
-        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
-            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
-            return
 
-        # String validation
+        if msg.mcu_name == "core":
-        malformed: bool = False
+            self.fromvic_core_pub_.publish(msg)
-        malformed_reason: str = ""
+        elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
-        if len(parts) < 3 or len(parts) > 7:
+            self.fromvic_arm_pub_.publish(msg)
-            malformed = True
+        elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
-            malformed_reason = (
+            self.fromvic_bio_pub_.publish(msg)
-                f"invalid argument count (expected [3,7], got {len(parts)})"
-            )
-        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
-            malformed = True
-            malformed_reason = f"invalid mcu_name '{parts[1]}'"
-        elif not (parts[2].isnumeric()) or int(parts[2]) < 0:
-            malformed = True
-            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
-        else:
-            for x in parts[3:]:
-                try:
-                    float(x)
-                except ValueError:
-                    malformed = True
-                    malformed_reason = f"data '{x}' is not a float"
-                    break
-
-        if malformed:
-            self.get_logger().warning(
-                f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
-            )
-            return
-
-        # Have valid VicCAN message
-
-        output = VicCAN()
-        output.mcu_name = parts[1]
-        output.command_id = int(parts[2])
-        if len(parts) > 3:
-            output.data = [float(x) for x in parts[3:]]
-        output.header = Header(
-            stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
-        )
-
-        # self.get_logger().info(f"Relaying from MCU: {output}")
-        if output.mcu_name == "core":
-            self.fromvic_core_pub_.publish(output)
-        elif output.mcu_name == "arm" or output.mcu_name == "digit":
-            self.fromvic_arm_pub_.publish(output)
-        elif output.mcu_name == "citadel" or output.mcu_name == "digit":
-            self.fromvic_bio_pub_.publish(output)
-
-    def on_relay_tovic_string(self, msg: String):
-        """Relay a raw string message to the MCU for debugging"""
-        message = msg.data
-        # self.get_logger().info(f"Sending command to MCU: {msg}")
-        self.serial_interface.write(bytes(message, "utf8"))
-
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-
-    def cleanup(self):
-        print("Cleaning up before terminating...")
-        if self.serial_interface.is_open:
-            self.serial_interface.close()
 
 
 def main(args=None):

src/anchor_pkg/anchor_pkg/connector.py

@@ -0,0 +1,194 @@
+from abc import ABC, abstractmethod
+import serial
+import serial.tools.list_ports
+from astra_msgs.msg import VicCAN
+from rclpy.impl.rcutils_logger import RcutilsLogger
+
+KNOWN_USBS = [
+    (0x2E8A, 0x00C0),  # Raspberry Pi Pico
+    (0x1A86, 0x55D4),  # Adafruit Feather ESP32 V2
+    (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
+    (0x1A86, 0x55D3),  # ESP32 S3 Development Board
+]
+BAUD_RATE = 115200
+
+
+class NoValidDeviceException(Exception):
+    pass
+
+
+class NoWorkingDeviceException(Exception):
+    pass
+
+
+class MultipleValidDevicesException(Exception):
+    pass
+
+
+class DeviceClosedException(Exception):
+    pass
+
+
+class Connector(ABC):
+    logger: RcutilsLogger
+
+    @abstractmethod
+    def read(self) -> VicCAN | None:
+        pass
+
+    @abstractmethod
+    def write(self, msg: VicCAN):
+        pass
+
+    def cleanup(self):
+        pass
+
+
+class SerialConnector(Connector):
+    port: str
+    mcu_name: str
+    serial_interface: serial.Serial
+    override: bool
+
+    def _get_name(self, port: str) -> str | None:
+        """
+        Get the name of the MCU (if it works)
+
+        returns: str name of the MCU, None if it doesn't work
+        """
+        # attempt to open the serial port
+        serial_interface: serial.Serial
+        try:
+            self.logger.info(f"asking {port} for its name")
+            serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
+
+            for i in range(4):
+                self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
+                response = serial_interface.read_until(bytes("\n", "utf8"))
+                try:
+                    if b"can_relay_ready" in response:
+                        args = response.decode("utf8").strip().split(",")
+                        if len(args) == 2:
+                            return args[1]
+                        break
+                except UnicodeDecodeError as e:
+                    self.logger.debug(
+                        f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
+                    )
+
+            if serial_interface.is_open:
+                serial_interface.close()
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException when asking for MCU name: {e}")
+
+        return None
+
+    def _find_ports(self) -> list[str]:
+        """
+        Finds all valid ports but does not test them
+
+        returns: all valid ports
+        """
+        comports = serial.tools.list_ports.comports()
+        valid_ports = list(
+            map(  # get just device strings
+                lambda p: p.device,
+                filter(  # make sure we have a known device
+                    lambda p: (p.vid, p.pid, p.device) in KNOWN_USBS
+                    and p.device is not None,
+                    comports,
+                ),
+            )
+        )
+        self.logger.debug(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
+        return valid_ports
+
+    def cleanup(self):
+        self.logger.info(f"closing serial port if open {self.port}")
+        try:
+            if self.serial_interface.is_open:
+                self.serial_interface.close()
+        except Exception as e:
+            self.logger.error(e)
+
+    def __init__(self, logger: RcutilsLogger, port_override: str | None = None):
+        self.logger = logger
+        self.override = bool(port_override)
+
+        ports: list[str]
+
+        if port_override:
+            ports = [port_override]
+        else:
+            ports = self._find_ports()
+
+        if len(ports) <= 0:
+            raise NoValidDeviceException("no valid serial device found")
+        if (l := len(ports)) > 1:
+            raise MultipleValidDevicesException(
+                f"too many ({l}) valid serial devices found"
+            )
+
+        # check each of our ports to make sure one of them is responding
+        port = ports[0]
+        mcu_name = "mock" if self.override else self._get_name(port)
+        if not mcu_name:
+            raise NoWorkingDeviceException(
+                f"found {port}, but it did not respond with its name"
+            )
+
+        self.port = port
+        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)
+
+    def read(self) -> VicCAN | None:
+        try:
+            raw = str(self.serial_interface.readline(), "utf8")
+
+            if not raw:
+                return None
+
+            parts = raw.split(",")
+
+            # don't need an extra check because len of .split output is always >= 1
+            if parts[0] != "can_relay_fromvic":
+                self.logger.debug(f"got non-CAN data from {self.mcu_name}: {raw}")
+                return None
+            elif len(parts) < 3:
+                self.logger.debug(
+                    f"got garbage (not enough parts) CAN data from {self.mcu_name}: {raw}"
+                )
+                return None
+            elif len(parts) > 7:
+                self.logger.debug(
+                    f"got garbage (too many parts) CAN data from {self.mcu_name}: {raw}"
+                )
+                return None
+
+            return VicCAN(
+                mcu_name=parts[1],
+                command_id=parts[2],
+                data=[float(x) for x in parts[3:]],
+            )
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException: {e}")
+            self._close_port()
+            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
+        except TypeError as e:
+            self.logger.error(f"TypeError: {e}")
+            self._close_port()
+            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
+        except Exception:
+            pass  # pretty much no other error matters
+
+    def write(self, msg: VicCAN):
+        # go from [ w, x, y, z ] -> "w,x,y,z" & round to 7 digits max
+        data = ",".join([str(round(x, 7)) for x in msg.data])
+        output = f"can_relay_tovic,{msg.mcu_name},{msg.command_id},{data}\n"
+        self.serial_interface.write(bytes(output, "utf8"))
+
+
+class CANConnector(Connector):
+    pass