CI/Simulator: Drive a loop in metadrive

.github/workflows/tools_tests.yaml

@@ -76,7 +76,7 @@ jobs:
         ${{ env.RUN }} "export MAPBOX_TOKEN='pk.eyJ1Ijoiam5ld2IiLCJhIjoiY2xxNW8zZXprMGw1ZzJwbzZneHd2NHljbSJ9.gV7VPRfbXFetD-1OVF0XZg' && \
                         source selfdrive/test/setup_xvfb.sh && \
                         source selfdrive/test/setup_vsound.sh && \
-                        CI=1 pytest tools/sim/tests/test_metadrive_bridge.py -W ignore::pyopencl.CompilerWarning"
+                        CI=1 pytest tools/sim/tests/test_metadrive_bridge.py --time_done=90 -W ignore::pyopencl.CompilerWarning"
 
   devcontainer:
     name: devcontainer

tools/sim/bridge/metadrive/metadrive_bridge.py

@@ -1,3 +1,4 @@
+import math
 from multiprocessing import Queue
 
 from metadrive.component.sensors.base_camera import _cuda_enable
@@ -27,29 +28,32 @@ def curve_block(length, angle=45, direction=0):
   }
 
 def create_map(track_size=60):
+  curve_len = track_size * 2
   return dict(
     type=MapGenerateMethod.PG_MAP_FILE,
     lane_num=2,
-    lane_width=3.5,
+    lane_width=4.5,
     config=[
       None,
       straight_block(track_size),
-      curve_block(track_size*2, 90),
+      curve_block(curve_len, 90),
       straight_block(track_size),
-      curve_block(track_size*2, 90),
+      curve_block(curve_len, 90),
       straight_block(track_size),
-      curve_block(track_size*2, 90),
+      curve_block(curve_len, 90),
       straight_block(track_size),
-      curve_block(track_size*2, 90),
+      curve_block(curve_len, 90),
     ]
   )
 
 
 class MetaDriveBridge(SimulatorBridge):
   TICKS_PER_FRAME = 5
 
-  def __init__(self, dual_camera, high_quality):
+  def __init__(self, dual_camera, high_quality, time_done=math.inf, test_run=False):
     self.should_render = False
+    self.test_run = test_run
+    self.time_done = time_done if self.test_run else math.inf
 
     super().__init__(dual_camera, high_quality)
 
@@ -83,4 +87,4 @@ def spawn_world(self, queue: Queue):
       preload_models=False
     )
 
-    return MetaDriveWorld(queue, config, self.dual_camera)
+    return MetaDriveWorld(queue, config, self.time_done, self.test_run, self.dual_camera)

tools/sim/bridge/metadrive/metadrive_process.py

@@ -1,4 +1,5 @@
 import math
+import time
 import numpy as np
 
 from collections import namedtuple
@@ -49,7 +50,7 @@ def arrive_destination_patch(self, *args, **kwargs):
 
 def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock,
                       controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection,
-                      exit_event):
+                      exit_event, start_time, time_done, test_run):
   arrive_dest_done = config.pop("arrive_dest_done", True)
   apply_metadrive_patches(arrive_dest_done)
 
@@ -111,10 +112,18 @@ def get_cam_as_rgb(cam):
         reset()
 
     if rk.frame % 5 == 0:
-      obs, _, terminated, _, info = env.step(vc)
+      _, _, terminated, _, _ = env.step(vc)
+      time_out = True if time.monotonic() - start_time >= time_done else False
+      out_of_lane = env.vehicle.on_broken_line or env.vehicle.on_yellow_continuous_line or env.vehicle.on_white_continuous_line or env.vehicle.crash_sidewalk
+
+      if terminated or ((out_of_lane or time_out) and test_run):
+        if terminated:
+          done_result = env.done_function("default_agent")
+        elif out_of_lane:
+          done_result = (True, {"out_of_lane" : True})
+        elif time_out:
+          done_result = (True, {"time_done" : True})
 
-      if terminated:
-        done_result = env.done_function("default_agent")
         simulation_state = metadrive_simulation_state(
           running=False,
           done=done_result[0],

tools/sim/bridge/metadrive/metadrive_world.py

@@ -14,7 +14,7 @@
 
 
 class MetaDriveWorld(World):
-  def __init__(self, status_q, config, dual_camera = False):
+  def __init__(self, status_q, config, time_done, test_run, dual_camera=False):
     super().__init__(dual_camera)
     self.status_q = status_q
     self.camera_array = Array(ctypes.c_uint8, W*H*3)
@@ -30,11 +30,12 @@ def __init__(self, status_q, config, dual_camera = False):
 
     self.exit_event = multiprocessing.Event()
 
+    start_time = time.monotonic()
     self.metadrive_process = multiprocessing.Process(name="metadrive process", target=
                               functools.partial(metadrive_process, dual_camera, config,
                                                 self.camera_array, self.wide_camera_array, self.image_lock,
                                                 self.controls_recv, self.simulation_state_send,
-                                                self.vehicle_state_send, self.exit_event))
+                                                self.vehicle_state_send, self.exit_event, start_time, time_done, test_run))
 
     self.metadrive_process.start()
     self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting"))

tools/sim/tests/conftest.py

@@ -0,0 +1,8 @@
+import pytest
+
+def pytest_addoption(parser):
+  parser.addoption("--time_done", action="store", default=60, type=int, help="Seconds to run metadrive drive")
+
+@pytest.fixture
+def time_done(request):
+  return request.config.getoption("--time_done")

tools/sim/tests/test_metadrive_bridge.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import pytest
 import warnings
+
 # Since metadrive depends on pkg_resources, and pkg_resources is deprecated as an API
 warnings.filterwarnings("ignore", category=DeprecationWarning)
 
@@ -9,5 +10,9 @@
 
 @pytest.mark.slow
 class TestMetaDriveBridge(TestSimBridgeBase):
+  @pytest.fixture(autouse=True)
+  def setup_class(self, time_done):
+    self.time_done = time_done
+
   def create_bridge(self):
-    return MetaDriveBridge(False, False)
+    return MetaDriveBridge(False, False, self.time_done, True)

tools/sim/tests/test_sim_bridge.py

@@ -7,6 +7,7 @@
 
 from cereal import messaging
 from openpilot.common.basedir import BASEDIR
+from openpilot.tools.sim.bridge.common import QueueMessageType
 
 SIM_DIR = os.path.join(BASEDIR, "tools/sim")
 
@@ -19,7 +20,7 @@ def setup_class(cls):
   def setup_method(self):
     self.processes = []
 
-  def test_engage(self):
+  def test_driving(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)
@@ -70,6 +71,18 @@ def test_engage(self):
 
     assert min_counts_control_active == control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}"
 
+    failure_states = []
+    while bridge.started.value:
+      continue
+
+    while not q.empty():
+      state = q.get()
+      if state.type == QueueMessageType.TERMINATION_INFO:
+        done_info = state.info
+        failure_states = [done_state for done_state in done_info if done_state != "time_done" and done_info[done_state]]
+        break
+    assert len(failure_states) == 0, f"Simulator fails to finish a loop. Failure states: {failure_states}"
+
   def teardown_method(self):
     print("Test shutting down. CommIssues are acceptable")
     for p in reversed(self.processes):