FrogPilot features - Add back navigate on openpilot

cereal/log.capnp

@@ -921,8 +921,8 @@ struct ModelDataV2 {
   # Model perceived motion
   temporalPose @21 :Pose;
 
-  navEnabledDEPRECATED @22 :Bool;
+  navEnabled @22 :Bool;
-  locationMonoTimeDEPRECATED @24 :UInt64;
+  locationMonoTime @24 :UInt64;
 
   # e2e lateral planner
   lateralPlannerSolutionDEPRECATED @25: LateralPlannerSolution;
@@ -2377,6 +2377,6 @@ struct Event {
     driverStateDEPRECATED @59 :DriverStateDEPRECATED;
     sensorEventsDEPRECATED @11 :List(SensorEventData);
     lateralPlanDEPRECATED @64 :LateralPlan;
-    navModelDEPRECATED @104 :NavModelData;
+    navModel @104 :NavModelData;
   }
 }

cereal/services.py

@@ -66,6 +66,8 @@ _services: dict[str, tuple] = {
   "navInstruction": (True, 1., 10),
   "navRoute": (True, 0.),
   "navThumbnail": (True, 0.),
+  "navModel": (True, 2., 4.),
+  "mapRenderState": (True, 2., 1.),
   "uiPlan": (True, 20., 40.),
   "qRoadEncodeIdx": (False, 20.),
   "userFlag": (True, 0., 1),

selfdrive/modeld/constants.py

@@ -18,6 +18,9 @@ class ModelConstants:
   HISTORY_BUFFER_LEN = 99
   DESIRE_LEN = 8
   TRAFFIC_CONVENTION_LEN = 2
+  NAV_FEATURE_LEN = 256
+  NAV_INSTRUCTION_LEN = 150
+  DRIVING_STYLE_LEN = 12
   LAT_PLANNER_STATE_LEN = 4
   LATERAL_CONTROL_PARAMS_LEN = 2
   PREV_DESIRED_CURV_LEN = 1

selfdrive/modeld/fill_model_msg.py

@@ -43,7 +43,8 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std
 
 def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], publish_state: PublishState,
                    vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
-                   timestamp_eof: int, model_execution_time: float, valid: bool) -> None:
+                   timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
+                   nav_enabled: bool, valid: bool) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   msg.valid = valid
 
@@ -53,7 +54,9 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
   modelV2.frameAge = frame_age
   modelV2.frameDropPerc = frame_drop * 100
   modelV2.timestampEof = timestamp_eof
+  modelV2.locationMonoTime = timestamp_llk
   modelV2.modelExecutionTime = model_execution_time
+  modelV2.navEnabled = nav_enabled
 
   # plan
   position = modelV2.position

selfdrive/modeld/modeld.py

@@ -152,7 +152,7 @@ def main(demo=False, frogpilot_toggles=None):
 
   # messaging
   pm = PubMaster(["modelV2", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "frogpilotPlan"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "frogpilotPlan"])
 
   publish_state = PublishState()
   params = Params()
@@ -166,6 +166,8 @@ def main(demo=False, frogpilot_toggles=None):
   model_transform_main = np.zeros((3, 3), dtype=np.float32)
   model_transform_extra = np.zeros((3, 3), dtype=np.float32)
   live_calib_seen = False
+  nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
+  nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
   buf_main, buf_extra = None, None
   meta_main = FrameMeta()
   meta_extra = FrameMeta()
@@ -235,6 +237,30 @@ def main(demo=False, frogpilot_toggles=None):
     if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
       vec_desire[desire] = 1
 
+    # Enable/disable nav features
+    timestamp_llk = sm["navModel"].locationMonoTime
+    nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9)
+    nav_enabled = nav_valid and params.get_bool("ExperimentalMode")
+
+    if not nav_enabled:
+      nav_features[:] = 0
+      nav_instructions[:] = 0
+
+    if nav_enabled and sm.updated["navModel"]:
+      nav_features = np.array(sm["navModel"].features)
+
+    if nav_enabled and sm.updated["navInstruction"]:
+      nav_instructions[:] = 0
+      for maneuver in sm["navInstruction"].allManeuvers:
+        distance_idx = 25 + int(maneuver.distance / 20)
+        direction_idx = 0
+        if maneuver.modifier in ("left", "slight left", "sharp left"):
+          direction_idx = 1
+        if maneuver.modifier in ("right", "slight right", "sharp right"):
+          direction_idx = 2
+        if 0 <= distance_idx < 50:
+          nav_instructions[distance_idx*3 + direction_idx] = 1
+
     # tracked dropped frames
     vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1)
     frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10))
