clean stuff up a bit to prep for CAN

.envrc

@@ -1 +1,2 @@
 use flake
+[[ -d install ]] && source install/setup.$(echo $0 | grep -oE '[^/]+$')

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/astra_description"]
 	path = src/astra_descriptions
 	url = ../astra_descriptions
+[submodule "src/astra_msgs"]
+	path = src/astra_msgs
+	url = git@github.com:SHC-ASTRA/astra_msgs.git

auto_start/auto_start_anchor.sh

@@ -14,8 +14,12 @@ echo "[INFO] Network interface is up!"
 # Your actual ROS node start command goes here
 echo "[INFO] Starting ROS node..."
 
-# Source ROS 2 Humble setup script (if we aren't using nix)
-command -v ros2 || source /opt/ros/humble/setup.bash
+# Source ROS 2 Humble setup script
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/auto_start_headless_full.sh

@@ -15,7 +15,11 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-command -v ros2 || source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/start_rosbag.sh

@@ -17,7 +17,11 @@ done
 echo "[INFO] Network interface is up!"
 
 
-command -v ros2 || source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 source $ANCHOR_WS/install/setup.bash
 [[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
 

flake.lock

@@ -1,29 +1,5 @@
 {
   "nodes": {
-    "astra-msgs": {
-      "inputs": {
-        "nix-ros-overlay": "nix-ros-overlay",
-        "nixpkgs": [
-          "astra-msgs",
-          "nix-ros-overlay",
-          "nixpkgs"
-        ]
-      },
-      "locked": {
-        "lastModified": 1771845042,
-        "narHash": "sha256-pGsb93ZlMhP3biy8S2eJc1sW+35vmaxlWHTbuwZDlQI=",
-        "owner": "SHC-ASTRA",
-        "repo": "astra_msgs",
-        "rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
-        "type": "github"
-      },
-      "original": {
-        "owner": "SHC-ASTRA",
-        "repo": "astra_msgs",
-        "rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
-        "type": "github"
-      }
-    },
     "flake-utils": {
       "inputs": {
         "systems": "systems"
@@ -42,55 +18,17 @@
         "type": "github"
       }
     },
-    "flake-utils_2": {
-      "inputs": {
-        "systems": "systems_2"
-      },
-      "locked": {
-        "lastModified": 1731533236,
-        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
-        "type": "github"
-      },
-      "original": {
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "type": "github"
-      }
-    },
     "nix-ros-overlay": {
       "inputs": {
         "flake-utils": "flake-utils",
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1770622967,
-        "narHash": "sha256-1LYjTugPSCa/5NkP6/dcZLH5TQYj3R8mAZ/9dgd7jDM=",
+        "lastModified": 1770108954,
+        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "d1b9f17eba909116356436d46b5192d299c6b49a",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "develop",
-        "repo": "nix-ros-overlay",
-        "type": "github"
-      }
-    },
-    "nix-ros-overlay_2": {
-      "inputs": {
-        "flake-utils": "flake-utils_2",
-        "nixpkgs": "nixpkgs_2"
-      },
-      "locked": {
-        "lastModified": 1771404745,
-        "narHash": "sha256-UVP3TsQJ4PezyQG3B1SsgsTxz32XBVzplJ/cgq7v/uk=",
-        "owner": "lopsided98",
-        "repo": "nix-ros-overlay",
-        "rev": "7cdf7b44ff186869baedbb3b82e5b409bb3e8dd9",
+        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
         "type": "github"
       },
       "original": {
@@ -116,26 +54,9 @@
         "type": "github"
       }
     },
-    "nixpkgs_2": {
-      "locked": {
-        "lastModified": 1759381078,
-        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "NixOS",
-        "repo": "nixpkgs",
-        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "nix-ros",
-        "repo": "nixpkgs",
-        "type": "github"
-      }
-    },
     "root": {
       "inputs": {
-        "astra-msgs": "astra-msgs",
-        "nix-ros-overlay": "nix-ros-overlay_2",
+        "nix-ros-overlay": "nix-ros-overlay",
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
@@ -156,21 +77,6 @@
         "repo": "default",
         "type": "github"
       }
-    },
-    "systems_2": {
-      "locked": {
-        "lastModified": 1681028828,
-        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
-        "owner": "nix-systems",
-        "repo": "default",
-        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
-        "type": "github"
-      },
-      "original": {
-        "owner": "nix-systems",
-        "repo": "default",
-        "type": "github"
-      }
     }
   },
   "root": "root",

flake.nix

@@ -3,13 +3,15 @@
 
   inputs = {
     nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs";
-    astra-msgs.url =
-      "github:SHC-ASTRA/astra_msgs/acabfd117d9711afc420612375b4e02f4ce4982d";
+    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
   };
 
   outputs =
-    { self, nix-ros-overlay, nixpkgs, astra-msgs }:
+    {
+      self,
+      nix-ros-overlay,
+      nixpkgs,
+    }:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
       let
@@ -17,31 +19,27 @@
           inherit system;
           overlays = [ nix-ros-overlay.overlays.default ];
         };
-
-        astra_msgs_pkgs = astra-msgs.packages.${system};
-        rosDistro = "humble";
       in
       {
         devShells.default = pkgs.mkShell {
           name = "ASTRA Anchor";
-
           packages = with pkgs; [
             colcon
-            astra_msgs_pkgs.astra-msgs
-
-            (pkgs.rosPackages.${rosDistro}.python3.withPackages (p: with p; [
+            (python313.withPackages (
+              p: with p; [
                 pyserial
+		python-can
                 pygame
                 scipy
                 crccheck
                 black
-            ]))
-
-            (with rosPackages.${rosDistro};
+              ]
+            ))
+            (
+              with rosPackages.humble;
               buildEnv {
                 paths = [
                   ros-core
-                  rqt-graph
                   ros2cli
                   ros2run
                   ros2bag
@@ -77,14 +75,11 @@
                   ros2-controllers
 		  chomp-motion-planner
                 ];
-              })
+              }
+            )
           ];
-
-          env = {
-            ASTRAMSGS = "${astra-msgs.outPath}";
-          };
-
           shellHook = ''
+            # Display stuff
             export DISPLAY=''${DISPLAY:-:0}
             export QT_X11_NO_MITSHM=1
           '';
@@ -94,7 +89,6 @@
 
   nixConfig = {
     extra-substituters = [ "https://ros.cachix.org" ];
-    extra-trusted-public-keys =
-      [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
+    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
   };
 }

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,29 +1,26 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import ExternalShutdownException
-from std_srvs.srv import Empty
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
 import signal
-import time
 import atexit
 
-import serial
-import serial.tools.list_ports
-import os
+from .connector import (
+    Connector,
+    MockConnector,
+    SerialConnector,
+    CANConnector,
+    NoValidDeviceException,
+    NoWorkingDeviceException,
+)
+from .convert import string_to_viccan
 import sys
 import threading
-import glob
 
 from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
 
-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
-]
-
 
 class Anchor(Node):
     """
@@ -42,132 +39,61 @@ class Anchor(Node):
         - For testing without an actual MCU, publish strings here as if they came from an MCU
     * /anchor/to_vic/relay
         - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
-    * /anchor/to_vic/relay_string
-        - 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'
+        super().__init__("anchor_node")
 
-        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
+        logger = 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,
+        self.declare_parameter(
+            "connector",
+            "auto",
+            ParameterDescriptor(
+                name="connector",
+                description="Declares which MCU connector should be used. Defaults to 'auto'.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.",
+            ),
         )
-            )
-            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}."
+        # Determine which connector to use. Options are Mock, Serial, and CAN
+        connector_select = (
+            self.get_parameter("connector").get_parameter_value().string_value
         )
-                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:
+        match connector_select:
+            case "serial":
+                logger.info("using serial connector")
+                self.connector = SerialConnector(self.get_logger())
+            case "can":
+                logger.info("using CAN connector")
+                self.connector = CANConnector(self.get_logger())
+            case "mock":
+                logger.info("using mock connector")
+                self.connector = MockConnector(self.get_logger())
+            case "auto":
+                logger.info("automatically determining connector")
                 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
+                    logger.info("trying CAN connector")
+                    self.connector = CANConnector(self.get_logger())
+                except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
+                    logger.info("CAN connector failed, trying serial connector")
+                    self.connector = SerialConnector(self.get_logger())
+            case _:
+                self.get_logger().fatal(
+                    f"invalid value for connector parameter: {connector_select}"
                 )
+                exit(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
+        # Close devices on exit
         atexit.register(self.cleanup)
 
-        ##################################################
         # ROS2 Topic Setup
 
-        # New pub/sub with VicCAN
+        # Publishers
         self.fromvic_debug_pub_ = self.create_publisher(
             String, "/anchor/from_vic/debug", 20
         )
@@ -181,163 +107,45 @@ class Anchor(Node):
             VicCAN, "/anchor/from_vic/bio", 20
         )
 
+        # Subscribers
+        self.tovic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/to_vic/relay", self.connector.write, 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
-        )
-        self.tovic_debug_sub_ = self.create_subscription(
-            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
-        )
 
-        # Create publishers
-        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
-        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 cleanup(self):
+        self.connector.cleanup()
 
     def read_MCU(self):
         """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
-        try:
-            output = str(self.serial_interface.readline(), "utf8")
+        output = self.connector.read()
 
-            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)
+        if not output:
             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)
