2020-01-30 20:12:44 -07:00
|
|
|
#!/usr/bin/env python3
|
2022-01-18 14:39:11 -07:00
|
|
|
import gc
|
|
|
|
|
|
|
|
import cereal.messaging as messaging
|
2020-05-14 16:21:21 -06:00
|
|
|
from cereal import car
|
2020-04-15 17:48:44 -06:00
|
|
|
from common.params import Params
|
2022-01-18 14:39:11 -07:00
|
|
|
from common.realtime import set_realtime_priority
|
2020-05-14 16:21:21 -06:00
|
|
|
from selfdrive.controls.lib.events import Events
|
2020-09-10 13:16:29 -06:00
|
|
|
from selfdrive.locationd.calibrationd import Calibration
|
2022-01-18 14:39:11 -07:00
|
|
|
from selfdrive.monitoring.driver_monitor import DriverStatus
|
2020-01-30 20:12:44 -07:00
|
|
|
|
2020-08-01 18:26:35 -06:00
|
|
|
|
2020-01-30 20:12:44 -07:00
|
|
|
def dmonitoringd_thread(sm=None, pm=None):
|
2022-01-18 14:39:11 -07:00
|
|
|
gc.disable()
|
|
|
|
set_realtime_priority(2)
|
|
|
|
|
2020-01-30 20:12:44 -07:00
|
|
|
if pm is None:
|
2021-02-03 20:57:30 -07:00
|
|
|
pm = messaging.PubMaster(['driverMonitoringState'])
|
2020-01-30 20:12:44 -07:00
|
|
|
|
|
|
|
if sm is None:
|
2021-01-19 18:15:16 -07:00
|
|
|
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState'])
|
2020-01-30 20:12:44 -07:00
|
|
|
|
2021-04-07 07:36:37 -06:00
|
|
|
driver_status = DriverStatus(rhd=Params().get_bool("IsRHD"))
|
2020-08-29 18:16:00 -06:00
|
|
|
|
2020-01-30 20:12:44 -07:00
|
|
|
sm['liveCalibration'].calStatus = Calibration.INVALID
|
2020-08-01 18:26:35 -06:00
|
|
|
sm['liveCalibration'].rpyCalib = [0, 0, 0]
|
2020-01-30 20:12:44 -07:00
|
|
|
sm['carState'].buttonEvents = []
|
|
|
|
sm['carState'].standstill = True
|
|
|
|
|
|
|
|
v_cruise_last = 0
|
|
|
|
driver_engaged = False
|
|
|
|
|
|
|
|
# 10Hz <- dmonitoringmodeld
|
|
|
|
while True:
|
|
|
|
sm.update()
|
|
|
|
|
2020-09-12 18:50:22 -06:00
|
|
|
if not sm.updated['driverState']:
|
|
|
|
continue
|
|
|
|
|
2020-01-30 20:12:44 -07:00
|
|
|
# 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 \
|
2020-07-20 07:20:24 -06:00
|
|
|
sm['carState'].steeringPressed or \
|
|
|
|
sm['carState'].gasPressed
|
2020-01-30 20:12:44 -07:00
|
|
|
v_cruise_last = v_cruise
|
|
|
|
|
2021-01-19 18:15:16 -07:00
|
|
|
if sm.updated['modelV2']:
|
2021-12-14 13:13:59 -07:00
|
|
|
driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo)
|
2020-01-30 20:12:44 -07:00
|
|
|
|
|
|
|
# Get data from dmonitoringmodeld
|
2020-09-01 17:41:09 -06:00
|
|
|
events = Events()
|
2022-03-15 20:02:21 -06:00
|
|
|
driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
|
2020-09-01 17:41:09 -06:00
|
|
|
|
|
|
|
# Block engaging after max number of distrations
|
2021-07-23 22:18:28 -06:00
|
|
|
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
|
|
|
|
driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION:
|
2020-09-01 17:41:09 -06:00
|
|
|
events.add(car.CarEvent.EventName.tooDistracted)
|
|
|
|
|
|
|
|
# Update events from driver state
|
2022-03-15 20:02:21 -06:00
|
|
|
driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
|
2020-09-01 17:41:09 -06:00
|
|
|
|
2021-02-03 20:57:30 -07:00
|
|
|
# build driverMonitoringState packet
|
|
|
|
dat = messaging.new_message('driverMonitoringState')
|
|
|
|
dat.driverMonitoringState = {
|
2020-09-01 17:41:09 -06:00
|
|
|
"events": events.to_msg(),
|
|
|
|
"faceDetected": driver_status.face_detected,
|
|
|
|
"isDistracted": driver_status.driver_distracted,
|
2022-03-15 20:02:21 -06:00
|
|
|
"distractedType": sum(driver_status.distracted_types),
|
2020-09-01 17:41:09 -06:00
|
|
|
"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,
|
2021-01-19 16:11:15 -07:00
|
|
|
"isActiveMode": driver_status.active_monitoring_mode,
|
2020-09-01 17:41:09 -06:00
|
|
|
}
|
2021-02-03 20:57:30 -07:00
|
|
|
pm.send('driverMonitoringState', dat)
|
2020-01-30 20:12:44 -07:00
|
|
|
|
2022-01-18 14:39:11 -07:00
|
|
|
|
2020-01-30 20:12:44 -07:00
|
|
|
def main(sm=None, pm=None):
|
|
|
|
dmonitoringd_thread(sm, pm)
|
|
|
|
|
2022-01-18 14:39:11 -07:00
|
|
|
|
2020-01-30 20:12:44 -07:00
|
|
|
if __name__ == '__main__':
|
2020-05-14 16:21:21 -06:00
|
|
|
main()
|