@@ -263,7 +289,7 @@ def main(demo=False, frogpilot_toggles=None):
       modelv2_send = messaging.new_message('modelV2')
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
-                      meta_main.timestamp_eof, model_execution_time, live_calib_seen)
+                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]

selfdrive/modeld/navmodeld.py

@@ -0,0 +1,117 @@
+#!/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 msgq.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()

selfdrive/navd/SConscript

@@ -17,4 +17,4 @@ if arch in ['larch64', 'aarch64', 'x86_64']:
   map_env["RPATH"].append(Dir('.').abspath)
   map_env["LIBPATH"].append(Dir('.').abspath)
   maplib = map_env.SharedLibrary("maprender", ["map_renderer.cc"], LIBS=libs)
-  # map_env.Program("mapsd", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs)
+  map_env.Program("mapsd", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs)

selfdrive/ui/qt/maps/map.cc

@@ -75,7 +75,7 @@ void MapWindow::initLayers() {
 
     QVariantMap transition;
     transition["duration"] = 400;  // ms
-    m_map->setPaintProperty("navLayer", "line-color", QColor("#31a1ee"));
+    m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(uiState()->scene.navigate_on_openpilot));
     m_map->setPaintProperty("navLayer", "line-color-transition", transition);
     m_map->setPaintProperty("navLayer", "line-width", 7.5);
     m_map->setLayoutProperty("navLayer", "line-cap", "round");
@@ -127,6 +127,21 @@ void MapWindow::updateState(const UIState &s) {
   }
   prev_time_valid = sm.valid("clocks");
 
+  if (sm.updated("modelV2")) {
+    // set path color on change, and show map on rising edge of navigate on openpilot
+    bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() &&
+                       sm["controlsState"].getControlsState().getEnabled();
+    if (nav_enabled != uiState()->scene.navigate_on_openpilot) {
+      if (loaded_once) {
+        m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled));
+      }
+      if (nav_enabled) {
+        emit requestVisible(true);
+      }
+    }
+    uiState()->scene.navigate_on_openpilot = nav_enabled;
+  }
+
   if (sm.updated("liveLocationKalman")) {
     auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
     auto locationd_pos = locationd_location.getPositionGeodetic();
@@ -366,6 +381,7 @@ void MapWindow::pinchTriggered(QPinchGesture *gesture) {
 void MapWindow::offroadTransition(bool offroad) {
   if (offroad) {
     clearRoute();
+    uiState()->scene.navigate_on_openpilot = false;
     routing_problem = false;
   } else {
     auto dest = coordinate_from_param("NavDestination");

selfdrive/ui/qt/maps/map.h

@@ -70,6 +70,11 @@ private:
   MapInstructions* map_instructions;
   MapETA* map_eta;
 
+  // Blue with normal nav, green when nav is input into the model
+  QColor getNavPathColor(bool nav_enabled) {
+    return nav_enabled ? QColor("#31ee73") : QColor("#31a1ee");
+  }
+
   void clearRoute();
   void updateDestinationMarker();
   uint64_t route_rcv_frame = 0;

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -79,8 +79,9 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
 
 #ifdef ENABLE_MAPS
   if (map != nullptr) {
+    // Switch between map and sidebar when using navigate on openpilot
     bool sidebarVisible = geometry().x() > 0;
-    bool show_map = !sidebarVisible;
+    bool show_map = scene.navigate_on_openpilot ? sidebarVisible : !sidebarVisible;
     map->setVisible(show_map && !map->isVisible());
   }
 #endif

selfdrive/ui/qt/window.cc

@@ -75,6 +75,10 @@ void MainWindow::closeSettings() {
 
   if (uiState()->scene.started) {
     homeWindow->showSidebar(false);
+    // Map is always shown when using navigate on openpilot
+    if (uiState()->scene.navigate_on_openpilot) {
+      homeWindow->showMapPanel(true);
+    }
   }
 }
 

selfdrive/ui/ui.h

@@ -105,6 +105,7 @@ typedef struct UIScene {
   float driver_pose_coss[3];
   vec3 face_kpts_draw[std::size(default_face_kpts_3d)];
 
+  bool navigate_on_openpilot = false;
   cereal::LongitudinalPersonality personality;
 
   float light_sensor = -1;

system/manager/process_config.py

@@ -56,6 +56,8 @@ procs = [
   NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
   NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging),
   NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], only_onroad),
+  NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad),
+  PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad),
   NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
   PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),