feat: add rtsi convertor

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_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")],
         )

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,13 +388,14 @@ 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())
+            msg: Msg = Msg.decode_msg(msg0.data())
-            if isinstance(msg, KillMsg):
+            match msg:
+                case KillMsg():
                     if msg.name == "":
                         self.close()
-            elif isinstance(msg, ImageArgMsg):
+                case ImageArgMsg():
                     self.arg = msg
                     self.arg.sender = "ui"
 
@@ -432,23 +434,23 @@ class Adv(QMainWindow, Ui_MainWindow):
                     self.sp_g7.setValue(msg.g7)
                     self.sp_g8.setValue(msg.g8)
 
-            elif isinstance(msg, MoveAxisMsg):
+                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]))
-            elif isinstance(msg, SetSidMsg):
+                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]))
-            elif isinstance(msg, SeqIdList):
+                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)))
-            elif isinstance(msg, PlaybackSeqListMsg):
+                case PlaybackSeqListMsg():
                     self.c_playback_seq_name.clear()
                     for name in msg.value:
                         self.c_playback_seq_name.addItem(name)
@@ -458,65 +460,67 @@ class Adv(QMainWindow, Ui_MainWindow):
                                 "playback", self.c_playback_seq_name.currentText()
                             )
                         )
-            elif isinstance(msg, SetSeqMetaMsg):
+                case 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):
+                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))
-            elif isinstance(msg, DeviceConnectedMsg):
+                case 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):
+                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):
+                case DeviceConfigListMsg():
                     for name, txt in msg.arr:
                         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)
-            elif isinstance(msg, RecordFrameMsg):
+                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))
-            elif isinstance(msg, SetWindowVisibleMsg):
+                case SetWindowVisibleMsg():
                     if msg.name == "bscan" and msg.sender != "ui":
                         self.cb_bscan.setChecked(msg.value)
-            elif isinstance(msg, ImagingConfigNameListMsg):
+                case 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):
+                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)

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
+                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(
                     RobotRtsiMsg(
                         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=(
-                            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()
                 )

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

flandre/utils/robot.py

@@ -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),
-        int(yy * 1000),
-        int(fx * 10),
-        int(fy * 10),
-        int(fz * 10),
-        int(fr * 100),
-        int(fp * 100),
-        int(fyy * 100),
 )
+
+
+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