Simulator: standardize queue messages

tools/sim/bridge/common.py

@@ -2,6 +2,8 @@
 import threading
 import functools
 
+from collections import namedtuple
+from enum import Enum
 from multiprocessing import Process, Queue, Value
 from abc import ABC, abstractmethod
 
@@ -14,6 +16,16 @@
 from openpilot.tools.sim.lib.simulated_car import SimulatedCar
 from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors
 
+QueueMessage = namedtuple("QueueMessage", ["type", "info"], defaults=[None])
+
+class QueueMessageType(Enum):
+  START_STATUS = 0
+  CONTROL_COMMAND = 1
+  TERMINATION_INFO = 2
+  CLOSE_STATUS = 3
+
+def control_cmd_gen(cmd: str):
+  return QueueMessage(QueueMessageType.CONTROL_COMMAND, cmd)
 
 def rk_loop(function, hz, exit_event: threading.Event):
   rk = Ratekeeper(hz, None)
@@ -80,11 +92,11 @@ def print_status(self):
     """)
 
   @abstractmethod
-  def spawn_world(self) -> World:
+  def spawn_world(self, q: Queue) -> World:
     pass
 
   def _run(self, q: Queue):
-    self.world = self.spawn_world()
+    self.world = self.spawn_world(q)
 
     self.simulated_car = SimulatedCar()
     self.simulated_sensors = SimulatedSensors(self.dual_camera)
@@ -114,33 +126,34 @@ def _run(self, q: Queue):
       # Read manual controls
       if not q.empty():
         message = q.get()
-        m = message.split('_')
-        if m[0] == "steer":
-          steer_manual = float(m[1])
-        elif m[0] == "throttle":
-          throttle_manual = float(m[1])
-        elif m[0] == "brake":
-          brake_manual = float(m[1])
-        elif m[0] == "cruise":
-          if m[1] == "down":
-            self.simulator_state.cruise_button = CruiseButtons.DECEL_SET
-          elif m[1] == "up":
-            self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL
-          elif m[1] == "cancel":
-            self.simulator_state.cruise_button = CruiseButtons.CANCEL
-          elif m[1] == "main":
-            self.simulator_state.cruise_button = CruiseButtons.MAIN
-        elif m[0] == "blinker":
-          if m[1] == "left":
-            self.simulator_state.left_blinker = True
-          elif m[1] == "right":
-            self.simulator_state.right_blinker = True
-        elif m[0] == "ignition":
-          self.simulator_state.ignition = not self.simulator_state.ignition
-        elif m[0] == "reset":
-          self.world.reset()
-        elif m[0] == "quit":
-          break
+        if message.type == QueueMessageType.CONTROL_COMMAND:
+          m = message.info.split('_')
+          if m[0] == "steer":
+            steer_manual = float(m[1])
+          elif m[0] == "throttle":
+            throttle_manual = float(m[1])
+          elif m[0] == "brake":
+            brake_manual = float(m[1])
+          elif m[0] == "cruise":
+            if m[1] == "down":
+              self.simulator_state.cruise_button = CruiseButtons.DECEL_SET
+            elif m[1] == "up":
+              self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL
+            elif m[1] == "cancel":
+              self.simulator_state.cruise_button = CruiseButtons.CANCEL
+            elif m[1] == "main":
+              self.simulator_state.cruise_button = CruiseButtons.MAIN
+          elif m[0] == "blinker":
+            if m[1] == "left":
+              self.simulator_state.left_blinker = True
+            elif m[1] == "right":
+              self.simulator_state.right_blinker = True
+          elif m[0] == "ignition":
+            self.simulator_state.ignition = not self.simulator_state.ignition
+          elif m[0] == "reset":
+            self.world.reset()
+          elif m[0] == "quit":
+            break
 
       self.simulator_state.user_brake = brake_manual
       self.simulator_state.user_gas = throttle_manual

tools/sim/bridge/metadrive/metadrive_bridge.py

@@ -53,7 +53,7 @@ def __init__(self, dual_camera, high_quality):
 
     super().__init__(dual_camera, high_quality)
 
-  def spawn_world(self):
+  def spawn_world(self, queue: Queue):
     sensors = {
       "rgb_road": (RGBCameraRoad, W, H, )
     }
@@ -83,4 +83,4 @@ def spawn_world(self):
       preload_models=False
     )
 
-    return MetaDriveWorld(Queue(), config, self.dual_camera)
+    return MetaDriveWorld(queue, config, self.dual_camera)

tools/sim/bridge/metadrive/metadrive_world.py

@@ -5,6 +5,8 @@
 import time
 
 from multiprocessing import Pipe, Array
+
+from openpilot.tools.sim.bridge.common import QueueMessage, QueueMessageType
 from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state,
                                                                     metadrive_vehicle_state)
 from openpilot.tools.sim.lib.common import SimulatorState, World
@@ -35,14 +37,14 @@ def __init__(self, status_q, config, dual_camera = False):
                                                 self.vehicle_state_send, self.exit_event))
 
     self.metadrive_process.start()
-    self.status_q.put({"status": "starting"})
+    self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting"))
 
     print("----------------------------------------------------------")
     print("---- Spawning Metadrive world, this might take awhile ----")
     print("----------------------------------------------------------")
 
     self.vehicle_state_recv.recv() # wait for a state message to ensure metadrive is launched
-    self.status_q.put({"status": "started"})
+    self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "started"))
 
     self.steer_ratio = 15
     self.vc = [0.0,0.0]
@@ -68,11 +70,7 @@ def read_state(self):
     while self.simulation_state_recv.poll(0):
       md_state: metadrive_simulation_state = self.simulation_state_recv.recv()
       if md_state.done:
-        self.status_q.put({
-          "status": "terminating",
-          "reason": "done",
-          "done_info": md_state.done_info
-        })
+        self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, md_state.done_info))
         self.exit_event.set()
 
   def read_sensors(self, state: SimulatorState):
@@ -94,9 +92,6 @@ def reset(self):
     self.should_reset = True
 
   def close(self, reason: str):
-    self.status_q.put({
-      "status": "terminating",
-      "reason": reason,
-    })
+    self.status_q.put(QueueMessage(QueueMessageType.CLOSE_STATUS, reason))
     self.exit_event.set()
     self.metadrive_process.join()

tools/sim/lib/keyboard_ctrl.py

@@ -2,10 +2,13 @@
 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
@@ -52,35 +55,35 @@ def getch() -> str:
 def print_keyboard_help():
   print(f"Keyboard Commands:\n{KEYBOARD_HELP}")
 
-def keyboard_poll_thread(q: 'Queue[str]'):
+def keyboard_poll_thread(q: 'Queue[QueueMessage]'):
   print_keyboard_help()
 
   while True:
     c = getch()
     if c == '1':
-      q.put("cruise_up")
+      q.put(control_cmd_gen("cruise_up"))
     elif c == '2':
-      q.put("cruise_down")
+      q.put(control_cmd_gen("cruise_down"))
     elif c == '3':
-      q.put("cruise_cancel")
+      q.put(control_cmd_gen("cruise_cancel"))
     elif c == 'w':
-      q.put("throttle_%f" % 1.0)
+      q.put(control_cmd_gen(f"throttle_{1.0}"))
     elif c == 'a':
-      q.put("steer_%f" % -0.15)
+      q.put(control_cmd_gen(f"steer_{-0.15}"))
     elif c == 's':
-      q.put("brake_%f" % 1.0)
+      q.put(control_cmd_gen(f"brake_{1.0}"))
     elif c == 'd':
-      q.put("steer_%f" % 0.15)
+      q.put(control_cmd_gen(f"steer_{0.15}"))
     elif c == 'z':
-      q.put("blinker_left")
+      q.put(control_cmd_gen("blinker_left"))
     elif c == 'x':
-      q.put("blinker_right")
+      q.put(control_cmd_gen("blinker_right"))
     elif c == 'i':
-      q.put("ignition")
+      q.put(control_cmd_gen("ignition"))
     elif c == 'r':
-      q.put("reset")
+      q.put(control_cmd_gen("reset"))
     elif c == 'q':
-      q.put("quit")
+      q.put(control_cmd_gen("quit"))
       break
     else:
       print_keyboard_help()
@@ -92,7 +95,7 @@ def test(q: 'Queue[str]') -> NoReturn:
 
 if __name__ == '__main__':
   from multiprocessing import Process, Queue
-  q: Queue[str] = Queue()
+  q: 'Queue[QueueMessage]' = Queue()
   p = Process(target=test, args=(q,))
   p.daemon = True
   p.start()

tools/sim/lib/manual_ctrl.py

@@ -6,6 +6,8 @@
 from fcntl import ioctl
 from typing import NoReturn
 
+from openpilot.tools.sim.bridge.common import control_cmd_gen
+
 # Iterate over the joystick devices.
 print('Available devices:')
 for fn in os.listdir('/dev/input'):
@@ -153,33 +155,33 @@ def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
         fvalue = value / 32767.0
         axis_states[axis] = fvalue
         normalized = (1 - fvalue) * 50
-        q.put(f"throttle_{normalized:f}")
+        q.put(control_cmd_gen(f"throttle_{normalized:f}"))
 
       elif axis == "rz":  # brake
         fvalue = value / 32767.0
         axis_states[axis] = fvalue
         normalized = (1 - fvalue) * 50
-        q.put(f"brake_{normalized:f}")
+        q.put(control_cmd_gen(f"brake_{normalized:f}"))
 
       elif axis == "x":  # steer angle
         fvalue = value / 32767.0
         axis_states[axis] = fvalue
         normalized = fvalue
-        q.put(f"steer_{normalized:f}")
+        q.put(control_cmd_gen(f"steer_{normalized:f}"))
 
     elif mtype & 0x01:  # buttons
       if value == 1: # press down
         if number in [0, 19]:  # X
-          q.put("cruise_down")
+          q.put(control_cmd_gen("cruise_down"))
 
         elif number in [3, 18]:  # triangle
-          q.put("cruise_up")
+          q.put(control_cmd_gen("cruise_up"))
 
         elif number in [1, 6]:  # square
-          q.put("cruise_cancel")
+          q.put(control_cmd_gen("cruise_cancel"))
 
         elif number in [10, 21]:  # R3
-          q.put("reverse_switch")
+          q.put(control_cmd_gen("reverse_switch"))
 
 if __name__ == '__main__':
   from multiprocessing import Process, Queue