diff --git a/.gitignore b/.gitignore index 6ed87a3f85..789f2728f3 100644 --- a/.gitignore +++ b/.gitignore @@ -44,7 +44,7 @@ selfdrive/sensord/_gpsd selfdrive/sensord/_sensord selfdrive/camerad/camerad selfdrive/modeld/_modeld -selfdrive/modeld/_monitoringd +selfdrive/modeld/_dmonitoringmodeld /src/ one diff --git a/cereal b/cereal index 8f9aa8fcc3..3f01dcf01c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 8f9aa8fcc3e9c7d566d1d388aea4cbf642abaa8b +Subproject commit 3f01dcf01cea9833a73e9779cc9237e9bda80e1e diff --git a/models/dmonitoring_model.current b/models/dmonitoring_model.current new file mode 100644 index 0000000000..e726531611 --- /dev/null +++ b/models/dmonitoring_model.current @@ -0,0 +1 @@ +579c4158-b98d-41ba-9932-53ecd62e8b9f \ No newline at end of file diff --git a/models/dmonitoring_model.keras b/models/dmonitoring_model.keras new file mode 100644 index 0000000000..7a227d0844 --- /dev/null +++ b/models/dmonitoring_model.keras @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6a09d27ea8e7e286044f7cbfb7796b9acde2ac6a41f767358d1a2dd2df2fa4c6 +size 814176 diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc new file mode 100644 index 0000000000..b957972d45 --- /dev/null +++ b/models/dmonitoring_model_q.dlc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:731e44cdc2d1ba04a4c18f20a08605a9f3c73a7ca4152d811e401d8f4a70fab5 +size 186615 diff --git a/models/monitoring_model.current b/models/monitoring_model.current deleted file mode 100644 index 130385b5cc..0000000000 --- a/models/monitoring_model.current +++ /dev/null @@ -1 +0,0 @@ -5f173fc6-cc91-4d62-8cdb-b33cf8713617 \ No newline at end of file diff --git a/models/monitoring_model.keras b/models/monitoring_model.keras deleted file mode 100644 index 939e653ef0..0000000000 --- a/models/monitoring_model.keras +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0465111974591de2954c0ff29ae025634cb5eb79d6e259ebc945fb8a77a96977 -size 814176 diff --git a/models/monitoring_model_q.dlc b/models/monitoring_model_q.dlc deleted file mode 100644 index 0187e55402..0000000000 --- a/models/monitoring_model_q.dlc +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8ed5fa77d02ed5f4c4ee4c4793916d24f024d07640ba277dec13b99c0d5a1d3a -size 186615 diff --git a/release/files_common b/release/files_common index b456af36b5..efead36b9c 100644 --- a/release/files_common +++ b/release/files_common @@ -55,7 +55,7 @@ common/transformations/orientation.py common/api/__init__.py models/supercombo.dlc -models/monitoring_model_q.dlc +models/dmonitoring_model_q.dlc selfdrive/version.py @@ -200,6 +200,7 @@ selfdrive/controls/tests/* selfdrive/controls/controlsd.py selfdrive/controls/plannerd.py selfdrive/controls/radard.py +selfdrive/controls/dmonitoringd.py selfdrive/controls/lib/__init__.py selfdrive/controls/lib/alertmanager.py selfdrive/controls/lib/alerts.py @@ -334,17 +335,17 @@ selfdrive/camerad/transforms/rgb_to_yuv_test.cc selfdrive/modeld/SConscript selfdrive/modeld/modeld.cc -selfdrive/modeld/monitoringd.cc +selfdrive/modeld/dmonitoringmodeld.cc selfdrive/modeld/constants.py selfdrive/modeld/modeld -selfdrive/modeld/monitoringd +selfdrive/modeld/dmonitoringmodeld selfdrive/modeld/models/commonmodel.c selfdrive/modeld/models/commonmodel.h selfdrive/modeld/models/driving.cc selfdrive/modeld/models/driving.h -selfdrive/modeld/models/monitoring.cc -selfdrive/modeld/models/monitoring.h +selfdrive/modeld/models/dmonitoring.cc +selfdrive/modeld/models/dmonitoring.h selfdrive/modeld/transforms/loadyuv.[c,h] selfdrive/modeld/transforms/loadyuv.cl diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 896a4cac67..7c65f75550 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -147,7 +147,7 @@ void* frontview_thread(void *arg) { // we subscribe to this for placement of the AE metering box // TODO: the loop is bad, ideally models shouldn't affect sensors 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); 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); cereal::Event::Reader event = cmsg.getRoot(); - float face_prob = event.getDriverMonitoring().getFaceProb(); + float face_prob = event.getDriverState().getFaceProb(); float face_position[2]; - face_position[0] = event.getDriverMonitoring().getFacePosition()[0]; - face_position[1] = event.getDriverMonitoring().getFacePosition()[1]; + face_position[0] = event.getDriverState().getFacePosition()[0]; + face_position[1] = event.getDriverState().getFacePosition()[1]; // set front camera metering target if (face_prob > 0.4) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index cea9038967..680e511e01 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -6,7 +6,7 @@ from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL from common.profiler import Profiler -from common.params import Params, put_nonblocking +from common.params import Params import cereal.messaging as messaging from selfdrive.config import Conversions as CV 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.alertmanager import AlertManager 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.gps_helpers import is_rhd_region from selfdrive.locationd.calibration_helpers import Calibration, Filter LANE_DEPARTURE_THRESHOLD = 0.1 @@ -67,7 +65,7 @@ def events_to_bytes(events): 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""" # 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) events = list(CS.events) + events += list(sm['dMonitoringState'].events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) @@ -103,27 +102,15 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca if CS.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 cal_status = sm['liveCalibration'].calStatus cal_perc = sm['liveCalibration'].calPerc - cal_rpy = [0,0,0] if cal_status != Calibration.CALIBRATED: if cal_status == Calibration.UNCALIBRATED: events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) else: 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 # 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: 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 @@ -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, - 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""" 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) 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: last_blinker_frame = frame - # add eventual driver distracted events - events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) - if plan.fcw: # send FCW alert if triggered by planner 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" 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, - 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): """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) 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 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, "alertType": AM.alert_type, "alertSound": AM.audible_alert, - "awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, - "driverMonitoringOn": bool(driver_status.face_detected), + "driverMonitoringOn": sm['dMonitoringState'].faceDetected, "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], "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']) if sm is None: - sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ - 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) + sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \ + 'model']) if can_sock is None: @@ -529,11 +497,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): elif CP.lateralTuning.which() == 'lqr': 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 soft_disable_timer = 0 v_cruise_kph = 255 @@ -547,6 +510,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True sm['thermal'].freeSpace = 1. + sm['dMonitoringState'].events = [] + sm['dMonitoringState'].awarenessStatus = 1. + sm['dMonitoringState'].faceDetected = False # 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') @@ -563,7 +529,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("Ratekeeper", ignore=True) # 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") # Create alerts @@ -605,14 +571,14 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # 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, - 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") # 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, is_ldw_enabled, can_error_counter) prof.checkpoint("Sent") diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py new file mode 100755 index 0000000000..09108a62f9 --- /dev/null +++ b/selfdrive/controls/dmonitoringd.py @@ -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() \ No newline at end of file diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index e54d105274..e6a64f1ea7 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -110,7 +110,7 @@ ALERTS = [ Alert( "preDriverDistracted", - "KEEP EYES ON ROAD: User Appears Distracted", + "KEEP EYES ON ROAD: Driver Appears Distracted", "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), @@ -118,14 +118,14 @@ ALERTS = [ Alert( "promptDriverDistracted", "KEEP EYES ON ROAD", - "User Appears Distracted", + "Driver Appears Distracted", AlertStatus.userPrompt, AlertSize.mid, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), Alert( "driverDistracted", "DISENGAGE IMMEDIATELY", - "User Was Distracted", + "Driver Was Distracted", AlertStatus.critical, AlertSize.full, Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), @@ -139,14 +139,14 @@ ALERTS = [ Alert( "promptDriverUnresponsive", "TOUCH STEERING WHEEL", - "User Is Unresponsive", + "Driver Is Unresponsive", AlertStatus.userPrompt, AlertSize.mid, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1), Alert( "driverUnresponsive", "DISENGAGE IMMEDIATELY", - "User Was Unresponsive", + "Driver Was Unresponsive", AlertStatus.critical, AlertSize.full, Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1), @@ -155,7 +155,7 @@ ALERTS = [ "CHECK DRIVER FACE VISIBILITY", "Driver Monitor Model Output Uncertain", AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, .4, 0., 1.), + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.), Alert( "geofence", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 88b0573655..8f0dbda706 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -1,13 +1,13 @@ from common.numpy_fast import interp 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 common.filter_simple import FirstOrderFilter 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_PRE_TIME_TILL_TERMINAL = 25. # 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_TIME = 70. # one minute limit without user touching steering wheels make the car enter a terminal status +_AWARENESS_PRE_TIME_TILL_TERMINAL = 15. # a first alert is issued 25s before expiration +_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. # a second alert is issued 15s before start decelerating the car _DISTRACTED_TIME = 11. _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _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 _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 _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_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory" +_POSE_OFFSET_MIN_COUNT = 60 # valid data counts before calibration completes, 1 seg is 600 counts +_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_MIN = 1.25 # relative to minus step change 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 RESIZED_FOCAL = 320.0 @@ -115,7 +116,7 @@ class DriverStatus(): def _set_timers(self, active_monitoring): if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: if active_monitoring: - self.step_change = DT_CTRL / _DISTRACTED_TIME + self.step_change = DT_DMON / _DISTRACTED_TIME else: self.step_change = 0. return # no exploit after orange alert @@ -130,7 +131,7 @@ class DriverStatus(): self.threshold_pre = _DISTRACTED_PRE_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 else: if self.active_monitoring_mode: @@ -139,7 +140,7 @@ class DriverStatus(): self.threshold_pre = _AWARENESS_PRE_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 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.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 - 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 - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy) - self.pose.pitch_std = driver_monitoring.faceOrientationStd[0] - self.pose.yaw_std = driver_monitoring.faceOrientationStd[1] - # self.pose.roll_std = driver_monitoring.faceOrientationStd[2] - max_std = max(self.pose.pitch_std, self.pose.yaw_std) - self.pose.low_std = max_std < _POSESTD_THRESHOLD - self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD) - self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD) - self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and \ - abs(driver_monitoring.facePosition[0]) <= 0.4 and abs(driver_monitoring.facePosition[1]) <= 0.45 and \ + 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_state.faceOrientationStd[0] + self.pose.yaw_std = driver_state.faceOrientationStd[1] + # self.pose.roll_std = driver_state.faceOrientationStd[2] + model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) + self.pose.low_std = model_std_max < _POSESTD_THRESHOLD + self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb>_EYE_THRESHOLD) + self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb>_EYE_THRESHOLD) + self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \ + abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 and \ not self.is_rhd_region 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.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: - 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 elif self.face_detected and self.pose.low_std: self.hi_stds = 0 @@ -219,7 +222,7 @@ class DriverStatus(): if self.face_detected and self.hi_stds * DT_DMON > _HI_STD_TIMEOUT: 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 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.: @@ -229,7 +232,7 @@ class DriverStatus(): return events # 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): self.awareness = max(self.awareness - self.step_change, -0.1) diff --git a/selfdrive/controls/tests/test_monitoring.py b/selfdrive/controls/tests/test_monitoring.py index 5949a55a1b..af44dc3329 100644 --- a/selfdrive/controls/tests/test_monitoring.py +++ b/selfdrive/controls/tests/test_monitoring.py @@ -1,6 +1,6 @@ import unittest 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, \ _AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \ _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \ @@ -63,9 +63,7 @@ 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]) # 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 assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong' @@ -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_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*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__": print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index 160f4f9d55..d59a148558 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -30,7 +30,7 @@ SLEEP_INTERVAL = 0.2 monitored_proc_names = [ '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'] timer = getattr(time, 'monotonic', time.time) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 358eb38937..9f7a35c6e2 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -136,6 +136,7 @@ managed_processes = { "controlsd": "selfdrive.controls.controlsd", "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", + "dmonitoringd": "selfdrive.controls.dmonitoringd", "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", @@ -152,7 +153,7 @@ managed_processes = { "clocksd": ("selfdrive/clocksd", ["./clocksd"]), "gpsd": ("selfdrive/sensord", ["./gpsd"]), "updated": "selfdrive.updated", - "monitoringd": ("selfdrive/modeld", ["./monitoringd"]), + "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), } @@ -194,6 +195,7 @@ car_started_processes = [ 'plannerd', 'loggerd', 'radard', + 'dmonitoringd', 'calibrationd', 'paramsd', 'camerad', @@ -206,7 +208,7 @@ if ANDROID: 'sensord', 'clocksd', 'gpsd', - 'monitoringd', + 'dmonitoringmodeld', 'deleter', ] diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index aa995fb3f4..63c4979b49 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -24,9 +24,9 @@ else: common = lenv.Object(common_src) -lenv.Program('_monitoringd', [ - "monitoringd.cc", - "models/monitoring.cc", +lenv.Program('_dmonitoringmodeld', [ + "dmonitoringmodeld.cc", + "models/dmonitoring.cc", ]+common, LIBS=libs) lenv.Program('_modeld', [ diff --git a/selfdrive/modeld/monitoringd b/selfdrive/modeld/dmonitoringmodeld similarity index 90% rename from selfdrive/modeld/monitoringd rename to selfdrive/modeld/dmonitoringmodeld index 24de81f1e9..710e0e595b 100755 --- a/selfdrive/modeld/monitoringd +++ b/selfdrive/modeld/dmonitoringmodeld @@ -1,5 +1,5 @@ #!/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 ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/" -exec ./_monitoringd +exec ./_dmonitoringmodeld diff --git a/selfdrive/modeld/monitoringd.cc b/selfdrive/modeld/dmonitoringmodeld.cc similarity index 71% rename from selfdrive/modeld/monitoringd.cc rename to selfdrive/modeld/dmonitoringmodeld.cc index 98863ec9ff..9fb67b6360 100644 --- a/selfdrive/modeld/monitoringd.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -8,7 +8,7 @@ #include "common/visionipc.h" #include "common/swaglog.h" -#include "models/monitoring.h" +#include "models/dmonitoring.h" #ifndef PATH_MAX #include @@ -27,11 +27,11 @@ int main(int argc, char **argv) { // messaging Context *msg_context = Context::create(); - PubSocket *monitoring_sock = PubSocket::create(msg_context, "driverMonitoring"); + PubSocket *dmonitoring_sock = PubSocket::create(msg_context, "driverState"); // init the models - MonitoringState monitoring; - monitoring_init(&monitoring); + DMonitoringModelState dmonitoringmodel; + dmonitoring_init(&dmonitoringmodel); // loop VisionStream stream; @@ -58,14 +58,14 @@ int main(int argc, char **argv) { 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(); // 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; } @@ -73,8 +73,8 @@ int main(int argc, char **argv) { visionstream_destroy(&stream); - delete monitoring_sock; - monitoring_free(&monitoring); + delete dmonitoring_sock; + dmonitoring_free(&dmonitoringmodel); return 0; } diff --git a/selfdrive/modeld/models/monitoring.cc b/selfdrive/modeld/models/dmonitoring.cc similarity index 93% rename from selfdrive/modeld/models/monitoring.cc rename to selfdrive/modeld/models/dmonitoring.cc index 126f742476..8f325ed15f 100644 --- a/selfdrive/modeld/models/monitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -1,5 +1,5 @@ #include -#include "monitoring.h" +#include "dmonitoring.h" #include "common/mat.h" #include "common/timing.h" @@ -10,11 +10,11 @@ #define MODEL_HEIGHT 320 #define FULL_W 426 -void monitoring_init(MonitoringState* s) { - s->m = new DefaultRunModel("../../models/monitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); +void dmonitoring_init(DMonitoringModelState* s) { + 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_y_buf = raw_buf; @@ -111,7 +111,7 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int s->m->execute(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_meta, &s->output[6], sizeof ret.face_orientation_meta); 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; } -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 capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); event.setLogMonoTime(nanos_since_boot()); - auto framed = event.initDriverMonitoring(); + auto framed = event.initDriverState(); framed.setFrameId(frame_id); kj::ArrayPtr 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()); } -void monitoring_free(MonitoringState* s) { +void dmonitoring_free(DMonitoringModelState* s) { delete s->m; } diff --git a/selfdrive/modeld/models/monitoring.h b/selfdrive/modeld/models/dmonitoring.h similarity index 54% rename from selfdrive/modeld/models/monitoring.h rename to selfdrive/modeld/models/dmonitoring.h index 009ca64f0d..80aae5cd91 100644 --- a/selfdrive/modeld/models/monitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -1,5 +1,5 @@ -#ifndef MONITORING_H -#define MONITORING_H +#ifndef DMONITORING_H +#define DMONITORING_H #include "common/util.h" #include "commonmodel.h" @@ -15,7 +15,7 @@ extern "C" { #define OUTPUT_SIZE 33 -typedef struct MonitoringResult { +typedef struct DMonitoringResult { float face_orientation[3]; float face_orientation_meta[3]; float face_position[2]; @@ -25,17 +25,17 @@ typedef struct MonitoringResult { float right_eye_prob; float left_blink_prob; float right_blink_prob; -} MonitoringResult; +} DMonitoringResult; -typedef struct MonitoringState { +typedef struct DMonitoringModelState { RunModel *m; float output[OUTPUT_SIZE]; -} MonitoringState; +} DMonitoringModelState; -void monitoring_init(MonitoringState* s); -MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height); -void monitoring_publish(PubSocket *sock, uint32_t frame_id, const MonitoringResult res); -void monitoring_free(MonitoringState* s); +void dmonitoring_init(DMonitoringModelState* s); +DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height); +void dmonitoring_publish(PubSocket *sock, uint32_t frame_id, const DMonitoringResult res); +void dmonitoring_free(DMonitoringModelState* s); #ifdef __cplusplus } diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 06b2437695..2170cb71f7 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -115,7 +115,7 @@ class Plant(): Plant.live_params = messaging.pub_sock('liveParameters') Plant.health = messaging.pub_sock('health') 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.controls_state = messaging.sub_sock('controlsState') Plant.plan = messaging.sub_sock('plan') @@ -366,11 +366,11 @@ class Plant(): live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) - driver_monitoring = messaging.new_message() - driver_monitoring.init('driverMonitoring') - driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3 - driver_monitoring.driverMonitoring.facePosition = [0.] * 2 - Plant.driverMonitoring.send(driver_monitoring.to_bytes()) + driver_state = messaging.new_message() + driver_state.init('driverState') + driver_state.driverState.faceOrientation = [0.] * 3 + driver_state.driverState.facePosition = [0.] * 2 + Plant.driverState.send(driver_state.to_bytes()) health = messaging.new_message() health.init('health') diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 79951162ea..c800d447e5 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -338,6 +338,7 @@ class LongitudinalControl(unittest.TestCase): manager.prepare_managed_process('radard') manager.prepare_managed_process('controlsd') manager.prepare_managed_process('plannerd') + manager.prepare_managed_process('dmonitoringd') @classmethod def tearDownClass(cls): @@ -354,13 +355,13 @@ def run_maneuver_worker(k): def run(self): print(man.title) - valid = False for retries in range(3): manager.start_managed_process('radard') manager.start_managed_process('controlsd') manager.start_managed_process('plannerd') + manager.start_managed_process('dmonitoringd') plot, valid = man.evaluate() 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('controlsd') manager.kill_managed_process('plannerd') + manager.kill_managed_process('dmonitoringd') if valid: break diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index d71818e226..9148dc6914 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -198,7 +198,7 @@ CONFIGS = [ proc_name="controlsd", pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], - "thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [], + "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "model": [], }, ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)], @@ -234,6 +234,16 @@ CONFIGS = [ init_callback=get_car_params, 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): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c74a64b439..7418267c78 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fe9ccb27b12c9d3788bed8a402293985f5eb0448 \ No newline at end of file +bc89e6f25e88a904ad905296d516aaebb77e2207 \ No newline at end of file diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index ba97d70789..cbc43dd12e 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -80,7 +80,7 @@ def test_logging(): time.sleep(1.0) @phone_only -@with_processes(['camerad', 'modeld', 'monitoringd']) +@with_processes(['camerad', 'modeld', 'dmonitoringmodeld']) def test_visiond(): print("VISIOND IS SET UP") time.sleep(5.0)