From 4c1416851e0bdef39e24b36f98fb8704dafd625e Mon Sep 17 00:00:00 2001 From: David Date: Mon, 10 Nov 2025 21:58:03 -0600 Subject: [PATCH 01/10] style: move pub/sub docs comment, rename SerialPub to Anchor --- src/anchor_pkg/anchor_pkg/anchor_node.py | 43 ++++++++++++------------ 1 file changed, 21 insertions(+), 22 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 44a6879..37b6079 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -19,28 +19,27 @@ serial_pub = None thread = None -""" -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' @@ -49,7 +48,7 @@ class SerialRelay(Node): self.port = None if port_override := os.getenv("PORT_OVERRIDE"): self.port = port_override - ports = SerialRelay.list_serial_ports() + ports = Anchor.list_serial_ports() for i in range(4): if self.port is not None: break @@ -270,7 +269,7 @@ def main(args=None): global serial_pub - serial_pub = SerialRelay() + serial_pub = Anchor() serial_pub.run() From 96f5eda0056f3bee1d284d3ca4b7c8a36e9d80db Mon Sep 17 00:00:00 2001 From: David Date: Mon, 10 Nov 2025 22:02:49 -0600 Subject: [PATCH 02/10] feat: (headless) detect incorrectly connected controller --- src/headless_pkg/src/headless_node.py | 33 +++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index 2ced85d..aba1676 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -11,6 +11,8 @@ import os import sys import threading import glob +import pwd +import grp from math import copysign from std_msgs.msg import String @@ -75,6 +77,19 @@ class Headless(Node): self.gamepad.init() print(f"Gamepad Found: {self.gamepad.get_name()}") + if self.gamepad.get_numhats() == 0: + 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." + ) + time.sleep(1) + sys.exit(1) + self.create_timer(0.15, self.send_controls) self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2) @@ -296,6 +311,24 @@ 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() From b84ca6757d69908f5bfb657fa2f0639f785888c7 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 10 Nov 2025 22:45:43 -0600 Subject: [PATCH 03/10] refactor: (anchor) cleanup structural ros2 code --- src/anchor_pkg/anchor_pkg/anchor_node.py | 46 ++++++++++-------------- 1 file changed, 19 insertions(+), 27 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 37b6079..fa825f4 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/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 @@ -15,9 +16,6 @@ import glob from std_msgs.msg import String, Header from astra_msgs.msg import VicCAN -serial_pub = None -thread = None - class Anchor(Node): """ @@ -76,8 +74,13 @@ class Anchor(Node): self.ser = serial.Serial(self.port, 115200) self.get_logger().info(f"Enabling Relay Mode") self.ser.write(b"can_relay_mode,on\n") + + # 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 @@ -114,18 +117,6 @@ class Anchor(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: @@ -257,24 +248,25 @@ class Anchor(Node): self.ser.close() -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 + 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 = Anchor() - 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 From 5e7776631dfa3ce8e335b3f27334559de21fb006 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 10 Nov 2025 23:24:14 -0600 Subject: [PATCH 04/10] feat: (anchor) add new Serial finder code Uses vendor and product ids to find a microcontroller, and detects its name after connecting. Upon failure, falls back to Areeb's code--just in case. Also renamed `self.ser` to `self.serial_interface` and `self.port` to `self.serial_port` for clarity. --- src/anchor_pkg/anchor_pkg/anchor_node.py | 162 ++++++++++++++++++----- 1 file changed, 127 insertions(+), 35 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index fa825f4..ea8c5f4 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -8,6 +8,7 @@ import time import atexit import serial +import serial.tools.list_ports import os import sys import threading @@ -16,6 +17,13 @@ import glob from std_msgs.msg import String, Header from astra_msgs.msg import VicCAN +KNOWN_USBS = [ + (0x2E8A, 0x00C0), # Raspberry Pi Pico + (0x1A86, 0x55D4), # Adafruit Feather ESP32 V2 + (0x10C4, 0xEA60), # DOIT ESP32 Devkit V1 + (0x1A86, 0x55D3), # ESP32 S3 Development Board +] + class Anchor(Node): """ @@ -42,38 +50,116 @@ class Anchor(Node): # 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 = Anchor.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 + 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__()}" + ) + # time.sleep(1) + # sys.exit(1) + elif ( + len(recog_ports) == 0 and len(real_ports) > 0 + ): # Found real ports but none recognized + self.get_logger().error( + f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}." + ) + # time.sleep(1) + # sys.exit(1) + else: # Found jack shit + self.get_logger().error("No valid Serial ports specified or found.") + # time.sleep(1) + # sys.exit(1) + + # We still don't have a serial port; fall back to legacy discovery (Areeb's code) + # Loop through all serial devices on the computer to check for the MCU + if self.serial_port is None: + self.get_logger().warning("Falling back to legacy MCU discovery...") + ports = Anchor.list_serial_ports() + for _ in range(4): + if self.serial_port is not None: + break + for port in ports: + try: + # connect and send a ping command + ser = serial.Serial(port, 115200, timeout=1) + # (f"Checking port {port}...") + ser.write(b"ping\n") + response = ser.read_until(bytes("\n", "utf8")) + + # if pong is in response, then we are talking with the MCU + if b"pong" in response: + self.serial_port = port + self.get_logger().info(f"Found MCU at {self.serial_port}!") + break + except: + pass + + # If port is still None then we ain't finding no mcu + if self.serial_port is None: + self.get_logger().error("Unable to find MCU. Exiting...") time.sleep(1) sys.exit(1) + # Found a Serial port, try to open it; above code has not officially opened a Serial port + else: + self.get_logger().debug( + f"Attempting to open Serial port '{self.serial_port}'..." + ) + try: + self.serial_interface = serial.Serial( + self.serial_port, 115200, timeout=1 + ) - 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")) + if b"can_relay_ready" in response: + args: list[str] = response.decode("utf8").strip().split(",") + if len(args) == 2: + mcu_name = args[1] + break + 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) @@ -120,7 +206,7 @@ class Anchor(Node): 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) @@ -146,14 +232,20 @@ class Anchor(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}") @@ -174,7 +266,7 @@ class Anchor(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""" @@ -236,7 +328,7 @@ class Anchor(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(): @@ -244,8 +336,8 @@ class Anchor(Node): def cleanup(self): print("Cleaning up before terminating...") - if self.ser.is_open: - self.ser.close() + if self.serial_interface.is_open: + self.serial_interface.close() def main(args=None): From 3bb3771dce2daedaff299b2ad867f89e58210e2e Mon Sep 17 00:00:00 2001 From: David Date: Tue, 11 Nov 2025 13:18:36 -0600 Subject: [PATCH 05/10] fix: (anchor) ignore UnicodeDecodeError when getting mcu name --- src/anchor_pkg/anchor_pkg/anchor_node.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index ea8c5f4..59f5780 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -144,11 +144,14 @@ class Anchor(Node): mcu_name: str = "" for _ in range(4): response = self.serial_interface.read_until(bytes("\n", "utf8")) - if b"can_relay_ready" in response: - args: list[str] = response.decode("utf8").strip().split(",") - if len(args) == 2: - mcu_name = args[1] - break + 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}'." ) From 40fa0d0ab8403496746a46e74fd4d529b9fc8a90 Mon Sep 17 00:00:00 2001 From: David Date: Fri, 21 Nov 2025 17:06:37 -0600 Subject: [PATCH 06/10] style: (anchor) better comment serial finding --- src/anchor_pkg/anchor_pkg/anchor_node.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 59f5780..a2374fa 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -77,26 +77,23 @@ class Anchor(Node): self.get_logger().info( f"Selecting MCU '{found_port.description}' at {found_port.device}." ) - self.serial_port = 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__()}" ) - # time.sleep(1) - # sys.exit(1) + # 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 + ): # 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__()}." ) - # time.sleep(1) - # sys.exit(1) + # 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.") - # time.sleep(1) - # sys.exit(1) + # 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 From df78575206f31d4765d0ca2706024d0462329b6e Mon Sep 17 00:00:00 2001 From: David Date: Sat, 13 Dec 2025 16:23:42 -0600 Subject: [PATCH 07/10] feat: (headless) add Ctrl+C try-except --- src/headless_pkg/src/headless_node.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index aba1676..bb02a2d 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/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 @@ -330,10 +331,15 @@ def is_user_in_group(group_name: str) -> bool: 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__": From c10a2a5ccafb284f10269aa6e3ebfd4f78d1ab91 Mon Sep 17 00:00:00 2001 From: ryleu <69326171+ryleu@users.noreply.github.com> Date: Wed, 14 Jan 2026 04:12:05 -0500 Subject: [PATCH 08/10] patch autostart scripts for nixos --- auto_start/auto_start_anchor.sh | 6 +++++- auto_start/auto_start_headless_full.sh | 6 +++++- auto_start/start_rosbag.sh | 6 +++++- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/auto_start/auto_start_anchor.sh b/auto_start/auto_start_anchor.sh index 12419bd..9eebc57 100755 --- a/auto_start/auto_start_anchor.sh +++ b/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 diff --git a/auto_start/auto_start_headless_full.sh b/auto_start/auto_start_headless_full.sh index 8a014e1..8fb6e25 100755 --- a/auto_start/auto_start_headless_full.sh +++ b/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 diff --git a/auto_start/start_rosbag.sh b/auto_start/start_rosbag.sh index dcc07d7..ac00fa0 100755 --- a/auto_start/start_rosbag.sh +++ b/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 From 0e775c65c621ea1de9deed71f4718d6891a275c5 Mon Sep 17 00:00:00 2001 From: SHC-ASTRA <90978381+ASTRA-SHC@users.noreply.github.com> Date: Wed, 14 Jan 2026 04:56:55 -0600 Subject: [PATCH 09/10] add trying multiple controllers to headless --- src/headless_pkg/src/headless_node.py | 37 ++++++++++++++++----------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index bb02a2d..1d72d6e 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -74,22 +74,29 @@ 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: + if id >= pygame.joystick.get_count(): + self.get_logger().fatal("Ran out of controllers to try") + sys.exit(1) - if self.gamepad.get_numhats() == 0: - 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." - ) - time.sleep(1) - sys.exit(1) + self.gamepad = pygame.joystick.Joystick(id) + self.gamepad.init() + 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) From b5be93e5f6c2dfa68754b230820870e3ce91e6b0 Mon Sep 17 00:00:00 2001 From: SHC-ASTRA <90978381+ASTRA-SHC@users.noreply.github.com> Date: Wed, 14 Jan 2026 19:49:33 -0600 Subject: [PATCH 10/10] add an error instead of a crash when a gamepad fails to initialize --- src/headless_pkg/src/headless_node.py | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index 1d72d6e..50717c5 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -76,12 +76,19 @@ class Headless(Node): # Initialize the gamepad id = 0 while True: - if id >= pygame.joystick.get_count(): + 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.gamepad = pygame.joystick.Joystick(id) - self.gamepad.init() + 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: @@ -138,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)