def test_context_manager(self): with LogWriter(prefix='tmpp', note='1st test') as log: self.assertTrue(log.filename.startswith('tmpp')) filename = log.filename start_time = log.start_time t1 = log.write(10, b'\x01\x02\x02\x04') time.sleep(0.01) t2 = log.write(10, b'\x05\x06\x07\x08') self.assertLess(t1, t2) with LogReader(filename) as log: self.assertEqual(start_time, log.start_time) __, stream_id, data = next(log) self.assertEqual(INFO_STREAM_ID, stream_id) t, stream_id, data = next(log) self.assertEqual(stream_id, 10) self.assertEqual(data, b'\x01\x02\x02\x04') t, stream_id, data = next(log) self.assertTrue(t.microseconds > 100) with self.assertRaises(StopIteration): __ = next(log) with LogReader(filename, only_stream_id=10) as log: for t, stream_id, data in log: self.assertEqual(stream_id, 10) self.assertEqual(data, b'\x01\x02\x02\x04') break os.remove(log.filename)
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 autodetect_name(logfile): stream_id = lookup_stream_id(logfile, ROBOT_NAME_STREAM) for dt, channel, data in LogReader(logfile, only_stream_id=stream_id): robot_name = deserialize(data).decode('ascii') return robot_name _, _, data = next(LogReader(logfile)) data = ast.literal_eval(str(data, 'ascii')) while data: if data[0] != "--robot-name": del data[0] continue return data[1] assert False, "Robot name autodetection failed!"
def test_no_eof(self): # similar to tail -f (--follow) with LogWriter(prefix='tmpB', note='test_EOF') as log: filename = log.filename t1 = log.write(1, b'\x01'*100) time.sleep(0.001) t2 = log.write(1, b'\x02'*100) time.sleep(0.001) t3 = log.write(1, b'\x03'*100000) with LogReader(filename, only_stream_id=1) as log: dt, channel, data = next(log) self.assertEqual(data, b'\x01'*100) dt, channel, data = next(log) self.assertEqual(data, b'\x02'*100) partial = filename + '.part' with open(filename, 'rb') as f_in: with open(partial, 'wb') as f_out: f_out.write(f_in.read(100)) with LogReader(partial, only_stream_id=1) as log: with self.assertLogs(osgar.logger.__name__, logging.ERROR): with self.assertRaises(StopIteration): dt, channel, data = next(log) proc = Timer(0.1, delayed_copy, [filename, partial, 100]) proc.start() with LogReader(partial, follow=True, only_stream_id=1) as log: dt, channel, data = next(log) self.assertEqual(data, b'\x01'*100) proc.join() partial = filename + '.part' with open(filename, 'rb') as f_in: with open(partial, 'wb') as f_out: f_out.write(f_in.read(100000)) with LogReader(partial, only_stream_id=1) as log: dt, channel, data = next(log) self.assertEqual(data, b'\x01'*100) dt, channel, data = next(log) self.assertEqual(data, b'\x02'*100) with self.assertRaises(AssertionError): dt, channel, data = next(log) os.remove(partial) os.remove(filename)
def test_time_overflow2(self): with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21)) with osgar.logger.LogWriter(prefix='tmp9', note='test_time_overflow') as log: filename = log.filename t1 = log.write(1, b'\x01') self.assertEqual(t1, timedelta(0)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21, 1)) t2 = log.write(1, b'\x02') self.assertEqual(t2, timedelta(hours=1)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21, 2)) t3 = log.write(1, b'\x03') self.assertEqual(t3, timedelta(hours=2)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21, 4)) # TODO this write should rise exception as the time gap is too big to track! t4 = log.write(1, b'\x04') self.assertEqual(t4, timedelta(hours=4)) with LogReader(filename, only_stream_id=1) as log: dt, channel, data = next(log) self.assertEqual(dt, timedelta(hours=0)) dt, channel, data = next(log) self.assertEqual(dt, timedelta(hours=1)) dt, channel, data = next(log) self.assertEqual(dt, timedelta(hours=2)) # dt, channel, data = next(log) # self.assertEqual(dt, timedelta(hours=4)) os.remove(filename)
def test_day_overflow(self): block_size = 10 with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill(datetime(2021, 7, 24)) with osgar.logger.LogWriter(prefix="tmpDay") as log: filename = log.filename for hour in range(50): # 2days+ ... note that overflow appears for 1day 22hours osgar.logger.datetime.datetime = TimeStandsStill(datetime(2021, 7, 24) + timedelta(seconds=3600*hour)) log.write(1, b'\x01'*block_size) with LogReader(filename, only_stream_id=1) as log: for i, (dt, stream, data) in enumerate(log): self.assertEqual(stream, 1) if i < 24: self.assertEqual(dt.seconds, i * 3600) self.assertEqual(dt.days, 0) elif i < 48: self.assertEqual(dt.seconds, (i - 24) * 3600) self.assertEqual(dt.days, 1) elif i < 72: self.assertEqual(dt.seconds, (i - 48) * 3600) self.assertEqual(dt.days, 2) else: assert False, "Test was just for 2 days" os.remove(filename)
def test_large_blocks_with_time_overflow(self): with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1)) with osgar.logger.LogWriter( prefix='tmpA', note='test_time_overflow with large blocks') as log: filename = log.filename t1 = log.write(1, b'\x01' * 100000) self.assertEqual(t1, timedelta(0)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 1)) t2 = log.write(1, b'\x02' * 100000) self.assertEqual(t2, timedelta(hours=1)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 2)) t3 = log.write(1, b'\x03' * 100000) self.assertEqual(t3, timedelta(hours=2)) with LogReader(filename, only_stream_id=1) as log: dt, channel, data = next(log) self.assertEqual(dt, timedelta(hours=0)) dt, channel, data = next(log) self.assertEqual(dt, timedelta(hours=1)) dt, channel, data = next(log) self.assertEqual(dt, timedelta(hours=2)) os.remove(filename)
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='recorded log file', nargs='+') parser.add_argument('--out', help='map output file') parser.add_argument('--pose2d', help='stream ID for pose2d messages', default='app.pose2d') parser.add_argument('--artf', help='stream ID with artifacts XYZ') args = parser.parse_args() out_filename = args.out if out_filename is None: out_filename = os.path.splitext(args.logfile[0])[0] + '.jpg' pts = [] artf = [] for filename in args.logfile: pose_id = lookup_stream_id(filename, args.pose2d) streams = [pose_id] artf_id = None if args.artf is not None: artf_id = lookup_stream_id(filename, args.artf) streams.append(artf_id) for dt, channel, data in LogReader(filename, only_stream_id=streams): if channel == pose_id: pose = deserialize(data) pts.append(pose[:2]) elif channel == artf_id: arr = deserialize(data) artf.extend(arr) pts2image(pts, artf, out_filename)
def mapping_server(logfile, cpr, loop=False): try: name = u'Skiddy' stream_id = lookup_stream_id(logfile, 'fromros.points') except ValueError: name = u'Kloubak' stream_id = lookup_stream_id(logfile, 'from_jetson_front.points') with LogReader(args.logfile, only_stream_id=stream_id) as logreader: for timestamp, stream, raw_data in logreader: arr = deserialize(raw_data) assert len( arr ) == 1, arr # array of arrays, but maybe it is mistake no ROS serializer side? if len(arr[0]) == 0: continue # but maybe we should report at least one empty map? print('SENDING', len(arr[0])) cloud = create_map(arr) print(cloud) if cpr is not None: res = cpr.send_map_msg(u'PointCloud2', msg=cloud, name=name) print(res) if not loop: break time.sleep(1.0)
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(logfile, streams): warning_event = False base_logname = os.path.basename(logfile) print("\n" + base_logname) print("-" * 60) relevant_streams_id = [ lookup_stream_id(logfile, name) for name in streams.keys() ] stat_list = [np.array([])] * len(relevant_streams_id) encoder_stream_id = lookup_stream_id(logfile, "kloubak.encoders") prev_time_in_sec = None time_diff = None with LogReader(logfile, only_stream_id=relevant_streams_id) as log: for timestamp, stream_id, data in log: time_in_sec = timestamp.total_seconds() item_id = relevant_streams_id.index(stream_id) stat_list[item_id] = np.append(stat_list[item_id], time_in_sec) if stream_id == encoder_stream_id: encoders = deserialize(data) for enc in encoders: if enc > abs(100): if prev_time_in_sec is not None: time_diff = time_in_sec - prev_time_in_sec print( f"\nEncoders: {encoders}, time diff: {time_diff}, at {timestamp}" ) prev_time_in_sec = time_in_sec for arr, name in zip(stat_list, streams): gaps_event = False if len(arr) <= 1: print(f'\n{name:>{20}}\t{"No data or only one msg received!!!"}') warning_event = True else: # average frequencies average_freq = len(arr) / (arr[-1] - arr[0] ) # average number of msg per second expected_freq = streams[name][0] acceptable_gap = streams[name][1] num_gaps = len(np.where(np.diff(arr) > acceptable_gap)[0]) if num_gaps > 0: max_gap = max(np.diff(arr)) print( f'\n{name:>{20}}\t{"number of gaps %d" %num_gaps}\t{"max gap %.2f s" %max_gap}\t{"acceptable gap %.2f s" %acceptable_gap}' ) gaps_event = True warning_event = True if average_freq < expected_freq: if not gaps_event: print("") print( f'{name:>{20}}\t{"received %.1f Hz" % average_freq}\t{"expected %.1f Hz" % expected_freq}' ) warning_event = True if not warning_event: print("log OK")
def strip_logfile(logfile, only_stream, outfile): if outfile is None: outfile = logfile[:-4] + '-strip' + logfile[-4:] print('Out:', outfile) with LogReader(logfile, only_stream_id=only_stream) as log: with LogWriter(filename=outfile, start_time=log.start_time) as out: for timestamp, stream_id, data in log: out.write(stream_id=stream_id, data=data, dt=timestamp)
def test_time_overflow(self): with LogWriter(prefix='tmp8', note='test_time_overflow') as log: log.start_time = datetime.utcnow() - timedelta(hours=1, minutes=30) t1 = log.write(1, b'\x01\x02') self.assertGreater(t1, timedelta(hours=1)) filename = log.filename with LogReader(filename, only_stream_id=1) as log: dt, channel, data = next(log) self.assertGreater(dt, timedelta(minutes=10)) os.remove(filename)
def test_incomplete(self): header = osgar.logger.format_header(datetime(2019, 1, 1, 1)) packet = osgar.logger.format_packet(1, b"\x00"*10, timedelta()) logdata = b"".join(header + packet) with tempfile.TemporaryDirectory(dir=".") as d: filename = Path(".") / d / "log" with open(filename, "wb") as f: f.write(logdata[:-1]) with LogReader(filename) as l: with self.assertLogs(osgar.logger.__name__, logging.ERROR): with self.assertRaises(StopIteration): next(l)
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 test_read_gen_seek(self): with LogWriter(prefix='tmpC', note='test_read_gen_seek') as log: filename = log.filename t1 = log.write(1, b'\x01') time.sleep(0.001) t2 = log.write(1, b'\x02') time.sleep(0.001) t3 = log.write(1, b'\x03') with LogReader(filename, only_stream_id=1) as log: dt, channel, data = next(log) self.assertEqual(data, b'\x01') dt, channel, data = next(log) self.assertEqual(data, b'\x02') os.remove(filename)
def test_large_invalid(self): header = osgar.logger.format_header(datetime(2019, 1, 1, 1)) packet = osgar.logger.format_packet(1, b"\x00"*(2**16), timedelta()) invalid = b"\xFF"*len(packet[-2]) # overwrite header of the last subpacket self.assertNotEqual(packet[-2], invalid) packet[-2] = invalid logdata = b"".join(header + packet) with tempfile.TemporaryDirectory(dir=".") as d: filename = Path(".") / d / "log" with open(filename, "wb") as f: f.write(logdata[:-1]) with LogReader(filename) as l: with self.assertRaises(StopIteration),\ self.assertLogs(logger=osgar.logger.__name__, level=logging.ERROR): next(l)
def test_read_two_streams(self): with LogWriter(prefix='tmp2', note='test_read_two_streams') as log: filename = log.filename t1 = log.write(1, b'\x01\x02\x02\x04') time.sleep(0.001) t2 = log.write(3, b'\x05\x06') time.sleep(0.001) t3 = log.write(2, b'\x07\x08') with LogReader(filename, only_stream_id=[1, 2]) as log: arr = [] for t, stream_id, data in log: self.assertIn(stream_id, [1, 2]) arr.append((t, stream_id)) self.assertEqual(arr, [(t1, 1), (t3, 2)]) os.remove(log.filename)
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 read_pose3d(filename, pose3d_name, seconds=MAX_SIMULATION_DURATION): stream_id = lookup_stream_id(filename, pose3d_name) sim_time_id = lookup_stream_id(filename, SIM_TIME_STREAM) poses = [] sim_time = None pose_sim_time = None # sim_time, but valid only for the first pose after time change for dt, channel, data in LogReader(filename, only_stream_id=[stream_id, sim_time_id]): value = deserialize(data) if len(poses) > seconds: break if channel == sim_time_id: if sim_time != value: sim_time = value pose_sim_time = sim_time else: # pose3d if pose_sim_time is not None: poses.append((pose_sim_time, value)) pose_sim_time = None return poses
def test_large_block(self): data = bytes([x for x in range(100)] * 1000) self.assertEqual(len(data), 100000) with LogWriter(prefix='tmp4', note='test_large_block') as log: filename = log.filename t1 = log.write(1, data) t2 = log.write(1, data[:0xFFFF]) t3 = log.write(1, b'') t4 = log.write(1, b'ABC') t5 = log.write(1, data + data) # multiple split with LogReader(filename, only_stream_id=1) as log: arr = [] for __, __, data in log: arr.append(data) self.assertEqual([len(x) for x in arr], [100000, 65535, 0, 3, 200000]) os.remove(log.filename)
def create_video(logfile, stream, outfile, add_time=False, start_time_sec=0, end_time_sec=None, fps=25): assert outfile.endswith(".avi"), outFilename only_stream = lookup_stream_id(logfile, stream) with LogReader(logfile, only_stream_id=only_stream) as log: writer = None for timestamp, stream_id, data in log: buf = deserialize(data) img = cv2.imdecode(np.fromstring(buf, dtype=np.uint8), 1) if writer is None: height, width = img.shape[:2] writer = cv2.VideoWriter( outfile, cv2.VideoWriter_fourcc('F', 'M', 'P', '4'), fps, (width, height)) if add_time: if (width, height) == (640, 480): x, y = 350, 50 thickness = 3 size = 3.0 else: x, y = 800, 100 thickness = 5 size = 5.0 # clip microseconds to miliseconds s = str(timestamp - timedelta(seconds=start_time_sec))[:-3] cv2.putText(img, s, (x, y), cv2.FONT_HERSHEY_PLAIN, size, (255, 255, 255), thickness=thickness) if timestamp.total_seconds() > start_time_sec: writer.write(img) if end_time_sec is not None and timestamp.total_seconds( ) > end_time_sec: break writer.release()
def test_register(self): with LogWriter(prefix='tmp2', note='test_register') as log: filename = log.filename self.assertEqual(log.register('raw'), 1) with self.assertRaises(AssertionError): log.register('raw') # duplicity name self.assertEqual(log.register('gps.position'), 2) with LogReader(filename, only_stream_id=INFO_STREAM_ID) as log: arr = [] for __, __, data in log: if b'names' in data: arr.append(data) self.assertEqual(len(arr), 2, arr) self.assertEqual(arr[0], b"{'names': ['raw']}") self.assertEqual(arr[1], b"{'names': ['raw', 'gps.position']}") os.remove(filename)
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 log2pcap(input_filepath, output_dir): """ Extract from logfile Velodyne UPD packets and save them in 'pcap' format undestandable by Wireshark and VeloView. """ output_filepath = os.path.join(output_dir, os.path.basename(input_filepath)) assert output_filepath.endswith('.log'), output_filepath output_filepath = output_filepath[:-4] + '.pcap' print(output_filepath) only_stream = lookup_stream_id(input_filepath, 'velodyne.raw') with LogReader(input_filepath, only_stream_id=only_stream) as log, open( output_filepath, 'wb') as out: out.write(unhexlify(FILE_HEADER)) for timestamp, stream_id, data in log: packet = deserialize(data) assert len(packet) == 1206, len(packet) out.write(unhexlify(PACKET_HEADER)) # TODO revise timestamps out.write(unhexlify(IP_HEADER)) out.write(packet)
parser.add_argument('--dbc', help='interpretation of raw data', choices=['spider', 'eduro']) args = parser.parse_args() dbc = {} if args.dbc == 'spider': dbc = { 0x200: 'H', # status 0x201: 'HHHH', # wheels 0x202: 'HHHH', # drive status 0x203: 'HHHH', # zero steering 0x204: 'HHBBH' # user input } elif args.dbc == 'eduro': dbc = { # 0x80: # SYNC, no data 0x181: '<i', # encoders L 0x182: '<i', # encoders R #0x187, 0x387, 0x487 # compass, acc 0x28A: '<H', # buttons 0x18B: '<H', # battery(V)/100.0 } stream_id = lookup_stream_id(args.logfile, args.stream) with LogReader(args.logfile, only_stream_id=stream_id) as log: for timestamp, stream_id, data in log: print(timestamp, print_packet(deserialize(data), dbc)) # vim: expandtab sw=4 ts=4
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')) assert 'detector' in config['robot']['modules'] fx = config['robot']['modules'][args.module_name]['init']['fx'] max_depth = config['robot']['modules'][args.module_name]['init'].get( 'max_depth', 10.0) last_artf = None # reported before debug_rgbd last_result = None last_cv_result = None with LogReader(args.logfile, only_stream_id=[ artf_stream_id, rgbd_stream_id, result_id, cv_result_id