import time from pathlib import Path from typing import Callable, Optional import grpc from PyQt5 import QtCore, QtGui, QtWidgets try: import av except Exception as exc: av = None AV_IMPORT_ERROR = exc else: AV_IMPORT_ERROR = None from clients._path_setup import ensure_paths ensure_paths() from clients.camera_client import CameraClient from clients.hlc_client import HlcClient from cmvr.api import camera_command_pb2 as pb TOUCH_TIMEOUT_SECONDS = 30.0 class ImagePreviewLabel(QtWidgets.QLabel): uv_changed = QtCore.pyqtSignal(object) def __init__(self, parent=None): super().__init__(parent) self._image = None self._selected_uv = None def set_image(self, image: QtGui.QImage): self._image = image self._selected_uv = None self._refresh_pixmap() def clear_image(self): self._image = None self._selected_uv = None self.clear() self.uv_changed.emit(None) def resizeEvent(self, event): super().resizeEvent(event) if self._image is not None: self._refresh_pixmap() def mousePressEvent(self, event): super().mousePressEvent(event) if event.button() != QtCore.Qt.LeftButton: return uv = self._map_to_image_uv(event.pos()) self._selected_uv = uv self._refresh_pixmap() self.uv_changed.emit(uv) def _map_to_image_uv(self, pos): if self._image is None: return None pixmap = self.pixmap() if pixmap is None or pixmap.isNull(): return None x_offset = (self.width() - pixmap.width()) / 2.0 y_offset = (self.height() - pixmap.height()) / 2.0 x = pos.x() - x_offset y = pos.y() - y_offset if x < 0 or y < 0 or x >= pixmap.width() or y >= pixmap.height(): return None u = int(x * self._image.width() / pixmap.width()) v = int(y * self._image.height() / pixmap.height()) u = max(0, min(u, self._image.width() - 1)) v = max(0, min(v, self._image.height() - 1)) return (u, v) def _refresh_pixmap(self): if self._image is None: return pixmap = QtGui.QPixmap.fromImage(self._image).scaled( self.size(), QtCore.Qt.KeepAspectRatio, QtCore.Qt.SmoothTransformation, ) if self._selected_uv is not None and not pixmap.isNull(): self._draw_crosshair(pixmap, self._selected_uv) self.setPixmap(pixmap) def _draw_crosshair(self, pixmap: QtGui.QPixmap, uv): if self._image is None: return u, v = uv x = int((u + 0.5) * pixmap.width() / self._image.width()) y = int((v + 0.5) * pixmap.height() / self._image.height()) x = max(0, min(x, pixmap.width() - 1)) y = max(0, min(y, pixmap.height() - 1)) arm = max(4, min(pixmap.width(), pixmap.height()) // 48) painter = QtGui.QPainter(pixmap) painter.setRenderHint(QtGui.QPainter.Antialiasing, False) outline_pen = QtGui.QPen(QtGui.QColor("white"), 2) painter.setPen(outline_pen) painter.drawLine(max(0, x - arm), y, min(pixmap.width() - 1, x + arm), y) painter.drawLine(x, max(0, y - arm), x, min(pixmap.height() - 1, y + arm)) cross_pen = QtGui.QPen(QtGui.QColor("red"), 1) painter.setPen(cross_pen) painter.drawLine(max(0, x - arm), y, min(pixmap.width() - 1, x + arm), y) painter.drawLine(x, max(0, y - arm), x, min(pixmap.height() - 1, y + arm)) painter.end() class ToggleSwitch(QtWidgets.QCheckBox): def __init__(self, parent=None): super().__init__(parent) self.setCursor(QtCore.Qt.PointingHandCursor) self.setFixedSize(56, 30) def sizeHint(self): return QtCore.QSize(56, 30) def paintEvent(self, event): del event radius = (self.height() - 2) / 2.0 track_rect = QtCore.QRectF(1, 1, self.width() - 2, self.height() - 2) knob_diameter = track_rect.height() - 6 knob_y = track_rect.top() + 3 knob_x = track_rect.left() + 3 if self.isChecked(): knob_x = track_rect.right() - knob_diameter - 3 if self.isEnabled(): track_color = QtGui.QColor("#2ca36a") if self.isChecked() else QtGui.QColor("#9aa4b2") border_color = QtGui.QColor("#1c7a4c") if self.isChecked() else QtGui.QColor("#6c7685") knob_color = QtGui.QColor("#ffffff") else: track_color = QtGui.QColor("#c7cdd6") border_color = QtGui.QColor("#afb7c2") knob_color = QtGui.QColor("#f5f6f8") painter = QtGui.QPainter(self) painter.setRenderHint(QtGui.QPainter.Antialiasing, True) painter.setPen(QtGui.QPen(border_color, 1)) painter.setBrush(track_color) painter.drawRoundedRect(track_rect, radius, radius) knob_rect = QtCore.QRectF(knob_x, knob_y, knob_diameter, knob_diameter) painter.setPen(QtGui.QPen(QtGui.QColor(0, 0, 0, 25), 1)) painter.setBrush(knob_color) painter.drawEllipse(knob_rect) class CameraRpcWorker(QtCore.QThread): image_ready = QtCore.pyqtSignal(object, dict) command_done = QtCore.pyqtSignal(str) error_occurred = QtCore.pyqtSignal(str) def __init__( self, address: str, timeout: float, device_id: str, action: str, u: Optional[int] = None, v: Optional[int] = None, max_force: Optional[float] = None, ): super().__init__() self.address = address self.timeout = timeout self.device_id = device_id self.action = action self.u = u self.v = v self.max_force = max_force def run(self): client = None try: if self.action == "touch": if self.u is None or self.v is None: raise RuntimeError("No UV selected for touch") client = HlcClient(address=self.address, timeout=self.timeout) rpc_timeout = max(float(self.timeout), TOUCH_TIMEOUT_SECONDS) max_force = -1.0 if self.max_force is None else float(self.max_force) response = client.touch( u=self.u, v=self.v, max_force=max_force, timeout=rpc_timeout, ) if not response.header.success: raise RuntimeError(response.header.error_message or "touch failed") self.command_done.emit("touch") return client = CameraClient(address=self.address, timeout=self.timeout) if self.action == "start": start_resp = client.start_camera(self.device_id) if not start_resp.header.success: raise RuntimeError(start_resp.header.error_message or "StartCamera failed") self.command_done.emit("start") return if self.action == "stop": stop_resp = client.stop_camera(self.device_id) if hasattr(stop_resp, "header") and not stop_resp.header.success: raise RuntimeError(stop_resp.header.error_message or "StopCamera failed") self.command_done.emit("stop") return response = client.get_rgb_image(self.device_id) if not response.header.success: raise RuntimeError(response.header.error_message or "GetRGBImage failed") image = self._decode_to_qimage(response.color_frame) codec_value = (response.color_frame.codec or "").strip() if codec_value.lower() in {"", "none", "raw"}: codec_value = "" metadata = { "width": response.color_frame.width, "height": response.color_frame.height, "codec": codec_value, "frame_type": pb.FrameData.FrameType.Name(response.color_frame.type), "is_key_frame": response.color_frame.is_key_frame, "data_size": len(response.color_frame.data), } self.image_ready.emit(image, metadata) except grpc.RpcError as exc: if exc.code() == grpc.StatusCode.DEADLINE_EXCEEDED: timeout_value = max(float(self.timeout), TOUCH_TIMEOUT_SECONDS) if self.action == "touch" else self.timeout self.error_occurred.emit(f"{self.action} timed out after {timeout_value:g}s") else: self.error_occurred.emit(f"{self.action} RPC failed: {exc.code().name}") except Exception as exc: self.error_occurred.emit(str(exc)) finally: if client is not None: client.close() def _decode_to_qimage(self, frame) -> QtGui.QImage: if not frame.data: raise RuntimeError("Empty RGB image payload") raw_image = self._decode_raw_qimage(frame) if raw_image is not None: return raw_image file_image = QtGui.QImage.fromData(frame.data) if not file_image.isNull(): return file_image if frame.codec: decoded_image = self._decode_with_av(frame) if decoded_image is not None: return decoded_image raise RuntimeError( "Failed to decode RGB image payload: " f"codec={frame.codec or '-'}, " f"type={pb.FrameData.FrameType.Name(frame.type)}, " f"width={frame.width}, height={frame.height}, bytes={len(frame.data)}" ) def _decode_raw_qimage(self, frame) -> Optional[QtGui.QImage]: if frame.width <= 0 or frame.height <= 0: return None if frame.type == pb.FrameData.U8C3: expected_size = frame.width * frame.height * 3 if len(frame.data) != expected_size: return None # The current C++ service returns raw cv::Mat bytes, which are BGR for CV_8UC3. image = QtGui.QImage( frame.data, frame.width, frame.height, 3 * frame.width, QtGui.QImage.Format_RGB888, ) return image.rgbSwapped().copy() if frame.type == pb.FrameData.U8C1: expected_size = frame.width * frame.height if len(frame.data) != expected_size: return None return QtGui.QImage( frame.data, frame.width, frame.height, frame.width, QtGui.QImage.Format_Grayscale8, ).copy() return None def _decode_with_av(self, frame) -> Optional[QtGui.QImage]: if av is None: raise RuntimeError(f"PyAV unavailable: {AV_IMPORT_ERROR}") codec_names = self._codec_candidates(frame.codec) packet = av.Packet(frame.data) last_error = None for codec_name in codec_names: try: decoder = av.CodecContext.create(codec_name, "r") frames = decoder.decode(packet) if not frames: continue image = frames[0].to_ndarray(format="rgb24") return QtGui.QImage( image.data, image.shape[1], image.shape[0], 3 * image.shape[1], QtGui.QImage.Format_RGB888, ).copy() except Exception as exc: last_error = exc if last_error is not None: raise RuntimeError( f"PyAV decode failed for codec={frame.codec!r}: {last_error}" ) return None def _codec_candidates(self, codec: str) -> list[str]: normalized = (codec or "").strip().lower() if normalized in {"", "none", "raw"}: return [] aliases = { "h265": ["hevc", "h265"], "hevc": ["hevc", "h265"], "h.265": ["hevc", "h265"], "h264": ["h264"], "avc": ["h264"], "h.264": ["h264"], "jpeg": ["mjpeg", "jpeg"], "jpg": ["mjpeg", "jpeg"], "mjpeg": ["mjpeg", "jpeg"], } return aliases.get(normalized, [normalized] if normalized else []) class RGBImagePage(QtWidgets.QWidget): def __init__( self, address_provider: Optional[Callable[[], str]] = None, default_device_id: str = "right_hand_cam", parent=None, ): super().__init__(parent) self.address_provider = address_provider self.worker: Optional[CameraRpcWorker] = None self.pending_action = None self.camera_started = False self.current_image: Optional[QtGui.QImage] = None self.current_uv = None self._touch_started_at: Optional[float] = None self.save_dir = Path("./captured_images") self.save_dir.mkdir(exist_ok=True) self._create_ui(default_device_id) def _create_ui(self, default_device_id: str): layout = QtWidgets.QVBoxLayout(self) settings_group = QtWidgets.QGroupBox("RGB Image") settings_layout = QtWidgets.QGridLayout(settings_group) settings_layout.addWidget(QtWidgets.QLabel("Device ID:"), 0, 0) self.device_id = QtWidgets.QLineEdit(default_device_id) settings_layout.addWidget(self.device_id, 0, 1) if self.address_provider is None: settings_layout.addWidget(QtWidgets.QLabel("Server IP:"), 0, 2) self.server_ip = QtWidgets.QLineEdit("192.168.0.222") settings_layout.addWidget(self.server_ip, 0, 3) settings_layout.addWidget(QtWidgets.QLabel("Port:"), 0, 4) self.server_port = QtWidgets.QLineEdit("50052") self.server_port.setMaximumWidth(100) settings_layout.addWidget(self.server_port, 0, 5) else: settings_layout.addWidget(QtWidgets.QLabel("Server:"), 0, 2) self.address_label = QtWidgets.QLabel("") self.address_label.setStyleSheet("font-weight: bold;") settings_layout.addWidget(self.address_label, 0, 3, 1, 3) settings_layout.addWidget(QtWidgets.QLabel("Timeout (s):"), 1, 0) self.timeout_spin = QtWidgets.QSpinBox() self.timeout_spin.setRange(1, 10) self.timeout_spin.setValue(10) settings_layout.addWidget(self.timeout_spin, 1, 1) settings_layout.addWidget(QtWidgets.QLabel("Save Dir:"), 2, 0) self.save_dir_edit = QtWidgets.QLineEdit(str(self.save_dir)) settings_layout.addWidget(self.save_dir_edit, 2, 1, 1, 4) self.browse_btn = QtWidgets.QPushButton("Browse") self.browse_btn.clicked.connect(self._browse_dir) settings_layout.addWidget(self.browse_btn, 2, 5) button_layout = QtWidgets.QHBoxLayout() button_layout.addWidget(QtWidgets.QLabel("Camera:")) self.camera_switch = ToggleSwitch() self.camera_switch.toggled.connect(self._on_camera_switch_toggled) button_layout.addWidget(self.camera_switch) button_layout.addSpacing(12) self.fetch_btn = QtWidgets.QPushButton("Get RGB Image") self.fetch_btn.clicked.connect(self._fetch_image) button_layout.addWidget(self.fetch_btn) self.save_btn = QtWidgets.QPushButton("Save Image") self.save_btn.clicked.connect(self._save_image) self.save_btn.setEnabled(False) button_layout.addWidget(self.save_btn) button_layout.addStretch() self.touch_btn = QtWidgets.QPushButton("Touch") self.touch_btn.clicked.connect(self._touch_uv) self.touch_btn.setEnabled(False) button_layout.addWidget(self.touch_btn) self.uv_label = QtWidgets.QLabel("UV: -") button_layout.addWidget(self.uv_label) button_layout.addWidget(QtWidgets.QLabel("max_force:")) self.max_force_spin = QtWidgets.QDoubleSpinBox() self.max_force_spin.setRange(-1.0, 1000.0) self.max_force_spin.setDecimals(4) self.max_force_spin.setSingleStep(0.0001) self.max_force_spin.setValue(-1.0) self.max_force_spin.setMaximumWidth(110) button_layout.addWidget(self.max_force_spin) settings_layout.addLayout(button_layout, 3, 0, 1, 6) layout.addWidget(settings_group) status_group = QtWidgets.QGroupBox("Image Status") status_layout = QtWidgets.QHBoxLayout(status_group) self.status_label = QtWidgets.QLabel("Idle") self.status_label.setStyleSheet("color: gray;") status_layout.addWidget(self.status_label) status_layout.addWidget(QtWidgets.QLabel("|")) self.camera_state_label = QtWidgets.QLabel("Camera: unknown") status_layout.addWidget(self.camera_state_label) status_layout.addWidget(QtWidgets.QLabel("|")) self.size_label = QtWidgets.QLabel("Resolution: -") status_layout.addWidget(self.size_label) status_layout.addWidget(QtWidgets.QLabel("|")) self.codec_label = QtWidgets.QLabel("Codec: -") status_layout.addWidget(self.codec_label) status_layout.addWidget(QtWidgets.QLabel("|")) self.type_label = QtWidgets.QLabel("Type: -") status_layout.addWidget(self.type_label) status_layout.addWidget(QtWidgets.QLabel("|")) self.data_size_label = QtWidgets.QLabel("Bytes: -") status_layout.addWidget(self.data_size_label) status_layout.addStretch() layout.addWidget(status_group) preview_group = QtWidgets.QGroupBox("RGB Image") preview_layout = QtWidgets.QVBoxLayout(preview_group) self.image_label = ImagePreviewLabel() self.image_label.setAlignment(QtCore.Qt.AlignCenter) self.image_label.setMinimumSize(640, 480) self.image_label.setStyleSheet("background-color: black; border: 1px solid gray;") self.image_label.uv_changed.connect(self._on_uv_changed) preview_layout.addWidget(self.image_label) layout.addWidget(preview_group, 1) info_frame = QtWidgets.QFrame() info_frame.setFrameStyle(QtWidgets.QFrame.StyledPanel | QtWidgets.QFrame.Sunken) info_layout = QtWidgets.QHBoxLayout(info_frame) info_label = QtWidgets.QLabel("Fetch a single RGB image through CameraService.GetRGBImage.") info_label.setStyleSheet("color: gray; padding: 5px;") info_layout.addWidget(info_label) layout.addWidget(info_frame) self.status_bar = QtWidgets.QStatusBar() self.status_bar.showMessage("Ready") layout.addWidget(self.status_bar) def refresh_address_hint(self): if self.address_provider is None or not hasattr(self, "address_label"): return try: self.address_label.setText(self.address_provider()) except Exception: self.address_label.setText("(app settings unavailable)") def _browse_dir(self): directory = QtWidgets.QFileDialog.getExistingDirectory(self, "Select save directory", self.save_dir_edit.text()) if directory: self.save_dir_edit.setText(directory) self.save_dir = Path(directory) self.save_dir.mkdir(exist_ok=True) def _get_server_addr(self) -> str: if self.address_provider is not None: return self.address_provider() return f"{self.server_ip.text().strip()}:{self.server_port.text().strip()}" def _fetch_image(self): self._start_worker("get") def _on_camera_switch_toggled(self, checked: bool): self._start_worker("start" if checked else "stop") def _touch_uv(self): if self.current_uv is None: QtWidgets.QMessageBox.warning(self, "Touch", "Please click the image to select UV first.") return self._start_worker("touch") def _start_touch_timer(self): self._touch_started_at = time.perf_counter() def _finish_touch_timer(self) -> Optional[float]: if self._touch_started_at is None: return None elapsed = time.perf_counter() - self._touch_started_at self._touch_started_at = None return elapsed def _format_touch_elapsed(self, elapsed: Optional[float]) -> str: if elapsed is None: return "" return f" ({elapsed:.3f}s)" def _start_worker(self, action: str): if self.worker is not None and self.worker.isRunning(): return self.refresh_address_hint() self.pending_action = action self.camera_switch.setEnabled(False) self.fetch_btn.setEnabled(False) self.touch_btn.setEnabled(False) self.save_btn.setEnabled(False) if action == "start": self.status_label.setText("Starting") self.status_label.setStyleSheet("color: orange;") self.status_bar.showMessage("Calling StartCamera...") elif action == "stop": self.status_label.setText("Stopping") self.status_label.setStyleSheet("color: orange;") self.status_bar.showMessage("Calling StopCamera...") elif action == "touch": self._start_touch_timer() self.status_label.setText("Touching") self.status_label.setStyleSheet("color: orange;") if self.current_uv is None: self.status_bar.showMessage("No UV selected") else: self.status_bar.showMessage( f"Calling touch(u={self.current_uv[0]}, v={self.current_uv[1]}, max_force={self.max_force_spin.value():.4f}, timeout={max(float(self.timeout_spin.value()), TOUCH_TIMEOUT_SECONDS):g}s)..." ) else: self.status_label.setText("Fetching") self.status_label.setStyleSheet("color: orange;") self.status_bar.showMessage("Calling GetRGBImage...") u = None v = None max_force = None if action == "touch" and self.current_uv is not None: u, v = self.current_uv max_force = float(self.max_force_spin.value()) self.worker = CameraRpcWorker( address=self._get_server_addr(), timeout=self.timeout_spin.value(), device_id=self.device_id.text().strip(), action=action, u=u, v=v, max_force=max_force, ) self.worker.image_ready.connect(self._on_image_ready) self.worker.command_done.connect(self._on_command_done) self.worker.error_occurred.connect(self._on_error) self.worker.finished.connect(self._on_worker_finished) self.worker.start() def _on_image_ready(self, image: QtGui.QImage, metadata: dict): self.current_image = image self.current_uv = None self.image_label.set_image(image) self.uv_label.setText("UV: -") self.status_label.setText("Fetched") self.status_label.setStyleSheet("color: green;") self.size_label.setText(f"Resolution: {metadata['width']}x{metadata['height']}") self.codec_label.setText(f"Codec: {metadata['codec'] or '-'}") self.type_label.setText(f"Type: {metadata['frame_type']}") self.data_size_label.setText(f"Bytes: {metadata['data_size']}") self.status_bar.showMessage("GetRGBImage completed", 3000) self.save_btn.setEnabled(True) def _on_uv_changed(self, uv): self.current_uv = uv if self.worker is None: self.touch_btn.setEnabled(uv is not None) if uv is None: self.uv_label.setText("UV: -") return u, v = uv self.uv_label.setText(f"UV: ({u}, {v})") def _on_command_done(self, action: str): if action == "start": self.camera_started = True self._set_camera_switch_checked(True) self.camera_state_label.setText("Camera: started") self.status_label.setText("Started") self.status_label.setStyleSheet("color: green;") self.status_bar.showMessage("StartCamera completed", 3000) return if action == "stop": self.camera_started = False self._set_camera_switch_checked(False) self.camera_state_label.setText("Camera: stopped") self.status_label.setText("Stopped") self.status_label.setStyleSheet("color: gray;") self.status_bar.showMessage("StopCamera completed", 3000) return if action == "touch": elapsed = self._finish_touch_timer() elapsed_text = self._format_touch_elapsed(elapsed) self.status_label.setText(f"Touched{elapsed_text}") self.status_label.setStyleSheet("color: green;") if self.current_uv is None: self.status_bar.showMessage(f"touch completed{elapsed_text}", 3000) else: self.status_bar.showMessage( f"touch completed at UV ({self.current_uv[0]}, {self.current_uv[1]}){elapsed_text}", 3000, ) def _on_error(self, message: str): if self.pending_action == "start": self.camera_started = False self._set_camera_switch_checked(False) self.camera_state_label.setText("Camera: stopped") elif self.pending_action == "stop": self.camera_started = True self._set_camera_switch_checked(True) self.camera_state_label.setText("Camera: started") if self.pending_action == "touch": elapsed = self._finish_touch_timer() elapsed_text = self._format_touch_elapsed(elapsed) message = f"{message}{elapsed_text}" self.status_label.setText(f"Touch Failed{elapsed_text}") else: self.status_label.setText("Error") self.status_label.setStyleSheet("color: red;") self.status_bar.showMessage(message, 5000) QtWidgets.QMessageBox.critical(self, "RGB Image Error", message) def _on_worker_finished(self): self.camera_switch.setEnabled(True) self.fetch_btn.setEnabled(True) self.touch_btn.setEnabled(self.current_uv is not None) if self.current_image is not None: self.save_btn.setEnabled(True) self.pending_action = None self.worker = None def _set_camera_switch_checked(self, checked: bool): blocker = QtCore.QSignalBlocker(self.camera_switch) self.camera_switch.setChecked(checked) del blocker def _save_image(self): if self.current_image is None: return self.save_dir = Path(self.save_dir_edit.text()) self.save_dir.mkdir(parents=True, exist_ok=True) timestamp = QtCore.QDateTime.currentDateTime().toString("yyyyMMdd_HHmmss") target = self.save_dir / f"rgb_{self.device_id.text().strip()}_{timestamp}.png" if self.current_image.save(str(target)): self.status_bar.showMessage(f"Saved to {target.name}", 3000) else: QtWidgets.QMessageBox.critical(self, "Save Error", "Failed to save image.") def shutdown(self): worker = self.worker if worker and worker.isRunning(): worker.wait(2000)