mirror of https://github.com/commaai/openpilot.git
joystickd: split into joystickd and joystick_control (#33632)
* Split joystickd into joystickd and joystick_control * Update process config * Undeprecate testJoystick * Static analysis fixes * Mark as +x * Update README * Add testJoystick back to services * reset if testJoystick not received * Fix quotes * Remove self * Add a send thread instead * Add joystick_control into process config * Add main * Add additional condition * Fix imports
This commit is contained in:
Kacper Rączy
GitHub
parent
251e2e9400
commit
d82c4509ea
|
|
@ -2413,6 +2413,7 @@ struct Event {
|
|||
uiDebug @102 :UIDebug;
|
||||
|
||||
# *********** debug ***********
|
||||
testJoystick @52 :Joystick;
|
||||
roadEncodeData @86 :EncodeData;
|
||||
driverEncodeData @87 :EncodeData;
|
||||
wideRoadEncodeData @88 :EncodeData;
|
||||
|
|
@ -2482,6 +2483,5 @@ struct Event {
|
|||
uiPlanDEPRECATED @106 :UiPlan;
|
||||
liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
|
||||
liveTracksDEPRECATED @16 :List(LiveTracksDEPRECATED);
|
||||
testJoystickDEPRECATED @52 :Joystick;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ _services: dict[str, tuple] = {
|
|||
|
||||
# debug
|
||||
"uiDebug": (True, 0., 1),
|
||||
"testJoystick": (True, 0.),
|
||||
"alertDebug": (True, 20., 5),
|
||||
"roadEncodeData": (False, 20.),
|
||||
"driverEncodeData": (False, 20.),
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
import os
|
||||
import operator
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
|
|
@ -53,6 +54,12 @@ def only_onroad(started: bool, params, CP: car.CarParams) -> bool:
|
|||
def only_offroad(started, params, CP: car.CarParams) -> bool:
|
||||
return not started
|
||||
|
||||
def or_(*fns):
|
||||
return lambda *args: operator.or_(*(fn(*args) for fn in fns))
|
||||
|
||||
def and_(*fns):
|
||||
return lambda *args: operator.and_(*(fn(*args) for fn in fns))
|
||||
|
||||
procs = [
|
||||
DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"),
|
||||
|
||||
|
|
@ -75,8 +82,8 @@ procs = [
|
|||
NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
|
||||
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
|
||||
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
|
||||
PythonProcess("controlsd", "selfdrive.controls.controlsd", not_joystick),
|
||||
PythonProcess("joystickd", "tools.joystick.joystickd", joystick),
|
||||
PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)),
|
||||
PythonProcess("joystickd", "tools.joystick.joystickd", or_(joystick, notcar)),
|
||||
PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad),
|
||||
PythonProcess("card", "selfdrive.car.card", only_onroad),
|
||||
PythonProcess("deleter", "system.loggerd.deleter", always_run),
|
||||
|
|
@ -100,6 +107,7 @@ procs = [
|
|||
NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
|
||||
PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar),
|
||||
PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
|
||||
PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)),
|
||||
]
|
||||
|
||||
managed_processes = {p.name: p for p in procs}
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ class TestStreamSession:
|
|||
tested_msgs = [
|
||||
{"type": "customReservedRawData0", "data": "test"}, # primitive
|
||||
{"type": "can", "data": [{"address": 0, "dat": "", "src": 0}]}, # list
|
||||
{"type": "testJoystickDEPRECATED", "data": {"axes": [0, 0], "buttons": [False]}}, # dict
|
||||
{"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict
|
||||
]
|
||||
|
||||
mocked_pubmaster = mocker.MagicMock(spec=messaging.PubMaster)
|
||||
|
|
|
|||
|
|
@ -2,33 +2,33 @@
|
|||
|
||||
**Hardware needed**: device running openpilot, laptop, joystick (optional)
|
||||
|
||||
With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard.
|
||||
joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
|
||||
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 `joystickd`.
|
||||
The car must be off, and openpilot must be offroad before starting `joystick_control`.
|
||||
|
||||
### Using a keyboard
|
||||
|
||||
SSH into your comma device and start joystickd with the following command:
|
||||
SSH into your comma device and start joystick_control with the following command:
|
||||
|
||||
```shell
|
||||
tools/joystick/joystickd.py --keyboard
|
||||
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 `joystickd.py`.
|
||||
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 joystickd locally from your laptop and have it send `testJoystick` packets over the network to the comma device.
|
||||
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 joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode:
|
||||
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
|
||||
|
|
@ -38,11 +38,11 @@ In order to use a joystick over the network, we need to run joystickd locally fr
|
|||
# on your comma device
|
||||
cereal/messaging/bridge {LAPTOP_IP} testJoystick
|
||||
```
|
||||
4. Start joystickd on your laptop in ZMQ mode.
|
||||
4. Start joystick_control on your laptop in ZMQ mode.
|
||||
```shell
|
||||
# on your laptop
|
||||
export ZMQ=1
|
||||
tools/joystick/joystickd.py
|
||||
tools/joystick/joystick_control.py
|
||||
```
|
||||
|
||||
---
|
||||
|
|
|
|||
|
|
@ -0,0 +1,147 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import argparse
|
||||
import threading
|
||||
from inputs import UnpluggedError, get_gamepad
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.numpy_fast import interp, clip
|
||||
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 = {ax: 0. for ax in self.axes_values}
|
||||
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] = 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 = {ax: 0. for ax in self.axes_values}
|
||||
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 = -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)
|
||||
|
|
@ -1,117 +1,30 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import argparse
|
||||
import threading
|
||||
from inputs import UnpluggedError, get_gamepad
|
||||
|
||||
import math
|
||||
|
||||
from cereal import messaging, car
|
||||
from openpilot.common.numpy_fast import clip
|
||||
from openpilot.common.realtime import DT_CTRL, Ratekeeper
|
||||
from openpilot.common.numpy_fast import interp, clip
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.tools.lib.kbhit import KBHit
|
||||
|
||||
EXPO = 0.4
|
||||
MAX_LAT_ACCEL = 2.5
|
||||
|
||||
|
||||
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 = {ax: 0. for ax in self.axes_values}
|
||||
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] = 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 = {ax: 0. for ax in self.axes_values}
|
||||
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 = -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):
|
||||
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'], frequency=1. / DT_CTRL)
|
||||
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)
|
||||
|
||||
joystick_axes = [joystick.axes_values[a] for a in joystick.axes_order]
|
||||
if rk.frame % 20 == 0:
|
||||
print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()))
|
||||
|
||||
cc_msg = messaging.new_message('carControl')
|
||||
cc_msg.valid = True
|
||||
CC = cc_msg.carControl
|
||||
|
|
@ -121,6 +34,14 @@ def send_thread(joystick):
|
|||
|
||||
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 * clip(joystick_axes[0], -1, 1)
|
||||
|
||||
|
|
@ -142,39 +63,9 @@ def send_thread(joystick):
|
|||
rk.keep_time()
|
||||
|
||||
|
||||
def joystick_thread(joystick):
|
||||
Params().put_bool('JoystickDebugMode', True)
|
||||
threading.Thread(target=send_thread, args=(joystick,), daemon=True).start()
|
||||
while True:
|
||||
joystick.update()
|
||||
|
||||
|
||||
def main():
|
||||
joystick_thread(Joystick())
|
||||
joystickd_thread()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
|
||||
'openpilot must be offroad before starting joystickd. 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 joystickd.")
|
||||
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_thread(joystick)
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
Loading…
Reference in New Issue