mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-05 04:45:41 +08:00
Compare commits
15 Commits
master-dev
..
wgpu
| Author | SHA1 | Date | |
|---|---|---|---|
| 77d09a7a16 | |||
| f57f1bd19d | |||
| 2f1d422990 | |||
| 28fa87d547 | |||
| 2430eed759 | |||
| b8fa2c7535 | |||
| a39aee2c24 | |||
| cff8b7c1f0 | |||
| c6991ead50 | |||
| 3e950b95ca | |||
| a5fd13ec4d | |||
| 6e99dc3482 | |||
| 791d63cd81 | |||
| f166bfcdc5 | |||
| 418c8154be |
@@ -0,0 +1,39 @@
|
||||
name: prebuilt
|
||||
on:
|
||||
schedule:
|
||||
- cron: '0 * * * *'
|
||||
workflow_dispatch:
|
||||
|
||||
env:
|
||||
DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
|
||||
BUILD: release/ci/docker_build_sp.sh
|
||||
|
||||
jobs:
|
||||
build_prebuilt:
|
||||
name: build prebuilt
|
||||
runs-on: ubuntu-latest
|
||||
if: github.repository == 'sunnypilot/sunnypilot'
|
||||
env:
|
||||
PUSH_IMAGE: true
|
||||
permissions:
|
||||
checks: read
|
||||
contents: read
|
||||
packages: write
|
||||
steps:
|
||||
- name: Wait for green check mark
|
||||
if: ${{ github.event_name != 'workflow_dispatch' }}
|
||||
uses: lewagon/wait-on-check-action@ccfb013c15c8afb7bf2b7c028fb74dc5a068cccc
|
||||
with:
|
||||
ref: master
|
||||
wait-interval: 30
|
||||
running-workflow-name: 'build prebuilt'
|
||||
repo-token: ${{ secrets.GITHUB_TOKEN }}
|
||||
check-regexp: ^((?!.*(build master-ci|create badges).*).)*$
|
||||
- uses: actions/checkout@v6
|
||||
with:
|
||||
submodules: true
|
||||
- run: git lfs pull
|
||||
- name: Build and Push docker image
|
||||
run: |
|
||||
$DOCKER_LOGIN
|
||||
eval "$BUILD"
|
||||
+1
-1
Submodule opendbc_repo updated: 4c64e8a95b...d6b9c1adaa
@@ -69,8 +69,6 @@ struct LeadData {
|
||||
struct SelfdriveStateSP @0x81c2f05a394cf4af {
|
||||
mads @0 :ModularAssistiveDrivingSystem;
|
||||
intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement;
|
||||
buttonsPressed @2 :UInt16;
|
||||
buttonsReleaseToggle @3 :UInt16;
|
||||
|
||||
enum AudibleAlert {
|
||||
none @0;
|
||||
@@ -139,16 +137,10 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
eta @2 :UInt32;
|
||||
}
|
||||
|
||||
struct Chunk {
|
||||
fileName @0 :Text;
|
||||
sha256 @1 :Text;
|
||||
}
|
||||
|
||||
struct Artifact {
|
||||
fileName @0 :Text;
|
||||
downloadUri @1 :DownloadUri;
|
||||
downloadProgress @2 :DownloadProgress;
|
||||
chunks @3 :List(Chunk);
|
||||
}
|
||||
|
||||
struct Model {
|
||||
@@ -163,7 +155,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
policy @3;
|
||||
offPolicy @4;
|
||||
onPolicy @5;
|
||||
chunked @6;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -132,7 +132,6 @@ struct OnroadEvent @0xc4fa6047f024e718 {
|
||||
userBookmark @95;
|
||||
excessiveActuation @96;
|
||||
audioFeedback @97;
|
||||
|
||||
soundsUnavailableDEPRECATED @47;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -134,6 +134,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"UsbGpuPresent", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||
{"UsbGpuCompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
|
||||
{"Version", {PERSISTENT, STRING}},
|
||||
{"WgpuEnabled", {CLEAR_ON_MANAGER_START | DEVELOPMENT_ONLY, BOOL}},
|
||||
{"WgpuModelName", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | DEVELOPMENT_ONLY, STRING}},
|
||||
{"WgpuReady", {CLEAR_ON_MANAGER_START | DEVELOPMENT_ONLY, BOOL}},
|
||||
|
||||
// --- sunnypilot params --- //
|
||||
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
||||
@@ -222,7 +225,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TeslaCoopSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
|
||||
@@ -158,7 +158,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
def publish(self, sm, pm):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
||||
plan_send.valid = sm.all_checks()
|
||||
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'radarState'])
|
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan
|
||||
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
|
||||
@@ -29,19 +29,19 @@ def main():
|
||||
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
||||
'liveMapDataSP', 'carStateSP', 'selfdriveStateSP', gps_location_service],
|
||||
poll='modelV2', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
|
||||
'liveMapDataSP', 'carStateSP', gps_location_service],
|
||||
poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
longitudinal_planner.sla.update_buttons(sm['selfdriveStateSP'].buttonsReleaseToggle)
|
||||
longitudinal_planner.sla.update_car_state(sm['carState'])
|
||||
if sm.updated['modelV2']:
|
||||
longitudinal_planner.update(sm)
|
||||
longitudinal_planner.publish(sm, pm)
|
||||
|
||||
ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
|
||||
msg = messaging.new_message('driverAssistance')
|
||||
msg.valid = sm.all_checks()
|
||||
msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'liveParameters'])
|
||||
msg.driverAssistance.leftLaneDeparture = ldw.left
|
||||
msg.driverAssistance.rightLaneDeparture = ldw.right
|
||||
pm.send('driverAssistance', msg)
|
||||
|
||||
@@ -37,6 +37,9 @@ available = probe_devices()
|
||||
if 'CUDA' in available:
|
||||
tg_backend = 'CUDA'
|
||||
tg_flags = f'DEV={tg_backend}'
|
||||
elif 'METAL' in available:
|
||||
tg_backend = 'METAL'
|
||||
tg_flags = f'DEV={tg_backend} FLOAT16=1'
|
||||
elif 'QCOM' in available:
|
||||
tg_backend = 'QCOM'
|
||||
tg_flags = f'DEV={tg_backend} IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1'
|
||||
@@ -55,6 +58,7 @@ tg_devices = { # which device to put jit inputs to at runtime
|
||||
}
|
||||
|
||||
USBGPU = usbgpu_present() # or release # TODO always build big model on release
|
||||
WGPU = os.getenv('WGPU') == '1'
|
||||
if USBGPU:
|
||||
usbgpu_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0'
|
||||
# the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
|
||||
@@ -84,13 +88,13 @@ compile_modeld_script = [
|
||||
model_w, model_h = MEDMODEL_INPUT_SIZE
|
||||
frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
|
||||
|
||||
for usbgpu in [False, True] if USBGPU else [False]:
|
||||
for usbgpu in [False, True] if USBGPU or WGPU else [False]:
|
||||
target_pkl_path = File(modeld_pkl_path(usbgpu)).abspath
|
||||
# BIG_INTO_SMALL=1 builds the default target from the big model, e.g. to test it without a USB GPU
|
||||
file_prefix, cmd_flags = ('big_', usbgpu_tg_flags) if usbgpu else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
|
||||
file_prefix, cmd_flags = ('big_', usbgpu_tg_flags if USBGPU else tg_flags) if usbgpu else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
|
||||
driving_onnx_deps = get_existing_chunks(File(f"models/{file_prefix}driving_supercombo.onnx").abspath)
|
||||
camera_res_args = ' '.join(f'{cw}x{ch}' for cw, ch in CAMERA_CONFIGS)
|
||||
cmd = (f'{cmd_flags} {mac_brew_string} python3 {modeld_dir}/compile_modeld.py '
|
||||
cmd = (f'{cmd_flags} {mac_brew_string} {sys.executable} {modeld_dir}/compile_modeld.py '
|
||||
f'--model-size {model_w}x{model_h} '
|
||||
f'--camera-resolutions {camera_res_args} '
|
||||
f'--onnx {File(f"models/{file_prefix}driving_supercombo.onnx").abspath} '
|
||||
@@ -104,7 +108,7 @@ for usbgpu in [False, True] if USBGPU else [False]:
|
||||
tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(chunk_targets), chunker_file],
|
||||
[cmd, Action(do_chunk, " [CHUNK] $TARGET")],
|
||||
)
|
||||
if usbgpu:
|
||||
if usbgpu and USBGPU:
|
||||
lenv.SideEffect(usbgpu_lock, node)
|
||||
|
||||
# get model metadata
|
||||
|
||||
@@ -26,6 +26,7 @@ from openpilot.common.file_chunker import open_file_chunked, get_manifest_path
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices, load_oob
|
||||
from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
|
||||
from openpilot.tools.wgpu.zmq import WGPU_CAR_PARAMS, ZmqPubMaster, ZmqSubMaster, ZmqSubSocket
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
@@ -76,26 +77,44 @@ class FrameMeta:
|
||||
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
|
||||
|
||||
|
||||
def copy_nv12_to_venus(buf: VisionBuf, dst: np.ndarray, nv12: tuple[int, int, int, int]) -> None:
|
||||
stride, y_height, uv_height, _ = nv12
|
||||
if buf.stride < buf.width:
|
||||
raise ValueError(f"invalid VisionIPC stride {buf.stride} for width {buf.width}")
|
||||
|
||||
src = np.frombuffer(buf.data, dtype=np.uint8)
|
||||
src_size = buf.uv_offset + buf.stride * (buf.height // 2)
|
||||
if src.size < src_size:
|
||||
raise ValueError(f"VisionIPC buffer has {src.size} bytes, expected at least {src_size}")
|
||||
|
||||
dst[:stride * y_height].reshape(y_height, stride)[:buf.height, :buf.width] = \
|
||||
src[:buf.stride * buf.height].reshape(buf.height, buf.stride)[:, :buf.width]
|
||||
dst[stride * y_height:stride * (y_height + uv_height)].reshape(uv_height, stride)[:buf.height // 2, :buf.width] = \
|
||||
src[buf.uv_offset:src_size].reshape(buf.height // 2, buf.stride)[:, :buf.width]
|
||||
|
||||
|
||||
class ModelState(ModelStateBase):
|
||||
prev_desire: np.ndarray # for tracking the rising edge of the pulse
|
||||
|
||||
def __init__(self, cam_w: int, cam_h: int, usbgpu: bool):
|
||||
def __init__(self, cam_w: int, cam_h: int, usbgpu: bool, big_model: bool = False):
|
||||
ModelStateBase.__init__(self)
|
||||
self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS
|
||||
input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu)
|
||||
self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV']
|
||||
jits = load_oob(open_file_chunked(modeld_pkl_path(usbgpu)))
|
||||
jits = load_oob(open_file_chunked(modeld_pkl_path(usbgpu or big_model)))
|
||||
metadata = jits['metadata']
|
||||
self.input_shapes = metadata['input_shapes']
|
||||
self.vision_input_names = [k for k in self.input_shapes if 'img' in k]
|
||||
self.output_slices = metadata['output_slices']
|
||||
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
self.copy_vision_buffers = self.WARP_DEV.split(":")[0] == "METAL"
|
||||
|
||||
self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
|
||||
self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
|
||||
self.full_frames: dict[str, Tensor] = {}
|
||||
self._blob_cache: dict[tuple[str, int], Tensor] = {}
|
||||
self._vision_staging: dict[str, np.ndarray] = {}
|
||||
self.parser = Parser()
|
||||
self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')}
|
||||
self.run_policy = jits['run_policy']
|
||||
@@ -108,13 +127,22 @@ class ModelState(ModelStateBase):
|
||||
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
|
||||
inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None:
|
||||
for key in bufs.keys():
|
||||
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
|
||||
yuv_size = self.frame_buf_params[key][3]
|
||||
# There is a ringbuffer of imgs, just cache tensors pointing to all of them
|
||||
cache_key = (key, ptr)
|
||||
if cache_key not in self._blob_cache:
|
||||
self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
|
||||
self.full_frames[key] = self._blob_cache[cache_key]
|
||||
nv12 = self.frame_buf_params[key]
|
||||
yuv_size = nv12[3]
|
||||
if self.copy_vision_buffers:
|
||||
# VisionIPC supplies a CPU pointer. Metal's external_ptr expects an MTLBuffer object,
|
||||
# so wrap its NV12 pixels in the Venus layout expected by the compiled warp, then copy.
|
||||
if key not in self._vision_staging:
|
||||
self._vision_staging[key] = np.zeros(yuv_size, dtype=np.uint8)
|
||||
copy_nv12_to_venus(bufs[key], self._vision_staging[key], nv12)
|
||||
self.full_frames[key] = Tensor(self._vision_staging[key], device=self.WARP_DEV).realize()
|
||||
else:
|
||||
# There is a ringbuffer of imgs, just cache tensors pointing to all of them.
|
||||
frame = np.frombuffer(bufs[key].data, dtype=np.uint8, count=yuv_size)
|
||||
cache_key = (key, frame.ctypes.data)
|
||||
if cache_key not in self._blob_cache:
|
||||
self._blob_cache[cache_key] = Tensor.from_blob(frame.ctypes.data, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
|
||||
self.full_frames[key] = self._blob_cache[cache_key]
|
||||
|
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
||||
inputs['desire_pulse'][0] = 0
|
||||
@@ -139,18 +167,32 @@ class ModelState(ModelStateBase):
|
||||
return outputs_dict
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
def main(demo=False, remote_addr: str | None = None, big_model: bool = False):
|
||||
cloudlog.warning("modeld init")
|
||||
|
||||
_present = usbgpu_present()
|
||||
_compiled = os.path.isfile(get_manifest_path(modeld_pkl_path(usbgpu=True)))
|
||||
USBGPU = _present and _compiled
|
||||
if big_model and not _compiled:
|
||||
raise FileNotFoundError(f"big model is not compiled: {modeld_pkl_path(usbgpu=True)}")
|
||||
params = Params()
|
||||
params.put_bool("UsbGpuPresent", _present)
|
||||
params.put_bool("UsbGpuCompiled", _compiled)
|
||||
|
||||
config_realtime_process(7, 54)
|
||||
|
||||
remote_CP = None
|
||||
if remote_addr is not None:
|
||||
# Do not attach to VisionIPC until all startup prerequisites are available.
|
||||
# Otherwise its notification queue grows while waiting for the infrequent
|
||||
# bridged carParams message and reconnect starts tens of seconds behind.
|
||||
cloudlog.warning("waiting for remote carParams")
|
||||
car_params_socket = ZmqSubSocket(WGPU_CAR_PARAMS, remote_addr, conflate=True)
|
||||
raw_car_params = car_params_socket.receive()
|
||||
assert raw_car_params is not None
|
||||
remote_CP = messaging.log_from_bytes(raw_car_params, car.CarParams)
|
||||
cloudlog.info("modeld got remote CarParams: %s", remote_CP.brand)
|
||||
|
||||
# visionipc clients
|
||||
while True:
|
||||
available_streams = VisionIpcClient.available_streams("camerad", block=False)
|
||||
@@ -178,12 +220,14 @@ def main(demo=False):
|
||||
wait_usbgpu_link()
|
||||
st = time.monotonic()
|
||||
cloudlog.warning("loading model")
|
||||
model = ModelState(vipc_client_main.width, vipc_client_main.height, USBGPU)
|
||||
model = ModelState(vipc_client_main.width, vipc_client_main.height, USBGPU, big_model=big_model)
|
||||
cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
|
||||
|
||||
# messaging
|
||||
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
|
||||
output_services = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]
|
||||
pm = ZmqPubMaster(output_services) if remote_addr is not None else PubMaster(output_services)
|
||||
services = ["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]
|
||||
sm = ZmqSubMaster(services, remote_addr) if remote_addr is not None else SubMaster(services)
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
@@ -203,6 +247,9 @@ def main(demo=False):
|
||||
|
||||
if demo:
|
||||
CP = get_demo_car_params()
|
||||
elif remote_addr is not None:
|
||||
assert remote_CP is not None
|
||||
CP = remote_CP
|
||||
else:
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("modeld got CarParams: %s", CP.brand)
|
||||
@@ -332,7 +379,9 @@ if __name__ == "__main__":
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
|
||||
parser.add_argument('--remote', metavar='ADDRESS', help='Run against a remote device over the cereal ZMQ bridge.')
|
||||
parser.add_argument('--big-model', action='store_true', help='Use the locally compiled big driving model.')
|
||||
args = parser.parse_args()
|
||||
main(demo=args.demo)
|
||||
main(demo=args.demo, remote_addr=args.remote, big_model=args.big_model)
|
||||
except KeyboardInterrupt:
|
||||
cloudlog.warning("got SIGINT")
|
||||
|
||||
@@ -722,7 +722,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"),
|
||||
ET.PERMANENT: modeld_lagging_alert,
|
||||
},
|
||||
|
||||
# Besides predicting the path, lane lines and lead car data the model also
|
||||
# predicts the current velocity and rotation speed of the car. If the model is
|
||||
# very uncertain about the current velocity while the car is moving, this
|
||||
|
||||
@@ -30,7 +30,6 @@ from openpilot.sunnypilot import get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
|
||||
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
|
||||
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
@@ -112,6 +111,7 @@ class SelfdriveD(CruiseHelper):
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
|
||||
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
|
||||
self.wgpu_enabled = self.params.get_bool("WgpuEnabled")
|
||||
|
||||
car_recognized = self.CP.brand != 'mock'
|
||||
|
||||
@@ -178,7 +178,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
|
||||
|
||||
CruiseHelper.__init__(self, self.CP)
|
||||
self.button_state_tracker = ButtonStateTracker()
|
||||
|
||||
def update_events(self, CS):
|
||||
"""Compute onroadEvents from carState"""
|
||||
@@ -400,12 +399,12 @@ class SelfdriveD(CruiseHelper):
|
||||
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
|
||||
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
|
||||
if not self.sm.all_checks() and no_system_errors:
|
||||
if not self.sm.all_alive():
|
||||
self.events.add(EventName.commIssue)
|
||||
elif not self.sm.all_freq_ok():
|
||||
self.events.add(EventName.commIssueAvgFreq)
|
||||
else:
|
||||
self.events.add(EventName.commIssue)
|
||||
# if not self.sm.all_alive():
|
||||
# self.events.add(EventName.commIssue)
|
||||
# elif not self.sm.all_freq_ok():
|
||||
# self.events.add(EventName.commIssueAvgFreq)
|
||||
# else:
|
||||
# self.events.add(EventName.commIssue)
|
||||
|
||||
logs = {
|
||||
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
|
||||
@@ -418,13 +417,13 @@ class SelfdriveD(CruiseHelper):
|
||||
else:
|
||||
self.logged_comm_issue = None
|
||||
|
||||
if not self.CP.notCar:
|
||||
if not self.sm['livePose'].posenetOK:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
if not self.sm['livePose'].inputsOK:
|
||||
self.events.add(EventName.locationdTemporaryError)
|
||||
if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
self.events.add(EventName.paramsdTemporaryError)
|
||||
# if not self.CP.notCar:
|
||||
# if not self.sm['livePose'].posenetOK:
|
||||
# self.events.add(EventName.posenetInvalid)
|
||||
# if not self.sm['livePose'].inputsOK:
|
||||
# self.events.add(EventName.locationdTemporaryError)
|
||||
# if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
# self.events.add(EventName.paramsdTemporaryError)
|
||||
|
||||
# conservative HW alert. if the data or frequency are off, locationd will throw an error
|
||||
if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
|
||||
@@ -469,9 +468,9 @@ class SelfdriveD(CruiseHelper):
|
||||
self.distance_traveled += abs(CS.vEgo) * DT_CTRL
|
||||
|
||||
# TODO: fix simulator
|
||||
if not SIMULATION or REPLAY:
|
||||
if self.sm['modelV2'].frameDropPerc > 1:
|
||||
self.events.add(EventName.modeldLagging)
|
||||
# if not SIMULATION or REPLAY:
|
||||
# if self.sm['modelV2'].frameDropPerc > 1 and not self.wgpu_enabled:
|
||||
# self.events.add(EventName.modeldLagging)
|
||||
|
||||
# mute canBusMissing event if in Park, as it sometimes may trigger a false alarm with MADS in Paused state
|
||||
if CS.gearShifter == car.CarState.GearShifter.park and self.mads.enabled:
|
||||
@@ -599,8 +598,6 @@ class SelfdriveD(CruiseHelper):
|
||||
icbm.sendButton = self.icbm.cruise_button
|
||||
icbm.vTarget = self.icbm.v_target
|
||||
|
||||
self.button_state_tracker.publish(ss_sp)
|
||||
|
||||
self.pm.send('selfdriveStateSP', ss_sp_msg)
|
||||
|
||||
# onroadEventsSP - logged every second or on change
|
||||
@@ -620,7 +617,6 @@ class SelfdriveD(CruiseHelper):
|
||||
self.mads.update(CS)
|
||||
self.update_alerts(CS)
|
||||
|
||||
self.button_state_tracker.update(CS)
|
||||
self.publish_selfdriveState(CS)
|
||||
|
||||
self.CS_prev = CS
|
||||
@@ -630,6 +626,7 @@ class SelfdriveD(CruiseHelper):
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
|
||||
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
|
||||
self.wgpu_enabled = self.params.get_bool("WgpuEnabled")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
|
||||
self.personality = self.params.get("LongitudinalPersonality", return_default=True)
|
||||
|
||||
|
||||
@@ -248,8 +248,10 @@ class MiciHomeLayout(Widget):
|
||||
|
||||
# ***** Center-aligned bottom section icons *****
|
||||
self._experimental_icon.set_visible(ui_state.experimental_mode)
|
||||
self._egpu_icon.set_visible(ui_state.usbgpu and ui_state.usbgpu_compiled)
|
||||
self._egpu_icon_gray.set_visible(ui_state.usbgpu and not ui_state.usbgpu_compiled)
|
||||
wgpu_running = ui_state.wgpu_enabled and ui_state.sm.alive["modelV2"] and ui_state.sm.valid["modelV2"]
|
||||
self._egpu_icon.set_visible((ui_state.usbgpu and ui_state.usbgpu_compiled) or wgpu_running)
|
||||
self._egpu_icon_gray.set_visible((ui_state.usbgpu and not ui_state.usbgpu_compiled) or
|
||||
(ui_state.wgpu_ready and not wgpu_running))
|
||||
self._mic_icon.set_visible(ui_state.recording_audio)
|
||||
self._body_icon.set_visible(bool(ui_state.is_body))
|
||||
|
||||
|
||||
@@ -1,3 +1,5 @@
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from openpilot.cereal import log
|
||||
@@ -160,6 +162,20 @@ class AugmentedRoadView(CameraView):
|
||||
text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)
|
||||
self._model_status_key_labels = [
|
||||
UnifiedLabel("", 21, FontWeight.ROMAN, text_color=rl.Color(210, 210, 210, 220),
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE,
|
||||
wrap_text=False)
|
||||
for _ in range(6)
|
||||
]
|
||||
self._model_status_value_labels = [
|
||||
UnifiedLabel("", 22, FontWeight.SEMI_BOLD,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE,
|
||||
wrap_text=False)
|
||||
for _ in range(6)
|
||||
]
|
||||
|
||||
self._fade_texture = gui_app.texture("icons_mici/onroad/onroad_fade.png")
|
||||
|
||||
@@ -230,6 +246,8 @@ class AugmentedRoadView(CameraView):
|
||||
self._driver_state_renderer.set_position(self._rect.x + 16, self._rect.y + 10)
|
||||
self._driver_state_renderer.render()
|
||||
|
||||
self._render_model_status()
|
||||
|
||||
self._hud_renderer.set_can_draw_top_icons(alert_to_render is None)
|
||||
self._hud_renderer.set_wheel_critical_icon(alert_to_render is not None and not not_animating_out and
|
||||
alert_to_render.visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired)
|
||||
@@ -248,6 +266,58 @@ class AugmentedRoadView(CameraView):
|
||||
|
||||
self._bookmark_icon.render(self.rect)
|
||||
|
||||
def _render_model_status(self):
|
||||
model = ui_state.sm["modelV2"]
|
||||
if ui_state.sm.seen["modelV2"] and model.timestampEof:
|
||||
model_age_ms = max(0., (time.monotonic_ns() - model.timestampEof) / 1e6)
|
||||
execution_ms = max(0., model.modelExecutionTime * 1e3)
|
||||
io_queue_ms = max(0., model_age_ms - execution_ms)
|
||||
frame_drop = model.frameDropPerc
|
||||
age_text = f"{model_age_ms:.0f} ms"
|
||||
execution_text = f"{execution_ms:.0f} ms"
|
||||
io_queue_text = f"{io_queue_ms:.0f} ms"
|
||||
frame_drop_text = f"{frame_drop:.1f}%"
|
||||
else:
|
||||
model_age_ms = float("inf")
|
||||
age_text = execution_text = io_queue_text = "-- ms"
|
||||
frame_drop_text = "--%"
|
||||
|
||||
if ui_state.wgpu_enabled:
|
||||
source, model_name = "WGPU", ui_state.wgpu_model_name
|
||||
else:
|
||||
source, model_name = "LOCAL", "DEVICE"
|
||||
|
||||
if model_age_ms < 200:
|
||||
color = rl.Color(100, 255, 120, 230)
|
||||
elif model_age_ms < 500:
|
||||
color = rl.Color(255, 210, 80, 230)
|
||||
else:
|
||||
color = rl.Color(255, 120, 80, 230)
|
||||
|
||||
panel_w, panel_h = 250, 178
|
||||
status_rect = rl.Rectangle(self._content_rect.x + self._content_rect.width - panel_w - 18,
|
||||
self._content_rect.y + 18, panel_w, panel_h)
|
||||
rl.draw_rectangle_rounded(status_rect, 0.14, 8, rl.Color(0, 0, 0, 175))
|
||||
|
||||
rows = (
|
||||
("SOURCE", source),
|
||||
("MODEL", model_name),
|
||||
("AGE", age_text),
|
||||
("EXEC", execution_text),
|
||||
("IO/QUEUE", io_queue_text),
|
||||
("DROPPED", frame_drop_text),
|
||||
)
|
||||
row_h = 26
|
||||
for i, ((key, value), key_label, value_label) in enumerate(
|
||||
zip(rows, self._model_status_key_labels, self._model_status_value_labels, strict=True)
|
||||
):
|
||||
row_rect = rl.Rectangle(status_rect.x + 14, status_rect.y + 11 + i * row_h, status_rect.width - 28, row_h)
|
||||
key_label.set_text(key)
|
||||
key_label.render(rl.Rectangle(row_rect.x, row_rect.y, 105, row_rect.height))
|
||||
value_label.set_text(value)
|
||||
value_label.set_text_color(color)
|
||||
value_label.render(rl.Rectangle(row_rect.x + 108, row_rect.y, row_rect.width - 108, row_rect.height))
|
||||
|
||||
def _switch_stream_if_needed(self, sm):
|
||||
if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
|
||||
v_ego = sm['carState'].vEgo
|
||||
|
||||
@@ -43,7 +43,7 @@ class ModelsLayout(Widget):
|
||||
self._initialize_items()
|
||||
|
||||
self.clear_cache_item.action_item.set_value(f"{self.calculate_cache_size():.2f} MB")
|
||||
for ctrl, key in [(self.lane_turn_value_control, "LaneTurnValue"), (self.delay_control, "LagdToggleDelay"), (self.camera_offset, "CameraOffset")]:
|
||||
for ctrl, key in [(self.lane_turn_value_control, "LaneTurnValue"), (self.delay_control, "LagdToggleDelay")]:
|
||||
ctrl.action_item.set_value(int(float(ui_state.params.get(key, return_default=True)) * 100))
|
||||
|
||||
self._scroller = Scroller(self.items, line_separator=True, spacing=0)
|
||||
@@ -93,14 +93,9 @@ class ModelsLayout(Widget):
|
||||
|
||||
self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle")
|
||||
|
||||
self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35,
|
||||
tr("Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)"),
|
||||
1, None, True, "", style.BUTTON_ACTION_WIDTH, None, True,
|
||||
lambda v: f"{v / 100:.2f} m")
|
||||
|
||||
self.items = [self.current_model_item, self.cancel_download_item, self.supercombo_label, self.vision_label,
|
||||
self.policy_label, self.off_policy_label, self.on_policy_label, self.refresh_item, self.clear_cache_item,
|
||||
self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control, self.camera_offset]
|
||||
self.policy_label, self.off_policy_label, self.on_policy_label, self.refresh_item, self.clear_cache_item, self.lane_turn_desire_toggle,
|
||||
self.lane_turn_value_control, self.lagd_toggle, self.delay_control]
|
||||
|
||||
def _update_lagd_description(self, lagd_toggle: bool):
|
||||
desc = tr("Enable this for the car to learn and adapt its steering response time. Disable to use a fixed steering response time. " +
|
||||
@@ -237,7 +232,6 @@ class ModelsLayout(Widget):
|
||||
advanced_controls: bool = ui_state.params.get_bool("ShowAdvancedControls")
|
||||
turn_desire: bool = ui_state.params.get_bool("LaneTurnDesire")
|
||||
live_delay: bool = ui_state.params.get_bool("LagdToggle")
|
||||
camera_offset: bool = ui_state.params.get("ModelManager_ActiveBundle") is not None
|
||||
|
||||
self.lane_turn_desire_toggle.action_item.set_state(turn_desire)
|
||||
self.lane_turn_value_control.set_visible(turn_desire and advanced_controls)
|
||||
@@ -246,7 +240,6 @@ class ModelsLayout(Widget):
|
||||
new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100
|
||||
if self.lane_turn_value_control.action_item is not None and self.lane_turn_value_control.action_item.value_change_step != new_step:
|
||||
self.lane_turn_value_control.action_item.value_change_step = new_step
|
||||
self.camera_offset.set_visible(camera_offset)
|
||||
|
||||
self._update_lagd_description(live_delay)
|
||||
self.model_manager = ui_state.sm["modelManagerSP"]
|
||||
|
||||
+2
-5
@@ -7,7 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
from collections.abc import Callable
|
||||
import pyray as rl
|
||||
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake
|
||||
from openpilot.system.ui.lib.multilang import tr, tr_noop
|
||||
@@ -96,10 +96,7 @@ class MadsSettingsLayout(Widget):
|
||||
if brand == "rivian":
|
||||
return True
|
||||
elif brand == "tesla":
|
||||
if ui_state.CP_SP is None or not ui_state.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS:
|
||||
return True
|
||||
screen_button = int(ui_state.params.get("TeslaMadsScreenButton", return_default=True))
|
||||
return screen_button == MadsScreenButtonType.OFF
|
||||
return not (ui_state.CP_SP is not None and ui_state.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
|
||||
return False
|
||||
|
||||
def _update_steering_mode_description(self, button_index: int):
|
||||
|
||||
@@ -4,11 +4,10 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.vehicle.brands.base import BrandSettings
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import multiple_button_item_sp, toggle_item_sp
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
|
||||
|
||||
COOP_STEERING_MIN_KMH = 23
|
||||
OEM_STEERING_MIN_KMH = 48
|
||||
@@ -19,14 +18,7 @@ class TeslaSettings(BrandSettings):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.coop_steering_toggle = toggle_item_sp(tr("Cooperative Steering (Beta)"), "", param="TeslaCoopSteering")
|
||||
self.mads_screen_button = multiple_button_item_sp(
|
||||
title=lambda: tr("MADS Screen Activation"),
|
||||
description="",
|
||||
buttons=[lambda: tr("Off"), lambda: tr("3-Finger"), lambda: tr("4-Finger"), lambda: tr("5-Finger")],
|
||||
param="TeslaMadsScreenButton",
|
||||
inline=False,
|
||||
)
|
||||
self.items = [self.coop_steering_toggle, self.mads_screen_button]
|
||||
self.items = [self.coop_steering_toggle]
|
||||
|
||||
def update_settings(self):
|
||||
is_metric = ui_state.is_metric
|
||||
@@ -49,18 +41,3 @@ class TeslaSettings(BrandSettings):
|
||||
|
||||
self.coop_steering_toggle.set_description(coop_steering_desc)
|
||||
self.coop_steering_toggle.action_item.set_enabled(ui_state.is_offroad())
|
||||
|
||||
has_vehicle_bus = ui_state.CP_SP is not None and bool(ui_state.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
|
||||
self.mads_screen_button.set_visible(has_vehicle_bus)
|
||||
|
||||
mads_screen_button_desc = (
|
||||
f"{tr('Use a multi-finger press on the infotainment screen to toggle MADS.')} " +
|
||||
f"{tr('This allows the use of full MADS functionality when enabled.')}<br><br>" +
|
||||
f"{tr('Selecting a higher finger count may reduce accidental activations.')}<br><br>" +
|
||||
f"<b>{tr('Note: Setting this to Off will reset your MADS settings to default.')}</b>"
|
||||
)
|
||||
if not ui_state.is_offroad():
|
||||
mads_screen_button_disabled_msg = tr("Enable \"Always Offroad\" in Device panel, or turn vehicle off to change.")
|
||||
mads_screen_button_desc = f"<b>{mads_screen_button_disabled_msg}</b><br><br>{mads_screen_button_desc}"
|
||||
self.mads_screen_button.set_description(mads_screen_button_desc)
|
||||
self.mads_screen_button.action_item.set_enabled(ui_state.is_offroad())
|
||||
|
||||
@@ -82,6 +82,9 @@ class UIState(UIStateSP):
|
||||
self.experimental_mode: bool = self.params.get_bool("ExperimentalMode")
|
||||
self.usbgpu: bool = self.params.get_bool("UsbGpuPresent")
|
||||
self.usbgpu_compiled: bool = self.params.get_bool("UsbGpuCompiled")
|
||||
self.wgpu_enabled: bool = self.params.get_bool("WgpuEnabled")
|
||||
self.wgpu_model_name: str = self.params.get("WgpuModelName") or "UNKNOWN"
|
||||
self.wgpu_ready: bool = self.params.get_bool("WgpuReady")
|
||||
self.started: bool = False
|
||||
self.ignition: bool = False
|
||||
self.recording_audio: bool = False
|
||||
@@ -213,6 +216,9 @@ class UIState(UIStateSP):
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
self.usbgpu = self.params.get_bool("UsbGpuPresent")
|
||||
self.usbgpu_compiled = self.params.get_bool("UsbGpuCompiled")
|
||||
self.wgpu_enabled = self.params.get_bool("WgpuEnabled")
|
||||
self.wgpu_model_name = self.params.get("WgpuModelName") or "UNKNOWN"
|
||||
self.wgpu_ready = self.params.get_bool("WgpuReady")
|
||||
|
||||
UIStateSP.update_params(self)
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@ from openpilot.common.params import Params
|
||||
from opendbc.car import structs
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
|
||||
|
||||
MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla")
|
||||
@@ -21,20 +21,17 @@ class MadsSteeringModeOnBrake:
|
||||
DISENGAGE = 2
|
||||
|
||||
|
||||
def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params) -> bool:
|
||||
def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool:
|
||||
if CP.brand == 'rivian':
|
||||
return True
|
||||
if CP.brand == 'tesla':
|
||||
if not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS:
|
||||
return True
|
||||
screen_button = int(params.get("TeslaMadsScreenButton", return_default=True))
|
||||
return screen_button == MadsScreenButtonType.OFF
|
||||
return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
|
||||
if get_mads_limited_brands(CP, CP_SP, params):
|
||||
if get_mads_limited_brands(CP, CP_SP):
|
||||
return MadsSteeringModeOnBrake.DISENGAGE
|
||||
|
||||
return params.get("MadsSteeringMode", return_default=True)
|
||||
@@ -66,7 +63,7 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p
|
||||
# MADS is currently partially supported for these platforms due to lack of consistent states to engage controls
|
||||
# Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms
|
||||
# TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling
|
||||
mads_partial_support = get_mads_limited_brands(CP, CP_SP, params)
|
||||
mads_partial_support = get_mads_limited_brands(CP, CP_SP)
|
||||
if mads_partial_support:
|
||||
params.put("MadsSteeringMode", 2, block=True)
|
||||
params.put_bool("MadsUnifiedEngagementMode", True, block=True)
|
||||
|
||||
@@ -13,7 +13,7 @@ from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param
|
||||
from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
|
||||
State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
||||
EventName = log.OnroadEvent.EventName
|
||||
@@ -38,12 +38,6 @@ def make_panda_state(mocker, controls_allowed_lateral=True):
|
||||
return ps
|
||||
|
||||
|
||||
def make_params_mock(mocker, values):
|
||||
params = mocker.MagicMock()
|
||||
params.get = mocker.MagicMock(side_effect=lambda k, **kwargs: values[k])
|
||||
return params
|
||||
|
||||
|
||||
def make_mads(mocker, steering_mode):
|
||||
sd = mocker.MagicMock()
|
||||
sd.CP = structs.CarParams()
|
||||
@@ -229,27 +223,15 @@ class TestBrandSteeringModeRestrictions:
|
||||
params = mocker.MagicMock()
|
||||
assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.DISENGAGE
|
||||
|
||||
@pytest.mark.parametrize("screen_button", [MadsScreenButtonType.THREE_FINGER,
|
||||
MadsScreenButtonType.FOUR_FINGER,
|
||||
MadsScreenButtonType.FIVE_FINGER])
|
||||
def test_tesla_with_vehicle_bus_uses_param(self, mocker, screen_button):
|
||||
def test_tesla_with_vehicle_bus_uses_param(self, mocker):
|
||||
CP = structs.CarParams()
|
||||
CP.brand = "tesla"
|
||||
CP_SP = structs.CarParamsSP()
|
||||
CP_SP.flags = TeslaFlagsSP.HAS_VEHICLE_BUS
|
||||
params = make_params_mock(mocker, {"TeslaMadsScreenButton": screen_button,
|
||||
"MadsSteeringMode": MadsSteeringModeOnBrake.REMAIN_ACTIVE})
|
||||
params = mocker.MagicMock()
|
||||
params.get = mocker.MagicMock(return_value=MadsSteeringModeOnBrake.REMAIN_ACTIVE)
|
||||
assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.REMAIN_ACTIVE
|
||||
|
||||
def test_tesla_with_vehicle_bus_screen_button_off_forced_to_disengage(self, mocker):
|
||||
CP = structs.CarParams()
|
||||
CP.brand = "tesla"
|
||||
CP_SP = structs.CarParamsSP()
|
||||
CP_SP.flags = TeslaFlagsSP.HAS_VEHICLE_BUS
|
||||
params = make_params_mock(mocker, {"TeslaMadsScreenButton": MadsScreenButtonType.OFF,
|
||||
"MadsSteeringMode": MadsSteeringModeOnBrake.REMAIN_ACTIVE})
|
||||
assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.DISENGAGE
|
||||
|
||||
@pytest.mark.parametrize("brand", ["hyundai", "toyota", "honda", "gm"])
|
||||
def test_other_brands_use_param(self, mocker, brand):
|
||||
CP = structs.CarParams()
|
||||
|
||||
@@ -10,291 +10,471 @@ import argparse
|
||||
import os
|
||||
import pickle
|
||||
import time
|
||||
from collections import defaultdict
|
||||
from functools import partial
|
||||
from collections import defaultdict
|
||||
|
||||
import numpy as np
|
||||
os.environ['GMMU'] = '0'
|
||||
|
||||
def _patch_tinygrad_fetch_fw():
|
||||
import hashlib
|
||||
import pathlib
|
||||
import zstandard
|
||||
from tinygrad import helpers
|
||||
_orig_fetch_fw = helpers.fetch_fw
|
||||
def fetch_fw(path, name, sha256):
|
||||
p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
|
||||
if p.is_file():
|
||||
blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
|
||||
if hashlib.sha256(blob).hexdigest() == sha256:
|
||||
return blob
|
||||
return _orig_fetch_fw(path, name, sha256)
|
||||
helpers.fetch_fw = fetch_fw
|
||||
_patch_tinygrad_fetch_fw()
|
||||
|
||||
from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare, sample_desire, sample_skip, shift_and_sample
|
||||
from tinygrad import dtypes
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.device import Device
|
||||
from tinygrad.engine.jit import TinyJit
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
from openpilot.selfdrive.modeld.compile_modeld import (
|
||||
NV12Frame, make_frame_prepare,
|
||||
shift_and_sample, sample_skip, sample_desire,
|
||||
)
|
||||
|
||||
MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
|
||||
|
||||
|
||||
def _detect_desire_key(shapes: dict) -> str | None:
|
||||
return next((key for key in shapes if key.startswith('desire')), None)
|
||||
def _detect_desire_key(policy_input_shapes):
|
||||
for k in policy_input_shapes:
|
||||
if k.startswith('desire'):
|
||||
return k
|
||||
return None
|
||||
|
||||
|
||||
def _detect_vision_keys(shapes: dict) -> tuple[str | None, str | None]:
|
||||
img_keys = sorted(key for key in shapes if 'img' in key)
|
||||
return (
|
||||
next((key for key in img_keys if 'big' not in key), None),
|
||||
next((key for key in img_keys if 'big' in key), None)
|
||||
)
|
||||
def _detect_vision_keys(vision_input_shapes):
|
||||
img_keys = sorted([k for k in vision_input_shapes if 'img' in k])
|
||||
road_key = next((k for k in img_keys if 'big' not in k), None)
|
||||
wide_key = next((k for k in img_keys if 'big' in k), None)
|
||||
if road_key is None or wide_key is None:
|
||||
raise ValueError(f"Cannot determine road/wide image keys from {list(vision_input_shapes.keys())}")
|
||||
return road_key, wide_key
|
||||
|
||||
|
||||
def derive_frame_skip(vision_input_shapes: dict, policy_input_shapes: dict) -> int:
|
||||
features_buffer = policy_input_shapes.get('features_buffer')
|
||||
return 1 if not features_buffer or features_buffer[1] >= 99 else 4
|
||||
def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
|
||||
road_key, _ = _detect_vision_keys(vision_input_shapes)
|
||||
img = vision_input_shapes[road_key]
|
||||
n_frames = img[1] // 6
|
||||
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
|
||||
|
||||
fb = policy_input_shapes['features_buffer']
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
dp = policy_input_shapes[desire_key]
|
||||
tc = policy_input_shapes.get('traffic_convention', (1, 2))
|
||||
|
||||
def generate_queues_and_npy(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
|
||||
road_key, _ = _detect_vision_keys(input_shapes)
|
||||
if not road_key:
|
||||
raise ValueError("Vision road key missing from input shapes.")
|
||||
|
||||
img_shape = input_shapes[road_key]
|
||||
n_frames = img_shape[1] // 6
|
||||
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
|
||||
|
||||
desire_key = _detect_desire_key(input_shapes)
|
||||
if not desire_key:
|
||||
raise ValueError("Desire key missing from input shapes.")
|
||||
|
||||
desire_shape = input_shapes[desire_key]
|
||||
features_buffer = input_shapes.get('features_buffer')
|
||||
|
||||
npy_arrays = {
|
||||
'desire': np.zeros(desire_shape[2], dtype=np.float32),
|
||||
npy = {
|
||||
'desire': np.zeros(dp[2], dtype=np.float32),
|
||||
'traffic_convention': np.zeros(tc, dtype=np.float32),
|
||||
'tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
'big_tfm': np.zeros((3, 3), dtype=np.float32)
|
||||
'big_tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
}
|
||||
|
||||
for key, shape in input_shapes.items():
|
||||
if key not in npy_arrays and 'img' not in key and key not in ('features_buffer', desire_key):
|
||||
npy_arrays[key] = np.zeros(shape, dtype=np.float32)
|
||||
handled = {'features_buffer', desire_key, 'traffic_convention'}
|
||||
for key, shape in policy_input_shapes.items():
|
||||
if key in handled:
|
||||
continue
|
||||
npy[key] = np.zeros(shape, dtype=np.float32)
|
||||
|
||||
queues = {
|
||||
input_queues = {
|
||||
'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
|
||||
'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
|
||||
'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
|
||||
dtype=np.float32), device=device).contiguous().realize()
|
||||
'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
|
||||
'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
|
||||
**{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
|
||||
}
|
||||
|
||||
if features_buffer:
|
||||
queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
|
||||
dtype=np.float32), device=device).contiguous().realize()
|
||||
|
||||
queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items()})
|
||||
return queues, npy_arrays
|
||||
return input_queues, npy
|
||||
|
||||
|
||||
def make_split_input_queues(vision_input_shapes: dict, policy_input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
|
||||
return generate_queues_and_npy({**vision_input_shapes, **policy_input_shapes}, frame_skip, device)
|
||||
|
||||
|
||||
def make_supercombo_input_queues(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
|
||||
return generate_queues_and_npy(input_shapes, frame_skip, device)
|
||||
|
||||
|
||||
def create_jit_runner(vision_runner, policy_runners: list, nv12: NV12Frame, model_size: tuple[int, int],
|
||||
features_slice: slice, frame_skip: int, input_shapes: dict, prepare_only: bool):
|
||||
frame_prepare = make_frame_prepare(nv12, *model_size)
|
||||
def make_run_split_policy(vision_runner, policy_runner, nv12: NV12Frame, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only=False):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
|
||||
desire_key = _detect_desire_key(input_shapes)
|
||||
road_key, wide_key = _detect_vision_keys(input_shapes)
|
||||
def run_policy(img_q, big_img_q, feat_q, desire_q, desire, traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
|
||||
npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
|
||||
desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
|
||||
extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
|
||||
Tensor.realize(*npy_tensors, *extra_device.values())
|
||||
tfm, big_tfm, desire, traffic_convention = npy_tensors
|
||||
|
||||
if not desire_key or not road_key or not wide_key:
|
||||
raise ValueError("Missing required vision or desire keys in input shapes.")
|
||||
|
||||
extra_keys = [key for key in input_shapes if key not in (desire_key, 'features_buffer', 'traffic_convention') and 'img' not in key]
|
||||
|
||||
def runner(img_q, big_img_q, feat_q, frame, big_frame, tfm, big_tfm, **kwargs):
|
||||
desire_q = kwargs['desire_q']
|
||||
desire = kwargs['desire']
|
||||
traffic_convention = kwargs.get('traffic_convention')
|
||||
|
||||
npys = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT), desire.to(Device.DEFAULT)]
|
||||
if traffic_convention is not None:
|
||||
npys.append(traffic_convention.to(Device.DEFAULT))
|
||||
|
||||
extra_tensors = {key: kwargs[key].to(Device.DEFAULT) for key in extra_keys if key in kwargs}
|
||||
Tensor.realize(*npys, *extra_tensors.values())
|
||||
|
||||
tfm_dev, big_tfm_dev, desire_dev = npys[:3]
|
||||
traffic_conv_dev = npys[3] if traffic_convention is not None else None
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm_dev).unsqueeze(0), sample_skip_fn).realize()
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm_dev).unsqueeze(0), sample_skip_fn).realize()
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
|
||||
|
||||
if prepare_only:
|
||||
return img, big_img
|
||||
|
||||
desire_buf = shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn).realize()
|
||||
inputs = {desire_key: desire_buf, **extra_tensors}
|
||||
vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
|
||||
|
||||
if traffic_conv_dev is not None:
|
||||
inputs['traffic_convention'] = traffic_conv_dev
|
||||
new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
|
||||
feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
|
||||
if vision_runner:
|
||||
vision_out_cast = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast('float32').realize()
|
||||
new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
|
||||
inputs['features_buffer'] = shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
|
||||
policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32').realize() for pol_runner in policy_runners]
|
||||
return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])
|
||||
inputs.update({road_key: img, wide_key: big_img, 'features_buffer': sample_skip_fn(feat_q)})
|
||||
policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32').realize()
|
||||
new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
|
||||
shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
|
||||
return policy_out
|
||||
inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
|
||||
policy_out = next(iter(policy_runner(inputs).values())).cast('float32')
|
||||
|
||||
return runner
|
||||
return vision_out, policy_out
|
||||
return run_policy
|
||||
|
||||
|
||||
def compile_and_warmup(nv12: NV12Frame, model_size: tuple[int, int], prepare_only: bool, frame_skip: int, vision_runner, policy_runners: list, metadata: dict):
|
||||
print(f"Compiling combined JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
def compile_split_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runner, vision_metadata, policy_metadata):
|
||||
print(f"Compiling combined policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
all_shapes = {key: value for meta in metadata.values() for key, value in meta['input_shapes'].items()}
|
||||
vision_features_slice = vision_metadata['output_slices']['hidden_state']
|
||||
vision_input_shapes = vision_metadata['input_shapes']
|
||||
policy_input_shapes = policy_metadata['input_shapes']
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
|
||||
vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
|
||||
|
||||
feat_meta = metadata.get('vision') or metadata.get('model') or metadata.get('policy')
|
||||
if not feat_meta:
|
||||
raise ValueError("Could not find vision, model, or policy metadata.")
|
||||
_run = make_run_split_policy(vision_runner, policy_runner, nv12, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only)
|
||||
run_policy_jit = TinyJit(_run, prune=True)
|
||||
|
||||
features_slice = feat_meta['output_slices']['hidden_state']
|
||||
WARP_DEV = 'CPU' if "USBGPU" in os.environ else Device.DEFAULT
|
||||
SEED = 42
|
||||
|
||||
run_func = create_jit_runner(vision_runner, policy_runners, nv12, model_size, features_slice, frame_skip, all_shapes, prepare_only)
|
||||
run_jit = TinyJit(run_func, prune=True)
|
||||
queues, npy_arrays = generate_queues_and_npy(all_shapes, frame_skip, Device.DEFAULT)
|
||||
def random_inputs_run_fn(fn, seed, test_val=None, test_buffers=None, expect_match=True):
|
||||
input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
|
||||
rng = np.random.default_rng(seed)
|
||||
Tensor.manual_seed(seed)
|
||||
|
||||
testing = test_val is not None or test_buffers is not None
|
||||
n_runs = 1 if testing else 3
|
||||
|
||||
for i in range(n_runs):
|
||||
frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
for v in npy.values():
|
||||
v[:] = rng.standard_normal(v.shape).astype(v.dtype)
|
||||
Device.default.synchronize()
|
||||
st = time.perf_counter()
|
||||
outs = fn(**input_queues, frame=frame, big_frame=big_frame)
|
||||
mt = time.perf_counter()
|
||||
Device.default.synchronize()
|
||||
et = time.perf_counter()
|
||||
print(f" [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
|
||||
|
||||
if i == 0:
|
||||
val = [np.copy(v.numpy()) for v in outs]
|
||||
buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
|
||||
|
||||
if test_val is not None:
|
||||
match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
|
||||
assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
|
||||
if test_buffers is not None:
|
||||
match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
|
||||
assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
|
||||
return fn, val, buffers
|
||||
|
||||
print('capture + replay')
|
||||
run_policy_jit, test_val, test_buffers = random_inputs_run_fn(run_policy_jit, SEED)
|
||||
|
||||
print('pickle round trip')
|
||||
run_policy_jit = pickle.loads(pickle.dumps(run_policy_jit))
|
||||
random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers, expect_match=True)
|
||||
random_inputs_run_fn(run_policy_jit, SEED+1, test_val, test_buffers, expect_match=False)
|
||||
return run_policy_jit
|
||||
|
||||
|
||||
def derive_frame_skip(vision_input_shapes, policy_input_shapes):
|
||||
fb = policy_input_shapes.get('features_buffer')
|
||||
if fb is None:
|
||||
return 1
|
||||
fb_history = fb[1]
|
||||
if fb_history >= 99:
|
||||
return 1
|
||||
return 4
|
||||
|
||||
|
||||
def make_supercombo_input_queues(input_shapes, frame_skip, device):
|
||||
img_shape = input_shapes.get('img', input_shapes.get('input_imgs'))
|
||||
if img_shape is None:
|
||||
raise ValueError("No img input found in model shapes")
|
||||
|
||||
n_frames = img_shape[1] // 6
|
||||
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
|
||||
|
||||
numpy_keys = {}
|
||||
queue_keys = {}
|
||||
|
||||
for key, shape in input_shapes.items():
|
||||
if 'img' in key:
|
||||
continue
|
||||
if len(shape) == 3 and shape[1] > 1:
|
||||
if key.startswith('desire'):
|
||||
numpy_keys[key] = np.zeros(shape[2], dtype=np.float32)
|
||||
queue_keys[f'{key}_q'] = Tensor(
|
||||
np.zeros((frame_skip * shape[1], shape[0], shape[2]), dtype=np.float32),
|
||||
device=device).contiguous().realize()
|
||||
elif key == 'features_buffer':
|
||||
queue_keys['feat_q'] = Tensor(
|
||||
np.zeros((frame_skip * (shape[1] - 1) + 1, shape[0], shape[2]), dtype=np.float32),
|
||||
device=device).contiguous().realize()
|
||||
else:
|
||||
numpy_keys[key] = np.zeros(shape, dtype=np.float32)
|
||||
elif len(shape) == 2:
|
||||
numpy_keys[key] = np.zeros(shape, dtype=np.float32)
|
||||
|
||||
if 'traffic_convention' not in numpy_keys:
|
||||
tc_shape = input_shapes.get('traffic_convention', (1, 2))
|
||||
numpy_keys['traffic_convention'] = np.zeros(tc_shape, dtype=np.float32)
|
||||
|
||||
numpy_keys['tfm'] = np.zeros((3, 3), dtype=np.float32)
|
||||
numpy_keys['big_tfm'] = np.zeros((3, 3), dtype=np.float32)
|
||||
|
||||
input_queues = {
|
||||
'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
|
||||
'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
|
||||
**queue_keys,
|
||||
**{k: Tensor(v, device='NPY').realize() for k, v in numpy_keys.items()},
|
||||
}
|
||||
return input_queues, numpy_keys
|
||||
|
||||
|
||||
def make_run_supercombo(model_runner, nv12: NV12Frame, model_w, model_h,
|
||||
features_slice, frame_skip, input_shapes, prepare_only=False):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
|
||||
desire_key = _detect_desire_key(input_shapes)
|
||||
if desire_key is None:
|
||||
raise ValueError(f"No desire* key found in input_shapes: {list(input_shapes.keys())}")
|
||||
road_img_key, wide_img_key = _detect_vision_keys(input_shapes)
|
||||
extra_policy_keys = [k for k in input_shapes
|
||||
if k not in (desire_key, 'features_buffer', 'traffic_convention')
|
||||
and 'img' not in k]
|
||||
|
||||
def run_supercombo(img_q, big_img_q, feat_q, desire_q,
|
||||
frame, big_frame, **kwargs):
|
||||
desire = kwargs.get(desire_key)
|
||||
traffic_convention = kwargs.get('traffic_convention')
|
||||
tfm = kwargs['tfm']
|
||||
big_tfm = kwargs['big_tfm']
|
||||
|
||||
tfm = tfm.to(Device.DEFAULT)
|
||||
big_tfm = big_tfm.to(Device.DEFAULT)
|
||||
desire = desire.to(Device.DEFAULT)
|
||||
traffic_convention = traffic_convention.to(Device.DEFAULT)
|
||||
Tensor.realize(tfm, big_tfm, desire, traffic_convention)
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
|
||||
|
||||
if prepare_only:
|
||||
return img, big_img
|
||||
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
feat_buf = sample_skip_fn(feat_q)
|
||||
|
||||
inputs = {road_img_key: img, wide_img_key: big_img,
|
||||
desire_key: desire_buf, 'features_buffer': feat_buf,
|
||||
'traffic_convention': traffic_convention}
|
||||
for k in extra_policy_keys:
|
||||
if k in kwargs:
|
||||
inputs[k] = kwargs[k].to(Device.DEFAULT)
|
||||
|
||||
model_out = next(iter(model_runner(inputs).values())).cast('float32')
|
||||
|
||||
new_feat = model_out[:, features_slice].reshape(1, -1).unsqueeze(0)
|
||||
shift_and_sample(feat_q, new_feat, sample_skip_fn)
|
||||
|
||||
return model_out
|
||||
|
||||
return run_supercombo
|
||||
|
||||
|
||||
def make_run_vision_multi_policy(vision_runner, policy_runners, nv12: NV12Frame, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only=False):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
|
||||
def run_multi_policy(img_q, big_img_q, feat_q, desire_q, desire,
|
||||
traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
|
||||
npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
|
||||
desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
|
||||
extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
|
||||
Tensor.realize(*npy_tensors, *extra_device.values())
|
||||
tfm, big_tfm, desire, traffic_convention = npy_tensors
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
|
||||
|
||||
if prepare_only:
|
||||
return img, big_img
|
||||
|
||||
vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
|
||||
|
||||
new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
|
||||
feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
|
||||
inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
|
||||
|
||||
policy_outputs = []
|
||||
for runner in policy_runners:
|
||||
policy_out = next(iter(runner(inputs).values())).cast('float32')
|
||||
policy_outputs.append(policy_out)
|
||||
|
||||
return (vision_out, *policy_outputs)
|
||||
|
||||
return run_multi_policy
|
||||
|
||||
|
||||
def _warmup_and_serialize(run_jit, input_queues, npy, nv12):
|
||||
for i in range(3):
|
||||
rng = np.random.default_rng(42 + i)
|
||||
frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
|
||||
big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
|
||||
for arr in npy_arrays.values():
|
||||
arr[:] = rng.standard_normal(arr.shape).astype(arr.dtype)
|
||||
|
||||
frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
for v in npy.values():
|
||||
v[:] = rng.standard_normal(v.shape).astype(v.dtype)
|
||||
Device.default.synchronize()
|
||||
start_time = time.perf_counter()
|
||||
run_jit(**queues, frame=frame, big_frame=big_frame)
|
||||
mid_time = time.perf_counter()
|
||||
st = time.perf_counter()
|
||||
run_jit(**input_queues, frame=frame, big_frame=big_frame)
|
||||
mt = time.perf_counter()
|
||||
Device.default.synchronize()
|
||||
print(f" [{i + 1}/3] enqueue {(mid_time - start_time) * 1e3:6.2f} ms -- total {(time.perf_counter() - start_time) * 1e3:6.2f} ms")
|
||||
|
||||
return pickle.loads(pickle.dumps(run_jit)) if not prepare_only else run_jit
|
||||
et = time.perf_counter()
|
||||
print(f" [{i + 1}/3] enqueue {(mt - st) * 1e3:6.2f} ms -- total {(et - st) * 1e3:6.2f} ms")
|
||||
return pickle.loads(pickle.dumps(run_jit))
|
||||
|
||||
|
||||
def _parse_size(size_str: str) -> tuple[int, int]:
|
||||
width, height = size_str.lower().split('x')
|
||||
return int(width), int(height)
|
||||
def compile_supercombo(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
|
||||
model_runner, metadata):
|
||||
print(f"Compiling combined supercombo JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
features_slice = metadata['output_slices']['hidden_state']
|
||||
input_shapes = metadata['input_shapes']
|
||||
|
||||
_run = make_run_supercombo(model_runner, nv12, model_w, model_h,
|
||||
features_slice, frame_skip, input_shapes, prepare_only)
|
||||
run_jit = TinyJit(_run, prune=True)
|
||||
|
||||
input_queues, npy = make_supercombo_input_queues(input_shapes, frame_skip, Device.DEFAULT)
|
||||
|
||||
run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
|
||||
return run_jit
|
||||
|
||||
|
||||
def read_file_chunked_to_shm(path):
|
||||
if not path:
|
||||
return None
|
||||
import atexit
|
||||
import shutil
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
|
||||
atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
|
||||
with open(shm_path, 'wb') as dst, open_file_chunked(path) as src:
|
||||
shutil.copyfileobj(src, dst)
|
||||
return shm_path
|
||||
def compile_multi_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runners, vision_metadata, policy_metadata):
|
||||
print(f"Compiling combined multi-policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
vision_features_slice = vision_metadata['output_slices']['hidden_state']
|
||||
vision_input_shapes = vision_metadata['input_shapes']
|
||||
policy_input_shapes = policy_metadata['input_shapes']
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
|
||||
vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
|
||||
|
||||
_run = make_run_vision_multi_policy(vision_runner, policy_runners, nv12, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only)
|
||||
run_jit = TinyJit(_run, prune=True)
|
||||
|
||||
input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
|
||||
|
||||
run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
|
||||
return run_jit
|
||||
|
||||
|
||||
def _compile_for_resolutions(camera_resolutions: list, model_size: tuple[int, int], frame_skip: int,
|
||||
vision_runner, policy_runners: list, metadata: dict) -> dict:
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
return {
|
||||
(cam_w, cam_h): {
|
||||
name: compile_and_warmup(NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)), model_size, prepare_only,
|
||||
frame_skip, vision_runner, policy_runners, metadata)
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
for cam_w, cam_h in camera_resolutions
|
||||
}
|
||||
|
||||
|
||||
def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
|
||||
runners, keys = [], []
|
||||
for name, onnx_arg in [('policy', args.policy_onnx), ('off_policy', args.off_policy_onnx), ('on_policy', args.on_policy_onnx)]:
|
||||
if onnx_arg:
|
||||
runners.append(OnnxRunner(onnx_arg))
|
||||
keys.append(name)
|
||||
return runners, keys
|
||||
def _parse_size(s):
|
||||
w, h = s.lower().split('x')
|
||||
return int(w), int(h)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
|
||||
from tinygrad.nn.onnx import OnnxRunner
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
|
||||
|
||||
parser = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
|
||||
parser.add_argument('--model-type', choices=MODEL_TYPES, required=True)
|
||||
parser.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
|
||||
parser.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
|
||||
parser.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
|
||||
parser.add_argument('--output', required=True)
|
||||
p = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
|
||||
p.add_argument('--model-type', choices=MODEL_TYPES, required=True)
|
||||
p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
|
||||
p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
|
||||
p.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
|
||||
p.add_argument('--output', required=True)
|
||||
|
||||
parser.add_argument('--vision-onnx', help='vision ONNX (for split models)')
|
||||
parser.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
|
||||
parser.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
|
||||
parser.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
|
||||
parser.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
|
||||
p.add_argument('--vision-onnx', help='vision ONNX (for split models)')
|
||||
p.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
|
||||
p.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
|
||||
p.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
|
||||
p.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
|
||||
|
||||
args = parser.parse_args()
|
||||
output_data = defaultdict(dict)
|
||||
|
||||
args.vision_onnx = read_file_chunked_to_shm(args.vision_onnx)
|
||||
args.policy_onnx = read_file_chunked_to_shm(args.policy_onnx)
|
||||
args.off_policy_onnx = read_file_chunked_to_shm(args.off_policy_onnx)
|
||||
args.on_policy_onnx = read_file_chunked_to_shm(args.on_policy_onnx)
|
||||
args.supercombo_onnx = read_file_chunked_to_shm(args.supercombo_onnx)
|
||||
|
||||
vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
|
||||
args = p.parse_args()
|
||||
out = defaultdict(dict)
|
||||
|
||||
if args.model_type == 'vision_policy':
|
||||
assert vision_runner and args.policy_onnx
|
||||
policy_runners = [OnnxRunner(args.policy_onnx)]
|
||||
output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
|
||||
assert args.vision_onnx and args.policy_onnx
|
||||
vision_runner = OnnxRunner(args.vision_onnx)
|
||||
policy_runner = OnnxRunner(args.policy_onnx)
|
||||
out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
|
||||
out['metadata']['policy'] = make_metadata_dict(args.policy_onnx)
|
||||
|
||||
frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
|
||||
out['metadata']['policy']['input_shapes'])
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
model_w, model_h = args.model_size
|
||||
out[(cam_w, cam_h)] = {
|
||||
name: compile_split_policy(nv12, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runner,
|
||||
out['metadata']['vision'], out['metadata']['policy'])
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
|
||||
elif args.model_type == 'supercombo':
|
||||
assert args.supercombo_onnx
|
||||
policy_runners = [OnnxRunner(args.supercombo_onnx)]
|
||||
output_data['metadata'] = {'model': make_metadata_dict(args.supercombo_onnx)}
|
||||
model_runner = OnnxRunner(args.supercombo_onnx)
|
||||
out['metadata']['model'] = make_metadata_dict(args.supercombo_onnx)
|
||||
|
||||
frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip({}, out['metadata']['model']['input_shapes'])
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
model_w, model_h = args.model_size
|
||||
out[(cam_w, cam_h)] = {
|
||||
name: compile_supercombo(nv12, model_w, model_h, prepare_only, frame_skip,
|
||||
model_runner, out['metadata']['model'])
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
|
||||
elif args.model_type == 'vision_multi_policy':
|
||||
assert vision_runner
|
||||
policy_runners, policy_names = _load_policy_runners(args)
|
||||
output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
|
||||
for name in policy_names:
|
||||
runner_arg = getattr(args, f"{name}_onnx")
|
||||
output_data['metadata'][name] = make_metadata_dict(runner_arg)
|
||||
assert args.vision_onnx
|
||||
vision_runner = OnnxRunner(args.vision_onnx)
|
||||
out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
|
||||
|
||||
policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
|
||||
first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
|
||||
vision_meta = output_data['metadata'].get('vision', {})
|
||||
policy_runners = []
|
||||
policy_onnxes = []
|
||||
if args.policy_onnx:
|
||||
policy_onnxes.append(('policy', args.policy_onnx))
|
||||
if args.off_policy_onnx:
|
||||
policy_onnxes.append(('off_policy', args.off_policy_onnx))
|
||||
if args.on_policy_onnx:
|
||||
policy_onnxes.append(('on_policy', args.on_policy_onnx))
|
||||
|
||||
derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
|
||||
output_data.update(_compile_for_resolutions(args.camera_resolutions, args.model_size, derived_frame_skip,
|
||||
vision_runner, policy_runners, output_data['metadata']))
|
||||
for name, onnx_path in policy_onnxes:
|
||||
runner = OnnxRunner(onnx_path)
|
||||
policy_runners.append(runner)
|
||||
out['metadata'][name] = make_metadata_dict(onnx_path)
|
||||
|
||||
with open(args.output, "wb") as file:
|
||||
pickle.dump(output_data, file)
|
||||
first_policy_key = policy_onnxes[0][0]
|
||||
frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
|
||||
out['metadata'][first_policy_key]['input_shapes'])
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
model_w, model_h = args.model_size
|
||||
out[(cam_w, cam_h)] = {
|
||||
name: compile_multi_policy(nv12, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runners,
|
||||
out['metadata']['vision'], out['metadata'][first_policy_key])
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
|
||||
with open(args.output, "wb") as f:
|
||||
pickle.dump(out, f)
|
||||
pkl_size = os.path.getsize(args.output)
|
||||
print(f"Saved combined JIT to {args.output} ({pkl_size / 1e6:.2f} MB)")
|
||||
|
||||
from openpilot.common.file_chunker import chunk_file, get_chunk_targets
|
||||
chunk_targets = get_chunk_targets(args.output, pkl_size)
|
||||
chunk_file(args.output, chunk_targets)
|
||||
print(f"Chunked into {len(chunk_targets) - 1} file(s)")
|
||||
num_chunks = len(chunk_targets) - 1
|
||||
print(f"Chunked into {num_chunks} file(s)")
|
||||
|
||||
@@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import os
|
||||
os.environ['GMMU'] = '0'
|
||||
from openpilot.common.hardware import TICI
|
||||
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
|
||||
USBGPU = "USBGPU" in os.environ
|
||||
@@ -24,11 +23,6 @@ from setproctitle import setproctitle
|
||||
from openpilot.cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from opendbc.car.car_helpers import get_demo_car_params
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.device import Device
|
||||
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
@@ -36,7 +30,6 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
|
||||
|
||||
@@ -44,7 +37,6 @@ from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_p
|
||||
from openpilot.sunnypilot.modeld_v2.constants import Plan
|
||||
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
@@ -107,12 +99,17 @@ class ModelState(ModelStateBase):
|
||||
self._init_combined(pkl_path, cam_w, cam_h, model_bundle)
|
||||
|
||||
def _init_combined(self, pkl_path, cam_w, cam_h, bundle):
|
||||
from tinygrad.tensor import Tensor
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
|
||||
from tinygrad.device import Device
|
||||
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
|
||||
cloudlog.warning(f"loading combined pkl: {pkl_path}")
|
||||
jits = pickle.load(open_file_chunked(pkl_path))
|
||||
|
||||
self.DEV = Device.DEFAULT
|
||||
self.WARP_DEV = 'CPU' if USBGPU else self.DEV
|
||||
self.QUEUE_DEV = self.DEV
|
||||
|
||||
metadata = jits['metadata']
|
||||
if 'model' in metadata:
|
||||
@@ -121,10 +118,10 @@ class ModelState(ModelStateBase):
|
||||
self.policy_output_slices = {}
|
||||
self._policy_slices_list = []
|
||||
self._combined_model_type = 'supercombo'
|
||||
self._vision_input_names = [key for key in model_metadata['input_shapes'] if 'img' in key]
|
||||
self._vision_input_names = [k for k in model_metadata['input_shapes'] if 'img' in k]
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import make_supercombo_input_queues
|
||||
frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
|
||||
self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.QUEUE_DEV)
|
||||
self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.DEV)
|
||||
else:
|
||||
vision_metadata = metadata['vision']
|
||||
policy_keys = [k for k in metadata if k != 'vision']
|
||||
@@ -142,11 +139,11 @@ class ModelState(ModelStateBase):
|
||||
policy_input_shapes = first_policy_metadata['input_shapes']
|
||||
self._vision_input_names = [k for k in vision_input_shapes if 'img' in k]
|
||||
frame_skip = derive_frame_skip(vision_input_shapes, policy_input_shapes)
|
||||
self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.QUEUE_DEV)
|
||||
self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.DEV)
|
||||
|
||||
self._desire_key = next(key for key in self.numpy_inputs if key.startswith('desire'))
|
||||
self._road_key = next(key for key in self._vision_input_names if 'big' not in key)
|
||||
self._wide_key = next(key for key in self._vision_input_names if 'big' in key)
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
self.parser = SplitParser() if self._combined_model_type != 'supercombo' else CombinedParser()
|
||||
|
||||
is_20hz = bundle.is20hz if bundle else self._combined_model_type in ('split', 'multi_policy')
|
||||
if is_20hz:
|
||||
@@ -156,13 +153,6 @@ class ModelState(ModelStateBase):
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
self.constants = ModelConstants()
|
||||
|
||||
if self._combined_model_type != 'supercombo':
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
self.parser = SplitParser()
|
||||
else:
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
self.parser = CombinedParser()
|
||||
|
||||
self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
|
||||
self.full_frames: dict = {}
|
||||
self._blob_cache: dict = {}
|
||||
@@ -171,11 +161,12 @@ class ModelState(ModelStateBase):
|
||||
|
||||
self._run_policy = jits[(cam_w, cam_h)]['run_policy']
|
||||
self._warp_enqueue = jits[(cam_w, cam_h)]['warp_enqueue']
|
||||
yuv_size = self.frame_buf_params[self._road_key][3]
|
||||
road_name = next(k for k in self._vision_input_names if 'big' not in k)
|
||||
yuv_size = self.frame_buf_params[road_name][3]
|
||||
self._warp_enqueue(
|
||||
**self.input_queues,
|
||||
frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize(),
|
||||
big_frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize())
|
||||
frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.DEV).contiguous().realize(),
|
||||
big_frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.DEV).contiguous().realize())
|
||||
|
||||
|
||||
@property
|
||||
@@ -188,7 +179,7 @@ class ModelState(ModelStateBase):
|
||||
|
||||
@property
|
||||
def desire_key(self) -> str:
|
||||
return self._desire_key
|
||||
return next(k for k in self.numpy_inputs if k.startswith('desire'))
|
||||
|
||||
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
@@ -199,19 +190,19 @@ class ModelState(ModelStateBase):
|
||||
yuv_size = self.frame_buf_params[key][3]
|
||||
cache_key = (key, ptr)
|
||||
if cache_key not in self._blob_cache:
|
||||
self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
|
||||
self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.DEV)
|
||||
self.full_frames[key] = self._blob_cache[cache_key]
|
||||
|
||||
desire_key = self.desire_key
|
||||
inputs[desire_key][0] = 0
|
||||
self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
|
||||
self.prev_desire[:] = inputs[desire_key]
|
||||
for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
|
||||
for key in ('traffic_convention', 'lateral_control_params'):
|
||||
if key in self.numpy_inputs and key in inputs:
|
||||
self.numpy_inputs[key][:] = inputs[key]
|
||||
|
||||
road_key = self._road_key
|
||||
wide_key = self._wide_key
|
||||
road_key = next(n for n in bufs if 'big' not in n)
|
||||
wide_key = next(n for n in bufs if 'big' in n)
|
||||
self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3)
|
||||
self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)
|
||||
|
||||
@@ -234,12 +225,8 @@ class ModelState(ModelStateBase):
|
||||
policy_output = raw_outputs[i + 1].numpy().flatten()
|
||||
policy_sliced = {k: policy_output[np.newaxis, v] for k, v in policy_slices.items()}
|
||||
parsed = self.parser.parse_policy_outputs(policy_sliced)
|
||||
if ('off' in self._policy_keys[i]
|
||||
and self._has_on_policy
|
||||
and any('plan' in self._policy_slices_list[j] for j, k in enumerate(self._policy_keys) if 'on' in k.lower())):
|
||||
|
||||
if 'off' in self._policy_keys[i] and self._has_on_policy:
|
||||
parsed.pop('plan', None)
|
||||
|
||||
outputs.update(parsed)
|
||||
|
||||
if 'planplus' in outputs and 'plan' in outputs:
|
||||
@@ -254,20 +241,13 @@ class ModelState(ModelStateBase):
|
||||
|
||||
def get_action_from_model(self, 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:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0]
|
||||
if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan)
|
||||
desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
|
||||
else:
|
||||
desired_accel = model_output['action'][0, 1]
|
||||
desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
|
||||
should_stop = (v_ego < 0.3 and desired_accel < 0.1)
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
curvature_plan = plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0] if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan
|
||||
desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
|
||||
if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models
|
||||
if v_ego > self.MIN_LAT_CONTROL_SPEED:
|
||||
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)
|
||||
@@ -420,12 +400,6 @@ def main(demo=False):
|
||||
|
||||
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
|
||||
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
|
||||
|
||||
frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
|
||||
action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
|
||||
lat_action_t = lat_delay + frame_delay + action_delay
|
||||
long_action_t = long_delay + frame_delay + action_delay
|
||||
|
||||
inputs:dict[str, np.ndarray] = {
|
||||
model.desire_key: vec_desire,
|
||||
'traffic_convention': traffic_convention,
|
||||
@@ -434,9 +408,6 @@ def main(demo=False):
|
||||
if 'lateral_control_params' in model.numpy_inputs:
|
||||
inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32)
|
||||
|
||||
if 'action_t' in model.numpy_inputs:
|
||||
inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
|
||||
|
||||
mt1 = time.perf_counter()
|
||||
model_output = model.run(bufs, transforms, inputs, prepare_only)
|
||||
mt2 = time.perf_counter()
|
||||
@@ -448,7 +419,7 @@ def main(demo=False):
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
mdv2sp_send = messaging.new_message('modelDataV2SP')
|
||||
|
||||
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
|
||||
action = model.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,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -1,16 +1,13 @@
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
|
||||
|
||||
def safe_exp(x, out=None):
|
||||
# -11 is around 10**14, more causes float16 overflow
|
||||
return np.exp(np.clip(x, -np.inf, 11), out=out)
|
||||
|
||||
|
||||
def sigmoid(x):
|
||||
return 1. / (1. + safe_exp(-x))
|
||||
|
||||
|
||||
def softmax(x, axis=-1):
|
||||
x -= np.max(x, axis=axis, keepdims=True)
|
||||
if x.dtype == np.float32 or x.dtype == np.float64:
|
||||
@@ -20,19 +17,6 @@ def softmax(x, axis=-1):
|
||||
x /= np.sum(x, axis=axis, keepdims=True)
|
||||
return x
|
||||
|
||||
|
||||
def _infer_mhp(slice_size: int, prod_out_shape: int, max_in_n: int = 16, max_out_n: int = 6) -> tuple[int, int]:
|
||||
for out_n in range(max_out_n + 1):
|
||||
per = 2 * prod_out_shape + out_n
|
||||
if per <= 0:
|
||||
continue
|
||||
if slice_size % per == 0:
|
||||
in_n = slice_size // per
|
||||
if 1 <= in_n <= max_in_n:
|
||||
return in_n, out_n
|
||||
return 1, 0 # single hypothesis, no weights — matches a non-MDN output
|
||||
|
||||
|
||||
class Parser:
|
||||
def __init__(self, ignore_missing=False):
|
||||
self.ignore_missing = ignore_missing
|
||||
@@ -56,22 +40,17 @@ class Parser:
|
||||
raw = outs[name]
|
||||
outs[name] = sigmoid(raw)
|
||||
|
||||
def parse_mdn(self, name, outs, out_shape, in_N=0, out_N=0):
|
||||
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
|
||||
if in_N == 0 and out_N == 0:
|
||||
prod = int(np.prod(out_shape))
|
||||
in_N, out_N = _infer_mhp(raw.shape[1], prod)
|
||||
|
||||
raw = raw.reshape((raw.shape[0], in_N, -1))
|
||||
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
|
||||
|
||||
n_values = (raw.shape[2] - out_N)//2
|
||||
pred_mu = raw[:,:,:n_values]
|
||||
pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
|
||||
|
||||
if in_N > 1 and out_N > 0:
|
||||
if in_N > 1:
|
||||
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
|
||||
for i in range(out_N):
|
||||
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
|
||||
@@ -82,6 +61,7 @@ class Parser:
|
||||
weights[fidx] = weights[fidx][idxs]
|
||||
pred_mu[fidx] = pred_mu[fidx][idxs]
|
||||
pred_std[fidx] = pred_std[fidx][idxs]
|
||||
assert out_shape is not None
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_weights'] = weights
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
@@ -94,43 +74,37 @@ class Parser:
|
||||
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
|
||||
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
|
||||
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
|
||||
elif in_N > 1 and out_N == 0:
|
||||
# MHP without weights: keep every hypothesis intact, surface them as
|
||||
# ``*_hypotheses`` and propagate the full multi-hypothesis tensor.
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
else:
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
|
||||
if out_N > 1 or (in_N > 1 and out_N == 0):
|
||||
n_selections = out_N if out_N > 1 else in_N
|
||||
final_shape = tuple([raw.shape[0], n_selections] + list(out_shape))
|
||||
if out_N > 1:
|
||||
assert out_shape is not None
|
||||
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
|
||||
else:
|
||||
assert out_shape is not None
|
||||
final_shape = tuple([raw.shape[0],] + list(out_shape))
|
||||
outs[name] = pred_mu_final.reshape(final_shape)
|
||||
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
||||
|
||||
def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
# supercombo (4955 / 102) and newer variants (e.g. 990 / 144).
|
||||
self.parse_mdn('plan', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
|
||||
self.parse_mdn('lane_lines', outs, out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
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('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
if 'sim_pose' in outs:
|
||||
self.parse_mdn('sim_pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('lead', outs, out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
|
||||
self.parse_mdn('sim_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('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, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
||||
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, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
|
||||
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', 'meta']:
|
||||
self.parse_binary_crossentropy(k, outs)
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
|
||||
return outs
|
||||
|
||||
@@ -123,7 +123,7 @@ class Parser:
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
|
||||
if 'lane_lines' in outs:
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
|
||||
out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
if 'lane_lines_prob' in outs:
|
||||
self.parse_binary_crossentropy('lane_lines_prob', outs)
|
||||
if 'lead_prob' in outs:
|
||||
@@ -134,11 +134,9 @@ class Parser:
|
||||
self.parse_binary_crossentropy('meta', outs)
|
||||
if 'road_edges' in outs:
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
|
||||
out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
if 'sim_pose' in outs:
|
||||
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
if 'action' in outs:
|
||||
self.parse_mdn('action', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.ACTION_WIDTH,))
|
||||
|
||||
def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
|
||||
@@ -67,14 +67,22 @@ class TestStockEquivalence:
|
||||
state = model_state_factory(ARCHETYPES['vision_policy_split'])
|
||||
|
||||
frame_skip = derive_frame_skip(SPLIT_VISION_INPUT_SHAPES, SPLIT_POLICY_INPUT_SHAPES)
|
||||
# action_t is a deep-model prerequisite the SP loader doesn't provide yet; see skip_keys below
|
||||
stock_shapes = {**SPLIT_VISION_INPUT_SHAPES, **SPLIT_POLICY_INPUT_SHAPES, 'action_t': (1, 2)}
|
||||
stock_queues, stock_npy = make_input_queues(stock_shapes, frame_skip, device='NPY')
|
||||
|
||||
assert set(state.input_queues.keys()) - {'desire', 'traffic_convention'} == \
|
||||
set(stock_queues.keys()) - {'packed_npy_inputs'}
|
||||
assert {'desire', 'traffic_convention'} <= set(state.input_queues.keys())
|
||||
# We generate action_t and prev_feat dynamically based on the metadata
|
||||
assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - {'action_t', 'prev_feat'}
|
||||
# TODO-SP: remove action_t skip once SP adds prerequisite for deep models (action_t input queue)
|
||||
# prev_feat is a stock QCOM corruption workaround handled inside the SP loader's JIT path
|
||||
skip_keys = {'action_t', 'prev_feat'}
|
||||
# stock packs the per-key policy inputs into packed_npy_inputs; the npy views carry the individual keys
|
||||
stock_queue_keys = set(stock_queues.keys())
|
||||
if 'packed_npy_inputs' in stock_queue_keys:
|
||||
stock_queue_keys.remove('packed_npy_inputs')
|
||||
stock_queue_keys |= set(stock_npy.keys())
|
||||
assert set(state.input_queues.keys()) == stock_queue_keys - skip_keys, \
|
||||
f"Queue keys differ: v2={set(state.input_queues.keys())}, stock={stock_queue_keys}"
|
||||
assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - skip_keys, \
|
||||
f"Npy keys differ: v2={set(state.numpy_inputs.keys())}, stock={set(stock_npy.keys())}"
|
||||
|
||||
def test_split_queue_keys_work_with_desire_key(self, model_state_factory):
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
import os
|
||||
os.environ['DEV'] = 'CPU'
|
||||
import pytest
|
||||
import numpy as np
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.sunnypilot.modeld_v2.warp import CAMERA_CONFIGS
|
||||
from openpilot.sunnypilot.modeld_v2.warp import Warp, MODEL_W, MODEL_H
|
||||
|
||||
VISION_NAME_PAIRS = [ # needed to account for supercombos input_imgs
|
||||
('img', 'big_img'),
|
||||
('input_imgs', 'big_input_imgs'),
|
||||
]
|
||||
|
||||
|
||||
class MockVisionBuf:
|
||||
def __init__(self, w, h):
|
||||
self.width = w
|
||||
self.height = h
|
||||
_, _, _, yuv_size = get_nv12_info(w, h)
|
||||
self.data = np.zeros(yuv_size, dtype=np.uint8)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("buffer_length", [2, 5])
|
||||
def test_warp_initialization(buffer_length):
|
||||
warp = Warp(buffer_length)
|
||||
assert warp.buffer_length == buffer_length
|
||||
assert warp.img_buffer_shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("buffer_length", [2, 5])
|
||||
@pytest.mark.parametrize("cam_w, cam_h", CAMERA_CONFIGS)
|
||||
@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
|
||||
def test_warp_process(buffer_length, cam_w, cam_h, road, wide):
|
||||
warp = Warp(buffer_length)
|
||||
mock_buf = MockVisionBuf(cam_w, cam_h)
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
bufs = {road: mock_buf, wide: mock_buf}
|
||||
transforms = {road: transform, wide: transform}
|
||||
|
||||
out = warp.process(bufs, transforms)
|
||||
assert isinstance(out, dict)
|
||||
assert road in out and wide in out
|
||||
assert out[road].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
|
||||
assert out[wide].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
key = (cam_w, cam_h)
|
||||
assert key in warp.jit_cache
|
||||
|
||||
out2 = warp.process(bufs, transforms)
|
||||
assert out2[road].shape == out[road].shape
|
||||
|
||||
|
||||
@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
|
||||
def test_warp_buffer_shift(road, wide):
|
||||
warp = Warp(2)
|
||||
cam_w, cam_h = CAMERA_CONFIGS[1]
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
|
||||
buf1 = MockVisionBuf(cam_w, cam_h)
|
||||
buf1.data[0] = 255
|
||||
bufs1 = {road: buf1, wide: buf1}
|
||||
transforms = {road: transform, wide: transform}
|
||||
out1 = warp.process(bufs1, transforms)
|
||||
road1 = out1[road].numpy().copy()
|
||||
|
||||
buf2 = MockVisionBuf(cam_w, cam_h)
|
||||
buf2.data[0] = 128
|
||||
bufs2 = {road: buf2, wide: buf2}
|
||||
out2 = warp.process(bufs2, transforms)
|
||||
assert not np.array_equal(road1, out2[road].numpy())
|
||||
|
||||
|
||||
@pytest.mark.parametrize("buffer_length", [2, 5])
|
||||
@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
|
||||
def test_warp_buffer_accumulation(buffer_length, road, wide):
|
||||
warp = Warp(buffer_length)
|
||||
cam_w, cam_h = CAMERA_CONFIGS[0]
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
transforms = {road: transform, wide: transform}
|
||||
outputs = []
|
||||
|
||||
for i in range(buffer_length + 1):
|
||||
buf = MockVisionBuf(cam_w, cam_h)
|
||||
buf.data[:] = i * 10
|
||||
out = warp.process({road: buf, wide: buf}, transforms)
|
||||
outputs.append(out[road].numpy().copy())
|
||||
|
||||
assert warp.full_buffers['img'].shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
for i in range(1, len(outputs)):
|
||||
assert not np.array_equal(outputs[i - 1], outputs[i])
|
||||
|
||||
|
||||
def test_warp_different_cameras_same_instance():
|
||||
warp = Warp(2)
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
|
||||
buf1 = MockVisionBuf(*CAMERA_CONFIGS[0])
|
||||
warp.process({'img': buf1, 'big_img': buf1}, {'img': transform, 'big_img': transform})
|
||||
assert len(warp.jit_cache) == 1
|
||||
|
||||
buf2 = MockVisionBuf(*CAMERA_CONFIGS[1])
|
||||
warp.process({'img': buf2, 'big_img': buf2}, {'img': transform, 'big_img': transform})
|
||||
assert len(warp.jit_cache) == 2
|
||||
@@ -0,0 +1,171 @@
|
||||
import pickle
|
||||
import time
|
||||
import numpy as np
|
||||
from pathlib import Path
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.engine.jit import TinyJit
|
||||
from tinygrad.device import Device
|
||||
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
|
||||
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
|
||||
from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare as _make_frame_prepare
|
||||
|
||||
CAMERA_CONFIGS = [
|
||||
(_ar_ox_fisheye.width, _ar_ox_fisheye.height),
|
||||
(_os_fisheye.width, _os_fisheye.height),
|
||||
]
|
||||
|
||||
|
||||
def make_frame_prepare(cam_w, cam_h, model_w, model_h):
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
return _make_frame_prepare(nv12, model_w, model_h)
|
||||
|
||||
|
||||
def warp_pkl_path(w, h):
|
||||
from openpilot.selfdrive.modeld.helpers import MODELS_DIR
|
||||
return MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl'
|
||||
|
||||
|
||||
def make_update_img_input(frame_prepare, model_w, model_h):
|
||||
def update_img_input_tinygrad(tensor, frame, M_inv):
|
||||
M_inv = M_inv.to(Device.DEFAULT)
|
||||
new_img = frame_prepare(frame, M_inv)
|
||||
tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous())
|
||||
return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2)
|
||||
return update_img_input_tinygrad
|
||||
|
||||
|
||||
def make_update_both_imgs(frame_prepare, model_w, model_h):
|
||||
update_img = make_update_img_input(frame_prepare, model_w, model_h)
|
||||
def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv,
|
||||
calib_big_img_buffer, new_big_img, M_inv_big):
|
||||
calib_img_pair = update_img(calib_img_buffer, new_img, M_inv)
|
||||
calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big)
|
||||
return calib_img_pair, calib_big_img_pair
|
||||
return update_both_imgs_tinygrad
|
||||
|
||||
MODELS_DIR = Path(__file__).parent / 'models'
|
||||
MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE
|
||||
UPSTREAM_BUFFER_LENGTH = 5
|
||||
|
||||
|
||||
def v2_warp_pkl_path(cam_w, cam_h, buffer_length):
|
||||
return MODELS_DIR / f'warp_{cam_w}x{cam_h}_b{buffer_length}_tinygrad.pkl'
|
||||
|
||||
|
||||
def compile_v2_warp(cam_w, cam_h, buffer_length):
|
||||
_, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
|
||||
img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
print(f"Compiling v2 warp for {cam_w}x{cam_h} buffer_length={buffer_length}...")
|
||||
|
||||
frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
|
||||
update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
|
||||
update_img_jit = TinyJit(update_both_imgs, prune=True)
|
||||
|
||||
full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
|
||||
big_full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
|
||||
new_frame_np = np.random.default_rng(0).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
new_big_frame_np = np.random.default_rng(1).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
for i in range(10):
|
||||
img_inputs = [full_buffer,
|
||||
Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
|
||||
big_img_inputs = [big_full_buffer,
|
||||
Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
|
||||
inputs = img_inputs + big_img_inputs
|
||||
Device.default.synchronize()
|
||||
|
||||
st = time.perf_counter()
|
||||
_ = update_img_jit(*inputs)
|
||||
mt = time.perf_counter()
|
||||
Device.default.synchronize()
|
||||
et = time.perf_counter()
|
||||
print(f" [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
|
||||
|
||||
pkl_path = v2_warp_pkl_path(cam_w, cam_h, buffer_length)
|
||||
with open(pkl_path, "wb") as f:
|
||||
pickle.dump(update_img_jit, f)
|
||||
print(f" Saved to {pkl_path}")
|
||||
|
||||
jit = pickle.load(open(pkl_path, "rb"))
|
||||
verify_frame = np.random.default_rng(0).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
verify_big_frame = np.random.default_rng(1).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
fresh_inputs = [
|
||||
Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
|
||||
Tensor.from_blob(verify_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
|
||||
Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
|
||||
Tensor.from_blob(verify_big_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
|
||||
]
|
||||
jit(*fresh_inputs)
|
||||
|
||||
|
||||
class Warp:
|
||||
def __init__(self, buffer_length=2):
|
||||
self.buffer_length = buffer_length
|
||||
self.img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
self.jit_cache = {}
|
||||
self.full_buffers = {k: Tensor.zeros(self.img_buffer_shape, dtype='uint8').contiguous().realize() for k in ['img', 'big_img']}
|
||||
self._blob_cache: dict[int, Tensor] = {}
|
||||
self._nv12_cache: dict[tuple[int, int], int] = {}
|
||||
self.transforms_np = {k: np.zeros((3, 3), dtype=np.float32) for k in ['img', 'big_img']}
|
||||
self.transforms = {k: Tensor(v, device='NPY').realize() for k, v in self.transforms_np.items()}
|
||||
|
||||
def process(self, bufs, transforms):
|
||||
if not bufs:
|
||||
return {}
|
||||
road = next(n for n in bufs if 'big' not in n)
|
||||
wide = next(n for n in bufs if 'big' in n)
|
||||
cam_w, cam_h = bufs[road].width, bufs[road].height
|
||||
key = (cam_w, cam_h)
|
||||
|
||||
if key not in self.jit_cache:
|
||||
v2_pkl = v2_warp_pkl_path(cam_w, cam_h, self.buffer_length)
|
||||
if v2_pkl.exists():
|
||||
with open(v2_pkl, 'rb') as f:
|
||||
self.jit_cache[key] = pickle.load(f)
|
||||
elif self.buffer_length == UPSTREAM_BUFFER_LENGTH:
|
||||
upstream_pkl = warp_pkl_path(cam_w, cam_h)
|
||||
if upstream_pkl.exists():
|
||||
with open(upstream_pkl, 'rb') as f:
|
||||
self.jit_cache[key] = pickle.load(f)
|
||||
if key not in self.jit_cache:
|
||||
frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
|
||||
update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
|
||||
self.jit_cache[key] = TinyJit(update_both_imgs, prune=True)
|
||||
|
||||
if key not in self._nv12_cache:
|
||||
self._nv12_cache[key] = get_nv12_info(cam_w, cam_h)[3]
|
||||
yuv_size = self._nv12_cache[key]
|
||||
|
||||
road_ptr = bufs[road].data.ctypes.data
|
||||
wide_ptr = bufs[wide].data.ctypes.data
|
||||
if road_ptr not in self._blob_cache:
|
||||
self._blob_cache[road_ptr] = Tensor.from_blob(road_ptr, (yuv_size,), dtype='uint8')
|
||||
if wide_ptr not in self._blob_cache:
|
||||
self._blob_cache[wide_ptr] = Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
|
||||
road_blob = self._blob_cache[road_ptr]
|
||||
wide_blob = self._blob_cache[wide_ptr] if wide_ptr != road_ptr else Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
|
||||
np.copyto(self.transforms_np['img'], transforms[road].reshape(3, 3))
|
||||
np.copyto(self.transforms_np['big_img'], transforms[wide].reshape(3, 3))
|
||||
|
||||
Device.default.synchronize()
|
||||
res = self.jit_cache[key](
|
||||
self.full_buffers['img'], road_blob, self.transforms['img'],
|
||||
self.full_buffers['big_img'], wide_blob, self.transforms['big_img'],
|
||||
)
|
||||
out_road = res[0].realize()
|
||||
out_wide = res[1].realize()
|
||||
|
||||
return {road: out_road, wide: out_wide}
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
for cam_w, cam_h in CAMERA_CONFIGS:
|
||||
for bl in [2, 5]:
|
||||
compile_v2_warp(cam_w, cam_h, bl)
|
||||
@@ -6,12 +6,11 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import time
|
||||
import os
|
||||
|
||||
import requests
|
||||
from requests.exceptions import (SSLError, RequestException, HTTPError)
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
from openpilot.sunnypilot.models.helpers import is_bundle_version_compatible
|
||||
|
||||
from openpilot.cereal import custom
|
||||
@@ -27,35 +26,11 @@ class ModelParser:
|
||||
download_uri.sha256 = download_uri_data.get("sha256")
|
||||
return download_uri
|
||||
|
||||
@staticmethod
|
||||
def _parse_chunk(chunk_data) -> custom.ModelManagerSP.Chunk:
|
||||
chunk = custom.ModelManagerSP.Chunk()
|
||||
chunk.fileName = chunk_data.get("file_name")
|
||||
chunk.sha256 = chunk_data.get("sha256")
|
||||
return chunk
|
||||
|
||||
@staticmethod
|
||||
def _parse_artifact(artifact_data) -> custom.ModelManagerSP.Artifact:
|
||||
artifact = custom.ModelManagerSP.Artifact()
|
||||
artifact.fileName = artifact_data.get("file_name")
|
||||
artifact.downloadUri = ModelParser._parse_download_uri(artifact_data.get("download_uri", {}))
|
||||
|
||||
if "chunks" in artifact_data:
|
||||
artifact.chunks = [ModelParser._parse_chunk(chunk_data) for chunk_data in artifact_data["chunks"]]
|
||||
|
||||
try:
|
||||
model_dir = Paths.model_root()
|
||||
os.makedirs(model_dir, exist_ok=True)
|
||||
manifest_path = os.path.join(model_dir, f"{artifact.fileName}.chunkmanifest")
|
||||
num_chunks = str(len(artifact.chunks))
|
||||
|
||||
if not os.path.exists(manifest_path) or open(manifest_path).read().strip() != num_chunks:
|
||||
with open(manifest_path, "w") as f:
|
||||
f.write(num_chunks)
|
||||
cloudlog.info(f"Wrote chunk manifest for {artifact.fileName}: {num_chunks} chunks")
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"Failed to write chunk manifest for {artifact.fileName}: {e}")
|
||||
|
||||
return artifact
|
||||
|
||||
@staticmethod
|
||||
@@ -64,6 +39,8 @@ class ModelParser:
|
||||
|
||||
model.type = model_data.get("type")
|
||||
model.artifact = ModelParser._parse_artifact(model_data.get("artifact", {}))
|
||||
if metadata := model_data.get("metadata"):
|
||||
model.metadata = ModelParser._parse_artifact(metadata)
|
||||
return model
|
||||
|
||||
@staticmethod
|
||||
@@ -139,7 +116,7 @@ class ModelCache:
|
||||
|
||||
class ModelFetcher:
|
||||
"""Handles fetching and caching of model data from remote source"""
|
||||
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v18.json"
|
||||
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v17.json"
|
||||
|
||||
def __init__(self, params: Params):
|
||||
self.params = params
|
||||
@@ -207,5 +184,4 @@ if __name__ == "__main__":
|
||||
# Print artifact details
|
||||
print(f"Artifact: {model.artifact.fileName}, Download URI: {model.artifact.downloadUri.uri}")
|
||||
# Print metadata details
|
||||
if model.artifact.chunks:
|
||||
print(f"Contains {len(model.artifact.chunks)} chunks.")
|
||||
print(f"Metadata: {model.metadata.fileName}, Download URI: {model.metadata.downloadUri.uri}")
|
||||
|
||||
@@ -18,7 +18,7 @@ from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRai
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
|
||||
# SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
|
||||
REQUIRED_JSON_VERSION = 16
|
||||
REQUIRED_JSON_VERSION = 15
|
||||
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
|
||||
@@ -56,20 +56,12 @@ def is_bundle_version_compatible(bundle: dict) -> bool:
|
||||
|
||||
def _bundle_artifacts(bundle: custom.ModelManagerSP.ModelBundle) -> list[tuple[str, str]]:
|
||||
artifacts = []
|
||||
from openpilot.common.file_chunker import get_chunk_name
|
||||
for model in getattr(bundle, 'models', []) or []:
|
||||
for artifact in (getattr(model, 'artifact', None),):
|
||||
if artifact and getattr(artifact, 'fileName', None):
|
||||
if len(artifact.chunks) > 0:
|
||||
for i, chunk in enumerate(artifact.chunks):
|
||||
chunk_name = get_chunk_name(artifact.fileName, i, len(artifact.chunks))
|
||||
if getattr(chunk, 'sha256', None):
|
||||
artifacts.append((chunk_name, chunk.sha256))
|
||||
else:
|
||||
if getattr(artifact, 'downloadUri', None):
|
||||
sha256 = getattr(artifact.downloadUri, 'sha256', None)
|
||||
if sha256:
|
||||
artifacts.append((artifact.fileName, sha256))
|
||||
for artifact in (getattr(model, 'artifact', None), getattr(model, 'metadata', None)):
|
||||
if artifact and getattr(artifact, 'fileName', None) and getattr(artifact, 'downloadUri', None):
|
||||
sha256 = getattr(artifact.downloadUri, 'sha256', None)
|
||||
if sha256:
|
||||
artifacts.append((artifact.fileName, sha256))
|
||||
return artifacts
|
||||
|
||||
|
||||
@@ -164,7 +156,8 @@ def _get_model():
|
||||
|
||||
|
||||
def load_metadata():
|
||||
metadata_path = METADATA_PATH
|
||||
model = _get_model()
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{model.metadata.fileName}" if model else METADATA_PATH
|
||||
|
||||
with open(metadata_path, 'rb') as f:
|
||||
return pickle.load(f)
|
||||
|
||||
@@ -38,11 +38,11 @@ class ModelManagerSP:
|
||||
if not self.selected_bundle:
|
||||
return
|
||||
for model in self.selected_bundle.models:
|
||||
artifact = model.artifact
|
||||
if artifact is not source_artifact and artifact.fileName == source_artifact.fileName:
|
||||
artifact.downloadProgress.status = source_artifact.downloadProgress.status
|
||||
artifact.downloadProgress.progress = source_artifact.downloadProgress.progress
|
||||
artifact.downloadProgress.eta = source_artifact.downloadProgress.eta
|
||||
for artifact in (model.artifact, model.metadata):
|
||||
if artifact is not source_artifact and artifact.fileName == source_artifact.fileName:
|
||||
artifact.downloadProgress.status = source_artifact.downloadProgress.status
|
||||
artifact.downloadProgress.progress = source_artifact.downloadProgress.progress
|
||||
artifact.downloadProgress.eta = source_artifact.downloadProgress.eta
|
||||
|
||||
def _calculate_eta(self, filename: str, progress: float) -> int:
|
||||
"""Calculate ETA based on elapsed time and current progress"""
|
||||
@@ -89,16 +89,20 @@ class ModelManagerSP:
|
||||
del self._download_start_times[model.fileName]
|
||||
|
||||
async def _download_chunked(self, base_url: str, base_path: str, artifact) -> None:
|
||||
from openpilot.common.file_chunker import get_chunk_name, get_manifest_path
|
||||
|
||||
num_chunks = len(artifact.chunks)
|
||||
if num_chunks == 0:
|
||||
raise ValueError("No chunks defined in artifact")
|
||||
|
||||
from openpilot.common.file_chunker import get_manifest_path, get_chunk_name
|
||||
manifest_url = get_manifest_path(base_url)
|
||||
manifest_path = get_manifest_path(base_path)
|
||||
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.get(manifest_url) as resp:
|
||||
if resp.status == 404:
|
||||
raise FileNotFoundError
|
||||
resp.raise_for_status()
|
||||
num_chunks = int((await resp.read()).strip())
|
||||
|
||||
self._download_start_times[artifact.fileName] = time.monotonic()
|
||||
|
||||
for i, _ in enumerate(artifact.chunks):
|
||||
for i in range(num_chunks):
|
||||
chunk_url = get_chunk_name(base_url, i, num_chunks)
|
||||
chunk_path = get_chunk_name(base_path, i, num_chunks)
|
||||
chunk_downloaded = 0
|
||||
@@ -113,7 +117,7 @@ class ModelManagerSP:
|
||||
if self.params.get("ModelManager_DownloadIndex") is None:
|
||||
raise Exception("Download cancelled")
|
||||
intra = chunk_downloaded / max(chunk_size, 1)
|
||||
progress = min(99.0, ((i + intra) / num_chunks) * 100)
|
||||
progress = min(99, (i + intra) / num_chunks * 100)
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.downloading
|
||||
artifact.downloadProgress.progress = progress
|
||||
artifact.downloadProgress.eta = self._calculate_eta(artifact.fileName, progress)
|
||||
@@ -136,22 +140,7 @@ class ModelManagerSP:
|
||||
full_path = os.path.join(destination_path, filename)
|
||||
|
||||
try:
|
||||
is_cached = False
|
||||
if len(artifact.chunks) > 0:
|
||||
from openpilot.common.file_chunker import get_chunk_name
|
||||
chunks_valid = True
|
||||
for i, chunk in enumerate(artifact.chunks):
|
||||
chunk_path = get_chunk_name(full_path, i, len(artifact.chunks))
|
||||
if not await verify_file(chunk_path, chunk.sha256):
|
||||
chunks_valid = False
|
||||
break
|
||||
if chunks_valid and len(artifact.chunks) > 0:
|
||||
is_cached = True
|
||||
else:
|
||||
if await verify_file(full_path, expected_hash):
|
||||
is_cached = True
|
||||
|
||||
if is_cached:
|
||||
if await verify_file(full_path, expected_hash):
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
|
||||
artifact.downloadProgress.progress = 100
|
||||
artifact.downloadProgress.eta = 0
|
||||
@@ -159,17 +148,13 @@ class ModelManagerSP:
|
||||
self._report_status()
|
||||
return
|
||||
|
||||
if len(artifact.chunks) > 0:
|
||||
try:
|
||||
await self._download_chunked(url, full_path, artifact)
|
||||
from openpilot.common.file_chunker import get_chunk_name
|
||||
for i, chunk in enumerate(artifact.chunks):
|
||||
chunk_path = get_chunk_name(full_path, i, len(artifact.chunks))
|
||||
if not await verify_file(chunk_path, chunk.sha256):
|
||||
raise ValueError(f"Hash validation failed for chunk {i+1} of {filename}")
|
||||
else:
|
||||
except (FileNotFoundError, aiohttp.ClientResponseError):
|
||||
await self._download_file(url, full_path, artifact)
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
artifact.downloadProgress.progress = 100
|
||||
@@ -185,15 +170,18 @@ class ModelManagerSP:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
self._sync_artifact_progress(artifact)
|
||||
if self.selected_bundle:
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
self._report_status()
|
||||
self._download_start_times.pop(artifact.fileName, None)
|
||||
raise
|
||||
|
||||
async def _process_model(self, model, destination_path: str) -> None:
|
||||
"""Processes a single model download including verification"""
|
||||
await self._process_artifact(model.artifact, destination_path)
|
||||
model_artifact = model.artifact
|
||||
metadata_artifact = model.metadata
|
||||
|
||||
await self._process_artifact(metadata_artifact, destination_path)
|
||||
await self._process_artifact(model_artifact, destination_path)
|
||||
|
||||
def _report_status(self) -> None:
|
||||
"""Reports current status through messaging system"""
|
||||
@@ -217,16 +205,16 @@ class ModelManagerSP:
|
||||
try:
|
||||
seen_artifacts: set[str] = set()
|
||||
for model in self.selected_bundle.models:
|
||||
artifact = model.artifact
|
||||
if not artifact.fileName:
|
||||
continue
|
||||
if artifact.fileName in seen_artifacts:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
|
||||
artifact.downloadProgress.progress = 100
|
||||
artifact.downloadProgress.eta = 0
|
||||
else:
|
||||
seen_artifacts.add(artifact.fileName)
|
||||
await self._process_artifact(artifact, destination_path)
|
||||
for artifact in (model.metadata, model.artifact):
|
||||
if not artifact.fileName:
|
||||
continue
|
||||
if artifact.fileName in seen_artifacts:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
|
||||
artifact.downloadProgress.progress = 100
|
||||
artifact.downloadProgress.eta = 0
|
||||
else:
|
||||
seen_artifacts.add(artifact.fileName)
|
||||
await self._process_artifact(artifact, destination_path)
|
||||
|
||||
self.active_bundle = self.selected_bundle
|
||||
self.active_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
@@ -287,6 +275,8 @@ class ModelManagerSP:
|
||||
for model in self.active_bundle.models:
|
||||
if hasattr(model, 'artifact') and model.artifact.fileName:
|
||||
active_files.append(model.artifact.fileName)
|
||||
if hasattr(model, 'metadata') and model.metadata.fileName:
|
||||
active_files.append(model.metadata.fileName)
|
||||
|
||||
# Remove all files except active ones (including their chunk files)
|
||||
model_dir = Paths.model_root()
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType
|
||||
|
||||
|
||||
def get_model_runner() -> ModelRunner:
|
||||
"""
|
||||
Factory function to create and return the appropriate ModelRunner instance.
|
||||
|
||||
Selects TinygradRunner, choosing TinygradSplitRunner if separate vision/policy
|
||||
models are detected in the active bundle.
|
||||
|
||||
:return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner).
|
||||
"""
|
||||
bundle = get_active_bundle()
|
||||
if bundle and bundle.models:
|
||||
model_types = {m.type.raw for m in bundle.models}
|
||||
# Check if the bundle uses separate vision and policy models (legacy or new split format)
|
||||
split_types = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy}
|
||||
if model_types & split_types:
|
||||
return TinygradSplitRunner()
|
||||
# Otherwise, assume a single model (likely supercombo)
|
||||
if bundle.models:
|
||||
return TinygradRunner(bundle.models[0].type.raw)
|
||||
|
||||
# Default fallback to TinygradRunner with the supercombo type if bundle info is missing/incomplete
|
||||
return TinygradRunner(ModelType.supercombo)
|
||||
@@ -0,0 +1,174 @@
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.constants import NumpyDict, ShapeDict, Model, SliceDict, SEND_RAW_PRED
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
import pickle
|
||||
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
|
||||
|
||||
class ModelData:
|
||||
"""
|
||||
Stores metadata and configuration for a specific machine learning model.
|
||||
|
||||
This class loads model metadata (like input shapes and output slices)
|
||||
from a pickle file associated with a model instance.
|
||||
|
||||
:param model: The machine learning model object containing metadata.
|
||||
"""
|
||||
def __init__(self, model: Model):
|
||||
self.model = model
|
||||
self.metadata = model.metadata
|
||||
self.input_shapes: ShapeDict = {}
|
||||
self.output_slices: SliceDict = {}
|
||||
if self.metadata:
|
||||
self._load_metadata()
|
||||
|
||||
def _load_metadata(self) -> None:
|
||||
"""Loads input shapes and output slices from the model's metadata pickle file."""
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{self.metadata.fileName}"
|
||||
with open(metadata_path, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
self.input_shapes = model_metadata.get('input_shapes', {})
|
||||
self.output_slices = model_metadata.get('output_slices', {})
|
||||
|
||||
|
||||
class ModularRunner(ABC):
|
||||
"""
|
||||
Represents a modular runner for handling and slicing model outputs.
|
||||
|
||||
This abstract base class is designed to provide an interface for modular
|
||||
parsing and processing of model outputs. Classes inheriting from it must
|
||||
implement the specified abstract methods, defining how model outputs
|
||||
should be handled and stored. The primary goal is to enable structured
|
||||
parsing of outputs through a dictionary-based method mapping.
|
||||
|
||||
:ivar parser_method_dict: Mapping dictionary containing parser methods
|
||||
for handling specific types of outputs.
|
||||
:type parser_method_dict: dict
|
||||
"""
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def parser_method_dict(self) -> dict:
|
||||
pass
|
||||
|
||||
@parser_method_dict.setter
|
||||
@abstractmethod
|
||||
def parser_method_dict(self, value: dict) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
pass
|
||||
|
||||
|
||||
class ModelRunner(ModularRunner):
|
||||
"""
|
||||
Abstract base class for managing and executing machine learning models.
|
||||
|
||||
Provides a common interface for loading models, preparing inputs, running
|
||||
inference, and slicing/parsing outputs based on model metadata. Derived
|
||||
classes implement the specifics of input preparation and model execution
|
||||
for different frameworks (e.g., Tinygrad, ONNX).
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
"""Initializes the model runner, loading the active model bundle."""
|
||||
self.is_20hz: bool | None = None
|
||||
self.is_20hz_3d: bool | None = None
|
||||
self.models: dict[int, ModelData] = {}
|
||||
self._model_data: ModelData | None = None # Active model data for current operation
|
||||
self._parser_method_dict: dict = {}
|
||||
self.inputs: dict = {}
|
||||
self._parser = None
|
||||
self._load_models()
|
||||
self._constants = None
|
||||
|
||||
@property
|
||||
def constants(self):
|
||||
return self._constants
|
||||
|
||||
@property
|
||||
def parser_method_dict(self) -> dict:
|
||||
"""Returns the dictionary mapping model types to their respective parsing methods."""
|
||||
return self._parser_method_dict
|
||||
|
||||
@parser_method_dict.setter
|
||||
def parser_method_dict(self, value: dict) -> None:
|
||||
"""Sets the dictionary mapping model types to their respective parsing methods."""
|
||||
self._parser_method_dict = value
|
||||
|
||||
def _load_models(self) -> None:
|
||||
"""Loads the active model bundle configuration and sets up ModelData."""
|
||||
bundle = get_active_bundle()
|
||||
if not bundle:
|
||||
raise ValueError("No active model bundle found, why are we being executed?")
|
||||
|
||||
self.models = {model.type.raw: ModelData(model) for model in bundle.models}
|
||||
self.is_20hz = bundle.is20hz
|
||||
self.is_20hz_3d = False
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the input shapes for the currently active model."""
|
||||
if self._model_data:
|
||||
return self._model_data.input_shapes
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
"""Returns the output slices for the currently active model."""
|
||||
if self._model_data:
|
||||
return self._model_data.output_slices
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
"""Returns the list of vision input names from the input shapes."""
|
||||
if self._model_data:
|
||||
return list(self._model_data.input_shapes.keys())
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@abstractmethod
|
||||
def prepare_inputs(self, numpy_inputs: NumpyDict) -> dict:
|
||||
"""
|
||||
Abstract method to prepare inputs for model inference.
|
||||
|
||||
:param numpy_inputs: Dictionary of numpy arrays for non-image inputs.
|
||||
:return: Dictionary of prepared inputs ready for the model.
|
||||
"""
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""
|
||||
Abstract method to execute model inference with prepared inputs.
|
||||
|
||||
:return: Dictionary containing the model's raw output arrays.
|
||||
"""
|
||||
raise NotImplementedError
|
||||
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""
|
||||
Slices the raw model output array based on the output_slices metadata.
|
||||
|
||||
:param model_outputs: The raw numpy array output from the model.
|
||||
:return: A dictionary where keys are output names and values are sliced numpy arrays.
|
||||
"""
|
||||
if not self._model_data:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
sliced_outputs = {k: model_outputs[np.newaxis, v] for k, v in self._model_data.output_slices.items()}
|
||||
if SEND_RAW_PRED:
|
||||
sliced_outputs['raw_pred'] = model_outputs.copy() # Optionally include the full raw output
|
||||
return sliced_outputs
|
||||
|
||||
def run_model(self) -> NumpyDict:
|
||||
"""
|
||||
Executes the model inference pipeline: runs the model and parses outputs.
|
||||
|
||||
:return: Dictionary containing the final parsed model outputs.
|
||||
"""
|
||||
return self._run_model() # Parsing is handled within specific runner implementations
|
||||
@@ -0,0 +1,91 @@
|
||||
import os
|
||||
from abc import ABC
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType, NumpyDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModularRunner
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
|
||||
|
||||
class OffPolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for off-policy models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the off-policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._off_policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.offPolicy] = self._parse_off_policy_outputs
|
||||
|
||||
def _parse_off_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses off-policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._off_policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
|
||||
class OnPolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for on-policy models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the on-policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._on_policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.onPolicy] = self._parse_on_policy_outputs
|
||||
|
||||
def _parse_on_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses on-policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._on_policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
|
||||
class PolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for policy-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.policy] = self._parse_policy_outputs
|
||||
|
||||
def _parse_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
class VisionTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for vision-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the vision part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._vision_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.vision] = self._parse_vision_outputs
|
||||
|
||||
def _parse_vision_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses vision model outputs using SplitParser."""
|
||||
result: NumpyDict = self._vision_parser.parse_vision_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
class SupercomboTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for vision-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the vision part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._supercombo_parser = CombinedParser()
|
||||
self.parser_method_dict[ModelType.supercombo] = self._parse_supercombo_outputs
|
||||
|
||||
def _parse_supercombo_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses vision model outputs using SplitParser."""
|
||||
result: NumpyDict = self._supercombo_parser.parse_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
@@ -0,0 +1,179 @@
|
||||
import pickle
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.models.runners.constants import NumpyDict, ModelType, ShapeDict, CUSTOM_MODEL_PATH, SliceDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.models.runners.tinygrad.model_types import PolicyTinygrad, VisionTinygrad, SupercomboTinygrad, OffPolicyTinygrad, OnPolicyTinygrad
|
||||
from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
|
||||
class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
|
||||
"""
|
||||
A ModelRunner implementation for executing Tinygrad models.
|
||||
|
||||
Handles loading Tinygrad model artifacts (.pkl), preparing inputs as Tinygrad
|
||||
Tensors (potentially using QCOM extensions on TICI), running inference,
|
||||
and parsing the outputs.
|
||||
|
||||
:param model_type: The type of model (e.g., supercombo) to load and run.
|
||||
"""
|
||||
def __init__(self, model_type: int = ModelType.supercombo):
|
||||
ModelRunner.__init__(self)
|
||||
SupercomboTinygrad.__init__(self)
|
||||
PolicyTinygrad.__init__(self)
|
||||
VisionTinygrad.__init__(self)
|
||||
OffPolicyTinygrad.__init__(self)
|
||||
OnPolicyTinygrad.__init__(self)
|
||||
self._constants = ModelConstants
|
||||
self._model_data = self.models.get(model_type)
|
||||
if not self._model_data or not self._model_data.model:
|
||||
raise ValueError(f"Model data for type {model_type} not available.")
|
||||
|
||||
artifact_filename = self._model_data.model.artifact.fileName
|
||||
assert artifact_filename.endswith('_tinygrad.pkl'), \
|
||||
f"Invalid model file {artifact_filename} for TinygradRunner"
|
||||
|
||||
model_pkl_path = f"{CUSTOM_MODEL_PATH}/{artifact_filename}"
|
||||
with open(model_pkl_path, "rb") as f:
|
||||
try:
|
||||
# Load the compiled Tinygrad model runner function
|
||||
self.model_run = pickle.load(f)
|
||||
except FileNotFoundError as e:
|
||||
# Provide a helpful error message if the model was built for a different platform
|
||||
assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC"
|
||||
raise
|
||||
|
||||
# Map input names to their required dtype and device from the loaded model
|
||||
self.input_to_dtype = {}
|
||||
self.input_to_device = {}
|
||||
for idx, name in enumerate(self.model_run.captured.expected_names):
|
||||
info = self.model_run.captured.expected_input_info[idx]
|
||||
self.input_to_dtype[name] = info[2] # dtype
|
||||
self.input_to_device[name] = info[3] # device
|
||||
self._policy_cached = False
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
"""Returns the list of vision input names from the input shapes."""
|
||||
return [name for name in self.input_shapes.keys() if 'img' in name]
|
||||
|
||||
|
||||
def prepare_policy_inputs(self, numpy_inputs: NumpyDict):
|
||||
if not self._policy_cached:
|
||||
for key, value in numpy_inputs.items():
|
||||
self.inputs[key] = Tensor(value, device='NPY').realize()
|
||||
self._policy_cached = True
|
||||
|
||||
def prepare_inputs(self, numpy_inputs: NumpyDict) -> dict:
|
||||
"""Prepares all vision and policy inputs for the model."""
|
||||
self.prepare_policy_inputs(numpy_inputs)
|
||||
for key in self.vision_input_names:
|
||||
if key in self.inputs:
|
||||
self.inputs[key] = self.inputs[key].cast(self.input_to_dtype[key])
|
||||
return self.inputs
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs the Tinygrad model inference and parses the outputs."""
|
||||
outputs = self.model_run(**self.inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
|
||||
return self._parse_outputs(outputs)
|
||||
|
||||
def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses the raw model outputs using the standard Parser."""
|
||||
if self._model_data is None:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
result: NumpyDict = self.parser_method_dict[self._model_data.model.type.raw](model_outputs)
|
||||
return result
|
||||
|
||||
|
||||
class TinygradSplitRunner(ModelRunner):
|
||||
"""
|
||||
A ModelRunner that coordinates separate TinygradVisionRunner and TinygradPolicyRunner instances.
|
||||
|
||||
Manages the execution of split vision and policy models, combining their inputs and outputs.
|
||||
"""
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.is_20hz_3d = True
|
||||
self.vision_runner = TinygradRunner(ModelType.vision)
|
||||
self.policy_runner = TinygradRunner(ModelType.policy) if self.models.get(ModelType.policy) else None
|
||||
self.off_policy_runner = TinygradRunner(ModelType.offPolicy) if self.models.get(ModelType.offPolicy) else None
|
||||
self.on_policy_runner = TinygradRunner(ModelType.onPolicy) if self.models.get(ModelType.onPolicy) else None
|
||||
self._constants = SplitModelConstants
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs both vision and policy models and merges their parsed outputs."""
|
||||
vision_output = self.vision_runner.run_model()
|
||||
outputs = {**vision_output}
|
||||
|
||||
if self.policy_runner:
|
||||
policy_output = self.policy_runner.run_model()
|
||||
outputs.update(policy_output)
|
||||
|
||||
if self.off_policy_runner:
|
||||
off_policy_output = self.off_policy_runner.run_model()
|
||||
if self.on_policy_runner:
|
||||
off_policy_output.pop('plan', None)
|
||||
outputs.update(off_policy_output)
|
||||
|
||||
if self.on_policy_runner:
|
||||
on_policy_output = self.on_policy_runner.run_model()
|
||||
outputs.update(on_policy_output)
|
||||
|
||||
if 'planplus' in outputs and 'plan' in outputs:
|
||||
outputs['plan'] = outputs['plan'] + outputs['planplus']
|
||||
|
||||
return outputs
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
"""Returns the list of vision input names from the vision runner."""
|
||||
return list(self.vision_runner.vision_input_names)
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the combined input shapes from both vision and policy models."""
|
||||
shapes = {**self.vision_runner.input_shapes}
|
||||
if self.policy_runner:
|
||||
shapes.update(self.policy_runner.input_shapes)
|
||||
if self.off_policy_runner:
|
||||
shapes.update(self.off_policy_runner.input_shapes)
|
||||
if self.on_policy_runner:
|
||||
shapes.update(self.on_policy_runner.input_shapes)
|
||||
return shapes
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
"""Returns the combined output slices from both vision and policy models."""
|
||||
slices = {**self.vision_runner.output_slices}
|
||||
if self.policy_runner:
|
||||
slices.update(self.policy_runner.output_slices)
|
||||
if self.off_policy_runner:
|
||||
slices.update(self.off_policy_runner.output_slices)
|
||||
if self.on_policy_runner:
|
||||
slices.update(self.on_policy_runner.output_slices)
|
||||
return slices
|
||||
|
||||
def prepare_inputs(self, numpy_inputs: NumpyDict) -> dict:
|
||||
"""Prepares inputs for both vision and policy models."""
|
||||
if self.policy_runner:
|
||||
self.policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
|
||||
for key in self.vision_input_names:
|
||||
if key in self.inputs:
|
||||
self.vision_runner.inputs[key] = self.inputs[key].cast(self.vision_runner.input_to_dtype[key])
|
||||
|
||||
inputs = {**self.vision_runner.inputs}
|
||||
if self.policy_runner:
|
||||
inputs.update(self.policy_runner.inputs)
|
||||
|
||||
if self.off_policy_runner:
|
||||
self.off_policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
inputs.update(self.off_policy_runner.inputs)
|
||||
if self.on_policy_runner:
|
||||
self.on_policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
inputs.update(self.on_policy_runner.inputs)
|
||||
return inputs
|
||||
@@ -43,7 +43,6 @@ class SplitModelConstants:
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
ACTION_WIDTH = 2
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
LAT_PLANNER_SOLUTION_WIDTH = 4
|
||||
DESIRED_CURV_WIDTH = 1
|
||||
|
||||
@@ -123,7 +123,6 @@ def initialize_params(params) -> list[dict[str, Any]]:
|
||||
# tesla
|
||||
keys.extend([
|
||||
"TeslaCoopSteering",
|
||||
"TeslaMadsScreenButton",
|
||||
])
|
||||
|
||||
# toyota
|
||||
|
||||
@@ -151,8 +151,8 @@ class SmartCruiseControlMap:
|
||||
a = 0.5 * TARGET_JERK
|
||||
b = self.a_ego
|
||||
c = self.v_ego - tv
|
||||
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / (2 * a)
|
||||
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / (2 * a)
|
||||
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
|
||||
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
|
||||
if not isinstance(t_a, complex) and t_a > 0:
|
||||
t = t_a
|
||||
else:
|
||||
|
||||
+1
-18
@@ -4,17 +4,13 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import R, SmartCruiseControlMap
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
|
||||
|
||||
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
|
||||
|
||||
@@ -59,17 +55,4 @@ class TestSmartCruiseControlMap:
|
||||
self.scc_m.update(True, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.enabled
|
||||
|
||||
def test_moderate_curve(self):
|
||||
# Regression: `... / 2 * a` parsed as `(.../2)*a` instead of `.../(2*a)`,
|
||||
# making max_d ~11x too small so the moderate-curve branch never tripped.
|
||||
# v_ego=25, a_ego=0, tv=24: fixed max_d≈45m vs buggy ≈4m at a 40m waypoint.
|
||||
waypoint_lon_deg = (40.0 / R) * (180.0 / math.pi)
|
||||
self.mem_params.put("LastGPSPosition", json.dumps({"latitude": 0.0, "longitude": 0.0}), block=True)
|
||||
self.mem_params.put("MapTargetVelocities",
|
||||
json.dumps([{"latitude": 0.0, "longitude": waypoint_lon_deg, "velocity": 24.0}]), block=True)
|
||||
|
||||
self.scc_m.update(True, False, 25.0, 0.0, 30.0)
|
||||
|
||||
assert self.scc_m.v_target == pytest.approx(24.0)
|
||||
|
||||
# TODO-SP: mock data from modelV2 to test other states
|
||||
|
||||
@@ -91,7 +91,7 @@ class SpeedLimitAssist:
|
||||
|
||||
self._plus_hold = 0.
|
||||
self._minus_hold = 0.
|
||||
self._release_toggle_prev = 0
|
||||
self._last_carstate_ts = 0.
|
||||
|
||||
# TODO-SP: SLA's own output_a_target for planner
|
||||
# Solution functions mapped to respective states
|
||||
@@ -146,16 +146,16 @@ class SpeedLimitAssist:
|
||||
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
|
||||
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
|
||||
|
||||
def update_buttons(self, release_toggle: int) -> None:
|
||||
released = self._release_toggle_prev ^ release_toggle
|
||||
self._release_toggle_prev = release_toggle
|
||||
if not released:
|
||||
return
|
||||
def update_car_state(self, CS: car.CarState) -> None:
|
||||
now = time.monotonic()
|
||||
if any((released >> b) & 1 for b in CRUISE_BUTTONS_PLUS):
|
||||
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
if any((released >> b) & 1 for b in CRUISE_BUTTONS_MINUS):
|
||||
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
self._last_carstate_ts = now
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if not b.pressed:
|
||||
if b.type in CRUISE_BUTTONS_PLUS:
|
||||
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
elif b.type in CRUISE_BUTTONS_MINUS:
|
||||
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
|
||||
def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
|
||||
now = time.monotonic()
|
||||
|
||||
+1
-91
@@ -5,14 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.rivian.values import CAR as RIVIAN
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.common.constants import CV
|
||||
@@ -24,13 +21,9 @@ from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfac
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
|
||||
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES, CRUISE_BUTTON_CONFIRM_HOLD
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
|
||||
ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
|
||||
@@ -283,86 +276,3 @@ class TestSpeedLimitAssist:
|
||||
assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
|
||||
elif initial_state in ACTIVE_STATES:
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
|
||||
|
||||
class TestButtonStateTrackerSLAIntegration:
|
||||
|
||||
def setup_method(self, method):
|
||||
self.tracker = ButtonStateTracker()
|
||||
self.params = Params()
|
||||
self.params.put("IsReleaseSpBranch", True, block=True)
|
||||
self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
|
||||
self.params.put_bool("IsMetric", False, block=True)
|
||||
self.params.put("SpeedLimitOffsetType", 0, block=True)
|
||||
self.params.put("SpeedLimitValueOffset", 0, block=True)
|
||||
|
||||
CarInterface = interfaces[DEFAULT_CAR]
|
||||
CP = CarInterface.get_non_essential_params(DEFAULT_CAR)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, DEFAULT_CAR)
|
||||
self.sla = SpeedLimitAssist(CP, CP_SP)
|
||||
|
||||
def _make_cs(self, events=None) -> car.CarState:
|
||||
CS = car.CarState()
|
||||
CS.buttonEvents = events or []
|
||||
return CS
|
||||
|
||||
def _run_ctrl_frames(self, frames: list[car.CarState]) -> None:
|
||||
for cs in frames:
|
||||
self.tracker.update(cs)
|
||||
|
||||
def test_button_confirm_via_tracker(self) -> None:
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
|
||||
self._make_cs(),
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
|
||||
self._make_cs(),
|
||||
self._make_cs(),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
|
||||
def test_rapid_press_release_between_polls(self) -> None:
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]),
|
||||
self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=False)]),
|
||||
self._make_cs(),
|
||||
self._make_cs(),
|
||||
self._make_cs(),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert self.sla._get_button_release(req_plus=False, req_minus=True)
|
||||
|
||||
def test_multiple_releases_between_polls(self) -> None:
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([
|
||||
ButtonEvent(type=ButtonType.accelCruise, pressed=True),
|
||||
ButtonEvent(type=ButtonType.decelCruise, pressed=True),
|
||||
]),
|
||||
self._make_cs([
|
||||
ButtonEvent(type=ButtonType.accelCruise, pressed=False),
|
||||
ButtonEvent(type=ButtonType.decelCruise, pressed=False),
|
||||
]),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
assert self.sla._get_button_release(req_plus=False, req_minus=True)
|
||||
|
||||
def test_no_false_positive_same_toggle(self) -> None:
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert not self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
assert not self.sla._get_button_release(req_plus=False, req_minus=True)
|
||||
|
||||
def test_button_confirm_expires(self) -> None:
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
time.sleep(CRUISE_BUTTON_CONFIRM_HOLD + 0.1)
|
||||
assert not self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
|
||||
@@ -1,26 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car import structs
|
||||
|
||||
|
||||
class ButtonStateTracker:
|
||||
def __init__(self) -> None:
|
||||
self.pressed: int = 0
|
||||
self.release_toggle: int = 0
|
||||
|
||||
def update(self, CS: structs.CarState) -> None:
|
||||
for b in CS.buttonEvents:
|
||||
bit = 1 << b.type.raw
|
||||
if b.pressed:
|
||||
self.pressed |= bit
|
||||
else:
|
||||
self.pressed &= ~bit
|
||||
self.release_toggle ^= bit
|
||||
|
||||
def publish(self, ss_sp) -> None:
|
||||
ss_sp.buttonsPressed = self.pressed
|
||||
ss_sp.buttonsReleaseToggle = self.release_toggle
|
||||
@@ -1,67 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class TestButtonStateTracker:
|
||||
def setup_method(self) -> None:
|
||||
self.tracker = ButtonStateTracker()
|
||||
|
||||
def make_cs(self, events: list) -> car.CarState:
|
||||
CS = car.CarState()
|
||||
CS.buttonEvents = events
|
||||
return CS
|
||||
|
||||
def test_initial_state(self) -> None:
|
||||
assert self.tracker.pressed == 0
|
||||
assert self.tracker.release_toggle == 0
|
||||
|
||||
def test_press_sets_bit(self) -> None:
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
|
||||
assert self.tracker.pressed == (1 << ButtonType.accelCruise)
|
||||
assert self.tracker.release_toggle == 0
|
||||
|
||||
def test_release_clears_and_toggles(self) -> None:
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
|
||||
assert self.tracker.pressed == 0
|
||||
assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
|
||||
|
||||
def test_multiple_buttons(self) -> None:
|
||||
self.tracker.update(self.make_cs([
|
||||
ButtonEvent(type=ButtonType.accelCruise, pressed=True),
|
||||
ButtonEvent(type=ButtonType.decelCruise, pressed=True),
|
||||
]))
|
||||
assert self.tracker.pressed == (1 << ButtonType.accelCruise) | (1 << ButtonType.decelCruise)
|
||||
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
|
||||
assert self.tracker.pressed == (1 << ButtonType.decelCruise)
|
||||
assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
|
||||
|
||||
def test_release_toggle_flips(self) -> None:
|
||||
for _ in range(2):
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]))
|
||||
assert self.tracker.release_toggle == 0
|
||||
|
||||
def test_publish(self) -> None:
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
|
||||
|
||||
class MockSP:
|
||||
buttonsPressed = 0
|
||||
buttonsReleaseToggle = 0
|
||||
|
||||
sp = MockSP()
|
||||
self.tracker.publish(sp)
|
||||
assert sp.buttonsPressed == self.tracker.pressed
|
||||
assert sp.buttonsReleaseToggle == self.tracker.release_toggle
|
||||
@@ -2161,42 +2161,6 @@
|
||||
"type": "offroad_only"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "TeslaMadsScreenButton",
|
||||
"widget": "multiple_button",
|
||||
"title": "MADS Screen Activation",
|
||||
"description": "Use a multi-finger press on the infotainment screen to toggle MADS. This allows the use of full MADS functionality when enabled. Selecting a higher finger count may reduce accidental activations. Note: Setting this to Off will reset your MADS settings to default.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Off"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "3-Finger"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "4-Finger"
|
||||
},
|
||||
{
|
||||
"value": 3,
|
||||
"label": "5-Finger"
|
||||
}
|
||||
],
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "tesla_has_vehicle_bus",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
|
||||
@@ -56,28 +56,6 @@ sections:
|
||||
title: Cooperative Steering (Beta)
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- key: TeslaMadsScreenButton
|
||||
widget: multiple_button
|
||||
title: MADS Screen Activation
|
||||
description: 'Use a multi-finger press on the infotainment screen to toggle MADS.
|
||||
This allows the use of full MADS functionality when enabled. Selecting a higher
|
||||
finger count may reduce accidental activations. Note: Setting this to Off will
|
||||
reset your MADS settings to default.'
|
||||
options:
|
||||
- value: 0
|
||||
label: 'Off'
|
||||
- value: 1
|
||||
label: 3-Finger
|
||||
- value: 2
|
||||
label: 4-Finger
|
||||
- value: 3
|
||||
label: 5-Finger
|
||||
visibility:
|
||||
- type: capability
|
||||
field: tesla_has_vehicle_bus
|
||||
equals: true
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- id: toyota
|
||||
title: Toyota / Lexus Settings
|
||||
description: ''
|
||||
|
||||
@@ -17,26 +17,6 @@ ONROAD_BRIGHTNESS_TIMER_VALUES = {0: 3, 1: 5, 2: 7, 3: 10, 4: 15, 5: 30, **{i: (
|
||||
VALID_TIMER_VALUES = set(ONROAD_BRIGHTNESS_TIMER_VALUES.values())
|
||||
|
||||
|
||||
def _resolve_brand(_params) -> str:
|
||||
bundle = _params.get("CarPlatformBundle")
|
||||
if isinstance(bundle, dict) and bundle.get("brand"):
|
||||
return str(bundle["brand"])
|
||||
|
||||
# Auto-fingerprinted cars have no bundle, fall back to the last known CarParams.
|
||||
CP_bytes = _params.get("CarParamsPersistent")
|
||||
if CP_bytes is None:
|
||||
return ""
|
||||
|
||||
# Never raises: callers rely on "" to mean "brand unknown, skip the migration".
|
||||
try:
|
||||
from openpilot.cereal import messaging # lazy: avoids heavy import at module level
|
||||
from opendbc.car.structs import car
|
||||
return str(messaging.log_from_bytes(CP_bytes, car.CarParams).brand)
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"params_migration: failed to resolve brand from CarParamsPersistent: {e}")
|
||||
return ""
|
||||
|
||||
|
||||
def _migrate_car_platform_bundle(_params):
|
||||
bundle = _params.get("CarPlatformBundle")
|
||||
if bundle is None:
|
||||
@@ -67,23 +47,6 @@ def _migrate_car_platform_bundle(_params):
|
||||
cloudlog.info(f"params_migration: CarPlatformBundle migrated {old_platform!r} -> {new_platform!r}")
|
||||
|
||||
|
||||
def _migrate_tesla_mads_screen_button(_params):
|
||||
# TeslaMadsScreenButton defaults to Off for fresh installs, but the screen button was previously always
|
||||
# active on Teslas with a vehicle bus. Seed existing Tesla installs with 3-finger to preserve that.
|
||||
try:
|
||||
if _params.get("TeslaMadsScreenButton") is not None:
|
||||
return
|
||||
|
||||
if _resolve_brand(_params) != "tesla":
|
||||
return
|
||||
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType # lazy: avoids heavy import at module level
|
||||
_params.put("TeslaMadsScreenButton", MadsScreenButtonType.THREE_FINGER, block=True)
|
||||
cloudlog.info("params_migration: seeded TeslaMadsScreenButton with 3-finger to preserve existing behavior")
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"Error migrating TeslaMadsScreenButton: {e}")
|
||||
|
||||
|
||||
def run_migration(_params):
|
||||
# migrate OnroadScreenOffBrightness
|
||||
if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
|
||||
@@ -117,6 +80,3 @@ def run_migration(_params):
|
||||
cloudlog.exception(f"Error migrating OnroadScreenOffTimer: {e}")
|
||||
|
||||
_migrate_car_platform_bundle(_params)
|
||||
|
||||
# seed TeslaMadsScreenButton for existing Tesla installs
|
||||
_migrate_tesla_mads_screen_button(_params)
|
||||
|
||||
@@ -95,6 +95,9 @@ def is_stock_model(started, params, CP: car.CarParams) -> bool:
|
||||
"""Check if the active model runner is stock."""
|
||||
return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.stock)
|
||||
|
||||
def not_wgpu(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return not params.get_bool("WgpuEnabled")
|
||||
|
||||
def mapd_ready(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return bool(os.path.exists(Paths.mapd_root()))
|
||||
|
||||
@@ -128,7 +131,7 @@ procs = [
|
||||
PythonProcess("micd", "openpilot.system.micd", iscar),
|
||||
PythonProcess("timed", "openpilot.system.timed", always_run, enabled=not PC),
|
||||
|
||||
PythonProcess("modeld", "openpilot.selfdrive.modeld.modeld", and_(only_onroad, is_stock_model)),
|
||||
PythonProcess("modeld", "openpilot.selfdrive.modeld.modeld", and_(and_(only_onroad, is_stock_model), not_wgpu)),
|
||||
PythonProcess("dmonitoringmodeld", "openpilot.selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)),
|
||||
|
||||
PythonProcess("sensord", "openpilot.system.sensord.sensord", only_onroad, enabled=not PC),
|
||||
@@ -177,7 +180,7 @@ procs = [
|
||||
procs += [
|
||||
# Models
|
||||
PythonProcess("models_manager", "openpilot.sunnypilot.models.manager", only_offroad),
|
||||
NativeProcess("modeld_tinygrad", "openpilot/sunnypilot/modeld_v2", ["./modeld"], and_(only_onroad, is_tinygrad_model)),
|
||||
NativeProcess("modeld_tinygrad", "openpilot/sunnypilot/modeld_v2", ["./modeld"], and_(and_(only_onroad, is_tinygrad_model), not_wgpu)),
|
||||
|
||||
# Backup
|
||||
PythonProcess("backup_manager", "openpilot.sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)),
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import argparse
|
||||
import multiprocessing
|
||||
import time
|
||||
@@ -10,6 +9,7 @@ from collections import deque
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from msgq.visionipc import VisionIpcServer, VisionStreamType
|
||||
from openpilot.tools.camerastream.ffmpeg_decoder import Decoder, FFmpegError
|
||||
from openpilot.tools.wgpu.zmq import ZmqSubMaster, ZmqSubSocket
|
||||
|
||||
V4L2_BUF_FLAG_KEYFRAME = 8
|
||||
|
||||
@@ -30,10 +30,7 @@ def decoder(addr, vipc_server, vst, W, H, debug=False):
|
||||
|
||||
codec = Decoder("hevc")
|
||||
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
sock = messaging.sub_sock(sock_name, None, addr=addr, conflate=False)
|
||||
cnt = 0
|
||||
sock = ZmqSubSocket(sock_name, addr)
|
||||
last_idx = -1
|
||||
seen_iframe = False
|
||||
|
||||
@@ -46,8 +43,9 @@ def decoder(addr, vipc_server, vst, W, H, debug=False):
|
||||
time_q.clear()
|
||||
|
||||
while 1:
|
||||
msgs = messaging.drain_sock(sock, wait_for_one=True)
|
||||
for evt in msgs:
|
||||
msgs = sock.drain(wait_for_one=True)
|
||||
for raw in msgs:
|
||||
evt = messaging.log_from_bytes(raw)
|
||||
evta = getattr(evt, evt.which())
|
||||
if last_idx != -1 and evta.idx.encodeId != (last_idx + 1):
|
||||
if debug:
|
||||
@@ -94,8 +92,9 @@ def decoder(addr, vipc_server, vst, W, H, debug=False):
|
||||
continue
|
||||
|
||||
frame_start_time = time_q.popleft()
|
||||
vipc_server.send(vst, img_yuv.data, cnt, int(frame_start_time*1e9), int(time.monotonic()*1e9))
|
||||
cnt += 1
|
||||
# Preserve the device camera metadata so remote model outputs line up with
|
||||
# the rest of the device's cereal timeline.
|
||||
vipc_server.send(vst, img_yuv.data, evta.idx.frameId, evta.idx.timestampSof, evta.idx.timestampEof)
|
||||
|
||||
pc_latency = (time.monotonic()-frame_start_time)*1000
|
||||
if debug:
|
||||
@@ -105,25 +104,30 @@ def decoder(addr, vipc_server, vst, W, H, debug=False):
|
||||
|
||||
class CompressedVipc:
|
||||
def __init__(self, addr, vision_streams, server_name, debug=False):
|
||||
print("getting frame sizes")
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
sm = messaging.SubMaster([ENCODE_SOCKETS[s] for s in vision_streams], addr=addr)
|
||||
print("waiting for remote camera stream metadata", flush=True)
|
||||
sm = ZmqSubMaster([ENCODE_SOCKETS[s] for s in vision_streams], addr)
|
||||
while min(sm.recv_frame.values()) == 0:
|
||||
sm.update(100)
|
||||
os.environ.pop("ZMQ")
|
||||
messaging.reset_context()
|
||||
|
||||
stream_dimensions = {
|
||||
vst: (sm[ENCODE_SOCKETS[vst]].width, sm[ENCODE_SOCKETS[vst]].height)
|
||||
for vst in vision_streams
|
||||
}
|
||||
# The metadata subscribers are setup-only. Leaving them connected creates a
|
||||
# second unread camera subscription whose TCP queues grow for the entire run.
|
||||
sm.close()
|
||||
|
||||
self.vipc_server = VisionIpcServer(server_name)
|
||||
for vst in vision_streams:
|
||||
ed = sm[ENCODE_SOCKETS[vst]]
|
||||
self.vipc_server.create_buffers(vst, 4, ed.width, ed.height)
|
||||
width, height = stream_dimensions[vst]
|
||||
self.vipc_server.create_buffers(vst, 4, width, height)
|
||||
self.vipc_server.start_listener()
|
||||
|
||||
self.procs = []
|
||||
process_context = multiprocessing.get_context("fork")
|
||||
for vst in vision_streams:
|
||||
ed = sm[ENCODE_SOCKETS[vst]]
|
||||
p = multiprocessing.Process(target=decoder, args=(addr, self.vipc_server, vst, ed.width, ed.height, debug))
|
||||
width, height = stream_dimensions[vst]
|
||||
p = process_context.Process(target=decoder, args=(addr, self.vipc_server, vst, width, height, debug))
|
||||
p.start()
|
||||
self.procs.append(p)
|
||||
|
||||
|
||||
@@ -46,10 +46,23 @@ def _bind(fn, restype, *argtypes):
|
||||
return fn
|
||||
|
||||
|
||||
def _library_path(name: str, major: int) -> str:
|
||||
candidates = (
|
||||
f"lib{name}.so.{major}",
|
||||
f"lib{name}.{major}.dylib",
|
||||
f"lib{name}.dylib",
|
||||
)
|
||||
for candidate in candidates:
|
||||
path = os.path.join(ffmpeg.LIB_DIR, candidate)
|
||||
if os.path.isfile(path):
|
||||
return path
|
||||
raise FileNotFoundError(f"FFmpeg library not found in {ffmpeg.LIB_DIR}: {', '.join(candidates)}")
|
||||
|
||||
|
||||
def _load_libraries():
|
||||
avutil = ctypes.CDLL(os.path.join(ffmpeg.LIB_DIR, "libavutil.so.59"), mode=ctypes.RTLD_GLOBAL)
|
||||
avcodec = ctypes.CDLL(os.path.join(ffmpeg.LIB_DIR, "libavcodec.so.61"), mode=ctypes.RTLD_GLOBAL)
|
||||
swscale = ctypes.CDLL(os.path.join(ffmpeg.LIB_DIR, "libswscale.so.8"), mode=ctypes.RTLD_GLOBAL)
|
||||
avutil = ctypes.CDLL(_library_path("avutil", 59), mode=ctypes.RTLD_GLOBAL)
|
||||
avcodec = ctypes.CDLL(_library_path("avcodec", 61), mode=ctypes.RTLD_GLOBAL)
|
||||
swscale = ctypes.CDLL(_library_path("swscale", 8), mode=ctypes.RTLD_GLOBAL)
|
||||
|
||||
c_int, c_char_p, c_void_p, c_size_t = ctypes.c_int, ctypes.c_char_p, ctypes.c_void_p, ctypes.c_size_t
|
||||
c_uint8_p = ctypes.POINTER(ctypes.c_uint8)
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
# Wireless modeld proof of concept
|
||||
|
||||
This runs driving `modeld` on a laptop and returns its cereal outputs to a comma
|
||||
device over the existing Wi-Fi network. It reuses the existing HEVC camera
|
||||
stream, VisionIPC decoder, and cereal ZMQ bridge.
|
||||
|
||||
This is for controlled bench testing only. Wi-Fi has no deterministic latency
|
||||
or availability guarantee. The device-side helper switches only after receiving
|
||||
a fresh remote model and after manager has stopped the local model publisher.
|
||||
It restores local `modeld` if the remote model is missing for one second.
|
||||
|
||||
## Build
|
||||
|
||||
Use the same commit on the laptop and comma device. Build the cereal bridge on
|
||||
the device:
|
||||
|
||||
```sh
|
||||
scons -u openpilot/cereal/messaging/bridge
|
||||
```
|
||||
|
||||
Build and test the normal model on the laptop first:
|
||||
|
||||
```sh
|
||||
PATH="$PWD/.venv/bin:$PATH" scons -u
|
||||
```
|
||||
|
||||
To compile the big external-GPU model for the laptop's local tinygrad backend:
|
||||
|
||||
```sh
|
||||
PATH="$PWD/.venv/bin:$PATH" WGPU=1 scons -u \
|
||||
openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl.chunkmanifest
|
||||
```
|
||||
|
||||
On macOS, the build selects tinygrad's Metal backend when it is available.
|
||||
On an 8-GPU-core M5 MacBook Air, the small model's compiled policy pass measured
|
||||
about 6–11 ms, while the big model measured about 79–81 ms. The latter already
|
||||
misses the 50 ms model cadence before network and codec latency, so start with
|
||||
the small model on that class of laptop.
|
||||
|
||||
## Run
|
||||
|
||||
Find the laptop's LAN IP address that the comma device can reach. The helper can
|
||||
start while onroad: it forwards camera/state while local `modeld` remains active,
|
||||
then performs an exclusive publisher handoff after the laptop produces a fresh
|
||||
valid model:
|
||||
|
||||
```sh
|
||||
cd /data/openpilot
|
||||
python3 -m openpilot.tools.wgpu.device LAPTOP_IP
|
||||
```
|
||||
|
||||
Keep that terminal open. On the laptop, run:
|
||||
|
||||
```sh
|
||||
cd /path/to/openpilot
|
||||
python3 -m openpilot.tools.wgpu.host COMMA_IP
|
||||
```
|
||||
|
||||
Add `--big-model` after `COMMA_IP` to use the locally compiled big model.
|
||||
|
||||
The device helper sends cached `carParams` over a dedicated startup channel, so
|
||||
the host does not attach to the camera streams and accumulate stale frames while
|
||||
waiting for the periodic state bridge. Stop either side with Ctrl+C. A host
|
||||
disconnect automatically stops remote publication and restores local `modeld`
|
||||
after a one-second timeout; the device helper then stays running and waits for
|
||||
the next host session. Stop the device helper itself with Ctrl+C before changing
|
||||
branches or rebooting.
|
||||
|
||||
While WGPU is active, model lag does not create an engagement-blocking alert.
|
||||
The fixed-column diagnostics panel at the top-right of the mici onroad UI shows
|
||||
the active source (`LOCAL` or `WGPU`), remote model size (`SMALL` or `BIG`),
|
||||
total model-frame age, execution time, remaining I/O and queue time, and dropped
|
||||
frames. The home GPU icon is gray while the device helper is ready and waiting,
|
||||
and green while a valid remote model is active.
|
||||
@@ -0,0 +1,152 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import signal
|
||||
import subprocess
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.tools.wgpu.zmq import WGPU_CAR_PARAMS, ZmqPubMaster, ZmqSubSocket
|
||||
|
||||
|
||||
MODEL_OUTPUTS = "modelV2,drivingModelData,cameraOdometry,modelDataV2SP"
|
||||
MODEL_PROCESSES = {"modeld", "modeld_tinygrad"}
|
||||
REMOTE_MODEL_TIMEOUT = 1.0
|
||||
WGPU_STATUS = "wgpuStatus"
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
BRIDGE = ROOT / "openpilot/cereal/messaging/bridge"
|
||||
|
||||
|
||||
def stop_process(proc: subprocess.Popen) -> None:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
proc.kill()
|
||||
proc.wait()
|
||||
|
||||
|
||||
def handle_sigterm(*_) -> None:
|
||||
raise KeyboardInterrupt
|
||||
|
||||
|
||||
def receive_model(sock: ZmqSubSocket) -> tuple[bool, float]:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
return False, float("inf")
|
||||
event = messaging.log_from_bytes(raw)
|
||||
if event.which() != "modelV2" or not event.valid:
|
||||
return True, float("inf")
|
||||
model_age = (time.monotonic_ns() - event.modelV2.timestampEof) / 1e9
|
||||
return True, model_age
|
||||
|
||||
|
||||
def receive_model_name(sock: ZmqSubSocket) -> str | None:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
return None
|
||||
model_name = raw.decode(errors="replace").upper()
|
||||
return model_name if model_name in ("SMALL", "BIG") else None
|
||||
|
||||
|
||||
def wait_for_local_modeld_stop(timeout: float = 10.) -> None:
|
||||
sm = messaging.SubMaster(["managerState"])
|
||||
deadline = time.monotonic() + timeout
|
||||
while time.monotonic() < deadline:
|
||||
sm.update(100)
|
||||
if sm.seen["managerState"]:
|
||||
running = {p.name for p in sm["managerState"].processes if p.running}
|
||||
if not running.intersection(MODEL_PROCESSES):
|
||||
return
|
||||
raise RuntimeError("timed out waiting for local modeld publisher to stop")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Route modeld traffic between this device and a wireless host.")
|
||||
parser.add_argument("host", help="Laptop IP address reachable from this device")
|
||||
args = parser.parse_args()
|
||||
|
||||
if not BRIDGE.is_file():
|
||||
raise FileNotFoundError(f"build the cereal bridge first: {BRIDGE}")
|
||||
|
||||
params = Params()
|
||||
forward: subprocess.Popen | None = None
|
||||
reverse: subprocess.Popen | None = None
|
||||
wgpu_enabled = False
|
||||
try:
|
||||
forward = subprocess.Popen([str(BRIDGE)])
|
||||
params.put_bool("WgpuReady", True, block=True)
|
||||
car_params_pub = ZmqPubMaster([WGPU_CAR_PARAMS])
|
||||
remote_model = ZmqSubSocket("modelV2", args.host, conflate=True)
|
||||
remote_status = ZmqSubSocket(WGPU_STATUS, args.host, conflate=True)
|
||||
car_params = params.get("CarParams") or params.get("CarParamsPersistent")
|
||||
|
||||
while True:
|
||||
# Keep local modeld publishing while a host connects and warms up.
|
||||
print(f"forwarding camera/state to {args.host}; waiting for a fresh remote model")
|
||||
model_name = None
|
||||
while True:
|
||||
if car_params is None:
|
||||
car_params = params.get("CarParams") or params.get("CarParamsPersistent")
|
||||
if car_params is not None:
|
||||
# This dedicated conflated startup channel lets remote modeld obtain CP
|
||||
# before it connects to VisionIPC and accumulates stale frame metadata.
|
||||
car_params_pub.send_raw(WGPU_CAR_PARAMS, car_params)
|
||||
received, model_age = receive_model(remote_model)
|
||||
model_name = receive_model_name(remote_status) or model_name
|
||||
if received and 0 <= model_age < REMOTE_MODEL_TIMEOUT and model_name is not None:
|
||||
break
|
||||
if forward.poll() is not None:
|
||||
raise RuntimeError(f"forward bridge exited with status {forward.returncode}")
|
||||
time.sleep(0.05)
|
||||
|
||||
# Stop the local publisher before attaching the reverse bridge. msgq permits
|
||||
# only one publisher for each model service.
|
||||
params.put("WgpuModelName", model_name, block=True)
|
||||
params.put_bool("WgpuEnabled", True, block=True)
|
||||
wgpu_enabled = True
|
||||
wait_for_local_modeld_stop()
|
||||
reverse = subprocess.Popen([str(BRIDGE), args.host, MODEL_OUTPUTS])
|
||||
print("wgpu active; Ctrl+C or loss of the remote model restores local modeld")
|
||||
|
||||
last_remote_model = time.monotonic()
|
||||
while True:
|
||||
received, _ = receive_model(remote_model)
|
||||
if received:
|
||||
last_remote_model = time.monotonic()
|
||||
if forward.poll() is not None:
|
||||
raise RuntimeError(f"forward bridge exited with status {forward.returncode}")
|
||||
if reverse.poll() is not None:
|
||||
print(f"reverse bridge exited with status {reverse.returncode}; restoring local modeld")
|
||||
break
|
||||
if time.monotonic() - last_remote_model > REMOTE_MODEL_TIMEOUT:
|
||||
print("remote model timed out; restoring local modeld")
|
||||
break
|
||||
time.sleep(0.05)
|
||||
|
||||
stop_process(reverse)
|
||||
reverse = None
|
||||
params.put_bool("WgpuEnabled", False, block=True)
|
||||
wgpu_enabled = False
|
||||
params.remove("WgpuModelName")
|
||||
print("wgpu disabled; local modeld restored; ready for the next host run")
|
||||
finally:
|
||||
# Stop remote publication before allowing the local publisher to restart.
|
||||
if reverse is not None and reverse.poll() is None:
|
||||
stop_process(reverse)
|
||||
if wgpu_enabled:
|
||||
params.put_bool("WgpuEnabled", False, block=True)
|
||||
params.put_bool("WgpuReady", False, block=True)
|
||||
params.remove("WgpuModelName")
|
||||
if forward is not None and forward.poll() is None:
|
||||
stop_process(forward)
|
||||
print("wgpu disabled; local modeld restored")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
signal.signal(signal.SIGTERM, handle_sigterm)
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
@@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.tools.wgpu.zmq import ZmqPubMaster
|
||||
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
CAMERASTREAM = ROOT / "openpilot/tools/camerastream/compressed_vipc.py"
|
||||
WGPU_STATUS = "wgpuStatus"
|
||||
|
||||
|
||||
def stop_process(proc: subprocess.Popen) -> None:
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
proc.wait()
|
||||
return
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
os.killpg(proc.pid, signal.SIGKILL)
|
||||
proc.wait()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Run modeld on this host for a remote comma device.")
|
||||
parser.add_argument("device", help="comma device hostname or IP address")
|
||||
parser.add_argument("--big-model", action="store_true", help="use the locally compiled big driving model")
|
||||
args = parser.parse_args()
|
||||
|
||||
camera = subprocess.Popen([sys.executable, str(CAMERASTREAM), args.device, "--cams", "0,2"], start_new_session=True)
|
||||
model_args = [sys.executable, "-m", "openpilot.selfdrive.modeld.modeld", "--remote", args.device]
|
||||
if args.big_model:
|
||||
model_args.append("--big-model")
|
||||
model = subprocess.Popen(model_args, cwd=ROOT, start_new_session=True)
|
||||
status = ZmqPubMaster([WGPU_STATUS])
|
||||
model_name = b"BIG" if args.big_model else b"SMALL"
|
||||
|
||||
procs = {"camera bridge": camera, "modeld": model}
|
||||
try:
|
||||
while all(proc.poll() is None for proc in procs.values()):
|
||||
status.send_raw(WGPU_STATUS, model_name)
|
||||
time.sleep(0.25)
|
||||
failed_name, failed = next((name, proc) for name, proc in procs.items() if proc.poll() is not None)
|
||||
raise RuntimeError(f"wgpu {failed_name} exited with status {failed.returncode}")
|
||||
finally:
|
||||
for proc in procs.values():
|
||||
if proc.poll() is None:
|
||||
stop_process(proc)
|
||||
print("wgpu host stopped")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
@@ -0,0 +1,105 @@
|
||||
import time
|
||||
|
||||
import zmq
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
|
||||
|
||||
WGPU_CAR_PARAMS = "wgpuCarParams"
|
||||
|
||||
|
||||
def service_port(endpoint: str) -> int:
|
||||
# Keep this in sync with cereal/messaging/bridge_zmq.cc.
|
||||
value = 0xcbf29ce484222325
|
||||
for char in endpoint.encode():
|
||||
value ^= char
|
||||
value = (value * 0x100000001b3) & 0xffffffffffffffff
|
||||
return 8023 + (value % (65535 - 8023))
|
||||
|
||||
|
||||
class ZmqSubSocket:
|
||||
def __init__(self, endpoint: str, address: str, conflate: bool = False):
|
||||
self.context = zmq.Context()
|
||||
self.socket = self.context.socket(zmq.SUB)
|
||||
self.socket.setsockopt(zmq.SUBSCRIBE, b"")
|
||||
self.socket.setsockopt(zmq.RECONNECT_IVL_MAX, 500)
|
||||
if conflate:
|
||||
self.socket.setsockopt(zmq.CONFLATE, 1)
|
||||
self.socket.connect(f"tcp://{address}:{service_port(endpoint)}")
|
||||
|
||||
def receive(self, non_blocking: bool = False) -> bytes | None:
|
||||
try:
|
||||
return self.socket.recv(flags=zmq.NOBLOCK if non_blocking else 0)
|
||||
except zmq.Again:
|
||||
return None
|
||||
|
||||
def drain(self, wait_for_one: bool = False) -> list[bytes]:
|
||||
messages = []
|
||||
if wait_for_one:
|
||||
message = self.receive()
|
||||
if message is not None:
|
||||
messages.append(message)
|
||||
while (message := self.receive(non_blocking=True)) is not None:
|
||||
messages.append(message)
|
||||
return messages
|
||||
|
||||
def close(self) -> None:
|
||||
self.socket.close(linger=0)
|
||||
self.context.term()
|
||||
|
||||
|
||||
class ZmqSubMaster:
|
||||
def __init__(self, services: list[str], address: str):
|
||||
self.services = services
|
||||
self.sockets = {service: ZmqSubSocket(service, address, conflate=True) for service in services}
|
||||
self.poller = zmq.Poller()
|
||||
self.socket_to_service = {}
|
||||
for service, sub in self.sockets.items():
|
||||
self.poller.register(sub.socket, zmq.POLLIN)
|
||||
self.socket_to_service[sub.socket] = service
|
||||
|
||||
self.data = {service: getattr(messaging.new_message(service).as_reader(), service) for service in services}
|
||||
self.seen = dict.fromkeys(services, False)
|
||||
self.updated = dict.fromkeys(services, False)
|
||||
self.recv_frame = dict.fromkeys(services, 0)
|
||||
self.frame = -1
|
||||
|
||||
def __getitem__(self, service: str):
|
||||
return self.data[service]
|
||||
|
||||
def update(self, timeout: int = 100) -> None:
|
||||
self.frame += 1
|
||||
self.updated = dict.fromkeys(self.services, False)
|
||||
for socket, _ in self.poller.poll(timeout):
|
||||
service = self.socket_to_service[socket]
|
||||
raw = self.sockets[service].receive(non_blocking=True)
|
||||
if raw is None:
|
||||
continue
|
||||
event = messaging.log_from_bytes(raw)
|
||||
self.data[service] = getattr(event, service)
|
||||
self.seen[service] = True
|
||||
self.updated[service] = True
|
||||
self.recv_frame[service] = self.frame
|
||||
|
||||
def close(self) -> None:
|
||||
for sub in self.sockets.values():
|
||||
self.poller.unregister(sub.socket)
|
||||
sub.close()
|
||||
|
||||
|
||||
class ZmqPubMaster:
|
||||
def __init__(self, services: list[str]):
|
||||
context = zmq.Context.instance()
|
||||
self.sockets = {}
|
||||
for service in services:
|
||||
socket = context.socket(zmq.PUB)
|
||||
socket.bind(f"tcp://*:{service_port(service)}")
|
||||
self.sockets[service] = socket
|
||||
# Give already-running bridge subscribers time to finish their handshake.
|
||||
time.sleep(0.1)
|
||||
|
||||
def send(self, service: str, message) -> None:
|
||||
self.sockets[service].send(message.to_bytes(), flags=zmq.NOBLOCK)
|
||||
|
||||
def send_raw(self, service: str, data: bytes) -> None:
|
||||
self.sockets[service].send(data, flags=zmq.NOBLOCK)
|
||||
Executable
+30
@@ -0,0 +1,30 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
SCRIPT_DIR=$(dirname "$0")
|
||||
OPENPILOT_DIR=$SCRIPT_DIR/../../
|
||||
|
||||
DOCKER_IMAGE=sunnypilot
|
||||
DOCKER_FILE=Dockerfile.openpilot
|
||||
DOCKER_REGISTRY=ghcr.io/sunnypilot
|
||||
COMMIT_SHA=$(git rev-parse HEAD)
|
||||
|
||||
if [ -n "$TARGET_ARCHITECTURE" ]; then
|
||||
PLATFORM="linux/$TARGET_ARCHITECTURE"
|
||||
TAG_SUFFIX="-$TARGET_ARCHITECTURE"
|
||||
else
|
||||
PLATFORM="linux/$(uname -m)"
|
||||
TAG_SUFFIX=""
|
||||
fi
|
||||
|
||||
LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX
|
||||
REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG
|
||||
REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA
|
||||
|
||||
DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR
|
||||
|
||||
if [ -n "$PUSH_IMAGE" ]; then
|
||||
docker push $REMOTE_TAG
|
||||
docker tag $REMOTE_TAG $REMOTE_SHA_TAG
|
||||
docker push $REMOTE_SHA_TAG
|
||||
fi
|
||||
+109
-38
@@ -5,6 +5,8 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import os
|
||||
import pickle
|
||||
import sys
|
||||
import hashlib
|
||||
import json
|
||||
@@ -12,8 +14,46 @@ import re
|
||||
from pathlib import Path
|
||||
from datetime import datetime, UTC
|
||||
|
||||
REQUIRED_OUTPUT_KEYS = frozenset({
|
||||
"plan",
|
||||
"lane_lines",
|
||||
"road_edges",
|
||||
"lead",
|
||||
"desire_state",
|
||||
"desire_pred",
|
||||
"meta",
|
||||
"lead_prob",
|
||||
"lane_lines_prob",
|
||||
"pose",
|
||||
"wide_from_device_euler",
|
||||
"road_transform",
|
||||
"hidden_state",
|
||||
})
|
||||
OPTIONAL_OUTPUT_KEYS = frozenset({
|
||||
"planplus",
|
||||
"sim_pose",
|
||||
"desired_curvature",
|
||||
})
|
||||
|
||||
def create_short_name(full_name: str) -> str:
|
||||
|
||||
def validate_model_outputs(metadata_paths: list[Path]) -> None:
|
||||
combined_keys: set[str] = set()
|
||||
for path in metadata_paths:
|
||||
if path.stat().st_size == 0:
|
||||
print(f"skipping empty metadata: {path}")
|
||||
continue
|
||||
with open(path, "rb") as f:
|
||||
metadata = pickle.load(f)
|
||||
combined_keys.update(metadata.get("output_slices", {}).keys())
|
||||
missing = REQUIRED_OUTPUT_KEYS - combined_keys
|
||||
if missing:
|
||||
raise ValueError(f"Combined model metadata is missing required output keys: {sorted(missing)}")
|
||||
detected_optional = sorted(OPTIONAL_OUTPUT_KEYS & combined_keys)
|
||||
if detected_optional:
|
||||
print(f"Optional output keys detected: {detected_optional}")
|
||||
|
||||
|
||||
def create_short_name(full_name):
|
||||
# Remove parentheses and extract alphanumeric words
|
||||
clean_name = re.sub(r'\([^)]*\)', '', full_name)
|
||||
words = [re.sub(r'[^a-zA-Z0-9]', '', word) for word in clean_name.split() if re.sub(r'[^a-zA-Z0-9]', '', word)]
|
||||
@@ -81,41 +121,49 @@ def _rename_pkl_with_chunks(old_pkl: Path, new_pkl: Path) -> Path:
|
||||
return old_pkl.rename(new_pkl)
|
||||
|
||||
|
||||
def generate_chunked_model(driving_pkl: Path) -> dict:
|
||||
def generate_metadata(model_path: Path, output_dir: Path, short_name: str, driving_pkl: Path):
|
||||
base = model_path.stem
|
||||
metadata_file = output_dir / f"{base}_metadata.pkl"
|
||||
|
||||
if short_name:
|
||||
renamed_meta = output_dir / f"{base}_{short_name.lower()}_metadata.pkl"
|
||||
if metadata_file.exists() and not renamed_meta.exists():
|
||||
metadata_file = metadata_file.rename(renamed_meta)
|
||||
elif renamed_meta.exists():
|
||||
metadata_file = renamed_meta
|
||||
|
||||
if not metadata_file.exists():
|
||||
print(f"Warning: Missing metadata for {base} ({metadata_file}), skipping", file=sys.stderr)
|
||||
return
|
||||
|
||||
tinygrad_hash = hashlib.sha256(_read_pkl_bytes(driving_pkl)).hexdigest()
|
||||
|
||||
chunks_config = []
|
||||
manifest_file = Path(f"{driving_pkl}.chunkmanifest")
|
||||
if manifest_file.exists():
|
||||
num_chunks = int(manifest_file.read_text().strip())
|
||||
for i in range(num_chunks):
|
||||
chunk_path = Path(f"{driving_pkl}.chunk{i + 1:02d}of{num_chunks:02d}")
|
||||
if chunk_path.exists():
|
||||
chunk_hash = hashlib.sha256(chunk_path.read_bytes()).hexdigest()
|
||||
chunks_config.append({
|
||||
"file_name": chunk_path.name,
|
||||
"sha256": chunk_hash
|
||||
})
|
||||
with open(metadata_file, 'rb') as f:
|
||||
metadata_hash = hashlib.sha256(f.read()).hexdigest()
|
||||
|
||||
artifact_data = {
|
||||
"file_name": driving_pkl.name,
|
||||
"download_uri": {
|
||||
"url": "https://gitlab.com/sunnypilot/public/docs.sunnypilot.ai/-/raw/main/",
|
||||
"sha256": tinygrad_hash
|
||||
model_type = "offPolicy" if "off_policy" in base else "onPolicy" if "on_policy" in base else base.split("_")[-1]
|
||||
|
||||
return {
|
||||
"type": model_type,
|
||||
"artifact": {
|
||||
"file_name": driving_pkl.name,
|
||||
"download_uri": {
|
||||
"url": "https://gitlab.com/sunnypilot/public/docs.sunnypilot.ai/-/raw/main/",
|
||||
"sha256": tinygrad_hash
|
||||
}
|
||||
},
|
||||
"metadata": {
|
||||
"file_name": metadata_file.name,
|
||||
"download_uri": {
|
||||
"url": "https://gitlab.com/sunnypilot/public/docs.sunnypilot.ai/-/raw/main/",
|
||||
"sha256": metadata_hash
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if chunks_config:
|
||||
artifact_data["chunks"] = chunks_config
|
||||
|
||||
return {
|
||||
"type": "chunked",
|
||||
"artifact": artifact_data,
|
||||
}
|
||||
|
||||
|
||||
def create_metadata_json(models: list, output_dir: Path, custom_name=None, short_name=None, is_20hz=False, upstream_branch="unknown") -> None:
|
||||
bundle_json = {
|
||||
def create_metadata_json(models: list, output_dir: Path, custom_name=None, short_name=None, is_20hz=False, upstream_branch="unknown"):
|
||||
metadata_json = {
|
||||
"short_name": short_name,
|
||||
"display_name": custom_name or upstream_branch,
|
||||
"is_20hz": is_20hz,
|
||||
@@ -131,26 +179,40 @@ def create_metadata_json(models: list, output_dir: Path, custom_name=None, short
|
||||
}
|
||||
|
||||
# Write metadata to output_dir
|
||||
metadata_json = {
|
||||
"bundles": [bundle_json]
|
||||
}
|
||||
|
||||
with open(output_dir / "metadata.json", "w") as f:
|
||||
json.dump(metadata_json, f, indent=2)
|
||||
print("Generated metadata.json")
|
||||
|
||||
print(f"Generated metadata.json with {len(models)} models.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
import glob
|
||||
|
||||
parser = argparse.ArgumentParser(description="Generate metadata JSON for the compiled JIT model")
|
||||
parser.add_argument("--model-dir", default="./models", help="Directory containing the model files")
|
||||
parser = argparse.ArgumentParser(description="Generate metadata for model files")
|
||||
parser.add_argument("--model-dir", default="./models", help="Directory containing ONNX model files")
|
||||
parser.add_argument("--output-dir", default="./output", help="Output directory for metadata")
|
||||
parser.add_argument("--custom-name", help="Custom display name for the model")
|
||||
parser.add_argument("--is-20hz", action="store_true", help="Whether this is a 20Hz model")
|
||||
parser.add_argument("--validate-only", action="store_true")
|
||||
parser.add_argument("--upstream-branch", default="unknown", help="Upstream branch name")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.validate_only:
|
||||
metadata_paths = glob.glob(os.path.join(args.model_dir, "*_metadata.pkl"))
|
||||
if not metadata_paths:
|
||||
print(f"No metadata files found in {args.model_dir}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
validate_model_outputs([Path(p) for p in metadata_paths])
|
||||
print(f"Validated {len(metadata_paths)} metadata files successfully.")
|
||||
sys.exit(0)
|
||||
|
||||
# Find all ONNX files in the given directory
|
||||
model_paths = glob.glob(os.path.join(args.model_dir, "*.onnx"))
|
||||
if not model_paths:
|
||||
print(f"No ONNX files found in {args.model_dir}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
|
||||
_output_dir = Path(args.output_dir)
|
||||
_output_dir.mkdir(exist_ok=True, parents=True)
|
||||
_short_name = create_short_name(args.custom_name) if args.custom_name else None
|
||||
@@ -167,5 +229,14 @@ if __name__ == "__main__":
|
||||
else:
|
||||
_driving_pkl = new_pkl
|
||||
|
||||
_model_metadata = generate_chunked_model(_driving_pkl)
|
||||
create_metadata_json([_model_metadata], _output_dir, args.custom_name, _short_name, args.is_20hz, args.upstream_branch)
|
||||
_models = []
|
||||
|
||||
for _model_path in model_paths:
|
||||
_model_metadata = generate_metadata(Path(_model_path), _output_dir, _short_name, _driving_pkl)
|
||||
if _model_metadata:
|
||||
_models.append(_model_metadata)
|
||||
|
||||
if _models:
|
||||
create_metadata_json(_models, _output_dir, args.custom_name, _short_name, args.is_20hz, args.upstream_branch)
|
||||
else:
|
||||
print("No models processed.", file=sys.stderr)
|
||||
|
||||
+1
-1
Submodule tinygrad_repo updated: 2fecac4e4a...ac1632ab96
Reference in New Issue
Block a user