From 2a84e1481c8ce347bd69e6a3eca1714fb2a584af Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Thu, 15 Oct 2026 11:02:26 -0700 Subject: [PATCH] modeld_v2: prep for nav models --- openpilot/selfdrive/navigation/__init__.py | 0 openpilot/selfdrive/navigation/helpers.py | 43 +++++++++++++++++++ .../sunnypilot/modeld_v2/frame_fusion.py | 10 +++-- .../sunnypilot/modeld_v2/model_adapters.py | 8 ++-- openpilot/sunnypilot/modeld_v2/modeld.py | 24 ++++++++++- 5 files changed, 76 insertions(+), 9 deletions(-) create mode 100644 openpilot/selfdrive/navigation/__init__.py create mode 100644 openpilot/selfdrive/navigation/helpers.py diff --git a/openpilot/selfdrive/navigation/__init__.py b/openpilot/selfdrive/navigation/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/selfdrive/navigation/helpers.py b/openpilot/selfdrive/navigation/helpers.py new file mode 100644 index 0000000000..260884634b --- /dev/null +++ b/openpilot/selfdrive/navigation/helpers.py @@ -0,0 +1,43 @@ +import time +from typing import Any + +import numpy as np + + +def latest_gps_source(sm) -> str | None: + """The service with the freshest usable GPS fix, or None.""" + now = time.monotonic() + sources = [s for s in ('gpsLocation', 'gpsLocationExternal') + if sm.valid[s] and sm[s].hasFix and 0 <= now - sm.recv_time[s] < 2] + return max(sources, key=lambda s: sm.logMonoTime[s]) if sources else None + + +# convert mapbox steps to valhalla enums the model was trained on +VALHALLA_COMMAND_COUNT = 46 +VALHALLA_SIDED_TYPES = { # (straight, right, left) + 'depart': (1, 2, 3), 'arrive': (4, 5, 6), + 'on ramp': (17, 18, 19), 'off ramp': (20, 20, 21), 'fork': (22, 23, 24), + 'roundabout': (26, 26, 26), 'rotary': (26, 26, 26), 'roundabout turn': (26, 26, 26), + 'exit roundabout': (27, 27, 27), 'exit rotary': (27, 27, 27), 'notification': (28, 28, 28), +} +VALHALLA_TURN_TYPES = {'straight': 8, 'slight right': 9, 'right': 10, 'sharp right': 11, 'uturn': 13, + 'sharp left': 14, 'left': 15, 'slight left': 16} # turn, continue, new name, end of road + + +def valhalla_type(maneuver_type: str, modifier: str) -> int | None: + if maneuver_type == 'merge': + return None # Valhalla gives no maneuver at a merge, so the model never saw one + if maneuver_type in VALHALLA_SIDED_TYPES: + straight, right, left = VALHALLA_SIDED_TYPES[maneuver_type] + return right if 'right' in modifier else left if 'left' in modifier else straight + return VALHALLA_TURN_TYPES.get(modifier, 8) + + +def navigation_input(maneuvers: Any, traveled: float) -> np.ndarray: + navigation = np.zeros(VALHALLA_COMMAND_COUNT + 1, dtype=np.float32) + for maneuver in maneuvers: + command, distance = valhalla_type(maneuver.type, maneuver.modifier), maneuver.distance - traveled + if command is not None and command > 6 and distance >= 0: + navigation[command], navigation[-1] = 1, distance / 100 + break + return navigation diff --git a/openpilot/sunnypilot/modeld_v2/frame_fusion.py b/openpilot/sunnypilot/modeld_v2/frame_fusion.py index ac7c1e16f1..0b088bc67b 100644 --- a/openpilot/sunnypilot/modeld_v2/frame_fusion.py +++ b/openpilot/sunnypilot/modeld_v2/frame_fusion.py @@ -9,14 +9,16 @@ import itertools import math from functools import partial +import numpy as np + from openpilot.common.basedir import BASEDIR from openpilot.sunnypilot.modeld_v2.helpers import _dtype, read_parts -def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]: +def pack_layout(specs: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]: offsets, offset = {}, 0 - for name, shape in ({"tfm": (2, 3, 3)} | shapes).items(): - offsets[name], offset = offset, offset + -(-math.prod(shape) * 4 // 128) * 128 + for name, (shape, dtype) in ({"tfm": ((2, 3, 3), np.float32)} | specs).items(): + offsets[name], offset = offset, offset + -(-math.prod(shape) * np.dtype(dtype).itemsize // 128) * 128 return offsets | {"frames": offset}, offset + 2 * frame_size @@ -38,7 +40,7 @@ def frame_part(warp_path: str, parts: dict, extras: dict) -> dict: return {} (model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"] states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"]) - host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"} + host = {name: (shape, dtype) for name, (shape, dtype, _) in extras["input_specs"].items() if name not in states and name != "new_img"} frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2 offsets, packed = pack_layout(host, frame_size) shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8")) diff --git a/openpilot/sunnypilot/modeld_v2/model_adapters.py b/openpilot/sunnypilot/modeld_v2/model_adapters.py index 657a707101..89fec96113 100644 --- a/openpilot/sunnypilot/modeld_v2/model_adapters.py +++ b/openpilot/sunnypilot/modeld_v2/model_adapters.py @@ -173,12 +173,12 @@ class NativeTinygradAdapter(BaseModelAdapter): self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize() for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs} - shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'} - offsets, packed = pack_layout(shapes, self.warp_frame_size) + specs = {name: spec for name, spec in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'} + offsets, packed = pack_layout(specs, self.warp_frame_size) self.packed_input = np.zeros(packed, dtype=np.uint8) self.host = Tensor(self.packed_input, device='NPY') - self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name]) - for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()} + self.numpy_inputs = {name: np.ndarray(shape, dtype=dtype, buffer=self.packed_input, offset=offsets[name]) + for name, (shape, dtype) in ({'tfm': ((2, 3, 3), np.dtype(np.float32))} | specs).items()} self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size) self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()} diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index 4b66996c40..8c7d8a9552 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -32,6 +32,7 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value +from openpilot.selfdrive.navigation.helpers import navigation_input from openpilot.selfdrive.modeld.modeld import ChestnutGpuState from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output @@ -169,6 +170,9 @@ class ModelState(ModelStateBase): for key in ('traffic_convention', 'lateral_control_params', 'action_t'): if key in self.numpy_inputs and key in inputs: self.numpy_inputs[key][:] = inputs[key] + for name in ('nav_map', 'navigation'): + if name in self.numpy_inputs: + self.numpy_inputs[name][:] = inputs.get(name, 0) if self.adapter.is_native: for i, key in enumerate(self._vision_input_names): @@ -248,6 +252,10 @@ class ModelState(ModelStateBase): return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), shouldStop=bool(stop)) +def takes_nav(*models) -> bool: + return any(name in m.numpy_inputs for m in models if m is not None for name in ('nav_map', 'navigation')) + + def main(demo=False): cloudlog.warning("modeld init") @@ -276,6 +284,7 @@ def main(demo=False): vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_NARROW_ROAD vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True) vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False) + vipc_client_map = VisionIpcClient("mapsd", VisionStreamType.VISION_STREAM_MAP, True) cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}") while not vipc_client_main.connect(False): @@ -317,7 +326,9 @@ def main(demo=False): # messaging pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else []) pm = PubMaster(pub_socks) - sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"]) + nav = takes_nav(model, small_model) + sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"] + + (["navInstruction"] if nav else [])) publish_state = PublishState() chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None @@ -350,6 +361,7 @@ def main(demo=False): DH = DesireHelper() meta_constants = load_meta_constants() RELC = RoadEdgeLaneChangeController() + nav_map, nav_map_time = None, 0. while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame @@ -439,6 +451,16 @@ def main(demo=False): if 'action_t' in model.numpy_inputs: inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32) + now = time.monotonic() + if nav and (vipc_client_map.is_connected() or vipc_client_map.connect(False)): + if (buf_map := vipc_client_map.recv(0)) is not None: + nav_map = np.frombuffer(buf_map.data, dtype=np.uint8, count=buf_map.width * buf_map.height).reshape(buf_map.height, buf_map.width).copy() + nav_map_time = now + if nav_map is not None and now - nav_map_time < 0.5: # map timeout + inputs['nav_map'] = nav_map + if nav and sm.valid['navInstruction'] and now - sm.recv_time['navInstruction'] < 2: + inputs['navigation'] = navigation_input(sm['navInstruction'].allManeuvers, v_ego * (now - sm.recv_time['navInstruction'])) + mt1 = time.perf_counter() try: send_chestnut = (chestnut_state is not None and