forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ 0798119
This commit is contained in:
IQ.Lvbs history cleanup
272
tools/joystick/joystick_control.py
Executable file
272
tools/joystick/joystick_control.py
Executable file
@@ -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)
|
||||
Reference in New Issue
Block a user