Simulator: cleanup in preparation for metadrive

tools/sim/bridge/carla.py

@@ -0,0 +1,163 @@
+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, SimulatorBridge
+from openpilot.tools.sim.lib.camerad import W, H
+
+
+class CarlaWorld(World):
+  def __init__(self, world, vehicle, high_quality=False, dual_camera=False):
+    super().__init__(dual_camera)
+    import carla
+    self.world = world
+    self.vc: carla.VehicleControl = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
+    self.vehicle = vehicle
+    self.max_steer_angle: float = vehicle.get_physics_control().wheels[0].max_steer_angle
+    self.params = Params()
+
+    self.steer_ratio = 15
+
+    self.carla_objects = []
+
+    blueprint_library = self.world.get_blueprint_library()
+    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=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=vehicle)
+
+    gps_bp = blueprint_library.find('sensor.other.gnss')
+    self.gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle)
+    self.params.put_bool("UbloxAvailable", True)
+
+    self.carla_objects = [self.imu, self.gps, self.road_camera, self.road_wide_camera]
+
+  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()
+
+
+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)
+
+    world = client.load_world(self.town)
+
+    settings = world.get_settings()
+    settings.fixed_delta_seconds = 0.01
+    world.apply_settings(settings)
+
+    world.set_weather(carla.WeatherParameters.ClearSunset)
+
+    if not self.high_quality:
+      world.unload_map_layer(carla.MapLayer.Foliage)
+      world.unload_map_layer(carla.MapLayer.Buildings)
+      world.unload_map_layer(carla.MapLayer.ParkedVehicles)
+      world.unload_map_layer(carla.MapLayer.Props)
+      world.unload_map_layer(carla.MapLayer.StreetLights)
+      world.unload_map_layer(carla.MapLayer.Particles)
+
+    blueprint_library = world.get_blueprint_library()
+
+    world_map = world.get_map()
+
+    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) > self.num_selected_spawn_point, \
+      f'''No spawn point {self.num_selected_spawn_point}, try a value between 0 and {len(spawn_points)} for this town.'''
+    spawn_point = spawn_points[self.num_selected_spawn_point]
+    vehicle = world.spawn_actor(vehicle_bp, spawn_point)
+
+    physics_control = 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
+    vehicle.apply_physics_control(physics_control)
+
+    return CarlaWorld(world, vehicle, dual_camera=self.dual_camera)

tools/sim/bridge/common.py

@@ -0,0 +1,163 @@
+import signal
+import threading
+import functools
+
+from multiprocessing import Process, Queue
+from abc import ABC, abstractmethod
+from typing import Optional
+
+import cereal.messaging as messaging
+
+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
+
+
+def rk_loop(function, hz, exit_event: threading.Event):
+  rk = Ratekeeper(hz)
+  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)
+
+    msg = messaging.new_message('liveCalibration')
+    msg.liveCalibration.validBlocks = 20
+    msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
+    self.params.put("CalibrationParams", msg.to_bytes())
+
+    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
+
+  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(target=self.bridge_keep_alive, args=(queue, retries), daemon=True)
+    bridge_p.start()
+    return bridge_p
+
+  @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()
+
+    rk = Ratekeeper(100, print_delay_threshold=None)
+
+    # Simulation tends to be slow in the initial steps. This prevents lagging later
+    for _ in range(20):
+      self.world.tick()
+
+    throttle_manual = steer_manual = brake_manual = 0.
+
+    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
+
+      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] == "ignition":
+          self.simulator_state.ignition = not self.simulator_state.ignition
+        elif m[0] == "quit":
+          break
+
+      self.simulator_state.user_brake = brake_manual
+      self.simulator_state.user_gas = throttle_manual
+
+      steer_manual = steer_manual * -40
+
+      # Update openpilot on current sensor state
+      self.simulated_sensors.update(self.simulator_state, self.world)
+
+      is_openpilot_engaged = self.simulated_car.sm['controlsState'].active
+
+      self.simulated_car.sm.update(0)
+
+      if is_openpilot_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
+
+      throttle_out = throttle_op if is_openpilot_engaged else throttle_manual
+      brake_out = brake_op if is_openpilot_engaged else brake_manual
+      steer_out = steer_op if is_openpilot_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()
+
+      self.started = True
+
+      rk.keep_time()