Release 260111

tools/joystick/README.md

@@ -0,0 +1,53 @@
+# Joystick
+
+**Hardware needed**: device running openpilot, laptop, joystick (optional)
+
+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 `joystick_control`.
+
+### Using a keyboard
+
+SSH into your comma device and start joystick_control with the following command:
+
+```shell
+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 `joystick_control.py`.
+
+### Joystick on your laptop
+
+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 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
+   ```
+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 joystick_control on your laptop in ZMQ mode.
+   ```shell
+   # on your laptop
+   export ZMQ=1
+   tools/joystick/joystick_control.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/joystick_control.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python3
+import os
+import argparse
+import threading
+import numpy as np
+from inputs import UnpluggedError, get_gamepad
+
+from cereal import messaging
+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 = dict.fromkeys(self.axes_values, 0.)
+    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] = float(np.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 = dict.fromkeys(self.axes_values, 0.)
+      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 = -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:
+      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

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+
+import math
+import numpy as np
+
+from cereal import messaging, car
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.common.realtime import DT_CTRL, Ratekeeper
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+LongCtrlState = car.CarControl.Actuators.LongControlState
+MAX_LAT_ACCEL = 2.5
+
+
+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', 'testJoystick'], frequency=1. / DT_CTRL)
+  pm = messaging.PubMaster(['carControl', 'controlsState'])
+
+  rk = Ratekeeper(100, print_delay_threshold=None)
+  while 1:
+    sm.update(0)
+
+    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.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
+
+    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 * float(np.clip(joystick_axes[0], -1, 1))
+      actuators.longControlState = LongCtrlState.pid if sm['carState'].vEgo > CP.vEgoStopping else LongCtrlState.stopping
+
+    if CC.latActive:
+      max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
+      max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))
+
+      actuators.torque = float(np.clip(joystick_axes[1], -1, 1))
+      actuators.steeringAngleDeg, actuators.curvature = actuators.torque * max_angle, actuators.torque * -max_curvature
+
+    pm.send('carControl', cc_msg)
+
+    cs_msg = messaging.new_message('controlsState')
+    cs_msg.valid = True
+    controlsState = cs_msg.controlsState
+    controlsState.lateralControlState.init('debugState')
+
+    lp = sm['liveParameters']
+    steer_angle_without_offset = math.radians(sm['carState'].steeringAngleDeg - lp.angleOffsetDeg)
+    controlsState.curvature = -VM.calc_curvature(steer_angle_without_offset, sm['carState'].vEgo, lp.roll)
+
+    pm.send('controlsState', cs_msg)
+
+    rk.keep_time()
+
+
+def main():
+  joystickd_thread()
+
+
+if __name__ == "__main__":
+  main()