def data_sample(CI, CC, plan_sock, path_plan_sock, calibration, poller,
cal_status, cal_perc, state, plan, path_plan,
cruise_speed_kph):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# receive the values
updateInternalCS(CI.CS, plan.plan.vTarget, path_plan.pathPlan.angleSteers,
0, cruise_speed_kph)
CS = returnNewCS(CI)
events = list(CS.events)
# Receive from sockets
cal = None
for socket, event in poller.poll(0):
if socket is calibration:
cal = messaging.recv_one(socket)
elif socket is plan_sock:
plan = messaging.recv_one(socket)
elif socket is path_plan_sock:
path_plan = messaging.recv_one(socket)
# Handle calibration
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
# if cal_status != Calibration.CALIBRATED:
# if cal_status == Calibration.UNCALIBRATED:
# events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
# else:
# events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
return CS, events, cal_status, cal_perc, plan, path_plan
def data_sample(CI, CC, thermal, calibration, health, poller, cal_status,
overtemp, free_space):
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
td = None
cal = None
hh = None
for socket, event in poller.poll(0):
if socket is thermal:
td = messaging.recv_one(socket)
elif socket is calibration:
cal = messaging.recv_one(socket)
elif socket is health:
hh = messaging.recv_one(socket)
# *** thermal checking logic ***
# thermal data, checked every second
if td is not None:
# CPU overtemp above 95 deg
overtemp_proc = any(t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1,
td.thermal.cpu2, td.thermal.cpu3,
td.thermal.mem, td.thermal.gpu))
overtemp_bat = td.thermal.bat > 60000 # 60c
overtemp = overtemp_proc or overtemp_bat
# under 15% of space free no enable allowed
free_space = td.thermal.freeSpace < 0.15
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# *** read calibration status ***
if cal is not None:
cal_status = cal.liveCalibration.calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationInProgress',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
# *** health checking logic ***
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed:
events.append(
create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
return CS, events, cal_status, overtemp, free_space
def getMessage(service=None, timeout=1000):
if service is None or service not in service_list:
raise Exception("invalid service")
socket = messaging.sub_sock(service_list[service].port)
socket.setsockopt(zmq.RCVTIMEO, timeout)
ret = messaging.recv_one(socket)
return ret.to_dict()
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
context = zmq.Context()
live100 = messaging.sub_sock(context,
service_list['live100'].port,
addr=addr,
conflate=True)
orbfeatures = messaging.sub_sock(context,
service_list['orbFeatures'].port,
addr=addr,
conflate=True)
livecalibration = messaging.pub_sock(context,
service_list['liveCalibration'].port)
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
of = messaging.recv_one(orbfeatures)
l100 = messaging.recv_one_or_none(live100)
new_vp = calibrator.handle_orb_features(of)
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
if l100 is not None:
calibrator.handle_live100(l100)
if DEBUG:
calibrator.handle_debug()
calibrator.send_data(livecalibration)
def getMessage(service=None, timeout=1000):
if service is None or service not in service_list:
raise Exception("invalid service")
socket = messaging.sub_sock(service)
socket.setTimeout(timeout)
ret = messaging.recv_one(socket)
return ret.to_dict()
def get_one_can(logcan):
while True:
try:
can = messaging.recv_one(logcan)
if len(can.can) > 0:
return can
except zmq.error.Again:
continue
def main(rate=200):
set_realtime_priority(3)
context = zmq.Context()
poller = zmq.Poller()
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
vEgo = 0.0
_live100 = None
frame_count = 0
skipped_count = 0
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
# simple version for working with CWD
#print len([name for name in os.listdir('.') if os.path.isfile(name)])
# path joining version for other paths
DIR = '/sdcard/tuning'
filenumber = len([name for name in os.listdir(DIR) if os.path.isfile(os.path.join(DIR, name))])
print("start")
with open(DIR + '/dashboard_file_%d.csv' % filenumber, mode='w') as dash_file:
print("opened")
dash_writer = csv.writer(dash_file, delimiter=',', quotechar='', quoting=csv.QUOTE_NONE)
print("initialized")
dash_writer.writerow(['angleSteersDes','angleSteers','vEgo','steerOverride','upSteer','uiSteer','ufSteer','time'])
print("first row")
while 1:
receiveTime = int(time.time() * 1000)
for socket, event in poller.poll(0):
if socket is live100:
_live100 = messaging.recv_one(socket)
vEgo = _live100.live100.vEgo
if vEgo >= 0:
frame_count += 1
dash_writer.writerow([str(round(_live100.live100.angleSteersDes, 2)),
str(round(_live100.live100.angleSteers, 2)),
str(round(_live100.live100.vEgo, 1)),
1 if _live100.live100.steerOverride else 0,
str(round(_live100.live100.upSteer, 4)),
str(round(_live100.live100.uiSteer, 4)),
str(round(_live100.live100.ufSteer, 4)),
str(receiveTime)])
else:
skipped_count += 1
else:
skipped_count += 1
if frame_count % 200 == 0:
print("captured = %d" % frame_count)
frame_count += 1
if skipped_count % 200 == 0:
print("skipped = %d" % skipped_count)
skipped_count += 1
rk.keep_time()
def ui_thread(addr, frame_address):
context = zmq.Context.instance()
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
size = (_FULL_FRAME_SIZE[0] * SCALE, _FULL_FRAME_SIZE[1] * SCALE)
pygame.display.set_caption("comma one debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
camera_surface = pygame.surface.Surface((_FULL_FRAME_SIZE[0] * SCALE, _FULL_FRAME_SIZE[1] * SCALE), 0, 24).convert()
frame = context.socket(zmq.SUB)
frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port))
frame.setsockopt(zmq.SUBSCRIBE, "")
img = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype='uint8')
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
while 1:
list(pygame.event.get())
screen.fill((64, 64, 64))
# ***** frame *****
fpkt = recv_one(frame)
yuv_img = fpkt.frame.image
if fpkt.frame.transform:
yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3)
else:
# assume frame is flipped
yuv_transform = np.array([[-1.0, 0.0, _FULL_FRAME_SIZE[0] - 1],
[0.0, -1.0, _FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]])
if yuv_img and len(yuv_img) == _FULL_FRAME_SIZE[0] * _FULL_FRAME_SIZE[1] * 3 // 2:
yuv_np = np.frombuffer(
yuv_img, dtype=np.uint8).reshape(_FULL_FRAME_SIZE[1] * 3 // 2, -1)
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
cv2.warpAffine(
imgff,
np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]),
dst=img,
flags=cv2.WARP_INVERSE_MAP)
else:
img.fill(0)
height, width = img.shape[:2]
img_resized = cv2.resize(
img, (SCALE * width, SCALE * height), interpolation=cv2.INTER_CUBIC)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1))
screen.blit(camera_surface, (0, 0))
# this takes time...vsync or something
pygame.display.flip()
def ASdata_sample(plan_sock, path_plan_sock, calibration, poller, cal_status,
cal_perc, state, plan, path_plan):
# Receive from sockets
cal = None
recv_plan = False
recv_pathplan = False
while not (recv_plan and recv_pathplan):
for socket, event in poller.poll(0):
if socket is calibration:
cal = messaging.recv_one(socket)
elif socket is plan_sock:
plan = messaging.recv_one(socket)
recv_plan = True
elif socket is path_plan_sock:
path_plan = messaging.recv_one(socket)
recv_pathplan = True
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
return cal_status, cal_perc, plan, path_plan
def replay_process(cfg, lr):
gc.disable() # gc can occasionally cause canparser to timeout
pub_socks, sub_socks = {}, {}
for pub, sub in cfg.pub_sub.iteritems():
pub_socks[pub] = messaging.pub_sock(service_list[pub].port)
for s in sub:
sub_socks[s] = messaging.sub_sock(service_list[s].port)
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = filter(lambda msg: msg.which() in pub_socks.keys(), all_msgs)
params = Params()
params.manager_start()
params.put("Passive", "0")
manager.gctx = {}
manager.prepare_managed_process(cfg.proc_name)
manager.start_managed_process(cfg.proc_name)
time.sleep(3) # Wait for started process to be ready
if cfg.init_callback is not None:
cfg.init_callback(all_msgs, pub_socks, sub_socks)
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
log_msgs = []
for msg in tqdm(pub_msgs):
if cfg.should_recv_callback is not None:
recv_socks = cfg.should_recv_callback(msg, CP)
else:
recv_socks = cfg.pub_sub[msg.which()]
pub_socks[msg.which()].send(msg.as_builder().to_bytes())
if len(recv_socks):
# TODO: add timeout
for sock in recv_socks:
m = messaging.recv_one(sub_socks[sock])
# make these values fixed for faster comparison
m_builder = m.as_builder()
m_builder.logMonoTime = 0
m_builder.valid = True
log_msgs.append(m_builder.as_reader())
gc.enable()
manager.kill_managed_process(cfg.proc_name)
return log_msgs
def plannerd_thread():
context = zmq.Context()
params = Params()
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
PL = Planner(CP, fcw_enabled)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
poller = zmq.Poller()
car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller)
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
live100 = messaging.new_message()
live100.init('live100')
model = messaging.new_message()
model.init('model')
live20 = messaging.new_message()
live20.init('live20')
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
live_parameters = messaging.new_message()
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
while True:
for socket, event in poller.poll():
if socket is live100_sock:
live100 = messaging.recv_one(socket)
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
elif socket is live_parameters_sock:
live_parameters = messaging.recv_one(socket)
elif socket is model_sock:
model = messaging.recv_one(socket)
PP.update(CP, VM, car_state, model, live100, live_parameters)
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
elif socket is live20_sock:
live20 = messaging.recv_one(socket)
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
def get_vin(logcan, sendcan, bus, query_time=1.):
vin_query = VinQuery(bus)
frame = 0
# 1s max of VIN query time
while frame < query_time * 100:
a = messaging.recv_one(logcan)
for can in a.can:
vin_query.check_response(can)
vin_query.send_query(sendcan)
frame += 1
return vin_query.get_vin()
def update_acc(self, enabled, CS, frame, actuators, pcm_speed,
speed_limit_kph, speed_limit_valid, set_speed_limit_active,
speed_limit_offset):
# Adaptive cruise control
current_time_ms = _current_time_millis()
if CruiseButtons.should_be_throttled(CS.cruise_buttons):
self.human_cruise_action_time = current_time_ms
button_to_press = None
# If ACC is disabled, disengage traditional cruise control.
if (self.prev_enable_adaptive_cruise
and not self.enable_adaptive_cruise
and CS.pcm_acc_status == CruiseState.ENABLED):
button_to_press = CruiseButtons.CANCEL
if self.enable_adaptive_cruise and enabled:
if CS.cstm_btns.get_button_label2(
ACCMode.BUTTON_NAME) in ["OP", "AutoOP"]:
button_to_press = self._calc_button(CS, pcm_speed)
self.new_speed = pcm_speed * CV.MS_TO_KPH
else:
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the Live20 feed
lead_1 = None
if enabled:
for socket, _ in self.poller.poll(0):
if socket is self.live20:
lead_1 = messaging.recv_one(socket).live20.leadOne
if lead_1.dRel:
self.lead_last_seen_time_ms = current_time_ms
button_to_press = self._calc_follow_button(
CS, lead_1, speed_limit_kph, speed_limit_valid,
set_speed_limit_active, speed_limit_offset)
if button_to_press:
self.automated_cruise_action_time = current_time_ms
# If trying to slow below the min cruise speed, just cancel cruise.
# This prevents a SCCM crash which is triggered by repeatedly pressing
# stalk-down when already at min cruise speed.
if (CruiseButtons.is_decel(button_to_press)
and CS.v_cruise_actual - 1 <
self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH):
button_to_press = CruiseButtons.CANCEL
if button_to_press == CruiseButtons.CANCEL:
self.fast_decel_time = current_time_ms
# Debug logging (disable in production to reduce latency of commands)
#print "***ACC command: %s***" % button_to_press
return button_to_press
def send_thread(serial):
panda = Panda(serial)
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
panda.set_can_loopback(False)
can_sock = messaging.sub_sock(service_list['can'].port)
while True:
# Send messages one bus 0 and 1
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = filter(lambda x: x[-1] <= 2, snd)
panda.can_send_many(snd)
# Drain panda message buffer
panda.can_recv()
def send_thread(serial):
try:
panda = Panda(serial)
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
panda.set_can_loopback(False)
can_sock = messaging.sub_sock('can')
while True:
# Send messages one bus 0 and 1
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = list(filter(lambda x: x[-1] <= 2, snd))
panda.can_send_many(snd)
# Drain panda message buffer
panda.can_recv()
except Exception:
traceback.print_exc()
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
context = zmq.Context()
cameraodometry = messaging.sub_sock(context,
service_list['cameraOdometry'].port,
addr=addr,
conflate=True)
livecalibration = messaging.pub_sock(context,
service_list['liveCalibration'].port)
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
co = messaging.recv_one(cameraodometry)
new_vp = calibrator.handle_cam_odom(co)
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
calibrator.send_data(livecalibration)
def showLeadCarOnICCanMessage(self, radarSocket):
messages = []
leads = messaging.recv_one(radarSocket).radarState
if leads is None:
return messages
lead_1 = leads.leadOne
lead_2 = leads.leadTwo
if (lead_1 is not None) and lead_1.status:
self.leadDx = lead_1.dRel
self.leadDy = self.curv0-lead_1.yRel
self.leadId = self.icLeadsData.lead1trackId
self.leadClass = self.icLeadsData.lead1oClass
self.leadVx = lead_1.vRel
if (self.leadId <= 0) or (self.leadId == 63):
self.leadId = 61
else:
self.leadDx = 0.
self.leadDy = 0.
self.leadClass = 0
self.leadId = 0
self.leadVx = 0xF
if (lead_2 is not None) and lead_2.status:
self.lead2Dx = lead_2.dRel
self.lead2Dy = self.curv0-lead_2.yRel
self.lead2Id = self.icLeadsData.lead2trackId
self.lead2Class = self.icLeadsData.lead2oClass
self.lead2Vx = lead_2.vRel
if (self.lead2Id <= 0) or (self.lead2Id == 63):
self.leadId = 62
else:
self.lead2Dx = 0.
self.lead2Dy = 0.
self.lead2Class = 0
self.lead2Id = 0
self.lead2Vx = 0xF
messages.append(teslacan.create_DAS_LR_object_msg(0,self.leadClass, self.leadId,
self.leadDx,self.leadDy,self.leadVx,self.lead2Class,
self.lead2Id,self.lead2Dx,self.lead2Dy,self.lead2Vx))
return messages
def radard_thread(gctx=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked= CP.radarName == "mock"
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.radarName)
exec('from selfdrive.radar.'+CP.radarName+'.interface import RadarInterface')
context = zmq.Context()
# *** subscribe to features and model from visiond
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
PP = PathPlanner()
RI = RadarInterface()
last_md_ts = 0
last_l100_ts = 0
# *** publish live20 and liveTracks
live20 = messaging.pub_sock(context, service_list['live20'].port)
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
# Time-alignment
rate = 20. # model and radar are both at 20Hz
tsv = 1./rate
v_len = 20 # how many speed data points to remember for t alignment with rdr data
active = 0
steer_angle = 0.
steer_override = False
tracks = defaultdict(dict)
# Kalman filter stuff:
ekfv = EKFV1D()
speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
v_ego = None
v_ego_array = np.zeros([2, v_len])
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
for pt in rr.points:
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = None
md = None
for socket, event in poller.poll(0):
if socket is live100:
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
v_ego_array = v_ego_array[:, 1:]
last_l100_ts = l100.logMonoTime
if v_ego is None:
continue
if md is not None:
last_md_ts = md.logMonoTime
# *** get path prediction from the model ***
PP.update(v_ego, md)
# run kalman filter only if prob is high enough
if PP.lead_prob > 0.7:
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), False)
else:
ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if (active and not steer_override) or mocked:
# use path from model (always when mocking as steering is too noisy)
path_y = np.polyval(PP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
if ids not in ar_pts:
tracks.pop(ids, None)
# *** compute the tracks ***
for ids in ar_pts:
# ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT and not mocked:
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
# create the track if it doesn't exist or it's a new track
if ids not in tracks:
tracks[ids] = Track()
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override)
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
if VISION_POINT in ar_pts:
fused_id = None
best_score = NO_FUSION_SCORE
for ids in tracks:
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2)
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel)
tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
if best_score > tracks[ids].vision_score:
fused_id = ids
best_score = tracks[ids].vision_score
if fused_id is not None:
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
if DEBUG:
print "NEW CYCLE"
if VISION_POINT in ar_pts:
print "vision", ar_pts[VISION_POINT]
idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
# If we have multiple points, cluster them
if len(track_pts) > 1:
link = linkage_vector(track_pts, method='centroid')
cluster_idxs = fcluster(link, 2.5, criterion='distance')
clusters = [None]*max(cluster_idxs)
for idx in xrange(len(track_pts)):
cluster_i = cluster_idxs[idx]-1
if clusters[cluster_i] == None:
clusters[cluster_i] = Cluster()
clusters[cluster_i].add(tracks[idens[idx]])
elif len(track_pts) == 1:
# TODO: why do we need this?
clusters = [Cluster()]
clusters[0].add(tracks[idens[0]])
else:
clusters = []
if DEBUG:
for i in clusters:
print i
# *** extract the lead car ***
lead_clusters = [c for c in clusters
if c.is_potential_lead(v_ego)]
lead_clusters.sort(key=lambda x: x.dRel)
lead_len = len(lead_clusters)
# *** extract the second lead from the whole set of leads ***
lead2_clusters = [c for c in lead_clusters
if c.is_potential_lead2(lead_clusters)]
lead2_clusters.sort(key=lambda x: x.dRel)
lead2_len = len(lead2_clusters)
# *** publish live20 ***
dat = messaging.new_message()
dat.init('live20')
dat.live20.mdMonoTime = last_md_ts
dat.live20.canMonoTimes = list(rr.canMonoTimes)
dat.live20.radarErrors = list(rr.errors)
dat.live20.l100MonoTime = last_l100_ts
if lead_len > 0:
lead_clusters[0].toLive20(dat.live20.leadOne)
if lead2_len > 0:
lead2_clusters[0].toLive20(dat.live20.leadTwo)
else:
dat.live20.leadTwo.status = False
else:
dat.live20.leadOne.status = False
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary)
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
dat.liveTracks[cnt].stationary = tracks[ids].stationary
dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration,
health, driver_monitor, poller, cal_status, cal_perc, overtemp,
free_space, low_battery, driver_status, state,
mismatch_counter, params, plan, path_plan):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
CS = CI.update(CC)
events = list(CS.events)
enabled = isEnabled(state)
# Receive from sockets
td = None
cal = None
hh = None
dm = None
for socket, event in poller.poll(0):
if socket is thermal:
td = messaging.recv_one(socket)
elif socket is calibration:
cal = messaging.recv_one(socket)
elif socket is health:
hh = messaging.recv_one(socket)
elif socket is driver_monitor:
dm = messaging.recv_one(socket)
elif socket is plan_sock:
plan = messaging.recv_one(socket)
elif socket is path_plan_sock:
path_plan = messaging.recv_one(socket)
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
free_space = td.thermal.freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = td.thermal.batteryPercent < 1 # at zero percent battery, OP should not be allowed
# Create events for battery, temperature and disk space
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# Handle calibration
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket than the CAN messages, therefore one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(
create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if dm is not None:
driver_status.get_pose(dm.driverMonitoring, params)
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan
def data_sample(CI, CC, thermal, calibration, health, driver_monitor,
gps_location, poller, cal_status, cal_perc, overtemp,
free_space, low_battery, driver_status, geofence, state,
mismatch_counter, params):
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
enabled = isEnabled(state)
td = None
cal = None
hh = None
dm = None
gps = None
for socket, event in poller.poll(0):
if socket is thermal:
td = messaging.recv_one(socket)
elif socket is calibration:
cal = messaging.recv_one(socket)
elif socket is health:
hh = messaging.recv_one(socket)
elif socket is driver_monitor:
dm = messaging.recv_one(socket)
elif socket is gps_location:
gps = messaging.recv_one(socket)
# *** thermal checking logic ***
# thermal data, checked every second
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
# under 15% of space free no enable allowed
free_space = td.thermal.freeSpace < 0.15
# at zero percent battery, OP should not be allowed
low_battery = td.thermal.batteryPercent < 1
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# *** read calibration status ***
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not enabled:
mismatch_counter = 0
# *** health checking logic ***
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(
create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
if dm is not None:
driver_status.get_pose(dm.driverMonitoring, params)
if geofence is not None and gps is not None:
geofence.update_geofence_status(gps.gpsLocationExternal, params)
if geofence is not None and not geofence.in_geofence:
events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING]))
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
parser.add_argument('--addr', default='127.0.0.1')
parser.add_argument("socket", type=str, nargs='*', help="socket name")
args = parser.parse_args()
for m in args.socket if len(args.socket) > 0 else service_list:
if m in service_list:
messaging.sub_sock(context,
service_list[m].port,
poller,
addr=args.addr)
elif m.isdigit():
messaging.sub_sock(context, int(m), poller, addr=args.addr)
else:
print("service not found")
exit(-1)
while 1:
polld = poller.poll(timeout=1000)
for sock, mode in polld:
if mode != zmq.POLLIN:
continue
if args.pipe:
sys.stdout.write(sock.recv())
sys.stdout.flush()
elif args.raw:
hexdump(sock.recv())
elif args.json:
print(json.loads(sock.recv()))
else:
print messaging.recv_one(sock)
def ui_thread(addr, frame_address):
context = zmq.Context()
# TODO: Detect car from replay and use that to select carparams
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017", {})
VM = VehicleModel(CP)
CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]])
vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]])
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
if HOR:
size = (640+384+640, 960)
write_x = 5
write_y = 680
else:
size = (640+384, 960+300)
write_x = 645
write_y = 970
pygame.display.set_caption("openpilot debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
alert1_font = pygame.font.SysFont("arial", 30)
alert2_font = pygame.font.SysFont("arial", 20)
info_font = pygame.font.SysFont("arial", 15)
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert()
cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24)
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y),0,8)
frame = context.socket(zmq.SUB)
frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port))
frame.setsockopt(zmq.SUBSCRIBE, "")
carState = sub_sock(context, service_list['carState'].port, addr=addr, conflate=True)
plan = sub_sock(context, service_list['plan'].port, addr=addr, conflate=True)
carControl = sub_sock(context, service_list['carControl'].port, addr=addr, conflate=True)
radar_state_sock = sub_sock(context, service_list['radarState'].port, addr=addr, conflate=True)
liveCalibration = sub_sock(context, service_list['liveCalibration'].port, addr=addr, conflate=True)
controls_state_sock = sub_sock(context, service_list['controlsState'].port, addr=addr, conflate=True)
liveTracks = sub_sock(context, service_list['liveTracks'].port, addr=addr, conflate=True)
model = sub_sock(context, service_list['model'].port, addr=addr, conflate=True)
test_model = sub_sock(context, 8040, addr=addr, conflate=True)
liveMpc = sub_sock(context, service_list['liveMpc'].port, addr=addr, conflate=True)
liveParameters = sub_sock(context, service_list['liveParameters'].port, addr=addr, conflate=True)
v_ego, angle_steers, angle_steers_des, model_bias = 0., 0., 0., 0.
params_ao, params_ao_average, params_stiffness, params_sr = None, None, None, None
enabled = False
gas = 0.
accel_override = 0.
computer_gas = 0.
brake = 0.
steer_torque = 0.
curvature = 0.
computer_brake = 0.
plan_source = 'none'
long_control_state = 'none'
model_data = None
test_model_data = None
a_ego = 0.0
a_target = 0.0
d_rel, y_rel, lead_status = 0., 0., False
d_rel2, y_rel2, lead_status2 = 0., 0., False
v_ego, v_pid, v_cruise, v_override = 0., 0., 0., 0.
brake_lights = False
alert_text1, alert_text2 = "", ""
intrinsic_matrix = None
calibration = None
#img = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype='uint8')
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image
good_lt = None
lid_overlay_blank = get_blank_lid_overlay(UP)
img_offset = (0, 0)
if vision_test:
visiontest = VisionTest(FULL_FRAME_SIZE, MODEL_INPUT_SIZE, None)
# plots
name_to_arr_idx = { "gas": 0,
"computer_gas": 1,
"user_brake": 2,
"computer_brake": 3,
"v_ego": 4,
"v_pid": 5,
"angle_steers_des": 6,
"angle_steers": 7,
"steer_torque": 8,
"v_override": 9,
"v_cruise": 10,
"a_ego": 11,
"a_target": 12,
"accel_override": 13}
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
plot_ylims = [(-0.1, 1.1), (-5., 5.), (0., 75.), (-3.0, 2.0)]
plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"],
["angle_steers", "angle_steers_des", "steer_torque"],
["v_ego", "v_override", "v_pid", "v_cruise"],
["a_ego", "a_target"]]
plot_colors = [["b", "b", "g", "r", "y"],
["b", "g", "r"],
["b", "g", "r", "y"],
["b", "r"]]
plot_styles = [["-", "-", "-", "-", "-"],
["-", "-", "-"],
["-", "-", "-", "-"],
["-", "-"]]
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True)
while 1:
list(pygame.event.get())
screen.fill((64,64,64))
lid_overlay = lid_overlay_blank.copy()
top_down = top_down_surface, lid_overlay
# ***** frame *****
fpkt = recv_one(frame)
yuv_img = fpkt.frame.image
if fpkt.frame.transform:
yuv_transform = np.array(fpkt.frame.transform).reshape(3,3)
else:
# assume frame is flipped
yuv_transform = np.array([
[-1.0, 0.0, FULL_FRAME_SIZE[0]-1],
[ 0.0, -1.0, FULL_FRAME_SIZE[1]-1],
[ 0.0, 0.0, 1.0]
])
if yuv_img and len(yuv_img) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3 // 2:
yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(FULL_FRAME_SIZE[1] * 3 // 2, -1)
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
cv2.warpAffine(imgff, np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2],
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = eon_intrinsics
else:
img.fill(0)
intrinsic_matrix = np.eye(3)
if calibration is not None and yuv_img and vision_test:
model_input_yuv = visiontest.transform_contiguous(yuv_img,
np.dot(yuv_transform, calibration.model_to_full_frame).reshape(-1).tolist())
cv2.cvtColor(
np.frombuffer(model_input_yuv, dtype=np.uint8).reshape(MODEL_INPUT_SIZE[1] * 3 // 2, -1),
cv2.COLOR_YUV2RGB_I420,
dst=imgw)
else:
imgw.fill(0)
imgw_test_model = imgw.copy()
# ***** controlsState *****
controls_state = recv_one_or_none(controls_state_sock)
if controls_state is not None:
v_ego = controls_state.controlsState.vEgo
angle_steers = controls_state.controlsState.angleSteers
model_bias = controls_state.controlsState.angleModelBias
curvature = controls_state.controlsState.curvature
v_pid = controls_state.controlsState.vPid
enabled = controls_state.controlsState.enabled
alert_text1 = controls_state.controlsState.alertText1
alert_text2 = controls_state.controlsState.alertText2
long_control_state = controls_state.controlsState.longControlState
cs = recv_one_or_none(carState)
if cs is not None:
gas = cs.carState.gas
brake_lights = cs.carState.brakeLights
a_ego = cs.carState.aEgo
brake = cs.carState.brake
v_cruise = cs.carState.cruiseState.speed
cc = recv_one_or_none(carControl)
if cc is not None:
v_override = cc.carControl.cruiseControl.speedOverride
computer_brake = cc.carControl.actuators.brake
computer_gas = cc.carControl.actuators.gas
steer_torque = cc.carControl.actuators.steer * 5.
angle_steers_des = cc.carControl.actuators.steerAngle
accel_override = cc.carControl.cruiseControl.accelOverride
p = recv_one_or_none(plan)
if p is not None:
a_target = p.plan.aTarget
plan_source = p.plan.longitudinalPlanSource
plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = angle_steers
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = angle_steers_des
plot_arr[-1, name_to_arr_idx['gas']] = gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = computer_gas
plot_arr[-1, name_to_arr_idx['user_brake']] = brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = steer_torque
plot_arr[-1, name_to_arr_idx['computer_brake']] = computer_brake
plot_arr[-1, name_to_arr_idx['v_ego']] = v_ego
plot_arr[-1, name_to_arr_idx['v_pid']] = v_pid
plot_arr[-1, name_to_arr_idx['v_override']] = v_override
plot_arr[-1, name_to_arr_idx['v_cruise']] = v_cruise
plot_arr[-1, name_to_arr_idx['a_ego']] = a_ego
plot_arr[-1, name_to_arr_idx['a_target']] = a_target
plot_arr[-1, name_to_arr_idx['accel_override']] = accel_override
# ***** model ****
# live model
md = recv_one_or_none(model)
if md:
model_data = extract_model_data(md)
if model_data:
plot_model(model_data, VM, v_ego, curvature, imgw, calibration,
top_down)
if test_model is not None:
test_md = recv_one_or_none(test_model)
if test_md:
test_model_data = extract_model_data(test_md)
if test_model_data:
plot_model(test_model_data, VM, v_ego, curvature, imgw_test_model,
calibration, top_down, 215)
# MPC
mpc = recv_one_or_none(liveMpc)
if mpc:
draw_mpc(mpc, top_down)
# LiveParams
params = recv_one_or_none(liveParameters)
if params:
params_ao = params.liveParameters.angleOffset
params_ao_average = params.liveParameters.angleOffsetAverage
params_stiffness = params.liveParameters.stiffnessFactor
params_sr = params.liveParameters.steerRatio
# **** tracks *****
# draw all radar points
lt = recv_one_or_none(liveTracks)
if lt is not None:
good_lt = lt
if good_lt is not None:
maybe_update_radar_points(good_lt, top_down[1])
# ***** radarState *****
radar_state = recv_one_or_none(radar_state_sock)
if radar_state is not None:
d_rel = radar_state.radarState.leadOne.dRel + RDR_TO_LDR
y_rel = radar_state.radarState.leadOne.yRel
lead_status = radar_state.radarState.leadOne.status
d_rel2 = radar_state.radarState.leadTwo.dRel + RDR_TO_LDR
y_rel2 = radar_state.radarState.leadTwo.yRel
lead_status2 = radar_state.radarState.leadTwo.status
lcal = recv_one_or_none(liveCalibration)
if lcal is not None:
calibration_message = lcal.liveCalibration
extrinsic_matrix = np.asarray(calibration_message.extrinsicMatrix).reshape(3, 4)
warp_matrix = np.asarray(calibration_message.warpMatrix2).reshape(3, 3)
calibration = CalibrationTransformsForWarpMatrix(warp_matrix, intrinsic_matrix, extrinsic_matrix)
# draw red pt for lead car in the main img
if lead_status:
if calibration is not None:
dx, dy = draw_lead_on(img, d_rel, y_rel, img_offset, calibration, color=(192,0,0))
# draw red line for lead car
draw_lead_car(d_rel, top_down)
# draw red pt for lead car2 in the main img
if lead_status2:
if calibration is not None:
dx2, dy2 = draw_lead_on(img, d_rel2, y_rel2, img_offset, calibration, color=(192,0,0))
# draw red line for lead car
draw_lead_car(d_rel2, top_down)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1))
screen.blit(camera_surface, (0, 0))
# display alerts
alert_line1 = alert1_font.render(alert_text1, True, (255,0,0))
alert_line2 = alert2_font.render(alert_text2, True, (255,0,0))
screen.blit(alert_line1, (180, 150))
screen.blit(alert_line2, (180, 190))
if calibration is not None and img is not None:
cpw = warp_points(CalP, calibration.model_to_bb)
vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb)
pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1)
pygame.draw.circle(screen, BLUE, map(int, map(round, vanishing_pointw[0])), 2)
if HOR:
screen.blit(draw_plots(plot_arr), (640+384, 0))
else:
screen.blit(draw_plots(plot_arr), (0, 600))
pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1))
screen.blit(cameraw_surface, (320, 480))
pygame.surfarray.blit_array(cameraw_test_surface, imgw_test_model.swapaxes(0, 1))
screen.blit(cameraw_test_surface, (0, 480))
pygame.surfarray.blit_array(*top_down)
screen.blit(top_down[0], (640,0))
i = 0
SPACING = 25
# enabled
enabled_line = info_font.render("ENABLED", True, GREEN if enabled else BLACK)
screen.blit(enabled_line, (write_x, write_y + i * SPACING))
i += 1
# brake lights
brake_lights_line = info_font.render("BRAKE LIGHTS", True, RED if brake_lights else BLACK)
screen.blit(brake_lights_line, (write_x, write_y + i * SPACING))
i += 1
# speed
v_ego_line = info_font.render("SPEED: " + str(round(v_ego, 1)) + " m/s", True, YELLOW)
screen.blit(v_ego_line, (write_x, write_y + i * SPACING))
i += 1
# angle offset
model_bias_line = info_font.render("MODEL BIAS: " + str(round(model_bias, 2)) + " deg", True, YELLOW)
screen.blit(model_bias_line, (write_x, write_y + i * SPACING))
i += 1
# long control state
long_control_state_line = info_font.render("LONG CONTROL STATE: " + str(long_control_state), True, YELLOW)
screen.blit(long_control_state_line, (write_x, write_y + i * SPACING))
i += 1
# long mpc source
plan_source_line = info_font.render("LONG MPC SOURCE: " + str(plan_source), True, YELLOW)
screen.blit(plan_source_line, (write_x, write_y + i * SPACING))
i += 1
if params_ao is not None:
i += 1
angle_offset_avg_line = info_font.render("ANGLE OFFSET (AVG): " + str(round(params_ao_average, 2)) + " deg", True, YELLOW)
screen.blit(angle_offset_avg_line, (write_x, write_y + i * SPACING))
i += 1
angle_offset_line = info_font.render("ANGLE OFFSET (INSTANT): " + str(round(params_ao, 2)) + " deg", True, YELLOW)
screen.blit(angle_offset_line, (write_x, write_y + i * SPACING))
i += 1
angle_offset_line = info_font.render("STIFFNESS: " + str(round(params_stiffness * 100., 2)) + " %", True, YELLOW)
screen.blit(angle_offset_line, (write_x, write_y + i * SPACING))
i += 1
steer_ratio_line = info_font.render("STEER RATIO: " + str(round(params_sr, 2)), True, YELLOW)
screen.blit(steer_ratio_line, (write_x, write_y + i * SPACING))
i += 1
# this takes time...vsync or something
pygame.display.flip()
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed):
cur_time = sec_since_boot()
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
if not CS.pedal_interceptor_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the Live20 feed
l20 = None
if enabled:
for socket, _ in self.poller.poll(0):
if socket is self.live20:
l20 = messaging.recv_one(socket)
break
if l20 is not None:
self.lead_1 = l20.live20.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
self.md_ts = l20.live20.mdMonoTime
self.l100_ts = l20.live20.l100MonoTime
brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
self.v_pid = self.calc_follow_speed_ms(CS)
# cruise speed can't be negative even is user is distracted
self.v_pid = max(self.v_pid, 0.)
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
if self.enable_pedal_cruise:
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.pedal_speed_kph,
self.lead_last_seen_time_ms)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, self.v_pid, accel_max,
brake_max, jerk_max, jerk_min, _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (
self.a_acc_sol + self.a_acc_start) / 2.0
# we will try to feed forward the pedal position.... we might want to feed the last output_gb....
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_pid)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_pid, vTarget,
self.vTargetFuture, self.a_acc_sol, CS.CP, None)
output_gb = t_go - t_brake
#print "Output GB Follow:", output_gb
else:
self.LoC.reset(CS.v_ego)
print "PID reset"
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
##############################################################
# This is an experimental mode that is probably broken.
#
# Don't use it.
#
# Ratios are centered at 1. They can be multiplied together.
# Factors are centered around 0. They can be multiplied by constants.
# For example +9% is a 1.06 ratio or 0.09 factor.
##############################################################
elif PCCModes.is_selected(ExperimentalMode(), CS.cstm_btns):
output_gb = 0.0
if enabled and self.enable_pedal_cruise:
MAX_DECEL_RATIO = 0
MAX_ACCEL_RATIO = 1.1
available_speed_kph = self.pedal_speed_kph - CS.v_ego * CV.MS_TO_KPH
# Hold accel if radar gives intermittent readings at great distance.
# Makes the car less skittish when first making radar contact.
if (_is_present(self.lead_1)
and self.continuous_lead_sightings < 8
and _sec_til_collision(self.lead_1) > 3
and self.lead_1.dRel > 60):
output_gb = self.last_output_gb
# Hold speed in turns if no car is seen
elif CS.angle_steers >= 5.0 and not _is_present(self.lead_1):
pass
# Try to stay 2 seconds behind lead, matching their speed.
elif _is_present(self.lead_1):
weighted_d_ratio = _weighted_distance_ratio(
self.lead_1, CS.v_ego, MAX_DECEL_RATIO,
MAX_ACCEL_RATIO)
weighted_v_ratio = _weighted_velocity_ratio(
self.lead_1, CS.v_ego, MAX_DECEL_RATIO,
MAX_ACCEL_RATIO)
# Don't bother decelerating if the lead is already pulling away
if weighted_d_ratio < 1 and weighted_v_ratio > 1.01:
gas_brake_ratio = max(1, self.last_output_gb + 1)
else:
gas_brake_ratio = weighted_d_ratio * weighted_v_ratio
# rescale around 0 rather than 1.
output_gb = gas_brake_ratio - 1
# If no lead has been seen recently, accelerate to max speed.
else:
# An acceleration factor that drops off as we aproach max speed.
max_speed_factor = min(available_speed_kph, 3) / 3
# An acceleration factor that increases as time passes without seeing
# a lead car.
time_factor = (_current_time_millis() -
self.lead_last_seen_time_ms) / 3000
time_factor = clip(time_factor, 0, 1)
output_gb = 0.14 * max_speed_factor * time_factor
# If going above the max configured PCC speed, slow. This should always
# be in force so it is not part of the if/else block above.
if available_speed_kph < 0:
# linearly brake harder, hitting -1 at 10kph over
speed_limited_gb = max(available_speed_kph, -10) / 10.0
# This is a minimum braking amount. The logic above may ask for more.
output_gb = min(output_gb, speed_limited_gb)
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) <
abs(self.lastTorqueForPedalForZeroTorque)):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print "Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)
#print "Torque level at detection %s" % (CS.torqueLevel)
#print "Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(output_gb, 0., accel_max)
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.)
# if speed is over 5mpg, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * MAX_PEDAL_VALUE, 0,
MAX_PEDAL_VALUE - tesla_brake)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal, self.accel_steady = accel_hysteresis(
tesla_pedal, self.accel_steady, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
self.last_md_ts = self.md_ts
self.last_l100_ts = self.l100_ts
return self.prev_tesla_pedal, enable_pedal, idx
def fingerprint(logcan, sendcan, is_panda_black):
if os.getenv("SIMULATOR2") is not None:
return ("simulator2", None, "")
elif os.getenv("SIMULATOR") is not None:
return ("simulator", None, "")
params = Params()
car_params = params.get("CarParams")
if car_params is not None:
# use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode
car_params = car.CarParams.from_bytes(car_params)
vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin
elif is_panda_black:
# Vin query only reliably works thorugh OBDII
vin = get_vin(logcan, sendcan, 1)
else:
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
finger = {i: {} for i in range(0, 4)} # collect on all buses
candidate_cars = {i: all_known_cars()
for i in [0, 1]
} # attempt fingerprint on both bus 0 and 1
frame = 0
frame_fingerprint = 10 # 0.1s
car_fingerprint = None
done = False
while not done:
a = messaging.recv_one(logcan)
for can in a.can:
# need to independently try to fingerprint both bus 0 and 1 to work
# for the combo black_panda and honda_bosch. Ignore extended messages
# and VIN query response.
# Include bus 2 for toyotas to disambiguate cars using camera messages
# (ideally should be done for all cars but we can't for Honda Bosch)
for b in candidate_cars:
if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
finger[can.src][can.address] = len(can.dat)
candidate_cars[b] = eliminate_incompatible_cars(
can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
# Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
if only_toyota_left(candidate_cars[b]):
frame_fingerprint = 100 # 1s
if len(candidate_cars[b]) == 1:
if frame > frame_fingerprint:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = all(len(cc) == 0
for cc in candidate_cars.itervalues()) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
cloudlog.warning("fingerprinted %s", car_fingerprint)
return car_fingerprint, finger, vin
def mapsd_thread():
global last_gps
context = zmq.Context()
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True)
map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
while True:
gps = messaging.recv_one(gps_sock)
gps_ext = messaging.recv_one_or_none(gps_external_sock)
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
gps = gps.gpsLocation
last_gps = gps
fix_ok = gps.flags & 1
if not fix_ok or last_query_result is None or not cache_valid:
cur_way = None
curvature = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x) for x in xs], [float(y) for y in ys]
if speed < 10:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 10] = np.inf
curvature = 1. / radii
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
# TODO: Determine left or right turn
curvature = np.nan_to_num(curvature)
# Outlier rejection
new_curvature = np.percentile(curvature, 90, interpolation='lower')
k = 0.6
upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
in_turn_indices = curvature > 0.8 * new_curvature
if np.any(in_turn_indices):
dist_to_turn = np.min(dists[in_turn_indices])
else:
dist_to_turn = 999
else:
upcoming_curvature = 0.
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Seed limit
max_speed = cur_way.max_speed()
if max_speed is not None:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
# TODO: use the function below to anticipate upcoming speed limits
#max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
#if max_speed_ahead is not None and max_speed_ahead_dist is not None:
# dat.liveMapData.speedLimitAheadValid = True
# dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
# dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
advisory_max_speed = cur_way.advisory_max_speed()
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [float(x) for x in curvature]
dat.liveMapData.mapValid = map_valid
map_data_sock.send(dat.to_bytes())
path1 = '/home/oj/oj_scripts/archive/oj_stop.xml'
haar_face_cascade =cv.CascadeClassifier(path1)
end1 = time.time();print('checkpoint1 (ms)',round(1000*(end1 - start), 1))
# gets data packets from zmq via port 8002
context = zmq.Context()
frame = context.socket(zmq.SUB)
frame.connect("tcp://127.0.0.1:%d" % (service_list['frame'].port)) # port 8002
frame.setsockopt(zmq.SUBSCRIBE, "")
# frame Display
while 1:
fpkt = recv_one(frame)
yuv_img = fpkt.frame.image
yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3)
if yuv_img and len(yuv_img) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3 // 2:
yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(FULL_FRAME_SIZE[1] * 3 // 2, -1)
im = cv.cvtColor(yuv_np, cv.COLOR_YUV2BGRA_I420) # yuv comes format comes from the sensor
gray = cv.cvtColor(im, cv.COLOR_RGB2GRAY) # yuv comes format comes from the sensor
#gray = cv.cvtColor(im, cv.COLOR_RGB2GRAY) # converts some funny BGR format into RGB
stops = haar_face_cascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=4)
# go over list of stop signs and draw them as rectangles on original colored image
for (x, y, w, h) in stops:
cv.rectangle(im, (x, y), (x + w, y + h), (0, 255, 0), 2)
def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = None
l20 = None
gps_planner_plan = None
for socket, event in self.poller.poll(0):
if socket is self.model:
md = messaging.recv_one(socket)
elif socket is self.live20:
l20 = messaging.recv_one(socket)
elif socket is self.gps_planner_plan:
gps_planner_plan = messaging.recv_one(socket)
elif socket is self.live_map_data:
self.last_live_map_data = messaging.recv_one(
socket).liveMapData
if gps_planner_plan is not None:
self.last_gps_planner_plan = gps_planner_plan
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
self.PP.update(CS.vEgo, md, LaC)
if self.last_gps_planner_plan is not None:
plan = self.last_gps_planner_plan.gpsPlannerPlan
self.gps_planner_active = plan.valid
if plan.valid:
self.PP.d_poly = plan.poly
self.PP.p_poly = plan.poly
self.PP.c_poly = plan.poly
self.PP.l_prob = 0.0
self.PP.r_prob = 0.0
self.PP.c_prob = 1.0
if l20 is not None:
self.perception_state = copy(l20.live20)
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
self.lead_1 = l20.live20.leadOne
self.lead_2 = l20.live20.leadTwo
enabled = (LoC.long_control_state
== LongCtrlState.pid) or (LoC.long_control_state
== LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
if self.last_live_map_data:
self.v_speedlimit = NO_CURVATURE_SPEED
self.v_curvature = NO_CURVATURE_SPEED
self.map_valid = self.last_live_map_data.mapValid
# Speed limit
if self.last_live_map_data.speedLimitValid:
speed_limit = self.last_live_map_data.speedLimit
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active:
offset = float(self.params.get("SpeedLimitOffset"))
self.v_speedlimit = speed_limit + offset
# Curvature
if self.last_live_map_data.curvatureValid:
curvature = abs(self.last_live_map_data.curvature)
v_curvature = math.sqrt(A_Y_MAX /
max(1e-4, curvature))
self.v_curvature = min(NO_CURVATURE_SPEED,
v_curvature)
# leave 1m/s margin on vEgo to asses if turn is limiting our speed.
self.decel_for_turn = bool(self.v_curvature < min(
[v_cruise_setpoint, self.v_speedlimit, CS.vEgo + 1.]))
v_cruise_setpoint = min(
[v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(
float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
]
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle,
accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
if self.decel_for_turn:
time_to_turn = max(
1.0, self.last_live_map_data.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(
0, (self.v_curvature - self.v_cruise) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(
create_event('modelCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if 'fault' in self.radar_errors:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if LaC.mpc_solution[
0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol +
self.a_acc_start) / 2.0
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol
plan_send.plan.aTarget = self.a_acc_sol
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.hasLeftLane = bool(self.PP.l_prob > 0.5)
plan_send.plan.hasRightLane = bool(self.PP.r_prob > 0.5)
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.gpsPlannerActive = self.gps_planner_active
plan_send.plan.vCurvature = self.v_curvature
plan_send.plan.decelForTurn = self.decel_for_turn
plan_send.plan.mapValid = self.map_valid
# Send out fcw
fcw = self.fcw and (self.fcw_enabled
or LoC.long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
return plan_send
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
"""Gets called when new live20 is available"""
cur_time = live20.logMonoTime / 1e9
v_ego = CS.carState.vEgo
gasbuttonstatus = CS.carState.gasbuttonstatus
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
for socket, event in self.poller.poll(0):
if socket is self.lat_Control:
self.lastlat_Control = messaging.recv_one(socket).latControl
self.lead_1 = live20.live20.leadOne
self.lead_2 = live20.live20.leadTwo
lead_1 = live20.live20.leadOne
lead_2 = live20.live20.leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
map_valid = live_map_data.liveMapData.mapValid
# Speed limit and curvature
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(
a_y_max / max(1e-4, curvature)) / 1.3 * _brake_factor
v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
decel_for_turn = bool(
v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(
float,
calc_cruise_accel_limits(v_ego, following, gasbuttonstatus))
if gasbuttonstatus == 0:
accellimitmaxdynamic = -0.0018 * v_ego + 0.2
jerk_limits = [
min(-0.1, accel_limits[0] * 0.5),
max(accellimitmaxdynamic, accel_limits[1])
] # dynamic
elif gasbuttonstatus == 1:
accellimitmaxsport = -0.002 * v_ego + 0.4
jerk_limits = [
min(-0.25, accel_limits[0]),
max(accellimitmaxsport, accel_limits[1])
] # sport
elif gasbuttonstatus == 2:
accellimitmaxeco = -0.0015 * v_ego + 0.1
jerk_limits = [
min(-0.1, accel_limits[0] * 0.5),
max(accellimitmaxeco, accel_limits[1])
] # eco
if not CS.carState.leftBlinker and not CS.carState.rightBlinker:
steering_angle = CS.carState.steeringAngle
if self.lastlat_Control and v_ego > 11:
angle_later = self.lastlat_Control.anglelater
else:
angle_later = 0
else:
angle_later = 0
steering_angle = 0
accel_limits = limit_accel_in_turns(
v_ego, steering_angle, accel_limits, self.CP,
angle_later * self.CP.steerRatio)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
if decel_for_turn:
time_to_turn = max(
1.0, live_map_data.liveMapData.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(0, (v_curvature - self.v_cruise) /
time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(CS.carState.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, lead_1, v_cruise_setpoint)
self.mpc2.update(CS, lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not CS.carState.brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
events.append(
create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
radar_errors = list(live20.live20.radarErrors)
if 'commIssue' in radar_errors:
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if 'fault' in radar_errors:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.hasrightLaneDepart = bool(
PP.r_poly[3] > -1.1 and not CS.carState.rightBlinker)
plan_send.plan.hasleftLaneDepart = bool(
PP.l_poly[3] < 1.05 and not CS.carState.leftBlinker)
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = v_curvature
plan_send.plan.decelForTurn = decel_for_turn
plan_send.plan.mapValid = map_valid
# Send out fcw
fcw = fcw and (self.fcw_enabled
or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
dt = 0.05 # s
a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
v_acc_sol = self.v_acc_start + dt * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, CS, LoC, v_cruise_kph, user_distracted):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = None
l20 = None
for socket, event in self.poller.poll(0):
if socket is self.model:
md = messaging.recv_one(socket)
elif socket is self.live20:
l20 = messaging.recv_one(socket)
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
self.PP.update(CS.vEgo, md)
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
self.lead_1 = l20.live20.leadOne
self.lead_2 = l20.live20.leadTwo
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP)
if user_distracted:
# if user is not responsive to awareness alerts, then start a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol
plan_send.plan.aTarget = self.a_acc_sol
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
# Send out fcw
fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
return plan_send
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
'carParams'
])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
'gpsLocation'], ignore_alive=['gpsLocation'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port,
timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
print("Waiting for CAN messages...")
get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available or CP.dashcamOnly
if read_only:
CP.safetyModel = CP.safetyModelPassive
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd = bool(int(is_rhd))
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
events_prev = []
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (
os.path.isdir('/proc/asound/card0')
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(
create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, AM,
driver_status, LaC, LoC, read_only,
start_time, v_acc, a_acc, lac_log,
events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()