From c00e1bb6cf328fa726fc80d6c0e1fe5550a36f8e Mon Sep 17 00:00:00 2001 From: flandre Date: Tue, 10 Jun 2025 20:34:27 +0800 Subject: [PATCH] feat(draft): sync time --- draft/sync_time.py | 148 +++++++++++++++++++-------------------------- 1 file changed, 63 insertions(+), 85 deletions(-) diff --git a/draft/sync_time.py b/draft/sync_time.py index 16965c7..f88bf27 100644 --- a/draft/sync_time.py +++ b/draft/sync_time.py @@ -1,130 +1,108 @@ -import matplotlib.pyplot as plt import json from pathlib import Path -import itertools +import matplotlib.pyplot as plt import numpy as np +import numpy.typing as npt from flandre.utils.RfFrame import b2t -rec_folder = Path( - '/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/') +def match(a: int, b: npt.NDArray[np.int_]): + arr = b.copy() + arr -= a + arr = np.abs(arr) + argmin = np.argmin(arr) + m: int = b[argmin] + return m, argmin, a - m -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('*.bin')} -camera_ts_map = {int(file.stem): file for file in camera_folder.glob('*.jpg')} +rec_folder = Path("/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/") -robot_ts_map = {int(k): v for k, v in json.loads( - (rec_folder/'robot.json').read_text()).items()} +robot_folder = rec_folder / "robot" +device_folder = rec_folder / "device" +camera_folder = rec_folder / "camera" -device_host_arr = np.array(sorted(device_ts_map.keys())) +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")} + +device_host_arr = np.array(sorted(device_ts_map.keys()))[100:-100] +device_host_zero = device_host_arr[0] + +robot_ts_map = { + int(k): v for k, v in json.loads((rec_folder / "robot.json").read_text()).items() +} 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_ts_map[k].open("rb").read(100) + ) device_device_list.append(device_deivce) - device_device_arr = np.array(device_device_list) +robot_host_arr = np.array(sorted(robot_ts_map.keys())) +robot_device_arr = np.array([robot_ts_map[k]["device_ts"] for k in robot_host_arr]) +camera_host_arr = np.array(sorted(camera_ts_map.keys())) -robot_host_arr = sorted(robot_ts_map.keys()) +device_host0 = device_host_arr[0] +device_host1 = device_host_arr[-1] +m0, robot_host_id0, robot_d0 = match(device_host0, robot_host_arr) +robot_device0 = robot_ts_map[m0]["device_ts"] +m1, robot_host_id1, robot_d1 = match(device_host1, robot_host_arr) +robot_device1 = robot_ts_map[m1]["device_ts"] - -robot_device_arr = np.array([robot_ts_map[k]['device_ts'] - for k in robot_host_arr]) - - -def func(a: int, b: list[int]): - """ - Finds the largest number in list `b` that is less than or equal to `a`. - - Args: - a: An integer. - b: A list of integers. - - Returns: - 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 - - 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()))[500:-1500] -device_host0 = ass[0] -device_host1 = ass[-1] - - -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'] - -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()))) +camera_host0, camera_host_id0, camera_d0 = match(device_host0, camera_host_arr) +camera_host1, camera_host_id1, camera_d1 = match(device_host1, camera_host_arr) seq, encoder, host_ts, device_deivce0, buffer = b2t( - device_ts_map[device_host0].read_bytes()) + 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_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 +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(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_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) +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())) +x = (device_host_arr - device_host_zero) / 10**9 y = np.zeros_like(x) plt.scatter(x, y) -x = np.array(list(robot_ts_map.keys())) -y = np.zeros_like(x)+1 +x = (robot_host_arr[robot_host_id0 : robot_host_id1 + 1] - device_host_zero) / 10**9 +y = np.zeros_like(x) + 1 plt.scatter(x, y) -x = np.array(list(camera_ts_map.keys())) -y = np.zeros_like(x)+2 +x = (camera_host_arr[camera_host_id0 : camera_host_id1 + 1] - device_host_zero) / 10**9 +y = np.zeros_like(x) + 2 plt.scatter(x, y) plt.show() # for i, a in enumerate(ass): -# try: -# # a - func(a, list(robot_ts_map.keys())) -# print(i, a-func(a, list(robot_ts_map.keys()))) -# except Exception: -# print(i,a) -# pass +# try: +# # a - func(a, list(robot_ts_map.keys())) +# print(i, a-func(a, list(robot_ts_map.keys()))) +# except Exception: +# print(i,a) +# pass # a = 174945200000000000 # print(func(a, list(robot_ts_map.keys())))