sim: fake IMU data in simulator

.github/workflows/tests.yaml

@@ -223,7 +223,7 @@ jobs:
       (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))
       && fromJSON('["namespace-profile-amd64-8x16", "namespace-experiments:docker.builds.local-cache=separate"]')
       || fromJSON('["ubuntu-24.04"]') }}
-    if: false  # FIXME: Started to timeout recently
+    if: true
     steps:
     - uses: actions/checkout@v4
       with:

tools/sim/bridge/metadrive/metadrive_world.py

@@ -3,13 +3,14 @@
 import multiprocessing
 import numpy as np
 import time
+import math
 
 from multiprocessing import Pipe, Array
 
 from openpilot.tools.sim.bridge.common import QueueMessage, QueueMessageType
 from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state,
                                                                     metadrive_vehicle_state)
-from openpilot.tools.sim.lib.common import SimulatorState, World
+from openpilot.tools.sim.lib.common import SimulatorState, World, vec3
 from openpilot.tools.sim.lib.camerad import W, H
 
 
@@ -58,6 +59,11 @@ def __init__(self, status_q, config, test_duration, test_run, dual_camera=False)
     self.reset_time = 0
     self.should_reset = False
 
+    # IMU state
+    self.prev_velocity = vec3(0, 0, 0)
+    self.prev_bearing = 0
+    self.last_update_time = time.monotonic()
+
   def apply_controls(self, steer_angle, throttle_out, brake_out):
     if (time.monotonic() - self.reset_time) > 2:
       self.vc[0] = steer_angle
@@ -91,6 +97,32 @@ def read_sensors(self, state: SimulatorState):
       state.gps.from_xy(curr_pos)
       state.valid = True
 
+      # IMU
+      current_time = time.monotonic()
+      dt = max(current_time - self.last_update_time, 1e-6)
+      self.last_update_time = current_time
+
+      # Gyroscope
+      bearing_rad = math.radians(state.bearing)
+      prev_bearing_rad = math.radians(self.prev_bearing)
+      delta_bearing = bearing_rad - prev_bearing_rad
+      delta_bearing = (delta_bearing + math.pi) % (2 * math.pi) - math.pi # wrap to [-pi, pi]
+      gyro_z = delta_bearing / dt
+      state.imu.gyroscope = vec3(0, 0, gyro_z)
+      self.prev_bearing = state.bearing
+
+      # Accelerometer
+      acceleration = vec3((state.velocity.x - self.prev_velocity.x) / dt,
+                          (state.velocity.y - self.prev_velocity.y) / dt,
+                          (state.velocity.z - self.prev_velocity.z) / dt)
+      self.prev_velocity = state.velocity
+
+      # Rotate acceleration from world frame to body frame
+      bearing_rad_current = -math.radians(state.bearing)
+      ax_body = acceleration.x * math.cos(bearing_rad_current) - acceleration.y * math.sin(bearing_rad_current)
+      ay_body = acceleration.x * math.sin(bearing_rad_current) + acceleration.y * math.cos(bearing_rad_current)
+      state.imu.accelerometer = vec3(ax_body, ay_body, acceleration.z)
+
       is_engaged = state.is_engaged
       if is_engaged and self.first_engage is None:
         self.first_engage = time.monotonic()

tools/sim/tests/test_metadrive_bridge.py

@@ -1,11 +1,16 @@
 import pytest
 import warnings
+import time
+import subprocess
+from multiprocessing import Queue
+import numpy as np
 
 # Since metadrive depends on pkg_resources, and pkg_resources is deprecated as an API
 warnings.filterwarnings("ignore", category=DeprecationWarning)
 
+from cereal import messaging
 from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge
-from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase
+from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase, SIM_DIR
 
 @pytest.mark.slow
 @pytest.mark.filterwarnings("ignore::pyopencl.CompilerWarning") # Unimportant warning of non-empty compile log
@@ -16,3 +21,54 @@ def setup_create_bridge(self, test_duration):
 
   def create_bridge(self):
     return MetaDriveBridge(False, False, self.test_duration, True)
+
+  def test_imu_data(self):
+    p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR)
+    self.processes.append(p_manager)
+
+    sm = messaging.SubMaster(['accelerometer', 'gyroscope', 'selfdriveState'])
+    q = Queue()
+    bridge = self.create_bridge()
+    p_bridge = bridge.run(q, retries=10)
+    self.processes.append(p_bridge)
+
+    max_time_per_step = 60
+
+    # Wait for bridge to startup
+    start_waiting = time.monotonic()
+    while not bridge.started.value and time.monotonic() < start_waiting + max_time_per_step:
+      time.sleep(0.1)
+    assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}"
+
+    # Wait for engagement
+    start_time = time.monotonic()
+    engaged = False
+    while time.monotonic() < start_time + max_time_per_step:
+      sm.update()
+      if sm.updated['selfdriveState'] and sm['selfdriveState'].active:
+        engaged = True
+        break
+      time.sleep(0.1)
+    assert engaged, "openpilot did not engage"
+
+    start_time = time.monotonic()
+    accel_values = []
+    gyro_values = []
+    # run for 10 seconds and collect some imu data
+    while time.monotonic() < start_time + 10:
+      sm.update()
+      if sm.updated['accelerometer']:
+        accel_values.append(list(sm['accelerometer'].acceleration.v))
+      if sm.updated['gyroscope']:
+        gyro_values.append(list(sm['gyroscope'].gyroUncalibrated.v))
+      time.sleep(0.1)
+
+    assert len(accel_values) > 10
+    assert len(gyro_values) > 10
+
+    # Check that the values are not all the same
+    accel_std = np.std(np.array(accel_values), axis=0)
+    gyro_std = np.std(np.array(gyro_values), axis=0)
+
+    assert any(x > 1e-3 for x in accel_std), f"accel_std: {accel_std}"
+    assert any(x > 1e-5 for x in gyro_std), f"gyro_std: {gyro_std}"  # Lower threshold for gyroscope