added docker compose

.envrc

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

auto_start/auto_start_anchor.sh

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

auto_start/auto_start_headless_full.sh

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

auto_start/start_rosbag.sh

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

docker-compose.yml

@@ -0,0 +1,18 @@
+
+services:
+  ros2:
+    image: osrf/ros:jazzy-desktop-full
+    container_name: ros2_jazzy_gui
+    network_mode: host
+    environment:
+      - DISPLAY=${DISPLAY}
+      - QT_X11_NO_MITSHM=1
+      - XAUTHORITY=/tmp/.docker.xauth
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=all
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      - ./ros_ws:/ros_ws
+    stdin_open: true
+    tty: true
+    privileged: true

flake.lock

@@ -24,27 +24,27 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1770108954,
-        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
+        "lastModified": 1761810010,
+        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
+        "rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "develop",
+        "ref": "master",
         "repo": "nix-ros-overlay",
         "type": "github"
       }
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1759381078,
-        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "NixOS",
+        "lastModified": 1744849697,
+        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
+        "owner": "lopsided98",
         "repo": "nixpkgs",
-        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
+        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
         "type": "github"
       },
       "original": {

flake.nix

@@ -2,7 +2,7 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
   };
 
@@ -25,10 +25,9 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
-            (python313.withPackages (
+            (python312.withPackages (
               p: with p; [
                 pyserial
-		python-can
                 pygame
                 scipy
                 crccheck
@@ -57,12 +56,10 @@
                   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
@@ -71,9 +68,9 @@
                   pilz-industrial-motion-planner
                   pick-ik
                   ompl
+                  chomp-motion-planner
                   joy
-                  ros2-controllers
-		  chomp-motion-planner
+                  # ros2-controllers nixpkg does not build :(
                 ];
               }
             )

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,99 +1,85 @@
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
-from rcl_interfaces.msg import ParameterDescriptor, ParameterType
+from std_srvs.srv import Empty
 
 import signal
+import time
 import atexit
 
-from .connector import (
-    Connector,
-    MockConnector,
-    SerialConnector,
-    CANConnector,
-    NoValidDeviceException,
-    NoWorkingDeviceException,
-)
-from .convert import string_to_viccan
+import serial
+import os
 import sys
 import threading
+import glob
 
 from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
 
+serial_pub = None
+thread = None
 
-class Anchor(Node):
-    """
-    Publishers:
-    * /anchor/from_vic/debug
-        - Every string received from the MCU is published here for debugging
-    * /anchor/from_vic/core
-        - VicCAN messages for Core node
-    * /anchor/from_vic/arm
-        - VicCAN messages for Arm node
-    * /anchor/from_vic/bio
-        - VicCAN messages for Bio node
 
-    Subscribers:
-    * /anchor/from_vic/mock_mcu
-        - For testing without an actual MCU, publish strings here as if they came from an MCU
-    * /anchor/to_vic/relay
-        - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
-    """
+"""
+Publishers:
+  * /anchor/from_vic/debug
+    - Every string received from the MCU is published here for debugging
+  * /anchor/from_vic/core
+    - VicCAN messages for Core node
+  * /anchor/from_vic/arm
+    - VicCAN messages for Arm node
+  * /anchor/from_vic/bio
+    - VicCAN messages for Bio node
 
-    connector: Connector
+Subscribers:
+  * /anchor/from_vic/mock_mcu
+    - For testing without an actual MCU, publish strings here as if they came from an MCU
+  * /anchor/to_vic/relay
+    - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
+  * /anchor/to_vic/relay_string
+    - Publish raw strings to this topic to send directly to the MCU for debugging
+"""
 
+
+class SerialRelay(Node):
     def __init__(self):
-        super().__init__("anchor_node")
+        # Initalize node with name
+        super().__init__("anchor_node")  # previously 'serial_publisher'
 
-        logger = self.get_logger()
-
-        self.declare_parameter(
-            "connector",
-            "auto",
-            ParameterDescriptor(
-                name="connector",
-                description="Declares which MCU connector should be used. Defaults to 'auto'.",
-                type=ParameterType.PARAMETER_STRING,
-                additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.",
-            ),
-        )
-
-        # Determine which connector to use. Options are Mock, Serial, and CAN
-        connector_select = (
-            self.get_parameter("connector").get_parameter_value().string_value
-        )
-
-        match connector_select:
-            case "serial":
-                logger.info("using serial connector")
-                self.connector = SerialConnector(self.get_logger())
-            case "can":
-                logger.info("using CAN connector")
-                self.connector = CANConnector(self.get_logger())
-            case "mock":
-                logger.info("using mock connector")
-                self.connector = MockConnector(self.get_logger())
-            case "auto":
-                logger.info("automatically determining connector")
+        # Loop through all serial devices on the computer to check for the MCU
+        self.port = None
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.port = port_override
+        ports = SerialRelay.list_serial_ports()
+        for i in range(4):
+            if self.port is not None:
+                break
+            for port in ports:
                 try:
-                    logger.info("trying CAN connector")
-                    self.connector = CANConnector(self.get_logger())
-                except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
-                    logger.info("CAN connector failed, trying serial connector")
-                    self.connector = SerialConnector(self.get_logger())
-            case _:
-                self.get_logger().fatal(
-                    f"invalid value for connector parameter: {connector_select}"
-                )
-                exit(1)
+                    # connect and send a ping command
+                    ser = serial.Serial(port, 115200, timeout=1)
+                    # (f"Checking port {port}...")
+                    ser.write(b"ping\n")
+                    response = ser.read_until(bytes("\n", "utf8"))
 
-        # Close devices on exit
+                    # if pong is in response, then we are talking with the MCU
+                    if b"pong" in response:
+                        self.port = port
+                        self.get_logger().info(f"Found MCU at {self.port}!")
+                        break
+                except:
+                    pass
+
+        if self.port is None:
+            self.get_logger().info("Unable to find MCU...")
+            time.sleep(1)
+            sys.exit(1)
+
+        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)
 
-        # ROS2 Topic Setup
-
-        # Publishers
+        # New pub/sub with VicCAN
         self.fromvic_debug_pub_ = self.create_publisher(
             String, "/anchor/from_vic/debug", 20
         )
@@ -107,66 +93,189 @@ class Anchor(Node):
             VicCAN, "/anchor/from_vic/bio", 20
         )
 
