mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-23 14:22:06 +08:00
Handle missing pygame import gracefully
Wrap the pygame import in a try-except block to catch ImportError. This prevents the script from crashing and provides a clear message prompting the user to install pygame if it's missing. Remove "inputs" package and update "pygame" dependency The "inputs" package has been removed from the lockfile and dependency list, while "pygame" is now included universally without the "dev" extra marker. This change simplifies dependencies and ensures consistency across environments. Update dependencies: replace 'inputs' with 'pygame' Replaced the 'inputs' library with 'pygame' for joystickd dependencies in `pyproject.toml`. Additionally, removed a redundant 'pygame' entry from the general dependencies. Ugly, I know, but soundd is unhappy with joystick Allowing lat with mads Invert steering input for joystick control The steering axis input is now multiplied by -1 to reverse its direction. This ensures correct handling of the left stick's horizontal input, aligning behavior with expected control dynamics. Refactor joystick control to use pygame for broader support Replaced the `inputs` library with `pygame` for joystick handling, providing improved compatibility with Xbox and PlayStation controllers. Added initialization, adaptive mappings, deadzone handling, and enhanced event processing for robust joystick operation. Updated README with dependencies and usage information for Xbox controllers.
This commit is contained in:
DevTekVE
+1
-2
@@ -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",
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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`.
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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" },
|
||||
|
||||
Reference in New Issue
Block a user