mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Model error deprecated with TR (#35628)

Browse Source 
* Model error deprecated with TR

* no get speed error

* import
This commit is contained in:
Harald Schäfer
2025-07-02 23:36:41 -07:00
committed by GitHub
parent 64fd3f9860
commit e503e657bc
3 changed files with 6 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cereal/log.capnp
View File

@@ -1083,7 +1083,7 @@ struct ModelDataV2 {
confidence @23: ConfidenceClass;
# Model perceived motion
temporalPose @21 :Pose;
temporalPoseDEPRECATED @21 :Pose;
# e2e lateral planner
action @26: Action;

9
selfdrive/controls/lib/drive_helpers.py
View File

@@ -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]

13
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -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