Update simulator to calculate and fill sensor values (IMUState)

tools/sim/bridge/metadrive/metadrive_world.py

@@ -36,6 +36,8 @@ def __init__(self, status_q, config, test_duration, test_run, dual_camera=False)
     self.first_engage = None
     self.last_check_timestamp = 0
     self.distance_moved = 0
+    self.prev_velocity = None
+    self.prev_bearing = None
 
     self.metadrive_process = multiprocessing.Process(name="metadrive process", target=
                               functools.partial(metadrive_process, dual_camera, config,
@@ -81,12 +83,13 @@ def read_state(self):
         self.exit_event.set()
 
   def read_sensors(self, state: SimulatorState):
+
     while self.vehicle_state_recv.poll(0):
       md_vehicle: metadrive_vehicle_state = self.vehicle_state_recv.recv()
       curr_pos = md_vehicle.position
 
       state.velocity = md_vehicle.velocity
-      state.bearing = md_vehicle.bearing
+      state.bearing = state.imu.bearing = md_vehicle.bearing
       state.steering_angle = md_vehicle.steering_angle
       state.gps.from_xy(curr_pos)
       state.valid = True
@@ -105,9 +108,21 @@ def read_sensors(self, state: SimulatorState):
       if x_dist >= dist_threshold or y_dist >= dist_threshold: # position not the same during staying still, > threshold is considered moving
         self.distance_moved += x_dist + y_dist
 
-      time_check_threshold = 29
       current_time = time.monotonic()
       since_last_check = current_time - self.last_check_timestamp
+
+      imu_update_interval = 1
+      if since_last_check >= imu_update_interval:
+        prev_velocity = self.prev_velocity if self.prev_velocity is not None else state.velocity
+        prev_bearing = self.prev_bearing if self.prev_bearing is not None else state.bearing
+        state.set_accelerometer(prev_velocity, since_last_check)
+        state.set_gyroscope(prev_bearing, since_last_check)
+
+        self.prev_velocity = state.velocity
+        self.prev_bearing = state.bearing
+
+
+      time_check_threshold = 29
       if since_last_check >= time_check_threshold:
         if after_engaged_check and self.distance_moved == 0:
           self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True}))

tools/sim/lib/common.py

@@ -58,7 +58,41 @@ def __init__(self):
 
   @property
   def speed(self):
-    return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2)
+    return math.sqrt(self.velocity.x**2 + self.velocity.y**2 + self.velocity.z**2)
+
+  def set_accelerometer(self, prev_velocity: vec3, dt: float) -> vec3:
+
+    if dt <= 0:
+      print("Warning: Time difference (dt) must be greater than zero. Skipping IMU update.")
+      return vec3(0, 0, 0)  # Return a default acceleration value
+
+    # Compute acceleration for each component
+    acc_x = (self.velocity.x - prev_velocity.x) / dt
+    acc_y = (self.velocity.y - prev_velocity.y) / dt
+    acc_z = (self.velocity.z - prev_velocity.z) / dt
+
+    self.imu.accelerometer = vec3(acc_x, acc_y, acc_z)
+    # Return acceleration as a vec3
+    return vec3(acc_x, acc_y, acc_z)
+
+  def set_gyroscope(self, prev_bearing: float, dt: float) -> vec3:
+    """
+    Calculate a simple gyroscope reading based on the rate of change of bearing (yaw).
+    For more advanced scenarios, you could also estimate pitch and roll rates.
+    """
+
+    if dt <= 0:
+      print("Warning: Time difference (dt) must be greater than zero. Skipping IMU update.")
+      return vec3(0, 0, 0)
+
+    # Calculate change in bearing (yaw)
+    delta_bearing = self.bearing - prev_bearing
+    yaw_rate = delta_bearing / dt
+
+    self.imu.gyroscope = vec3(0, 0, yaw_rate)
+
+    # For now, assume no pitch or roll rate 0 car probably not rotating along its axis and not tilting its nose most of the time
+    return vec3(0, 0, yaw_rate)
 
 
 class World(ABC):