feat(core): change rover_platform parameter to override, default with hostname instead of environment variable

Launch file uses clucky, testbed, auto. Node was using core, testbed, auto. Replaced core with clucky in the node.

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

@@ -15,7 +15,11 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-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/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
-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!"
 
 
-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

@@ -24,27 +24,27 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1761810010,
+        "lastModified": 1770108954,
-        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
+        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
+        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "master",
+        "ref": "develop",
         "repo": "nix-ros-overlay",
         "type": "github"
       }
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1744849697,
+        "lastModified": 1759381078,
-        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
+        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "lopsided98",
+        "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
+        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
         "type": "github"
       },
       "original": {
@@ -60,7 +60,8 @@
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
-        ]
+        ],
+        "treefmt-nix": "treefmt-nix"
       }
     },
     "systems": {
@@ -77,6 +78,26 @@
         "repo": "default",
         "type": "github"
       }
+    },
+    "treefmt-nix": {
+      "inputs": {
+        "nixpkgs": [
+          "nixpkgs"
+        ]
+      },
+      "locked": {
+        "lastModified": 1773297127,
+        "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=",
+        "owner": "numtide",
+        "repo": "treefmt-nix",
+        "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "treefmt-nix",
+        "type": "github"
+      }
     }
   },
   "root": "root",

flake.nix

@@ -2,8 +2,13 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
+
+    treefmt-nix = {
+      url = "github:numtide/treefmt-nix";
+      inputs.nixpkgs.follows = "nixpkgs";
+    };
   };
 
   outputs =
@@ -11,19 +16,14 @@
       self,
       nix-ros-overlay,
       nixpkgs,
-    }:
+      ...
+    }@inputs:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
       let
         pkgs = import nixpkgs {
           inherit system;
           overlays = [ nix-ros-overlay.overlays.default ];
-          config = {
-            # Allow the insecure freeimage package
-            permittedInsecurePackages = [
-              "freeimage-3.18.0-unstable-2024-04-18"
-            ];
-          };
         };
       in
       {
@@ -31,9 +31,10 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
-            (python312.withPackages (
+            (python313.withPackages (
               p: with p; [
                 pyserial
+                python-can
                 pygame
                 scipy
                 crccheck
@@ -41,7 +42,7 @@
               ]
             ))
             (
-              with rosPackages.jazzy;
+              with rosPackages.humble;
               buildEnv {
                 paths = [
                   ros-core
@@ -62,22 +63,23 @@
                   control-msgs
                   control-toolbox
                   moveit-core
+                  moveit-planners
                   moveit-common
                   moveit-msgs
                   moveit-ros-planning
                   moveit-ros-planning-interface
+                  moveit-ros-visualization
                   moveit-configs-utils
                   moveit-ros-move-group
                   moveit-servo
                   moveit-simple-controller-manager
+                  topic-based-ros2-control
                   pilz-industrial-motion-planner
                   pick-ik
                   ompl
-                  chomp-motion-planner
                   joy
-                  ros-gz-sim
+                  ros2-controllers
-                  ros-gz-bridge
+                  chomp-motion-planner
-                  # ros2-controllers nixpkg does not build :(
                 ];
               }
             )
@@ -86,12 +88,10 @@
             # Display stuff
             export DISPLAY=''${DISPLAY:-:0}
             export QT_X11_NO_MITSHM=1
-	    if [[ ! $DIRENV_IN_ENVRC ]]; then
-              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete ros2)"
-              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete colcon)"
-            fi
           '';
         };
+
+        formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper;
       }
     );
 

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,282 +1,250 @@
+from warnings import deprecated
 import rclpy
 from rclpy.node import Node
-from std_srvs.srv import Empty
+from rclpy.executors import ExternalShutdownException
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
-import signal
-import time
 import atexit
 
