chore: bump astra_msgs submodule

Refactor anchor & add direct CAN connector

.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

README.md

@@ -16,6 +16,9 @@ You will use these packages to launch all rover-side ROS2 nodes.
   - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
 - [Common Problems/Toubleshooting](#common-problemstroubleshooting)
 - [Packages](#packages)
+- [Graphs](#graphs)
+  - [Full System](#full-system)
+  - [Individual Nodes](#individual-nodes)
 - [Maintainers](#maintainers)
 
 ## Software Prerequisites
@@ -57,6 +60,33 @@ $ ros2 launch anchor_pkg rover.launch.py  # Must be run on a computer connected
 $ ros2 run headless_pkg headless_full  # Optionally run in a separate shell on the same or different computer.
 ```
 
+### Using the Mock Connector
+
+Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:="mock"
+```
+
+To see all data that would be sent over the CAN network (and thus to the microcontrollers), use this command:
+
+```bash
+$ ros2 topic echo /anchor/to_vic/debug
+```
+
+To send data to the mock connector (as if you were a ROS2 node), use the normal relay topic:
+
+```bash
+$ ros2 topic pub /anchor/to_vic/relay astra_msgs/msg/VicCAN '{mcu_name: "core", command_id: 50, data: [0.0, 2.0, 0.0, 1.0]}'
+
+```
+
+To send data to the mock connector (as if you were a microcontroller), publish to the dedicated topic:
+
+```bash
+$ ros2 topic pub /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN '{mcu_name: "arm", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}'
+```
+
 ### Testing Serial
 
 You can fake the presence of a Serial device (i.e., MCU) by using the following command:
@@ -65,10 +95,31 @@ You can fake the presence of a Serial device (i.e., MCU) by using the following
 $ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
 ```
 
-When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
+When you go to run anchor, use the `serial_override` ROS2 parameter to point it to the fake serial port, like so:
 
 ```bash
-$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
+$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
+```
+
+### Testing CAN
+
+You can create a virtual CAN network by using the following commands to create and then enable it:
+
+```bash
+sudo ip link add dev vcan0 type vcan
+sudo ip link set vcan0 up
+```
+
+When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0
+```
+
+Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter:
+
+```bash
+$ sudo ip link delete vcan0
 ```
 
 ### Connecting the GuliKit Controller
@@ -140,6 +191,40 @@ A: To find a microcontroller to talk to, Anchor sends a ping to every Serial por
 - [ros2\_interfaces\_pkg](./src/ros2_interfaces_pkg) - Contains custom message types for communication between basestation and the rover over ROS2. (being renamed to `astra_msgs`).
 - [servo\_arm\_twist\_pkg](./src/servo_arm_twist_pkg) - A temporary node to translate controller state from `ros2_joy` to `Twist` messages to control the Arm via IK.
 
+## Graphs
+
+### Full System
+
+> **Anchor stand-alone** (`ros2 launch anchor_pkg rover.launch.py`)
+>
+> ![rqt_graph of Anchor by itself, ran with command: ros2 launch anchor_pkg rover.launch.py](./docs-resources/graph-anchor-standalone.png)
+
+> **Anchor with [basestation-classic](https://github.com/SHC-ASTRA/basestation-classic)**
+>
+> ![rqt_graph of Anchor ran with the same command as above, talking to basestation-classic](./docs-resources/graph-anchor-w-basestation-classic.png)
+
+> **Anchor with Headless** (`ros2 run headless_pkg headless_full`)
+>
+> ![rqt_graph of Anchor ran with Headless](./docs-resources/graph-anchor-w-headless.png)
+
+### Individual Nodes
+
+> **Anchor** (`ros2 run anchor_pkg anchor`)
+>
+> ![rqt_graph of Anchor node running by itself](./docs-resources/graph-anchor-anchor-standalone.png)
+
+> **Core** (`ros2 run core_pkg core --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Core node running by itself](./docs-resources/graph-anchor-core-standalone.png)
+
+> **Arm** (`ros2 run arm_pkg arm --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Arm node running by itself](./docs-resources/graph-anchor-arm-standalone.png)
+
+> **Bio** (`ros2 run bio_pkg bio --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Bio node running by itself](./docs-resources/graph-anchor-bio-standalone.png)
+
 ## Maintainers
 
 | Name | Email | Discord |

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,30 +54,14 @@
         "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"
-        ]
+        ],
+        "treefmt-nix": "treefmt-nix"
       }
     },
     "systems": {
@@ -157,18 +79,23 @@
         "type": "github"
       }
     },
-    "systems_2": {
+    "treefmt-nix": {
+      "inputs": {
+        "nixpkgs": [
+          "nixpkgs"
+        ]
+      },
       "locked": {
-        "lastModified": 1681028828,
-        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
-        "owner": "nix-systems",
-        "repo": "default",
-        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "lastModified": 1773297127,
+        "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=",
+        "owner": "numtide",
+        "repo": "treefmt-nix",
+        "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016",
         "type": "github"
       },
       "original": {
-        "owner": "nix-systems",
-        "repo": "default",
+        "owner": "numtide",
+        "repo": "treefmt-nix",
         "type": "github"
       }
     }

flake.nix

@@ -3,13 +3,21 @@
 
   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!!!
+
+    treefmt-nix = {
+      url = "github:numtide/treefmt-nix";
+      inputs.nixpkgs.follows = "nixpkgs";
+    };
   };
 
   outputs =
-    { self, nix-ros-overlay, nixpkgs, astra-msgs }:
+    {
+      self,
+      nix-ros-overlay,
+      nixpkgs,
+      ...
+    }@inputs:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
       let
@@ -17,31 +25,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; [
-              pyserial
-              pygame
-              scipy
-              crccheck
-              black
-            ]))
-
-            (with rosPackages.${rosDistro};
+            (python313.withPackages (
+              p: with p; [
+                pyserial
+                python-can
+                pygame
+                scipy
+                crccheck
+                black
+              ]
+            ))
+            (
+              with rosPackages.humble;
               buildEnv {
                 paths = [
                   ros-core
-                  rqt-graph
                   ros2cli
                   ros2run
                   ros2bag
@@ -77,24 +81,22 @@
                   ros2-controllers
                   chomp-motion-planner
                 ];
-              })
+              }
+            )
           ];
-
-          env = {
-            ASTRAMSGS = "${astra-msgs.outPath}";
-          };
-
           shellHook = ''
+            # Display stuff
             export DISPLAY=''${DISPLAY:-:0}
             export QT_X11_NO_MITSHM=1
           '';
         };
+
+        formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper;
       }
     );
 
   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,28 +1,24 @@
+from warnings import deprecated
 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
-import sys
+from .connector import (
+    Connector,
+    MockConnector,
+    SerialConnector,
+    CANConnector,
+    NoValidDeviceException,
+    NoWorkingDeviceException,
+)
+from .convert import string_to_viccan
 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
-]
+from std_msgs.msg import String
 
 
 class Anchor(Node):
@@ -36,308 +32,204 @@ class Anchor(Node):
         - VicCAN messages for Arm node
     * /anchor/from_vic/bio
         - VicCAN messages for Bio node
+    * /anchor/to_vic/debug
+        - A string copy of the messages published to ./relay are published here
 
     Subscribers:
     * /anchor/from_vic/mock_mcu
-        - For testing without an actual MCU, publish strings here as if they came from an MCU
+        - For testing without an actual MCU, publish ViCAN messages 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
+        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
+    * /anchor/relay
+        - 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
+
     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
+        # ROS2 Parameter Setup
 
-        ##################################################
-        # Serial MCU Discovery
+        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'.",
+            ),
+        )
 
-        # 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(
+            "can_override",
+            "",
+            ParameterDescriptor(
+                name="can_override",
+                description="Overrides which CAN channel will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
+            ),
+        )
+
+        self.declare_parameter(
+            "serial_override",
+            "",
+            ParameterDescriptor(
+                name="serial_override",
+                description="Overrides which serial port will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
+            ),
+        )
+
+        # Determine which connector to use. Options are Mock, Serial, and CAN
+        connector_select = (
+            self.get_parameter("connector").get_parameter_value().string_value
+        )
+        can_override = (
+            self.get_parameter("can_override").get_parameter_value().string_value
+        )
+        serial_override = (
+            self.get_parameter("serial_override").get_parameter_value().string_value
+        )
+        match connector_select:
+            case "serial":
+                logger.info("using serial connector")
+                self.connector = SerialConnector(
+                    logger, self.get_clock(), serial_override
                 )
-            )
-            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}."
+            case "can":
+                logger.info("using CAN connector")
+                self.connector = CANConnector(logger, self.get_clock(), can_override)
+            case "mock":
+                logger.info("using mock connector")
+                self.connector = MockConnector(logger, self.get_clock())
+            case "auto":
+                logger.info("automatically determining connector")
+                try:
+                    logger.info("trying CAN connector")
+                    self.connector = CANConnector(
+                        logger, self.get_clock(), can_override
+                    )
+                except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
+                    logger.info("CAN connector failed, trying serial connector")
+                    self.connector = SerialConnector(
+                        logger, self.get_clock(), serial_override
+                    )
+            case _:
+                logger.fatal(
+                    f"invalid value for connector parameter: {connector_select}"
                 )
-                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()
+                exit(1)
 
-        # 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)
-
-        ##################################################
         # ROS2 Topic Setup
 
-        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(
-            String, "/anchor/from_vic/debug", 20
+        # Publishers
+        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
+            String,
+            "/anchor/from_vic/debug",
+            20,
         )
         self.fromvic_core_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/core", 20
+            VicCAN,
+            "/anchor/from_vic/core",
+            20,
         )
         self.fromvic_arm_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/arm", 20
+            VicCAN,
+            "/anchor/from_vic/arm",
+            20,
         )
         self.fromvic_bio_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/bio", 20
+            VicCAN,
+            "/anchor/from_vic/bio",
+            20,
+        )
+        # Debug publisher
+        self.tovic_debug_pub_ = self.create_publisher(
+            VicCAN,
+            "/anchor/to_vic/debug",
+            20,
         )
 
-        self.mock_mcu_sub_ = self.create_subscription(
-            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
-        )
+        # Subscribers
         self.tovic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+            VicCAN,
+            "/anchor/to_vic/relay",
+            self.write_connector,
+            20,
         )
-        self.tovic_debug_sub_ = self.create_subscription(
-            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
+        self.tovic_sub_legacy_ = self.create_subscription(
+            String,
+            "/anchor/relay",
+            self.write_connector_legacy,
+            20,
+        )
+        self.mock_mcu_sub_ = self.create_subscription(
+            String,
+            "/anchor/from_vic/mock_mcu",
+            self.relay_fromvic,
+            20,
+        )
+        self.tovic_string_sub_ = self.create_subscription(
+            String,
+            "/anchor/to_vic/relay_string",
+            self.connector.write_raw,
+            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 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")
-
-            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):
-        """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)})"
-            )
-        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*")
+        # Close devices on exit
+        atexit.register(self.cleanup)
 
     def cleanup(self):
-        print("Cleaning up before terminating...")
-        if self.serial_interface.is_open:
-            self.serial_interface.close()
+        self.connector.cleanup()
+
+    def read_connector(self):
+        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
+        viccan, raw = self.connector.read()
+
+        if raw:
+            self.fromvic_debug_pub_.publish(String(data=raw))
+
+        if viccan:
+            self.relay_fromvic(viccan)
+
+    def write_connector(self, msg: VicCAN):
+        """Write to the connector and send a copy to /anchor/to_vic/debug"""
+        self.connector.write(msg)
+        self.tovic_debug_pub_.publish(msg)
+
+    @deprecated(
+        "Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay"
+    )
+    def write_connector_legacy(self, msg: String):
+        """Write to the connector by first attempting to convert String to VicCAN"""
+        # please do not reference this code. ~riley
+        for cmd in msg.data.split("\n"):
+            viccan = string_to_viccan(
+                cmd,
+                "anchor",
+                self.get_logger(),
+                self.get_clock().now().to_msg(),
+            )
+            if viccan:
+                self.write_connector(viccan)
+
+    def relay_fromvic(self, msg: VicCAN):
+        """Relay a message from the MCU to the appropriate VicCAN topic"""
+        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 main(args=None):
@@ -350,16 +242,9 @@ def main(args=None):
 
         rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
         while rclpy.ok():
-            anchor_node.read_MCU()  # Check the MCU for updates
+            anchor_node.read_connector()  # Check the connector for updates
             rate.sleep()
     except (KeyboardInterrupt, ExternalShutdownException):
         print("Caught shutdown signal, shutting down...")
     finally:
         rclpy.try_shutdown()
-
-
-if __name__ == "__main__":
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
-    main()

src/anchor_pkg/anchor_pkg/connector.py

@@ -0,0 +1,438 @@
+from abc import ABC, abstractmethod
+from astra_msgs.msg import VicCAN
+from rclpy.clock import Clock
+from rclpy.impl.rcutils_logger import RcutilsLogger
+from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
+
+# CAN
+import can
+import can.interfaces.socketcan
+import struct
+
+# 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
+
+MCU_IDS = [
+    "broadcast",
+    "core",
+    "arm",
+    "digit",
+    "faerie",
+    "citadel",
+    "libs",
+]
+
+
+class NoValidDeviceException(Exception):
+    pass
+
+
+class NoWorkingDeviceException(Exception):
+    pass
+
+
+class MultipleValidDevicesException(Exception):
+    pass
+
+
+class DeviceClosedException(Exception):
+    pass
+
+
+class Connector(ABC):
+    logger: RcutilsLogger
+    clock: Clock
+
+    def string_to_viccan(self, msg: str, mcu_name: str):
+        """function currying so that we do not need to pass logger and clock every time"""
+        return _string_to_viccan(
+            msg,
+            mcu_name,
+            self.logger,
+            self.clock.now().to_msg(),
+        )
+
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
+        self.logger = logger
+        self.clock = clock
+
+    @abstractmethod
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        """
+        Must return a tuple of (VicCAN, debug message or string repr of VicCAN)
+        """
+        pass
+
+    @abstractmethod
+    def write(self, msg: VicCAN):
+        pass
+
+    @abstractmethod
+    def write_raw(self, msg: str):
+        pass
+
+    def cleanup(self):
+        pass
+
+
+class SerialConnector(Connector):
+    port: str
+    mcu_name: str
+    serial_interface: serial.Serial
+
+    def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
+        super().__init__(logger, clock)
+
+        ports = self._find_ports()
+        mcu_name: str | None = None
+
+        if serial_override:
+            logger.warn(
+                f"using serial_override: `{serial_override}`! this will bypass several checks."
+            )
+            ports = [serial_override]
+            mcu_name = "override"
+
+        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]
+        # we might already have a name by now if we overrode earlier
+        mcu_name = mcu_name or 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:
+                # turn relay mode off if it failed to respond with its name
+                serial_interface.write(b"can_relay_mode,off\n")
+                serial_interface.close()
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException when asking for MCU name: {e}")
+
+        return None
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        try:
+            raw = str(self.serial_interface.readline(), "utf8")
+
+            if not raw:
+                return (None, None)
+
+            return (
+                self.string_to_viccan(raw, self.mcu_name),
+                raw,
+            )
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException: {e}")
+            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
+        except Exception:
+            return (None, None)  # pretty much no other error matters
+
+    def write(self, msg: VicCAN):
+        self.write_raw(viccan_to_string(msg))
+
+    def write_raw(self, msg: str):
+        self.serial_interface.write(bytes(msg, "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, clock: Clock, can_override: str):
+        super().__init__(logger, clock)
+
+        self.can_channel: str | None = None
+        self.can_bus: can.BusABC | None = None
+
+        avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
+
+        if len(avail) == 0:
+            raise NoValidDeviceException("no CAN interfaces found")
+
+        # filter to busses whose channel matches the can_override
+        if can_override:
+            self.logger.info(f"overrode can interface with {can_override}")
+            avail = list(
+                filter(
+                    lambda b: b.get("channel") == can_override,
+                    avail,
+                )
+            )
+
+        if (l := len(avail)) > 1:
+            channels = ", ".join(str(b.get("channel")) for b in avail)
+            raise MultipleValidDevicesException(
+                f"too many ({l}) CAN interfaces found: [{channels}]"
+            )
+
+        bus = avail[0]
+        self.can_channel = str(bus.get("channel"))
+        self.logger.info(f"found CAN interface '{self.can_channel}'")
+
+        try:
+            self.can_bus = can.Bus(
+                interface="socketcan",
+                channel=self.can_channel,
+                bitrate=1_000_000,
+            )
+        except can.CanError as e:
+            raise NoWorkingDeviceException(
+                f"could not open CAN channel '{self.can_channel}': {e}"
+            )
+
+        if self.can_channel and self.can_channel.startswith("v"):
+            self.logger.warn("CAN interface is likely virtual")
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        if not self.can_bus:
+            raise DeviceClosedException("CAN bus not initialized")
+
+        try:
+            message = self.can_bus.recv(timeout=0.0)
+        except can.CanError as e:
+            self.logger.error(f"CAN error while receiving: {e}")
+            raise DeviceClosedException("CAN bus closed unexpectedly")
+
+        if message is None:
+            return (None, None)
+
+        arbitration_id = message.arbitration_id & 0x7FF
+        data_bytes = bytes(message.data)
+
+        mcu_key = (arbitration_id >> 8) & 0b111
+        data_type_key = (arbitration_id >> 6) & 0b11
+        command = arbitration_id & 0x3F
+
+        try:
+            mcu_name = MCU_IDS[mcu_key]
+        except IndexError:
+            self.logger.warn(
+                f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}"
+            )
+            return (None, None)
+
+        data: list[float] = []
+
+        try:
+            if data_type_key == 3:
+                data = []
+            elif data_type_key == 0:
+                if len(data_bytes) < 8:
+                    self.logger.warn(
+                        f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
+                    )
+                    return (None, None)
+                (value,) = struct.unpack(">d", data_bytes[:8])
+                data = [float(value)]
+            elif data_type_key == 1:
+                if len(data_bytes) < 8:
+                    self.logger.warn(
+                        f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
+                    )
+                    return (None, None)
+                v1, v2 = struct.unpack(">ff", data_bytes[:8])
+                data = [float(v1), float(v2)]
+            elif data_type_key == 2:
+                if len(data_bytes) < 8:
+                    self.logger.warn(
+                        f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
+                    )
+                    return (None, None)
+                i1, i2, i3, i4 = struct.unpack(">hhhh", data_bytes[:8])
+                data = [float(i1), float(i2), float(i3), float(i4)]
+            else:
+                self.logger.warn(
+                    f"received CAN frame with unknown data_type_key {data_type_key}; id=0x{arbitration_id:X}"
+                )
+                return (None, None)
+        except struct.error as e:
+            self.logger.error(f"error unpacking CAN payload: {e}")
+            return (None, None)
+
+        viccan = VicCAN(
+            mcu_name=mcu_name,
+            command_id=int(command),
+            data=data,
+        )
+
+        self.logger.debug(
+            f"received CAN frame id=0x{message.arbitration_id:X}, "
+            f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
+        )
+
+        return (
+            viccan,
+            f"{viccan.mcu_name},{viccan.command_id},"
+            + ",".join(map(str, list(viccan.data))),
+        )
+
+    def write(self, msg: VicCAN):
+        if not self.can_bus:
+            raise DeviceClosedException("CAN bus not initialized")
+
+        # build 11-bit arbitration ID according to VicCAN spec:
+        # bits 10..8: targeted MCU key
+        # bits 7..6:  data type key
+        # bits 5..0:  command
+
+        # map MCU name to 3-bit key.
+        try:
+            mcu_id = MCU_IDS.index((msg.mcu_name or "").lower())
+        except ValueError:
+            self.logger.error(
+                f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message"
+            )
+            return
+
+        # determine data type from length:
+        # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
+        match data_len := len(msg.data):
+            case 0:
+                data_type = 3
+                data = bytes()
+            case 1:
+                data_type = 0
+                data = struct.pack(">d", *msg.data)
+            case 2:
+                data_type = 1
+                data = struct.pack(">ff", *msg.data)
+            case 3 | 4:  # 3 gets treated as 4
+                data_type = 2
+                if data_len == 3:
+                    msg.data.append(0)
+                data = struct.pack(">hhhh", *[int(x) for x in msg.data])
+            case _:
+                self.logger.error(
+                    f"unexpected VicCAN data length: {data_len}; dropping message"
+                )
+                return
+
+        # command is limited to 6 bits.
+        command = int(msg.command_id)
+        if command < 0 or command > 0x3F:
+            self.logger.error(
+                f"invalid command_id for CAN frame: {command}; dropping message"
+            )
+            return
+
+        try:
+            can_message = can.Message(
+                arbitration_id=(mcu_id << 8) | (data_type << 6) | command,
+                data=data,
+                is_extended_id=False,
+            )
+        except Exception as e:
+            self.logger.error(f"failed to construct CAN message: {e}")
+            return
+
+        try:
+            self.can_bus.send(can_message)
+            self.logger.debug(
+                f"sent CAN frame id=0x{can_message.arbitration_id:X}, "
+                f"data={list(can_message.data)}"
+            )
+        except can.CanError as e:
+            self.logger.error(f"CAN error while sending: {e}")
+            raise DeviceClosedException("CAN bus closed unexpectedly")
+
+    def write_raw(self, msg: str):
+        self.logger.warn(f"write_raw is not supported for CANConnector. msg: {msg}")
+
+    def cleanup(self):
+        try:
+            if self.can_bus is not None:
+                self.logger.info("shutting down CAN bus")
+                self.can_bus.shutdown()
+        except Exception as e:
+            self.logger.error(e)
+
+
+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.
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        return (None, None)
+
+    def write(self, msg: VicCAN):
+        pass
+
+    def write_raw(self, msg: str):
+        pass

src/anchor_pkg/anchor_pkg/convert.py

@@ -0,0 +1,65 @@
+from astra_msgs.msg import VicCAN
+from std_msgs.msg import Header
+from builtin_interfaces.msg import Time
+from rclpy.impl.rcutils_logger import RcutilsLogger
+
+
+def string_to_viccan(
+    msg: str, mcu_name: str, logger: RcutilsLogger, time: Time
+) -> VicCAN | None:
+    """
+    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 and return None.
+    """
+
+    parts: list[str] = msg.strip().split(",")
+
+    # don't need an extra check because len of .split output is always >= 1
+    if not parts[0].startswith("can_relay_"):
+        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
+
+    try:
+        command_id = int(parts[2])
+    except ValueError:
+        logger.debug(
+            f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    if command_id not in range(64):
+        logger.debug(
+            f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    try:
+        return VicCAN(
+            header=Header(
+                stamp=time,
+                frame_id="from_vic",
+            ),
+            mcu_name=parts[1],
+            command_id=command_id,
+            data=[float(x) for x in parts[3:]],
+        )
+    except ValueError:
+        logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}")
+        return None
+
+
+def viccan_to_string(viccan: VicCAN) -> str:
+    """Converts a ROS2 VicCAN message to the serial string VicCAN format."""
+    # make sure we accept 3 digits and treat it as 4
+    if len(viccan.data) == 3:
+        viccan.data.append(0)
+    # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max
+    data = "".join([f",{round(val,7)}" for val in viccan.data])
+    return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n"

src/anchor_pkg/launch/rover.launch.py

@@ -1,136 +1,174 @@
-#!/usr/bin/env python3
-
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
-from launch.substitutions import (
-    LaunchConfiguration,
-    ThisLaunchFileDir,
-    PathJoinSubstitution,
-)
+from launch.actions import DeclareLaunchArgument, Shutdown, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
-
-
-# Prevent making __pycache__ directories
-from sys import dont_write_bytecode
-
-dont_write_bytecode = True
-
-
-def launch_setup(context, *args, **kwargs):
-    # Retrieve the resolved value of the launch argument 'mode'
-    mode = LaunchConfiguration("mode").perform(context)
-    nodes = []
-
-    if mode == "anchor":
-        # Launch every node and pass "anchor" as the parameter
-
-        nodes.append(
-            Node(
-                package="arm_pkg",
-                executable="arm",  # change as needed
-                name="arm",
-                output="both",
-                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown(),
-            )
-        )
-        nodes.append(
-            Node(
-                package="core_pkg",
-                executable="core",  # change as needed
-                name="core",
-                output="both",
-                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown(),
-            )
-        )
-        nodes.append(
-            Node(
-                package="core_pkg",
-                executable="ptz",  # change as needed
-                name="ptz",
-                output="both",
-                # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
-                # on_exit=Shutdown()  # Uncomment if you want to shutdown on PTZ failure
-            )
-        )
-        nodes.append(
-            Node(
-                package="bio_pkg",
-                executable="bio",  # change as needed
-                name="bio",
-                output="both",
-                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown(),
-            )
-        )
-        nodes.append(
-            Node(
-                package="anchor_pkg",
-                executable="anchor",  # change as needed
-                name="anchor",
-                output="both",
-                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown(),
-            )
-        )
-    elif mode in ["arm", "core", "bio", "ptz"]:
-        # Only launch the node corresponding to the provided mode.
-        if mode == "arm":
-            nodes.append(
-                Node(
-                    package="arm_pkg",
-                    executable="arm",
-                    name="arm",
-                    output="both",
-                    parameters=[{"launch_mode": mode}],
-                    on_exit=Shutdown(),
-                )
-            )
-        elif mode == "core":
-            nodes.append(
-                Node(
-                    package="core_pkg",
-                    executable="core",
-                    name="core",
-                    output="both",
-                    parameters=[{"launch_mode": mode}],
-                    on_exit=Shutdown(),
-                )
-            )
-        elif mode == "bio":
-            nodes.append(
-                Node(
-                    package="bio_pkg",
-                    executable="bio",
-                    name="bio",
-                    output="both",
-                    parameters=[{"launch_mode": mode}],
-                    on_exit=Shutdown(),
-                )
-            )
-        elif mode == "ptz":
-            nodes.append(
-                Node(
-                    package="core_pkg",
-                    executable="ptz",
-                    name="ptz",
-                    output="both",
-                    on_exit=Shutdown(),  # on fail, shutdown if this was the only node to be launched
-                )
-            )
-    else:
-        # If an invalid mode is provided, print an error.
-        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
-
-    return nodes
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
-    declare_arg = DeclareLaunchArgument(
-        "mode",
-        default_value="anchor",
-        description="Launch mode: arm, core, bio, anchor, or ptz",
+    connector = LaunchConfiguration("connector")
+    serial_override = LaunchConfiguration("serial_override")
+    can_override = LaunchConfiguration("can_override")
+    use_ptz = LaunchConfiguration("use_ptz")
+
+    ld = LaunchDescription()
+
+    # arguments
+    ld.add_action(
+        DeclareLaunchArgument(
+            "connector",
+            default_value="auto",
+            description="Connector parameter for anchor node (default: 'auto')",
+        )
     )
 
-    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
+    ld.add_action(
+        DeclareLaunchArgument(
+            "serial_override",
+            default_value="",
+            description="Serial port override parameter for anchor node (default: '')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "can_override",
+            default_value="",
+            description="CAN network override parameter for anchor node (default: '')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "use_ptz",
+            default_value="true",  # must be string for launch system
+            description="Whether to launch PTZ node (default: true)",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "use_ros2_control",
+            default_value="false",
+            description="Whether to use DiffDriveController for driving instead of direct Twist",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "rover_platform",
+            default_value="auto",
+            description="Choose the rover platform (either clucky or testbed). If left on auto, will defer to ROVER_PLATFORM environment variable.",
+            choices=["clucky", "testbed", "auto"],
+        )
+    )
+
+    # nodes
+    ld.add_action(
+        Node(
+            package="anchor_pkg",
+            executable="anchor",
+            name="anchor",
+            output="both",
+            parameters=[
+                {
+                    "launch_mode": "anchor",
+                    "connector": connector,
+                    "serial_override": serial_override,
+                    "can_override": can_override,
+                }
+            ],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="arm_pkg",
+            executable="arm",
+            name="arm",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="bio_pkg",
+            executable="bio",
+            name="bio",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="core",
+            name="core",
+            output="both",
+            parameters=[
+                {"launch_mode": "anchor"},
+                {
+                    "use_ros2_control": LaunchConfiguration(
+                        "use_ros2_control", default=False
+                    )
+                },
+                {
+                    "rover_platform": LaunchConfiguration(
+                        "rover_platform", default="auto"
+                    )
+                },
+            ],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="ptz",
+            name="ptz",
+            output="both",
+            condition=IfCondition(use_ptz),
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("core_description"),
+                        "launch",
+                        "robot_state_publisher.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+            launch_arguments={("hardware_mode", "physical")},
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("core_description"),
+                        "launch",
+                        "spawn_controllers.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+            launch_arguments={("hardware_mode", "physical")},
+        )
+    )
+
+    return ld

src/anchor_pkg/package.xml

@@ -3,13 +3,20 @@
 <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>ASTRA VicCAN driver package, using python-can and pyserial.</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>
+
+  <exec_depend>common_interfaces</exec_depend>
+  <exec_depend>python3-serial</exec_depend>
+  <exec_depend>python3-can</exec_depend>
+
+  <exec_depend>core_pkg</exec_depend>
+  <exec_depend>arm_pkg</exec_depend>
+  <exec_depend>bio_pkg</exec_depend>
+  <exec_depend>core_description</exec_depend>
 
   <build_depend>black</build_depend>
 

src/anchor_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/anchor_pkg
 [install]
 install_scripts=$base/lib/anchor_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/anchor_pkg/setup.py

@@ -6,7 +6,7 @@ package_name = "anchor_pkg"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -17,8 +17,8 @@ setup(
     zip_safe=True,
     maintainer="tristan",
     maintainer_email="tristanmcginnis26@gmail.com",
-    description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
-    license="All Rights Reserved",
+    description="ASTRA VicCAN driver package, using python-can and pyserial.",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
     },

src/arm_pkg/arm_pkg/arm_headless.py

@@ -1,279 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-import pygame
-
-import time
-
-import serial
-import sys
-import threading
-import glob
-import os
-
-from std_msgs.msg import String
-from astra_msgs.msg import ControllerState
-from astra_msgs.msg import ArmManual
-from astra_msgs.msg import ArmIK
-
-
-os.environ["SDL_AUDIODRIVER"] = (
-    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
-)
-os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
-
-
-class Headless(Node):
-    def __init__(self):
-        # Initalize node with name
-        super().__init__("arm_headless")
-
-        # Depricated, kept temporarily for reference
-        # self.create_timer(0.20, self.send_controls)#read and send controls
-
-        self.create_timer(0.1, self.send_manual)
-
-        # Create a publisher to publish any output the pico sends
-
-        # Depricated, kept temporarily for reference
-        # self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10)
-        self.manual_pub = self.create_publisher(ArmManual, "/arm/control/manual", 10)
-
-        # Create a subscriber to listen to any commands sent for the pico
-
-        # Depricated, kept temporarily for reference
-        # self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)
-
-        # self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
-
-        self.laser_status = 0
-
-        # Initialize pygame
-        pygame.init()
-
-        # Initialize the gamepad module
-        pygame.joystick.init()
-
-        # Check if any gamepad is connected
-        if pygame.joystick.get_count() == 0:
-            print("No gamepad found.")
-            pygame.quit()
-            exit()
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                exit()
-
-        # Initialize the first gamepad, print name to terminal
-        self.gamepad = pygame.joystick.Joystick(0)
-        self.gamepad.init()
-        print(f"Gamepad Found: {self.gamepad.get_name()}")
-        #
-        #
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                # Check the pico for updates
-
-                # self.read_feedback()
-                if (
-                    pygame.joystick.get_count() == 0
-                ):  # if controller disconnected, wait for it to be reconnected
-                    print(f"Gamepad disconnected: {self.gamepad.get_name()}")
-
-                    while pygame.joystick.get_count() == 0:
-                        # self.send_controls() #depricated, kept for reference temporarily
-                        self.send_manual()
-                        # self.read_feedback()
-                    self.gamepad = pygame.joystick.Joystick(0)
-                    self.gamepad.init()  # re-initialized gamepad
-                    print(f"Gamepad reconnected: {self.gamepad.get_name()}")
-
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def send_manual(self):
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                exit()
-        input = ArmManual()
-
-        # Triggers for gripper control
-        if self.gamepad.get_axis(2) > 0:  # left trigger
-            input.gripper = -1
-        elif self.gamepad.get_axis(5) > 0:  # right trigger
-            input.gripper = 1
-
-        # Toggle Laser
-        if self.gamepad.get_button(7):  # Start
-            self.laser_status = 1
-        elif self.gamepad.get_button(6):  # Back
-            self.laser_status = 0
-        input.laser = self.laser_status
-
-        if self.gamepad.get_button(5):  # right bumper, control effector
-
-            # Left stick X-axis for effector yaw
-            if self.gamepad.get_axis(0) > 0:
-                input.effector_yaw = 1
-            elif self.gamepad.get_axis(0) < 0:
-                input.effector_yaw = -1
-
-            # Right stick X-axis for effector roll
-            if self.gamepad.get_axis(3) > 0:
-                input.effector_roll = 1
-            elif self.gamepad.get_axis(3) < 0:
-                input.effector_roll = -1
-
-        else:  # Control arm axis
-            dpad_input = self.gamepad.get_hat(0)
-            input.axis0 = 0
-            if dpad_input[0] == 1:
-                input.axis0 = 1
-            elif dpad_input[0] == -1:
-                input.axis0 = -1
-
-            if self.gamepad.get_axis(0) > 0.15 or self.gamepad.get_axis(0) < -0.15:
-                input.axis1 = round(self.gamepad.get_axis(0))
-
-            if self.gamepad.get_axis(1) > 0.15 or self.gamepad.get_axis(1) < -0.15:
-                input.axis2 = -1 * round(self.gamepad.get_axis(1))
-
-            if self.gamepad.get_axis(4) > 0.15 or self.gamepad.get_axis(4) < -0.15:
-                input.axis3 = -1 * round(self.gamepad.get_axis(4))
-
-            # input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis
-            # input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis
-            # input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis
-
-        # Button Mappings
-        # axis2 -> LT
-        # axis5 -> RT
-        # Buttons0 -> A
-        # Buttons1 -> B
-        # Buttons2 -> X
-        # Buttons3 -> Y
-        # Buttons4 -> LB
-        # Buttons5 -> RB
-        # Buttons6 -> Back
-        # Buttons7 -> Start
-
-        input.linear_actuator = 0
-
-        if pygame.joystick.get_count() != 0:
-
-            self.get_logger().info(
-                f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n"
-            )
-            self.manual_pub.publish(input)
-        else:
-            pass
-
-        pass
-
-    # Depricated, kept temporarily for reference
-    # def send_controls(self):
-
-    #     for event in pygame.event.get():
-    #         if event.type == pygame.QUIT:
-    #             pygame.quit()
-    #             exit()
-    #     input = ControllerState()
-
-    #     input.lt = self.gamepad.get_axis(2)#left trigger
-    #     input.rt = self.gamepad.get_axis(5)#right trigger
-
-    #     #input.lb = self.gamepad.get_button(9)#Value must be converted to bool
-    #     if(self.gamepad.get_button(4)):#left bumper
-    #         input.lb = True
-    #     else:
-    #         input.lb = False
-
-    #     #input.rb = self.gamepad.get_button(10)#Value must be converted to bool
-    #     if(self.gamepad.get_button(5)):#right bumper
-    #         input.rb = True
-    #     else:
-    #         input.rb = False
-
-    #     #input.plus = self.gamepad.get_button(6)#plus button
-    #     if(self.gamepad.get_button(7)):#plus button
-    #         input.plus = True
-    #     else:
-    #         input.plus = False
-
-    #     #input.minus = self.gamepad.get_button(4)#minus button
-    #     if(self.gamepad.get_button(6)):#minus button
-    #         input.minus = True
-    #     else:
-    #         input.minus = False
-
-    #     input.ls_x = round(self.gamepad.get_axis(0),2)#left x-axis
-    #     input.ls_y = round(self.gamepad.get_axis(1),2)#left y-axis
-    #     input.rs_x = round(self.gamepad.get_axis(3),2)#right x-axis
-    #     input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis
-
-    #     #input.a = self.gamepad.get_button(1)#A button
-    #     if(self.gamepad.get_button(0)):#A button
-    #         input.a = True
-    #     else:
-    #         input.a = False
-    #     #input.b = self.gamepad.get_button(0)#B button
-    #     if(self.gamepad.get_button(1)):#B button
-    #         input.b = True
-    #     else:
-    #         input.b = False
-    #     #input.x = self.gamepad.get_button(3)#X button
-    #     if(self.gamepad.get_button(2)):#X button
-    #         input.x = True
-    #     else:
-    #         input.x = False
-    #     #input.y = self.gamepad.get_button(2)#Y button
-    #     if(self.gamepad.get_button(3)):#Y button
-    #         input.y = True
-    #     else:
-    #         input.y = False
-
-    #     dpad_input = self.gamepad.get_hat(0)#D-pad input
-
-    #     #not using up/down on DPad
-    #     input.d_up = False
-    #     input.d_down = False
-
-    #     if(dpad_input[0] == 1):#D-pad right
-    #         input.d_right = True
-    #     else:
-    #         input.d_right = False
-    #     if(dpad_input[0] == -1):#D-pad left
-    #         input.d_left = True
-    #     else:
-    #         input.d_left = False
-
-    #     if pygame.joystick.get_count() != 0:
-
-    #         self.get_logger().info(f"[Ctrl] Updated Controller State\n")
-
-    #         self.publisher.publish(input)
-    #     else:
-    #         pass
-
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    node = Headless()
-
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-    # tb_bs = BaseStation()
-    # node.run()
-
-
-if __name__ == "__main__":
-    main()

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,181 +1,276 @@
+import sys
+import signal
+import math
+from warnings import deprecated
+
 import rclpy
 from rclpy.node import Node
-import serial
-import sys
-import threading
-import glob
-import time
-import atexit
-import signal
-from std_msgs.msg import String
-from astra_msgs.msg import ArmManual
-from astra_msgs.msg import SocketFeedback
-from astra_msgs.msg import DigitFeedback
+from rclpy.executors import ExternalShutdownException
+from rclpy import qos
+
+from std_msgs.msg import String, Header
 from sensor_msgs.msg import JointState
-import math
+from control_msgs.msg import JointJog
+from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual  # TODO: Old topics
+from astra_msgs.msg import ArmFeedback, ArmCtrlState, VicCAN, RevMotorState
 
-# control_qos = qos.QoSProfile(
-#     history=qos.QoSHistoryPolicy.KEEP_LAST,
-#     depth=1,
-#     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-#     durability=qos.QoSDurabilityPolicy.VOLATILE,
-#     deadline=1000,
-#     lifespan=500,
-#     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-#     liveliness_lease_duration=5000
-# )
-
-serial_pub = None
-thread = None
+control_qos = qos.QoSProfile(
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    depth=2,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,  # Best Effort subscribers are still compatible with Reliable publishers
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # deadline=Duration(seconds=1),
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness_lease_duration=Duration(seconds=5),
+)
 
 
-class SerialRelay(Node):
+class ArmNode(Node):
+    """Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""
+
+    # Every non-fixed joint defined in Arm's URDF
+    # Used for JointState and JointJog messsages
+    all_joint_names = [
+        "axis_0_joint",
+        "axis_1_joint",
+        "axis_2_joint",
+        "axis_3_joint",
+        "wrist_yaw_joint",
+        "wrist_roll_joint",
+        "ef_gripper_left_joint",
+    ]
+
+    # Used to verify the length of an incoming VicCAN feedback message
+    # Key is VicCAN command_id, value is expected length of data list
+    viccan_socket_msg_len_dict = {
+        53: 4,
+        54: 4,
+        55: 4,
+        58: 4,
+        59: 4,
+    }
+
+    viccan_digit_msg_len_dict = {
+        54: 4,
+        55: 2,
+        59: 2,
+    }
+
     def __init__(self):
-        # Initialize node
         super().__init__("arm_node")
 
-        # Get launch mode parameter
-        self.declare_parameter("launch_mode", "arm")
-        self.launch_mode = self.get_parameter("launch_mode").value
-        self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
+        self.get_logger().info(f"arm launch_mode is: anchor")  # Hey I like the output
 
-        # Create publishers
-        self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
-        self.socket_pub = self.create_publisher(
-            SocketFeedback, "/arm/feedback/socket", 10
-        )
-        self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
-        self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
+        ##################################################
+        # Parameters
 
-        # Create subscribers
-        self.man_sub = self.create_subscription(
-            ArmManual, "/arm/control/manual", self.send_manual, 2
+        self.declare_parameter("use_old_topics", True)
+        self.use_old_topics = (
+            self.get_parameter("use_old_topics").get_parameter_value().bool_value
         )
 
-        # New messages
-        self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
-        self.joint_state = JointState()
-        self.joint_state.name = [
-            "Axis_0_Joint",
-            "Axis_1_Joint",
-            "Axis_2_Joint",
-            "Axis_3_Joint",
-            "Wrist_Differential_Joint",
-            "Wrist-EF_Roll_Joint",
-            "Gripper_Slider_Left",
-        ]
-        self.joint_state.position = [0.0] * len(
-            self.joint_state.name
-        )  # Initialize with zeros
+        ##################################################
+        # Old topics
 
-        self.joint_command_sub = self.create_subscription(
-            JointState, "/joint_commands", self.joint_command_callback, 10
-        )
-
-        # Topics used in anchor mode
-        if self.launch_mode == "anchor":
+        if self.use_old_topics:
+            # Anchor topics
             self.anchor_sub = self.create_subscription(
                 String, "/anchor/arm/feedback", self.anchor_feedback, 10
             )
-            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+            self.anchor_pub = self.create_publisher(
+                String, "/anchor/to_vic/relay_string", 10
+            )
 
-        self.arm_feedback = SocketFeedback()
-        self.digit_feedback = DigitFeedback()
+            # Create publishers
+            self.socket_pub = self.create_publisher(
+                SocketFeedback, "/arm/feedback/socket", 10
+            )
+            self.arm_feedback = SocketFeedback()
+            self.digit_pub = self.create_publisher(
+                DigitFeedback, "/arm/feedback/digit", 10
+            )
+            self.digit_feedback = DigitFeedback()
+            self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
 
-        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == "arm":
-            # Loop through all serial devices on the computer to check for the MCU
-            self.port = None
-            ports = SerialRelay.list_serial_ports()
-            for _ in range(4):
-                for port in ports:
-                    try:
-                        # connect and send a ping command
-                        ser = serial.Serial(port, 115200, timeout=1)
-                        # print(f"Checking port {port}...")
-                        ser.write(b"ping\n")
-                        response = ser.read_until("\n")  # type: ignore
+            # Create subscribers
+            self.man_sub = self.create_subscription(
+                ArmManual, "/arm/control/manual", self.send_manual, 10
+            )
 
-                        # if pong is in response, then we are talking with the MCU
-                        if b"pong" in response:
-                            self.port = port
-                            self.get_logger().info(f"Found MCU at {self.port}!")
-                            break
-                    except:
-                        pass
-                if self.port is not None:
-                    break
+        ###################################################
+        # New topics
 
-            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)
+        # Anchor topics
 
-            self.ser = serial.Serial(self.port, 115200)
-            atexit.register(self.cleanup)
+        # from_vic
+        self.anchor_fromvic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/from_vic/arm", self.relay_fromvic, 20
+        )
+        # to_vic
+        self.anchor_tovic_pub_ = self.create_publisher(
+            VicCAN, "/anchor/to_vic/relay", 20
+        )
 
-    def run(self):
-        global thread
-        thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
-        thread.start()
+        # Control
 
-        # if in arm mode, will need to read from the MCU
+        # Manual: /arm/control/joint_jog is published by Basestation or Headless
+        self.man_jointjog_sub_ = self.create_subscription(
+            JointJog,
+            "/arm/control/joint_jog",
+            self.jointjog_callback,
+            qos_profile=control_qos,
+        )
+        # IK: /joint_commands is published by JointTrajectoryController via topic_based_control
+        self.joint_command_sub_ = self.create_subscription(
+            JointState,
+            "/joint_commands",
+            self.joint_command_callback,
+            qos_profile=control_qos,
+        )
+        # State: /arm/control/state is published by Basestation or Headless
+        self.man_state_sub_ = self.create_subscription(
+            ArmCtrlState,
+            "/arm/control/state",
+            self.man_state_callback,
+            qos_profile=control_qos,
+        )
 
-        try:
-            while rclpy.ok():
-                if self.launch_mode == "arm":
-                    if self.ser.in_waiting:
-                        self.read_mcu()
-                    else:
-                        time.sleep(0.1)
-        except KeyboardInterrupt:
-            pass
-        finally:
-            self.cleanup()
+        # Feedback
 
-    # 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
+        # Combined Socket and Digit feedback
+        self.arm_feedback_pub_ = self.create_publisher(
+            ArmFeedback,
+            "/arm/feedback",
+            qos_profile=qos.qos_profile_sensor_data,
+        )
+        # IK arm pose: /joint_states is published from here to topic_based_control
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        ###################################################
+        # Saved state
+
+        # Combined Socket and Digit feedback
+        self.arm_feedback_new = ArmFeedback()
+        self.arm_feedback_new.axis0_motor.id = 1
+        self.arm_feedback_new.axis1_motor.id = 2
+        self.arm_feedback_new.axis2_motor.id = 3
+        self.arm_feedback_new.axis3_motor.id = 4
+
+        # IK Arm pose
+        self.saved_joint_state = JointState()
+        self.saved_joint_state.header.frame_id = "base_link"
+        self.saved_joint_state.name = self.all_joint_names
+        # ... initialize with zeros
+        self.saved_joint_state.position = [0.0] * len(self.saved_joint_state.name)
+        self.saved_joint_state.velocity = [0.0] * len(self.saved_joint_state.name)
+
+    def jointjog_callback(self, msg: JointJog):
+        if len(msg.joint_names) != len(msg.velocities):
+            self.get_logger().debug("Ignoring malformed /arm/manual/joint_jog message.")
+            return
+
+        # Grab velocities from message
+        velocities = [
+            (
+                msg.velocities[msg.joint_names.index(joint_name)]  # type: ignore
+                if joint_name in msg.joint_names
+                else 0.0
+            )
+            for joint_name in self.all_joint_names
+        ]
+        # Deadzone
+        velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=39,
+                data=velocities[0:4],
+                header=msg.header,
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="digit",
+                command_id=39,
+                data=velocities[4:6],
+                header=msg.header,
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="digit",
+                command_id=26,
+                data=[velocities[6]],
+                header=msg.header,
+            )
+        )
+
+        # TODO: use msg.duration
+
+    def man_state_callback(self, msg: ArmCtrlState):
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=18,
+                data=[1.0 if msg.brake_mode else 0.0],
+                header=Header(stamp=self.get_clock().now().to_msg()),
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=34,
+                data=[1.0 if msg.laser else 0.0],
+                header=Header(stamp=self.get_clock().now().to_msg()),
+            )
+        )
 
     def joint_command_callback(self, msg: JointState):
-        # Embedded takes deg*10, ROS2 uses Radians
-        positions = [math.degrees(pos) * 10 for pos in msg.position]
-        # Axis 2 & 3 URDF direction is inverted
-        positions[2] = -positions[2]
-        positions[3] = -positions[3]
+        if len(msg.position) < 7 and len(msg.velocity) < 7:
+            self.get_logger().debug("Ignoring malformed /joint_command message.")
+            return  # command needs either position or velocity for all 7 joints
 
-        # Set target angles for each arm axis for embedded IK PID to handle
-        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
-        # Wrist yaw and roll
-        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
-        # Gripper IK does not have adequate hardware yet
-        self.send_cmd(command)
+        # Grab velocities from message
+        velocities = [
+            (
+                msg.velocity[msg.name.index(joint_name)]  # type: ignore
+                if joint_name in msg.name
+                else 0.0
+            )
+            for joint_name in self.all_joint_names
+        ]
 
+        self.send_velocities(velocities, msg.header)
+
+    def send_velocities(self, velocities: list[float], header: Header):
+        # ROS2's rad/s to VicCAN's deg/s*10; don't convert gripper's m/s
+        velocities = [
+            math.degrees(vel) * 10 if i < 6 else vel for i, vel in enumerate(velocities)
+        ]
+
+        # Send Axis 0-3
+        self.anchor_tovic_pub_.publish(
+            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:4], header=header)
+        )
+        # Send Wrist yaw and roll
+        # TODO: Verify embedded
+        self.anchor_tovic_pub_.publish(
+            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:6], header=header)
+        )
+        # Send End Effector Gripper
+        # TODO: Verify m/s received correctly by embedded
+        self.anchor_tovic_pub_.publish(
+            VicCAN(mcu_name="digit", command_id=26, data=[velocities[6]], header=header)
+        )
+
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
         axis1 = -1 * msg.axis1
@@ -200,24 +295,17 @@ class SerialRelay(Node):
 
         return
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_cmd(self, msg: str):
-        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)
-        elif self.launch_mode == "arm":  # if in standalone mode, send to MCU directly
-            self.get_logger().info(f"[Arm to MCU] {msg}")
-            self.ser.write(bytes(msg, "utf8"))
+        output = String(data=msg)
+        self.anchor_pub.publish(output)
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def anchor_feedback(self, msg: String):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
-            # pass
             self.updateAngleFeedback(output)
         elif output.startswith("can_relay_fromvic,arm,54"):
-            # pass
             self.updateBusVoltage(output)
         elif output.startswith("can_relay_fromvic,arm,53"):
             self.updateMotorFeedback(output)
@@ -235,19 +323,138 @@ class SerialRelay(Node):
             if len(parts) >= 4:
                 self.digit_feedback.wrist_angle = float(parts[3])
                 # self.digit_feedback.wrist_roll = float(parts[4])
-                self.joint_state.position[4] = math.radians(
-                    float(parts[4])
-                )  # Wrist roll
-                self.joint_state.position[5] = math.radians(
-                    float(parts[3])
-                )  # Wrist yaw
         else:
             return
 
+    def relay_fromvic(self, msg: VicCAN):
+        # Code for socket and digit are broken out for cleaner code
+        if msg.mcu_name == "arm":
+            self.process_fromvic_arm(msg)
+        elif msg.mcu_name == "digit":
+            self.process_fromvic_digit(msg)
+
+    def process_fromvic_arm(self, msg: VicCAN):
+        assert msg.mcu_name == "arm"
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in self.viccan_socket_msg_len_dict:
+            expected_len = self.viccan_socket_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
+
+        self.arm_feedback_new.header.stamp = msg.header.stamp
+
+        match msg.command_id:
+            case 53:  # REV SPARK MAX feedback
+                motorId = round(msg.data[0])
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.arm_feedback_new.axis1_motor
+                    case 2:
+                        motor = self.arm_feedback_new.axis2_motor
+                    case 3:
+                        motor = self.arm_feedback_new.axis3_motor
+                    case 4:
+                        motor = self.arm_feedback_new.axis0_motor
+
+                if motor:
+                    motor.temperature = float(msg.data[1]) / 10.0
+                    motor.voltage = float(msg.data[2]) / 10.0
+                    motor.current = float(msg.data[3]) / 10.0
+                    motor.header.stamp = msg.header.stamp
+
+                self.arm_feedback_pub_.publish(self.arm_feedback_new)
+            case 54:  # Board voltages
+                self.arm_feedback_new.socket_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.arm_feedback_new.socket_voltage.v12 = float(msg.data[1]) / 100.0
+                self.arm_feedback_new.socket_voltage.v5 = float(msg.data[2]) / 100.0
+                self.arm_feedback_new.socket_voltage.v3 = float(msg.data[3]) / 100.0
+                self.arm_feedback_new.socket_voltage.header.stamp = msg.header.stamp
+            case 55:  # Arm joint positions
+                angles = [angle / 10.0 for angle in msg.data]  # VicCAN sends deg*10
+                # Joint state publisher for URDF visualization
+                self.saved_joint_state.position[0] = math.radians(angles[0])  # Axis 0
+                self.saved_joint_state.position[1] = math.radians(angles[1])  # Axis 1
+                self.saved_joint_state.position[2] = math.radians(angles[2])  # Axis 2
+                self.saved_joint_state.position[3] = math.radians(angles[3])  # Axis 3
+                # Wrist is handled by digit feedback
+                self.saved_joint_state.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(self.saved_joint_state)
+            case 58:  # REV SPARK MAX position and velocity feedback
+                motorId = round(msg.data[0])
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.arm_feedback_new.axis1_motor
+                    case 2:
+                        motor = self.arm_feedback_new.axis2_motor
+                    case 3:
+                        motor = self.arm_feedback_new.axis3_motor
+                    case 4:
+                        motor = self.arm_feedback_new.axis0_motor
+
+                if motor:
+                    motor.position = float(msg.data[1])
+                    motor.velocity = float(msg.data[2])
+                    motor.header.stamp = msg.header.stamp
+
+                self.arm_feedback_pub_.publish(self.arm_feedback_new)
+            case 59:  # Arm joint velocities
+                velocities = [vel / 100.0 for vel in msg.data]  # VicCAN sends deg/s*100
+                self.saved_joint_state.velocity[0] = math.radians(
+                    velocities[0]
+                )  # Axis 0
+                self.saved_joint_state.velocity[1] = math.radians(
+                    velocities[1]
+                )  # Axis 1
+                self.saved_joint_state.velocity[2] = math.radians(
+                    velocities[2]
+                )  # Axis 2
+                self.saved_joint_state.velocity[3] = math.radians(
+                    velocities[3]
+                )  # Axis 3
+                # Wrist is handled by digit feedback
+                self.saved_joint_state.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(self.saved_joint_state)
+
+    def process_fromvic_digit(self, msg: VicCAN):
+        assert msg.mcu_name == "digit"
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in self.viccan_digit_msg_len_dict:
+            expected_len = self.viccan_digit_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
+
+        self.arm_feedback_new.header.stamp = msg.header.stamp
+
+        match msg.command_id:
+            case 54:  # Board voltages
+                self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
+                self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
+                self.arm_feedback_new.digit_voltage.header.stamp = msg.header.stamp
+            case 55:  # Arm joint positions
+                self.saved_joint_state.position[4] = math.radians(
+                    msg.data[0]
+                )  # Wrist roll
+                self.saved_joint_state.position[5] = math.radians(
+                    msg.data[1]
+                )  # Wrist yaw
+
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def publish_feedback(self):
         self.socket_pub.publish(self.arm_feedback)
         self.digit_pub.publish(self.digit_feedback)
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateAngleFeedback(self, msg: str):
         # Angle feedbacks,
         # split the msg.data by commas
@@ -263,19 +470,10 @@ class SerialRelay(Node):
             self.arm_feedback.axis1_angle = angles[1]
             self.arm_feedback.axis2_angle = angles[2]
             self.arm_feedback.axis3_angle = angles[3]
-
-            # Joint state publisher for URDF visualization
-            self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
-            self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
-            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
-            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
-            # Wrist is handled by digit feedback
-            self.joint_state.header.stamp = self.get_clock().now().to_msg()
-            self.joint_state_pub.publish(self.joint_state)
-
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateBusVoltage(self, msg: str):
         # Bus Voltage feedbacks
         parts = msg.split(",")
@@ -290,6 +488,7 @@ class SerialRelay(Node):
         else:
             self.get_logger().info("Invalid voltage feedback input format")
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateMotorFeedback(self, msg: str):
         parts = str(msg.strip()).split(",")
         motorId = round(float(parts[3]))
@@ -313,38 +512,28 @@ class SerialRelay(Node):
             self.arm_feedback.axis0_voltage = voltage
             self.arm_feedback.axis0_current = current
 
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        # return glob.glob("/dev/tty[A-Za-z]*")
 
-    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 exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
 
 
 def main(args=None):
     rclpy.init(args=args)
-    sys.excepthook = myexcepthook
 
-    global serial_pub
-    serial_pub = SerialRelay()
-    serial_pub.run()
+    # Catch termination signals and exit cleanly
+    signal.signal(signal.SIGTERM, exit_handler)
+
+    arm_node = ArmNode()
+
+    try:
+        rclpy.spin(arm_node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":
-    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
     main()

src/arm_pkg/package.xml

@@ -3,14 +3,15 @@
 <package format="3">
   <name>arm_pkg</name>
   <version>1.0.0</version>
-  <description>Core arm package which handles ROS2 commnuication.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <description>Relays topics related to Arm between VicCAN (through Anchor) and basestation.</description>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-numpy</depend>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.py

@@ -1,27 +1,22 @@
-from setuptools import find_packages, setup
-import os
-from glob import glob
+from setuptools import setup
 
 package_name = "arm_pkg"
 
 setup(
     name=package_name,
     version="1.0.0",
-    packages=find_packages(exclude=["test"]),
+    packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="tristan",
-    maintainer_email="tristanmcginnis26@gmail.com",
-    description="TODO: Package description",
-    license="All Rights Reserved",
+    maintainer="David Sharpe",
+    maintainer_email="ds0196@uah.edu",
+    description="Relays topics related to Arm between VicCAN (through Anchor) and basestation.",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
-            "arm = arm_pkg.arm_node:main",
-            "headless = arm_pkg.arm_headless:main",
-        ],
+        "console_scripts": ["arm = arm_pkg.arm_node:main"],
     },
 )

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
         )
 
-        self.anchor_sub = self.create_subscription(
-            String, "/anchor/bio/feedback", self.anchor_feedback, 10
-        )
-        self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        # Create a publisher for telemetry
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
 
-        # Messages
-        self.citadel_sub = self.create_subscription(
-            CitadelControl,
-            "/bio/control/citadel",
-            self.citadel_callback,
-            10,
-        )
+        # 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)
 
-        self.faerie_sub = self.create_subscription(
-            FaerieControl,
-            "/bio/control/faerie",
-            self.faerie_callback,
-            10,
-        )
+        self.bio_feedback = BioFeedback()
 
-        # 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
-        )
+        # 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")
 
-        # Actions
-        self._action_server = ActionServer(
-            self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum
-        )
+                    # 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
+
+            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():
-                time.sleep(0.1)
+                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
-        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
+        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)
+        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):

src/bio_pkg/package.xml

@@ -2,14 +2,16 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>bio_pkg</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <version>1.0.0</version>
+  <description>Biosensor package to handle command interpretation and embedded interfacing.</description>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_depend>
+
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/bio_pkg
 [install]
 install_scripts=$base/lib/bio_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/bio_pkg/setup.py

@@ -14,8 +14,8 @@ setup(
     zip_safe=True,
     maintainer="tristan",
     maintainer_email="tristanmcginnis26@gmail.com",
-    description="TODO: Package description",
-    license="TODO: License declaration",
+    description="Relays topics related to Biosensor between VicCAN (through Anchor) and basestation.",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["bio = bio_pkg.bio_node:main"],
     },

src/core_pkg/core_pkg/core_headless.py

@@ -1,112 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-import pygame
-
-import time
-
-import serial
-import sys
-import threading
-import glob
-import os
-
-import importlib
-from std_msgs.msg import String
-from astra_msgs.msg import CoreControl
-
-
-os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
-os.environ["SDL_AUDIODRIVER"] = (
-    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
-)
-
-
-max_speed = 90  # Max speed as a duty cycle percentage (1-100)
-
-
-class Headless(Node):
-    def __init__(self):
-        # Initialize pygame first
-        pygame.init()
-        pygame.joystick.init()
-
-        # Wait for a gamepad to be connected
-        self.gamepad = None
-        print("Waiting for gamepad connection...")
-        while pygame.joystick.get_count() == 0:
-            # Process any pygame events to keep it responsive
-            for event in pygame.event.get():
-                if event.type == pygame.QUIT:
-                    pygame.quit()
-                    sys.exit(0)
-            time.sleep(1.0)  # Check every second
-            print("No gamepad found. Waiting...")
-
-        # Initialize the gamepad
-        self.gamepad = pygame.joystick.Joystick(0)
-        self.gamepad.init()
-        print(f"Gamepad Found: {self.gamepad.get_name()}")
-
-        # Now initialize the ROS2 node
-        super().__init__("core_headless")
-        self.create_timer(0.15, self.send_controls)
-        self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
-        self.lastMsg = (
-            String()
-        )  # Used to ignore sending controls repeatedly when they do not change
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                self.send_controls()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def send_controls(self):
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                sys.exit(0)
-
-        # Check if controller is still connected
-        if pygame.joystick.get_count() == 0:
-            print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
-            input = CoreControl()
-            input.left_stick = 0
-            input.right_stick = 0
-            input.max_speed = 0
-            self.publisher.publish(input)
-            self.get_logger().info("Final stop command sent. Shutting down.")
-            # Clean up
-            pygame.quit()
-            sys.exit(0)
-
-        input = CoreControl()
-        input.max_speed = max_speed
-        input.right_stick = -1 * round(self.gamepad.get_axis(4), 2)  # right y-axis
-        if self.gamepad.get_axis(5) > 0:
-            input.left_stick = input.right_stick
-        else:
-            input.left_stick = -1 * round(self.gamepad.get_axis(1), 2)  # left y-axis
-
-        output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
-        self.get_logger().info(f"[Ctrl] {output}")
-        self.publisher.publish(input)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = Headless()
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

src/core_pkg/core_pkg/core_node.py

@@ -1,20 +1,16 @@
-import rclpy
-from rclpy.node import Node
-from rclpy import qos
-from rclpy.duration import Duration
-from std_srvs.srv import Empty
-
-import signal
-import time
-import atexit
-
-import serial
-import os
 import sys
-import threading
-import glob
+import signal
+from typing import Literal, cast
 from scipy.spatial.transform import Rotation
 from math import copysign, pi
+from warnings import deprecated
+from os import getenv
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
+from rclpy import qos
+from rclpy.duration import Duration
 
 from std_msgs.msg import String, Header
 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
@@ -23,225 +19,192 @@ from astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
 from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
 
 
-serial_pub = None
-thread = None
-
 CORE_WHEELBASE = 0.836  # meters
 CORE_WHEEL_RADIUS = 0.171  # meters
-CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
+CORE_GEAR_RATIO = 100.0  # Clucky: 100:1
+
+# TODO: update core_description or add testbed_description
+TESTBED_WHEELBASE = 0.368  # meters
+TESTBED_WHEEL_RADIUS = 0.108  # meters
+TESTBED_GEAR_RATIO = 64  # Testbed: 64:1
 
 control_qos = qos.QoSProfile(
-    # history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    # durability=qos.QoSDurabilityPolicy.VOLATILE,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,  # Best Effort subscribers are still compatible with Reliable publishers
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
     # deadline=Duration(seconds=1),
     # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
     # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
     # liveliness_lease_duration=Duration(seconds=5),
 )
 
-# Used to verify the length of an incoming VicCAN feedback message
-# Key is VicCAN command_id, value is expected length of data list
-viccan_msg_len_dict = {
-    48: 1,
-    49: 1,
-    50: 2,
-    51: 4,
-    52: 4,
-    53: 4,
-    54: 4,
-    56: 4,  # really 3, but viccan
-    58: 4,  # ditto
-}
 
+class CoreNode(Node):
+    """Relay between Anchor and Basestation/Headless/Moveit2 for Core related topics."""
+
+    # Used to verify the length of an incoming VicCAN feedback message
+    # Key is VicCAN command_id, value is expected length of data list
+    viccan_msg_len_dict = {
+        48: 1,
+        49: 1,
+        50: 2,
+        51: 4,
+        52: 4,
+        53: 4,
+        54: 4,
+        56: 4,  # really 3, but viccan
+        58: 4,  # ditto
+    }
+    rover_platform: Literal["core", "testbed"]
 
-class SerialRelay(Node):
     def __init__(self):
-        # Initalize node with name
         super().__init__("core_node")
 
-        # Launch mode -- anchor vs core
-        self.declare_parameter("launch_mode", "core")
-        self.launch_mode = self.get_parameter("launch_mode").value
-        self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
+        self.get_logger().info(f"core launch_mode is: anchor")
 
         ##################################################
-        # Topics
+        # Parameters
+
+        self.declare_parameter("use_ros2_control", False)
+        self.use_ros2_control = (
+            self.get_parameter("use_ros2_control").get_parameter_value().bool_value
+        )
+
+        self.declare_parameter("rover_platform", "auto")
+        rover_platform = (
+            self.get_parameter("rover_platform").get_parameter_value().string_value
+        )
+        if rover_platform == "auto":
+            self.get_logger().info(
+                "rover_platform parameter is unset, falling back to environment variable"
+            )
+            rover_platform = getenv("ROVER_PLATFORM", "auto")
+        if rover_platform not in ("core", "testbed"):  # make sure we have a valid value
+            raise ValueError("rover platform must be either 'core' or 'testbed'.")
+        else:
+            self.rover_platform = cast(Literal["core", "testbed"], rover_platform)
+
+        if self.rover_platform == "testbed":
+            global TESTBED_WHEELBASE, TESTBED_WHEEL_RADIUS, TESTBED_GEAR_RATIO
+            self.wheelbase = TESTBED_WHEELBASE
+            self.wheel_radius = TESTBED_WHEEL_RADIUS
+            self.gear_ratio = TESTBED_GEAR_RATIO
+        else:  # default in case of unset or invalid environment variable
+            global CORE_WHEELBASE, CORE_WHEEL_RADIUS, CORE_GEAR_RATIO
+            self.wheelbase = CORE_WHEELBASE
+            self.wheel_radius = CORE_WHEEL_RADIUS
+            self.gear_ratio = CORE_GEAR_RATIO
+
+        ##################################################
+        # Old Topics
+
+        self.anchor_sub = self.create_subscription(
+            String, "/anchor/core/feedback", self.anchor_feedback, 10
+        )
+        self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+
+        if not self.use_ros2_control:
+            # /core/control
+            self.control_sub = self.create_subscription(
+                CoreControl, "/core/control", self.send_controls, 10
+            )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
+        # /core/feedback
+        self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
+        self.core_feedback = CoreFeedback()
+
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
+
+        ##################################################
+        # New Topics
 
         # Anchor
-        if self.launch_mode == "anchor":
-            self.anchor_fromvic_sub_ = self.create_subscription(
-                VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
-            )
-            self.anchor_tovic_pub_ = self.create_publisher(
-                VicCAN, "/anchor/to_vic/relay", 20
-            )
 
-            self.anchor_sub = self.create_subscription(
-                String, "/anchor/core/feedback", self.anchor_feedback, 10
-            )
-            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        self.anchor_fromvic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
+        )
+        self.anchor_tovic_pub_ = self.create_publisher(
+            VicCAN, "/anchor/to_vic/relay", 20
+        )
 
         # Control
 
-        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        self.cmd_vel_sub_ = self.create_subscription(
-            TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
-        )
-        # manual twist -- [-1, 1] rather than real units
-        self.twist_man_sub_ = self.create_subscription(
-            Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
-        )
-        # manual flags -- brake mode and max duty cycle
-        self.control_state_sub_ = self.create_subscription(
-            CoreCtrlState,
-            "/core/control/state",
-            self.control_state_callback,
-            qos_profile=control_qos,
-        )
-        self.twist_max_duty = (
-            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
-        )
+        if self.use_ros2_control:
+            # Joint state control for topic-based controller
+            self.joint_command_sub_ = self.create_subscription(
+                JointState, "/core/joint_commands", self.joint_command_callback, 2
+            )
+        else:
+            # manual twist -- [-1, 1] rather than real units
+            # TODO: change topic to '/core/control/twist'
+            self.twist_man_sub_ = self.create_subscription(
+                Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
+            )
+            # manual flags -- brake mode and max duty cycle
+            self.control_state_sub_ = self.create_subscription(
+                CoreCtrlState,
+                "/core/control/state",
+                self.control_state_callback,
+                qos_profile=control_qos,
+            )
+            self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
 
         # Feedback
 
-        # Consolidated and organized core feedback
+        # Consolidated and organized main core feedback
+        # TODO: change topic to something like '/core/feedback/main'
         self.feedback_new_pub_ = self.create_publisher(
             NewCoreFeedback,
             "/core/feedback_new",
             qos_profile=qos.qos_profile_sensor_data,
         )
+
+        # Joint states for topic-based controller
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        # IMU (embedded BNO-055)
+        self.imu_pub_ = self.create_publisher(
+            Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        # GPS (embedded u-blox M9N)
+        self.gps_pub_ = self.create_publisher(
+            NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        # Barometer (embedded BMP-388)
+        self.baro_pub_ = self.create_publisher(
+            Barometer, "/core/feedback/baro", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        ###################################################
+        # Saved state
+
+        # Main Core feedback
         self.feedback_new_state = NewCoreFeedback()
         self.feedback_new_state.fl_motor.id = 1
         self.feedback_new_state.bl_motor.id = 2
         self.feedback_new_state.fr_motor.id = 3
         self.feedback_new_state.br_motor.id = 4
-        self.telemetry_pub_timer = self.create_timer(
-            1.0, self.publish_feedback
-        )  # TODO: not sure about this
-        # Joint states for topic-based controller
-        self.joint_state_pub_ = self.create_publisher(
-            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
-        )
-        # IMU (embedded BNO-055)
-        self.imu_pub_ = self.create_publisher(
-            Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
-        )
+
+        # IMU
         self.imu_state = Imu()
         self.imu_state.header.frame_id = "core_bno055"
-        # GPS (embedded u-blox M9N)
-        self.gps_pub_ = self.create_publisher(
-            NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
-        )
+
+        # GPS
         self.gps_state = NavSatFix()
         self.gps_state.header.frame_id = "core_gps_antenna"
         self.gps_state.status.service = NavSatStatus.SERVICE_GPS
         self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
         self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
-        # Barometer (embedded BMP-388)
-        self.baro_pub_ = self.create_publisher(
-            Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
-        )
+
+        # Barometer
         self.baro_state = Barometer()
         self.baro_state.header.frame_id = "core_bmp388"
 
-        # Old
-
-        # /core/control
-        self.control_sub = self.create_subscription(
-            CoreControl, "/core/control", self.send_controls, 10
-        )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
-        # /core/feedback
-        self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
-        self.core_feedback = CoreFeedback()
-        # Debug
-        self.debug_pub = self.create_publisher(String, "/core/debug", 10)
-        self.ping_service = self.create_service(
-            Empty, "/astra/core/ping", self.ping_callback
-        )
-
-        ##################################################
-        # Find microcontroller (Non-anchor only)
-
-        # Core (non-anchor) specific
-        if self.launch_mode == "core":
-            # Loop through all serial devices on the computer to check for the MCU
-            self.port = None
-            ports = SerialRelay.list_serial_ports()
-            for i in range(2):
-                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("\n")  # type: ignore
-
-                        # if pong is in response, then we are talking with the MCU
-                        if b"pong" in response:
-                            self.port = port
-                            self.get_logger().info(f"Found MCU at {self.port}!")
-                            self.get_logger().info(f"Enabling Relay Mode")
-                            ser.write(b"can_relay_mode,on\n")
-                            break
-                    except:
-                        pass
-                if self.port is not None:
-                    break
-
-            if self.port is None:
-                self.get_logger().info("Unable to find MCU...")
-                time.sleep(1)
-                sys.exit(1)
-
-            self.ser = serial.Serial(self.port, 115200)
-            atexit.register(self.cleanup)
-        # end __init__()
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        global thread
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                if self.launch_mode == "core":
-                    self.read_MCU()  # Check the MCU for updates
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def read_MCU(self):  # NON-ANCHOR SPECIFIC
-        try:
-            output = str(self.ser.readline(), "utf8")
-
-            if output:
-                # All output over debug temporarily
-                print(f"[MCU] {output}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-                return
-        except serial.SerialException as e:
-            print(f"SerialException: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-        except TypeError as e:
-            print(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-        except Exception as e:
-            print(f"Exception: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def scale_duty(self, value: float, max_speed: float):
         leftMin = -1
         leftMax = 1
@@ -258,6 +221,7 @@ class SerialRelay(Node):
         # Convert the 0-1 range into a value in the right range.
         return str(rightMin + (valueScaled * rightSpan))
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_controls(self, msg: CoreControl):
         if msg.turn_to_enable:
             command = (
@@ -283,17 +247,42 @@ class SerialRelay(Node):
 
         # print(f"[Sys] Relaying: {command}")
 
-    def cmd_vel_callback(self, msg: TwistStamped):
-        linear = msg.twist.linear.x
-        angular = -msg.twist.angular.z
+    def joint_command_callback(self, msg: JointState):
+        # So... topic based control node publishes JointState messages over /joint_commands
+        #  with len(msg.name) == 5 and len(msg.velocity) == 4... all 5 non-fixed joints
+        #  are included in msg.name, but ig it is implied that msg.velocity only
+        #  includes velocities for the commanded joints (ros__parameters.joints).
+        # So, this will be much more hacky and less adaptable than I would like it to be.
+        if (
+            len(msg.name) != (4 if self.rover_platform == "testbed" else 5)
+            or len(msg.velocity) != 4
+            or len(msg.position) != 0
+        ):
+            self.get_logger().warning(
+                f"Received joint control message with unexpected number of joints. Ignoring."
+            )
+            return
+        if msg.name[-4:] != [  # type: ignore
+            "bl_wheel_joint",
+            "br_wheel_joint",
+            "fl_wheel_joint",
+            "fr_wheel_joint",
+        ]:
+            self.get_logger().warning(
+                f"Received joint control message with unexpected name[]. Ignoring."
+            )
+            return
 
-        vel_left_rads = (linear - (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-        vel_right_rads = (linear + (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
+        (bl_vel, br_vel, fl_vel, fr_vel) = msg.velocity
 
-        vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-        vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
+        bl_rpm = radps_to_rpm(bl_vel) * self.gear_ratio
+        br_rpm = radps_to_rpm(br_vel) * self.gear_ratio
+        fl_rpm = radps_to_rpm(fl_vel) * self.gear_ratio
+        fr_rpm = radps_to_rpm(fr_vel) * self.gear_ratio
 
-        self.send_viccan(20, [vel_left_rpm, vel_right_rpm])
+        self.send_viccan(
+            20, [fl_rpm, bl_rpm, fr_rpm, br_rpm]
+        )  # order expected by embedded
 
     def twist_man_callback(self, msg: Twist):
         linear = msg.linear.x  # [-1 1] for forward/back from left stick y
@@ -334,15 +323,9 @@ class SerialRelay(Node):
         # Max duty cycle
         self.twist_max_duty = msg.max_duty  # twist_man_callback will handle this
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_cmd(self, msg: str):
-        if self.launch_mode == "anchor":
-            # self.get_logger().info(f"[Core to Anchor Relay] {msg}")
-            output = String()  # Convert to std_msg string
-            output.data = msg
-            self.anchor_pub.publish(output)
-        elif self.launch_mode == "core":
-            self.get_logger().info(f"[Core to MCU] {msg}")
-            self.ser.write(bytes(msg, "utf8"))
+        self.anchor_pub.publish(String(data=msg))  # Publish to anchor for relay
 
     def send_viccan(self, cmd_id: int, data: list[float]):
         self.anchor_tovic_pub_.publish(
@@ -354,6 +337,7 @@ class SerialRelay(Node):
             )
         )
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
@@ -423,14 +407,16 @@ class SerialRelay(Node):
         # skill diff if not
 
         # Check message len to prevent crashing on bad data
-        if msg.command_id in viccan_msg_len_dict:
-            expected_len = viccan_msg_len_dict[msg.command_id]
+        if msg.command_id in self.viccan_msg_len_dict:
+            expected_len = self.viccan_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
 
+        self.feedback_new_state.header.stamp = msg.header.stamp
+
         match msg.command_id:
             # GNSS
             case 48:  # GNSS Latitude
@@ -457,6 +443,7 @@ class SerialRelay(Node):
                 self.imu_state.linear_acceleration.x = float(msg.data[0])
                 self.imu_state.linear_acceleration.y = float(msg.data[1])
                 self.imu_state.linear_acceleration.z = float(msg.data[2])
+                self.feedback_new_state.orientation = float(msg.data[3])
                 # Deal with quaternion
                 r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
                 q = r.as_quat()
@@ -501,6 +488,7 @@ class SerialRelay(Node):
                 self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
                 self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
                 self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
+                self.feedback_new_state.board_voltage.header.stamp = msg.header.stamp
             # Baro
             case 56:  # BMP temperature, altitude, pressure
                 self.baro_state.temperature = float(msg.data[0])
@@ -513,14 +501,12 @@ class SerialRelay(Node):
                 motorId = round(float(msg.data[0]))
                 position = float(msg.data[1])
                 velocity = float(msg.data[2])
-                joint_state_msg = (
-                    JointState()
-                )  # TODO: not sure if all motors should be in each message or not
+                joint_state_msg = JointState()
                 joint_state_msg.position = [
-                    position * (2 * pi) / CORE_GEAR_RATIO
+                    position * (2 * pi) / self.gear_ratio
                 ]  # revolutions to radians
                 joint_state_msg.velocity = [
-                    velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
+                    velocity * (2 * pi / 60.0) / self.gear_ratio
                 ]  # RPM to rad/s
 
                 motor: RevMotorState | None = None
@@ -528,52 +514,42 @@ class SerialRelay(Node):
                 match motorId:
                     case 1:
                         motor = self.feedback_new_state.fl_motor
-                        joint_state_msg.name = ["fl_motor_joint"]
+                        joint_state_msg.name = ["fl_wheel_joint"]
                     case 2:
                         motor = self.feedback_new_state.bl_motor
-                        joint_state_msg.name = ["bl_motor_joint"]
+                        joint_state_msg.name = ["bl_wheel_joint"]
                     case 3:
                         motor = self.feedback_new_state.fr_motor
-                        joint_state_msg.name = ["fr_motor_joint"]
+                        joint_state_msg.name = ["fr_wheel_joint"]
                     case 4:
                         motor = self.feedback_new_state.br_motor
-                        joint_state_msg.name = ["br_motor_joint"]
+                        joint_state_msg.name = ["br_wheel_joint"]
                     case _:
                         self.get_logger().warning(
                             f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
                         )
                         return
 
+                if motor:
+                    motor.position = position
+                    motor.velocity = velocity
+
+                # make the fucking shit work
+                if self.rover_platform == "clucky":
+                    joint_state_msg.name.append("left_suspension_joint")
+                    joint_state_msg.position.append(0.0)
+                    joint_state_msg.velocity.append(0.0)
+
                 joint_state_msg.header.stamp = msg.header.stamp
                 self.joint_state_pub_.publish(joint_state_msg)
             case _:
                 return
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def publish_feedback(self):
         # self.get_logger().info(f"[Core] {self.core_feedback}")
         self.feedback_pub.publish(self.core_feedback)
 
-    def ping_callback(self, request, response):
-        return response
-
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-
-    def cleanup(self):
-        print("Cleaning up before terminating...")
-        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 map_range(
     value: float, in_min: float, in_max: float, out_min: float, out_max: float
@@ -581,19 +557,31 @@ def map_range(
     return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
 
+def radps_to_rpm(radps: float):
+    return radps * 60 / (2 * pi)
+
+
+def exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
+
+
 def main(args=None):
     rclpy.init(args=args)
-    sys.excepthook = myexcepthook
 
-    global serial_pub
+    # Catch termination signals and exit cleanly
+    signal.signal(signal.SIGTERM, exit_handler)
 
-    serial_pub = SerialRelay()
-    serial_pub.run()
+    core_node = CoreNode()
+
+    try:
+        rclpy.spin(core_node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":
-    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
     main()

src/core_pkg/core_pkg/core_ptz.py

@@ -262,7 +262,7 @@ class PtzNode(Node):
             f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
         )
         self.debug_pub.publish(msg)
-        self.get_logger().info(message_text)
+        self.get_logger().debug(message_text)
 
     def run_async_func(self, coro):
         """Run an async function in the event loop."""

src/core_pkg/package.xml

@@ -3,15 +3,17 @@
 <package format="3">
   <name>core_pkg</name>
   <version>1.0.0</version>
-  <description>Core rover control package to handle command interpretation and embedded interfacing.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <description>Relays topics related to Core between VicCAN (through Anchor) and basestation.</description>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-scipy</depend>
-  <depend>python-crccheck-pip</depend>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+  <exec_depend>python-crccheck-pip</exec_depend>
+
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/core_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/core_pkg
 [install]
 install_scripts=$base/lib/core_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/core_pkg/setup.py

@@ -4,7 +4,7 @@ package_name = "core_pkg"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -12,14 +12,13 @@ setup(
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="tristan",
-    maintainer_email="tristanmcginnis26@gmail.com",
-    description="Core rover control package to handle command interpretation and embedded interfacing.",
-    license="All Rights Reserved",
+    maintainer="David Sharpe",
+    maintainer_email="ds0196@uah.edu",
+    description="Relays topics related to Core between VicCAN (through Anchor) and basestation.",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": [
             "core = core_pkg.core_node:main",
-            "headless = core_pkg.core_headless:main",
             "ptz = core_pkg.core_ptz:main",
         ],
     },

src/headless_pkg/package.xml

@@ -3,14 +3,15 @@
 <package format="3">
   <name>headless_pkg</name>
   <version>1.0.0</version>
-  <description>Headless rover control package to handle command interpretation and embedded interfacing.</description>
+  <description>Provides headless rover control, similar to Basestation.</description>
   <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
-  <depend>python3-pygame</depend>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_depend>
+  <exec_depend>python3-pygame</exec_depend>
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/headless_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/headless_pkg
 [install]
 install_scripts=$base/lib/headless_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/headless_pkg/setup.py

@@ -14,11 +14,9 @@ setup(
     zip_safe=True,
     maintainer="David Sharpe",
     maintainer_email="ds0196@uah.edu",
-    description="Headless rover control package to handle command interpretation and embedded interfacing.",
-    license="All Rights Reserved",
+    description="Provides headless rover control, similar to Basestation.",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
-            "headless_full = src.headless_node:main",
-        ],
+        "console_scripts": ["headless_full = src.headless_node:main"],
     },
 )

src/headless_pkg/src/headless_node.py

@@ -6,20 +6,27 @@ from rclpy.duration import Duration
 
 import signal
 import time
-import atexit
 
 import os
 import sys
-import threading
-import glob
 import pwd
 import grp
 from math import copysign
 
-from std_msgs.msg import String
-from geometry_msgs.msg import Twist
+from std_srvs.srv import Trigger
+from std_msgs.msg import Header
+from geometry_msgs.msg import Twist, TwistStamped
+from control_msgs.msg import JointJog
 from astra_msgs.msg import CoreControl, ArmManual, BioControl
-from astra_msgs.msg import CoreCtrlState
+from astra_msgs.msg import CoreCtrlState, ArmCtrlState
+
+import warnings
+
+# Literally headless
+warnings.filterwarnings(
+    "ignore",
+    message="Your system is avx2 capable but pygame was not built with support for it.",
+)
 
 import pygame
 
@@ -34,26 +41,44 @@ CORE_STOP_TWIST_MSG = Twist()  # "
 ARM_STOP_MSG = ArmManual()  # "
 BIO_STOP_MSG = BioControl()  # "
 
+
 control_qos = qos.QoSProfile(
-    # history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    # durability=qos.QoSDurabilityPolicy.VOLATILE,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
     # deadline=Duration(seconds=1),
     # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
     # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
     # liveliness_lease_duration=Duration(seconds=5),
 )
 
-CORE_MODE = "twist"  # "twist" or "duty"
+
+STICK_DEADZONE = float(os.getenv("STICK_DEADZONE", "0.05"))
+ARM_DEADZONE = float(os.getenv("ARM_DEADZONE", "0.2"))
 
 
 class Headless(Node):
+    # Every non-fixed joint defined in Arm's URDF
+    # Used for JointState and JointJog messsages
+    all_joint_names = [
+        "axis_0_joint",
+        "axis_1_joint",
+        "axis_2_joint",
+        "axis_3_joint",
+        "wrist_yaw_joint",
+        "wrist_roll_joint",
+        "ef_gripper_left_joint",
+    ]
+
     def __init__(self):
         # Initialize pygame first
         pygame.init()
         pygame.joystick.init()
-        super().__init__("headless")
+        super().__init__("headless_node")
+
+        ##################################################
+        # Preamble
 
         # Wait for anchor to start
         pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
@@ -105,37 +130,144 @@ class Headless(Node):
                 break
             id += 1
 
-        self.create_timer(0.15, self.send_controls)
+        ##################################################
+        # Parameters
 
-        self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
-        self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
-        self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
+        self.declare_parameter("use_old_topics", True)
+        self.use_old_topics = (
+            self.get_parameter("use_old_topics").get_parameter_value().bool_value
+        )
 
-        self.core_twist_pub_ = self.create_publisher(
-            Twist, "/core/twist", qos_profile=control_qos
+        self.declare_parameter("use_cmd_vel", False)
+        self.use_cmd_vel = (
+            self.get_parameter("use_cmd_vel").get_parameter_value().bool_value
         )
-        self.core_state_pub_ = self.create_publisher(
-            CoreCtrlState, "/core/control/state", qos_profile=control_qos
+
+        self.declare_parameter("use_bio", False)
+        self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
+
+        self.declare_parameter("use_arm_ik", False)
+        self.use_arm_ik = (
+            self.get_parameter("use_arm_ik").get_parameter_value().bool_value
         )
+        # NOTE: only applicable if use_old_topics == True
+        self.declare_parameter("use_new_arm_manual_scheme", True)
+        self.use_new_arm_manual_scheme = (
+            self.get_parameter("use_new_arm_manual_scheme")
+            .get_parameter_value()
+            .bool_value
+        )
+
+        # Check parameter validity
+        if self.use_cmd_vel:
+            self.get_logger().info("Using cmd_vel for core control")
+            global CORE_MODE
+            CORE_MODE = "twist"
+        else:
+            self.get_logger().info("Using astra_msgs/CoreControl for core control")
+
+        if self.use_arm_ik and self.use_old_topics:
+            self.get_logger().fatal("Old topics do not support arm IK control.")
+            sys.exit(1)
+        if not self.use_new_arm_manual_scheme and not self.use_old_topics:
+            self.get_logger().warn(
+                f"New arm manual does not support old control scheme. Defaulting to new scheme."
+            )
 
         self.ctrl_mode = "core"  # Start in core mode
         self.core_brake_mode = False
         self.core_max_duty = 0.5  # Default max duty cycle (walking speed)
+        self.arm_brake_mode = False
+        self.arm_laser = False
+
+        ##################################################
+        # Old Topics
+
+        if self.use_old_topics:
+            self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
+            self.arm_publisher = self.create_publisher(
+                ArmManual, "/arm/control/manual", 2
+            )
+            self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
+
+        ##################################################
+        # New Topics
+
+        if not self.use_old_topics:
+            self.core_twist_pub_ = self.create_publisher(
+                Twist, "/core/twist", qos_profile=control_qos
+            )
+            self.core_cmd_vel_pub_ = self.create_publisher(
+                TwistStamped, "/diff_controller/cmd_vel", qos_profile=control_qos
+            )
+            self.core_state_pub_ = self.create_publisher(
+                CoreCtrlState, "/core/control/state", qos_profile=control_qos
+            )
+
+            self.arm_manual_pub_ = self.create_publisher(
+                JointJog, "/arm/control/joint_jog", qos_profile=control_qos
+            )
+            self.arm_state_pub_ = self.create_publisher(
+                ArmCtrlState, "/arm/control/state", qos_profile=control_qos
+            )
+
+            self.arm_ik_twist_publisher = self.create_publisher(
+                TwistStamped, "/servo_node/delta_twist_cmds", qos_profile=control_qos
+            )
+            self.arm_ik_jointjog_publisher = self.create_publisher(
+                JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos
+            )
+
+            # TODO: add new bio topics
+
+        ##################################################
+        # Timers
+
+        self.create_timer(0.1, self.send_controls)
+
+        ##################################################
+        # Services
+
+        # If using IK control, we have to "start" the servo node to enable it to accept commands
+        self.servo_start_client = None
+        if self.use_arm_ik:
+            self.get_logger().info("Starting servo node for IK control...")
+            self.servo_start_client = self.create_client(
+                Trigger, "/servo_node/start_servo"
+            )
+            timeout_counter = 0
+            while not self.servo_start_client.wait_for_service(timeout_sec=1.0):
+                self.get_logger().info("Waiting for servo_node/start_servo service...")
+                timeout_counter += 1
+                if timeout_counter >= 10:
+                    self.get_logger().error(
+                        "Servo's start service not available. IK control will not work."
+                    )
+                    break
+            if self.servo_start_client.service_is_ready():
+                self.servo_start_client.call_async(Trigger.Request())
 
         # Rumble when node is ready (returns False if rumble not supported)
         self.gamepad.rumble(0.7, 0.8, 150)
 
-    def run(self):
-        # This thread makes all the update processes run in the background
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
+        # Added so you can tell when it starts running after changing the constant logging to debug from info
+        self.get_logger().info("Defaulting to Core mode. Ready.")
 
-        try:
-            while rclpy.ok():
-                self.send_controls()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
-        except KeyboardInterrupt:
-            sys.exit(0)
+    def stop_all(self):
+        if self.use_old_topics:
+            self.core_publisher.publish(CORE_STOP_MSG)
+            self.arm_publisher.publish(ARM_STOP_MSG)
+            self.bio_publisher.publish(BIO_STOP_MSG)
+        else:
+            if self.use_cmd_vel:
+                self.core_cmd_vel_pub_.publish(self.core_cmd_vel_stop_msg())
+            else:
+                self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
+            if self.use_arm_ik:
+                self.arm_ik_twist_publisher.publish(self.arm_ik_twist_stop_msg())
+            else:
+                self.arm_manual_pub_.publish(self.arm_manual_stop_msg())
+            # TODO: add bio here after implementing new topics
 
     def send_controls(self):
         """Read the gamepad state and publish control messages"""
@@ -147,10 +279,8 @@ class Headless(Node):
         # Check if controller is still connected
         if pygame.joystick.get_count() != self.num_gamepads:
             print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
-            self.core_publisher.publish(CORE_STOP_MSG)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            self.bio_publisher.publish(BIO_STOP_MSG)
+            # Stop the rover if controller disconnected
+            self.stop_all()
             self.get_logger().info("Final stop commands sent. Shutting down.")
             # Clean up
             pygame.quit()
@@ -166,20 +296,51 @@ class Headless(Node):
             new_ctrl_mode = "core"
 
         if new_ctrl_mode != self.ctrl_mode:
+            self.stop_all()
             self.gamepad.rumble(0.6, 0.7, 75)
             self.ctrl_mode = new_ctrl_mode
             self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
+            if self.ctrl_mode == "arm" and self.use_bio:
+                self.get_logger().warning("NOTE: Using bio instead of arm.")
 
-        # CORE
-        if self.ctrl_mode == "core" and CORE_MODE == "duty":
+        # Actually send the controls
+        if self.ctrl_mode == "core":
+            self.send_core()
+            if self.use_old_topics:
+                if self.use_bio:
+                    self.bio_publisher.publish(BIO_STOP_MSG)
+                else:
+                    self.arm_publisher.publish(ARM_STOP_MSG)
+            # New topics shouldn't need to constantly send zeroes imo
+        else:
+            if self.use_bio:
+                self.send_bio()
+            else:
+                self.send_arm()
+            if self.use_old_topics:
+                self.core_publisher.publish(CORE_STOP_MSG)
+            # Ditto
+
+    def send_core(self):
+        # Collect controller state
+        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
+        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
+        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
+        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
+        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
+        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
+        button_a = self.gamepad.get_button(0)
+        button_b = self.gamepad.get_button(1)
+        button_x = self.gamepad.get_button(2)
+        button_y = self.gamepad.get_button(3)
+        left_bumper = self.gamepad.get_button(4)
+        right_bumper = self.gamepad.get_button(5)
+        dpad_input = self.gamepad.get_hat(0)
+
+        if self.use_old_topics:
             input = CoreControl()
             input.max_speed = 90
 
-            # Collect controller state
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            right_stick_y = deadzone(self.gamepad.get_axis(4))
-            right_trigger = deadzone(self.gamepad.get_axis(5))
-
             # Right wheels
             input.right_stick = float(round(-1 * right_stick_y, 2))
 
@@ -194,31 +355,30 @@ class Headless(Node):
             self.get_logger().info(f"[Ctrl] {output}")
 
             self.core_publisher.publish(input)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            # self.bio_publisher.publish(BIO_STOP_MSG)
 
-        elif self.ctrl_mode == "core" and CORE_MODE == "twist":
-            input = Twist()
-
-            # Collect controller state
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
-            button_a = self.gamepad.get_button(0)
-            left_bumper = self.gamepad.get_button(4)
-            right_bumper = self.gamepad.get_button(5)
+        else:  # New topics
+            twist = Twist()
 
             # Forward/back and Turn
-            input.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * copysign(
+            twist.linear.x = -1.0 * left_stick_y
+            twist.angular.z = -1.0 * copysign(
                 right_stick_x**2, right_stick_x
             )  # Exponent for finer control (curve)
 
+            # This kinda looks dumb being seperate from the following block, but this
+            # maintains the separation between modifying the control message and sending it
+            if self.use_cmd_vel:
+                twist.linear.x *= 1.5
+                twist.angular.z *= 0.5
+
             # Publish
-            self.core_twist_pub_.publish(input)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            # self.bio_publisher.publish(BIO_STOP_MSG)
-            self.get_logger().info(
-                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
+            if self.use_cmd_vel:
+                header = Header(stamp=self.get_clock().now().to_msg())
+                self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist))
+            else:
+                self.core_twist_pub_.publish(twist)
+            self.get_logger().debug(
+                f"[Core Ctrl] Linear: {round(twist.linear.x, 2)}, Angular: {round(twist.angular.z, 2)}"
             )
 
             # Brake mode
@@ -241,91 +401,328 @@ class Headless(Node):
                 state_msg = CoreCtrlState()
                 state_msg.brake_mode = bool(self.core_brake_mode)
                 state_msg.max_duty = float(self.core_max_duty)
+
                 self.core_state_pub_.publish(state_msg)
                 self.get_logger().info(
                     f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
                 )
 
-        # ARM and BIO
-        if self.ctrl_mode == "arm":
+    def send_arm(self):
+        # Collect controller state
+        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
+        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
+        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
+        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
+        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
+        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
+        button_a = self.gamepad.get_button(0)
+        button_b = self.gamepad.get_button(1)
+        button_x = self.gamepad.get_button(2)
+        button_y = self.gamepad.get_button(3)
+        left_bumper = self.gamepad.get_button(4)
+        right_bumper = self.gamepad.get_button(5)
+        dpad_input = self.gamepad.get_hat(0)
+
+        # OLD MANUAL
+        # ==========
+
+        if not self.use_arm_ik and self.use_old_topics:
             arm_input = ArmManual()
 
-            # Collect controller state
-            left_stick_x = deadzone(self.gamepad.get_axis(0))
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            left_trigger = deadzone(self.gamepad.get_axis(2))
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
-            right_stick_y = deadzone(self.gamepad.get_axis(4))
-            right_trigger = deadzone(self.gamepad.get_axis(5))
-            right_bumper = self.gamepad.get_button(5)
-            dpad_input = self.gamepad.get_hat(0)
+            # OLD ARM MANUAL CONTROL SCHEME
+            if not self.use_new_arm_manual_scheme:
+                # EF Grippers
+                if left_trigger > 0 and right_trigger > 0:
+                    arm_input.gripper = 0
+                elif left_trigger > 0:
+                    arm_input.gripper = -1
+                elif right_trigger > 0:
+                    arm_input.gripper = 1
 
-            # EF Grippers
-            if left_trigger > 0 and right_trigger > 0:
-                arm_input.gripper = 0
-            elif left_trigger > 0:
-                arm_input.gripper = -1
-            elif right_trigger > 0:
-                arm_input.gripper = 1
+                # Axis 0
+                if dpad_input[0] == 1:
+                    arm_input.axis0 = 1
+                elif dpad_input[0] == -1:
+                    arm_input.axis0 = -1
 
-            # Axis 0
-            if dpad_input[0] == 1:
-                arm_input.axis0 = 1
-            elif dpad_input[0] == -1:
-                arm_input.axis0 = -1
+                if right_bumper:  # Control end effector
 
-            if right_bumper:  # Control end effector
+                    # Effector yaw
+                    if left_stick_x > 0:
+                        arm_input.effector_yaw = 1
+                    elif left_stick_x < 0:
+                        arm_input.effector_yaw = -1
 
-                # Effector yaw
-                if left_stick_x > 0:
-                    arm_input.effector_yaw = 1
-                elif left_stick_x < 0:
-                    arm_input.effector_yaw = -1
+                    # Effector roll
+                    if right_stick_x > 0:
+                        arm_input.effector_roll = 1
+                    elif right_stick_x < 0:
+                        arm_input.effector_roll = -1
 
-                # Effector roll
-                if right_stick_x > 0:
-                    arm_input.effector_roll = 1
-                elif right_stick_x < 0:
+                else:  # Control arm axis
+
+                    # Axis 1
+                    if abs(left_stick_x) > 0.15:
+                        arm_input.axis1 = round(left_stick_x)
+
+                    # Axis 2
+                    if abs(left_stick_y) > 0.15:
+                        arm_input.axis2 = -1 * round(left_stick_y)
+
+                    # Axis 3
+                    if abs(right_stick_y) > 0.15:
+                        arm_input.axis3 = -1 * round(right_stick_y)
+
+            # NEW ARM MANUAL CONTROL SCHEME
+            if self.use_new_arm_manual_scheme:
+                # Right stick: EF yaw and axis 3
+                # Left stick: axis 1 and 2
+                # D-pad: axis 0 and _
+                # Triggers: EF grippers
+                # Bumpers: EF roll
+                # A: brake
+                # B: linear actuator in
+                # X: _
+                # Y: linear actuator out
+
+                # Right stick: EF yaw and axis 3
+                arm_input.effector_yaw = stick_to_arm_direction(right_stick_x)
+                arm_input.axis3 = -1 * stick_to_arm_direction(right_stick_y)
+
+                # Left stick: axis 1 and 2
+                arm_input.axis1 = stick_to_arm_direction(left_stick_x)
+                arm_input.axis2 = -1 * stick_to_arm_direction(left_stick_y)
+
+                # D-pad: axis 0 and _
+                arm_input.axis0 = int(dpad_input[0])
+
+                # Triggers: EF Grippers
+                if left_trigger > 0 and right_trigger > 0:
+                    arm_input.gripper = 0
+                elif left_trigger > 0:
+                    arm_input.gripper = -1
+                elif right_trigger > 0:
+                    arm_input.gripper = 1
+
+                # Bumpers: EF roll
+                if left_bumper > 0 and right_bumper > 0:
+                    arm_input.effector_roll = 0
+                elif left_bumper > 0:
                     arm_input.effector_roll = -1
+                elif right_bumper > 0:
+                    arm_input.effector_roll = 1
 
-            else:  # Control arm axis
+                # A: brake
+                if button_a:
+                    arm_input.brake = True
 
-                # Axis 1
-                if abs(left_stick_x) > 0.15:
-                    arm_input.axis1 = round(left_stick_x)
+                # Y: linear actuator
+                if button_y and not button_b:
+                    arm_input.linear_actuator = 1
+                elif button_b and not button_y:
+                    arm_input.linear_actuator = -1
+                else:
+                    arm_input.linear_actuator = 0
 
-                # Axis 2
-                if abs(left_stick_y) > 0.15:
-                    arm_input.axis2 = -1 * round(left_stick_y)
+            self.arm_publisher.publish(arm_input)
 
-                # Axis 3
-                if abs(right_stick_y) > 0.15:
-                    arm_input.axis3 = -1 * round(right_stick_y)
+        # NEW MANUAL
+        # ==========
 
-            # BIO
+        elif not self.use_arm_ik and not self.use_old_topics:
+            arm_input = JointJog()
+            arm_input.header.frame_id = "base_link"
+            arm_input.header.stamp = self.get_clock().now().to_msg()
+            arm_input.joint_names = self.all_joint_names
+            arm_input.velocities = [0.0] * len(self.all_joint_names)
+
+            # Right stick: EF yaw and axis 3
+            # Left stick: axis 1 and 2
+            # D-pad: axis 0 and _
+            # Triggers: EF grippers
+            # Bumpers: EF roll
+            # A: brake
+            # B: linear actuator in
+            # X: _
+            # Y: linear actuator out
+
+            # Right stick: EF yaw and axis 3
+            arm_input.velocities[self.all_joint_names.index("wrist_yaw_joint")] = float(
+                stick_to_arm_direction(right_stick_x)
+            )
+            arm_input.velocities[self.all_joint_names.index("axis_3_joint")] = float(
+                stick_to_arm_direction(right_stick_y)
+            )
+
+            # Left stick: axis 1 and 2
+            arm_input.velocities[self.all_joint_names.index("axis_1_joint")] = float(
+                stick_to_arm_direction(left_stick_x)
+            )
+            arm_input.velocities[self.all_joint_names.index("axis_2_joint")] = float(
+                stick_to_arm_direction(left_stick_y)
+            )
+
+            # D-pad: axis 0 and _
+            arm_input.velocities[self.all_joint_names.index("axis_0_joint")] = float(
+                dpad_input[0]
+            )
+
+            # Triggers: EF Grippers
+            if left_trigger > 0 and right_trigger > 0:
+                arm_input.velocities[
+                    self.all_joint_names.index("ef_gripper_left_joint")
+                ] = 0.0
+            elif left_trigger > 0:
+                arm_input.velocities[
+                    self.all_joint_names.index("ef_gripper_left_joint")
+                ] = -1.0
+            elif right_trigger > 0:
+                arm_input.velocities[
+                    self.all_joint_names.index("ef_gripper_left_joint")
+                ] = 1.0
+
+            # Bumpers: EF roll
+            arm_input.velocities[self.all_joint_names.index("wrist_roll_joint")] = (
+                right_bumper - left_bumper
+            )
+
+            # A: brake
+            new_brake_mode = button_a
+
+            # X: laser
+            new_laser = button_x
+
+            self.arm_manual_pub_.publish(arm_input)
+
+            # Only publish state if needed
+            if new_brake_mode != self.arm_brake_mode or new_laser != self.arm_laser:
+                self.arm_brake_mode = new_brake_mode
+                self.arm_laser = new_laser
+                state_msg = ArmCtrlState()
+                state_msg.brake_mode = bool(self.arm_brake_mode)
+                state_msg.laser = bool(self.arm_laser)
+
+                self.arm_state_pub_.publish(state_msg)
+                self.get_logger().info(
+                    f"[Arm State] Brake: {self.arm_brake_mode}, Laser: {self.arm_laser}"
+                )
+
+        # IK (ONLY NEW)
+        # =============
+
+        elif self.use_arm_ik:
+            arm_twist = TwistStamped()
+            arm_twist.header.frame_id = "base_link"
+            arm_twist.header.stamp = self.get_clock().now().to_msg()
+            arm_jointjog = JointJog()
+            arm_jointjog.header.frame_id = "base_link"
+            arm_jointjog.header.stamp = self.get_clock().now().to_msg()
+
+            # Right stick: linear y and linear x
+            # Left stick: angular z and linear z
+            # D-pad: angular y and _
+            # Triggers: EF grippers
+            # Bumpers: angular x
+            # A: brake
+            # B: IK mode
+            # X: manual mode
+            # Y: linear actuator
+
+            # Right stick: linear y and linear x
+            arm_twist.twist.linear.y = float(right_stick_x)
+            arm_twist.twist.linear.x = float(right_stick_y)
+
+            # Left stick: angular z and linear z
+            arm_twist.twist.angular.z = float(-1 * left_stick_x)
+            arm_twist.twist.linear.z = float(-1 * left_stick_y)
+            # D-pad: angular y and _
+            arm_twist.twist.angular.y = (
+                float(0)
+                if dpad_input[0] == 0
+                else float(-1 * copysign(0.75, dpad_input[0]))
+            )
+
+            # Triggers: EF Grippers
+            if left_trigger > 0 or right_trigger > 0:
+                arm_jointjog.joint_names.append("ef_gripper_left_joint")  # type: ignore
+                arm_jointjog.velocities.append(float(right_trigger - left_trigger))
+
+            # Bumpers: angular x
+            if left_bumper > 0 and right_bumper > 0:
+                arm_twist.twist.angular.x = float(0)
+            elif left_bumper > 0:
+                arm_twist.twist.angular.x = float(1)
+            elif right_bumper > 0:
+                arm_twist.twist.angular.x = float(-1)
+
+            self.arm_ik_twist_publisher.publish(arm_twist)
+            # self.arm_ik_jointjog_publisher.publish(arm_jointjog)  # TODO: Figure this shit out
+
+    def send_bio(self):
+        # Collect controller state
+        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
+        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
+        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
+        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
+        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
+        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
+        button_a = self.gamepad.get_button(0)
+        button_b = self.gamepad.get_button(1)
+        button_x = self.gamepad.get_button(2)
+        button_y = self.gamepad.get_button(3)
+        left_bumper = self.gamepad.get_button(4)
+        right_bumper = self.gamepad.get_button(5)
+        dpad_input = self.gamepad.get_hat(0)
+
+        if self.use_old_topics:
             bio_input = BioControl(
                 bio_arm=int(left_stick_y * -100),
                 drill_arm=int(round(right_stick_y) * -100),
             )
 
             # Drill motor (FAERIE)
-            if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
+            if left_trigger > 0 or right_trigger > 0:
                 bio_input.drill = int(
                     30 * (right_trigger - left_trigger)
                 )  # Max duty cycle 30%
 
-            self.core_publisher.publish(CORE_STOP_MSG)
-            self.arm_publisher.publish(arm_input)
-            # self.bio_publisher.publish(bio_input)
+            self.bio_publisher.publish(bio_input)
+
+        else:
+            pass  # TODO: implement new bio control topics
+
+    def core_cmd_vel_stop_msg(self):
+        return TwistStamped(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
+        )
+
+    def arm_manual_stop_msg(self):
+        return JointJog(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),
+            joint_names=self.all_joint_names,
+            velocities=[0.0] * len(self.all_joint_names),
+        )
+
+    def arm_ik_twist_stop_msg(self):
+        return TwistStamped(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
+        )
 
 
-def deadzone(value: float, threshold=0.05) -> float:
+def stick_deadzone(value: float, threshold=STICK_DEADZONE) -> float:
     """Apply a deadzone to a joystick input so the motors don't sound angry"""
     if abs(value) < threshold:
         return 0
     return value
 
 
+def stick_to_arm_direction(value: float, threshold=ARM_DEADZONE) -> int:
+    """Apply a larger deadzone to a stick input and make digital/binary instead of analog"""
+    if abs(value) < threshold:
+        return 0
+    return int(copysign(1, value))
+
+
 def is_user_in_group(group_name: str) -> bool:
     # Copied from https://zetcode.com/python/os-getgrouplist/
     try:
@@ -344,20 +741,26 @@ def is_user_in_group(group_name: str) -> bool:
         return False
 
 
+def exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
+
+
 def main(args=None):
     try:
         rclpy.init(args=args)
 
+        # Catch termination signals and exit cleanly
+        signal.signal(signal.SIGTERM, exit_handler)
+
         node = Headless()
         rclpy.spin(node)
     except (KeyboardInterrupt, ExternalShutdownException):
         print("Caught shutdown signal. Exiting...")
     finally:
-        rclpy.shutdown()
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
     main()

src/latency_tester/src/embedded_ping.cpp

@@ -26,7 +26,7 @@ class LatencyTester : public rclcpp::Node
 {
 public:
     LatencyTester()
-    : Node("latency_tester"), count_(0), target_mcu_("core")
+    : Node("latency_tester"), count_(0)
     {
         publisher_ = this->create_publisher<std_msgs::msg::String>("/anchor/relay", 10);
         timer_ = this->create_wall_timer(
@@ -35,6 +35,8 @@ public:
             "/anchor/debug",
             10,
             std::bind(&LatencyTester::response_callback, this, std::placeholders::_1));
+
+        target_mcu_ = this->declare_parameter<std::string>("target_mcu", "core");
     }
 
 private:

src/servo_arm_twist_pkg/CMakeLists.txt

@@ -1,80 +0,0 @@
-cmake_minimum_required(VERSION 3.22)
-project(servo_arm_twist_pkg)
-
-# C++ Libraries #################################################
-
-# Core C++ library for calculations and collision checking.
-# Provides interface used by the component node.
-set(SERVO_LIB_NAME servo_arm_twist_lib)
-
-# Pose Tracking
-set(POSE_TRACKING pose_tracking)
-
-# Component Nodes (Shared libraries) ############################
-set(SERVO_COMPONENT_NODE servo_node)
-set(SERVO_CONTROLLER_INPUT servo_controller_input)
-
-# Executable Nodes ##############################################
-set(SERVO_NODE_MAIN_NAME servo_node_main)
-set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
-set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
-
-#################################################################
-
-# Common cmake code applied to all moveit packages
-find_package(moveit_common REQUIRED)
-moveit_package()
-
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  control_msgs
-  control_toolbox
-  geometry_msgs
-  moveit_core
-  moveit_msgs
-  moveit_ros_planning
-  pluginlib
-  rclcpp
-  rclcpp_components
-  sensor_msgs
-  std_msgs
-  std_srvs
-  tf2_eigen
-  trajectory_msgs
-)
-
-find_package(ament_cmake REQUIRED)
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3 REQUIRED)
-foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  find_package(${Dependency} REQUIRED)
-endforeach()
-
-#####################
-## Component Nodes ##
-#####################
-
-# Add executable for using a controller
-add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
-ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
-rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
-
-#############
-## Install ##
-#############
-
-# Install Libraries
-install(
-  TARGETS
-    ${SERVO_CONTROLLER_INPUT}
-  EXPORT export_${PROJECT_NAME}
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
-)
-
-# Install Binaries
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-ament_package()

src/servo_arm_twist_pkg/README.md

@@ -1,3 +0,0 @@
-# Moveit Servo
-
-See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.

src/servo_arm_twist_pkg/package.xml

@@ -1,58 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>servo_arm_twist_pkg</name>
-  <version>2.5.9</version>
-  <description>Provides real-time manipulator Cartesian and joint servoing.</description>
-  <maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
-  <maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
-  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
-  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
-
-  <license>BSD 3-Clause</license>
-
-  <url type="website">https://ros-planning.github.io/moveit_tutorials</url>
-
-  <author>Brian O'Neil</author>
-  <author email="andyz@utexas.edu">Andy Zelenak</author>
-  <author>Blake Anderson</author>
-  <author email="alex@machinekoder.com">Alexander Rössler</author>
-  <author email="tyler@picknik.ai">Tyler Weaver</author>
-  <author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>moveit_common</depend>
-
-  <depend>control_msgs</depend>
-  <depend>control_toolbox</depend>
-  <depend>geometry_msgs</depend>
-  <depend>moveit_msgs</depend>
-  <depend>moveit_core</depend>
-  <depend>moveit_ros_planning_interface</depend>
-  <depend>pluginlib</depend>
-  <depend>sensor_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>std_srvs</depend>
-  <depend>tf2_eigen</depend>
-  <depend>trajectory_msgs</depend>
-
-  <exec_depend>gripper_controllers</exec_depend>
-  <exec_depend>joint_state_broadcaster</exec_depend>
-  <exec_depend>joint_trajectory_controller</exec_depend>
-  <exec_depend>joy</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>moveit_configs_utils</exec_depend>
-  <exec_depend>launch_param_builder</exec_depend>
-
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>controller_manager</test_depend>
-  <test_depend>ros_testing</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-
-</package>

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -1,271 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2020, PickNik Inc.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of PickNik Inc. nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/*      Title     : joystick_servo_example.cpp
- *      Project   : servo_arm_twist_pkg
- *      Created   : 08/07/2020
- *      Author    : Adam Pettinger
- */
-
-#include <sensor_msgs/msg/joy.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <control_msgs/msg/joint_jog.hpp>
-#include <std_srvs/srv/trigger.hpp>
-#include <moveit_msgs/msg/planning_scene.hpp>
-#include <rclcpp/client.hpp>
-#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/qos_event.hpp>
-#include <rclcpp/subscription.hpp>
-#include <rclcpp/time.hpp>
-#include <rclcpp/utilities.hpp>
-#include <thread>
-
-// We'll just set up parameters here
-const std::string JOY_TOPIC = "/joy";
-const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
-const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
-const std::string EEF_FRAME_ID = "End_Effector";
-const std::string BASE_FRAME_ID = "base_link";
-
-// Enums for button names -> axis/button array index
-// For XBOX 1 controller
-enum Axis
-{
-    LEFT_STICK_X = 0,
-    LEFT_STICK_Y = 1,
-    LEFT_TRIGGER = 2,
-    RIGHT_STICK_X = 3,
-    RIGHT_STICK_Y = 4,
-    RIGHT_TRIGGER = 5,
-    D_PAD_X = 6,
-    D_PAD_Y = 7
-};
-enum Button
-{
-    A = 0,
-    B = 1,
-    X = 2,
-    Y = 3,
-    LEFT_BUMPER = 4,
-    RIGHT_BUMPER = 5,
-    CHANGE_VIEW = 6,
-    MENU = 7,
-    HOME = 8,
-    LEFT_STICK_CLICK = 9,
-    RIGHT_STICK_CLICK = 10
-};
-
-// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
-// This will map the default values for the axes
-std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
-std::map<Button, double> BUTTON_DEFAULTS;
-
-// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
-// functions
-/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
- * @param axes The vector of continuous controller joystick axes
- * @param buttons The vector of discrete controller button values
- * @param twist A TwistStamped message to update in prep for publishing
- * @param joint A JointJog message to update in prep for publishing
- * @return return true if you want to publish a Twist, false if you want to publish a JointJog
- */
-bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
-                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
-                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
-{
-    // // Give joint jogging priority because it is only buttons
-    // // If any joint jog command is requested, we are only publishing joint commands
-    // if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
-    // {
-    //   // Map the D_PAD to the proximal joints
-    //   joint->joint_names.push_back("panda_joint1");
-    //   joint->velocities.push_back(axes[D_PAD_X]);
-    //   joint->joint_names.push_back("panda_joint2");
-    //   joint->velocities.push_back(axes[D_PAD_Y]);
-
-    //   // Map the diamond to the distal joints
-    //   joint->joint_names.push_back("panda_joint7");
-    //   joint->velocities.push_back(buttons[B] - buttons[X]);
-    //   joint->joint_names.push_back("panda_joint6");
-    //   joint->velocities.push_back(buttons[Y] - buttons[A]);
-    //   return false;
-    // }
-
-    // The bread and butter: map buttons to twist commands
-    twist->twist.linear.z = axes[RIGHT_STICK_Y];
-    twist->twist.linear.y = axes[RIGHT_STICK_X];
-
-    double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
-    double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
-    twist->twist.linear.x = lin_x_right + lin_x_left;
-
-    twist->twist.angular.y = axes[LEFT_STICK_Y];
-    twist->twist.angular.x = axes[LEFT_STICK_X];
-
-    double roll_positive = buttons[RIGHT_BUMPER];
-    double roll_negative = -1 * (buttons[LEFT_BUMPER]);
-    twist->twist.angular.z = roll_positive + roll_negative;
-
-    return true;
-}
-
-/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
- * @param frame_name Set the command frame to this
- * @param buttons The vector of discrete controller button values
- */
-void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
-{
-    if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
-        frame_name = BASE_FRAME_ID;
-    else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
-        frame_name = EEF_FRAME_ID;
-}
-
-namespace servo_arm_twist_pkg
-{
-class JoyToServoPub : public rclcpp::Node
-{
-public:
-    JoyToServoPub(const rclcpp::NodeOptions& options)
-        : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
-    {
-        // Setup pub/sub
-        joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
-            JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
-            [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
-
-        twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
-        joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
-        // collision_pub_ =
-        //     this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
-
-        // Create a service client to start the ServoNode
-        servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
-        servo_start_client_->wait_for_service(std::chrono::seconds(1));
-        servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
-
-        // // Load the collision scene asynchronously
-        // collision_pub_thread_ = std::thread([this]() {
-        //   rclcpp::sleep_for(std::chrono::seconds(3));
-        //   // Create collision object, in the way of servoing
-        //   moveit_msgs::msg::CollisionObject collision_object;
-        //   collision_object.header.frame_id = "panda_link0";
-        //   collision_object.id = "box";
-
-        //   shape_msgs::msg::SolidPrimitive table_1;
-        //   table_1.type = table_1.BOX;
-        //   table_1.dimensions = { 0.4, 0.6, 0.03 };
-
-        //   geometry_msgs::msg::Pose table_1_pose;
-        //   table_1_pose.position.x = 0.6;
-        //   table_1_pose.position.y = 0.0;
-        //   table_1_pose.position.z = 0.4;
-
-        //   shape_msgs::msg::SolidPrimitive table_2;
-        //   table_2.type = table_2.BOX;
-        //   table_2.dimensions = { 0.6, 0.4, 0.03 };
-
-        //   geometry_msgs::msg::Pose table_2_pose;
-        //   table_2_pose.position.x = 0.0;
-        //   table_2_pose.position.y = 0.5;
-        //   table_2_pose.position.z = 0.25;
-
-        //   collision_object.primitives.push_back(table_1);
-        //   collision_object.primitive_poses.push_back(table_1_pose);
-        //   collision_object.primitives.push_back(table_2);
-        //   collision_object.primitive_poses.push_back(table_2_pose);
-        //   collision_object.operation = collision_object.ADD;
-
-        //   moveit_msgs::msg::PlanningSceneWorld psw;
-        //   psw.collision_objects.push_back(collision_object);
-
-        //   auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
-        //   ps->world = psw;
-        //   ps->is_diff = true;
-        //   collision_pub_->publish(std::move(ps));
-        // });
-    }
-
-    // ~JoyToServoPub() override
-    // {
-    //     if (collision_pub_thread_.joinable())
-    //         collision_pub_thread_.join();
-    // }
-
-    void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
-    {
-        // Create the messages we might publish
-        auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
-        auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
-
-        // This call updates the frame for twist commands
-        updateCmdFrame(frame_to_publish_, msg->buttons);
-
-        // Convert the joystick message to Twist or JointJog and publish
-        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
-        {
-            // publish the TwistStamped
-            twist_msg->header.frame_id = frame_to_publish_;
-            twist_msg->header.stamp = this->now();
-            twist_pub_->publish(std::move(twist_msg));
-        }
-        // else
-        // {
-        //     // publish the JointJog
-        //     joint_msg->header.stamp = this->now();
-        //     joint_msg->header.frame_id = "panda_link3";
-        //     joint_pub_->publish(std::move(joint_msg));
-        // }
-    }
-
-private:
-    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
-    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
-    rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
-    rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
-    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
-
-    std::string frame_to_publish_;
-
-    // std::thread collision_pub_thread_;
-};  // class JoyToServoPub
-
-}  // namespace servo_arm_twist_pkg
-
-// Register the component with class_loader
-#include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)

treefmt.nix

@@ -0,0 +1,8 @@
+{ pkgs, ... }:
+{
+  projectRootFile = "flake.nix";
+  programs = {
+    nixfmt.enable = true;
+    black.enable = true;
+  };
+}