Fixed README

-> python 3.13

.dockerignore

@@ -0,0 +1,4 @@
+.git
+*.log
+build/
+install/

.env

@@ -0,0 +1 @@
+DOCKER_IMAGE=main-jazzy-tutorial-source

README.md

@@ -11,6 +11,7 @@ You will use these packages to launch all rover-side ROS2 nodes.
 - [Software Prerequisites](#software-prerequisites)
   - [Nix](#nix)
   - [ROS2 Humble + rosdep](#ros2-humble--rosdep)
+  - [Docker](#docker)
 - [Running](#running)
   - [Testing Serial](#testing-serial)
   - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
@@ -47,6 +48,17 @@ $ cd path/to/rover-ros2
 $ rosdep install --from-paths src -y --ignore-src
 ```
 
+### Docker
+
+Using the docker compose file automatically builds the workspace and allows you to choose between running on the CPU or GPU for applications like Rviz2 and Gazebo:
+
+```bash
+  # Run on CPU
+$ docker compose run --rm --name rover-ros2-container cpu
+  # Run on GPU (NVidia only)
+$ docker compose run --rm --name rover-ros2-container gpu
+```
+
 ## Running
 
 ```bash

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
 

docker-compose.yml

@@ -1,18 +1,38 @@
-
 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
+  cpu:
+    image: moveit/moveit2:${DOCKER_IMAGE}
+    container_name: moveit2_container
     privileged: true
+    network_mode: host
+    command: ["bash", "-c", "colcon build --symlink-install && source /ros2_ws/install/setup.bash && exec bash"]
+    volumes:
+      - ./:/ros2_ws
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - $XAUTHORITY:/root/.Xauthority
+    working_dir: /ros2_ws
+    environment:
+      QT_X11_NO_MITSHM: 1
+      DISPLAY: $DISPLAY
+  gpu:
+    image: moveit/moveit2:${DOCKER_IMAGE}
+    container_name: moveit2_container
+    privileged: true
+    network_mode: host
+    command: ["bash", "-c", "colcon build --symlink-install && source /ros2_ws/install/setup.bash && exec bash"]
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [gpu]
+    volumes:
+      - ./:/ros2_ws
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - $XAUTHORITY:/root/.Xauthority
+    working_dir: /ros2_ws
+    environment:
+      QT_X11_NO_MITSHM: 1
+      DISPLAY: $DISPLAY
+      NVIDIA_VISIBLE_DEVICES: all
+      NVIDIA_DRIVER_CAPABILITIES: all

flake.lock

@@ -24,27 +24,27 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1761810010,
-        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
+        "lastModified": 1770108954,
+        "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,
-        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
-        "owner": "lopsided98",
+        "lastModified": 1759381078,
+        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
+        "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
+        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
         "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/master";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
   };
 
@@ -25,7 +25,7 @@
           name = "ASTRA Anchor";
           packages = with pkgs; [
             colcon
-            (python312.withPackages (
+            (python313.withPackages (
               p: with p; [
                 pyserial
                 pygame
@@ -56,10 +56,12 @@
                   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
@@ -68,9 +70,9 @@
                   pilz-industrial-motion-planner
                   pick-ik
                   ompl
-                  chomp-motion-planner
                   joy
-                  # ros2-controllers nixpkg does not build :(
+                  ros2-controllers
+		  chomp-motion-planner
                 ];
               }
             )

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from std_srvs.srv import Empty
 
 import signal
@@ -7,6 +8,7 @@ import time
 import atexit
 
 import serial
+import serial.tools.list_ports
 import os
 import sys
 import threading
@@ -15,70 +17,156 @@ import glob
 from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
 
-serial_pub = None
-thread = None
+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
+]
 
 
-"""
-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
+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
-  * /anchor/to_vic/relay_string
-    - Publish raw strings to this topic to send directly to the MCU for debugging
-"""
+    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):
         # Initalize node with name
         super().__init__("anchor_node")  # previously 'serial_publisher'
 
-        # Loop through all serial devices on the computer to check for the MCU
-        self.port = None
+        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
+
+        # Serial port override
         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:
-                    # 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"))
+            self.serial_port = port_override
 
-                    # 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
+        ##################################################
+        # Serial MCU Discovery
 
-        if self.port is None:
-            self.get_logger().info("Unable to find MCU...")
+        # If there was not a port override, look for a MCU over USB for Serial.
+        if self.serial_port is None:
+            comports = serial.tools.list_ports.comports()
+            real_ports = list(
+                filter(
+                    lambda p: p.vid is not None
+                    and p.pid is not None
+                    and p.device is not None,
+                    comports,
+                )
+            )
+            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
+
+            if len(recog_ports) == 1:  # Found singular recognized MCU
+                found_port = recog_ports[0]
+                self.get_logger().info(
+                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
+                )
+                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
+            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
+                # Kinda jank log message
+                self.get_logger().error(
+                    f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            elif (
+                len(recog_ports) == 0 and len(real_ports) > 0
+            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
+                self.get_logger().error(
+                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            else:  # Found jack shit
+                self.get_logger().error("No valid Serial ports specified or found.")
+                # Don't set self.serial_port; later if-statement will exit()
+
+        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
+        # Loop through all serial devices on the computer to check for the MCU
+        if self.serial_port is None:
+            self.get_logger().warning("Falling back to legacy MCU discovery...")
+            ports = Anchor.list_serial_ports()
+            for _ in range(4):
+                if self.serial_port is not None:
+                    break
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        # (f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until(bytes("\n", "utf8"))
+
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.serial_port = port
+                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
+                            break
+                    except:
+                        pass
+
+        # If port is still None then we ain't finding no mcu
+        if self.serial_port is None:
+            self.get_logger().error("Unable to find MCU. Exiting...")
             time.sleep(1)
             sys.exit(1)
+        # Found a Serial port, try to open it; above code has not officially opened a Serial port
+        else:
+            self.get_logger().debug(
+                f"Attempting to open Serial port '{self.serial_port}'..."
+            )
+            try:
+                self.serial_interface = serial.Serial(
+                    self.serial_port, 115200, timeout=1
+                )
 
-        self.ser = serial.Serial(self.port, 115200)
-        self.get_logger().info(f"Enabling Relay Mode")
-        self.ser.write(b"can_relay_mode,on\n")
+                # Attempt to get name of connected MCU
+                self.serial_interface.write(
+                    b"can_relay_mode,on\n"
+                )  # can_relay_ready,[mcu]
+                mcu_name: str = ""
+                for _ in range(4):
+                    response = self.serial_interface.read_until(bytes("\n", "utf8"))
+                    try:
+                        if b"can_relay_ready" in response:
+                            args: list[str] = response.decode("utf8").strip().split(",")
+                            if len(args) == 2:
+                                mcu_name = args[1]
+                            break
+                    except UnicodeDecodeError:
+                        pass  # ignore malformed responses
+                self.get_logger().info(
+                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
+                )
+
+            except serial.SerialException as e:
+                self.get_logger().error(
+                    f"Could not open Serial port '{self.serial_port}' for reason:"
+                )
+                self.get_logger().error(e.strerror)
+                time.sleep(1)
+                sys.exit(1)
+
+        # Close serial port on exit
         atexit.register(self.cleanup)
 
+        ##################################################
+        # ROS2 Topic Setup
+
         # New pub/sub with VicCAN
         self.fromvic_debug_pub_ = self.create_publisher(
             String, "/anchor/from_vic/debug", 20
@@ -115,22 +203,10 @@ class SerialRelay(Node):
             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")
+            output = str(self.serial_interface.readline(), "utf8")
 
             if output:
                 self.relay_fromvic(output)
@@ -156,14 +232,20 @@ class SerialRelay(Node):
         except serial.SerialException as e:
             print(f"SerialException: {e}")
             print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
             exit(1)
         except TypeError as e:
             print(f"TypeError: {e}")
             print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
             exit(1)
         except Exception as e:
             print(f"Exception: {e}")
@@ -184,7 +266,7 @@ class SerialRelay(Node):
             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"))
+        self.serial_interface.write(bytes(output, "utf8"))
 
     def relay_fromvic(self, msg: str):
         """Relay a string message from the MCU to the appropriate VicCAN topic"""
@@ -246,7 +328,7 @@ class SerialRelay(Node):
         """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"))
+        self.serial_interface.write(bytes(message, "utf8"))
 
     @staticmethod
     def list_serial_ports():
@@ -254,28 +336,29 @@ class SerialRelay(Node):
 
     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()
+        if self.serial_interface.is_open:
+            self.serial_interface.close()
 
 
 def main(args=None):
-    rclpy.init(args=args)
-    sys.excepthook = myexcepthook
+    try:
+        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()
-    serial_pub.run()
+        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()
 
 
 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/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, 10
+            ArmManual, "/arm/control/manual", self.send_manual, 2
         )
 
         # 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,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from rclpy import qos
 from rclpy.duration import Duration
 
@@ -11,6 +12,8 @@ import os
 import sys
 import threading
 import glob
+import pwd
+import grp
 from math import copysign
 
 from std_msgs.msg import String
@@ -32,14 +35,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"
@@ -71,9 +74,36 @@ class Headless(Node):
             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()}")
+        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.create_timer(0.15, self.send_controls)
 
@@ -115,7 +145,7 @@ 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
             self.core_publisher.publish(CORE_STOP_MSG)
@@ -296,11 +326,34 @@ 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):
-    rclpy.init(args=args)
-    node = Headless()
-    rclpy.spin(node)
-    rclpy.shutdown()
+    try:
+        rclpy.init(args=args)
+
+        node = Headless()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        print("Caught shutdown signal. Exiting...")
+    finally:
+        rclpy.shutdown()
 
 
 if __name__ == "__main__":

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -43,16 +43,12 @@
 #include <control_msgs/msg/joint_jog.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #include <moveit_msgs/msg/planning_scene.hpp>
-#include <rclcpp/client.hpp>
-#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
+
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/qos_event.hpp>
 #include <rclcpp/subscription.hpp>
+#include <rclcpp/qos.hpp>
 #include <rclcpp/time.hpp>
-#include <rclcpp/utilities.hpp>
-#include <thread>
 
 // We'll just set up parameters here
 const std::string JOY_TOPIC = "/joy";
@@ -104,8 +100,9 @@ std::map<Button, double> BUTTON_DEFAULTS;
  * @return return true if you want to publish a Twist, false if you want to publish a JointJog
  */
 bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
-                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
-                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
+                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist
+                     // std::unique_ptr<control_msgs::msg::JointJog>& joint
+                    )
 {
     // // Give joint jogging priority because it is only buttons
     // // If any joint jog command is requested, we are only publishing joint commands
@@ -236,7 +233,7 @@ public:
         updateCmdFrame(frame_to_publish_, msg->buttons);
 
         // Convert the joystick message to Twist or JointJog and publish
-        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
+        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg))
         {
             // publish the TwistStamped
             twist_msg->header.frame_id = frame_to_publish_;