-import serial
+from .connector import (
-import os
+    Connector,
-import sys
+    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
-
+from std_msgs.msg import String
-serial_pub = None
-thread = None
 
 
-"""
+class Anchor(Node):
-Publishers:
+    """
-  * /anchor/from_vic/debug
+    Publishers:
-    - Every string received from the MCU is published here for debugging
+    * /anchor/from_vic/debug
-  * /anchor/from_vic/core
+        - Every string received from the MCU is published here for debugging
-    - VicCAN messages for Core node
+    * /anchor/from_vic/core
-  * /anchor/from_vic/arm
+        - VicCAN messages for Core node
-    - VicCAN messages for Arm node
+    * /anchor/from_vic/arm
-  * /anchor/from_vic/bio
+        - VicCAN messages for Arm node
-    - VicCAN messages for Bio 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:
+    Subscribers:
-  * /anchor/from_vic/mock_mcu
+    * /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
+    * /anchor/to_vic/relay
-    - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
+        - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
-  * /anchor/to_vic/relay_string
+    * /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
 
-class SerialRelay(Node):
     def __init__(self):
-        # Initalize node with name
+        super().__init__("anchor_node")
-        super().__init__("anchor_node")  # previously 'serial_publisher'
 
-        # Loop through all serial devices on the computer to check for the MCU
+        logger = self.get_logger()
-        self.port = None
+
-        if port_override := os.getenv("PORT_OVERRIDE"):
+        # ROS2 Parameter Setup
-            self.port = port_override
+
-        ports = SerialRelay.list_serial_ports()
+        self.declare_parameter(
-        for i in range(4):
+            "connector",
-            if self.port is not None:
+            "auto",
-                break
+            ParameterDescriptor(
-            for port in ports:
+                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'.",
+            ),
+        )
+
+        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
+                )
+            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:
-                    # connect and send a ping command
+                    logger.info("trying CAN connector")
-                    ser = serial.Serial(port, 115200, timeout=1)
+                    self.connector = CANConnector(
-                    # (f"Checking port {port}...")
+                        logger, self.get_clock(), can_override
-                    ser.write(b"ping\n")
+                    )
-                    response = ser.read_until(bytes("\n", "utf8"))
+                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}"
+                )
+                exit(1)
 
-                    # if pong is in response, then we are talking with the MCU
+        # ROS2 Topic Setup
-                    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 None:
+        # Publishers
-            self.get_logger().info("Unable to find MCU...")
+        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
-            time.sleep(1)
+            String,
-            sys.exit(1)
+            "/anchor/from_vic/debug",
-
+            20,
-        self.ser = serial.Serial(self.port, 115200)
-        self.get_logger().info(f"Enabling Relay Mode")
-        self.ser.write(b"can_relay_mode,on\n")
-        atexit.register(self.cleanup)
-
-        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(
-            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(
+        # Subscribers
-            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
-        )
         self.tovic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+            VicCAN,
+            "/anchor/to_vic/relay",
+            self.write_connector,
+            20,
         )
