import sys import time from collections import deque from pathlib import Path from typing import Deque, Dict, Optional, Tuple import grpc import numpy as np from PyQt5 import QtCore, QtWidgets from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar from matplotlib.figure import Figure try: from yourdfpy import URDF except Exception: # pragma: no cover - fallback if yourdfpy not available from urdfpy import URDF from clients._path_setup import ensure_paths ensure_paths() from google.protobuf import timestamp_pb2 from cmvr.api import humanoid_robot_command_pb2 as pb from cmvr.api import humanoid_robot_service_pb2_grpc as rpc DEFAULT_JOINT_ORDER = [ "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R", "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R", "HEAD_P", "HEAD_Y", "HEAD_R", "WAIST_Y", "WAIST_R", ] URDF_PATH = Path(__file__).resolve().parents[1] / "robot_model" / "xiaoyan_description" / "dual_arm.urdf" class JointStateWorker(QtCore.QThread): updated = QtCore.pyqtSignal(dict) log = QtCore.pyqtSignal(str) def __init__( self, address: str, device_id: str, interval: float, joint_names: list[str], timeout: float = 2.0, ) -> None: super().__init__() self.address = address self.device_id = device_id self.interval = interval self.joint_names = joint_names self.timeout = timeout self._stop = False self._channel: Optional[grpc.Channel] = None def stop(self) -> None: self._stop = True def run(self) -> None: try: self._channel = grpc.insecure_channel(self.address) stub = rpc.HumanoidRobotServiceStub(self._channel) except Exception as exc: self.log.emit(f"[error] connect failed: {exc}") return while not self._stop: req = pb.JointRequest() req.header.device_id = self.device_id ts = timestamp_pb2.Timestamp() ts.GetCurrentTime() req.header.timestamp.CopyFrom(ts) try: resp = stub.getJointState(req, timeout=10) except Exception as exc: self.log.emit(f"[error] getJointState failed: {exc}") time.sleep(self.interval) continue joint_pos: Dict[str, float] = {} joint_vel: Dict[str, float] = {} for state in resp.state: for name, pos, vel in zip(state.name, state.position, state.velocity): joint_pos[name] = pos joint_vel[name] = vel payload: Dict[str, Tuple[float, float]] = {} for name in self.joint_names: payload[name] = (joint_pos.get(name, 0.0), joint_vel.get(name, 0.0)) self.updated.emit(payload) time.sleep(self.interval) if self._channel: self._channel.close() class PoseWorker(QtCore.QThread): updated = QtCore.pyqtSignal(dict) log = QtCore.pyqtSignal(str) def __init__( self, address: str, device_id: str, interval: float, base_link: str, ee_link: str, timeout: float = 2.0, ) -> None: super().__init__() self.address = address self.device_id = device_id self.interval = interval self.base_link = base_link self.ee_link = ee_link self.timeout = timeout self._stop = False self._channel: Optional[grpc.Channel] = None def stop(self) -> None: self._stop = True def run(self) -> None: try: self._channel = grpc.insecure_channel(self.address) stub = rpc.HumanoidRobotServiceStub(self._channel) except Exception as exc: self.log.emit(f"[error] connect failed: {exc}") return while not self._stop: req = pb.GetPose.Request() req.header.device_id = self.device_id req.base_link = self.base_link req.ee_link = self.ee_link ts = timestamp_pb2.Timestamp() ts.GetCurrentTime() req.header.timestamp.CopyFrom(ts) try: resp = stub.getPose(req, timeout=10) except Exception as exc: self.log.emit(f"[error] getPose failed: {exc}") time.sleep(self.interval) continue pose = resp.pose payload = { "x": pose.x, "y": pose.y, "z": pose.z, "rx": pose.rx, "ry": pose.ry, "rz": pose.rz, } self.updated.emit(payload) time.sleep(self.interval) if self._channel: self._channel.close() class MainWindow(QtWidgets.QMainWindow): def __init__(self) -> None: super().__init__() self.setWindowTitle("gRPC Client UI") self.resize(900, 600) self._pose_process: Optional[QtCore.QProcess] = None self._joint_worker: Optional[JointStateWorker] = None self._pose_worker: Optional[PoseWorker] = None self._plot_selected: set[str] = set() self._last_data: Dict[str, Tuple[float, float]] = {} self._updating_movej_table = False self.urdf: Optional[URDF] = None self.urdf_error: Optional[str] = None self.joint_names: list[str] = DEFAULT_JOINT_ORDER.copy() self.link_names: list[str] = [] self._warned_no_joint_match = False self._load_urdf() central = self._build_center_panel() self.setCentralWidget(central) self._build_docks() self.statusBar().showMessage("Ready") def _load_urdf(self) -> None: try: self.urdf = URDF.load(str(URDF_PATH)) self.link_names = [link.name for link in self._urdf_links()] self.joint_names = [ joint.name for joint in self._urdf_joints() if getattr(joint, "joint_type", "") != "fixed" ] except Exception as exc: self.urdf_error = str(exc) self.urdf = None self.link_names = [] self.joint_names = DEFAULT_JOINT_ORDER.copy() def _urdf_links(self) -> list: if not self.urdf: return [] if hasattr(self.urdf, "links"): return list(self.urdf.links) if hasattr(self.urdf, "link_map"): return list(self.urdf.link_map.values()) return [] def _urdf_joints(self) -> list: if not self.urdf: return [] if hasattr(self.urdf, "joints"): return list(self.urdf.joints) if hasattr(self.urdf, "joint_map"): return list(self.urdf.joint_map.values()) return [] def _build_center_panel(self) -> QtWidgets.QWidget: panel = QtWidgets.QWidget() layout = QtWidgets.QVBoxLayout(panel) layout.setContentsMargins(0, 0, 0, 0) layout.setSpacing(0) self.center_tabs = QtWidgets.QTabWidget() self.center_tabs.addTab(self._build_live_panel(), "Plot") self.center_tabs.setCurrentIndex(0) self.center_tabs.currentChanged.connect(self._on_center_tab_changed) layout.addWidget(self.center_tabs) return panel def _build_docks(self) -> None: control = QtWidgets.QDockWidget("Control") control.setWidget(self._build_control_panel()) control.setMinimumWidth(550) control.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures) self.addDockWidget(QtCore.Qt.LeftDockWidgetArea, control) status = QtWidgets.QDockWidget("Joint Status") status.setWidget(self._build_status_panel()) status.setMinimumWidth(650) status.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures) self.addDockWidget(QtCore.Qt.RightDockWidgetArea, status) log_dock = QtWidgets.QDockWidget("Log") log_dock.setWidget(self._build_log_panel()) log_dock.setMinimumHeight(140) log_dock.setMaximumHeight(220) log_dock.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures) self.addDockWidget(QtCore.Qt.BottomDockWidgetArea, log_dock) def _build_status_panel(self) -> QtWidgets.QWidget: panel = QtWidgets.QWidget() layout = QtWidgets.QVBoxLayout(panel) splitter = QtWidgets.QSplitter() splitter.setOrientation(QtCore.Qt.Vertical) splitter.addWidget(self._build_table()) splitter.addWidget(self._build_pose_panel()) splitter.setStretchFactor(0, 3) splitter.setStretchFactor(1, 1) layout.addWidget(splitter) return panel def _build_control_panel(self) -> QtWidgets.QWidget: panel = QtWidgets.QWidget() layout = QtWidgets.QVBoxLayout(panel) layout.addWidget(self._build_settings_group()) layout.addWidget(self._build_movej_group()) layout.addWidget(self._build_actions_group()) layout.addStretch(1) return panel def _build_settings_group(self) -> QtWidgets.QGroupBox: group = QtWidgets.QGroupBox("Settings") layout = QtWidgets.QVBoxLayout(group) layout.addLayout(self._build_form()) return group def _build_movej_group(self) -> QtWidgets.QGroupBox: group = QtWidgets.QGroupBox("MoveJ") layout = QtWidgets.QVBoxLayout(group) params = QtWidgets.QFormLayout() params.addRow("MoveJ Vel", self.vel_edit) params.addRow("MoveJ Acc", self.acc_edit) layout.addLayout(params) layout.addWidget(self._build_movej_table()) self.movej_btn = QtWidgets.QPushButton("Send MoveJ") self.movej_btn.clicked.connect(self._on_movej) layout.addWidget(self.movej_btn) return group def _build_actions_group(self) -> QtWidgets.QGroupBox: group = QtWidgets.QGroupBox("Actions") layout = QtWidgets.QVBoxLayout(group) self.jointstate_toggle = self._build_switch_row("JointState") self.torque_on_btn = QtWidgets.QPushButton("Torque On") self.torque_off_btn = QtWidgets.QPushButton("Torque Off") self.jointstate_toggle.toggled.connect(self._on_jointstate_toggle) self.torque_on_btn.clicked.connect(self._on_torque_on) self.torque_off_btn.clicked.connect(self._on_torque_off) layout.addLayout(self._wrap_switch_row(self.jointstate_toggle, "Start JointState")) layout.addWidget(self.torque_on_btn) layout.addWidget(self.torque_off_btn) return group def _build_switch_row(self, name: str) -> QtWidgets.QCheckBox: toggle = QtWidgets.QCheckBox() toggle.setChecked(False) toggle.setFixedSize(56, 28) toggle.setStyleSheet( """ QCheckBox { background-color: #bfc5cf; border-radius: 14px; padding: 0px; } QCheckBox:checked { background-color: #2f6fed; } QCheckBox::indicator { width: 22px; height: 22px; border-radius: 11px; background: #ffffff; margin-left: 3px; } QCheckBox::indicator:checked { margin-left: 31px; } """ ) toggle.setTristate(False) toggle.setObjectName(f"{name}_toggle") return toggle def _wrap_switch_row(self, toggle: QtWidgets.QCheckBox, label: str) -> QtWidgets.QHBoxLayout: row = QtWidgets.QHBoxLayout() row.addWidget(toggle) row.addWidget(QtWidgets.QLabel(label)) row.addStretch(1) return row def _build_form(self) -> QtWidgets.QLayout: form = QtWidgets.QFormLayout() self.host_edit = QtWidgets.QLineEdit("192.168.0.222") self.port_edit = QtWidgets.QSpinBox() self.port_edit.setRange(1, 65535) self.port_edit.setValue(50052) self.device_edit = QtWidgets.QLineEdit("hc01") self.interval_edit = QtWidgets.QDoubleSpinBox() self.interval_edit.setDecimals(3) self.interval_edit.setRange(0.001, 10.0) self.interval_edit.setValue(0.05) self.vel_edit = QtWidgets.QDoubleSpinBox() self.vel_edit.setRange(0.1, 10.0) self.vel_edit.setValue(1.0) self.acc_edit = QtWidgets.QDoubleSpinBox() self.acc_edit.setRange(0.1, 10.0) self.acc_edit.setValue(0.5) form.addRow("Host", self.host_edit) form.addRow("Port", self.port_edit) form.addRow("Device ID", self.device_edit) form.addRow("Interval (s)", self.interval_edit) return form def _build_log(self) -> QtWidgets.QWidget: self.log = QtWidgets.QTextEdit() self.log.setReadOnly(True) return self.log def _build_log_panel(self) -> QtWidgets.QWidget: panel = QtWidgets.QWidget() layout = QtWidgets.QVBoxLayout(panel) header = QtWidgets.QHBoxLayout() header.addWidget(QtWidgets.QLabel("Runtime logs")) header.addStretch(1) log_widget = self._build_log() self.clear_log_btn = QtWidgets.QPushButton("Clear") self.clear_log_btn.clicked.connect(log_widget.clear) header.addWidget(self.clear_log_btn) layout.addLayout(header) layout.addWidget(log_widget) return panel def _build_pose_panel(self) -> QtWidgets.QGroupBox: group = QtWidgets.QGroupBox("GetPose") layout = QtWidgets.QVBoxLayout(group) self.getpose_toggle = self._build_switch_row("GetPose") self.getpose_toggle.toggled.connect(self._on_getpose_toggle) layout.addLayout(self._wrap_switch_row(self.getpose_toggle, "Start GetPose")) form = QtWidgets.QFormLayout() self.base_link_edit = QtWidgets.QComboBox() self.ee_link_edit = QtWidgets.QComboBox() if self.link_names: self.base_link_edit.addItems(self.link_names) self.ee_link_edit.addItems(self.link_names) if "PELVIS_S" in self.link_names: self.base_link_edit.setCurrentText("PELVIS_S") if "R_FINGER_TIP" in self.link_names: self.ee_link_edit.setCurrentText("R_FINGER_TIP") else: self.base_link_edit.addItem("PELVIS_S") self.ee_link_edit.addItem("R_FINGER_TIP") form.addRow("Base Link", self.base_link_edit) form.addRow("EE Link", self.ee_link_edit) layout.addLayout(form) self.pose_labels = {} grid = QtWidgets.QGridLayout() for i, key in enumerate(["x", "y", "z", "rx", "ry", "rz"]): label = QtWidgets.QLabel(f"{key}: 0.000000") self.pose_labels[key] = label grid.addWidget(label, i // 3, i % 3) layout.addLayout(grid) return group def _build_live_panel(self) -> QtWidgets.QWidget: tab = QtWidgets.QWidget() layout = QtWidgets.QVBoxLayout(tab) layout.setContentsMargins(0, 0, 0, 0) layout.setSpacing(4) self.figure = Figure(figsize=(5, 4)) self.canvas = FigureCanvas(self.figure) self.ax_pos = self.figure.add_subplot(2, 1, 1) self.ax_vel = self.figure.add_subplot(2, 1, 2) self.ax_pos.set_title("Position (rad)") self.ax_vel.set_title("Velocity (rad/s)") self.ax_pos.grid(True, alpha=0.3) self.ax_vel.grid(True, alpha=0.3) self.pos_lines: Dict[str, object] = {} self.vel_lines: Dict[str, object] = {} self.time_hist: Deque[float] = deque(maxlen=300) self._last_plot_time: Optional[float] = None self.pos_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names} self.vel_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names} self._plot_start = time.time() self.canvas.setSizePolicy( QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding ) self.toolbar = NavigationToolbar(self.canvas, self) layout.addWidget(self.toolbar) layout.addWidget(self.canvas) return tab def _build_table(self) -> QtWidgets.QWidget: self.table = QtWidgets.QTableWidget(len(self.joint_names), 4) self.table.setHorizontalHeaderLabels( ["Joint", "Position (rad)", "Position (deg)", "Velocity (rad/s)"] ) self.table.verticalHeader().setVisible(False) self.table.setEditTriggers(QtWidgets.QAbstractItemView.NoEditTriggers) self.table.setSelectionMode(QtWidgets.QAbstractItemView.ExtendedSelection) self.table.setSelectionBehavior(QtWidgets.QAbstractItemView.SelectRows) for row, name in enumerate(self.joint_names): self.table.setItem(row, 0, QtWidgets.QTableWidgetItem(name)) self.table.setItem(row, 1, QtWidgets.QTableWidgetItem("0.0")) self.table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0")) self.table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0")) self.table.horizontalHeader().setStretchLastSection(True) self.table.itemSelectionChanged.connect(self._on_status_selection_changed) return self.table def _build_movej_table(self) -> QtWidgets.QWidget: self.movej_table = QtWidgets.QTableWidget(len(self.joint_names), 4) self.movej_table.setHorizontalHeaderLabels(["Use", "Joint", "Deg", "Rad"]) self.movej_table.verticalHeader().setVisible(False) self.movej_table.setSizePolicy( QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding ) for row, name in enumerate(self.joint_names): use_item = QtWidgets.QTableWidgetItem() use_item.setCheckState(QtCore.Qt.Unchecked) self.movej_table.setItem(row, 0, use_item) self.movej_table.setItem(row, 1, QtWidgets.QTableWidgetItem(name)) self.movej_table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0")) self.movej_table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0")) self.movej_table.horizontalHeader().setStretchLastSection(True) self._fit_table_height(self.movej_table) self.movej_table.itemChanged.connect(self._on_movej_angle_changed) return self.movej_table def _fit_table_height(self, table: QtWidgets.QTableWidget) -> None: header_height = table.horizontalHeader().height() row_height = table.verticalHeader().defaultSectionSize() total = header_height + row_height * table.rowCount() + 2 table.setMinimumHeight(total) def _append_log(self, text: str) -> None: if hasattr(self, "log") and self.log is not None: self.log.append(text.rstrip()) def _base_args(self) -> list[str]: return [ "-m", "ui.run_action", "--host", self.host_edit.text().strip(), "--port", str(self.port_edit.value()), "--device-id", self.device_edit.text().strip(), ] def _start_process(self, args: list[str], keep: bool = False) -> QtCore.QProcess: proc = QtCore.QProcess(self) proc.setProgram(sys.executable) proc.setArguments(args) proc.setWorkingDirectory(str(Path(__file__).resolve().parents[1])) proc.setProcessChannelMode(QtCore.QProcess.MergedChannels) proc.readyReadStandardOutput.connect( lambda p=proc: self._append_log(p.readAllStandardOutput().data().decode("utf-8", "ignore")) ) proc.finished.connect( lambda code, status: self._append_log(f"[exit] code={code} status={int(status)}") ) proc.start() if keep: return proc proc.finished.connect(proc.deleteLater) return proc def _on_movej(self) -> None: joint_list = self._get_selected_movej_joints() if not joint_list: self._append_log("[warn] No joints selected for MoveJ.") return args = self._base_args() + [ "movej", "--vel", str(self.vel_edit.value()), "--acc", str(self.acc_edit.value()), ] self._start_process(args + ["--joint-list", self._encode_joint_list(joint_list)]) def _on_get_pose(self) -> None: if self._joint_worker and self._joint_worker.isRunning(): self._append_log("[info] JointState already running.") return address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}" self._joint_worker = JointStateWorker( address=address, device_id=self.device_edit.text().strip(), interval=float(self.interval_edit.value()), joint_names=self.joint_names, ) self._joint_worker.updated.connect(self._update_joint_table) self._joint_worker.log.connect(self._append_log) self._joint_worker.start() def _on_stop_pose(self) -> None: if self._joint_worker: self._append_log("[info] Stopping JointState...") self._joint_worker.stop() self._joint_worker = None def _on_torque_on(self) -> None: args = self._base_args() + ["torque_on"] self._start_process(args) def _on_torque_off(self) -> None: args = self._base_args() + ["torque_off"] self._start_process(args) def _on_jointstate_toggle(self, checked: bool) -> None: if checked: self._on_get_pose() else: self._on_stop_pose() def _on_getpose_toggle(self, checked: bool) -> None: if checked: self._on_start_get_pose() else: self._on_stop_get_pose() def _update_joint_table(self, data: Dict[str, Tuple[float, float]]) -> None: self._last_data = data self._render_joint_table_and_plot() def _append_plot(self, name: str, pos: float, vel: float) -> None: t = time.time() - self._plot_start if self._last_plot_time is None or t > self._last_plot_time: self.time_hist.append(t) self._last_plot_time = t self.pos_hist[name].append(pos) self.vel_hist[name].append(vel) def _refresh_plot(self, selected: set) -> None: for name in list(self.pos_lines.keys()): if name not in selected: self.pos_lines[name].remove() self.vel_lines[name].remove() del self.pos_lines[name] del self.vel_lines[name] for name in selected: if name not in self.pos_lines: (pos_line,) = self.ax_pos.plot([], [], label=name) (vel_line,) = self.ax_vel.plot([], [], label=name) self.pos_lines[name] = pos_line self.vel_lines[name] = vel_line x_data = list(self.time_hist) y_pos_full = list(self.pos_hist[name]) y_vel_full = list(self.vel_hist[name]) n_pos = min(len(x_data), len(y_pos_full)) n_vel = min(len(x_data), len(y_vel_full)) if n_pos: self.pos_lines[name].set_data(x_data[-n_pos:], y_pos_full[-n_pos:]) else: self.pos_lines[name].set_data([], []) if n_vel: self.vel_lines[name].set_data(x_data[-n_vel:], y_vel_full[-n_vel:]) else: self.vel_lines[name].set_data([], []) if len(self.time_hist) > 1 and selected: self.ax_pos.set_xlim(self.time_hist[0], self.time_hist[-1]) self.ax_vel.set_xlim(self.time_hist[0], self.time_hist[-1]) self.ax_pos.relim() self.ax_pos.autoscale_view(scalex=False, scaley=True) self.ax_vel.relim() self.ax_vel.autoscale_view(scalex=False, scaley=True) if selected: self.ax_pos.legend(loc="upper right", fontsize=8, ncol=2) self.ax_vel.legend(loc="upper right", fontsize=8, ncol=2) self.canvas.draw_idle() def _reset_plot_data(self) -> None: self.time_hist.clear() for name in self.joint_names: self.pos_hist[name].clear() self.vel_hist[name].clear() self._plot_start = time.time() self._last_plot_time = None for line in self.pos_lines.values(): line.set_data([], []) for line in self.vel_lines.values(): line.set_data([], []) self.canvas.draw_idle() def _encode_joint_list(self, joint_list: list[dict]) -> str: return ";".join(f"{j['joint_name']}:{j['rad']}" for j in joint_list) def _get_selected_movej_joints(self) -> list[dict]: joint_list = [] for row, name in enumerate(self.joint_names): use_item = self.movej_table.item(row, 0) if use_item and use_item.checkState() == QtCore.Qt.Checked: rad_item = self.movej_table.item(row, 3) try: rad = float(rad_item.text()) if rad_item else 0.0 except ValueError: rad = 0.0 joint_list.append({"joint_name": name, "rad": rad}) return joint_list def _on_movej_angle_changed(self, item: QtWidgets.QTableWidgetItem) -> None: if self._updating_movej_table: return col = item.column() if col not in (2, 3): return row = item.row() self._updating_movej_table = True try: if col == 2: try: deg = float(item.text()) except ValueError: deg = 0.0 rad = deg * 3.141592653589793 / 180.0 rad_item = self.movej_table.item(row, 3) if rad_item is None: rad_item = QtWidgets.QTableWidgetItem("0.0") self.movej_table.setItem(row, 3, rad_item) rad_item.setText(f"{rad:.6f}") elif col == 3: try: rad = float(item.text()) except ValueError: rad = 0.0 deg = rad * 180.0 / 3.141592653589793 deg_item = self.movej_table.item(row, 2) if deg_item is None: deg_item = QtWidgets.QTableWidgetItem("0.0") self.movej_table.setItem(row, 2, deg_item) deg_item.setText(f"{deg:.3f}") finally: self._updating_movej_table = False def _apply_plot_selection(self) -> None: # Deprecated: plot selection now follows right-side Joint Status selection. pass def _on_status_selection_changed(self) -> None: selected_rows = {idx.row() for idx in self.table.selectionModel().selectedRows()} self._plot_selected = {self.joint_names[row] for row in selected_rows} self.statusBar().showMessage(f"Selected joints: {len(self._plot_selected)}") self._render_joint_table_and_plot() def _on_start_get_pose(self) -> None: if self._pose_worker and self._pose_worker.isRunning(): self._append_log("[info] GetPose already running.") return address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}" self._pose_worker = PoseWorker( address=address, device_id=self.device_edit.text().strip(), interval=float(self.interval_edit.value()), base_link=self.base_link_edit.currentText().strip(), ee_link=self.ee_link_edit.currentText().strip(), ) self._pose_worker.updated.connect(self._update_pose) self._pose_worker.log.connect(self._append_log) self._pose_worker.start() def _on_stop_get_pose(self) -> None: if self._pose_worker: self._append_log("[info] Stopping GetPose...") self._pose_worker.stop() self._pose_worker = None def _update_pose(self, data: Dict[str, float]) -> None: for key, value in data.items(): if key in self.pose_labels: self.pose_labels[key].setText(f"{key}: {value:.6f}") return def _render_joint_table_and_plot(self) -> None: selected = set(self._plot_selected) if not self._last_data: self._last_data = {name: (0.0, 0.0) for name in self.joint_names} selected_lines = [] for row, name in enumerate(self.joint_names): pos, vel = self._last_data.get(name, (0.0, 0.0)) self.table.item(row, 1).setText(f"{pos:.6f}") self.table.item(row, 2).setText(f"{pos * 180.0 / 3.141592653589793:.3f}") self.table.item(row, 3).setText(f"{vel:.6f}") if name in selected: selected_lines.append(f"{name}: {pos:.4f} rad, {vel:.4f} rad/s") self._append_plot(name, pos, vel) if selected_lines: self.statusBar().showMessage( f"Selected joints: {len(selected)} | " + " | ".join(selected_lines) ) else: self.statusBar().showMessage(f"Selected joints: {len(selected)}") if self._is_plot_visible(): self._refresh_plot(selected) return def _is_plot_visible(self) -> bool: return hasattr(self, "center_tabs") and self.center_tabs.currentIndex() == 0 def _on_center_tab_changed(self, index: int) -> None: if index == 0: self._render_joint_table_and_plot() def main() -> int: app = QtWidgets.QApplication(sys.argv) window = MainWindow() window.show() return app.exec_() if __name__ == "__main__": raise SystemExit(main())