mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-01 05:33:49 +08:00
Prabhaav Pillai
116 lines
2.9 KiB
Python
116 lines
2.9 KiB
Python
import sys
|
|
import termios
|
|
import time
|
|
|
|
from multiprocessing import Queue
|
|
from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
|
|
ISTRIP, IXON, PARENB, VMIN, VTIME)
|
|
from typing import NoReturn
|
|
|
|
from openpilot.tools.sim.bridge.common import QueueMessage, control_cmd_gen
|
|
|
|
# Indexes for termios list.
|
|
IFLAG = 0
|
|
OFLAG = 1
|
|
CFLAG = 2
|
|
LFLAG = 3
|
|
ISPEED = 4
|
|
OSPEED = 5
|
|
CC = 6
|
|
|
|
|
|
KEYBOARD_HELP = """
|
|
| key | functionality |
|
|
|------|-----------------------|
|
|
| 1 | Cruise Resume / Accel |
|
|
| 2 | Cruise Set / Decel |
|
|
| 3 | Cruise Cancel |
|
|
| r | Reset Simulation |
|
|
| i | Toggle Ignition |
|
|
| q | Exit all |
|
|
| wasd | Control manually |
|
|
"""
|
|
|
|
|
|
def getch() -> str:
|
|
STDIN_FD = sys.stdin.fileno()
|
|
try:
|
|
old_settings = termios.tcgetattr(STDIN_FD)
|
|
except (termios.error, OSError):
|
|
# No controlling terminal (e.g. headless / background run). Idle instead of crashing.
|
|
return None
|
|
try:
|
|
# set
|
|
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(STDIN_FD, termios.TCSAFLUSH, mode)
|
|
|
|
ch = sys.stdin.read(1)
|
|
except (termios.error, OSError):
|
|
ch = None
|
|
finally:
|
|
try:
|
|
termios.tcsetattr(STDIN_FD, termios.TCSADRAIN, old_settings)
|
|
except (termios.error, OSError):
|
|
pass
|
|
return ch
|
|
|
|
def print_keyboard_help():
|
|
print(f"Keyboard Commands:\n{KEYBOARD_HELP}")
|
|
|
|
def keyboard_poll_thread(q: 'Queue[QueueMessage]'):
|
|
print_keyboard_help()
|
|
|
|
while True:
|
|
c = getch()
|
|
if c is None:
|
|
# No terminal input available (headless/background); avoid busy-looping.
|
|
time.sleep(0.05)
|
|
elif c == '1':
|
|
q.put(control_cmd_gen("cruise_up"))
|
|
elif c == '2':
|
|
q.put(control_cmd_gen("cruise_down"))
|
|
elif c == '3':
|
|
q.put(control_cmd_gen("cruise_cancel"))
|
|
elif c == 'w':
|
|
q.put(control_cmd_gen(f"throttle_{1.0}"))
|
|
elif c == 'a':
|
|
q.put(control_cmd_gen(f"steer_{-0.15}"))
|
|
elif c == 's':
|
|
q.put(control_cmd_gen(f"brake_{1.0}"))
|
|
elif c == 'd':
|
|
q.put(control_cmd_gen(f"steer_{0.15}"))
|
|
elif c == 'z':
|
|
q.put(control_cmd_gen("blinker_left"))
|
|
elif c == 'x':
|
|
q.put(control_cmd_gen("blinker_right"))
|
|
elif c == 'i':
|
|
q.put(control_cmd_gen("ignition"))
|
|
elif c == 'r':
|
|
q.put(control_cmd_gen("reset"))
|
|
elif c == 'q':
|
|
q.put(control_cmd_gen("quit"))
|
|
break
|
|
else:
|
|
print_keyboard_help()
|
|
|
|
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: 'Queue[QueueMessage]' = Queue()
|
|
p = Process(target=test, args=(q,))
|
|
p.daemon = True
|
|
p.start()
|
|
|
|
keyboard_poll_thread(q)
|