diff --git a/src/main.py b/src/main.py index 28336be..9bdee59 100644 --- a/src/main.py +++ b/src/main.py @@ -8,6 +8,7 @@ from nodes.Device import Device from nodes.ImageCV import ImageCV from nodes.Loader import Loader from nodes.MainUI import MainUI +from nodes.Robot import Robot from nodes.WebRTC import WebRTC from utils.Msg import KillMsg @@ -24,6 +25,7 @@ if __name__ == '__main__': ImageCV(), Beamformer(), Loader(), + Robot(), ] for p in ps: pps.append(multiprocessing.Process(target=p)) diff --git a/src/nodes/MainUI.py b/src/nodes/MainUI.py index 777bca9..e4f6177 100644 --- a/src/nodes/MainUI.py +++ b/src/nodes/MainUI.py @@ -10,7 +10,7 @@ from ZMQReceiver import ZMQReceiver from nodes.Node import Node from utils.Msg import KillMsg, Msg, ImageArgMsg, SelectSeqMsg, SeqIdMinMax, MoveAxisMsg, SeqListMsg, SetBaseMsg, \ SeqMetaMsg, SetPlayMode, DeviceConnectedMsg, DeviceEnabledMsg, DeviceOnlineMsg, SetDeviceEnabledMsg, \ - SetDeviceConnectedMsg, DeviceConfigListMsg, SetDeviceConfigMsg, SetRecordMsg + SetDeviceConnectedMsg, DeviceConfigListMsg, SetDeviceConfigMsg, SetRecordMsg, RobotRtsiMsg from utils.RfFile import RfSequenceMeta @@ -29,7 +29,6 @@ class Adv(QMainWindow, Ui_MainWindow): self.arg = ImageArgMsg('ui', t_start=0, t_end=1499) self.b_base.clicked.connect(lambda: self.p.send(SetBaseMsg(self.l_base.text()))) self.seq_meta: RfSequenceMeta | None = None - self.b_exit.clicked.connect(lambda: self.p.send(KillMsg(''))) self.b_play_live.clicked.connect(self.on_play_live) self.b_play_playback.clicked.connect(self.on_play_playback) self.b_record.clicked.connect(self.on_record) @@ -122,7 +121,14 @@ class Adv(QMainWindow, Ui_MainWindow): elif isinstance(msg, DeviceConfigListMsg): for name, txt in msg.arr: self.c_seq_meta.addItem(name, txt) + elif isinstance(msg, RobotRtsiMsg): + self.l_robot_pos.setText(','.join([f'{v:.3f}' for v in msg.pos])) + self.l_robot_force.setText(','.join([f'{v:.3f}' for v in msg.force])) + def closeEvent(self, event): + self.p.send(KillMsg('')) + # event.accept() + # event.ignore() @QtCore.pyqtSlot(int) def on_t_start(self, v): if self.s_t_end.sender() is None: @@ -155,7 +161,8 @@ class Adv(QMainWindow, Ui_MainWindow): class MainUI(Node): topics = [ImageArgMsg, SeqIdMinMax, MoveAxisMsg, SeqListMsg, SeqMetaMsg, - DeviceConnectedMsg, DeviceEnabledMsg, DeviceOnlineMsg, DeviceConfigListMsg] + DeviceConnectedMsg, DeviceEnabledMsg, DeviceOnlineMsg, DeviceConfigListMsg, + RobotRtsiMsg] def __init__(self, level=logging.INFO): super().__init__(level=level) diff --git a/src/nodes/Robot.py b/src/nodes/Robot.py new file mode 100644 index 0000000..245aea5 --- /dev/null +++ b/src/nodes/Robot.py @@ -0,0 +1,63 @@ +import logging +import threading + +import zmq + +from nodes.Node import Node +from utils.Msg import KillMsg, RobotRtsiMsg, Msg +from utils.rtsi import rtsi +from utils.rtsi.serialize import DataObject + +logger = logging.getLogger(__name__) + + +class Robot(Node): + topics = [] + + def __init__(self): + super(Robot, self).__init__() + self.rtsi_thread_stop = False + + 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_joint_positions,actual_TCP_force', 125) # 输出订阅,配方1 + self.rt.start() # rtsi 开始 + while not self.rtsi_thread_stop: + recv_out: DataObject = self.rt.get_output_data() + if recv_out is None: + continue + if recv_out.recipe_id == output1.id: + rtsi_push_socket.send(RobotRtsiMsg( + pos=recv_out.actual_joint_positions, + force=recv_out.actual_TCP_force, + ).encode_msg()) + self.rt.disconnect() + + def loop(self): + t = threading.Thread(target=self.rtsi_thread) + t.start() + rtsi_pull_socket = self.context.socket(zmq.PULL) + rtsi_pull_socket.connect('inproc://rtsi') + self.c.poller.register(rtsi_pull_socket, zmq.POLLIN) + while True: + socks = dict(self.c.poller.poll()) + if rtsi_pull_socket in socks and socks[rtsi_pull_socket] == zmq.POLLIN: + msg = Msg.decode_msg(rtsi_pull_socket.recv()) + self.send(msg) + if self.c.sub in socks and socks[self.c.sub] == zmq.POLLIN: + msg = self.recv() + if isinstance(msg, KillMsg): + if msg.name == '': + self.rtsi_thread_stop = True + return + + +if __name__ == '__main__': + r = Robot() + r() diff --git a/src/ui/Main.py b/src/ui/Main.py index b9ecd8b..e2ab047 100644 --- a/src/ui/Main.py +++ b/src/ui/Main.py @@ -17,117 +17,53 @@ class Ui_MainWindow(object): self.centralwidget.setObjectName("centralwidget") self.gridLayout = QtWidgets.QGridLayout(self.centralwidget) self.gridLayout.setObjectName("gridLayout") - self.gridLayout_3 = QtWidgets.QGridLayout() - self.gridLayout_3.setObjectName("gridLayout_3") - self.c_seq_meta = QtWidgets.QComboBox(parent=self.centralwidget) - self.c_seq_meta.setObjectName("c_seq_meta") - self.gridLayout_3.addWidget(self.c_seq_meta, 3, 0, 1, 1) - self.b_device_enabled = QtWidgets.QPushButton(parent=self.centralwidget) - self.b_device_enabled.setObjectName("b_device_enabled") - self.gridLayout_3.addWidget(self.b_device_enabled, 2, 0, 1, 1) - self.b_device_connected = QtWidgets.QPushButton(parent=self.centralwidget) - self.b_device_connected.setObjectName("b_device_connected") - self.gridLayout_3.addWidget(self.b_device_connected, 1, 0, 1, 1) - self.l_online = QtWidgets.QLabel(parent=self.centralwidget) - self.l_online.setStyleSheet("background-color: pink;") - self.l_online.setObjectName("l_online") - self.gridLayout_3.addWidget(self.l_online, 0, 0, 1, 1) - self.gridLayout.addLayout(self.gridLayout_3, 0, 0, 1, 1) - self.gridLayout_5 = QtWidgets.QGridLayout() - self.gridLayout_5.setObjectName("gridLayout_5") - self.spinBox_2 = QtWidgets.QSpinBox(parent=self.centralwidget) - self.spinBox_2.setObjectName("spinBox_2") - self.gridLayout_5.addWidget(self.spinBox_2, 3, 2, 1, 1) - self.label_7 = QtWidgets.QLabel(parent=self.centralwidget) - self.label_7.setObjectName("label_7") - self.gridLayout_5.addWidget(self.label_7, 4, 0, 1, 1) - self.label_6 = QtWidgets.QLabel(parent=self.centralwidget) - self.label_6.setObjectName("label_6") - self.gridLayout_5.addWidget(self.label_6, 3, 0, 1, 1) - self.s_t_end = QtWidgets.QSlider(parent=self.centralwidget) - self.s_t_end.setMinimum(1) - self.s_t_end.setMaximum(1500) - self.s_t_end.setOrientation(QtCore.Qt.Orientation.Horizontal) - self.s_t_end.setObjectName("s_t_end") - self.gridLayout_5.addWidget(self.s_t_end, 2, 1, 1, 1) - self.label_2 = QtWidgets.QLabel(parent=self.centralwidget) - self.label_2.setObjectName("label_2") - self.gridLayout_5.addWidget(self.label_2, 0, 0, 1, 3) - self.horizontalSlider_4 = QtWidgets.QSlider(parent=self.centralwidget) - self.horizontalSlider_4.setOrientation(QtCore.Qt.Orientation.Horizontal) - self.horizontalSlider_4.setObjectName("horizontalSlider_4") - self.gridLayout_5.addWidget(self.horizontalSlider_4, 4, 1, 1, 1) - spacerItem = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Policy.Minimum, QtWidgets.QSizePolicy.Policy.Expanding) - self.gridLayout_5.addItem(spacerItem, 5, 1, 1, 1) - self.spinBox = QtWidgets.QSpinBox(parent=self.centralwidget) - self.spinBox.setObjectName("spinBox") - self.gridLayout_5.addWidget(self.spinBox, 2, 2, 1, 1) - self.label_5 = QtWidgets.QLabel(parent=self.centralwidget) - self.label_5.setObjectName("label_5") - self.gridLayout_5.addWidget(self.label_5, 2, 0, 1, 1) - self.horizontalSlider_2 = QtWidgets.QSlider(parent=self.centralwidget) - self.horizontalSlider_2.setOrientation(QtCore.Qt.Orientation.Horizontal) - self.horizontalSlider_2.setObjectName("horizontalSlider_2") - self.gridLayout_5.addWidget(self.horizontalSlider_2, 3, 1, 1, 1) - self.spinBox_3 = QtWidgets.QSpinBox(parent=self.centralwidget) - self.spinBox_3.setObjectName("spinBox_3") - self.gridLayout_5.addWidget(self.spinBox_3, 4, 2, 1, 1) - self.spinBox_12 = QtWidgets.QSpinBox(parent=self.centralwidget) - self.spinBox_12.setObjectName("spinBox_12") - self.gridLayout_5.addWidget(self.spinBox_12, 1, 2, 1, 1) - self.s_t_start = QtWidgets.QSlider(parent=self.centralwidget) - self.s_t_start.setOrientation(QtCore.Qt.Orientation.Horizontal) - self.s_t_start.setObjectName("s_t_start") - self.gridLayout_5.addWidget(self.s_t_start, 1, 1, 1, 1) - self.label_15 = QtWidgets.QLabel(parent=self.centralwidget) - self.label_15.setObjectName("label_15") - self.gridLayout_5.addWidget(self.label_15, 1, 0, 1, 1) - self.gridLayout.addLayout(self.gridLayout_5, 1, 0, 1, 1) - self.gridLayout_2 = QtWidgets.QGridLayout() - self.gridLayout_2.setObjectName("gridLayout_2") - self.l_record_commit = QtWidgets.QLineEdit(parent=self.centralwidget) - self.l_record_commit.setEnabled(False) - self.l_record_commit.setObjectName("l_record_commit") - self.gridLayout_2.addWidget(self.l_record_commit, 1, 1, 1, 1) + self.gridLayout_7 = QtWidgets.QGridLayout() + self.gridLayout_7.setObjectName("gridLayout_7") + self.s_sid = QtWidgets.QSlider(parent=self.centralwidget) + self.s_sid.setOrientation(QtCore.Qt.Orientation.Horizontal) + self.s_sid.setObjectName("s_sid") + self.gridLayout_7.addWidget(self.s_sid, 2, 1, 1, 1) + self.label_4 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_4.setObjectName("label_4") + self.gridLayout_7.addWidget(self.label_4, 2, 0, 1, 1) + self.spinBox_7 = QtWidgets.QSpinBox(parent=self.centralwidget) + self.spinBox_7.setObjectName("spinBox_7") + self.gridLayout_7.addWidget(self.spinBox_7, 2, 2, 1, 1) self.b_play_playback = QtWidgets.QPushButton(parent=self.centralwidget) self.b_play_playback.setEnabled(False) self.b_play_playback.setStyleSheet("background-color : red") self.b_play_playback.setObjectName("b_play_playback") - self.gridLayout_2.addWidget(self.b_play_playback, 2, 0, 1, 1) - self.b_record = QtWidgets.QPushButton(parent=self.centralwidget) - self.b_record.setEnabled(False) - self.b_record.setObjectName("b_record") - self.gridLayout_2.addWidget(self.b_record, 1, 2, 1, 1) - self.s_sid = QtWidgets.QSlider(parent=self.centralwidget) - self.s_sid.setOrientation(QtCore.Qt.Orientation.Horizontal) - self.s_sid.setObjectName("s_sid") - self.gridLayout_2.addWidget(self.s_sid, 3, 1, 1, 1) - self.label_4 = QtWidgets.QLabel(parent=self.centralwidget) - self.label_4.setObjectName("label_4") - self.gridLayout_2.addWidget(self.label_4, 3, 0, 1, 1) - self.b_base = QtWidgets.QPushButton(parent=self.centralwidget) - self.b_base.setObjectName("b_base") - self.gridLayout_2.addWidget(self.b_base, 0, 2, 1, 1) - self.l_base = QtWidgets.QLineEdit(parent=self.centralwidget) - self.l_base.setObjectName("l_base") - self.gridLayout_2.addWidget(self.l_base, 0, 1, 1, 1) - self.label = QtWidgets.QLabel(parent=self.centralwidget) - self.label.setObjectName("label") - self.gridLayout_2.addWidget(self.label, 0, 0, 1, 1) - self.b_play_live = QtWidgets.QPushButton(parent=self.centralwidget) - self.b_play_live.setObjectName("b_play_live") - self.gridLayout_2.addWidget(self.b_play_live, 1, 0, 1, 1) + self.gridLayout_7.addWidget(self.b_play_playback, 0, 0, 1, 3) self.comboBox = QtWidgets.QComboBox(parent=self.centralwidget) self.comboBox.setEnabled(False) self.comboBox.setEditable(False) self.comboBox.setObjectName("comboBox") self.comboBox.addItem("") - self.gridLayout_2.addWidget(self.comboBox, 2, 1, 1, 2) - self.spinBox_7 = QtWidgets.QSpinBox(parent=self.centralwidget) - self.spinBox_7.setObjectName("spinBox_7") - self.gridLayout_2.addWidget(self.spinBox_7, 3, 2, 1, 1) - self.gridLayout_2.setColumnStretch(1, 1) - self.gridLayout.addLayout(self.gridLayout_2, 0, 1, 1, 1) + self.gridLayout_7.addWidget(self.comboBox, 1, 1, 1, 2) + self.label_16 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_16.setObjectName("label_16") + self.gridLayout_7.addWidget(self.label_16, 1, 0, 1, 1) + self.gridLayout.addLayout(self.gridLayout_7, 1, 1, 1, 1) + self.gridLayout_6 = QtWidgets.QGridLayout() + self.gridLayout_6.setObjectName("gridLayout_6") + self.l_record_commit = QtWidgets.QLineEdit(parent=self.centralwidget) + self.l_record_commit.setEnabled(False) + self.l_record_commit.setObjectName("l_record_commit") + self.gridLayout_6.addWidget(self.l_record_commit, 1, 1, 1, 1) + self.b_record = QtWidgets.QPushButton(parent=self.centralwidget) + self.b_record.setEnabled(False) + self.b_record.setObjectName("b_record") + self.gridLayout_6.addWidget(self.b_record, 1, 2, 1, 1) + self.label_17 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_17.setObjectName("label_17") + self.gridLayout_6.addWidget(self.label_17, 1, 0, 1, 1) + self.b_play_live = QtWidgets.QPushButton(parent=self.centralwidget) + self.b_play_live.setObjectName("b_play_live") + self.gridLayout_6.addWidget(self.b_play_live, 0, 0, 1, 3) + self.label_18 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_18.setObjectName("label_18") + self.gridLayout_6.addWidget(self.label_18, 2, 0, 1, 3) + self.gridLayout.addLayout(self.gridLayout_6, 1, 0, 1, 1) self.gridLayout_4 = QtWidgets.QGridLayout() self.gridLayout_4.setObjectName("gridLayout_4") self.spinBox_4 = QtWidgets.QSpinBox(parent=self.centralwidget) @@ -161,13 +97,19 @@ class Ui_MainWindow(object): self.label_9 = QtWidgets.QLabel(parent=self.centralwidget) self.label_9.setObjectName("label_9") self.gridLayout_4.addWidget(self.label_9, 2, 0, 1, 1) - spacerItem1 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Policy.Minimum, QtWidgets.QSizePolicy.Policy.Expanding) - self.gridLayout_4.addItem(spacerItem1, 8, 1, 1, 1) + spacerItem = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Policy.Minimum, QtWidgets.QSizePolicy.Policy.Expanding) + self.gridLayout_4.addItem(spacerItem, 8, 1, 1, 1) self.horizontalSlider_7 = QtWidgets.QSlider(parent=self.centralwidget) self.horizontalSlider_7.setOrientation(QtCore.Qt.Orientation.Horizontal) self.horizontalSlider_7.setObjectName("horizontalSlider_7") self.gridLayout_4.addWidget(self.horizontalSlider_7, 7, 1, 1, 1) self.label_3 = QtWidgets.QLabel(parent=self.centralwidget) + font = QtGui.QFont() + font.setPointSize(20) + font.setBold(False) + font.setWeight(50) + self.label_3.setFont(font) + self.label_3.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) self.label_3.setObjectName("label_3") self.gridLayout_4.addWidget(self.label_3, 0, 0, 1, 3) self.label_11 = QtWidgets.QLabel(parent=self.centralwidget) @@ -205,10 +147,126 @@ class Ui_MainWindow(object): self.spinBox_11 = QtWidgets.QSpinBox(parent=self.centralwidget) self.spinBox_11.setObjectName("spinBox_11") self.gridLayout_4.addWidget(self.spinBox_11, 7, 2, 1, 1) - self.gridLayout.addLayout(self.gridLayout_4, 1, 1, 1, 1) - self.b_exit = QtWidgets.QPushButton(parent=self.centralwidget) - self.b_exit.setObjectName("b_exit") - self.gridLayout.addWidget(self.b_exit, 2, 0, 1, 2) + self.gridLayout.addLayout(self.gridLayout_4, 2, 1, 1, 1) + self.gridLayout_5 = QtWidgets.QGridLayout() + self.gridLayout_5.setObjectName("gridLayout_5") + self.spinBox_2 = QtWidgets.QSpinBox(parent=self.centralwidget) + self.spinBox_2.setObjectName("spinBox_2") + self.gridLayout_5.addWidget(self.spinBox_2, 3, 2, 1, 1) + self.label_7 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_7.setObjectName("label_7") + self.gridLayout_5.addWidget(self.label_7, 4, 0, 1, 1) + self.label_6 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_6.setObjectName("label_6") + self.gridLayout_5.addWidget(self.label_6, 3, 0, 1, 1) + self.s_t_end = QtWidgets.QSlider(parent=self.centralwidget) + self.s_t_end.setMinimum(1) + self.s_t_end.setMaximum(1500) + self.s_t_end.setOrientation(QtCore.Qt.Orientation.Horizontal) + self.s_t_end.setObjectName("s_t_end") + self.gridLayout_5.addWidget(self.s_t_end, 2, 1, 1, 1) + self.label_2 = QtWidgets.QLabel(parent=self.centralwidget) + font = QtGui.QFont() + font.setPointSize(20) + font.setBold(False) + font.setWeight(50) + self.label_2.setFont(font) + self.label_2.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.label_2.setObjectName("label_2") + self.gridLayout_5.addWidget(self.label_2, 0, 0, 1, 3) + self.horizontalSlider_4 = QtWidgets.QSlider(parent=self.centralwidget) + self.horizontalSlider_4.setOrientation(QtCore.Qt.Orientation.Horizontal) + self.horizontalSlider_4.setObjectName("horizontalSlider_4") + self.gridLayout_5.addWidget(self.horizontalSlider_4, 4, 1, 1, 1) + spacerItem1 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Policy.Minimum, QtWidgets.QSizePolicy.Policy.Expanding) + self.gridLayout_5.addItem(spacerItem1, 5, 1, 1, 1) + self.spinBox = QtWidgets.QSpinBox(parent=self.centralwidget) + self.spinBox.setObjectName("spinBox") + self.gridLayout_5.addWidget(self.spinBox, 2, 2, 1, 1) + self.label_5 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_5.setObjectName("label_5") + self.gridLayout_5.addWidget(self.label_5, 2, 0, 1, 1) + self.horizontalSlider_2 = QtWidgets.QSlider(parent=self.centralwidget) + self.horizontalSlider_2.setOrientation(QtCore.Qt.Orientation.Horizontal) + self.horizontalSlider_2.setObjectName("horizontalSlider_2") + self.gridLayout_5.addWidget(self.horizontalSlider_2, 3, 1, 1, 1) + self.spinBox_3 = QtWidgets.QSpinBox(parent=self.centralwidget) + self.spinBox_3.setObjectName("spinBox_3") + self.gridLayout_5.addWidget(self.spinBox_3, 4, 2, 1, 1) + self.spinBox_12 = QtWidgets.QSpinBox(parent=self.centralwidget) + self.spinBox_12.setObjectName("spinBox_12") + self.gridLayout_5.addWidget(self.spinBox_12, 1, 2, 1, 1) + self.s_t_start = QtWidgets.QSlider(parent=self.centralwidget) + self.s_t_start.setOrientation(QtCore.Qt.Orientation.Horizontal) + self.s_t_start.setObjectName("s_t_start") + self.gridLayout_5.addWidget(self.s_t_start, 1, 1, 1, 1) + self.label_15 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_15.setObjectName("label_15") + self.gridLayout_5.addWidget(self.label_15, 1, 0, 1, 1) + self.gridLayout.addLayout(self.gridLayout_5, 2, 0, 1, 1) + self.gridLayout_3 = QtWidgets.QGridLayout() + self.gridLayout_3.setObjectName("gridLayout_3") + self.c_seq_meta = QtWidgets.QComboBox(parent=self.centralwidget) + self.c_seq_meta.setObjectName("c_seq_meta") + self.gridLayout_3.addWidget(self.c_seq_meta, 3, 0, 1, 1) + self.b_device_enabled = QtWidgets.QPushButton(parent=self.centralwidget) + self.b_device_enabled.setObjectName("b_device_enabled") + self.gridLayout_3.addWidget(self.b_device_enabled, 2, 0, 1, 1) + self.b_device_connected = QtWidgets.QPushButton(parent=self.centralwidget) + self.b_device_connected.setObjectName("b_device_connected") + self.gridLayout_3.addWidget(self.b_device_connected, 1, 0, 1, 1) + self.l_online = QtWidgets.QLabel(parent=self.centralwidget) + font = QtGui.QFont() + font.setPointSize(15) + self.l_online.setFont(font) + self.l_online.setStyleSheet("background-color: pink;") + self.l_online.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.l_online.setObjectName("l_online") + self.gridLayout_3.addWidget(self.l_online, 0, 0, 1, 1) + self.gridLayout.addLayout(self.gridLayout_3, 0, 0, 1, 1) + self.horizontalLayout_4 = QtWidgets.QHBoxLayout() + self.horizontalLayout_4.setObjectName("horizontalLayout_4") + self.label = QtWidgets.QLabel(parent=self.centralwidget) + self.label.setObjectName("label") + self.horizontalLayout_4.addWidget(self.label) + self.l_base = QtWidgets.QLineEdit(parent=self.centralwidget) + self.l_base.setObjectName("l_base") + self.horizontalLayout_4.addWidget(self.l_base) + self.b_base = QtWidgets.QPushButton(parent=self.centralwidget) + self.b_base.setObjectName("b_base") + self.horizontalLayout_4.addWidget(self.b_base) + self.gridLayout.addLayout(self.horizontalLayout_4, 3, 0, 1, 2) + self.gridLayout_2 = QtWidgets.QGridLayout() + self.gridLayout_2.setObjectName("gridLayout_2") + self.l_robot_pos = QtWidgets.QLabel(parent=self.centralwidget) + font = QtGui.QFont() + font.setPointSize(15) + self.l_robot_pos.setFont(font) + self.l_robot_pos.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.l_robot_pos.setObjectName("l_robot_pos") + self.gridLayout_2.addWidget(self.l_robot_pos, 1, 1, 1, 1) + self.l_robot_force = QtWidgets.QLabel(parent=self.centralwidget) + font = QtGui.QFont() + font.setPointSize(15) + self.l_robot_force.setFont(font) + self.l_robot_force.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.l_robot_force.setObjectName("l_robot_force") + self.gridLayout_2.addWidget(self.l_robot_force, 2, 1, 1, 1) + self.label_22 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_22.setObjectName("label_22") + self.gridLayout_2.addWidget(self.label_22, 1, 0, 1, 1) + self.label_23 = QtWidgets.QLabel(parent=self.centralwidget) + self.label_23.setObjectName("label_23") + self.gridLayout_2.addWidget(self.label_23, 2, 0, 1, 1) + self.label_19 = QtWidgets.QLabel(parent=self.centralwidget) + font = QtGui.QFont() + font.setPointSize(15) + self.label_19.setFont(font) + self.label_19.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + self.label_19.setObjectName("label_19") + self.gridLayout_2.addWidget(self.label_19, 0, 0, 1, 2) + self.gridLayout_2.setColumnStretch(1, 1) + self.gridLayout.addLayout(self.gridLayout_2, 0, 1, 1, 1) MainWindow.setCentralWidget(self.centralwidget) self.menubar = QtWidgets.QMenuBar(parent=MainWindow) self.menubar.setGeometry(QtCore.QRect(0, 0, 1177, 30)) @@ -224,22 +282,14 @@ class Ui_MainWindow(object): def retranslateUi(self, MainWindow): _translate = QtCore.QCoreApplication.translate MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow")) - self.b_device_enabled.setText(_translate("MainWindow", "Beam")) - self.b_device_connected.setText(_translate("MainWindow", "Connection")) - self.l_online.setText(_translate("MainWindow", "Device Offline")) - self.label_7.setText(_translate("MainWindow", "TextLabel")) - self.label_6.setText(_translate("MainWindow", "TextLabel")) - self.label_2.setText(_translate("MainWindow", "Imaging")) - self.label_5.setText(_translate("MainWindow", "t_end")) - self.label_15.setText(_translate("MainWindow", "t_start")) - self.b_play_playback.setText(_translate("MainWindow", "Playback")) - self.b_record.setText(_translate("MainWindow", "Record")) self.label_4.setText(_translate("MainWindow", "Frame ID")) - self.b_base.setText(_translate("MainWindow", "SetBase")) - self.l_base.setText(_translate("MainWindow", "/mnt/16T/private_dataset/us/")) - self.label.setText(_translate("MainWindow", "Base Path")) - self.b_play_live.setText(_translate("MainWindow", "Live")) + self.b_play_playback.setText(_translate("MainWindow", "Playback")) self.comboBox.setItemText(0, _translate("MainWindow", "Unset")) + self.label_16.setText(_translate("MainWindow", "SeqName")) + self.b_record.setText(_translate("MainWindow", "Record")) + self.label_17.setText(_translate("MainWindow", "RecordName")) + self.b_play_live.setText(_translate("MainWindow", "Live")) + self.label_18.setText(_translate("MainWindow", "Space left 1000GB")) self.label_10.setText(_translate("MainWindow", "Z")) self.label_9.setText(_translate("MainWindow", "Y")) self.label_3.setText(_translate("MainWindow", "Probe Position")) @@ -248,4 +298,19 @@ class Ui_MainWindow(object): self.label_12.setText(_translate("MainWindow", "Roll")) self.label_13.setText(_translate("MainWindow", "Pitch")) self.label_14.setText(_translate("MainWindow", "Yal")) - self.b_exit.setText(_translate("MainWindow", "EXIT")) + self.label_7.setText(_translate("MainWindow", "TextLabel")) + self.label_6.setText(_translate("MainWindow", "TextLabel")) + self.label_2.setText(_translate("MainWindow", "Imaging")) + self.label_5.setText(_translate("MainWindow", "t_end")) + self.label_15.setText(_translate("MainWindow", "t_start")) + self.b_device_enabled.setText(_translate("MainWindow", "Beam")) + self.b_device_connected.setText(_translate("MainWindow", "Connection")) + self.l_online.setText(_translate("MainWindow", "Device Offline")) + self.label.setText(_translate("MainWindow", "Base Path")) + self.l_base.setText(_translate("MainWindow", "/mnt/16T/private_dataset/us/")) + self.b_base.setText(_translate("MainWindow", "Open")) + self.l_robot_pos.setText(_translate("MainWindow", "0,0,0,0,0,0")) + self.l_robot_force.setText(_translate("MainWindow", "0,0,0,0,0,0")) + self.label_22.setText(_translate("MainWindow", "Position")) + self.label_23.setText(_translate("MainWindow", "Force")) + self.label_19.setText(_translate("MainWindow", "Robot Offline")) diff --git a/src/ui/Main.ui b/src/ui/Main.ui index 23b7c71..21c6062 100644 --- a/src/ui/Main.ui +++ b/src/ui/Main.ui @@ -14,146 +14,27 @@ MainWindow - - - - - + + + + + + + Qt::Horizontal + + - + - Beam + Frame ID - - - - Connection - - - - - - - background-color: pink; - - - Device Offline - - - - - - - - - - - - - - TextLabel - - - - - - - TextLabel - - - - - - - 1 - - - 1500 - - - Qt::Horizontal - - - - - - - Imaging - - - - - - - Qt::Horizontal - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - + - - - - t_end - - - - - - - Qt::Horizontal - - - - - - - - - - - - - Qt::Horizontal - - - - - - - t_start - - - - - - - - - - - false - - - - + false @@ -166,59 +47,7 @@ - - - - false - - - Record - - - - - - - Qt::Horizontal - - - - - - - Frame ID - - - - - - - SetBase - - - - - - - /mnt/16T/private_dataset/us/ - - - - - - - Base Path - - - - - - - Live - - - - + false @@ -233,12 +62,58 @@ - - + + + + SeqName + + - + + + + + + false + + + + + + + false + + + Record + + + + + + + RecordName + + + + + + + Live + + + + + + + Space left 1000GB + + + + + + @@ -313,9 +188,19 @@ + + + 20 + 50 + false + + Probe Position + + Qt::AlignCenter + @@ -381,12 +266,243 @@ - - - - EXIT - - + + + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + 1 + + + 1500 + + + Qt::Horizontal + + + + + + + + 20 + 50 + false + + + + Imaging + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + t_end + + + + + + + Qt::Horizontal + + + + + + + + + + + + + Qt::Horizontal + + + + + + + t_start + + + + + + + + + + + + + + Beam + + + + + + + Connection + + + + + + + + 15 + + + + background-color: pink; + + + Device Offline + + + Qt::AlignCenter + + + + + + + + + + + Base Path + + + + + + + /mnt/16T/private_dataset/us/ + + + + + + + Open + + + + + + + + + + + + 15 + + + + 0,0,0,0,0,0 + + + Qt::AlignCenter + + + + + + + + 15 + + + + 0,0,0,0,0,0 + + + Qt::AlignCenter + + + + + + + Position + + + + + + + Force + + + + + + + + 15 + + + + background-color: pink; + + + Robot Offline + + + Qt::AlignCenter + + + + diff --git a/src/ui/Main.ui.bak b/src/ui/Main.ui.bak new file mode 100644 index 0000000..14764b6 --- /dev/null +++ b/src/ui/Main.ui.bak @@ -0,0 +1,442 @@ + + + MainWindow + + + + 0 + 0 + 1177 + 910 + + + + MainWindow + + + + + + + EXIT + + + + + + + + + + + + TextLabel + + + + + + + TextLabel + + + + + + + 1 + + + 1500 + + + Qt::Horizontal + + + + + + + + 20 + 50 + false + + + + Imaging + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + t_end + + + + + + + Qt::Horizontal + + + + + + + + + + + + + Qt::Horizontal + + + + + + + t_start + + + + + + + + + + + + + + + + + Z + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + + + + + + + Qt::Horizontal + + + + + + + Y + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + + + + + + 20 + 50 + false + + + + Probe Position + + + Qt::AlignCenter + + + + + + + E + + + + + + + X + + + + + + + Qt::Horizontal + + + + + + + Qt::Horizontal + + + + + + + Roll + + + + + + + Pitch + + + + + + + Yal + + + + + + + + + + + + + + + + + + + + + TextLabel + + + + + + + + + + + + Beam + + + + + + + Connection + + + + + + + + 15 + + + + background-color: pink; + + + Device Offline + + + Qt::AlignCenter + + + + + + + + + + + false + + + + + + + false + + + background-color : red + + + Playback + + + + + + + false + + + Record + + + + + + + Qt::Horizontal + + + + + + + Frame ID + + + + + + + SetBase + + + + + + + /mnt/16T/private_dataset/us/ + + + + + + + Base Path + + + + + + + Live + + + + + + + false + + + false + + + + Unset + + + + + + + + + + + + + + + 0 + 0 + 1177 + 30 + + + + + + + + diff --git a/src/utils/Msg.py b/src/utils/Msg.py index 06a491a..e43c7d8 100644 --- a/src/utils/Msg.py +++ b/src/utils/Msg.py @@ -27,6 +27,7 @@ class BG(Enum): DeviceOnlineMsg = auto() DeviceConfigListMsg = auto() SetRecordMsg = auto() + RobotRtsiMsg = auto() class Msg: @@ -182,3 +183,9 @@ class BMMsg(Msg): struct.unpack('I', data[:4])[0], data[4:] ) + + +@dataclasses.dataclass +class RobotRtsiMsg(Msg): + pos: tuple[float, float, float, float, float, float] + force: tuple[float, float, float, float, float, float] diff --git a/src/utils/rtsi/__init__.py b/src/utils/rtsi/__init__.py new file mode 100644 index 0000000..697d8cd --- /dev/null +++ b/src/utils/rtsi/__init__.py @@ -0,0 +1 @@ +from .rtsi import * \ No newline at end of file diff --git a/src/utils/rtsi/rtsi.py b/src/utils/rtsi/rtsi.py new file mode 100644 index 0000000..7df8d6f --- /dev/null +++ b/src/utils/rtsi/rtsi.py @@ -0,0 +1,390 @@ +import struct +import socket +import select +import sys +import logging + +import utils.rtsi.serialize as serialize + +DEFAULT_TIMEOUT = 10.0 + +LOGNAME = 'rtsi' +_log = logging.getLogger(LOGNAME) + + +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_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 + self.port = port + self.__conn_state = ConnectionState.DISCONNECTED + self.__sock = None + self.__output_config = {} + self.__input_config = {} + self.__skipped_package_count = 0 + self.__protocolVersion = RTSI_PROTOCOL_VERSION_1 + + def connect(self): + if self.__sock: + return + + 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) + self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + self.__sock.settimeout(DEFAULT_TIMEOUT) + self.__skipped_package_count = 0 + self.__sock.connect((self.hostname, self.port)) + self.__conn_state = ConnectionState.CONNECTED + except (socket.timeout, socket.error): + self.__sock = None + raise + + def disconnect(self): + if self.__sock: + self.__sock.close() + self.__sock = None + self.__conn_state = ConnectionState.DISCONNECTED + + def is_connected(self): + return self.__conn_state is not ConnectionState.DISCONNECTED + + def controller_version(self): + 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)) + 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() + return version.major, version.minor, version.bugfix, version.build + return None, None, None, None + + def version_check(self): + cmd = Command.RTSI_REQUEST_PROTOCOL_VERSION + 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): + cmd = Command.RTSI_CONTROL_PACKAGE_SETUP_INPUTS + payload = variables.encode() + result = self.__sendAndReceive(cmd, payload) + 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): + cmd = Command.RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS + payload = struct.pack('>d', frequency) + payload = payload + variables.encode() + result = self.__sendAndReceive(cmd, payload) + result.names = variables.split(',') + self.__output_config[result.id] = result + return result + + def start(self): + cmd = Command.RTSI_CONTROL_PACKAGE_START + success = self.__sendAndReceive(cmd) + if success: + _log.info('RTSI synchronization started') + self.__conn_state = ConnectionState.STARTED + else: + _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') + self.__conn_state = ConnectionState.PAUSED + else: + _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') + return + if not input_data.recipe_id 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 + and only the newest one will be returned. Will block untill a package + is received or the connection is lost + """ + if self.__conn_state != ConnectionState.STARTED: + 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): + """Recieve the next data package. + If muliple packages has been received they are buffered and will + be returned on subsequent calls to this function. + Returns None if no data is available. + """ + + if self._rtsi__output_config is None: + logging.error("Output configuration not initialized") + return None + + try: + while ( + self.is_connected() + and (buffer_limit == None or len(self.__buf) < buffer_limit) + and self.__recv_to_buffer(0) + ): + pass + except RTSIException as e: + data = self.__recv_from_buffer(Command.RTSI_DATA_PACKAGE, False) + if data == None: + raise e + else: + data = self.__recv_from_buffer(Command.RTSI_DATA_PACKAGE, False) + + return data + + 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)) + payload = struct.pack(fmt, len(message), message, len(source), source, type) + return self.__sendall(cmd, payload) + + def __on_packet(self, cmd, payload): + if cmd == Command.RTSI_REQUEST_PROTOCOL_VERSION: + return self.__unpack_protocol_version_package(payload) + elif cmd == Command.RTSI_GET_ELITECONTROL_VERSION: + return self.__unpack_elitecontrol_version_package(payload) + elif cmd == Command.RTSI_TEXT_MESSAGE: + return self.__unpack_text_message(payload) + elif cmd == Command.RTSI_CONTROL_PACKAGE_SETUP_OUTPUTS: + return self.__unpack_setup_outputs_package(payload) + elif cmd == Command.RTSI_CONTROL_PACKAGE_SETUP_INPUTS: + return self.__unpack_setup_inputs_package(payload) + elif cmd == Command.RTSI_CONTROL_PACKAGE_START: + return self.__unpack_start_package(payload) + elif cmd == Command.RTSI_CONTROL_PACKAGE_PAUSE: + return self.__unpack_pause_package(payload) + 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)) + + 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' + 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') + return False + + _, writable, _ = select.select([], [self.__sock], [], DEFAULT_TIMEOUT) + if len(writable): + self.__sock.sendall(buf) + return True + else: + self.__trigger_disconnected() + return False + + def has_data(self): + timeout = 0 + readable, _, _ = select.select([self.__sock], [], [], timeout) + return len(readable)!=0 + + def __recv(self, command, binary=False): + while self.is_connected(): + try: + self.__recv_to_buffer(DEFAULT_TIMEOUT) + except RTSITimeoutException: + return None + + # unpack_from requires a buffer of at least 3 bytes + while len(self.__buf) >= 3: + # Attempts to extract a packet + 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:] + 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)') + self.__skipped_package_count += 1 + continue + if packet_header.command == command: + if(binary): + return packet[1:] + + return data + else: + _log.debug('skipping package(2)') + else: + break + 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(4096) + #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') + + 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) + 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: + # Attempts to extract a packet + 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:] + data = self.__on_packet(packet_header.command, packet) + if packet_header.command == command: + if(binary): + return packet[1:] + + return data + else: + print('skipping package(2)') + else: + return None + + def __trigger_disconnected(self): + _log.info("RTSI disconnected") + self.disconnect() #clean-up + + def __unpack_protocol_version_package(self, payload): + if len(payload) != 1: + _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') + 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') + return None + 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) + elif msg.level == serialize.Message.WARNING_MESSAGE: + _log.warning(msg.source + ': ' + msg.message) + elif msg.level == serialize.Message.INFO_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') + 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') + 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') + 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') + 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') + return None + output = output_config.unpack(payload) + return output + + def __list_equals(self, l1, l2): + if len(l1) != len(l2): + return False + for i in range(len((l1))): + if l1[i] != l2[i]: + return False + return True + + @property + def skipped_package_count(self): + """The skipped package count, resets on connect""" + return self.__skipped_package_count diff --git a/src/utils/rtsi/serialize.py b/src/utils/rtsi/serialize.py new file mode 100644 index 0000000..7bf6071 --- /dev/null +++ b/src/utils/rtsi/serialize.py @@ -0,0 +1,182 @@ +import struct + + +class ControlHeader(object): + __slots__ = ['command', 'size',] + + @staticmethod + def unpack(buf): + rmd = ControlHeader() + (rmd.size, rmd.command) = struct.unpack_from('>HB', buf) + return rmd + + +class ControlVersion(object): + __slots__ = ['major', 'minor', 'bugfix', 'build'] + + @staticmethod + def unpack(buf): + rmd = ControlVersion() + (rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from('>IIII', buf) + return rmd + + +class ReturnValue(object): + __slots__ = ['success'] + + @staticmethod + def unpack(buf): + rmd = ReturnValue() + rmd.success = bool(struct.unpack_from('>B', buf)[0]) + return rmd + +class MessageV1(object): + @staticmethod + def unpack(buf): + rmd = Message() # use V2 message object + offset = 0 + rmd.level = struct.unpack_from(">B", buf, offset)[0] + offset = offset + 1 + rmd.message = str(buf[offset:]) + rmd.source = "" + + return rmd + + +class Message(object): + __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]) + offset = offset + msg_length + + src_length = struct.unpack_from(">B", buf, offset)[0] + offset = offset + 1 + rmd.source = str(buf[offset:offset+src_length]) + offset = offset + src_length + rmd.level = struct.unpack_from(">B", buf, offset)[0] + + return rmd + + +def get_item_size(data_type): + if data_type.startswith('VECTOR6'): + return 6 + 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'): + return float(data[offset]) + 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'): + return int(data[offset]) + elif(data_type == 'BOOL'): + return bool(data[offset]) + 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.') + l = [] + 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'): + 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.') + obj = DataObject() + offset = 0 + obj.recipe_id = data[0] + for i in range(len(names)): + obj.__dict__[names[i]] = unpack_field(data[1:], offset, types[i]) + offset += get_item_size(types[i]) + return obj + + @staticmethod + def create_empty(names, recipe_id): + obj = DataObject() + for i in range(len(names)): + obj.__dict__[names[i]] = None + obj.recipe_id = recipe_id + return obj + + +class DataConfig(object): + __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' + 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.') + else: + 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) + return DataObject.unpack(li, self.names, self.types) +