mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-02 12:02:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tomb Raider 6 (#35087)

Browse Source 
* 5ec366c3-7883-4004-84a2-e4b14bac5b1d/400

* Use lat plan

* fix import

* fix

* 8d0a1b3b-9972-4e53-b9c5-3e13e5e3e404/400

* whitespace

* whitespace
This commit is contained in:
Harald Schäfer
2025-04-30 17:19:53 -07:00
committed by GitHub
parent c5ba5c9c23
commit 9f1e462faa
4 changed files with 17 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/drive_helpers.py
+10
View File
@@ -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)
selfdrive/modeld/modeld.py
+4 -3
View File
@@ -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:
selfdrive/modeld/models/driving_policy.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:b86d130c5db2d772da1b139c136ed86976f37137129a19a6b881fdf641bca198
size 15578328
oid sha256:8a478376723ba79e2393ca95e2d3e497571ba6fed113e5f13a36f0e4b4d4a7c5
size 15588463
selfdrive/modeld/models/driving_vision.onnx
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f222d2c528f1763828de01bb55e8979b1e4056e1dbb41350f521d2d2bb09d177
oid sha256:dad289ae367cefcb862ef1d707fb4919d008f0eeaa1ebaf18df58d8de5a7d96e
size 46265585