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: 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() diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 04ce5d584f96b3..d62852acf79b81 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,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