feat(draft): sync time

draft/sync_time.py

@@ -1,87 +1,66 @@
-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(
+def match(a: int, b: npt.NDArray[np.int_]):
-    '/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/')
+    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')}
+rec_folder = Path("/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/")
-camera_ts_map = {int(file.stem): file for file in camera_folder.glob('*.jpg')}
 
-robot_ts_map = {int(k): v for k, v in json.loads(
+robot_folder = rec_folder / "robot"
-    (rec_folder/'robot.json').read_text()).items()}
+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"]
 
-
+camera_host0, camera_host_id0, camera_d0 = match(device_host0, camera_host_arr)
-robot_device_arr = np.array([robot_ts_map[k]['device_ts']
+camera_host1, camera_host_id1, camera_d1 = match(device_host1, camera_host_arr)
-                            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())))
 
 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
@@ -92,27 +71,26 @@ 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_host_diff", device_host_diff / 10**9)
-print('device_deivce_diff', device_deivce_diff/10**9)
+print("device_deivce_diff", device_deivce_diff / 10**9)
-print('robot_device_diff', robot_device_diff/10**9)
+print("robot_device_diff", robot_device_diff / 10**9)
-print('camera_host_diff', camera_host_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 - 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 - 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_host - camera_host", (device_host_diff - camera_host_diff) / 10**9)
-print('device_deivce - robot_device',
+print("device_deivce - robot_device", (device_deivce_diff - robot_device_diff) / 10**9)
-      (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()))
+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()))
+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()