openpilot v0.11.1 release

date: 2026-06-04T09:49:56
master commit: c0ab3550eca2e9daf197c46b7e4b24aa9637cf2e

tools/sim/README.md

@@ -0,0 +1,50 @@
+openpilot in simulator
+=====================
+
+openpilot implements a [bridge](run_bridge.py) that allows it to run in the [MetaDrive simulator](https://github.com/metadriverse/metadrive).
+
+## Launching openpilot
+First, start openpilot.
+``` bash
+# Run locally
+./tools/sim/launch_openpilot.sh
+```
+
+## Bridge usage
+```
+$ ./run_bridge.py -h
+usage: run_bridge.py [-h] [--joystick] [--high_quality] [--dual_camera]
+Bridge between the simulator and openpilot.
+
+options:
+  -h, --help            show this help message and exit
+  --joystick
+  --high_quality
+  --dual_camera
+```
+
+#### Bridge Controls:
+- To engage openpilot press 2, then press 1 to increase the speed and 2 to decrease.
+- To disengage, press "S" (simulates a user brake)
+
+#### All inputs:
+
+```
+| 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      |
+```
+
+## MetaDrive
+
+### Launching Metadrive
+Start bridge processes located in tools/sim:
+``` bash
+./run_bridge.py
+```

tools/sim/bridge/common.py

@@ -0,0 +1,206 @@
+import signal
+import threading
+import functools
+import numpy as np
+
+from collections import namedtuple
+from enum import Enum
+from multiprocessing import Process, Queue, Value
+from abc import ABC, abstractmethod
+
+from opendbc.car.honda.values import CruiseButtons
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.test.helpers import set_params_enabled
+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)
+  while not exit_event.is_set():
+    function()
+    rk.keep_time()
+
+
+class SimulatorBridge(ABC):
+  TICKS_PER_FRAME = 5
+
+  def __init__(self, dual_camera, high_quality):
+    set_params_enabled()
+    self.params = Params()
+    self.params.put_bool("AlphaLongitudinalEnabled", True, block=True)
+
+    self.rk = Ratekeeper(100, None)
+
+    self.dual_camera = dual_camera
+    self.high_quality = high_quality
+
+    self._exit_event: threading.Event | None = None
+    self._threads = []
+    self._keep_alive = True
+    self.started = Value('i', False)
+    signal.signal(signal.SIGTERM, self._on_shutdown)
+    self.simulator_state = SimulatorState()
+
+    self.world: World | None = None
+
+    self.past_startup_engaged = False
+    self.startup_button_prev = True
+
+    self.test_run = False
+
+  def _on_shutdown(self, signal, frame):
+    self.shutdown()
+
+  def shutdown(self):
+    self._keep_alive = False
+
+  def bridge_keep_alive(self, q: Queue, retries: int):
+    try:
+      self._run(q)
+    finally:
+      self.close("bridge terminated")
+
+  def close(self, reason):
+    self.started.value = False
+
+    if self._exit_event is not None:
+      self._exit_event.set()
+
+    if self.world is not None:
+      self.world.close(reason)
+
+  def run(self, queue, retries=-1):
+    bridge_p = Process(name="bridge", target=self.bridge_keep_alive, args=(queue, retries))
+    bridge_p.start()
+    return bridge_p
+
+  def print_status(self):
+    print(
+    f"""
+State:
+Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_engaged}
+    """)
+
+  @abstractmethod
+  def spawn_world(self, q: Queue) -> World:
+    pass
+
+  def _run(self, q: Queue):
+    self.world = self.spawn_world(q)
+
+    self.simulated_car = SimulatedCar()
+    self.simulated_sensors = SimulatedSensors(self.dual_camera)
+
+    self._exit_event = threading.Event()
+
+    self.simulated_car_thread = threading.Thread(target=rk_loop, args=(functools.partial(self.simulated_car.update, self.simulator_state),
+                                                                        100, self._exit_event))
+    self.simulated_car_thread.start()
+
+    self.simulated_camera_thread = threading.Thread(target=rk_loop, args=(functools.partial(self.simulated_sensors.send_camera_images, self.world),
+                                                                        20, self._exit_event))
+    self.simulated_camera_thread.start()
+
+    # Simulation tends to be slow in the initial steps. This prevents lagging later
+    for _ in range(20):
+      self.world.tick()
+
+    while self._keep_alive:
+      throttle_out = steer_out = brake_out = 0.0
+      throttle_op = steer_op = brake_op = 0.0
+
+      self.simulator_state.cruise_button = 0
+      self.simulator_state.left_blinker = False
+      self.simulator_state.right_blinker = False
+
+      throttle_manual = steer_manual = brake_manual = 0.
+
+      # Read manual controls
+      if not q.empty():
+        message = q.get()
+        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
+      self.simulator_state.user_torque = steer_manual * -10000
+
+      steer_manual = steer_manual * -40
+
+      # Update openpilot on current sensor state
+      self.simulated_sensors.update(self.simulator_state, self.world)
+
+      self.simulated_car.sm.update(0)
+      self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
+
+      if self.simulator_state.is_engaged:
+        throttle_op = np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
+        brake_op = np.clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
+        steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
+
+        self.past_startup_engaged = True
+      elif not self.past_startup_engaged and self.simulated_car.sm['selfdriveState'].engageable:
+        self.simulator_state.cruise_button = CruiseButtons.DECEL_SET if self.startup_button_prev else CruiseButtons.MAIN # force engagement on startup
+        self.startup_button_prev = not self.startup_button_prev
+
+      throttle_out = throttle_op if self.simulator_state.is_engaged else throttle_manual
+      brake_out = brake_op if self.simulator_state.is_engaged else brake_manual
+      steer_out = steer_op if self.simulator_state.is_engaged else steer_manual
+
+      self.world.apply_controls(steer_out, throttle_out, brake_out)
+      self.world.read_state()
+      self.world.read_sensors(self.simulator_state)
+
+      if self.world.exit_event.is_set():
+        self.shutdown()
+
+      if self.rk.frame % self.TICKS_PER_FRAME == 0:
+        self.world.tick()
+        self.world.read_cameras()
+
+      # don't print during test, so no print/IO Block between OP and metadrive processes
+      if not self.test_run and self.rk.frame % 25 == 0:
+        self.print_status()
+
+      self.started.value = True
+
+      self.rk.keep_time()

