mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-31 21:23:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Reapply "Tomb Raider 6""

Browse Source 
This reverts commit f3de8169b9.
This commit is contained in:
firestar5683
2025-05-06 16:32:19 -05:00
parent 9d6211ee7c
commit 68b97a87db
17 changed files with 34 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/log.capnp
-2
View File
@@ -1048,8 +1048,6 @@ struct ModelDataV2 {
struct Action {
desiredCurvature @0 :Float32;
desiredAcceleration @1 :Float32;
shouldStop @2 :Bool;
}
}
frogpilot/assets/model_manager.py
+1 -1
View File
@@ -13,7 +13,7 @@ from openpilot.frogpilot.assets.download_functions import GITLAB_URL, download_f
from openpilot.frogpilot.common.frogpilot_utilities import delete_file
from openpilot.frogpilot.common.frogpilot_variables import DEFAULT_CLASSIC_MODEL, DEFAULT_MODEL, DEFAULT_TINYGRAD_MODEL, MODELS_PATH, params, params_default, params_memory
VERSION = "v15"
VERSION = "v14"
CANCEL_DOWNLOAD_PARAM = "CancelModelDownload"
DOWNLOAD_PROGRESS_PARAM = "ModelDownloadProgress"
frogpilot/common/frogpilot_variables.py
+2 -2
View File
@@ -58,8 +58,8 @@ DEFAULT_MODEL = "national-public-radio"
DEFAULT_MODEL_NAME = "National Public Radio 👀📡"
DEFAULT_MODEL_VERSION = "v6"
DEFAULT_TINYGRAD_MODEL = "tomb-raider"
DEFAULT_TINYGRAD_MODEL_NAME = "Tomb Raider 👀📡"
DEFAULT_TINYGRAD_MODEL = "filet-o-fish"
DEFAULT_TINYGRAD_MODEL_NAME = "Filet-O-Fish 👀📡"
DEFAULT_TINYGRAD_MODEL_VERSION = "v8"
EXCLUDED_KEYS = {
frogpilot/tinygrad_modeld/fill_model_msg.py
+9 -10
View File
@@ -56,7 +56,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
builder.rightProb = lane_line_probs[2]
def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
valid: bool) -> None:
@@ -71,8 +71,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
driving_model_data.frameIdExtra = vipc_frame_id_extra
driving_model_data.frameDropPerc = frame_drop_perc
driving_model_data.modelExecutionTime = model_execution_time
driving_model_data.action = action
driving_model_data.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
modelV2 = extended_msg.modelV2
modelV2.frameId = vipc_frame_id
@@ -90,17 +89,17 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# temporal pose
#temporal_pose = modelV2.temporalPose
#temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
#temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
#temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
#temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
temporal_pose = modelV2.temporalPose
temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
# poly path
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
# action
modelV2.action = action
# lateral planning
modelV2.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
# times at X_IDXS of edges and lines aren't used
LINE_T_IDXS: list[float] = []
frogpilot/tinygrad_modeld/models/dmonitoring_model_tinygrad.pkl
BIN
View File
Binary file not shown.
frogpilot/tinygrad_modeld/models/driving_policy_metadata.pkl
BIN
View File
Binary file not shown.
frogpilot/tinygrad_modeld/models/driving_policy_tinygrad.pkl
BIN
View File
Binary file not shown.
frogpilot/tinygrad_modeld/models/driving_vision_metadata.pkl
BIN
View File
Binary file not shown.
frogpilot/tinygrad_modeld/models/driving_vision_tinygrad.pkl
BIN
View File
Binary file not shown.
frogpilot/tinygrad_modeld/parse_model_outputs.py
+7 -6
View File
@@ -88,12 +88,6 @@ class Parser:
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
for k in ['lead_prob', 'lane_lines_prob']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
self.parse_binary_crossentropy('meta', outs)
return outs
@@ -101,10 +95,17 @@ class Parser:
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
if 'lat_planner_solution' in outs:
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
if 'desired_curvature' in outs:
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
for k in ['lead_prob', 'lane_lines_prob']:
self.parse_binary_crossentropy(k, outs)
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
return outs
frogpilot/tinygrad_modeld/tinygrad_modeld.py
+6 -38
View File
@@ -20,21 +20,19 @@ from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
from openpilot.common.swaglog import cloudlog
from openpilot.common.params import Params
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import config_realtime_process, DT_MDL
from openpilot.common.realtime import config_realtime_process
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.car.car_helpers import get_demo_car_params
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value, get_curvature_from_plan
from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import Parser
from openpilot.frogpilot.tinygrad_modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants, Plan
from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants
from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles
PROCESS_NAME = "frogpilot.tinygrad_modeld.tinygrad_modeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -43,30 +41,6 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl'
VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
LAT_SMOOTH_SECONDS = 0.3
LONG_SMOOTH_SECONDS = 0.3
MIN_LAT_CONTROL_SPEED = 0.3
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
plan = model_output['plan'][0]
desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0],
plan[:,Plan.ACCELERATION][:,0],
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]
if v_ego > MIN_LAT_CONTROL_SPEED:
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
else:
desired_curvature = prev_action.desiredCurvature
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
desiredAcceleration=float(desired_accel),
shouldStop=bool(should_stop))
class FrameMeta:
frame_id: int = 0
timestamp_sof: int = 0
@@ -174,7 +148,7 @@ class ModelState:
# TODO model only uses last value now
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
self.numpy_inputs['prev_desired_curv'][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs]
self.numpy_inputs['prev_desired_curv'][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
if SEND_RAW_PRED:
@@ -249,10 +223,7 @@ def main(demo=False):
cloudlog.info("tinygrad_modeld got CarParams: %s", CP.carName)
# TODO this needs more thought, use .2s extra for now to estimate other delays
# TODO Move smooth seconds to action function
lat_delay = CP.steerActuatorDelay + .2 + LAT_SMOOTH_SECONDS
long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
prev_action = log.ModelDataV2.Action()
steer_delay = CP.steerActuatorDelay + .2
DH = DesireHelper()
@@ -297,7 +268,7 @@ def main(demo=False):
is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
v_ego = max(sm["carState"].vEgo, 0.)
lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32)
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -340,10 +311,7 @@ def main(demo=False):
modelv2_send = messaging.new_message('modelV2')
drivingdata_send = messaging.new_message('drivingModelData')
posenet_send = messaging.new_message('cameraOdometry')
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
prev_action = action
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
selfdrive/controls/lib/drive_helpers.py
+1 -31
View File
@@ -1,10 +1,9 @@
import math
import numpy as np
from cereal import car, log
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.realtime import DT_CTRL, DT_MDL
from openpilot.common.realtime import DT_CTRL
# WARNING: this value was determined based on the model's training distribution,
# model predictions above this speed can be unpredictable
@@ -179,9 +178,6 @@ def apply_center_deadzone(error, deadzone):
def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def smooth_value(val, prev_val, tau, dt=DT_MDL):
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
return alpha * val + (1 - alpha) * prev_val
def clip_curvature(v_ego, prev_curvature, new_curvature, planner_curves):
if planner_curves:
@@ -212,29 +208,3 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
vel_err = 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_tomb_raider(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
v_target = np.interp(action_t, t_idxs, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
else:
v_target = 0.0
v_target_1sec = 0.0
a_target = 0.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/controls/lib/longitudinal_planner.py
+6 -23
View File
@@ -13,7 +13,7 @@ from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
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, LEAD_ACCEL_TAU
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_speed_error, get_accel_from_plan_tomb_raider
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_speed_error
from openpilot.common.swaglog import cloudlog
LON_MPC_STEP = 0.2 # first step is 0.2s
@@ -203,12 +203,8 @@ class LongitudinalPlanner:
throttle_prob = 1.0
return x, v, a, j, throttle_prob
def update(self, radarless_model, tomb_raider, sm, frogpilot_toggles):
if tomb_raider:
self.mpc.mode = 'acc'
self.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
else:
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
def update(self, radarless_model, sm, frogpilot_toggles):
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
if len(sm['carControl'].orientationNED) == 3:
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -298,7 +294,7 @@ class LongitudinalPlanner:
self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
def publish(self, classic_model, tomb_raider, sm, pm, frogpilot_toggles):
def publish(self, classic_model, sm, pm, frogpilot_toggles):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
@@ -318,25 +314,12 @@ class LongitudinalPlanner:
if classic_model:
a_target, should_stop = get_accel_from_plan_classic(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels, vEgoStopping=frogpilot_toggles.vEgoStopping)
elif tomb_raider:
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_tomb_raider(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
if self.mode == 'acc':
a_target = output_a_target_mpc
should_stop = output_should_stop_mpc
else:
a_target = min(output_a_target_mpc, output_a_target_e2e)
should_stop = output_should_stop_e2e or output_should_stop_mpc
else:
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
longitudinalPlan.aTarget = float(a_target)
longitudinalPlan.shouldStop = bool(should_stop)
longitudinalPlan.aTarget = a_target
longitudinalPlan.shouldStop = should_stop
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = self.allow_throttle
selfdrive/controls/plannerd.py
+2 -3
View File
@@ -39,13 +39,12 @@ def plannerd_thread():
classic_model = frogpilot_toggles.classic_model
radarless_model = frogpilot_toggles.radarless_model
tomb_raider = frogpilot_toggles.model == "tomb-raider"
while True:
sm.update()
if sm.updated['modelV2']:
longitudinal_planner.update(radarless_model, tomb_raider, sm, frogpilot_toggles)
longitudinal_planner.publish(classic_model, tomb_raider, sm, pm, frogpilot_toggles)
longitudinal_planner.update(radarless_model, sm, frogpilot_toggles)
longitudinal_planner.publish(classic_model, sm, pm, frogpilot_toggles)
publish_ui_plan(sm, pm, longitudinal_planner)
# Update FrogPilot parameters
selfdrive/ui/_spinner
BIN
View File
Binary file not shown.
selfdrive/ui/_text
BIN
View File
Binary file not shown.
selfdrive/ui/ui
BIN
View File
Binary file not shown.