style: format ruff init
This commit is contained in:
parent
c00e1bb6cf
commit
66c0094fe2
@ -1,15 +1,13 @@
|
||||
import struct
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import zmq
|
||||
|
||||
from flandre.config import C
|
||||
from flandre.nodes.Device import Device, DeviceCmd
|
||||
from flandre.nodes.Mi import Mi
|
||||
from flandre.nodes.Device import DeviceCmd
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
context = zmq.Context()
|
||||
req_driver_socket = context.socket(zmq.REQ)
|
||||
req_driver_socket.connect(C.live_rep_socket)
|
||||
@ -17,9 +15,9 @@ if __name__ == '__main__':
|
||||
cmd = DeviceCmd.GetData
|
||||
cv2.namedWindow("test", cv2.WINDOW_AUTOSIZE)
|
||||
while True:
|
||||
req_driver_socket.send(struct.pack('i', magic) + struct.pack('i', cmd.value))
|
||||
req_driver_socket.send(struct.pack("i", magic) + struct.pack("i", cmd.value))
|
||||
b = req_driver_socket.recv()
|
||||
arr = np.frombuffer(b, dtype=np.int16, offset=4 + 8 + 4).reshape((256, 5002))
|
||||
cv2.imshow('test', arr)
|
||||
cv2.imshow("test", arr)
|
||||
cv2.waitKey(10)
|
||||
# print(b.__len__())
|
||||
|
||||
@ -11,7 +11,7 @@ global_config = Config(arg=2)
|
||||
|
||||
|
||||
def process1():
|
||||
print('Process1 arg=', global_config.arg)
|
||||
print("Process1 arg=", global_config.arg)
|
||||
|
||||
|
||||
def main():
|
||||
@ -20,8 +20,8 @@ def main():
|
||||
p1 = multiprocessing.Process(target=process1)
|
||||
p1.start()
|
||||
p1.join()
|
||||
print('main arg=', global_config.arg)
|
||||
print("main arg=", global_config.arg)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
import platformdirs
|
||||
|
||||
if __name__ == '__main__':
|
||||
print(platformdirs.user_data_dir('Flandre', 'Scarlet'))
|
||||
print(platformdirs.user_data_dir('Flandre'))
|
||||
if __name__ == "__main__":
|
||||
print(platformdirs.user_data_dir("Flandre", "Scarlet"))
|
||||
print(platformdirs.user_data_dir("Flandre"))
|
||||
|
||||
@ -1,28 +1,26 @@
|
||||
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/')
|
||||
rec_folder = Path("/run/media/lambda/b86dccdc-f134-464b-a310-6575ee9ae85c/record2/")
|
||||
|
||||
robot_folder = rec_folder / 'robot'
|
||||
robot_folder = rec_folder / 'robot'
|
||||
robot_folder = rec_folder / "robot"
|
||||
robot_folder = rec_folder / "robot"
|
||||
|
||||
d: dict[int, dict] = dict()
|
||||
for file in robot_folder.glob('*'):
|
||||
for file in robot_folder.glob("*"):
|
||||
for line in file.read_text().splitlines():
|
||||
s = line.split(',')
|
||||
s = line.split(",")
|
||||
v = dict(
|
||||
host_ts=int(s[0]),
|
||||
device_ts=int(float(s[1])*10**9),
|
||||
device_ts=int(float(s[1]) * 10**9),
|
||||
x=float(s[2]),
|
||||
y=float(s[3]),
|
||||
z=float(s[4]),
|
||||
@ -36,17 +34,19 @@ for file in robot_folder.glob('*'):
|
||||
fry=float(s[12]),
|
||||
frz=float(s[13]),
|
||||
)
|
||||
d[v['host_ts']] = v
|
||||
(rec_folder/'robot.json').write_text(json.dumps(d))
|
||||
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'):
|
||||
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 = 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)
|
||||
encoder.write(
|
||||
(file.parent.parent / "camera" / file.with_suffix(".jpg").name).__str__(), b
|
||||
)
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
import cupy as cp
|
||||
if __name__ == '__main__':
|
||||
p = cp.ndarray([1,2,3])
|
||||
|
||||
if __name__ == "__main__":
|
||||
p = cp.ndarray([1, 2, 3])
|
||||
print(p.__class__.__name__)
|
||||
print(type(p).__qualname__)
|
||||
|
||||
@ -3,22 +3,23 @@ import logging
|
||||
import zmq
|
||||
from zmq import Context, Socket
|
||||
|
||||
from flandre.utils.Msg import Msg, InterruptMsg
|
||||
from flandre.utils.Msg import InterruptMsg, Msg
|
||||
|
||||
|
||||
class BusClient:
|
||||
fp = 5001
|
||||
bp = 5002
|
||||
|
||||
def __init__(self,
|
||||
*msgs: type(Msg),
|
||||
ctx=None,
|
||||
pub=True,
|
||||
sub=True,
|
||||
conflare=False,
|
||||
poller=False,
|
||||
req_socket_str: str = None,
|
||||
):
|
||||
def __init__(
|
||||
self,
|
||||
*msgs: type(Msg),
|
||||
ctx=None,
|
||||
pub=True,
|
||||
sub=True,
|
||||
conflare=False,
|
||||
poller=False,
|
||||
req_socket_str: str = None,
|
||||
):
|
||||
self.sub: Socket = None
|
||||
if ctx is None:
|
||||
self.ctx: Context = zmq.Context()
|
||||
@ -29,21 +30,23 @@ class BusClient:
|
||||
for msg in msgs:
|
||||
self.sub.setsockopt(zmq.SUBSCRIBE, msg.magic() + msg.eid())
|
||||
if msgs.__len__() == 0:
|
||||
self.sub.setsockopt(zmq.SUBSCRIBE, b'')
|
||||
self.sub.setsockopt(zmq.SUBSCRIBE, b"")
|
||||
if conflare:
|
||||
self.sub.setsockopt(zmq.CONFLATE, 1)
|
||||
self.sub.connect(f'tcp://127.0.0.1:{self.bp}')
|
||||
self.sub.connect(f"tcp://127.0.0.1:{self.bp}")
|
||||
if poller:
|
||||
self.poller = zmq.Poller()
|
||||
self.poller.register(self.sub, zmq.POLLIN)
|
||||
if pub:
|
||||
self.pub = self.ctx.socket(zmq.PUB)
|
||||
self.pub.connect(f'tcp://127.0.0.1:{self.fp}')
|
||||
self.pub.connect(f"tcp://127.0.0.1:{self.fp}")
|
||||
self.req_socket = None
|
||||
if req_socket_str is not None:
|
||||
self.poller_for_interrupt = zmq.Poller()
|
||||
self.sub_for_interrupt = self.ctx.socket(zmq.SUB)
|
||||
self.sub_for_interrupt.setsockopt(zmq.SUBSCRIBE, InterruptMsg.magic() + InterruptMsg.eid())
|
||||
self.sub_for_interrupt.setsockopt(
|
||||
zmq.SUBSCRIBE, InterruptMsg.magic() + InterruptMsg.eid()
|
||||
)
|
||||
# self.sub2.connect(f'tcp://127.0.0.1:{self.bp}')
|
||||
self.req_socket_str = req_socket_str
|
||||
self.req_socket = self.ctx.socket(zmq.REQ)
|
||||
@ -71,20 +74,21 @@ class BusClient:
|
||||
async def send_async(self, msg: Msg):
|
||||
return self.pub.send(msg.encode_msg())
|
||||
|
||||
def req_interrupt(self,
|
||||
data: bytes,
|
||||
interrupt_name: str,
|
||||
timeout=3000,
|
||||
retry_times=114514,
|
||||
cb_retry=None,
|
||||
):
|
||||
self.sub_for_interrupt.connect(f'tcp://127.0.0.1:{self.bp}')
|
||||
def req_interrupt(
|
||||
self,
|
||||
data: bytes,
|
||||
interrupt_name: str,
|
||||
timeout=3000,
|
||||
retry_times=114514,
|
||||
cb_retry=None,
|
||||
):
|
||||
self.sub_for_interrupt.connect(f"tcp://127.0.0.1:{self.bp}")
|
||||
|
||||
for _ in range(retry_times):
|
||||
self.req_socket.send(data)
|
||||
r = dict(self.poller_for_interrupt.poll(timeout))
|
||||
if self.req_socket in r:
|
||||
self.sub_for_interrupt.disconnect(f'tcp://127.0.0.1:{self.bp}')
|
||||
self.sub_for_interrupt.disconnect(f"tcp://127.0.0.1:{self.bp}")
|
||||
return self.req_socket.recv()
|
||||
if cb_retry is not None:
|
||||
cb_retry()
|
||||
@ -97,8 +101,8 @@ class BusClient:
|
||||
msg = Msg.decode_msg(self.sub_for_interrupt.recv())
|
||||
if isinstance(msg, InterruptMsg):
|
||||
if msg.value == interrupt_name:
|
||||
self.sub_for_interrupt.disconnect(f'tcp://127.0.0.1:{self.bp}')
|
||||
self.sub_for_interrupt.disconnect(f"tcp://127.0.0.1:{self.bp}")
|
||||
return None
|
||||
logging.warning('timeout')
|
||||
self.sub_for_interrupt.disconnect(f'tcp://127.0.0.1:{self.bp}')
|
||||
return 'timeout'
|
||||
logging.warning("timeout")
|
||||
self.sub_for_interrupt.disconnect(f"tcp://127.0.0.1:{self.bp}")
|
||||
return "timeout"
|
||||
|
||||
@ -8,24 +8,24 @@ MODULE_FOLDER = Path(__file__).parent
|
||||
|
||||
|
||||
class P:
|
||||
ASSETS = MODULE_FOLDER / 'assets'
|
||||
PYQT = MODULE_FOLDER / 'pyqt'
|
||||
ASSETS = MODULE_FOLDER / "assets"
|
||||
PYQT = MODULE_FOLDER / "pyqt"
|
||||
DEV_PROJECT_FOLDER = MODULE_FOLDER.parent
|
||||
|
||||
|
||||
@dataclasses.dataclass
|
||||
class SoftwareConfig:
|
||||
def make_uri(self, host, port, proto='tcp'):
|
||||
return f'{proto}://{host}:{port}'
|
||||
def make_uri(self, host, port, proto="tcp"):
|
||||
return f"{proto}://{host}:{port}"
|
||||
|
||||
data_folder: Path = platformdirs.user_data_path('Flandre', 'Scarlet')
|
||||
record_folder_: Path = data_folder / 'record'
|
||||
log_folder_: Path = data_folder / 'log'
|
||||
config_folder: Path = platformdirs.user_config_path('Flandre', 'Scarlet')
|
||||
data_folder: Path = platformdirs.user_data_path("Flandre", "Scarlet")
|
||||
record_folder_: Path = data_folder / "record"
|
||||
log_folder_: Path = data_folder / "log"
|
||||
config_folder: Path = platformdirs.user_config_path("Flandre", "Scarlet")
|
||||
|
||||
@property
|
||||
def software_config_file(self):
|
||||
return self.config_folder / 'software.json'
|
||||
return self.config_folder / "software.json"
|
||||
|
||||
@property
|
||||
def log_folder(self):
|
||||
@ -47,20 +47,20 @@ class SoftwareConfig:
|
||||
|
||||
@property
|
||||
def imaging_config_folder(self):
|
||||
p = self.config_folder / 'imaging'
|
||||
p = self.config_folder / "imaging"
|
||||
p.mkdir(exist_ok=True, parents=True)
|
||||
return p
|
||||
|
||||
@property
|
||||
def device_config_folder(self):
|
||||
p = self.config_folder / 'device'
|
||||
p = self.config_folder / "device"
|
||||
p.mkdir(exist_ok=True, parents=True)
|
||||
return p
|
||||
|
||||
video_height: int = 1080
|
||||
video_width: int = 960
|
||||
|
||||
live_ip: str = 'scarlet'
|
||||
live_ip: str = "scarlet"
|
||||
live_push_port: int = 5555
|
||||
live_rep_port: int = 5556
|
||||
device_py_rep_port: int = 5558
|
||||
@ -75,13 +75,13 @@ class SoftwareConfig:
|
||||
|
||||
@property
|
||||
def live_rep_socket_http(self):
|
||||
return self.make_uri(self.live_ip, self.live_rep_port, proto='http')
|
||||
return self.make_uri(self.live_ip, self.live_rep_port, proto="http")
|
||||
|
||||
@property
|
||||
def device_py_rep_socket(self):
|
||||
return self.make_uri(self.live_ip, self.device_py_rep_port)
|
||||
|
||||
local_ip: str = '127.0.0.1'
|
||||
local_ip: str = "127.0.0.1"
|
||||
mi_rep_port: int = 5557
|
||||
muxer_rep_port: int = 5560
|
||||
driver_rep_port: int = 5561
|
||||
@ -104,14 +104,14 @@ class SoftwareConfig:
|
||||
def driver_rep_socket(self):
|
||||
return self.make_uri(self.local_ip, self.driver_rep_port)
|
||||
|
||||
switch1_ip: str = 'c1'
|
||||
switch1_token: str = '7ad51e0016e7a9d22f753d5110f76c7d'
|
||||
switch2_ip: str = 'c2'
|
||||
switch2_token: str = 'bf5a7b77a1ba3761ea63fafd8427b7d6'
|
||||
switch1_ip: str = "c1"
|
||||
switch1_token: str = "7ad51e0016e7a9d22f753d5110f76c7d"
|
||||
switch2_ip: str = "c2"
|
||||
switch2_token: str = "bf5a7b77a1ba3761ea63fafd8427b7d6"
|
||||
|
||||
@staticmethod
|
||||
def read_config(path: Path):
|
||||
return SoftwareConfig.read_config_text(path.read_text(encoding='utf-8'))
|
||||
return SoftwareConfig.read_config_text(path.read_text(encoding="utf-8"))
|
||||
|
||||
@staticmethod
|
||||
def read_config_text(text: str):
|
||||
@ -121,7 +121,7 @@ class SoftwareConfig:
|
||||
try:
|
||||
v = j[field.name]
|
||||
match field.type.__name__:
|
||||
case 'Path':
|
||||
case "Path":
|
||||
arg_d[field.name] = Path(v)
|
||||
case _:
|
||||
arg_d[field.name] = v
|
||||
@ -136,7 +136,7 @@ class SoftwareConfig:
|
||||
for field in dataclasses.fields(SoftwareConfig):
|
||||
v = self.__getattribute__(field.name)
|
||||
match field.type.__name__:
|
||||
case 'Path':
|
||||
case "Path":
|
||||
arg_d[field.name] = str(v)
|
||||
case _:
|
||||
arg_d[field.name] = v
|
||||
@ -146,9 +146,9 @@ class SoftwareConfig:
|
||||
if config_file is None:
|
||||
config_file = self.software_config_file
|
||||
config_file.parent.mkdir(exist_ok=True, parents=True)
|
||||
config_file.write_text(self.json_text, encoding='utf-8')
|
||||
config_file.write_text(self.json_text, encoding="utf-8")
|
||||
|
||||
def copy_form(self, arg: 'SoftwareConfig'):
|
||||
def copy_form(self, arg: "SoftwareConfig"):
|
||||
for field in dataclasses.fields(SoftwareConfig):
|
||||
self.__setattr__(field.name, arg.__getattribute__(field.name))
|
||||
|
||||
|
||||
@ -4,12 +4,12 @@ from typing import Callable
|
||||
import cupy as cp
|
||||
import numpy as np
|
||||
|
||||
from flandre.beamformer.dist import refraction_dist, direct_dist
|
||||
from flandre.beamformer.dist import direct_dist, refraction_dist
|
||||
from flandre.beamformer.kernels import dist_mat_to_yids, dist_mat_to_yids_pwi
|
||||
from flandre.utils.Config import DeviceConfig
|
||||
|
||||
|
||||
def repeat_range_in_axis(shape: tuple, axis: int,p=cp):
|
||||
def repeat_range_in_axis(shape: tuple, axis: int, p=cp):
|
||||
idx = [None for _ in shape]
|
||||
idx[axis] = slice(None)
|
||||
idx = tuple(idx)
|
||||
@ -39,8 +39,16 @@ class TFM:
|
||||
for gpu_id in range(4):
|
||||
with cp.cuda.Device(gpu_id):
|
||||
self.canvas.append(cp.zeros((self.device_cfg.rows, 64), dtype=cp.int32))
|
||||
self.eks.append(repeat_range_in_axis((device_cfg.rows, self.s2, device_cfg.cols, device_cfg.cols), 2))
|
||||
self.xks.append(repeat_range_in_axis((device_cfg.rows, self.s2, device_cfg.cols, device_cfg.cols), 3))
|
||||
self.eks.append(
|
||||
repeat_range_in_axis(
|
||||
(device_cfg.rows, self.s2, device_cfg.cols, device_cfg.cols), 2
|
||||
)
|
||||
)
|
||||
self.xks.append(
|
||||
repeat_range_in_axis(
|
||||
(device_cfg.rows, self.s2, device_cfg.cols, device_cfg.cols), 3
|
||||
)
|
||||
)
|
||||
|
||||
def load_yids(self, gkx: Callable[[int], cp.ndarray]):
|
||||
self.yks.clear()
|
||||
@ -55,15 +63,18 @@ class TFM:
|
||||
self.inputs.append(cp.asarray(bscan_mat_cpu))
|
||||
|
||||
def get_idx_mat(self):
|
||||
t = np.zeros((
|
||||
self.device_cfg.rows,
|
||||
self.device_cfg.cols,
|
||||
self.device_cfg.cols,
|
||||
self.device_cfg.cols
|
||||
), dtype=np.uint16)
|
||||
t = np.zeros(
|
||||
(
|
||||
self.device_cfg.rows,
|
||||
self.device_cfg.cols,
|
||||
self.device_cfg.cols,
|
||||
self.device_cfg.cols,
|
||||
),
|
||||
dtype=np.uint16,
|
||||
)
|
||||
|
||||
def send_thread(target, arr, idx):
|
||||
target[:, idx * 64:(idx + 1) * 64, :, :] = arr[i].get()
|
||||
target[:, idx * 64 : (idx + 1) * 64, :, :] = arr[i].get()
|
||||
|
||||
ts = []
|
||||
for i in range(4):
|
||||
@ -97,7 +108,9 @@ class TFM:
|
||||
self.canvas[gpu_id][:, i_start:i_end] = res
|
||||
|
||||
def send_thread(canvas_, gpu_id_):
|
||||
canvas_cpu[:, gpu_id_ * 64:(gpu_id_ + 1) * 64] = canvas_[gpu_id_].get()
|
||||
canvas_cpu[:, gpu_id_ * 64 : (gpu_id_ + 1) * 64] = canvas_[
|
||||
gpu_id_
|
||||
].get()
|
||||
|
||||
t = Thread(target=send_thread, args=(self.canvas, gpu_id))
|
||||
ts.append(t)
|
||||
@ -118,7 +131,9 @@ def test2():
|
||||
device_cfg = DeviceConfig()
|
||||
tfm = TFM()
|
||||
tfm.load_yids(dist_mat_to_yids(direct_dist()))
|
||||
r = tfm(np.ones((device_cfg.cols, device_cfg.cols, device_cfg.rows), dtype=np.int16))
|
||||
r = tfm(
|
||||
np.ones((device_cfg.cols, device_cfg.cols, device_cfg.rows), dtype=np.int16)
|
||||
)
|
||||
print(r.shape)
|
||||
|
||||
|
||||
@ -127,12 +142,13 @@ def test3():
|
||||
pwi, pwi_row_idx = gen_pwi(refraction_dist())
|
||||
pwi(cp.ones((device_cfg.cols, device_cfg.rows), dtype=np.int16))
|
||||
|
||||
|
||||
def test4():
|
||||
device_cfg = DeviceConfig()
|
||||
pwi, pwi_row_idx = gen_pwi(refraction_dist())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
# test1()
|
||||
# test2()
|
||||
# test3()
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
dist function return mat f(y,x),
|
||||
where f(y,x) is the pixel distance in echo pulse RF signal between point(0,0) and point(x,y).
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import fsolve
|
||||
|
||||
@ -20,7 +21,7 @@ def refraction_dist(dev_cfg=DeviceConfig()):
|
||||
SECONDS_PER_Y_PIX = dev_cfg.second_per_y_pix
|
||||
METER_PER_X_PIX = dev_cfg.meter_per_x_pix
|
||||
|
||||
save_path = DS / f'{y1}_{v1x}_{v2}_fslove.npy'
|
||||
save_path = DS / f"{y1}_{v1x}_{v2}_fslove.npy"
|
||||
if save_path.exists():
|
||||
return np.load(str(save_path))
|
||||
|
||||
@ -28,13 +29,15 @@ def refraction_dist(dev_cfg=DeviceConfig()):
|
||||
def f(arg):
|
||||
θ1 = arg[0]
|
||||
return [
|
||||
y1 * np.tan(θ1) + v2 * SECONDS_PER_Y_PIX * y2 * np.tan(
|
||||
np.arcsin(v2 / v1x * np.sin(θ1))) - METER_PER_X_PIX * x]
|
||||
y1 * np.tan(θ1)
|
||||
+ v2 * SECONDS_PER_Y_PIX * y2 * np.tan(np.arcsin(v2 / v1x * np.sin(θ1)))
|
||||
- METER_PER_X_PIX * x
|
||||
]
|
||||
|
||||
return f
|
||||
|
||||
iter_start = np.arcsin(v1x / v2 * np.sin(np.deg2rad(90)))
|
||||
iter_start = iter_start // 1e-7 / 1e+7
|
||||
iter_start = iter_start // 1e-7 / 1e7
|
||||
m = np.zeros((dev_cfg.rows, dev_cfg.cols - 1))
|
||||
for y2_ in range(m.shape[0]):
|
||||
for x_ in range(m.shape[1]):
|
||||
@ -47,7 +50,7 @@ def refraction_dist(dev_cfg=DeviceConfig()):
|
||||
x2_mat = y2_mat * np.tan(np.arcsin((v2 / v1x) * np.sin(m)))
|
||||
d1 = np.hypot(x1_mat, y1)
|
||||
d2 = np.hypot(x2_mat, y2_mat)
|
||||
dmat[:, 1:] = (d1 / v1 + d2 / v2)
|
||||
dmat[:, 1:] = d1 / v1 + d2 / v2
|
||||
dmat[:, 0] = y1 / v1 + t1
|
||||
dmat /= SECONDS_PER_Y_PIX
|
||||
np.save(save_path.__str__(), dmat)
|
||||
@ -55,7 +58,7 @@ def refraction_dist(dev_cfg=DeviceConfig()):
|
||||
|
||||
|
||||
def direct_dist(dev_cfg=DeviceConfig(), p=np):
|
||||
y, x = p.ogrid[:dev_cfg.rows, :dev_cfg.cols]
|
||||
y, x = p.ogrid[: dev_cfg.rows, : dev_cfg.cols]
|
||||
y1_meter = dev_cfg.y1
|
||||
y2_meter = y * dev_cfg.second_per_y_pix * dev_cfg.v2
|
||||
y_meter = y1_meter + y2_meter
|
||||
@ -67,6 +70,6 @@ def direct_dist(dev_cfg=DeviceConfig(), p=np):
|
||||
return m / dev_cfg.second_per_y_pix
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
print(refraction_dist())
|
||||
print(direct_dist())
|
||||
|
||||
@ -6,9 +6,10 @@ xfs: x fire shape
|
||||
xri: x receive index
|
||||
xrs: x receive shape
|
||||
"""
|
||||
import cupy as cp
|
||||
from flandre.beamformer.dist import direct_dist
|
||||
|
||||
import cupy as cp
|
||||
|
||||
from flandre.beamformer.dist import direct_dist
|
||||
from flandre.utils.Config import DeviceConfig
|
||||
|
||||
|
||||
@ -18,7 +19,8 @@ def dist_mat_to_yids(dist_mat: cp.ndarray, dev_cfg=DeviceConfig()):
|
||||
xfs = 64
|
||||
xss = dev_cfg.cols
|
||||
xrs = dev_cfg.cols
|
||||
kernel = cp.RawKernel(f"""
|
||||
kernel = cp.RawKernel(
|
||||
f"""
|
||||
extern "C" __global__ void k(unsigned short int *result, float * dist_mat, int offset)
|
||||
{{
|
||||
const long long cols = {dev_cfg.cols};
|
||||
@ -39,13 +41,15 @@ def dist_mat_to_yids(dist_mat: cp.ndarray, dev_cfg=DeviceConfig()):
|
||||
float r = dist_mat[yfi*cols+abs(xsi-(xfi + offset))] + dist_mat[yfi*cols+abs(xri-(xfi + offset))];
|
||||
result[idx] = min(__float2int_rd(r), (int)yfs);
|
||||
}}
|
||||
""", 'k')
|
||||
""",
|
||||
"k",
|
||||
)
|
||||
result = cp.zeros(yfs * xfs * xss * xrs, dtype=cp.uint16)
|
||||
kernel((yfs, xfs, xss), (xrs, 1, 1), (
|
||||
result,
|
||||
cp.asarray(dist_mat.astype(cp.float32)),
|
||||
cp.int32(offset)
|
||||
))
|
||||
kernel(
|
||||
(yfs, xfs, xss),
|
||||
(xrs, 1, 1),
|
||||
(result, cp.asarray(dist_mat.astype(cp.float32)), cp.int32(offset)),
|
||||
)
|
||||
result = result.reshape((yfs, xfs, xss, xrs))
|
||||
return result
|
||||
|
||||
@ -64,7 +68,8 @@ def dist_mat_to_yids_pwi(dist_mat: cp.ndarray, dev_cfg=DeviceConfig()):
|
||||
yfs = dev_cfg.rows
|
||||
xfs = dev_cfg.cols
|
||||
xrs = dev_cfg.cols
|
||||
kernel = cp.RawKernel(f"""
|
||||
kernel = cp.RawKernel(
|
||||
f"""
|
||||
extern "C" __global__ void k(unsigned short int *result, float * dist_mat)
|
||||
{{
|
||||
const long long cols = {dev_cfg.cols};
|
||||
@ -82,9 +87,13 @@ def dist_mat_to_yids_pwi(dist_mat: cp.ndarray, dev_cfg=DeviceConfig()):
|
||||
float r = dist_mat[yfi*cols] + dist_mat[yfi*cols+abs(xri-xfi)];
|
||||
result[idx] = min(__float2int_rd(r), (int)yfs);
|
||||
}}
|
||||
""", 'k')
|
||||
""",
|
||||
"k",
|
||||
)
|
||||
result = cp.zeros(yfs * xfs * xrs, dtype=cp.uint16)
|
||||
kernel((yfs, xfs), (xrs, 1, 1), (result, cp.asarray(dist_mat.astype(cp.float32))))
|
||||
kernel(
|
||||
(yfs, xfs), (xrs, 1, 1), (result, cp.asarray(dist_mat.astype(cp.float32)))
|
||||
)
|
||||
result = result.reshape((yfs, xfs, xrs))
|
||||
return result
|
||||
|
||||
@ -94,8 +103,8 @@ def dist_mat_to_yids_pwi(dist_mat: cp.ndarray, dev_cfg=DeviceConfig()):
|
||||
def test1():
|
||||
f = dist_mat_to_yids(direct_dist())
|
||||
r = f(0)
|
||||
print(r.flags['C_CONTIGUOUS'], r.flags['F_CONTIGUOUS'])
|
||||
print(r.flags["C_CONTIGUOUS"], r.flags["F_CONTIGUOUS"])
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
test1()
|
||||
|
||||
@ -10,50 +10,63 @@ tfm_cache = [None]
|
||||
|
||||
|
||||
def pwi_process(s: ScanData, icfg: ImagingConfig, pwi):
|
||||
return (s
|
||||
.filter_max_persent(icfg.bscan_max / 1000, bid=True)
|
||||
.sum(cond=s.d == 3)
|
||||
.dct(icfg.dct_start, icfg.dct_end)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(pwi)
|
||||
.call(cp.asarray, order='C')
|
||||
.argrelextrema()
|
||||
.conv_guass(b=icfg.beta * 0.01)
|
||||
.clip(icfg.focus_start, icfg.focus_end)
|
||||
.filter_max_persent(icfg.focus_max / 100, mmax=icfg.focus_mmax if icfg.uafm else None)
|
||||
.time_gain_compensation_linear_max(icfg.tgcl, mmax=icfg.focus_mmax if icfg.uafm else None)
|
||||
.cpu()
|
||||
.get()
|
||||
)
|
||||
return (
|
||||
s.filter_max_persent(icfg.bscan_max / 1000, bid=True)
|
||||
.sum(cond=s.d == 3)
|
||||
.dct(icfg.dct_start, icfg.dct_end)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(pwi)
|
||||
.call(cp.asarray, order="C")
|
||||
.argrelextrema()
|
||||
.conv_guass(b=icfg.beta * 0.01)
|
||||
.clip(icfg.focus_start, icfg.focus_end)
|
||||
.filter_max_persent(
|
||||
icfg.focus_max / 100, mmax=icfg.focus_mmax if icfg.uafm else None
|
||||
)
|
||||
.time_gain_compensation_linear_max(
|
||||
icfg.tgcl, mmax=icfg.focus_mmax if icfg.uafm else None
|
||||
)
|
||||
.cpu()
|
||||
.get()
|
||||
)
|
||||
|
||||
|
||||
def tfm_process(s: ScanData, icfg: ImagingConfig, disable_cache: bool, tfm: TFM):
|
||||
# print(icfg.changed_field, icfg.changed_field in ['dct_start', 'dct_end', 'bscan_max'], disable_cache)
|
||||
if icfg.changed_field in ['dct_start', 'dct_end', 'bscan_max'] or tfm_cache[0] is None or disable_cache:
|
||||
tfm_cache[0] = (s.dct(icfg.dct_start, icfg.dct_end)
|
||||
.filter_max_persent(icfg.bscan_max / 1000, bid=True)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(tfm))
|
||||
return (tfm_cache[0]
|
||||
.call(cp.asarray, order='C')
|
||||
.argrelextrema(axis=1)
|
||||
.conv_guass(b=icfg.beta * 0.01, axis=1)
|
||||
.clip(icfg.focus_start, icfg.focus_end)
|
||||
.filter_max_persent(icfg.focus_max, mmax=icfg.focus_mmax if icfg.uafm else None)
|
||||
.time_gain_compensation_linear_max(icfg.tgcl, mmax=icfg.focus_mmax if icfg.uafm else None)
|
||||
.cpu()
|
||||
.get()
|
||||
)
|
||||
if (
|
||||
icfg.changed_field in ["dct_start", "dct_end", "bscan_max"]
|
||||
or tfm_cache[0] is None
|
||||
or disable_cache
|
||||
):
|
||||
tfm_cache[0] = (
|
||||
s.dct(icfg.dct_start, icfg.dct_end)
|
||||
.filter_max_persent(icfg.bscan_max / 1000, bid=True)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(tfm)
|
||||
)
|
||||
return (
|
||||
tfm_cache[0]
|
||||
.call(cp.asarray, order="C")
|
||||
.argrelextrema(axis=1)
|
||||
.conv_guass(b=icfg.beta * 0.01, axis=1)
|
||||
.clip(icfg.focus_start, icfg.focus_end)
|
||||
.filter_max_persent(icfg.focus_max, mmax=icfg.focus_mmax if icfg.uafm else None)
|
||||
.time_gain_compensation_linear_max(
|
||||
icfg.tgcl, mmax=icfg.focus_mmax if icfg.uafm else None
|
||||
)
|
||||
.cpu()
|
||||
.get()
|
||||
)
|
||||
|
||||
|
||||
def process_pwi_v2(data: RfMat, arg: ImageArgMsg, pwi):
|
||||
return (data
|
||||
.dct_center(arg.dct_center, arg.dct_bandwidth)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(pwi)
|
||||
.call(cp.asarray, order='C')
|
||||
.argrelextrema()
|
||||
.conv_guass(b=arg.beta * 0.01)
|
||||
.crop_center(arg.t_start, arg.t_end)
|
||||
.time_gain_compensation_global((1 - arg.g8 * (1.0 / 128)) ** 2)
|
||||
)
|
||||
return (
|
||||
data.dct_center(arg.dct_center, arg.dct_bandwidth)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(pwi)
|
||||
.call(cp.asarray, order="C")
|
||||
.argrelextrema()
|
||||
.conv_guass(b=arg.beta * 0.01)
|
||||
.crop_center(arg.t_start, arg.t_end)
|
||||
.time_gain_compensation_global((1 - arg.g8 * (1.0 / 128)) ** 2)
|
||||
)
|
||||
|
||||
@ -9,16 +9,18 @@ from flandre.nodes.MainUI import MainUI
|
||||
|
||||
def kde_pyqt6_mainui(software_config):
|
||||
flandre.C.write_config()
|
||||
subprocess.run(['python', __file__],
|
||||
env=dict(XDG_CURRENT_DESKTOP="KDE",
|
||||
XDG_RUNTIME_DIR="/run/user/1000",
|
||||
XDG_SESSION_TYPE="wayland",
|
||||
PYTHONPATH=os.environ.get('PYTHONPATH', flandre.MODULE_FOLDER.parent),
|
||||
FLANDRE_CONFIG=software_config.json_text,
|
||||
)
|
||||
)
|
||||
subprocess.run(
|
||||
["python", __file__],
|
||||
env=dict(
|
||||
XDG_CURRENT_DESKTOP="KDE",
|
||||
XDG_RUNTIME_DIR="/run/user/1000",
|
||||
XDG_SESSION_TYPE="wayland",
|
||||
PYTHONPATH=os.environ.get("PYTHONPATH", flandre.MODULE_FOLDER.parent),
|
||||
FLANDRE_CONFIG=software_config.json_text,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
MainUI()(software_config=C.read_config_text(os.environ['FLANDRE_CONFIG']))
|
||||
MainUI()(software_config=C.read_config_text(os.environ["FLANDRE_CONFIG"]))
|
||||
|
||||
@ -4,10 +4,8 @@ import json
|
||||
import logging
|
||||
import multiprocessing
|
||||
import os
|
||||
import queue
|
||||
import shutil
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
import tomllib
|
||||
from enum import Enum, auto
|
||||
@ -19,16 +17,15 @@ import platformdirs
|
||||
import zmq
|
||||
from zmq import Socket
|
||||
|
||||
from flandre import C
|
||||
from flandre import P
|
||||
from flandre import C, P
|
||||
from flandre.BusClient import BusClient
|
||||
from flandre.kde_pyqt6_mainui import kde_pyqt6_mainui
|
||||
from flandre.nodes.Broker import Broker
|
||||
from flandre.nodes.Device import Device
|
||||
from flandre.nodes.Robot import Robot
|
||||
from flandre.utils.Msg import KillMsg, NodeOnlineMsg, Msg1, Msg2
|
||||
from flandre.utils.RfFrame import b2t
|
||||
from flandre.utils.mi import MiSwitch
|
||||
from flandre.utils.Msg import KillMsg, Msg1, Msg2, NodeOnlineMsg
|
||||
from flandre.utils.RfFrame import b2t
|
||||
from flandre.utils.rtsi.serialize import DataObject
|
||||
|
||||
|
||||
@ -51,13 +48,17 @@ class LaunchComponent(Enum):
|
||||
|
||||
def launch(arg: dict[LaunchComponent, dict]):
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
multiprocessing.set_start_method('spawn')
|
||||
bp = multiprocessing.Process(target=Broker(
|
||||
broker=True), kwargs=dict(software_config=C))
|
||||
multiprocessing.set_start_method("spawn")
|
||||
bp = multiprocessing.Process(
|
||||
target=Broker(broker=True), kwargs=dict(software_config=C)
|
||||
)
|
||||
bp.start()
|
||||
ps = []
|
||||
for k, v in arg.items():
|
||||
if k == LaunchComponent.MainUI and os.environ.get('XDG_CURRENT_DESKTOP', None) == 'KDE':
|
||||
if (
|
||||
k == LaunchComponent.MainUI
|
||||
and os.environ.get("XDG_CURRENT_DESKTOP", None) == "KDE"
|
||||
):
|
||||
ps.append(kde_pyqt6_mainui)
|
||||
continue
|
||||
m = importlib.import_module(f"flandre.nodes.{k.name}")
|
||||
@ -66,8 +67,7 @@ def launch(arg: dict[LaunchComponent, dict]):
|
||||
|
||||
pps = []
|
||||
for p in ps:
|
||||
pps.append(multiprocessing.Process(
|
||||
target=p, kwargs=dict(software_config=C)))
|
||||
pps.append(multiprocessing.Process(target=p, kwargs=dict(software_config=C)))
|
||||
for p in pps:
|
||||
p.start()
|
||||
|
||||
@ -84,7 +84,7 @@ def launch(arg: dict[LaunchComponent, dict]):
|
||||
if msg is None:
|
||||
continue
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
break
|
||||
if not ready:
|
||||
if isinstance(msg, NodeOnlineMsg):
|
||||
@ -92,7 +92,7 @@ def launch(arg: dict[LaunchComponent, dict]):
|
||||
logging.info(msg)
|
||||
dd[msg.name] = 1
|
||||
if dd.keys().__len__() == len(ps):
|
||||
logging.info(f'launcher stand by ready')
|
||||
logging.info("launcher stand by ready")
|
||||
c.send(Msg1())
|
||||
ready = True
|
||||
for p in pps:
|
||||
@ -110,39 +110,40 @@ def launch_from_file(file: Path):
|
||||
|
||||
|
||||
@click.group()
|
||||
@click.option('--dev/--no-dev', default=True)
|
||||
@click.option("--dev/--no-dev", default=True)
|
||||
def entrypoint(dev):
|
||||
if dev:
|
||||
C.config_folder = P.DEV_PROJECT_FOLDER / 'config'
|
||||
C.log_folder = P.DEV_PROJECT_FOLDER / 'log'
|
||||
C.config_folder = P.DEV_PROJECT_FOLDER / "config"
|
||||
C.log_folder = P.DEV_PROJECT_FOLDER / "log"
|
||||
else:
|
||||
C.read_config(C.software_config_file)
|
||||
|
||||
|
||||
@entrypoint.command()
|
||||
@click.option('--data_folder', default=None)
|
||||
@click.option('--generate_pyqt', default=False)
|
||||
@click.option('-p', '--path', type=str,
|
||||
default=platformdirs.user_config_path(
|
||||
'Flandre', 'Scarlet') / 'launch.toml',
|
||||
help='Path to launch.toml'
|
||||
)
|
||||
@click.option("--data_folder", default=None)
|
||||
@click.option("--generate_pyqt", default=False)
|
||||
@click.option(
|
||||
"-p",
|
||||
"--path",
|
||||
type=str,
|
||||
default=platformdirs.user_config_path("Flandre", "Scarlet") / "launch.toml",
|
||||
help="Path to launch.toml",
|
||||
)
|
||||
def gui(data_folder, generate_pyqt, path):
|
||||
if (pyuic6 := shutil.which('pyuic6')) is None:
|
||||
print('pyuic6 is not installed')
|
||||
if (pyuic6 := shutil.which("pyuic6")) is None:
|
||||
print("pyuic6 is not installed")
|
||||
return
|
||||
if generate_pyqt:
|
||||
subprocess.run([pyuic6, '-o', P.PYQT / 'Main.py', P.PYQT / 'Main.ui'])
|
||||
subprocess.run(
|
||||
[pyuic6, '-o', P.PYQT / 'Image.py', P.PYQT / 'Image.ui'])
|
||||
subprocess.run([pyuic6, "-o", P.PYQT / "Main.py", P.PYQT / "Main.ui"])
|
||||
subprocess.run([pyuic6, "-o", P.PYQT / "Image.py", P.PYQT / "Image.ui"])
|
||||
if data_folder is not None:
|
||||
C.record_folder = Path(data_folder)
|
||||
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text('[MainUI]\n')
|
||||
print('Use launch config', path)
|
||||
path.write_text("[MainUI]\n")
|
||||
print("Use launch config", path)
|
||||
launch_from_file(path)
|
||||
|
||||
|
||||
@ -158,30 +159,30 @@ def mi():
|
||||
|
||||
|
||||
@mi.command()
|
||||
@click.argument('name')
|
||||
@click.argument("name")
|
||||
def on(name):
|
||||
if name == 'c1':
|
||||
if name == "c1":
|
||||
mi1.on()
|
||||
if name == 'c2':
|
||||
if name == "c2":
|
||||
mi2.on()
|
||||
|
||||
|
||||
@mi.command()
|
||||
@click.argument('name')
|
||||
@click.argument("name")
|
||||
def off(name):
|
||||
if name == 'c1':
|
||||
if name == "c1":
|
||||
mi1.off()
|
||||
if name == 'c2':
|
||||
if name == "c2":
|
||||
mi2.off()
|
||||
|
||||
|
||||
@mi.command()
|
||||
@click.argument('name')
|
||||
@click.argument("name")
|
||||
def status(name):
|
||||
if name == 'c1':
|
||||
if name == "c1":
|
||||
print(mi1.is_on())
|
||||
print(mi1.power())
|
||||
if name == 'c2':
|
||||
if name == "c2":
|
||||
print(mi2.is_on())
|
||||
|
||||
|
||||
@ -191,7 +192,7 @@ dd: Device = None
|
||||
|
||||
|
||||
@entrypoint.group()
|
||||
@click.option('--ip', default=None)
|
||||
@click.option("--ip", default=None)
|
||||
def device(ip):
|
||||
if ip is not None:
|
||||
C.live_ip = ip
|
||||
@ -204,52 +205,52 @@ def device(ip):
|
||||
dd.setup()
|
||||
|
||||
|
||||
@device.command('connect')
|
||||
@device.command("connect")
|
||||
def device_connect():
|
||||
dd.connect()
|
||||
|
||||
|
||||
@device.command('disconnect')
|
||||
@device.command("disconnect")
|
||||
def device_disconnect():
|
||||
dd.disconnect()
|
||||
|
||||
|
||||
@device.command('enable')
|
||||
@device.command("enable")
|
||||
def device_enable():
|
||||
dd.enable()
|
||||
|
||||
|
||||
@device.command('disable')
|
||||
@device.command("disable")
|
||||
def device_disable():
|
||||
dd.disable()
|
||||
|
||||
|
||||
@device.command('start_capture')
|
||||
@device.command("start_capture")
|
||||
def device_start_capture():
|
||||
dd.start_capture()
|
||||
|
||||
|
||||
@device.command('end_capture')
|
||||
@device.command("end_capture")
|
||||
def device_end_capture():
|
||||
dd.end_capture()
|
||||
|
||||
|
||||
@device.command('upload')
|
||||
@click.argument('name')
|
||||
@click.argument('file', type=click.File('r'))
|
||||
@device.command("upload")
|
||||
@click.argument("name")
|
||||
@click.argument("file", type=click.File("r"))
|
||||
def device_upload(name, file: TextIOWrapper):
|
||||
dd.set_name_and_file_only(name, file.read())
|
||||
|
||||
|
||||
@device.command('recv_single')
|
||||
@device.command("recv_single")
|
||||
def device_recvmonitor():
|
||||
r = dd.get_data()
|
||||
seq, encoder, host_ts, device_ts_low, device_ts_high, buffer = b2t(r)
|
||||
print(seq, encoder, host_ts, device_ts_low, device_ts_high)
|
||||
print((buffer.__len__()//2)/256)
|
||||
print((buffer.__len__() // 2) / 256)
|
||||
|
||||
|
||||
@device.command('monitor')
|
||||
@device.command("monitor")
|
||||
def device_recvmonitor():
|
||||
ctx = zmq.Context()
|
||||
pull = ctx.socket(zmq.PULL)
|
||||
@ -257,39 +258,38 @@ def device_recvmonitor():
|
||||
while True:
|
||||
b = pull.recv()
|
||||
seq, encoder, host_ts, device_ts, buffer = b2t(b)
|
||||
print(f'S={seq} E={encoder} HT={host_ts} DT={device_ts} BS={buffer.__len__()}')
|
||||
print(f"S={seq} E={encoder} HT={host_ts} DT={device_ts} BS={buffer.__len__()}")
|
||||
|
||||
|
||||
def b2b(b):
|
||||
return hashlib.blake2b(b, digest_size=4).hexdigest()
|
||||
|
||||
|
||||
@device.command('recvzero')
|
||||
@device.command("recvzero")
|
||||
def device_recvzero():
|
||||
ctx = zmq.Context()
|
||||
pull = ctx.socket(zmq.PULL)
|
||||
pull.connect(C.live_push_socket)
|
||||
last_ts = 0
|
||||
last_sequence_id = 0
|
||||
last_b = b''
|
||||
last_b = b""
|
||||
first = True
|
||||
while True:
|
||||
b = pull.recv()
|
||||
ts, sequence_id, encoder, buffer = b2t(b)
|
||||
if ts == last_ts:
|
||||
print('tszero', sequence_id, encoder,
|
||||
(ts - last_ts) / 10 ** 6, flush=True)
|
||||
print("tszero", sequence_id, encoder, (ts - last_ts) / 10**6, flush=True)
|
||||
print(b2b(b), b2b(last_b))
|
||||
if sequence_id == last_sequence_id:
|
||||
print('szero', sequence_id, encoder, flush=True)
|
||||
print("szero", sequence_id, encoder, flush=True)
|
||||
print(b2b(b), b2b(last_b))
|
||||
last_ts = ts
|
||||
last_sequence_id = sequence_id
|
||||
last_b = b
|
||||
|
||||
|
||||
@device.command('record')
|
||||
@click.argument('folder', type=click.Path())
|
||||
@device.command("record")
|
||||
@click.argument("folder", type=click.Path())
|
||||
def device_record(folder):
|
||||
p = Path(folder)
|
||||
p.mkdir(parents=True, exist_ok=True)
|
||||
@ -302,7 +302,7 @@ def device_record(folder):
|
||||
b = pull.recv()
|
||||
seq, encoder, host_ts, device_ts, buffer = b2t(b)
|
||||
if cnt % 5 == 0:
|
||||
(p / f'{host_ts}.bin').write_bytes(b)
|
||||
(p / f"{host_ts}.bin").write_bytes(b)
|
||||
|
||||
|
||||
robot: Robot = None
|
||||
@ -314,11 +314,12 @@ def robot():
|
||||
robot = Robot()
|
||||
|
||||
|
||||
@robot.command('monitor')
|
||||
@robot.command("monitor")
|
||||
def robot_monitor():
|
||||
robot.setup()
|
||||
output1 = robot.rt.output_subscribe(
|
||||
'actual_TCP_pose,actual_TCP_force,timestamp', 250) # 输出订阅,配方1
|
||||
"actual_TCP_pose,actual_TCP_force,timestamp", 250
|
||||
) # 输出订阅,配方1
|
||||
robot.rt.start() # rtsi 开始
|
||||
while True:
|
||||
recv_out: DataObject = robot.rt.get_output_data()
|
||||
@ -327,18 +328,19 @@ def robot_monitor():
|
||||
if recv_out.recipe_id == output1.id:
|
||||
x, y, z, r, p, yy = recv_out.actual_TCP_pose
|
||||
fx, fy, fz, fr, fp, fyy = recv_out.actual_TCP_force
|
||||
print(f'X={x}, DT={recv_out.timestamp}')
|
||||
print(f"X={x}, DT={recv_out.timestamp}")
|
||||
|
||||
|
||||
@robot.command('record')
|
||||
@click.argument('folder', type=click.Path())
|
||||
@robot.command("record")
|
||||
@click.argument("folder", type=click.Path())
|
||||
def robot_record(folder):
|
||||
p = Path(folder)
|
||||
p.mkdir(parents=True, exist_ok=True)
|
||||
robot.setup()
|
||||
|
||||
output1 = robot.rt.output_subscribe(
|
||||
'actual_TCP_pose,actual_TCP_force,timestamp', 250) # 输出订阅,配方1
|
||||
"actual_TCP_pose,actual_TCP_force,timestamp", 250
|
||||
) # 输出订阅,配方1
|
||||
robot.rt.start() # rtsi 开始
|
||||
arr = []
|
||||
last_device_ts = 0
|
||||
@ -374,8 +376,9 @@ def robot_record(folder):
|
||||
ns = time.time_ns()
|
||||
# print(q.get()[0]['ns'])
|
||||
# print('write')
|
||||
(p / f'{ns}.json').write_text(json.dumps(arr))
|
||||
(p / f"{ns}.json").write_text(json.dumps(arr))
|
||||
arr = []
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if __name__ == "__main__":
|
||||
entrypoint()
|
||||
|
||||
@ -5,13 +5,20 @@ import traceback
|
||||
import cupy as cp
|
||||
import zmq
|
||||
|
||||
from flandre.beamformer.das import gen_pwi, TFM
|
||||
from flandre import C
|
||||
from flandre.beamformer.das import TFM, gen_pwi
|
||||
from flandre.beamformer.dist import direct_dist
|
||||
from flandre.beamformer.kernels import dist_mat_to_yids
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Config import DeviceConfig
|
||||
from flandre.utils.Msg import ImageArgMsg, Msg, BeamformerMsg, RfMatMsg, RfFrameMsg, MaxMsg
|
||||
from flandre.utils.Msg import (
|
||||
BeamformerMsg,
|
||||
ImageArgMsg,
|
||||
MaxMsg,
|
||||
Msg,
|
||||
RfFrameMsg,
|
||||
RfMatMsg,
|
||||
)
|
||||
from flandre.utils.RfFrame import RfFrameFile
|
||||
from flandre.utils.RfMat import RfMat
|
||||
from flandre.utils.RfMeta import RfSequenceMeta
|
||||
@ -35,51 +42,52 @@ class Beamformer(Node):
|
||||
def process_pwi(self, data: RfMat, arg: ImageArgMsg, pwi):
|
||||
if data is None:
|
||||
return
|
||||
d2 = (data
|
||||
# .dct(80, 1500)
|
||||
.dct_center(arg.dct_center, arg.dct_bandwidth)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(pwi)
|
||||
.call(cp.asarray, order='C')
|
||||
.argrelextrema()
|
||||
.conv_guass(b=arg.beta * 0.01)
|
||||
.crop_center(arg.t_start, arg.t_end)
|
||||
# .time_gain_compensation([
|
||||
# ( (1 - arg.g1 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g2 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g3 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g4 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g5 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g6 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g7 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g8 * (1.0 / 128))** 2),
|
||||
# ])
|
||||
.time_gain_compensation_global((1 - arg.g8 * (1.0 / 128)) ** 2)
|
||||
.rotate90()
|
||||
.grey()
|
||||
.cpu()
|
||||
# .call(cv2.cvtColor, cv2.COLOR_GRAY2RGB)
|
||||
# .pseudo_color()
|
||||
)
|
||||
d2 = (
|
||||
data
|
||||
# .dct(80, 1500)
|
||||
.dct_center(arg.dct_center, arg.dct_bandwidth)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(pwi)
|
||||
.call(cp.asarray, order="C")
|
||||
.argrelextrema()
|
||||
.conv_guass(b=arg.beta * 0.01)
|
||||
.crop_center(arg.t_start, arg.t_end)
|
||||
# .time_gain_compensation([
|
||||
# ( (1 - arg.g1 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g2 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g3 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g4 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g5 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g6 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g7 * (1.0 / 128))** 2),
|
||||
# ( (1 - arg.g8 * (1.0 / 128))** 2),
|
||||
# ])
|
||||
.time_gain_compensation_global((1 - arg.g8 * (1.0 / 128)) ** 2)
|
||||
.rotate90()
|
||||
.grey()
|
||||
.cpu()
|
||||
# .call(cv2.cvtColor, cv2.COLOR_GRAY2RGB)
|
||||
# .pseudo_color()
|
||||
)
|
||||
# time.sleep(0.01)
|
||||
self.send(RfMatMsg(d2))
|
||||
# self.send(RGB888Msg(d2.__bytes__(), d2.w, d2.h))
|
||||
|
||||
def process_tfm(self, data: RfMat, arg: ImageArgMsg, tfm):
|
||||
d2 = (data
|
||||
.dct_center(arg.dct_center, arg.dct_bandwidth)
|
||||
# .filter_max_persent(icfg.bscan_max / 1000, bid=True)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(tfm)
|
||||
.call(cp.asarray, order='C')
|
||||
.argrelextrema(axis=1)
|
||||
.conv_guass(b=arg.beta * 0.01)
|
||||
.crop_center(arg.t_start, arg.t_end)
|
||||
.time_gain_compensation_global((1 - arg.g8 * (1.0 / 128)) ** 2)
|
||||
.rotate90()
|
||||
.grey()
|
||||
.cpu()
|
||||
)
|
||||
d2 = (
|
||||
data.dct_center(arg.dct_center, arg.dct_bandwidth)
|
||||
# .filter_max_persent(icfg.bscan_max / 1000, bid=True)
|
||||
.call(lambda m: m.astype(cp.int16))
|
||||
.call(tfm)
|
||||
.call(cp.asarray, order="C")
|
||||
.argrelextrema(axis=1)
|
||||
.conv_guass(b=arg.beta * 0.01)
|
||||
.crop_center(arg.t_start, arg.t_end)
|
||||
.time_gain_compensation_global((1 - arg.g8 * (1.0 / 128)) ** 2)
|
||||
.rotate90()
|
||||
.grey()
|
||||
.cpu()
|
||||
)
|
||||
self.send(RfMatMsg(d2))
|
||||
|
||||
def loop(self):
|
||||
@ -90,16 +98,16 @@ class Beamformer(Node):
|
||||
last_f_rows = 0
|
||||
last_blake2b = None
|
||||
while True:
|
||||
self.muxer_req_socket.send(b'')
|
||||
self.muxer_req_socket.send(b"")
|
||||
r = dict(self.c.poller.poll())
|
||||
if self.c.sub in r:
|
||||
msg = self.recv()
|
||||
if self.muxer_req_socket in r:
|
||||
msg: BeamformerMsg = Msg.decode_msg(self.muxer_req_socket.recv())
|
||||
if msg.value == b'init':
|
||||
if msg.value == b"init":
|
||||
time.sleep(1)
|
||||
continue
|
||||
if msg.value == b'nop':
|
||||
if msg.value == b"nop":
|
||||
continue
|
||||
r = msg.value
|
||||
id2 = r.index(Msg.magic(), 1)
|
||||
@ -107,14 +115,18 @@ class Beamformer(Node):
|
||||
rf_frame_msg: RfFrameMsg = Msg.decode_msg(r[id2:])
|
||||
current_frame = rf_frame_msg.rf_frame
|
||||
|
||||
|
||||
if isinstance(current_frame, RfFrameFile):
|
||||
if current_frame.meta.blake2b is not None and current_frame.meta.blake2b == last_blake2b:
|
||||
if (
|
||||
current_frame.meta.blake2b is not None
|
||||
and current_frame.meta.blake2b == last_blake2b
|
||||
):
|
||||
continue
|
||||
mat = RfMat.from_rf_frame(rf_frame_msg.rf_frame, 'gpu')
|
||||
mat = RfMat.from_rf_frame(rf_frame_msg.rf_frame, "gpu")
|
||||
# logger.info(mat.frame_meta.blake2b)
|
||||
if mat is None:
|
||||
logger.warning(f"{rf_frame_msg.rf_frame.seq_meta.prod()} , {rf_frame_msg.rf_frame.__bytes__().__len__() // 2}")
|
||||
logger.warning(
|
||||
f"{rf_frame_msg.rf_frame.seq_meta.prod()} , {rf_frame_msg.rf_frame.__bytes__().__len__() // 2}"
|
||||
)
|
||||
continue
|
||||
|
||||
last_blake2b = mat.frame_meta.blake2b
|
||||
|
||||
@ -15,7 +15,7 @@ class Broker(Node):
|
||||
if isinstance(msg, NodeOnlineMsg):
|
||||
pass
|
||||
elif isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.context.term()
|
||||
break
|
||||
|
||||
@ -26,5 +26,5 @@ class Broker(Node):
|
||||
backend.bind(f"tcp://*:{self.bp}")
|
||||
try:
|
||||
zmq.proxy(frontend, backend)
|
||||
except ContextTerminated as e:
|
||||
except ContextTerminated:
|
||||
return
|
||||
|
||||
@ -9,9 +9,22 @@ import zmq
|
||||
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import ImageArgMsg, KillMsg, SetDeviceConnectedMsg, SetDeviceEnabledMsg, DeviceEnabledMsg, \
|
||||
DeviceConnectedMsg, SetDeviceConfigMsg, DeviceOnlineMsg, DeviceConfigListMsg, RequestRfFrameMsg, \
|
||||
DeviceZero, DeviceSwitchMsg, SeqMetaMsg, RfFrameMsg
|
||||
from flandre.utils.Msg import (
|
||||
DeviceConfigListMsg,
|
||||
DeviceConnectedMsg,
|
||||
DeviceEnabledMsg,
|
||||
DeviceOnlineMsg,
|
||||
DeviceSwitchMsg,
|
||||
DeviceZero,
|
||||
ImageArgMsg,
|
||||
KillMsg,
|
||||
RequestRfFrameMsg,
|
||||
RfFrameMsg,
|
||||
SeqMetaMsg,
|
||||
SetDeviceConfigMsg,
|
||||
SetDeviceConnectedMsg,
|
||||
SetDeviceEnabledMsg,
|
||||
)
|
||||
from flandre.utils.RfFrame import RfFrameMemory
|
||||
from flandre.utils.RfMeta import RfFrameMeta, RfSequenceMeta
|
||||
|
||||
@ -40,16 +53,21 @@ class DeviceCmd(Enum):
|
||||
|
||||
class Device(Node):
|
||||
magic = 7355608
|
||||
topics = [SetDeviceConnectedMsg, SetDeviceEnabledMsg, SetDeviceConfigMsg, RequestRfFrameMsg, DeviceZero,
|
||||
DeviceSwitchMsg
|
||||
]
|
||||
topics = [
|
||||
SetDeviceConnectedMsg,
|
||||
SetDeviceEnabledMsg,
|
||||
SetDeviceConfigMsg,
|
||||
RequestRfFrameMsg,
|
||||
DeviceZero,
|
||||
DeviceSwitchMsg,
|
||||
]
|
||||
|
||||
def __init__(self, level=logging.INFO):
|
||||
super(Device, self).__init__(level=level, req=C.live_rep_socket)
|
||||
self.arg = ImageArgMsg('', t_start=0, t_end=1499)
|
||||
self.arg = ImageArgMsg("", t_start=0, t_end=1499)
|
||||
self.seq_meta: RfSequenceMeta | None = None
|
||||
self.rep_socket: zmq.Socket = None
|
||||
self.ok = b'ok\x00'
|
||||
self.ok = b"ok\x00"
|
||||
self.loop2_t = None
|
||||
self.switch = False
|
||||
|
||||
@ -59,15 +77,15 @@ class Device(Node):
|
||||
time.sleep(1)
|
||||
|
||||
@classmethod
|
||||
def generate_cmd_bytes(cls, cmd: DeviceCmd, v: bytes = b''):
|
||||
return struct.pack('i', cls.magic) + struct.pack('i', cmd.value) + v
|
||||
def generate_cmd_bytes(cls, cmd: DeviceCmd, v: bytes = b""):
|
||||
return struct.pack("i", cls.magic) + struct.pack("i", cmd.value) + v
|
||||
|
||||
def device_cmd(self, cmd: DeviceCmd, v: bytes = b''):
|
||||
def device_cmd(self, cmd: DeviceCmd, v: bytes = b""):
|
||||
return self.c.req_interrupt(
|
||||
self.generate_cmd_bytes(cmd, v),
|
||||
interrupt_name='device123',
|
||||
interrupt_name="device123",
|
||||
retry_times=6,
|
||||
cb_retry=lambda: logger.warning(f'retry {cmd}'),
|
||||
cb_retry=lambda: logger.warning(f"retry {cmd}"),
|
||||
)
|
||||
|
||||
def connect(self):
|
||||
@ -81,7 +99,7 @@ class Device(Node):
|
||||
self.send(DeviceConnectedMsg(True))
|
||||
else:
|
||||
if r is None:
|
||||
logger.error('interrupt')
|
||||
logger.error("interrupt")
|
||||
else:
|
||||
logger.error(f"Device msg: {r}")
|
||||
self.send(DeviceConnectedMsg(False))
|
||||
@ -112,9 +130,12 @@ class Device(Node):
|
||||
return False
|
||||
|
||||
def online(self):
|
||||
code = subprocess.run(['curl', '-m', '1', C.live_rep_socket_http], stderr=subprocess.DEVNULL,
|
||||
stdout=subprocess.DEVNULL).returncode
|
||||
logger.debug(f'detect curl code: {code}')
|
||||
code = subprocess.run(
|
||||
["curl", "-m", "1", C.live_rep_socket_http],
|
||||
stderr=subprocess.DEVNULL,
|
||||
stdout=subprocess.DEVNULL,
|
||||
).returncode
|
||||
logger.debug(f"detect curl code: {code}")
|
||||
match code:
|
||||
case 28 | 7:
|
||||
self.send(DeviceOnlineMsg(False))
|
||||
@ -126,10 +147,10 @@ class Device(Node):
|
||||
def get_enable(self):
|
||||
rb = self.device_cmd(DeviceCmd.GetEnable)
|
||||
match rb:
|
||||
case b'true':
|
||||
case b"true":
|
||||
self.send(DeviceEnabledMsg(True))
|
||||
return True
|
||||
case b'false':
|
||||
case b"false":
|
||||
self.send(DeviceEnabledMsg(False))
|
||||
return False
|
||||
case _:
|
||||
@ -137,18 +158,18 @@ class Device(Node):
|
||||
|
||||
def get_seq_meta_name(self):
|
||||
rb = self.device_cmd(DeviceCmd.GetName)
|
||||
if rb != b'' and rb is not None:
|
||||
if rb != b"" and rb is not None:
|
||||
name = rb.decode()
|
||||
self.seq_meta = RfSequenceMeta.from_name(name)
|
||||
self.send(SeqMetaMsg('live', name))
|
||||
self.send(SeqMetaMsg("live", name))
|
||||
|
||||
def get_connection(self):
|
||||
rb = self.device_cmd(DeviceCmd.GetConnection)
|
||||
match rb:
|
||||
case b'true':
|
||||
case b"true":
|
||||
self.send(DeviceConnectedMsg(True))
|
||||
return True
|
||||
case b'false':
|
||||
case b"false":
|
||||
self.send(DeviceConnectedMsg(False))
|
||||
return False
|
||||
case _:
|
||||
@ -162,12 +183,14 @@ class Device(Node):
|
||||
elif self.get_connection():
|
||||
self.set_name_and_file_only(name, config_str)
|
||||
else:
|
||||
logger.warning(f"Device not connect, cannot set config")
|
||||
logger.warning("Device not connect, cannot set config")
|
||||
|
||||
def set_name_and_file_only(self, name: str, txt: str):
|
||||
name_encoded = name.encode()
|
||||
rb = self.device_cmd(DeviceCmd.SetNameAndFileOnly,
|
||||
struct.pack('I', name_encoded.__len__()) + name.encode() + txt.encode())
|
||||
rb = self.device_cmd(
|
||||
DeviceCmd.SetNameAndFileOnly,
|
||||
struct.pack("I", name_encoded.__len__()) + name.encode() + txt.encode(),
|
||||
)
|
||||
if rb == self.ok:
|
||||
return True
|
||||
else:
|
||||
@ -200,18 +223,18 @@ class Device(Node):
|
||||
self.get_connection()
|
||||
self.get_enable()
|
||||
self.get_seq_meta_name()
|
||||
for f in C.device_config_folder.glob('*.txt'):
|
||||
for f in C.device_config_folder.glob("*.txt"):
|
||||
arr.append((f.stem, f.read_text()))
|
||||
self.send(DeviceConfigListMsg(arr))
|
||||
# if arr.__len__() > 0:
|
||||
# self.setfile(arr[0][1])
|
||||
logger.debug(f'device start loop')
|
||||
logger.debug("device start loop")
|
||||
while True:
|
||||
d = dict(self.c.poller.poll())
|
||||
if self.c.sub in d:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
return
|
||||
elif isinstance(msg, SetDeviceEnabledMsg):
|
||||
if msg.value:
|
||||
@ -219,7 +242,7 @@ class Device(Node):
|
||||
else:
|
||||
self.disable()
|
||||
elif isinstance(msg, SetDeviceConnectedMsg):
|
||||
logger.info('connecting to device')
|
||||
logger.info("connecting to device")
|
||||
if msg.value:
|
||||
self.connect()
|
||||
else:
|
||||
@ -227,30 +250,37 @@ class Device(Node):
|
||||
elif isinstance(msg, SetDeviceConfigMsg):
|
||||
self.set_name_and_file(msg.name, msg.txt)
|
||||
self.seq_meta = RfSequenceMeta.from_name(msg.name)
|
||||
self.send(SeqMetaMsg('live', msg.name))
|
||||
self.send(SeqMetaMsg("live", msg.name))
|
||||
elif isinstance(msg, RequestRfFrameMsg):
|
||||
raise NotImplementedError()
|
||||
if self.switch:
|
||||
braw = self.get_data()
|
||||
if braw == b'':
|
||||
logger.warning('empty msg!')
|
||||
if braw == b"":
|
||||
logger.warning("empty msg!")
|
||||
continue
|
||||
_, sequence_id, encoder = struct.unpack_from('=iqi', braw)
|
||||
buffer = braw[4 + 8 + 4:]
|
||||
_, sequence_id, encoder = struct.unpack_from("=iqi", braw)
|
||||
buffer = braw[4 + 8 + 4 :]
|
||||
if self.seq_meta is not None:
|
||||
self.send(RfFrameMsg(0, RfFrameMemory(
|
||||
RfFrameMeta(encoder=encoder, sequence_id=sequence_id),
|
||||
self.seq_meta,
|
||||
buffer,
|
||||
)))
|
||||
self.send(
|
||||
RfFrameMsg(
|
||||
0,
|
||||
RfFrameMemory(
|
||||
RfFrameMeta(
|
||||
encoder=encoder, sequence_id=sequence_id
|
||||
),
|
||||
self.seq_meta,
|
||||
buffer,
|
||||
),
|
||||
)
|
||||
)
|
||||
else:
|
||||
logger.warning(f'no seq meta msg')
|
||||
logger.warning("no seq meta msg")
|
||||
else:
|
||||
logger.warning('device not online')
|
||||
logger.warning("device not online")
|
||||
elif isinstance(msg, DeviceZero):
|
||||
self.set_zero()
|
||||
elif isinstance(msg, DeviceSwitchMsg):
|
||||
self.switch = msg.value == 'GREEN'
|
||||
self.switch = msg.value == "GREEN"
|
||||
# elif isinstance(msg, SetDeviceSwitchMsg):
|
||||
# logger.info(f'{msg.value}')
|
||||
# if msg.value:
|
||||
|
||||
@ -6,7 +6,7 @@ import zmq
|
||||
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import BMMsg, SetWindowVisibleMsg, RfMatMsg
|
||||
from flandre.utils.Msg import BMMsg, RfMatMsg, SetWindowVisibleMsg
|
||||
from flandre.utils.RfMat import RfMat
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
@ -21,7 +21,7 @@ class ImageCV(Node):
|
||||
self.buffer = np.zeros((C.video_height, C.video_width, 3), dtype=np.uint8) + 128
|
||||
|
||||
def loop(self):
|
||||
cv2.namedWindow('image', cv2.WINDOW_NORMAL)
|
||||
cv2.namedWindow("image", cv2.WINDOW_NORMAL)
|
||||
while True:
|
||||
socks = dict(self.c.poller.poll(0.001))
|
||||
if self.c.sub in socks and socks[self.c.sub] == zmq.POLLIN:
|
||||
@ -35,17 +35,17 @@ class ImageCV(Node):
|
||||
b = rfmat.pseudo_color().m
|
||||
self.buffer = b
|
||||
elif isinstance(msg, SetWindowVisibleMsg):
|
||||
if msg.name == 'bscan' and msg.sender != 'cv':
|
||||
if msg.name == "bscan" and msg.sender != "cv":
|
||||
self.show = msg.value
|
||||
if msg.value:
|
||||
cv2.namedWindow('image', cv2.WINDOW_NORMAL)
|
||||
cv2.namedWindow("image", cv2.WINDOW_NORMAL)
|
||||
else:
|
||||
cv2.destroyWindow('image')
|
||||
cv2.destroyWindow("image")
|
||||
if self.show:
|
||||
if cv2.getWindowProperty('image', cv2.WND_PROP_VISIBLE) < 1:
|
||||
if cv2.getWindowProperty("image", cv2.WND_PROP_VISIBLE) < 1:
|
||||
self.show = False
|
||||
self.send(SetWindowVisibleMsg('cv', 'bscan', False))
|
||||
cv2.destroyWindow('image')
|
||||
self.send(SetWindowVisibleMsg("cv", "bscan", False))
|
||||
cv2.destroyWindow("image")
|
||||
continue
|
||||
cv2.imshow('image', self.buffer)
|
||||
cv2.imshow("image", self.buffer)
|
||||
cv2.waitKey(1)
|
||||
|
||||
@ -5,8 +5,8 @@ import time
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from flandre.BusClient import BusClient
|
||||
from flandre import C
|
||||
from flandre.BusClient import BusClient
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import Msg, RfMatMsg
|
||||
from flandre.utils.RfMat import RfMat
|
||||
@ -24,33 +24,46 @@ class ImageFFMPEG(Node):
|
||||
|
||||
def loop(self):
|
||||
self.c = BusClient(RfMatMsg, pub=False, conflare=True, poller=True)
|
||||
p = subprocess.Popen(['ffmpeg',
|
||||
'-f', 'rawvideo',
|
||||
'-pixel_format', 'rgb24',
|
||||
'-video_size', f'{C.video_width}x{C.video_height}',
|
||||
'-framerate', '24',
|
||||
'-hwaccel', 'nvdec',
|
||||
'-i', '-',
|
||||
'-vcodec', 'h264_nvenc',
|
||||
'-preset', 'fast',
|
||||
'-gpu', '1',
|
||||
|
||||
# '-profile:v', 'high',
|
||||
'-zerolatency', '1',
|
||||
# '-tune', 'ull',
|
||||
# '-level', '42',
|
||||
|
||||
'-pix_fmt', 'yuv420p',
|
||||
# '-vcodec', 'libx264',
|
||||
'-b:v', '20M',
|
||||
'-f', 'flv',
|
||||
'rtmp://q1hyb.as/live/bscan',
|
||||
# 'rtmp://11.2.1.21/live/bscan',
|
||||
# '-f', 'mpegts',
|
||||
# 'srt://localhost:10080?streamid=#!::r=live/livestream,m=publish'
|
||||
],
|
||||
stdin=subprocess.PIPE,
|
||||
)
|
||||
p = subprocess.Popen(
|
||||
[
|
||||
"ffmpeg",
|
||||
"-f",
|
||||
"rawvideo",
|
||||
"-pixel_format",
|
||||
"rgb24",
|
||||
"-video_size",
|
||||
f"{C.video_width}x{C.video_height}",
|
||||
"-framerate",
|
||||
"24",
|
||||
"-hwaccel",
|
||||
"nvdec",
|
||||
"-i",
|
||||
"-",
|
||||
"-vcodec",
|
||||
"h264_nvenc",
|
||||
"-preset",
|
||||
"fast",
|
||||
"-gpu",
|
||||
"1",
|
||||
# '-profile:v', 'high',
|
||||
"-zerolatency",
|
||||
"1",
|
||||
# '-tune', 'ull',
|
||||
# '-level', '42',
|
||||
"-pix_fmt",
|
||||
"yuv420p",
|
||||
# '-vcodec', 'libx264',
|
||||
"-b:v",
|
||||
"20M",
|
||||
"-f",
|
||||
"flv",
|
||||
"rtmp://q1hyb.as/live/bscan",
|
||||
# 'rtmp://11.2.1.21/live/bscan',
|
||||
# '-f', 'mpegts',
|
||||
# 'srt://localhost:10080?streamid=#!::r=live/livestream,m=publish'
|
||||
],
|
||||
stdin=subprocess.PIPE,
|
||||
)
|
||||
lasttime = time.time()
|
||||
while True:
|
||||
# socks = dict(self.c.poller.poll(1 / 30))
|
||||
@ -58,20 +71,19 @@ class ImageFFMPEG(Node):
|
||||
if events:
|
||||
msg: RfMatMsg = Msg.decode_msg(events[0][0].recv())
|
||||
rfmat: RfMat = msg.rfmat
|
||||
self.buffer = (rfmat
|
||||
.grey()
|
||||
.resize((C.video_width, C.video_height))
|
||||
.watermark()
|
||||
.call(cv2.cvtColor, cv2.COLOR_GRAY2RGB)
|
||||
.__bytes__()
|
||||
)
|
||||
self.buffer = (
|
||||
rfmat.grey()
|
||||
.resize((C.video_width, C.video_height))
|
||||
.watermark()
|
||||
.call(cv2.cvtColor, cv2.COLOR_GRAY2RGB)
|
||||
.__bytes__()
|
||||
)
|
||||
p.stdin.write(self.buffer)
|
||||
# time.sleep(1 / 60)
|
||||
currenttime = time.time()
|
||||
logger.debug(f'{currenttime - lasttime}')
|
||||
logger.debug(f"{currenttime - lasttime}")
|
||||
lasttime = currenttime
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
ImageFFMPEG()()
|
||||
|
||||
@ -1,13 +1,18 @@
|
||||
import sys
|
||||
|
||||
from PyQt6.QtCore import QByteArray, Qt
|
||||
from PyQt6.QtGui import QImage, QPixmap, QKeyEvent, QWheelEvent
|
||||
from PyQt6.QtWidgets import QMainWindow, QApplication, QGraphicsPixmapItem, QGraphicsScene
|
||||
from PyQt6.QtGui import QImage, QKeyEvent, QPixmap, QWheelEvent
|
||||
from PyQt6.QtWidgets import (
|
||||
QApplication,
|
||||
QGraphicsPixmapItem,
|
||||
QGraphicsScene,
|
||||
QMainWindow,
|
||||
)
|
||||
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.pyqt.Image import Ui_MainWindow
|
||||
from flandre.pyqt.ZMQReceiver import ZMQReceiver
|
||||
from flandre.utils.Msg import KillMsg, Msg, BMMsg, RfMatMsg, KeyPressMsg
|
||||
from flandre.utils.Msg import BMMsg, KeyPressMsg, KillMsg, Msg, RfMatMsg
|
||||
from flandre.utils.RfMat import RfMat
|
||||
|
||||
|
||||
@ -32,13 +37,13 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
def keyPressEvent(self, a0: QKeyEvent):
|
||||
t = a0.text()
|
||||
match t:
|
||||
case 'm':
|
||||
case "m":
|
||||
self.grey = not self.grey
|
||||
case 's':
|
||||
case "s":
|
||||
self.scale = not self.scale
|
||||
if not self.scale:
|
||||
self.need_fit = True
|
||||
case 't':
|
||||
case "t":
|
||||
self.watermark = not self.watermark
|
||||
|
||||
def wheelEvent(self, a0: QWheelEvent):
|
||||
@ -50,30 +55,27 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
def on_zmq_event(self, msg: QByteArray):
|
||||
msg = Msg.decode_msg(msg.data())
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.close()
|
||||
elif isinstance(msg, RfMatMsg):
|
||||
w = msg.rfmat.w
|
||||
h = msg.rfmat.h
|
||||
d: RfMat = msg.rfmat
|
||||
d2 = (d
|
||||
.resize((int(w * self.zoom), int(h * self.zoom)))
|
||||
.watermark(cond=self.watermark)
|
||||
)
|
||||
d2 = d.resize((int(w * self.zoom), int(h * self.zoom))).watermark(
|
||||
cond=self.watermark
|
||||
)
|
||||
w = d2.w
|
||||
h = d2.h
|
||||
qImg = QImage(
|
||||
d2.__bytes__(),
|
||||
w, h, 1 * w,
|
||||
QImage.Format.Format_Grayscale8
|
||||
)
|
||||
qImg = QImage(d2.__bytes__(), w, h, 1 * w, QImage.Format.Format_Grayscale8)
|
||||
self.g.setPixmap(QPixmap(qImg))
|
||||
self.s.setSceneRect(0.0, 0.0, w, h)
|
||||
if self.scale:
|
||||
self.graphicsView.fitInView(self.s.sceneRect())
|
||||
else:
|
||||
if self.need_fit:
|
||||
self.graphicsView.fitInView(self.s.sceneRect(), Qt.AspectRatioMode.KeepAspectRatio)
|
||||
self.graphicsView.fitInView(
|
||||
self.s.sceneRect(), Qt.AspectRatioMode.KeepAspectRatio
|
||||
)
|
||||
self.need_fit = False
|
||||
|
||||
|
||||
|
||||
@ -1,14 +1,13 @@
|
||||
import logging
|
||||
from threading import Thread
|
||||
|
||||
import mido
|
||||
import pyjoystick
|
||||
import zmq
|
||||
from mido.backends.rtmidi import Input, Output
|
||||
from pyjoystick.sdl2 import Key, Joystick, run_event_loop
|
||||
from pyjoystick.sdl2 import Joystick, run_event_loop
|
||||
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import KillMsg, Msg, ImageArgMsg, JoystickMsg
|
||||
from flandre.utils.Msg import ImageArgMsg, JoystickMsg, KillMsg, Msg
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@ -30,14 +29,16 @@ class Joystick(Node):
|
||||
|
||||
def sendj(self, t=0.1):
|
||||
d2 = self.d.copy()
|
||||
self.s2.send_json(dict(
|
||||
x=clamp(d2['x'], t),
|
||||
y=clamp(d2['y'], t),
|
||||
z=clamp(d2['z'], t),
|
||||
rx=clamp(d2['rx'], t),
|
||||
ry=clamp(d2['ry'], t),
|
||||
rz=clamp(d2['rz'], t),
|
||||
))
|
||||
self.s2.send_json(
|
||||
dict(
|
||||
x=clamp(d2["x"], t),
|
||||
y=clamp(d2["y"], t),
|
||||
z=clamp(d2["z"], t),
|
||||
rx=clamp(d2["rx"], t),
|
||||
ry=clamp(d2["ry"], t),
|
||||
rz=clamp(d2["rz"], t),
|
||||
)
|
||||
)
|
||||
|
||||
def custom_setup(self):
|
||||
self.isa = self.c.ctx.socket(zmq.PUSH)
|
||||
@ -48,7 +49,7 @@ class Joystick(Node):
|
||||
|
||||
self.t_joystick_event_loop = Thread(target=self.joystick_event_loop)
|
||||
self.t_joystick_event_loop.start()
|
||||
self.arg = ImageArgMsg('joystick', 0, 0)
|
||||
self.arg = ImageArgMsg("joystick", 0, 0)
|
||||
self.arg1 = 0
|
||||
self.d = dict(
|
||||
x=0.0,
|
||||
@ -66,27 +67,27 @@ class Joystick(Node):
|
||||
|
||||
def joystick_event_loop(self):
|
||||
def key_received(key: pyjoystick.interface.Key):
|
||||
msg = JoystickMsg(str(key).replace('-', ''), key.value)
|
||||
msg = JoystickMsg(str(key).replace("-", ""), key.value)
|
||||
print(msg)
|
||||
match msg.key:
|
||||
case 'Axis 0':
|
||||
self.d['x'] = -msg.value
|
||||
case 'Axis 1':
|
||||
self.d['y'] = msg.value
|
||||
case 'Axis 4':
|
||||
self.d['rx'] = -msg.value
|
||||
case 'Axis 3':
|
||||
self.d['ry'] = -msg.value
|
||||
case 'Axis 2':
|
||||
case "Axis 0":
|
||||
self.d["x"] = -msg.value
|
||||
case "Axis 1":
|
||||
self.d["y"] = msg.value
|
||||
case "Axis 4":
|
||||
self.d["rx"] = -msg.value
|
||||
case "Axis 3":
|
||||
self.d["ry"] = -msg.value
|
||||
case "Axis 2":
|
||||
self.z1 = msg.value
|
||||
case 'Axis 5':
|
||||
self.z2 = - msg.value
|
||||
case 'Button 4':
|
||||
self.rz1 = - msg.value
|
||||
case 'Button 5':
|
||||
case "Axis 5":
|
||||
self.z2 = -msg.value
|
||||
case "Button 4":
|
||||
self.rz1 = -msg.value
|
||||
case "Button 5":
|
||||
self.rz2 = msg.value
|
||||
self.d['rz'] = float(self.rz1 + self.rz2)
|
||||
self.d['z'] = float(self.z1 + self.z2)
|
||||
self.d["rz"] = float(self.rz1 + self.rz2)
|
||||
self.d["z"] = float(self.z1 + self.z2)
|
||||
|
||||
self.sendj()
|
||||
|
||||
@ -106,18 +107,18 @@ class Joystick(Node):
|
||||
if isb in p:
|
||||
msg = Msg.decode_msg(isb.recv())
|
||||
match msg.type:
|
||||
case 'pitchwheel':
|
||||
case "pitchwheel":
|
||||
match msg.channel:
|
||||
case 0:
|
||||
# print(msg.pitch)
|
||||
self.send(ImageArgMsg('midi', 0, msg.pitch * 3))
|
||||
self.send(ImageArgMsg("midi", 0, msg.pitch * 3))
|
||||
if self.c.sub in p:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.do_loop = False
|
||||
return
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
Joystick()()
|
||||
|
||||
@ -5,8 +5,18 @@ import zmq
|
||||
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import MoveAxisMsg, KillMsg, SetSeqMetaMsg, SeqIdMinMax, SetBaseMsg, PlaybackSeqListMsg, \
|
||||
SeqIdList, SetSidMsg, RfFrameMsg, RobotRtsiMsg
|
||||
from flandre.utils.Msg import (
|
||||
KillMsg,
|
||||
MoveAxisMsg,
|
||||
PlaybackSeqListMsg,
|
||||
RfFrameMsg,
|
||||
RobotRtsiMsg,
|
||||
SeqIdList,
|
||||
SeqIdMinMax,
|
||||
SetBaseMsg,
|
||||
SetSeqMetaMsg,
|
||||
SetSidMsg,
|
||||
)
|
||||
from flandre.utils.RfSequence import RfSequence
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
@ -25,8 +35,8 @@ class Loader(Node):
|
||||
while True:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, MoveAxisMsg) and rff is not None:
|
||||
logger.debug(f'Move axis: {msg}')
|
||||
if msg.axis == 'S':
|
||||
logger.debug(f"Move axis: {msg}")
|
||||
if msg.axis == "S":
|
||||
pass
|
||||
elif isinstance(msg, SetSidMsg):
|
||||
selected_frame = rff.frames[msg.value]
|
||||
@ -36,21 +46,21 @@ class Loader(Node):
|
||||
elif isinstance(msg, SetSeqMetaMsg):
|
||||
if base is None:
|
||||
continue
|
||||
if msg.target == 'playback':
|
||||
logger.info(f'load {msg.name}')
|
||||
if msg.target == "playback":
|
||||
logger.info(f"load {msg.name}")
|
||||
rff = RfSequence(base / msg.name)
|
||||
self.send(SeqIdMinMax(*rff.seq_id_minmax))
|
||||
self.send(SeqIdList([f.meta.sequence_id for f in rff.frames]))
|
||||
self.send(SetSidMsg(0))
|
||||
elif isinstance(msg, SetBaseMsg):
|
||||
base = Path(msg.value)
|
||||
seq_list = [f.name for f in base.glob('*')]
|
||||
seq_list = [f.name for f in base.glob("*")]
|
||||
if not seq_list:
|
||||
logger.warning(f'No sequences found in {base}')
|
||||
logger.warning(f"No sequences found in {base}")
|
||||
else:
|
||||
C.record_folder = base
|
||||
C.write_config()
|
||||
self.send(PlaybackSeqListMsg(seq_list))
|
||||
elif isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
break
|
||||
|
||||
@ -7,20 +7,52 @@ from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
|
||||
import zmq
|
||||
from PyQt6 import QtWidgets, QtGui
|
||||
from PyQt6 import QtGui, QtWidgets
|
||||
from PyQt6.QtCore import QByteArray, pyqtSlot
|
||||
from PyQt6.QtWidgets import QMainWindow, QApplication, QFrame, QMessageBox, QFileDialog, QLineEdit
|
||||
from PyQt6.QtWidgets import (
|
||||
QApplication,
|
||||
QFileDialog,
|
||||
QFrame,
|
||||
QLineEdit,
|
||||
QMainWindow,
|
||||
QMessageBox,
|
||||
)
|
||||
|
||||
from flandre import P
|
||||
from flandre import C
|
||||
from flandre import C, P
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.pyqt.Main import Ui_MainWindow
|
||||
from flandre.pyqt.ZMQReceiver import ZMQReceiver
|
||||
from flandre.utils.Msg import KillMsg, Msg, ImageArgMsg, SeqIdMinMax, MoveAxisMsg, PlaybackSeqListMsg, SetBaseMsg, \
|
||||
SetSeqMetaMsg, SetPlayMode, DeviceConnectedMsg, DeviceEnabledMsg, DeviceOnlineMsg, SetDeviceEnabledMsg, \
|
||||
SetDeviceConnectedMsg, DeviceConfigListMsg, SetDeviceConfigMsg, SetRecordMsg, RobotRtsiMsg, RecordFrameMsg, \
|
||||
SeqIdList, SetWindowVisibleMsg, SetSidMsg, ImagingConfigNameListMsg, DeviceZero, DeviceSwitchMsg, \
|
||||
SetDeviceSwitchMsg, SeqMetaMsg, RefreshDeviceMsg, MaxMsg
|
||||
from flandre.utils.Msg import (
|
||||
DeviceConfigListMsg,
|
||||
DeviceConnectedMsg,
|
||||
DeviceEnabledMsg,
|
||||
DeviceOnlineMsg,
|
||||
DeviceSwitchMsg,
|
||||
DeviceZero,
|
||||
ImageArgMsg,
|
||||
ImagingConfigNameListMsg,
|
||||
KillMsg,
|
||||
MaxMsg,
|
||||
MoveAxisMsg,
|
||||
Msg,
|
||||
PlaybackSeqListMsg,
|
||||
RecordFrameMsg,
|
||||
RefreshDeviceMsg,
|
||||
RobotRtsiMsg,
|
||||
SeqIdList,
|
||||
SeqIdMinMax,
|
||||
SeqMetaMsg,
|
||||
SetBaseMsg,
|
||||
SetDeviceConfigMsg,
|
||||
SetDeviceConnectedMsg,
|
||||
SetDeviceEnabledMsg,
|
||||
SetDeviceSwitchMsg,
|
||||
SetPlayMode,
|
||||
SetRecordMsg,
|
||||
SetSeqMetaMsg,
|
||||
SetSidMsg,
|
||||
SetWindowVisibleMsg,
|
||||
)
|
||||
from flandre.utils.RfMeta import RfSequenceMeta
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
@ -46,20 +78,20 @@ def humanbytes(B):
|
||||
"""Return the given bytes as a human friendly KB, MB, GB, or TB string."""
|
||||
B = float(B)
|
||||
KB = float(1024)
|
||||
MB = float(KB ** 2) # 1,048,576
|
||||
GB = float(KB ** 3) # 1,073,741,824
|
||||
TB = float(KB ** 4) # 1,099,511,627,776
|
||||
MB = float(KB**2) # 1,048,576
|
||||
GB = float(KB**3) # 1,073,741,824
|
||||
TB = float(KB**4) # 1,099,511,627,776
|
||||
|
||||
if B < KB:
|
||||
return '{0} {1}'.format(B, 'Bytes' if 0 == B > 1 else 'Byte')
|
||||
return "{0} {1}".format(B, "Bytes" if 0 == B > 1 else "Byte")
|
||||
elif KB <= B < MB:
|
||||
return '{0:.2f} KB'.format(B / KB)
|
||||
return "{0:.2f} KB".format(B / KB)
|
||||
elif MB <= B < GB:
|
||||
return '{0:.2f} MB'.format(B / MB)
|
||||
return "{0:.2f} MB".format(B / MB)
|
||||
elif GB <= B < TB:
|
||||
return '{0:.2f} GB'.format(B / GB)
|
||||
return "{0:.2f} GB".format(B / GB)
|
||||
elif TB <= B:
|
||||
return '{0:.2f} TB'.format(B / TB)
|
||||
return "{0:.2f} TB".format(B / TB)
|
||||
|
||||
|
||||
class Adv(QMainWindow, Ui_MainWindow):
|
||||
@ -73,28 +105,29 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
|
||||
self.device_switch_state: LinkStatus = LinkStatus.RED
|
||||
icon = QtGui.QIcon()
|
||||
icon.addPixmap(QtGui.QPixmap(str(P.ASSETS / 'switch_button.png')))
|
||||
icon.addPixmap(QtGui.QPixmap(str(P.ASSETS / "switch_button.png")))
|
||||
self.b_probe_head_switch.setIcon(icon)
|
||||
self.b_us_switch.setIcon(icon)
|
||||
self.b_cobot_switch.setIcon(icon)
|
||||
icon = QtGui.QIcon()
|
||||
icon.addPixmap(QtGui.QPixmap(str(P.ASSETS / 'refresh_button.png')))
|
||||
icon.addPixmap(QtGui.QPixmap(str(P.ASSETS / "refresh_button.png")))
|
||||
self.b_us_refresh.setIcon(icon)
|
||||
zmq_receiver = ZMQReceiver(self)
|
||||
zmq_receiver.zmq_event.connect(self.on_zmq_event)
|
||||
zmq_receiver.start()
|
||||
self.s_t_start.valueChanged.connect(self.on_t_start)
|
||||
self.s_t_end.valueChanged.connect(self.on_t_end)
|
||||
self.arg = ImageArgMsg('ui', t_start=0, t_end=1499)
|
||||
self.arg = ImageArgMsg("ui", t_start=0, t_end=1499)
|
||||
self.playback_seq_meta: RfSequenceMeta | None = None
|
||||
self.live_seq_meta: RfSequenceMeta | None = None
|
||||
self.record = False
|
||||
self.device_connected = False
|
||||
self.device_enabled = False
|
||||
self.l_base.textChanged.connect(lambda e:
|
||||
self.l_base.setStyleSheet("")
|
||||
if Path(e).exists() else
|
||||
self.l_base.setStyleSheet("background-color: pink;"))
|
||||
self.l_base.textChanged.connect(
|
||||
lambda e: self.l_base.setStyleSheet("")
|
||||
if Path(e).exists()
|
||||
else self.l_base.setStyleSheet("background-color: pink;")
|
||||
)
|
||||
|
||||
self.record_size_cnt = 0
|
||||
self.record_frame_cnt = 0
|
||||
@ -105,10 +138,10 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
self.b_select_base.clicked.connect(self.on_select_base)
|
||||
|
||||
self.cb_bscan.stateChanged.connect(self.on_cb_bscan)
|
||||
self.b_probe_single.clicked.connect(self.on_probe('single'))
|
||||
self.b_probe_orig.clicked.connect(self.on_probe('orig'))
|
||||
self.b_probe_start.clicked.connect(self.on_probe('start'))
|
||||
self.b_probe_stop.clicked.connect(self.on_probe('stop'))
|
||||
self.b_probe_single.clicked.connect(self.on_probe("single"))
|
||||
self.b_probe_orig.clicked.connect(self.on_probe("orig"))
|
||||
self.b_probe_start.clicked.connect(self.on_probe("start"))
|
||||
self.b_probe_stop.clicked.connect(self.on_probe("stop"))
|
||||
self.b_device_zero.clicked.connect(lambda: self.p.send(DeviceZero()))
|
||||
|
||||
self.mi_req_socket = zmq.Context().socket(zmq.REQ)
|
||||
@ -119,7 +152,7 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
match self.device_switch_state:
|
||||
case LinkStatus.RED:
|
||||
self.p.send(SetDeviceSwitchMsg(True))
|
||||
self.p.send(DeviceSwitchMsg('YELLOW'))
|
||||
self.p.send(DeviceSwitchMsg("YELLOW"))
|
||||
case LinkStatus.GREEN | LinkStatus.YELLOW | LinkStatus.ORANGE:
|
||||
self.p.send(SetDeviceSwitchMsg(False))
|
||||
|
||||
@ -136,40 +169,42 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
match self.device_switch_state:
|
||||
case LinkStatus.RED:
|
||||
self.g_device.setEnabled(False)
|
||||
self.g_us.setStyleSheet(get_style_sheet('g_us', 'pink'))
|
||||
self.g_us.setStyleSheet(get_style_sheet("g_us", "pink"))
|
||||
self.set_device_enable(LinkStatus.RED)
|
||||
self.set_device_connection(LinkStatus.RED)
|
||||
self.update_device_buttons()
|
||||
case LinkStatus.YELLOW:
|
||||
self.g_device.setEnabled(False)
|
||||
self.g_us.setStyleSheet(get_style_sheet('g_us', 'yellow'))
|
||||
self.g_us.setStyleSheet(get_style_sheet("g_us", "yellow"))
|
||||
case LinkStatus.GREEN:
|
||||
self.g_device.setEnabled(True)
|
||||
self.g_us.setStyleSheet(get_style_sheet('g_us', 'LightGreen'))
|
||||
self.g_us.setStyleSheet(get_style_sheet("g_us", "LightGreen"))
|
||||
case LinkStatus.ORANGE:
|
||||
self.g_device.setEnabled(False)
|
||||
self.g_us.setStyleSheet(get_style_sheet('g_us', 'orange'))
|
||||
self.g_us.setStyleSheet(get_style_sheet("g_us", "orange"))
|
||||
|
||||
def on_probe(self, arg):
|
||||
def f():
|
||||
ctx = zmq.Context()
|
||||
p = ctx.socket(zmq.PUSH)
|
||||
p.connect('tcp://q1hyb.as:23456')
|
||||
p.connect("tcp://q1hyb.as:23456")
|
||||
time.sleep(0.1)
|
||||
p.send_string(arg)
|
||||
p.disconnect('tcp://q1hyb.as:23456')
|
||||
p.disconnect("tcp://q1hyb.as:23456")
|
||||
|
||||
return f
|
||||
|
||||
@pyqtSlot()
|
||||
def on_b_new_imaging_config_clicked(self):
|
||||
filename, okPressed = QtWidgets.QInputDialog.getText(None,
|
||||
"Set New Imaging Config Name",
|
||||
"Config Name:",
|
||||
QLineEdit.EchoMode.Normal,
|
||||
self.c_imaging_config.currentText())
|
||||
if okPressed and filename != '':
|
||||
(C.imaging_config_folder / f'{filename}.json').write_text(self.arg.json())
|
||||
filename, okPressed = QtWidgets.QInputDialog.getText(
|
||||
None,
|
||||
"Set New Imaging Config Name",
|
||||
"Config Name:",
|
||||
QLineEdit.EchoMode.Normal,
|
||||
self.c_imaging_config.currentText(),
|
||||
)
|
||||
if okPressed and filename != "":
|
||||
(C.imaging_config_folder / f"{filename}.json").write_text(self.arg.json())
|
||||
idx = self.c_imaging_config.findText(filename)
|
||||
if idx == -1:
|
||||
self.c_imaging_config.addItem(filename)
|
||||
@ -181,15 +216,17 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
@pyqtSlot(int)
|
||||
def on_c_imaging_config_currentIndexChanged(self, i):
|
||||
name = self.c_imaging_config.itemText(i)
|
||||
self.p.send(ImageArgMsg.from_path(C.imaging_config_folder / f'{name}.json'))
|
||||
self.p.send(ImageArgMsg.from_path(C.imaging_config_folder / f"{name}.json"))
|
||||
|
||||
def on_select_base(self):
|
||||
base = QFileDialog.getExistingDirectory(self, 'Select Base Folder', DS.__str__())
|
||||
base = QFileDialog.getExistingDirectory(
|
||||
self, "Select Base Folder", DS.__str__()
|
||||
)
|
||||
self.l_base.setText(Path(base).__str__())
|
||||
|
||||
@pyqtSlot()
|
||||
def on_b_test1_clicked(self):
|
||||
logger.info(f'test1 {self.arg}')
|
||||
logger.info(f"test1 {self.arg}")
|
||||
|
||||
@pyqtSlot()
|
||||
def on_b_base_clicked(self):
|
||||
@ -202,48 +239,48 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
case LinkStatus.RED:
|
||||
self.device_connected = False
|
||||
self.c_live_seq_name.setEnabled(False)
|
||||
self.lb_device_connection.setText('Disconnected')
|
||||
self.lb_device_connection.setStyleSheet('background-color: pink;')
|
||||
self.lb_device_connection.setText("Disconnected")
|
||||
self.lb_device_connection.setStyleSheet("background-color: pink;")
|
||||
case LinkStatus.YELLOW:
|
||||
self.lb_device_connection.setText('Waiting')
|
||||
self.lb_device_connection.setStyleSheet('background-color: yellow;')
|
||||
self.lb_device_connection.setText("Waiting")
|
||||
self.lb_device_connection.setStyleSheet("background-color: yellow;")
|
||||
case LinkStatus.GREEN:
|
||||
self.device_connected = True
|
||||
self.c_live_seq_name.setEnabled(True)
|
||||
self.lb_device_connection.setText('Connected')
|
||||
self.lb_device_connection.setStyleSheet('background-color: LightGreen;')
|
||||
self.lb_device_connection.setText("Connected")
|
||||
self.lb_device_connection.setStyleSheet("background-color: LightGreen;")
|
||||
|
||||
def set_device_enable(self, status: LinkStatus):
|
||||
match status:
|
||||
case LinkStatus.RED:
|
||||
self.device_enabled = False
|
||||
self.lb_device_enable.setText('Disabled')
|
||||
self.lb_device_enable.setStyleSheet('background-color: pink;')
|
||||
self.lb_device_enable.setText("Disabled")
|
||||
self.lb_device_enable.setStyleSheet("background-color: pink;")
|
||||
case LinkStatus.YELLOW:
|
||||
self.lb_device_enable.setText('Waiting')
|
||||
self.lb_device_enable.setStyleSheet('background-color: yellow;')
|
||||
self.lb_device_enable.setText("Waiting")
|
||||
self.lb_device_enable.setStyleSheet("background-color: yellow;")
|
||||
case LinkStatus.GREEN:
|
||||
self.device_enabled = True
|
||||
self.lb_device_enable.setText('Enabled')
|
||||
self.lb_device_enable.setStyleSheet('background-color: LightGreen;')
|
||||
self.lb_device_enable.setText("Enabled")
|
||||
self.lb_device_enable.setStyleSheet("background-color: LightGreen;")
|
||||
|
||||
@pyqtSlot(bool)
|
||||
def on_g_live_clicked(self, b):
|
||||
if b:
|
||||
self.g_playback.setChecked(False)
|
||||
self.p.send(SetPlayMode('live'))
|
||||
self.p.send(SetPlayMode("live"))
|
||||
if self.live_seq_meta is not None:
|
||||
self.update_max(max(self.live_seq_meta.shape))
|
||||
logger.info(f'set playmode live')
|
||||
logger.info("set playmode live")
|
||||
|
||||
@pyqtSlot(bool)
|
||||
def on_g_playback_clicked(self, b):
|
||||
if b:
|
||||
self.g_live.setChecked(False)
|
||||
self.p.send(SetPlayMode('playback'))
|
||||
self.p.send(SetPlayMode("playback"))
|
||||
if self.playback_seq_meta is not None:
|
||||
self.update_max(max(self.playback_seq_meta.shape))
|
||||
logger.info(f'set playmode playback')
|
||||
logger.info("set playmode playback")
|
||||
|
||||
def on_device_disable(self):
|
||||
self.p.send(SetDeviceEnabledMsg(False))
|
||||
@ -261,16 +298,16 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
self.set_device_connection(LinkStatus.YELLOW)
|
||||
|
||||
def on_device_connect(self):
|
||||
logger.info('btn pre')
|
||||
logger.info("btn pre")
|
||||
self.p.send(SetDeviceConnectedMsg(True))
|
||||
self.b_device_connection.setEnabled(False)
|
||||
self.set_device_connection(LinkStatus.YELLOW)
|
||||
|
||||
def update_device_buttons(self):
|
||||
if self.device_connected and self.device_enabled:
|
||||
self.b_device_connection.setText('Disconnect')
|
||||
self.b_device_connection.setText("Disconnect")
|
||||
self.b_device_connection.setEnabled(False)
|
||||
self.b_device_enable.setText('Disable')
|
||||
self.b_device_enable.setText("Disable")
|
||||
self.b_device_enable.setEnabled(True)
|
||||
try:
|
||||
self.b_device_connection.clicked.disconnect()
|
||||
@ -282,9 +319,9 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
pass
|
||||
self.b_device_enable.clicked.connect(self.on_device_disable)
|
||||
if self.device_connected and not self.device_enabled:
|
||||
self.b_device_connection.setText('Disconnect')
|
||||
self.b_device_connection.setText("Disconnect")
|
||||
self.b_device_connection.setEnabled(True)
|
||||
self.b_device_enable.setText('Enable')
|
||||
self.b_device_enable.setText("Enable")
|
||||
self.b_device_enable.setEnabled(True)
|
||||
try:
|
||||
self.b_device_connection.clicked.disconnect()
|
||||
@ -299,9 +336,9 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
if not self.device_connected and self.device_enabled:
|
||||
raise Exception("wtf?")
|
||||
if not self.device_connected and not self.device_enabled:
|
||||
self.b_device_connection.setText('Connect')
|
||||
self.b_device_connection.setText("Connect")
|
||||
self.b_device_connection.setEnabled(True)
|
||||
self.b_device_enable.setText('Enable')
|
||||
self.b_device_enable.setText("Enable")
|
||||
self.b_device_enable.setEnabled(False)
|
||||
try:
|
||||
self.b_device_connection.clicked.disconnect()
|
||||
@ -319,17 +356,19 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
self.l_record_commit.setEnabled(True)
|
||||
self.p.send(SetRecordMsg(False))
|
||||
self.record = False
|
||||
self.b_record.setStyleSheet('')
|
||||
self.b_record.setStyleSheet("")
|
||||
else:
|
||||
if self.l_record_commit.text() != '':
|
||||
if self.l_record_commit.text() != "":
|
||||
self.l_record_commit.setEnabled(False)
|
||||
self.record_size_cnt = 0
|
||||
self.record_frame_cnt = 0
|
||||
self.p.send(SetRecordMsg(True, self.l_record_commit.text(), self.l_base.text()))
|
||||
self.p.send(
|
||||
SetRecordMsg(True, self.l_record_commit.text(), self.l_base.text())
|
||||
)
|
||||
self.record = True
|
||||
self.b_record.setStyleSheet('background-color: red;')
|
||||
self.b_record.setStyleSheet("background-color: red;")
|
||||
else:
|
||||
QMessageBox.warning(None, 'hint', 'Commit is empty!!')
|
||||
QMessageBox.warning(None, "hint", "Commit is empty!!")
|
||||
|
||||
def update_max_2(self, m):
|
||||
self.s_t_start.setMaximum(m)
|
||||
@ -352,11 +391,11 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
try:
|
||||
msg = Msg.decode_msg(msg.data())
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.close()
|
||||
elif isinstance(msg, ImageArgMsg):
|
||||
self.arg = msg
|
||||
self.arg.sender = 'ui'
|
||||
self.arg.sender = "ui"
|
||||
|
||||
self.s_t_start.setValue(msg.t_start)
|
||||
self.s_t_end.setValue(msg.t_end)
|
||||
@ -393,11 +432,9 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
self.sp_g7.setValue(msg.g7)
|
||||
self.sp_g8.setValue(msg.g8)
|
||||
|
||||
|
||||
|
||||
elif isinstance(msg, MoveAxisMsg):
|
||||
match msg.axis:
|
||||
case 'S':
|
||||
case "S":
|
||||
pass
|
||||
# self.s_sid.setValue(msg.value)
|
||||
# self.l_seq_current.setText(str(self.seq_id_list[msg.value]))
|
||||
@ -416,13 +453,17 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
for name in msg.value:
|
||||
self.c_playback_seq_name.addItem(name)
|
||||
if msg.value.__len__() > 0:
|
||||
self.p.send(SetSeqMetaMsg('playback', self.c_playback_seq_name.currentText()))
|
||||
self.p.send(
|
||||
SetSeqMetaMsg(
|
||||
"playback", self.c_playback_seq_name.currentText()
|
||||
)
|
||||
)
|
||||
elif isinstance(msg, SetSeqMetaMsg):
|
||||
if msg.target == 'playback':
|
||||
if msg.target == "playback":
|
||||
self.playback_seq_meta = RfSequenceMeta.from_name(msg.name)
|
||||
self.update_max(max(self.playback_seq_meta.shape))
|
||||
elif isinstance(msg, SeqMetaMsg):
|
||||
if msg.target == 'live':
|
||||
if msg.target == "live":
|
||||
self.l_live_seq_name.setText(msg.name)
|
||||
self.b_live_seq_apply.setEnabled(True)
|
||||
self.live_seq_meta = RfSequenceMeta.from_name(msg.name)
|
||||
@ -460,14 +501,13 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
elif isinstance(msg, MaxMsg):
|
||||
self.update_max_2(msg.value)
|
||||
elif isinstance(msg, RecordFrameMsg):
|
||||
|
||||
self.record_frame_cnt += 1
|
||||
self.record_size_cnt += msg.size
|
||||
self.l_record_size.setText(humanbytes(self.record_size_cnt))
|
||||
self.l_record_frames.setText(str(self.record_frame_cnt))
|
||||
self.l_record_max_sid.setText(str(msg.current_sid))
|
||||
elif isinstance(msg, SetWindowVisibleMsg):
|
||||
if msg.name == 'bscan' and msg.sender != 'ui':
|
||||
if msg.name == "bscan" and msg.sender != "ui":
|
||||
self.cb_bscan.setChecked(msg.value)
|
||||
elif isinstance(msg, ImagingConfigNameListMsg):
|
||||
# print(msg, flush=True) todo fix
|
||||
@ -482,14 +522,14 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
traceback.print_exception(e)
|
||||
|
||||
def closeEvent(self, event):
|
||||
self.p.send(KillMsg(''))
|
||||
self.p.send(KillMsg(""))
|
||||
# event.accept()
|
||||
# event.ignore()
|
||||
|
||||
@pyqtSlot(int)
|
||||
def on_cb_bscan(self, v):
|
||||
if self.cb_bscan.sender() is None:
|
||||
self.p.send(SetWindowVisibleMsg('ui', 'bscan', v == 2))
|
||||
self.p.send(SetWindowVisibleMsg("ui", "bscan", v == 2))
|
||||
|
||||
@pyqtSlot(int)
|
||||
def on_sp_crop_center_valueChanged(self, v):
|
||||
@ -613,8 +653,10 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
|
||||
@pyqtSlot(int)
|
||||
def on_c_playback_seq_name_currentIndexChanged(self, v):
|
||||
if self.c_playback_seq_name.sender() is None or isinstance(self.c_playback_seq_name.sender(), QFrame):
|
||||
self.p.send(SetSeqMetaMsg('playback', self.c_playback_seq_name.itemText(v)))
|
||||
if self.c_playback_seq_name.sender() is None or isinstance(
|
||||
self.c_playback_seq_name.sender(), QFrame
|
||||
):
|
||||
self.p.send(SetSeqMetaMsg("playback", self.c_playback_seq_name.itemText(v)))
|
||||
|
||||
@pyqtSlot(int)
|
||||
def on_s_sid_valueChanged(self, v):
|
||||
@ -631,18 +673,32 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
def on_b_live_seq_apply_clicked(self):
|
||||
v = self.c_live_seq_name.currentIndex()
|
||||
name = self.c_live_seq_name.currentText()
|
||||
if name != 'Empty':
|
||||
if name != "Empty":
|
||||
self.b_live_seq_apply.setEnabled(False)
|
||||
self.p.send(SetDeviceConfigMsg(name, self.c_live_seq_name.itemData(v)))
|
||||
|
||||
|
||||
class MainUI(Node):
|
||||
topics = [ImageArgMsg, SeqIdMinMax, MoveAxisMsg,
|
||||
ImagingConfigNameListMsg,
|
||||
PlaybackSeqListMsg, SetSeqMetaMsg, SeqIdList, SetWindowVisibleMsg, SetSidMsg,
|
||||
DeviceConnectedMsg, DeviceEnabledMsg, DeviceOnlineMsg, DeviceConfigListMsg,
|
||||
RobotRtsiMsg, DeviceSwitchMsg,
|
||||
RecordFrameMsg, SeqMetaMsg, MaxMsg]
|
||||
topics = [
|
||||
ImageArgMsg,
|
||||
SeqIdMinMax,
|
||||
MoveAxisMsg,
|
||||
ImagingConfigNameListMsg,
|
||||
PlaybackSeqListMsg,
|
||||
SetSeqMetaMsg,
|
||||
SeqIdList,
|
||||
SetWindowVisibleMsg,
|
||||
SetSidMsg,
|
||||
DeviceConnectedMsg,
|
||||
DeviceEnabledMsg,
|
||||
DeviceOnlineMsg,
|
||||
DeviceConfigListMsg,
|
||||
RobotRtsiMsg,
|
||||
DeviceSwitchMsg,
|
||||
RecordFrameMsg,
|
||||
SeqMetaMsg,
|
||||
MaxMsg,
|
||||
]
|
||||
|
||||
def __init__(self, level=logging.INFO):
|
||||
super().__init__(level=level)
|
||||
@ -650,9 +706,9 @@ class MainUI(Node):
|
||||
def loop(self):
|
||||
try:
|
||||
app = QApplication(sys.argv)
|
||||
app.setDesktopFileName('TestTest')
|
||||
if platform.system() == 'Windows':
|
||||
app.setStyle('windowsvista')
|
||||
app.setDesktopFileName("TestTest")
|
||||
if platform.system() == "Windows":
|
||||
app.setStyle("windowsvista")
|
||||
MainWindow = Adv(self)
|
||||
# icon = QtGui.QIcon()
|
||||
# icon.addPixmap(QtGui.QPixmap("remilia3.png"),
|
||||
|
||||
@ -6,9 +6,15 @@ import zmq
|
||||
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import KillMsg, SetDeviceSwitchMsg, DeviceSwitchMsg, RefreshDeviceMsg, InterruptMsg
|
||||
from flandre.utils.mi import MiSwitch
|
||||
from flandre.utils.network import check_port, check_socket
|
||||
from flandre.utils.Msg import (
|
||||
DeviceSwitchMsg,
|
||||
InterruptMsg,
|
||||
KillMsg,
|
||||
RefreshDeviceMsg,
|
||||
SetDeviceSwitchMsg,
|
||||
)
|
||||
from flandre.utils.network import check_socket
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@ -45,53 +51,53 @@ class Mi(Node):
|
||||
def ping1s(self):
|
||||
while True:
|
||||
if self.ping1enable:
|
||||
self.device_py_req_socket1s.send(b'get_device_status')
|
||||
self.device_py_req_socket1s.send(b"get_device_status")
|
||||
match self.device_py_req_socket1s.recv():
|
||||
case b'on':
|
||||
self.send(DeviceSwitchMsg('GREEN'))
|
||||
case b"on":
|
||||
self.send(DeviceSwitchMsg("GREEN"))
|
||||
self.ping1enable = False
|
||||
self.ping60enable = True
|
||||
self.device_py_req_socket1s.send(b'start')
|
||||
self.device_py_req_socket1s.send(b"start")
|
||||
self.device_py_req_socket1s.recv()
|
||||
case b'off':
|
||||
case b"off":
|
||||
pass
|
||||
case b'err':
|
||||
self.send(DeviceSwitchMsg('ORANGE'))
|
||||
case b"err":
|
||||
self.send(DeviceSwitchMsg("ORANGE"))
|
||||
self.ping1enable = False
|
||||
time.sleep(1)
|
||||
|
||||
def ping60s(self):
|
||||
while True:
|
||||
if self.ping60enable:
|
||||
self.device_py_req_socket60s.send(b'get_device_status')
|
||||
self.device_py_req_socket60s.send(b"get_device_status")
|
||||
match self.device_py_req_socket60s.recv():
|
||||
case b'off' | b'err':
|
||||
self.send(DeviceSwitchMsg('ORANGE'))
|
||||
case b"off" | b"err":
|
||||
self.send(DeviceSwitchMsg("ORANGE"))
|
||||
self.ping60enable = False
|
||||
self.device_py_req_socket60s.send(b'get_driver_status')
|
||||
self.device_py_req_socket60s.send(b"get_driver_status")
|
||||
match self.device_py_req_socket60s.recv():
|
||||
case b'off' | b'err':
|
||||
self.send(DeviceSwitchMsg('ORANGE'))
|
||||
case b"off" | b"err":
|
||||
self.send(DeviceSwitchMsg("ORANGE"))
|
||||
self.ping60enable = False
|
||||
time.sleep(10)
|
||||
|
||||
def start(self):
|
||||
self.device_py_req_socket.send(b'start')
|
||||
self.device_py_req_socket.send(b"start")
|
||||
self.device_py_req_socket.recv()
|
||||
|
||||
def loop(self):
|
||||
if self.mi.is_on() and check_socket(C.device_py_rep_socket):
|
||||
self.device_py_req_socket.send(b'get_device_status')
|
||||
self.device_py_req_socket.send(b"get_device_status")
|
||||
r = self.device_py_req_socket.recv()
|
||||
if r == b'on':
|
||||
self.device_py_req_socket.send(b'get_driver_status')
|
||||
if r == b"on":
|
||||
self.device_py_req_socket.send(b"get_driver_status")
|
||||
r = self.device_py_req_socket.recv()
|
||||
if r == b'on':
|
||||
self.send(DeviceSwitchMsg('GREEN'))
|
||||
if r == b"on":
|
||||
self.send(DeviceSwitchMsg("GREEN"))
|
||||
else:
|
||||
self.send(DeviceSwitchMsg('ORANGE'))
|
||||
self.send(DeviceSwitchMsg("ORANGE"))
|
||||
else:
|
||||
self.send(DeviceSwitchMsg('YELLOW'))
|
||||
self.send(DeviceSwitchMsg("YELLOW"))
|
||||
self.ping1enable = True
|
||||
while True:
|
||||
r = dict(self.c.poller.poll())
|
||||
@ -109,7 +115,7 @@ class Mi(Node):
|
||||
msg = self.recv()
|
||||
# logger.info(f'{msg}')
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
return
|
||||
if isinstance(msg, SetDeviceSwitchMsg):
|
||||
if msg.value:
|
||||
@ -118,14 +124,14 @@ class Mi(Node):
|
||||
else:
|
||||
self.ping60enable = False
|
||||
self.mi.off()
|
||||
self.device_py_req_socket.send(b'kill')
|
||||
self.device_py_req_socket.send(b"kill")
|
||||
self.device_py_req_socket.recv()
|
||||
self.send(DeviceSwitchMsg('RED'))
|
||||
self.send(DeviceSwitchMsg("RED"))
|
||||
elif isinstance(msg, RefreshDeviceMsg):
|
||||
self.send(InterruptMsg('device123'))
|
||||
self.device_py_req_socket.send(b'kill')
|
||||
self.send(InterruptMsg("device123"))
|
||||
self.device_py_req_socket.send(b"kill")
|
||||
self.device_py_req_socket.recv()
|
||||
self.device_py_req_socket.send(b'start')
|
||||
self.device_py_req_socket.send(b"start")
|
||||
self.device_py_req_socket.recv()
|
||||
self.send(DeviceSwitchMsg('GREEN'))
|
||||
self.send(DeviceSwitchMsg("GREEN"))
|
||||
self.ping60enable = True
|
||||
|
||||
@ -1,7 +1,5 @@
|
||||
import logging
|
||||
import time
|
||||
from threading import Thread
|
||||
from unittest import case
|
||||
|
||||
import mido
|
||||
import zmq
|
||||
@ -9,7 +7,14 @@ from mido import Message
|
||||
from mido.backends.rtmidi import Input, Output
|
||||
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import KillMsg, MidiMsg, Msg, ImageArgMsg, SetSeqMetaMsg, SetSidMsg, MaxMsg
|
||||
from flandre.utils.Msg import (
|
||||
ImageArgMsg,
|
||||
KillMsg,
|
||||
MaxMsg,
|
||||
MidiMsg,
|
||||
Msg,
|
||||
SetSeqMetaMsg,
|
||||
)
|
||||
from flandre.utils.RfMeta import RfSequenceMeta
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
@ -25,7 +30,7 @@ class Midi(Node):
|
||||
self.do_loop = True
|
||||
self.t_midi_event_loop: Thread = None
|
||||
self.isa: zmq.Socket = None
|
||||
self.arg = ImageArgMsg('midi', 0)
|
||||
self.arg = ImageArgMsg("midi", 0)
|
||||
|
||||
self.m_t_start = 100
|
||||
self.m_t_end = 100
|
||||
@ -39,23 +44,23 @@ class Midi(Node):
|
||||
self.isa = self.c.ctx.socket(zmq.PUSH)
|
||||
self.isa.bind("inproc://midi")
|
||||
|
||||
self.m_input: Input = mido.open_input('SMC-Mixer:SMC-Mixer Bluetooth 128:0')
|
||||
self.m_output: Output = mido.open_output('SMC-Mixer:SMC-Mixer Bluetooth 128:0')
|
||||
self.m_input: Input = mido.open_input("SMC-Mixer:SMC-Mixer Bluetooth 128:0")
|
||||
self.m_output: Output = mido.open_output("SMC-Mixer:SMC-Mixer Bluetooth 128:0")
|
||||
self.t_midi_event_loop = Thread(target=self.midi_event_loop)
|
||||
self.t_midi_event_loop.start()
|
||||
|
||||
self.pp = self.c.ctx.socket(zmq.PUSH)
|
||||
self.pp.connect('tcp://q1hyb.as:23456')
|
||||
self.pp.connect("tcp://q1hyb.as:23456")
|
||||
|
||||
def midi_event_loop(self):
|
||||
while self.do_loop:
|
||||
midi_msg: Message = self.m_input.receive()
|
||||
d = midi_msg.dict()
|
||||
match d['type']:
|
||||
case 'pitchwheel':
|
||||
channel = d['channel']
|
||||
match d["type"]:
|
||||
case "pitchwheel":
|
||||
channel = d["channel"]
|
||||
# 0-127
|
||||
pitch = int(d['pitch'] / 128) + 64
|
||||
pitch = int(d["pitch"] / 128) + 64
|
||||
if channel not in self.last_pitch:
|
||||
self.last_pitch[channel] = pitch
|
||||
# elif abs(pitch - self.last_pitch[channel]) > 5:
|
||||
@ -63,18 +68,38 @@ class Midi(Node):
|
||||
else:
|
||||
self.last_pitch[channel] = pitch
|
||||
# pitch_p = int(pitch / 127)
|
||||
self.isa.send(MidiMsg(type='pitchwheel', channel=channel, pitch=pitch).encode_msg())
|
||||
case 'control_change':
|
||||
self.isa.send(
|
||||
MidiMsg(
|
||||
type="pitchwheel", channel=channel, pitch=pitch
|
||||
).encode_msg()
|
||||
)
|
||||
case "control_change":
|
||||
for i in range(16, 24):
|
||||
if i in [d['control'], d['value']]:
|
||||
if 1 in [d['control'], d['value']]:
|
||||
self.isa.send(MidiMsg(type='control_change', value=1, control=i - 16).encode_msg())
|
||||
if i in [d["control"], d["value"]]:
|
||||
if 1 in [d["control"], d["value"]]:
|
||||
self.isa.send(
|
||||
MidiMsg(
|
||||
type="control_change", value=1, control=i - 16
|
||||
).encode_msg()
|
||||
)
|
||||
else:
|
||||
self.isa.send(MidiMsg(type='control_change', value=-1, control=i - 16).encode_msg())
|
||||
case 'note_on':
|
||||
self.isa.send(MidiMsg(type='note_on', note=d['note'], velocity=d['velocity']).encode_msg())
|
||||
case 'note_off':
|
||||
self.isa.send(MidiMsg(type='note_off', note=d['note'], velocity=d['velocity']).encode_msg())
|
||||
self.isa.send(
|
||||
MidiMsg(
|
||||
type="control_change", value=-1, control=i - 16
|
||||
).encode_msg()
|
||||
)
|
||||
case "note_on":
|
||||
self.isa.send(
|
||||
MidiMsg(
|
||||
type="note_on", note=d["note"], velocity=d["velocity"]
|
||||
).encode_msg()
|
||||
)
|
||||
case "note_off":
|
||||
self.isa.send(
|
||||
MidiMsg(
|
||||
type="note_off", note=d["note"], velocity=d["velocity"]
|
||||
).encode_msg()
|
||||
)
|
||||
|
||||
def loop(self):
|
||||
isb = self.c.ctx.socket(zmq.PULL)
|
||||
@ -85,7 +110,7 @@ class Midi(Node):
|
||||
if isb in p:
|
||||
msg: MidiMsg = Msg.decode_msg(isb.recv())
|
||||
match msg.type:
|
||||
case 'pitchwheel':
|
||||
case "pitchwheel":
|
||||
match msg.channel:
|
||||
case 0:
|
||||
self.arg.g1 = msg.pitch
|
||||
@ -104,36 +129,66 @@ class Midi(Node):
|
||||
case 7:
|
||||
self.arg.g8 = msg.pitch
|
||||
self.send(self.arg)
|
||||
case 'control_change':
|
||||
case "control_change":
|
||||
match msg.control:
|
||||
case 0:
|
||||
self.arg.t_start = sorted((1, self.arg.t_start + msg.value * 10, self.m_t_start))[1]
|
||||
self.arg.t_start = sorted(
|
||||
(
|
||||
1,
|
||||
self.arg.t_start + msg.value * 10,
|
||||
self.m_t_start,
|
||||
)
|
||||
)[1]
|
||||
case 1:
|
||||
self.arg.t_end = sorted((1, self.arg.t_end + msg.value * 10, self.m_t_end))[1]
|
||||
self.arg.t_end = sorted(
|
||||
(1, self.arg.t_end + msg.value * 10, self.m_t_end)
|
||||
)[1]
|
||||
case 2:
|
||||
self.arg.v2 = sorted((500, self.arg.v2 + msg.value * 10, 7000))[1]
|
||||
self.arg.v2 = sorted(
|
||||
(500, self.arg.v2 + msg.value * 10, 7000)
|
||||
)[1]
|
||||
case 3:
|
||||
self.arg.dct_center = \
|
||||
sorted((500, self.arg.dct_center + msg.value * 10, self.m_dct_center))[1]
|
||||
self.arg.dct_center = sorted(
|
||||
(
|
||||
500,
|
||||
self.arg.dct_center + msg.value * 10,
|
||||
self.m_dct_center,
|
||||
)
|
||||
)[1]
|
||||
case 4:
|
||||
self.arg.dct_bandwidth = \
|
||||
sorted((500, self.arg.dct_bandwidth + msg.value * 10, self.m_dct_bandwidth))[1]
|
||||
self.arg.dct_bandwidth = sorted(
|
||||
(
|
||||
500,
|
||||
self.arg.dct_bandwidth + msg.value * 10,
|
||||
self.m_dct_bandwidth,
|
||||
)
|
||||
)[1]
|
||||
case 5:
|
||||
self.arg.f_rows = sorted((500, self.arg.f_rows + msg.value * 10, self.m_f_rows))[1]
|
||||
self.arg.f_rows = sorted(
|
||||
(
|
||||
500,
|
||||
self.arg.f_rows + msg.value * 10,
|
||||
self.m_f_rows,
|
||||
)
|
||||
)[1]
|
||||
case 6:
|
||||
self.arg.beta = sorted((1, self.arg.beta + msg.value, 60))[1]
|
||||
self.arg.beta = sorted(
|
||||
(1, self.arg.beta + msg.value, 60)
|
||||
)[1]
|
||||
case 7:
|
||||
self.arg.tgc = sorted((1, self.arg.tgc + msg.value, 127))[1]
|
||||
case 'note_on':
|
||||
self.arg.tgc = sorted(
|
||||
(1, self.arg.tgc + msg.value, 127)
|
||||
)[1]
|
||||
case "note_on":
|
||||
match msg.note:
|
||||
case 23:
|
||||
self.pp.send_string('start')
|
||||
self.pp.send_string("start")
|
||||
case 15:
|
||||
self.pp.send_string('stop')
|
||||
self.pp.send_string("stop")
|
||||
case 7:
|
||||
self.pp.send_string('single')
|
||||
self.pp.send_string("single")
|
||||
case 31:
|
||||
self.pp.send_string('orig')
|
||||
self.pp.send_string("orig")
|
||||
# self.sid += 1
|
||||
# self.send(SetSidMsg(self.sid))
|
||||
|
||||
@ -141,13 +196,13 @@ class Midi(Node):
|
||||
if self.c.sub in p:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.do_loop = False
|
||||
return
|
||||
elif isinstance(msg, ImageArgMsg):
|
||||
if msg.sender != 'midi':
|
||||
if msg.sender != "midi":
|
||||
self.arg = msg
|
||||
self.arg.sender = 'midi'
|
||||
self.arg.sender = "midi"
|
||||
elif isinstance(msg, SetSeqMetaMsg):
|
||||
seq_meta = RfSequenceMeta.from_name(msg.name)
|
||||
mmax_shape0 = max(seq_meta.shape)
|
||||
|
||||
@ -2,8 +2,8 @@ import logging
|
||||
|
||||
import zmq
|
||||
|
||||
from flandre.BusClient import BusClient
|
||||
from flandre import C
|
||||
from flandre.BusClient import BusClient
|
||||
from flandre.nodes.Node import Node
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@ -1,7 +1,6 @@
|
||||
import logging
|
||||
import struct
|
||||
import time
|
||||
from pathlib import Path
|
||||
from threading import Thread
|
||||
|
||||
import zmq
|
||||
@ -9,8 +8,19 @@ import zmq
|
||||
from flandre import C
|
||||
from flandre.nodes.Device import Device, DeviceCmd
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import ImageArgMsg, KillMsg, SetSeqMetaMsg, SetPlayMode, SetDeviceConfigMsg, \
|
||||
ImagingConfigNameListMsg, BeamformerMsg, RobotRtsiMsg, SeqMetaMsg, DeviceEnabledMsg, RfFrameMsg
|
||||
from flandre.utils.Msg import (
|
||||
BeamformerMsg,
|
||||
DeviceEnabledMsg,
|
||||
ImageArgMsg,
|
||||
ImagingConfigNameListMsg,
|
||||
KillMsg,
|
||||
RfFrameMsg,
|
||||
RobotRtsiMsg,
|
||||
SeqMetaMsg,
|
||||
SetDeviceConfigMsg,
|
||||
SetPlayMode,
|
||||
SetSeqMetaMsg,
|
||||
)
|
||||
from flandre.utils.RfFrame import RfFrameMemory, b2t
|
||||
from flandre.utils.RfMeta import RfFrameMeta, RfSequenceMeta
|
||||
|
||||
@ -18,8 +28,16 @@ logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class Muxer(Node):
|
||||
topics = [SetSeqMetaMsg, SetPlayMode, SetDeviceConfigMsg, RfFrameMsg,
|
||||
ImageArgMsg, RobotRtsiMsg, SeqMetaMsg, DeviceEnabledMsg]
|
||||
topics = [
|
||||
SetSeqMetaMsg,
|
||||
SetPlayMode,
|
||||
SetDeviceConfigMsg,
|
||||
RfFrameMsg,
|
||||
ImageArgMsg,
|
||||
RobotRtsiMsg,
|
||||
SeqMetaMsg,
|
||||
DeviceEnabledMsg,
|
||||
]
|
||||
|
||||
def __init__(self, level=logging.INFO):
|
||||
super(Muxer, self).__init__(level=level)
|
||||
@ -32,12 +50,12 @@ class Muxer(Node):
|
||||
self.driver_pull_socket: zmq.Socket = None
|
||||
self.playback_rf_msg: RfFrameMsg | None = None
|
||||
self.device_enabled = False
|
||||
self.driver_data_raw = b''
|
||||
self.driver_data_raw = b""
|
||||
self.run_p_thread = True
|
||||
|
||||
def custom_setup(self):
|
||||
self.rep_socket: zmq.Socket = self.c.ctx.socket(zmq.REP)
|
||||
self.rep_socket.bind(f'tcp://localhost:{C.muxer_rep_port}')
|
||||
self.rep_socket.bind(f"tcp://localhost:{C.muxer_rep_port}")
|
||||
self.req_driver_socket: zmq.Socket = self.c.ctx.socket(zmq.REQ)
|
||||
# self.driver_pull_socket = self.c.ctx.socket(zmq.PULL)
|
||||
# self.driver_pull_socket.connect(C.live_push_socket)
|
||||
@ -47,11 +65,14 @@ class Muxer(Node):
|
||||
|
||||
def p_thread(self):
|
||||
while self.run_p_thread:
|
||||
if self.play_mode == 'live':
|
||||
if self.play_mode == "live":
|
||||
# ii = self.driver_pull_socket.poll(timeout=1000)
|
||||
# if ii > 0:
|
||||
# self.driver_data_raw = self.driver_pull_socket.recv()
|
||||
self.req_driver_socket.send(struct.pack('i', Device.magic) + struct.pack('i', DeviceCmd.GetData.value))
|
||||
self.req_driver_socket.send(
|
||||
struct.pack("i", Device.magic)
|
||||
+ struct.pack("i", DeviceCmd.GetData.value)
|
||||
)
|
||||
self.driver_data_raw = self.req_driver_socket.recv()
|
||||
else:
|
||||
time.sleep(1)
|
||||
@ -59,58 +80,67 @@ class Muxer(Node):
|
||||
def handle_rep_socket(self):
|
||||
self.rep_socket.recv()
|
||||
if self.play_mode is None:
|
||||
self.rep_socket.send(BeamformerMsg(b'init').encode_msg())
|
||||
self.rep_socket.send(BeamformerMsg(b"init").encode_msg())
|
||||
return
|
||||
match self.play_mode:
|
||||
case 'playback':
|
||||
case "playback":
|
||||
# logger.warning(f'test, {self.playback_rf_msg}')
|
||||
if self.playback_rf_msg is None:
|
||||
self.rep_socket.send(BeamformerMsg(b'nop').encode_msg())
|
||||
self.rep_socket.send(BeamformerMsg(b"nop").encode_msg())
|
||||
return
|
||||
data_msg = self.playback_rf_msg
|
||||
case 'live':
|
||||
case "live":
|
||||
if not self.device_enabled:
|
||||
self.rep_socket.send(BeamformerMsg(b'init').encode_msg())
|
||||
logger.warning('Device not enabled')
|
||||
self.rep_socket.send(BeamformerMsg(b"init").encode_msg())
|
||||
logger.warning("Device not enabled")
|
||||
return
|
||||
# self.req_driver_socket.send(b'')
|
||||
# self.driver_data_raw = self.req_driver_socket.recv()
|
||||
if self.driver_data_raw == b'':
|
||||
if self.driver_data_raw == b"":
|
||||
# todo fixit driver no empty
|
||||
self.rep_socket.send(BeamformerMsg(b'nop').encode_msg())
|
||||
self.rep_socket.send(BeamformerMsg(b"nop").encode_msg())
|
||||
return
|
||||
# _, sequence_id, encoder = struct.unpack_from('=IQi', self.driver_data_raw)
|
||||
# ts, sequence_id, encoder, driver_data_body = b2t(self.driver_data_raw)
|
||||
sequence_id, encoder, host_ts, device_ts_low, device_ts_high, driver_data_body = b2t(self.driver_data_raw)
|
||||
data_msg = RfFrameMsg(0, RfFrameMemory(
|
||||
RfFrameMeta(
|
||||
encoder=encoder,
|
||||
sequence_id=sequence_id,
|
||||
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_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],
|
||||
(
|
||||
sequence_id,
|
||||
encoder,
|
||||
host_ts,
|
||||
device_ts_low,
|
||||
device_ts_high,
|
||||
driver_data_body,
|
||||
) = b2t(self.driver_data_raw)
|
||||
data_msg = RfFrameMsg(
|
||||
0,
|
||||
RfFrameMemory(
|
||||
RfFrameMeta(
|
||||
encoder=encoder,
|
||||
sequence_id=sequence_id,
|
||||
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_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],
|
||||
),
|
||||
self.seq_meta_live,
|
||||
driver_data_body,
|
||||
),
|
||||
self.seq_meta_live,
|
||||
driver_data_body
|
||||
))
|
||||
)
|
||||
case _:
|
||||
raise NotImplementedError()
|
||||
# if (data_msg.data.__len__() // 2) != data_msg.rf_frame.prod():
|
||||
# self.rep_socket.send(BeamformerMsg(b'nop').encode_msg())
|
||||
# return
|
||||
self.rep_socket.send(BeamformerMsg(
|
||||
self.arg.encode_msg() +
|
||||
data_msg.encode_msg()
|
||||
).encode_msg())
|
||||
self.rep_socket.send(
|
||||
BeamformerMsg(self.arg.encode_msg() + data_msg.encode_msg()).encode_msg()
|
||||
)
|
||||
|
||||
def loop(self):
|
||||
t = Thread(target=self.p_thread).start()
|
||||
@ -120,9 +150,13 @@ class Muxer(Node):
|
||||
)
|
||||
device_socket = self.context.socket(zmq.PULL)
|
||||
|
||||
self.arg = ImageArgMsg('', t_start=0, t_end=1499)
|
||||
self.arg = ImageArgMsg("", t_start=0, t_end=1499)
|
||||
self.c.poller.register(device_socket, zmq.POLLIN)
|
||||
self.send(ImagingConfigNameListMsg([path.stem for path in C.imaging_config_folder.glob('*.json')]))
|
||||
self.send(
|
||||
ImagingConfigNameListMsg(
|
||||
[path.stem for path in C.imaging_config_folder.glob("*.json")]
|
||||
)
|
||||
)
|
||||
while True:
|
||||
socks = dict(self.c.poller.poll())
|
||||
for k in socks:
|
||||
@ -134,7 +168,7 @@ class Muxer(Node):
|
||||
msg = self.recv()
|
||||
if isinstance(msg, KillMsg):
|
||||
self.run_p_thread = False
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
return
|
||||
elif isinstance(msg, RfFrameMsg):
|
||||
if msg.sender == 1:
|
||||
@ -143,10 +177,10 @@ class Muxer(Node):
|
||||
self.arg = msg
|
||||
elif isinstance(msg, SeqMetaMsg):
|
||||
match msg.target:
|
||||
case 'live':
|
||||
case "live":
|
||||
self.seq_meta_live = RfSequenceMeta.from_name(msg.name)
|
||||
elif isinstance(msg, SetPlayMode):
|
||||
logger.info(f'set playmode {msg}')
|
||||
logger.info(f"set playmode {msg}")
|
||||
self.play_mode = msg.value
|
||||
elif isinstance(msg, RobotRtsiMsg):
|
||||
self.rtsi = msg
|
||||
|
||||
@ -4,9 +4,9 @@ from abc import abstractmethod
|
||||
|
||||
import zmq
|
||||
|
||||
from flandre.BusClient import BusClient
|
||||
import flandre
|
||||
from flandre.utils.Msg import Msg, KillMsg, NodeOnlineMsg, Msg1, Msg2
|
||||
from flandre.BusClient import BusClient
|
||||
from flandre.utils.Msg import KillMsg, Msg, Msg1, Msg2, NodeOnlineMsg
|
||||
|
||||
|
||||
class Node:
|
||||
@ -14,7 +14,14 @@ class Node:
|
||||
bp = BusClient.bp
|
||||
topics: list[type[Msg]] = []
|
||||
|
||||
def __init__(self, enable_init=True, level=logging.INFO, conflare=False, broker=False, req=None):
|
||||
def __init__(
|
||||
self,
|
||||
enable_init=True,
|
||||
level=logging.INFO,
|
||||
conflare=False,
|
||||
broker=False,
|
||||
req=None,
|
||||
):
|
||||
self.enable_init = enable_init
|
||||
self.isalive = True
|
||||
self.level = level
|
||||
@ -43,15 +50,23 @@ class Node:
|
||||
pass
|
||||
|
||||
def base_setup(self):
|
||||
FORMAT = '[%(asctime)s] p%(process)s {%(pathname)s:%(lineno)d} %(levelname)s - %(message)s'
|
||||
FORMAT = "[%(asctime)s] p%(process)s {%(pathname)s:%(lineno)d} %(levelname)s - %(message)s"
|
||||
FORMAT = '"%(pathname)s:%(lineno)d" [%(asctime)s.%(msecs)03d] %(levelname)s - %(message)s'
|
||||
logging.basicConfig(level=self.level, format=FORMAT,
|
||||
datefmt='%Y-%m-%d %H:%M:%S', )
|
||||
logging.basicConfig(
|
||||
level=self.level,
|
||||
format=FORMAT,
|
||||
datefmt="%Y-%m-%d %H:%M:%S",
|
||||
)
|
||||
rootLogger = logging.getLogger()
|
||||
|
||||
logFormatter = logging.Formatter(FORMAT, datefmt='%Y-%m-%d %H:%M:%S', )
|
||||
logFormatter = logging.Formatter(
|
||||
FORMAT,
|
||||
datefmt="%Y-%m-%d %H:%M:%S",
|
||||
)
|
||||
|
||||
log_file = flandre.C.log_folder / datetime.datetime.today().strftime('%Y-%m-%d.log')
|
||||
log_file = flandre.C.log_folder / datetime.datetime.today().strftime(
|
||||
"%Y-%m-%d.log"
|
||||
)
|
||||
fileHandler = logging.FileHandler(log_file)
|
||||
fileHandler.setFormatter(logFormatter)
|
||||
rootLogger.addHandler(fileHandler)
|
||||
@ -62,12 +77,16 @@ class Node:
|
||||
|
||||
self.context = zmq.Context()
|
||||
if self.enable_init:
|
||||
self.c = BusClient(*([KillMsg, Msg1, Msg2] + self.topics),
|
||||
poller=True, conflare=self.conflare, req_socket_str=self.req)
|
||||
self.c = BusClient(
|
||||
*([KillMsg, Msg1, Msg2] + self.topics),
|
||||
poller=True,
|
||||
conflare=self.conflare,
|
||||
req_socket_str=self.req,
|
||||
)
|
||||
|
||||
def __call__(self, *args, **kwargs):
|
||||
assert 'software_config' in kwargs, self.__class__
|
||||
flandre.C.copy_form(kwargs['software_config'])
|
||||
assert "software_config" in kwargs, self.__class__
|
||||
flandre.C.copy_form(kwargs["software_config"])
|
||||
self.setup()
|
||||
if not self.broker:
|
||||
while True:
|
||||
@ -80,4 +99,4 @@ class Node:
|
||||
if isinstance(msg, Msg1):
|
||||
break
|
||||
self.loop()
|
||||
print(self.__class__.__name__, 'exiting')
|
||||
print(self.__class__.__name__, "exiting")
|
||||
|
||||
@ -1,6 +1,5 @@
|
||||
import json
|
||||
import logging
|
||||
import struct
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
@ -8,17 +7,35 @@ import zmq
|
||||
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import ImageArgMsg, KillMsg, SetSeqMetaMsg, SetPlayMode, SetDeviceConfigMsg, SetRecordMsg, \
|
||||
RequestRfFrameMsg, RecordFrameMsg, RobotRtsiMsg, SeqMetaMsg
|
||||
from flandre.utils.Msg import (
|
||||
ImageArgMsg,
|
||||
KillMsg,
|
||||
RecordFrameMsg,
|
||||
RequestRfFrameMsg,
|
||||
RobotRtsiMsg,
|
||||
SeqMetaMsg,
|
||||
SetDeviceConfigMsg,
|
||||
SetPlayMode,
|
||||
SetRecordMsg,
|
||||
SetSeqMetaMsg,
|
||||
)
|
||||
from flandre.utils.RfFrame import b2t
|
||||
from flandre.utils.RfMeta import RfFrameMeta, RfSequenceMeta
|
||||
from flandre.utils.RfMeta import RfSequenceMeta
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class Recorder(Node):
|
||||
topics = [SetSeqMetaMsg, SeqMetaMsg, SetPlayMode, SetDeviceConfigMsg, SetRecordMsg, RequestRfFrameMsg,
|
||||
ImageArgMsg, RobotRtsiMsg]
|
||||
topics = [
|
||||
SetSeqMetaMsg,
|
||||
SeqMetaMsg,
|
||||
SetPlayMode,
|
||||
SetDeviceConfigMsg,
|
||||
SetRecordMsg,
|
||||
RequestRfFrameMsg,
|
||||
ImageArgMsg,
|
||||
RobotRtsiMsg,
|
||||
]
|
||||
|
||||
def __init__(self, level=logging.INFO):
|
||||
super(Recorder, self).__init__(level=level)
|
||||
@ -47,17 +64,19 @@ class Recorder(Node):
|
||||
if last_ts == ts:
|
||||
logger.warning(f"Record {ts}")
|
||||
last_ts = ts
|
||||
if self.seq_meta_live is not None and s.__len__() // 2 == np.prod(self.seq_meta_live.shape):
|
||||
(self.record_path / f'{ts}.bin').write_bytes(s)
|
||||
if self.seq_meta_live is not None and s.__len__() // 2 == np.prod(
|
||||
self.seq_meta_live.shape
|
||||
):
|
||||
(self.record_path / f"{ts}.bin").write_bytes(s)
|
||||
self.send(RecordFrameMsg(s.__len__(), sequence_id))
|
||||
if k == self.c.sub:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
return
|
||||
elif isinstance(msg, SeqMetaMsg):
|
||||
match msg.target:
|
||||
case 'live':
|
||||
case "live":
|
||||
self.seq_meta_live = RfSequenceMeta.from_name(msg.name)
|
||||
elif isinstance(msg, SetRecordMsg):
|
||||
self.record_enable = msg.enable
|
||||
@ -77,7 +96,9 @@ class Recorder(Node):
|
||||
arrj = []
|
||||
for msg in self.arr:
|
||||
arrj.append(msg.dict)
|
||||
(self.record_path / f'rtsi.{self.arr[0].ts}.json').write_text(json.dumps(arrj))
|
||||
(
|
||||
self.record_path / f"rtsi.{self.arr[0].ts}.json"
|
||||
).write_text(json.dumps(arrj))
|
||||
|
||||
elif isinstance(msg, RobotRtsiMsg):
|
||||
self.rtsi = msg
|
||||
|
||||
@ -4,7 +4,7 @@ import threading
|
||||
import zmq
|
||||
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import KillMsg, RobotRtsiMsg, Msg
|
||||
from flandre.utils.Msg import KillMsg, Msg, RobotRtsiMsg
|
||||
from flandre.utils.network import check_port
|
||||
from flandre.utils.rtsi import rtsi
|
||||
from flandre.utils.rtsi.serialize import DataObject
|
||||
@ -19,19 +19,21 @@ class Robot(Node):
|
||||
super(Robot, self).__init__()
|
||||
self.rtsi_thread_stop = False
|
||||
|
||||
if not check_port('11.6.1.53', 30004):
|
||||
logger.warning('Robot is not running on port 11.6.1.53')
|
||||
if not check_port("11.6.1.53", 30004):
|
||||
logger.warning("Robot is not running on port 11.6.1.53")
|
||||
self.rtsi_thread_stop = True
|
||||
return
|
||||
self.rt = rtsi('11.6.1.53') # 创建rtsi类
|
||||
self.rt = rtsi("11.6.1.53") # 创建rtsi类
|
||||
self.rt.connect() # socket链接远程rtsi
|
||||
self.rt.version_check() # rtsi版本协议检查
|
||||
version = self.rt.controller_version() # 获取控制器协议版本
|
||||
|
||||
def rtsi_thread(self):
|
||||
rtsi_push_socket = self.context.socket(zmq.PUSH)
|
||||
rtsi_push_socket.bind('inproc://rtsi')
|
||||
output1 = self.rt.output_subscribe('actual_TCP_pose,actual_TCP_force', 125) # 输出订阅,配方1
|
||||
rtsi_push_socket.bind("inproc://rtsi")
|
||||
output1 = self.rt.output_subscribe(
|
||||
"actual_TCP_pose,actual_TCP_force", 125
|
||||
) # 输出订阅,配方1
|
||||
self.rt.start() # rtsi 开始
|
||||
while not self.rtsi_thread_stop:
|
||||
recv_out: DataObject = self.rt.get_output_data()
|
||||
@ -40,24 +42,26 @@ class Robot(Node):
|
||||
if recv_out.recipe_id == output1.id:
|
||||
x, y, z, r, p, yy = recv_out.actual_TCP_pose
|
||||
fx, fy, fz, fr, fp, fyy = recv_out.actual_TCP_force
|
||||
rtsi_push_socket.send(RobotRtsiMsg(
|
||||
pos=(
|
||||
int(x * 100000),
|
||||
int(y * 100000),
|
||||
int(z * 100000),
|
||||
int(r * 1000),
|
||||
int(p * 1000),
|
||||
int(yy * 1000),
|
||||
),
|
||||
force=(
|
||||
int(fx * 10),
|
||||
int(fy * 10),
|
||||
int(fz * 10),
|
||||
int(fr * 100),
|
||||
int(fp * 100),
|
||||
int(fyy * 100),
|
||||
),
|
||||
).encode_msg())
|
||||
rtsi_push_socket.send(
|
||||
RobotRtsiMsg(
|
||||
pos=(
|
||||
int(x * 100000),
|
||||
int(y * 100000),
|
||||
int(z * 100000),
|
||||
int(r * 1000),
|
||||
int(p * 1000),
|
||||
int(yy * 1000),
|
||||
),
|
||||
force=(
|
||||
int(fx * 10),
|
||||
int(fy * 10),
|
||||
int(fz * 10),
|
||||
int(fr * 100),
|
||||
int(fp * 100),
|
||||
int(fyy * 100),
|
||||
),
|
||||
).encode_msg()
|
||||
)
|
||||
self.rt.disconnect()
|
||||
|
||||
def loop(self):
|
||||
@ -66,7 +70,7 @@ class Robot(Node):
|
||||
t = threading.Thread(target=self.rtsi_thread)
|
||||
t.start()
|
||||
rtsi_pull_socket = self.context.socket(zmq.PULL)
|
||||
rtsi_pull_socket.connect('inproc://rtsi')
|
||||
rtsi_pull_socket.connect("inproc://rtsi")
|
||||
self.c.poller.register(rtsi_pull_socket, zmq.POLLIN)
|
||||
while True:
|
||||
socks = dict(self.c.poller.poll())
|
||||
@ -76,11 +80,11 @@ class Robot(Node):
|
||||
if self.c.sub in socks and socks[self.c.sub] == zmq.POLLIN:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.rtsi_thread_stop = True
|
||||
return
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
r = Robot()
|
||||
r()
|
||||
|
||||
@ -1,18 +1,19 @@
|
||||
import sys
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PyQt6.QtCore import QByteArray, Qt
|
||||
from PyQt6.QtGui import QImage, QPixmap, QKeyEvent, QWheelEvent
|
||||
from PyQt6.QtWidgets import QMainWindow, QApplication, QGraphicsPixmapItem, QGraphicsScene
|
||||
from PyQt6.QtGui import QImage, QKeyEvent, QPixmap, QWheelEvent
|
||||
from PyQt6.QtWidgets import (
|
||||
QApplication,
|
||||
QGraphicsPixmapItem,
|
||||
QGraphicsScene,
|
||||
QMainWindow,
|
||||
)
|
||||
|
||||
from flandre import C
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.pyqt.FFmpegReceiver import FFmpegReceiver
|
||||
from flandre.pyqt.Image import Ui_MainWindow
|
||||
from flandre.pyqt.ZMQReceiver import ZMQReceiver
|
||||
from flandre.utils.Msg import KillMsg, Msg, BMMsg, RfMatMsg, KeyPressMsg, RGB888Msg
|
||||
from flandre.utils.Msg import KeyPressMsg, KillMsg, Msg, RfMatMsg
|
||||
from flandre.utils.RfMat import RfMat
|
||||
|
||||
|
||||
@ -25,7 +26,9 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
zmq_receiver.zmq_event.connect(self.on_zmq_event)
|
||||
zmq_receiver.start()
|
||||
|
||||
ffmpeg_receiver = FFmpegReceiver('rtsp://admin:ab12ab12@11.6.2.2:554/h264/ch1/main/av_stream', self)
|
||||
ffmpeg_receiver = FFmpegReceiver(
|
||||
"rtsp://admin:ab12ab12@11.6.2.2:554/h264/ch1/main/av_stream", self
|
||||
)
|
||||
ffmpeg_receiver.zmq_event.connect(self.on_ffmpeg_event)
|
||||
ffmpeg_receiver.start()
|
||||
|
||||
@ -42,13 +45,13 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
def keyPressEvent(self, a0: QKeyEvent):
|
||||
t = a0.text()
|
||||
match t:
|
||||
case 'm':
|
||||
case "m":
|
||||
self.grey = not self.grey
|
||||
case 's':
|
||||
case "s":
|
||||
self.scale = not self.scale
|
||||
if not self.scale:
|
||||
self.need_fit = True
|
||||
case 't':
|
||||
case "t":
|
||||
self.watermark = not self.watermark
|
||||
|
||||
def wheelEvent(self, a0: QWheelEvent):
|
||||
@ -61,42 +64,37 @@ class Adv(QMainWindow, Ui_MainWindow):
|
||||
b = msg.data()
|
||||
w = 1920
|
||||
h = 1080
|
||||
qImg = QImage(
|
||||
b,
|
||||
w, h, 3 * w,
|
||||
QImage.Format.Format_RGB888
|
||||
)
|
||||
qImg = QImage(b, w, h, 3 * w, QImage.Format.Format_RGB888)
|
||||
self.g.setPixmap(QPixmap(qImg))
|
||||
self.s.setSceneRect(0.0, 0.0, w, h)
|
||||
self.graphicsView.fitInView(self.s.sceneRect(), Qt.AspectRatioMode.KeepAspectRatio)
|
||||
self.graphicsView.fitInView(
|
||||
self.s.sceneRect(), Qt.AspectRatioMode.KeepAspectRatio
|
||||
)
|
||||
|
||||
def on_zmq_event(self, msg: QByteArray):
|
||||
msg = Msg.decode_msg(msg.data())
|
||||
if isinstance(msg, KillMsg):
|
||||
if msg.name == '':
|
||||
if msg.name == "":
|
||||
self.close()
|
||||
elif isinstance(msg, RfMatMsg):
|
||||
w = msg.rfmat.w
|
||||
h = msg.rfmat.h
|
||||
d: RfMat = msg.rfmat
|
||||
d2 = (d
|
||||
.resize((int(w * self.zoom), int(h * self.zoom)))
|
||||
.watermark(cond=self.watermark)
|
||||
)
|
||||
d2 = d.resize((int(w * self.zoom), int(h * self.zoom))).watermark(
|
||||
cond=self.watermark
|
||||
)
|
||||
w = d2.w
|
||||
h = d2.h
|
||||
qImg = QImage(
|
||||
d2.__bytes__(),
|
||||
w, h, 1 * w,
|
||||
QImage.Format.Format_Grayscale8
|
||||
)
|
||||
qImg = QImage(d2.__bytes__(), w, h, 1 * w, QImage.Format.Format_Grayscale8)
|
||||
self.g.setPixmap(QPixmap(qImg))
|
||||
self.s.setSceneRect(0.0, 0.0, w, h)
|
||||
if self.scale:
|
||||
self.graphicsView.fitInView(self.s.sceneRect())
|
||||
else:
|
||||
if self.need_fit:
|
||||
self.graphicsView.fitInView(self.s.sceneRect(), Qt.AspectRatioMode.KeepAspectRatio)
|
||||
self.graphicsView.fitInView(
|
||||
self.s.sceneRect(), Qt.AspectRatioMode.KeepAspectRatio
|
||||
)
|
||||
self.need_fit = False
|
||||
# elif isinstance(msg, RGB888Msg):
|
||||
# w = msg.w
|
||||
|
||||
@ -1,21 +1,19 @@
|
||||
import asyncio
|
||||
import dataclasses
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from threading import Thread
|
||||
|
||||
from fastapi import FastAPI, Response
|
||||
|
||||
from flandre.nodes.Node import Node
|
||||
from flandre.utils.Msg import ImageArgMsg, RobotRtsiMsg, SetSidMsg
|
||||
from flandre.utils.RfSequence import RfSequence
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
import uvicorn
|
||||
|
||||
from typing import List
|
||||
|
||||
from fastapi import FastAPI, WebSocket, WebSocketDisconnect, APIRouter
|
||||
from fastapi import APIRouter, WebSocket, WebSocketDisconnect
|
||||
|
||||
app = FastAPI()
|
||||
|
||||
@ -78,26 +76,27 @@ class Web(Node):
|
||||
def __init__(self, level=logging.INFO):
|
||||
super(Web, self).__init__(level=level)
|
||||
self.wss: list[WebSocket] = []
|
||||
self.arg = ImageArgMsg(sender='frontend', t_end=123)
|
||||
self.arg = ImageArgMsg(sender="frontend", t_end=123)
|
||||
|
||||
def custom_setup(self):
|
||||
self.router = APIRouter()
|
||||
self.router.add_api_route("/hello", self.hello, methods=["GET"])
|
||||
self.router.add_websocket_route("/ws", self.websocket_endpoint)
|
||||
self.router.add_api_route("/p", self.get_image, response_class=Response, responses={
|
||||
200: {
|
||||
"content": {"image/png": {}}
|
||||
}
|
||||
})
|
||||
self.router.add_api_route(
|
||||
"/p",
|
||||
self.get_image,
|
||||
response_class=Response,
|
||||
responses={200: {"content": {"image/png": {}}}},
|
||||
)
|
||||
|
||||
def hello(self):
|
||||
return {"Hello": 'asd'}
|
||||
return {"Hello": "asd"}
|
||||
|
||||
def get_image(self, i: int):
|
||||
rfs = RfSequence()
|
||||
f = rfs.frames[i]
|
||||
|
||||
image_bytes: bytes = b''
|
||||
image_bytes: bytes = b""
|
||||
return Response(content=image_bytes, media_type="image/png")
|
||||
|
||||
async def websocket_endpoint(self, websocket: WebSocket):
|
||||
@ -106,11 +105,11 @@ class Web(Node):
|
||||
try:
|
||||
while True:
|
||||
data = await websocket.receive_json()
|
||||
if data['type'] == 'ImageArgMsg':
|
||||
if data["type"] == "ImageArgMsg":
|
||||
t = dataclasses.replace(self.arg)
|
||||
t.sender = data['sender']
|
||||
t.t_start = data['t_start']
|
||||
t.g8 = data['g8']
|
||||
t.sender = data["sender"]
|
||||
t.t_start = data["t_start"]
|
||||
t.g8 = data["g8"]
|
||||
self.send(t)
|
||||
except WebSocketDisconnect:
|
||||
self.wss.remove(websocket)
|
||||
@ -132,7 +131,7 @@ class Web(Node):
|
||||
while True:
|
||||
msg = self.recv()
|
||||
if isinstance(msg, ImageArgMsg):
|
||||
if msg.sender != 'frontend':
|
||||
if msg.sender != "frontend":
|
||||
self.arg = msg
|
||||
self.boardcast(msg.dict)
|
||||
elif isinstance(msg, RobotRtsiMsg):
|
||||
@ -141,5 +140,5 @@ class Web(Node):
|
||||
self.boardcast(msg.dict)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
Web()()
|
||||
|
||||
@ -3,11 +3,9 @@ import threading
|
||||
|
||||
from PyQt6 import QtCore
|
||||
|
||||
from flandre.nodes.Node import Node
|
||||
|
||||
|
||||
class FFmpegReceiver(QtCore.QObject):
|
||||
zmq_event = QtCore.pyqtSignal('QByteArray')
|
||||
zmq_event = QtCore.pyqtSignal("QByteArray")
|
||||
|
||||
def __init__(self, url: str, parent=None):
|
||||
super().__init__(parent)
|
||||
@ -17,15 +15,25 @@ class FFmpegReceiver(QtCore.QObject):
|
||||
threading.Thread(target=self._execute, daemon=True).start()
|
||||
|
||||
def _execute(self):
|
||||
p = subprocess.Popen([
|
||||
'ffmpeg',
|
||||
'-loglevel', 'quiet',
|
||||
'-flags', 'low_delay',
|
||||
'-fflags', 'nobuffer',
|
||||
'-i', self.url,
|
||||
'-pix_fmt', 'rgb24',
|
||||
'-f', 'rawvideo',
|
||||
'-'
|
||||
], stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
|
||||
p = subprocess.Popen(
|
||||
[
|
||||
"ffmpeg",
|
||||
"-loglevel",
|
||||
"quiet",
|
||||
"-flags",
|
||||
"low_delay",
|
||||
"-fflags",
|
||||
"nobuffer",
|
||||
"-i",
|
||||
self.url,
|
||||
"-pix_fmt",
|
||||
"rgb24",
|
||||
"-f",
|
||||
"rawvideo",
|
||||
"-",
|
||||
],
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
)
|
||||
while True:
|
||||
self.zmq_event.emit(p.stdout.read(1920 * 1080 * 3))
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
# run again. Do not edit this file unless you know what you are doing.
|
||||
|
||||
|
||||
from PyQt6 import QtCore, QtGui, QtWidgets
|
||||
from PyQt6 import QtCore, QtWidgets
|
||||
|
||||
|
||||
class Ui_MainWindow(object):
|
||||
@ -19,7 +19,9 @@ class Ui_MainWindow(object):
|
||||
self.gridLayout.setObjectName("gridLayout")
|
||||
self.graphicsView = MyGraphicsView(parent=self.centralwidget)
|
||||
self.graphicsView.setDragMode(QtWidgets.QGraphicsView.DragMode.ScrollHandDrag)
|
||||
self.graphicsView.setResizeAnchor(QtWidgets.QGraphicsView.ViewportAnchor.AnchorUnderMouse)
|
||||
self.graphicsView.setResizeAnchor(
|
||||
QtWidgets.QGraphicsView.ViewportAnchor.AnchorUnderMouse
|
||||
)
|
||||
self.graphicsView.setObjectName("graphicsView")
|
||||
self.gridLayout.addWidget(self.graphicsView, 0, 0, 1, 1)
|
||||
MainWindow.setCentralWidget(self.centralwidget)
|
||||
@ -37,4 +39,6 @@ class Ui_MainWindow(object):
|
||||
def retranslateUi(self, MainWindow):
|
||||
_translate = QtCore.QCoreApplication.translate
|
||||
MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
|
||||
|
||||
|
||||
from flandre.pyqt.MyGraphicsView import MyGraphicsView
|
||||
|
||||
@ -67,7 +67,12 @@ class Ui_MainWindow(object):
|
||||
self.s_v2.setOrientation(QtCore.Qt.Orientation.Horizontal)
|
||||
self.s_v2.setObjectName("s_v2")
|
||||
self.gridLayout_5.addWidget(self.s_v2, 5, 1, 1, 1)
|
||||
spacerItem = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Policy.Minimum, QtWidgets.QSizePolicy.Policy.Expanding)
|
||||
spacerItem = QtWidgets.QSpacerItem(
|
||||
20,
|
||||
40,
|
||||
QtWidgets.QSizePolicy.Policy.Minimum,
|
||||
QtWidgets.QSizePolicy.Policy.Expanding,
|
||||
)
|
||||
self.gridLayout_5.addItem(spacerItem, 11, 1, 1, 1)
|
||||
self.sp_f_rows = QtWidgets.QSpinBox(parent=self.centralwidget)
|
||||
self.sp_f_rows.setMaximum(7000)
|
||||
@ -263,10 +268,9 @@ class Ui_MainWindow(object):
|
||||
self.horizontalLayout_8.addWidget(self.g_playback)
|
||||
self.gridLayout.addWidget(self.g_cap, 2, 0, 1, 2)
|
||||
self.g_cobot = QtWidgets.QGroupBox(parent=self.centralwidget)
|
||||
self.g_cobot.setStyleSheet("#g_cobot {\n"
|
||||
" background: pink;\n"
|
||||
"border-radius: 7px;\n"
|
||||
"}")
|
||||
self.g_cobot.setStyleSheet(
|
||||
"#g_cobot {\n background: pink;\nborder-radius: 7px;\n}"
|
||||
)
|
||||
self.g_cobot.setTitle("")
|
||||
self.g_cobot.setObjectName("g_cobot")
|
||||
self.gridLayout_3 = QtWidgets.QGridLayout(self.g_cobot)
|
||||
@ -360,10 +364,7 @@ class Ui_MainWindow(object):
|
||||
self.gridLayout_3.setRowStretch(1, 1)
|
||||
self.gridLayout.addWidget(self.g_cobot, 0, 1, 2, 1)
|
||||
self.g_us = QtWidgets.QGroupBox(parent=self.centralwidget)
|
||||
self.g_us.setStyleSheet("#g_us{\n"
|
||||
" background: pink;\n"
|
||||
"border-radius: 7px;\n"
|
||||
"}")
|
||||
self.g_us.setStyleSheet("#g_us{\n background: pink;\nborder-radius: 7px;\n}")
|
||||
self.g_us.setObjectName("g_us")
|
||||
self.verticalLayout_2 = QtWidgets.QVBoxLayout(self.g_us)
|
||||
self.verticalLayout_2.setObjectName("verticalLayout_2")
|
||||
@ -454,7 +455,12 @@ class Ui_MainWindow(object):
|
||||
self.s_g3.setOrientation(QtCore.Qt.Orientation.Horizontal)
|
||||
self.s_g3.setObjectName("s_g3")
|
||||
self.gridLayout_4.addWidget(self.s_g3, 3, 1, 1, 1)
|
||||
spacerItem1 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Policy.Minimum, QtWidgets.QSizePolicy.Policy.Expanding)
|
||||
spacerItem1 = QtWidgets.QSpacerItem(
|
||||
20,
|
||||
40,
|
||||
QtWidgets.QSizePolicy.Policy.Minimum,
|
||||
QtWidgets.QSizePolicy.Policy.Expanding,
|
||||
)
|
||||
self.gridLayout_4.addItem(spacerItem1, 9, 1, 1, 1)
|
||||
self.label_14 = QtWidgets.QLabel(parent=self.centralwidget)
|
||||
self.label_14.setObjectName("label_14")
|
||||
@ -561,14 +567,18 @@ class Ui_MainWindow(object):
|
||||
self.b_test1 = QtWidgets.QPushButton(parent=self.centralwidget)
|
||||
self.b_test1.setObjectName("b_test1")
|
||||
self.horizontalLayout_6.addWidget(self.b_test1)
|
||||
spacerItem2 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Policy.Expanding, QtWidgets.QSizePolicy.Policy.Minimum)
|
||||
spacerItem2 = QtWidgets.QSpacerItem(
|
||||
40,
|
||||
20,
|
||||
QtWidgets.QSizePolicy.Policy.Expanding,
|
||||
QtWidgets.QSizePolicy.Policy.Minimum,
|
||||
)
|
||||
self.horizontalLayout_6.addItem(spacerItem2)
|
||||
self.gridLayout.addLayout(self.horizontalLayout_6, 5, 0, 1, 2)
|
||||
self.g_probe_head = QtWidgets.QGroupBox(parent=self.centralwidget)
|
||||
self.g_probe_head.setStyleSheet("#g_probe_head {\n"
|
||||
" background: pink;\n"
|
||||
"border-radius: 7px;\n"
|
||||
"}")
|
||||
self.g_probe_head.setStyleSheet(
|
||||
"#g_probe_head {\n background: pink;\nborder-radius: 7px;\n}"
|
||||
)
|
||||
self.g_probe_head.setTitle("")
|
||||
self.g_probe_head.setObjectName("g_probe_head")
|
||||
self.verticalLayout_5 = QtWidgets.QVBoxLayout(self.g_probe_head)
|
||||
@ -627,7 +637,9 @@ class Ui_MainWindow(object):
|
||||
|
||||
def retranslateUi(self, MainWindow):
|
||||
_translate = QtCore.QCoreApplication.translate
|
||||
MainWindow.setWindowTitle(_translate("MainWindow", "Scarlet Flandre Control Panel"))
|
||||
MainWindow.setWindowTitle(
|
||||
_translate("MainWindow", "Scarlet Flandre Control Panel")
|
||||
)
|
||||
self.label_35.setText(_translate("MainWindow", "beta"))
|
||||
self.label_34.setText(_translate("MainWindow", "f_rows"))
|
||||
self.label_7.setText(_translate("MainWindow", "dct_center"))
|
||||
@ -670,8 +682,18 @@ class Ui_MainWindow(object):
|
||||
self.lb_device_connection.setText(_translate("MainWindow", "Disconnected"))
|
||||
self.lb_device_enable.setText(_translate("MainWindow", "Disabled"))
|
||||
self.b_device_enable.setText(_translate("MainWindow", "Disconnect"))
|
||||
self.label_24.setText(_translate("MainWindow", "<html><head/><body><p style=\"line-height:0.3\" align=\"center\">Beam</p><p align=\"center\">Status</p></body></html>"))
|
||||
self.label_21.setText(_translate("MainWindow", "<html><head/><body><p style=\"line-height:0.3\" align=\"center\">Connection</p><p align=\"center\">Status</p></body></html>"))
|
||||
self.label_24.setText(
|
||||
_translate(
|
||||
"MainWindow",
|
||||
'<html><head/><body><p style="line-height:0.3" align="center">Beam</p><p align="center">Status</p></body></html>',
|
||||
)
|
||||
)
|
||||
self.label_21.setText(
|
||||
_translate(
|
||||
"MainWindow",
|
||||
'<html><head/><body><p style="line-height:0.3" align="center">Connection</p><p align="center">Status</p></body></html>',
|
||||
)
|
||||
)
|
||||
self.label_32.setText(_translate("MainWindow", "current"))
|
||||
self.c_live_seq_name.setItemText(0, _translate("MainWindow", "Empty"))
|
||||
self.label_20.setText(_translate("MainWindow", "DeviceConfig"))
|
||||
@ -696,4 +718,6 @@ class Ui_MainWindow(object):
|
||||
self.b_probe_single.setText(_translate("MainWindow", "Single"))
|
||||
self.b_probe_orig.setText(_translate("MainWindow", "Orig"))
|
||||
self.l_probe_angle.setText(_translate("MainWindow", "TextLabel"))
|
||||
|
||||
|
||||
from flandre.pyqt.QJumpSlider import QJumpSlider
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
from PyQt6.QtWidgets import *
|
||||
from PyQt6.QtCore import Qt
|
||||
from PyQt6.QtWidgets import *
|
||||
|
||||
|
||||
class ProxyStyle(QProxyStyle):
|
||||
@ -12,4 +12,4 @@ class ProxyStyle(QProxyStyle):
|
||||
class QJumpSlider(QSlider):
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self.setStyle(ProxyStyle())
|
||||
self.setStyle(ProxyStyle())
|
||||
|
||||
@ -6,7 +6,7 @@ from flandre.nodes.Node import Node
|
||||
|
||||
|
||||
class ZMQReceiver(QtCore.QObject):
|
||||
zmq_event = QtCore.pyqtSignal('QByteArray')
|
||||
zmq_event = QtCore.pyqtSignal("QByteArray")
|
||||
|
||||
def start(self):
|
||||
threading.Thread(target=self._execute, daemon=True).start()
|
||||
|
||||
@ -31,12 +31,16 @@ class ImagingConfig:
|
||||
@staticmethod
|
||||
def from_file(input_format, focus_mode, folder=None):
|
||||
try:
|
||||
return ImagingConfig(**json.load((folder / f'icfg_{input_format}_{focus_mode}.json').open()))
|
||||
except Exception as e:
|
||||
return ImagingConfig(
|
||||
**json.load((folder / f"icfg_{input_format}_{focus_mode}.json").open())
|
||||
)
|
||||
except Exception:
|
||||
return ImagingConfig()
|
||||
|
||||
def save(self, input_format: str, focus_mode: str, folder=None):
|
||||
json.dump(self.__dict__, (folder / f'icfg_{input_format}_{focus_mode}.json').open('w'))
|
||||
json.dump(
|
||||
self.__dict__, (folder / f"icfg_{input_format}_{focus_mode}.json").open("w")
|
||||
)
|
||||
|
||||
|
||||
@dataclasses.dataclass
|
||||
|
||||
@ -2,12 +2,12 @@ import dataclasses
|
||||
import json
|
||||
import struct
|
||||
import time
|
||||
from enum import auto, Enum
|
||||
from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from flandre.utils.RfFrame import RfFrame, RfFrameMemory, RfFrameFile
|
||||
from flandre.utils.RfFrame import RfFrame, RfFrameFile, RfFrameMemory
|
||||
from flandre.utils.RfMeta import RfFrameMeta, RfSequenceMeta
|
||||
from flandre.utils.RfSequence import RfSequence
|
||||
|
||||
@ -71,44 +71,44 @@ class Msg:
|
||||
self.ts = time.time_ns()
|
||||
|
||||
@classmethod
|
||||
def decode_base(cls, data: bytes) -> 'Msg':
|
||||
def decode_base(cls, data: bytes) -> "Msg":
|
||||
j = json.loads(data.decode())
|
||||
j2 = j.copy()
|
||||
if 'ts' in j:
|
||||
del j2['ts']
|
||||
if "ts" in j:
|
||||
del j2["ts"]
|
||||
c = cls(**j2)
|
||||
if 'ts' in j:
|
||||
c.ts = j['ts']
|
||||
if "ts" in j:
|
||||
c.ts = j["ts"]
|
||||
return c
|
||||
|
||||
def encode_ts(self):
|
||||
if not hasattr(self, 'ts'):
|
||||
if not hasattr(self, "ts"):
|
||||
self.ts = time.time_ns()
|
||||
return struct.pack('Q', self.ts)
|
||||
return struct.pack("Q", self.ts)
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return json.dumps(self.__dict__).encode()
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data: bytes) -> 'Msg':
|
||||
def decode(cls, data: bytes) -> "Msg":
|
||||
return cls.decode_base(data)
|
||||
|
||||
@classmethod
|
||||
def decode_msg(cls, msg: bytes):
|
||||
magic, eid, ts = struct.unpack('IIQ', msg[:16])
|
||||
magic, eid, ts = struct.unpack("IIQ", msg[:16])
|
||||
assert magic == cls.MAGIC
|
||||
class_: 'Msg' = globals()[BG(eid).name]
|
||||
class_: "Msg" = globals()[BG(eid).name]
|
||||
msg2 = class_.decode(msg[16:])
|
||||
msg2.ts = ts
|
||||
return msg2
|
||||
|
||||
@classmethod
|
||||
def eid(cls):
|
||||
return struct.pack('I', BG[cls.__name__].value)
|
||||
return struct.pack("I", BG[cls.__name__].value)
|
||||
|
||||
@classmethod
|
||||
def magic(cls):
|
||||
return struct.pack('I', cls.MAGIC)
|
||||
return struct.pack("I", cls.MAGIC)
|
||||
|
||||
def encode_msg(self):
|
||||
return self.magic() + self.eid() + self.encode_ts() + self.encode()
|
||||
@ -116,7 +116,7 @@ class Msg:
|
||||
@property
|
||||
def dict(self):
|
||||
d = self.__dict__.copy()
|
||||
d['type'] = self.__class__.__name__
|
||||
d["type"] = self.__class__.__name__
|
||||
return d
|
||||
|
||||
|
||||
@ -128,17 +128,17 @@ class HeaderByteMsg(Msg):
|
||||
self.header = header
|
||||
self.data = data
|
||||
if self.data is None:
|
||||
self.data = b''
|
||||
self.data = b""
|
||||
|
||||
def encode(self) -> bytes:
|
||||
e = json.dumps(self.header).encode()
|
||||
return struct.pack('I', e.__len__()) + e + self.data
|
||||
return struct.pack("I", e.__len__()) + e + self.data
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data: bytes) -> 'HeaderByteMsg':
|
||||
header_len = struct.unpack('I', data[:4])[0]
|
||||
header = json.loads(data[4:4 + header_len])
|
||||
data = data[4 + header_len:]
|
||||
def decode(cls, data: bytes) -> "HeaderByteMsg":
|
||||
header_len = struct.unpack("I", data[:4])[0]
|
||||
header = json.loads(data[4 : 4 + header_len])
|
||||
data = data[4 + header_len :]
|
||||
return HeaderByteMsg(header, data)
|
||||
|
||||
|
||||
@ -154,7 +154,7 @@ class RefreshDeviceMsg(Msg):
|
||||
|
||||
@dataclasses.dataclass
|
||||
class KillMsg(Msg):
|
||||
name: str = ''
|
||||
name: str = ""
|
||||
|
||||
|
||||
@dataclasses.dataclass
|
||||
@ -170,7 +170,7 @@ class NodeOnlineMsg(Msg):
|
||||
|
||||
@dataclasses.dataclass
|
||||
class StrMsg(Msg):
|
||||
value: str = ''
|
||||
value: str = ""
|
||||
|
||||
|
||||
@dataclasses.dataclass
|
||||
@ -227,8 +227,8 @@ class MaxMsg(IntMsg):
|
||||
@dataclasses.dataclass
|
||||
class SetRecordMsg(Msg):
|
||||
enable: bool
|
||||
commit: str = ''
|
||||
base: str = ''
|
||||
commit: str = ""
|
||||
base: str = ""
|
||||
|
||||
|
||||
@dataclasses.dataclass
|
||||
@ -308,16 +308,13 @@ class ImageArgMsg(Msg):
|
||||
|
||||
@staticmethod
|
||||
def from_path(p: Path):
|
||||
return ImageArgMsg(
|
||||
sender='load',
|
||||
**json.loads(p.read_text())
|
||||
)
|
||||
return ImageArgMsg(sender="load", **json.loads(p.read_text()))
|
||||
|
||||
def json(self):
|
||||
arg_d = dict()
|
||||
for field in dataclasses.fields(ImageArgMsg):
|
||||
match field.name:
|
||||
case 'sender':
|
||||
case "sender":
|
||||
pass
|
||||
case _:
|
||||
arg_d[field.name] = self.__getattribute__(field.name)
|
||||
@ -346,14 +343,11 @@ class BMMsg(Msg):
|
||||
self.t = t
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return struct.pack('I', self.t) + self.data
|
||||
return struct.pack("I", self.t) + self.data
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data: bytes) -> 'Msg':
|
||||
return cls(
|
||||
struct.unpack('I', data[:4])[0],
|
||||
data[4:]
|
||||
)
|
||||
def decode(cls, data: bytes) -> "Msg":
|
||||
return cls(struct.unpack("I", data[:4])[0], data[4:])
|
||||
|
||||
|
||||
class RfFrameMsg(HeaderByteMsg):
|
||||
@ -361,80 +355,90 @@ class RfFrameMsg(HeaderByteMsg):
|
||||
self.sender = sender
|
||||
self.rf_frame = rf_frame
|
||||
if isinstance(rf_frame, RfFrameFile):
|
||||
super().__init__(dict(
|
||||
sender=sender,
|
||||
type='RfFrameFile',
|
||||
frame_meta=rf_frame.meta.name,
|
||||
frame_filename=rf_frame.filename,
|
||||
seq_path=rf_frame.seq.path.__str__(),
|
||||
), rf_frame.data)
|
||||
super().__init__(
|
||||
dict(
|
||||
sender=sender,
|
||||
type="RfFrameFile",
|
||||
frame_meta=rf_frame.meta.name,
|
||||
frame_filename=rf_frame.filename,
|
||||
seq_path=rf_frame.seq.path.__str__(),
|
||||
),
|
||||
rf_frame.data,
|
||||
)
|
||||
elif isinstance(rf_frame, RfFrameMemory):
|
||||
super().__init__(dict(
|
||||
sender=sender,
|
||||
type='RfFrameMemory',
|
||||
frame_meta=rf_frame.meta.name,
|
||||
seq_meta=rf_frame.seq_meta.name,
|
||||
), rf_frame.data)
|
||||
super().__init__(
|
||||
dict(
|
||||
sender=sender,
|
||||
type="RfFrameMemory",
|
||||
frame_meta=rf_frame.meta.name,
|
||||
seq_meta=rf_frame.seq_meta.name,
|
||||
),
|
||||
rf_frame.data,
|
||||
)
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data: bytes) -> 'RfFrameMsg':
|
||||
def decode(cls, data: bytes) -> "RfFrameMsg":
|
||||
# return RfFrameMemory(RfFrameMeta(),RfSequenceMeta(),b'')
|
||||
msg = super(RfFrameMsg, cls).decode(data)
|
||||
if msg.header['type'] == 'RfFrameFile':
|
||||
if msg.header["type"] == "RfFrameFile":
|
||||
return RfFrameMsg(
|
||||
msg.header['sender'],
|
||||
msg.header["sender"],
|
||||
RfFrameFile(
|
||||
RfFrameMeta.from_name(msg.header['frame_meta']),
|
||||
RfSequence(msg.header['seq_path']),
|
||||
RfFrameMeta.from_name(msg.header["frame_meta"]),
|
||||
RfSequence(msg.header["seq_path"]),
|
||||
msg.data if msg.data.__len__() > 0 else None,
|
||||
msg.header['frame_filename']
|
||||
))
|
||||
elif msg.header['type'] == 'RfFrameMemory':
|
||||
msg.header["frame_filename"],
|
||||
),
|
||||
)
|
||||
elif msg.header["type"] == "RfFrameMemory":
|
||||
return RfFrameMsg(
|
||||
msg.header['sender'],
|
||||
msg.header["sender"],
|
||||
RfFrameMemory(
|
||||
RfFrameMeta.from_name(msg.header['frame_meta']),
|
||||
RfSequenceMeta.from_name(msg.header['seq_meta']),
|
||||
RfFrameMeta.from_name(msg.header["frame_meta"]),
|
||||
RfSequenceMeta.from_name(msg.header["seq_meta"]),
|
||||
msg.data,
|
||||
))
|
||||
),
|
||||
)
|
||||
raise NotImplementedError()
|
||||
|
||||
|
||||
class RGB888Msg(HeaderByteMsg):
|
||||
|
||||
def __init__(self, b: bytes, w: int, h: int):
|
||||
self.w = w
|
||||
self.h = h
|
||||
super().__init__(dict(h=h, w=w), b)
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data) -> 'RGB888Msg':
|
||||
def decode(cls, data) -> "RGB888Msg":
|
||||
msg = super(RGB888Msg, cls).decode(data)
|
||||
return RGB888Msg(msg.data, msg.header['w'], msg.header['h'])
|
||||
return RGB888Msg(msg.data, msg.header["w"], msg.header["h"])
|
||||
|
||||
|
||||
class RfMatMsg(HeaderByteMsg):
|
||||
|
||||
def __init__(self, rfmat: 'RfMat'):
|
||||
def __init__(self, rfmat: "RfMat"):
|
||||
self.rfmat = rfmat
|
||||
|
||||
super().__init__(dict(
|
||||
frame_meta=rfmat.frame_meta.name,
|
||||
seq_meta=rfmat.seq_meta.name,
|
||||
data_shape=rfmat.m.shape,
|
||||
dtype=str(rfmat.m.dtype),
|
||||
), rfmat.m.tobytes())
|
||||
super().__init__(
|
||||
dict(
|
||||
frame_meta=rfmat.frame_meta.name,
|
||||
seq_meta=rfmat.seq_meta.name,
|
||||
data_shape=rfmat.m.shape,
|
||||
dtype=str(rfmat.m.dtype),
|
||||
),
|
||||
rfmat.m.tobytes(),
|
||||
)
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data) -> 'RfMatMsg':
|
||||
def decode(cls, data) -> "RfMatMsg":
|
||||
from flandre.utils.RfMat import RfMat
|
||||
|
||||
msg = super(RfMatMsg, cls).decode(data)
|
||||
dt = np.dtype(msg.header['dtype'])
|
||||
mat = np.frombuffer(msg.data, dtype=dt).reshape(msg.header['data_shape'])
|
||||
dt = np.dtype(msg.header["dtype"])
|
||||
mat = np.frombuffer(msg.data, dtype=dt).reshape(msg.header["data_shape"])
|
||||
rfmat = RfMat(
|
||||
mat,
|
||||
RfFrameMeta.from_name(msg.header['frame_meta']),
|
||||
RfSequenceMeta.from_name(msg.header['seq_meta']),
|
||||
RfFrameMeta.from_name(msg.header["frame_meta"]),
|
||||
RfSequenceMeta.from_name(msg.header["seq_meta"]),
|
||||
)
|
||||
return RfMatMsg(rfmat)
|
||||
|
||||
@ -445,19 +449,19 @@ class RfMatMsg(HeaderByteMsg):
|
||||
# sender: int
|
||||
# meta: str
|
||||
# is_zip: bool
|
||||
#
|
||||
#
|
||||
# def __init__(self, sender: int, meta: RfFrameMeta, data: bytes, is_zip: bool = False):
|
||||
# self._header = self.Header(sender, meta.name, is_zip)
|
||||
# super().__init__(self._header.__dict__, data)
|
||||
#
|
||||
#
|
||||
# @property
|
||||
# def sender(self) -> int:
|
||||
# return self._header.sender
|
||||
#
|
||||
#
|
||||
# @property
|
||||
# def meta(self) -> RfFrameMeta:
|
||||
# return RfFrameMeta.from_name(self._header.meta)
|
||||
#
|
||||
#
|
||||
# @classmethod
|
||||
# def decode(cls, data) -> 'RfFrameWithMetaMsg':
|
||||
# msg = super(RfFrameWithMetaMsg, cls).decode(data)
|
||||
@ -473,7 +477,7 @@ class BytesMsg(Msg):
|
||||
return self.value
|
||||
|
||||
@classmethod
|
||||
def decode(cls, data: bytes) -> 'Msg':
|
||||
def decode(cls, data: bytes) -> "Msg":
|
||||
return cls(data)
|
||||
|
||||
def split(self):
|
||||
@ -528,13 +532,21 @@ class RobotRtsiMsg(Msg):
|
||||
def from_meta(meta: RfFrameMeta):
|
||||
return RobotRtsiMsg(
|
||||
pos=(
|
||||
meta.robot_x, meta.robot_y, meta.robot_z,
|
||||
meta.robot_roll, meta.robot_pitch, meta.robot_yal,
|
||||
meta.robot_x,
|
||||
meta.robot_y,
|
||||
meta.robot_z,
|
||||
meta.robot_roll,
|
||||
meta.robot_pitch,
|
||||
meta.robot_yal,
|
||||
),
|
||||
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_x,
|
||||
meta.robot_force_y,
|
||||
meta.robot_force_z,
|
||||
meta.robot_force_roll,
|
||||
meta.robot_force_pitch,
|
||||
meta.robot_force_yal,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
@ -545,14 +557,14 @@ class RequestRfFrameMsg(Msg):
|
||||
def test():
|
||||
values = set(item.name for item in BG)
|
||||
for k in globals().keys():
|
||||
if k.endswith('Msg') and k not in ['Msg', 'BoolMsg']:
|
||||
if k.endswith("Msg") and k not in ["Msg", "BoolMsg"]:
|
||||
if k not in values:
|
||||
raise RuntimeError(f"Unknown msg type: {k}")
|
||||
|
||||
|
||||
test()
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
# c = HeaderByteMsg(dict(a=1, b='s'), b'asdasd')
|
||||
# c2 = c.decode(c.encode())
|
||||
# print(c2.header, c2.data)
|
||||
|
||||
@ -11,7 +11,13 @@ class RfFrame:
|
||||
|
||||
|
||||
class RfFrameFile(RfFrame):
|
||||
def __init__(self, meta: RfFrameMeta, seq: RfSequence, data: bytes = None, filename: str = None):
|
||||
def __init__(
|
||||
self,
|
||||
meta: RfFrameMeta,
|
||||
seq: RfSequence,
|
||||
data: bytes = None,
|
||||
filename: str = None,
|
||||
):
|
||||
self.seq = seq
|
||||
self.meta = meta
|
||||
self.data = data
|
||||
@ -21,11 +27,12 @@ class RfFrameFile(RfFrame):
|
||||
if self.data is not None:
|
||||
return self.data
|
||||
match self.seq.type:
|
||||
case 'zip':
|
||||
case "zip":
|
||||
from flandre.utils.archive import zip_to_bytes
|
||||
|
||||
if self.filename is not None:
|
||||
return zip_to_bytes(self.seq.path, int(Path(self.filename).stem))
|
||||
case 'dir':
|
||||
case "dir":
|
||||
return (self.seq.path / self.filename).read_bytes()
|
||||
raise NotImplementedError()
|
||||
|
||||
@ -47,11 +54,11 @@ def b2t(b: bytes):
|
||||
# buffer = b[4 + 8 + 8 + 8 + 4:]
|
||||
# return seq, encoder, host_ts, driver_ts, buffer
|
||||
|
||||
magic, seq, encoder, host_ts, device_ts = struct.unpack_from('iQiQQ', b)
|
||||
magic, seq, encoder, host_ts, device_ts = struct.unpack_from("iQiQQ", b)
|
||||
# bb = b[4 + 8 + 4 + 8:4 + 8 + 4 + 8 + 4]
|
||||
# device_ts_low = struct.unpack('<I', bb)
|
||||
# device_ts_low = b[4 + 8 + 4 + 8:4 + 8 + 4 + 8 + 4]
|
||||
# print(bin(device_ts_low[0]),bin(device_ts_low[1]),bin(device_ts_low[2]),bin(device_ts_low[3]))
|
||||
buffer = b[4 + 8 + 4 + 8 + 4 + 4+8:]
|
||||
buffer = b[4 + 8 + 4 + 8 + 4 + 4 + 8 :]
|
||||
|
||||
return seq, encoder, host_ts, device_ts, buffer
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
import inspect
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import scipy
|
||||
import scipy.signal
|
||||
|
||||
# from cupyx.scipy.fft import dctn, idctn
|
||||
from scipy.stats import norm as norms
|
||||
|
||||
@ -31,12 +31,14 @@ def hsv_to_rgb(hsv):
|
||||
|
||||
# check length of the last dimension, should be _some_ sort of rgb
|
||||
if hsv.shape[-1] != 3:
|
||||
raise ValueError("Last dimension of input array must be 3; "
|
||||
f"shape {hsv.shape} was found.")
|
||||
raise ValueError(
|
||||
f"Last dimension of input array must be 3; shape {hsv.shape} was found."
|
||||
)
|
||||
|
||||
in_shape = hsv.shape
|
||||
hsv = np.array(
|
||||
hsv, copy=False,
|
||||
hsv,
|
||||
copy=False,
|
||||
dtype=np.promote_types(hsv.dtype, np.float32), # Don't work on ints.
|
||||
ndmin=2, # In case input was 1D.
|
||||
)
|
||||
@ -97,10 +99,10 @@ def hsv_to_rgb(hsv):
|
||||
|
||||
def bypass(f):
|
||||
def wrapper(self, *args, **kwargs):
|
||||
if 'cond' not in kwargs:
|
||||
if "cond" not in kwargs:
|
||||
return f(self, *args, **kwargs)
|
||||
if kwargs['cond']:
|
||||
del kwargs['cond']
|
||||
if kwargs["cond"]:
|
||||
del kwargs["cond"]
|
||||
return f(self, *args, **kwargs)
|
||||
else:
|
||||
return self
|
||||
@ -117,7 +119,7 @@ def bypassClass(original_class):
|
||||
@bypassClass
|
||||
class RfMat:
|
||||
@staticmethod
|
||||
def from_rf_frame(frame: RfFrame, device='cpu'):
|
||||
def from_rf_frame(frame: RfFrame, device="cpu"):
|
||||
if isinstance(frame, RfFrameFile):
|
||||
seq_meta = frame.seq.meta
|
||||
elif isinstance(frame, RfFrameMemory):
|
||||
@ -126,25 +128,31 @@ class RfMat:
|
||||
raise NotImplementedError()
|
||||
if seq_meta.prod() != frame.__bytes__().__len__() // 2:
|
||||
return None
|
||||
m = np.frombuffer(frame.__bytes__(), dtype=np.int16).reshape(seq_meta.shape).copy()
|
||||
if device == 'gpu':
|
||||
m = (
|
||||
np.frombuffer(frame.__bytes__(), dtype=np.int16)
|
||||
.reshape(seq_meta.shape)
|
||||
.copy()
|
||||
)
|
||||
if device == "gpu":
|
||||
import cupy as cp
|
||||
|
||||
m = cp.asarray(m)
|
||||
return RfMat(m, frame.meta, seq_meta)
|
||||
|
||||
def __init__(self,
|
||||
data: np.ndarray,
|
||||
frame_meta: RfFrameMeta = None,
|
||||
seq_meta: RfSequenceMeta = None,
|
||||
):
|
||||
def __init__(
|
||||
self,
|
||||
data: np.ndarray,
|
||||
frame_meta: RfFrameMeta = None,
|
||||
seq_meta: RfSequenceMeta = None,
|
||||
):
|
||||
self.m = data
|
||||
self.cv = False
|
||||
self.frame_meta = frame_meta
|
||||
self.seq_meta = seq_meta
|
||||
if isinstance(data, np.ndarray):
|
||||
self.device = 'cpu'
|
||||
elif type(data).__module__ == 'cupy' and type(data).__name__ == 'ndarray':
|
||||
self.device = 'gpu'
|
||||
self.device = "cpu"
|
||||
elif type(data).__module__ == "cupy" and type(data).__name__ == "ndarray":
|
||||
self.device = "gpu"
|
||||
else:
|
||||
raise NotImplementedError
|
||||
|
||||
@ -174,16 +182,17 @@ class RfMat:
|
||||
|
||||
@property
|
||||
def p(self):
|
||||
if self.device == 'cpu':
|
||||
if self.device == "cpu":
|
||||
return np
|
||||
import cupy as cp
|
||||
|
||||
return cp
|
||||
|
||||
def __bytes__(self):
|
||||
return self.m.tobytes()
|
||||
|
||||
def init_cv(self):
|
||||
cv2.namedWindow('image')
|
||||
cv2.namedWindow("image")
|
||||
self.cv = True
|
||||
|
||||
def norm(self):
|
||||
@ -206,14 +215,14 @@ class RfMat:
|
||||
s = p.zeros_like(h) + 1
|
||||
v = p.zeros_like(h) + 1
|
||||
hsv = p.stack((h, s, v), axis=2)
|
||||
if self.device == 'gpu':
|
||||
if self.device == "gpu":
|
||||
rgb = hsv_to_rgb(hsv.get())
|
||||
else:
|
||||
rgb = hsv_to_rgb(hsv)
|
||||
return self.copy((rgb * 255).astype(np.uint8))
|
||||
|
||||
def cpu(self):
|
||||
if self.device == 'cpu':
|
||||
if self.device == "cpu":
|
||||
return self
|
||||
return self.copy(self.m.get())
|
||||
|
||||
@ -236,8 +245,8 @@ class RfMat:
|
||||
canvas = np.zeros(self.m.shape, dtype=np.uint8)
|
||||
ccp = self.copy()
|
||||
|
||||
line1 = ''
|
||||
line2 = ''
|
||||
line1 = ""
|
||||
line2 = ""
|
||||
if watermark is not None:
|
||||
line1 = watermark
|
||||
else:
|
||||
@ -257,8 +266,12 @@ class RfMat:
|
||||
cv2.putText(canvas, line1, (0, 60), cv2.FONT_HERSHEY_PLAIN, fontsize, color1, 8)
|
||||
cv2.putText(canvas, line1, (0, 60), cv2.FONT_HERSHEY_PLAIN, fontsize, color2, 4)
|
||||
|
||||
cv2.putText(canvas, line2, (0, 120), cv2.FONT_HERSHEY_PLAIN, fontsize, color1, 8)
|
||||
cv2.putText(canvas, line2, (0, 120), cv2.FONT_HERSHEY_PLAIN, fontsize, color2, 4)
|
||||
cv2.putText(
|
||||
canvas, line2, (0, 120), cv2.FONT_HERSHEY_PLAIN, fontsize, color1, 8
|
||||
)
|
||||
cv2.putText(
|
||||
canvas, line2, (0, 120), cv2.FONT_HERSHEY_PLAIN, fontsize, color2, 4
|
||||
)
|
||||
|
||||
if canvas.shape.__len__() == 2:
|
||||
ccp.m[canvas == 255] = 255
|
||||
@ -273,22 +286,24 @@ class RfMat:
|
||||
def show(self, shape=None, watermark=None):
|
||||
if not self.cv:
|
||||
self.init_cv()
|
||||
cv2.imshow('image', self
|
||||
.grey()
|
||||
.cpu()
|
||||
.resize(shape, bypass=shape is None)
|
||||
.watermark(watermark).m
|
||||
)
|
||||
cv2.imshow(
|
||||
"image",
|
||||
self.grey()
|
||||
.cpu()
|
||||
.resize(shape, bypass=shape is None)
|
||||
.watermark(watermark)
|
||||
.m,
|
||||
)
|
||||
return cv2.waitKey(0)
|
||||
|
||||
def info(self):
|
||||
print(f'shape: {self.m.shape},device: {self.device}')
|
||||
print(f"shape: {self.m.shape},device: {self.device}")
|
||||
print(self.frame_meta)
|
||||
print(self.seq_meta)
|
||||
return self
|
||||
|
||||
def resize(self, shape):
|
||||
if self.device == 'cpu':
|
||||
if self.device == "cpu":
|
||||
return self.copy(cv2.resize(self.m, shape))
|
||||
raise NotImplementedError()
|
||||
|
||||
@ -298,8 +313,9 @@ class RfMat:
|
||||
def dct(self, mmin, mmax):
|
||||
dct_ = scipy.fft.dct
|
||||
idct = scipy.fft.idct
|
||||
if self.device == 'gpu':
|
||||
if self.device == "gpu":
|
||||
import cupyx.scipy.fft
|
||||
|
||||
dct_ = cupyx.scipy.fft.dct
|
||||
idct = cupyx.scipy.fft.idct
|
||||
m_dct = dct_(self.m)
|
||||
@ -322,8 +338,9 @@ class RfMat:
|
||||
arg = scipy.signal.argrelextrema
|
||||
m = self.m
|
||||
p = self.p
|
||||
if self.device == 'gpu':
|
||||
if self.device == "gpu":
|
||||
import cupyx.scipy.signal
|
||||
|
||||
arg = cupyx.scipy.signal.argrelextrema
|
||||
rm = p.zeros_like(m)
|
||||
indies1 = arg(m, p.greater, axis=axis)
|
||||
@ -342,13 +359,14 @@ class RfMat:
|
||||
cv = scipy.ndimage.convolve1d
|
||||
m = self.m
|
||||
p = self.p
|
||||
if self.device == 'gpu':
|
||||
if self.device == "gpu":
|
||||
import cupyx.scipy.ndimage
|
||||
|
||||
cv = cupyx.scipy.ndimage.convolve1d
|
||||
rv = norms(loc=0, scale=b)
|
||||
x2 = np.arange(-1, 1.1, 0.1)
|
||||
w = rv.pdf(x2)
|
||||
if self.device == 'gpu':
|
||||
if self.device == "gpu":
|
||||
w = p.asarray(w)
|
||||
rm = cv(m, w, axis=axis)
|
||||
return self.copy(rm)
|
||||
@ -365,11 +383,11 @@ class RfMat:
|
||||
addend = self.p.zeros((1, h), dtype=np.int64)
|
||||
addend[:, start:] = self.p.arange(h - start) * scale
|
||||
self.m[:, start:] *= addend
|
||||
s2 = self.m[:, h - 500:]
|
||||
m1 = self.m[:, h - 500:].max()
|
||||
s2 = self.m[:, h - 500 :]
|
||||
m1 = self.m[:, h - 500 :].max()
|
||||
m2 = m1 * scale
|
||||
s2[s2 > m2] = m2
|
||||
self.m[:, h - 500:] = s2 * (1 / scale)
|
||||
self.m[:, h - 500 :] = s2 * (1 / scale)
|
||||
return self
|
||||
|
||||
def time_gain_compensation(self, scales: list[float]):
|
||||
@ -379,10 +397,10 @@ class RfMat:
|
||||
ssss = 0
|
||||
mmax = self.m.max()
|
||||
for scale in scales:
|
||||
s2 = self.m[:, ssss:ssss + block]
|
||||
s2 = self.m[:, ssss : ssss + block]
|
||||
new_max = mmax * scale
|
||||
s2[s2 > new_max] = new_max
|
||||
self.m[:, ssss:ssss + block] = s2 * (mmax / new_max)
|
||||
self.m[:, ssss : ssss + block] = s2 * (mmax / new_max)
|
||||
ssss += block
|
||||
return self
|
||||
|
||||
@ -396,7 +414,9 @@ class RfMat:
|
||||
self.m = s2 * (mmax / new_max)
|
||||
return self
|
||||
|
||||
def time_gain_compensation_linear_max(self, scale: float, mmax: int | None = None, start: int = 0):
|
||||
def time_gain_compensation_linear_max(
|
||||
self, scale: float, mmax: int | None = None, start: int = 0
|
||||
):
|
||||
if scale == 0:
|
||||
return self
|
||||
if mmax is None:
|
||||
@ -408,15 +428,16 @@ class RfMat:
|
||||
for i in range(h):
|
||||
# a[1, a[1, :] > 99] = 99
|
||||
self.m[self.m[:, i] > mmax_arr[i], i] = mmax_arr[i]
|
||||
self.m[:, i] *= (mmax / mmax_arr[i])
|
||||
self.m[:, i] *= mmax / mmax_arr[i]
|
||||
self.m[self.m > mmax] = mmax
|
||||
self.m = self.m.astype(np.int64)
|
||||
return self
|
||||
|
||||
def jupyter(self, figsize=(40, 20), aspect=None):
|
||||
from matplotlib import pyplot as plt
|
||||
|
||||
plt.figure(figsize=figsize)
|
||||
plt.imshow(self.m, cmap='grey', aspect=aspect)
|
||||
plt.imshow(self.m, cmap="grey", aspect=aspect)
|
||||
|
||||
def png(self, path: Path, color=(255, 0, 0), pre=0):
|
||||
canvas = np.zeros((self.h, self.w, 4), dtype=np.uint8)
|
||||
@ -426,5 +447,5 @@ class RfMat:
|
||||
cv2.imwrite(str(path), canvas)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
cp.zeros((1, 2, 3)) + 1
|
||||
|
||||
@ -3,7 +3,7 @@ from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
from typing import Annotated, get_type_hints
|
||||
|
||||
COMMIT_KEY = 'COMMIT'
|
||||
COMMIT_KEY = "COMMIT"
|
||||
|
||||
|
||||
class RfMeta:
|
||||
@ -29,22 +29,22 @@ class RfMeta:
|
||||
if COMMIT_KEY in a2p:
|
||||
cp = a2p[COMMIT_KEY]
|
||||
del p2a[cp]
|
||||
arr.append(f'{self.__getattribute__(cp)}')
|
||||
arr.append(f"{self.__getattribute__(cp)}")
|
||||
for p in p2a:
|
||||
t = p2t[p]
|
||||
v = self.__getattribute__(p)
|
||||
if issubclass(t, Enum):
|
||||
vs = v.name
|
||||
elif issubclass(t, tuple):
|
||||
vs = f'({' '.join([str(vv) for vv in v])})'
|
||||
vs = f"({' '.join([str(vv) for vv in v])})"
|
||||
else:
|
||||
vs = str(v)
|
||||
if v is not None:
|
||||
arr.append(f'{p2a[p]}={vs}')
|
||||
arr.append(f"{p2a[p]}={vs}")
|
||||
return ",".join(arr)
|
||||
|
||||
def filename(self, ts=0):
|
||||
return self.name + f'.{ts}bin'
|
||||
return self.name + f".{ts}bin"
|
||||
|
||||
@classmethod
|
||||
def from_path(clz, path: Path | str):
|
||||
@ -60,22 +60,22 @@ class RfMeta:
|
||||
|
||||
@classmethod
|
||||
def from_name(clz, name: str):
|
||||
if Path(name).suffix == '.zip':
|
||||
if Path(name).suffix == ".zip":
|
||||
name = Path(name).stem
|
||||
p2t = clz.p2t()
|
||||
a2p = clz.a2p()
|
||||
c = clz()
|
||||
sp = name.split(',')
|
||||
sp = name.split(",")
|
||||
if COMMIT_KEY in a2p:
|
||||
c.__setattr__(a2p[COMMIT_KEY], sp.pop(0))
|
||||
for pv in sp:
|
||||
a, v = pv.split('=')
|
||||
a, v = pv.split("=")
|
||||
p = a2p[a]
|
||||
t = p2t[p]
|
||||
if issubclass(t, Enum):
|
||||
c.__setattr__(p, t[v])
|
||||
elif issubclass(t, tuple):
|
||||
c.__setattr__(p, tuple(int(i) for i in v[1:-1].split(' ')))
|
||||
c.__setattr__(p, tuple(int(i) for i in v[1:-1].split(" ")))
|
||||
else:
|
||||
c.__setattr__(p, t(v))
|
||||
|
||||
@ -84,25 +84,25 @@ class RfMeta:
|
||||
|
||||
@dataclass
|
||||
class RfFrameMeta(RfMeta):
|
||||
encoder: Annotated[int, 'E'] = None
|
||||
sequence_id: Annotated[int, 'S'] = None # test3
|
||||
robot_x: Annotated[int, 'X'] = None
|
||||
robot_y: Annotated[int, 'Y'] = None
|
||||
robot_z: Annotated[int, 'Z'] = None
|
||||
encoder: Annotated[int, "E"] = None
|
||||
sequence_id: Annotated[int, "S"] = None # test3
|
||||
robot_x: Annotated[int, "X"] = None
|
||||
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_roll: Annotated[int, "RX"] = None
|
||||
robot_pitch: Annotated[int, "RY"] = None
|
||||
robot_yal: 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_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_roll: Annotated[int, "FRX"] = None
|
||||
robot_force_pitch: Annotated[int, "FRY"] = None
|
||||
robot_force_yal: Annotated[int, "FRZ"] = None
|
||||
|
||||
blake2b: Annotated[str, 'B2B'] = None
|
||||
blake2b: Annotated[str, "B2B"] = None
|
||||
|
||||
|
||||
@dataclass
|
||||
@ -112,9 +112,9 @@ class RfSequenceMeta(RfMeta):
|
||||
TFM = auto()
|
||||
|
||||
commit: Annotated[str, COMMIT_KEY] = None
|
||||
shape: Annotated[tuple, 'S'] = None
|
||||
mode: Annotated[RfSequenceMode, 'M'] = RfSequenceMode.PWI
|
||||
us: Annotated[int, 'U'] = None
|
||||
shape: Annotated[tuple, "S"] = None
|
||||
mode: Annotated[RfSequenceMode, "M"] = RfSequenceMode.PWI
|
||||
us: Annotated[int, "U"] = None
|
||||
|
||||
def prod(self):
|
||||
res = 1
|
||||
@ -126,7 +126,7 @@ class RfSequenceMeta(RfMeta):
|
||||
return self.shape.__len__()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
# r = RfSequenceMeta.from_name('asdasd1,S=(64 1501),M=PWI,U=30')
|
||||
# print(r)
|
||||
# print(r.json_str)
|
||||
@ -134,7 +134,7 @@ if __name__ == '__main__':
|
||||
# r2 = RfSequenceMeta.from_json_str(r.json_str)
|
||||
# print(r2)
|
||||
|
||||
r = RfFrameMeta.from_name('E=1,S=123')
|
||||
r = RfFrameMeta.from_name("E=1,S=123")
|
||||
print(r)
|
||||
print(r.json_str)
|
||||
|
||||
|
||||
@ -7,13 +7,14 @@ from flandre.utils.RfMeta import RfFrameMeta, RfSequenceMeta
|
||||
class RfSequence:
|
||||
def __init__(self, path: Path | str):
|
||||
from flandre.utils.RfFrame import RfFrameFile
|
||||
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise FileNotFoundError()
|
||||
if path.is_dir():
|
||||
self.type = 'dir'
|
||||
elif path.suffix == '.zip':
|
||||
self.type = 'zip'
|
||||
self.type = "dir"
|
||||
elif path.suffix == ".zip":
|
||||
self.type = "zip"
|
||||
self.path = path
|
||||
self.meta = RfSequenceMeta.from_name(path.stem)
|
||||
self._frames: list[RfFrameFile] | None = None
|
||||
@ -21,17 +22,22 @@ class RfSequence:
|
||||
@property
|
||||
def frames(self):
|
||||
from flandre.utils.RfFrame import RfFrameFile
|
||||
|
||||
if self._frames is None:
|
||||
arr = []
|
||||
match self.type:
|
||||
case 'zip':
|
||||
case "zip":
|
||||
zip_file = zipfile.ZipFile(self.path)
|
||||
max_idx = max([int(Path(item.filename).stem) for item in zip_file.infolist()])
|
||||
max_idx = max(
|
||||
[int(Path(item.filename).stem) for item in zip_file.infolist()]
|
||||
)
|
||||
for i in range(max_idx + 1):
|
||||
meta = RfFrameMeta.from_name(zip_file.open(f'{i}.meta').read().decode())
|
||||
arr.append(RfFrameFile(meta, seq=self, filename=f'{i}.zst'))
|
||||
case 'dir':
|
||||
for f in self.path.glob('*bin'):
|
||||
meta = RfFrameMeta.from_name(
|
||||
zip_file.open(f"{i}.meta").read().decode()
|
||||
)
|
||||
arr.append(RfFrameFile(meta, seq=self, filename=f"{i}.zst"))
|
||||
case "dir":
|
||||
for f in self.path.glob("*bin"):
|
||||
meta = RfFrameMeta.from_name(f.stem)
|
||||
arr.append(RfFrameFile(meta, seq=self, filename=f.name))
|
||||
self._frames = arr
|
||||
@ -39,7 +45,7 @@ class RfSequence:
|
||||
|
||||
@property
|
||||
def seq_id_minmax(self):
|
||||
mmin = 2 ** 32
|
||||
mmin = 2**32
|
||||
mmax = 0
|
||||
for f in self.frames:
|
||||
mmin = min(mmin, f.meta.sequence_id)
|
||||
|
||||
@ -16,10 +16,10 @@ from scipy.stats import norm as norms
|
||||
|
||||
def skip(f):
|
||||
def wrapper(self, **kwargs):
|
||||
if 'cond' not in kwargs:
|
||||
if "cond" not in kwargs:
|
||||
return f(self, **kwargs)
|
||||
if kwargs['cond']:
|
||||
del kwargs['cond']
|
||||
if kwargs["cond"]:
|
||||
del kwargs["cond"]
|
||||
return f(self, **kwargs)
|
||||
else:
|
||||
return self
|
||||
@ -30,7 +30,9 @@ def skip(f):
|
||||
class ScanData:
|
||||
@staticmethod
|
||||
def from_buffer(buf: bytes | memoryview, shape, p=np):
|
||||
return ScanData(p.frombuffer(buf, dtype=p.int16, count=np.prod(shape)).reshape(shape))
|
||||
return ScanData(
|
||||
p.frombuffer(buf, dtype=p.int16, count=np.prod(shape)).reshape(shape)
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def from_file(filename: Path | str, shape, p=np):
|
||||
@ -47,7 +49,7 @@ class ScanData:
|
||||
elif isinstance(mat, cp.ndarray):
|
||||
self.p = cp
|
||||
else:
|
||||
raise Exception('wrong type mat!!')
|
||||
raise Exception("wrong type mat!!")
|
||||
self.m = mat
|
||||
self.d = mat.shape.__len__()
|
||||
|
||||
@ -199,11 +201,13 @@ class ScanData:
|
||||
return ScanData(self.m[:, start:end])
|
||||
|
||||
def print_contiguous(self):
|
||||
print(self.m.flags['C_CONTIGUOUS'], self.m.flags['F_CONTIGUOUS'])
|
||||
print(self.m.flags["C_CONTIGUOUS"], self.m.flags["F_CONTIGUOUS"])
|
||||
return self
|
||||
|
||||
def rgb(self):
|
||||
return ScanData((self.norm().m * 255).astype(np.uint8)[:, :, None].repeat(3, axis=2))
|
||||
return ScanData(
|
||||
(self.norm().m * 255).astype(np.uint8)[:, :, None].repeat(3, axis=2)
|
||||
)
|
||||
|
||||
def pseudo_color(self):
|
||||
m = self.m * 0.7
|
||||
@ -249,7 +253,9 @@ class ScanData:
|
||||
print(addend)
|
||||
return ScanData(self.m * addend)
|
||||
|
||||
def time_gain_compensation_linear_max(self, scale: float, mmax: int | None = None, start: int = 0):
|
||||
def time_gain_compensation_linear_max(
|
||||
self, scale: float, mmax: int | None = None, start: int = 0
|
||||
):
|
||||
if scale == 0:
|
||||
return self
|
||||
if mmax is None:
|
||||
@ -261,7 +267,7 @@ class ScanData:
|
||||
for i in range(h):
|
||||
# a[1, a[1, :] > 99] = 99
|
||||
self.m[self.m[:, i] > mmax_arr[i], i] = mmax_arr[i]
|
||||
self.m[:, i] *= (mmax / mmax_arr[i])
|
||||
self.m[:, i] *= mmax / mmax_arr[i]
|
||||
self.m[self.m > mmax] = mmax
|
||||
self.m = self.m.astype(np.int64)
|
||||
return self
|
||||
@ -279,8 +285,9 @@ class ScanData:
|
||||
def log(self):
|
||||
return ScanData(self.p.log(self.m))
|
||||
|
||||
def show(self, cmap='grey', w=40, h=20, no_axis=False, hsv=False):
|
||||
def show(self, cmap="grey", w=40, h=20, no_axis=False, hsv=False):
|
||||
from matplotlib import pyplot as plt
|
||||
|
||||
# fig = plt.figure(figsize=(w, h))
|
||||
fig, ax = plt.subplots(figsize=(w, h))
|
||||
if no_axis:
|
||||
@ -294,6 +301,7 @@ class ScanData:
|
||||
|
||||
def plot_center_line(self):
|
||||
from matplotlib import pyplot as plt
|
||||
|
||||
plt.figure(figsize=(40, 5))
|
||||
m = self.m if self.p == np else self.m.get()
|
||||
plt.plot(m[128, :])
|
||||
@ -316,7 +324,7 @@ class ScanData:
|
||||
window = fn(end - start)
|
||||
filter = np.zeros(seq_len) + window[0]
|
||||
filter[start:end] = window
|
||||
filter[seq_len - end:seq_len - start] = window
|
||||
filter[seq_len - end : seq_len - start] = window
|
||||
fft_seq *= filter
|
||||
return ScanData(self.p.real(ifft(fft_seq)))
|
||||
|
||||
@ -335,7 +343,9 @@ class ScanData:
|
||||
print(self.m.shape[0])
|
||||
if self.p != np:
|
||||
raise NotImplementedError()
|
||||
return ScanData(cv2.resize(self.m, (int(self.m.shape[1] * xdyd), self.m.shape[0])))
|
||||
return ScanData(
|
||||
cv2.resize(self.m, (int(self.m.shape[1] * xdyd), self.m.shape[0]))
|
||||
)
|
||||
|
||||
|
||||
def test1():
|
||||
@ -343,5 +353,5 @@ def test1():
|
||||
print(s.m)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
test1()
|
||||
|
||||
@ -1,20 +1,20 @@
|
||||
import hashlib
|
||||
import json
|
||||
import shutil
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
import zipfile
|
||||
import zstd
|
||||
import click
|
||||
from pathlib import Path
|
||||
|
||||
TEMP_FOLDER = Path('/mnt/16T/private_dataset/New Folder/temp')
|
||||
import zstd
|
||||
|
||||
TEMP_FOLDER = Path("/mnt/16T/private_dataset/New Folder/temp")
|
||||
|
||||
|
||||
def folder_to_zip(folder: Path):
|
||||
shutil.rmtree(TEMP_FOLDER)
|
||||
TEMP_FOLDER.mkdir(parents=True, exist_ok=True)
|
||||
from flandre.utils.RfMeta import RfFrameMeta
|
||||
for i, file in enumerate(folder.glob('*')):
|
||||
|
||||
for i, file in enumerate(folder.glob("*")):
|
||||
file = Path(file)
|
||||
|
||||
b2b = hashlib.blake2b(file.read_bytes(), digest_size=4).hexdigest()
|
||||
@ -23,48 +23,65 @@ def folder_to_zip(folder: Path):
|
||||
print(i, b2b, file, meta.json_str)
|
||||
|
||||
src = file
|
||||
dst = TEMP_FOLDER / f'{i}.zst'
|
||||
dstj = TEMP_FOLDER / f'{i}.json'
|
||||
dst = TEMP_FOLDER / f"{i}.zst"
|
||||
dstj = TEMP_FOLDER / f"{i}.json"
|
||||
|
||||
dstj.write_text(meta.json_str)
|
||||
|
||||
subprocess.run(['zstd', '-f', src, '-o', dst])
|
||||
subprocess.run(['zip', '-0', '-j', '-r', TEMP_FOLDER.parent / f'{folder.name}.zip', TEMP_FOLDER])
|
||||
subprocess.run(["zstd", "-f", src, "-o", dst])
|
||||
subprocess.run(
|
||||
[
|
||||
"zip",
|
||||
"-0",
|
||||
"-j",
|
||||
"-r",
|
||||
TEMP_FOLDER.parent / f"{folder.name}.zip",
|
||||
TEMP_FOLDER,
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def to_zip(li: list[tuple[Path, 'RfFrameMeta', list[tuple[Path, str]]]], temp_dst: Path, zipdst: Path):
|
||||
def to_zip(
|
||||
li: list[tuple[Path, "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()
|
||||
meta.blake2b = b2b
|
||||
src = file
|
||||
dst = temp_dst / f'{i}.zst'
|
||||
subprocess.run(['zstd', '-f', src, '-o', dst])
|
||||
dst = temp_dst / f"{i}.zst"
|
||||
subprocess.run(["zstd", "-f", src, "-o", dst])
|
||||
|
||||
dstj = temp_dst / f'{i}.meta'
|
||||
dstj = temp_dst / f"{i}.meta"
|
||||
dstj.write_text(meta.name)
|
||||
|
||||
for srcf, suffix in farr:
|
||||
dstf = temp_dst / f'{i}{suffix}'
|
||||
dstf = temp_dst / f"{i}{suffix}"
|
||||
shutil.copy(srcf, dstf)
|
||||
subprocess.run(['zip', '-0', '-j', '-r', zipdst, temp_dst])
|
||||
subprocess.run(["zip", "-0", "-j", "-r", zipdst, temp_dst])
|
||||
|
||||
|
||||
def zip_to_bytes(file: Path, name: int):
|
||||
return zstd.loads(zipfile.ZipFile(file).open(f'{name}.zst').read())
|
||||
return zstd.loads(zipfile.ZipFile(file).open(f"{name}.zst").read())
|
||||
|
||||
|
||||
def zip_to_bytes2(file: Path, name: str):
|
||||
return zstd.loads(zipfile.ZipFile(file).open(name).read())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
from flandre.utils.RfMeta import RfFrameMeta
|
||||
|
||||
# folder_to_zip()
|
||||
arr = []
|
||||
for i, f in enumerate(Path('/mnt/16T/private_dataset/New Folder/T1,U=30,M=FMC,S=(256 256 4502)').glob('*')):
|
||||
for i, f in enumerate(
|
||||
Path("/mnt/16T/private_dataset/New Folder/T1,U=30,M=FMC,S=(256 256 4502)").glob(
|
||||
"*"
|
||||
)
|
||||
):
|
||||
m = RfFrameMeta.from_path(f)
|
||||
m.encoder = 0
|
||||
arr.append((Path(f), m, []))
|
||||
@ -72,7 +89,11 @@ if __name__ == '__main__':
|
||||
for i, e in enumerate(arr):
|
||||
e[1].sequence_id = i
|
||||
|
||||
to_zip(arr, TEMP_FOLDER, Path('/mnt/16T/private_dataset/T1,U=30,M=TFM,S=(256 256 4502)'))
|
||||
to_zip(
|
||||
arr,
|
||||
TEMP_FOLDER,
|
||||
Path("/mnt/16T/private_dataset/T1,U=30,M=TFM,S=(256 256 4502)"),
|
||||
)
|
||||
# to_zip()
|
||||
# pass
|
||||
# cli()
|
||||
|
||||
@ -6,7 +6,9 @@ class MiSwitch:
|
||||
self.protocol = MiIOProtocol(ip, token)
|
||||
|
||||
def set_connect(self, status: bool):
|
||||
self.protocol.send('set_properties', [{'did': 'MYDID', 'siid': 2, 'piid': 1, 'value': status}])
|
||||
self.protocol.send(
|
||||
"set_properties", [{"did": "MYDID", "siid": 2, "piid": 1, "value": status}]
|
||||
)
|
||||
|
||||
def on(self):
|
||||
self.set_connect(True)
|
||||
@ -15,13 +17,17 @@ class MiSwitch:
|
||||
self.set_connect(False)
|
||||
|
||||
def power(self):
|
||||
return self.protocol.send('get_properties', [{'did': '845778715', 'siid': 11, 'piid': 2}])[0]['value']
|
||||
return self.protocol.send(
|
||||
"get_properties", [{"did": "845778715", "siid": 11, "piid": 2}]
|
||||
)[0]["value"]
|
||||
|
||||
def is_on(self):
|
||||
return self.protocol.send('get_properties', [{'did': 'MYDID', 'siid': 2, 'piid': 1}])[0]['value']
|
||||
return self.protocol.send(
|
||||
"get_properties", [{"did": "MYDID", "siid": 2, "piid": 1}]
|
||||
)[0]["value"]
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
from flandre import C
|
||||
|
||||
mi = MiSwitch(C.switch1_ip, C.switch1_token)
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
import socket
|
||||
import subprocess
|
||||
|
||||
|
||||
def check_port(host, port):
|
||||
@ -18,8 +17,10 @@ def check_port(host, port):
|
||||
|
||||
|
||||
def check_socket(socket: str):
|
||||
host, port = socket.split('//')[-1].split(':')
|
||||
host, port = socket.split("//")[-1].split(":")
|
||||
return check_port(host, int(port))
|
||||
|
||||
|
||||
# def c2():
|
||||
# code = subprocess.run(['curl', '-m', '1', f'http://{LIVE_REP_SOCKET}'], stderr=subprocess.DEVNULL,
|
||||
# stdout=subprocess.DEVNULL).returncode
|
||||
|
||||
@ -1 +1 @@
|
||||
from .rtsi import *
|
||||
from .rtsi import *
|
||||
|
||||
@ -1,49 +1,56 @@
|
||||
import struct
|
||||
import socket
|
||||
import select
|
||||
import sys
|
||||
import logging
|
||||
import platform
|
||||
import select
|
||||
import socket
|
||||
import struct
|
||||
import sys
|
||||
|
||||
import flandre.utils.rtsi.serialize as serialize
|
||||
|
||||
DEFAULT_TIMEOUT = 10.0
|
||||
|
||||
LOGNAME = 'rtsi'
|
||||
LOGNAME = "rtsi"
|
||||
_log = logging.getLogger(LOGNAME)
|
||||
|
||||
TCP_BUFFER_SIZE = 4096
|
||||
if platform.system() == 'Windows':
|
||||
if platform.system() == "Windows":
|
||||
TCP_BUFFER_SIZE = 3
|
||||
|
||||
|
||||
class Command:
|
||||
RTSI_REQUEST_PROTOCOL_VERSION = 86 # ascii V
|
||||
RTSI_GET_ELITECONTROL_VERSION = 118 # ascii v
|
||||
RTSI_TEXT_MESSAGE = 77 # ascii M
|
||||
RTSI_DATA_PACKAGE = 85 # ascii U
|
||||
RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ascii O
|
||||
RTSI_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ascii I
|
||||
RTSI_CONTROL_PACKAGE_START = 83 # ascii S
|
||||
RTSI_CONTROL_PACKAGE_PAUSE = 80 # ascii P
|
||||
RTSI_REQUEST_PROTOCOL_VERSION = 86 # ascii V
|
||||
RTSI_GET_ELITECONTROL_VERSION = 118 # ascii v
|
||||
RTSI_TEXT_MESSAGE = 77 # ascii M
|
||||
RTSI_DATA_PACKAGE = 85 # ascii U
|
||||
RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ascii O
|
||||
RTSI_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ascii I
|
||||
RTSI_CONTROL_PACKAGE_START = 83 # ascii S
|
||||
RTSI_CONTROL_PACKAGE_PAUSE = 80 # ascii P
|
||||
|
||||
|
||||
RTSI_PROTOCOL_VERSION_1 = 1
|
||||
|
||||
|
||||
class ConnectionState:
|
||||
DISCONNECTED = 0
|
||||
CONNECTED = 1
|
||||
STARTED = 2
|
||||
PAUSED = 3
|
||||
|
||||
|
||||
class RTSIException(Exception):
|
||||
def __init__(self, msg):
|
||||
self.msg = msg
|
||||
|
||||
def __str__(self):
|
||||
return repr(self.msg)
|
||||
|
||||
|
||||
class RTSITimeoutException(RTSIException):
|
||||
def __init__(self, msg):
|
||||
super(RTSITimeoutException, self).__init__(msg)
|
||||
|
||||
|
||||
class rtsi(object):
|
||||
def __init__(self, hostname, port=30004):
|
||||
self.hostname = hostname
|
||||
@ -59,7 +66,7 @@ class rtsi(object):
|
||||
if self.__sock:
|
||||
return
|
||||
|
||||
self.__buf = b'' # buffer data in binary format
|
||||
self.__buf = b"" # buffer data in binary format
|
||||
try:
|
||||
self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
@ -85,7 +92,16 @@ class rtsi(object):
|
||||
cmd = Command.RTSI_GET_ELITECONTROL_VERSION
|
||||
version = self.__sendAndReceive(cmd)
|
||||
if version:
|
||||
_log.info('Controller version: ' + str(version.major) + '.' + str(version.minor) + '.' + str(version.bugfix)+ '.' + str(version.build))
|
||||
_log.info(
|
||||
"Controller version: "
|
||||
+ str(version.major)
|
||||
+ "."
|
||||
+ str(version.minor)
|
||||
+ "."
|
||||
+ str(version.bugfix)
|
||||
+ "."
|
||||
+ str(version.build)
|
||||
)
|
||||
if version.major == 3 and version.minor <= 2 and version.bugfix < 19171:
|
||||
_log.error("Please upgrade your controller to minimally version 2.10")
|
||||
sys.exit()
|
||||
@ -94,26 +110,26 @@ class rtsi(object):
|
||||
|
||||
def version_check(self):
|
||||
cmd = Command.RTSI_REQUEST_PROTOCOL_VERSION
|
||||
payload = struct.pack('>H', RTSI_PROTOCOL_VERSION_1)
|
||||
payload = struct.pack(">H", RTSI_PROTOCOL_VERSION_1)
|
||||
success = self.__sendAndReceive(cmd, payload)
|
||||
if success:
|
||||
self.__protocolVersion = RTSI_PROTOCOL_VERSION_1
|
||||
return success
|
||||
|
||||
def input_subscribe(self, variables:str):
|
||||
def input_subscribe(self, variables: str):
|
||||
cmd = Command.RTSI_CONTROL_PACKAGE_SETUP_INPUTS
|
||||
payload = variables.encode()
|
||||
result = self.__sendAndReceive(cmd, payload)
|
||||
result.names = variables.split(',')
|
||||
result.names = variables.split(",")
|
||||
self.__input_config[result.id] = result
|
||||
return serialize.DataObject.create_empty(variables, result.id)
|
||||
|
||||
def output_subscribe(self, variables:str, frequency=125):
|
||||
def output_subscribe(self, variables: str, frequency=125):
|
||||
cmd = Command.RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS
|
||||
payload = struct.pack('>d', frequency)
|
||||
payload = struct.pack(">d", frequency)
|
||||
payload = payload + variables.encode()
|
||||
result = self.__sendAndReceive(cmd, payload)
|
||||
result.names = variables.split(',')
|
||||
result.names = variables.split(",")
|
||||
self.__output_config[result.id] = result
|
||||
return result
|
||||
|
||||
@ -121,33 +137,32 @@ class rtsi(object):
|
||||
cmd = Command.RTSI_CONTROL_PACKAGE_START
|
||||
success = self.__sendAndReceive(cmd)
|
||||
if success:
|
||||
_log.info('RTSI synchronization started')
|
||||
_log.info("RTSI synchronization started")
|
||||
self.__conn_state = ConnectionState.STARTED
|
||||
else:
|
||||
_log.error('RTSI synchronization failed to start')
|
||||
_log.error("RTSI synchronization failed to start")
|
||||
return success
|
||||
|
||||
def pause(self):
|
||||
cmd = Command.RTSI_CONTROL_PACKAGE_PAUSE
|
||||
success = self.__sendAndReceive(cmd)
|
||||
if success:
|
||||
_log.info('RTSI synchronization paused')
|
||||
_log.info("RTSI synchronization paused")
|
||||
self.__conn_state = ConnectionState.PAUSED
|
||||
else:
|
||||
_log.error('RTSI synchronization failed to pause')
|
||||
_log.error("RTSI synchronization failed to pause")
|
||||
return success
|
||||
|
||||
def set_input(self, input_data):
|
||||
if self.__conn_state != ConnectionState.STARTED:
|
||||
_log.error('Cannot send when RTSI synchronization is inactive')
|
||||
_log.error("Cannot send when RTSI synchronization is inactive")
|
||||
return
|
||||
if not input_data.recipe_id in self.__input_config:
|
||||
_log.error('Input configuration id not found: ' + str(input_data.recipe_id))
|
||||
if input_data.recipe_id not in self.__input_config:
|
||||
_log.error("Input configuration id not found: " + str(input_data.recipe_id))
|
||||
return
|
||||
config = self.__input_config[input_data.recipe_id]
|
||||
return self.__sendall(Command.RTSI_DATA_PACKAGE, config.pack(input_data))
|
||||
|
||||
|
||||
def get_output_data(self):
|
||||
"""Recieve the latest data package.
|
||||
If muliple packages has been received, older ones are discarded
|
||||
@ -155,10 +170,10 @@ class rtsi(object):
|
||||
is received or the connection is lost
|
||||
"""
|
||||
if self.__conn_state != ConnectionState.STARTED:
|
||||
raise RTSIException('Cannot receive when RTSI synchronization is inactive')
|
||||
raise RTSIException("Cannot receive when RTSI synchronization is inactive")
|
||||
return self.__recv(Command.RTSI_DATA_PACKAGE, False)
|
||||
|
||||
def get_output_data_buffered(self, buffer_limit = None):
|
||||
def get_output_data_buffered(self, buffer_limit=None):
|
||||
"""Recieve the next data package.
|
||||
If muliple packages has been received they are buffered and will
|
||||
be returned on subsequent calls to this function.
|
||||
@ -185,9 +200,11 @@ class rtsi(object):
|
||||
|
||||
return data
|
||||
|
||||
def send_message(self, message, source = b"Python Client", type = serialize.Message.INFO_MESSAGE):
|
||||
def send_message(
|
||||
self, message, source=b"Python Client", type=serialize.Message.INFO_MESSAGE
|
||||
):
|
||||
cmd = Command.RTSI_TEXT_MESSAGE
|
||||
fmt = '>B%dsB%dsB' % (len(message), len(source))
|
||||
fmt = ">B%dsB%dsB" % (len(message), len(source))
|
||||
payload = struct.pack(fmt, len(message), message, len(source), source, type)
|
||||
return self.__sendall(cmd, payload)
|
||||
|
||||
@ -209,21 +226,21 @@ class rtsi(object):
|
||||
elif cmd == Command.RTSI_DATA_PACKAGE:
|
||||
return self.__unpack_data_package(payload, self.__output_config[payload[0]])
|
||||
else:
|
||||
_log.error('Unknown package command: ' + str(cmd))
|
||||
_log.error("Unknown package command: " + str(cmd))
|
||||
|
||||
def __sendAndReceive(self, cmd, payload=b''):
|
||||
def __sendAndReceive(self, cmd, payload=b""):
|
||||
if self.__sendall(cmd, payload):
|
||||
return self.__recv(cmd)
|
||||
else:
|
||||
return None
|
||||
|
||||
def __sendall(self, command, payload=b''):
|
||||
fmt = '>HB'
|
||||
def __sendall(self, command, payload=b""):
|
||||
fmt = ">HB"
|
||||
size = struct.calcsize(fmt) + len(payload)
|
||||
buf = struct.pack(fmt, size, command) + payload
|
||||
|
||||
if self.__sock is None:
|
||||
_log.error('Unable to send: not connected to Robot')
|
||||
_log.error("Unable to send: not connected to Robot")
|
||||
return False
|
||||
|
||||
_, writable, _ = select.select([], [self.__sock], [], DEFAULT_TIMEOUT)
|
||||
@ -237,7 +254,7 @@ class rtsi(object):
|
||||
def has_data(self):
|
||||
timeout = 0
|
||||
readable, _, _ = select.select([self.__sock], [], [], timeout)
|
||||
return len(readable)!=0
|
||||
return len(readable) != 0
|
||||
|
||||
def __recv(self, command, binary=False):
|
||||
while self.is_connected():
|
||||
@ -252,45 +269,51 @@ class rtsi(object):
|
||||
packet_header = serialize.ControlHeader.unpack(self.__buf)
|
||||
|
||||
if len(self.__buf) >= packet_header.size:
|
||||
packet, self.__buf = self.__buf[3:packet_header.size], self.__buf[packet_header.size:]
|
||||
packet, self.__buf = (
|
||||
self.__buf[3 : packet_header.size],
|
||||
self.__buf[packet_header.size :],
|
||||
)
|
||||
data = self.__on_packet(packet_header.command, packet)
|
||||
if len(self.__buf) >= 3 and command == Command.RTSI_DATA_PACKAGE:
|
||||
next_packet_header = serialize.ControlHeader.unpack(self.__buf)
|
||||
if next_packet_header.command == command:
|
||||
_log.debug('skipping package(1)')
|
||||
_log.debug("skipping package(1)")
|
||||
self.__skipped_package_count += 1
|
||||
continue
|
||||
if packet_header.command == command:
|
||||
if(binary):
|
||||
if binary:
|
||||
return packet[1:]
|
||||
|
||||
return data
|
||||
else:
|
||||
_log.debug('skipping package(2)')
|
||||
_log.debug("skipping package(2)")
|
||||
else:
|
||||
break
|
||||
raise RTSIException(' _recv() Connection lost ')
|
||||
raise RTSIException(" _recv() Connection lost ")
|
||||
|
||||
def __recv_to_buffer(self, timeout):
|
||||
readable, _, xlist = select.select([self.__sock], [], [self.__sock], timeout)
|
||||
if len(readable):
|
||||
more = self.__sock.recv(TCP_BUFFER_SIZE)
|
||||
#When the controller stops while the script is running
|
||||
# When the controller stops while the script is running
|
||||
if len(more) == 0:
|
||||
_log.error('received 0 bytes from Controller, probable cause: Controller has stopped')
|
||||
self.__trigger_disconnected()
|
||||
raise RTSIException('received 0 bytes from Controller')
|
||||
_log.error(
|
||||
"received 0 bytes from Controller, probable cause: Controller has stopped"
|
||||
)
|
||||
self.__trigger_disconnected()
|
||||
raise RTSIException("received 0 bytes from Controller")
|
||||
|
||||
self.__buf = self.__buf + more
|
||||
return True
|
||||
|
||||
if (len(xlist) or len(readable) == 0) and timeout != 0: # Effectively a timeout of timeout seconds
|
||||
_log.warning('no data received in last %d seconds ',timeout)
|
||||
if (
|
||||
len(xlist) or len(readable) == 0
|
||||
) and timeout != 0: # Effectively a timeout of timeout seconds
|
||||
_log.warning("no data received in last %d seconds ", timeout)
|
||||
raise RTSITimeoutException("no data received within timeout")
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def __recv_from_buffer(self, command, binary=False):
|
||||
# unpack_from requires a buffer of at least 3 bytes
|
||||
while len(self.__buf) >= 3:
|
||||
@ -298,84 +321,89 @@ class rtsi(object):
|
||||
packet_header = serialize.ControlHeader.unpack(self.__buf)
|
||||
|
||||
if len(self.__buf) >= packet_header.size:
|
||||
packet, self.__buf = self.__buf[3:packet_header.size], self.__buf[packet_header.size:]
|
||||
packet, self.__buf = (
|
||||
self.__buf[3 : packet_header.size],
|
||||
self.__buf[packet_header.size :],
|
||||
)
|
||||
data = self.__on_packet(packet_header.command, packet)
|
||||
if packet_header.command == command:
|
||||
if(binary):
|
||||
if binary:
|
||||
return packet[1:]
|
||||
|
||||
return data
|
||||
else:
|
||||
print('skipping package(2)')
|
||||
print("skipping package(2)")
|
||||
else:
|
||||
return None
|
||||
|
||||
def __trigger_disconnected(self):
|
||||
_log.info("RTSI disconnected")
|
||||
self.disconnect() #clean-up
|
||||
self.disconnect() # clean-up
|
||||
|
||||
def __unpack_protocol_version_package(self, payload):
|
||||
if len(payload) != 1:
|
||||
_log.error('RTSI_REQUEST_PROTOCOL_VERSION: Wrong payload size')
|
||||
_log.error("RTSI_REQUEST_PROTOCOL_VERSION: Wrong payload size")
|
||||
return None
|
||||
result = serialize.ReturnValue.unpack(payload)
|
||||
return result.success
|
||||
|
||||
def __unpack_elitecontrol_version_package(self, payload):
|
||||
if len(payload) != 16:
|
||||
_log.error('RTSI_GET_ELITECONTROL_VERSION: Wrong payload size')
|
||||
_log.error("RTSI_GET_ELITECONTROL_VERSION: Wrong payload size")
|
||||
return None
|
||||
version = serialize.ControlVersion.unpack(payload)
|
||||
return version
|
||||
|
||||
def __unpack_text_message(self, payload):
|
||||
if len(payload) < 1:
|
||||
_log.error('RTSIE_TEXT_MESSAGE: No payload')
|
||||
_log.error("RTSIE_TEXT_MESSAGE: No payload")
|
||||
return None
|
||||
if(self.__protocolVersion == RTSI_PROTOCOL_VERSION_1):
|
||||
if self.__protocolVersion == RTSI_PROTOCOL_VERSION_1:
|
||||
msg = serialize.MessageV1.unpack(payload)
|
||||
else:
|
||||
msg = serialize.Message.unpack(payload)
|
||||
|
||||
if(msg.level == serialize.Message.EXCEPTION_MESSAGE or
|
||||
msg.level == serialize.Message.ERROR_MESSAGE):
|
||||
_log.error(msg.source + ': ' + msg.message)
|
||||
if (
|
||||
msg.level == serialize.Message.EXCEPTION_MESSAGE
|
||||
or msg.level == serialize.Message.ERROR_MESSAGE
|
||||
):
|
||||
_log.error(msg.source + ": " + msg.message)
|
||||
elif msg.level == serialize.Message.WARNING_MESSAGE:
|
||||
_log.warning(msg.source + ': ' + msg.message)
|
||||
_log.warning(msg.source + ": " + msg.message)
|
||||
elif msg.level == serialize.Message.INFO_MESSAGE:
|
||||
_log.info(msg.source + ': ' + msg.message)
|
||||
_log.info(msg.source + ": " + msg.message)
|
||||
|
||||
def __unpack_setup_outputs_package(self, payload):
|
||||
if len(payload) < 1:
|
||||
_log.error('RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload')
|
||||
_log.error("RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload")
|
||||
return None
|
||||
output_config = serialize.DataConfig.unpack_recipe(payload)
|
||||
return output_config
|
||||
|
||||
def __unpack_setup_inputs_package(self, payload):
|
||||
if len(payload) < 1:
|
||||
_log.error('RTSI_CONTROL_PACKAGE_SETUP_INPUTS: No payload')
|
||||
_log.error("RTSI_CONTROL_PACKAGE_SETUP_INPUTS: No payload")
|
||||
return None
|
||||
input_config = serialize.DataConfig.unpack_recipe(payload)
|
||||
return input_config
|
||||
|
||||
def __unpack_start_package(self, payload):
|
||||
if len(payload) != 1:
|
||||
_log.error('RTSI_CONTROL_PACKAGE_START: Wrong payload size')
|
||||
_log.error("RTSI_CONTROL_PACKAGE_START: Wrong payload size")
|
||||
return None
|
||||
result = serialize.ReturnValue.unpack(payload)
|
||||
return result.success
|
||||
|
||||
def __unpack_pause_package(self, payload):
|
||||
if len(payload) != 1:
|
||||
_log.error('RTSI_CONTROL_PACKAGE_PAUSE: Wrong payload size')
|
||||
_log.error("RTSI_CONTROL_PACKAGE_PAUSE: Wrong payload size")
|
||||
return None
|
||||
result = serialize.ReturnValue.unpack(payload)
|
||||
return result.success
|
||||
|
||||
def __unpack_data_package(self, payload, output_config):
|
||||
if output_config is None:
|
||||
_log.error('RTSI_DATA_PACKAGE: Missing output configuration')
|
||||
_log.error("RTSI_DATA_PACKAGE: Missing output configuration")
|
||||
return None
|
||||
output = output_config.unpack(payload)
|
||||
return output
|
||||
@ -387,7 +415,7 @@ class rtsi(object):
|
||||
if l1[i] != l2[i]:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
@property
|
||||
def skipped_package_count(self):
|
||||
"""The skipped package count, resets on connect"""
|
||||
|
||||
@ -2,38 +2,42 @@ import struct
|
||||
|
||||
|
||||
class ControlHeader(object):
|
||||
__slots__ = ['command', 'size',]
|
||||
|
||||
__slots__ = [
|
||||
"command",
|
||||
"size",
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def unpack(buf):
|
||||
rmd = ControlHeader()
|
||||
(rmd.size, rmd.command) = struct.unpack_from('>HB', buf)
|
||||
(rmd.size, rmd.command) = struct.unpack_from(">HB", buf)
|
||||
return rmd
|
||||
|
||||
|
||||
class ControlVersion(object):
|
||||
__slots__ = ['major', 'minor', 'bugfix', 'build']
|
||||
|
||||
__slots__ = ["major", "minor", "bugfix", "build"]
|
||||
|
||||
@staticmethod
|
||||
def unpack(buf):
|
||||
rmd = ControlVersion()
|
||||
(rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from('>IIII', buf)
|
||||
(rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from(">IIII", buf)
|
||||
return rmd
|
||||
|
||||
|
||||
class ReturnValue(object):
|
||||
__slots__ = ['success']
|
||||
|
||||
__slots__ = ["success"]
|
||||
|
||||
@staticmethod
|
||||
def unpack(buf):
|
||||
rmd = ReturnValue()
|
||||
rmd.success = bool(struct.unpack_from('>B', buf)[0])
|
||||
rmd.success = bool(struct.unpack_from(">B", buf)[0])
|
||||
return rmd
|
||||
|
||||
|
||||
class MessageV1(object):
|
||||
@staticmethod
|
||||
def unpack(buf):
|
||||
rmd = Message() # use V2 message object
|
||||
rmd = Message() # use V2 message object
|
||||
offset = 0
|
||||
rmd.level = struct.unpack_from(">B", buf, offset)[0]
|
||||
offset = offset + 1
|
||||
@ -44,24 +48,24 @@ class MessageV1(object):
|
||||
|
||||
|
||||
class Message(object):
|
||||
__slots__ = ['level', 'message', 'source']
|
||||
__slots__ = ["level", "message", "source"]
|
||||
EXCEPTION_MESSAGE = 0
|
||||
ERROR_MESSAGE = 1
|
||||
WARNING_MESSAGE = 2
|
||||
INFO_MESSAGE = 3
|
||||
|
||||
|
||||
@staticmethod
|
||||
def unpack(buf):
|
||||
rmd = Message()
|
||||
offset = 0
|
||||
msg_length = struct.unpack_from(">B", buf, offset)[0]
|
||||
offset = offset + 1
|
||||
rmd.message = str(buf[offset:offset+msg_length])
|
||||
rmd.message = str(buf[offset : offset + msg_length])
|
||||
offset = offset + msg_length
|
||||
|
||||
src_length = struct.unpack_from(">B", buf, offset)[0]
|
||||
offset = offset + 1
|
||||
rmd.source = str(buf[offset:offset+src_length])
|
||||
rmd.source = str(buf[offset : offset + src_length])
|
||||
offset = offset + src_length
|
||||
rmd.level = struct.unpack_from(">B", buf, offset)[0]
|
||||
|
||||
@ -69,55 +73,54 @@ class Message(object):
|
||||
|
||||
|
||||
def get_item_size(data_type):
|
||||
if data_type.startswith('VECTOR6'):
|
||||
if data_type.startswith("VECTOR6"):
|
||||
return 6
|
||||
elif data_type.startswith('VECTOR3'):
|
||||
elif data_type.startswith("VECTOR3"):
|
||||
return 3
|
||||
return 1
|
||||
|
||||
|
||||
def unpack_field(data, offset, data_type):
|
||||
size = get_item_size(data_type)
|
||||
if(data_type == 'VECTOR6D' or
|
||||
data_type == 'VECTOR3D'):
|
||||
return [float(data[offset+i]) for i in range(size)]
|
||||
elif(data_type == 'VECTOR6UINT32'):
|
||||
return [int(data[offset+i]) for i in range(size)]
|
||||
elif(data_type == 'DOUBLE'):
|
||||
if data_type == "VECTOR6D" or data_type == "VECTOR3D":
|
||||
return [float(data[offset + i]) for i in range(size)]
|
||||
elif data_type == "VECTOR6UINT32":
|
||||
return [int(data[offset + i]) for i in range(size)]
|
||||
elif data_type == "DOUBLE":
|
||||
return float(data[offset])
|
||||
elif(data_type == 'UINT32' or
|
||||
data_type == 'UINT64'):
|
||||
elif data_type == "UINT32" or data_type == "UINT64":
|
||||
return int(data[offset])
|
||||
elif(data_type == 'VECTOR6INT32'):
|
||||
return [int(data[offset+i]) for i in range(size)]
|
||||
elif(data_type == 'INT32' or
|
||||
data_type == 'UINT8'):
|
||||
elif data_type == "VECTOR6INT32":
|
||||
return [int(data[offset + i]) for i in range(size)]
|
||||
elif data_type == "INT32" or data_type == "UINT8":
|
||||
return int(data[offset])
|
||||
elif(data_type == 'BOOL'):
|
||||
elif data_type == "BOOL":
|
||||
return bool(data[offset])
|
||||
raise ValueError('unpack_field: unknown data type: ' + data_type)
|
||||
raise ValueError("unpack_field: unknown data type: " + data_type)
|
||||
|
||||
|
||||
class DataObject(object):
|
||||
recipe_id = None
|
||||
|
||||
def pack(self, names, types):
|
||||
if len(names) != len(types):
|
||||
raise ValueError('List sizes are not identical.')
|
||||
raise ValueError("List sizes are not identical.")
|
||||
l = []
|
||||
if(self.recipe_id is not None):
|
||||
if self.recipe_id is not None:
|
||||
l.append(self.recipe_id)
|
||||
for i in range(len(names)):
|
||||
if self.__dict__[names[i]] is None:
|
||||
raise ValueError('Uninitialized parameter: ' + names[i])
|
||||
if types[i].startswith('VECTOR'):
|
||||
raise ValueError("Uninitialized parameter: " + names[i])
|
||||
if types[i].startswith("VECTOR"):
|
||||
l.extend(self.__dict__[names[i]])
|
||||
else:
|
||||
l.append(self.__dict__[names[i]])
|
||||
return l
|
||||
|
||||
|
||||
@staticmethod
|
||||
def unpack(data, names, types):
|
||||
if len(names) != len(types):
|
||||
raise ValueError('List sizes are not identical.')
|
||||
raise ValueError("List sizes are not identical.")
|
||||
obj = DataObject()
|
||||
offset = 0
|
||||
obj.recipe_id = data[0]
|
||||
@ -136,47 +139,47 @@ class DataObject(object):
|
||||
|
||||
|
||||
class DataConfig(object):
|
||||
__slots__ = ['id', 'names', 'types', 'fmt']
|
||||
__slots__ = ["id", "names", "types", "fmt"]
|
||||
|
||||
@staticmethod
|
||||
def unpack_recipe(buf):
|
||||
rmd = DataConfig()
|
||||
rmd.id = struct.unpack_from('>B', buf)[0]
|
||||
rmd.types = buf.decode('utf-8')[1:].split(',')
|
||||
rmd.fmt = '>B'
|
||||
rmd.id = struct.unpack_from(">B", buf)[0]
|
||||
rmd.types = buf.decode("utf-8")[1:].split(",")
|
||||
rmd.fmt = ">B"
|
||||
for i in rmd.types:
|
||||
if i=='INT32':
|
||||
rmd.fmt += 'i'
|
||||
elif i=='UINT32':
|
||||
rmd.fmt += 'I'
|
||||
elif i=='VECTOR6D':
|
||||
rmd.fmt += 'd'*6
|
||||
elif i=='VECTOR3D':
|
||||
rmd.fmt += 'd'*3
|
||||
elif i=='VECTOR6INT32':
|
||||
rmd.fmt += 'i'*6
|
||||
elif i=='VECTOR6UINT32':
|
||||
rmd.fmt += 'I'*6
|
||||
elif i=='DOUBLE':
|
||||
rmd.fmt += 'd'
|
||||
elif i=='UINT64':
|
||||
rmd.fmt += 'Q'
|
||||
elif i=='UINT8':
|
||||
rmd.fmt += 'B'
|
||||
elif i =='BOOL':
|
||||
rmd.fmt += '?'
|
||||
elif i == 'UINT16':
|
||||
rmd.fmt += 'H'
|
||||
elif i=='IN_USE':
|
||||
raise ValueError('An input parameter is already in use.')
|
||||
if i == "INT32":
|
||||
rmd.fmt += "i"
|
||||
elif i == "UINT32":
|
||||
rmd.fmt += "I"
|
||||
elif i == "VECTOR6D":
|
||||
rmd.fmt += "d" * 6
|
||||
elif i == "VECTOR3D":
|
||||
rmd.fmt += "d" * 3
|
||||
elif i == "VECTOR6INT32":
|
||||
rmd.fmt += "i" * 6
|
||||
elif i == "VECTOR6UINT32":
|
||||
rmd.fmt += "I" * 6
|
||||
elif i == "DOUBLE":
|
||||
rmd.fmt += "d"
|
||||
elif i == "UINT64":
|
||||
rmd.fmt += "Q"
|
||||
elif i == "UINT8":
|
||||
rmd.fmt += "B"
|
||||
elif i == "BOOL":
|
||||
rmd.fmt += "?"
|
||||
elif i == "UINT16":
|
||||
rmd.fmt += "H"
|
||||
elif i == "IN_USE":
|
||||
raise ValueError("An input parameter is already in use.")
|
||||
else:
|
||||
raise ValueError('Unknown data type: ' + i)
|
||||
raise ValueError("Unknown data type: " + i)
|
||||
return rmd
|
||||
|
||||
|
||||
def pack(self, state):
|
||||
l = state.pack(self.names, self.types)
|
||||
return struct.pack(self.fmt, *l)
|
||||
|
||||
def unpack(self, data):
|
||||
li = struct.unpack_from(self.fmt, data)
|
||||
li = struct.unpack_from(self.fmt, data)
|
||||
return DataObject.unpack(li, self.names, self.types)
|
||||
|
||||
|
||||
File diff suppressed because one or more lines are too long
6
tool.py
6
tool.py
@ -11,15 +11,15 @@ def cli():
|
||||
|
||||
|
||||
@cli.command()
|
||||
@click.argument('folder')
|
||||
@click.argument("folder")
|
||||
def f2z(folder):
|
||||
folder_to_zip(Path(folder))
|
||||
|
||||
|
||||
@cli.command()
|
||||
def dropdb():
|
||||
click.echo('Dropped the database')
|
||||
click.echo("Dropped the database")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
cli()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user