tools/sim/bridge/metadrive/metadrive_bridge.py

@@ -0,0 +1,93 @@
+import math
+from multiprocessing import Queue
+
+from metadrive.component.sensors.base_camera import _cuda_enable
+from metadrive.component.map.pg_map import MapGenerateMethod
+
+from openpilot.tools.sim.bridge.common import SimulatorBridge
+from openpilot.tools.sim.bridge.metadrive.metadrive_common import RGBCameraRoad, RGBCameraWide
+from openpilot.tools.sim.bridge.metadrive.metadrive_world import MetaDriveWorld
+from openpilot.tools.sim.lib.camerad import W, H
+
+
+def straight_block(length):
+  return {
+    "id": "S",
+    "pre_block_socket_index": 0,
+    "length": length
+  }
+
+def curve_block(length, angle=45, direction=0):
+  return {
+    "id": "C",
+    "pre_block_socket_index": 0,
+    "length": length,
+    "radius": length,
+    "angle": angle,
+    "dir": direction
+  }
+
+def create_map(track_size=60):
+  curve_len = track_size * 2
+  return dict(
+    type=MapGenerateMethod.PG_MAP_FILE,
+    lane_num=2,
+    lane_width=4.5,
+    config=[
+      None,
+      straight_block(track_size),
+      curve_block(curve_len, 90),
+      straight_block(track_size),
+      curve_block(curve_len, 90),
+      straight_block(track_size),
+      curve_block(curve_len, 90),
+      straight_block(track_size),
+      curve_block(curve_len, 90),
+    ]
+  )
+
+
+class MetaDriveBridge(SimulatorBridge):
+  TICKS_PER_FRAME = 5
+
+  def __init__(self, dual_camera, high_quality, test_duration=math.inf, test_run=False):
+    super().__init__(dual_camera, high_quality)
+
+    self.should_render = False
+    self.test_run = test_run
+    self.test_duration = test_duration if self.test_run else math.inf
+
+  def spawn_world(self, queue: Queue):
+    sensors = {
+      "rgb_road": (RGBCameraRoad, W, H, )
+    }
+
+    if self.dual_camera:
+      sensors["rgb_wide"] = (RGBCameraWide, W, H)
+
+    config = dict(
+      use_render=self.should_render,
+      vehicle_config=dict(
+        enable_reverse=False,
+        render_vehicle=False,
+        image_source="rgb_road",
+      ),
+      sensors=sensors,
+      image_on_cuda=_cuda_enable,
+      image_observation=True,
+      interface_panel=[],
+      out_of_route_done=False,
+      on_continuous_line_done=False,
+      crash_vehicle_done=False,
+      crash_object_done=False,
+      arrive_dest_done=False,
+      traffic_density=0.0, # traffic is incredibly expensive
+      map_config=create_map(),
+      decision_repeat=1,
+      physics_world_step_size=self.TICKS_PER_FRAME/100,
+      preload_models=False,
+      show_logo=False,
+      anisotropic_filtering=False
+    )
+
+    return MetaDriveWorld(queue, config, self.test_duration, self.test_run, self.dual_camera)

tools/sim/bridge/metadrive/metadrive_common.py

@@ -0,0 +1,35 @@
+import numpy as np
+
+from metadrive.component.sensors.rgb_camera import RGBCamera
+from panda3d.core import Texture, GraphicsOutput
+
+
+class CopyRamRGBCamera(RGBCamera):
+  """Camera which copies its content into RAM during the render process, for faster image grabbing."""
+  def __init__(self, *args, **kwargs):
+    super().__init__(*args, **kwargs)
+    self.cpu_texture = Texture()
+    self.buffer.addRenderTexture(self.cpu_texture, GraphicsOutput.RTMCopyRam)
+
+  def get_rgb_array_cpu(self):
+    origin_img = self.cpu_texture
+    img = np.frombuffer(origin_img.getRamImageAs("RGB").getData(), dtype=np.uint8)
+    img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), 3))
+    img = img[::-1]  # Flip on vertical axis
+    return img
+
+
+class RGBCameraWide(CopyRamRGBCamera):
+  def __init__(self, *args, **kwargs):
+    super().__init__(*args, **kwargs)
+    lens = self.get_lens()
+    lens.setFov(120)
+    lens.setNear(0.1)
+
+
+class RGBCameraRoad(CopyRamRGBCamera):
+  def __init__(self, *args, **kwargs):
+    super().__init__(*args, **kwargs)
+    lens = self.get_lens()
+    lens.setFov(40)
+    lens.setNear(0.1)

tools/sim/bridge/metadrive/metadrive_process.py

