feat: add to_zip for sync time

fix: robot rtsi record ratio

.gitignore

@@ -8,4 +8,5 @@ launch.toml
 doc
 flandre.egg-info
 build
-dist
+dist
+config/software.json

draft/sync_time.py

@@ -1,11 +1,16 @@
 import json
 from pathlib import Path
+import subprocess
+import tempfile
 
 import matplotlib.pyplot as plt
 import numpy as np
 import numpy.typing as npt
+from tqdm import tqdm
 
 from flandre.utils.RfFrame import b2t
+from flandre.utils.RfMeta import RfFrameMeta
+from flandre.utils.archive import to_zip
 
 
 def match(a: int, b: npt.NDArray[np.int_]):
@@ -119,15 +124,59 @@ camera_host_cut = (
 
 # plt.show()
 
-for i, k in enumerate(device_device_cut):
+avif_folder = Path(tempfile.gettempdir()) / "avif_temp"
+avif_folder.mkdir(exist_ok=True)
+
+zip_li: list[tuple[bytes, RfFrameMeta, list[tuple[Path, str]]]] = []
+for i, k in enumerate(tqdm(device_device_cut)):
     res1, idx1, offset1 = match(k, robot_device_cut)
     res2, idx2, offset2 = match(k, camera_host_cut)
 
     r = robot_ts_map[robot_host_arr[idx1 + robot_host_id0]]
     c = camera_ts_map[camera_host_arr[idx2 + camera_host_id0]]
-    print(
+
-        offset1 * 10**-9,
+    target = avif_folder / c.with_suffix(".avif").name
-        # (device_host_arr[i] - r["host_ts"]) * 10**-9,
+    if not target.exists():
-        offset2 * 10**-9,
+        subprocess.run(
-        # (device_host_arr[i] - camera_host_arr[idx2 + camera_host_id0]) * 10**-9,
+            [
+                "avifenc",
+                "-y",
+                "420",
+                "-q",
+                "50",
+                c,
+                target,
+            ],
+            stdout=subprocess.DEVNULL,
+        )
+    file = device_ts_map[device_host_arr[i]]
+    seq, encoder, host_ts, device_ts, buffer = b2t(file.read_bytes())
+    zip_li.append(
+        (
+            buffer,
+            RfFrameMeta(
+                sequence_id=seq,
+                encoder=encoder,
+                robot_x=int(r["x"] * 100000),
+                robot_y=int(r["y"] * 100000),
+                robot_z=int(r["z"] * 100000),
+                robot_rx=int(r["rx"] * 1000),
+                robot_ry=int(r["ry"] * 1000),
+                robot_rz=int(r["rz"] * 1000),
+                robot_force_x=int(r["fx"] * 1000),
+                robot_force_y=int(r["fy"] * 1000),
+                robot_force_z=int(r["fz"] * 1000),
+                robot_force_rx=int(r["frx"] * 10000),
+                robot_force_ry=int(r["fry"] * 10000),
+                robot_force_rz=int(r["frz"] * 10000),
+            ),
+            [(c, ".avif")],
+        )
     )
+to_zip(zip_li, Path("/tmp/f1"), Path("/tmp/f2"))
+# print(
+#     offset1 * 10**-9,
+#     # (device_host_arr[i] - r["host_ts"]) * 10**-9,
+#     offset2 * 10**-9,
+#     # (device_host_arr[i] - camera_host_arr[idx2 + camera_host_id0]) * 10**-9,
+# )

flandre/nodes/Muxer.py

@@ -119,15 +119,15 @@ class Muxer(Node):
                             robot_x=self.rtsi.pos[0],
                             robot_y=self.rtsi.pos[1],
                             robot_z=self.rtsi.pos[2],
-                            robot_roll=self.rtsi.pos[3],
+                            robot_rx=self.rtsi.pos[3],
-                            robot_pitch=self.rtsi.pos[4],
+                            robot_ry=self.rtsi.pos[4],
-                            robot_yal=self.rtsi.pos[5],
+                            robot_rz=self.rtsi.pos[5],
                             robot_force_x=self.rtsi.force[0],
                             robot_force_y=self.rtsi.force[1],
                             robot_force_z=self.rtsi.force[2],
-                            robot_force_roll=self.rtsi.force[3],
+                            robot_force_rx=self.rtsi.force[3],
-                            robot_force_pitch=self.rtsi.force[4],
+                            robot_force_ry=self.rtsi.force[4],
-                            robot_force_yal=self.rtsi.force[5],
+                            robot_force_rz=self.rtsi.force[5],
                         ),
                         self.seq_meta_live,
                         driver_data_body,

flandre/utils/Msg.py

@@ -535,17 +535,17 @@ class RobotRtsiMsg(Msg):
                 meta.robot_x,
                 meta.robot_y,
                 meta.robot_z,
-                meta.robot_roll,
+                meta.robot_rx,
-                meta.robot_pitch,
+                meta.robot_ry,
-                meta.robot_yal,
+                meta.robot_rz,
             ),
             force=(
                 meta.robot_force_x,
                 meta.robot_force_y,
                 meta.robot_force_z,
-                meta.robot_force_roll,
+                meta.robot_force_rx,
-                meta.robot_force_pitch,
+                meta.robot_force_ry,
-                meta.robot_force_yal,
+                meta.robot_force_rz,
             ),
         )
 

