From 0fbeaf2f6e07b085b3a0100bb6c5277557efcd6b Mon Sep 17 00:00:00 2001 From: flandre Date: Wed, 11 Jun 2025 14:08:36 +0800 Subject: [PATCH] feat: add to_zip for sync time fix: robot rtsi record ratio --- .gitignore | 3 +- draft/sync_time.py | 61 ++++++++++++++++++++++++++++++++++++---- flandre/nodes/Muxer.py | 12 ++++---- flandre/utils/Msg.py | 12 ++++---- flandre/utils/RfMeta.py | 12 ++++---- flandre/utils/archive.py | 16 ++++++----- flandre/utils/robot.py | 28 ++++++++++++++++++ 7 files changed, 112 insertions(+), 32 deletions(-) create mode 100644 flandre/utils/robot.py diff --git a/.gitignore b/.gitignore index eddb310..c31216b 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,5 @@ launch.toml doc flandre.egg-info build -dist \ No newline at end of file +dist +config/software.json \ No newline at end of file diff --git a/draft/sync_time.py b/draft/sync_time.py index 3239725..76aeed2 100644 --- a/draft/sync_time.py +++ b/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, - # (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, + + target = avif_folder / c.with_suffix(".avif").name + if not target.exists(): + subprocess.run( + [ + "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, +# ) diff --git a/flandre/nodes/Muxer.py b/flandre/nodes/Muxer.py index 26a67ec..d3891d7 100644 --- a/flandre/nodes/Muxer.py +++ b/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_pitch=self.rtsi.pos[4], - robot_yal=self.rtsi.pos[5], + robot_rx=self.rtsi.pos[3], + robot_ry=self.rtsi.pos[4], + 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_pitch=self.rtsi.force[4], - robot_force_yal=self.rtsi.force[5], + robot_force_rx=self.rtsi.force[3], + robot_force_ry=self.rtsi.force[4], + robot_force_rz=self.rtsi.force[5], ), self.seq_meta_live, driver_data_body, diff --git a/flandre/utils/Msg.py b/flandre/utils/Msg.py index f941781..819fc06 100644 --- a/flandre/utils/Msg.py +++ b/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_pitch, - meta.robot_yal, + meta.robot_rx, + meta.robot_ry, + meta.robot_rz, ), force=( meta.robot_force_x, meta.robot_force_y, meta.robot_force_z, - meta.robot_force_roll, - meta.robot_force_pitch, - meta.robot_force_yal, + meta.robot_force_rx, + meta.robot_force_ry, + meta.robot_force_rz, ), ) diff --git a/flandre/utils/RfMeta.py b/flandre/utils/RfMeta.py index 262e824..2449529 100644 --- a/flandre/utils/RfMeta.py +++ b/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_pitch: Annotated[int, "RY"] = None - robot_yal: Annotated[int, "RZ"] = None + robot_rx: Annotated[int, "RX"] = None + robot_ry: Annotated[int, "RY"] = 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_pitch: Annotated[int, "FRY"] = None - robot_force_yal: Annotated[int, "FRZ"] = None + robot_force_rx: Annotated[int, "FRX"] = None + robot_force_ry: Annotated[int, "FRY"] = None + robot_force_rz: Annotated[int, "FRZ"] = None blake2b: Annotated[str, "B2B"] = None diff --git a/flandre/utils/archive.py b/flandre/utils/archive.py index 8917c8b..6e8b0af 100644 --- a/flandre/utils/archive.py +++ b/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): - b2b = hashlib.blake2b(file.read_bytes(), digest_size=4).hexdigest() + for i, (file, meta, farr) in enumerate(tqdm(li)): + 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) diff --git a/flandre/utils/robot.py b/flandre/utils/robot.py new file mode 100644 index 0000000..963ccb8 --- /dev/null +++ b/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), + )