@@ -0,0 +1,154 @@
+import math
+import time
+import numpy as np
+
+from collections import namedtuple
+from panda3d.core import Vec3
+from multiprocessing.connection import Connection
+
+from metadrive.engine.core.engine_core import EngineCore
+from metadrive.engine.core.image_buffer import ImageBuffer
+from metadrive.envs.metadrive_env import MetaDriveEnv
+from metadrive.obs.image_obs import ImageObservation
+
+from openpilot.common.realtime import Ratekeeper
+
+from openpilot.tools.sim.lib.common import vec3
+from openpilot.tools.sim.lib.camerad import W, H
+
+C3_POSITION = Vec3(0.0, 0, 1.22)
+C3_HPR = Vec3(0, 0,0)
+
+
+metadrive_simulation_state = namedtuple("metadrive_simulation_state", ["running", "done", "done_info"])
+metadrive_vehicle_state = namedtuple("metadrive_vehicle_state", ["velocity", "position", "bearing", "steering_angle"])
+
+def apply_metadrive_patches(arrive_dest_done=True):
+  # By default, metadrive won't try to use cuda images unless it's used as a sensor for vehicles, so patch that in
+  def add_image_sensor_patched(self, name: str, cls, args):
+    if self.global_config["image_on_cuda"]:# and name == self.global_config["vehicle_config"]["image_source"]:
+        sensor = cls(*args, self, cuda=True)
+    else:
+        sensor = cls(*args, self, cuda=False)
+    assert isinstance(sensor, ImageBuffer), "This API is for adding image sensor"
+    self.sensors[name] = sensor
+
+  EngineCore.add_image_sensor = add_image_sensor_patched
+
+  # we aren't going to use the built-in observation stack, so disable it to save time
+  def observe_patched(self, *args, **kwargs):
+    return self.state
+
+  ImageObservation.observe = observe_patched
+
+  # disable destination, we want to loop forever
+  def arrive_destination_patch(self, *args, **kwargs):
+    return False
+
+  if not arrive_dest_done:
+    MetaDriveEnv._is_arrive_destination = arrive_destination_patch
+
+def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock,
+                      controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection,
+                      exit_event, op_engaged, test_duration, test_run):
+  arrive_dest_done = config.pop("arrive_dest_done", True)
+  apply_metadrive_patches(arrive_dest_done)
+
+  road_image = np.frombuffer(camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+  if dual_camera:
+    assert wide_camera_array is not None
+    wide_road_image = np.frombuffer(wide_camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+
+  env = MetaDriveEnv(config)
+
+  def get_current_lane_info(vehicle):
+    _, lane_info, on_lane = vehicle.navigation._get_current_lane(vehicle)
+    lane_idx = lane_info[2] if lane_info is not None else None
+    return lane_idx, on_lane
+
+  def reset():
+    env.reset()
+    env.vehicle.config["max_speed_km_h"] = 1000
+    lane_idx_prev, _ = get_current_lane_info(env.vehicle)
+
+    simulation_state = metadrive_simulation_state(
+      running=True,
+      done=False,
+      done_info=None,
+    )
+    simulation_state_send.send(simulation_state)
+
+    return lane_idx_prev
+
+  lane_idx_prev = reset()
+  start_time = None
+
+  def get_cam_as_rgb(cam):
+    cam = env.engine.sensors[cam]
+    cam.get_cam().reparentTo(env.vehicle.origin)
+    cam.get_cam().setPos(C3_POSITION)
+    cam.get_cam().setHpr(C3_HPR)
+    img = cam.perceive(to_float=False)
+    if not isinstance(img, np.ndarray):
+      img = img.get() # convert cupy array to numpy
+    return img
+
+  rk = Ratekeeper(100, None)
+
+  steer_ratio = 8
+  vc = [0,0]
+
+  while not exit_event.is_set():
+    vehicle_state = metadrive_vehicle_state(
+      velocity=vec3(x=float(env.vehicle.velocity[0]), y=float(env.vehicle.velocity[1]), z=0),
+      position=env.vehicle.position,
+      bearing=float(math.degrees(env.vehicle.heading_theta)),
+      steering_angle=env.vehicle.steering * env.vehicle.MAX_STEERING
+    )
+    vehicle_state_send.send(vehicle_state)
+
+    if controls_recv.poll(0):
+      while controls_recv.poll(0):
+        steer_angle, gas, should_reset = controls_recv.recv()
+
+      steer_metadrive = steer_angle * 1 / (env.vehicle.MAX_STEERING * steer_ratio)
+      steer_metadrive = np.clip(steer_metadrive, -1, 1)
+
+      vc = [steer_metadrive, gas]
+
+      if should_reset:
+        lane_idx_prev = reset()
+        start_time = None
+
+    is_engaged = op_engaged.is_set()
+    if is_engaged and start_time is None:
+      start_time = time.monotonic()
+
+    if rk.frame % 5 == 0:
+      _, _, terminated, _, _ = env.step(vc)
+      timeout = True if start_time is not None and time.monotonic() - start_time >= test_duration else False
+      lane_idx_curr, on_lane = get_current_lane_info(env.vehicle)
+      out_of_lane = lane_idx_curr != lane_idx_prev or not on_lane
+      lane_idx_prev = lane_idx_curr
+
+      if terminated or ((out_of_lane or timeout) and test_run):
+        if terminated:
+          done_result = env.done_function("default_agent")
+        elif out_of_lane:
+          done_result = (True, {"out_of_lane" : True})
+        elif timeout:
+          done_result = (True, {"timeout" : True})
+
+        simulation_state = metadrive_simulation_state(
+          running=False,
+          done=done_result[0],
+          done_info=done_result[1],
+        )
+        simulation_state_send.send(simulation_state)
+
+      if dual_camera:
+        wide_road_image[...] = get_cam_as_rgb("rgb_wide")
+      road_image[...] = get_cam_as_rgb("rgb_road")
+      image_lock.release()
+
+    rk.keep_time()

tools/sim/bridge/metadrive/metadrive_world.py

@@ -0,0 +1,132 @@
+import ctypes
+import functools
+import multiprocessing
+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
+from openpilot.tools.sim.lib.camerad import W, H
+
+
+class MetaDriveWorld(World):
+  def __init__(self, status_q, config, test_duration, test_run, dual_camera=False):
+    super().__init__(dual_camera)
+    self.status_q = status_q
+    self.camera_array = Array(ctypes.c_uint8, W*H*3)
+    self.road_image = np.frombuffer(self.camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+    self.wide_camera_array = None
+    if dual_camera:
+      self.wide_camera_array = Array(ctypes.c_uint8, W*H*3)
+      self.wide_road_image = np.frombuffer(self.wide_camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+
+    self.controls_send, self.controls_recv = Pipe()
+    self.simulation_state_send, self.simulation_state_recv = Pipe()
+    self.vehicle_state_send, self.vehicle_state_recv = Pipe()
+
+    self.exit_event = multiprocessing.Event()
+    self.op_engaged = multiprocessing.Event()
+
+    self.test_run = test_run
+
+    self.first_engage = None
+    self.last_check_timestamp = 0
+    self.distance_moved = 0
+
+    self.metadrive_process = multiprocessing.Process(name="metadrive process", target=
+                              functools.partial(metadrive_process, dual_camera, config,
+                                                self.camera_array, self.wide_camera_array, self.image_lock,
+                                                self.controls_recv, self.simulation_state_send,
+                                                self.vehicle_state_send, self.exit_event, self.op_engaged, test_duration, self.test_run))
+
+    self.metadrive_process.start()
+    self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting"))
+
+    print("----------------------------------------------------------")
+    print("---- Spawning Metadrive world, this might take awhile ----")
+    print("----------------------------------------------------------")
+
+    self.vehicle_last_pos = self.vehicle_state_recv.recv().position # wait for a state message to ensure metadrive is launched
+    self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "started"))
+
+    self.steer_ratio = 15
+    self.vc = [0.0,0.0]
+    self.reset_time = 0
+    self.should_reset = False
+
+  def apply_controls(self, steer_angle, throttle_out, brake_out):
+    if (time.monotonic() - self.reset_time) > 2:
+      self.vc[0] = steer_angle
+
+      if throttle_out:
+        self.vc[1] = throttle_out
+      else:
+        self.vc[1] = -brake_out
+    else:
+      self.vc[0] = 0
+      self.vc[1] = 0
+
+    self.controls_send.send([*self.vc, self.should_reset])
+    self.should_reset = False
+
+  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(QueueMessage(QueueMessageType.TERMINATION_INFO, md_state.done_info))
+        self.exit_event.set()
+
+  def read_sensors(self, state: SimulatorState):
+    while self.vehicle_state_recv.poll(0):
+      md_vehicle: metadrive_vehicle_state = self.vehicle_state_recv.recv()
+      curr_pos = md_vehicle.position
+
+      state.velocity = md_vehicle.velocity
+      state.bearing = md_vehicle.bearing
+      state.steering_angle = md_vehicle.steering_angle
+      state.gps.from_xy(curr_pos)
+      state.valid = True
+
+      is_engaged = state.is_engaged
+      if is_engaged and self.first_engage is None:
+        self.first_engage = time.monotonic()
+        self.op_engaged.set()
+
+      # check moving 5 seconds after engaged, doesn't move right away
+      after_engaged_check = is_engaged and time.monotonic() - self.first_engage >= 5 and self.test_run
+
+      x_dist = abs(curr_pos[0] - self.vehicle_last_pos[0])
+      y_dist = abs(curr_pos[1] - self.vehicle_last_pos[1])
+      dist_threshold = 1
+      if x_dist >= dist_threshold or y_dist >= dist_threshold: # position not the same during staying still, > threshold is considered moving
+        self.distance_moved += x_dist + y_dist
+
+      time_check_threshold = 29
+      current_time = time.monotonic()
+      since_last_check = current_time - self.last_check_timestamp
+      if since_last_check >= time_check_threshold:
+        if after_engaged_check and self.distance_moved == 0:
+          self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True}))
+          self.exit_event.set()
+
+        self.last_check_timestamp = current_time
+        self.distance_moved = 0
+        self.vehicle_last_pos = curr_pos
+
+  def read_cameras(self):
+    pass
+
+  def tick(self):
+    pass
+
+  def reset(self):
+    self.should_reset = True
+
+  def close(self, reason: str):
+    self.status_q.put(QueueMessage(QueueMessageType.CLOSE_STATUS, reason))
+    self.exit_event.set()
+    self.metadrive_process.join()

