IQ.Pilot Release Commit @ d58810f

tools/sim/lib/keyboard_ctrl.py

@@ -0,0 +1,103 @@
+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()
+  old_settings = termios.tcgetattr(STDIN_FD)
+  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)
+  finally:
+    termios.tcsetattr(STDIN_FD, termios.TCSADRAIN, old_settings)
+  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 == '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)