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
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 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 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 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 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 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 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 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 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)
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)
action='store_true',
help='Enable visualization of the scene and detected ' + 'artifacts.')
args = parser.parse_args()
assert (args.model)
assert ((args.logfile and args.image_stream) or (args.image))
use_cuda = torch.cuda.is_available()
device = torch.device("cuda" if use_cuda else "cpu")
print('Using:', device, file=sys.stderr)
model, categories = subt.artf_model.load_model(args.model, device)
detector = Detector(model, args.confidence_thresholds, categories, device,
args.max_gap, args.min_group_size)
n = 0
if args.logfile:
image_stream_id = lookup_stream_id(args.logfile, args.image_stream)
with LogReader(args.logfile,
only_stream_id=image_stream_id) as logreader:
for time, stream, data in logreader:
assert (stream == image_stream_id)
img = cv2.imdecode(np.frombuffer(deserialize(data), np.uint8),
cv2.IMREAD_COLOR)
artifacts = detector(img)
print('---')
for name, group in artifacts:
print(name)
is_main_pt = True
for x, y, confidence in group:
if args.show or args.output:
cv2.circle(img, (x, y), 5, (0, 0, 255),
-1 if is_main_pt else 1)
from osgar.lib.serialize import deserialize
from osgar.logger import LogReader, lookup_stream_id
parser = argparse.ArgumentParser()
parser.add_argument('--logfile', help='Path to Osgar log.', required=True)
parser.add_argument(
'--pose3d-stream',
help='Stream id or name with pose and orientation data.',
required=True)
parser.add_argument('--debug',
help='Provide debug info.',
action='store_true')
args = parser.parse_args()
pose3d_stream_id = lookup_stream_id(args.logfile, args.pose3d_stream)
loop_detector = LoopDetector()
with LogReader(args.logfile, only_stream_id=pose3d_stream_id) as logreader:
last_detected_start = None
for time, stream, data in logreader:
pose, orientation = deserialize(data)
loop_detector.add(pose, orientation)
loop = loop_detector.loop()
if loop:
if args.debug:
print('xxxx', loop)
else:
if last_detected_start is None:
last_detected_start = time
else:
if last_detected_start is not None:
parser.add_argument('--verbose',
'-v',
help="verbose mode",
action='store_true')
parser.add_argument('--draw',
'-d',
help="draw result",
action='store_true')
parser.add_argument('--index', '-i', help="scan index", type=int)
parser.add_argument('--transporter',
help="detect_transporter",
action='store_true')
args = parser.parse_args()
filename = args.filename
only_stream = lookup_stream_id(filename, 'lidar.scan')
index = args.index
offset_y = 0
with LogReader(filename, only_stream_id=only_stream) as log:
for ind, row in enumerate(log):
if index is not None and ind < index:
continue
timestamp, stream_id, data = row
scan = deserialize(data)
if args.transporter:
trans = detect_transporter(scan, offset_y)
if trans is None:
print('%d\tNone' % ind)
else:
parser.add_argument('--stream', help='stream ID or name', default='can.can')
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
if __name__ == "__main__":
import argparse
import matplotlib.pyplot as plt
from osgar.logger import LogReader, lookup_stream_id
from osgar.lib.serialize import deserialize
parser = argparse.ArgumentParser(description='Extract features in laser scan')
parser.add_argument('filename', help='input log file')
parser.add_argument('--verbose', '-v', help="verbose mode", action='store_true')
parser.add_argument('--draw', '-d', help="draw result", action='store_true')
parser.add_argument('--index', '-i', help="scan index", type=int)
parser.add_argument('--transporter', help="detect_transporter", action='store_true')
args = parser.parse_args()
filename = args.filename
only_stream = lookup_stream_id(filename, 'lidar.scan')
index = args.index
offset_y = 0
with LogReader(filename, only_stream_id=only_stream) as log:
for ind, row in enumerate(log):
if index is not None and ind < index:
continue
timestamp, stream_id, data = row
scan = deserialize(data)
if args.transporter:
trans = detect_transporter(scan, offset_y)
if trans is None:
print('%d\tNone' % ind)
else:
def test_lookup_stream_id(self):
self.assertEqual(lookup_stream_id('dummy.log', None), None)
self.assertEqual(lookup_stream_id('dummy.log', '3'), 3)
def log_eval(log_file):
print(log_file)
data_dir = log_file + ".d"
try:
os.makedirs(data_dir, exist_ok=False) # do not overwrite existing data
except FileExistsError:
print("Directory already exist")
return None
detection_log = open(os.path.join(data_dir, "detection.log"), "w")
confidence_thresholds = {
'survivor': 0.5,
'backpack': 0.5,
'phone': 0.5,
'helmet': 0.5,
'rope': 0.5,
'fire_extinguisher': 0.5,
'drill': 0.5,
'vent': 0.5,
'cube': 0.5
}
detector = create_detector(confidence_thresholds)
CvDetector().min_score = MIN_SCORES
cv_detector = CvDetector().subt_detector
for stream_name in g_streams:
prefix = stream_name.split(".")[0]
try:
only_stream = lookup_stream_id(log_file, stream_name)
except Exception as e:
print(e)
continue # continue with another stream name
with LogReader(log_file, only_stream_id=only_stream) as log:
ii = 0
for timestamp, stream_id, data_raw in log:
buf_color = deserialize(data_raw)
img = cv2.imdecode(np.frombuffer(buf_color, dtype=np.uint8), 1)
result_cv = cv_detector(img)
result_torch = detector(img)
result = check_results(result_torch, result_cv)
#print(result)
im_file_name = prefix + "_im_%06d.jpg" % ii
im_path = os.path.join(data_dir, im_file_name)
for res in result:
artf_name, res_t, res_cv = res
print(im_file_name, artf_name)
score_torch, points = proces_torch_result(res_t)
__, score_cv, bbox = res_cv
detection_log.write(
"%s;%s;%f;%f;%s;%s;%s\r\n" %
(str(timestamp), artf_name, score_torch, score_cv,
str(points), str(bbox), im_file_name))
detection_log.flush()
cv2.imwrite(im_path, img)
ii += 1
detection_log.close()
def test_lookup_stream_id(self):
self.assertEqual(lookup_stream_id('dummy.log', None), None)
self.assertEqual(lookup_stream_id('dummy.log', '3'), 3)
from osgar.logger import LogReader, lookup_stream_id
from osgar.lib.serialize import deserialize
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(
description='Parse logged detected_gas and extract "Gas" artifact')
parser.add_argument('logfile', help='recorded log file')
args = parser.parse_args()
only_stream = lookup_stream_id(args.logfile, 'gas_detector.co2')
best = None
with LogReader(args.logfile, only_stream_id=only_stream) as log:
for timestamp, stream_id, data in log:
co2 = deserialize(data)
if best is None or co2 > best[1]:
print(timestamp, co2)
best = timestamp, co2
print('--------------------')
if best is not None:
print('best:', best[0], best[1])
else:
print("No Gas, sorry")
# vim: expandtab sw=4 ts=4
for im_file in im_file_list:
im_path = os.path.join(path, im_file)
img = cv2.imread(im_path)
if img is not None:
img = detector.run_on_image(img)
cv2.imshow('img', img)
k = cv2.waitKey(0) & 0xFF
if k == ord("q"):
cv2.destroyAllWindows()
break
elif path.endswith(".log"):
from osgar.logger import LogReader, lookup_stream_id
from osgar.lib.serialize import deserialize
stream_name = args.stream
only_stream = lookup_stream_id(path, stream_name)
with LogReader(path, only_stream_id=only_stream) as log:
for timestamp, stream_id, data_raw in log:
buf_color = deserialize(data_raw)
img = cv2.imdecode(
np.frombuffer(buf_color, dtype=np.uint8), 1)
img = detector.run_on_image(img)
cv2.imshow('img', img)
k = cv2.waitKey(200) & 0xFF
if k == ord("q"):
cv2.destroyAllWindows()
break
else:
img = cv2.imread(path)
if img is not None:
def draw_wifi(logs_dir):
path_logs = []
if os.path.isdir(logs_dir):
sub_dirs = os.listdir(logs_dir)
for robot_dir in sub_dirs:
robot_dir_path = os.path.join(logs_dir, robot_dir)
if os.path.isdir(robot_dir_path):
logs = os.listdir(robot_dir_path)
for log_name in logs:
path_logs.append(os.path.join(robot_dir_path, log_name))
else:
path_logs.append(
robot_dir_path) # logs_dir is for one specific robot
else:
path_logs.append(logs_dir)
wifi_pose = []
co2_pose = []
for path_log in path_logs:
print(path_log)
try:
only_stream_wifi = lookup_stream_id(path_log, "wifi.wifiscan")
only_stream_co2 = lookup_stream_id(path_log, "gas_detector.co2")
only_stream_pose = lookup_stream_id(path_log, "app.pose2d")
with LogReader(path_log,
only_stream_id=[
only_stream_wifi, only_stream_co2,
only_stream_pose
]) as log:
x = y = 0
for timestamp, stream_id, data_raw in log:
if stream_id == only_stream_pose:
x, y, __ = deserialize(data_raw) # last pose2d
elif stream_id == only_stream_wifi:
wifi = deserialize(data_raw)
for item in wifi:
if "PhoneArtifac" in item[0]:
wifi_pose.append([x, y, item[1]])
elif stream_id == only_stream_co2:
co2_pose.append([x, y, deserialize(data_raw)])
except:
pass
#print("End..") # exception during log reading
fig = plt.figure(figsize=(12, 16))
fig.subplots_adjust(left=0.1, bottom=0.1, right=0.9, top=0.9)
if len(wifi_pose) != 0:
ax = fig.add_subplot(211)
wifi_ar = np.array(wifi_pose)
im = ax.scatter(wifi_ar[:, 0] / 1000,
wifi_ar[:, 1] / 1000,
c=wifi_ar[:, 2],
cmap=plt.cm.jet)
ax.set_title("WIFI")
ax.axis('equal')
ax.set_xlabel("x (m)")
ax.set_ylabel("y (m)")
cb = fig.colorbar(im,
ax=ax,
orientation="vertical",
pad=.15,
aspect=60)
cb.ax.tick_params(labelsize=10)
cb.set_label("Wifi", rotation=0)
else:
print("No wifi")
if len(co2_pose) != 0:
co2_ar = np.array(co2_pose)
ax2 = fig.add_subplot(212)
im_co2 = ax2.scatter(co2_ar[:, 0] / 1000,
co2_ar[:, 1] / 1000,
c=co2_ar[:, 2],
cmap=plt.cm.jet)
ax2.set_title("CO2")
ax2.axis('equal')
ax2.set_xlabel("x (m)")
ax2.set_ylabel("y (m)")
cb2 = fig.colorbar(im_co2,
ax=ax2,
orientation="vertical",
pad=.15,
aspect=60)
cb2.ax.tick_params(labelsize=10)
cb2.set_label("co2", rotation=0)
else:
print("No co2")
plt.show()
def create_video(logfile,
stream,
outfile,
add_time=False,
start_time_sec=0,
end_time_sec=None,
fps=25,
flip=False,
camera2=None):
if outfile is None:
outfile = str(pathlib.Path(logfile).with_suffix(".mp4"))
assert outfile.endswith(".mp4"), outfile
only_stream = lookup_stream_id(logfile, stream)
if camera2 is not None:
dual_cam_id = lookup_stream_id(logfile, camera2)
only_stream = [only_stream, dual_cam_id]
img1, img2 = None, None
else:
dual_cam_id = None
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 flip:
img = cv2.flip(img, 0)
if dual_cam_id is not None:
if stream_id == dual_cam_id:
img2 = img
else:
img1 = img
# make sure that both images are defined before you create writer
if img1 is None or img2 is None:
continue
img = np.concatenate((img1, img2), axis=1)
if writer is None:
height, width = img.shape[:2]
writer = cv2.VideoWriter(outfile,
cv2.VideoWriter_fourcc(*"mp4v"), 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()
from osgar.logger import LogReader, lookup_stream_id
from osgar.lib.serialize import deserialize
CELL_PREFIX = "PhoneArtifact"
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description='Parse logged WiFi AP scans and extract "Cell Phone" artifact')
parser.add_argument('logfile', help='recorded log file')
args = parser.parse_args()
only_stream = lookup_stream_id(args.logfile, 'wifi.wifiscan')
best = None
with LogReader(args.logfile, only_stream_id=only_stream) as log:
for timestamp, stream_id, data in log:
wifi_list = deserialize(data)
for name, signal in wifi_list:
if name.startswith(CELL_PREFIX):
print(timestamp, name, signal)
if best is None or signal > best[2]:
best = timestamp, name, signal
print('--------------------')
if best is not None:
print('best:', best[0], best[1], best[2])
else:
print("No Cell Phone, sorry")
# vim: expandtab sw=4 ts=4
from osgar.lib.serialize import deserialize
from osgar.logger import LogReader, LogWriter, lookup_stream_id, lookup_stream_names
parser = argparse.ArgumentParser("Analyze crash_rgbd data")
parser.add_argument('logfile', help='path to logfile')
parser.add_argument('--out', help='custom output (default [logfile]-crash.log)')
parser.add_argument('--verbose', '-v', help="verbose mode", action='store_true')
args = parser.parse_args()
outfile = args.out
if outfile is None:
assert args.logfile.endswith('.log')
outfile = args.logfile[:-4] + '-crash.log'
print('Crash outfile:', outfile)
crash_stream_id = lookup_stream_id(args.logfile, 'black_box.crash_rgbd')
pose3d_stream_id = lookup_stream_id(args.logfile, 'fromrospy.pose3d')
def nearest_pose(poses, pose3d):
xyz_arr = np.array([xyz for xyz, quat in poses])
xyz_diff = xyz_arr - pose3d[0]
dist = np.linalg.norm(xyz_diff, axis=1)
index = np.argmin(dist)
return index
stream_names = lookup_stream_names(args.logfile)
def get_camera_name_id(camera_pose):
x, y = camera_pose[0][:2]
if y == 0:
camera_direction = 'front' if x > 0 else 'rear'
if __name__ == "__main__":
import argparse
from osgar.lib.serialize import deserialize
from osgar.logger import LogReader, lookup_stream_id
parser = argparse.ArgumentParser("Analyze ocotomap data")
parser.add_argument('logfile', help='path to logfile with octomap data')
parser.add_argument('--out',
help='output path to PNG image',
default='out.png')
parser.add_argument('--draw',
action='store_true',
help='draw pyplot frontiers')
args = parser.parse_args()
octomap_stream_id = lookup_stream_id(args.logfile, 'fromrospy.octomap')
pose3d_stream_id = lookup_stream_id(args.logfile, 'fromrospy.pose3d')
waypoints_stream_id = lookup_stream_id(args.logfile, 'octomap.waypoints')
pose3d = None
x, y, z = 0, 0, 0
resolution = 0.5
waypoints = None
last_octo_data = None
with LogReader(args.logfile,
only_stream_id=[
octomap_stream_id, pose3d_stream_id, waypoints_stream_id
]) as logreader:
level = 2
scaled = True
for time, stream, data in logreader:
data = deserialize(data)
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
rs_scan = "rosmsg.scan"
scan = "lidar.scan"
stream_names = [rs_scan, scan]
bus = bus.Bus(MagicMock())
tester = bus.handle('tester')
tester.register("scan", "rs_scan")
mixer = ScanMixer(config={}, bus=bus.handle('mixer'))
bus.connect("tester.scan", "mixer.scan")
bus.connect("tester.rs_scan", "mixer.rs_scan")
bus.connect("mixer.scan", "tester.mixed")
mixer.start()
args = parser.parse_args()
stream_ids = [
logger.lookup_stream_id(args.logfile, stream)
for stream in stream_names
]
id2name = {
stream_id: name
for stream_id, name in zip(stream_ids, ["rs_scan", "scan"])
}
with logger.LogReader(args.logfile, only_stream_id=stream_ids) as log:
for timestamp, stream_id, bytes_data in log:
data = deserialize(bytes_data)
tester.publish(id2name[stream_id], data)
if id2name[stream_id] == "scan":
t, s, d = tester.listen()
mixer.request_stop()
mixer.join()