-        # Subscribers
-        self.tovic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/to_vic/relay", self.connector.write, 20
-        )
         self.mock_mcu_sub_ = self.create_subscription(
             String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
         )
+        self.tovic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+        )
+        self.tovic_debug_sub_ = self.create_subscription(
+            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
+        )
 
-    def cleanup(self):
-        self.connector.cleanup()
+        # Create publishers
+        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
+        self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
+        self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
+
+        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
+
+        # Create a subscriber
+        self.relay_sub = self.create_subscription(
+            String, "/anchor/relay", self.on_relay_tovic_string, 10
+        )
+
+    def 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"""
-        output = self.connector.read()
+        try:
+            output = str(self.ser.readline(), "utf8")
 
-        if not output:
-            return
-
-        self.relay_fromvic(output)
-
-    def relay_fromvic(self, msg: VicCAN):
-        """Relay a string message from the MCU to the appropriate VicCAN topic"""
-        msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
-
-        if msg.mcu_name == "core":
-            self.fromvic_core_pub_.publish(msg)
-        elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
-            self.fromvic_arm_pub_.publish(msg)
-        elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
-            self.fromvic_bio_pub_.publish(msg)
+            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):
-        viccan = string_to_viccan(
-            msg.data,
-            "mock",
-            self.get_logger(),
+        """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"
         )
-        if viccan:
-            self.relay_fromvic(viccan)
+
+        # 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...")
+        if self.ser.is_open:
+            self.ser.close()
+
+
+def myexcepthook(type, value, tb):
+    print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
 
 
 def main(args=None):
