From 36a9687d5d60c0cf26387f8edcf5f6ccc9214d0d Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Thu, 26 Sep 2024 11:01:56 -0700 Subject: [PATCH] long planner: use temporalPose for v_model_error calibration (#33655) * long planner: add flag to disable v_model_error calibration * use temporal pose instead of flag * udpate model replay ref --- selfdrive/controls/lib/drive_helpers.py | 4 ++-- selfdrive/modeld/fill_model_msg.py | 7 +++++++ selfdrive/test/process_replay/model_replay_ref_commit | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 369aeaf984..64cbf473d6 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.velocity.x): - vel_err = clip(modelV2.velocity.x[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) + if len(modelV2.temporalPose.trans): + vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) return float(vel_err) return 0.0 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 18dd9c7bac..10a1860b58 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -87,6 +87,13 @@ 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 + 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) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index de6f6027c4..7d588b26fa 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -9d4d653d8cc361fe2ba53f74fd8fcfe1f1d559ed +666448fce191e196aac68d06e29a0745e6620db9