diff --git a/draft/sync_time.py b/draft/sync_time.py index 76aeed2..b6c5926 100644 --- a/draft/sync_time.py +++ b/draft/sync_time.py @@ -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")], ) diff --git a/flandre/nodes/MainUI.py b/flandre/nodes/MainUI.py index 4e0c0a8..48da119 100644 --- a/flandre/nodes/MainUI.py +++ b/flandre/nodes/MainUI.py @@ -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) diff --git a/flandre/nodes/Robot.py b/flandre/nodes/Robot.py index 9217d37..3908c9c 100644 --- a/flandre/nodes/Robot.py +++ b/flandre/nodes/Robot.py @@ -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() ) diff --git a/flandre/utils/Msg.py b/flandre/utils/Msg.py index 819fc06..8b56e77 100644 --- a/flandre/utils/Msg.py +++ b/flandre/utils/Msg.py @@ -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 diff --git a/flandre/utils/robot.py b/flandre/utils/robot.py index 963ccb8..9361903 100644 --- a/flandre/utils/robot.py +++ b/flandre/utils/robot.py @@ -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