Vehicle Researcher
6 years ago
parent
a70457bb26
commit
dfe603c178
112 changed files with 2070 additions and 1108 deletions
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:9eba15d8e77847b242a83fbd43dd67a5bd5d0c6d76f4f11f6fd6820621f69771 |
||||
size 17096653 |
||||
oid sha256:e1d77c74a7e8122ee0c0595f55b5f23b6365b454f6a369fa77ca237c3ea4b0d8 |
||||
size 17715672 |
||||
|
||||
@ -0,0 +1,73 @@ |
||||
import numpy as np |
||||
|
||||
class RunningStat(): |
||||
# tracks realtime mean and standard deviation without storing any data |
||||
def __init__(self, priors=None, max_trackable=-1): |
||||
self.max_trackable = max_trackable |
||||
if priors is not None: |
||||
# initialize from history |
||||
self.M = priors[0] |
||||
self.S = priors[1] |
||||
self.n = priors[2] |
||||
self.M_last = self.M |
||||
self.S_last = self.S |
||||
|
||||
else: |
||||
self.reset() |
||||
|
||||
def reset(self): |
||||
self.M = 0. |
||||
self.S = 0. |
||||
self.M_last = 0. |
||||
self.S_last = 0. |
||||
self.n = 0 |
||||
|
||||
def push_data(self, new_data): |
||||
# short term memory hack |
||||
if self.max_trackable < 0 or self.n < self.max_trackable: |
||||
self.n += 1 |
||||
if self.n == 0: |
||||
self.M_last = new_data |
||||
self.M = self.M_last |
||||
self.S_last = 0. |
||||
else: |
||||
self.M = self.M_last + (new_data - self.M_last) / self.n |
||||
self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M); |
||||
self.M_last = self.M |
||||
self.S_last = self.S |
||||
|
||||
def mean(self): |
||||
return self.M |
||||
|
||||
def variance(self): |
||||
if self.n >= 2: |
||||
return self.S / (self.n - 1.) |
||||
else: |
||||
return 0 |
||||
|
||||
def std(self): |
||||
return np.sqrt(self.variance()) |
||||
|
||||
def params_to_save(self): |
||||
return [self.M, self.S, self.n] |
||||
|
||||
class RunningStatFilter(): |
||||
def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1): |
||||
self.raw_stat = RunningStat(raw_priors, max_trackable) |
||||
self.filtered_stat = RunningStat(filtered_priors, max_trackable) |
||||
|
||||
def reset(self): |
||||
self.raw_stat.reset() |
||||
self.filtered_stat.reset() |
||||
|
||||
def push_and_update(self, new_data): |
||||
_std_last = self.raw_stat.std() |
||||
self.raw_stat.push_data(new_data) |
||||
_delta_std = self.raw_stat.std() - _std_last |
||||
if _delta_std<=0: |
||||
self.filtered_stat.push_data(new_data) |
||||
else: |
||||
pass |
||||
# self.filtered_stat.push_data(self.filtered_stat.mean()) |
||||
|
||||
# class SequentialBayesian(): |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:da43154c2563cfda1af50018a412a6105338c4fafc021f8b3c7442fcec514d44 |
||||
size 2464536 |
||||
oid sha256:9d5a44fd0dcf94172c1637c951e1da3b52d6a7049c9c12e14a02900a0f9e9aa4 |
||||
size 2468632 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:f5fb0036eaaa6136b8b605c24ab255de45b14ce827af538e15c47a04a6b200af |
||||
size 14438998 |
||||
oid sha256:7696d34bb06568c6a0a152404cff258e9ebd716aac3dc013287733940be58b2e |
||||
size 14488833 |
||||
|
||||
Binary file not shown.
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:64fee5ec1a356f70c7c258b344b64e87a98a66fe1bd49d5fd34573350e43567b |
||||
size 13660 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:4919cfb14a73cd64fcef67b107613970cf1659a09aa675dba31314f373bc7204 |
||||
size 1901 |
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:bb7a23963ff80018961778bc2946da9a027ba3ed15658af91cfd9fdadad7e384 |
||||
size 772536 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:2b2cc1180c7e6988328ad2033b04b80117419db9c4c584918bbb3cfec7e9364f |
||||
size 1506 |
||||
@ -1 +0,0 @@ |
||||
libopenblas_armv8p-r0.2.19.so |
||||
@ -1,3 +0,0 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:52a864869d30995fbde8192d7d4fa406cd98a4cbac16c79556bcf591cd8da23c |
||||
size 9623424 |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:aa6fcc27be034e41e21dd832f9175bfe694a48491d9e14ff0fa278e19ad14f1b |
||||
size 1061 |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:f888f84627b4b69395cdf3f8bedf7a6a50c13253b30b7ddc2eda010c8be16b66 |
||||
oid sha256:5db18c6fb1e975d6d2bdb62372214a981e179d9af790a0e579e7d7a4c32bbb75 |
||||
size 40662 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:113afc45a281e491a771bdd749f1375b96b174e9c74a8dff5ab1cd83f3cad406 |
||||
oid sha256:165a121885f67004ffbeb34d6ad265c6e54dc5456674379434ff6f66ca6f688a |
||||
size 40654 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:301eef864fa7b306527828bb9e8295208c08c24382026391a9206b19aca58a4e |
||||
oid sha256:bfe1b73efabfcbd36dae63d39d0c1e2d4c1ead63daac0430a648e4686307e06c |
||||
size 40650 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:9cffe1d8cd2dec399fb3fce5ce2833e0405bd44191d3a24d6085592cd8d31dd5 |
||||
oid sha256:80b08552196740078647d35ef20781018a8fe97e75a1ff99e297f40fc3721b72 |
||||
size 21468 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:62580fd60d5784505f4f7697e5a5011cf0ae1f3c92ed1104700159bfbcd7de3e |
||||
oid sha256:e466c0086561f821386a072ff67a8b0d10bfcea6bd7426d086754ed5f15c691b |
||||
size 52800 |
||||
|
||||
@ -0,0 +1,87 @@ |
||||
from cereal import car |
||||
from selfdrive.car.ford.fordcan import make_can_msg, create_steer_command, create_lkas_ui, \ |
||||
spam_cancel_button |
||||
from selfdrive.can.packer import CANPacker |
||||
|
||||
|
||||
MAX_STEER_DELTA = 1 |
||||
TOGGLE_DEBUG = False |
||||
|
||||
class CarController(object): |
||||
def __init__(self, dbc_name, enable_camera, vehicle_model): |
||||
self.packer = CANPacker(dbc_name) |
||||
self.enable_camera = enable_camera |
||||
self.enabled_last = False |
||||
self.main_on_last = False |
||||
self.vehicle_model = vehicle_model |
||||
self.generic_toggle_last = 0 |
||||
self.steer_alert_last = False |
||||
self.lkas_action = 0 |
||||
|
||||
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): |
||||
|
||||
can_sends = [] |
||||
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired |
||||
|
||||
apply_steer = actuators.steer |
||||
|
||||
if self.enable_camera: |
||||
|
||||
if pcm_cancel: |
||||
#print "CANCELING!!!!" |
||||
can_sends.append(spam_cancel_button(self.packer)) |
||||
|
||||
if (frame % 3) == 0: |
||||
|
||||
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego) |
||||
|
||||
# The use of the toggle below is handy for trying out the various LKAS modes |
||||
if TOGGLE_DEBUG: |
||||
self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last) |
||||
self.lkas_action &= 0xf |
||||
else: |
||||
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy |
||||
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, enabled, |
||||
CS.lkas_state, CS.angle_steers, curvature, self.lkas_action)) |
||||
self.generic_toggle_last = CS.generic_toggle |
||||
|
||||
if (frame % 100) == 0: |
||||
|
||||
can_sends.append(make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False)) |
||||
#can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False)) |
||||
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \ |
||||
(self.steer_alert_last != steer_alert): |
||||
can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert)) |
||||
|
||||
if (frame % 200) == 0: |
||||
can_sends.append(make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False)) |
||||
|
||||
if (frame % 10) == 0: |
||||
|
||||
can_sends.append(make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False)) |
||||
can_sends.append(make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False)) |
||||
|
||||
can_sends.append(make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False)) |
||||
can_sends.append(make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False)) |
||||
can_sends.append(make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False)) |
||||
can_sends.append(make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False)) |
||||
|
||||
can_sends.append(make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False)) |
||||
can_sends.append(make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False)) |
||||
can_sends.append(make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False)) |
||||
can_sends.append(make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False)) |
||||
can_sends.append(make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False)) |
||||
can_sends.append(make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False)) |
||||
|
||||
static_msgs = range(1653, 1658) |
||||
for addr in static_msgs: |
||||
cnt = (frame % 10) + 1 |
||||
can_sends.append(make_can_msg(addr, chr(cnt<<4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False)) |
||||
|
||||
self.enabled_last = enabled |
||||
self.main_on_last = CS.main_on |
||||
self.steer_alert_last = steer_alert |
||||
|
||||
return can_sends |
||||
@ -0,0 +1,54 @@ |
||||
from common.numpy_fast import clip |
||||
from selfdrive.car.ford.values import MAX_ANGLE |
||||
|
||||
|
||||
def make_can_msg(addr, dat, alt, cks=False): |
||||
return [addr, 0, dat, alt] |
||||
|
||||
|
||||
def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action): |
||||
"""Creates a CAN message for the Ford Steer Command.""" |
||||
|
||||
#if enabled and lkas available: |
||||
if enabled and lkas_state in [2,3]: #and (frame % 500) >= 3: |
||||
action = lkas_action |
||||
else: |
||||
action = 0xf |
||||
angle_cmd = angle_steers/MAX_ANGLE |
||||
|
||||
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE) |
||||
|
||||
values = { |
||||
"Lkas_Action": action, |
||||
"Lkas_Alert": 0xf, # no alerts |
||||
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug? |
||||
#"Lane_Curvature": 0, # is it just for debug? |
||||
"Steer_Angle_Req": angle_cmd |
||||
} |
||||
return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values) |
||||
|
||||
|
||||
def create_lkas_ui(packer, main_on, enabled, steer_alert): |
||||
"""Creates a CAN message for the Ford Steer Ui.""" |
||||
|
||||
if not main_on: |
||||
lines = 0xf |
||||
elif enabled: |
||||
lines = 0x3 |
||||
else: |
||||
lines = 0x6 |
||||
|
||||
values = { |
||||
"Set_Me_X80": 0x80, |
||||
"Set_Me_X45": 0x45, |
||||
"Set_Me_X30": 0x30, |
||||
"Lines_Hud": lines, |
||||
"Hands_Warning_W_Chime": steer_alert, |
||||
} |
||||
return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values) |
||||
|
||||
def spam_cancel_button(packer): |
||||
values = { |
||||
"Cancel": 1 |
||||
} |
||||
return packer.make_can_msg("Steering_Buttons", 0, values) |
||||
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.6.3-release" |
||||
#define COMMA_VERSION "0.6.4-release" |
||||
|
||||
@ -0,0 +1,17 @@ |
||||
_RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \ |
||||
['IN1', 6.75, 28.10, 68.17, 97.4], \ |
||||
['IN2', 28.09, 35.99, 72.18, 80.87], \ |
||||
['IRL', 51.42, 55.38, -10.58, -5.99], \ |
||||
['JP1', 32.66, 45.52, 137.27, 146.02], \ |
||||
['JP2', 32.79, 37.60, 131.41, 137.28], \ |
||||
['JP3', 24.04, 34.78, 122.93, 131.42], \ |
||||
['NZ', -52.61, -29.24, 166, 178.84], \ |
||||
['SF', -35.14, -22.13, 16.07, 33.21], \ |
||||
['UK', 49.9, 60.84, -8.62, 1.77] ] |
||||
|
||||
def is_rhd_region(latitude, longitude): |
||||
for region in _RHD_REGION_MAP: |
||||
if region[1] <= latitude <= region[2] and \ |
||||
region[3] <= longitude <= region[4]: |
||||
return True |
||||
return False |
||||
@ -1,62 +0,0 @@ |
||||
import numpy as np |
||||
import math |
||||
from common.numpy_fast import interp |
||||
|
||||
_K_CURV_V = [1., 0.6] |
||||
_K_CURV_BP = [0., 0.002] |
||||
|
||||
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm |
||||
_LANE_WIDTH_V = [3., 3.8] |
||||
|
||||
# break points of speed |
||||
_LANE_WIDTH_BP = [0., 31.] |
||||
|
||||
|
||||
def calc_d_lookahead(v_ego, d_poly): |
||||
# this function computes how far too look for lateral control |
||||
# howfar we look ahead is function of speed and how much curvy is the path |
||||
offset_lookahead = 1. |
||||
k_lookahead = 7. |
||||
# integrate abs value of second derivative of poly to get a measure of path curvature |
||||
pts_len = 50. # m |
||||
if len(d_poly) > 0: |
||||
pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len)) |
||||
else: |
||||
pts = 0. |
||||
curv = np.sum(np.abs(pts)) / pts_len |
||||
|
||||
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V) |
||||
|
||||
# sqrt on speed is needed to keep, for a given curvature, the y_des |
||||
# proportional to speed. Indeed, y_des is prop to d_lookahead^2 |
||||
# 36m at 25m/s |
||||
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv |
||||
return d_lookahead |
||||
|
||||
|
||||
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset): |
||||
# this function returns the lateral offset given the steering angle, speed and the lookahead distance |
||||
sa = math.radians(angle_steers - angle_offset) |
||||
curvature = VM.calc_curvature(sa, v_ego) |
||||
# clip is to avoid arcsin NaNs due to too sharp turns |
||||
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.) |
||||
return y_actual, curvature |
||||
|
||||
|
||||
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset): |
||||
# inverse of the above function |
||||
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead |
||||
steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset |
||||
return steer_des, curvature |
||||
|
||||
|
||||
def compute_path_pinv(l=50): |
||||
deg = 3 |
||||
x = np.arange(l*1.0) |
||||
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T |
||||
pinv = np.linalg.pinv(X) |
||||
return pinv |
||||
|
||||
|
||||
def model_polyfit(points, path_pinv): |
||||
return np.dot(path_pinv, [float(x) for x in points]) |
||||
@ -0,0 +1,193 @@ |
||||
import unittest |
||||
import numpy as np |
||||
from common.realtime import DT_CTRL, 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, \ |
||||
_DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL |
||||
from selfdrive.controls.lib.gps_helpers import is_rhd_region |
||||
|
||||
_TEST_TIMESPAN = 120 # seconds |
||||
_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 |
||||
_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1 |
||||
_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 |
||||
_INVISIBLE_SECONDS_TO_RED = _AWARENESS_TIME + 1 |
||||
|
||||
class fake_DM_msg(): |
||||
def __init__(self, is_face_detected, is_distracted=False): |
||||
self.faceOrientation = [0.,0.,0.] |
||||
self.facePosition = [0.,0.] |
||||
self.faceProb = 1. * is_face_detected |
||||
self.leftEyeProb = 1. |
||||
self.rightEyeProb = 1. |
||||
self.leftBlinkProb = 1. * is_distracted |
||||
self.rightBlinkProb = 1. * is_distracted |
||||
|
||||
|
||||
# driver state from neural net, 10Hz |
||||
msg_NO_FACE_DETECTED = fake_DM_msg(is_face_detected=False) |
||||
msg_ATTENTIVE = fake_DM_msg(is_face_detected=True) |
||||
msg_DISTRACTED = fake_DM_msg(is_face_detected=True, is_distracted=True) |
||||
|
||||
# driver interaction with car |
||||
car_interaction_DETECTED = True |
||||
car_interaction_NOT_DETECTED = False |
||||
|
||||
# openpilot state |
||||
openpilot_ENGAGED = True |
||||
openpilot_NOT_ENGAGED = False |
||||
|
||||
# car standstill state |
||||
car_STANDSTILL = True |
||||
car_NOT_STANDSTILL = False |
||||
|
||||
# some common state vectors |
||||
always_no_face = [msg_NO_FACE_DETECTED] * int(_TEST_TIMESPAN/DT_DMON) |
||||
always_attentive = [msg_ATTENTIVE] * int(_TEST_TIMESPAN/DT_DMON) |
||||
always_distracted = [msg_DISTRACTED] * int(_TEST_TIMESPAN/DT_DMON) |
||||
always_true = [True] * int(_TEST_TIMESPAN/DT_DMON) |
||||
always_false = [False] * int(_TEST_TIMESPAN/DT_DMON) |
||||
|
||||
def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status): |
||||
# inputs are all 10Hz |
||||
DS = DriverStatus() |
||||
events_from_DM = [] |
||||
for idx in range(len(driver_state_msgs)): |
||||
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]) |
||||
events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests |
||||
|
||||
assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong' |
||||
return events_from_DM |
||||
|
||||
class TestMonitoring(unittest.TestCase): |
||||
# -1. rhd parser sanity check |
||||
def test_rhd_parser(self): |
||||
cities = [[32.7, -117.1, 0],\ |
||||
[51.5, 0.129, 1],\ |
||||
[35.7, 139.7, 1],\ |
||||
[-37.8, 144.9, 1],\ |
||||
[32.1, 41.74, 0],\ |
||||
[55.7, 12.69, 0]] |
||||
result = [] |
||||
for city in cities: |
||||
result.append(int(is_rhd_region(city[0],city[1]))) |
||||
self.assertEqual(result,[int(city[2]) for city in cities]) |
||||
|
||||
# 0. op engaged, driver is doing fine all the time |
||||
def test_fully_aware_driver(self): |
||||
events_output = run_DState_seq(always_attentive, always_false, always_true, always_false) |
||||
self.assertTrue(np.sum([len(event) for event in events_output])==0) |
||||
|
||||
# 1. op engaged, driver is distracted and does nothing |
||||
def test_fully_distracted_driver(self): |
||||
events_output = run_DState_seq(always_distracted, always_false, always_true, always_false) |
||||
self.assertTrue(len(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+\ |
||||
((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverDistracted') |
||||
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\ |
||||
((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverDistracted') |
||||
self.assertEqual(events_output[int((_DISTRACTED_TIME+\ |
||||
((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)][0].name, 'driverDistracted') |
||||
|
||||
# 2. op engaged, no face detected the whole time, no action |
||||
def test_fully_invisible_driver(self): |
||||
events_output = run_DState_seq(always_no_face, always_false, always_true, always_false) |
||||
self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL+\ |
||||
((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverUnresponsive') |
||||
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\ |
||||
((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverUnresponsive') |
||||
self.assertEqual(events_output[int((_AWARENESS_TIME+\ |
||||
((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)][0].name, 'driverUnresponsive') |
||||
|
||||
# 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel |
||||
# - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch |
||||
def test_normal_driver(self): |
||||
ds_vector = [msg_DISTRACTED] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ |
||||
[msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ |
||||
[msg_DISTRACTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON)) |
||||
interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ |
||||
[car_interaction_DETECTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) |
||||
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false) |
||||
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverDistracted') |
||||
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)][0].name, 'promptDriverDistracted') |
||||
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0) |
||||
|
||||
# 4. op engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ |
||||
# driver dodges, and then touches wheel to no avail, disengages and reengages |
||||
# - orange/red alert should remain after disappearance, and only disengaging clears red |
||||
def test_biggest_comma_fan(self): |
||||
_invisible_time = 2 # seconds |
||||
ds_vector = always_distracted[:] |
||||
interaction_vector = always_false[:] |
||||
op_vector = always_true[:] |
||||
ds_vector[int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((_DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) |
||||
ds_vector[int((_DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) |
||||
interaction_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON) |
||||
op_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON) |
||||
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false) |
||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)][0].name, 'promptDriverDistracted') |
||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)][0].name, 'driverDistracted') |
||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)][0].name, 'driverDistracted') |
||||
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0) |
||||
|
||||
# 5. op engaged, invisible driver, down to orange, driver appears; then down to orange again, driver touches wheel |
||||
# - both actions should clear the alert |
||||
def test_sometimes_transparent_commuter(self): |
||||
_visible_time = 2 # seconds |
||||
ds_vector = always_no_face[:]*2 |
||||
interaction_vector = always_false[:]*2 |
||||
ds_vector[int(_INVISIBLE_SECONDS_TO_ORANGE/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) |
||||
interaction_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+_visible_time)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) |
||||
events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) |
||||
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive') |
||||
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*1.5+_visible_time)/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+_visible_time-0.5)/DT_DMON)][0].name, 'promptDriverUnresponsive') |
||||
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+_visible_time+0.1)/DT_DMON)])==0) |
||||
|
||||
# 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages |
||||
# - only disengage will clear the alert |
||||
def test_last_second_responder(self): |
||||
_visible_time = 2 # seconds |
||||
ds_vector = always_no_face[:] |
||||
interaction_vector = always_false[:] |
||||
op_vector = always_true[:] |
||||
ds_vector[int(_INVISIBLE_SECONDS_TO_RED/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) |
||||
interaction_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) |
||||
op_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) |
||||
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false) |
||||
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0) |
||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive') |
||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)][0].name, 'driverUnresponsive') |
||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)][0].name, 'driverUnresponsive') |
||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)][0].name, 'driverUnresponsive') |
||||
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0) |
||||
|
||||
# 7. op not engaged, always distracted driver |
||||
# - dm should stay quiet when not engaged |
||||
def test_pure_dashcam_user(self): |
||||
events_output = run_DState_seq(always_distracted, always_false, always_false, always_false) |
||||
self.assertTrue(np.sum([len(event) for event in events_output])==0) |
||||
|
||||
# 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving |
||||
# - should only reach green when stopped, but continues counting down on launch |
||||
def test_long_traffic_light_victim(self): |
||||
_redlight_time = 60 # seconds |
||||
standstill_vector = always_true[:] |
||||
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON) |
||||
events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector) |
||||
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)][0].name, 'preDriverDistracted') |
||||
self.assertEqual(events_output[int((_redlight_time-0.1)/DT_DMON)][0].name, 'preDriverDistracted') |
||||
self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted') |
||||
|
||||
if __name__ == "__main__": |
||||
print 'MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS |
||||
unittest.main() |
||||
@ -1 +1 @@ |
||||
e3388c62ffb80f4b3ca8721da56a581a93c44e79 |
||||
8a11bcbc9833e154e10b59a8babb2b4545372f56 |
||||
@ -0,0 +1,74 @@ |
||||
CC = clang
|
||||
CXX = clang++
|
||||
|
||||
PHONELIBS = ../../../phonelibs
|
||||
|
||||
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||
-Werror=incompatible-pointer-types \
|
||||
-Werror=int-conversion \
|
||||
-Werror=return-type \
|
||||
-Werror=format-extra-args
|
||||
|
||||
CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
|
||||
CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
|
||||
|
||||
NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
|
||||
|
||||
OPENGL_LIBS = -lGLESv3
|
||||
|
||||
FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
|
||||
|
||||
OBJS = spinner.o \
|
||||
../../common/framebuffer.o \
|
||||
$(PHONELIBS)/nanovg/nanovg.o \
|
||||
../../common/spinner.o \
|
||||
opensans_semibold.o \
|
||||
img_spinner_track.o \
|
||||
img_spinner_comma.o
|
||||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
.PHONY: all |
||||
all: spinner |
||||
|
||||
spinner: $(OBJS) |
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
-s \
|
||||
$(FRAMEBUFFER_LIBS) \
|
||||
-L/system/vendor/lib64 \
|
||||
$(OPENGL_LIBS) \
|
||||
-lm -llog
|
||||
|
||||
../../common/framebuffer.o: ../../common/framebuffer.cc |
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-I$(PHONELIBS)/android_frameworks_native/include \
|
||||
-I$(PHONELIBS)/android_system_core/include \
|
||||
-I$(PHONELIBS)/android_hardware_libhardware/include \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
opensans_semibold.o: ../../assets/fonts/opensans_semibold.ttf |
||||
@echo "[ bin2o ] $@"
|
||||
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
|
||||
|
||||
img_spinner_track.o: ../../assets/img_spinner_track.png |
||||
@echo "[ bin2o ] $@"
|
||||
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
|
||||
|
||||
img_spinner_comma.o: ../../assets/img_spinner_comma.png |
||||
@echo "[ bin2o ] $@"
|
||||
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
|
||||
|
||||
%.o: %.c |
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
-I../.. \
|
||||
$(NANOVG_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
.PHONY: clean |
||||
clean: |
||||
rm -f spinner $(OBJS) $(DEPS)
|
||||
|
||||
-include $(DEPS) |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:7a4f45d19c1d85792e4c48a60f7d9beb0264ce187ff5edccf8532fda37fe831d |
||||
size 239976 |
||||
oid sha256:ca4dc718c959f0c5dc3d99bc8844ebe9c4366e7ab182144ad62bd7576b35b104 |
||||
size 518368 |
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue