dmonitoringd (#1016)

old-commit-hash: 6322a275d6
commatwo_master
ZwX1616 5 years ago committed by GitHub
parent 1dee8638d6
commit 2ad685b194
  1. 2
      .gitignore
  2. 2
      cereal
  3. 1
      models/dmonitoring_model.current
  4. 3
      models/dmonitoring_model.keras
  5. 3
      models/dmonitoring_model_q.dlc
  6. 1
      models/monitoring_model.current
  7. 3
      models/monitoring_model.keras
  8. 3
      models/monitoring_model_q.dlc
  9. 11
      release/files_common
  10. 8
      selfdrive/camerad/main.cc
  11. 68
      selfdrive/controls/controlsd.py
  12. 108
      selfdrive/controls/dmonitoringd.py
  13. 12
      selfdrive/controls/lib/alerts.py
  14. 57
      selfdrive/controls/lib/driver_monitor.py
  15. 6
      selfdrive/controls/tests/test_monitoring.py
  16. 2
      selfdrive/debug/cpu_usage_stat.py
  17. 6
      selfdrive/manager.py
  18. 6
      selfdrive/modeld/SConscript
  19. 2
      selfdrive/modeld/dmonitoringmodeld
  20. 18
      selfdrive/modeld/dmonitoringmodeld.cc
  21. 16
      selfdrive/modeld/models/dmonitoring.cc
  22. 20
      selfdrive/modeld/models/dmonitoring.h
  23. 12
      selfdrive/test/longitudinal_maneuvers/plant.py
  24. 4
      selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
  25. 12
      selfdrive/test/process_replay/process_replay.py
  26. 2
      selfdrive/test/process_replay/ref_commit
  27. 2
      selfdrive/test/test_openpilot.py

2
.gitignore vendored

@ -44,7 +44,7 @@ selfdrive/sensord/_gpsd
selfdrive/sensord/_sensord selfdrive/sensord/_sensord
selfdrive/camerad/camerad selfdrive/camerad/camerad
selfdrive/modeld/_modeld selfdrive/modeld/_modeld
selfdrive/modeld/_monitoringd selfdrive/modeld/_dmonitoringmodeld
/src/ /src/
one one

@ -1 +1 @@
Subproject commit 8f9aa8fcc3e9c7d566d1d388aea4cbf642abaa8b Subproject commit 3f01dcf01cea9833a73e9779cc9237e9bda80e1e

@ -0,0 +1 @@
579c4158-b98d-41ba-9932-53ecd62e8b9f

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6a09d27ea8e7e286044f7cbfb7796b9acde2ac6a41f767358d1a2dd2df2fa4c6
size 814176

@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:731e44cdc2d1ba04a4c18f20a08605a9f3c73a7ca4152d811e401d8f4a70fab5
size 186615

@ -1 +0,0 @@
5f173fc6-cc91-4d62-8cdb-b33cf8713617

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0465111974591de2954c0ff29ae025634cb5eb79d6e259ebc945fb8a77a96977
size 814176

@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:8ed5fa77d02ed5f4c4ee4c4793916d24f024d07640ba277dec13b99c0d5a1d3a
size 186615

@ -55,7 +55,7 @@ common/transformations/orientation.py
common/api/__init__.py common/api/__init__.py
models/supercombo.dlc models/supercombo.dlc
models/monitoring_model_q.dlc models/dmonitoring_model_q.dlc
selfdrive/version.py selfdrive/version.py
@ -200,6 +200,7 @@ selfdrive/controls/tests/*
selfdrive/controls/controlsd.py selfdrive/controls/controlsd.py
selfdrive/controls/plannerd.py selfdrive/controls/plannerd.py
selfdrive/controls/radard.py selfdrive/controls/radard.py
selfdrive/controls/dmonitoringd.py
selfdrive/controls/lib/__init__.py selfdrive/controls/lib/__init__.py
selfdrive/controls/lib/alertmanager.py selfdrive/controls/lib/alertmanager.py
selfdrive/controls/lib/alerts.py selfdrive/controls/lib/alerts.py
@ -334,17 +335,17 @@ selfdrive/camerad/transforms/rgb_to_yuv_test.cc
selfdrive/modeld/SConscript selfdrive/modeld/SConscript
selfdrive/modeld/modeld.cc selfdrive/modeld/modeld.cc
selfdrive/modeld/monitoringd.cc selfdrive/modeld/dmonitoringmodeld.cc
selfdrive/modeld/constants.py selfdrive/modeld/constants.py
selfdrive/modeld/modeld selfdrive/modeld/modeld
selfdrive/modeld/monitoringd selfdrive/modeld/dmonitoringmodeld
selfdrive/modeld/models/commonmodel.c selfdrive/modeld/models/commonmodel.c
selfdrive/modeld/models/commonmodel.h selfdrive/modeld/models/commonmodel.h
selfdrive/modeld/models/driving.cc selfdrive/modeld/models/driving.cc
selfdrive/modeld/models/driving.h selfdrive/modeld/models/driving.h
selfdrive/modeld/models/monitoring.cc selfdrive/modeld/models/dmonitoring.cc
selfdrive/modeld/models/monitoring.h selfdrive/modeld/models/dmonitoring.h
selfdrive/modeld/transforms/loadyuv.[c,h] selfdrive/modeld/transforms/loadyuv.[c,h]
selfdrive/modeld/transforms/loadyuv.cl selfdrive/modeld/transforms/loadyuv.cl

@ -147,7 +147,7 @@ void* frontview_thread(void *arg) {
// we subscribe to this for placement of the AE metering box // we subscribe to this for placement of the AE metering box
// TODO: the loop is bad, ideally models shouldn't affect sensors // TODO: the loop is bad, ideally models shouldn't affect sensors
Context *msg_context = Context::create(); Context *msg_context = Context::create();
SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverMonitoring", "127.0.0.1", true); SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverState", "127.0.0.1", true);
assert(monitoring_sock != NULL); assert(monitoring_sock != NULL);
cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err);
@ -194,10 +194,10 @@ void* frontview_thread(void *arg) {
capnp::FlatArrayMessageReader cmsg(amsg); capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
float face_prob = event.getDriverMonitoring().getFaceProb(); float face_prob = event.getDriverState().getFaceProb();
float face_position[2]; float face_position[2];
face_position[0] = event.getDriverMonitoring().getFacePosition()[0]; face_position[0] = event.getDriverState().getFacePosition()[0];
face_position[1] = event.getDriverMonitoring().getFacePosition()[1]; face_position[1] = event.getDriverState().getFacePosition()[1];
// set front camera metering target // set front camera metering target
if (face_prob > 0.4) if (face_prob > 0.4)

@ -6,7 +6,7 @@ from cereal import car, log
from common.numpy_fast import clip from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
from common.profiler import Profiler from common.profiler import Profiler
from common.params import Params, put_nonblocking from common.params import Params
import cereal.messaging as messaging import cereal.messaging as messaging
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.boardd.boardd import can_list_to_can_capnp
@ -23,9 +23,7 @@ from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.controls.lib.gps_helpers import is_rhd_region
from selfdrive.locationd.calibration_helpers import Calibration, Filter from selfdrive.locationd.calibration_helpers import Calibration, Filter
LANE_DEPARTURE_THRESHOLD = 0.1 LANE_DEPARTURE_THRESHOLD = 0.1
@ -67,7 +65,7 @@ def events_to_bytes(events):
return ret return ret
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params): def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space""" """Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events # Update carstate from CAN and create events
@ -77,6 +75,7 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca
sm.update(0) sm.update(0)
events = list(CS.events) events = list(CS.events)
events += list(sm['dMonitoringState'].events)
add_lane_change_event(events, sm['pathPlan']) add_lane_change_event(events, sm['pathPlan'])
enabled = isEnabled(state) enabled = isEnabled(state)
@ -103,27 +102,15 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca
if CS.stockAeb: if CS.stockAeb:
events.append(create_event('stockAeb', [])) events.append(create_event('stockAeb', []))
# GPS coords RHD parsing, once every restart
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", "1" if is_rhd else "0")
# Handle calibration # Handle calibration
cal_status = sm['liveCalibration'].calStatus cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc cal_perc = sm['liveCalibration'].calPerc
cal_rpy = [0,0,0]
if cal_status != Calibration.CALIBRATED: if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED: if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else: else:
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
rpy = sm['liveCalibration'].rpyCalib
if len(rpy) == 3:
cal_rpy = rpy
# When the panda and controlsd do not agree on controls_allowed # When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through # we want to disengage openpilot. However the status from the panda goes through
@ -138,16 +125,6 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca
if mismatch_counter >= 200: if mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if sm.updated['model']:
driver_status.set_policy(sm['model'])
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_perc, mismatch_counter, can_error_counter return CS, events, cal_perc, mismatch_counter, can_error_counter
@ -239,7 +216,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame):
"""Given the state, this function returns an actuators packet""" """Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message() actuators = car.CarControl.Actuators.new_message()
@ -247,17 +224,9 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
enabled = isEnabled(state) enabled = isEnabled(state)
active = isActive(state) active = isActive(state)
# check if user has interacted with the car
driver_engaged = len(CS.buttonEvents) > 0 or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed
if CS.leftBlinker or CS.rightBlinker: if CS.leftBlinker or CS.rightBlinker:
last_blinker_frame = frame last_blinker_frame = frame
# add eventual driver distracted events
events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill)
if plan.fcw: if plan.fcw:
# send FCW alert if triggered by planner # send FCW alert if triggered by planner
AM.add(frame, "fcw", enabled) AM.add(frame, "fcw", enabled)
@ -315,11 +284,11 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
last_blinker_frame, is_ldw_enabled, can_error_counter): last_blinker_frame, is_ldw_enabled, can_error_counter):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging""" """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
@ -373,7 +342,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk
can_sends = CI.apply(CC) can_sends = CI.apply(CC)
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = driver_status.awareness < 0. force_decel = sm['dMonitoringState'].awarenessStatus < 0.
# controlsState # controlsState
dat = messaging.new_message() dat = messaging.new_message()
@ -387,8 +356,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk
"alertBlinkingRate": AM.alert_rate, "alertBlinkingRate": AM.alert_rate,
"alertType": AM.alert_type, "alertType": AM.alert_type,
"alertSound": AM.audible_alert, "alertSound": AM.audible_alert,
"awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, "driverMonitoringOn": sm['dMonitoringState'].faceDetected,
"driverMonitoringOn": bool(driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes), "canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": sm.logMonoTime['plan'], "planMonoTime": sm.logMonoTime['plan'],
"pathPlanMonoTime": sm.logMonoTime['pathPlan'], "pathPlanMonoTime": sm.logMonoTime['pathPlan'],
@ -483,8 +451,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
if sm is None: if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
'model', 'gpsLocation'], ignore_alive=['gpsLocation']) 'model'])
if can_sock is None: if can_sock is None:
@ -529,11 +497,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
elif CP.lateralTuning.which() == 'lqr': elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP) LaC = LatControlLQR(CP)
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd = bool(int(is_rhd))
state = State.disabled state = State.disabled
soft_disable_timer = 0 soft_disable_timer = 0
v_cruise_kph = 255 v_cruise_kph = 255
@ -547,6 +510,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
sm['pathPlan'].sensorValid = True sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1. sm['thermal'].freeSpace = 1.
sm['dMonitoringState'].events = []
sm['dMonitoringState'].awarenessStatus = 1.
sm['dMonitoringState'].faceDetected = False
# detect sound card presence # detect sound card presence
sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
@ -563,7 +529,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
prof.checkpoint("Ratekeeper", ignore=True) prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events # Sample data and compute car events
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params) CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params)
prof.checkpoint("Sample") prof.checkpoint("Sample")
# Create alerts # Create alerts
@ -605,14 +571,14 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
prof.checkpoint("State transition") prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC) # Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame)
prof.checkpoint("State Control") prof.checkpoint("State Control")
# Publish data # Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC,
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter) is_ldw_enabled, can_error_counter)
prof.checkpoint("Sent") prof.checkpoint("Sent")

@ -0,0 +1,108 @@
#!/usr/bin/env python3
import gc
from common.realtime import set_realtime_priority
from common.params import Params, put_nonblocking
import cereal.messaging as messaging
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.controls.lib.gps_helpers import is_rhd_region
def dmonitoringd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model', 'gpsLocation'], ignore_alive=['gpsLocation'])
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd_region = bool(int(is_rhd))
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'].standstill = True
cal_rpy = [0,0,0]
v_cruise_last = 0
driver_engaged = False
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
# GPS coords RHD parsing, once every restart
if not driver_status.is_rhd_region_checked and sm.updated['gpsLocation']:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", "1" if is_rhd else "0")
# 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
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 = []
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.append(create_event("tooDistracted", [ET.NO_ENTRY]))
# Update events from driver state
events = driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
# dMonitoringState packet
dat = messaging.new_message()
dat.init('dMonitoringState')
dat.dMonitoringState = {
"events": events,
"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,
}
pm.send('dMonitoringState', dat)
def main(sm=None, pm=None):
dmonitoringd_thread(sm, pm)
if __name__ == '__main__':
main()

@ -110,7 +110,7 @@ ALERTS = [
Alert( Alert(
"preDriverDistracted", "preDriverDistracted",
"KEEP EYES ON ROAD: User Appears Distracted", "KEEP EYES ON ROAD: Driver Appears Distracted",
"", "",
AlertStatus.normal, AlertSize.small, AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
@ -118,14 +118,14 @@ ALERTS = [
Alert( Alert(
"promptDriverDistracted", "promptDriverDistracted",
"KEEP EYES ON ROAD", "KEEP EYES ON ROAD",
"User Appears Distracted", "Driver Appears Distracted",
AlertStatus.userPrompt, AlertSize.mid, AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
Alert( Alert(
"driverDistracted", "driverDistracted",
"DISENGAGE IMMEDIATELY", "DISENGAGE IMMEDIATELY",
"User Was Distracted", "Driver Was Distracted",
AlertStatus.critical, AlertSize.full, AlertStatus.critical, AlertSize.full,
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
@ -139,14 +139,14 @@ ALERTS = [
Alert( Alert(
"promptDriverUnresponsive", "promptDriverUnresponsive",
"TOUCH STEERING WHEEL", "TOUCH STEERING WHEEL",
"User Is Unresponsive", "Driver Is Unresponsive",
AlertStatus.userPrompt, AlertSize.mid, AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
Alert( Alert(
"driverUnresponsive", "driverUnresponsive",
"DISENGAGE IMMEDIATELY", "DISENGAGE IMMEDIATELY",
"User Was Unresponsive", "Driver Was Unresponsive",
AlertStatus.critical, AlertSize.full, AlertStatus.critical, AlertSize.full,
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
@ -155,7 +155,7 @@ ALERTS = [
"CHECK DRIVER FACE VISIBILITY", "CHECK DRIVER FACE VISIBILITY",
"Driver Monitor Model Output Uncertain", "Driver Monitor Model Output Uncertain",
AlertStatus.normal, AlertSize.mid, AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 0., 1.), Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.),
Alert( Alert(
"geofence", "geofence",

@ -1,13 +1,13 @@
from common.numpy_fast import interp from common.numpy_fast import interp
from math import atan2, sqrt from math import atan2, sqrt
from common.realtime import DT_CTRL, DT_DMON from common.realtime import DT_DMON
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from common.filter_simple import FirstOrderFilter from common.filter_simple import FirstOrderFilter
from common.stat_live import RunningStatFilter from common.stat_live import RunningStatFilter
_AWARENESS_TIME = 100. # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status _AWARENESS_TIME = 70. # one minute limit without user touching steering wheels make the car enter a terminal status
_AWARENESS_PRE_TIME_TILL_TERMINAL = 25. # a first alert is issued 25s before expiration _AWARENESS_PRE_TIME_TILL_TERMINAL = 15. # a first alert is issued 25s before expiration
_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15. # a second alert is issued 15s before start decelerating the car _AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. # a second alert is issued 15s before start decelerating the car
_DISTRACTED_TIME = 11. _DISTRACTED_TIME = 11.
_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
@ -26,18 +26,19 @@ _PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
_PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up _PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) _YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
_HI_STD_TIMEOUT = 2 _HI_STD_TIMEOUT = 5
_HI_STD_FALLBACK_TIME = 10 # fall back to wheel touch if model is uncertain for a long time
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz _DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
_POSE_CALIB_MIN_SPEED = 13 # 30 mph _POSE_CALIB_MIN_SPEED = 13 # 30 mph
_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts _POSE_OFFSET_MIN_COUNT = 60 # valid data counts before calibration completes, 1 seg is 600 counts
_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory" _POSE_OFFSET_MAX_COUNT = 360 # stop deweighting new data after 6 min, aka "short term memory"
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change _RECOVERY_FACTOR_MAX = 5. # relative to minus step change
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change _RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
MAX_TERMINAL_DURATION = 3000 # 30s MAX_TERMINAL_DURATION = 300 # 30s
# model output refers to center of cropped image, so need to apply the x displacement offset # model output refers to center of cropped image, so need to apply the x displacement offset
RESIZED_FOCAL = 320.0 RESIZED_FOCAL = 320.0
@ -115,7 +116,7 @@ class DriverStatus():
def _set_timers(self, active_monitoring): def _set_timers(self, active_monitoring):
if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
if active_monitoring: if active_monitoring:
self.step_change = DT_CTRL / _DISTRACTED_TIME self.step_change = DT_DMON / _DISTRACTED_TIME
else: else:
self.step_change = 0. self.step_change = 0.
return # no exploit after orange alert return # no exploit after orange alert
@ -130,7 +131,7 @@ class DriverStatus():
self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.step_change = DT_CTRL / _DISTRACTED_TIME self.step_change = DT_DMON / _DISTRACTED_TIME
self.active_monitoring_mode = True self.active_monitoring_mode = True
else: else:
if self.active_monitoring_mode: if self.active_monitoring_mode:
@ -139,7 +140,7 @@ class DriverStatus():
self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.step_change = DT_CTRL / _AWARENESS_TIME self.step_change = DT_DMON / _AWARENESS_TIME
self.active_monitoring_mode = False self.active_monitoring_mode = False
def _is_driver_distracted(self, pose, blink): def _is_driver_distracted(self, pose, blink):
@ -168,21 +169,21 @@ class DriverStatus():
self.pose.cfactor = interp(ep, [0, 0.5, 1], [_METRIC_THRESHOLD_STRICT, _METRIC_THRESHOLD, _METRIC_THRESHOLD_SLACK])/_METRIC_THRESHOLD self.pose.cfactor = interp(ep, [0, 0.5, 1], [_METRIC_THRESHOLD_STRICT, _METRIC_THRESHOLD, _METRIC_THRESHOLD_SLACK])/_METRIC_THRESHOLD
self.blink.cfactor = interp(ep, [0, 0.5, 1], [_BLINK_THRESHOLD_STRICT, _BLINK_THRESHOLD, _BLINK_THRESHOLD_SLACK])/_BLINK_THRESHOLD self.blink.cfactor = interp(ep, [0, 0.5, 1], [_BLINK_THRESHOLD_STRICT, _BLINK_THRESHOLD, _BLINK_THRESHOLD_SLACK])/_BLINK_THRESHOLD
def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged): def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged):
# 10 Hz # 10 Hz
if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0 or len(driver_monitoring.faceOrientationStd) == 0 or len(driver_monitoring.facePositionStd) == 0: if len(driver_state.faceOrientation) == 0 or len(driver_state.facePosition) == 0 or len(driver_state.faceOrientationStd) == 0 or len(driver_state.facePositionStd) == 0:
return return
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy) self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy)
self.pose.pitch_std = driver_monitoring.faceOrientationStd[0] self.pose.pitch_std = driver_state.faceOrientationStd[0]
self.pose.yaw_std = driver_monitoring.faceOrientationStd[1] self.pose.yaw_std = driver_state.faceOrientationStd[1]
# self.pose.roll_std = driver_monitoring.faceOrientationStd[2] # self.pose.roll_std = driver_state.faceOrientationStd[2]
max_std = max(self.pose.pitch_std, self.pose.yaw_std) model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
self.pose.low_std = max_std < _POSESTD_THRESHOLD self.pose.low_std = model_std_max < _POSESTD_THRESHOLD
self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD) self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb>_EYE_THRESHOLD)
self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD) self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb>_EYE_THRESHOLD)
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and \ self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \
abs(driver_monitoring.facePosition[0]) <= 0.4 and abs(driver_monitoring.facePosition[1]) <= 0.45 and \ abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 and \
not self.is_rhd_region not self.is_rhd_region
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0
@ -198,9 +199,11 @@ class DriverStatus():
self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \ self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
self._set_timers(self.face_detected) is_model_uncertain = self.hi_stds * DT_DMON > _HI_STD_FALLBACK_TIME
self._set_timers(self.face_detected and not is_model_uncertain)
if self.face_detected and not self.pose.low_std: if self.face_detected and not self.pose.low_std:
self.step_change *= max(0, (max_std-0.5)*(max_std-2)) if not is_model_uncertain:
self.step_change *= max(0, (model_std_max-0.5)*(model_std_max-2))
self.hi_stds += 1 self.hi_stds += 1
elif self.face_detected and self.pose.low_std: elif self.face_detected and self.pose.low_std:
self.hi_stds = 0 self.hi_stds = 0
@ -219,7 +222,7 @@ class DriverStatus():
if self.face_detected and self.hi_stds * DT_DMON > _HI_STD_TIMEOUT: if self.face_detected and self.hi_stds * DT_DMON > _HI_STD_TIMEOUT:
events.append(create_event('driverMonitorLowAcc', [ET.WARNING])) events.append(create_event('driverMonitorLowAcc', [ET.WARNING]))
if (driver_attentive and self.face_detected and self.awareness > 0): if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0):
# only restore awareness when paying attention and alert is not red # only restore awareness when paying attention and alert is not red
self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.) self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
if self.awareness == 1.: if self.awareness == 1.:
@ -229,7 +232,7 @@ class DriverStatus():
return events return events
# should always be counting if distracted unless at standstill and reaching orange # should always be counting if distracted unless at standstill and reaching orange
if (not self.face_detected or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \ if (not (self.face_detected and self.hi_stds * DT_DMON <= _HI_STD_FALLBACK_TIME) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
not (standstill and self.awareness - self.step_change <= self.threshold_prompt): not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
self.awareness = max(self.awareness - self.step_change, -0.1) self.awareness = max(self.awareness - self.step_change, -0.1)

@ -1,6 +1,6 @@
import unittest import unittest
import numpy as np import numpy as np
from common.realtime import DT_CTRL, DT_DMON from common.realtime import DT_DMON
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \ from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \
_AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \ _AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \
_AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \ _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \
@ -63,8 +63,6 @@ def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status,
DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx]) DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx])
# cal_rpy and car_speed don't matter here # cal_rpy and car_speed don't matter here
# to match frequency of controlsd (100Hz)
for _ in range(int(DT_DMON/DT_CTRL)):
event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx]) event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx])
events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests
@ -228,7 +226,7 @@ class TestMonitoring(unittest.TestCase):
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'preDriverDistracted') self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'preDriverDistracted')
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'promptDriverDistracted') self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)][1].name, 'promptDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_TIME+1)/DT_DMON)][1].name, 'promptDriverDistracted') self.assertEqual(events_output[int((_DISTRACTED_TIME+1)/DT_DMON)][1].name, 'promptDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_TIME*2.5)/DT_DMON)][1].name, 'driverDistracted') self.assertEqual(events_output[int((_DISTRACTED_TIME*2.5)/DT_DMON)][1].name, 'promptDriverDistracted') # set_timer blocked
if __name__ == "__main__": if __name__ == "__main__":
print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS) print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS)

@ -30,7 +30,7 @@ SLEEP_INTERVAL = 0.2
monitored_proc_names = [ monitored_proc_names = [
'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned', 'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned',
'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'monitoringd', 'camerad', 'sensord', 'updated', 'gpsd', 'athena'] 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena']
cpu_time_names = ['user', 'system', 'children_user', 'children_system'] cpu_time_names = ['user', 'system', 'children_user', 'children_system']
timer = getattr(time, 'monotonic', time.time) timer = getattr(time, 'monotonic', time.time)

@ -136,6 +136,7 @@ managed_processes = {
"controlsd": "selfdrive.controls.controlsd", "controlsd": "selfdrive.controls.controlsd",
"plannerd": "selfdrive.controls.plannerd", "plannerd": "selfdrive.controls.plannerd",
"radard": "selfdrive.controls.radard", "radard": "selfdrive.controls.radard",
"dmonitoringd": "selfdrive.controls.dmonitoringd",
"ubloxd": ("selfdrive/locationd", ["./ubloxd"]), "ubloxd": ("selfdrive/locationd", ["./ubloxd"]),
"loggerd": ("selfdrive/loggerd", ["./loggerd"]), "loggerd": ("selfdrive/loggerd", ["./loggerd"]),
"logmessaged": "selfdrive.logmessaged", "logmessaged": "selfdrive.logmessaged",
@ -152,7 +153,7 @@ managed_processes = {
"clocksd": ("selfdrive/clocksd", ["./clocksd"]), "clocksd": ("selfdrive/clocksd", ["./clocksd"]),
"gpsd": ("selfdrive/sensord", ["./gpsd"]), "gpsd": ("selfdrive/sensord", ["./gpsd"]),
"updated": "selfdrive.updated", "updated": "selfdrive.updated",
"monitoringd": ("selfdrive/modeld", ["./monitoringd"]), "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
"modeld": ("selfdrive/modeld", ["./modeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]),
} }
@ -194,6 +195,7 @@ car_started_processes = [
'plannerd', 'plannerd',
'loggerd', 'loggerd',
'radard', 'radard',
'dmonitoringd',
'calibrationd', 'calibrationd',
'paramsd', 'paramsd',
'camerad', 'camerad',
@ -206,7 +208,7 @@ if ANDROID:
'sensord', 'sensord',
'clocksd', 'clocksd',
'gpsd', 'gpsd',
'monitoringd', 'dmonitoringmodeld',
'deleter', 'deleter',
] ]

@ -24,9 +24,9 @@ else:
common = lenv.Object(common_src) common = lenv.Object(common_src)
lenv.Program('_monitoringd', [ lenv.Program('_dmonitoringmodeld', [
"monitoringd.cc", "dmonitoringmodeld.cc",
"models/monitoring.cc", "models/dmonitoring.cc",
]+common, LIBS=libs) ]+common, LIBS=libs)
lenv.Program('_modeld', [ lenv.Program('_modeld', [

@ -1,5 +1,5 @@
#!/bin/sh #!/bin/sh
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/" export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/"
exec ./_monitoringd exec ./_dmonitoringmodeld

@ -8,7 +8,7 @@
#include "common/visionipc.h" #include "common/visionipc.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "models/monitoring.h" #include "models/dmonitoring.h"
#ifndef PATH_MAX #ifndef PATH_MAX
#include <linux/limits.h> #include <linux/limits.h>
@ -27,11 +27,11 @@ int main(int argc, char **argv) {
// messaging // messaging
Context *msg_context = Context::create(); Context *msg_context = Context::create();
PubSocket *monitoring_sock = PubSocket::create(msg_context, "driverMonitoring"); PubSocket *dmonitoring_sock = PubSocket::create(msg_context, "driverState");
// init the models // init the models
MonitoringState monitoring; DMonitoringModelState dmonitoringmodel;
monitoring_init(&monitoring); dmonitoring_init(&dmonitoringmodel);
// loop // loop
VisionStream stream; VisionStream stream;
@ -58,14 +58,14 @@ int main(int argc, char **argv) {
double t1 = millis_since_boot(); double t1 = millis_since_boot();
MonitoringResult res = monitoring_eval_frame(&monitoring, buf->addr, buf_info.width, buf_info.height); DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf_info.width, buf_info.height);
double t2 = millis_since_boot(); double t2 = millis_since_boot();
// send dm packet // send dm packet
monitoring_publish(monitoring_sock, extra.frame_id, res); dmonitoring_publish(dmonitoring_sock, extra.frame_id, res);
LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1; last = t1;
} }
@ -73,8 +73,8 @@ int main(int argc, char **argv) {
visionstream_destroy(&stream); visionstream_destroy(&stream);
delete monitoring_sock; delete dmonitoring_sock;
monitoring_free(&monitoring); dmonitoring_free(&dmonitoringmodel);
return 0; return 0;
} }

@ -1,5 +1,5 @@
#include <string.h> #include <string.h>
#include "monitoring.h" #include "dmonitoring.h"
#include "common/mat.h" #include "common/mat.h"
#include "common/timing.h" #include "common/timing.h"
@ -10,11 +10,11 @@
#define MODEL_HEIGHT 320 #define MODEL_HEIGHT 320
#define FULL_W 426 #define FULL_W 426
void monitoring_init(MonitoringState* s) { void dmonitoring_init(DMonitoringModelState* s) {
s->m = new DefaultRunModel("../../models/monitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); s->m = new DefaultRunModel("../../models/dmonitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME);
} }
MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height) { DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
uint8_t *raw_buf = (uint8_t*) stream_buf; uint8_t *raw_buf = (uint8_t*) stream_buf;
uint8_t *raw_y_buf = raw_buf; uint8_t *raw_y_buf = raw_buf;
@ -111,7 +111,7 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int
s->m->execute(net_input_buf); s->m->execute(net_input_buf);
delete[] net_input_buf; delete[] net_input_buf;
MonitoringResult ret = {0}; DMonitoringResult ret = {0};
memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
memcpy(&ret.face_orientation_meta, &s->output[6], sizeof ret.face_orientation_meta); memcpy(&ret.face_orientation_meta, &s->output[6], sizeof ret.face_orientation_meta);
memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position); memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position);
@ -129,13 +129,13 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int
return ret; return ret;
} }
void monitoring_publish(PubSocket* sock, uint32_t frame_id, const MonitoringResult res) { void dmonitoring_publish(PubSocket* sock, uint32_t frame_id, const DMonitoringResult res) {
// make msg // make msg
capnp::MallocMessageBuilder msg; capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot()); event.setLogMonoTime(nanos_since_boot());
auto framed = event.initDriverMonitoring(); auto framed = event.initDriverState();
framed.setFrameId(frame_id); framed.setFrameId(frame_id);
kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
@ -158,6 +158,6 @@ void monitoring_publish(PubSocket* sock, uint32_t frame_id, const MonitoringResu
sock->send((char*)bytes.begin(), bytes.size()); sock->send((char*)bytes.begin(), bytes.size());
} }
void monitoring_free(MonitoringState* s) { void dmonitoring_free(DMonitoringModelState* s) {
delete s->m; delete s->m;
} }

@ -1,5 +1,5 @@
#ifndef MONITORING_H #ifndef DMONITORING_H
#define MONITORING_H #define DMONITORING_H
#include "common/util.h" #include "common/util.h"
#include "commonmodel.h" #include "commonmodel.h"
@ -15,7 +15,7 @@ extern "C" {
#define OUTPUT_SIZE 33 #define OUTPUT_SIZE 33
typedef struct MonitoringResult { typedef struct DMonitoringResult {
float face_orientation[3]; float face_orientation[3];
float face_orientation_meta[3]; float face_orientation_meta[3];
float face_position[2]; float face_position[2];
@ -25,17 +25,17 @@ typedef struct MonitoringResult {
float right_eye_prob; float right_eye_prob;
float left_blink_prob; float left_blink_prob;
float right_blink_prob; float right_blink_prob;
} MonitoringResult; } DMonitoringResult;
typedef struct MonitoringState { typedef struct DMonitoringModelState {
RunModel *m; RunModel *m;
float output[OUTPUT_SIZE]; float output[OUTPUT_SIZE];
} MonitoringState; } DMonitoringModelState;
void monitoring_init(MonitoringState* s); void dmonitoring_init(DMonitoringModelState* s);
MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height); DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height);
void monitoring_publish(PubSocket *sock, uint32_t frame_id, const MonitoringResult res); void dmonitoring_publish(PubSocket *sock, uint32_t frame_id, const DMonitoringResult res);
void monitoring_free(MonitoringState* s); void dmonitoring_free(DMonitoringModelState* s);
#ifdef __cplusplus #ifdef __cplusplus
} }

@ -115,7 +115,7 @@ class Plant():
Plant.live_params = messaging.pub_sock('liveParameters') Plant.live_params = messaging.pub_sock('liveParameters')
Plant.health = messaging.pub_sock('health') Plant.health = messaging.pub_sock('health')
Plant.thermal = messaging.pub_sock('thermal') Plant.thermal = messaging.pub_sock('thermal')
Plant.driverMonitoring = messaging.pub_sock('driverMonitoring') Plant.driverState = messaging.pub_sock('driverState')
Plant.cal = messaging.pub_sock('liveCalibration') Plant.cal = messaging.pub_sock('liveCalibration')
Plant.controls_state = messaging.sub_sock('controlsState') Plant.controls_state = messaging.sub_sock('controlsState')
Plant.plan = messaging.sub_sock('plan') Plant.plan = messaging.sub_sock('plan')
@ -366,11 +366,11 @@ class Plant():
live_parameters.liveParameters.stiffnessFactor = 1.0 live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes()) Plant.live_params.send(live_parameters.to_bytes())
driver_monitoring = messaging.new_message() driver_state = messaging.new_message()
driver_monitoring.init('driverMonitoring') driver_state.init('driverState')
driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3 driver_state.driverState.faceOrientation = [0.] * 3
driver_monitoring.driverMonitoring.facePosition = [0.] * 2 driver_state.driverState.facePosition = [0.] * 2
Plant.driverMonitoring.send(driver_monitoring.to_bytes()) Plant.driverState.send(driver_state.to_bytes())
health = messaging.new_message() health = messaging.new_message()
health.init('health') health.init('health')

@ -338,6 +338,7 @@ class LongitudinalControl(unittest.TestCase):
manager.prepare_managed_process('radard') manager.prepare_managed_process('radard')
manager.prepare_managed_process('controlsd') manager.prepare_managed_process('controlsd')
manager.prepare_managed_process('plannerd') manager.prepare_managed_process('plannerd')
manager.prepare_managed_process('dmonitoringd')
@classmethod @classmethod
def tearDownClass(cls): def tearDownClass(cls):
@ -354,13 +355,13 @@ def run_maneuver_worker(k):
def run(self): def run(self):
print(man.title) print(man.title)
valid = False valid = False
for retries in range(3): for retries in range(3):
manager.start_managed_process('radard') manager.start_managed_process('radard')
manager.start_managed_process('controlsd') manager.start_managed_process('controlsd')
manager.start_managed_process('plannerd') manager.start_managed_process('plannerd')
manager.start_managed_process('dmonitoringd')
plot, valid = man.evaluate() plot, valid = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(k + 1).zfill(2)) plot.write_plot(output_dir, "maneuver" + str(k + 1).zfill(2))
@ -368,6 +369,7 @@ def run_maneuver_worker(k):
manager.kill_managed_process('radard') manager.kill_managed_process('radard')
manager.kill_managed_process('controlsd') manager.kill_managed_process('controlsd')
manager.kill_managed_process('plannerd') manager.kill_managed_process('plannerd')
manager.kill_managed_process('dmonitoringd')
if valid: if valid:
break break

@ -198,7 +198,7 @@ CONFIGS = [
proc_name="controlsd", proc_name="controlsd",
pub_sub={ pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [], "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [],
"model": [], "model": [],
}, },
ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)], ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)],
@ -234,6 +234,16 @@ CONFIGS = [
init_callback=get_car_params, init_callback=get_car_params,
should_recv_callback=calibration_rcv_callback, should_recv_callback=calibration_rcv_callback,
), ),
ProcessConfig(
proc_name="dmonitoringd",
pub_sub={
"driverState": ["dMonitoringState"],
"liveCalibration": [], "carState": [], "model": [], "gpsLocation": [],
},
ignore=[("logMonoTime", 0), ("valid", True)],
init_callback=get_car_params,
should_recv_callback=None,
),
] ]
def replay_process(cfg, lr): def replay_process(cfg, lr):

@ -1 +1 @@
fe9ccb27b12c9d3788bed8a402293985f5eb0448 bc89e6f25e88a904ad905296d516aaebb77e2207

@ -80,7 +80,7 @@ def test_logging():
time.sleep(1.0) time.sleep(1.0)
@phone_only @phone_only
@with_processes(['camerad', 'modeld', 'monitoringd']) @with_processes(['camerad', 'modeld', 'dmonitoringmodeld'])
def test_visiond(): def test_visiond():
print("VISIOND IS SET UP") print("VISIOND IS SET UP")
time.sleep(5.0) time.sleep(5.0)

Loading…
Cancel
Save