tools/sim/launch_openpilot.sh

@@ -0,0 +1,21 @@
+#!/usr/bin/env bash
+
+export PASSIVE="0"
+export NOBOARD="1"
+export SIMULATION="1"
+export SKIP_FW_QUERY="1"
+export FINGERPRINT="HONDA_CIVIC_2022"
+
+export BLOCK="${BLOCK},camerad,loggerd,encoderd,micd,logmessaged,manage_athenad"
+if [[ "$CI" ]]; then
+  # TODO: offscreen UI should work
+  export BLOCK="${BLOCK},ui"
+fi
+
+python3 -c "from openpilot.selfdrive.test.helpers import set_params_enabled; set_params_enabled()"
+
+SCRIPT_DIR=$(dirname "$0")
+OPENPILOT_DIR=$SCRIPT_DIR/../../
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
+cd $OPENPILOT_DIR/system/manager && exec ./manager.py

tools/sim/lib/camerad.py

@@ -0,0 +1,78 @@
+import numpy as np
+
+from msgq.visionipc import VisionIpcServer, VisionStreamType
+from cereal import messaging
+
+from openpilot.tools.sim.lib.common import W, H
+
+
+def rgb_to_nv12(rgb):
+  """Convert RGB image to NV12 (YUV420) format using BT.601 coefficients."""
+  h, w = rgb.shape[:2]
+  r = rgb[:, :, 0].astype(np.int32)
+  g = rgb[:, :, 1].astype(np.int32)
+  b = rgb[:, :, 2].astype(np.int32)
+
+  # Y plane - BT.601 coefficients (matches original OpenCL kernel)
+  y = (((b * 13 + g * 65 + r * 33) + 64) >> 7) + 16
+  y = np.clip(y, 0, 255).astype(np.uint8)
+
+  # Subsample RGB for UV (2x2 box filter)
+  r_sub = (r[0::2, 0::2] + r[0::2, 1::2] + r[1::2, 0::2] + r[1::2, 1::2] + 2) >> 2
+  g_sub = (g[0::2, 0::2] + g[0::2, 1::2] + g[1::2, 0::2] + g[1::2, 1::2] + 2) >> 2
+  b_sub = (b[0::2, 0::2] + b[0::2, 1::2] + b[1::2, 0::2] + b[1::2, 1::2] + 2) >> 2
+
+  # U and V planes
+  u = np.clip((b_sub * 56 - g_sub * 37 - r_sub * 19 + 0x8080) >> 8, 0, 255).astype(np.uint8)
+  v = np.clip((r_sub * 56 - g_sub * 47 - b_sub * 9 + 0x8080) >> 8, 0, 255).astype(np.uint8)
+
+  # Interleave UV for NV12 format
+  uv = np.empty((h // 2, w), dtype=np.uint8)
+  uv[:, 0::2] = u
+  uv[:, 1::2] = v
+
+  return np.concatenate([y.ravel(), uv.ravel()]).tobytes()
+
+
+class Camerad:
+  """Simulates the camerad daemon"""
+  def __init__(self, dual_camera):
+    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState'])
+
+    self.frame_road_id = 0
+    self.frame_wide_id = 0
+    self.vipc_server = VisionIpcServer("camerad")
+
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 5, W, H)
+    if dual_camera:
+      self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 5, W, H)
+
+    self.vipc_server.start_listener()
+
+  def cam_send_yuv_road(self, yuv):
+    self._send_yuv(yuv, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD)
+    self.frame_road_id += 1
+
+  def cam_send_yuv_wide_road(self, yuv):
+    self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD)
+    self.frame_wide_id += 1
+
+  def rgb_to_yuv(self, rgb):
+    """Convert RGB to NV12 YUV format."""
+    assert rgb.shape == (H, W, 3), f"{rgb.shape}"
+    assert rgb.dtype == np.uint8
+    return rgb_to_nv12(rgb)
+
+  def _send_yuv(self, yuv, frame_id, pub_type, yuv_type):
+    eof = int(frame_id * 0.05 * 1e9)
+    self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof)
+
+    dat = messaging.new_message(pub_type, valid=True)
+    msg = {
+      "frameId": frame_id,
+      "transform": [1.0, 0.0, 0.0,
+                    0.0, 1.0, 0.0,
+                    0.0, 0.0, 1.0]
+    }
+    setattr(dat, pub_type, msg)
+    self.pm.send(pub_type, dat)

