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

@@ -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;
   }
 }

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.),

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("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)),
-  PythonProcess("joystickd", "tools.joystick.joystickd", joystick),
+  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}

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)

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.
+With joystick_control, 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.
+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
    ```
 
 ---

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)

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 joystickd_thread():
-  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):
   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):
+def main():
-  Params().put_bool('JoystickDebugMode', True)
+  joystickd_thread()
-  threading.Thread(target=send_thread, args=(joystick,), daemon=True).start()
-  while True:
-    joystick.update()
 
 
-def main():
+if __name__ == "__main__":
-  joystick_thread(Joystick())
+  main()
-
-
-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)