sim: fix sensor freq and timestamps (#25937)

* sim: fix sensor freq and timestamps

* 100hz

* fix that too

selfdrive/locationd/locationd.cc

@@ -499,7 +499,7 @@ int Localizer::locationd_thread() {
   }
   const std::initializer_list<const char *> service_list = {gps_location_socket,
     "cameraOdometry", "liveCalibration", "carState", "carParams",
-    "accelerometer", "gyroscope", "magnetometer"};
+    "accelerometer", "gyroscope"};
   PubMaster pm({"liveLocationKalman"});
 
   // TODO: remove carParams once we're always sending at 100Hz
@@ -526,7 +526,7 @@ int Localizer::locationd_thread() {
     if (sm.updated(trigger_msg)) {
       bool inputsOK = sm.allAliveAndValid();
       bool gpsOK = this->isGpsOK();
-      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope", "magnetometer"});
+      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
 
       MessageBuilder msg_builder;
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);

tools/sim/bridge.py

@@ -122,21 +122,26 @@ class Camerad:
     pm.send(pub_type, dat)
 
 def imu_callback(imu, vehicle_state):
-  vehicle_state.bearing_deg = math.degrees(imu.compass)
+  # send 5x since 'sensor_tick' doesn't seem to work. limited by the world tick?
-  dat = messaging.new_message('accelerometer')
+  for _ in range(5):
-  dat.accelerometer.sensor = 4
+    vehicle_state.bearing_deg = math.degrees(imu.compass)
-  dat.accelerometer.type = 0x1
+    dat = messaging.new_message('accelerometer')
-  dat.accelerometer.init('acceleration')
+    dat.accelerometer.sensor = 4
-  dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
+    dat.accelerometer.type = 0x1
-  pm.send('accelerometer', dat)
+    dat.accelerometer.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
-
+    dat.accelerometer.init('acceleration')
-  # copied these numbers from locationd
+    dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
-  dat = messaging.new_message('gyroscope')
+    pm.send('accelerometer', dat)
-  dat.gyroscope.sensor = 5
+
-  dat.gyroscope.type = 0x10
+    # copied these numbers from locationd
-  dat.gyroscope.init('gyroUncalibrated')
+    dat = messaging.new_message('gyroscope')
-  dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
+    dat.gyroscope.sensor = 5
-  pm.send('gyroscope', dat)
+    dat.gyroscope.type = 0x10
+    dat.gyroscope.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
+    dat.gyroscope.init('gyroUncalibrated')
+    dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
+    pm.send('gyroscope', dat)
+    time.sleep(0.01)
 
 
 def panda_state_function(vs: VehicleState, exit_event: threading.Event):
@@ -351,6 +356,7 @@ class CarlaBridge:
 
     # re-enable IMU
     imu_bp = blueprint_library.find('sensor.other.imu')
+    imu_bp.set_attribute('sensor_tick', '0.01')
     imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
     imu.listen(lambda imu: imu_callback(imu, vehicle_state))