mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-09-30 19:33:42 +08:00
Add type hints, small cleanups (#21080)
* improve tools.lib.kbhit and tools.sim.lib.keyboard_ctrl * unpack more efficiently * minor improvements * agnos.py match spec better * manual_ctrl test missing queue arg * fix incorrect type annotation * queues are generic * varname reuse resulting in incorrect type inference * bytes().hex() rather than bytes.hex(bytes()) * a bit of type hinting stuff
This commit is contained in:
Josh Smith
+8
-8
@@ -248,9 +248,9 @@ def bridge(q):
|
||||
# 3. Send current carstate to op via can
|
||||
|
||||
cruise_button = 0
|
||||
throttle_out = steer_out = brake_out = 0
|
||||
throttle_out = steer_out = brake_out = 0.0
|
||||
throttle_op = steer_op = brake_op = 0
|
||||
throttle_manual = steer_manual = brake_manual = 0
|
||||
throttle_manual = steer_manual = brake_manual = 0.0
|
||||
|
||||
# --------------Step 1-------------------------------
|
||||
if not q.empty():
|
||||
@@ -259,24 +259,24 @@ def bridge(q):
|
||||
if m[0] == "steer":
|
||||
steer_manual = float(m[1])
|
||||
is_openpilot_engaged = False
|
||||
if m[0] == "throttle":
|
||||
elif m[0] == "throttle":
|
||||
throttle_manual = float(m[1])
|
||||
is_openpilot_engaged = False
|
||||
if m[0] == "brake":
|
||||
elif m[0] == "brake":
|
||||
brake_manual = float(m[1])
|
||||
is_openpilot_engaged = False
|
||||
if m[0] == "reverse":
|
||||
elif m[0] == "reverse":
|
||||
#in_reverse = not in_reverse
|
||||
cruise_button = CruiseButtons.CANCEL
|
||||
is_openpilot_engaged = False
|
||||
if m[0] == "cruise":
|
||||
elif m[0] == "cruise":
|
||||
if m[1] == "down":
|
||||
cruise_button = CruiseButtons.DECEL_SET
|
||||
is_openpilot_engaged = True
|
||||
if m[1] == "up":
|
||||
elif m[1] == "up":
|
||||
cruise_button = CruiseButtons.RES_ACCEL
|
||||
is_openpilot_engaged = True
|
||||
if m[1] == "cancel":
|
||||
elif m[1] == "cancel":
|
||||
cruise_button = CruiseButtons.CANCEL
|
||||
is_openpilot_engaged = False
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@ import termios
|
||||
import time
|
||||
from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
|
||||
ISTRIP, IXON, PARENB, VMIN, VTIME)
|
||||
from typing import Any
|
||||
from typing import NoReturn
|
||||
|
||||
# Indexes for termios list.
|
||||
IFLAG = 0
|
||||
@@ -14,55 +14,56 @@ ISPEED = 4
|
||||
OSPEED = 5
|
||||
CC = 6
|
||||
|
||||
def getch():
|
||||
fd = sys.stdin.fileno()
|
||||
old_settings = termios.tcgetattr(fd)
|
||||
STDIN_FD = sys.stdin.fileno()
|
||||
|
||||
def getch() -> str:
|
||||
old_settings = termios.tcgetattr(STDIN_FD)
|
||||
try:
|
||||
# set
|
||||
mode = termios.tcgetattr(fd)
|
||||
mode[IFLAG] = mode[IFLAG] & ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON)
|
||||
#mode[OFLAG] = mode[OFLAG] & ~(OPOST)
|
||||
mode[CFLAG] = mode[CFLAG] & ~(CSIZE | PARENB)
|
||||
mode[CFLAG] = mode[CFLAG] | CS8
|
||||
mode[LFLAG] = mode[LFLAG] & ~(ECHO | ICANON | IEXTEN)
|
||||
mode = old_settings.copy()
|
||||
mode[IFLAG] &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON)
|
||||
#mode[OFLAG] &= ~(OPOST)
|
||||
mode[CFLAG] &= ~(CSIZE | PARENB)
|
||||
mode[CFLAG] |= CS8
|
||||
mode[LFLAG] &= ~(ECHO | ICANON | IEXTEN)
|
||||
mode[CC][VMIN] = 1
|
||||
mode[CC][VTIME] = 0
|
||||
termios.tcsetattr(fd, termios.TCSAFLUSH, mode)
|
||||
termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, mode)
|
||||
|
||||
ch = sys.stdin.read(1)
|
||||
finally:
|
||||
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
|
||||
termios.tcsetattr(STDIN_FD, termios.TCSADRAIN, old_settings)
|
||||
return ch
|
||||
|
||||
def keyboard_poll_thread(q):
|
||||
def keyboard_poll_thread(q: 'Queue[str]') -> NoReturn:
|
||||
while True:
|
||||
c = getch()
|
||||
# print("got %s" % c)
|
||||
if c == '1':
|
||||
q.put(str("cruise_up"))
|
||||
if c == '2':
|
||||
q.put(str("cruise_down"))
|
||||
if c == '3':
|
||||
q.put(str("cruise_cancel"))
|
||||
if c == 'w':
|
||||
q.put(str("throttle_%f" % 1.0))
|
||||
if c == 'a':
|
||||
q.put(str("steer_%f" % 0.15))
|
||||
if c == 's':
|
||||
q.put(str("brake_%f" % 1.0))
|
||||
if c == 'd':
|
||||
q.put(str("steer_%f" % -0.15))
|
||||
if c == 'q':
|
||||
q.put("cruise_up")
|
||||
elif c == '2':
|
||||
q.put("cruise_down")
|
||||
elif c == '3':
|
||||
q.put("cruise_cancel")
|
||||
elif c == 'w':
|
||||
q.put("throttle_%f" % 1.0)
|
||||
elif c == 'a':
|
||||
q.put("steer_%f" % 0.15)
|
||||
elif c == 's':
|
||||
q.put("brake_%f" % 1.0)
|
||||
elif c == 'd':
|
||||
q.put("steer_%f" % -0.15)
|
||||
elif c == 'q':
|
||||
exit(0)
|
||||
|
||||
def test(q):
|
||||
while 1:
|
||||
print("hello")
|
||||
time.sleep(1.0)
|
||||
def test(q: 'Queue[str]') -> NoReturn:
|
||||
while True:
|
||||
print([q.get_nowait() for _ in range(q.qsize())] or None)
|
||||
time.sleep(0.25)
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import Process, Queue
|
||||
q : Any = Queue()
|
||||
q: Queue[str] = Queue()
|
||||
p = Process(target=test, args=(q,))
|
||||
p.daemon = True
|
||||
p.start()
|
||||
|
||||
@@ -4,6 +4,7 @@ import array
|
||||
import os
|
||||
import struct
|
||||
from fcntl import ioctl
|
||||
from typing import NoReturn
|
||||
|
||||
# Iterate over the joystick devices.
|
||||
print('Available devices:')
|
||||
@@ -90,7 +91,7 @@ button_names = {
|
||||
axis_map = []
|
||||
button_map = []
|
||||
|
||||
def wheel_poll_thread(q):
|
||||
def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
|
||||
# Open the joystick device.
|
||||
fn = '/dev/input/js0'
|
||||
print('Opening %s...' % fn)
|
||||
@@ -116,8 +117,8 @@ def wheel_poll_thread(q):
|
||||
buf = array.array('B', [0] * 0x40)
|
||||
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
|
||||
|
||||
for axis in buf[:num_axes]:
|
||||
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
|
||||
for _axis in buf[:num_axes]:
|
||||
axis_name = axis_names.get(_axis, 'unknown(0x%02x)' % _axis)
|
||||
axis_map.append(axis_name)
|
||||
axis_states[axis_name] = 0.0
|
||||
|
||||
@@ -143,7 +144,7 @@ def wheel_poll_thread(q):
|
||||
|
||||
while True:
|
||||
evbuf = jsdev.read(8)
|
||||
_, value, mtype, number = struct.unpack('IhBB', evbuf)
|
||||
value, mtype, number = struct.unpack('4xhBB', evbuf)
|
||||
# print(mtype, number, value)
|
||||
if mtype & 0x02: # wheel & paddles
|
||||
axis = axis_map[number]
|
||||
@@ -152,38 +153,36 @@ def wheel_poll_thread(q):
|
||||
fvalue = value / 32767.0
|
||||
axis_states[axis] = fvalue
|
||||
normalized = (1 - fvalue) * 50
|
||||
q.put(str("throttle_%f" % normalized))
|
||||
q.put("throttle_%f" % normalized)
|
||||
|
||||
if axis == "rz": # brake
|
||||
elif axis == "rz": # brake
|
||||
fvalue = value / 32767.0
|
||||
axis_states[axis] = fvalue
|
||||
normalized = (1 - fvalue) * 50
|
||||
q.put(str("brake_%f" % normalized))
|
||||
q.put("brake_%f" % normalized)
|
||||
|
||||
if axis == "x": # steer angle
|
||||
elif axis == "x": # steer angle
|
||||
fvalue = value / 32767.0
|
||||
axis_states[axis] = fvalue
|
||||
normalized = fvalue
|
||||
q.put(str("steer_%f" % normalized))
|
||||
q.put("steer_%f" % normalized)
|
||||
|
||||
if mtype & 0x01: # buttons
|
||||
if number in [0, 19]: # X
|
||||
if value == 1: # press down
|
||||
q.put(str("cruise_down"))
|
||||
elif mtype & 0x01: # buttons
|
||||
if value == 1: # press down
|
||||
if number in [0, 19]: # X
|
||||
q.put("cruise_down")
|
||||
|
||||
if number in [3, 18]: # triangle
|
||||
if value == 1: # press down
|
||||
q.put(str("cruise_up"))
|
||||
elif number in [3, 18]: # triangle
|
||||
q.put("cruise_up")
|
||||
|
||||
if number in [1, 6]: # square
|
||||
if value == 1: # press down
|
||||
q.put(str("cruise_cancel"))
|
||||
elif number in [1, 6]: # square
|
||||
q.put("cruise_cancel")
|
||||
|
||||
if number in [10, 21]: # R3
|
||||
if value == 1: # press down
|
||||
q.put(str("reverse_switch"))
|
||||
elif number in [10, 21]: # R3
|
||||
q.put("reverse_switch")
|
||||
|
||||
if __name__ == '__main__':
|
||||
from multiprocessing import Process
|
||||
p = Process(target=wheel_poll_thread)
|
||||
from multiprocessing import Process, Queue
|
||||
q: Queue[str] = Queue()
|
||||
p = Process(target=wheel_poll_thread, args=(q,))
|
||||
p.start()
|
||||
|
||||
Reference in New Issue
Block a user