From 84ee2c1f84f0ece9af7e442ae875133129a0dcac Mon Sep 17 00:00:00 2001
From: remilia <remilia@flandre.net>
Date: Mon, 26 May 2025 23:12:52 +0800
Subject: [PATCH] add robot record

---
 flandre/launcher.py | 60 +++++++++++++++++++--------------------------
 1 file changed, 25 insertions(+), 35 deletions(-)

diff --git a/flandre/launcher.py b/flandre/launcher.py
index b7268b4..bce5cb2 100644
--- a/flandre/launcher.py
+++ b/flandre/launcher.py
@@ -412,13 +412,6 @@ def robot_record(folder):
     p = Path(folder)
     p.mkdir(parents=True, exist_ok=True)
     robot.setup()
-
-    # last_ns = time.perf_counter_ns()
-    # while True:
-    #     ns = time.perf_counter_ns()
-    #     print(ns - last_ns)
-    #     last_ns = ns
-
     q = queue.Queue()
 
     def rtsi_thread():
@@ -426,8 +419,6 @@ def robot_record(folder):
             'actual_TCP_pose,actual_TCP_force,timestamp', 250)  # 输出订阅,配方1
         robot.rt.start()  # rtsi 开始
         arr = []
-        last_ns = time.time_ns()
-        last_device_ts = 0
         while True:
             recv_out: DataObject = robot.rt.get_output_data()
             if recv_out is None:
@@ -435,42 +426,41 @@ 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
-                ns = time.time_ns()
-                print(ns - last_ns, recv_out.timestamp - last_device_ts, x)
-                last_ns = ns
-                last_device_ts = recv_out.timestamp
+                local_ns = time.time_ns()
 
                 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
+                    x=x,
+                    y=y,
+                    z=z,
+                    fx=fx,
+                    fy=fy,
+                    fz=fz,
+                    rx=rx,
+                    ry=ry,
+                    rz=rz,
+                    frx=frx,
+                    fry=fry,
+                    frz=frz,
+                    local_ns=local_ns,
+                    device_ts=recv_out.timestamp,
                 )
-                # arr.append(d)
-                # print(arr.__len__())
-                # if arr.__len__() == 100:
-                #     q.put(arr)
-                #     arr = []
+                arr.append(d)
+                if arr.__len__() == 100:
+                    q.put(arr)
+                    arr = []
 
     def write_thread():
         while True:
-            print(q.get()[0]['ns'])
+            ns = time.time_ns()
+            # print(q.get()[0]['ns'])
+            (p / f'{ns}.json').write_text(json.dumps(q.get()))
 
     tr = threading.Thread(target=rtsi_thread)
-    # tw = threading.Thread(target=write_thread)
+    tw = threading.Thread(target=write_thread)
     tr.start()
-    # tw.start()
+    tw.start()
     tr.join()
-    # tw.join()
+    tw.join()
 
 
 if __name__ == '__main__':