-        self.tovic_debug_sub_ = self.create_subscription(
+        self.tovic_sub_legacy_ = self.create_subscription(
-            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
+            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
+        # Close devices on exit
-        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
+        atexit.register(self.cleanup)
-        self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
-        self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
-
-        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
-
-        # Create a subscriber
-        self.relay_sub = self.create_subscription(
-            String, "/anchor/relay", self.on_relay_tovic_string, 10
-        )
-
-    def 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():
-                self.read_MCU()  # Check the MCU for updates
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    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.ser.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.")
-            if self.ser.is_open:
-                self.ser.close()
-            exit(1)
-        except TypeError as e:
-            print(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            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.ser.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.ser.write(bytes(message, "utf8"))
-
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
 
     def cleanup(self):
-        print("Cleaning up before terminating...")
+        self.connector.cleanup()
-        if self.ser.is_open:
-            self.ser.close()
 
+    def read_connector(self):
+        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
+        viccan, raw = self.connector.read()
 
-def myexcepthook(type, value, tb):
+        if raw:
-    print("Uncaught exception:", type, value)
+            self.fromvic_debug_pub_.publish(String(data=raw))
-    if serial_pub:
+
-        serial_pub.cleanup()
+        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):
-    rclpy.init(args=args)
+    try:
-    sys.excepthook = myexcepthook
+        rclpy.init(args=args)
+        anchor_node = Anchor()
 
-    global serial_pub
+        thread = threading.Thread(target=rclpy.spin, args=(anchor_node,), daemon=True)
+        thread.start()
 
-    serial_pub = SerialRelay()
+        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
-    serial_pub.run()
+        while rclpy.ok():
-
+            anchor_node.read_connector()  # Check the connector for updates
-
+            rate.sleep()
-if __name__ == "__main__":
+    except (KeyboardInterrupt, ExternalShutdownException):
-    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+        print("Caught shutdown signal, shutting down...")
-    signal.signal(
+    finally:
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+        rclpy.try_shutdown()
-    )  # 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.actions import DeclareLaunchArgument, Shutdown, IncludeLaunchDescription
-from launch.substitutions import (
+from launch.conditions import IfCondition
-    LaunchConfiguration,
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-    ThisLaunchFileDir,
-    PathJoinSubstitution,
-)
 from launch_ros.actions import Node
-
+from launch_ros.substitutions import FindPackageShare
-
+from launch.launch_description_sources import PythonLaunchDescriptionSource
-# 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
 
 
 def generate_launch_description():
-    declare_arg = DeclareLaunchArgument(
+    connector = LaunchConfiguration("connector")
-        "mode",
+    serial_override = LaunchConfiguration("serial_override")
-        default_value="anchor",
+    can_override = LaunchConfiguration("can_override")
-        description="Launch mode: arm, core, bio, anchor, or ptz",
+    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_override",
+            default_value="",
+            description="Override the rover platform (either clucky or testbed). If unset, hostname is used; defaults to clucky without hostname.",
+            choices=["clucky", "testbed", ""],
+        )
+    )
+
+    # 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_override": LaunchConfiguration(
+                        "rover_platform_override", 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>
+  <description>ASTRA VicCAN driver package, using python-can and pyserial.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <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",
+    description="ASTRA VicCAN driver package, using python-can and pyserial.",
-    license="All Rights Reserved",
+    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,274 @@
+import sys
+import signal
+import math
+from warnings import deprecated
+
 import rclpy
 from rclpy.node import Node
-import serial
+from rclpy.executors import ExternalShutdownException
-import sys
+from rclpy import qos
-import threading
+
-import glob
+from std_msgs.msg import String, Header
-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 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(
+control_qos = qos.QoSProfile(
-#     history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
-#     depth=1,
+    depth=2,
-#     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,  # Best Effort subscribers are still compatible with Reliable publishers
-#     durability=qos.QoSDurabilityPolicy.VOLATILE,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
-#     deadline=1000,
+    # deadline=Duration(seconds=1),
-#     lifespan=500,
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-#     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-#     liveliness_lease_duration=5000
+    # liveliness_lease_duration=Duration(seconds=5),
-# )
+)
-
-serial_pub = None
-thread = None
 
 
-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.get_logger().info(f"arm launch_mode is: anchor")  # Hey I like the output
-        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}")
 
-        # Create publishers
+        ##################################################
-        self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
+        # Parameters
-        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)
 
