Simulator Tests (#24274)

* squash #24009

* Fix from other pr
Add low-quality arg

* Update tools/sim/test/test_carla_integration.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Updates for comments. Not finished yet

* commit

* fix arguments

* Final changes for comments

* Final fixes

* increase carla client timeout to 10

* make test executable

* actually wait for controlsd to send messages

* Error proof test. Starting up carla at each test and closing down using docker (tried many things).

* commit test carla

* Removed some time.sleeps
Add some more retries for bridge.

* Stop while loop on shutdown

* Increase teardown waiting time

Co-authored-by: Willem Melching <willem.melching@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

tools/sim/bridge.py

@@ -1,9 +1,10 @@
 #!/usr/bin/env python3
 import argparse
 import math
+import os
+import signal
 import threading
 import time
-import os
 from multiprocessing import Process, Queue
 from typing import Any
 
@@ -12,8 +13,6 @@ import numpy as np
 import pyopencl as cl
 import pyopencl.array as cl_array
 
-from lib.can import can_function
-
 import cereal.messaging as messaging
 from cereal import log
 from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
@@ -22,15 +21,9 @@ from common.numpy_fast import clip
 from common.params import Params
 from common.realtime import DT_DMON, Ratekeeper
 from selfdrive.car.honda.values import CruiseButtons
+from selfdrive.manager.helpers import unblock_stdout
 from selfdrive.test.helpers import set_params_enabled
-
-parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.')
-parser.add_argument('--joystick', action='store_true')
-parser.add_argument('--low_quality', action='store_true')
-parser.add_argument('--town', type=str, default='Town04_Opt')
-parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16)
-
-args = parser.parse_args()
+from tools.sim.lib.can import can_function
 
 W, H = 1928, 1208
 REPEAT_COUNTER = 5
@@ -41,6 +34,16 @@ pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'sensorEvent
 sm = messaging.SubMaster(['carControl', 'controlsState'])
 
 
+def parse_args(add_args=None):
+  parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.')
+  parser.add_argument('--joystick', action='store_true')
+  parser.add_argument('--low_quality', action='store_true')
+  parser.add_argument('--town', type=str, default='Town04_Opt')
+  parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16)
+
+  return parser.parse_args(add_args)
+
+
 class VehicleState:
   def __init__(self):
     self.speed = 0
@@ -80,11 +83,13 @@ class Camerad:
     # 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 "
+    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 "
 
     # TODO: move rgb_to_yuv.cl to local dir once the frame stream camera is removed
     kernel_fn = os.path.join(BASEDIR, "selfdrive", "camerad", "transforms", "rgb_to_yuv.cl")
-    prg = cl.Program(self.ctx, open(kernel_fn).read()).build(cl_arg)
+    self._kernel_file = open(kernel_fn)
+
+    prg = cl.Program(self.ctx, self._kernel_file.read()).build(cl_arg)
     self.krnl = prg.rgb_to_yuv
     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
@@ -126,6 +131,9 @@ class Camerad:
     setattr(dat, pub_type, msg)
     pm.send(pub_type, dat)
 
+  def close(self):
+    self._kernel_file.close()
+
 
 def imu_callback(imu, vehicle_state):
   vehicle_state.bearing_deg = math.degrees(imu.compass)
@@ -231,265 +239,300 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event):
     i += 1
 
 
-def bridge(q):
-  # setup CARLA
+def connect_carla_client():
   client = carla.Client("127.0.0.1", 2000)
