From 9f1e462faabdc000b5c574b0e38818bcd6068e88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Wed, 30 Apr 2025 17:19:53 -0700 Subject: [PATCH] Tomb Raider 6 (#35087) * 5ec366c3-7883-4004-84a2-e4b14bac5b1d/400 * Use lat plan * fix import * fix * 8d0a1b3b-9972-4e53-b9c5-3e13e5e3e404/400 * whitespace * whitespace --- selfdrive/controls/lib/drive_helpers.py | 10 ++++++++++ selfdrive/modeld/modeld.py | 7 ++++--- selfdrive/modeld/models/driving_policy.onnx | 4 ++-- selfdrive/modeld/models/driving_vision.onnx | 2 +- 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 3f99f1bbbb..56eeac3bdf 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -62,3 +62,13 @@ def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0. should_stop = (v_target < vEgoStopping and v_target_1sec < vEgoStopping) return a_target, should_stop + +def curv_from_psis(psi_target, psi_rate, vego, action_t): + vego = np.clip(vego, MIN_SPEED, np.inf) + curv_from_psi = psi_target / (vego * action_t) + return 2*curv_from_psi - psi_rate / vego + +def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t): + psi_target = np.interp(action_t, t_idxs, yaws) + psi_rate = yaw_rates[0] + return curv_from_psis(psi_target, psi_rate, vego, action_t) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 9805506432..c1b3dd52d8 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -26,7 +26,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -54,8 +54,9 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. ModelConstants.T_IDXS, action_t=long_action_t) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) - - desired_curvature = model_output['desired_curvature'][0, 0] + desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], + plan[:, Plan.ORIENTATION_RATE][:, 2], + ModelConstants.T_IDXS, v_ego, lat_action_t) if v_ego > MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) else: diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 2075792f4d..5b9b686829 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b86d130c5db2d772da1b139c136ed86976f37137129a19a6b881fdf641bca198 -size 15578328 +oid sha256:8a478376723ba79e2393ca95e2d3e497571ba6fed113e5f13a36f0e4b4d4a7c5 +size 15588463 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index 5e08b78472..0a7b4a803d 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f222d2c528f1763828de01bb55e8979b1e4056e1dbb41350f521d2d2bb09d177 +oid sha256:dad289ae367cefcb862ef1d707fb4919d008f0eeaa1ebaf18df58d8de5a7d96e size 46265585