Merge remote-tracking branch 'upstream/master' into disengage-on-gas
commit
d5b6e30389
2
cereal
2
cereal
|
@ -1 +1 @@
|
|||
Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b
|
||||
Subproject commit 64b6014193a69fe2e2b3b7a17f9e228162081ddf
|
|
@ -1,2 +1,2 @@
|
|||
4e19be90-bd5b-485d-b79a-2462f7f1b49e
|
||||
08f7ec37b78228cd1cb750b6ddb9c6ba1769e911
|
||||
0ff292a8-3a9c-47e7-9134-2c3b0f69a1fe
|
||||
d7c2883ee58d7b757e588bdf13ec45891caf397c
|
BIN
models/dmonitoring_model.onnx (Stored with Git LFS)
BIN
models/dmonitoring_model.onnx (Stored with Git LFS)
Binary file not shown.
BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
Binary file not shown.
2
panda
2
panda
|
@ -1 +1 @@
|
|||
Subproject commit 5a7af82f06bf5f8039df8f58f9b1114f7d3436ee
|
||||
Subproject commit e6722c7f99b8f7b9f81bb0216edb88cc2e5ad7c0
|
|
@ -33,7 +33,6 @@ class CarInterfaceBase(ABC):
|
|||
self.steering_unpressed = 0
|
||||
self.low_speed_alert = False
|
||||
self.silent_steer_warning = True
|
||||
self.disengage_on_gas = Params().get_bool("DisengageOnGas")
|
||||
|
||||
if CarState is not None:
|
||||
self.CS = CarState(CP)
|
||||
|
@ -74,7 +73,6 @@ class CarInterfaceBase(ABC):
|
|||
def get_std_params(candidate, fingerprint):
|
||||
ret = car.CarParams.new_message()
|
||||
ret.carFingerprint = candidate
|
||||
ret.unsafeMode = 1 if Params().get_bool("DisengageOnGas") else 0 # see panda/board/safety_declarations.h for allowed values
|
||||
|
||||
# standard ALC params
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
@ -144,7 +142,6 @@ class CarInterfaceBase(ABC):
|
|||
if cs_out.parkingBrake:
|
||||
events.add(EventName.parkBrake)
|
||||
|
||||
|
||||
# Handle permanent and temporary steering faults
|
||||
self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
|
||||
if cs_out.steerFaultTemporary:
|
||||
|
@ -159,11 +156,6 @@ class CarInterfaceBase(ABC):
|
|||
if cs_out.steerFaultPermanent:
|
||||
events.add(EventName.steerUnavailable)
|
||||
|
||||
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
|
||||
if (self.disengage_on_gas and cs_out.gasPressed and not self.CS.out.gasPressed) or \
|
||||
(cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)):
|
||||
events.add(EventName.pedalPressed)
|
||||
|
||||
# we engage when pcm is active (rising edge)
|
||||
if pcm_enable:
|
||||
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
|
||||
|
|
|
@ -50,6 +50,9 @@ class TestCarModel(unittest.TestCase):
|
|||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
#if cls.car_model != "HONDA RIDGELINE 2017":
|
||||
# raise unittest.SkipTest
|
||||
|
||||
if cls.route is None:
|
||||
if cls.car_model in non_tested_cars:
|
||||
print(f"Skipping tests for {cls.car_model}: missing route")
|
||||
|
@ -59,7 +62,12 @@ class TestCarModel(unittest.TestCase):
|
|||
params = Params()
|
||||
params.clear_all()
|
||||
|
||||
for seg in [2, 1, 0]:
|
||||
for seg in (2, 1, 0):
|
||||
#from tools.lib.route import Route
|
||||
#from tools.lib.logreader import MultiLogIterator
|
||||
#r = Route("97bbe58ea225ad1d|2022-03-08--12-54-42")
|
||||
#lr = MultiLogIterator(r.log_paths()[3:4])
|
||||
|
||||
try:
|
||||
lr = LogReader(get_url(cls.route, seg))
|
||||
except Exception:
|
||||
|
@ -189,6 +197,7 @@ class TestCarModel(unittest.TestCase):
|
|||
self.safety.safety_rx_hook(to_send)
|
||||
self.CI.update(CC, (can_list_to_can_capnp([msg, ]), ))
|
||||
|
||||
CS_prev = car.CarState.new_message()
|
||||
checks = defaultdict(lambda: 0)
|
||||
for can in self.can_msgs:
|
||||
CS = self.CI.update(CC, (can.as_builder().to_bytes(), ))
|
||||
|
@ -217,11 +226,21 @@ class TestCarModel(unittest.TestCase):
|
|||
checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev()
|
||||
|
||||
if self.CP.pcmCruise:
|
||||
checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
|
||||
# On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
|
||||
# On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
|
||||
# openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
|
||||
if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
# only the rising edges are expected to match
|
||||
if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
|
||||
checks['controlsAllowed'] += not self.safety.get_controls_allowed()
|
||||
else:
|
||||
checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
|
||||
|
||||
if self.CP.carName == "honda":
|
||||
checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
|
||||
|
||||
CS_prev = CS
|
||||
|
||||
# TODO: add flag to toyota safety
|
||||
if self.CP.carFingerprint == TOYOTA.SIENNA and checks['brakePressed'] < 25:
|
||||
checks['brakePressed'] = 0
|
||||
|
|
|
@ -95,6 +95,9 @@ class Controls:
|
|||
|
||||
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
|
||||
|
||||
# see panda/board/safety_declarations.h for allowed values
|
||||
self.CP.unsafeMode = 1 if Params().get_bool("DisengageOnGas") else 0
|
||||
|
||||
# read params
|
||||
self.is_metric = params.get_bool("IsMetric")
|
||||
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
|
||||
|
@ -119,6 +122,7 @@ class Controls:
|
|||
put_nonblocking("CarParamsCache", cp_bytes)
|
||||
|
||||
self.CC = car.CarControl.new_message()
|
||||
self.CS_prev = car.CarState.new_message()
|
||||
self.AM = AlertManager()
|
||||
self.events = Events()
|
||||
|
||||
|
@ -192,6 +196,11 @@ class Controls:
|
|||
self.events.add(EventName.controlsInitializing)
|
||||
return
|
||||
|
||||
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0
|
||||
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_gas) or \
|
||||
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)):
|
||||
self.events.add(EventName.pedalPressed)
|
||||
|
||||
self.events.add_from_msg(CS.events)
|
||||
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
|
||||
|
||||
|
@ -733,6 +742,7 @@ class Controls:
|
|||
self.prof.checkpoint("Sent")
|
||||
|
||||
self.update_button_timers(CS.buttonEvents)
|
||||
self.CS_prev = CS
|
||||
|
||||
def controlsd_thread(self):
|
||||
while True:
|
||||
|
|
|
@ -8,10 +8,10 @@ extern "C" {
|
|||
}
|
||||
|
||||
void localizer_get_message_bytes(Localizer *localizer, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size)
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size)
|
||||
{
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK).asChars();
|
||||
kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
|
||||
assert(buff_size >= arr.size());
|
||||
memcpy(buff, arr.begin(), arr.size());
|
||||
}
|
||||
|
|
|
@ -457,15 +457,16 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
|||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK)
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid)
|
||||
{
|
||||
cereal::Event::Builder evt = msg_builder.initEvent();
|
||||
evt.setLogMonoTime(logMonoTime);
|
||||
evt.setValid(inputsOK);
|
||||
evt.setValid(msgValid);
|
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
|
||||
this->build_live_location(liveLoc);
|
||||
liveLoc.setSensorsOK(sensorsOK);
|
||||
liveLoc.setGpsOK(gpsOK);
|
||||
liveLoc.setInputsOK(inputsOK);
|
||||
return msg_builder.toBytes();
|
||||
}
|
||||
|
||||
|
@ -497,19 +498,21 @@ int Localizer::locationd_thread() {
|
|||
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
|
||||
|
||||
uint64_t cnt = 0;
|
||||
bool filterInitialized = false;
|
||||
|
||||
while (!do_exit) {
|
||||
sm.update();
|
||||
if (sm.allAliveAndValid()){
|
||||
if (filterInitialized){
|
||||
for (const char* service : service_list) {
|
||||
if (sm.updated(service)){
|
||||
if (sm.updated(service) && sm.valid(service)){
|
||||
const cereal::Event::Reader log = sm[service];
|
||||
this->handle_msg(log);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
filterInitialized = sm.allAliveAndValid();
|
||||
}
|
||||
|
||||
|
||||
if (sm.updated("cameraOdometry")) {
|
||||
uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
|
||||
bool inputsOK = sm.allAliveAndValid();
|
||||
|
@ -517,7 +520,7 @@ int Localizer::locationd_thread() {
|
|||
bool gpsOK = this->isGpsOK();
|
||||
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||
|
||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
||||
|
|
|
@ -36,7 +36,7 @@ public:
|
|||
void determine_gps_mode(double current_time);
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK);
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
|
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
||||
|
||||
Eigen::VectorXd get_position_geodetic();
|
||||
|
|
|
@ -24,7 +24,7 @@ class TestLocationdLib(unittest.TestCase):
|
|||
def setUp(self):
|
||||
header = '''typedef ...* Localizer_t;
|
||||
Localizer_t localizer_init();
|
||||
void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size);
|
||||
void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
|
||||
void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);'''
|
||||
|
||||
self.ffi = FFI()
|
||||
|
@ -40,8 +40,8 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
|
|||
bytstr = msg_builder.to_bytes()
|
||||
self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr))
|
||||
|
||||
def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True):
|
||||
self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
|
||||
def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True):
|
||||
self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
|
||||
return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8)
|
||||
|
||||
def test_liblocalizer(self):
|
||||
|
|
|
@ -13,6 +13,8 @@ ExitHandler do_exit;
|
|||
|
||||
void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
|
||||
PubMaster pm({"driverState"});
|
||||
SubMaster sm({"liveCalibration"});
|
||||
float calib[CALIB_LEN] = {0};
|
||||
double last = 0;
|
||||
|
||||
while (!do_exit) {
|
||||
|
@ -20,8 +22,16 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
|
|||
VisionBuf *buf = vipc_client.recv(&extra);
|
||||
if (buf == nullptr) continue;
|
||||
|
||||
sm.update(0);
|
||||
if (sm.updated("liveCalibration")) {
|
||||
auto calib_msg = sm["liveCalibration"].getLiveCalibration().getRpyCalib();
|
||||
for (int i = 0; i < CALIB_LEN; i++) {
|
||||
calib[i] = calib_msg[i];
|
||||
}
|
||||
}
|
||||
|
||||
double t1 = millis_since_boot();
|
||||
DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height);
|
||||
DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib);
|
||||
double t2 = millis_since_boot();
|
||||
|
||||
// send dm packet
|
||||
|
|
|
@ -70,7 +70,3 @@ void softmax(const float* input, float* output, size_t len) {
|
|||
float sigmoid(float input) {
|
||||
return 1 / (1 + expf(-input));
|
||||
}
|
||||
|
||||
float softplus(float input) {
|
||||
return log1p(expf(input));
|
||||
}
|
||||
|
|
|
@ -19,7 +19,6 @@
|
|||
const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
|
||||
|
||||
void softmax(const float* input, float* output, size_t len);
|
||||
float softplus(float input);
|
||||
float sigmoid(float input);
|
||||
|
||||
class ModelFrame {
|
||||
|
|
|
@ -43,6 +43,8 @@ void dmonitoring_init(DMonitoringModelState* s) {
|
|||
#else
|
||||
s->m = new SNPEModel("../../models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME);
|
||||
#endif
|
||||
|
||||
s->m->addCalib(s->calib, CALIB_LEN);
|
||||
}
|
||||
|
||||
static inline auto get_yuv_buf(std::vector<uint8_t> &buf, const int width, int height) {
|
||||
|
@ -65,7 +67,7 @@ void crop_yuv(uint8_t *raw, int width, int height, uint8_t *y, uint8_t *u, uint8
|
|||
}
|
||||
}
|
||||
|
||||
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
|
||||
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) {
|
||||
Rect crop_rect;
|
||||
if (width == TICI_CAM_WIDTH) {
|
||||
const int cropped_height = tici_dm_crop::width / 1.33;
|
||||
|
@ -167,29 +169,38 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
|
|||
|
||||
double t1 = millis_since_boot();
|
||||
s->m->addImage(net_input_buf, yuv_buf_len);
|
||||
for (int i = 0; i < CALIB_LEN; i++) {
|
||||
s->calib[i] = calib[i];
|
||||
}
|
||||
s->m->execute();
|
||||
double t2 = millis_since_boot();
|
||||
|
||||
DMonitoringResult ret = {0};
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
ret.face_orientation[i] = s->output[i];
|
||||
ret.face_orientation_meta[i] = softplus(s->output[6 + i]);
|
||||
ret.face_orientation[i] = s->output[i] * REG_SCALE;
|
||||
ret.face_orientation_meta[i] = exp(s->output[6 + i]);
|
||||
}
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
ret.face_position[i] = s->output[3 + i];
|
||||
ret.face_position_meta[i] = softplus(s->output[9 + i]);
|
||||
ret.face_position[i] = s->output[3 + i] * REG_SCALE;
|
||||
ret.face_position_meta[i] = exp(s->output[9 + i]);
|
||||
}
|
||||
ret.face_prob = s->output[12];
|
||||
ret.left_eye_prob = s->output[21];
|
||||
ret.right_eye_prob = s->output[30];
|
||||
ret.left_blink_prob = s->output[31];
|
||||
ret.right_blink_prob = s->output[32];
|
||||
ret.sg_prob = s->output[33];
|
||||
ret.poor_vision = s->output[34];
|
||||
ret.partial_face = s->output[35];
|
||||
ret.distracted_pose = s->output[36];
|
||||
ret.distracted_eyes = s->output[37];
|
||||
ret.occluded_prob = s->output[38];
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
ret.ready_prob[i] = sigmoid(s->output[39 + i]);
|
||||
}
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
ret.not_ready_prob[i] = sigmoid(s->output[43 + i]);
|
||||
}
|
||||
ret.face_prob = sigmoid(s->output[12]);
|
||||
ret.left_eye_prob = sigmoid(s->output[21]);
|
||||
ret.right_eye_prob = sigmoid(s->output[30]);
|
||||
ret.left_blink_prob = sigmoid(s->output[31]);
|
||||
ret.right_blink_prob = sigmoid(s->output[32]);
|
||||
ret.sg_prob = sigmoid(s->output[33]);
|
||||
ret.poor_vision = sigmoid(s->output[34]);
|
||||
ret.partial_face = sigmoid(s->output[35]);
|
||||
ret.distracted_pose = sigmoid(s->output[36]);
|
||||
ret.distracted_eyes = sigmoid(s->output[37]);
|
||||
ret.occluded_prob = sigmoid(s->output[38]);
|
||||
ret.dsp_execution_time = (t2 - t1) / 1000.;
|
||||
return ret;
|
||||
}
|
||||
|
@ -217,6 +228,8 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
|
|||
framed.setDistractedPose(res.distracted_pose);
|
||||
framed.setDistractedEyes(res.distracted_eyes);
|
||||
framed.setOccludedProb(res.occluded_prob);
|
||||
framed.setReadyProb(res.ready_prob);
|
||||
framed.setNotReadyProb(res.not_ready_prob);
|
||||
if (send_raw_pred) {
|
||||
framed.setRawPredictions(raw_pred.asBytes());
|
||||
}
|
||||
|
|
|
@ -7,7 +7,10 @@
|
|||
#include "selfdrive/modeld/models/commonmodel.h"
|
||||
#include "selfdrive/modeld/runners/run.h"
|
||||
|
||||
#define OUTPUT_SIZE 39
|
||||
#define CALIB_LEN 3
|
||||
|
||||
#define OUTPUT_SIZE 45
|
||||
#define REG_SCALE 0.25f
|
||||
|
||||
typedef struct DMonitoringResult {
|
||||
float face_orientation[3];
|
||||
|
@ -25,6 +28,8 @@ typedef struct DMonitoringResult {
|
|||
float distracted_pose;
|
||||
float distracted_eyes;
|
||||
float occluded_prob;
|
||||
float ready_prob[4];
|
||||
float not_ready_prob[2];
|
||||
float dsp_execution_time;
|
||||
} DMonitoringResult;
|
||||
|
||||
|
@ -36,11 +41,12 @@ typedef struct DMonitoringModelState {
|
|||
std::vector<uint8_t> cropped_buf;
|
||||
std::vector<uint8_t> premirror_cropped_buf;
|
||||
std::vector<float> net_input_buf;
|
||||
float calib[CALIB_LEN];
|
||||
float tensor[UINT8_MAX + 1];
|
||||
} DMonitoringModelState;
|
||||
|
||||
void dmonitoring_init(DMonitoringModelState* s);
|
||||
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height);
|
||||
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib);
|
||||
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr<const float> raw_pred);
|
||||
void dmonitoring_free(DMonitoringModelState* s);
|
||||
|
||||
|
|
|
@ -100,6 +100,11 @@ void ONNXModel::addTrafficConvention(float *state, int state_size) {
|
|||
traffic_convention_size = state_size;
|
||||
}
|
||||
|
||||
void ONNXModel::addCalib(float *state, int state_size) {
|
||||
calib_input_buf = state;
|
||||
calib_size = state_size;
|
||||
}
|
||||
|
||||
void ONNXModel::addImage(float *image_buf, int buf_size) {
|
||||
image_input_buf = image_buf;
|
||||
image_buf_size = buf_size;
|
||||
|
@ -124,6 +129,9 @@ void ONNXModel::execute() {
|
|||
if (traffic_convention_input_buf != NULL) {
|
||||
pwrite(traffic_convention_input_buf, traffic_convention_size);
|
||||
}
|
||||
if (calib_input_buf != NULL) {
|
||||
pwrite(calib_input_buf, calib_size);
|
||||
}
|
||||
if (rnn_input_buf != NULL) {
|
||||
pwrite(rnn_input_buf, rnn_state_size);
|
||||
}
|
||||
|
|
|
@ -11,6 +11,7 @@ public:
|
|||
void addRecurrent(float *state, int state_size);
|
||||
void addDesire(float *state, int state_size);
|
||||
void addTrafficConvention(float *state, int state_size);
|
||||
void addCalib(float *state, int state_size);
|
||||
void addImage(float *image_buf, int buf_size);
|
||||
void addExtra(float *image_buf, int buf_size);
|
||||
void execute();
|
||||
|
@ -26,6 +27,8 @@ private:
|
|||
int desire_state_size;
|
||||
float *traffic_convention_input_buf = NULL;
|
||||
int traffic_convention_size;
|
||||
float *calib_input_buf = NULL;
|
||||
int calib_size;
|
||||
float *image_input_buf = NULL;
|
||||
int image_buf_size;
|
||||
float *extra_input_buf = NULL;
|
||||
|
|
|
@ -5,6 +5,7 @@ public:
|
|||
virtual void addRecurrent(float *state, int state_size) {}
|
||||
virtual void addDesire(float *state, int state_size) {}
|
||||
virtual void addTrafficConvention(float *state, int state_size) {}
|
||||
virtual void addCalib(float *state, int state_size) {}
|
||||
virtual void addImage(float *image_buf, int buf_size) {}
|
||||
virtual void addExtra(float *image_buf, int buf_size) {}
|
||||
virtual void execute() {}
|
||||
|
|
|
@ -141,6 +141,11 @@ void SNPEModel::addDesire(float *state, int state_size) {
|
|||
desireBuffer = this->addExtra(state, state_size, 1);
|
||||
}
|
||||
|
||||
void SNPEModel::addCalib(float *state, int state_size) {
|
||||
calib = state;
|
||||
calibBuffer = this->addExtra(state, state_size, 1);
|
||||
}
|
||||
|
||||
void SNPEModel::addImage(float *image_buf, int buf_size) {
|
||||
input = image_buf;
|
||||
input_size = buf_size;
|
||||
|
|
|
@ -25,6 +25,7 @@ public:
|
|||
SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false);
|
||||
void addRecurrent(float *state, int state_size);
|
||||
void addTrafficConvention(float *state, int state_size);
|
||||
void addCalib(float *state, int state_size);
|
||||
void addDesire(float *state, int state_size);
|
||||
void addImage(float *image_buf, int buf_size);
|
||||
void addExtra(float *image_buf, int buf_size);
|
||||
|
@ -71,4 +72,6 @@ private:
|
|||
std::unique_ptr<zdl::DlSystem::IUserBuffer> trafficConventionBuffer;
|
||||
float *desire;
|
||||
std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
|
||||
float *calib;
|
||||
std::unique_ptr<zdl::DlSystem::IUserBuffer> calibBuffer;
|
||||
};
|
||||
|
|
|
@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
|
||||
# Get data from dmonitoringmodeld
|
||||
events = Events()
|
||||
driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
|
||||
driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
|
||||
|
||||
# Block engaging after max number of distrations
|
||||
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
|
||||
|
@ -59,7 +59,7 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
events.add(car.CarEvent.EventName.tooDistracted)
|
||||
|
||||
# Update events from driver state
|
||||
driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
|
||||
driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
|
||||
|
||||
# build driverMonitoringState packet
|
||||
dat = messaging.new_message('driverMonitoringState')
|
||||
|
@ -67,6 +67,7 @@ def dmonitoringd_thread(sm=None, pm=None):
|
|||
"events": events.to_msg(),
|
||||
"faceDetected": driver_status.face_detected,
|
||||
"isDistracted": driver_status.driver_distracted,
|
||||
"distractedType": sum(driver_status.distracted_types),
|
||||
"awarenessStatus": driver_status.awareness,
|
||||
"posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
|
||||
"posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
|
||||
|
|
|
@ -27,13 +27,18 @@ class DRIVER_MONITOR_SETTINGS():
|
|||
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||
|
||||
self._FACE_THRESHOLD = 0.5
|
||||
self._PARTIAL_FACE_THRESHOLD = 0.765 if TICI else 0.43
|
||||
self._EYE_THRESHOLD = 0.61 if TICI else 0.55
|
||||
self._SG_THRESHOLD = 0.89 if TICI else 0.86
|
||||
self._BLINK_THRESHOLD = 0.82 if TICI else 0.588
|
||||
self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.77
|
||||
self._PARTIAL_FACE_THRESHOLD = 0.8 if TICI else 0.45
|
||||
self._EYE_THRESHOLD = 0.55
|
||||
self._SG_THRESHOLD = 0.88 if TICI else 0.86
|
||||
self._BLINK_THRESHOLD = 0.61 if TICI else 0.59
|
||||
self._BLINK_THRESHOLD_SLACK = 0.8 if TICI else 0.75
|
||||
self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD
|
||||
|
||||
self._EE_THRESH11 = 0.75 if TICI else 0.4
|
||||
self._EE_THRESH12 = 3.25 if TICI else 2.45
|
||||
self._EE_THRESH21 = 0.01
|
||||
self._EE_THRESH22 = 0.35
|
||||
|
||||
self._POSE_PITCH_THRESHOLD = 0.3237
|
||||
self._POSE_PITCH_THRESHOLD_SLACK = 0.3657
|
||||
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
|
||||
|
@ -47,7 +52,7 @@ class DRIVER_MONITOR_SETTINGS():
|
|||
self._YAW_MAX_OFFSET = 0.289
|
||||
self._YAW_MIN_OFFSET = -0.0246
|
||||
|
||||
self._POSESTD_THRESHOLD = 0.38 if TICI else 0.3
|
||||
self._POSESTD_THRESHOLD = 0.315
|
||||
self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s
|
||||
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||
|
||||
|
@ -68,8 +73,9 @@ H, W, FULL_W = 320, 160, 426
|
|||
|
||||
class DistractedType:
|
||||
NOT_DISTRACTED = 0
|
||||
BAD_POSE = 1
|
||||
BAD_BLINK = 2
|
||||
DISTRACTED_POSE = 1
|
||||
DISTRACTED_BLINK = 2
|
||||
DISTRACTED_E2E = 4
|
||||
|
||||
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd):
|
||||
# the output of these angles are in device frame
|
||||
|
@ -119,9 +125,17 @@ class DriverStatus():
|
|||
self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.pose_calibrated = False
|
||||
self.blink = DriverBlink()
|
||||
self.eev1 = 0.
|
||||
self.eev2 = 1.
|
||||
self.ee1_offseter = RunningStatFilter(max_trackable=self.settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.ee2_offseter = RunningStatFilter(max_trackable=self.settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.ee1_calibrated = False
|
||||
self.ee2_calibrated = False
|
||||
|
||||
self.awareness = 1.
|
||||
self.awareness_active = 1.
|
||||
self.awareness_passive = 1.
|
||||
self.distracted_types = []
|
||||
self.driver_distracted = False
|
||||
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
|
||||
self.face_detected = False
|
||||
|
@ -167,26 +181,38 @@ class DriverStatus():
|
|||
self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
|
||||
self.active_monitoring_mode = False
|
||||
|
||||
def _is_driver_distracted(self, pose, blink):
|
||||
if not self.pose_calibrated:
|
||||
pitch_error = pose.pitch - self.settings._PITCH_NATURAL_OFFSET
|
||||
yaw_error = pose.yaw - self.settings._YAW_NATURAL_OFFSET
|
||||
else:
|
||||
pitch_error = pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
|
||||
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
|
||||
yaw_error = pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
|
||||
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
|
||||
def _get_distracted_types(self):
|
||||
distracted_types = []
|
||||
|
||||
if not self.pose_calibrated:
|
||||
pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
|
||||
yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
|
||||
else:
|
||||
pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
|
||||
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
|
||||
yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
|
||||
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
|
||||
pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
|
||||
yaw_error = abs(yaw_error)
|
||||
if pitch_error > self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch or \
|
||||
yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw:
|
||||
distracted_types.append(DistractedType.DISTRACTED_POSE)
|
||||
|
||||
if pitch_error > self.settings._POSE_PITCH_THRESHOLD*pose.cfactor_pitch or \
|
||||
yaw_error > self.settings._POSE_YAW_THRESHOLD*pose.cfactor_yaw:
|
||||
return DistractedType.BAD_POSE
|
||||
elif (blink.left_blink + blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*blink.cfactor:
|
||||
return DistractedType.BAD_BLINK
|
||||
if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*self.blink.cfactor:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.ee1_calibrated:
|
||||
ee1_dist = self.eev1 > self.ee1_offseter.filtered_stat.M * self.settings._EE_THRESH12
|
||||
else:
|
||||
return DistractedType.NOT_DISTRACTED
|
||||
ee1_dist = self.eev1 > self.settings._EE_THRESH11
|
||||
if self.ee2_calibrated:
|
||||
ee2_dist = self.eev2 < self.ee2_offseter.filtered_stat.M * self.settings._EE_THRESH22
|
||||
else:
|
||||
ee2_dist = self.eev2 < self.settings._EE_THRESH21
|
||||
if ee1_dist or ee2_dist:
|
||||
distracted_types.append(DistractedType.DISTRACTED_E2E)
|
||||
|
||||
return distracted_types
|
||||
|
||||
def set_policy(self, model_data, car_speed):
|
||||
ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob
|
||||
|
@ -205,9 +231,10 @@ class DriverStatus():
|
|||
[self.settings._POSE_YAW_THRESHOLD_SLACK,
|
||||
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
|
||||
|
||||
def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged):
|
||||
def update_states(self, driver_state, cal_rpy, car_speed, op_engaged):
|
||||
if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition,
|
||||
driver_state.faceOrientationStd, driver_state.facePositionStd)):
|
||||
driver_state.faceOrientationStd, driver_state.facePositionStd,
|
||||
driver_state.readyProb, driver_state.notReadyProb)):
|
||||
return
|
||||
|
||||
self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD
|
||||
|
@ -220,9 +247,13 @@ class DriverStatus():
|
|||
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD and not self.face_partial
|
||||
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.eev1 = driver_state.notReadyProb[1]
|
||||
self.eev2 = driver_state.readyProb[0]
|
||||
|
||||
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \
|
||||
driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
|
||||
self.distracted_types = self._get_distracted_types()
|
||||
self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or
|
||||
DistractedType.DISTRACTED_BLINK in self.distracted_types) and \
|
||||
driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
|
||||
self.driver_distraction_filter.update(self.driver_distracted)
|
||||
|
||||
# update offseter
|
||||
|
@ -230,9 +261,13 @@ class DriverStatus():
|
|||
if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
|
||||
self.pose.pitch_offseter.push_and_update(self.pose.pitch)
|
||||
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
|
||||
self.ee1_offseter.push_and_update(self.eev1)
|
||||
self.ee2_offseter.push_and_update(self.eev2)
|
||||
|
||||
self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \
|
||||
self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
|
||||
self.ee1_calibrated = self.ee1_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
|
||||
self.ee2_calibrated = self.ee2_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
|
||||
|
||||
self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME
|
||||
self._set_timers(self.face_detected and not self.is_model_uncertain)
|
||||
|
@ -241,7 +276,7 @@ class DriverStatus():
|
|||
elif self.face_detected and self.pose.low_std:
|
||||
self.hi_stds = 0
|
||||
|
||||
def update(self, events, driver_engaged, ctrl_active, standstill):
|
||||
def update_events(self, events, driver_engaged, ctrl_active, standstill):
|
||||
if (driver_engaged and self.awareness > 0) or not ctrl_active:
|
||||
# reset only when on disengagement if red reached
|
||||
self.awareness = 1.
|
||||
|
|
|
@ -27,6 +27,9 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
|
|||
ds.rightBlinkProb = 1. * distracted
|
||||
ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
|
||||
ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
|
||||
# TODO: test both separately when e2e is used
|
||||
ds.readyProb = [0., 0., 0., 0.]
|
||||
ds.notReadyProb = [0., 0.]
|
||||
return ds
|
||||
|
||||
|
||||
|
@ -57,11 +60,11 @@ class TestMonitoring(unittest.TestCase):
|
|||
events = []
|
||||
for idx in range(len(msgs)):
|
||||
e = Events()
|
||||
DS.get_pose(msgs[idx], [0, 0, 0], 0, engaged[idx])
|
||||
DS.update_states(msgs[idx], [0, 0, 0], 0, engaged[idx])
|
||||
# cal_rpy and car_speed don't matter here
|
||||
|
||||
# evaluate events at 10Hz for tests
|
||||
DS.update(e, interaction[idx], engaged[idx], standstill[idx])
|
||||
DS.update_events(e, interaction[idx], engaged[idx], standstill[idx])
|
||||
events.append(e)
|
||||
assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs"
|
||||
return events, DS
|
||||
|
|
|
@ -1 +1 @@
|
|||
710313545a2b5e0899c6ee0b99ecd9d322cfecb7
|
||||
012f7c1b4aca0e0f38a1a2ddea41e738debe0003
|
|
@ -1 +1 @@
|
|||
37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30
|
||||
927918307b45657df0a4ac0255c3e8e3dc62d7cd
|
Loading…
Reference in New Issue