yukari/flandre
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add rtsi convertor

Browse Source
This commit is contained in:
flandre 2025-06-11 17:42:06 +08:00
parent 0fbeaf2f6e
commit 1485c9446c
5 changed files with 240 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
draft/sync_time.py
View File

@ -11,6 +11,7 @@ from tqdm import tqdm
from flandre.utils.RfFrame import b2t from flandre.utils.RfFrame import b2t
from flandre.utils.RfMeta import RfFrameMeta from flandre.utils.RfMeta import RfFrameMeta
from flandre.utils.archive import to_zip from flandre.utils.archive import to_zip
from flandre.utils.robot import rtsi_float2int
def match(a: int, b: npt.NDArray[np.int_]): def match(a: int, b: npt.NDArray[np.int_]):
@ -151,24 +152,27 @@ for i, k in enumerate(tqdm(device_device_cut)):
) )
file = device_ts_map[device_host_arr[i]] file = device_ts_map[device_host_arr[i]]
seq, encoder, host_ts, device_ts, buffer = b2t(file.read_bytes()) seq, encoder, host_ts, device_ts, buffer = b2t(file.read_bytes())
robot_rtsi = rtsi_float2int(r)
zip_li.append( zip_li.append(
( (
buffer, buffer,
RfFrameMeta( RfFrameMeta(
sequence_id=seq, sequence_id=seq,
encoder=encoder, encoder=encoder,
robot_x=int(r["x"] * 100000), robot_x=robot_rtsi["x"],
robot_y=int(r["y"] * 100000), robot_y=robot_rtsi["y"],
robot_z=int(r["z"] * 100000), robot_z=robot_rtsi["z"],
robot_rx=int(r["rx"] * 1000), robot_rx=robot_rtsi["rx"],
robot_ry=int(r["ry"] * 1000), robot_ry=robot_rtsi["ry"],
robot_rz=int(r["rz"] * 1000), robot_rz=robot_rtsi["rz"],
robot_force_x=int(r["fx"] * 1000), robot_force_x=robot_rtsi["fx"],
robot_force_y=int(r["fy"] * 1000), robot_force_y=robot_rtsi["fy"],
robot_force_z=int(r["fz"] * 1000), robot_force_z=robot_rtsi["fz"],
robot_force_rx=int(r["frx"] * 10000), robot_force_rx=robot_rtsi["frx"],
robot_force_ry=int(r["fry"] * 10000), robot_force_ry=robot_rtsi["fry"],
robot_force_rz=int(r["frz"] * 10000), robot_force_rz=robot_rtsi["frz"],
), ),
[(c, ".avif")], [(c, ".avif")],
) )

246
flandre/nodes/MainUI.py
View File

@ -54,6 +54,7 @@ from flandre.utils.Msg import (
SetWindowVisibleMsg, SetWindowVisibleMsg,
) )
from flandre.utils.RfMeta import RfSequenceMeta from flandre.utils.RfMeta import RfSequenceMeta
from flandre.utils.robot import rtsi_int2str
logger = logging.getLogger(__name__) logger = logging.getLogger(__name__)
@ -387,136 +388,139 @@ class Adv(QMainWindow, Ui_MainWindow):
self.sp_dct_center.setMaximum(m) self.sp_dct_center.setMaximum(m)
self.sp_dct_bandwidth.setMaximum(m) self.sp_dct_bandwidth.setMaximum(m)
def on_zmq_event(self, msg: QByteArray): def on_zmq_event(self, msg0: QByteArray):
try: try:
msg = Msg.decode_msg(msg.data()) msg: Msg = Msg.decode_msg(msg0.data())
if isinstance(msg, KillMsg): match msg:
if msg.name == "": case KillMsg():
self.close() if msg.name == "":
elif isinstance(msg, ImageArgMsg): self.close()
self.arg = msg case ImageArgMsg():
self.arg.sender = "ui" self.arg = msg
self.arg.sender = "ui"
self.s_t_start.setValue(msg.t_start) self.s_t_start.setValue(msg.t_start)
self.s_t_end.setValue(msg.t_end) self.s_t_end.setValue(msg.t_end)
self.s_f_rows.setValue(msg.f_rows) self.s_f_rows.setValue(msg.f_rows)
self.s_v2.setValue(msg.v2) self.s_v2.setValue(msg.v2)
self.s_dct_center.setValue(msg.dct_center) self.s_dct_center.setValue(msg.dct_center)
self.s_dct_bandwidth.setValue(msg.dct_bandwidth) self.s_dct_bandwidth.setValue(msg.dct_bandwidth)
self.s_beta.setValue(msg.beta) self.s_beta.setValue(msg.beta)
self.sp_crop_center.setValue(msg.t_start) self.sp_crop_center.setValue(msg.t_start)
self.sp_crop_width.setValue(msg.t_end) self.sp_crop_width.setValue(msg.t_end)
self.sp_f_rows.setValue(msg.f_rows) self.sp_f_rows.setValue(msg.f_rows)
self.sp_v2.setValue(msg.v2) self.sp_v2.setValue(msg.v2)
self.sp_dct_center.setValue(msg.dct_center) self.sp_dct_center.setValue(msg.dct_center)
self.sp_dct_bandwidth.setValue(msg.dct_bandwidth) self.sp_dct_bandwidth.setValue(msg.dct_bandwidth)
self.sp_beta.setValue(msg.beta) self.sp_beta.setValue(msg.beta)
self.s_g1.setValue(msg.g1) self.s_g1.setValue(msg.g1)
self.s_g2.setValue(msg.g2) self.s_g2.setValue(msg.g2)
self.s_g3.setValue(msg.g3) self.s_g3.setValue(msg.g3)
self.s_g4.setValue(msg.g4) self.s_g4.setValue(msg.g4)
self.s_g5.setValue(msg.g5) self.s_g5.setValue(msg.g5)
self.s_g6.setValue(msg.g6) self.s_g6.setValue(msg.g6)
self.s_g7.setValue(msg.g7) self.s_g7.setValue(msg.g7)
self.s_g8.setValue(msg.g8) self.s_g8.setValue(msg.g8)
self.sp_g1.setValue(msg.g1) self.sp_g1.setValue(msg.g1)
self.sp_g2.setValue(msg.g2) self.sp_g2.setValue(msg.g2)
self.sp_g3.setValue(msg.g3) self.sp_g3.setValue(msg.g3)
self.sp_g4.setValue(msg.g4) self.sp_g4.setValue(msg.g4)
self.sp_g5.setValue(msg.g5) self.sp_g5.setValue(msg.g5)
self.sp_g6.setValue(msg.g6) self.sp_g6.setValue(msg.g6)
self.sp_g7.setValue(msg.g7) self.sp_g7.setValue(msg.g7)
self.sp_g8.setValue(msg.g8) self.sp_g8.setValue(msg.g8)
elif isinstance(msg, MoveAxisMsg): case MoveAxisMsg():
match msg.axis: match msg.axis:
case "S": case "S":
pass pass
# self.s_sid.setValue(msg.value) # self.s_sid.setValue(msg.value)
# self.l_seq_current.setText(str(self.seq_id_list[msg.value])) # self.l_seq_current.setText(str(self.seq_id_list[msg.value]))
elif isinstance(msg, SetSidMsg): case SetSidMsg():
self.s_sid.setValue(msg.value) self.s_sid.setValue(msg.value)
self.sp_sid.setValue(msg.value) self.sp_sid.setValue(msg.value)
self.l_seq_current.setText(str(self.seq_id_list[msg.value])) self.l_seq_current.setText(str(self.seq_id_list[msg.value]))
elif isinstance(msg, SeqIdList): case SeqIdList():
self.seq_id_list = msg.li self.seq_id_list = msg.li
self.s_sid.setMaximum(msg.li.__len__() - 1) self.s_sid.setMaximum(msg.li.__len__() - 1)
self.sp_sid.setMaximum(msg.li.__len__() - 1) self.sp_sid.setMaximum(msg.li.__len__() - 1)
self.l_seq_min.setText(str(min(msg.li))) self.l_seq_min.setText(str(min(msg.li)))
self.l_seq_max.setText(str(max(msg.li))) self.l_seq_max.setText(str(max(msg.li)))
elif isinstance(msg, PlaybackSeqListMsg): case PlaybackSeqListMsg():
self.c_playback_seq_name.clear() self.c_playback_seq_name.clear()
for name in msg.value: for name in msg.value:
self.c_playback_seq_name.addItem(name) self.c_playback_seq_name.addItem(name)
if msg.value.__len__() > 0: if msg.value.__len__() > 0:
self.p.send( self.p.send(
SetSeqMetaMsg( SetSeqMetaMsg(
"playback", self.c_playback_seq_name.currentText() "playback", self.c_playback_seq_name.currentText()
)
) )
) case SetSeqMetaMsg():
elif isinstance(msg, SetSeqMetaMsg): if msg.target == "playback":
if msg.target == "playback": self.playback_seq_meta = RfSequenceMeta.from_name(msg.name)
self.playback_seq_meta = RfSequenceMeta.from_name(msg.name) self.update_max(max(self.playback_seq_meta.shape))
self.update_max(max(self.playback_seq_meta.shape)) case SeqMetaMsg():
elif isinstance(msg, SeqMetaMsg): if msg.target == "live":
if msg.target == "live": self.l_live_seq_name.setText(msg.name)
self.l_live_seq_name.setText(msg.name) self.b_live_seq_apply.setEnabled(True)
self.b_live_seq_apply.setEnabled(True) self.live_seq_meta = RfSequenceMeta.from_name(msg.name)
self.live_seq_meta = RfSequenceMeta.from_name(msg.name) self.update_max(max(self.live_seq_meta.shape))
self.update_max(max(self.live_seq_meta.shape)) case DeviceConnectedMsg():
elif isinstance(msg, DeviceConnectedMsg): if msg.value:
if msg.value: self.set_device_connection(LinkStatus.GREEN)
self.set_device_connection(LinkStatus.GREEN) else:
else: self.set_device_connection(LinkStatus.RED)
self.set_device_connection(LinkStatus.RED) self.update_device_buttons()
self.update_device_buttons() case DeviceEnabledMsg():
elif isinstance(msg, DeviceEnabledMsg): if msg.value:
if msg.value: self.set_device_enable(LinkStatus.GREEN)
self.set_device_enable(LinkStatus.GREEN) else:
else: self.set_device_enable(LinkStatus.RED)
self.set_device_enable(LinkStatus.RED) self.update_device_buttons()
self.update_device_buttons()
elif isinstance(msg, DeviceConfigListMsg): case DeviceConfigListMsg():
for name, txt in msg.arr: for name, txt in msg.arr:
self.c_live_seq_name.addItem(name, txt) self.c_live_seq_name.addItem(name, txt)
elif isinstance(msg, RobotRtsiMsg): case RobotRtsiMsg():
self.ln_x.display(f"{msg.pos[0] / 100:.2f}") r = rtsi_int2str(msg.to_dict())
self.ln_y.display(f"{msg.pos[1] / 100:.2f}") self.ln_x.display(r["x"])
self.ln_z.display(f"{msg.pos[2] / 100:.2f}") self.ln_y.display(r["y"])
self.ln_rx.display(f"{msg.pos[3] / 1000:.3f}") self.ln_z.display(r["z"])
self.ln_ry.display(f"{msg.pos[4] / 1000:.3f}") self.ln_rx.display(r["rx"])
self.ln_rz.display(f"{msg.pos[5] / 1000:.3f}") self.ln_ry.display(r["ry"])
self.ln_rz.display(r["rz"])
self.ln_fx.display(f"{msg.force[0] / 10:.1f}") self.ln_fx.display(r["fx"])
self.ln_fy.display(f"{msg.force[1] / 10:.1f}") self.ln_fy.display(r["fy"])
self.ln_fz.display(f"{msg.force[2] / 10:.1f}") self.ln_fz.display(r["fz"])
self.ln_frx.display(f"{msg.force[3] / 100:.2f}") self.ln_frx.display(r["frx"])
self.ln_fry.display(f"{msg.force[4] / 100:.2f}") self.ln_fry.display(r["fry"])
self.ln_frz.display(f"{msg.force[5] / 100:.2f}") self.ln_frz.display(r["frz"])
elif isinstance(msg, MaxMsg): case MaxMsg():
self.update_max_2(msg.value) self.update_max_2(msg.value)
elif isinstance(msg, RecordFrameMsg): case RecordFrameMsg():
self.record_frame_cnt += 1 self.record_frame_cnt += 1
self.record_size_cnt += msg.size self.record_size_cnt += msg.size
self.l_record_size.setText(humanbytes(self.record_size_cnt)) self.l_record_size.setText(humanbytes(self.record_size_cnt))
self.l_record_frames.setText(str(self.record_frame_cnt)) self.l_record_frames.setText(str(self.record_frame_cnt))
self.l_record_max_sid.setText(str(msg.current_sid)) self.l_record_max_sid.setText(str(msg.current_sid))
elif isinstance(msg, SetWindowVisibleMsg): case SetWindowVisibleMsg():
if msg.name == "bscan" and msg.sender != "ui": if msg.name == "bscan" and msg.sender != "ui":
self.cb_bscan.setChecked(msg.value) self.cb_bscan.setChecked(msg.value)
elif isinstance(msg, ImagingConfigNameListMsg): case ImagingConfigNameListMsg():
# print(msg, flush=True) todo fix # print(msg, flush=True) todo fix
self.c_imaging_config.clear() self.c_imaging_config.clear()
for name in msg.value: for name in msg.value:
self.c_imaging_config.addItem(name) self.c_imaging_config.addItem(name)
elif isinstance(msg, DeviceSwitchMsg): case DeviceSwitchMsg():
self.device_switch_state = LinkStatus[msg.value] self.device_switch_state = LinkStatus[msg.value]
self.on_device_switch_state() self.on_device_switch_state()
case _:
pass
except Exception as e: except Exception as e:
logger.error(e) logger.error(e)
traceback.print_exception(e) traceback.print_exception(e)

47
flandre/nodes/Robot.py
View File

@ -6,6 +6,7 @@ import zmq
from flandre.nodes.Node import Node from flandre.nodes.Node import Node
from flandre.utils.Msg import KillMsg, Msg, RobotRtsiMsg from flandre.utils.Msg import KillMsg, Msg, RobotRtsiMsg
from flandre.utils.network import check_port from flandre.utils.network import check_port
from flandre.utils.robot import rtsi_float2int
from flandre.utils.rtsi import rtsi from flandre.utils.rtsi import rtsi
from flandre.utils.rtsi.serialize import DataObject from flandre.utils.rtsi.serialize import DataObject
@ -40,25 +41,43 @@ class Robot(Node):
if recv_out is None: if recv_out is None:
continue continue
if recv_out.recipe_id == output1.id: if recv_out.recipe_id == output1.id:
x, y, z, r, p, yy = recv_out.actual_TCP_pose x, y, z, rx, ry, rz = recv_out.actual_TCP_pose
fx, fy, fz, fr, fp, fyy = recv_out.actual_TCP_force fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force
r = rtsi_float2int(
dict(
x=x,
y=y,
z=z,
rx=rx,
ry=ry,
rz=rz,
fx=fx,
fy=fy,
fz=fz,
frx=frx,
fry=fry,
frz=frz,
)
)
rtsi_push_socket.send( rtsi_push_socket.send(
RobotRtsiMsg( RobotRtsiMsg(
pos=( pos=(
int(x * 100000), r["x"],
int(y * 100000), r["y"],
int(z * 100000), r["z"],
int(r * 1000), r["rx"],
int(p * 1000), r["ry"],
int(yy * 1000), r["rz"],
), ),
force=( force=(
int(fx * 10), r["fx"],
int(fy * 10), r["fy"],
int(fz * 10), r["fz"],
int(fr * 100), r["frx"],
int(fp * 100), r["fry"],
int(fyy * 100), r["frz"],
), ),
).encode_msg() ).encode_msg()
) )

16
flandre/utils/Msg.py
View File

@ -549,6 +549,22 @@ class RobotRtsiMsg(Msg):
), ),
) )
def to_dict(self):
return dict(
x=self.pos[0],
y=self.pos[1],
z=self.pos[2],
rx=self.pos[3],
ry=self.pos[4],
rz=self.pos[5],
fx=self.force[0],
fy=self.force[1],
fz=self.force[2],
frx=self.force[3],
fry=self.force[4],
frz=self.force[5],
)
class RequestRfFrameMsg(Msg): class RequestRfFrameMsg(Msg):
pass pass

