IQ.Pilot Release Commit @ b6534c0

iqpilot/tools/joystick/joystick_control.py

@@ -0,0 +1,296 @@
+#!/usr/bin/env python3
+import argparse
+import atexit
+import os
+from select import select
+import signal
+import socket
+import struct
+import sys
+import termios
+import threading
+import time
+
+import numpy as np
+from inputs import UnpluggedError, get_gamepad
+
+from iqpilot.cereal import messaging
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import Ratekeeper
+from iqpilot.system.hardware import HARDWARE
+
+REMOTE_PORT_DEFAULT = 8765
+REMOTE_TIMEOUT_S = 0.25
+REMOTE_PUBLISH_HZ = 30
+LOCAL_PUBLISH_HZ = 100
+
+
+class KBHit:
+  def __init__(self) -> None:
+    self.stdin_fd = sys.stdin.fileno()
+    self.old_term = termios.tcgetattr(self.stdin_fd)
+    self.new_term = self.old_term.copy()
+    self.new_term[3] &= ~(termios.ICANON | termios.ECHO)
+    termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.new_term)
+    atexit.register(self.set_normal_term)
+
+  def set_normal_term(self) -> None:
+    termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.old_term)
+
+  @staticmethod
+  def getch() -> str:
+    return sys.stdin.read(1)
+
+  @staticmethod
+  def kbhit():
+    return select([sys.stdin], [], [], 0)[0] != []
+
+
+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 TimeoutError:
+      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)

iqpilot/tools/joystick/joystickd.py

@@ -0,0 +1,139 @@
+#!/usr/bin/env python3
+
+import math
+import numpy as np
+
+from iqpilot.cereal import messaging, car, custom
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL, Ratekeeper
+from iqpilot.common.params import Params
+from iqpilot.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', 'vehicleParameters', '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['vehicleParameters'].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['vehicleParameters']
+    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()

iqpilot/tools/joystick/tests/test_joystickd.py

@@ -0,0 +1,33 @@
+from iqpilot.cereal import car
+
+from iqpilot.tools.joystick.joystickd import get_lateral_joystick_outputs
+
+
+class StubVehicleModel:
+  def calc_curvature(self, steer_angle: float, v_ego: float, roll: float) -> float:
+    return steer_angle
+
+  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