def _run_seq(self, msgs, interaction, engaged, standstill): DS = DriverStatus() events = [] for idx in range(len(msgs)): e = Events() DS.update_states(msgs[idx], [0, 0, 0], 0, engaged[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests DS.update_events(e, interaction[idx], engaged[idx], standstill[idx]) events.append(e) assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" return events, DS
def dmonitoringd_thread(sm=None, pm=None): gc.disable() set_realtime_priority(2) if pm is None: pm = messaging.PubMaster(['driverMonitoringState']) if sm is None: sm = messaging.SubMaster([ 'driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2' ], poll=['driverState']) driver_status = DriverStatus(rhd=Params().get_bool("IsRHD")) sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].buttonEvents = [] sm['carState'].standstill = True v_cruise_last = 0 driver_engaged = False # 10Hz <- dmonitoringmodeld while True: sm.update() if not sm.updated['driverState']: continue # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ v_cruise != v_cruise_last or \ sm['carState'].steeringPressed or \ sm['carState'].gasPressed v_cruise_last = v_cruise if sm.updated['modelV2']: driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo) # Get data from dmonitoringmodeld events = Events() driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState') dat.driverMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "distractedType": sum(driver_status.distracted_types), "awarenessStatus": driver_status.awareness, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, "stepChange": driver_status.step_change, "awarenessActive": driver_status.awareness_active, "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isActiveMode": driver_status.active_monitoring_mode, } pm.send('driverMonitoringState', dat)