-  client.set_timeout(10.0)
-  world = client.load_world(args.town)
-
-  settings = world.get_settings()
-  settings.synchronous_mode = True # Enables synchronous mode
-  settings.fixed_delta_seconds = 0.05
-  world.apply_settings(settings)
-
-  world.set_weather(carla.WeatherParameters.ClearSunset)
-
-  if args.low_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]
-  spawn_points = world_map.get_spawn_points()
-  assert len(spawn_points) > args.num_selected_spawn_point, \
-    f'''No spawn point {args.num_selected_spawn_point}, try a value between 0 and
-    {len(spawn_points)} for this town.'''
-  spawn_point = spawn_points[args.num_selected_spawn_point]
-  vehicle = world.spawn_actor(vehicle_bp, spawn_point)
-
-  max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle
-
-  # make tires less slippery
-  # wheel_control = carla.WheelPhysicsControl(tire_friction=5)
-  physics_control = vehicle.get_physics_control()
-  physics_control.mass = 2326
-  # physics_control.wheels = [wheel_control]*4
-  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)
-
-  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', '0.05')
-    camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
-    camera.listen(callback)
-    return camera
-
-  camerad = Camerad()
-  road_camera = create_camera(fov=40, callback=camerad.cam_callback_road)
-  road_wide_camera = create_camera(fov=163, callback=camerad.cam_callback_wide_road)  # fov bigger than 163 shows unwanted artifacts
-
-  cameras = [road_camera, road_wide_camera]
-
-  vehicle_state = VehicleState()
-
-  # reenable IMU
-  imu_bp = blueprint_library.find('sensor.other.imu')
-  imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
-  imu.listen(lambda imu: imu_callback(imu, vehicle_state))
-
-  gps_bp = blueprint_library.find('sensor.other.gnss')
-  gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle)
-  gps.listen(lambda gps: gps_callback(gps, vehicle_state))
-
-  # launch fake car threads
-  threads = []
-  exit_event = threading.Event()
-  threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, exit_event,)))
-  threads.append(threading.Thread(target=peripheral_state_function, args=(exit_event,)))
-  threads.append(threading.Thread(target=fake_driver_monitoring, args=(exit_event,)))
-  threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, exit_event,)))
-  for t in threads:
-    t.start()
-
-  # can loop
-  rk = Ratekeeper(100, print_delay_threshold=0.05)
-
-  # init
-  throttle_ease_out_counter = REPEAT_COUNTER
-  brake_ease_out_counter = REPEAT_COUNTER
-  steer_ease_out_counter = REPEAT_COUNTER
-
-  vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
-
-  is_openpilot_engaged = False
-  throttle_out = steer_out = brake_out = 0
-  throttle_op = steer_op = brake_op = 0
-  throttle_manual = steer_manual = brake_manual = 0
-
-  old_steer = old_brake = old_throttle = 0
-  throttle_manual_multiplier = 0.7  # keyboard signal is always 1
-  brake_manual_multiplier = 0.7  # keyboard signal is always 1
-  steer_manual_multiplier = 45 * STEER_RATIO  # keyboard signal is always 1
-
-  while True:
-    # 1. Read the throttle, steer and brake from op or manual controls
-    # 2. Set instructions in Carla
-    # 3. Send current carstate to op via can
-
-    cruise_button = 0
-    throttle_out = steer_out = brake_out = 0.0
-    throttle_op = steer_op = brake_op = 0
-    throttle_manual = steer_manual = brake_manual = 0.0
-
-    # --------------Step 1-------------------------------
-    if not q.empty():
-      message = q.get()
-      m = message.split('_')
-      if m[0] == "steer":
-        steer_manual = float(m[1])
-        is_openpilot_engaged = False
-      elif m[0] == "throttle":
-        throttle_manual = float(m[1])
-        is_openpilot_engaged = False
-      elif m[0] == "brake":
-        brake_manual = float(m[1])
-        is_openpilot_engaged = False
-      elif m[0] == "reverse":
-        cruise_button = CruiseButtons.CANCEL
-        is_openpilot_engaged = False
-      elif m[0] == "cruise":
-        if m[1] == "down":
-          cruise_button = CruiseButtons.DECEL_SET
-          is_openpilot_engaged = True
-        elif m[1] == "up":
-          cruise_button = CruiseButtons.RES_ACCEL
-          is_openpilot_engaged = True
-        elif m[1] == "cancel":
-          cruise_button = CruiseButtons.CANCEL
-          is_openpilot_engaged = False
-      elif m[0] == "ignition":
-        vehicle_state.ignition = not vehicle_state.ignition
-      elif m[0] == "quit":
-        break
+  client.set_timeout(10)
+  return client
 
