def main(rate=100):
prius_can = Prius()
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
while 1:
prius_can.recv(identifier='all')
rk.keep_time()
def main(rate=100):
global can_recv
global cap
global thread2
cap = cv2.VideoCapture(DATA_PATH + '/0524.avi')
thread_stop = threading.Event()
thread = threading.Thread(target=upload_data, args=(thread_stop, ))
thread.start()
thread2 = threading.Thread(target=video, args=(thread_stop, ))
thread2.start()
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
try:
while 1:
# prius_can.recv_print('dbc')
can_recv = prius_can.recv()
# RUN in 20 Hz
# if (rk.frame % (rate / 100)) == 0:
# print(can_recv)
rk.keep_time()
except KeyboardInterrupt:
thread_stop.set()
sys.exit()
def boardd_proxy_loop(rate=100, address="192.168.2.251"):
rk = Ratekeeper(rate)
can_init()
# *** subscribes can
logcan = messaging.sub_sock('can', addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock('sendcan')
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
def send_thread(sender, core):
config_realtime_process(core, 55)
if "Jungle" in str(type(sender)):
for i in [0, 1, 2, 3, 0xFFFF]:
sender.can_clear(i)
sender.set_ignition(False)
time.sleep(5)
sender.set_ignition(True)
sender.set_panda_power(True)
else:
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
sender.set_can_loopback(False)
log_idx = 0
rk = Ratekeeper(100)
while True:
snd = CAN_MSGS[log_idx]
log_idx = (log_idx + 1) % len(CAN_MSGS)
snd = list(filter(lambda x: x[-1] <= 2, snd))
sender.can_send_many(snd)
# Drain panda message buffer
sender.can_recv()
rk.keep_time()
def boardd_proxy_loop(rate=200, address="192.168.2.251"):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** subscribes can
logcan = messaging.sub_sock(context, service_list['can'].port, addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print "R:",hex(m[0]), str(m[2]).encode("hex")
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat.to_bytes())
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print "S:",hex(m[0]), str(m[2]).encode("hex")
can_send_many(cl)
rk.keep_time()
def replay_panda_states(s, msgs):
pm = messaging.PubMaster([s, 'peripheralState'])
rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
smsgs = [
m for m in msgs
if m.which() in ['pandaStates', 'pandaStateDEPRECATED']
]
# Migrate safety param base on carState
cp = [m for m in msgs if m.which() == 'carParams'][0].carParams
if len(cp.safetyConfigs):
safety_param = cp.safetyConfigs[0].safetyParam
else:
safety_param = cp.safetyParamDEPRECATED
while True:
for m in smsgs:
if m.which() == 'pandaStateDEPRECATED':
new_m = messaging.new_message('pandaStates', 1)
new_m.pandaStates[0] = m.pandaStateDEPRECATED
new_m.pandaStates[0].safetyParam = safety_param
pm.send(s, new_m)
else:
new_m = m.as_builder()
new_m.logMonoTime = int(sec_since_boot() * 1e9)
pm.send(s, new_m)
new_m = messaging.new_message('peripheralState')
pm.send('peripheralState', new_m)
rk.keep_time()
def main():
op_params = opParams()
rk = Ratekeeper(1.0, print_delay_threshold=None)
sm = messaging.SubMaster(['model'])
while 1:
sm.update()
log_text = 'not_valid:\n'
service_list = sm.valid.keys()
for s in service_list:
if not sm.valid[s]:
log_text += str(s)
log_text += '\n'
log_text += 'not_alive:\n'
service_list = sm.alive.keys()
for s in service_list:
if not sm.alive[s]:
if s not in sm.ignore_alive:
log_text += str(s)
log_text += '\n'
text_file = open("/tmp/test.txt", "wt")
text_file.write(log_text)
text_file.close()
#sm.update()
rk.keep_time()
def send_thread(s, flock):
if "Jungle" in str(type(s)):
if "FLASH" in os.environ:
with flock:
s.flash()
for i in [0, 1, 2, 3, 0xFFFF]:
s.can_clear(i)
s.set_ignition(False)
time.sleep(5)
s.set_ignition(True)
s.set_panda_power(True)
else:
s.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
s.set_can_loopback(False)
idx = 0
ign = True
rk = Ratekeeper(1 / DT_CTRL, print_delay_threshold=None)
while True:
# handle ignition cycling
if ENABLE_IGN:
i = (rk.frame*DT_CTRL) % (IGN_ON + IGN_OFF) < IGN_ON
if i != ign:
ign = i
s.set_ignition(ign)
snd = CAN_MSGS[idx]
snd = list(filter(lambda x: x[-1] <= 2, snd))
s.can_send_many(snd)
idx = (idx + 1) % len(CAN_MSGS)
# Drain panda message buffer
s.can_recv()
rk.keep_time()
def replay_camera(s, stream, dt, vipc_server, frames, size,
use_extra_client):
services = [(s, stream)]
if use_extra_client:
services.append(("wideRoadCameraState",
VisionStreamType.VISION_STREAM_WIDE_ROAD))
pm = messaging.PubMaster([s for s, _ in services])
rk = Ratekeeper(1 / dt, print_delay_threshold=None)
img = b"\x00" * int(size[0] * size[1] * 3 / 2)
while True:
if frames is not None:
img = frames[rk.frame % len(frames)]
rk.keep_time()
for s, stream in services:
m = messaging.new_message(s)
msg = getattr(m, s)
msg.frameId = rk.frame
msg.timestampSof = m.logMonoTime
msg.timestampEof = m.logMonoTime
pm.send(s, m)
vipc_server.send(stream, img, msg.frameId, msg.timestampSof,
msg.timestampEof)
def main(s_ip):
ip = '192.168.3.3'
if s_ip:
ip = s_ip
sync_sock, sync_content = create_sub_sock(ip, timeout=1000)
status = ping(ip)
print('ping ' + ip + ' status=' + str(status))
rk = Ratekeeper(5.0, print_delay_threshold=None)
while 1:
sync_data = None
if sync_sock:
sync_data = sync_sock.receive_golden()
if sync_data:
sync_data_str = sync_data.decode("utf-8")
#print (sync_data_str)
parsed_json = json.loads(sync_data_str)
print(parsed_json)
rk.keep_time()
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 replay_service(s, msgs):
pm = messaging.PubMaster([s, ])
rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
smsgs = [m for m in msgs if m.which() == s]
while True:
for m in smsgs:
# TODO: use logMonoTime
pm.send(s, m.as_builder())
rk.keep_time()
def sample_power(seconds=5) -> List[float]:
rate = 123
rk = Ratekeeper(rate, print_delay_threshold=None)
pwrs = []
for _ in range(rate*seconds):
pwrs.append(read_power())
rk.keep_time()
return pwrs
def replay_manager_state(s, msgs):
pm = messaging.PubMaster([s, ])
rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
while True:
new_m = messaging.new_message('managerState')
new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes]
pm.send(s, new_m)
rk.keep_time()
def replay_service(s, msgs):
pm = messaging.PubMaster([s, ])
rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
smsgs = [m for m in msgs if m.which() == s]
while True:
for m in smsgs:
new_m = m.as_builder()
new_m.logMonoTime = int(sec_since_boot() * 1e9)
pm.send(s, new_m)
rk.keep_time()
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', '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 sample_power(seconds=5) -> List[float]:
rate = 123
rk = Ratekeeper(rate, print_delay_threshold=None)
pwrs = []
for _ in range(rate * seconds):
with open(
"/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
pwrs.append(int(f.read()) / 1e6)
rk.keep_time()
return pwrs
def health_function():
pm = messaging.PubMaster(['health'])
rk = Ratekeeper(1.0)
while 1:
dat = messaging.new_message('health')
dat.valid = True
dat.health = {
'ignitionLine': True,
'hwType': "whitePanda",
'controlsAllowed': True
}
pm.send('health', dat)
rk.keep_time()
def main():
rk = Ratekeeper(5.0, print_delay_threshold=None)
pm = messaging.PubMaster(['LiveMapData'])
while 1:
time.sleep(1)
speed_limit = 60.0
has_exit = True
dist_to_next_step = 1000.0
remain_dist = 2500.0
nav_icon = 2
dat = messaging.new_message('LiveMapData')
dat.valid = True
# struct LiveMapData {
# speedLimitValid @0 :Bool;
# speedLimit @1 :Float32;
# speedAdvisoryValid @12 :Bool;
# speedAdvisory @13 :Float32;
# speedLimitAheadValid @14 :Bool;
# speedLimitAhead @15 :Float32;
# speedLimitAheadDistance @16 :Float32;
# curvatureValid @2 :Bool;
# curvature @3 :Float32;
# wayId @4 :UInt64;
# roadX @5 :List(Float32);
# roadY @6 :List(Float32);
# lastGps @7: GpsLocationData;
# roadCurvatureX @8 :List(Float32);
# roadCurvature @9 :List(Float32);
# distToTurn @10 :Float32;
# mapValid @11 :Bool;
# }
live_map_data = dat.LiveMapData
live_map_data.speedLimit = speed_limit * 1.08
live_map_data.distToTurn = dist_to_next_step
live_map_data.speedAdvisoryValid = has_exit
live_map_data.speedAdvisory = remain_dist
live_map_data.wayId = nav_icon
pm.send('LiveMapData', dat)
#sm.update()
rk.keep_time()
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 replay_camera(s, stream, dt, vipc_server, frames, size):
pm = messaging.PubMaster([s, ])
rk = Ratekeeper(1 / dt, print_delay_threshold=None)
img = b"\x00" * int(size[0]*size[1]*3/2)
while True:
if frames is not None:
img = frames[rk.frame % len(frames)]
rk.keep_time()
m = messaging.new_message(s)
msg = getattr(m, s)
msg.frameId = rk.frame
pm.send(s, m)
vipc_server.send(stream, img, msg.frameId, msg.timestampSof, msg.timestampEof)
def boardd_loop(rate=200):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** publishes can and health
logcan = messaging.pub_sock(context, service_list['can'].port)
health_sock = messaging.pub_sock(context, service_list['health'].port)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 1hz
if (rk.frame % rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
msg.health.controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
def main():
frame_cache = LRU(16)
md_cache = LRU(16)
plan_cache = LRU(16)
frame_sock = messaging.sub_sock('frame')
md_sock = messaging.sub_sock('model')
plan_sock = messaging.sub_sock('plan')
controls_state_sock = messaging.sub_sock('controlsState')
proc = messaging.sub_sock('procLog')
pls = [None, None]
rk = Ratekeeper(10)
while True:
for msg in messaging.drain_sock(frame_sock):
frame_cache[msg.frame.frameId] = msg
for msg in messaging.drain_sock(md_sock):
md_cache[msg.logMonoTime] = msg
for msg in messaging.drain_sock(plan_sock):
plan_cache[msg.logMonoTime] = msg
controls_state = messaging.recv_sock(controls_state_sock)
if controls_state is not None:
plan_time = controls_state.controlsState.planMonoTime
if plan_time != 0 and plan_time in plan_cache:
plan = plan_cache[plan_time]
md_time = plan.plan.mdMonoTime
if md_time != 0 and md_time in md_cache:
md = md_cache[md_time]
frame_id = md.model.frameId
if frame_id != 0 and frame_id in frame_cache:
frame = frame_cache[frame_id]
print("controls lag: %.2fms" %
((controls_state.logMonoTime -
frame.frame.timestampEof) / 1e6))
pls = (pls + messaging.drain_sock(proc))[-2:]
if None not in pls:
display_cpu(*pls)
rk.keep_time()
def boardd_loop(rate=100):
rk = Ratekeeper(rate)
can_init()
# *** publishes can and health
logcan = messaging.pub_sock('can')
health_sock = messaging.pub_sock('pandaStates')
# *** subscribes to can send
sendcan = messaging.sub_sock('sendcan')
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 2hz
if (rk.frame % (rate // 2)) == 0:
health = can_health()
msg = messaging.new_message('pandaStates', 1)
# store the health to be logged
msg.pandaState[0].voltage = health['voltage']
msg.pandaState[0].current = health['current']
msg.pandaState[0].ignitionLine = health['ignition_line']
msg.pandaState[0].ignitionCan = health['ignition_can']
msg.pandaState[0].controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
def mapd_thread(sm=None, pm=None):
mapd = MapD()
rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz
# *** setup messaging
if sm is None:
sm = messaging.SubMaster(['gpsLocationExternal', 'controlsState'])
if pm is None:
pm = messaging.PubMaster(['liveMapData'])
while True:
sm.update()
mapd.udpate_state(sm)
mapd.update_gps(sm)
mapd.updated_osm_data()
mapd.update_route()
mapd.publish(pm, sm)
rk.keep_time()
def replay_camera(s, stream, dt, vipc_server, fr, size):
pm = messaging.PubMaster([s, ])
rk = Ratekeeper(1 / dt, print_delay_threshold=None)
img = b"\x00" * int(size[0]*size[1]*3/2)
while True:
if fr is not None:
img = fr.get(rk.frame % fr.frame_count, pix_fmt='yuv420p')[0]
img = img.flatten().tobytes()
rk.keep_time()
m = messaging.new_message(s)
msg = getattr(m, s)
msg.frameId = rk.frame
pm.send(s, m)
vipc_server.send(stream, img, msg.frameId, msg.timestampSof, msg.timestampEof)
def main():
op_params = opParams()
rk = Ratekeeper(1.0, print_delay_threshold=None)
sm = messaging.SubMaster(['liveMapData'])
while 1:
sm.update()
if sm.updated['liveMapData']:
print(sm['liveMapData'])
camera_offset = op_params.get('camera_offset')
print(camera_offset)
#sm.update()
rk.keep_time()
def replay_panda_states(s, msgs):
pm = messaging.PubMaster([s, 'peripheralState'])
rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None)
smsgs = [
m for m in msgs
if m.which() in ['pandaStates', 'pandaStateDEPRECATED']
]
# TODO: new safety params from flags, remove after getting new routes for Toyota
safety_param_migration = {
"TOYOTA PRIUS 2017":
EPS_SCALE["TOYOTA PRIUS 2017"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL,
"TOYOTA RAV4 2017":
EPS_SCALE["TOYOTA RAV4 2017"] | Panda.FLAG_TOYOTA_ALT_BRAKE,
}
# Migrate safety param base on carState
cp = [m for m in msgs if m.which() == 'carParams'][0].carParams
if cp.carFingerprint in safety_param_migration:
safety_param = safety_param_migration[cp.carFingerprint]
elif len(cp.safetyConfigs):
safety_param = cp.safetyConfigs[0].safetyParam
if cp.safetyConfigs[0].safetyParamDEPRECATED != 0:
safety_param = cp.safetyConfigs[0].safetyParamDEPRECATED
else:
safety_param = cp.safetyParamDEPRECATED
while True:
for m in smsgs:
if m.which() == 'pandaStateDEPRECATED':
new_m = messaging.new_message('pandaStates', 1)
new_m.pandaStates[0] = m.pandaStateDEPRECATED
new_m.pandaStates[0].safetyParam = safety_param
pm.send(s, new_m)
else:
new_m = m.as_builder()
new_m.logMonoTime = int(sec_since_boot() * 1e9)
pm.send(s, new_m)
new_m = messaging.new_message('peripheralState')
pm.send('peripheralState', new_m)
rk.keep_time()
def send_thread(joystick):
joystick_sock = messaging.pub_sock('testJoystick')
rk = Ratekeeper(100, print_delay_threshold=None)
while 1:
dat = messaging.new_message('testJoystick')
dat.testJoystick.axes = [
joystick.axes_values[a] for a in joystick.axes_order
]
dat.testJoystick.buttons = [joystick.cancel]
joystick_sock.send(dat.to_bytes())
print('\n' + ', '.join(f'{name}: {round(v, 3)}'
for name, v in joystick.axes_values.items()))
if "WEB" in os.environ:
import requests
requests.get(
"http://" + os.environ["WEB"] + ":5000/control/%f/%f" %
tuple([joystick.axes_values[a]
for a in joystick.axes_order][::-1]))
rk.keep_time()
def main(rate=100):
prius_can = Prius()
joystick = Joystick()
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
loopQuit = False
lock = False
# Send CAN Bus 100Hz
while not loopQuit:
left_y, right_y, start, loopQuit = joystick.update()
if left_y > 0:
speed = np.ceil(left_y * 65025) # 65535
else:
speed = 0
# send car speed signal
prius_can.send_speed(speed=speed)
print 'Speed: {}'.format(speed * 0.0062)
# send door lock signal
if start:
prius_can.send_door_lock(lock)
rk.keep_time()
