retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

RHD support for driver monitoring (#1299)

albatross
ZwX1616 2020-04-15 16:48:44 -07:00 committed by GitHub
parent a18832748c
commit 133b1a20b4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
21 changed files with 324 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
apk/ai.comma.plus.offroad.apk (Stored with Git LFS)
View File

Binary file not shown.

2
apks

@ -1 +1 @@
Subproject commit e4bf49bc65e8e680e5afc629240b4ebbcedc4ebd
Subproject commit f5d2c1715c9482d898062110ce4c612093aa5d4f

2
cereal

@ -1 +1 @@
Subproject commit c8e5db830d30e5ea0d0abef0479d9c48cfd24504
Subproject commit 442e914d73b163823fceb91c2b57062965fad045

1
common/params.py
View File

@ -70,6 +70,7 @@ keys = {
"GithubSshKeys": [TxType.PERSISTENT],
"HasAcceptedTerms": [TxType.PERSISTENT],
"HasCompletedSetup": [TxType.PERSISTENT],
"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START],
"IsLdwEnabled": [TxType.PERSISTENT],
"IsGeofenceEnabled": [TxType.PERSISTENT],
"IsMetric": [TxType.PERSISTENT],

2
models/dmonitoring_model.current
View File

@ -1 +1 @@
2ad118ee-df4f-49ff-a374-6dd693cee37b
43221d85-46fd-40b9-bff0-2b1b18a86b07

BIN
models/dmonitoring_model.keras (Stored with Git LFS)
View File

Binary file not shown.

BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
View File

Binary file not shown.

1
release/files_common
View File

@ -217,6 +217,7 @@ selfdrive/controls/lib/fcw.py
selfdrive/controls/lib/long_mpc.py
selfdrive/controls/lib/long_mpc_model.py
selfdrive/controls/lib/gps_helpers.py
selfdrive/controls/lib/driverview.py
selfdrive/controls/lib/cluster/*

34
selfdrive/camerad/main.cc
View File

@ -114,6 +114,8 @@ struct VisionState {
int rgb_front_width, rgb_front_height, rgb_front_stride;
VisionBuf rgb_front_bufs[UI_BUF_COUNT];
cl_mem rgb_front_bufs_cl[UI_BUF_COUNT];
bool rhd_front;
bool rhd_front_checked;
int front_meteringbox_xmin, front_meteringbox_xmax;
int front_meteringbox_ymin, front_meteringbox_ymax;
@ -152,7 +154,9 @@ void* frontview_thread(void *arg) {
// TODO: the loop is bad, ideally models shouldn't affect sensors
Context *msg_context = Context::create();
SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverState", "127.0.0.1", true);
SubSocket *dmonstate_sock = SubSocket::create(msg_context, "dMonitoringState", "127.0.0.1", true);
assert(monitoring_sock != NULL);
assert(dmonstate_sock != NULL);
cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err);
assert(err == 0);
@ -195,7 +199,24 @@ void* frontview_thread(void *arg) {
clReleaseEvent(debayer_event);
tbuffer_release(&s->cameras.front.camera_tb, buf_idx);
visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE);
// set front camera metering target
// no more check after gps check
if (!s->rhd_front_checked) {
Message *msg_dmon = dmonstate_sock->receive(true);
if (msg_dmon != NULL) {
auto amsg = kj::heapArray<capnp::word>((msg_dmon->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg_dmon->getData(), msg_dmon->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
s->rhd_front = event.getDMonitoringState().getIsRHD();
s->rhd_front_checked = event.getDMonitoringState().getRhdChecked();
delete msg_dmon;
}
}
Message *msg = monitoring_sock->receive(true);
if (msg != NULL) {
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
@ -212,7 +233,7 @@ void* frontview_thread(void *arg) {
// set front camera metering target
if (face_prob > 0.4)
{
int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height;
int x_offset = s->rhd_front ? 0:s->rgb_front_width - 0.5 * s->rgb_front_height;
s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72;
s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72;
s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72;
@ -222,8 +243,8 @@ void* frontview_thread(void *arg) {
{
s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3;
s->front_meteringbox_ymax = s->rgb_front_height * 1;
s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5;
s->front_meteringbox_xmax = s->rgb_front_width;
s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
}
delete msg;
@ -252,8 +273,8 @@ void* frontview_thread(void *arg) {
{
y_start = s->rgb_front_height * 1 / 3;
y_end = s->rgb_front_height * 1;
x_start = s->rgb_front_width * 3 / 5;
x_end = s->rgb_front_width;
x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
}
uint32_t lum_binning[256] = {0,};
@ -336,6 +357,7 @@ void* frontview_thread(void *arg) {
}
delete monitoring_sock;
delete dmonstate_sock;
return NULL;
}

14
selfdrive/controls/dmonitoringd.py
View File

@ -1,12 +1,11 @@
#!/usr/bin/env python3
import gc
from common.realtime import set_realtime_priority
from common.params import Params, put_nonblocking
from common.params import Params
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()
@ -21,12 +20,13 @@ def dmonitoringd_thread(sm=None, pm=None):
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model', 'gpsLocation'], ignore_alive=['gpsLocation'])
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'])
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
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['carState'].vEgo = 0.
@ -44,13 +44,6 @@ def dmonitoringd_thread(sm=None, pm=None):
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:
@ -99,6 +92,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,
}
pm.send('dMonitoringState', dat)

13
selfdrive/controls/lib/driver_monitor.py
View File

@ -31,8 +31,8 @@ _HI_STD_FALLBACK_TIME = 10 # fall back to wheel touch if model is uncertain for
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
_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"
_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"
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
@ -49,7 +49,7 @@ class DistractedType():
BAD_POSE = 1
BAD_BLINK = 2
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
@ -67,7 +67,7 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
# no calib for roll
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> +=
return roll, pitch, yaw
class DriverPose():
@ -174,7 +174,7 @@ class DriverStatus():
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_state.faceOrientation, driver_state.facePosition, cal_rpy)
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region)
self.pose.pitch_std = driver_state.faceOrientationStd[0]
self.pose.yaw_std = driver_state.faceOrientationStd[1]
# self.pose.roll_std = driver_state.faceOrientationStd[2]
@ -183,8 +183,7 @@ class DriverStatus():
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
abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0
# first order filters

74
selfdrive/controls/lib/driverview.py 100755
View File

@ -0,0 +1,74 @@
#!/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)
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
if __name__ == '__main__':
main()

10
selfdrive/manager.py
View File

@ -187,6 +187,7 @@ managed_processes = {
"updated": "selfdrive.updated",
"dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
"modeld": ("selfdrive/modeld", ["./modeld"]),
"driverview": "selfdrive.controls.lib.driverview",
}
daemon_processes = {
@ -471,7 +472,7 @@ def manager_thread():
if msg.thermal.freeSpace < 0.05:
logger_dead = True
if msg.thermal.started:
if msg.thermal.started and "driverview" not in running:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
@ -481,6 +482,11 @@ def manager_thread():
logger_dead = False
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")
# 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]
@ -538,6 +544,7 @@ def main():
default_params = [
("CommunityFeaturesToggle", "0"),
("CompletedTrainingVersion", "0"),
("IsRHD", "0"),
("IsMetric", "0"),
("RecordFront", "0"),
("HasAcceptedTerms", "0"),
@ -552,6 +559,7 @@ def main():
("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
("LaneChangeEnabled", "1"),
("IsDriverViewEnabled", "0"),
]
# set unset params

21
selfdrive/modeld/dmonitoringmodeld.cc
View File

@ -28,6 +28,8 @@ int main(int argc, char **argv) {
// messaging
Context *msg_context = Context::create();
PubSocket *dmonitoring_sock = PubSocket::create(msg_context, "driverState");
SubSocket *dmonstate_sock = SubSocket::create(msg_context, "dMonitoringState", "127.0.0.1", true);
assert(dmonstate_sock != NULL);
// init the models
DMonitoringModelState dmonitoringmodel;
@ -46,6 +48,7 @@ int main(int argc, char **argv) {
LOGW("connected with buffer size: %d", buf_info.buf_len);
double last = 0;
int chk_counter = 0;
while (!do_exit) {
VIPCBuf *buf;
VIPCBufExtra extra;
@ -55,6 +58,23 @@ int main(int argc, char **argv) {
break;
}
//printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height);
if (!dmonitoringmodel.is_rhd_checked) {
if (chk_counter >= RHD_CHECK_INTERVAL) {
Message *msg = dmonstate_sock->receive(true);
if (msg != NULL) {
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
dmonitoringmodel.is_rhd = event.getDMonitoringState().getIsRHD();
dmonitoringmodel.is_rhd_checked = event.getDMonitoringState().getRhdChecked();
}
chk_counter = 0;
}
chk_counter += 1;
}
double t1 = millis_since_boot();
@ -74,6 +94,7 @@ int main(int argc, char **argv) {
visionstream_destroy(&stream);
delete dmonitoring_sock;
delete dmonstate_sock;
dmonitoring_free(&dmonitoringmodel);
return 0;

4
selfdrive/modeld/models/dmonitoring.cc
View File

@ -23,6 +23,8 @@ void dmonitoring_init(DMonitoringModelState* s) {
const char* model_path = "../../models/dmonitoring_model.dlc";
#endif
s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME);
s->is_rhd = false;
s->is_rhd_checked = false;
}
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
@ -43,7 +45,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height);
uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2));
if (true) {
if (!s->is_rhd) {
for (int r = 0; r < height/2; r++) {
memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width);
memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width);

3
selfdrive/modeld/models/dmonitoring.h
View File

@ -14,6 +14,7 @@ extern "C" {
#endif
#define OUTPUT_SIZE 33
#define RHD_CHECK_INTERVAL 10
typedef struct DMonitoringResult {
float face_orientation[3];
@ -29,6 +30,8 @@ typedef struct DMonitoringResult {
typedef struct DMonitoringModelState {
RunModel *m;
bool is_rhd;
bool is_rhd_checked;
float output[OUTPUT_SIZE];
} DMonitoringModelState;

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
ea2820c0708a95e8f392d503c7b76090ade380b0
3523742130b9e0554bab4ac5bc5ab535f1342e90

3
selfdrive/thermald/thermald.py
View File

@ -339,8 +339,9 @@ def thermald_thread():
# don't start while taking snapshot
if not should_start_prev:
is_viewing_driver = params.get("IsDriverViewEnabled") == b"1"
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
should_start = should_start and (not is_taking_snapshot)
should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver)
if fw_version_match and not fw_version_match_prev:
params.delete("Offroad_PandaFirmwareMismatch")

103
selfdrive/ui/paint.cc
View File

@ -279,7 +279,6 @@ static void draw_steering(UIState *s, float curvature) {
static void draw_frame(UIState *s) {
const UIScene *scene = &s->scene;
float x1, x2, y1, y2;
if (s->scene.frontview) {
glBindVertexArray(s->frame_vao[1]);
} else {
@ -703,6 +702,100 @@ static void ui_draw_vision_face(UIState *s) {
ui_draw_image(s->vg, face_img_x, face_img_y, face_img_size, face_img_size, s->img_face, face_img_alpha);
}
static void ui_draw_driver_view(UIState *s) {
const UIScene *scene = &s->scene;
s->scene.uilayout_sidebarcollapsed = true;
const int frame_x = scene->ui_viz_rx;
const int frame_w = scene->ui_viz_rw;
const int valid_frame_w = 4 * box_h / 3;
const int valid_frame_x = frame_x + (frame_w - valid_frame_w) / 2 + ff_xoffset;
// blackout
if (!scene->is_rhd) {
nvgBeginPath(s->vg);
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));
nvgFillPaint(s->vg, gradient);
nvgRect(s->vg, valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h);
nvgFill(s->vg);
} else {
nvgBeginPath(s->vg);
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));
nvgFillPaint(s->vg, gradient);
nvgRect(s->vg, valid_frame_x, box_y, valid_frame_w - box_h / 2, box_h);
nvgFill(s->vg);
}
nvgBeginPath(s->vg);
nvgRect(s->vg, scene->is_rhd ? valid_frame_x:valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h);
nvgFillColor(s->vg, COLOR_BLACK_ALPHA(144));
nvgFill(s->vg);
// borders
nvgBeginPath(s->vg);
nvgRect(s->vg, frame_x, box_y, valid_frame_x - frame_x, box_h);
nvgFillColor(s->vg, nvgRGBA(23,51,73,255));
nvgFill(s->vg);
nvgBeginPath(s->vg);
nvgRect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h);
nvgFillColor(s->vg, nvgRGBA(23,51,73,255));
nvgFill(s->vg);
// draw face box
if (scene->face_prob > 0.4) {
int fbox_x;
int fbox_y = box_y + (scene->face_y + 0.5) * box_h - 0.5 * 0.6 * box_h / 2;;
if (!scene->is_rhd) {
fbox_x = valid_frame_x + (1 - (scene->face_x + 0.5)) * (box_h / 2) - 0.5 * 0.6 * box_h / 2;
} else {
fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + (scene->face_x + 0.5) * (box_h / 2) - 0.5 * 0.6 * box_h / 2;
}
if (abs(scene->face_x) <= 0.35 && abs(scene->face_y) <= 0.4) {
nvgBeginPath(s->vg);
nvgRoundedRect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, 35);
nvgStrokeColor(s->vg, nvgRGBAf(1.0, 1.0, 1.0, 0.8 - ((abs(scene->face_x) > abs(scene->face_y) ? abs(scene->face_x):abs(scene->face_y))) * 0.6 / 0.375));
nvgStrokeWidth(s->vg, 10);
nvgStroke(s->vg);
} else {
nvgBeginPath(s->vg);
nvgRoundedRect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, 35);
nvgStrokeColor(s->vg, nvgRGBAf(1.0, 1.0, 1.0, 0.2));
nvgStrokeWidth(s->vg, 10);
nvgStroke(s->vg);
}
} else {
;
}
// draw face icon
const int face_size = 85;
const int face_x = (valid_frame_x + face_size + (bdr_s * 2)) + (scene->is_rhd ? valid_frame_w - box_h / 2:0);
const int face_y = (box_y + box_h - face_size - bdr_s - (bdr_s * 1.5));
const int face_img_size = (face_size * 1.5);
const int face_img_x = (face_x - (face_img_size / 2));
const int face_img_y = (face_y - (face_size / 4));
float face_img_alpha = scene->face_prob > 0.4 ? 1.0f : 0.15f;
float face_bg_alpha = scene->face_prob > 0.4 ? 0.3f : 0.1f;
NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha));
NVGpaint face_img = nvgImagePattern(s->vg, face_img_x, face_img_y,
face_img_size, face_img_size, 0, s->img_face, face_img_alpha);
nvgBeginPath(s->vg);
nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size);
nvgFillColor(s->vg, face_bg);
nvgFill(s->vg);
nvgBeginPath(s->vg);
nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size);
nvgFillPaint(s->vg, face_img);
nvgFill(s->vg);
}
static void ui_draw_vision_header(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
@ -828,14 +921,18 @@ static void ui_draw_vision(UIState *s) {
nvgRestore(s->vg);
// Set Speed, Current Speed, Status/Events
ui_draw_vision_header(s);
if (!scene->frontview) {
ui_draw_vision_header(s);
} else {
ui_draw_driver_view(s);
}
if (s->scene.alert_size != ALERTSIZE_NONE) {
// Controls Alerts
ui_draw_vision_alert(s, s->scene.alert_size, s->status,
s->scene.alert_text1, s->scene.alert_text2);
} else {
ui_draw_vision_footer(s);
if (!scene->frontview){ui_draw_vision_footer(s);}
}

78
selfdrive/ui/ui.cc
View File

@ -134,10 +134,18 @@ static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) {
}
}
static void handle_driver_view_touch(UIState *s, int touch_x, int touch_y) {
int err = write_db_value(NULL, "IsDriverViewEnabled", "0", 1);
}
static void handle_vision_touch(UIState *s, int touch_x, int touch_y) {
if (s->started && (touch_x >= s->scene.ui_viz_rx - bdr_s)
&& (s->active_app != cereal_UiLayoutState_App_settings)) {
s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed;
if (!s->scene.frontview) {
s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed;
} else {
handle_driver_view_touch(s, touch_x, touch_y);
}
update_offroad_layout_state(s);
}
}
@ -226,6 +234,8 @@ static void ui_init(UIState *s) {
s->thermal_sock = SubSocket::create(s->ctx, "thermal");
s->health_sock = SubSocket::create(s->ctx, "health");
s->ubloxgnss_sock = SubSocket::create(s->ctx, "ubloxGnss");
s->driverstate_sock = SubSocket::create(s->ctx, "driverState");
s->dmonitoring_sock = SubSocket::create(s->ctx, "dMonitoringState");
s->offroad_sock = PubSocket::create(s->ctx, "offroadLayout");
assert(s->model_sock != NULL);
@ -236,6 +246,8 @@ static void ui_init(UIState *s) {
assert(s->thermal_sock != NULL);
assert(s->health_sock != NULL);
assert(s->ubloxgnss_sock != NULL);
assert(s->driverstate_sock != NULL);
assert(s->dmonitoring_sock != NULL);
assert(s->offroad_sock != NULL);
s->poller = Poller::create({
@ -246,7 +258,9 @@ static void ui_init(UIState *s) {
s->radarstate_sock,
s->thermal_sock,
s->health_sock,
s->ubloxgnss_sock
s->ubloxgnss_sock,
s->driverstate_sock,
s->dmonitoring_sock
});
#ifdef SHOW_SPEEDLIMIT
@ -388,7 +402,8 @@ void handle_message(UIState *s, Message * msg) {
cereal_read_ControlsState(&datad, eventd.controlsState);
s->controls_timeout = 1 * UI_FREQ;
s->controls_seen = true;
s->scene.frontview = datad.rearViewCam;
if (!s->scene.frontview){s->controls_seen = true;}
if (datad.vCruise != s->scene.v_cruise) {
s->scene.v_cruise_update_ts = eventd.logMonoTime;
@ -401,8 +416,6 @@ void handle_message(UIState *s, Message * msg) {
s->scene.gps_planner_active = datad.gpsPlannerActive;
s->scene.monitoring_active = datad.driverMonitoringOn;
s->scene.frontview = datad.rearViewCam;
s->scene.decel_for_model = datad.decelForModel;
if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
@ -430,7 +443,6 @@ void handle_message(UIState *s, Message * msg) {
} else {
s->scene.alert_text2[0] = '\0';
}
s->scene.awareness_status = datad.awarenessStatus;
s->scene.alert_ts = eventd.logMonoTime;
@ -543,24 +555,8 @@ void handle_message(UIState *s, Message * msg) {
s->scene.thermalStatus = datad.thermalStatus;
s->scene.paTemp = datad.pa0;
s->started = datad.started;
s->thermal_started = datad.started;
// Handle onroad/offroad transition
if (!datad.started) {
if (s->status != STATUS_STOPPED) {
update_status(s, STATUS_STOPPED);
s->alert_sound_timeout = 0;
s->vision_seen = false;
s->controls_seen = false;
s->active_app = cereal_UiLayoutState_App_home;
update_offroad_layout_state(s);
}
} else if (s->status == STATUS_STOPPED) {
update_status(s, STATUS_DISENGAGED);
s->active_app = cereal_UiLayoutState_App_none;
update_offroad_layout_state(s);
}
} else if (eventd.which == cereal_Event_ubloxGnss) {
struct cereal_UbloxGnss datad;
cereal_read_UbloxGnss(&datad, eventd.ubloxGnss);
@ -575,7 +571,43 @@ void handle_message(UIState *s, Message * msg) {
s->scene.hwType = datad.hwType;
s->hardware_timeout = 5*30; // 5 seconds at 30 fps
} else if (eventd.which == cereal_Event_driverState) {
struct cereal_DriverState datad;
cereal_read_DriverState(&datad, eventd.driverState);
s->scene.face_prob = datad.faceProb;
capn_list32 fxy_list = datad.facePosition;
capn_resolve(&fxy_list.p);
s->scene.face_x = capn_to_f32(capn_get32(fxy_list, 0));
s->scene.face_y = capn_to_f32(capn_get32(fxy_list, 1));
} else if (eventd.which == cereal_Event_dMonitoringState) {
struct cereal_DMonitoringState datad;
cereal_read_DMonitoringState(&datad, eventd.dMonitoringState);
s->scene.is_rhd = datad.isRHD;
s->scene.awareness_status = datad.awarenessStatus;
s->preview_started = datad.isPreview;
}
s->started = s->thermal_started || s->preview_started ;
// Handle onroad/offroad transition
if (!s->started) {
if (s->status != STATUS_STOPPED) {
update_status(s, STATUS_STOPPED);
s->alert_sound_timeout = 0;
s->vision_seen = false;
s->controls_seen = false;
s->active_app = cereal_UiLayoutState_App_home;
update_offroad_layout_state(s);
}
} else if (s->status == STATUS_STOPPED) {
update_status(s, STATUS_DISENGAGED);
s->active_app = cereal_UiLayoutState_App_none;
update_offroad_layout_state(s);
}
capn_free(&ctx);
}

8
selfdrive/ui/ui.hpp
View File

@ -69,6 +69,7 @@ const int box_y = bdr_s;
const int box_w = vwp_w-sbr_w-(bdr_s*2);
const int box_h = vwp_h-(bdr_s*2);
const int viz_w = vwp_w-(bdr_s*2);
const int ff_xoffset = 32;
const int header_h = 420;
const int footer_h = 280;
const int footer_y = vwp_h-bdr_s-footer_h;
@ -140,6 +141,10 @@ typedef struct UIScene {
int lead_status2;
float lead_d_rel2, lead_y_rel2, lead_v_rel2;
float face_prob;
bool is_rhd;
float face_x, face_y;
int front_box_x, front_box_y, front_box_width, front_box_height;
uint64_t alert_ts;
@ -216,6 +221,8 @@ typedef struct UIState {
SubSocket *thermal_sock;
SubSocket *health_sock;
SubSocket *ubloxgnss_sock;
SubSocket *driverstate_sock;
SubSocket *dmonitoring_sock;
PubSocket *offroad_sock;
Poller * poller;
Poller * ublox_poller;
@ -281,6 +288,7 @@ typedef struct UIState {
float alert_blinking_alpha;
bool alert_blinked;
bool started;
bool thermal_started, preview_started;
bool vision_seen;
float light_sensor;