-      throttle_out = throttle_manual * throttle_manual_multiplier
-      steer_out = steer_manual * steer_manual_multiplier
-      brake_out = brake_manual * brake_manual_multiplier
-
-      old_steer = steer_out
-      old_throttle = throttle_out
-      old_brake = brake_out
-
-    if is_openpilot_engaged:
-      sm.update(0)
-
-      # TODO gas and brake is deprecated
-      throttle_op = clip(sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
-      brake_op = clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
-      steer_op = sm['carControl'].actuators.steeringAngleDeg
-
-      throttle_out = throttle_op
-      steer_out = steer_op
-      brake_out = brake_op
-
-      steer_out = steer_rate_limit(old_steer, steer_out)
-      old_steer = steer_out
-
-    else:
-      if throttle_out == 0 and old_throttle > 0:
-        if throttle_ease_out_counter > 0:
-          throttle_out = old_throttle
-          throttle_ease_out_counter += -1
-        else:
-          throttle_ease_out_counter = REPEAT_COUNTER
-          old_throttle = 0
-
-      if brake_out == 0 and old_brake > 0:
-        if brake_ease_out_counter > 0:
-          brake_out = old_brake
-          brake_ease_out_counter += -1
-        else:
-          brake_ease_out_counter = REPEAT_COUNTER
-          old_brake = 0
-
-      if steer_out == 0 and old_steer != 0:
-        if steer_ease_out_counter > 0:
-          steer_out = old_steer
-          steer_ease_out_counter += -1
-        else:
-          steer_ease_out_counter = REPEAT_COUNTER
-          old_steer = 0
-
-    # --------------Step 2-------------------------------
-    steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1)
-
-    steer_carla = np.clip(steer_carla, -1, 1)
-    steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1)
-    old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1)
-
-    vc.throttle = throttle_out / 0.6
-    vc.steer = steer_carla
-    vc.brake = brake_out
-    vehicle.apply_control(vc)
-
-    # --------------Step 3-------------------------------
-    vel = vehicle.get_velocity()
-    speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)  # in m/s
-    vehicle_state.speed = speed
-    vehicle_state.vel = vel
-    vehicle_state.angle = steer_out
-    vehicle_state.cruise_button = cruise_button
-    vehicle_state.is_engaged = is_openpilot_engaged
-
-    if rk.frame % PRINT_DECIMATION == 0:
-      print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(vc.throttle, 3), "; steer(c/deg): ", round(vc.steer, 3), round(steer_out, 3), "; brake: ", round(vc.brake, 3))
-
-    if rk.frame % 5 == 0:
-      world.tick()
-
-    rk.keep_time()
-
-  # Clean up resources in the opposite order they were created.
-  exit_event.set()
-  for t in reversed(threads):
-    t.join()
-  gps.destroy()
-  imu.destroy()
-  for c in cameras:
-    c.destroy()
-  vehicle.destroy()
-
-
-def bridge_keep_alive(q: Any):
-  while 1:
-    try:
-      bridge(q)
-      break
-    except RuntimeError as e:
-      print("Restarting bridge. Error:", e)
 
+class CarlaBridge:
 
-if __name__ == "__main__":
-  # make sure params are in a good state
-  set_params_enabled()
+  def __init__(self, args):
+    set_params_enabled()
 
-  msg = messaging.new_message('liveCalibration')
-  msg.liveCalibration.validBlocks = 20
-  msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
-  Params().put("CalibrationParams", msg.to_bytes())
+    msg = messaging.new_message('liveCalibration')
+    msg.liveCalibration.validBlocks = 20
+    msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
+    Params().put("CalibrationParams", msg.to_bytes())
 
