parent
b08fff33a0
commit
6051061ff8
|
@ -17,7 +17,7 @@ repos:
|
||||||
- id: flake8
|
- id: flake8
|
||||||
exclude: '^(pyextra)|(external)|(cereal)|(rednose)|(panda)|(laika)|(laika_repo)|(rednose_repo)/'
|
exclude: '^(pyextra)|(external)|(cereal)|(rednose)|(panda)|(laika)|(laika_repo)|(rednose_repo)/'
|
||||||
args:
|
args:
|
||||||
- --ignore=E111,E114,E121,E122,E123,E124,E126,E127,E128,E201,E202,E203,E221,E225,E226,E231,E241,E251,E261,E265,E266,E302,E303,E305,E402,E501,E502,E722,E741,W504
|
- --ignore=E111,E114,E121,E122,E123,E124,E126,E127,E128,E201,E202,E203,E226,E231,E241,E251,E261,E265,E266,E302,E303,E305,E402,E501,E502,E722,E741,W504
|
||||||
- --statistics
|
- --statistics
|
||||||
- repo: local
|
- repo: local
|
||||||
hooks:
|
hooks:
|
||||||
|
|
|
@ -64,7 +64,7 @@ class RunningStatFilter():
|
||||||
_std_last = self.raw_stat.std()
|
_std_last = self.raw_stat.std()
|
||||||
self.raw_stat.push_data(new_data)
|
self.raw_stat.push_data(new_data)
|
||||||
_delta_std = self.raw_stat.std() - _std_last
|
_delta_std = self.raw_stat.std() - _std_last
|
||||||
if _delta_std<=0:
|
if _delta_std <= 0:
|
||||||
self.filtered_stat.push_data(new_data)
|
self.filtered_stat.push_data(new_data)
|
||||||
else:
|
else:
|
||||||
pass
|
pass
|
||||||
|
|
|
@ -124,8 +124,8 @@ def rot2euler(rots):
|
||||||
quats_from_rotations = rot2quat
|
quats_from_rotations = rot2quat
|
||||||
quat_from_rot = rot2quat
|
quat_from_rot = rot2quat
|
||||||
rotations_from_quats = quat2rot
|
rotations_from_quats = quat2rot
|
||||||
rot_from_quat= quat2rot
|
rot_from_quat = quat2rot
|
||||||
rot_from_quat= quat2rot
|
rot_from_quat = quat2rot
|
||||||
euler_from_rot = rot2euler
|
euler_from_rot = rot2euler
|
||||||
euler_from_quat = quat2euler
|
euler_from_quat = quat2euler
|
||||||
rot_from_euler = euler2rot
|
rot_from_euler = euler2rot
|
||||||
|
|
2
opendbc
2
opendbc
|
@ -1 +1 @@
|
||||||
Subproject commit b15edbc1b5a68fd725ea45ba9442a6c9be875971
|
Subproject commit 73685b609d25cfc8f838d53f69cf78e136c612c2
|
|
@ -6,7 +6,7 @@ from PIL import ImageFont, ImageDraw, Image
|
||||||
font = ImageFont.truetype("arial", size=72)
|
font = ImageFont.truetype("arial", size=72)
|
||||||
def get_frame(idx):
|
def get_frame(idx):
|
||||||
img = np.zeros((874, 1164, 3), np.uint8)
|
img = np.zeros((874, 1164, 3), np.uint8)
|
||||||
img[100:400, 100:100+(idx % 10)* 100] = 255
|
img[100:400, 100:100+(idx % 10) * 100] = 255
|
||||||
|
|
||||||
# big number
|
# big number
|
||||||
im2 = Image.new("RGB", (200,200))
|
im2 = Image.new("RGB", (200,200))
|
||||||
|
|
|
@ -16,7 +16,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_mo
|
||||||
lines = 1
|
lines = 1
|
||||||
alerts = 0
|
alerts = 0
|
||||||
|
|
||||||
if hud_count < (1 *4): # first 3 seconds, 4Hz
|
if hud_count < (1 * 4): # first 3 seconds, 4Hz
|
||||||
alerts = 1
|
alerts = 1
|
||||||
# CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID
|
# CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID
|
||||||
# had color = 1 and lines = 1 but trying 2017 hybrid style for now.
|
# had color = 1 and lines = 1 but trying 2017 hybrid style for now.
|
||||||
|
|
|
@ -60,7 +60,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
# events
|
# events
|
||||||
events = self.create_common_events(ret)
|
events = self.create_common_events(ret)
|
||||||
|
|
||||||
if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled:
|
if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled:
|
||||||
events.add(car.CarEvent.EventName.steerTempUnavailableMute)
|
events.add(car.CarEvent.EventName.steerTempUnavailableMute)
|
||||||
|
|
||||||
ret.events = events.to_msg()
|
ret.events = events.to_msg()
|
||||||
|
|
|
@ -50,7 +50,7 @@ class RadarInterface(RadarInterfaceBase):
|
||||||
if cpt['X_Rel'] > 0.00001:
|
if cpt['X_Rel'] > 0.00001:
|
||||||
self.validCnt[ii] += 1
|
self.validCnt[ii] += 1
|
||||||
else:
|
else:
|
||||||
self.validCnt[ii] = max(self.validCnt[ii] -1, 0)
|
self.validCnt[ii] = max(self.validCnt[ii] - 1, 0)
|
||||||
#print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
|
#print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
|
||||||
|
|
||||||
# radar point only valid if there have been enough valid measurements
|
# radar point only valid if there have been enough valid measurements
|
||||||
|
|
|
@ -28,7 +28,7 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
|
||||||
}
|
}
|
||||||
|
|
||||||
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
|
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
|
||||||
values["GasRegenChecksum"] = (((0xff -dat[1]) & 0xff) << 16) | \
|
values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \
|
||||||
(((0xff - dat[2]) & 0xff) << 8) | \
|
(((0xff - dat[2]) & 0xff) << 8) | \
|
||||||
((0x100 - dat[3] - idx) & 0xff)
|
((0x100 - dat[3] - idx) & 0xff)
|
||||||
|
|
||||||
|
|
|
@ -11,24 +11,24 @@ class CAR:
|
||||||
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
|
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
|
||||||
|
|
||||||
class CruiseButtons:
|
class CruiseButtons:
|
||||||
INIT = 0
|
INIT = 0
|
||||||
UNPRESS = 1
|
UNPRESS = 1
|
||||||
RES_ACCEL = 2
|
RES_ACCEL = 2
|
||||||
DECEL_SET = 3
|
DECEL_SET = 3
|
||||||
MAIN = 5
|
MAIN = 5
|
||||||
CANCEL = 6
|
CANCEL = 6
|
||||||
|
|
||||||
class AccState:
|
class AccState:
|
||||||
OFF = 0
|
OFF = 0
|
||||||
ACTIVE = 1
|
ACTIVE = 1
|
||||||
FAULTED = 3
|
FAULTED = 3
|
||||||
STANDSTILL = 4
|
STANDSTILL = 4
|
||||||
|
|
||||||
class CanBus:
|
class CanBus:
|
||||||
POWERTRAIN = 0
|
POWERTRAIN = 0
|
||||||
OBSTACLE = 1
|
OBSTACLE = 1
|
||||||
CHASSIS = 2
|
CHASSIS = 2
|
||||||
SW_GMLAN = 3
|
SW_GMLAN = 3
|
||||||
|
|
||||||
def is_eps_status_ok(eps_status, car_fingerprint):
|
def is_eps_status_ok(eps_status, car_fingerprint):
|
||||||
return eps_status in [0, 1]
|
return eps_status in [0, 1]
|
||||||
|
|
|
@ -6,10 +6,10 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||||
|
|
||||||
# Car button codes
|
# Car button codes
|
||||||
class CruiseButtons:
|
class CruiseButtons:
|
||||||
RES_ACCEL = 4
|
RES_ACCEL = 4
|
||||||
DECEL_SET = 3
|
DECEL_SET = 3
|
||||||
CANCEL = 2
|
CANCEL = 2
|
||||||
MAIN = 1
|
MAIN = 1
|
||||||
|
|
||||||
# See dbc files for info on values"
|
# See dbc files for info on values"
|
||||||
VISUAL_HUD = {
|
VISUAL_HUD = {
|
||||||
|
|
|
@ -9,22 +9,22 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
|
||||||
|
|
||||||
b1 = int(lkas["BIT_1"])
|
b1 = int(lkas["BIT_1"])
|
||||||
ldw = int(lkas["LDW"])
|
ldw = int(lkas["LDW"])
|
||||||
er1= int(lkas["ERR_BIT_1"])
|
er1 = int(lkas["ERR_BIT_1"])
|
||||||
lnv = 0
|
lnv = 0
|
||||||
er2= int(lkas["ERR_BIT_2"])
|
er2 = int(lkas["ERR_BIT_2"])
|
||||||
|
|
||||||
steering_angle = int(lkas["STEERING_ANGLE"])
|
steering_angle = int(lkas["STEERING_ANGLE"])
|
||||||
b2 = int(lkas["ANGLE_ENABLED"])
|
b2 = int(lkas["ANGLE_ENABLED"])
|
||||||
|
|
||||||
tmp = steering_angle + 2048
|
tmp = steering_angle + 2048
|
||||||
ahi = tmp >> 10
|
ahi = tmp >> 10
|
||||||
amd = (tmp & 0x3FF) >> 2
|
amd = (tmp & 0x3FF) >> 2
|
||||||
amd = (amd >> 4) | (( amd & 0xF) << 4)
|
amd = (amd >> 4) | (( amd & 0xF) << 4)
|
||||||
alo = (tmp & 0x3) << 2
|
alo = (tmp & 0x3) << 2
|
||||||
|
|
||||||
ctr = frame % 16
|
ctr = frame % 16
|
||||||
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
|
# bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ]
|
||||||
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
|
csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5)
|
||||||
|
|
||||||
#bytes [ 5 ] [ 6 ] [ 7 ]
|
#bytes [ 5 ] [ 6 ] [ 7 ]
|
||||||
csum = csum - ahi - amd - alo - b2
|
csum = csum - ahi - amd - alo - b2
|
||||||
|
|
|
@ -78,12 +78,12 @@ class RadarInterface(RadarInterfaceBase):
|
||||||
if ii in self.RADAR_A_MSGS:
|
if ii in self.RADAR_A_MSGS:
|
||||||
cpt = self.rcp.vl[ii]
|
cpt = self.rcp.vl[ii]
|
||||||
|
|
||||||
if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']:
|
if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
|
||||||
self.valid_cnt[ii] = 0 # reset counter
|
self.valid_cnt[ii] = 0 # reset counter
|
||||||
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
|
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
|
||||||
self.valid_cnt[ii] += 1
|
self.valid_cnt[ii] += 1
|
||||||
else:
|
else:
|
||||||
self.valid_cnt[ii] = max(self.valid_cnt[ii] -1, 0)
|
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
|
||||||
|
|
||||||
score = self.rcp.vl[ii+16]['SCORE']
|
score = self.rcp.vl[ii+16]['SCORE']
|
||||||
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
|
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
|
||||||
|
|
|
@ -25,5 +25,5 @@ class UIParams:
|
||||||
lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1
|
lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1
|
||||||
car_hwidth = 1.7272/2 * lidar_zoom
|
car_hwidth = 1.7272/2 * lidar_zoom
|
||||||
car_front = 2.6924 * lidar_zoom
|
car_front = 2.6924 * lidar_zoom
|
||||||
car_back = 1.8796 * lidar_zoom
|
car_back = 1.8796 * lidar_zoom
|
||||||
car_color = 110
|
car_color = 110
|
||||||
|
|
|
@ -189,8 +189,8 @@ class DriverStatus():
|
||||||
# self.pose.roll_std = driver_state.faceOrientationStd[2]
|
# self.pose.roll_std = driver_state.faceOrientationStd[2]
|
||||||
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
|
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
|
||||||
self.pose.low_std = model_std_max < _POSESTD_THRESHOLD
|
self.pose.low_std = model_std_max < _POSESTD_THRESHOLD
|
||||||
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb>_EYE_THRESHOLD)
|
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.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD)
|
||||||
self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \
|
self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \
|
||||||
abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45
|
abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45
|
||||||
|
|
||||||
|
@ -200,7 +200,7 @@ class DriverStatus():
|
||||||
|
|
||||||
# update offseter
|
# update offseter
|
||||||
# only update when driver is actively driving the car above a certain speed
|
# only update when driver is actively driving the car above a certain speed
|
||||||
if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
|
if self.face_detected and car_speed > _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.pitch_offseter.push_and_update(self.pose.pitch)
|
||||||
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
|
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
|
||||||
|
|
||||||
|
|
|
@ -116,7 +116,7 @@ class LatControlINDI():
|
||||||
indi_log.delta = float(delta_u)
|
indi_log.delta = float(delta_u)
|
||||||
indi_log.output = float(self.output_steer)
|
indi_log.output = float(self.output_steer)
|
||||||
|
|
||||||
check_saturation = (CS.vEgo> 10.) and not CS.steeringRateLimited and not CS.steeringPressed
|
check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed
|
||||||
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
|
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
|
||||||
|
|
||||||
return float(self.output_steer), float(self.angle_steers_des), indi_log
|
return float(self.output_steer), float(self.angle_steers_des), indi_log
|
||||||
|
|
|
@ -23,7 +23,7 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract
|
||||||
|
|
||||||
# lookup tables VS speed to determine min and max accels in cruise
|
# lookup tables VS speed to determine min and max accels in cruise
|
||||||
# make sure these accelerations are smaller than mpc limits
|
# make sure these accelerations are smaller than mpc limits
|
||||||
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
|
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
|
||||||
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
||||||
|
|
||||||
# need fast accel at very low speed for stop and go
|
# need fast accel at very low speed for stop and go
|
||||||
|
|
|
@ -77,29 +77,29 @@ class TestMonitoring(unittest.TestCase):
|
||||||
# 0. op engaged, driver is doing fine all the time
|
# 0. op engaged, driver is doing fine all the time
|
||||||
def test_fully_aware_driver(self):
|
def test_fully_aware_driver(self):
|
||||||
events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)[0]
|
events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)[0]
|
||||||
self.assertTrue(np.sum([len(event) for event in events_output])==0)
|
self.assertTrue(np.sum([len(event) for event in events_output]) == 0)
|
||||||
|
|
||||||
# 1. op engaged, driver is distracted and does nothing
|
# 1. op engaged, driver is distracted and does nothing
|
||||||
def test_fully_distracted_driver(self):
|
def test_fully_distracted_driver(self):
|
||||||
events_output, d_status = run_DState_seq(always_distracted, always_false, always_true, always_false)
|
events_output, d_status = 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.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+\
|
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)].names[0], EventName.preDriverDistracted)
|
((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.preDriverDistracted)
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\
|
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL +\
|
||||||
((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverDistracted)
|
((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverDistracted)
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_TIME+\
|
self.assertEqual(events_output[int((_DISTRACTED_TIME +\
|
||||||
((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)].names[0], EventName.driverDistracted)
|
((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)].names[0], EventName.driverDistracted)
|
||||||
self.assertIs(type(d_status.awareness), float)
|
self.assertIs(type(d_status.awareness), float)
|
||||||
|
|
||||||
# 2. op engaged, no face detected the whole time, no action
|
# 2. op engaged, no face detected the whole time, no action
|
||||||
def test_fully_invisible_driver(self):
|
def test_fully_invisible_driver(self):
|
||||||
events_output = run_DState_seq(always_no_face, always_false, always_true, always_false)[0]
|
events_output = run_DState_seq(always_no_face, always_false, always_true, always_false)[0]
|
||||||
self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
|
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+\
|
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)].names[0], EventName.preDriverUnresponsive)
|
((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.preDriverUnresponsive)
|
||||||
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\
|
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL +\
|
||||||
((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
||||||
self.assertEqual(events_output[int((_AWARENESS_TIME+\
|
self.assertEqual(events_output[int((_AWARENESS_TIME +\
|
||||||
((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)].names[0], EventName.driverUnresponsive)
|
((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)].names[0], EventName.driverUnresponsive)
|
||||||
|
|
||||||
# 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
|
# 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
|
||||||
|
@ -111,11 +111,11 @@ class TestMonitoring(unittest.TestCase):
|
||||||
interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/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))
|
[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)[0]
|
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)[0]
|
||||||
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
|
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)].names[0], EventName.promptDriverDistracted)
|
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted)
|
||||||
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0)
|
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)].names[0], EventName.promptDriverDistracted)
|
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0], EventName.promptDriverDistracted)
|
||||||
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0)
|
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, \
|
# 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
|
# driver dodges, and then touches wheel to no avail, disengages and reengages
|
||||||
|
@ -133,7 +133,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0], EventName.promptDriverDistracted)
|
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0], EventName.promptDriverDistracted)
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0], EventName.driverDistracted)
|
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0], EventName.driverDistracted)
|
||||||
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0], EventName.driverDistracted)
|
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0], EventName.driverDistracted)
|
||||||
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0)
|
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 touches wheel; then down to orange again, driver appears
|
# 5. op engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
|
||||||
# - both actions should clear the alert, but momentary appearence should not
|
# - both actions should clear the alert, but momentary appearence should not
|
||||||
|
@ -145,15 +145,15 @@ class TestMonitoring(unittest.TestCase):
|
||||||
ds_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
|
ds_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
|
||||||
interaction_vector[int((_INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
|
interaction_vector[int((_INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
|
||||||
events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)[0]
|
events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)[0]
|
||||||
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
|
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)].names[0], EventName.promptDriverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
||||||
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)])==0)
|
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0)
|
||||||
if _visible_time == 1:
|
if _visible_time == 1:
|
||||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
||||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0], EventName.preDriverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0], EventName.preDriverUnresponsive)
|
||||||
elif _visible_time == 10:
|
elif _visible_time == 10:
|
||||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
||||||
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)])==0)
|
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0)
|
||||||
else:
|
else:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
@ -168,18 +168,18 @@ class TestMonitoring(unittest.TestCase):
|
||||||
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)
|
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)
|
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)[0]
|
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)[0]
|
||||||
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
|
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)].names[0], EventName.promptDriverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0], EventName.promptDriverUnresponsive)
|
||||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0], EventName.driverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0], EventName.driverUnresponsive)
|
||||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0], EventName.driverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0], EventName.driverUnresponsive)
|
||||||
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0], EventName.driverUnresponsive)
|
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0], EventName.driverUnresponsive)
|
||||||
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0)
|
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
|
# 7. op not engaged, always distracted driver
|
||||||
# - dm should stay quiet when not engaged
|
# - dm should stay quiet when not engaged
|
||||||
def test_pure_dashcam_user(self):
|
def test_pure_dashcam_user(self):
|
||||||
events_output = run_DState_seq(always_distracted, always_false, always_false, always_false)[0]
|
events_output = run_DState_seq(always_distracted, always_false, always_false, always_false)[0]
|
||||||
self.assertTrue(np.sum([len(event) for event in events_output])==0)
|
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
|
# 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
|
# - should only reach green when stopped, but continues counting down on launch
|
||||||
|
@ -201,9 +201,9 @@ class TestMonitoring(unittest.TestCase):
|
||||||
[msg_DISTRACTED_UNCERTAIN] * (int(_TEST_TIMESPAN/DT_DMON)-int((_DISTRACTED_SECONDS_TO_ORANGE+_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON))
|
[msg_DISTRACTED_UNCERTAIN] * (int(_TEST_TIMESPAN/DT_DMON)-int((_DISTRACTED_SECONDS_TO_ORANGE+_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON))
|
||||||
interaction_vector = always_false[:]
|
interaction_vector = always_false[:]
|
||||||
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)[0]
|
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)[0]
|
||||||
self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)])==0)
|
self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)]) == 0)
|
||||||
self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN-0.1)/DT_DMON)].names[0], EventName.driverMonitorLowAcc)
|
self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN-0.1)/DT_DMON)].names[0], EventName.driverMonitorLowAcc)
|
||||||
self.assertTrue(len(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN+_DISTRACTED_SECONDS_TO_ORANGE-0.5)/DT_DMON)])==0)
|
self.assertTrue(len(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN+_DISTRACTED_SECONDS_TO_ORANGE-0.5)/DT_DMON)]) == 0)
|
||||||
self.assertEqual(events_output[int((_TEST_TIMESPAN-5.)/DT_DMON)].names[0], EventName.driverMonitorLowAcc)
|
self.assertEqual(events_output[int((_TEST_TIMESPAN-5.)/DT_DMON)].names[0], EventName.driverMonitorLowAcc)
|
||||||
|
|
||||||
# 10. op engaged, model is somehow uncertain and driver is distracted
|
# 10. op engaged, model is somehow uncertain and driver is distracted
|
||||||
|
@ -212,7 +212,7 @@ class TestMonitoring(unittest.TestCase):
|
||||||
ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(_TEST_TIMESPAN/DT_DMON)
|
ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(_TEST_TIMESPAN/DT_DMON)
|
||||||
interaction_vector = always_false[:]
|
interaction_vector = always_false[:]
|
||||||
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)[0]
|
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)[0]
|
||||||
self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)])==0)
|
self.assertTrue(len(events_output[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)]) == 0)
|
||||||
self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON)].names[0], EventName.driverMonitorLowAcc)
|
self.assertEqual(events_output[int((_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON)].names[0], EventName.driverMonitorLowAcc)
|
||||||
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.preDriverDistracted)
|
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.preDriverDistracted)
|
||||||
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.promptDriverDistracted)
|
self.assertEqual(events_output[int((2.5*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)].names[1], EventName.promptDriverDistracted)
|
||||||
|
|
|
@ -81,10 +81,10 @@ def mpc_vwr_thread(addr="127.0.0.1"):
|
||||||
lineP.set_ydata(path_x)
|
lineP.set_ydata(path_x)
|
||||||
|
|
||||||
if lMpc is not None:
|
if lMpc is not None:
|
||||||
mpc_path_x = list(lMpc.liveMpc.x)[1:]
|
mpc_path_x = list(lMpc.liveMpc.x)[1:]
|
||||||
mpc_path_y = list(lMpc.liveMpc.y)[1:]
|
mpc_path_y = list(lMpc.liveMpc.y)[1:]
|
||||||
mpc_steer_angle = list(lMpc.liveMpc.delta)[1:]
|
mpc_steer_angle = list(lMpc.liveMpc.delta)[1:]
|
||||||
mpc_psi = list(lMpc.liveMpc.psi)[1:]
|
mpc_psi = list(lMpc.liveMpc.psi)[1:]
|
||||||
|
|
||||||
line1.set_xdata(mpc_path_y)
|
line1.set_xdata(mpc_path_y)
|
||||||
line1.set_ydata(mpc_path_x)
|
line1.set_ydata(mpc_path_x)
|
||||||
|
|
|
@ -163,7 +163,7 @@ def gen_nav_data(msg, nav_frame_buffer):
|
||||||
|
|
||||||
# parse GPS ephem
|
# parse GPS ephem
|
||||||
gnssId = msg_meta_data['gnssId']
|
gnssId = msg_meta_data['gnssId']
|
||||||
if gnssId == 0:
|
if gnssId == 0:
|
||||||
svId = msg_meta_data['svid']
|
svId = msg_meta_data['svid']
|
||||||
subframeId = GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
|
subframeId = GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
|
||||||
words = []
|
words = []
|
||||||
|
|
|
@ -163,7 +163,7 @@ class Plant():
|
||||||
Plant.live_params.close()
|
Plant.live_params.close()
|
||||||
|
|
||||||
def speed_sensor(self, speed):
|
def speed_sensor(self, speed):
|
||||||
if speed<0.3:
|
if speed < 0.3:
|
||||||
return 0
|
return 0
|
||||||
else:
|
else:
|
||||||
return speed * CV.MS_TO_KPH
|
return speed * CV.MS_TO_KPH
|
||||||
|
|
|
@ -190,7 +190,7 @@ def calibration_rcv_callback(msg, CP, cfg, fsm):
|
||||||
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
|
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
|
||||||
# should_recv always true to increment frame
|
# should_recv always true to increment frame
|
||||||
if msg.which() == 'carState':
|
if msg.which() == 'carState':
|
||||||
if ((fsm.frame +1)% 25) == 0:
|
if ((fsm.frame + 1) % 25) == 0:
|
||||||
recv_socks = ["liveCalibration"]
|
recv_socks = ["liveCalibration"]
|
||||||
else:
|
else:
|
||||||
recv_socks = []
|
recv_socks = []
|
||||||
|
|
|
@ -2,8 +2,8 @@
|
||||||
import pygame # pylint: disable=import-error
|
import pygame # pylint: disable=import-error
|
||||||
|
|
||||||
# Define some colors
|
# Define some colors
|
||||||
BLACK = ( 0, 0, 0)
|
BLACK = ( 0, 0, 0)
|
||||||
WHITE = ( 255, 255, 255)
|
WHITE = ( 255, 255, 255)
|
||||||
|
|
||||||
# This is a simple class that will help us print to the screen
|
# This is a simple class that will help us print to the screen
|
||||||
# It has nothing to do with the joysticks, just outputting the
|
# It has nothing to do with the joysticks, just outputting the
|
||||||
|
@ -55,7 +55,7 @@ while not done:
|
||||||
# EVENT PROCESSING STEP
|
# EVENT PROCESSING STEP
|
||||||
for event in pygame.event.get(): # User did something
|
for event in pygame.event.get(): # User did something
|
||||||
if event.type == pygame.QUIT: # If user clicked close
|
if event.type == pygame.QUIT: # If user clicked close
|
||||||
done=True # Flag that we are done so we exit this loop
|
done = True # Flag that we are done so we exit this loop
|
||||||
|
|
||||||
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
||||||
if event.type == pygame.JOYBUTTONDOWN:
|
if event.type == pygame.JOYBUTTONDOWN:
|
||||||
|
|
|
@ -312,19 +312,19 @@ def keyboard_controller_thread(q, route_start_time):
|
||||||
kb = KBHit()
|
kb = KBHit()
|
||||||
while 1:
|
while 1:
|
||||||
c = kb.getch()
|
c = kb.getch()
|
||||||
if c=='m': # Move forward by 1m
|
if c == 'm': # Move forward by 1m
|
||||||
q.send_pyobj(SeekRelativeTime(60))
|
q.send_pyobj(SeekRelativeTime(60))
|
||||||
elif c=='M': # Move backward by 1m
|
elif c == 'M': # Move backward by 1m
|
||||||
q.send_pyobj(SeekRelativeTime(-60))
|
q.send_pyobj(SeekRelativeTime(-60))
|
||||||
elif c=='s': # Move forward by 10s
|
elif c == 's': # Move forward by 10s
|
||||||
q.send_pyobj(SeekRelativeTime(10))
|
q.send_pyobj(SeekRelativeTime(10))
|
||||||
elif c=='S': # Move backward by 10s
|
elif c == 'S': # Move backward by 10s
|
||||||
q.send_pyobj(SeekRelativeTime(-10))
|
q.send_pyobj(SeekRelativeTime(-10))
|
||||||
elif c=='G': # Move backward by 10s
|
elif c == 'G': # Move backward by 10s
|
||||||
q.send_pyobj(SeekAbsoluteTime(0.))
|
q.send_pyobj(SeekAbsoluteTime(0.))
|
||||||
elif c=="\x20": # Space bar.
|
elif c == "\x20": # Space bar.
|
||||||
q.send_pyobj(TogglePause())
|
q.send_pyobj(TogglePause())
|
||||||
elif c=="\n":
|
elif c == "\n":
|
||||||
try:
|
try:
|
||||||
seek_time_input = input('time: ')
|
seek_time_input = input('time: ')
|
||||||
seek_time = absolute_time_str(seek_time_input, route_start_time)
|
seek_time = absolute_time_str(seek_time_input, route_start_time)
|
||||||
|
|
Loading…
Reference in New Issue