From 8ee80deb960f5e90b3524dc3f3c745d34e261d3a Mon Sep 17 00:00:00 2001
From: Kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Sat, 28 Sep 2024 15:01:41 -0700
Subject: [PATCH] maybe>?

---
 selfdrive/controls/radard.py            |  4 +--
 selfdrive/modeld/fill_model_msg.py      | 38 ++++++++++++-------------
 selfdrive/modeld/parse_model_outputs.py |  2 --
 3 files changed, 21 insertions(+), 23 deletions(-)

diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 0c8c9ff179..9ed9ce6dfc 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -255,8 +255,8 @@ class RadarD:
     self.radar_state.radarErrors = list(radar_errors)
     self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
 
-    if len(sm['modelV2'].temporalPose.trans):
-      model_v_ego = sm['modelV2'].temporalPose.trans[0]
+    if len(sm['modelV2'].velocity.x):
+      model_v_ego = sm['modelV2'].velocity.x[0]
     else:
       model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3
diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py
index 92a419eed5..c794f8cf86 100644
--- a/selfdrive/modeld/fill_model_msg.py
+++ b/selfdrive/modeld/fill_model_msg.py
@@ -99,6 +99,25 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   orientation_rate = modelV2.orientationRate
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
+  # temporal pose
+  temporal_pose = modelV2.temporalPose
+  if custom_model.valid:
+    if custom_model.capabilities & ModelCapabilities.PlanTemporalPose:
+      temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
+      temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
+      temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
+      temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
+    else:
+      temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
+      temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
+      temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
+      temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+  else:
+    temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
+    temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
+    temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
+    temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
+
   # poly path
   poly_path = driving_model_data.path
   fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
@@ -185,25 +204,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
     (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
   meta.hardBrakePredicted = hard_brake_predicted.item()
 
-  # temporal pose
-  temporal_pose = modelV2.temporalPose
-  if custom_model.valid:
-    if custom_model.capabilities & ModelCapabilities.PlanTemporalPose:
-      temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
-      temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
-      temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
-      temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
-    else:
-      temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
-      temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
-      temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
-      temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
-  else:
-    temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
-    temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
-    temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
-    temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
-
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
     # any disengage prob
diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py
index d3c090649e..4367e9db8a 100644
--- a/selfdrive/modeld/parse_model_outputs.py
+++ b/selfdrive/modeld/parse_model_outputs.py
@@ -91,8 +91,6 @@ class Parser:
     self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    if 'sim_pose' in outs:
-      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
                    out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))