yukari/flandre
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add to_zip for sync time

Browse Source 
fix: robot rtsi record ratio
This commit is contained in:
flandre 2025-06-11 14:08:36 +08:00
parent b979dc9a68
commit 0fbeaf2f6e
7 changed files with 112 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

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

61
draft/sync_time.py
View File

@ -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,
# )

12
flandre/nodes/Muxer.py
View File

@ -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,

12
flandre/utils/Msg.py
View File

@ -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,
),
)

12
flandre/utils/RfMeta.py
View File

@ -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

16
flandre/utils/archive.py
View File

@ -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)

28
flandre/utils/robot.py Normal file
View File

@ -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),
)