-        # Create subscribers
+        self.declare_parameter("use_old_topics", True)
-        self.man_sub = self.create_subscription(
+        self.use_old_topics = (
-            ArmManual, "/arm/control/manual", self.send_manual, 10
+            self.get_parameter("use_old_topics").get_parameter_value().bool_value
         )
 
-        # New messages
+        ##################################################
-        self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
+        # Old topics
-        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
 
-        self.joint_command_sub = self.create_subscription(
+        if self.use_old_topics:
-            JointState, "/joint_commands", self.joint_command_callback, 10
+            # Anchor topics
-        )
-
-        # Topics used in anchor mode
-        if self.launch_mode == "anchor":
             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.arm_feedback = SocketFeedback()
+            # Create publishers
-        self.digit_feedback = DigitFeedback()
+            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
+            # Create subscribers
-        if self.launch_mode == "arm":
+            self.man_sub = self.create_subscription(
-            # Loop through all serial devices on the computer to check for the MCU
+                ArmManual, "/arm/control/manual", self.send_manual, 10
-            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
 
-                        # if pong is in response, then we are talking with the MCU
+        ###################################################
-                        if b"pong" in response:
+        # New topics
-                            self.port = port
-                            self.get_logger().info(f"Found MCU at {self.port}!")
-                            break
-                    except:
-                        pass
-                if self.port is not None:
-                    break
 
-            if self.port is None:
+        # Anchor topics
-                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)
+        # from_vic
-            atexit.register(self.cleanup)
+        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):
+        # Control
-        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
+        # 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:
+        # Feedback
-            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()
 
-    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
+        # Combined Socket and Digit feedback
-    def read_mcu(self):
+        self.arm_feedback_pub_ = self.create_publisher(
-        try:
+            ArmFeedback,
-            output = str(self.ser.readline(), "utf8")
+            "/arm/feedback",
-            if output:
+            qos_profile=qos.qos_profile_sensor_data,
-                # self.get_logger().info(f"[MCU] {output}")
+        )
-                msg = String()
+        # IK arm pose: /joint_states is published from here to topic_based_control
-                msg.data = output
+        self.joint_state_pub_ = self.create_publisher(
-                self.debug_pub.publish(msg)
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
-        except serial.SerialException:
+        )
-            self.get_logger().info("SerialException caught... closing serial port.")
+
-            if self.ser.is_open:
+        ###################################################
-                self.ser.close()
+        # Saved state
-            pass
+
-        except TypeError as e:
+        # Combined Socket and Digit feedback
-            self.get_logger().info(f"TypeError: {e}")
+        self.arm_feedback_new = ArmFeedback()
-            print("Closing serial port.")
+        self.arm_feedback_new.axis0_motor.id = 1
-            if self.ser.is_open:
+        self.arm_feedback_new.axis1_motor.id = 2
-                self.ser.close()
+        self.arm_feedback_new.axis2_motor.id = 3
-            pass
+        self.arm_feedback_new.axis3_motor.id = 4
-        except Exception as e:
+
-            print(f"Exception: {e}")
+        # IK Arm pose
-            print("Closing serial port.")
+        self.saved_joint_state = JointState()
-            if self.ser.is_open:
+        self.saved_joint_state.header.frame_id = "base_link"
-                self.ser.close()
+        self.saved_joint_state.name = self.all_joint_names
-            pass
+        # ... 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
+        if len(msg.position) < 7 and len(msg.velocity) < 7:
-        positions = [math.degrees(pos) * 10 for pos in msg.position]
+            self.get_logger().debug("Ignoring malformed /joint_command message.")
-        # Axis 2 & 3 URDF direction is inverted
+            return  # command needs either position or velocity for all 7 joints
-        positions[2] = -positions[2]
-        positions[3] = -positions[3]
 
-        # Set target angles for each arm axis for embedded IK PID to handle
+        # Grab velocities from message
-        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
+        velocities = [
-        # Wrist yaw and roll
+            (
-        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
+                msg.velocity[msg.name.index(joint_name)]  # type: ignore
-        # Gripper IK does not have adequate hardware yet
+                if joint_name in msg.name
-        self.send_cmd(command)
+                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
@@ -190,7 +283,7 @@ class SerialRelay(Node):
 
         command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
 
-        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
+        command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
 
         command += f"can_relay_tovic,digit,28,{msg.laser}\n"
 
@@ -200,24 +293,17 @@ class SerialRelay(Node):
 
         return
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_cmd(self, msg: str):
-        if (
+        output = String(data=msg)
-            self.launch_mode == "anchor"
+        self.anchor_pub.publish(output)
-        ):  # 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"))
 
+    @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 +321,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 +468,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 +486,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 +510,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):
+def exit_handler(signum, frame):
-        print("Cleaning up...")
+    print("Caught SIGTERM. Exiting...")
-        try:
+    rclpy.try_shutdown()
-            if self.ser.is_open:
+    sys.exit(0)
-                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):
     rclpy.init(args=args)
