From 5d33199905cbf9d9b45ef722a40530b08d5cecf4 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Thu, 29 Sep 2022 22:36:09 -0700
Subject: [PATCH] sim: fix sensor freq and timestamps (#25937)

* sim: fix sensor freq and timestamps

* 100hz

* fix that too
---
 selfdrive/locationd/locationd.cc |  4 ++--
 tools/sim/bridge.py              | 36 +++++++++++++++++++-------------
 2 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 087710d846..ac340fb4aa 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/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);
diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py
index cf836f3db7..c400eb93f5 100755
--- a/tools/sim/bridge.py
+++ b/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)
-  dat = messaging.new_message('accelerometer')
-  dat.accelerometer.sensor = 4
-  dat.accelerometer.type = 0x1
-  dat.accelerometer.init('acceleration')
-  dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
-  pm.send('accelerometer', dat)
-
-  # copied these numbers from locationd
-  dat = messaging.new_message('gyroscope')
-  dat.gyroscope.sensor = 5
-  dat.gyroscope.type = 0x10
-  dat.gyroscope.init('gyroUncalibrated')
-  dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
-  pm.send('gyroscope', dat)
+  # send 5x since 'sensor_tick' doesn't seem to work. limited by the world tick?
+  for _ in range(5):
+    vehicle_state.bearing_deg = math.degrees(imu.compass)
+    dat = messaging.new_message('accelerometer')
+    dat.accelerometer.sensor = 4
+    dat.accelerometer.type = 0x1
+    dat.accelerometer.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
+    dat.accelerometer.init('acceleration')
+    dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
+    pm.send('accelerometer', dat)
+
+    # copied these numbers from locationd
+    dat = messaging.new_message('gyroscope')
+    dat.gyroscope.sensor = 5
+    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))