openpilot v0.9.6 release

date: 2024-02-21T23:02:42
master commit: 0b4d08fab8e35a264bc7383e878538f8083c33e5

tools/joystick/README.md

@@ -0,0 +1,53 @@
+# Joystick
+
+**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.
+
+## Usage
+
+The car must be off, and openpilot must be offroad before starting `joystickd`.
+
+### Using a keyboard
+
+SSH into your comma device and start joystickd with the following command:
+
+```shell
+tools/joystick/joystickd.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`.
+
+### 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.
+
+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:
+   ```shell
+   # on your comma device
+   echo -n "1" > /data/params/d/JoystickDebugMode
+   ```
+3. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them:
+   ```shell
+   # on your comma device
+   cereal/messaging/bridge {LAPTOP_IP} testJoystick
+   ```
+4. Start joystickd on your laptop in ZMQ mode.
+   ```shell
+   # on your laptop
+   export ZMQ=1
+   tools/joystick/joystickd.py
+   ```
+
+---
+Now start your car and openpilot should go into joystick mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell.
+
+Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls.
+
+![](https://github.com/commaai/openpilot/assets/8762862/e640cbca-cb7a-4dcb-abce-b23b036ad8e7)

tools/joystick/joystickd.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python
+import os
+import argparse
+import threading
+from inputs import get_gamepad
+
+import cereal.messaging as messaging
+from openpilot.common.realtime import Ratekeeper
+from openpilot.common.numpy_fast import interp, clip
+from openpilot.common.params import Params
+from openpilot.tools.lib.kbhit import KBHit
+
+
+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, gamepad=False):
+    # TODO: find a way to get this from API, perhaps "inputs" doesn't support it
+    if gamepad:
+      self.cancel_button = 'BTN_NORTH'  # (BTN_NORTH=X, ABS_RZ=Right Trigger)
+      accel_axis = 'ABS_Y'
+      steer_axis = 'ABS_RX'
+    else:
+      self.cancel_button = 'BTN_TRIGGER'
+      accel_axis = 'ABS_Y'
+      steer_axis = 'ABS_RX'
+    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):
+    joystick_event = get_gamepad()[0]
+    event = (joystick_event.code, joystick_event.state)
+    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.])
+      self.axes_values[event[0]] = norm if abs(norm) > 0.05 else 0.  # center can be noisy, deadzone of 5%
+    else:
+      return False
+    return True
+
+
+def send_thread(joystick):
+  joystick_sock = messaging.pub_sock('testJoystick')
+  rk = Ratekeeper(100, print_delay_threshold=None)
+  while 1:
+    dat = messaging.new_message('testJoystick')
+    dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_order]
+    dat.testJoystick.buttons = [joystick.cancel]
+    joystick_sock.send(dat.to_bytes())
+    print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()))
+    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()
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
+                                               'openpilot must be offroad before starting joysticked.',
+                                   formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+  parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
+  parser.add_argument('--gamepad', action='store_true', help='Use gamepad configuration instead of 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!')
+
+  joystick = Keyboard() if args.keyboard else Joystick(args.gamepad)
+  joystick_thread(joystick)