78
flandre/utils/robot.py
View File

@ -1,28 +1,50 @@
def float2int( from typing import Mapping
x: float,
y: float,
z: float, arg_map: dict[str, tuple[int, str]] = dict(
r: float, x=(100000, "{v:.2f}"),
p: float, y=(100000, "{v:.2f}"),
yy: float, z=(100000, "{v:.2f}"),
fx: float, rx=(1000, "{v:.3f}"),
fy: float, ry=(1000, "{v:.3f}"),
fz: float, rz=(1000, "{v:.3f}"),
fr: float, fx=(1000, "{v:.1f}"),
fp: float, fy=(1000, "{v:.1f}"),
fyy: float, fz=(1000, "{v:.1f}"),
): frx=(10000, "{v:.2f}"),
return ( fry=(10000, "{v:.2f}"),
int(x * 100000), frz=(10000, "{v:.2f}"),
int(y * 100000), )
int(z * 100000),
int(r * 1000),
int(p * 1000), def rtsi_float2int(d: Mapping[str, float | None]) -> Mapping[str, int | None]:
int(yy * 1000), return_d: dict[str, int | None] = dict()
int(fx * 10), for k, v in d.items():
int(fy * 10), assert k in arg_map
int(fz * 10), if v is not None:
int(fr * 100), return_d[k] = int(v * arg_map[k][0])
int(fp * 100), else:
int(fyy * 100), return_d[k] = None
) return return_d
def rtsi_int2float(d: Mapping[str, int | None]) -> Mapping[str, float | None]:
return_d: dict[str, float | None] = dict()
for k, v in d.items():
assert k in arg_map
if v is not None:
return_d[k] = float(v) / arg_map[k][0]
else:
return_d[k] = None
return return_d
def rtsi_int2str(d: Mapping[str, int | None]) -> Mapping[str, str | None]:
return_d: dict[str, str | None] = dict()
for k, v in d.items():
assert k in arg_map
if v is not None:
return_d[k] = arg_map[k][1].format(v=float(v) / arg_map[k][0])
else:
return_d[k] = None
return return_d