From 7ea8d9e91d0a5909ea342d22a5953d8fab09e73c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kacper=20R=C4=85czy?= <gfw.kra@gmail.com>
Date: Mon, 29 Jul 2024 20:20:31 -0700
Subject: [PATCH] torqued: use livePose (#33136)

* Use livePose

* Replace it in process replay

* Add liveCalibration to messages

* Update ref commit
old-commit-hash: 86d8d1d99617a0808e0a17031965a056cdc5a1b8
---
 selfdrive/locationd/torqued.py                | 19 ++++++++++++++-----
 .../test/process_replay/process_replay.py     |  4 ++--
 selfdrive/test/process_replay/ref_commit      |  2 +-
 3 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py
index e0784eb8e6..edcf42ebcd 100755
--- a/selfdrive/locationd/torqued.py
+++ b/selfdrive/locationd/torqued.py
@@ -8,6 +8,7 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
+from openpilot.common.transformations.orientation import rot_from_euler
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
 
@@ -77,6 +78,8 @@ class TorqueEstimator(ParameterEstimator):
       self.offline_friction = CP.lateralTuning.torque.friction
       self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
 
+    self.calib_from_device = np.eye(3)
+
     self.reset()
 
     initial_params = {
@@ -171,12 +174,18 @@ class TorqueEstimator(ParameterEstimator):
       # TODO: check if high aEgo affects resulting lateral accel
       self.raw_points["vego"].append(msg.vEgo)
       self.raw_points["steer_override"].append(msg.steeringPressed)
+    elif which == "liveCalibration":
+      device_from_calib = rot_from_euler(np.array(msg.rpyCalib))
+      self.calib_from_device = device_from_calib.T
 
     # calculate lateral accel from past steering torque
-    elif which == "liveLocationKalman":
+    elif which == "livePose":
       if len(self.raw_points['steer_torque']) == self.hist_len:
-        yaw_rate = msg.angularVelocityCalibrated.value[2]
-        roll = msg.orientationNED.value[0]
+        angular_velocity_device = np.array([msg.angularVelocityDevice.x, msg.angularVelocityDevice.y, msg.angularVelocityDevice.z])
+        angular_velocity_calibrated = np.matmul(self.calib_from_device, angular_velocity_device)
+
+        yaw_rate = angular_velocity_calibrated[2]
+        roll = msg.orientationNED.x
         # check lat active up to now (without lag compensation)
         lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
                                self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
@@ -233,7 +242,7 @@ def main(demo=False):
   config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose'], poll='livePose')
 
   params = Params()
   estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
@@ -246,7 +255,7 @@ def main(demo=False):
           t = sm.logMonoTime[which] * 1e-9
           estimator.handle_log(t, which, sm[which])
 
-    # 4Hz driven by liveLocationKalman
+    # 4Hz driven by livePose
     if sm.frame % 5 == 0:
       pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
 
diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py
index 84669a8bff..1731bf2e1e 100755
--- a/selfdrive/test/process_replay/process_replay.py
+++ b/selfdrive/test/process_replay/process_replay.py
@@ -390,7 +390,7 @@ def calibration_rcv_callback(msg, cfg, frame):
 
 def torqued_rcv_callback(msg, cfg, frame):
   # should_recv always true to increment frame
-  return (frame - 1) == 0 or msg.which() == 'liveLocationKalman'
+  return (frame - 1) == 0 or msg.which() == 'livePose'
 
 
 def dmonitoringmodeld_rcv_callback(msg, cfg, frame):
@@ -555,7 +555,7 @@ CONFIGS = [
   ),
   ProcessConfig(
     proc_name="torqued",
-    pubs=["liveLocationKalman", "carState", "carControl", "carOutput"],
+    pubs=["livePose", "liveCalibration", "carState", "carControl", "carOutput"],
     subs=["liveTorqueParameters"],
     ignore=["logMonoTime"],
     init_callback=get_car_params_callback,
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 4da168c270..3af1f282fb 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-3e8feca20cd64f6a05dcbbdc0ace04a8c12bf86d
\ No newline at end of file
+7eb60abe8030b97f79f858315d67d080704733f7
\ No newline at end of file