add queue record rtsi

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,13 +351,20 @@ 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
+
+    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(
@@ -365,8 +380,24 @@ def robot_record(folder):
                     frx=frx,
                     fry=fry,
                     frz=frz,
+                    ns=ns
                 )
-            (p / f'{time.time_ns()}.rtsi.json').write_text(json.dumps(d))
+                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__':

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