+    self._args = args
+    self._carla_objects = []
+    self._camerad = None
+    self._threads_exit_event = threading.Event()
+    self._threads = []
+    self._shutdown = False
+    self.started = False
+    signal.signal(signal.SIGTERM, self._on_shutdown)
+
+  def _on_shutdown(self, signal, frame):
+    self._shutdown = True
+
+  def bridge_keep_alive(self, q: Queue, retries: int):
+    while not self._shutdown:
+      try:
+        self._run(q)
+        break
+      except RuntimeError as e:
+        if retries == 0:
+          raise
+        retries -= 1
+        print(f"Restarting bridge. Retries left {retries}. Error: {e} ")
+
+  def _run(self, q: Queue):
+    client = connect_carla_client()
+    world = client.load_world(self._args.town)
+    settings = world.get_settings()
+    settings.synchronous_mode = True  # Enables synchronous mode
+    settings.fixed_delta_seconds = 0.05
+    world.apply_settings(settings)
+
+    world.set_weather(carla.WeatherParameters.ClearSunset)
+
+    if self._args.low_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]
+    spawn_points = world_map.get_spawn_points()
+    assert len(spawn_points) > self._args.num_selected_spawn_point, f'''No spawn point {self._args.num_selected_spawn_point}, try a value between 0 and
+      {len(spawn_points)} for this town.'''
+    spawn_point = spawn_points[self._args.num_selected_spawn_point]
+    vehicle = world.spawn_actor(vehicle_bp, spawn_point)
+    self._carla_objects.append(vehicle)
+    max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle
+
+    # make tires less slippery
+    # wheel_control = carla.WheelPhysicsControl(tire_friction=5)
+    physics_control = vehicle.get_physics_control()
+    physics_control.mass = 2326
+    # physics_control.wheels = [wheel_control]*4
+    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)
+
+    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))
+      if self._args.low_quality:
+        blueprint.set_attribute('enable_postprocess_effects', 'False')
+      camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
+      camera.listen(callback)
+      return camera
+
+    self._camerad = Camerad()
+    road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road)
+    road_wide_camera = create_camera(fov=163, callback=self._camerad.cam_callback_wide_road)  # fov bigger than 163 shows unwanted artifacts
+
+    self._carla_objects.extend([road_camera, road_wide_camera])
+
+    vehicle_state = VehicleState()
+
+    # reenable IMU
+    imu_bp = blueprint_library.find('sensor.other.imu')
+    imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
+    imu.listen(lambda imu: imu_callback(imu, vehicle_state))
+
+    gps_bp = blueprint_library.find('sensor.other.gnss')
+    gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle)
+    gps.listen(lambda gps: gps_callback(gps, vehicle_state))
+
+    self._carla_objects.extend([imu, gps])
+    # launch fake car threads
+    self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._threads_exit_event,)))
+    self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._threads_exit_event,)))
+    self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._threads_exit_event,)))
+    self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._threads_exit_event,)))
+    for t in self._threads:
+      t.start()
+
+    # can loop
+    rk = Ratekeeper(100, print_delay_threshold=0.05)
+
+    # init
+    throttle_ease_out_counter = REPEAT_COUNTER
+    brake_ease_out_counter = REPEAT_COUNTER
+    steer_ease_out_counter = REPEAT_COUNTER
+
+    vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
+
+    is_openpilot_engaged = False
+    throttle_out = steer_out = brake_out = 0.
+    throttle_op = steer_op = brake_op = 0.
+    throttle_manual = steer_manual = brake_manual = 0.
+
+    old_steer = old_brake = old_throttle = 0.
+    throttle_manual_multiplier = 0.7  # keyboard signal is always 1
+    brake_manual_multiplier = 0.7  # keyboard signal is always 1
+    steer_manual_multiplier = 45 * STEER_RATIO  # keyboard signal is always 1
+
+    while not self._shutdown:
+      # 1. Read the throttle, steer and brake from op or manual controls
+      # 2. Set instructions in Carla
+      # 3. Send current carstate to op via can
+
+      cruise_button = 0
+      throttle_out = steer_out = brake_out = 0.0
+      throttle_op = steer_op = brake_op = 0
+      throttle_manual = steer_manual = brake_manual = 0.0
+
+      # --------------Step 1-------------------------------
+      if not q.empty():
+        message = q.get()
+        m = message.split('_')
+        if m[0] == "steer":
+          steer_manual = float(m[1])
+          is_openpilot_engaged = False
+        elif m[0] == "throttle":
+          throttle_manual = float(m[1])
+          is_openpilot_engaged = False
+        elif m[0] == "brake":
+          brake_manual = float(m[1])
+          is_openpilot_engaged = False
+        elif m[0] == "reverse":
+          cruise_button = CruiseButtons.CANCEL
+          is_openpilot_engaged = False
+        elif m[0] == "cruise":
+          if m[1] == "down":
+            cruise_button = CruiseButtons.DECEL_SET
+            is_openpilot_engaged = True
+          elif m[1] == "up":
+            cruise_button = CruiseButtons.RES_ACCEL
+            is_openpilot_engaged = True
+          elif m[1] == "cancel":
+            cruise_button = CruiseButtons.CANCEL
+            is_openpilot_engaged = False
+        elif m[0] == "ignition":
+          vehicle_state.ignition = not vehicle_state.ignition
+        elif m[0] == "quit":
+          break
+
+        throttle_out = throttle_manual * throttle_manual_multiplier
+        steer_out = steer_manual * steer_manual_multiplier
+        brake_out = brake_manual * brake_manual_multiplier
+
+        old_steer = steer_out
+        old_throttle = throttle_out
+        old_brake = brake_out
+
+      if is_openpilot_engaged:
+        sm.update(0)
+
+        # TODO gas and brake is deprecated
+        throttle_op = clip(sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
+        brake_op = clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
+        steer_op = sm['carControl'].actuators.steeringAngleDeg
+
+        throttle_out = throttle_op
+        steer_out = steer_op
+        brake_out = brake_op
+
+        steer_out = steer_rate_limit(old_steer, steer_out)
+        old_steer = steer_out
+
+      else:
+        if throttle_out == 0 and old_throttle > 0:
+          if throttle_ease_out_counter > 0:
+            throttle_out = old_throttle
+            throttle_ease_out_counter += -1
+          else:
+            throttle_ease_out_counter = REPEAT_COUNTER
+            old_throttle = 0
+
+        if brake_out == 0 and old_brake > 0:
+          if brake_ease_out_counter > 0:
+            brake_out = old_brake
+            brake_ease_out_counter += -1
+          else:
+            brake_ease_out_counter = REPEAT_COUNTER
+            old_brake = 0
+
+        if steer_out == 0 and old_steer != 0:
+          if steer_ease_out_counter > 0:
+            steer_out = old_steer
+            steer_ease_out_counter += -1
+          else:
+            steer_ease_out_counter = REPEAT_COUNTER
+            old_steer = 0
+
+      # --------------Step 2-------------------------------
+      steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1)
+
+      steer_carla = np.clip(steer_carla, -1, 1)
+      steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1)
+      old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1)
+
+      vc.throttle = throttle_out / 0.6
+      vc.steer = steer_carla
+      vc.brake = brake_out
+      vehicle.apply_control(vc)
+
+      # --------------Step 3-------------------------------
+      vel = vehicle.get_velocity()
+      speed = math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2)  # in m/s
+      vehicle_state.speed = speed
+      vehicle_state.vel = vel
+      vehicle_state.angle = steer_out
+      vehicle_state.cruise_button = cruise_button
+      vehicle_state.is_engaged = is_openpilot_engaged
+
+      if rk.frame % PRINT_DECIMATION == 0:
+        print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(vc.throttle, 3), "; steer(c/deg): ",
+              round(vc.steer, 3), round(steer_out, 3), "; brake: ", round(vc.brake, 3))
+
+      if rk.frame % 5 == 0:
+        world.tick()
+      rk.keep_time()
+      self.started = True
+
+    # Clean up resources in the opposite order they were created.
+    self.close()
+
+  def close(self):
+    self._threads_exit_event.set()
+    if self._camerad is not None:
+      self._camerad.close()
+    for s in self._carla_objects:
+      try:
+        s.destroy()
+      except Exception as e:
+        print("Failed to destroy carla object", e)
+    for t in reversed(self._threads):
+      t.join()
+
+  def run(self, queue, retries=-1):
+    unblock_stdout()  # Fix error when publishing too many lag message
+    bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True)
+    bridge_p.start()
+    return bridge_p
+
+
+if __name__ == "__main__":
   q: Any = Queue()
