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()