DM test mode (#36631)

Author: adeebshihadeh

selfdrive/monitoring/dmonitoringd.py

@@ -13,6 +13,7 @@ def dmonitoringd_thread():
   sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2')
 
   DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
+  demo_mode=False
 
   # 20Hz <- dmonitoringmodeld
   while True:
@@ -22,8 +23,10 @@ def dmonitoringd_thread():
       continue
 
     valid = sm.all_checks()
-    if valid:
-      DM.run_step(sm)
+    if demo_mode and sm.valid['driverStateV2']:
+      DM.run_step(sm, demo=demo_mode)
+    elif valid:
+      DM.run_step(sm, demo=demo_mode)
 
     # publish
     dat = DM.get_state_packet(valid=valid)
@@ -32,6 +35,7 @@ def dmonitoringd_thread():
     # load live always-on toggle
     if sm['driverStateV2'].frameId % 40 == 1:
       DM.always_on = params.get_bool("AlwaysOnDM")
+      demo_mode = params.get_bool("IsDriverViewEnabled")
 
     # save rhd virtual toggle every 5 mins
     if (sm['driverStateV2'].frameId % 6000 == 0 and

selfdrive/monitoring/helpers.py

@@ -211,8 +211,8 @@ def _set_timers(self, active_monitoring):
       self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
       self.active_monitoring_mode = False
 
-  def _set_policy(self, model_data, car_speed):
-    bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
+  def _set_policy(self, brake_disengage_prob, car_speed):
+    bp = brake_disengage_prob
     k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
     bp_normal = max(min(bp / k1, 0.5),0)
     self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
@@ -417,27 +417,42 @@ def get_state_packet(self, valid=True):
     }
     return dat
 
-  def run_step(self, sm):
-    # Set strictness
+  def run_step(self, sm, demo=False):
+    if demo:
+      highway_speed = 30
+      enabled = True
+      wrong_gear = False
+      standstill = False
+      driver_engaged = False
+      brake_disengage_prob = 1.0
+      rpyCalib = [0., 0., 0.]
+    else:
+      highway_speed = sm['carState'].vEgo
+      enabled = sm['selfdriveState'].enabled
+      wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
+      standstill = sm['carState'].standstill
+      driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
+      brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
+      rpyCalib = sm['liveCalibration'].rpyCalib
     self._set_policy(
-      model_data=sm['modelV2'],
-      car_speed=sm['carState'].vEgo
+      brake_disengage_prob=brake_disengage_prob,
+      car_speed=highway_speed,
     )
 
     # Parse data from dmonitoringmodeld
     self._update_states(
       driver_state=sm['driverStateV2'],
-      cal_rpy=sm['liveCalibration'].rpyCalib,
-      car_speed=sm['carState'].vEgo,
-      op_engaged=sm['selfdriveState'].enabled,
-      standstill=sm['carState'].standstill,
+      cal_rpy=rpyCalib,
+      car_speed=highway_speed,
+      op_engaged=enabled,
+      standstill=standstill,
     )
 
     # Update distraction events
     self._update_events(
-      driver_engaged=sm['carState'].steeringPressed or sm['carState'].gasPressed,
-      op_engaged=sm['selfdriveState'].enabled,
-      standstill=sm['carState'].standstill,
-      wrong_gear=sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park],
-      car_speed=sm['carState'].vEgo
+      driver_engaged=driver_engaged,
+      op_engaged=enabled,
+      standstill=standstill,
+      wrong_gear=wrong_gear,
+      car_speed=highway_speed
     )