diff --git a/cereal b/cereal index 430cc73a3b..10a25c8b98 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 430cc73a3b7c31de902bd0a62fbac7cd3a05cf3e +Subproject commit 10a25c8b98ad295308e6825cef7cf42dff6f4396 diff --git a/release/files_common b/release/files_common index 8f926ac3c3..a4417b3767 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index e415dff50f..62d4827ca7 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -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] diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index a7fb78a729..a99d53ca1e 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -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) diff --git a/selfdrive/monitoring/driverview.py b/selfdrive/monitoring/driverview.py deleted file mode 100755 index 4581a06549..0000000000 --- a/selfdrive/monitoring/driverview.py +++ /dev/null @@ -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() diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index d0e88f4dd7..8802526cb8 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -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) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 0a1566c2ab..0ddc306f87 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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"; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 1094f7cca3..bf64ef27aa 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -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 light_sensor;