IQ.Pilot Prebuilt Release @ 27f668a
This commit is contained in:
IQ.Lvbs CI [bot]
296
iqpilot/tools/joystick/joystick_control.py
Executable file
296
iqpilot/tools/joystick/joystick_control.py
Executable file
@@ -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)
|
||||
139
iqpilot/tools/joystick/joystickd.py
Executable file
139
iqpilot/tools/joystick/joystickd.py
Executable file
@@ -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()
|
||||
33
iqpilot/tools/joystick/tests/test_joystickd.py
Normal file
33
iqpilot/tools/joystick/tests/test_joystickd.py
Normal file
@@ -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
|
||||
Reference in New Issue
Block a user