From e8bea2c78ffa92685ece511e9b554122aaf1a79d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Mon, 23 Sep 2024 14:26:28 -0700 Subject: [PATCH] Tomb Raider Model (#33629) * 69acff08-5383-4103-beea-822f0d228c76/160 * 53f39907-4763-4d19-ba26-e757527c2b61/200 * 21afb89f-1397-4652-b423-abc2da32417a/200 * 21afb89f-1397-4652-b423-abc2da32417a/400 * 53f39907-4763-4d19-ba26-e757527c2b61/400 * 3fb967fd-16a8-4569-ba57-359e54deeab3/395 * Try other policy again * 3fb967fd-16a8-4569-ba57-359e54deeab3/400 * 0136cabf-539a-4a43-bc7f-06c3654a493c/4400 * 473c0686-1ac9-4c05-9b0b-d1f1afdb6cc3/400 * 83b7993d-51a9-4e3f-904e-3fcd5763c231/400 * 021566c7-cff3-431d-8da2-17a96c888c5f/400 * Fix bugs * Update longitudinal_planner.py * Update fill_model_msg.py * 8be6b59d-4449-42fd-b8e0-93c48387070f/400 * f670a748-3591-4489-a0b3-215118ddad01/400 * 790a2950-c713-4eec-838b-4f55f4fe0ccb/400 * Update model ref * Plans start at ego * Update ref --- selfdrive/controls/lib/drive_helpers.py | 4 ++-- selfdrive/controls/radard.py | 4 ++-- selfdrive/modeld/fill_model_msg.py | 7 ------- selfdrive/modeld/models/supercombo.onnx | 4 ++-- selfdrive/modeld/parse_model_outputs.py | 1 - selfdrive/test/longitudinal_maneuvers/plant.py | 1 + selfdrive/test/process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 8 files changed, 9 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 64cbf473d6..369aeaf984 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -22,7 +22,7 @@ def clip_curvature(v_ego, prev_curvature, new_curvature): def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: # ToDo: Try relative error, and absolute speed - if len(modelV2.temporalPose.trans): - vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) + if len(modelV2.velocity.x): + vel_err = clip(modelV2.velocity.x[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f577025fa4..a5987b4bcc 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -243,8 +243,8 @@ class RadarD: self.radar_state.radarErrors = list(rr.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 9cdd7e4c52..18dd9c7bac 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -168,13 +168,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 - 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() - # confidence if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0: # any disengage prob diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 2e1a60a23e..111cb76dbb 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4 -size 51453312 +oid sha256:d42319e42e8ba1a818c845ee0c687ece6afd98e59fef142895be33dd230974c2 +size 62481231 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 03d03c5e1c..d57b6eab3f 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -87,7 +87,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,)) - 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)) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 85e2ddf8df..21f2b3b57f 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -107,6 +107,7 @@ class Plant: model.modelV2.position = position velocity = log.XYZTData.new_message() velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + velocity.x[0] = float(self.speed) # always start at current speed model.modelV2.velocity = velocity acceleration = log.XYZTData.new_message() acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 66971ebde1..315a15975b 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -32fe8cf4a0daa8d10a689c9ae2e51a879151c87c +f0504b66f3f736b5bea39cc01e1a073e3be95659 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 04b3c0a6fd..e154284e3d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -96c2e2ab2e3a11476f1207c531893cc8e45d2b3c +d8a02c23f530dd236553b47ff7f0355929fe15fc