You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
113 lines
3.6 KiB
113 lines
3.6 KiB
#!/usr/bin/env python3
|
|
import time
|
|
import threading
|
|
import argparse
|
|
import numpy as np
|
|
from pprint import pprint
|
|
from inputs import get_gamepad
|
|
|
|
from kbhit import KBHit
|
|
|
|
from opendbc.car.structs import CarControl
|
|
from opendbc.car.panda_runner import PandaRunner
|
|
|
|
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()
|
|
print(key)
|
|
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] = float(np.clip(self.axes_values[axis] + incr, -1, 1))
|
|
else:
|
|
return False
|
|
return True
|
|
|
|
class Joystick:
|
|
def __init__(self, gamepad=False):
|
|
# TODO: find a way to get this from API, perhaps "inputs" doesn't support it
|
|
if gamepad:
|
|
self.cancel_button = 'BTN_NORTH' # (BTN_NORTH=X, ABS_RZ=Right Trigger)
|
|
accel_axis = 'ABS_Y'
|
|
steer_axis = 'ABS_RX'
|
|
else:
|
|
self.cancel_button = 'BTN_TRIGGER'
|
|
accel_axis = 'ABS_Y'
|
|
steer_axis = 'ABS_RX'
|
|
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):
|
|
joystick_event = get_gamepad()[0]
|
|
event = (joystick_event.code, joystick_event.state)
|
|
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.]))
|
|
self.axes_values[event[0]] = norm if abs(norm) > 0.05 else 0. # center can be noisy, deadzone of 5%
|
|
else:
|
|
return False
|
|
return True
|
|
|
|
def joystick_thread(joystick):
|
|
while True:
|
|
joystick.update()
|
|
|
|
def main(joystick):
|
|
threading.Thread(target=joystick_thread, args=(joystick,), daemon=True).start()
|
|
with PandaRunner() as p:
|
|
CC = CarControl(enabled=False)
|
|
while True:
|
|
CC.actuators.accel = float(4.0*np.clip(joystick.axes_values['gb'], -1, 1))
|
|
CC.actuators.torque = float(np.clip(joystick.axes_values['steer'], -1, 1))
|
|
pprint(CC)
|
|
|
|
p.read()
|
|
p.write(CC)
|
|
|
|
# 100Hz
|
|
time.sleep(0.01)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
parser = argparse.ArgumentParser(description='Test the car interface with a joystick. Uses keyboard by default.',
|
|
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
|
|
|
parser.add_argument('--mode', choices=['keyboard', 'gamepad', 'joystick'], default='keyboard')
|
|
args = parser.parse_args()
|
|
|
|
print()
|
|
joystick: Keyboard | Joystick
|
|
if args.mode == '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')
|
|
joystick = Keyboard()
|
|
else:
|
|
joystick = Joystick(gamepad=(args.mode == 'gamepad'))
|
|
main(joystick)
|
|
|