|
|
|
@ -40,7 +40,7 @@ def dmonitoringd_thread(sm=None, pm=None): |
|
|
|
|
|
|
|
|
|
# 10Hz <- dmonitoringmodeld |
|
|
|
|
while True: |
|
|
|
|
sm.update() |
|
|
|
|
sm.update(wait_for="driverState") |
|
|
|
|
|
|
|
|
|
# Get interaction |
|
|
|
|
if sm.updated['carState']: |
|
|
|
@ -57,37 +57,36 @@ def dmonitoringd_thread(sm=None, pm=None): |
|
|
|
|
driver_status.set_policy(sm['model']) |
|
|
|
|
|
|
|
|
|
# Get data from dmonitoringmodeld |
|
|
|
|
if sm.updated['driverState']: |
|
|
|
|
events = Events() |
|
|
|
|
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) |
|
|
|
|
events = Events() |
|
|
|
|
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) |
|
|
|
|
|
|
|
|
|
def main(sm=None, pm=None): |
|
|
|
|
dmonitoringd_thread(sm, pm) |
|
|
|
|