flandre/utils/RfMeta.py

@@ -90,17 +90,17 @@ class RfFrameMeta(RfMeta):
     robot_y: Annotated[int, "Y"] = None
     robot_z: Annotated[int, "Z"] = None
 
-    robot_roll: Annotated[int, "RX"] = None
+    robot_rx: Annotated[int, "RX"] = None
-    robot_pitch: Annotated[int, "RY"] = None
+    robot_ry: Annotated[int, "RY"] = None
-    robot_yal: Annotated[int, "RZ"] = None
+    robot_rz: Annotated[int, "RZ"] = None
 
     robot_force_x: Annotated[int, "FX"] = None
     robot_force_y: Annotated[int, "FY"] = None
     robot_force_z: Annotated[int, "FZ"] = None
 
-    robot_force_roll: Annotated[int, "FRX"] = None
+    robot_force_rx: Annotated[int, "FRX"] = None
-    robot_force_pitch: Annotated[int, "FRY"] = None
+    robot_force_ry: Annotated[int, "FRY"] = None
-    robot_force_yal: Annotated[int, "FRZ"] = None
+    robot_force_rz: Annotated[int, "FRZ"] = None
 
     blake2b: Annotated[str, "B2B"] = None
 

flandre/utils/archive.py

@@ -4,6 +4,7 @@ import subprocess
 import zipfile
 from pathlib import Path
 
+from tqdm import tqdm
 import zstd
 
 TEMP_FOLDER = Path("/mnt/16T/private_dataset/New Folder/temp")
@@ -42,22 +43,23 @@ def folder_to_zip(folder: Path):
 
 
 def to_zip(
-    li: list[tuple[Path, "RfFrameMeta", list[tuple[Path, str]]]],
+    li: list[tuple[Path | bytes, "RfFrameMeta", list[tuple[Path, str]]]],
     temp_dst: Path,
     zipdst: Path,
 ):
     shutil.rmtree(temp_dst)
     temp_dst.mkdir(parents=True, exist_ok=True)
-    for i, (file, meta, farr) in enumerate(li):
+    for i, (file, meta, farr) in enumerate(tqdm(li)):
-        b2b = hashlib.blake2b(file.read_bytes(), digest_size=4).hexdigest()
+        if isinstance(file, Path):
+            b = file.read_bytes()
+        else:
+            b = file
+        b2b = hashlib.blake2b(b, digest_size=4).hexdigest()
         meta.blake2b = b2b
-        src = file
         dst = temp_dst / f"{i}.zst"
-        subprocess.run(["zstd", "-f", src, "-o", dst])
+        dst.write_bytes(zstd.compress(b, 3))
-
         dstj = temp_dst / f"{i}.meta"
         dstj.write_text(meta.name)
-
         for srcf, suffix in farr:
             dstf = temp_dst / f"{i}{suffix}"
             shutil.copy(srcf, dstf)

flandre/utils/robot.py

@@ -0,0 +1,28 @@
+def float2int(
+    x: float,
+    y: float,
+    z: float,
+    r: float,
+    p: float,
+    yy: float,
+    fx: float,
+    fy: float,
+    fz: float,
+    fr: float,
+    fp: float,
+    fyy: float,
+):
+    return (
+        int(x * 100000),
+        int(y * 100000),
+        int(z * 100000),
+        int(r * 1000),
+        int(p * 1000),
+        int(yy * 1000),
+        int(fx * 10),
+        int(fy * 10),
+        int(fz * 10),
+        int(fr * 100),
+        int(fp * 100),
+        int(fyy * 100),
+    )