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.RfMeta import RfFrameMeta
from flandre.utils.archive import to_zip
from flandre.utils.robot import rtsi_float2int
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]]
seq, encoder, host_ts, device_ts, buffer = b2t(file.read_bytes())
robot_rtsi = rtsi_float2int(r)
zip_li.append(
(
buffer,
RfFrameMeta(
sequence_id=seq,
encoder=encoder,
robot_x=int(r["x"] * 100000),
robot_y=int(r["y"] * 100000),
robot_z=int(r["z"] * 100000),
robot_rx=int(r["rx"] * 1000),
robot_ry=int(r["ry"] * 1000),
robot_rz=int(r["rz"] * 1000),
robot_force_x=int(r["fx"] * 1000),
robot_force_y=int(r["fy"] * 1000),
robot_force_z=int(r["fz"] * 1000),
robot_force_rx=int(r["frx"] * 10000),
robot_force_ry=int(r["fry"] * 10000),
robot_force_rz=int(r["frz"] * 10000),
robot_x=robot_rtsi["x"],
robot_y=robot_rtsi["y"],
robot_z=robot_rtsi["z"],
robot_rx=robot_rtsi["rx"],
robot_ry=robot_rtsi["ry"],
robot_rz=robot_rtsi["rz"],
robot_force_x=robot_rtsi["fx"],
robot_force_y=robot_rtsi["fy"],
robot_force_z=robot_rtsi["fz"],
robot_force_rx=robot_rtsi["frx"],
robot_force_ry=robot_rtsi["fry"],
robot_force_rz=robot_rtsi["frz"],
),
[(c, ".avif")],
)

246
flandre/nodes/MainUI.py
View File

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

78
flandre/utils/robot.py
View File

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