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 cerealpull/32141/head
YassineYousfi
1 year ago
committed by
GitHub
parent
97db26b6a9
commit
754dd45ffa
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