-  p = Process(target=bridge_keep_alive, args=(q,), daemon=True)
-  p.start()
+  args = parse_args()
+
+  carla_bridge = CarlaBridge(args)
+  p = carla_bridge.run(q)
 
   if args.joystick:
     # start input poll for joystick
-    from lib.manual_ctrl import wheel_poll_thread
+    from tools.sim.lib.manual_ctrl import wheel_poll_thread
+
     wheel_poll_thread(q)
   else:
     # start input poll for keyboard
-    from lib.keyboard_ctrl import keyboard_poll_thread
+    from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread
+
     keyboard_poll_thread(q)
+  p.join()

tools/sim/launch_openpilot.sh

@@ -8,4 +8,4 @@ export FINGERPRINT="HONDA CIVIC 2016"
 export BLOCK="camerad,loggerd"
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
-cd ../../selfdrive/manager && ./manager.py
+cd ../../selfdrive/manager && exec ./manager.py

tools/sim/start_carla.sh

@@ -18,6 +18,7 @@ fi
 docker pull carlasim/carla:0.9.12
 
 docker run \
+  --name carla_sim \
   --rm \
   --gpus all \
   --net=host \

tools/sim/test/test_carla_integration.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+import subprocess
+import time
+import unittest
+from multiprocessing import Queue
+
+from cereal import messaging
+from tools.sim import bridge
+from tools.sim.bridge import CarlaBridge
+
+
+class TestCarlaIntegration(unittest.TestCase):
+  """
+  Tests need Carla simulator to run
+  """
+  processes = None
+
+  def setUp(self):
+    self.processes = []
+    # We want to make sure that carla_sim docker is still running. Skip output shell
+    subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False)
+
+    self.processes.append(subprocess.Popen(".././start_carla.sh"))
+
+  def test_run_bridge(self):
+    # Test bridge connect with carla and runs without any errors for 60 seconds
+    test_duration = 60
+
+    carla_bridge = CarlaBridge(bridge.parse_args(['--low_quality']))
+    p = carla_bridge.run(Queue(), retries=3)
+    self.processes = [p]
+
+    time.sleep(test_duration)
+
+    self.assertEqual(p.exitcode, None, f"Bridge process should be running, but exited with code {p.exitcode}")
+
+  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='../')
+    self.processes.append(p_manager)
+
+    sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState'])
+    q = Queue()
+    carla_bridge = CarlaBridge(bridge.parse_args(['--low_quality']))
+    p_bridge = carla_bridge.run(q, retries=3)
+    self.processes.append(p_bridge)
+
+    max_time_per_step = 20
+
+    # 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['carEvents'] 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 sm offline, 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_up")  # 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)
+    subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False)
+
+
+
+if __name__ == "__main__":
+  unittest.main()