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 run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status): # inputs are all 10Hz DS = DriverStatus() events_from_DM = [] for idx in range(len(driver_state_msgs)): e = Events() DS.get_pose(driver_state_msgs[idx], [0, 0, 0], 0, openpilot_status[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests DS.update(e, driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx]) events_from_DM.append(e) assert len(events_from_DM) == len(driver_state_msgs), 'somethings wrong' return events_from_DM, DS
def dmonitoringd_thread(sm=None, pm=None): 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'].vEgo = 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 if driver_engaged: driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill, sm['carState'].vEgo) v_cruise_last = v_cruise if sm.updated['modelV2']: driver_status.set_policy(sm['modelV2']) # Get data from dmonitoringmodeld events = Events() driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill, sm['carState'].vEgo) # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState') dat.driverMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "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)
def dmonitoringd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(53) params = Params() # Pub/Sub Sockets if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster( ['driverState', 'liveCalibration', 'carState', 'model']) driver_status = DriverStatus() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].gasPressed = False sm['carState'].standstill = True cal_rpy = [0, 0, 0] v_cruise_last = 0 driver_engaged = False # 10Hz <- dmonitoringmodeld while True: sm.update() # Handle calibration if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: if len(sm['liveCalibration'].rpyCalib) == 3: cal_rpy = sm['liveCalibration'].rpyCalib # 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 if driver_engaged: driver_status.update(Events(), True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) v_cruise_last = v_cruise # Get model meta if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld if sm.updated['driverState']: events = Events() driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) # Block any engage after certain distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "rhdChecked": driver_status.is_rhd_region_checked, "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, "isPreview": False, } pm.send('dMonitoringState', dat)
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([ 'driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2' ], poll=['driverStateV2']) driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected")) 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['driverStateV2']: 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['driverStateV2'], 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, "isRHD": driver_status.wheel_on_right, } pm.send('driverMonitoringState', dat) # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and driver_status.wheelpos_learner.filtered_stat.n > driver_status.settings._WHEELPOS_FILTER_MIN_COUNT and driver_status.wheel_on_right == (driver_status.wheelpos_learner.filtered_stat.M > driver_status.settings._WHEELPOS_THRESHOLD)): put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right)
def dmonitoringd_thread(sm=None, pm=None): if pm is None: pm = messaging.PubMaster(['dMonitoringState']) if sm is None: sm = messaging.SubMaster([ 'driverState', 'liveCalibration', 'carState', 'model', 'dragonConf' ], poll=['driverState']) driver_status = DriverStatus() driver_status.is_rhd_region = Params().get("IsRHD") == b"1" offroad = Params().get("IsOffroad") == b"1" sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].vEgo = 0. sm['carState'].cruiseState.enabled = False sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] sm['carState'].steeringPressed = False sm['carState'].gasPressed = False sm['carState'].brakePressed = False sm['carState'].standstill = True v_cruise_last = 0 driver_engaged = False # dp sm['dragonConf'].dpDriverMonitor = True sm['dragonConf'].dpSteeringMonitor = True sm['dragonConf'].dpSteeringMonitorTimer = 70 # 10Hz <- dmonitoringmodeld while True: start_time = sec_since_boot() sm.update() # dp if not sm['dragonConf'].dpDriverMonitor: driver_status.active_monitoring_mode = False driver_status.face_detected = False driver_status.threshold_pre = 15. / sm[ 'dragonConf'].dpSteeringMonitorTimer driver_status.threshold_prompt = 6. / sm[ 'dragonConf'].dpSteeringMonitorTimer driver_status.step_change = DT_DMON / sm[ 'dragonConf'].dpSteeringMonitorTimer if not sm['dragonConf'].dpSteeringMonitor: driver_status.awareness = 1. driver_status.awareness_active = 1. driver_status.awareness_passive = 1. driver_status.terminal_alert_cnt = 0 driver_status.terminal_time = 0 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 or \ sm['carState'].brakePressed if driver_engaged: driver_status.update(Events(), True, sm['carState'].cruiseState.enabled, sm['carState'].standstill) v_cruise_last = v_cruise if sm.updated['model']: driver_status.set_policy(sm['model']) # Get data from dmonitoringmodeld events = Events() if sm['dragonConf'].dpDriverMonitor: driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # build dMonitoringState packet dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, "isRHD": driver_status.is_rhd_region, "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, "isPreview": offroad, } pm.send('dMonitoringState', dat) diff = sec_since_boot() - start_time if not sm['dragonConf'].dpDriverMonitor and diff < 0.1: time.sleep(0.1 - diff)