tools/sim/lib/common.py

@@ -0,0 +1,100 @@
+import math
+import multiprocessing
+import numpy as np
+
+from abc import ABC, abstractmethod
+from collections import namedtuple
+
+W, H = 1928, 1208
+
+
+vec3 = namedtuple("vec3", ["x", "y", "z"])
+
+class GPSState:
+  def __init__(self):
+    self.latitude = 0
+    self.longitude = 0
+    self.altitude = 0
+
+  def from_xy(self, xy):
+    """Simulates a lat/lon from an xy coordinate on a plane, for simple simulation. TODO: proper global projection?"""
+    BASE_LAT = 32.75308505188913
+    BASE_LON = -117.2095393365393
+    DEG_TO_METERS = 100000
+
+    self.latitude = float(BASE_LAT + xy[0] / DEG_TO_METERS)
+    self.longitude = float(BASE_LON + xy[1] / DEG_TO_METERS)
+    self.altitude = 0
+
+
+class IMUState:
+  def __init__(self):
+    self.accelerometer: vec3 = vec3(0,0,0)
+    self.gyroscope: vec3 = vec3(0,0,0)
+    self.bearing: float = 0
+
+
+class SimulatorState:
+  def __init__(self):
+    self.valid = False
+    self.is_engaged = False
+    self.ignition = True
+
+    self.velocity: vec3 = None
+    self.bearing: float = 0
+    self.gps = GPSState()
+    self.imu = IMUState()
+
+    self.steering_angle: float = 0
+
+    self.user_gas: float = 0
+    self.user_brake: float = 0
+    self.user_torque: float = 0
+
+    self.cruise_button = 0
+
+    self.left_blinker = False
+    self.right_blinker = False
+
+  @property
+  def speed(self):
+    return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
+
+
+class World(ABC):
+  def __init__(self, dual_camera):
+    self.dual_camera = dual_camera
+
+    self.image_lock = multiprocessing.Semaphore(value=0)
+    self.road_image = np.zeros((H, W, 3), dtype=np.uint8)
+    self.wide_road_image = np.zeros((H, W, 3), dtype=np.uint8)
+
+    self.exit_event = multiprocessing.Event()
+
+  @abstractmethod
+  def apply_controls(self, steer_sim, throttle_out, brake_out):
+    pass
+
+  @abstractmethod
+  def tick(self):
+    pass
+
+  @abstractmethod
+  def read_state(self):
+    pass
+
+  @abstractmethod
+  def read_sensors(self, simulator_state: SimulatorState):
+    pass
+
+  @abstractmethod
+  def read_cameras(self):
+    pass
+
+  @abstractmethod
+  def close(self, reason: str):
+    pass
+
+  @abstractmethod
+  def reset(self):
+    pass

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)

