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)