IQ.Pilot Release Commit @ 0798119

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,272 @@
+#!/usr/bin/env python3
+import argparse
+import os
+import signal
+import socket
+import struct
+import threading
+import time
+
+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
+
+REMOTE_PORT_DEFAULT = 8765
+REMOTE_TIMEOUT_S = 0.25
+REMOTE_PUBLISH_HZ = 30
+LOCAL_PUBLISH_HZ = 100
+
+
+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
+    self.idle_sleep_s = 0.0
+
+  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
+
+  def get_buttons(self):
+    return [False, self.cancel]
+
+
+class Joystick:
+  def __init__(self):
+    # This class supports a PlayStation 5 DualSense controller on the comma 3X
+    # Using both analog sticks: left stick Y for gas/brake, right stick X for steering
+    self.cancel_button = 'BTN_NORTH'  # BTN_NORTH=X/triangle
+    if HARDWARE.get_device_type() == 'pc':
+      accel_axis = 'ABS_Y'      # Left stick Y-axis
+      steer_axis = 'ABS_RX'     # Right stick X-axis
+      self.flip_map = {}         # No flipping needed
+    else:
+      accel_axis = 'ABS_Y'      # Left stick Y-axis
+      steer_axis = 'ABS_Z'      # Right stick X-axis
+      self.flip_map = {}         # No flipping needed
+
+    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
+    self.idle_sleep_s = 0.0
+
+  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]] = norm
+    else:
+      return False
+    return True
+
+  def get_buttons(self):
+    return [False, self.cancel]
+
+
+class RemoteJoystick:
+  def __init__(self, host: str, port: int):
+    self.addr = (host, port)
+    self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    self.socket.bind(self.addr)
+    self.socket.settimeout(0.1)
+
+    self.axes_values = {'gb': 0.0, 'steer': 0.0}
+    self.axes_order = ['gb', 'steer']
+    self.buttons = [False, False]
+    self.last_update = 0.0
+    self.authenticated = False
+    self.client_addr = None
+    self.idle_sleep_s = 0.01
+
+  def _clamp(self, value: float) -> float:
+    return float(np.clip(value, -1.0, 1.0))
+
+  def _handle_timeout(self, now: float) -> None:
+    if self.authenticated and (now - self.last_update) > REMOTE_TIMEOUT_S:
+      self.axes_values = {'gb': 0.0, 'steer': 0.0}
+      self.buttons = [False, False]
+
+  def _send_auth_ok(self, addr) -> None:
+    try:
+      self.socket.sendto(bytes([1]), addr)
+    except OSError:
+      pass
+
+  def update(self):
+    now = time.monotonic()
+    try:
+      data, addr = self.socket.recvfrom(64)
+    except socket.timeout:
+      self._handle_timeout(now)
+      return False
+    except OSError:
+      return False
+
+    if not data:
+      return False
+
+    msg_type = data[0]
+    if msg_type == 0:
+      self.client_addr = addr
+      self.authenticated = True
+      self._send_auth_ok(addr)
+      return True
+
+    if msg_type == 2:
+      try:
+        payload = data[1:].decode("utf-8", errors="strict").strip()
+        steer_s, accel_s, engage_s, disengage_s = payload.split(",", 3)
+        steer = float(steer_s)
+        accel = float(accel_s)
+        engage = engage_s == "1"
+        disengage = disengage_s == "1"
+      except (UnicodeDecodeError, ValueError):
+        return False
+
+      self.axes_values['steer'] = self._clamp(steer)
+      self.axes_values['gb'] = self._clamp(accel)
+      self.buttons = [engage, disengage]
+      self.last_update = now
+      return True
+
+    if msg_type != 1 or len(data) < 9:
+      return False
+
+    steer, accel = struct.unpack_from("<ff", data, 1)
+    engage = bool(data[9]) if len(data) > 9 else False
+    disengage = bool(data[10]) if len(data) > 10 else False
+
+    self.axes_values['steer'] = self._clamp(steer)
+    self.axes_values['gb'] = self._clamp(accel)
+    self.buttons = [engage, disengage]
+    self.last_update = now
+    return True
+
+  def get_buttons(self):
+    return self.buttons
+
+
+def send_thread(joystick, show_values: bool):
+  pm = messaging.PubMaster(['testJoystick'])
+
+  publish_hz = REMOTE_PUBLISH_HZ if isinstance(joystick, RemoteJoystick) else LOCAL_PUBLISH_HZ
+  rk = Ratekeeper(publish_hz, print_delay_threshold=None)
+
+  while True:
+    if show_values and 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]
+    joystick_msg.testJoystick.buttons = joystick.get_buttons()
+
+    pm.send('testJoystick', joystick_msg)
+
+    rk.keep_time()
+
+
+def joystick_control_thread(joystick, show_values: bool):
+  Params().put_bool('JoystickDebugMode', True)
+  try:
+    threading.Thread(target=send_thread, args=(joystick, show_values), daemon=True).start()
+    while True:
+      updated = joystick.update()
+      if not updated and joystick.idle_sleep_s > 0:
+        time.sleep(joystick.idle_sleep_s)
+  finally:
+    Params().put_bool('JoystickDebugMode', False)
+
+
+def main():
+  joystick_control_thread(Joystick(), True)
+
+
+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')
+  parser.add_argument('--remote', action='store_true', help='Listen for UDP joystick input')
+  parser.add_argument('--listen', type=int, default=REMOTE_PORT_DEFAULT, help='UDP port for remote joystick input')
+  parser.add_argument('--listen-address', default='0.0.0.0', help='UDP address to bind for remote input')
+  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()
+
+  if args.remote and args.keyboard:
+    print("Choose only one input mode.")
+    exit()
+
+  print()
+  if args.remote:
+    print(f'Listening for remote joystick on {args.listen_address}:{args.listen}')
+  elif 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.')
+
+  def handle_exit(signum, _frame):
+    Params().put_bool('JoystickDebugMode', False)
+    raise SystemExit
+
+  signal.signal(signal.SIGINT, handle_exit)
+  signal.signal(signal.SIGTERM, handle_exit)
+
+  if args.remote:
+    joystick = RemoteJoystick(args.listen_address, int(args.listen))
+    joystick_control_thread(joystick, False)
+  else:
+    joystick = Keyboard() if args.keyboard else Joystick()
+    joystick_control_thread(joystick, True)

