def _process_commands(self, cmd, route, pub_types):
seek_to = None
if route is None or (isinstance(cmd, SetRoute) and route.name != cmd.name):
seek_to = cmd.start_time
route = Route(cmd.name, cmd.data_dir)
self._lr = MultiLogIterator(route.log_paths(), wraparound=True)
if self._frame_reader is not None:
self._frame_reader.close()
if "frame" in pub_types or "encodeIdx" in pub_types:
# reset frames for a route
self._frame_id_lookup = {}
self._frame_reader = RouteFrameReader(
route.camera_paths(), None, self._frame_id_lookup, readahead=True)
# always reset this on a seek
if isinstance(cmd, SeekRelativeTime):
seek_to = self._lr.tell() + cmd.secs
elif isinstance(cmd, SeekAbsoluteTime):
seek_to = cmd.secs
elif isinstance(cmd, StopAndQuit):
exit()
if seek_to is not None:
print("seeking", seek_to)
if not self._lr.seek(seek_to):
print("Can't seek: time out of bounds")
else:
next(self._lr) # ignore one
return route
def main(argv):
args = get_arg_parser().parse_args(sys.argv[1:])
if not args.data_dir:
print('Data directory invalid.')
return
if not args.route_name:
# Extract route name from path
args.route_name = os.path.basename(args.data_dir)
args.data_dir = os.path.dirname(args.data_dir)
route = Route(args.route_name, args.data_dir)
lr = MultiLogIterator(route.log_paths())
with open(args.out_path, 'wb') as f:
try:
done = False
i = 0
while not done:
msg = next(lr)
if not msg:
break
smsg = msg.as_builder()
typ = smsg.which()
if typ == 'ubloxRaw':
f.write(smsg.to_bytes())
i += 1
except StopIteration:
print('All done')
print(f'Writed {i} msgs')
from tools.lib.logreader import MultiLogIterator
from xx.chffr.lib.route import Route # pylint: disable=import-error
def get_fingerprint(lr):
can_msgs = [m for m in lr if m.which() == 'can']
msgs = {}
for msg in can_msgs:
for c in msg.can:
# read also msgs sent by EON on CAN bus 0x80 and filter out the
# addr with more than 11 bits
if c.src % 0x80 == 0 and c.address < 0x800:
msgs[c.address] = len(c.dat)
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
print("number of messages {0}:".format(len(msgs)))
print("fingerprint {0}".format(fingerprint))
if __name__ == "__main__":
if len(sys.argv) < 2:
print("Usage: ./get_fingerprint_internal.py <route>")
sys.exit(1)
route = sys.argv[1]
route = Route(route)
lr = MultiLogIterator(route.log_paths()[:5], wraparound=False)
get_fingerprint(lr)
class UnloggerWorker(object):
def __init__(self):
self._frame_reader = None
self._cookie = None
self._readahead = deque()
def run(self, commands_address, data_address, pub_types):
zmq.Context._instance = None
commands_socket = zmq.Context.instance().socket(zmq.PULL)
commands_socket.connect(commands_address)
data_socket = zmq.Context.instance().socket(zmq.PUSH)
data_socket.connect(data_address)
poller = zmq.Poller()
poller.register(commands_socket, zmq.POLLIN)
# We can't publish frames without encodeIdx, so add when it's missing.
if "frame" in pub_types:
pub_types["encodeIdx"] = None
# gc.set_debug(gc.DEBUG_LEAK | gc.DEBUG_OBJECTS | gc.DEBUG_STATS | gc.DEBUG_SAVEALL |
# gc.DEBUG_UNCOLLECTABLE)
# TODO: WARNING pycapnp leaks memory all over the place after unlogger runs for a while, gc
# pauses become huge because there are so many tracked objects solution will be to switch to new
# cython capnp
try:
route = None
while True:
while poller.poll(0.) or route is None:
cookie, cmd = commands_socket.recv_pyobj()
route = self._process_commands(cmd, route, pub_types)
# **** get message ****
self._read_logs(cookie, pub_types)
self._send_logs(data_socket)
finally:
if self._frame_reader is not None:
self._frame_reader.close()
data_socket.close()
commands_socket.close()
def _read_logs(self, cookie, pub_types):
fullHEVC = capnp_log.EncodeIndex.Type.fullHEVC
lr = self._lr
while len(self._readahead) < 1000:
route_time = lr.tell()
msg = next(lr)
typ = msg.which()
if typ not in pub_types:
continue
# **** special case certain message types ****
if typ == "encodeIdx" and msg.encodeIdx.type == fullHEVC:
# this assumes the encodeIdx always comes before the frame
self._frame_id_lookup[
msg.encodeIdx.frameId] = msg.encodeIdx.segmentNum, msg.encodeIdx.segmentId
#print "encode", msg.encodeIdx.frameId, len(self._readahead), route_time
self._readahead.appendleft((typ, msg, route_time, cookie))
def _send_logs(self, data_socket):
while len(self._readahead) > 500:
typ, msg, route_time, cookie = self._readahead.pop()
smsg = msg.as_builder()
if typ == "frame":
frame_id = msg.frame.frameId
# Frame exists, make sure we have a framereader.
# load the frame readers as needed
s1 = time.time()
try:
img = self._frame_reader.get(frame_id, pix_fmt="rgb24")
except Exception:
img = None
fr_time = time.time() - s1
if fr_time > 0.05:
print("FRAME(%d) LAG -- %.2f ms" % (frame_id, fr_time*1000.0))
if img is not None:
img = img[:, :, ::-1] # Convert RGB to BGR, which is what the camera outputs
img = img.flatten()
bts = img.tobytes()
smsg.frame.image = bts
extra = (smsg.frame.frameId, smsg.frame.timestampSof, smsg.frame.timestampEof)
data_socket.send_pyobj((cookie, VIPC_TYP, msg.logMonoTime, route_time, extra), flags=zmq.SNDMORE)
data_socket.send(bts, copy=False)
data_socket.send_pyobj((cookie, typ, msg.logMonoTime, route_time), flags=zmq.SNDMORE)
data_socket.send(smsg.to_bytes(), copy=False)
def _process_commands(self, cmd, route, pub_types):
seek_to = None
if route is None or (isinstance(cmd, SetRoute) and route.name != cmd.name):
seek_to = cmd.start_time
route = Route(cmd.name, cmd.data_dir)
self._lr = MultiLogIterator(route.log_paths(), wraparound=True)
if self._frame_reader is not None:
self._frame_reader.close()
if "frame" in pub_types or "encodeIdx" in pub_types:
# reset frames for a route
self._frame_id_lookup = {}
self._frame_reader = RouteFrameReader(
route.camera_paths(), None, self._frame_id_lookup, readahead=True)
# always reset this on a seek
if isinstance(cmd, SeekRelativeTime):
seek_to = self._lr.tell() + cmd.secs
elif isinstance(cmd, SeekAbsoluteTime):
seek_to = cmd.secs
elif isinstance(cmd, StopAndQuit):
exit()
if seek_to is not None:
print("seeking", seek_to)
if not self._lr.seek(seek_to):
print("Can't seek: time out of bounds")
else:
next(self._lr) # ignore one
return route
# read also msgs sent by EON on CAN bus 0x80 and filter out the
# addr with more than 11 bits
if c.src % 0x80 == 0 and c.address < 0x800:
msgs[c.address] = len(c.dat)
# show CAN fingerprint
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
print(f"\nfound {len(msgs)} messages. CAN fingerprint:\n")
print(fingerprint)
# TODO: also print the fw fingerprint merged with the existing ones
# show FW fingerprint
print("\nFW fingerprint:\n")
for f in fw:
print(
f" (Ecu.{f.ecu}, {hex(f.address)}, {None if f.subAddress == 0 else f.subAddress}): ["
)
print(f" {f.fwVersion},")
print(" ],")
print()
if __name__ == "__main__":
if len(sys.argv) < 2:
print("Usage: ./fingerprint_from_route.py <route>")
sys.exit(1)
route = Route(sys.argv[1])
lr = MultiLogIterator(route.log_paths()[:5])
get_fingerprint(lr)
elif m.address == 0x260 and m.src == 0:
eps_torque = to_signed((m.dat[5] << 8) | m.dat[6], 16)
if engaged and torque_cmd is not None and eps_torque is not None:
cmds.append(torque_cmd)
eps.append(eps_torque)
else:
if len(cmds) > MIN_SAMPLES:
break
cmds, eps = [], []
if len(cmds) < MIN_SAMPLES:
raise Exception("too few samples found in route")
lm = linear_model.LinearRegression(fit_intercept=False)
lm.fit(np.array(cmds).reshape(-1, 1), eps)
scale_factor = 1. / lm.coef_[0]
if plot:
plt.plot(np.array(eps) * scale_factor)
plt.plot(cmds)
plt.show()
return scale_factor
if __name__ == "__main__":
r = Route(sys.argv[1])
lr = MultiLogIterator(r.log_paths(), wraparound=False)
n = get_eps_factor(lr, plot="--plot" in sys.argv)
print("EPS torque factor: ", n)
if not recv:
rx_invalid += 1
invalid_addrs.add(canmsg.address)
rx_tot += 1
print("\nRX")
print("total rx msgs:", rx_tot)
print("invalid rx msgs:", rx_invalid)
print("invalid addrs:", invalid_addrs)
print("\nTX")
print("total openpilot msgs:", tx_tot)
print("total msgs with controls allowed:", tx_controls)
print("blocked msgs:", tx_blocked)
print("blocked with controls allowed:", tx_controls_blocked)
print("blocked addrs:", blocked_addrs)
return tx_controls_blocked == 0 and rx_invalid == 0
if __name__ == "__main__":
from tools.lib.route import Route
from tools.lib.logreader import MultiLogIterator # pylint: disable=import-error
mode = int(sys.argv[2])
param = 0 if len(sys.argv) < 4 else int(sys.argv[3])
r = Route(sys.argv[1])
lr = MultiLogIterator(r.log_paths())
print("replaying drive %s with safety mode %d and param %d" % (sys.argv[1], mode, param))
replay_drive(lr, mode, param)
