Driver view refactor (#1869)
* quick driver view cleanup * send at correct freq * let manager handle that * don't block starting on driver view * fix UI hacks * ui cleanup * move driver view functionality into dmonitoringd * dmonitoring thresholds shouldnt be in UI * remove driver view file from release files * timeout on frontview * bump cerealpull/1961/head
Adeeb Shihadeh
GitHub
parent
f80acad519
commit
a7ee2a53a8
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit 430cc73a3b7c31de902bd0a62fbac7cd3a05cf3e
|
||||
Subproject commit 10a25c8b98ad295308e6825cef7cf42dff6f4396
|
||||
|
|
@ -385,7 +385,6 @@ selfdrive/modeld/runners/run.h
|
|||
|
||||
selfdrive/monitoring/dmonitoringd.py
|
||||
selfdrive/monitoring/driver_monitor.py
|
||||
selfdrive/monitoring/driverview.py
|
||||
|
||||
selfdrive/assets
|
||||
selfdrive/assets/fonts/*.ttf
|
||||
|
|
|
|||
|
|
@ -189,7 +189,6 @@ managed_processes = {
|
|||
"updated": "selfdrive.updated",
|
||||
"dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
|
||||
"modeld": ("selfdrive/modeld", ["./modeld"]),
|
||||
"driverview": "selfdrive.monitoring.driverview",
|
||||
}
|
||||
|
||||
daemon_processes = {
|
||||
|
|
@ -242,6 +241,12 @@ car_started_processes = [
|
|||
'locationd',
|
||||
]
|
||||
|
||||
driver_view_processes = [
|
||||
'camerad',
|
||||
'dmonitoringd',
|
||||
'dmonitoringmodeld'
|
||||
]
|
||||
|
||||
if WEBCAM:
|
||||
car_started_processes += [
|
||||
'dmonitoringmodeld',
|
||||
|
|
@ -470,7 +475,7 @@ def manager_thread():
|
|||
if msg.thermal.freeSpace < 0.05:
|
||||
logger_dead = True
|
||||
|
||||
if msg.thermal.started and "driverview" not in running:
|
||||
if msg.thermal.started:
|
||||
for p in car_started_processes:
|
||||
if p == "loggerd" and logger_dead:
|
||||
kill_managed_process(p)
|
||||
|
|
@ -478,13 +483,18 @@ def manager_thread():
|
|||
start_managed_process(p)
|
||||
else:
|
||||
logger_dead = False
|
||||
driver_view = params.get("IsDriverViewEnabled") == b"1"
|
||||
|
||||
# TODO: refactor how manager manages processes
|
||||
for p in reversed(car_started_processes):
|
||||
kill_managed_process(p)
|
||||
# this is ugly
|
||||
if "driverview" not in running and params.get("IsDriverViewEnabled") == b"1":
|
||||
start_managed_process("driverview")
|
||||
elif "driverview" in running and params.get("IsDriverViewEnabled") == b"0":
|
||||
kill_managed_process("driverview")
|
||||
if p not in driver_view_processes or not driver_view:
|
||||
kill_managed_process(p)
|
||||
|
||||
for p in driver_view_processes:
|
||||
if driver_view:
|
||||
start_managed_process(p)
|
||||
else:
|
||||
kill_managed_process(p)
|
||||
|
||||
# check the status of all processes, did any of them die?
|
||||
running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running]
|
||||
|
|
|
|||
|
|
@ -1,21 +1,18 @@
|
|||
#!/usr/bin/env python3
|
||||
import gc
|
||||
from cereal import car
|
||||
from common.realtime import set_realtime_priority
|
||||
from common.realtime import set_realtime_priority, DT_DMON
|
||||
from common.params import Params
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.controls.lib.events import Events
|
||||
from selfdrive.monitoring.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
|
||||
from selfdrive.locationd.calibration_helpers import Calibration
|
||||
|
||||
|
||||
def dmonitoringd_thread(sm=None, pm=None):
|
||||
gc.disable()
|
||||
|
||||
# start the loop
|
||||
set_realtime_priority(53)
|
||||
|
||||
params = Params()
|
||||
|
||||
# Pub/Sub Sockets
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['dMonitoringState'])
|
||||
|
|
@ -23,13 +20,15 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
if sm is None:
|
||||
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'])
|
||||
|
||||
params = Params()
|
||||
|
||||
driver_status = DriverStatus()
|
||||
is_rhd = params.get("IsRHD")
|
||||
if is_rhd is not None:
|
||||
driver_status.is_rhd_region = bool(int(is_rhd))
|
||||
driver_status.is_rhd_region_checked = True
|
||||
driver_status.is_rhd_region = is_rhd == b"1"
|
||||
driver_status.is_rhd_region_checked = is_rhd is not None
|
||||
|
||||
sm['liveCalibration'].calStatus = Calibration.INVALID
|
||||
sm['liveCalibration'].rpyCalib = [0, 0, 0]
|
||||
sm['carState'].vEgo = 0.
|
||||
sm['carState'].cruiseState.enabled = False
|
||||
sm['carState'].cruiseState.speed = 0.
|
||||
|
|
@ -38,20 +37,14 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
sm['carState'].gasPressed = False
|
||||
sm['carState'].standstill = True
|
||||
|
||||
cal_rpy = [0, 0, 0]
|
||||
v_cruise_last = 0
|
||||
driver_engaged = False
|
||||
offroad = params.get("IsOffroad") == b"1"
|
||||
|
||||
# 10Hz <- dmonitoringmodeld
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
# 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
|
||||
|
|
@ -67,17 +60,23 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
if sm.updated['model']:
|
||||
driver_status.set_policy(sm['model'])
|
||||
|
||||
# Check once a second if we're offroad
|
||||
if sm.frame % 1/DT_DMON == 0:
|
||||
offroad = params.get("IsOffroad") == b"1"
|
||||
|
||||
# Get data from dmonitoringmodeld
|
||||
if sm.updated['driverState']:
|
||||
events = Events()
|
||||
driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
|
||||
# Block any engage after certain distrations
|
||||
driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
|
||||
|
||||
# Block engaging after max number of distrations
|
||||
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
|
||||
events.add(car.CarEvent.EventName.tooDistracted)
|
||||
|
||||
# Update events from driver state
|
||||
driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
|
||||
|
||||
# dMonitoringState packet
|
||||
# build dMonitoringState packet
|
||||
dat = messaging.new_message('dMonitoringState')
|
||||
dat.dMonitoringState = {
|
||||
"events": events.to_msg(),
|
||||
|
|
@ -95,7 +94,7 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
"awarenessPassive": driver_status.awareness_passive,
|
||||
"isLowStd": driver_status.pose.low_std,
|
||||
"hiStdCount": driver_status.hi_stds,
|
||||
"isPreview": False,
|
||||
"isPreview": offroad,
|
||||
}
|
||||
pm.send('dMonitoringState', dat)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,81 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import subprocess
|
||||
import multiprocessing
|
||||
import signal
|
||||
import time
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from common.params import Params
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
|
||||
KILL_TIMEOUT = 15
|
||||
|
||||
|
||||
def send_controls_packet(pm):
|
||||
while True:
|
||||
dat = messaging.new_message('controlsState')
|
||||
dat.controlsState = {
|
||||
"rearViewCam": True,
|
||||
}
|
||||
pm.send('controlsState', dat)
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
def send_dmon_packet(pm, d):
|
||||
dat = messaging.new_message('dMonitoringState')
|
||||
dat.dMonitoringState = {
|
||||
"isRHD": d[0],
|
||||
"rhdChecked": d[1],
|
||||
"isPreview": d[2],
|
||||
}
|
||||
pm.send('dMonitoringState', dat)
|
||||
|
||||
|
||||
def main():
|
||||
pm = messaging.PubMaster(['controlsState', 'dMonitoringState'])
|
||||
controls_sender = multiprocessing.Process(target=send_controls_packet, args=[pm])
|
||||
controls_sender.start()
|
||||
|
||||
# TODO: refactor with manager start/kill
|
||||
proc_cam = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad"))
|
||||
proc_mon = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/modeld/dmonitoringmodeld"), cwd=os.path.join(BASEDIR, "selfdrive/modeld"))
|
||||
|
||||
params = Params()
|
||||
is_rhd = False
|
||||
is_rhd_checked = False
|
||||
should_exit = False
|
||||
|
||||
def terminate(signalNumber, frame):
|
||||
print('got SIGTERM, exiting..')
|
||||
should_exit = True
|
||||
send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit])
|
||||
proc_cam.send_signal(signal.SIGINT)
|
||||
proc_mon.send_signal(signal.SIGINT)
|
||||
kill_start = time.time()
|
||||
while proc_cam.poll() is None:
|
||||
if time.time() - kill_start > KILL_TIMEOUT:
|
||||
from selfdrive.swaglog import cloudlog
|
||||
cloudlog.critical("FORCE REBOOTING PHONE!")
|
||||
os.system("date >> /sdcard/unkillable_reboot")
|
||||
os.system("reboot")
|
||||
raise RuntimeError
|
||||
continue
|
||||
controls_sender.terminate()
|
||||
exit()
|
||||
|
||||
signal.signal(signal.SIGTERM, terminate)
|
||||
|
||||
while True:
|
||||
send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit])
|
||||
|
||||
if not is_rhd_checked:
|
||||
is_rhd = params.get("IsRHD") == b"1"
|
||||
is_rhd_checked = True
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -560,7 +560,7 @@ static void ui_draw_vision_face(UIState *s) {
|
|||
const int face_size = 96;
|
||||
const int face_x = (s->scene.ui_viz_rx + face_size + (bdr_s * 2));
|
||||
const int face_y = (footer_y + ((footer_h - face_size) / 2));
|
||||
ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.controls_state.getDriverMonitoringOn());
|
||||
ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.dmonitoring_state.getFaceDetected());
|
||||
}
|
||||
|
||||
static void ui_draw_driver_view(UIState *s) {
|
||||
|
|
@ -572,19 +572,11 @@ static void ui_draw_driver_view(UIState *s) {
|
|||
const int valid_frame_x = frame_x + (frame_w - valid_frame_w) / 2 + ff_xoffset;
|
||||
|
||||
// blackout
|
||||
if (!scene->is_rhd) {
|
||||
NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x + valid_frame_w,
|
||||
box_y,
|
||||
valid_frame_x + box_h / 2, box_y,
|
||||
nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0));
|
||||
ui_draw_rect(s->vg, valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, gradient);
|
||||
} else {
|
||||
NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x,
|
||||
box_y,
|
||||
valid_frame_w - box_h / 2, box_y,
|
||||
nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0));
|
||||
ui_draw_rect(s->vg, valid_frame_x, box_y, valid_frame_w - box_h / 2, box_h, gradient);
|
||||
}
|
||||
NVGpaint gradient = nvgLinearGradient(s->vg, scene->is_rhd ? valid_frame_x : (valid_frame_x + valid_frame_w),
|
||||
box_y,
|
||||
scene->is_rhd ? (valid_frame_w - box_h / 2) : (valid_frame_x + box_h / 2), box_y,
|
||||
COLOR_BLACK, COLOR_BLACK_ALPHA(0));
|
||||
ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : (valid_frame_x + box_h / 2), box_y, valid_frame_w - box_h / 2, box_h, gradient);
|
||||
ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, COLOR_BLACK_ALPHA(144));
|
||||
|
||||
// borders
|
||||
|
|
@ -592,7 +584,7 @@ static void ui_draw_driver_view(UIState *s) {
|
|||
ui_draw_rect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h, nvgRGBA(23, 51, 73, 255));
|
||||
|
||||
// draw face box
|
||||
if (scene->driver_state.getFaceProb() > 0.4) {
|
||||
if (scene->dmonitoring_state.getFaceDetected()) {
|
||||
auto fxy_list = scene->driver_state.getFacePosition();
|
||||
const float face_x = fxy_list[0];
|
||||
const float face_y = fxy_list[1];
|
||||
|
|
@ -603,6 +595,7 @@ static void ui_draw_driver_view(UIState *s) {
|
|||
} else {
|
||||
fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + (face_x + 0.5) * (box_h / 2) - 0.5 * 0.6 * box_h / 2;
|
||||
}
|
||||
|
||||
if (std::abs(face_x) <= 0.35 && std::abs(face_y) <= 0.4) {
|
||||
ui_draw_rect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2,
|
||||
nvgRGBAf(1.0, 1.0, 1.0, 0.8 - ((std::abs(face_x) > std::abs(face_y) ? std::abs(face_x) : std::abs(face_y))) * 0.6 / 0.375),
|
||||
|
|
@ -616,7 +609,7 @@ static void ui_draw_driver_view(UIState *s) {
|
|||
const int face_size = 85;
|
||||
const int x = (valid_frame_x + face_size + (bdr_s * 2)) + (scene->is_rhd ? valid_frame_w - box_h / 2:0);
|
||||
const int y = (box_y + box_h - face_size - bdr_s - (bdr_s * 1.5));
|
||||
ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->driver_state.getFaceProb() > 0.4);
|
||||
ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->dmonitoring_state.getFaceDetected());
|
||||
}
|
||||
|
||||
static void ui_draw_vision_header(UIState *s) {
|
||||
|
|
|
|||
|
|
@ -269,8 +269,7 @@ void handle_message(UIState *s, SubMaster &sm) {
|
|||
auto event = sm["controlsState"];
|
||||
scene.controls_state = event.getControlsState();
|
||||
s->controls_timeout = 1 * UI_FREQ;
|
||||
scene.frontview = scene.controls_state.getRearViewCam();
|
||||
if (!scene.frontview){ s->controls_seen = true; }
|
||||
s->controls_seen = true;
|
||||
|
||||
auto alert_sound = scene.controls_state.getAlertSound();
|
||||
if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) {
|
||||
|
|
@ -364,11 +363,17 @@ void handle_message(UIState *s, SubMaster &sm) {
|
|||
}
|
||||
if (sm.updated("dMonitoringState")) {
|
||||
auto data = sm["dMonitoringState"].getDMonitoringState();
|
||||
scene.dmonitoring_state = sm["dMonitoringState"].getDMonitoringState();
|
||||
scene.is_rhd = data.getIsRHD();
|
||||
s->preview_started = data.getIsPreview();
|
||||
scene.frontview = data.getIsPreview();
|
||||
}
|
||||
|
||||
s->started = scene.thermal.getStarted() || s->preview_started;
|
||||
// timeout on frontview
|
||||
if ((sm.frame - sm.rcv_frame("dMonitoringState")) > 1*UI_FREQ) {
|
||||
scene.frontview = false;
|
||||
}
|
||||
|
||||
s->started = scene.thermal.getStarted() || scene.frontview;
|
||||
// Handle onroad/offroad transition
|
||||
if (!s->started) {
|
||||
if (s->status != STATUS_STOPPED) {
|
||||
|
|
@ -815,7 +820,7 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
if (s->controls_timeout > 0) {
|
||||
s->controls_timeout--;
|
||||
} else if (s->started) {
|
||||
} else if (s->started && !s->scene.frontview) {
|
||||
if (!s->controls_seen) {
|
||||
// car is started, but controlsState hasn't been seen at all
|
||||
s->scene.alert_text1 = "openpilot Unavailable";
|
||||
|
|
|
|||
|
|
@ -125,6 +125,7 @@ typedef struct UIScene {
|
|||
cereal::RadarState::LeadData::Reader lead_data[2];
|
||||
cereal::ControlsState::Reader controls_state;
|
||||
cereal::DriverState::Reader driver_state;
|
||||
cereal::DMonitoringState::Reader dmonitoring_state;
|
||||
} UIScene;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -224,7 +225,6 @@ typedef struct UIState {
|
|||
float alert_blinking_alpha;
|
||||
bool alert_blinked;
|
||||
bool started;
|
||||
bool preview_started;
|
||||
bool vision_seen;
|
||||
|
||||
std::atomic<float> light_sensor;
|
||||
|
|
|
|||
Loading…
Reference in New Issue