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