Non-Inflatable Model 🎈 (#29003)

* Added all maneuvers to navInstruction message

* Added nav instruction logic to modeld

* New model: fcee01c1-96bb-414f-b00d-e4a994a00922/700

* Fixed bug in navd

* Added sharp/slight modifiers

* Updated refs
old-commit-hash: c04e5d12fc

selfdrive/modeld/modeld.cc

@@ -60,7 +60,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", "navModel"});
+  SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"});
 
   Params params;
   PublishState ps = {};
@@ -78,6 +78,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
   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};
+  float nav_instructions[NAV_INSTRUCTION_LEN] = {0};
 
   VisionBuf *buf_main = nullptr;
   VisionBuf *buf_extra = nullptr;
@@ -148,6 +149,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       nav_enabled = true;
     } else if (nav_enabled && !use_nav) {
       memset(nav_features, 0, sizeof(float)*NAV_FEATURE_LEN);
+      memset(nav_instructions, 0, sizeof(float)*NAV_INSTRUCTION_LEN);
       nav_enabled = false;
     }
 
@@ -158,6 +160,21 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       }
     }
 
+    if (nav_enabled && sm.updated("navInstruction")) {
+      memset(nav_instructions, 0, sizeof(float)*NAV_INSTRUCTION_LEN);
+      auto maneuvers = sm["navInstruction"].getNavInstruction().getAllManeuvers();
+      for (int i=0; i<maneuvers.size(); i++) {
+        int distance_idx = 25 + (int)(maneuvers[i].getDistance() / 20);
+        std::string direction = maneuvers[i].getModifier();
+        int direction_idx = 0;
+        if (direction == "left" || direction == "slight left" || direction == "sharp left") direction_idx = 1;
+        if (direction == "right" || direction == "slight right" || direction == "sharp right") direction_idx = 2;
+        if (distance_idx >= 0 && distance_idx < 50) {
+          nav_instructions[distance_idx*3 + direction_idx] = 1;
+        }
+      }
+    }
+
     // 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));
@@ -175,7 +192,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
     }
 
     double mt1 = millis_since_boot();
-    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only);
+    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, nav_instructions, prepare_only);
     double mt2 = millis_since_boot();
     float model_execution_time = (mt2 - mt1) / 1000.0;
 

selfdrive/modeld/models/driving.cc

@@ -47,6 +47,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
 
 #ifdef NAV
   s->m->addInput("nav_features", s->nav_features, NAV_FEATURE_LEN);
+  s->m->addInput("nav_instructions", s->nav_instructions, NAV_INSTRUCTION_LEN);
 #endif
 
 #ifdef TEMPORAL
@@ -55,8 +56,8 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
 
 }
 
-ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
+ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, const mat3 &transform, const mat3 &transform_wide,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) {
+                              float *desire_in, bool is_rhd, float *driving_style, float *nav_features, float *nav_instructions, bool prepare_only) {
 #ifdef DESIRE
   std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN);
   if (desire_in != NULL) {
@@ -76,6 +77,7 @@ LOGT("Desire enqueued");
 
 #ifdef NAV
   std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN);
+  std::memcpy(s->nav_instructions, nav_instructions, sizeof(float)*NAV_INSTRUCTION_LEN);
 #endif
 
 #ifdef DRIVING_STYLE

selfdrive/modeld/models/driving.h

@@ -280,6 +280,7 @@ struct ModelState {
 #endif
 #ifdef NAV
   float nav_features[NAV_FEATURE_LEN] = {};
+  float nav_instructions[NAV_INSTRUCTION_LEN] = {};
 #endif
 };
 
@@ -290,8 +291,8 @@ struct PublishState {
 };
 
 void model_init(ModelState* s, cl_device_id device_id, cl_context context);
-ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
+ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, const mat3 &transform, const mat3 &transform_wide,
-                              const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only);
+                              float *desire_in, bool is_rhd, float *driving_style, float *nav_features, float *nav_instructions, bool prepare_only);
 void model_free(ModelState* s);
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, ModelState &s, PublishState &ps, uint64_t timestamp_eof, uint64_t timestamp_llk,

selfdrive/modeld/models/nav.h

@@ -9,6 +9,7 @@
 
 constexpr int NAV_INPUT_SIZE = 256*256;
 constexpr int NAV_FEATURE_LEN = 256;
+constexpr int NAV_INSTRUCTION_LEN = 150;
 constexpr int NAV_DESIRE_LEN = 32;
 
 struct NavModelOutputXY {

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:d7f95f6bfa1c76567e186c806e75fc0ab900e3bd99f9514bdece99364ffaf8a8
+oid sha256:c63ea3eb6c9b5a20c7420c2dc6d6d0f80a6949a39f6d8b74e574f52734154820
-size 47501059
+size 47654714

selfdrive/navd/helpers.py

@@ -135,32 +135,33 @@ def field_valid(dat: dict, field: str) -> bool:
   return field in dat and dat[field] is not None
 
 
-def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None:
+def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]:
   if not len(banners):
-    return
+    return None
 
-  current_banner = banners[0]
+  instruction = {}
 
   # A segment can contain multiple banners, find one that we need to show now
+  current_banner = banners[0]
   for banner in banners:
     if distance_to_maneuver < banner['distanceAlongGeometry']:
       current_banner = banner
 
   # Only show banner when close enough to maneuver
-  instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry']
+  instruction['showFull'] = distance_to_maneuver < current_banner['distanceAlongGeometry']
 
   # Primary
   p = current_banner['primary']
   if field_valid(p, 'text'):
-    instruction.maneuverPrimaryText = p['text']
+    instruction['maneuverPrimaryText'] = p['text']
   if field_valid(p, 'type'):
-    instruction.maneuverType = p['type']
+    instruction['maneuverType'] = p['type']
   if field_valid(p, 'modifier'):
-    instruction.maneuverModifier = p['modifier']
+    instruction['maneuverModifier'] = p['modifier']
 
   # Secondary
   if field_valid(current_banner, 'secondary'):
-    instruction.maneuverSecondaryText = current_banner['secondary']['text']
+    instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
 
   # Lane lines
   if field_valid(current_banner, 'sub'):
@@ -178,4 +179,6 @@ def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuv
         lane['activeDirection'] = string_to_direction(component['active_direction'])
 
       lanes.append(lane)
-    instruction.lanes = lanes
+    instruction['lanes'] = lanes
+
+  return instruction

selfdrive/navd/navd.py

@@ -226,7 +226,32 @@ class RouteEngine:
 
     # Current instruction
     msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
-    parse_banner_instructions(msg.navInstruction, banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    if instruction is not None:
+      for k,v in instruction.items():
+        setattr(msg.navInstruction, k, v)
+
+    # All instructions
+    maneuvers = []
+    for i, step_i in enumerate(self.route):
+      if i < self.step_idx:
+        distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry
+      elif i == self.step_idx:
+        distance_to_maneuver = distance_to_maneuver_along_geometry
+      else:
+        distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1))
+
+      instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver)
+      if instruction is None:
+        continue
+      maneuver = {'distance': distance_to_maneuver}
+      if 'maneuverType' in instruction:
+        maneuver['type'] = instruction['maneuverType']
+      if 'maneuverModifier' in instruction:
+        maneuver['modifier'] = instruction['maneuverModifier']
+      maneuvers.append(maneuver)
+
+    msg.navInstruction.allManeuvers = maneuvers
 
     # Compute total remaining time and distance
     remaining = 1.0 - along_geometry / max(step['distance'], 1)

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-c464c63c1e36710a2eee53f11e982d60989bbb1d
+377ed362faa9410a8b81d50978d74ee60b8e6f6a