import sys import time from pathlib import Path from typing import Dict, Optional, Tuple import grpc from PyQt5 import QtCore, QtGui, QtWidgets 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 ui.camera_page import CameraPage from ui.plot_page import PlotPage from ui.rgb_image_page import RGBImagePage 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", ] STATUS_PREVIEW_JOINTS = 4 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.camera_page: Optional[CameraPage] = None self.plot_page: Optional[PlotPage] = None self.rgb_image_page: Optional[RGBImagePage] = None self._warned_no_joint_match = False self._load_urdf() central = self._build_center_panel() self.setCentralWidget(central) self._build_docks() if self.camera_page is not None: self.host_edit.textChanged.connect(self.camera_page.refresh_address_hint) self.port_edit.valueChanged.connect(self.camera_page.refresh_address_hint) self.camera_page.refresh_address_hint() if self.rgb_image_page is not None: self.host_edit.textChanged.connect(self.rgb_image_page.refresh_address_hint) self.port_edit.valueChanged.connect(self.rgb_image_page.refresh_address_hint) self.rgb_image_page.refresh_address_hint() 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.plot_page = PlotPage(self.joint_names, parent=self) self.center_tabs.addTab(self.plot_page, "Plot") self.camera_page = CameraPage(address_provider=self._grpc_address, parent=self) self.center_tabs.addTab(self.camera_page, "Camera") self.rgb_image_page = RGBImagePage(address_provider=self._grpc_address, parent=self) self.center_tabs.addTab(self.rgb_image_page, "RGB Image") 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) 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_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_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 _grpc_address(self) -> str: return f"{self.host_edit.text().strip()}:{self.port_edit.value()}" 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._stop_jointstate_worker() 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 _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._stop_pose_worker() 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") if selected_lines: preview = selected_lines[:STATUS_PREVIEW_JOINTS] hidden_count = max(0, len(selected_lines) - STATUS_PREVIEW_JOINTS) suffix = f" | +{hidden_count} more" if hidden_count else "" self.statusBar().showMessage( f"Selected joints: {len(selected)} | " + " | ".join(preview) + suffix ) else: self.statusBar().showMessage(f"Selected joints: {len(selected)}") if self.plot_page is not None: self.plot_page.update_data( self._last_data, selected, refresh=self._is_plot_visible(), ) 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 and self.plot_page is not None: self.plot_page.refresh(self._plot_selected) def _stop_jointstate_worker(self) -> None: worker = self._joint_worker self._joint_worker = None if worker: worker.stop() worker.wait(2000) def _stop_pose_worker(self) -> None: worker = self._pose_worker self._pose_worker = None if worker: worker.stop() worker.wait(2000) def closeEvent(self, event: QtGui.QCloseEvent) -> None: self._stop_jointstate_worker() self._stop_pose_worker() if self._pose_process and self._pose_process.state() != QtCore.QProcess.NotRunning: self._pose_process.terminate() self._pose_process.waitForFinished(1000) if self.camera_page is not None: self.camera_page.shutdown() if self.rgb_image_page is not None: self.rgb_image_page.shutdown() super().closeEvent(event) def main() -> int: app = QtWidgets.QApplication(sys.argv) window = MainWindow() window.show() return app.exec_() if __name__ == "__main__": raise SystemExit(main())