disable navigate on openpilot (#32106)
* 2eedcd90-b7db-46cb-86be-740f48ded7ab/700
* noop ciao
* here too
* mapsd too
* update translations
* disable mapsd test from test onroad
* disable mapRenderState test from timings
* lint
* fix exp mode toggle panel
* update tr
* french done
* dont build mapd
* only no nav
* just comment
* deprecate nav fields
* rm not comment
* dont deprecate too much
* remove from services
* merge cereal
old-commit-hash: 754dd45ffa
pull/32199/head
YassineYousfi
1 year ago
committed by
GitHub
parent
deb8ff1865
commit
75a69e12b3
32 changed files with 53 additions and 400 deletions
@ -1 +1 @@ |
||||
Subproject commit b9871482a3cba70c2ab40ab80017019313dc3f31 |
||||
Subproject commit db1359ec22f6ab60c6e76abc4531bc843345d6b5 |
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:8254b569878b7472e3f63ed9f3527a87bde785c9037aee3ed66f972e072b5899 |
||||
size 14166696 |
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:89fda8380efa3e421fbcdb6bb204c36a4991f137ee01d47f3d0380895aa7c036 |
||||
size 3630942 |
||||
@ -1,117 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import gc |
||||
import math |
||||
import time |
||||
import ctypes |
||||
import numpy as np |
||||
from pathlib import Path |
||||
|
||||
from cereal import messaging |
||||
from cereal.messaging import PubMaster, SubMaster |
||||
from cereal.visionipc import VisionIpcClient, VisionStreamType |
||||
from openpilot.common.swaglog import cloudlog |
||||
from openpilot.common.params import Params |
||||
from openpilot.common.realtime import set_realtime_priority |
||||
from openpilot.selfdrive.modeld.constants import ModelConstants |
||||
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime |
||||
|
||||
NAV_INPUT_SIZE = 256*256 |
||||
NAV_FEATURE_LEN = 256 |
||||
NAV_DESIRE_LEN = 32 |
||||
NAV_OUTPUT_SIZE = 2*2*ModelConstants.IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN |
||||
MODEL_PATHS = { |
||||
ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc', |
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'} |
||||
|
||||
class NavModelOutputXY(ctypes.Structure): |
||||
_fields_ = [ |
||||
("x", ctypes.c_float), |
||||
("y", ctypes.c_float)] |
||||
|
||||
class NavModelOutputPlan(ctypes.Structure): |
||||
_fields_ = [ |
||||
("mean", NavModelOutputXY*ModelConstants.IDX_N), |
||||
("std", NavModelOutputXY*ModelConstants.IDX_N)] |
||||
|
||||
class NavModelResult(ctypes.Structure): |
||||
_fields_ = [ |
||||
("plan", NavModelOutputPlan), |
||||
("desire_pred", ctypes.c_float*NAV_DESIRE_LEN), |
||||
("features", ctypes.c_float*NAV_FEATURE_LEN)] |
||||
|
||||
class ModelState: |
||||
inputs: dict[str, np.ndarray] |
||||
output: np.ndarray |
||||
model: ModelRunner |
||||
|
||||
def __init__(self): |
||||
assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float) |
||||
self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32) |
||||
self.inputs = {'input_img': np.zeros(NAV_INPUT_SIZE, dtype=np.uint8)} |
||||
self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None) |
||||
self.model.addInput("input_img", None) |
||||
|
||||
def run(self, buf:np.ndarray) -> tuple[np.ndarray, float]: |
||||
self.inputs['input_img'][:] = buf |
||||
|
||||
t1 = time.perf_counter() |
||||
self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32)) |
||||
self.model.execute() |
||||
t2 = time.perf_counter() |
||||
return self.output, t2 - t1 |
||||
|
||||
def get_navmodel_packet(model_output: np.ndarray, valid: bool, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float): |
||||
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(NavModelResult)).contents |
||||
msg = messaging.new_message('navModel') |
||||
msg.valid = valid |
||||
msg.navModel.frameId = frame_id |
||||
msg.navModel.locationMonoTime = location_ts |
||||
msg.navModel.modelExecutionTime = execution_time |
||||
msg.navModel.dspExecutionTime = dsp_execution_time |
||||
msg.navModel.features = model_result.features[:] |
||||
msg.navModel.desirePrediction = model_result.desire_pred[:] |
||||
msg.navModel.position.x = [p.x for p in model_result.plan.mean] |
||||
msg.navModel.position.y = [p.y for p in model_result.plan.mean] |
||||
msg.navModel.position.xStd = [math.exp(p.x) for p in model_result.plan.std] |
||||
msg.navModel.position.yStd = [math.exp(p.y) for p in model_result.plan.std] |
||||
return msg |
||||
|
||||
|
||||
def main(): |
||||
gc.disable() |
||||
set_realtime_priority(1) |
||||
|
||||
# there exists a race condition when two processes try to create a |
||||
# SNPE model runner at the same time, wait for dmonitoringmodeld to finish |
||||
cloudlog.warning("waiting for dmonitoringmodeld to initialize") |
||||
if not Params().get_bool("DmModelInitialized", True): |
||||
return |
||||
|
||||
model = ModelState() |
||||
cloudlog.warning("models loaded, navmodeld starting") |
||||
|
||||
vipc_client = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True) |
||||
while not vipc_client.connect(False): |
||||
time.sleep(0.1) |
||||
assert vipc_client.is_connected() |
||||
cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") |
||||
|
||||
sm = SubMaster(["navInstruction"]) |
||||
pm = PubMaster(["navModel"]) |
||||
|
||||
while True: |
||||
buf = vipc_client.recv() |
||||
if buf is None: |
||||
continue |
||||
|
||||
sm.update(0) |
||||
t1 = time.perf_counter() |
||||
model_output, dsp_execution_time = model.run(buf.data[:buf.uv_offset]) |
||||
t2 = time.perf_counter() |
||||
|
||||
valid = vipc_client.valid and sm.valid["navInstruction"] |
||||
pm.send("navModel", get_navmodel_packet(model_output, valid, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time)) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
Loading…
Reference in new issue