From a446c1fa5665086d690a017f453cad3a4c38f00a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= <harald.the.engineer@gmail.com>
Date: Fri, 2 Jun 2023 14:38:28 -0700
Subject: [PATCH] Radard: just get relative speed from model (#27493)

* refactor radard

* Revert "refactor radard"

This reverts commit 4b3507ff58e753334969b6ba2c2f6b1b35a200cc.

* May work

* No radar for test

* check length

* no accel for now

* First accel

* Cleaner way

* Re-enable radar

* update proc replay

* This might cause oscillation

* Update ref commit
---
 selfdrive/controls/lib/radar_helpers.py  | 13 +++++++------
 selfdrive/controls/radard.py             | 12 ++++++++----
 selfdrive/test/process_replay/ref_commit |  2 +-
 3 files changed, 16 insertions(+), 11 deletions(-)

diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py
index 4bb0179267..4184340dc5 100644
--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/selfdrive/controls/lib/radar_helpers.py
@@ -130,15 +130,16 @@ class Cluster():
       "aLeadTau": float(self.aLeadTau)
     }
 
-  def get_RadarState_from_vision(self, lead_msg, v_ego):
+  def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego):
+    lead_v_rel_pred = lead_msg.v[0] - model_v_ego
     return {
       "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
       "yRel": float(-lead_msg.y[0]),
-      "vRel": float(lead_msg.v[0] - v_ego),
-      "vLead": float(lead_msg.v[0]),
-      "vLeadK": float(lead_msg.v[0]),
-      "aLeadK": float(0),
-      "aLeadTau": _LEAD_ACCEL_TAU,
+      "vRel": float(lead_v_rel_pred),
+      "vLead": float(v_ego + lead_v_rel_pred),
+      "vLeadK": float(v_ego + lead_v_rel_pred),
+      "aLeadK": 0.0,
+      "aLeadTau": 0.3,
       "fcw": False,
       "modelProb": float(lead_msg.prob),
       "radar": False,
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 34f0f274fe..521cea816f 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -64,7 +64,7 @@ def match_vision_to_cluster(v_ego, lead, clusters):
     return None
 
 
-def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
+def get_lead(v_ego, ready, clusters, lead_msg, model_v_ego, low_speed_override=True):
   # Determine leads, this is where the essential logic happens
   if len(clusters) > 0 and ready and lead_msg.prob > .5:
     cluster = match_vision_to_cluster(v_ego, lead_msg, clusters)
@@ -75,7 +75,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
   if cluster is not None:
     lead_dict = cluster.get_RadarState(lead_msg.prob)
   elif (cluster is None) and ready and (lead_msg.prob > .5):
-    lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
+    lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
 
   if low_speed_override:
     low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
@@ -168,10 +168,14 @@ class RadarD():
     radarState.radarErrors = list(rr.errors)
     radarState.carStateMonoTime = sm.logMonoTime['carState']
 
+    if len(sm['modelV2'].temporalPose.trans):
+      model_v_ego = sm['modelV2'].temporalPose.trans[0]
+    else:
+      model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3
     if len(leads_v3) > 1:
-      radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True)
-      radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False)
+      radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], model_v_ego, low_speed_override=True)
+      radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], model_v_ego, low_speed_override=False)
     return dat
 
 
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 52735b686b..7fa8dd9530 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-29f846f525a4e14f380afd20ae8fa0804011ab6e
\ No newline at end of file
+73549898edd3d9a428fa6f58e25c2c0300290ef2