Add openpilot tools

tools/sim/Dockerfile.sim

@@ -0,0 +1,37 @@
+FROM ghcr.io/commaai/openpilot-base-cl:latest
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+      tmux \
+      vim \
+    && rm -rf /var/lib/apt/lists/*
+
+# get same tmux config used on NEOS for debugging
+RUN cd $HOME && \
+    curl -O https://raw.githubusercontent.com/commaai/eon-neos-builder/master/devices/eon/home/.tmux.conf
+
+ENV OPENPILOT_PATH /tmp/openpilot
+ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH}
+
+RUN mkdir -p ${OPENPILOT_PATH}
+WORKDIR ${OPENPILOT_PATH}
+
+COPY SConstruct ${OPENPILOT_PATH}
+
+COPY ./openpilot ${OPENPILOT_PATH}/openpilot
+COPY ./body ${OPENPILOT_PATH}/body
+COPY ./third_party ${OPENPILOT_PATH}/third_party
+COPY ./site_scons ${OPENPILOT_PATH}/site_scons
+COPY ./rednose ${OPENPILOT_PATH}/rednose
+COPY ./rednose_repo/site_scons ${OPENPILOT_PATH}/rednose_repo/site_scons
+COPY ./common ${OPENPILOT_PATH}/common
+COPY ./opendbc ${OPENPILOT_PATH}/opendbc
+COPY ./cereal ${OPENPILOT_PATH}/cereal
+COPY ./panda ${OPENPILOT_PATH}/panda
+COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive
+COPY ./system ${OPENPILOT_PATH}/system
+COPY ./tools ${OPENPILOT_PATH}/tools
+COPY ./release ${OPENPILOT_PATH}/release
+
+RUN --mount=type=bind,source=.ci_cache/scons_cache,target=/tmp/scons_cache,rw scons -j$(nproc) --cache-readonly
+
+RUN python -c "from openpilot.selfdrive.test.helpers import set_params_enabled; set_params_enabled()"

tools/sim/Dockerfile.sim_nvidia

@@ -0,0 +1,21 @@
+FROM ubuntu:20.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update && \
+    apt-get install -y --no-install-recommends \
+      apt-utils \
+      sudo \
+      ssh \
+      curl \
+      ca-certificates \
+      git \
+      git-lfs && \
+    rm -rf /var/lib/apt/lists/*
+
+RUN curl -fsSL https://get.docker.com -o get-docker.sh && \
+    sudo sh get-docker.sh && \
+    distribution=$(. /etc/os-release;echo $ID$VERSION_ID) && \
+    curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - && \
+    curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list && \
+    sudo apt-get update && \
+    sudo apt-get install -y nvidia-docker2

tools/sim/README.md

@@ -0,0 +1,84 @@
+openpilot in simulator
+=====================
+
+openpilot implements a [bridge](run_bridge.py) that allows it to run in the [MetaDrive simulator](https://github.com/metadriverse/metadrive) or [CARLA simulator](https://carla.org/).
+
+## 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] [--simulator SIMULATOR] [--town TOWN] [--spawn_point NUM_SELECTED_SPAWN_POINT] [--host HOST] [--port PORT]
+
+Bridge between the simulator and openpilot.
+
+options:
+  -h, --help            show this help message and exit
+  --joystick
+  --high_quality
+  --dual_camera
+  --simulator SIMULATOR
+  --town TOWN
+  --spawn_point NUM_SELECTED_SPAWN_POINT
+  --host HOST
+  --port PORT
+```
+
+#### 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 --simulator metadrive
+```
+
+## Carla
+
+CARLA is also partially supported, though the performance is not great. It needs to be manually installed with:
+
+```bash
+poetry install --with=carla
+```
+
+openpilot doesn't have any extreme hardware requirements, however CARLA requires an NVIDIA graphics card and is very resource-intensive and may not run smoothly on your system.
+For this case, we have the simulator in low quality by default.
+
+You can also check out the [CARLA python documentation](https://carla.readthedocs.io/en/latest/python_api/) to find more parameters to tune that might increase performance on your system.
+
+### Launching Carla
+Start Carla simulator and bridge processes located in tools/sim:
+``` bash
+# Terminal 1
+./start_carla.sh
+
+# Terminal 2
+./run_bridge.py --simulator carla
+```
+
+## Further Reading
+
+The following resources contain more details and troubleshooting tips.
+* [CARLA on the openpilot wiki](https://github.com/commaai/openpilot/wiki/CARLA)

tools/sim/bridge/carla/carla_bridge.py

@@ -0,0 +1,22 @@
+from openpilot.tools.sim.bridge.common import SimulatorBridge
+from openpilot.tools.sim.bridge.carla.carla_world import CarlaWorld
+
+
+class CarlaBridge(SimulatorBridge):
+  TICKS_PER_FRAME = 5
+
+  def __init__(self, arguments):
+    super().__init__(arguments)
+    self.host = arguments.host
+    self.port = arguments.port
+    self.town = arguments.town
+    self.num_selected_spawn_point = arguments.num_selected_spawn_point
+
+  def spawn_world(self):
+    import carla
+
+    client = carla.Client(self.host, self.port)
+    client.set_timeout(5)
+
+    return CarlaWorld(client, high_quality=self.high_quality, dual_camera=self.dual_camera,
+                      num_selected_spawn_point=self.num_selected_spawn_point, town=self.town)

tools/sim/bridge/carla/carla_world.py

@@ -0,0 +1,145 @@
+import numpy as np
+
+from openpilot.common.params import Params
+from openpilot.tools.sim.lib.common import SimulatorState, vec3
+from openpilot.tools.sim.bridge.common import World
+from openpilot.tools.sim.lib.camerad import W, H
+
+
+class CarlaWorld(World):
+  def __init__(self, client, high_quality, dual_camera, num_selected_spawn_point, town):
+    super().__init__(dual_camera)
+    import carla
+
+    low_quality_layers = carla.MapLayer(carla.MapLayer.Ground | carla.MapLayer.Walls | carla.MapLayer.Decals)
+
+    layers = carla.MapLayer.All if high_quality else low_quality_layers
+
+    world = client.load_world(town, map_layers=layers)
+
+    settings = world.get_settings()
+    settings.fixed_delta_seconds = 0.01
+    world.apply_settings(settings)
+
+    world.set_weather(carla.WeatherParameters.ClearSunset)
+
+    self.world = world
+    world_map = world.get_map()
+
+    blueprint_library = world.get_blueprint_library()
+
+    vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1]
+    vehicle_bp.set_attribute('role_name', 'hero')
+    spawn_points = world_map.get_spawn_points()
+    assert len(spawn_points) > num_selected_spawn_point, \
+      f'''No spawn point {num_selected_spawn_point}, try a value between 0 and {len(spawn_points)} for this town.'''
+    self.spawn_point = spawn_points[num_selected_spawn_point]
+    self.vehicle = world.spawn_actor(vehicle_bp, self.spawn_point)
+
+    physics_control = self.vehicle.get_physics_control()
+    physics_control.mass = 2326
+    physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
+    physics_control.gear_switch_time = 0.0
+    self.vehicle.apply_physics_control(physics_control)
+
+    self.vc: carla.VehicleControl = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
+    self.max_steer_angle: float = self.vehicle.get_physics_control().wheels[0].max_steer_angle
+    self.params = Params()
+
+    self.steer_ratio = 15
+
+    self.carla_objects = []
+
+    transform = carla.Transform(carla.Location(x=0.8, z=1.13))
+
+    def create_camera(fov, callback):
+      blueprint = blueprint_library.find('sensor.camera.rgb')
+      blueprint.set_attribute('image_size_x', str(W))
+      blueprint.set_attribute('image_size_y', str(H))
+      blueprint.set_attribute('fov', str(fov))
+      blueprint.set_attribute('sensor_tick', str(1/20))
+      if not high_quality:
+        blueprint.set_attribute('enable_postprocess_effects', 'False')
+      camera = world.spawn_actor(blueprint, transform, attach_to=self.vehicle)
+      camera.listen(callback)
+      return camera
+
+    self.road_camera = create_camera(fov=40, callback=self.cam_callback_road)
+    if dual_camera:
+      self.road_wide_camera = create_camera(fov=120, callback=self.cam_callback_wide_road)  # fov bigger than 120 shows unwanted artifacts
+    else:
+      self.road_wide_camera = None
+
+    # re-enable IMU
+    imu_bp = blueprint_library.find('sensor.other.imu')
+    imu_bp.set_attribute('sensor_tick', '0.01')
+    self.imu = world.spawn_actor(imu_bp, transform, attach_to=self.vehicle)
+
+    gps_bp = blueprint_library.find('sensor.other.gnss')
+    self.gps = world.spawn_actor(gps_bp, transform, attach_to=self.vehicle)
+    self.params.put_bool("UbloxAvailable", True)
+
+    self.carla_objects = [self.imu, self.gps, self.road_camera, self.road_wide_camera, self.vehicle]
+
+  def close(self):
+    for s in self.carla_objects:
+      if s is not None:
+        try:
+          s.destroy()
+        except Exception as e:
+          print("Failed to destroy carla object", e)
+
+  def carla_image_to_rgb(self, image):
+    rgb = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
+    rgb = np.reshape(rgb, (H, W, 4))
+    return np.ascontiguousarray(rgb[:, :, [0, 1, 2]])
+
+  def cam_callback_road(self, image):
+    with self.image_lock:
+      self.road_image = self.carla_image_to_rgb(image)
+
+  def cam_callback_wide_road(self, image):
+    with self.image_lock:
+      self.wide_road_image = self.carla_image_to_rgb(image)
+
+  def apply_controls(self, steer_angle, throttle_out, brake_out):
+    self.vc.throttle = throttle_out
+
+    steer_carla = steer_angle * -1 / (self.max_steer_angle * self.steer_ratio)
+    steer_carla = np.clip(steer_carla, -1, 1)
+
+    self.vc.steer = steer_carla
+    self.vc.brake = brake_out
+    self.vehicle.apply_control(self.vc)
+
+  def read_sensors(self, simulator_state: SimulatorState):
+    simulator_state.imu.bearing = self.imu.get_transform().rotation.yaw
+
+    simulator_state.imu.accelerometer = vec3(
+      self.imu.get_acceleration().x,
+      self.imu.get_acceleration().y,
+      self.imu.get_acceleration().z
+    )
+
+    simulator_state.imu.gyroscope = vec3(
+      self.imu.get_angular_velocity().x,
+      self.imu.get_angular_velocity().y,
+      self.imu.get_angular_velocity().z
+    )
+
+    simulator_state.gps.from_xy([self.vehicle.get_location().x, self.vehicle.get_location().y])
+
+    simulator_state.velocity = self.vehicle.get_velocity()
+    simulator_state.valid = True
+    simulator_state.steering_angle = self.vc.steer * self.max_steer_angle
+
+  def read_cameras(self):
+    pass # cameras are read within a callback for carla
+
+  def tick(self):
+    self.world.tick()
+
+  def reset(self):
+    import carla
+    self.vehicle.set_transform(self.spawn_point)
+    self.vehicle.set_target_velocity(carla.Vector3D())

tools/sim/bridge/common.py

@@ -0,0 +1,186 @@
+import signal
+import threading
+import functools
+
+from multiprocessing import Process, Queue
+from abc import ABC, abstractmethod
+from typing import Optional
+
+from openpilot.common.params import Params
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.test.helpers import set_params_enabled
+from openpilot.selfdrive.car.honda.values import CruiseButtons
+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
+from openpilot.tools.sim.lib.keyboard_ctrl import KEYBOARD_HELP
+
+
+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, arguments):
+    set_params_enabled()
+    self.params = Params()
+
+    self.rk = Ratekeeper(100, None)
+
+    self.dual_camera = arguments.dual_camera
+    self.high_quality = arguments.high_quality
+
+    self._exit_event = threading.Event()
+    self._threads = []
+    self._keep_alive = True
+    self.started = False
+    signal.signal(signal.SIGTERM, self._on_shutdown)
+    self._exit = threading.Event()
+    self.simulator_state = SimulatorState()
+
+    self.world: Optional[World] = None
+
+    self.past_startup_engaged = 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()
+
+  def close(self):
+    self.started = False
+    self._exit_event.set()
+
+    if self.world is not None:
+      self.world.close()
+
+  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"""
+Keyboard Commands:
+{KEYBOARD_HELP}
+
+State:
+Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_engaged}
+    """)
+
+  @abstractmethod
+  def spawn_world(self) -> World:
+    pass
+
+  def _run(self, q: Queue):
+    self.world = self.spawn_world()
+
+    self.simulated_car = SimulatedCar()
+    self.simulated_sensors = SimulatedSensors(self.dual_camera)
+
+    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()
+        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
+
+      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)
+      controlsState = self.simulated_car.sm['controlsState']
+      self.simulator_state.is_engaged = controlsState.active
+
+      if self.simulator_state.is_engaged:
+        throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
+        brake_op = 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 controlsState.engageable:
+        self.simulator_state.cruise_button = CruiseButtons.DECEL_SET # force engagement on startup
+
+      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_sensors(self.simulator_state)
+
+      if self.rk.frame % self.TICKS_PER_FRAME == 0:
+        self.world.tick()
+        self.world.read_cameras()
+
+      if self.rk.frame % 25 == 0:
+        self.print_status()
+
+      self.started = True
+
+      self.rk.keep_time()

tools/sim/bridge/metadrive/metadrive_bridge.py

@@ -0,0 +1,119 @@
+import numpy as np
+
+from metadrive.component.sensors.rgb_camera import RGBCamera
+from metadrive.component.sensors.base_camera import _cuda_enable
+from metadrive.component.map.pg_map import MapGenerateMethod
+from panda3d.core import Vec3, Texture, GraphicsOutput
+
+from openpilot.tools.sim.bridge.common import SimulatorBridge
+from openpilot.tools.sim.bridge.metadrive.metadrive_world import MetaDriveWorld
+from openpilot.tools.sim.lib.camerad import W, H
+
+
+C3_POSITION = Vec3(0, 0, 1)
+
+
+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.getRamImage().getData(), dtype=np.uint8)
+    img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), -1))
+    img = img[:,:,:3] # RGBA to RGB
+    # img = np.swapaxes(img, 1, 0)
+    img = img[::-1] # Flip on vertical axis
+    return img
+
+
+class RGBCameraWide(CopyRamRGBCamera):
+  def __init__(self, *args, **kwargs):
+    super(RGBCameraWide, self).__init__(*args, **kwargs)
+    cam = self.get_cam()
+    cam.setPos(C3_POSITION)
+    lens = self.get_lens()
+    lens.setFov(160)
+
+class RGBCameraRoad(CopyRamRGBCamera):
+  def __init__(self, *args, **kwargs):
+    super(RGBCameraRoad, self).__init__(*args, **kwargs)
+    cam = self.get_cam()
+    cam.setPos(C3_POSITION)
+    lens = self.get_lens()
+    lens.setFov(40)
+
+
+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
+  }
+
+
+class MetaDriveBridge(SimulatorBridge):
+  TICKS_PER_FRAME = 5
+
+  def __init__(self, args):
+    self.should_render = False
+
+    super(MetaDriveBridge, self).__init__(args)
+
+  def spawn_world(self):
+    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,
+        image_source="rgb_road",
+        spawn_longitude=15
+      ),
+      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,
+      traffic_density=0.0, # traffic is incredibly expensive
+      map_config=dict(
+        type=MapGenerateMethod.PG_MAP_FILE,
+        config=[
+          None,
+          straight_block(120),
+          curve_block(240, 90),
+          straight_block(120),
+          curve_block(240, 90),
+          straight_block(120),
+          curve_block(240, 90),
+          straight_block(120),
+          curve_block(240, 90),
+        ]
+      ),
+      decision_repeat=1,
+      physics_world_step_size=self.TICKS_PER_FRAME/100,
+      preload_models=False
+    )
+
+    return MetaDriveWorld(config)

tools/sim/bridge/metadrive/metadrive_process.py

@@ -0,0 +1,99 @@
+import math
+import numpy as np
+
+from collections import namedtuple
+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
+
+
+metadrive_state = namedtuple("metadrive_state", ["velocity", "position", "bearing", "steering_angle"])
+
+def apply_metadrive_patches():
+  # 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, vehicle):
+    return self.state
+
+  ImageObservation.observe = observe_patched
+
+  def arrive_destination_patch(self, vehicle):
+    return False
+
+  MetaDriveEnv._is_arrive_destination = arrive_destination_patch
+
+def metadrive_process(dual_camera: bool, config: dict, camera_array, controls_recv: Connection, state_send: Connection, exit_event):
+  apply_metadrive_patches()
+
+  road_image = np.frombuffer(camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+
+  env = MetaDriveEnv(config)
+
+  def reset():
+    env.reset()
+    env.vehicle.config["max_speed_km_h"] = 1000
+
+  reset()
+
+  def get_cam_as_rgb(cam):
+    cam = env.engine.sensors[cam]
+    img = cam.perceive(env.vehicle, clip=False)
+    if type(img) != np.ndarray:
+      img = img.get() # convert cupy array to numpy
+    return img
+
+  rk = Ratekeeper(100, None)
+
+  steer_ratio = 15
+  vc = [0,0]
+
+  while not exit_event.is_set():
+    state = metadrive_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
+    )
+
+    state_send.send(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:
+        reset()
+
+    if rk.frame % 5 == 0:
+      obs, _, terminated, _, info = env.step(vc)
+
+      if terminated:
+        reset()
+
+      #if dual_camera:
+      #  wide_road_image = get_cam_as_rgb("rgb_wide")
+      road_image[...] = get_cam_as_rgb("rgb_road")
+
+    rk.keep_time()

tools/sim/bridge/metadrive/metadrive_world.py

@@ -0,0 +1,75 @@
+import ctypes
+import functools
+import multiprocessing
+import numpy as np
+import time
+
+from multiprocessing import Pipe, Array
+from openpilot.tools.sim.bridge.metadrive.metadrive_process import metadrive_process, metadrive_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, config, dual_camera = False):
+    super().__init__(dual_camera)
+    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.controls_send, self.controls_recv = Pipe()
+    self.state_send, self.state_recv = Pipe()
+
+    self.exit_event = multiprocessing.Event()
+
+    self.metadrive_process = multiprocessing.Process(name="metadrive process", target=
+                              functools.partial(metadrive_process, dual_camera, config,
+                                                self.camera_array, self.controls_recv, self.state_send, self.exit_event))
+    self.metadrive_process.start()
+
+    print("----------------------------------------------------------")
+    print("---- Spawning Metadrive world, this might take awhile ----")
+    print("----------------------------------------------------------")
+
+    self.state_recv.recv() # wait for a state message to ensure metadrive is launched
+
+    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_sensors(self, state: SimulatorState):
+    while self.state_recv.poll(0):
+      md_state: metadrive_state = self.state_recv.recv()
+      state.velocity = md_state.velocity
+      state.bearing = md_state.bearing
+      state.steering_angle = md_state.steering_angle
+      state.gps.from_xy(md_state.position)
+      state.valid = True
+
+  def read_cameras(self):
+    pass
+
+  def tick(self):
+    pass
+
+  def reset(self):
+    self.should_reset = True
+
+  def close(self):
+    self.exit_event.set()
+    self.metadrive_process.join()

tools/sim/build_container.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+cd $DIR/../../
+
+docker pull ghcr.io/commaai/openpilot-base-cl:latest
+docker build \
+  --cache-from ghcr.io/commaai/openpilot-sim:latest \
+  -t ghcr.io/commaai/openpilot-sim:latest \
+  -f tools/sim/Dockerfile.sim .

tools/sim/launch_openpilot.sh

@@ -0,0 +1,21 @@
+#!/bin/bash
+
+export PASSIVE="0"
+export NOBOARD="1"
+export SIMULATION="1"
+export SKIP_FW_QUERY="1"
+export FINGERPRINT="HONDA CIVIC 2016"
+
+export BLOCK="${BLOCK},camerad,loggerd,encoderd,micd,logmessaged"
+if [[ "$CI" ]]; then
+  # TODO: offscreen UI should work
+  export BLOCK="${BLOCK},ui"
+fi
+
+python -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/selfdrive/manager && exec ./manager.py

tools/sim/lib/camerad.py

@@ -0,0 +1,70 @@
+import numpy as np
+import os
+import pyopencl as cl
+import pyopencl.array as cl_array
+
+from cereal.visionipc import VisionIpcServer, VisionStreamType
+from cereal import messaging
+
+from openpilot.common.basedir import BASEDIR
+from openpilot.tools.sim.lib.common import W, H
+
+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, False, W, H)
+    if dual_camera:
+      self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 5, False, W, H)
+
+    self.vipc_server.start_listener()
+
+    # set up for pyopencl rgb to yuv conversion
+    self.ctx = cl.create_some_context()
+    self.queue = cl.CommandQueue(self.ctx)
+    cl_arg = f" -DHEIGHT={H} -DWIDTH={W} -DRGB_STRIDE={W * 3} -DUV_WIDTH={W // 2} -DUV_HEIGHT={H // 2} -DRGB_SIZE={W * H} -DCL_DEBUG "
+
+    kernel_fn = os.path.join(BASEDIR, "tools/sim/rgb_to_nv12.cl")
+    with open(kernel_fn) as f:
+      prg = cl.Program(self.ctx, f.read()).build(cl_arg)
+      self.krnl = prg.rgb_to_nv12
+    self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4
+    self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4
+
+  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
+
+  # Returns: yuv bytes
+  def rgb_to_yuv(self, rgb):
+    assert rgb.shape == (H, W, 3), f"{rgb.shape}"
+    assert rgb.dtype == np.uint8
+
+    rgb_cl = cl_array.to_device(self.queue, rgb)
+    yuv_cl = cl_array.empty_like(rgb_cl)
+    self.krnl(self.queue, (self.Wdiv4, self.Hdiv4), None, rgb_cl.data, yuv_cl.data).wait()
+    yuv = np.resize(yuv_cl.get(), rgb.size // 2)
+    return yuv.data.tobytes()
+
+  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)
+    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,94 @@
+import math
+import threading
+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 simlation. 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 = threading.Lock()
+    self.road_image = np.zeros((H, W, 3), dtype=np.uint8)
+    self.wide_road_image = np.zeros((H, W, 3), dtype=np.uint8)
+
+  @abstractmethod
+  def apply_controls(self, steer_sim, throttle_out, brake_out):
+    pass
+
+  @abstractmethod
+  def tick(self):
+    pass
+
+  @abstractmethod
+  def read_sensors(self, simulator_state: SimulatorState):
+    pass
+
+  @abstractmethod
+  def read_cameras(self):
+    pass
+
+  @abstractmethod
+  def close(self):
+    pass
+
+  @abstractmethod
+  def reset(self):
+    pass

tools/sim/lib/keyboard_ctrl.py

@@ -0,0 +1,94 @@
+import sys
+import termios
+import time
+
+from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
+                     ISTRIP, IXON, PARENB, VMIN, VTIME)
+from typing import NoReturn
+
+# Indexes for termios list.
+IFLAG = 0
+OFLAG = 1
+CFLAG = 2
+LFLAG = 3
+ISPEED = 4
+OSPEED = 5
+CC = 6
+
+STDIN_FD = sys.stdin.fileno()
+
+
+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:
+  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 keyboard_poll_thread(q: 'Queue[str]'):
+  while True:
+    c = getch()
+    if c == '1':
+      q.put("cruise_up")
+    elif c == '2':
+      q.put("cruise_down")
+    elif c == '3':
+      q.put("cruise_cancel")
+    elif c == 'w':
+      q.put("throttle_%f" % 1.0)
+    elif c == 'a':
+      q.put("steer_%f" % 0.15)
+    elif c == 's':
+      q.put("brake_%f" % 1.0)
+    elif c == 'd':
+      q.put("steer_%f" % -0.15)
+    elif c == 'z':
+      q.put("blinker_left")
+    elif c == 'x':
+      q.put("blinker_right")
+    elif c == 'i':
+      q.put("ignition")
+    elif c == 'r':
+      q.put("reset")
+    elif c == 'q':
+      q.put("quit")
+      break
+
+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[str] = 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,188 @@
+#!/usr/bin/env python3
+# set up wheel
+import array
+import os
+import struct
+from fcntl import ioctl
+from typing import NoReturn, Dict, List
+
+# 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('%d axes found: %s' % (num_axes, ', '.join(axis_name_list)))
+  print('%d buttons found: %s' % (num_buttons, ', '.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(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}")
+
+      elif axis == "x":  # steer angle
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = fvalue
+        q.put(f"steer_{normalized:f}")
+
+    elif mtype & 0x01:  # buttons
+      if value == 1: # press down
+        if number in [0, 19]:  # X
+          q.put("cruise_down")
+
+        elif number in [3, 18]:  # triangle
+          q.put("cruise_up")
+
+        elif number in [1, 6]:  # square
+          q.put("cruise_cancel")
+
+        elif number in [10, 21]:  # R3
+          q.put("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,119 @@
+import cereal.messaging as messaging
+
+from opendbc.can.packer import CANPacker
+from opendbc.can.parser import CANParser
+from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
+from openpilot.selfdrive.car import crc8_pedal
+from openpilot.tools.sim.lib.common import SimulatorState
+
+
+class SimulatedCar:
+  """Simulates a honda civic 2016 (panda state + can messages) to OpenPilot"""
+  packer = CANPacker("honda_civic_touring_2016_can_generated")
+  rpacker = CANPacker("acura_ilx_2016_nidec")
+
+  def __init__(self):
+    self.pm = messaging.PubMaster(['can', 'pandaStates'])
+    self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams'])
+    self.cp = self.get_car_can_parser()
+    self.idx = 0
+
+  @staticmethod
+  def get_car_can_parser():
+    dbc_f = 'honda_civic_touring_2016_can_generated'
+    checks = [
+      (0xe4, 100),
+      (0x1fa, 50),
+      (0x200, 50),
+    ]
+    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}))
+
+    values = {
+      "COUNTER_PEDAL": self.idx & 0xF,
+      "INTERCEPTOR_GAS": simulator_state.user_gas * 2**12,
+      "INTERCEPTOR_GAS2": simulator_state.user_gas * 2**12,
+    }
+    checksum = crc8_pedal(self.packer.make_can_msg("GAS_SENSOR", 0, values)[2][:-1])
+    values["CHECKSUM_PEDAL"] = checksum
+    msg.append(self.packer.make_can_msg("GAS_SENSOR", 0, values))
+
+    msg.append(self.packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8}))
+    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_PARAMS", 0, {}))
+    msg.append(self.packer.make_can_msg("CRUISE", 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("HUD_SETTING", 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, {}))
+    msg.append(self.packer.make_can_msg("BRAKE_COMMAND", 2, {}))
+
+    # *** radar bus ***
+    if self.idx % 5 == 0:
+      msg.append(self.rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79}))
+      for i in range(16):
+        msg.append(self.rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5}))
+
+    self.pm.send('can', can_list_to_can_capnp(msg))
+
+  def send_panda_state(self, simulator_state):
+    self.sm.update(0)
+    dat = messaging.new_message('pandaStates', 1)
+    dat.valid = True
+    dat.pandaStates[0] = {
+      'ignitionLine': simulator_state.ignition,
+      'pandaType': "blackPanda",
+      'controlsAllowed': True,
+      'safetyModel': 'hondaNidec',
+      'alternativeExperience': self.sm["carParams"].alternativeExperience
+    }
+    self.pm.send('pandaStates', dat)
+
+  def update(self, simulator_state: SimulatorState):
+    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

tools/sim/lib/simulated_sensors.py

@@ -0,0 +1,125 @@
+import time
+
+from cereal import log
+import cereal.messaging as messaging
+
+from openpilot.common.params import Params
+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')
+      dat.accelerometer.sensor = 4
+      dat.accelerometer.type = 0x10
+      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)
+
+      # copied these numbers from locationd
+      dat = messaging.new_message('gyroscope')
+      dat.gyroscope.sensor = 5
+      dat.gyroscope.type = 0x10
+      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 vel from carla to NED
+    # north is -Y in CARLA
+    velNED = [
+      -simulator_state.velocity.y,  # north/south component of NED is negative when moving south
+      simulator_state.velocity.x,  # positive when moving east, which is x in carla
+      simulator_state.velocity.z,
+    ]
+
+    for _ in range(10):
+      dat = messaging.new_message('gpsLocationExternal')
+      dat.gpsLocationExternal = {
+        "unixTimestampMillis": int(time.time() * 1000),
+        "flags": 1,  # valid fix
+        "accuracy": 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
+    }
+    Params().put_bool("ObdMultiplexingEnabled", False)
+    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')
+    dat.driverMonitoringState = {
+      "faceDetected": True,
+      "isDistracted": False,
+      "awarenessStatus": 1.,
+    }
+    self.pm.send('driverMonitoringState', dat)
+
+  def send_camera_images(self, world: 'World'):
+    with world.image_lock:
+      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.time()
+    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/rgb_to_nv12.cl

@@ -0,0 +1,119 @@
+#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
+#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
+#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
+#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)
+
+inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) {
+  uchar2 yy = (uchar2)(
+    RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0),
+    RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3)
+  );
+#ifdef CL_DEBUG
+  if(yi >= RGB_SIZE)
+    printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE);
+#endif
+  vstore2(yy, 0, out_yuv + yi);
+}
+
+inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) {
+  const uchar4 yy = (uchar4)(
+    RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0),
+    RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3),
+    RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6),
+    RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1)
+  );
+#ifdef CL_DEBUG
+  if(yi > RGB_SIZE - 4)
+    printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4);
+#endif
+  vstore4(yy, 0, out_yuv + yi);
+}
+
+inline void convert_uv(__global uchar * out_yuv, int uvi,
+                    const uchar8 rgbs1, const uchar8 rgbs2) {
+  // U & V: average of 2x2 pixels square
+  const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3);
+  const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4);
+  const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5);
+#ifdef CL_DEBUG
+  if(uvi >= RGB_SIZE  + RGB_SIZE / 2)
+    printf("UV overflow, %d >= %d\n", uvi, RGB_SIZE  + RGB_SIZE / 2);
+#endif
+  out_yuv[uvi] = RGB_TO_U(ar, ag, ab);
+  out_yuv[uvi+1] = RGB_TO_V(ar, ag, ab);
+}
+
+inline void convert_2_uvs(__global uchar * out_yuv, int uvi,
+                    const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) {
+  // U & V: average of 2x2 pixels square
+  const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3);
+  const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4);
+  const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5);
+  const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1);
+  const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2);
+  const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3);
+  uchar4 uv = (uchar4)(
+    RGB_TO_U(ar1, ag1, ab1),
+    RGB_TO_V(ar1, ag1, ab1),
+    RGB_TO_U(ar2, ag2, ab2),
+    RGB_TO_V(ar2, ag2, ab2)
+  );
+#ifdef CL_DEBUG1
+  if(uvi > RGB_SIZE  + RGB_SIZE / 2 - 4)
+    printf("UV2 overflow, %d >= %d\n", uvi, RGB_SIZE  + RGB_SIZE / 2 - 2);
+#endif
+  vstore4(uv, 0, out_yuv + uvi);
+}
+
+__kernel void rgb_to_nv12(__global uchar const * const rgb,
+                    __global uchar * out_yuv)
+{
+  const int dx = get_global_id(0);
+  const int dy = get_global_id(1);
+  const int col = mul24(dx, 4); // Current column in rgb image
+  const int row = mul24(dy, 4); // Current row in rgb image
+  const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted
+  const int yi_start = mad24(row,  WIDTH, col); // Start offset in the target yuv buffer
+  int uvi = mad24(row / 2, WIDTH, RGB_SIZE + col);
+  int num_col = min(WIDTH - col, 4);
+  int num_row = min(HEIGHT - row, 4);
+  if(num_row == 4) {
+    const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start);
+    const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8);
+    const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE);
+    const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8);
+    const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2);
+    const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8);
+    const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3);
+    const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8);
+    if(num_col == 4) {
+      convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1);
+      convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1);
+      convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1);
+      convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1);
+      convert_2_uvs(out_yuv, uvi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1);
+      convert_2_uvs(out_yuv, uvi + WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1);
+    } else if(num_col == 2) {
+      convert_2_ys(out_yuv, yi_start, rgbs0_0);
+      convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0);
+      convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0);
+      convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0);
+      convert_uv(out_yuv, uvi, rgbs0_0, rgbs1_0);
+      convert_uv(out_yuv, uvi + WIDTH, rgbs2_0, rgbs3_0);
+    }
+  } else {
+    const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start);
+    const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8);
+    const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE);
+    const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8);
+    if(num_col == 4) {
+      convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1);
+      convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1);
+      convert_2_uvs(out_yuv, uvi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1);
+    } else if(num_col == 2) {
+      convert_2_ys(out_yuv, yi_start, rgbs0_0);
+      convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0);
+      convert_uv(out_yuv, uvi, rgbs0_0, rgbs1_0);
+    }
+  }
+}

tools/sim/run_bridge.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+import argparse
+import os
+
+from typing import Any
+from multiprocessing import Queue
+
+from openpilot.tools.sim.bridge.common import SimulatorBridge
+from openpilot.tools.sim.bridge.carla.carla_bridge import CarlaBridge
+from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge
+
+
+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')
+  parser.add_argument('--simulator', dest='simulator', type=str, default='metadrive')
+
+  # Carla specific
+  parser.add_argument('--town', type=str, default='Town04_Opt')
+  parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16)
+  parser.add_argument('--host', dest='host', type=str, default=os.environ.get("CARLA_HOST", '127.0.0.1'))
+  parser.add_argument('--port', dest='port', type=int, default=2000)
+
+  return parser.parse_args(add_args)
+
+if __name__ == "__main__":
+  q: Any = Queue()
+  args = parse_args()
+
+  simulator_bridge: SimulatorBridge
+  if args.simulator == "carla":
+    simulator_bridge = CarlaBridge(args)
+  elif args.simulator == "metadrive":
+    simulator_bridge = MetaDriveBridge(args)
+  else:
+    raise AssertionError("simulator type not supported")
+  p = simulator_bridge.run(q)
+
+  if args.joystick:
+    # start input poll for joystick
+    from openpilot.tools.sim.lib.manual_ctrl import wheel_poll_thread
+
+    wheel_poll_thread(q)
+  else:
+    # start input poll for keyboard
+    from openpilot.tools.sim.lib.keyboard_ctrl import keyboard_poll_thread
+
+    keyboard_poll_thread(q)
+
+  simulator_bridge.shutdown()
+
+  p.join()

tools/sim/start_carla.sh

@@ -0,0 +1,34 @@
+#!/bin/bash
+
+# Requires nvidia docker - https://github.com/NVIDIA/nvidia-docker
+if ! $(apt list --installed | grep -q nvidia-container-toolkit); then
+  read -p "Nvidia docker is required. Do you want to install it now? (y/n)";
+  if [ "${REPLY}" == "y" ]; then
+    distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
+    echo $distribution
+    curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add -
+    curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
+    sudo apt-get update && sudo apt-get install -y nvidia-docker2 # Also installs docker-ce and nvidia-container-toolkit
+    sudo systemctl restart docker
+  else
+    exit 0
+  fi
+fi
+
+docker pull carlasim/carla:0.9.14
+
+EXTRA_ARGS="-it"
+if [[ "$DETACH" ]]; then
+  EXTRA_ARGS="-d"
+fi
+
+docker kill carla_sim || true
+docker run \
+  --name carla_sim \
+  --rm \
+  --gpus all \
+  --net=host \
+  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
+  $EXTRA_ARGS \
+  carlasim/carla:0.9.14 \
+  /bin/bash ./CarlaUE4.sh -opengl -nosound -RenderOffScreen -benchmark -fps=20 -quality-level=Low

tools/sim/start_openpilot_docker.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
+cd $DIR
+
+OPENPILOT_DIR="/tmp/openpilot"
+if ! [[ -z "$MOUNT_OPENPILOT" ]]; then
+  OPENPILOT_DIR="$(dirname $(dirname $DIR))"
+  EXTRA_ARGS="-v $OPENPILOT_DIR:$OPENPILOT_DIR -e PYTHONPATH=$OPENPILOT_DIR:$PYTHONPATH"
+fi
+
+if [[ "$CI" ]]; then
+  CMD="CI=1 ${OPENPILOT_DIR}/tools/sim/tests/test_carla_integration.py"
+else
+  # expose X to the container
+  xhost +local:root
+
+  docker pull ghcr.io/commaai/openpilot-sim:latest
+  CMD="./tmux_script.sh $*"
+  EXTRA_ARGS="${EXTRA_ARGS} -it"
+fi
+
+docker kill openpilot_client || true
+docker run --net=host\
+  --name openpilot_client \
+  --rm \
+  --gpus all \
+  --device=/dev/dri:/dev/dri \
+  --device=/dev/input:/dev/input \
+  -v /tmp/.X11-unix:/tmp/.X11-unix \
+  --shm-size 1G \
+  -e DISPLAY=$DISPLAY \
+  -e QT_X11_NO_MITSHM=1 \
+  -w "$OPENPILOT_DIR/tools/sim" \
+  $EXTRA_ARGS \
+  ghcr.io/commaai/openpilot-sim:latest \
+  /bin/bash -c "$CMD"

tools/sim/tests/test_carla_bridge.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+import subprocess
+import time
+import unittest
+from subprocess import Popen
+
+from openpilot.selfdrive.manager.helpers import unblock_stdout
+from openpilot.tools.sim.run_bridge import parse_args
+from openpilot.tools.sim.bridge.carla.carla_bridge import CarlaBridge
+from openpilot.tools.sim.tests.test_sim_bridge import SIM_DIR, TestSimBridgeBase
+
+from typing import Optional
+
+class TestCarlaBridge(TestSimBridgeBase):
+  """
+  Tests need Carla simulator to run
+  """
+  carla_process: Optional[Popen] = None
+
+  def setUp(self):
+    super().setUp()
+
+    # We want to make sure that carla_sim docker isn't still running.
+    subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False)
+    self.carla_process = subprocess.Popen("./start_carla.sh", cwd=SIM_DIR)
+
+    # Too many lagging messages in bridge.py can cause a crash. This prevents it.
+    unblock_stdout()
+    # Wait 10 seconds to startup carla
+    time.sleep(10)
+
+  def create_bridge(self):
+    return CarlaBridge(parse_args([]))
+
+  def tearDown(self):
+    super().tearDown()
+
+    # Stop carla simulator by removing docker container
+    subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False)
+    if self.carla_process is not None:
+      self.carla_process.wait()
+
+
+if __name__ == "__main__":
+  unittest.main()

tools/sim/tests/test_metadrive_bridge.py

@@ -0,0 +1,15 @@
+#!/usr/bin/env python3
+import unittest
+
+from openpilot.tools.sim.run_bridge import parse_args
+from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge
+from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase
+
+
+class TestMetaDriveBridge(TestSimBridgeBase):
+  def create_bridge(self):
+    return MetaDriveBridge(parse_args([]))
+
+
+if __name__ == "__main__":
+  unittest.main()

tools/sim/tests/test_sim_bridge.py

@@ -0,0 +1,88 @@
+import os
+import subprocess
+import time
+import unittest
+
+from multiprocessing import Queue
+
+from cereal import messaging
+from openpilot.common.basedir import BASEDIR
+
+SIM_DIR = os.path.join(BASEDIR, "tools/sim")
+
+class TestSimBridgeBase(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    if cls is TestSimBridgeBase:
+      raise unittest.SkipTest("Don't run this base class, run test_carla_bridge.py instead")
+
+  def setUp(self):
+    self.processes = []
+
+  def test_engage(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(['controlsState', 'onroadEvents', 'managerState'])
+    q = Queue()
+    carla_bridge = self.create_bridge()
+    p_bridge = carla_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 carla_bridge.started and time.monotonic() < start_waiting + max_time_per_step:
+      time.sleep(0.1)
+    self.assertEqual(p_bridge.exitcode, 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
+
+    self.assertTrue(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()
+
+      q.put("cruise_down")  # Try engaging
+
+      if sm.all_alive() and sm['controlsState'].active:
+        control_active += 1
+
+        if control_active == min_counts_control_active:
+          break
+
+    self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}")
+
+  def tearDown(self):
+    print("Test shutting down. CommIssues are acceptable")
+    for p in reversed(self.processes):
+      p.terminate()
+
+    for p in reversed(self.processes):
+      if isinstance(p, subprocess.Popen):
+        p.wait(15)
+      else:
+        p.join(15)
+
+
+if __name__ == "__main__":
+  unittest.main()

tools/sim/tmux_script.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+tmux new -d -s carla-sim
+tmux send-keys "./launch_openpilot.sh" ENTER
+tmux neww
+tmux send-keys "./run_bridge.py $*" ENTER
+tmux a -t carla-sim