diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 46d09c7b9d..275a9a7640 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -2,6 +2,8 @@ import signal 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.common import SimulatorState, World 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 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga """) @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 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga # 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 diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 5c91238c55..cbac215092 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -53,7 +53,7 @@ class MetaDriveBridge(SimulatorBridge): 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 @@ class MetaDriveBridge(SimulatorBridge): preload_models=False ) - return MetaDriveWorld(Queue(), config, self.dual_camera) + return MetaDriveWorld(queue, config, self.dual_camera) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 3570205069..35a8288680 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -5,6 +5,8 @@ import numpy as np 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 @@ class MetaDriveWorld(World): 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 @@ class MetaDriveWorld(World): 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 @@ class MetaDriveWorld(World): 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() diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index ea255d9ce4..0a17f0ee85 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -2,10 +2,13 @@ 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 @@ -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() diff --git a/tools/sim/lib/manual_ctrl.py b/tools/sim/lib/manual_ctrl.py index 8a72296538..972f7023bb 100755 --- a/tools/sim/lib/manual_ctrl.py +++ b/tools/sim/lib/manual_ctrl.py @@ -6,6 +6,8 @@ import struct 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