From 0267e0500c49ccbd00c58a8911e2446c7b8c614f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kacper=20R=C4=85czy?= <gfw.kra@gmail.com>
Date: Thu, 7 Sep 2023 11:24:54 -0700
Subject: [PATCH] radard: expose radarTrackId for radar leads (#29809)

* Add radarState track identifier for debugging purposes

* update process replay ref
---
 selfdrive/controls/radard.py             | 11 +++++++----
 selfdrive/test/process_replay/ref_commit |  2 +-
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 8a21fdd8ab..ec42ae66f2 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -50,7 +50,8 @@ class KalmanParams:
 
 
 class Track:
-  def __init__(self, v_lead: float, kalman_params: KalmanParams):
+  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
+    self.identifier = identifier
     self.cnt = 0
     self.aLeadTau = _LEAD_ACCEL_TAU
     self.K_A = kalman_params.A
@@ -98,11 +99,12 @@ class Track:
       "vLead": float(self.vLead),
       "vLeadK": float(self.vLeadK),
       "aLeadK": float(self.aLeadK),
+      "aLeadTau": float(self.aLeadTau),
       "status": True,
       "fcw": self.is_potential_fcw(model_prob),
       "modelProb": model_prob,
       "radar": True,
-      "aLeadTau": float(self.aLeadTau)
+      "radarTrackId": self.identifier,
     }
 
   def potential_low_speed_lead(self, v_ego: float):
@@ -158,8 +160,9 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
     "aLeadTau": 0.3,
     "fcw": False,
     "modelProb": float(lead_msg.prob),
+    "status": True,
     "radar": False,
-    "status": True
+    "radarTrackId": -1,
   }
 
 
@@ -237,7 +240,7 @@ class RadarD:
 
       # create the track if it doesn't exist or it's a new track
       if ids not in self.tracks:
-        self.tracks[ids] = Track(v_lead, self.kalman_params)
+        self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
       self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
 
     # *** publish radarState ***
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 9e0f4d7b3d..19a8a56e55 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-89e571c56fb6c053177cd28c3e0d688c641b46d5
\ No newline at end of file
+98c21236f831ca3cff63939cb760b213460e84de
\ No newline at end of file