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