-    sys.excepthook = myexcepthook
 
-    global serial_pub
+    # Catch termination signals and exit cleanly
-    serial_pub = SerialRelay()
+    signal.signal(signal.SIGTERM, exit_handler)
-    serial_pub.run()
+
+    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>
+  <description>Relays topics related to Arm between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <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>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>astra_msgs</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
+from setuptools import setup
-import os
-from glob import glob
 
 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="David Sharpe",
-    maintainer_email="tristanmcginnis26@gmail.com",
+    maintainer_email="ds0196@uah.edu",
-    description="TODO: Package description",
+    description="Relays topics related to Arm between VicCAN (through Anchor) and basestation.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
+        "console_scripts": ["arm = arm_pkg.arm_node:main"],
-            "arm = arm_pkg.arm_node:main",
-            "headless = arm_pkg.arm_headless:main",
-        ],
     },
 )

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>
+  <version>1.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Biosensor package to handle command interpretation and embedded interfacing.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <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",
+    description="Relays topics related to Biosensor between VicCAN (through Anchor) and basestation.",
-    license="TODO: License declaration",
+    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_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>
+  <description>Relays topics related to Core between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <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>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>python-crccheck-pip</depend>
+  <exec_depend>python3-scipy</exec_depend>
-  <depend>astra_msgs</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="David Sharpe",
-    maintainer_email="tristanmcginnis26@gmail.com",
+    maintainer_email="ds0196@uah.edu",
-    description="Core rover control package to handle command interpretation and embedded interfacing.",
+    description="Relays topics related to Core between VicCAN (through Anchor) and basestation.",
-    license="All Rights Reserved",
+    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>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>astra_msgs</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.",
+    description="Provides headless rover control, similar to Basestation.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
+        "console_scripts": ["headless_full = src.headless_node:main"],
-            "headless_full = src.headless_node:main",
-        ],
     },
 )

src/headless_pkg/src/headless_node.py

@@ -1,22 +1,32 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from rclpy import qos
 from rclpy.duration import Duration
 
 import signal
 import time
-import atexit
 
 import os
 import sys
-import threading
+import pwd
-import glob
+import grp
 from math import copysign
 
-from std_msgs.msg import String
+from std_srvs.srv import Trigger
-from geometry_msgs.msg import Twist
+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
 
@@ -31,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,
     depth=2,
     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
     durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
+    # deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5),
+    # 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")
@@ -71,41 +99,175 @@ class Headless(Node):
             print("No gamepad found. Waiting...")
 
         # Initialize the gamepad
-        self.gamepad = pygame.joystick.Joystick(0)
+        id = 0
-        self.gamepad.init()
+        while True:
-        print(f"Gamepad Found: {self.gamepad.get_name()}")
+            self.num_gamepads = pygame.joystick.get_count()
+            if id >= self.num_gamepads:
+                self.get_logger().fatal("Ran out of controllers to try")
+                sys.exit(1)
 
-        self.create_timer(0.15, self.send_controls)
+            try:
+                self.gamepad = pygame.joystick.Joystick(id)
+                self.gamepad.init()
+            except Exception as e:
+                self.get_logger().error("Error when initializing gamepad")
+                self.get_logger().error(e)
+                id += 1
+                continue
+            print(f"Gamepad Found: {self.gamepad.get_name()}")
 