tools/joystick/joystickd.py

@@ -0,0 +1,139 @@
+#!/usr/bin/env python3
+
+import math
+import numpy as np
+
+from cereal import messaging, car, custom
+from iqdbc.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 = 5.0
+MAX_STEERING_ANGLE_DEG = 500.0
+ACCEL_RELEASE_THRESHOLD = 0.01
+DECEL_REQUEST_THRESHOLD = -0.02
+STOPPING_HOLD_SPEED_MARGIN = 0.3
+STOPPING_SPEED = 0.25
+
+
+def get_lateral_joystick_outputs(CP: car.CarParams, VM: VehicleModel, v_ego: float, roll: float, steer_axis: float) -> tuple[float, float, float]:
+  steer_axis = float(np.clip(steer_axis, -1, 1))
+  steering_angle_deg = steer_axis * MAX_STEERING_ANGLE_DEG
+  curvature = -VM.calc_curvature(math.radians(steering_angle_deg), v_ego, roll)
+
+  if CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+    return 0.0, steering_angle_deg, curvature
+
+  max_curvature = MAX_LAT_ACCEL / max(v_ego ** 2, 5)
+  max_angle = min(math.degrees(VM.get_steer_from_curvature(max_curvature, v_ego, roll)), MAX_STEERING_ANGLE_DEG)
+  return steer_axis, steer_axis * max_angle, steer_axis * -max_curvature
+
+
+def joystickd_thread():
+  params = Params()
+  cloudlog.info("joystickd is waiting for CarParams")
+  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+  CP_IQ = messaging.log_from_bytes(params.get("IQCarParams", block=True), custom.IQCarParams)
+  VM = VehicleModel(CP)
+
+  sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'iqState', 'testJoystick'], frequency=1. / DT_CTRL)
+  pm = messaging.PubMaster(['carControl', 'controlsState'])
+
+  # Stop-hold behavior for joystick long control:
+  # - enter hold only when user requested decel and we are near/at stop
+  # - neutral input does not request decel while rolling
+  # - release hold on positive accel request
+  decel_intent_latched = False
+  stop_hold_latched = False
+
+  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
+    ss = sm['selfdriveState']
+    ss_iq = sm['iqState']
+    aol_enabled = bool(getattr(ss_iq.aol, 'enabled', False))
+    aol_active = bool(getattr(ss_iq.aol, 'active', False))
+    joystick_angle_lat_active = aol_active or (
+      aol_enabled and CP.steerControlType == car.CarParams.SteerControlType.angle
+    )
+
+    CC.enabled = bool(ss.enabled or aol_enabled)
+    CC.latActive = bool(ss.active or joystick_angle_lat_active) and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
+    long_through_override = CP_IQ.longActiveWithGasOverride and CP.openpilotLongitudinalControl
+    override_longitudinal = any(e.overrideLongitudinal for e in sm['onroadEvents'])
+    CC.longActive = bool(ss.enabled) and (not override_longitudinal or long_through_override) 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:
+      accel_cmd = float(np.clip(joystick_axes[0], -1, 1))
+      actuators.accel = 4.0 * accel_cmd
+
+      positive_accel_requested = accel_cmd > ACCEL_RELEASE_THRESHOLD
+      negative_accel_requested = accel_cmd < DECEL_REQUEST_THRESHOLD
+      near_stop = sm['carState'].standstill or sm['carState'].vEgo <= (STOPPING_SPEED + STOPPING_HOLD_SPEED_MARGIN)
+
+      if positive_accel_requested:
+        stop_hold_latched = False
+        decel_intent_latched = False
+      elif negative_accel_requested:
+        decel_intent_latched = True
+
+      if decel_intent_latched and near_stop and not positive_accel_requested:
+        stop_hold_latched = True
+
+      # If we are moving again and driver is not asking for decel, clear stale hold state.
+      if stop_hold_latched and sm['carState'].vEgo > (STOPPING_SPEED + STOPPING_HOLD_SPEED_MARGIN) and not negative_accel_requested:
+        stop_hold_latched = False
+        decel_intent_latched = False
+
+      actuators.longControlState = LongCtrlState.stopping if stop_hold_latched else LongCtrlState.pid
+      CC.cruiseControl.resume = positive_accel_requested
+    else:
+      decel_intent_latched = False
+      stop_hold_latched = False
+
+    if CC.latActive:
+      torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, VM, sm['carState'].vEgo, sm['liveParameters'].roll, joystick_axes[1])
+      actuators.torque = torque
+      actuators.steeringAngleDeg = steering_angle_deg
+      actuators.curvature = 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()

tools/joystick/tests/test_joystickd.py

@@ -0,0 +1,30 @@
+from cereal import car
+
+from openpilot.tools.joystick.joystickd import get_lateral_joystick_outputs
+
+
+class StubVehicleModel:
+  def get_steer_from_curvature(self, curvature: float, v_ego: float, roll: float) -> float:
+    return curvature
+
+
+def test_angle_cars_use_angle_outputs():
+  CP = car.CarParams.new_message()
+  CP.steerControlType = car.CarParams.SteerControlType.angle
+
+  torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, StubVehicleModel(), 20.0, 0.0, 0.5)
+
+  assert torque == 0.0
+  assert steering_angle_deg != 0.0
+  assert curvature < 0.0
+
+
+def test_torque_cars_keep_torque_outputs():
+  CP = car.CarParams.new_message()
+  CP.steerControlType = car.CarParams.SteerControlType.torque
+
+  torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, StubVehicleModel(), 20.0, 0.0, 0.5)
+
+  assert torque == 0.5
+  assert steering_angle_deg != 0.0
+  assert curvature < 0.0