tools/sim/lib/manual_ctrl.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+# set up wheel
+import array
+import os
+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'):
+  if fn.startswith('js'):
+    print(f'  /dev/input/{fn}')
+
+# We'll store the states here.
+axis_states: dict[str, float] = {}
+button_states: dict[str, float] = {}
+
+# These constants were borrowed from linux/input.h
+axis_names = {
+  0x00 : 'x',
+  0x01 : 'y',
+  0x02 : 'z',
+  0x03 : 'rx',
+  0x04 : 'ry',
+  0x05 : 'rz',
+  0x06 : 'trottle',
+  0x07 : 'rudder',
+  0x08 : 'wheel',
+  0x09 : 'gas',
+  0x0a : 'brake',
+  0x10 : 'hat0x',
+  0x11 : 'hat0y',
+  0x12 : 'hat1x',
+  0x13 : 'hat1y',
+  0x14 : 'hat2x',
+  0x15 : 'hat2y',
+  0x16 : 'hat3x',
+  0x17 : 'hat3y',
+  0x18 : 'pressure',
+  0x19 : 'distance',
+  0x1a : 'tilt_x',
+  0x1b : 'tilt_y',
+  0x1c : 'tool_width',
+  0x20 : 'volume',
+  0x28 : 'misc',
+}
+
+button_names = {
+  0x120 : 'trigger',
+  0x121 : 'thumb',
+  0x122 : 'thumb2',
+  0x123 : 'top',
+  0x124 : 'top2',
+  0x125 : 'pinkie',
+  0x126 : 'base',
+  0x127 : 'base2',
+  0x128 : 'base3',
+  0x129 : 'base4',
+  0x12a : 'base5',
+  0x12b : 'base6',
+  0x12f : 'dead',
+  0x130 : 'a',
+  0x131 : 'b',
+  0x132 : 'c',
+  0x133 : 'x',
+  0x134 : 'y',
+  0x135 : 'z',
+  0x136 : 'tl',
+  0x137 : 'tr',
+  0x138 : 'tl2',
+  0x139 : 'tr2',
+  0x13a : 'select',
+  0x13b : 'start',
+  0x13c : 'mode',
+  0x13d : 'thumbl',
+  0x13e : 'thumbr',
+
+  0x220 : 'dpad_up',
+  0x221 : 'dpad_down',
+  0x222 : 'dpad_left',
+  0x223 : 'dpad_right',
+
+  # XBox 360 controller uses these codes.
+  0x2c0 : 'dpad_left',
+  0x2c1 : 'dpad_right',
+  0x2c2 : 'dpad_up',
+  0x2c3 : 'dpad_down',
+}
+
+axis_name_list: list[str] = []
+button_name_list: list[str] = []
+
+def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
+  # Open the joystick device.
+  fn = '/dev/input/js0'
+  print(f'Opening {fn}...')
+  jsdev = open(fn, 'rb')
+
+  # Get the device name.
+  #buf = bytearray(63)
+  buf = array.array('B', [0] * 64)
+  ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf)  # JSIOCGNAME(len)
+  js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
+  print(f'Device name: {js_name}')
+
+  # Get number of axes and buttons.
+  buf = array.array('B', [0])
+  ioctl(jsdev, 0x80016a11, buf)  # JSIOCGAXES
+  num_axes = buf[0]
+
+  buf = array.array('B', [0])
+  ioctl(jsdev, 0x80016a12, buf)  # JSIOCGBUTTONS
+  num_buttons = buf[0]
+
+  # Get the axis map.
+  buf = array.array('B', [0] * 0x40)
+  ioctl(jsdev, 0x80406a32, buf)  # JSIOCGAXMAP
+
+  for _axis in buf[:num_axes]:
+    axis_name = axis_names.get(_axis, f'unknown(0x{_axis:02x})')
+    axis_name_list.append(axis_name)
+    axis_states[axis_name] = 0.0
+
+  # Get the button map.
+  buf = array.array('H', [0] * 200)
+  ioctl(jsdev, 0x80406a34, buf)  # JSIOCGBTNMAP
+
+  for btn in buf[:num_buttons]:
+    btn_name = button_names.get(btn, f'unknown(0x{btn:03x})')
+    button_name_list.append(btn_name)
+    button_states[btn_name] = 0
+
+  print(f'{num_axes} axes found: {", ".join(axis_name_list)}')
+  print(f'{num_buttons} buttons found: {", ".join(button_name_list)}')
+
+  # Enable FF
+  import evdev
+  from evdev import ecodes, InputDevice
+  device = evdev.list_devices()[0]
+  evtdev = InputDevice(device)
+  val = 24000
+  evtdev.write(ecodes.EV_FF, ecodes.FF_AUTOCENTER, val)
+
+  while True:
+    evbuf = jsdev.read(8)
+    value, mtype, number = struct.unpack('4xhBB', evbuf)
+    # print(mtype, number, value)
+    if mtype & 0x02:  # wheel & paddles
+      axis = axis_name_list[number]
+
+      if axis == "z":  # gas
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = (1 - fvalue) * 50
+        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(control_cmd_gen(f"brake_{normalized:f}"))
+
+      elif axis == "x":  # steer angle
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = fvalue
+        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(control_cmd_gen("cruise_down"))
+
+        elif number in [3, 18]:  # triangle
+          q.put(control_cmd_gen("cruise_up"))
+
+        elif number in [1, 6]:  # square
+          q.put(control_cmd_gen("cruise_cancel"))
+
+        elif number in [10, 21]:  # R3
+          q.put(control_cmd_gen("reverse_switch"))
+
+if __name__ == '__main__':
+  from multiprocessing import Process, Queue
+  q: 'Queue[str]' = Queue()
+  p = Process(target=wheel_poll_thread, args=(q,))
+  p.start()

tools/sim/lib/simulated_car.py

