152 lines
4.3 KiB
Python
152 lines
4.3 KiB
Python
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/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('*.bin')}
|
|
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(
|
|
(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]):
|
|
"""
|
|
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_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):
|
|
|
|
# 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())))
|
|
|
|
# print(ass[1766-1:1766+2])
|
|
# seq, encoder, host_ts, device_ts, buffer = b2t(device_ts_map[1749444627751252600].read_bytes())
|
|
# print(seq)
|
|
# seq, encoder, host_ts, device_ts, buffer = b2t(device_ts_map[1749442951214858600].read_bytes())
|
|
# print(seq)
|
|
# seq, encoder, host_ts, device_ts, buffer = b2t(device_ts_map[1749444583738329700].read_bytes())
|
|
# print(seq)
|
|
|
|
# 1766 1749442951214858600
|
|
# 2271 1749442950915344400
|
|
# 2272 1749442950944313600
|
|
# 2273 1749442950961272400
|
|
# 2274 1749442950993404700
|
|
# 2275 1749442951056585700
|
|
# 2276 1749442951061834900
|
|
# 2277 1749442951119567900
|
|
# 2278 1749442951139249100
|
|
# 2279 1749442951144567800
|
|
# 2280 1749442951149875300
|
|
# 2281 1749442951169837900
|
|
# 2282 1749442951197964400
|