From 77b5e197aa7a438580b63adb8ae9c68d98452afc Mon Sep 17 00:00:00 2001
From: Mitchell Goff <mitchellgoffpc@gmail.com>
Date: Fri, 16 Jun 2023 21:27:35 -0700
Subject: [PATCH] Enable nav features in modeld (#28448)

* Enable nav features in modeld

* Enable mapsd/navmodeld

* Updated model_replay_ref_commit and added mapsd/navmodeld to test_onroad

* fixed process name

* always publish from mapsd
old-commit-hash: aadd9ae269e682b6893557a89a02a7ae357206ee
---
 selfdrive/manager/process_config.py |  4 ++--
 selfdrive/modeld/modeld.cc          | 22 +++++++++++++++++++++-
 selfdrive/test/test_onroad.py       |  4 ++++
 3 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
index 5ea8e68041..8945093584 100644
--- a/selfdrive/manager/process_config.py
+++ b/selfdrive/manager/process_config.py
@@ -43,8 +43,8 @@ procs = [
   NativeProcess("encoderd", "system/loggerd", ["./encoderd"]),
   NativeProcess("loggerd", "system/loggerd", ["./loggerd"], onroad=False, callback=logging),
   NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
-  NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], enabled=False),
-  NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False),
+  NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"]),
+  NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"]),
   NativeProcess("sensord", "system/sensord", ["./sensord"], enabled=not PC),
   NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)),
   NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"]),
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 265bd4071b..adfcf7e33d 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -61,7 +61,7 @@ mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camer
 void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
   // messaging
   PubMaster pm({"modelV2", "cameraOdometry"});
-  SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState"});
+  SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "mapRenderState", "navInstruction", "navModel"});
 
   // setup filter to track dropped frames
   FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ);
@@ -72,6 +72,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
 
   mat3 model_transform_main = {};
   mat3 model_transform_extra = {};
+  bool nav_enabled = false;
   bool live_calib_seen = false;
   float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0};
   float nav_features[NAV_FEATURE_LEN] = {0};
@@ -137,6 +138,25 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       vec_desire[desire] = 1.0;
     }
 
+    // Enable/disable nav features
+    double tsm = (float)(nanos_since_boot() - sm["mapRenderState"].getMapRenderState().getLocationMonoTime()) / 1e6;
+    bool route_valid = sm["navInstruction"].getValid() && (tsm < 1000);
+    bool render_valid = sm["mapRenderState"].getValid() && (sm["navModel"].getNavModel().getFrameId() == sm["mapRenderState"].getMapRenderState().getFrameId());
+    bool nav_valid = route_valid && render_valid;
+    if (!nav_enabled && nav_valid) {
+      nav_enabled = true;
+    } else if (nav_enabled && !nav_valid) {
+      memset(nav_features, 0, sizeof(float)*NAV_FEATURE_LEN);
+      nav_enabled = false;
+    }
+
+    if (nav_enabled && sm.updated("navModel")) {
+      auto nav_model_features = sm["navModel"].getNavModel().getFeatures();
+      for (int i=0; i<NAV_FEATURE_LEN; i++) {
+        nav_features[i] = nav_model_features[i];
+      }
+    }
+
     // tracked dropped frames
     uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1;
     float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
index ef6ce65201..72d67b14db 100755
--- a/selfdrive/test/test_onroad.py
+++ b/selfdrive/test/test_onroad.py
@@ -30,6 +30,7 @@ PROCS = {
   "./encoderd": 17.0,
   "./camerad": 14.5,
   "./locationd": 11.0,
+  "./mapsd": 2.0,
   "selfdrive.controls.plannerd": 16.5,
   "./_ui": 21.0,
   "selfdrive.locationd.paramsd": 9.0,
@@ -37,6 +38,7 @@ PROCS = {
   "selfdrive.controls.radard": 4.5,
   "./_modeld": 4.48,
   "./_dmonitoringmodeld": 5.0,
+  "./_navmodeld": 1.0,
   "selfdrive.thermald.thermald": 3.87,
   "selfdrive.locationd.calibrationd": 2.0,
   "selfdrive.locationd.torqued": 5.0,
@@ -84,6 +86,8 @@ TIMINGS = {
   "driverCameraState": [2.5, 0.35],
   "modelV2": [2.5, 0.35],
   "driverStateV2": [2.5, 0.40],
+  "navModel": [2.5, 0.35],
+  "mapRenderState": [2.5, 0.35],
   "liveLocationKalman": [2.5, 0.35],
   "wideRoadCameraState": [1.5, 0.35],
 }