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
-
+from tools.sim.lib.can import can_function
-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()
 
 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
@@ -84,7 +87,9 @@ class Camerad:
 
     # 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,12 +239,48 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event):
     i += 1
 
 
-def bridge(q):
+def connect_carla_client():
-  # setup CARLA
   client = carla.Client("127.0.0.1", 2000)
-  client.set_timeout(10.0)
+  client.set_timeout(10)
-  world = client.load_world(args.town)
+  return client
+
 
+class CarlaBridge:
+
+  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())
+
+    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
@@ -244,7 +288,7 @@ def bridge(q):
 
     world.set_weather(carla.WeatherParameters.ClearSunset)
 
-  if args.low_quality:
+    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)
@@ -258,12 +302,11 @@ def bridge(q):
 
     vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1]
     spawn_points = world_map.get_spawn_points()
-  assert len(spawn_points) > args.num_selected_spawn_point, \
+    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
-    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]
+    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
@@ -282,16 +325,17 @@ def bridge(q):
       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')
+      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
 
-  camerad = Camerad()
+    self._camerad = Camerad()
-  road_camera = create_camera(fov=40, callback=camerad.cam_callback_road)
+    road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road)
-  road_wide_camera = create_camera(fov=163, callback=camerad.cam_callback_wide_road)  # fov bigger than 163 shows unwanted artifacts
+    road_wide_camera = create_camera(fov=163, callback=self._camerad.cam_callback_wide_road)  # fov bigger than 163 shows unwanted artifacts
 
-  cameras = [road_camera, road_wide_camera]
+    self._carla_objects.extend([road_camera, road_wide_camera])
 
     vehicle_state = VehicleState()
 
@@ -304,14 +348,13 @@ def bridge(q):
     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
-  threads = []
+    self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._threads_exit_event,)))
-  exit_event = threading.Event()
+    self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._threads_exit_event,)))
-  threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, exit_event,)))
+    self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._threads_exit_event,)))
-  threads.append(threading.Thread(target=peripheral_state_function, args=(exit_event,)))
+    self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._threads_exit_event,)))
-  threads.append(threading.Thread(target=fake_driver_monitoring, args=(exit_event,)))
+    for t in self._threads:
-  threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, exit_event,)))
-  for t in threads:
       t.start()
 
     # can loop
@@ -325,16 +368,16 @@ def bridge(q):
     vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
 
     is_openpilot_engaged = False
-  throttle_out = steer_out = brake_out = 0
+    throttle_out = steer_out = brake_out = 0.
-  throttle_op = steer_op = brake_op = 0
+    throttle_op = steer_op = brake_op = 0.
-  throttle_manual = steer_manual = brake_manual = 0
+    throttle_manual = steer_manual = brake_manual = 0.
 
-  old_steer = old_brake = old_throttle = 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:
+    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
@@ -445,51 +488,51 @@ def bridge(q):
       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))
+        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.
-  exit_event.set()
+    self.close()
-  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):
+  def close(self):
-  while 1:
+    self._threads_exit_event.set()
+    if self._camerad is not None:
+      self._camerad.close()
+    for s in self._carla_objects:
       try:
-      bridge(q)
+        s.destroy()
-      break
+      except Exception as e:
-    except RuntimeError as e:
+        print("Failed to destroy carla object", e)
-      print("Restarting bridge. Error:", 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__":
-  # make sure params are in a good state
-  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())
 
+if __name__ == "__main__":
   q: Any = Queue()
-  p = Process(target=bridge_keep_alive, args=(q,), daemon=True)
+  args = parse_args()
-  p.start()
+
+  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()