711 lines
27 KiB
Python
711 lines
27 KiB
Python
|
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)
|