From a65f707e310c15fe81e6e250bdca646536da38c5 Mon Sep 17 00:00:00 2001 From: Alex Strizhak Date: Wed, 28 Jan 2026 15:42:33 +0000 Subject: [PATCH 1/6] sim: fake IMU data in simulator --- tools/sim/bridge/metadrive/metadrive_world.py | 34 ++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index c5111289d01394..ddb307353a5d9b 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/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() From 0d178abffb97761923f739b0e2d040401a5f9938 Mon Sep 17 00:00:00 2001 From: Alex Strizhak Date: Wed, 28 Jan 2026 15:42:56 +0000 Subject: [PATCH 2/6] test: add test for sim IMU data --- tools/sim/tests/test_metadrive_bridge.py | 60 +++++++++++++++++++++++- 1 file changed, 59 insertions(+), 1 deletion(-) diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 04ce5d584f96b3..e524454de24276 100644 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/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,56 @@ 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(['sensorEvents', '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['sensorEvents']: + for event in sm['sensorEvents']: + if event.which == 'acceleration': + accel_values.append(list(event.acceleration.v)) + elif event.which == 'gyro': + gyro_values.append(list(event.gyro.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-3 for x in gyro_std), f"gyro_std: {gyro_std}" From e0ae866f7aa5077c9c1b571719db63e79661e637 Mon Sep 17 00:00:00 2001 From: Alex Strizhak Date: Wed, 28 Jan 2026 16:10:58 +0000 Subject: [PATCH 3/6] [WIP] enable simulator_driving tests --- .github/workflows/test_imu.yaml | 30 ++++++++++++++++++++++++++++++ .github/workflows/tests.yaml | 2 +- 2 files changed, 31 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/test_imu.yaml diff --git a/.github/workflows/test_imu.yaml b/.github/workflows/test_imu.yaml new file mode 100644 index 00000000000000..bd26be86cf21e4 --- /dev/null +++ b/.github/workflows/test_imu.yaml @@ -0,0 +1,30 @@ +name: IMU Test + +on: + pull_request: + paths: + - 'tools/sim/**' + +jobs: + test_imu: + name: Test IMU Data + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + with: + submodules: true + - name: Setup Python + uses: actions/setup-python@v4 + with: + python-version: '3.11' + - name: Install dependencies + run: | + pip install numpy pytest + - name: Test IMU functionality + run: | + python3 -c " + import sys; sys.path.insert(0, '.') + from cereal import messaging + sm = messaging.SubMaster(['accelerometer', 'gyroscope']) + print('IMU services available') + " diff --git a/.github/workflows/tests.yaml b/.github/workflows/tests.yaml index c5802b5cb2c9c3..1f015463d31416 100644 --- a/.github/workflows/tests.yaml +++ b/.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: From 04de81d0cc9cec43da0493f29b8b6425f7b20179 Mon Sep 17 00:00:00 2001 From: Alex Strizhak Date: Wed, 28 Jan 2026 16:31:41 +0000 Subject: [PATCH 4/6] fix: use correct IMU services in test --- tools/sim/tests/test_metadrive_bridge.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index e524454de24276..5773245fb570d1 100644 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -26,7 +26,7 @@ def test_imu_data(self): p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) self.processes.append(p_manager) - sm = messaging.SubMaster(['sensorEvents', 'selfdriveState']) + sm = messaging.SubMaster(['accelerometer', 'gyroscope', 'selfdriveState']) q = Queue() bridge = self.create_bridge() p_bridge = bridge.run(q, retries=10) @@ -57,12 +57,10 @@ def test_imu_data(self): # run for 10 seconds and collect some imu data while time.monotonic() < start_time + 10: sm.update() - if sm.updated['sensorEvents']: - for event in sm['sensorEvents']: - if event.which == 'acceleration': - accel_values.append(list(event.acceleration.v)) - elif event.which == 'gyro': - gyro_values.append(list(event.gyro.v)) + if sm.updated['accelerometer']: + accel_values.append(list(sm['accelerometer'].v)) + if sm.updated['gyroscope']: + gyro_values.append(list(sm['gyroscope'].v)) time.sleep(0.1) assert len(accel_values) > 10 From cba0d1111d376174affafd9e9bc5871ae7dbb2f6 Mon Sep 17 00:00:00 2001 From: Alex Strizhak Date: Wed, 28 Jan 2026 16:37:55 +0000 Subject: [PATCH 5/6] fix: correct IMU data access in test --- tools/sim/tests/test_metadrive_bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 5773245fb570d1..31eefb66bc3b76 100644 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -58,9 +58,9 @@ def test_imu_data(self): while time.monotonic() < start_time + 10: sm.update() if sm.updated['accelerometer']: - accel_values.append(list(sm['accelerometer'].v)) + accel_values.append(list(sm['accelerometer'].acceleration.v)) if sm.updated['gyroscope']: - gyro_values.append(list(sm['gyroscope'].v)) + gyro_values.append(list(sm['gyroscope'].gyro.v)) time.sleep(0.1) assert len(accel_values) > 10 From 63ac40ccb90d434ec0846b0b204f8ae041479588 Mon Sep 17 00:00:00 2001 From: Alex Strizhak Date: Wed, 28 Jan 2026 16:44:01 +0000 Subject: [PATCH 6/6] fix: use gyroUncalibrated field for gyroscope data --- .github/workflows/test_imu.yaml | 30 ------------------------ tools/sim/tests/test_metadrive_bridge.py | 4 ++-- 2 files changed, 2 insertions(+), 32 deletions(-) delete mode 100644 .github/workflows/test_imu.yaml diff --git a/.github/workflows/test_imu.yaml b/.github/workflows/test_imu.yaml deleted file mode 100644 index bd26be86cf21e4..00000000000000 --- a/.github/workflows/test_imu.yaml +++ /dev/null @@ -1,30 +0,0 @@ -name: IMU Test - -on: - pull_request: - paths: - - 'tools/sim/**' - -jobs: - test_imu: - name: Test IMU Data - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - submodules: true - - name: Setup Python - uses: actions/setup-python@v4 - with: - python-version: '3.11' - - name: Install dependencies - run: | - pip install numpy pytest - - name: Test IMU functionality - run: | - python3 -c " - import sys; sys.path.insert(0, '.') - from cereal import messaging - sm = messaging.SubMaster(['accelerometer', 'gyroscope']) - print('IMU services available') - " diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 31eefb66bc3b76..d62852acf79b81 100644 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -60,7 +60,7 @@ def test_imu_data(self): if sm.updated['accelerometer']: accel_values.append(list(sm['accelerometer'].acceleration.v)) if sm.updated['gyroscope']: - gyro_values.append(list(sm['gyroscope'].gyro.v)) + gyro_values.append(list(sm['gyroscope'].gyroUncalibrated.v)) time.sleep(0.1) assert len(accel_values) > 10 @@ -71,4 +71,4 @@ def test_imu_data(self): 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-3 for x in gyro_std), f"gyro_std: {gyro_std}" + assert any(x > 1e-5 for x in gyro_std), f"gyro_std: {gyro_std}" # Lower threshold for gyroscope