add confidence

selfdrive/modeld/constants.py

@@ -5,16 +5,43 @@ IDX_N = 33
 def index_function(idx, max_val=192, max_idx=32):
   return (max_val) * ((idx/max_idx)**2)
 
-
 T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
 X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
 LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
 LEAD_T_OFFSETS = [0., 2., 4.]
 META_T_IDXS = [2., 4., 6., 8., 10.]
 
+MODEL_FREQ = 20
+
 FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
 FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
 
+DISENGAGE_WIDTH = 5
+POSE_WIDTH = 6
+SIM_POSE_WIDTH = 6
+LEAD_WIDTH = 4
+LANE_LINES_WIDTH = 2
+ROAD_EDGES_WIDTH = 2
+PLAN_WIDTH = 15
+DESIRE_PRED_WIDTH = 8
+
+NUM_LANE_LINES = 4
+NUM_ROAD_EDGES = 2
+
+LEAD_TRAJ_LEN = 6
+
+PLAN_MHP_N = 5
+LEAD_MHP_N = 2
+PLAN_MHP_SELECTION = 1
+LEAD_MHP_SELECTION = 3
+
+FCW_THRESHOLD_5MS2_HIGH = 0.15
+FCW_THRESHOLD_5MS2_LOW = 0.05
+FCW_THRESHOLD_3MS2 = 0.7
+
+RYG_GREEN = 0.01165
+RYG_YELLOW = 0.06157
+
 class Plan:
   POSITION = slice(0, 3)
   VELOCITY = slice(3, 6)
@@ -34,4 +61,4 @@ class Meta:
   GAS_PRESS = slice(7, 36, 7)
   # next 0, 2, 4, 6, 8, 10 seconds
   LEFT_BLINKER = slice(36, 48, 2)
-  RIGHT_BLINKER = slice(37, 48, 2)
+  RIGHT_BLINKER = slice(37, 48, 2)

selfdrive/modeld/fill_model_msg.py

@@ -1,13 +1,14 @@
 import capnp
 import numpy as np
-from typing import List, Dict
-from openpilot.selfdrive.modeld.constants import T_IDXS, X_IDXS, LEAD_T_IDXS, META_T_IDXS, LEAD_T_OFFSETS, Meta, Plan, FCW_THRESHOLDS_5MS2, FCW_THRESHOLDS_3MS2
+from typing import Dict
+from cereal import log
+from openpilot.selfdrive.modeld.constants import *
 
 class PublishState:
   def __init__(self):
-    self.disengage_buffer = np.zeros(5*5, dtype=np.float32)
-    self.prev_brake_5ms2_probs = np.zeros(5, dtype=np.float32)
-    self.prev_brake_3ms2_probs = np.zeros(3, dtype=np.float32)
+    self.disengage_buffer = np.zeros(DISENGAGE_WIDTH*DISENGAGE_WIDTH, dtype=np.float32)
+    self.prev_brake_5ms2_probs = np.zeros(DISENGAGE_WIDTH, dtype=np.float32)
+    self.prev_brake_3ms2_probs = np.zeros(DISENGAGE_WIDTH, dtype=np.float32)
 
 def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
   builder.t = t
@@ -82,10 +83,6 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
     lead.prob = net_output_data['lead_prob'][0,i].tolist()
     lead.probTime = LEAD_T_OFFSETS[i]
 
-  # confidence
-  # TODO
-  modelV2.confidence = 0
-
   # meta
   meta = modelV2.meta
   meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
@@ -115,6 +112,25 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
   temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
   temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
 
+  # confidence
+  if vipc_frame_id % (2*MODEL_FREQ) == 0:
+    # any disengage prob
+    any_disengage_probs = 1-((1-net_output_data['meta'][0,Meta.BRAKE_DISENGAGE])*(1-net_output_data['meta'][0,Meta.GAS_DISENGAGE])*(1-net_output_data['meta'][0,Meta.STEER_OVERRIDE]))
+    # independent disengage prob for each 2s slice
+    ind_disengage_probs = np.zeros(DISENGAGE_WIDTH, dtype=np.float32)
+    ind_disengage_probs[0] = any_disengage_probs[0]
+    ind_disengage_probs[1:] = np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])
+    # rolling buf for 2, 4, 6, 8, 10s
+    publish_state.disengage_buffer[:-DISENGAGE_WIDTH] = publish_state.disengage_buffer[DISENGAGE_WIDTH:]
+    publish_state.disengage_buffer[DISENGAGE_WIDTH*(DISENGAGE_WIDTH-1):] = ind_disengage_probs
+
+  score = publish_state.disengage_buffer[DISENGAGE_WIDTH-1:DISENGAGE_WIDTH*DISENGAGE_WIDTH-1:DISENGAGE_WIDTH-1].sum()/DISENGAGE_WIDTH
+  if score < RYG_GREEN:
+    modelV2.confidence = log.ModelDataV2.ConfidenceClass.green
+  elif score < RYG_YELLOW:
+    modelV2.confidence = log.ModelDataV2.ConfidenceClass.yellow
+  else:
+    modelV2.confidence = log.ModelDataV2.ConfidenceClass.red
 
 def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray],
                   vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:

selfdrive/modeld/parse_model_outputs.py

@@ -1,28 +1,6 @@
 import numpy as np
 from typing import Dict
-from openpilot.selfdrive.modeld.constants import IDX_N
-
-POSE_WIDTH = 6
-SIM_POSE_WIDTH = 6
-LEAD_WIDTH = 4
-LANE_LINES_WIDTH = 2
-ROAD_EDGES_WIDTH = 2
-PLAN_WIDTH = 15
-DESIRE_PRED_WIDTH = 8
-
-NUM_LANE_LINES = 4
-NUM_ROAD_EDGES = 2
-
-LEAD_TRAJ_LEN = 6
-
-PLAN_MHP_N = 5
-LEAD_MHP_N = 2
-PLAN_MHP_SELECTION = 1
-LEAD_MHP_SELECTION = 3
-
-FCW_THRESHOLD_5MS2_HIGH = 0.15
-FCW_THRESHOLD_5MS2_LOW = 0.05
-FCW_THRESHOLD_3MS2 = 0.7
+from openpilot.selfdrive.modeld.constants import *
 
 def sigmoid(x):
   return 1. / (1. + np.exp(-x))