@@ -0,0 +1,111 @@
+import traceback
+import cereal.messaging as messaging
+
+from opendbc.can.packer import CANPacker
+from opendbc.can.parser import CANParser
+from opendbc.car.honda.values import HondaSafetyFlags
+from openpilot.common.params import Params
+from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp
+from openpilot.tools.sim.lib.common import SimulatorState
+
+
+class SimulatedCar:
+  """Simulates a honda civic 2022 (panda state + can messages) to OpenPilot"""
+  packer = CANPacker("honda_bosch_radarless_generated")
+
+  def __init__(self):
+    self.pm = messaging.PubMaster(['can', 'pandaStates'])
+    self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams', 'selfdriveState'])
+    self.cp = self.get_car_can_parser()
+    self.idx = 0
+    self.params = Params()
+    self.obd_multiplexing = False
+
+  @staticmethod
+  def get_car_can_parser():
+    dbc_f = 'honda_bosch_radarless_generated'
+    checks = []
+    return CANParser(dbc_f, checks, 0)
+
+  def send_can_messages(self, simulator_state: SimulatorState):
+    if not simulator_state.valid:
+      return
+
+    msg = []
+
+    # *** powertrain bus ***
+
+    speed = simulator_state.speed * 3.6 # convert m/s to kph
+    msg.append(self.packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}))
+    msg.append(self.packer.make_can_msg("WHEEL_SPEEDS", 0, {
+      "WHEEL_SPEED_FL": speed,
+      "WHEEL_SPEED_FR": speed,
+      "WHEEL_SPEED_RL": speed,
+      "WHEEL_SPEED_RR": speed
+    }))
+
+    msg.append(self.packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": simulator_state.cruise_button}))
+
+    msg.append(self.packer.make_can_msg("GEARBOX_AUTO", 0, {"GEAR_SHIFTER": 4}))
+    msg.append(self.packer.make_can_msg("GAS_PEDAL_2", 0, {}))
+    msg.append(self.packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}))
+    msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {"STEER_TORQUE_SENSOR": simulator_state.user_torque}))
+    msg.append(self.packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": simulator_state.steering_angle}))
+    msg.append(self.packer.make_can_msg("VSA_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if simulator_state.speed >= 1.0 else 0}))
+    msg.append(self.packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {}))
+    msg.append(self.packer.make_can_msg("EPB_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("DOORS_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("CRUISE", 0, {}))
+    msg.append(self.packer.make_can_msg("CRUISE_FAULT_STATUS", 0, {}))
+    msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0,
+                                    {
+                                      "MAIN_ON": 1,
+                                      "LEFT_BLINKER": simulator_state.left_blinker,
+                                      "RIGHT_BLINKER": simulator_state.right_blinker
+                                    }))
+    msg.append(self.packer.make_can_msg("POWERTRAIN_DATA", 0,
+                                    {
+                                    "ACC_STATUS": int(simulator_state.is_engaged),
+                                    "PEDAL_GAS": simulator_state.user_gas,
+                                    "BRAKE_PRESSED": simulator_state.user_brake > 0
+                                    }))
+    msg.append(self.packer.make_can_msg("CAR_SPEED", 0, {}))
+
+    # *** cam bus ***
+    msg.append(self.packer.make_can_msg("STEERING_CONTROL", 2, {}))
+    msg.append(self.packer.make_can_msg("ACC_HUD", 2, {}))
+    msg.append(self.packer.make_can_msg("LKAS_HUD", 2, {}))
+
+    self.pm.send('can', can_list_to_can_capnp(msg))
+
+  def send_panda_state(self, simulator_state):
+    self.sm.update(0)
+
+    if self.params.get_bool("ObdMultiplexingEnabled") != self.obd_multiplexing:
+      self.obd_multiplexing = not self.obd_multiplexing
+      self.params.put_bool("ObdMultiplexingChanged", True, block=True)
+
+    dat = messaging.new_message('pandaStates', 1)
+    dat.valid = True
+    dat.pandaStates[0] = {
+      'ignitionLine': simulator_state.ignition,
+      'pandaType': "blackPanda",
+      'controlsAllowed': True,
+      'safetyModel': 'hondaBosch',
+      'alternativeExperience': self.sm["carParams"].alternativeExperience,
+      'safetyParam': HondaSafetyFlags.RADARLESS.value | HondaSafetyFlags.BOSCH_LONG.value,
+    }
+    self.pm.send('pandaStates', dat)
+
+  def update(self, simulator_state: SimulatorState):
+    try:
+      self.send_can_messages(simulator_state)
+
+      if self.idx % 50 == 0: # only send panda states at 2hz
+        self.send_panda_state(simulator_state)
+
+      self.idx += 1
+    except Exception:
+      traceback.print_exc()
+      raise

tools/sim/lib/simulated_sensors.py

@@ -0,0 +1,118 @@
+import time
+
+from cereal import log
+import cereal.messaging as messaging
+
+from openpilot.common.realtime import DT_DMON
+from openpilot.tools.sim.lib.camerad import Camerad
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+  from openpilot.tools.sim.lib.common import World, SimulatorState
+
+
+class SimulatedSensors:
+  """Simulates the C3 sensors (acc, gyro, gps, peripherals, dm state, cameras) to OpenPilot"""
+
+  def __init__(self, dual_camera=False):
+    self.pm = messaging.PubMaster(['accelerometer', 'gyroscope', 'gpsLocationExternal', 'driverStateV2', 'driverMonitoringState', 'peripheralState'])
+    self.camerad = Camerad(dual_camera=dual_camera)
+    self.last_perp_update = 0
+    self.last_dmon_update = 0
+
+  def send_imu_message(self, simulator_state: 'SimulatorState'):
+    for _ in range(5):
+      dat = messaging.new_message('accelerometer', valid=True)
+      dat.accelerometer.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
+      dat.accelerometer.init('acceleration')
+      dat.accelerometer.acceleration.v = [simulator_state.imu.accelerometer.x, simulator_state.imu.accelerometer.y, simulator_state.imu.accelerometer.z]
+      self.pm.send('accelerometer', dat)
+
+      dat = messaging.new_message('gyroscope', valid=True)
+      dat.gyroscope.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
+      dat.gyroscope.init('gyroUncalibrated')
+      dat.gyroscope.gyroUncalibrated.v = [simulator_state.imu.gyroscope.x, simulator_state.imu.gyroscope.y, simulator_state.imu.gyroscope.z]
+      self.pm.send('gyroscope', dat)
+
+  def send_gps_message(self, simulator_state: 'SimulatorState'):
+    if not simulator_state.valid:
+      return
+
+    # transform from vel to NED
+    velNED = [
+      -simulator_state.velocity.y,
+      simulator_state.velocity.x,
+      simulator_state.velocity.z,
+    ]
+
+    for _ in range(10):
+      dat = messaging.new_message('gpsLocationExternal', valid=True)
+      dat.gpsLocationExternal = {
+        "unixTimestampMillis": int(time.time() * 1000),  # noqa: TID251
+        "flags": 1,  # valid fix
+        "horizontalAccuracy": 1.0,
+        "verticalAccuracy": 1.0,
+        "speedAccuracy": 0.1,
+        "bearingAccuracyDeg": 0.1,
+        "vNED": velNED,
+        "bearingDeg": simulator_state.imu.bearing,
+        "latitude": simulator_state.gps.latitude,
+        "longitude": simulator_state.gps.longitude,
+        "altitude": simulator_state.gps.altitude,
+        "speed": simulator_state.speed,
+        "source": log.GpsLocationData.SensorSource.ublox,
+      }
+
+      self.pm.send('gpsLocationExternal', dat)
+
+  def send_peripheral_state(self):
+    dat = messaging.new_message('peripheralState')
+    dat.valid = True
+    dat.peripheralState = {
+      'pandaType': log.PandaState.PandaType.blackPanda,
+      'voltage': 12000,
+      'current': 5678,
+      'fanSpeedRpm': 1000
+    }
+    self.pm.send('peripheralState', dat)
+
+  def send_fake_driver_monitoring(self):
+    # dmonitoringmodeld output
+    dat = messaging.new_message('driverStateV2')
+    dat.driverStateV2.leftDriverData.faceOrientation = [0., 0., 0.]
+    dat.driverStateV2.leftDriverData.faceProb = 1.0
+    dat.driverStateV2.rightDriverData.faceOrientation = [0., 0., 0.]
+    dat.driverStateV2.rightDriverData.faceProb = 1.0
+    self.pm.send('driverStateV2', dat)
+
+    # dmonitoringd output
+    dat = messaging.new_message('driverMonitoringState', valid=True)
+    dm = dat.driverMonitoringState
+    dm.alertLevel = log.DriverMonitoringState.AlertLevel.none
+    dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision
+    dm.visionPolicyState.faceDetected = True
+    dm.visionPolicyState.isDistracted = False
+    dm.visionPolicyState.awarenessPercent = 100
+    self.pm.send('driverMonitoringState', dat)
+
+  def send_camera_images(self, world: 'World'):
+    world.image_lock.acquire()
+    yuv = self.camerad.rgb_to_yuv(world.road_image)
+    self.camerad.cam_send_yuv_road(yuv)
+
+    if world.dual_camera:
+      yuv = self.camerad.rgb_to_yuv(world.wide_road_image)
+      self.camerad.cam_send_yuv_wide_road(yuv)
+
+  def update(self, simulator_state: 'SimulatorState', world: 'World'):
+    now = time.monotonic()
+    self.send_imu_message(simulator_state)
+    self.send_gps_message(simulator_state)
+
+    if (now - self.last_dmon_update) > DT_DMON/2:
+      self.send_fake_driver_monitoring()
+      self.last_dmon_update = now
+
+    if (now - self.last_perp_update) > 0.25:
+      self.send_peripheral_state()
+      self.last_perp_update = now

tools/sim/run_bridge.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+import argparse
+
+from typing import Any
+from multiprocessing import Queue
+
+from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge
+
+def create_bridge(dual_camera, high_quality):
+  queue: Any = Queue()
+
+  simulator_bridge = MetaDriveBridge(dual_camera, high_quality)
+  simulator_process = simulator_bridge.run(queue)
+
+  return queue, simulator_process, simulator_bridge
+
+def main():
+  _, simulator_process, _ = create_bridge(True, False)
+  simulator_process.join()
+
+def parse_args(add_args=None):
+  parser = argparse.ArgumentParser(description='Bridge between the simulator and openpilot.')
+  parser.add_argument('--joystick', action='store_true')
+  parser.add_argument('--high_quality', action='store_true')
+  parser.add_argument('--dual_camera', action='store_true')
+
+  return parser.parse_args(add_args)
+
+if __name__ == "__main__":
+  args = parse_args()
+
+  queue, simulator_process, simulator_bridge = create_bridge(args.dual_camera, args.high_quality)
+
+  if args.joystick:
+    # start input poll for joystick
+    from openpilot.tools.sim.lib.manual_ctrl import wheel_poll_thread
+
+    wheel_poll_thread(queue)
+  else:
+    # start input poll for keyboard
+    from openpilot.tools.sim.lib.keyboard_ctrl import keyboard_poll_thread
+
+    keyboard_poll_thread(queue)
+
+  simulator_bridge.shutdown()
+
+  simulator_process.join()

tools/sim/tests/conftest.py

@@ -0,0 +1,8 @@
+import pytest
+
+def pytest_addoption(parser):
+  parser.addoption("--test_duration", action="store", default=60, type=int, help="Seconds to run metadrive drive")
+
+@pytest.fixture
+def test_duration(request):
+  return request.config.getoption("--test_duration")

tools/sim/tests/test_metadrive_bridge.py

@@ -0,0 +1,17 @@
+import pytest
+import warnings
+
+# Since metadrive depends on pkg_resources, and pkg_resources is deprecated as an API
+warnings.filterwarnings("ignore", category=DeprecationWarning)
+
+from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge
+from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase
+
+@pytest.mark.slow
+class TestMetaDriveBridge(TestSimBridgeBase):
+  @pytest.fixture(autouse=True)
+  def setup_create_bridge(self, test_duration):
+    self.test_duration = 30
+
+  def create_bridge(self):
+    return MetaDriveBridge(False, False, self.test_duration, True)

tools/sim/tests/test_sim_bridge.py

@@ -0,0 +1,92 @@
+import os
+import subprocess
+import time
+import pytest
+
+from multiprocessing import Queue
+
+from cereal import messaging
+from openpilot.common.basedir import BASEDIR
+from openpilot.tools.sim.bridge.common import QueueMessageType
+
+SIM_DIR = os.path.join(BASEDIR, "tools/sim")
+
+class TestSimBridgeBase:
+  @classmethod
+  def setup_class(cls):
+    if cls is TestSimBridgeBase:
+      raise pytest.skip("Don't run this base class, run test_metadrive_bridge.py instead")
+
+  def setup_method(self):
+    self.processes = []
+
+  def test_driving(self):
+    # Startup manager and bridge.py. Check processes are running, then engage and verify.
+    p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR)
+    self.processes.append(p_manager)
+
+    sm = messaging.SubMaster(['selfdriveState', 'onroadEvents', 'managerState'])
+    q = Queue()
+    bridge = self.create_bridge()
+    p_bridge = bridge.run(q, retries=10)
+    self.processes.append(p_bridge)
+
+    max_time_per_step = 60
+
+    # Wait for bridge to startup
+    start_waiting = time.monotonic()
+    while not bridge.started.value and time.monotonic() < start_waiting + max_time_per_step:
+      time.sleep(0.1)
+    assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}"
+
+    start_time = time.monotonic()
+    no_car_events_issues_once = False
+    car_event_issues = []
+    not_running = []
+    while time.monotonic() < start_time + max_time_per_step:
+      sm.update()
+
+      not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning]
+      car_event_issues = [event.name for event in sm['onroadEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])]
+
+      if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0:
+        no_car_events_issues_once = True
+        break
+
+    assert no_car_events_issues_once, \
+                    f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'"
+
+    start_time = time.monotonic()
+    min_counts_control_active = 100
+    control_active = 0
+
+    while time.monotonic() < start_time + max_time_per_step:
+      sm.update()
+
+      if sm.all_alive() and sm['selfdriveState'].active:
+        control_active += 1
+
+        if control_active == min_counts_control_active:
+          break
+
+    assert min_counts_control_active == control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}"
+
+    failure_states = []
+    while bridge.started.value:
+      continue
+
+    while not q.empty():
+      state = q.get()
+      if state.type == QueueMessageType.TERMINATION_INFO:
+        done_info = state.info
+        failure_states = [done_state for done_state in done_info if done_state != "timeout" and done_info[done_state]]
+        break
+    assert len(failure_states) == 0, f"Simulator fails to finish a loop. Failure states: {failure_states}"
+
+  def teardown_method(self):
+    print("Test shutting down. CommIssues are acceptable")
+    for p in reversed(self.processes):
+      p.terminate()
+
+    for p in reversed(self.processes):
+      p.kill()