-        self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
+            if self.gamepad.get_numhats() == 0 or self.gamepad.get_numaxes() < 5:
-        self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
+                self.get_logger().error("Controller not correctly initialized.")
-        self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
+                if not is_user_in_group("input"):
+                    self.get_logger().warning(
+                        "If using NixOS, you may need to add yourself to the 'input' group."
+                    )
+                if is_user_in_group("plugdev"):
+                    self.get_logger().warning(
+                        "If using NixOS, you may need to remove yourself from the 'plugdev' group."
+                    )
+            else:
+                break
+            id += 1
 
-        self.core_twist_pub_ = self.create_publisher(
+        ##################################################
-            Twist, "/core/twist", qos_profile=control_qos
+        # Parameters
+
+        self.declare_parameter("use_old_topics", True)
+        self.use_old_topics = (
+            self.get_parameter("use_old_topics").get_parameter_value().bool_value
         )
-        self.core_state_pub_ = self.create_publisher(
+
-            CoreCtrlState, "/core/control/state", 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.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):
+        # Added so you can tell when it starts running after changing the constant logging to debug from info
-        # This thread makes all the update processes run in the background
+        self.get_logger().info("Defaulting to Core mode. Ready.")
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
 
-        try:
+    def stop_all(self):
-            while rclpy.ok():
+        if self.use_old_topics:
-                self.send_controls()
+            self.core_publisher.publish(CORE_STOP_MSG)
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
+            self.arm_publisher.publish(ARM_STOP_MSG)
-        except KeyboardInterrupt:
+            self.bio_publisher.publish(BIO_STOP_MSG)
-            sys.exit(0)
+        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"""
@@ -115,12 +277,10 @@ class Headless(Node):
                 sys.exit(0)
 
         # Check if controller is still connected
-        if pygame.joystick.get_count() == 0:
+        if pygame.joystick.get_count() != self.num_gamepads:
             print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
+            # Stop the rover if controller disconnected
-            self.core_publisher.publish(CORE_STOP_MSG)
+            self.stop_all()
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            self.bio_publisher.publish(BIO_STOP_MSG)
             self.get_logger().info("Final stop commands sent. Shutting down.")
             # Clean up
             pygame.quit()
@@ -136,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
+        # Actually send the controls
-        if self.ctrl_mode == "core" and CORE_MODE == "duty":
+        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))
 
@@ -164,31 +355,34 @@ 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":
+        else:  # New topics
-            input = Twist()
+            twist = 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)
 
             # Forward/back and Turn
-            input.linear.x = -1.0 * left_stick_y
+            twist.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * copysign(
+            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:
+                # These scaling factors convert raw stick inputs to absolute m/s and rad/s
+                # values that DiffDriveController will convert to motor RPM, rather than
+                # the plain Twist, which just sends the stick values as duty cycle and
+                # sends that scaled to the motors.
+                twist.linear.x *= 1.0
+                twist.angular.z *= 1.5
+
             # Publish
-            self.core_twist_pub_.publish(input)
+            if self.use_cmd_vel:
-            self.arm_publisher.publish(ARM_STOP_MSG)
+                header = Header(stamp=self.get_clock().now().to_msg())
-            # self.bio_publisher.publish(BIO_STOP_MSG)
+                self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist))
-            self.get_logger().info(
+            else:
-                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
+                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
@@ -211,100 +405,366 @@ 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
+    def send_arm(self):
-        if self.ctrl_mode == "arm":
+        # 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
+            # OLD ARM MANUAL CONTROL SCHEME
-            left_stick_x = deadzone(self.gamepad.get_axis(0))
+            if not self.use_new_arm_manual_scheme:
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
+                # EF Grippers
-            left_trigger = deadzone(self.gamepad.get_axis(2))
+                if left_trigger > 0 and right_trigger > 0:
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
+                    arm_input.gripper = 0
-            right_stick_y = deadzone(self.gamepad.get_axis(4))
+                elif left_trigger > 0:
-            right_trigger = deadzone(self.gamepad.get_axis(5))
+                    arm_input.gripper = -1
-            right_bumper = self.gamepad.get_button(5)
+                elif right_trigger > 0:
-            dpad_input = self.gamepad.get_hat(0)
+                    arm_input.gripper = 1
 
-            # EF Grippers
+                # Axis 0
-            if left_trigger > 0 and right_trigger > 0:
+                if dpad_input[0] == 1:
-                arm_input.gripper = 0
+                    arm_input.axis0 = 1
-            elif left_trigger > 0:
+                elif dpad_input[0] == -1:
-                arm_input.gripper = -1
+                    arm_input.axis0 = -1
-            elif right_trigger > 0:
-                arm_input.gripper = 1
 
-            # Axis 0
+                if right_bumper:  # Control end effector
-            if dpad_input[0] == 1:
-                arm_input.axis0 = 1
-            elif dpad_input[0] == -1:
-                arm_input.axis0 = -1
 
-            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
+                    # Effector roll
-                if left_stick_x > 0:
+                    if right_stick_x > 0:
-                    arm_input.effector_yaw = 1
+                        arm_input.effector_roll = 1
-                elif left_stick_x < 0:
+                    elif right_stick_x < 0:
-                    arm_input.effector_yaw = -1
+                        arm_input.effector_roll = -1
 
-                # Effector roll
+                else:  # Control arm axis
-                if right_stick_x > 0:
+
-                    arm_input.effector_roll = 1
+                    # Axis 1
-                elif right_stick_x < 0:
+                    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
+                # Y: linear actuator
-                if abs(left_stick_x) > 0.15:
+                if button_y and not button_b:
-                    arm_input.axis1 = round(left_stick_x)
+                    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
+            self.arm_publisher.publish(arm_input)
-                if abs(left_stick_y) > 0.15:
-                    arm_input.axis2 = -1 * round(left_stick_y)
 
-                # Axis 3
+        # NEW MANUAL
-                if abs(right_stick_y) > 0.15:
+        # ==========
-                    arm_input.axis3 = -1 * round(right_stick_y)
 
-            # 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.bio_publisher.publish(bio_input)
-            self.arm_publisher.publish(arm_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:
+        username = os.getlogin()
+
+        # Get group ID from name
+        group_info = grp.getgrnam(group_name)
+        target_gid = group_info.gr_gid
+
+        # Get user's groups
+        user_info = pwd.getpwnam(username)
+        user_groups = os.getgrouplist(username, user_info.pw_gid)
+
+        return target_gid in user_groups
+    except KeyError:
+        return False
+
+
+def exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
+
+
 def main(args=None):
-    rclpy.init(args=args)
+    try:
-    node = Headless()
+        rclpy.init(args=args)
-    rclpy.spin(node)
+
-    rclpy.shutdown()
+        # 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.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/moveit_servo/CMakeLists.txt

@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(moveit_servo)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

src/moveit_servo/config/kinematics.yaml

@@ -1,4 +0,0 @@
-panda_arm:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.05

src/moveit_servo/launch/servo.launch.py

@@ -1,25 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from moveit_configs_utils import MoveItConfigsBuilder
-
-def generate_launch_description():
-    moveit_config = (
-        MoveItConfigsBuilder("astra_descriptions")
-        .robot_description(file_path="config/robot.urdf.xacro")
-        .to_moveit_configs()
-    )
-
-    servo_node = Node(
-        package="moveit_servo",
-        executable="servo_node",
-        name="servo_node",
-        parameters=[
-            "config/servo_parameters.yaml",
-            moveit_config.robot_description,
-            moveit_config.robot_description_semantic,
-            moveit_config.robot_description_kinematics,
-        ],
-        output="screen",
-    )
-
-    return LaunchDescription([servo_node])

src/moveit_servo/package.xml

@@ -1,18 +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>moveit_servo</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="aaron.m.davis@comcast.net">aaron</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

treefmt.nix

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