ZwX1616
GitHub
parent
1dee8638d6
commit
2ad685b194
|
|
@ -44,7 +44,7 @@ selfdrive/sensord/_gpsd
|
|||
selfdrive/sensord/_sensord
|
||||
selfdrive/camerad/camerad
|
||||
selfdrive/modeld/_modeld
|
||||
selfdrive/modeld/_monitoringd
|
||||
selfdrive/modeld/_dmonitoringmodeld
|
||||
/src/
|
||||
|
||||
one
|
||||
|
|
|
|||
2
cereal
2
cereal
|
|
@ -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
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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<cereal::Event>();
|
||||
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
"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",
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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',
|
||||
]
|
||||
|
||||
|
|
|
|||
|
|
@ -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', [
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -8,7 +8,7 @@
|
|||
#include "common/visionipc.h"
|
||||
#include "common/swaglog.h"
|
||||
|
||||
#include "models/monitoring.h"
|
||||
#include "models/dmonitoring.h"
|
||||
|
||||
#ifndef PATH_MAX
|
||||
#include <linux/limits.h>
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
#include <string.h>
|
||||
#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<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
|
||||
auto framed = event.initDriverMonitoring();
|
||||
auto framed = event.initDriverState();
|
||||
framed.setFrameId(frame_id);
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
void monitoring_free(MonitoringState* s) {
|
||||
void dmonitoring_free(DMonitoringModelState* s) {
|
||||
delete s->m;
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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')
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
fe9ccb27b12c9d3788bed8a402293985f5eb0448
|
||||
bc89e6f25e88a904ad905296d516aaebb77e2207
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue