mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
22a8575199
Keep official openpilot ancestry and dependency licenses. Store resources directly in Git, remove LFS setup requirements, and leave the downloadable big model outside the source tree.
118 lines
3.6 KiB
Python
Executable File
118 lines
3.6 KiB
Python
Executable File
#!/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)
|