def debug2dir(filename, out_dir): from osgar.logger import LogReader, lookup_stream_names from osgar.lib.serialize import deserialize names = lookup_stream_names(filename) assert 'detector.debug_artf' in names, names assert 'detector.artf' in names, names assert 'rosmsg.sim_time_sec' in names, names image_id = names.index('detector.debug_artf') + 1 artf_id = names.index('detector.artf') + 1 sim_sec_id = names.index('rosmsg.sim_time_sec') + 1 sim_time_sec = None image = None artf = None for dt, channel, data in LogReader( filename, only_stream_id=[image_id, artf_id, sim_sec_id]): data = deserialize(data) if channel == sim_sec_id: sim_time_sec = data elif channel == image_id: image = data assert artf is not None time_sec = sim_time_sec if sim_time_sec is not None else int( dt.total_seconds()) name = os.path.basename(filename)[:-4] + '-' + artf[0] + '-' + str( time_sec) + '.jpg' print(name) with open(os.path.join(out_dir, name), 'wb') as f: f.write(image) elif channel == artf_id: artf = data
def main(): import argparse parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('logfile', help='input log file') parser.add_argument('--keep', help='stream ID or names to keep', nargs='+') parser.add_argument('--remove', help='stream ID or names to be removed', nargs='+') parser.add_argument('--out', '-o', help='output logfile') args = parser.parse_args() if args.keep is None and args.remove is None: only_stream = None else: only_stream = [ 0, ] if args.keep is not None: names = args.keep elif args.remove is not None: names = set(lookup_stream_names(args.logfile)) - set(args.remove) for name in names: only_stream.append(lookup_stream_id(args.logfile, name)) strip_logfile(args.logfile, only_stream, args.out)
def main(): import argparse import sys import io from pprint import pprint parser = argparse.ArgumentParser(description='') parser.add_argument('log', help='filepaths', nargs='+') parser.add_argument('--threads', help='how many threads to use', type=int, default=1) parser.add_argument('--gui', help='show found tags', default=False, action='store_true') parser.add_argument('--margin', help='apriltag decision margin threshold', default=30, type=int) args = parser.parse_args() detector = apriltag.apriltag('tag16h5', threads=args.threads) for filepath in args.log: print(filepath) streams = lookup_stream_names(filepath) processing = [] for i, stream in enumerate(streams): if stream == "camera.raw" or stream.endswith('.jpg') or stream.endswith('.image'): print("processing stream {} => {}".format(i, stream)) processing.append(i+1) if len(processing) == 0: print("no jpeg found in streams:") pprint(streams) continue with LogReader(filepath, only_stream_id=processing) as log: for dt, channel, data in log: try: jpeg = deserialize(data) except Exception as e: print(e) continue np_jpeg = numpy.frombuffer(jpeg, dtype='u1') gray = cv2.imdecode(np_jpeg, cv2.IMREAD_GRAYSCALE) found = detector.detect(gray) found = [tag for tag in found if tag['margin'] > args.margin and tag['hamming'] == 0] if len(found) > 0: ids = list(tag['id'] for tag in found) print(dt, end=' ') if args.gui: img = cv2.imdecode(np_jpeg, cv2.IMREAD_COLOR) for tag in found: rect = tag['lb-rb-rt-lt'].astype('float32') area = cv2.contourArea(rect) print('{:2d}: {{margin: {:3d}, area: {:4d}}}'.format(tag['id'], int(tag['margin']), int(area)), end=' ') center = tuple(tag['center'].astype(int)) poly = rect.astype('int32') if args.gui: cv2.circle(img, center, 3, (0, 0 ,255), -1) cv2.polylines(img, [poly], True, (0, 255, 255), 3) print() if args.gui: cv2.imshow('image', img) key = cv2.waitKey(0) if key == 27: return
def main(): import signal logging.basicConfig( level=logging.DEBUG, format='%(name)-12s %(levelname)-8s %(message)s', #datefmt='%Y-%m-%d %H:%M', ) logging.getLogger("matplotlib").setLevel(logging.INFO) parser = argparse.ArgumentParser(description='Replay module from log') parser.add_argument('logfile', help='recorded log file') parser.add_argument('--force', '-F', dest='force', action='store_true', help='force replay even for failing output asserts') parser.add_argument('--config', nargs='+', help='force alternative configuration file') parser.add_argument('--module', help='module name for analysis') # TODO default "all" parser.add_argument('--verbose', '-v', help="verbose mode", action='store_true') parser.add_argument('--draw', help="draw debug results", action='store_true') parser.add_argument('--debug', help="print debug info about I/O streams", action='store_true') args = parser.parse_args() if args.module is None: modules = set() for s in logger.lookup_stream_names(args.logfile): modules.add(s.split('.')[0]) print("Modules available for replay with `--module`:") for m in modules: print(" ", m) return module_instance = replay(args) module_instance.verbose = args.verbose signal.signal(signal.SIGINT, lambda signum, frame: module_instance.request_stop()) module_instance.start() # now wait until the module is alive while module_instance.is_alive(): module_instance.join(0.2) if not args.force: print("maximum delay:", module_instance.bus.max_delay) if args.draw: module_instance.draw()
def autodetect_pose3d(logfile): streams = lookup_stream_names(logfile) nodes = set(s.split('.')[0] for s in streams if s.endswith('.pose3d')) if len(nodes) == 1: return nodes.pop()+'.pose3d' if 'offseter' in nodes: return 'offseter.pose3d' if 'localization' in nodes: return 'localization.pose3d' assert False, f"pose3d stream autodetection failed: {nodes}"
def replay(args, application=None): log = LogReader(args.logfile, only_stream_id=0) print("original args:", next(log)[-1]) # old arguments config_str = next(log)[-1] config = literal_eval(config_str.decode('ascii')) if args.config is not None: config = config_load(*args.config, application=application) names = logger.lookup_stream_names(args.logfile) if args.debug: print("streams:") for i, name in enumerate(names): print(f" {i+1:2d} {name}") module = args.module assert module in config['robot']['modules'], ( module, list(config['robot']['modules'].keys())) module_config = config['robot']['modules'][module] inputs = {} for edge_from, edge_to in config['robot']['links']: if edge_to.split('.')[0] == module: if edge_from not in names: g_logger.warning('Missing name: %s' % edge_from) names.append(edge_from) inputs[1 + names.index(edge_from)] = edge_to.split('.')[1] outputs = { i + 1: out.split('.')[1] for i, out in enumerate(names) if out.startswith(f"{module}.") } if args.debug: print("inputs:") for i, name in sorted(inputs.items()): print(f" {i:2d} {name}") print("outputs:") for i, name in sorted(outputs.items()): print(f" {i:2d} {name}") if args.force: reader = LogReader(args.logfile, only_stream_id=inputs.keys()) bus = LogBusHandlerInputsOnly(reader, inputs=inputs) else: streams = list(inputs.keys()) + list(outputs.keys()) reader = LogReader(args.logfile, only_stream_id=streams) bus = LogBusHandler(reader, inputs, outputs) driver_name = module_config['driver'] module_class = get_class_by_name(driver_name) module_instance = module_class(module_config.get('init', {}), bus=bus) bus.node = module_instance # needed for slots return module_instance
def replay(args, application=None): log = LogReader(args.logfile, only_stream_id=0) print("original args:", next(log)[-1]) # old arguments config_str = next(log)[-1] config = literal_eval(config_str.decode('ascii')) if args.config is not None: config = config_load(*args.config) names = logger.lookup_stream_names(args.logfile) print("stream names:") for name in names: print(" ", name) module = args.module assert module in config['robot']['modules'], ( module, list(config['robot']['modules'].keys())) module_config = config['robot']['modules'][module] input_names = module_config['in'] output_names = module_config['out'] print("inputs:", input_names) print("outputs:", output_names) inputs = {} for edge_from, edge_to in config['robot']['links']: if edge_to.split('.')[0] == module: if edge_from not in names: logging.warning(f'Missing name: {edge_from}') names.append(edge_from) inputs[1 + names.index(edge_from)] = edge_to.split('.')[1] # start reading log from the beginning again if args.force: log = LogReader(args.logfile, only_stream_id=inputs.keys()) bus = LogBusHandlerInputsOnly(log, inputs=inputs) else: outputs = dict([(1 + names.index('.'.join([module, name])), name) for name in output_names]) streams = list(inputs.keys()) + list(outputs.keys()) log = LogReader(args.logfile, only_stream_id=streams) bus = LogBusHandler(log, inputs=inputs, outputs=outputs) driver_name = module_config['driver'] if driver_name == 'application': assert application is not None module_class = application else: module_class = get_class_by_name(driver_name) module_instance = module_class(module_config['init'], bus=bus) bus.node = module_instance return module_instance
def main(): import argparse import os.path global g_rotation_offset_rad parser = argparse.ArgumentParser(description='View lidar scans') parser.add_argument('logfile', help='recorded log file') parser.add_argument('--legacy', help='use old text lidar log format', action='store_true') parser.add_argument('--lidar', help='stream ID') pose = parser.add_mutually_exclusive_group() pose.add_argument('--pose2d', help='stream ID for pose2d messages') pose.add_argument('--pose3d', help='stream ID for pose3d messages') parser.add_argument('--camera', help='stream ID for JPEG images') parser.add_argument('--callback', help='callback function for lidar scans') parser.add_argument('--rotate', help='rotate poses by angle in degrees, offset', type=float, default=0.0) args = parser.parse_args() if not any([args.lidar, args.pose2d, args.pose3d, args.camera]): print("Available streams:") for stream in lookup_stream_names(args.logfile): print(" ", stream) return callback = None if args.callback is not None: callback = get_class_by_name(args.callback) filename = os.path.basename(args.logfile) g_rotation_offset_rad = math.radians(args.rotate) if args.legacy: lidarview(scans_gen_legacy(args.logfile), caption_filename=filename, callback=callback) else: with Framer(args.logfile, lidar_name=args.lidar, pose2d_name=args.pose2d, pose3d_name=args.pose3d, camera_name=args.camera) as framer: lidarview(framer, caption_filename=filename, callback=callback)
def run_input(self): names = lookup_stream_names(self.filename) print(names) ids = [i + 1 for i, name in enumerate(names) if name in self.pins] print(ids) for timestamp, channel_index, data_raw in LogReader( self.filename, only_stream_id=ids): if not self.bus.is_alive(): break channel = names[channel_index - 1] assert channel in self.pins data = deserialize(data_raw) # TODO reuse timestamp self.bus.publish(self.pins[channel], data) print('Replay completed!')
def run_input(self): names = lookup_stream_names(self.filename) print(names) ids = [i + 1 for i, name in enumerate(names) if name in self.pins] print(ids) for timestamp, channel_index, data_raw in LogReader(self.filename, only_stream_id=ids): if not self.bus.is_alive(): break channel = names[channel_index - 1] assert channel in self.pins data = deserialize(data_raw) # TODO reuse timestamp self.bus.publish(self.pins[channel], data) print('Replay completed!')
def deserialize(self): from osgar.logger import LogReader, lookup_stream_names, lookup_stream_id from osgar.lib.serialize import deserialize streams = lookup_stream_names(self.log_file) with LogReader(self.log_file, only_stream_id=lookup_stream_id(self.log_file, 'can.can')) as log: log_list = [] for timestamp, stream_id, data in log: sec = timestamp.total_seconds() stream_id = stream_id data = deserialize(data) log_list.append( [stream_id, sec, data[0], data[1].hex(), len(data[1])]) return streams, log_list
def scans_gen(logfile, lidar_name=None, poses_name=None, camera_name=None): """ Generator for (timestamp, pose, lidar, image) where freqency is defined by LIDAR and pose and image is used the most recent """ names = lookup_stream_names(logfile) assert not (lidar_name is None and poses_name is None and camera_name is None), names lidar_id, poses_id, camera_id = None, None, None if lidar_name is not None: lidar_id = names.index(lidar_name) + 1 if poses_name is not None: poses_id = names.index(poses_name) + 1 if camera_name is not None: camera_id = names.index(camera_name) + 1 pose = (0, 0, 0) image = None scan = [] eof = False streams = [s for s in [lidar_id, poses_id, camera_id] if s is not None] with LogReader(logfile, only_stream_id=streams) as log: for timestamp, stream_id, data in log: if stream_id == lidar_id: scan = deserialize(data) yield timestamp, pose, scan, image, eof elif stream_id == camera_id: jpeg = deserialize(data) image = pygame.image.load(io.BytesIO(jpeg), 'JPG').convert() if lidar_id is None: yield timestamp, pose, scan, image, eof elif stream_id == poses_id: arr = deserialize(data) assert len(arr) == 3 pose = (arr[0] / 1000.0, arr[1] / 1000.0, math.radians(arr[2] / 100.0)) if lidar_id is None and camera_id is None: yield timestamp, pose, scan, image, eof # generate last message with EOF ... eof = True yield timestamp, pose, scan, image, eof
def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None, camera2_name=None, bbox_name=None, joint_name=None, keyframes_name=None, title_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0],[1, 0, 0, 0]] self.scan = [] self.scan2 = [] self.image = None self.image2 = None self.bbox = None self.joint = None self.keyframe = None self.title = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None self.lidar2_id = None self.camera2_id = None self.bbox_id = None self.joint_id = None self.keyframes_id = None self.title_id = None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if lidar2_name is not None: self.lidar2_id = names.index(lidar2_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 if camera2_name is not None: self.camera2_id = names.index(camera2_name) + 1 if bbox_name is not None: self.bbox_id = names.index(bbox_name) + 1 if joint_name is not None: self.joint_id = names.index(joint_name) + 1 if keyframes_name is not None: self.keyframes_id = names.index(keyframes_name) + 1 if title_name is not None: self.title_id = names.index(title_name) + 1
def deserialize(self): from osgar.logger import LogReader, lookup_stream_names, lookup_stream_id from osgar.lib.serialize import deserialize streams = lookup_stream_names(log_file) # streams.index("can.can") # for list_id in range(len(streams)): # if 'can.can' == streams[list_id]: break with LogReader(log_file, only_stream_id=lookup_stream_id(log_file, 'can.can')) as log: log_list = [] for timestamp, stream_id, data in log: sec = timestamp.total_seconds() stream_id = stream_id data = deserialize(data) log_list.append( [stream_id, sec, data[0], data[1].hex(), len(data[1])]) return log_list
def __init__(self, filepath, lidar_name=None, pose2d_name=None, pose3d_name=None, camera_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0], [1, 0, 0, 0]] self.scan = [] self.image = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1
def main(args_in=None, startswith=None): import argparse import os.path global g_rotation_offset_rad, g_lidar_fov_deg, MAX_SCAN_LIMIT, WINDOW_SIZE parser = argparse.ArgumentParser(description='View lidar scans') parser.add_argument('logfile', help='recorded log file') parser.add_argument('--lidar', help='stream ID') parser.add_argument('--lidar2', help='stream ID of second lidar (back or slope)') parser.add_argument('--lidar-limit', help='display scan limit in millimeters', type=int, default=MAX_SCAN_LIMIT) pose = parser.add_mutually_exclusive_group() pose.add_argument('--pose2d', help='stream ID for pose2d messages') pose.add_argument('--pose3d', help='stream ID for pose3d messages') parser.add_argument('--camera', help='stream ID for JPEG images') parser.add_argument('--camera2', help='stream ID for 2nd JPEG images') parser.add_argument('--bbox', help='stream ID for detection bounding box') parser.add_argument('--joint', help='stream ID joint angle for articulated robots (Kloubak)') parser.add_argument('--keyframes', help='stream ID typically for artifacts detection') parser.add_argument('--title', help='stream ID of data to be displayed in title') parser.add_argument('--window-size', help='set window size in pixels', type=int, nargs=2) parser.add_argument('--callback', help='callback function for lidar scans') parser.add_argument('--rotate', help='rotate poses by angle in degrees, offset', type=float, default=0.0) parser.add_argument('--deg', help='lidar field of view in degrees', type=float, default=270) parser.add_argument('--jump', '-j', help='jump to given time in seconds', type=int) parser.add_argument('--create-video', help='filename of output video') args = parser.parse_args(args_in) p = pathlib.Path(args.logfile) if p.is_dir(): if startswith is not None: g = iter(child for child in p.iterdir() if child.name.startswith(startswith)) else: g = p.iterdir() args.logfile = max(g, key=lambda a: a.stat().st_mtime) print(args.logfile) if not any([args.lidar, args.pose2d, args.pose3d, args.camera]): print("Available streams:") for stream in lookup_stream_names(args.logfile): print(" ", stream) return callback = None if args.callback is not None: callback = get_class_by_name(args.callback) if args.lidar_limit is not None: MAX_SCAN_LIMIT = args.lidar_limit if args.window_size is not None: WINDOW_SIZE = args.window_size filename = os.path.basename(args.logfile) g_rotation_offset_rad = math.radians(args.rotate) g_lidar_fov_deg = args.deg with Framer(args.logfile, lidar_name=args.lidar, lidar2_name=args.lidar2, pose2d_name=args.pose2d, pose3d_name=args.pose3d, camera_name=args.camera, camera2_name=args.camera2, bbox_name=args.bbox, joint_name=args.joint, keyframes_name=args.keyframes, title_name=args.title) as framer: lidarview(framer, caption_filename=filename, callback=callback, out_video=args.create_video, jump=args.jump)
parser.add_argument('logfile', help='OSGAR logfile') parser.add_argument('--time-limit-sec', '-t', help='cut time in seconds', type=float) parser.add_argument('--verbose', '-v', help="verbose mode", action='store_true') parser.add_argument('--module-name', '-m', help='name of the detector module in the log', default='detector') args = parser.parse_args() names = lookup_stream_names(args.logfile) assert 'detector.localized_artf' in names, names # XYZ world coordinates assert 'detector.debug_rgbd' in names, names assert 'detector.debug_result' in names, names assert 'detector.debug_cv_result' in names, names artf_stream_id = names.index('detector.localized_artf') + 1 rgbd_stream_id = names.index('detector.debug_rgbd') + 1 result_id = names.index('detector.debug_result') + 1 cv_result_id = names.index('detector.debug_cv_result') + 1 # read config file from log with LogReader(args.logfile, only_stream_id=0) as log: print("original args:", next(log)[-1]) # old arguments config_str = next(log)[-1] config = literal_eval(config_str.decode('ascii'))
""" Created on Sat Dec 14 11:19:54 2019 @author: xkadj """ from osgar.logger import LogReader, lookup_stream_names from osgar.lib.serialize import deserialize import pandas as pd #import struct log_file = r"C:\Users\xkadj\OneDrive\PROJEKTY\Projekt_ROBOTIKA\logs\kloubak2-subt-estop-lora-191213_162253.log" #log_file = r"C:\Users\xkadj\OneDrive\PROJEKTY\Projekt_ROBOTIKA\logs\K2_200217\test-pcan-200218_004148.log" #log_file = r"C:\Users\xkadj\OneDrive\PROJEKTY\Projekt_ROBOTIKA\logs\K2_200128\test-pcan-200128_185745.log" #log_file = r"C:\Users\xkadj\OneDrive\PROJEKTY\Projekt_ROBOTIKA\logs\K2_200204\test-pcan-200204_142841.log" streams = lookup_stream_names(log_file) for list_id in range(len(streams)): if 'can.can' == streams[list_id]: break with LogReader(log_file, only_stream_id=20) as log: log_list = [] for timestamp, stream_id, data in log: sec = timestamp.total_seconds() stream_id = stream_id # msg_id = data[2] #hex(data[2]) # msg_len = len(data[-9:-1]) # msg_content = struct.unpack(str(msg_len)+'B', data[-9:-1]) # log_list.append([sec,stream_id,msg_id,msg_len,msg_content,str(data)]) data_des = deserialize(data) # msg_id = data[2] #hex(data[2]) # msg_len = len(data[-9:-1])
def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None, camera2_name=None, bbox_name=None, rgbd_name=None, joint_name=None, keyframes_name=None, title_name=None, lidar_up_name=None, lidar_down_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.frame = Frame() self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0], [1, 0, 0, 0]] self.scan = [] self.scan2 = [] self.image = None self.image2 = None self.bbox = None self.joint = None self.keyframe = None self.title = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None self.lidar2_id = None self.camera2_id = None self.bbox_id = None self.rgbd_id = None self.joint_id = None self.keyframes_id = None self.title_id = [] self.lidar_up_id = None self.lidar_down_id = None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if lidar2_name is not None: self.lidar2_id = names.index(lidar2_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 if camera2_name is not None: self.camera2_id = names.index(camera2_name) + 1 if bbox_name is not None: self.bbox_id = names.index(bbox_name) + 1 if rgbd_name is not None: self.rgbd_id = names.index(rgbd_name) + 1 if joint_name is not None: self.joint_id = names.index(joint_name) + 1 if keyframes_name is not None: self.keyframes_id = names.index(keyframes_name) + 1 if title_name is not None: title_list = title_name.split(",") for ttn in title_list: self.title_id.append(names.index(ttn) + 1) if lidar_up_name is not None: self.lidar_up_id = names.index(lidar_up_name) + 1 if lidar_down_name is not None: self.lidar_down_id = names.index(lidar_down_name) + 1