+
+        self.relay_fromvic(output)
+
+    def relay_fromvic(self, msg: VicCAN):
+        """Relay a string message from the MCU to the appropriate VicCAN topic"""
+        msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
+
+        if msg.mcu_name == "core":
+            self.fromvic_core_pub_.publish(msg)
+        elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
+            self.fromvic_arm_pub_.publish(msg)
+        elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
+            self.fromvic_bio_pub_.publish(msg)
 
     def on_mock_fromvic(self, msg: String):
-        """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))
-        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
-        malformed: bool = False
-        malformed_reason: str = ""
-        if len(parts) < 3 or len(parts) > 7:
-            malformed = True
-            malformed_reason = (
-                f"invalid argument count (expected [3,7], got {len(parts)})"
+        viccan = string_to_viccan(
+            msg.data,
+            "mock",
+            self.get_logger(),
         )
-        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()
+        if viccan:
+            self.relay_fromvic(viccan)
 
 
 def main(args=None):

src/anchor_pkg/anchor_pkg/connector.py

@@ -0,0 +1,189 @@
+from abc import ABC, abstractmethod
+from astra_msgs.msg import VicCAN
+from rclpy.impl.rcutils_logger import RcutilsLogger
+from .convert import string_to_viccan
+
+# CAN
+import can
+import can.interfaces.socketcan
+
+# Serial
+import serial
+import serial.tools.list_ports
+
+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
+
+    def __init__(self, logger: RcutilsLogger):
+        self.logger = logger
+
+    @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 __init__(self, logger: RcutilsLogger):
+        super().__init__(logger)
+
+        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 = 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 _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) in KNOWN_USBS and p.device is not None,
+                    comports,
+                ),
+            )
+        )
+        self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
+        return valid_ports
+
+    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)
+
+            serial_interface.write(b"can_relay_mode,on\n")
+
+            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: list[str] = response.decode("utf8").strip().split(",")
+                        if len(args) == 2:
+                            self.logger.info(f"we are talking to {args[1]}")
+                            return args[1]
+                        break
+                except UnicodeDecodeError as e:
+                    self.logger.info(
+                        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 read(self) -> VicCAN | None:
+        try:
+            raw = str(self.serial_interface.readline(), "utf8")
+
+            if not raw:
+                return None
+
+            return string_to_viccan(raw, self.mcu_name, self.logger)
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException: {e}")
+            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
+        except TypeError as e:
+            self.logger.error(f"TypeError: {e}")
+            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"))
+
+    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)
+
+
+class CANConnector(Connector):
+    def __init__(self, logger: RcutilsLogger):
+        super().__init__(logger)
+
+
+class MockConnector(Connector):
+    def __init__(self, logger: RcutilsLogger):
+        super().__init__(logger)
+
+    def read(self) -> VicCAN | None:
+        return None
+
+    def write(self, msg: VicCAN):
+        print(msg)

src/anchor_pkg/anchor_pkg/convert.py

@@ -0,0 +1,42 @@
+from astra_msgs.msg import VicCAN
+from rclpy.impl.rcutils_logger import RcutilsLogger
+
+def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger):
+    """
+    Converts the serial string VicCAN format to a ROS2 VicCAN message.
+    Does not fill out the Header of the message.
+    On a failure, it will log at a debug level why it failed.
+
+    Parameters:
+      * msg: str
+        - The message in serial VicCAN format
+      * mcu_name: str
+        - The name of the MCU (e.g. core, citadel, arm)
+      * logger: RcutilsLogger
+        - A logger retrieved from node.get_logger()
+
+    Returns:
+      * VicCAN | None
+        - The VicCAN message on a success or None on a failure
+    """
+
+    parts: list[str] = msg.split(",")
+
+    # don't need an extra check because len of .split output is always >= 1
+    if parts[0] != "can_relay_fromvic":
+        logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
+        return None
+    elif len(parts) < 3:
+        logger.debug(
+            f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+    elif len(parts) > 7:
+        logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
+        return None
+
+    return VicCAN(
+        mcu_name=parts[1],
+        command_id=int(parts[2]),
+        data=[float(x) for x in parts[3:]],
+    )

src/anchor_pkg/package.xml

@@ -3,13 +3,14 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <description>Anchor -- ROS and CAN relay node</description>
+  <maintainer email="rjm0037@uah.edu">Riley</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
   <depend>common_interfaces</depend>
   <depend>python3-serial</depend>
+  <depend>python3-can</depend>
 
   <build_depend>black</build_depend>
 

src/bio_pkg/bio_pkg/bio_node.py

@@ -1,251 +1,241 @@
-import signal
+import rclpy
+from rclpy.node import Node
+import serial
 import sys
 import threading
+import os
+import glob
 import time
-
-import rclpy
-from astra_msgs.action import BioVacuum
-from astra_msgs.msg import CitadelControl, FaerieControl, VicCAN
-from astra_msgs.srv import BioTestTube, LibsSystem
-from rclpy.action import ActionServer
-from rclpy.node import Node
-from std_msgs.msg import Header, String
+import atexit
+import signal
+from std_msgs.msg import String
+from astra_msgs.msg import BioControl
+from astra_msgs.msg import BioFeedback
 
 serial_pub = None
 thread = None
 
-# used to verify the length of an incoming VicCAN feedback message
-# key is VicCAN command_id, value is expected length of data list
-viccan_citadel_msg_len_dict = {
-    # empty because not expecting any VicCAN from citadel atm
-}
-viccan_lance_msg_len_dict = {
-    # empty because not sure what VicCAN commands LANCE sends
-}
-
 
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
         super().__init__("bio_node")
 
-        # Anchor Topics
-        self.anchor_fromvic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20
-        )
-        self.anchor_tovic_pub_ = self.create_publisher(
-            VicCAN, "/anchor/to_vic/relay", 20
+        # Get launch mode parameter
+        self.declare_parameter("launch_mode", "bio")
+        self.launch_mode = self.get_parameter("launch_mode").value
+        self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")
+
+        # Create publishers
+        self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
+        self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
+
+        # Create subscribers
+        self.control_sub = self.create_subscription(
+            BioControl, "/bio/control", self.send_control, 10
         )
 
+        # Create a publisher for telemetry
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
+
+        # Topics used in anchor mode
+        if self.launch_mode == "anchor":
             self.anchor_sub = self.create_subscription(
                 String, "/anchor/bio/feedback", self.anchor_feedback, 10
             )
             self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
-        # Messages
-        self.citadel_sub = self.create_subscription(
-            CitadelControl,
-            "/bio/control/citadel",
-            self.citadel_callback,
-            10,
-        )
+        self.bio_feedback = BioFeedback()
 
-        self.faerie_sub = self.create_subscription(
-            FaerieControl,
-            "/bio/control/faerie",
-            self.faerie_callback,
-            10,
+        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
+        if self.launch_mode == "bio":
+            # Loop through all serial devices on the computer to check for the MCU
+            self.port = None
+            for i in range(2):
+                try:
+                    # connect and send a ping command
+                    set_port = (
+                        "/dev/ttyACM0"  # MCU is controlled through GPIO pins on the PI
                     )
+                    ser = serial.Serial(set_port, 115200, timeout=1)
+                    # print(f"Checking port {port}...")
+                    ser.write(b"ping\n")
+                    response = ser.read_until("\n")
 
-        # Services
-        self.test_tube_service = self.create_service(
-            BioTestTube, "/bio/control/test_tube", self.test_tube_callback
-        )
-        self.libs_service = self.create_service(
-            LibsSystem, "bio/control/libs_system", self.libs_callback
-        )
+                    # if pong is in response, then we are talking with the MCU
+                    if b"pong" in response:
+                        self.port = set_port
+                        self.get_logger().info(f"Found MCU at {set_port}!")
+                        break
+                except:
+                    pass
 
-        # Actions
-        self._action_server = ActionServer(
-            self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum
+            if self.port is None:
+                self.get_logger().info(
+                    "Unable to find MCU... please make sure it is connected."
                 )
+                time.sleep(1)
+                sys.exit(1)
+
+            self.ser = serial.Serial(self.port, 115200)
+            atexit.register(self.cleanup)
 
     def run(self):
         global thread
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
 
+        # if in arm mode, will need to read from the MCU
+
         try:
             while rclpy.ok():
+                if self.launch_mode == "bio":
+                    if self.ser.in_waiting:
+                        self.read_mcu()
+                    else:
                         time.sleep(0.1)
         except KeyboardInterrupt:
             pass
+        finally:
+            self.cleanup()
+
+    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    def read_mcu(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
+            if output:
+                self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+        except serial.SerialException:
+            self.get_logger().info("SerialException caught... closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except TypeError as e:
+            self.get_logger().info(f"TypeError: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except Exception as e:
+            print(f"Exception: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+
+    def send_ik(self, msg):
+        pass
+
+    def send_control(self, msg: BioControl):
+        # CITADEL Control Commands
+        ################
+
+        # Chem Pumps, only send if not zero
+        if msg.pump_id != 0:
+            command = (
+                "can_relay_tovic,citadel,27,"
+                + str(msg.pump_id)
+                + ","
+                + str(msg.pump_amount)
+                + "\n"
+            )
+            self.send_cmd(command)
+        # Fans, only send if not zero
+        if msg.fan_id != 0:
+            command = (
+                "can_relay_tovic,citadel,40,"
+                + str(msg.fan_id)
+                + ","
+                + str(msg.fan_duration)
+                + "\n"
+            )
+            self.send_cmd(command)
+        # Servos, only send if not zero
+        if msg.servo_id != 0:
+            command = (
+                "can_relay_tovic,citadel,25,"
+                + str(msg.servo_id)
+                + ","
+                + str(int(msg.servo_state))
+                + "\n"
+            )
+            self.send_cmd(command)
+
+        # LSS (SCYTHE)
+        command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
+        # self.send_cmd(command)
+
+        # Vibration Motor
+        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
+        # self.send_cmd(command)
+
+        # FAERIE Control Commands
+        ################
+
+        # To be reviewed before use#
+
+        # Laser
+        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        # self.send_cmd(command)
+
+        # Drill (SCABBARD)
+        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
+        # self.send_cmd(command)
+
+        # Bio linear actuator
+        command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
+        self.send_cmd(command)
 
     def send_cmd(self, msg: str):
-        # send to anchor node to relay
+        if (
+            self.launch_mode == "anchor"
+        ):  # if in anchor mode, send to anchor node to relay
             output = String()
             output.data = msg
             self.anchor_pub.publish(output)
-
-    def relay_fromvic(self, msg: VicCAN):
-        # self.get_logger().info(msg)
-        if msg.mcu_name == "citadel":
-            self.process_fromvic_citadel(msg)
-
-    def process_fromvic_citadel(self, msg: VicCAN):
-        # Check message len to prevent crashing on bad data
-        if msg.command_id in viccan_citadel_msg_len_dict:
-            expected_len = viccan_citadel_msg_len_dict[msg.command_id]
-            if len(msg.data) != expected_len:
-                self.get_logger().warning(
-                    f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
-                )
-            return
+        elif self.launch_mode == "bio":  # if in standalone mode, send to MCU directly
+            self.get_logger().info(f"[Bio to MCU] {msg}")
+            self.ser.write(bytes(msg, "utf8"))
 
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
-        self.get_logger().info(f"[Bio Anchor] {msg.data}")
-        # no data planned to be received from citadel, not sure about lance
+        # self.get_logger().info(f"[Bio Anchor] {msg.data}")
 
-    def citadel_callback(self, msg: CitadelControl):
-        distributor_arr = msg.distributor_id
-        # Distributor Control
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="citadel",
-            command_id=40,
-            data=[
-                clamp_short(distributor_arr[0]),
-                clamp_short(distributor_arr[1]),
-                clamp_short(distributor_arr[2]),
-                0,
-            ],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
-        # Move Scythe
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="digit",
-            command_id=42,
-            data=[float(msg.move_scythe)],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
+        if output.startswith(
+            "can_relay_fromvic,citadel,54"
+        ):  # bat, 12, 5, Voltage readings * 100
+            self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
+            self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
+            self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
+        elif output.startswith("can_relay_fromvic,digit,57"):
+            self.bio_feedback.drill_temp = float(parts[3])
+            self.bio_feedback.drill_humidity = float(parts[4])
 
-    def faerie_callback(self, msg: FaerieControl):
-        # Move Faerie
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="digit",
-            command_id=42,
-            data=[float(msg.move_faerie)],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
-        # Drill Speed
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="digit",
-            command_id=19,
-            data=[
-                float(msg.drill_speed * 100)
-            ],  # change on embedded so we can go (-1,1)
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
-        # Vanity Laser Control
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="digit",
-            command_id=28,
-            data=[float(msg.vanity_laser)],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
+    def publish_feedback(self):
+        self.feedback_pub.publish(self.bio_feedback)
 
-    def test_tube_callback(self, request, response):
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="citadel",
-            command_id=40,
-            data=[
-                float(int(request.tube_id)),
-                float(request.milliliters),
-            ],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
-        return response
+    @staticmethod
+    def list_serial_ports():
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+        # return glob.glob("/dev/tty[A-Za-z]*")
 
-    def libs_callback(self, request, response):
-        print("todo")
-
-    def execute_vacuum(self, goal_handle):
-        valve_id = int(goal_handle.request.valve_id)
-        duty = int(goal_handle.request.fan_duty_cycle_percent)
-        total = goal_handle.request.fan_time_ms
-
-        # open valve
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=40,
-                data=[float(valve_id)],
-            )
-        )
-
-        feedback = BioVacuum.Feedback()
-        start = time.time()
-
-        while True:
-            # set fan duty cycle
-            self.anchor_tovic_pub_.publish(
-                VicCAN(
-                    header=Header(stamp=self.get_clock().now().to_msg()),
-                    mcu_name="citadel",
-                    command_id=19,
-                    data=[float(duty)],
-                )
-            )
-
-            elapsed = int((time.time() - start) * 1000)
-            remaining = max(0, total - elapsed)
-
-            feedback.fan_time_remaining_ms = remaining
-            goal_handle.publish_feedback(feedback)
-
-            if remaining == 0:
-                break
-
-            time.sleep(0.1)
-
-        # stop fan
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=19,
-                data=[0.0],
-            )
-        )
-
-        # close valve
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=40,
-                data=[-1.0],
-            )
-        )
-
-        goal_handle.succeed()
-        return BioVacuum.Result()
-
-
-def clamp_short(x: int) -> int:
-    return max(-32768, min(32767, x))
+    def cleanup(self):
+        print("Cleaning up...")
+        try:
+            if self.ser.is_open:
+                self.ser.close()
+        except Exception as e:
+            exit(0)
 
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
 
 
 def main(args=None):