-    try:
-        rclpy.init(args=args)
-        anchor_node = Anchor()
+    rclpy.init(args=args)
+    sys.excepthook = myexcepthook
 
-        thread = threading.Thread(target=rclpy.spin, args=(anchor_node,), daemon=True)
-        thread.start()
+    global serial_pub
 
-        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
-        while rclpy.ok():
-            anchor_node.read_MCU()  # Check the MCU for updates
-            rate.sleep()
-    except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal, shutting down...")
-    finally:
-        rclpy.try_shutdown()
+    serial_pub = SerialRelay()
+    serial_pub.run()
 
 
 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

src/anchor_pkg/anchor_pkg/connector.py

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

src/anchor_pkg/anchor_pkg/convert.py

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

src/anchor_pkg/package.xml

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

src/arm_pkg/arm_pkg/arm_node.py

@@ -49,7 +49,7 @@ class SerialRelay(Node):
 
         # Create subscribers
         self.man_sub = self.create_subscription(
-            ArmManual, "/arm/control/manual", self.send_manual, 2
+            ArmManual, "/arm/control/manual", self.send_manual, 10
         )
 
         # New messages
@@ -190,7 +190,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"
 

src/core_pkg/core_pkg/core_node.py

@@ -31,14 +31,14 @@ CORE_WHEEL_RADIUS = 0.171  # meters
 CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
 
 control_qos = qos.QoSProfile(
-    # history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    # durability=qos.QoSDurabilityPolicy.VOLATILE,
-    # deadline=Duration(seconds=1),
-    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    # liveliness_lease_duration=Duration(seconds=5),
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    deadline=Duration(seconds=1),
+    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    liveliness_lease_duration=Duration(seconds=5),
 )
 
 # Used to verify the length of an incoming VicCAN feedback message

src/headless_pkg/src/headless_node.py

@@ -1,6 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
 from rclpy import qos
 from rclpy.duration import Duration
 
@@ -12,8 +11,6 @@ import os
 import sys
 import threading
 import glob
-import pwd
-import grp
 from math import copysign
 
 from std_msgs.msg import String
@@ -35,14 +32,14 @@ ARM_STOP_MSG = ArmManual()  # "
 BIO_STOP_MSG = BioControl()  # "
 
 control_qos = qos.QoSProfile(
-    # history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    # durability=qos.QoSDurabilityPolicy.VOLATILE,
-    # deadline=Duration(seconds=1),
-    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    # liveliness_lease_duration=Duration(seconds=5),
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    deadline=Duration(seconds=1),
+    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    liveliness_lease_duration=Duration(seconds=5),
 )
 
 CORE_MODE = "twist"  # "twist" or "duty"
@@ -74,36 +71,9 @@ class Headless(Node):
             print("No gamepad found. Waiting...")
 
         # Initialize the gamepad
-        id = 0
-        while True:
-            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)
-
-            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()}")
-
-            if self.gamepad.get_numhats() == 0 or self.gamepad.get_numaxes() < 5:
-                self.get_logger().error("Controller not correctly initialized.")
-                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.gamepad = pygame.joystick.Joystick(0)
+        self.gamepad.init()
+        print(f"Gamepad Found: {self.gamepad.get_name()}")
 
         self.create_timer(0.15, self.send_controls)
 
@@ -145,7 +115,7 @@ class Headless(Node):
                 sys.exit(0)
 
         # Check if controller is still connected
-        if pygame.joystick.get_count() != self.num_gamepads:
+        if pygame.joystick.get_count() == 0:
             print("Gamepad disconnected. Exiting...")
             # Send one last zero control message
             self.core_publisher.publish(CORE_STOP_MSG)
@@ -326,34 +296,11 @@ def deadzone(value: float, threshold=0.05) -> float:
     return 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 main(args=None):
-    try:
-        rclpy.init(args=args)
-
-        node = Headless()
-        rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal. Exiting...")
-    finally:
-        rclpy.shutdown()
+    rclpy.init(args=args)
+    node = Headless()
+    rclpy.spin(node)
+    rclpy.shutdown()
 
 
 if __name__ == "__main__":