From 35d0cac7c6ab398e95e4448e5d7da420f1ba54b3 Mon Sep 17 00:00:00 2001 From: flandre Date: Tue, 10 Jun 2025 19:19:25 +0800 Subject: [PATCH] feat(test): add sync prepare --- pyproject.toml | 5 ++ test/sync_time.py | 106 ++++++++++++++++++++++++++++---------- test/sync_time_prepare.py | 52 +++++++++++++++++++ uv.lock | 17 ++++++ 4 files changed, 154 insertions(+), 26 deletions(-) create mode 100644 test/sync_time_prepare.py diff --git a/pyproject.toml b/pyproject.toml index 31d876a..14fc600 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -33,3 +33,8 @@ package-data = { "flandre.assets" = ["*.svg", "*.png"], "flandre.pyqt" = ["*.ui" [project.scripts] flandre = "flandre.launcher:entrypoint" + +[project.optional-dependencies] +to = [ + "nvidia-nvimgcodec-cu12>=0.5.0.13", +] diff --git a/test/sync_time.py b/test/sync_time.py index 63b8762..16965c7 100644 --- a/test/sync_time.py +++ b/test/sync_time.py @@ -1,23 +1,41 @@ +import matplotlib.pyplot as plt import json from pathlib import Path import itertools +import numpy as np + from flandre.utils.RfFrame import b2t rec_folder = Path( - '/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record/') + '/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/') robot_folder = rec_folder / 'robot' device_folder = rec_folder / 'device' camera_folder = rec_folder / 'camera' -device_ts_map = {int(file.stem): file for file in device_folder.glob('*')} -camera_ts_map = {int(file.stem): file for file in camera_folder.glob('*')} +device_ts_map = {int(file.stem): file for file in device_folder.glob('*.bin')} +camera_ts_map = {int(file.stem): file for file in camera_folder.glob('*.jpg')} -js = itertools.chain(*[json.loads(file.read_text()) - for file in robot_folder.glob('*')]) -robot_ts_map = {v['host_ts']: v for v in js} +robot_ts_map = {int(k): v for k, v in json.loads( + (rec_folder/'robot.json').read_text()).items()} + +device_host_arr = np.array(sorted(device_ts_map.keys())) + +device_device_list: list[int] = [] +for k in device_host_arr: + seq, encoder, host_ts, device_deivce, buffer = b2t( + device_ts_map[k].open('rb').read(100)) + device_device_list.append(device_deivce) + +device_device_arr = np.array(device_device_list) + +robot_host_arr = sorted(robot_ts_map.keys()) + + +robot_device_arr = np.array([robot_ts_map[k]['device_ts'] + for k in robot_host_arr]) def func(a: int, b: list[int]): @@ -32,36 +50,72 @@ def func(a: int, b: list[int]): The largest number in `b` <= `a`, or None if no such number exists. """ # Create a new list containing only the numbers from b that are <= a - candidates = [num for num in b if num <= a] - # If the candidates list is not empty, return the largest number from it - if candidates: - return max(candidates) - else: - # If no number in b was less than or equal to a, return None - return None + arr = np.array(b) + arr -= a + arr = np.abs(arr) + argmin = np.argmin(arr) + m = b[argmin] + return m, a-m # print(device_ts_map.__len__()) # print(robot_ts_map.__len__()) -ass = sorted(list(device_ts_map.keys()))[50:-100] -a0 = ass[0] -a1 = ass[-1] +ass = sorted(list(device_ts_map.keys()))[500:-1500] +device_host0 = ass[0] +device_host1 = ass[-1] -print((a1-a0)*10**-9) -r0 = robot_ts_map[func(a0, list(robot_ts_map.keys()))]['device_ts'] -r1 = robot_ts_map[func(a1, list(robot_ts_map.keys()))]['device_ts'] +m0, robot_d0 = func(device_host0, sorted(list(robot_ts_map.keys()))) +robot_device0 = robot_ts_map[m0]['device_ts'] +m1, robot_d1 = func(device_host1, sorted(list(robot_ts_map.keys()))) +robot_device1 = robot_ts_map[m1]['device_ts'] -seq, encoder, host_ts, device_ts0, buffer = b2t(device_ts_map[a0].read_bytes()) -print(device_ts0) -seq, encoder, host_ts, device_ts1, buffer = b2t(device_ts_map[a1].read_bytes()) -print(device_ts1) +camera_host0, camera_d0 = func( + device_host0, sorted(list(camera_ts_map.keys()))) +camera_host1, camera_d1 = func( + device_host1, sorted(list(camera_ts_map.keys()))) -print(a1-a0, (device_ts1-device_ts0)*10**3, int((r1-r0)*10**9)) -print((a1-a0-(device_ts1-device_ts0)*10**3)*10**-9) -print((a1-a0-int((r1-r0)*10**9))*10**-9) +seq, encoder, host_ts, device_deivce0, buffer = b2t( + device_ts_map[device_host0].read_bytes()) +device_deivce0 *= 10**3 +seq, encoder, host_ts, device_deivce1, buffer = b2t( + device_ts_map[device_host1].read_bytes()) +device_deivce1 *= 10**3 + +device_host_diff = device_host1-device_host0 +device_deivce_diff = device_deivce1-device_deivce0 +robot_device_diff = robot_device1-robot_device0 +camera_host_diff = camera_host1 - camera_host0 + +print(robot_d0/10**9, robot_d1/10**9) +print(camera_d0/10**9, camera_d1/10**9) + +print('device_host_diff', device_host_diff/10**9) +print('device_deivce_diff', device_deivce_diff/10**9) +print('robot_device_diff', robot_device_diff/10**9) +print('camera_host_diff', camera_host_diff/10**9) + +print('device_host - device_deivce', (device_host_diff-device_deivce_diff)/10**9) +print('device_host - robot_device', (device_host_diff-robot_device_diff)/10**9) +print('device_host - camera_host', (device_host_diff-camera_host_diff)/10**9) +print('device_deivce - robot_device', + (device_deivce_diff-robot_device_diff)/10**9) + + +x = np.array(list(device_ts_map.keys())) +y = np.zeros_like(x) +plt.scatter(x, y) + +x = np.array(list(robot_ts_map.keys())) +y = np.zeros_like(x)+1 +plt.scatter(x, y) + +x = np.array(list(camera_ts_map.keys())) +y = np.zeros_like(x)+2 +plt.scatter(x, y) +plt.show() # for i, a in enumerate(ass): diff --git a/test/sync_time_prepare.py b/test/sync_time_prepare.py new file mode 100644 index 0000000..e70aa3a --- /dev/null +++ b/test/sync_time_prepare.py @@ -0,0 +1,52 @@ +from flandre.utils.RfFrame import b2t +import json +from pathlib import Path +import itertools + +import cv2 +import numpy as np +from nvidia import nvimgcodec +decoder = nvimgcodec.Decoder() +encoder = nvimgcodec.Encoder() + + +rec_folder = Path( + '/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/') + +robot_folder = rec_folder / 'robot' +robot_folder = rec_folder / 'robot' + +d: dict[int, dict] = dict() +for file in robot_folder.glob('*'): + for line in file.read_text().splitlines(): + s = line.split(',') + v = dict( + host_ts=int(s[0]), + device_ts=int(float(s[1])*10**9), + x=float(s[2]), + y=float(s[3]), + z=float(s[4]), + rx=float(s[5]), + ry=float(s[6]), + rz=float(s[7]), + fx=float(s[8]), + fy=float(s[9]), + fz=float(s[10]), + frx=float(s[11]), + fry=float(s[12]), + frz=float(s[13]), + ) + d[v['host_ts']] = v +(rec_folder/'robot.json').write_text(json.dumps(d)) + +for file in Path('/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/camera_raw').glob('*.bin'): + fb = Path(file).read_bytes() + b = np.frombuffer(fb, dtype=np.uint8).reshape( + (1080, 1920, 3)) + b = cv2.cvtColor(b, cv2.COLOR_BGR2RGB) + # cv2.imshow('',) + # cv2.waitKey(0) + # enc_params = nvimgcodec.EncodeParams(quality=5, chroma_subsampling=nvimgcodec.ChromaSubsampling.CSS_GRAY) + # encoder.write('a.jpg', b, params=enc_params) + encoder.write((file.parent.parent/'camera' / + file.with_suffix('.jpg').name).__str__(), b) diff --git a/uv.lock b/uv.lock index 9fb8327..ad8ca9f 100644 --- a/uv.lock +++ b/uv.lock @@ -501,6 +501,11 @@ dependencies = [ { name = "zstd" }, ] +[package.optional-dependencies] +to = [ + { name = "nvidia-nvimgcodec-cu12" }, +] + [package.metadata] requires-dist = [ { name = "click", specifier = ">=8.1.8" }, @@ -509,6 +514,7 @@ requires-dist = [ { name = "jupyter", specifier = ">=1.1.1" }, { name = "matplotlib", specifier = ">=3.10.1" }, { name = "mido", extras = ["ports-rtmidi"], specifier = ">=1.3.3" }, + { name = "nvidia-nvimgcodec-cu12", marker = "extra == 'to'", specifier = ">=0.5.0.13" }, { name = "opencv-python", specifier = ">=4.10.0.84" }, { name = "platformdirs", specifier = ">=4.3.6" }, { name = "pyjoystick", specifier = ">=1.2.4" }, @@ -520,6 +526,7 @@ requires-dist = [ { name = "websockets", specifier = ">=15.0.1" }, { name = "zstd", specifier = ">=1.5.6.7" }, ] +provides-extras = ["to"] [[package]] name = "fonttools" @@ -1222,6 +1229,16 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/89/ea/00537f599eb230771157bc509f6ea5b2dddf05d4b09f9d2f1d7096a18781/numpy-2.2.0-cp312-cp312-win_amd64.whl", hash = "sha256:3a4199f519e57d517ebd48cb76b36c82da0360781c6a0353e64c0cac30ecaad3", size = 12613227, upload-time = "2024-12-08T15:32:34.792Z" }, ] +[[package]] +name = "nvidia-nvimgcodec-cu12" +version = "0.5.0.13" +source = { registry = "https://pypi.org/simple" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/28/9a/f6c9105cb045f52af2096417ce92e7e8fba4d24ffe24d2cc82eb9bbe5534/nvidia_nvimgcodec_cu12-0.5.0.13-py3-none-manylinux2014_aarch64.whl", hash = "sha256:d76fadc2ed0f9075871627e45f2592c7807a0e944a0505afc21f87ccceb75caa", size = 17967646, upload-time = "2025-03-18T10:40:26.76Z" }, + { url = "https://files.pythonhosted.org/packages/f0/6d/1c9919912ee97a4f52674f1c2deec7ab80df8fdd9a8b76f8ed4d75ebf799/nvidia_nvimgcodec_cu12-0.5.0.13-py3-none-manylinux2014_x86_64.whl", hash = "sha256:24cf0a759b1b02a6c3c0aedf8bf6602643f74c4c6df68c4b1c3c4ec1d48d71b0", size = 23304465, upload-time = "2025-03-18T10:40:54.19Z" }, + { url = "https://files.pythonhosted.org/packages/3b/a8/58243c8f855b29116b1da9ee222b1afeb6a18a2120e66a0af2f85bee13ac/nvidia_nvimgcodec_cu12-0.5.0.13-py3-none-win_amd64.whl", hash = "sha256:331fc77cc844b982ab0d5c0e9d660b2e777e82c61471657c18f40ca3d15640c2", size = 27621245, upload-time = "2025-03-18T10:38:26.251Z" }, +] + [[package]] name = "opencv-python" version = "4.10.0.84"