diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py index 1324932131..6c101c7b8c 100644 --- a/tools/sim/lib/common.py +++ b/tools/sim/lib/common.py @@ -1,13 +1,10 @@ import math import multiprocessing import numpy as np - from abc import ABC, abstractmethod from collections import namedtuple W, H = 1928, 1208 - - vec3 = namedtuple("vec3", ["x", "y", "z"]) class GPSState: @@ -59,16 +56,16 @@ class SimulatorState: @property def speed(self): return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2) - - +sim_state = SimulatorState() +sim_state.velocity = vec3(3.25, 10.50, 0) +sim_state.bearing = 72.51 +sim_state.steering_angle = 7.87 class World(ABC): def __init__(self, dual_camera): self.dual_camera = dual_camera - self.image_lock = multiprocessing.Semaphore(value=0) self.road_image = np.zeros((H, W, 3), dtype=np.uint8) self.wide_road_image = np.zeros((H, W, 3), dtype=np.uint8) - self.exit_event = multiprocessing.Event() @abstractmethod @@ -83,10 +80,6 @@ class World(ABC): def read_state(self): pass - @abstractmethod - def read_sensors(self, simulator_state: SimulatorState): - pass - @abstractmethod def read_cameras(self): pass @@ -98,3 +91,34 @@ class World(ABC): @abstractmethod def reset(self): pass + def read_sensors(self, simulator_state: SimulatorState): + """ + Populate IMUState from simulator kinematics. + - accelerometer: m/s^2 (Δv/Δt) + - gyroscope: rad/s (yaw rate from Δbearing) + - bearing: radians (absolute yaw) + """ + import time + now = time.monotonic() + imu = simulator_state.imu + v = simulator_state.velocity if simulator_state.velocity is not None else vec3(0.0, 0.0, 0.0) + bearing_rad = math.radians(simulator_state.bearing) + prev = getattr(self, "_imu_prev", None) + if prev is None: + imu.accelerometer = vec3(0.0, 0.0, 0.0) + imu.gyroscope = vec3(0.0, 0.0, 0.0) + else: + dt = max(1e-3, now - prev["t"]) + dvx = v.x - prev["vx"] + dvy = v.y - prev["vy"] + dvz = v.z - prev["vz"] + ax = dvx / dt + ay = dvy / dt + az = dvz / dt + db = (bearing_rad - prev["bearing"] + math.pi) % (2 * math.pi) - math.pi + yaw_rate = db / dt + imu.accelerometer = vec3(ax, ay, az) + imu.gyroscope = vec3(0.0, 0.0, yaw_rate) + imu.bearing = bearing_rad + self._imu_prev = {"t": now, "vx": v.x, "vy": v.y, "vz": v.z, "bearing": bearing_rad} +