def wait_for_sockets(socks, timeout=10.0):
sm = messaging.SubMaster(socks)
t = time.time()
recvd = []
while time.time() - t < timeout and len(recvd) < len(socks):
sm.update()
for s in socks:
if s not in recvd and sm.updated[s]:
recvd.append(s)
return recvd
def model_replay(lr_list, frs):
spinner = Spinner()
spinner.update("starting model replay")
vipc_server = VisionIpcServer("camerad")
vipc_server.create_buffers(
VisionStreamType.VISION_STREAM_YUV_BACK, 40, False,
*(tici_f_frame_size if TICI else eon_f_frame_size))
vipc_server.create_buffers(
VisionStreamType.VISION_STREAM_YUV_FRONT, 40, False,
*(tici_d_frame_size if TICI else eon_d_frame_size))
vipc_server.start_listener()
pm = messaging.PubMaster([
'roadCameraState', 'driverCameraState', 'liveCalibration',
'lateralPlan'
])
sm = messaging.SubMaster(['modelV2', 'driverState'])
try:
managed_processes['modeld'].start()
managed_processes['dmonitoringmodeld'].start()
time.sleep(5)
sm.update(1000)
last_desire = None
log_msgs = []
frame_idxs = dict.fromkeys(['roadCameraState', 'driverCameraState'], 0)
cal = [msg for msg in lr if msg.which() == "liveCalibration"]
for msg in cal[:5]:
pm.send(msg.which(), replace_calib(msg, None))
for msg in tqdm(lr_list):
if msg.which() == "liveCalibration":
last_calib = list(msg.liveCalibration.rpyCalib)
pm.send(msg.which(), replace_calib(msg, last_calib))
elif msg.which() == "lateralPlan":
last_desire = msg.lateralPlan.desire
elif msg.which() in ["roadCameraState", "driverCameraState"]:
ret = process_frame(msg, pm, sm, log_msgs, vipc_server,
spinner, frs, frame_idxs, last_desire)
if ret is None:
break
except KeyboardInterrupt:
pass
finally:
spinner.close()
managed_processes['modeld'].stop()
managed_processes['dmonitoringmodeld'].stop()
return log_msgs
def radard_thread(sm=None, pm=None, can_sock=None):
config_realtime_process(5, Priority.CTRL_LOW)
# 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))
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState']) # Can't check average frequency, since radar determines timing
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(sm, rr)
dat.radarState.cumLagMs = -rk.remaining*1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def cycle_alerts(duration=200, is_metric=False):
alerts = list(EVENTS.keys())
print(alerts)
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
sm = messaging.SubMaster([
'thermal', 'health', 'frame', 'model', 'liveCalibration',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
])
controls_state = messaging.pub_sock('controlsState')
thermal = messaging.pub_sock('thermal')
idx, last_alert_millis = 0, 0
events = Events()
AM = AlertManager()
frame = 0
while 1:
if frame % duration == 0:
idx = (idx + 1) % len(alerts)
events.clear()
events.add(alerts[idx])
current_alert_types = [
ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING
]
a = events.create_alerts(current_alert_types, [CP, sm, is_metric])
AM.add_many(frame, a)
AM.process_alerts(frame)
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.alertText1 = AM.alert_text_1
dat.controlsState.alertText2 = AM.alert_text_2
dat.controlsState.alertSize = AM.alert_size
dat.controlsState.alertStatus = AM.alert_status
dat.controlsState.alertBlinkingRate = AM.alert_rate
dat.controlsState.alertType = AM.alert_type
dat.controlsState.alertSound = AM.audible_alert
controls_state.send(dat.to_bytes())
dat = messaging.new_message()
dat.init('thermal')
dat.thermal.started = True
thermal.send(dat.to_bytes())
frame += 1
time.sleep(0.01)
def main():
sm = None
if sm is None:
sm = messaging.SubMaster(['plan', 'pathPlan', 'model'])
v_ego = 50 * CV.KPH_TO_MS
print('curvature test ')
while True:
sm.update(0)
value, model_sum = sc.calc_va(sm, v_ego)
print('curvature={:.3f} sum={:.5f}'.format(value, model_sum))
def test_update(self):
sock = "carState"
pub_sock = messaging.pub_sock(sock)
sm = messaging.SubMaster([sock,])
zmq_sleep()
for i in range(10):
msg = messaging.new_message(sock)
pub_sock.send(msg.to_bytes())
sm.update(1000)
self.assertEqual(sm.frame, i)
self.assertTrue(all(sm.updated.values()))
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
#subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
ignore = []
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
if os.getenv("BLOCK") is not None:
ignore += os.getenv("BLOCK").split(",")
if EON:
pm_apply_packages('enable')
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
params = Params()
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
not_run = ignore[:]
if sm['deviceState'].freeSpacePercent < 5:
not_run.append("loggerd")
started = sm['deviceState'].started
driverview = params.get("IsDriverViewEnabled") == b"1"
ensure_running(managed_processes.values(), started, driverview, not_run)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
os.sync()
managed_processes['updated'].signal(signal.SIGHUP)
started_prev = started
running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc]
cloudlog.debug(' '.join(running_list))
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
pm.send('managerState', msg)
# Exit main loop when uninstall is needed
if params.get("DoUninstall", encoding='utf8') == "1":
break
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(
['liveLocationKalman', 'gnssMeasurements', 'managerState'])
if pm is None:
pm = messaging.PubMaster(['navInstruction', 'navRoute'])
rk = Ratekeeper(1.0)
route_engine = RouteEngine(sm, pm)
while True:
route_engine.update()
rk.keep_time()
def locationd_thread(sm, pm, disabled_logs=None):
gc.disable()
if disabled_logs is None:
disabled_logs = []
if sm is None:
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'], ignore_avg_freq=['sensorEvents'])
if pm is None:
pm = messaging.PubMaster(['liveLocationKalman'])
params = Params()
localizer = Localizer(disabled_logs=disabled_logs)
while True:
sm.update()
for sock, updated in sm.updated.items():
if updated and sm.valid[sock]:
t = sm.logMonoTime[sock] * 1e-9
if sock == "sensorEvents":
localizer.handle_sensors(t, sm[sock])
elif sock == "gpsLocationExternal":
localizer.handle_gps(t, sm[sock])
elif sock == "carState":
localizer.handle_car_state(t, sm[sock])
elif sock == "cameraOdometry":
localizer.handle_cam_odo(t, sm[sock])
elif sock == "liveCalibration":
localizer.handle_live_calib(t, sm[sock])
if sm.updated['cameraOdometry']:
t = sm.logMonoTime['cameraOdometry']
msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t
msg.liveLocationKalman = localizer.liveLocationMsg()
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
gps_age = (t / 1e9) - localizer.last_gps_fix
msg.liveLocationKalman.gpsOK = gps_age < 1.0
pm.send('liveLocationKalman', msg)
if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute
location = {
'latitude': msg.liveLocationKalman.positionGeodetic.value[0],
'longitude': msg.liveLocationKalman.positionGeodetic.value[1],
'altitude': msg.liveLocationKalman.positionGeodetic.value[2],
}
params.put("LastGPSPosition", json.dumps(location))
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
# TODO: Read from car params at runtime
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.toyota.values import CAR
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
learner = ParamsLearner(CP)
while True:
sm.update()
for which, updated in sm.updated.items():
if not updated:
continue
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
# TODO: set valid to false when locationd stops sending
# TODO: make sure controlsd knows when there is no gyro
# TODO: move posenetValid somewhere else to show the model uncertainty alert
# TODO: Save and resume values from param
# TODO: Change KF to allow mass, etc to be inputs in predict step
if sm.updated['carState']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.valid = True # TODO: Check if learned values are sane
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
x = learner.kf.x
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(
x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = math.degrees(
x[States.ANGLE_OFFSET_FAST])
# P = learner.kf.P
# print()
# print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
# print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
# print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
# print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)
pm.send('liveParameters', msg)
def test_boardd_loopback():
# wait for boardd to init
spinner = Spinner()
time.sleep(2)
with Timeout(60, "boardd didn't start"):
sm = messaging.SubMaster(['pandaState'])
while sm.rcv_frame['pandaState'] < 1:
sm.update(1000)
# boardd blocks on CarVin and CarParams
cp = car.CarParams.new_message()
cp.safetyModel = car.CarParams.SafetyModel.allOutput
Params().put("CarVin", b"0"*17)
Params().put("CarParams", cp.to_bytes())
sendcan = messaging.pub_sock('sendcan')
can = messaging.sub_sock('can', conflate=False, timeout=100)
time.sleep(1)
n = 1000
for i in range(n):
spinner.update(f"boardd loopback {i}/{n}")
sent_msgs = defaultdict(set)
for _ in range(random.randrange(10)):
to_send = []
for __ in range(random.randrange(100)):
bus = random.randrange(3)
addr = random.randrange(1, 1<<29)
dat = bytes([random.getrandbits(8) for _ in range(random.randrange(1, 9))])
sent_msgs[bus].add((addr, dat))
to_send.append(make_can_msg(addr, dat, bus))
sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
max_recv = 10
while max_recv > 0 and any(len(sent_msgs[bus]) for bus in range(3)):
recvd = messaging.drain_sock(can, wait_for_one=True)
for msg in recvd:
for m in msg.can:
if m.src >= 128:
k = (m.address, m.dat)
assert k in sent_msgs[m.src-128]
sent_msgs[m.src-128].discard(k)
max_recv -= 1
# if a set isn't empty, messages got dropped
for bus in range(3):
assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages"
spinner.close()
def __init__(self, CP=None):
self.long_control_state = 0 # initialized to off
self.seq_step_debug = ""
self.long_curv_timer = 0
self.path_x = np.arange(192)
self.traceSC = trace1.Loger("SPD_CTRL")
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
self.l_poly = []
self.r_poly = []
self.movAvg = moveavg1.MoveAvg()
self.Timer1 = tm.CTime1000("SPD")
self.time_no_lean = 0
self.wait_timer2 = 0
self.cruise_set_speed_kph = 0
self.curise_set_first = 0
self.curise_sw_check = 0
self.prev_clu_CruiseSwState = 0
self.prev_VSetDis = 0
self.sc_clu_speed = 0
self.btn_type = Buttons.NONE
self.active_time = 0
self.old_model_speed = 0
self.old_model_init = 0
self.curve_speed = 0
self.curvature_gain = 1
self.params = Params()
self.cruise_set_mode = int(
self.params.get("CruiseStatemodeSelInit", encoding="utf8"))
self.map_spd_limit_offset = int(
self.params.get("OpkrSpeedLimitOffset", encoding="utf8"))
self.map_spd_enable = False
self.map_spd_camera = 0
self.map_enabled = False
self.second = 0
self.sm = messaging.SubMaster(['liveMapData'])
def test_conflate(self):
sock = "carState"
pub_sock = messaging.pub_sock(sock)
sm = messaging.SubMaster([sock,])
n = 10
for i in range(n+1):
msg = messaging.new_message(sock)
msg.carState.vEgo = i
pub_sock.send(msg.to_bytes())
time.sleep(0.01)
sm.update(1000)
self.assertEqual(sm[sock].vEgo, n)
def test_init_state(self):
socks = random_socks()
sm = messaging.SubMaster(socks)
self.assertEqual(sm.frame, -1)
self.assertFalse(any(sm.updated.values()))
self.assertFalse(any(sm.alive.values()))
self.assertTrue(all(t == 0. for t in sm.rcv_time.values()))
self.assertTrue(all(f == 0 for f in sm.rcv_frame.values()))
self.assertTrue(all(t == 0 for t in sm.logMonoTime.values()))
for p in [sm.updated, sm.rcv_time, sm.rcv_frame, sm.alive,
sm.sock, sm.freq, sm.data, sm.logMonoTime, sm.valid]:
self.assertEqual(len(p), len(socks))
def setUp(self):
self.vipc_server = VisionIpcServer("camerad")
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size)
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size)
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size)
self.vipc_server.start_listener()
self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration', 'lateralPlan'])
managed_processes['modeld'].start()
time.sleep(0.2)
self.sm.update(1000)
def main():
sm = messaging.SubMaster(['ubloxGnss'])
pm = messaging.PubMaster(['gnssMeasurements'])
# todo get last_known_position
laikad = Laikad(save_ephemeris=True)
while True:
sm.update()
if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'])
if msg is not None:
pm.send('gnssMeasurements', msg)
def uploader_fn(exit_event):
params = Params()
dongle_id = params.get("DongleId", encoding='utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
offroad = params.get_bool("IsOffroad")
network_type = sm[
'deviceState'].networkType if not force_wifi else NetworkType.wifi
if network_type == NetworkType.none:
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
good_internet = network_type in [
NetworkType.wifi, NetworkType.ethernet
]
allow_raw_upload = params.get_bool("UploadRaw")
d = uploader.next_file_to_upload(
with_raw=allow_raw_upload and good_internet and offroad)
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
key, fn = d
cloudlog.debug("upload %r over %s", d, network_type)
success = uploader.upload(key, fn)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
pm.send("uploaderState", uploader.get_msg())
cloudlog.info("upload done, success=%r", success)
def camera_replay(lr, fr):
spinner = Spinner()
pm = messaging.PubMaster(['frame', 'liveCalibration'])
sm = messaging.SubMaster(['model'])
# TODO: add dmonitoringmodeld
print("preparing procs")
manager.prepare_managed_process("camerad")
manager.prepare_managed_process("modeld")
try:
print("starting procs")
manager.start_managed_process("camerad")
manager.start_managed_process("modeld")
time.sleep(5)
print("procs started")
cal = [msg for msg in lr if msg.which() == "liveCalibration"]
for msg in cal[:5]:
pm.send(msg.which(), msg.as_builder())
log_msgs = []
frame_idx = 0
for msg in tqdm(lr):
if msg.which() == "liveCalibrationd":
pm.send(msg.which(), msg.as_builder())
elif msg.which() == "frame":
f = msg.as_builder()
img = fr.get(frame_idx, pix_fmt="rgb24")[0][:, ::, -1]
f.frame.image = img.flatten().tobytes()
frame_idx += 1
pm.send(msg.which(), f)
with Timeout(seconds=15):
log_msgs.append(messaging.recv_one(sm.sock['model']))
spinner.update("modeld replay %d/%d" %
(frame_idx, fr.frame_count))
if frame_idx >= fr.frame_count:
break
except KeyboardInterrupt:
pass
print("replay done")
spinner.close()
manager.kill_managed_process('modeld')
time.sleep(2)
manager.kill_managed_process('camerad')
return log_msgs
def test_engage(self):
# Startup manager and bridge.py. Check processes are running, then engage and verify.
p_manager = subprocess.Popen("./launch_openpilot.sh", cwd='../')
self.processes.append(p_manager)
sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState'])
q = Queue()
carla_bridge = CarlaBridge(bridge.parse_args([]))
p_bridge = carla_bridge.run(q, retries=3)
self.processes.append(p_bridge)
max_time_per_step = 20
# Wait for bridge to startup
start_waiting = time.monotonic()
while not carla_bridge.started and time.monotonic() < start_waiting + max_time_per_step:
time.sleep(0.1)
self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}")
start_time = time.monotonic()
no_car_events_issues_once = False
car_event_issues = []
not_running = []
while time.monotonic() < start_time + max_time_per_step:
sm.update()
not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning]
car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])]
if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0:
no_car_events_issues_once = True
break
self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'")
start_time = time.monotonic()
min_counts_control_active = 100
control_active = 0
while time.monotonic() < start_time + max_time_per_step:
sm.update()
q.put("cruise_up") # Try engaging
if sm.all_alive() and sm['controlsState'].active:
control_active += 1
if control_active == min_counts_control_active:
break
self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}")
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
frame_sizes = [
eon_f_frame_size, eon_d_frame_size, leon_d_frame_size,
tici_f_frame_size
]
frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes}
sm = messaging.SubMaster([frame, front_frame])
while min(sm.logMonoTime.values()) == 0:
sm.update()
rear = extract_image(sm[frame].image, frame_sizes)
front = extract_image(sm[front_frame].image, frame_sizes)
return rear, front
def __init__(self, sm=None, pm=None, can_sock=None):
# Initialize received messages queue
self.msgs_queue = Queue()
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['testJoystick'])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster(self.publishers)
self.rk = Ratekeeper(RATE, print_delay_threshold=None)
def uploader_fn(exit_event):
cloudlog.info("uploader_fn")
params = Params()
dongle_id = params.get("DongleId").decode('utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
uploader = Uploader(dongle_id, ROOT)
# dp
sm = messaging.SubMaster(['dragonConf'])
backoff = 0.1
while True:
offroad = params.get("IsOffroad") == b'1'
allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") and offroad
on_hotspot = is_on_hotspot()
on_wifi = is_on_wifi()
sm.update(1000)
if sm.updated['dragonConf']:
on_wifi = True if sm['dragonConf'].dpUploadOnMobile else on_wifi
on_hotspot = False if sm['dragonConf'].dpUploadOnHotspot else on_hotspot
should_upload = on_wifi and not on_hotspot
if exit_event.is_set():
return
d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload)
if d is None: # Nothing to upload
time.sleep(60 if offroad else 5)
continue
key, fn = d
cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi)
cloudlog.info("to upload %r", d)
success = uploader.upload(key, fn)
if success:
backoff = 0.1
else:
cloudlog.info("backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff*2, 120)
cloudlog.info("upload done, success=%r", success)
def status_monitor():
# use driverState socker to drive timing.
driverState = messaging.sub_sock('driverState',
addr=args.addr,
conflate=True)
sm = messaging.SubMaster(['carState', 'dMonitoringState'], addr=args.addr)
steering_status = HandsOnWheelStatus()
v_cruise_last = 0
while messaging.recv_one(driverState):
try:
sm.update()
v_cruise = sm['carState'].cruiseState.speed
steering_wheel_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or sm['carState'].steeringPressed
v_cruise_last = v_cruise
# Get status from our own instance of SteeringStatus
steering_status.update(Events(), steering_wheel_engaged,
sm['carState'].cruiseState.enabled,
sm['carState'].vEgo)
steering_state = steering_status.hands_on_wheel_state
state_name = "Unknown "
if steering_state == HandsOnWheelState.none:
state_name = "Not Active "
elif steering_state == HandsOnWheelState.ok:
state_name = "Hands On Wheel "
elif steering_state == HandsOnWheelState.minor:
state_name = "Hands Off Wheel - Minor "
elif steering_state == HandsOnWheelState.warning:
state_name = "Hands Off Wheel - Warning "
elif steering_state == HandsOnWheelState.critical:
state_name = "Hands Off Wheel - Critical"
elif steering_state == HandsOnWheelState.terminal:
state_name = "Hands Off Wheel - Terminal"
# Get events from `dMonitoringState`
events = sm['dMonitoringState'].events
event_name = events[0].name if len(events) else "None"
event_name = "{:<30}".format(event_name[:30])
# Print output
sys.stdout.write(
f'\rSteering State: {state_name} | event: {event_name}')
sys.stdout.flush()
except Exception as e:
print(e)
def main():
sm = messaging.SubMaster(['ubloxGnss'])
pm = messaging.PubMaster(['gnssMeasurements'])
laikad = Laikad(use_internet=True)
while True:
sm.update()
if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = laikad.process_ublox_msg(ublox_msg,
sm.logMonoTime['ubloxGnss'])
if msg is not None:
pm.send('gnssMeasurements', msg)
def manager_thread() -> None:
Process(name="road_speed_limiter", target=launcher, args=("selfdrive.road_speed_limiter", "road_speed_limiter")).start()
cloudlog.bind(daemon="manager")
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
ignore: List[str] = []
if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
ignore += ["manage_athenad", "uploader"]
if os.getenv("NOBOARD") is not None:
ignore.append("pandad")
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
ensure_running(managed_processes.values(), started=False, not_run=ignore)
sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
ensure_running(managed_processes.values(), started=started, driverview=driverview, notcar=sm['carParams'].notCar, not_run=ignore)
running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
print(running)
cloudlog.debug(running)
# send managerState
msg = messaging.new_message('managerState')
msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()]
pm.send('managerState', msg)
# Exit main loop when uninstall/shutdown/reboot is needed
shutdown = False
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
shutdown = True
params.put("LastManagerExitReason", param)
cloudlog.warning(f"Shutting down manager - {param} set")
if shutdown:
break
def setUpClass(cls):
if "DEBUG" in os.environ:
segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog.bz2")), Path(ROOT).iterdir())
segs = sorted(segs, key=lambda x: x.stat().st_mtime)
cls.lr = list(LogReader(os.path.join(segs[-2], "rlog.bz2")))
return
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019"
set_params_enabled()
# Make sure athena isn't running
Params().delete("DongleId")
Params().delete("AthenadPid")
os.system("pkill -9 -f athena")
logger_root = Path(ROOT)
initial_segments = set()
if logger_root.exists():
initial_segments = set(Path(ROOT).iterdir())
# start manager and run openpilot for a minute
try:
manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py")
proc = subprocess.Popen(["python", manager_path])
sm = messaging.SubMaster(['carState'])
with Timeout(150, "controls didn't start"):
while sm.rcv_frame['carState'] < 0:
sm.update(1000)
# make sure we get at least two full segments
cls.segments = []
with Timeout(300, "timed out waiting for logs"):
while len(cls.segments) < 3:
new_paths = set()
if logger_root.exists():
new_paths = set(logger_root.iterdir()) - initial_segments
segs = [p for p in new_paths if "--" in str(p)]
cls.segments = sorted(segs, key=lambda s: int(str(s).rsplit('--')[-1]))
time.sleep(5)
finally:
proc.terminate()
if proc.wait(60) is None:
proc.kill()
cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog.bz2")))
def main():
dog = AstroDog()
sm = messaging.SubMaster(['ubloxGnss'])
pm = messaging.PubMaster(['gnssMeasurements'])
while True:
sm.update()
# Todo if no internet available use latest ephemeris
if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = process_ublox_msg(ublox_msg, dog,
sm.logMonoTime['ubloxGnss'])
if msg is None:
msg = messaging.new_message('gnssMeasurements')
pm.send('gnssMeasurements', msg)
def gpxd_thread(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['gpsLocationExternal', 'carState'])
wait_helper = WaitTimeHelper()
gpxd = GpxD()
rk = Ratekeeper(LOG_HERTZ, print_delay_threshold=None)
while True:
sm.update(0)
gpxd.log(sm)
gpxd.write_log()
if wait_helper.shutdown:
gpxd.write_log(True)
break
rk.keep_time()
def locationd_thread(sm, pm, disabled_logs=None):
if disabled_logs is None:
disabled_logs = []
if sm is None:
socks = [
'gpsLocationExternal', 'sensorEvents', 'cameraOdometry',
'liveCalibration', 'carState'
]
sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'])
if pm is None:
pm = messaging.PubMaster(['liveLocationKalman'])
localizer = Localizer(disabled_logs=disabled_logs)
camera_odometry_cnt = 0
while True:
sm.update()
for sock, updated in sm.updated.items():
if updated:
t = sm.logMonoTime[sock] * 1e-9
if sock == "sensorEvents":
localizer.handle_sensors(t, sm[sock])
elif sock == "gpsLocationExternal":
localizer.handle_gps(t, sm[sock])
elif sock == "carState":
localizer.handle_car_state(t, sm[sock])
elif sock == "cameraOdometry":
localizer.handle_cam_odo(t, sm[sock])
elif sock == "liveCalibration":
localizer.handle_live_calib(t, sm[sock])
if sm.updated['cameraOdometry']:
camera_odometry_cnt += 1
if camera_odometry_cnt % OUTPUT_DECIMATION == 0:
t = sm.logMonoTime['cameraOdometry']
msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
gps_age = (t / 1e9) - localizer.last_gps_fix
msg.liveLocationKalman.gpsOK = gps_age < 1.0
pm.send('liveLocationKalman', msg)
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
frame_sizes = [eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size]
frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes}
sockets = []
if frame is not None:
sockets.append(frame)
if front_frame is not None:
sockets.append(front_frame)
sm = messaging.SubMaster(sockets)
while min(sm.logMonoTime.values()) == 0:
sm.update()
rear = extract_image(sm[frame].image, frame_sizes) if frame is not None else None
front = extract_image(sm[front_frame].image, frame_sizes) if front_frame is not None else None
return rear, front
