From e503e657bc06bd6c47a290f62fe0984af58b1b16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Wed, 2 Jul 2025 23:36:41 -0700 Subject: [PATCH] Model error deprecated with TR (#35628) * Model error deprecated with TR * no get speed error * import --- cereal/log.capnp | 2 +- selfdrive/controls/lib/drive_helpers.py | 9 --------- selfdrive/controls/lib/longitudinal_planner.py | 13 +++++-------- 3 files changed, 6 insertions(+), 18 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index a754b9ef00..7f41cffa15 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1083,7 +1083,7 @@ struct ModelDataV2 { confidence @23: ConfidenceClass; # Model perceived motion - temporalPose @21 :Pose; + temporalPoseDEPRECATED @21 :Pose; # e2e lateral planner action @26: Action; diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 56eeac3bdf..51eb694b4c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,5 +1,4 @@ import numpy as np -from cereal import log from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.realtime import DT_CTRL, DT_MDL @@ -40,14 +39,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll): return float(new_curvature), limited_accel or limited_max_curv -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 = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR) - return float(vel_err) - return 0.0 - - def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): if len(speeds) == len(t_idxs): v_now = speeds[0] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f5c273cf1c..ba622416ac 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,7 +11,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error, get_accel_from_plan +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog @@ -61,7 +61,6 @@ class LongitudinalPlanner: self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] - self.v_model_error = 0.0 self.output_a_target = 0.0 self.output_should_stop = False @@ -71,12 +70,12 @@ class LongitudinalPlanner: self.solverExecutionTime = 0.0 @staticmethod - def parse_model(model_msg, model_error): + def parse_model(model_msg): if (len(model_msg.position.x) == ModelConstants.IDX_N and len(model_msg.velocity.x) == ModelConstants.IDX_N and len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error + x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) + v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) j = np.zeros(len(T_IDXS_MPC)) else: @@ -128,9 +127,7 @@ class LongitudinalPlanner: # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - # TODO v_model_error is deprecated with TR - self.v_model_error = get_speed_error(sm['modelV2'], v_ego) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2']) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED