From 9e25a12683f63f36b83f208882e2a10bf4144011 Mon Sep 17 00:00:00 2001 From: vmaosmini Date: Wed, 14 May 2025 13:34:34 +0800 Subject: [PATCH] add queue record rtsi --- flandre/launcher.py | 91 ++++++++++++++++++++++++++++++--------------- test/device_gui.py | 25 +++++++++++++ 2 files changed, 86 insertions(+), 30 deletions(-) create mode 100644 test/device_gui.py diff --git a/flandre/launcher.py b/flandre/launcher.py index 827adcb..6912c23 100644 --- a/flandre/launcher.py +++ b/flandre/launcher.py @@ -4,8 +4,10 @@ import json import logging import multiprocessing import os +import queue import shutil import subprocess +import threading import time import tomllib from enum import Enum, auto @@ -50,7 +52,8 @@ class LaunchComponent(Enum): def launch(arg: dict[LaunchComponent, dict]): logging.basicConfig(level=logging.INFO) multiprocessing.set_start_method('spawn') - bp = multiprocessing.Process(target=Broker(broker=True), kwargs=dict(software_config=C)) + bp = multiprocessing.Process(target=Broker( + broker=True), kwargs=dict(software_config=C)) bp.start() ps = [] for k, v in arg.items(): @@ -63,7 +66,8 @@ def launch(arg: dict[LaunchComponent, dict]): pps = [] for p in ps: - pps.append(multiprocessing.Process(target=p, kwargs=dict(software_config=C))) + pps.append(multiprocessing.Process( + target=p, kwargs=dict(software_config=C))) for p in pps: p.start() @@ -119,7 +123,8 @@ def entrypoint(dev): @click.option('--data_folder', default=None) @click.option('--generate_pyqt', default=True) @click.option('-p', '--path', type=str, - default=platformdirs.user_config_path('Flandre', 'Scarlet') / 'launch.toml', + default=platformdirs.user_config_path( + 'Flandre', 'Scarlet') / 'launch.toml', help='Path to launch.toml' ) def gui(data_folder, generate_pyqt, path): @@ -128,7 +133,8 @@ def gui(data_folder, generate_pyqt, path): return if generate_pyqt: subprocess.run([pyuic6, '-o', P.PYQT / 'Main.py', P.PYQT / 'Main.ui']) - subprocess.run([pyuic6, '-o', P.PYQT / 'Image.py', P.PYQT / 'Image.ui']) + subprocess.run( + [pyuic6, '-o', P.PYQT / 'Image.py', P.PYQT / 'Image.ui']) if data_folder is not None: C.record_folder = Path(data_folder) @@ -289,7 +295,8 @@ def device_recvzero(): b = pull.recv() ts, sequence_id, encoder, buffer = b2t(b) if ts == last_ts: - print('tszero', sequence_id, encoder, (ts - last_ts) / 10 ** 6, flush=True) + print('tszero', sequence_id, encoder, + (ts - last_ts) / 10 ** 6, flush=True) print(b2b(b), b2b(last_b)) if sequence_id == last_sequence_id: print('szero', sequence_id, encoder, flush=True) @@ -325,7 +332,8 @@ def robot(): @robot.command('monitor') def robot_monitor(): robot.setup() - output1 = robot.rt.output_subscribe('actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1 + output1 = robot.rt.output_subscribe( + 'actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1 robot.rt.start() # rtsi 开始 while True: recv_out: DataObject = robot.rt.get_output_data() @@ -343,30 +351,53 @@ def robot_record(folder): p = Path(folder) p.mkdir(parents=True, exist_ok=True) robot.setup() - output1 = robot.rt.output_subscribe('actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1 - robot.rt.start() # rtsi 开始 - while True: - recv_out: DataObject = robot.rt.get_output_data() - if recv_out is None: - continue - 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 - d = dict( - x=x, - y=y, - z=z, - fx=fx, - fy=fy, - fz=fz, - rx=rx, - ry=ry, - rz=rz, - frx=frx, - fry=fry, - frz=frz, - ) - (p / f'{time.time_ns()}.rtsi.json').write_text(json.dumps(d)) + + q = queue.Queue() + + def rtsi_thread(): + output1 = robot.rt.output_subscribe( + 'actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1 + robot.rt.start() # rtsi 开始 + arr = [] + while True: + recv_out: DataObject = robot.rt.get_output_data() + if recv_out is None: + continue + if recv_out.recipe_id == output1.id: + ns = time.time_ns() + x, y, z, rx, ry, rz = recv_out.actual_TCP_pose + fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force + d = dict( + x=x, + y=y, + z=z, + fx=fx, + fy=fy, + fz=fz, + rx=rx, + ry=ry, + rz=rz, + frx=frx, + fry=fry, + frz=frz, + ns=ns + ) + arr.append(d) + print(arr.__len__()) + # if arr.__len__() == 100: + # q.put(arr) + # arr = [] + + def write_thread(): + while True: + print(q.get()[0]['ns']) + + tr = threading.Thread(target=rtsi_thread) + tw = threading.Thread(target=write_thread) + tr.start() + tw.start() + tr.join() + tw.join() if __name__ == '__main__': diff --git a/test/device_gui.py b/test/device_gui.py new file mode 100644 index 0000000..b067145 --- /dev/null +++ b/test/device_gui.py @@ -0,0 +1,25 @@ +import struct +import time + +import cv2 +import numpy as np +import zmq + +from flandre.config import C +from flandre.nodes.Device import Device, DeviceCmd +from flandre.nodes.Mi import Mi + +if __name__ == '__main__': + context = zmq.Context() + req_driver_socket = context.socket(zmq.REQ) + req_driver_socket.connect(C.live_rep_socket) + magic = 7355608 + cmd = DeviceCmd.GetData + cv2.namedWindow("test", cv2.WINDOW_AUTOSIZE) + while True: + req_driver_socket.send(struct.pack('i', magic) + struct.pack('i', cmd.value)) + b = req_driver_socket.recv() + arr = np.frombuffer(b, dtype=np.int16, offset=4 + 8 + 4).reshape((256, 5002)) + cv2.imshow('test', arr) + cv2.waitKey(10) + # print(b.__len__())