diff --git a/pyproject.toml b/pyproject.toml index 713993274c..0a06540fad 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -58,7 +58,7 @@ dependencies = [ "future-fstrings", # joystickd - "inputs", + "pygame", # these should be removed "psutil", @@ -105,7 +105,6 @@ dev = [ "matplotlib", "parameterized >=0.8, <0.9", "pyautogui", - "pygame", "pyopencl; platform_machine != 'aarch64'", # broken on arm64 "pytools < 2024.1.11; platform_machine != 'aarch64'", # pyopencl use a broken version "pywinctl", diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 7a37e59d93..dde7728e66 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -110,7 +110,7 @@ procs = [ NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), - PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), + PythonProcess("soundd", "selfdrive.ui.soundd", and_(only_onroad, not_joystick)), PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), diff --git a/tools/joystick/README.md b/tools/joystick/README.md index eb67060ca8..116db3d49c 100644 --- a/tools/joystick/README.md +++ b/tools/joystick/README.md @@ -5,6 +5,10 @@ With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard. joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. +## Note +You might need to install [xow](https://github.com/medusalix/xow) to use the Xbox One controller on comma device. +Even though **xone** is recommended on the xow page, **xow** is the one that works with the comma device. + ## Usage The car must be off, and openpilot must be offroad before starting `joystick_control`. diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py index 11d17e587e..ebbcaf3f27 100755 --- a/tools/joystick/joystick_control.py +++ b/tools/joystick/joystick_control.py @@ -3,7 +3,10 @@ import os import argparse import threading import numpy as np -from inputs import UnpluggedError, get_gamepad +try: + import pygame as pg +except ImportError: + print("pygame is not installed. Please install it with 'uv pip install pygame'") from cereal import messaging from openpilot.common.params import Params @@ -11,7 +14,10 @@ from openpilot.common.realtime import Ratekeeper from openpilot.system.hardware import HARDWARE from openpilot.tools.lib.kbhit import KBHit -EXPO = 0.4 +# Set SDL environment variable to avoid using a video driver for headless operation +os.environ["SDL_VIDEODRIVER"] = "dummy" + +EXPO = 0.4 # Exponential factor for joystick response curve class Keyboard: @@ -28,7 +34,7 @@ class Keyboard: key = self.kb.getch().lower() self.cancel = False if key == 'r': - self.axes_values = dict.fromkeys(self.axes_values, 0.) + self.axes_values = {ax: 0. for ax in self.axes_values} elif key == 'c': self.cancel = True elif key in self.axes_map: @@ -40,60 +46,122 @@ class Keyboard: return True -class Joystick: +class PyGameJoystick: def __init__(self): - # This class supports a PlayStation 5 DualSense controller on the comma 3X - # TODO: find a way to get this from API or detect gamepad/PC, perhaps "inputs" doesn't support it - self.cancel_button = 'BTN_NORTH' # BTN_NORTH=X/triangle - if HARDWARE.get_device_type() == 'pc': - accel_axis = 'ABS_Z' - steer_axis = 'ABS_RX' - # TODO: once the longcontrol API is finalized, we can replace this with outputting gas/brake and steering - self.flip_map = {'ABS_RZ': accel_axis} - else: - accel_axis = 'ABS_RX' - steer_axis = 'ABS_Z' - self.flip_map = {'ABS_RY': accel_axis} + # Initialize pygame and joystick subsystem + pg.init() + if not pg.joystick.get_init(): + pg.joystick.init() - self.min_axis_value = {accel_axis: 0., steer_axis: 0.} - self.max_axis_value = {accel_axis: 255., steer_axis: 255.} - self.axes_values = {accel_axis: 0., steer_axis: 0.} - self.axes_order = [accel_axis, steer_axis] + # Find connected joysticks + joystick_count = pg.joystick.get_count() + if joystick_count == 0: + print("No joysticks found. Please connect a controller.") + exit(1) + + # Initialize the first joystick + self.joystick = pg.joystick.Joystick(0) + self.joystick.init() + + print(f"Using joystick: {self.joystick.get_name()}") + print(f"Number of axes: {self.joystick.get_numaxes()}") + print(f"Number of buttons: {self.joystick.get_numbuttons()}") + print(f"Number of hats: {self.joystick.get_numhats()}") + + # This supports PlayStation and Xbox controllers on different platforms + if HARDWARE.get_device_type() == 'pc': + # Xbox mapping on PC + self.accel_axis = 5 # Right trigger + self.brake_axis = 4 # Left trigger + self.steer_axis = 0 # Left stick horizontal + self.cancel_button = 3 # Y/Triangle button + else: + # PlayStation mapping on comma device + self.accel_axis = 5 # R2 + self.brake_axis = 4 # L2 + self.steer_axis = 0 # Left stick horizontal + self.cancel_button = 3 # Triangle button + + # Configure for adaptive mappings based on detected controller + controller_name = self.joystick.get_name().lower() + if "xbox" in controller_name: + print("Xbox controller detected, using Xbox mappings") + self.accel_axis = 5 # Right trigger (RT) + self.brake_axis = 4 # Left trigger (LT) + self.cancel_button = 3 # Y button + elif "playstation" in controller_name or "dual" in controller_name: + print("PlayStation controller detected, using PlayStation mappings") + self.accel_axis = 5 # R2 + self.brake_axis = 4 # L2 + self.cancel_button = 3 # Triangle + + # Initialize values + self.axes_values = {'gb': 0., 'steer': 0.} # Maintain same keys as Keyboard class + self.axes_order = ['gb', 'steer'] # Match expected format self.cancel = False + self.deadzone = 0.03 # 3% deadzone for noisy joysticks + + # Process events once to clear the event queue + pg.event.pump() def update(self): + # Process pygame events try: - joystick_event = get_gamepad()[0] - except (OSError, UnpluggedError): - self.axes_values = dict.fromkeys(self.axes_values, 0.) + for event in pg.event.get(): + if event.type == pg.JOYDEVICEREMOVED: + if event.instance_id == self.joystick.get_instance_id(): + print("Joystick disconnected!") + self.axes_values = {'gb': 0., 'steer': 0.} + return False + + elif event.type == pg.JOYBUTTONDOWN: + if event.button == self.cancel_button: + self.cancel = True + + elif event.type == pg.JOYBUTTONUP: + if event.button == self.cancel_button: + self.cancel = False + except Exception as e: + print(f"Error processing events: {e}") return False - event = (joystick_event.code, joystick_event.state) + # Read current joystick state directly + try: + if not self.joystick.get_init(): + print("Joystick not initialized") + return False - # flip left trigger to negative accel - if event[0] in self.flip_map: - event = (self.flip_map[event[0]], -event[1]) + # Read steering (left stick horizontal) + steer_raw = self.joystick.get_axis(self.steer_axis) * -1 + steer = steer_raw if abs(steer_raw) > self.deadzone else 0.0 - if event[0] == self.cancel_button: - if event[1] == 1: - self.cancel = True - elif event[1] == 0: # state 0 is falling edge - self.cancel = False - elif event[0] in self.axes_values: - self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]]) - self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]]) + # Read gas (right trigger) + accel_raw = self.joystick.get_axis(self.accel_axis) + # Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed) + accel = (accel_raw + 1) / 2 if self.accel_axis in [4, 5] else accel_raw + accel = accel if accel > self.deadzone else 0.0 - norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])) - norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3% - self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control - else: + # Read brake (left trigger) + brake_raw = self.joystick.get_axis(self.brake_axis) + # Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed) + brake = (brake_raw + 1) / 2 if self.brake_axis in [4, 5] else brake_raw + brake = brake if brake > self.deadzone else 0.0 + + # Apply expo for steering + self.axes_values['steer'] = EXPO * steer**3 + (1 - EXPO) * steer # Apply expo for fine control + + # Calculate combined gas/brake value for output [-1, 1] where negative is brake + self.axes_values['gb'] = accel - brake + + except Exception as e: + print(f"Error reading joystick: {e}") + self.axes_values = {'gb': 0., 'steer': 0.} return False return True def send_thread(joystick): pm = messaging.PubMaster(['testJoystick']) - rk = Ratekeeper(100, print_delay_threshold=None) while True: @@ -105,7 +173,6 @@ def send_thread(joystick): joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order] pm.send('testJoystick', joystick_msg) - rk.keep_time() @@ -117,13 +184,12 @@ def joystick_control_thread(joystick): def main(): - joystick_control_thread(Joystick()) - + joystick_control_thread(PyGameJoystick()) if __name__ == '__main__': parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + 'openpilot must be offroad before starting joystick_control. This tool supports ' + - 'a PlayStation 5 DualSense controller on the comma 3X.', + 'PlayStation and Xbox controllers on various platforms.', formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') args = parser.parse_args() @@ -139,9 +205,14 @@ if __name__ == '__main__': print('Buttons') print('- `R`: Resets axes') print('- `C`: Cancel cruise control') + joystick = Keyboard() else: - print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!') - print('If not running on a comma device, the mapping may need to be adjusted.') + print('Using pygame joystick') + print('Standard controller mapping:') + print('- Left stick: Steering') + print('- Right trigger (R2): Gas') + print('- Left trigger (L2): Brake') + print('- Triangle/Y button: Cancel') + joystick = PyGameJoystick() - joystick = Keyboard() if args.keyboard else Joystick() joystick_control_thread(joystick) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index b775f18f70..6e5ea890dd 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -19,18 +19,24 @@ def joystickd_thread(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) VM = VehicleModel(CP) - sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL) + sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick', 'selfdriveStateSP'], frequency=1. / DT_CTRL) pm = messaging.PubMaster(['carControl', 'controlsState']) rk = Ratekeeper(100, print_delay_threshold=None) while 1: sm.update(0) + ss_sp = sm['selfdriveStateSP'] + _lat_active = False + if ss_sp.mads.available: + _lat_active = ss_sp.mads.active + else: + _lat_active = sm['selfdriveState'].active cc_msg = messaging.new_message('carControl') cc_msg.valid = True CC = cc_msg.carControl CC.enabled = sm['selfdriveState'].enabled - CC.latActive = sm['selfdriveState'].active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent + CC.latActive = _lat_active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in sm['onroadEvents']) and CP.openpilotLongitudinalControl CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise) CC.hudControl.leadDistanceBars = 2 diff --git a/uv.lock b/uv.lock index aa49b605a5..d76f71070c 100644 --- a/uv.lock +++ b/uv.lock @@ -693,15 +693,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/2c/e1/e6716421ea10d38022b952c159d5161ca1193197fb744506875fbb87ea7b/iniconfig-2.1.0-py3-none-any.whl", hash = "sha256:9deba5723312380e77435581c6bf4935c94cbfab9b1ed33ef8d238ea168eb760", size = 6050 }, ] -[[package]] -name = "inputs" -version = "0.5" -source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/d1/cd/5f434220920f76eb73d19bb7aab8d857445f40aa642718e6e51e850cd663/inputs-0.5.tar.gz", hash = "sha256:a31d5b96a3525f1232f326be9e7ce8ccaf873c6b1fb84d9f3c9bc3d79b23eae4", size = 33393 } -wheels = [ - { url = "https://files.pythonhosted.org/packages/d0/94/040a0d9c81f018c39bd887b7b825013b024deb0a6c795f9524797e2cd41b/inputs-0.5-py2.py3-none-any.whl", hash = "sha256:13f894564e52134cf1e3862b1811da034875eb1f2b62e6021e3776e9669a96ec", size = 33630 }, -] - [[package]] name = "isodate" version = "0.7.2" @@ -1251,7 +1242,6 @@ dependencies = [ { name = "crcmod" }, { name = "cython" }, { name = "future-fstrings" }, - { name = "inputs" }, { name = "json-rpc" }, { name = "libusb1" }, { name = "llvmlite" }, @@ -1261,6 +1251,7 @@ dependencies = [ { name = "pyaudio" }, { name = "pycapnp" }, { name = "pycryptodome" }, + { name = "pygame" }, { name = "pyjwt" }, { name = "pyopenssl" }, { name = "pyserial" }, @@ -1290,7 +1281,6 @@ dev = [ { name = "matplotlib" }, { name = "parameterized" }, { name = "pyautogui" }, - { name = "pygame" }, { name = "pyopencl", marker = "platform_machine != 'aarch64'" }, { name = "pyprof2calltree" }, { name = "pytools", marker = "platform_machine != 'aarch64'" }, @@ -1345,7 +1335,6 @@ requires-dist = [ { name = "dictdiffer", marker = "extra == 'dev'" }, { name = "future-fstrings" }, { name = "hypothesis", marker = "extra == 'testing'", specifier = "==6.47.*" }, - { name = "inputs" }, { name = "jinja2", marker = "extra == 'docs'" }, { name = "json-rpc" }, { name = "libusb1" }, @@ -1365,7 +1354,7 @@ requires-dist = [ { name = "pyautogui", marker = "extra == 'dev'" }, { name = "pycapnp" }, { name = "pycryptodome" }, - { name = "pygame", marker = "extra == 'dev'" }, + { name = "pygame" }, { name = "pyjwt" }, { name = "pyopencl", marker = "platform_machine != 'aarch64' and extra == 'dev'" }, { name = "pyopenssl", specifier = "<24.3.0" },