From d82c4509ea6f6e85da04bd8ba9175c35fb91caf0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 23 Sep 2024 20:47:28 -0700 Subject: [PATCH] joystickd: split into joystickd and joystick_control (#33632) * Split joystickd into joystickd and joystick_control * Update process config * Undeprecate testJoystick * Static analysis fixes * Mark as +x * Update README * Add testJoystick back to services * reset if testJoystick not received * Fix quotes * Remove self * Add a send thread instead * Add joystick_control into process config * Add main * Add additional condition * Fix imports --- cereal/log.capnp | 2 +- cereal/services.py | 1 + system/manager/process_config.py | 12 +- system/webrtc/tests/test_stream_session.py | 2 +- tools/joystick/README.md | 20 +-- tools/joystick/joystick_control.py | 147 +++++++++++++++++++++ tools/joystick/joystickd.py | 141 +++----------------- 7 files changed, 186 insertions(+), 139 deletions(-) create mode 100755 tools/joystick/joystick_control.py diff --git a/cereal/log.capnp b/cereal/log.capnp index 87be2ceb99..c955cbf154 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2413,6 +2413,7 @@ struct Event { uiDebug @102 :UIDebug; # *********** debug *********** + testJoystick @52 :Joystick; roadEncodeData @86 :EncodeData; driverEncodeData @87 :EncodeData; wideRoadEncodeData @88 :EncodeData; @@ -2482,6 +2483,5 @@ struct Event { uiPlanDEPRECATED @106 :UiPlan; liveLocationKalmanDEPRECATED @72 :LiveLocationKalman; liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED); - testJoystickDEPRECATED @52 :Joystick; } } diff --git a/cereal/services.py b/cereal/services.py index e56f8f0c1b..771338f507 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -75,6 +75,7 @@ _services: dict[str, tuple] = { # debug "uiDebug": (True, 0., 1), + "testJoystick": (True, 0.), "alertDebug": (True, 20., 5), "roadEncodeData": (False, 20.), "driverEncodeData": (False, 20.), diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 538c55813b..bdb549fa41 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -1,4 +1,5 @@ import os +import operator from cereal import car from openpilot.common.params import Params @@ -53,6 +54,12 @@ def only_onroad(started: bool, params, CP: car.CarParams) -> bool: def only_offroad(started, params, CP: car.CarParams) -> bool: return not started +def or_(*fns): + return lambda *args: operator.or_(*(fn(*args) for fn in fns)) + +def and_(*fns): + return lambda *args: operator.and_(*(fn(*args) for fn in fns)) + procs = [ DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), @@ -75,8 +82,8 @@ procs = [ NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), - PythonProcess("controlsd", "selfdrive.controls.controlsd", not_joystick), - PythonProcess("joystickd", "tools.joystick.joystickd", joystick), + PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)), + PythonProcess("joystickd", "tools.joystick.joystickd", or_(joystick, notcar)), PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), @@ -100,6 +107,7 @@ procs = [ NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), + PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)), ] managed_processes = {p.name: p for p in procs} diff --git a/system/webrtc/tests/test_stream_session.py b/system/webrtc/tests/test_stream_session.py index 6683c65804..113fa5e7e6 100644 --- a/system/webrtc/tests/test_stream_session.py +++ b/system/webrtc/tests/test_stream_session.py @@ -50,7 +50,7 @@ class TestStreamSession: tested_msgs = [ {"type": "customReservedRawData0", "data": "test"}, # primitive {"type": "can", "data": [{"address": 0, "dat": "", "src": 0}]}, # list - {"type": "testJoystickDEPRECATED", "data": {"axes": [0, 0], "buttons": [False]}}, # dict + {"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict ] mocked_pubmaster = mocker.MagicMock(spec=messaging.PubMaster) diff --git a/tools/joystick/README.md b/tools/joystick/README.md index aea93dad53..eb67060ca8 100644 --- a/tools/joystick/README.md +++ b/tools/joystick/README.md @@ -2,33 +2,33 @@ **Hardware needed**: device running openpilot, laptop, joystick (optional) -With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard. -joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. +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. ## Usage -The car must be off, and openpilot must be offroad before starting `joystickd`. +The car must be off, and openpilot must be offroad before starting `joystick_control`. ### Using a keyboard -SSH into your comma device and start joystickd with the following command: +SSH into your comma device and start joystick_control with the following command: ```shell -tools/joystick/joystickd.py --keyboard +tools/joystick/joystick_control.py --keyboard ``` The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments. ### Joystick on your comma three -Plug the joystick into your comma three aux USB-C port. Then, SSH into the device and start `joystickd.py`. +Plug the joystick into your comma three aux USB-C port. Then, SSH into the device and start `joystick_control.py`. ### Joystick on your laptop -In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` packets over the network to the comma device. +In order to use a joystick over the network, we need to run joystick_control locally from your laptop and have it send `testJoystick` packets over the network to the comma device. 1. Connect a joystick to your PC. -2. Connect your laptop to your comma device's hotspot and open a new SSH shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: +2. Connect your laptop to your comma device's hotspot and open a new SSH shell. Since joystick_control is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: ```shell # on your comma device echo -n "1" > /data/params/d/JoystickDebugMode @@ -38,11 +38,11 @@ In order to use a joystick over the network, we need to run joystickd locally fr # on your comma device cereal/messaging/bridge {LAPTOP_IP} testJoystick ``` -4. Start joystickd on your laptop in ZMQ mode. +4. Start joystick_control on your laptop in ZMQ mode. ```shell # on your laptop export ZMQ=1 - tools/joystick/joystickd.py + tools/joystick/joystick_control.py ``` --- diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py new file mode 100755 index 0000000000..d31546e058 --- /dev/null +++ b/tools/joystick/joystick_control.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +import os +import argparse +import threading +from inputs import UnpluggedError, get_gamepad + +from cereal import messaging +from openpilot.common.numpy_fast import interp, clip +from openpilot.common.params import Params +from openpilot.common.realtime import Ratekeeper +from openpilot.system.hardware import HARDWARE +from openpilot.tools.lib.kbhit import KBHit + +EXPO = 0.4 + + +class Keyboard: + def __init__(self): + self.kb = KBHit() + self.axis_increment = 0.05 # 5% of full actuation each key press + self.axes_map = {'w': 'gb', 's': 'gb', + 'a': 'steer', 'd': 'steer'} + self.axes_values = {'gb': 0., 'steer': 0.} + self.axes_order = ['gb', 'steer'] + self.cancel = False + + def update(self): + key = self.kb.getch().lower() + self.cancel = False + if key == 'r': + self.axes_values = {ax: 0. for ax in self.axes_values} + elif key == 'c': + self.cancel = True + elif key in self.axes_map: + axis = self.axes_map[key] + incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment + self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1) + else: + return False + return True + + +class Joystick: + 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} + + 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] + self.cancel = False + + def update(self): + try: + joystick_event = get_gamepad()[0] + except (OSError, UnpluggedError): + self.axes_values = {ax: 0. for ax in self.axes_values} + return False + + event = (joystick_event.code, joystick_event.state) + + # flip left trigger to negative accel + if event[0] in self.flip_map: + event = (self.flip_map[event[0]], -event[1]) + + 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]]) + + norm = -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: + return False + return True + + +def send_thread(joystick): + pm = messaging.PubMaster(['testJoystick']) + + rk = Ratekeeper(100, print_delay_threshold=None) + + while True: + if rk.frame % 20 == 0: + print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) + + joystick_msg = messaging.new_message('testJoystick') + joystick_msg.valid = True + joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order] + + pm.send('testJoystick', joystick_msg) + + rk.keep_time() + + +def joystick_control_thread(joystick): + Params().put_bool('JoystickDebugMode', True) + threading.Thread(target=send_thread, args=(joystick,), daemon=True).start() + while True: + joystick.update() + + +def main(): + joystick_control_thread(Joystick()) + + +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.', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') + args = parser.parse_args() + + if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ: + print("The car must be off before running joystick_control.") + exit() + + print() + if args.keyboard: + print('Gas/brake control: `W` and `S` keys') + print('Steering control: `A` and `D` keys') + print('Buttons') + print('- `R`: Resets axes') + print('- `C`: Cancel cruise control') + 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.') + + joystick = Keyboard() if args.keyboard else Joystick() + joystick_control_thread(joystick) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index f39dec7d4b..f8c000c259 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -1,117 +1,30 @@ #!/usr/bin/env python3 -import os -import argparse -import threading -from inputs import UnpluggedError, get_gamepad + import math from cereal import messaging, car +from openpilot.common.numpy_fast import clip from openpilot.common.realtime import DT_CTRL, Ratekeeper -from openpilot.common.numpy_fast import interp, clip from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.system.hardware import HARDWARE -from openpilot.tools.lib.kbhit import KBHit -EXPO = 0.4 MAX_LAT_ACCEL = 2.5 -class Keyboard: - def __init__(self): - self.kb = KBHit() - self.axis_increment = 0.05 # 5% of full actuation each key press - self.axes_map = {'w': 'gb', 's': 'gb', - 'a': 'steer', 'd': 'steer'} - self.axes_values = {'gb': 0., 'steer': 0.} - self.axes_order = ['gb', 'steer'] - self.cancel = False - - def update(self): - key = self.kb.getch().lower() - self.cancel = False - if key == 'r': - self.axes_values = {ax: 0. for ax in self.axes_values} - elif key == 'c': - self.cancel = True - elif key in self.axes_map: - axis = self.axes_map[key] - incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment - self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1) - else: - return False - return True - - -class Joystick: - 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} - - 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] - self.cancel = False - - def update(self): - try: - joystick_event = get_gamepad()[0] - except (OSError, UnpluggedError): - self.axes_values = {ax: 0. for ax in self.axes_values} - return False - - event = (joystick_event.code, joystick_event.state) - - # flip left trigger to negative accel - if event[0] in self.flip_map: - event = (self.flip_map[event[0]], -event[1]) - - 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]]) - - norm = -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: - return False - return True - - -def send_thread(joystick): +def joystickd_thread(): params = Params() cloudlog.info("joystickd is waiting for CarParams") CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) VM = VehicleModel(CP) - sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState'], frequency=1. / DT_CTRL) + sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL) pm = messaging.PubMaster(['carControl', 'controlsState']) rk = Ratekeeper(100, print_delay_threshold=None) while 1: sm.update(0) - joystick_axes = [joystick.axes_values[a] for a in joystick.axes_order] - if rk.frame % 20 == 0: - print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) - cc_msg = messaging.new_message('carControl') cc_msg.valid = True CC = cc_msg.carControl @@ -121,6 +34,14 @@ def send_thread(joystick): actuators = CC.actuators + # reset joystick if it hasn't been received in a while + should_reset_joystick = sm.recv_frame['testJoystick'] == 0 or (sm.frame - sm.recv_frame['testJoystick'])*DT_CTRL > 0.2 + + if not should_reset_joystick: + joystick_axes = sm['testJoystick'].axes + else: + joystick_axes = [0.0, 0.0] + if CC.longActive: actuators.accel = 4.0 * clip(joystick_axes[0], -1, 1) @@ -142,39 +63,9 @@ def send_thread(joystick): rk.keep_time() -def joystick_thread(joystick): - Params().put_bool('JoystickDebugMode', True) - threading.Thread(target=send_thread, args=(joystick,), daemon=True).start() - while True: - joystick.update() +def main(): + joystickd_thread() -def main(): - joystick_thread(Joystick()) - - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + - 'openpilot must be offroad before starting joystickd. This tool supports ' + - 'a PlayStation 5 DualSense controller on the comma 3X.', - formatter_class=argparse.ArgumentDefaultsHelpFormatter) - parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') - args = parser.parse_args() - - if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ: - print("The car must be off before running joystickd.") - exit() - - print() - if args.keyboard: - print('Gas/brake control: `W` and `S` keys') - print('Steering control: `A` and `D` keys') - print('Buttons') - print('- `R`: Resets axes') - print('- `C`: Cancel cruise control') - 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.') - - joystick = Keyboard() if args.keyboard else Joystick() - joystick_thread(joystick) +if __name__ == "__main__": + main()