Merge branch 'comma-pkl-converter' into reimplement-nav

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

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"))

openpilot/sunnypilot/modeld_v2/model_adapters.py

@@ -172,12 +172,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()}

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