Add openpilot tools

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()