diff --git a/flandre/launcher.py b/flandre/launcher.py index b29937f..b177205 100644 --- a/flandre/launcher.py +++ b/flandre/launcher.py @@ -121,7 +121,7 @@ def entrypoint(dev): @entrypoint.command() @click.option('--data_folder', default=None) -@click.option('--generate_pyqt', default=True) +@click.option('--generate_pyqt', default=False) @click.option('-p', '--path', type=str, default=platformdirs.user_config_path( 'Flandre', 'Scarlet') / 'launch.toml', @@ -240,6 +240,7 @@ def device_end_capture(): def device_upload(name, file: TextIOWrapper): dd.set_name_and_file_only(name, file.read()) + @device.command('recv_single') def device_recvmonitor(): r = dd.get_data() @@ -247,102 +248,16 @@ def device_recvmonitor(): print(seq, encoder, host_ts, device_ts_low, device_ts_high) print((buffer.__len__()//2)/256) -@device.command('recvm') + +@device.command('monitor') def device_recvmonitor(): ctx = zmq.Context() pull = ctx.socket(zmq.PULL) pull.connect(C.live_push_socket) - last_device_ts_low = 0 - last_host_ts = 0 while True: b = pull.recv() - seq, encoder, host_ts, device_ts_low, device_ts_high, buffer = b2t(b) - print( - f'S={seq} E={encoder} HT={host_ts} DTL={device_ts_low} HTDF={host_ts - last_host_ts} DTLDF={device_ts_low - last_device_ts_low} DTH={device_ts_high} BS={buffer.__len__()}') - last_device_ts_low = device_ts_low - last_host_ts = host_ts - - -@device.command('recvplot') -def device_recvplot(): - ctx = zmq.Context() - pull = ctx.socket(zmq.PULL) - pull.connect(C.live_push_socket) - last_ts = 0 - first = True - host_host_ts_last = time.time_ns() - while True: - b = pull.recv() - host_host_ts = time.time_ns() - - seq, encoder, host_ts, device_ts_low, device_ts_high, buffer = b2t(b) - ts = host_ts - if first: - last_ts = ts - first = False - continue - print((ts - last_ts) / 10 ** 6, flush=True) - last_ts = ts - host_host_ts_last = host_host_ts - - -@device.command('recvplot2') -def device_recvplot2(): - ctx = zmq.Context() - pull = ctx.socket(zmq.PULL) - pull.connect(C.live_push_socket) - host_host_ts_last = time.time_ns() - while True: - b = pull.recv() - host_host_ts = time.time_ns() - _ = b2t(b) - print((host_host_ts - host_host_ts_last) / 10 ** 6, flush=True) - host_host_ts_last = host_host_ts - - -@device.command('recvplot3') -def device_recvplot3(): - ctx = zmq.Context() - pull = ctx.socket(zmq.PULL) - pull.connect(C.live_push_socket) - print(C.live_push_socket) - last_ts = 0 - first = True - host_host_ts_last = time.time_ns() - while True: - b = pull.recv() - host_host_ts = time.time_ns() - - seq, encoder, host_ts, device_ts_low, device_ts_high, buffer = b2t(b) - ts = host_ts - if first: - last_ts = ts - first = False - continue - print((ts - last_ts) / 10 ** 6, (host_host_ts - host_host_ts_last) / 10 ** 6, flush=True) - last_ts = ts - host_host_ts_last = host_host_ts - - -@device.command('recvplotdiff') -def device_recvplotdiff(): - ctx = zmq.Context() - pull = ctx.socket(zmq.PULL) - pull.connect(C.live_push_socket) - last_ts = 0 - max_interval = 0 - first = True - while True: - b = pull.recv() - - ts, sequence_id, encoder, buffer = b2t(b) - if first: - last_ts = ts - first = False - continue - max_interval = max(max_interval, ts - last_ts) - print(max_interval / 10 ** 6, flush=True) - last_ts = ts + seq, encoder, host_ts, device_ts, buffer = b2t(b) + print(f'S={seq} E={encoder} HT={host_ts} DT={device_ts} BS={buffer.__len__()}') def b2b(b): @@ -381,10 +296,13 @@ def device_record(folder): ctx = zmq.Context() pull = ctx.socket(zmq.PULL) pull.connect(C.live_push_socket) + cnt = 0 while True: + cnt += 1 b = pull.recv() - seq, encoder, host_ts, device_ts_low, device_ts_high, buffer = b2t(b) - (p / f'{host_ts}.bin').write_bytes(b) + seq, encoder, host_ts, device_ts, buffer = b2t(b) + if cnt % 5 == 0: + (p / f'{host_ts}.bin').write_bytes(b) robot: Robot = None @@ -409,7 +327,7 @@ def robot_monitor(): if recv_out.recipe_id == output1.id: x, y, z, r, p, yy = recv_out.actual_TCP_pose fx, fy, fz, fr, fp, fyy = recv_out.actual_TCP_force - print('xts', x, recv_out.timestamp) + print(f'X={x}, DT={recv_out.timestamp}') @robot.command('record') @@ -425,6 +343,7 @@ def robot_record(folder): 'actual_TCP_pose,actual_TCP_force,timestamp', 250) # 输出订阅,配方1 robot.rt.start() # rtsi 开始 arr = [] + last_device_ts = 0 while True: recv_out: DataObject = robot.rt.get_output_data() if recv_out is None: @@ -432,8 +351,10 @@ def robot_record(folder): if recv_out.recipe_id == output1.id: x, y, z, rx, ry, rz = recv_out.actual_TCP_pose fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force - local_ns = time.time_ns() - + host_ts = time.time_ns() + device_ts = recv_out.timestamp + # print(device_ts - last_device_ts) + last_device_ts = device_ts d = dict( x=x, y=y, @@ -447,8 +368,8 @@ def robot_record(folder): frx=frx, fry=fry, frz=frz, - local_ns=local_ns, - device_ts=recv_out.timestamp, + host_ts=host_ts, + device_ts=device_ts, ) arr.append(d) if arr.__len__() == 100: diff --git a/flandre/utils/RfFrame.py b/flandre/utils/RfFrame.py index 5411645..dfefaed 100644 --- a/flandre/utils/RfFrame.py +++ b/flandre/utils/RfFrame.py @@ -47,11 +47,11 @@ def b2t(b: bytes): # buffer = b[4 + 8 + 8 + 8 + 4:] # return seq, encoder, host_ts, driver_ts, buffer - magic, seq, encoder, host_ts, device_ts_low, device_ts_high = struct.unpack_from('iQiQII', b) + magic, seq, encoder, host_ts, device_ts = struct.unpack_from('iQiQQ', b) # bb = b[4 + 8 + 4 + 8:4 + 8 + 4 + 8 + 4] # device_ts_low = struct.unpack('