update csv export and added fixed mesh axis drawing

2026-07-15 14:25:03 +08:00
parent 3d1509df9d
commit 87ac152907
11 changed files with 578 additions and 35 deletions
@@ -2,6 +2,7 @@

 from __future__ import annotations

+import math
 import sys
 import time

@@ -17,6 +18,7 @@ from PyQt5.QtWidgets import (
 )

 from pipeline.config import PipelineConfig
+from pipeline.csv_recorder import CsvRecorder
 from pipeline.service import FingerPipeline

 from .app_paths import resource_path
@@ -54,6 +56,7 @@ class FingerDashboardWindow(QMainWindow):
        self.config = config
        self.layout = layout
        self.pipeline = FingerPipeline(config)
+        self.csv_recorder = CsvRecorder(config.csv_save_dir)
        self._running = False
        self._frame_index = 0
        self._wall_fps = 0.0
@@ -103,6 +106,7 @@ class FingerDashboardWindow(QMainWindow):
        self.settings_panel.set_config_path(self.config.config)
        self.settings_panel.set_calibration_path(self.config.calib or "")
        self.settings_panel.set_finger_export_dir(self.config.export_dir)
+        self.settings_panel.set_csv_save_dir(self.config.csv_save_dir)

        self.settings_panel.log_visibility_changed.connect(self._set_log_panel_visible)
        self.settings_panel.free_rotation_changed.connect(self.mesh_viewport.set_free_rotation)
@@ -112,9 +116,12 @@ class FingerDashboardWindow(QMainWindow):
        self.settings_panel.browse_video_requested.connect(self._select_camera)
        self.settings_panel.browse_config_requested.connect(self._select_config)
        self.settings_panel.browse_finger_assets_requested.connect(self._select_finger_assets)
+        self.settings_panel.browse_csv_dir_requested.connect(self._select_csv_dir)
+        self.settings_panel.csv_record_toggled.connect(self._on_csv_record_toggled)
        self.settings_panel.btn_play_pause.clicked.connect(self._on_play_pause_clicked)
        self.settings_panel.btn_reset_tracker.clicked.connect(self._on_reset)
        self.settings_panel.set_play_pause_state("run")
+        self.settings_panel.set_csv_controls_for_pipeline(False)

        self._panel_widgets: dict[str, QWidget] = {}
        self._panel_widgets["mesh"] = self.mesh_viewport
@@ -314,6 +321,7 @@ class FingerDashboardWindow(QMainWindow):
        cal = self.settings_panel.calibration_path()
        self.config.calib = cal or None
        self.config.export_dir = self.settings_panel.finger_export_dir()
+        self.config.csv_save_dir = self.settings_panel.csv_save_dir() or "./outputs/csv"

        try:
            self.pipeline = FingerPipeline(self.config)
@@ -334,6 +342,7 @@ class FingerDashboardWindow(QMainWindow):
        self.timer.start(self.layout.timer_interval_ms)
        self._set_play_pause_button("pause")
        self.settings_panel.btn_reset_tracker.setEnabled(True)
+        self.settings_panel.set_csv_controls_for_pipeline(True)
        print(
            f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, "
            f"标定={self.config.calib or '(none)'}, 资产={self.config.export_dir}"
@@ -342,6 +351,7 @@ class FingerDashboardWindow(QMainWindow):
    def _on_stop(self) -> None:
        if not self._running:
            return
+        self._stop_csv_recording(announce=True)
        self._running = False
        self.timer.stop()
        self.log_handler.clear_status_line()
@@ -354,6 +364,7 @@ class FingerDashboardWindow(QMainWindow):
        self._set_play_pause_button("run")
        self.settings_panel.btn_reset_tracker.setEnabled(False)
        self.settings_panel.set_finger_export_dir_enabled(True)
+        self.settings_panel.set_csv_controls_for_pipeline(False)

    def _on_reset(self) -> None:
        if not self._running:
@@ -413,10 +424,26 @@ class FingerDashboardWindow(QMainWindow):
            profile.record("total", total_ms)
            self.pipeline.record_total_frame(total_ms)

+        measurement = self.pipeline.measure_for_csv(frame, self._frame_index)
+        if self.csv_recorder.is_recording:
+            self.csv_recorder.write(measurement)
+
        flow_mag = 0.0
        if frame.flow.size:
            flow_mag = float((frame.flow[:, :, 0] ** 2 + frame.flow[:, :, 1] ** 2).mean() ** 0.5)

+        contact_xyz_mm = None
+        if (
+            math.isfinite(measurement.contact_x)
+            and math.isfinite(measurement.contact_y)
+            and math.isfinite(measurement.contact_z)
+        ):
+            contact_xyz_mm = (
+                measurement.contact_x,
+                measurement.contact_y,
+                measurement.contact_z,
+            )
+
        log_cfg = layout.log_config
        if log_cfg.status_enabled:
            self.log_handler.set_status_line(
@@ -428,6 +455,7 @@ class FingerDashboardWindow(QMainWindow):
                    flow_rms=flow_mag,
                    mask_coverage=frame.mask_coverage,
                    wall_fps=self._wall_fps,
+                    contact_xyz_mm=contact_xyz_mm,
                )
            )

@@ -460,6 +488,50 @@ class FingerDashboardWindow(QMainWindow):
        if path:
            self.settings_panel.set_finger_export_dir(path)

+    def _select_csv_dir(self) -> None:
+        path = QFileDialog.getExistingDirectory(
+            self,
+            "选择 CSV 保存目录",
+            self.settings_panel.csv_save_dir() or "./outputs/csv",
+        )
+        if path:
+            self.settings_panel.set_csv_save_dir(path)
+            self.config.csv_save_dir = path
+
+    def _on_csv_record_toggled(self, start: bool) -> None:
+        if start:
+            self._start_csv_recording()
+        else:
+            self._stop_csv_recording(announce=True)
+
+    def _start_csv_recording(self) -> None:
+        if not self._running:
+            print("请先启动传感器后再记录 CSV。")
+            self.settings_panel.set_csv_recording(False)
+            return
+        save_dir = self.settings_panel.csv_save_dir() or "./outputs/csv"
+        self.config.csv_save_dir = save_dir
+        try:
+            path = self.csv_recorder.start(save_dir)
+        except OSError as exc:
+            print(f"无法开始 CSV 记录：{exc}")
+            self.settings_panel.set_csv_recording(False)
+            return
+        self.settings_panel.set_csv_recording(True)
+        print(f"开始 CSV 记录：{path}")
+
+    def _stop_csv_recording(self, *, announce: bool = False) -> None:
+        if not self.csv_recorder.is_recording:
+            self.settings_panel.set_csv_recording(False)
+            return
+        path, count = self.csv_recorder.stop()
+        self.settings_panel.set_csv_recording(False)
+        if announce:
+            if path is not None:
+                print(f"CSV 记录结束：{path}，总行数：{count}")
+            else:
+                print(f"CSV 记录结束，总行数：{count}")
+
    def keyPressEvent(self, event):
        key = event.key()
        if key == Qt.Key_Escape:
@@ -483,6 +555,7 @@ class FingerDashboardWindow(QMainWindow):

    def closeEvent(self, event):
        self.timer.stop()
+        self._stop_csv_recording(announce=True)
        self.mesh_viewport.close_renderer()
        self.pipeline.close()
        event.accept()
@@ -493,6 +566,7 @@ def run_dashboard(config: PipelineConfig, *, layout: DashboardLayout = LAYOUT) -
    from .app import get_or_create_qapp

    print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏")
+    print("GUI：Record CSV / Stop CSV 写入接触点与力到所选目录。")
    if config.profile:
        print(f"Profiling enabled (report every {config.profile_every} frames).")
        print("Log FPS= is end-to-end dashboard rate; sdk= is Sensor internal rate.")
@@ -19,6 +19,9 @@ MeshLighting (below) controls the 3D mesh panel sun light:

 MESH_BACKGROUND (below) sets the 3D mesh panel clear color (RGB 0–255).

+MESH_AXIS_OVERLAY (below) controls the fixed top-right pre-rendered 3D axis triad:
+  enabled, padding_px, size_px, opacity
+
 MESH_CONTACT_ARROW (below) controls the yellow 3D contact arrow on the mesh panel:
  reference_normal_force — Fn at which arrow reaches full size (log/power/linear curve)
  force_strength_mode    — "log" | "power" | "linear"
@@ -87,7 +90,7 @@ TOP_ROW = PanelRow(
        PanelSlot("mesh", "3D mesh", width_px=400, height_px=400),
        PanelSlot("arrows", "UV Unrolled", width_px=400, height_px=400, enabled=True),
        PanelSlot("charts", "Force charts", width_px=300, height_px=400),
-        PanelSlot("settings", "Settings", width_px=200, height_px=0),
+        PanelSlot("settings", "Settings", width_px=250, height_px=0),
    ),
 )

@@ -142,6 +145,20 @@ class MeshBackground:

 MESH_BACKGROUND = MeshBackground(color=(0, 0, 0))

+
+@dataclass(frozen=True)
+class MeshAxisOverlay:
+    """Fixed pre-rendered XYZ triad HUD on the 3D mesh RGB output."""
+
+    enabled: bool = True
+    padding_px: int = 12
+    size_px: int = 96
+    opacity: float = 1.0
+
+
+MESH_AXIS_OVERLAY = MeshAxisOverlay()
+
+
 # 3D mesh contact arrow — edit here for dashboard tuning (see ContactArrowStyle in viz/contact_arrow.py).
 MESH_CONTACT_ARROW = ContactArrowStyle(
    reference_normal_force=10000.0,
@@ -197,6 +214,7 @@ class LogConfig:
        flow_rms: float = 0.0,
        mask_coverage: float = 0.0,
        wall_fps: float | None = None,
+        contact_xyz_mm: tuple[float, float, float] | None = None,
    ) -> str:
        shear_mag = float((fshearx**2 + fsheary**2) ** 0.5)
        if wall_fps is not None:
@@ -206,11 +224,17 @@ class LogConfig:
        parts.extend(
            [
                f"Fn={fnormal:.1f}",
-                f"|Fs|={shear_mag:.1f}",
-                f"Fs=({fshearx:.1f}, {fsheary:.1f})",
+                f"|(Fx,Fy)|={shear_mag:.1f}",
+                f"Fx, Fy=({fshearx:.1f}, {fsheary:.1f})",
            ]
        )
-        return "  ".join(parts)
+        force_line = "  ".join(parts)
+        if contact_xyz_mm is None:
+            contact_line = "contact=---"
+        else:
+            cx, cy, cz = contact_xyz_mm
+            contact_line = f"contact=({cx:.1f}, {cy:.1f}, {cz:.1f}) mm"
+        return f"{force_line}\n{contact_line}"


 LOG_CONFIG = LogConfig()
@@ -224,7 +248,7 @@ class DashboardLayout:

    window_title: str = "Orisys Finger Dashboard"
    window_width: int = 1300
-    window_height: int = 520
+    window_height: int = 560

    root_margin_px: int = 6
    root_spacing_px: int = 6
@@ -249,6 +273,7 @@ class DashboardLayout:
    mesh_background: MeshBackground = MESH_BACKGROUND
    mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW
    mesh_flow_arrow: FlowArrowStyle = MESH_FLOW_ARROW
+    mesh_axis_overlay: MeshAxisOverlay = MESH_AXIS_OVERLAY

    log_config: LogConfig = LOG_CONFIG

@@ -22,12 +22,13 @@ class TeeStream:


 class WidgetLogHandler:
-    """Mirror print output into a QPlainTextEdit with a live status line at the bottom."""
+    """Mirror print output into a QPlainTextEdit with pinned status lines at the bottom."""

    def __init__(self, widget: QPlainTextEdit):
        self.widget = widget
        self.buf = ""
-        self._has_status_line = False
+        self._status_text: str | None = None
+        self._status_block_count = 0

    def write(self, text: str):
        self.buf += text
@@ -42,34 +43,42 @@ class WidgetLogHandler:
            self.buf = ""

    def set_status_line(self, text: str) -> None:
-        """Update the pinned bottom line (FPS / forces) without scrolling the log."""
+        """Update pinned bottom status (supports multi-line) without scrolling away."""
        text = text.rstrip("\n")
-        if self._has_status_line:
-            cursor = QTextCursor(self.widget.document().lastBlock())
-            cursor.select(QTextCursor.BlockUnderCursor)
-            cursor.insertText(text)
-        else:
-            self.widget.appendPlainText(text)
-            self._has_status_line = True
+        self._remove_status_blocks()
+        self.widget.appendPlainText(text)
+        self._status_text = text
+        self._status_block_count = max(1, text.count("\n") + 1)
        scrollbar = self.widget.verticalScrollBar()
        scrollbar.setValue(scrollbar.maximum())

    def clear_status_line(self) -> None:
-        if not self._has_status_line:
+        self._remove_status_blocks()
+        self._status_text = None
+        self._status_block_count = 0
+
+    def _remove_status_blocks(self) -> None:
+        if self._status_block_count <= 0:
            return
-        cursor = QTextCursor(self.widget.document().lastBlock())
-        cursor.select(QTextCursor.BlockUnderCursor)
+        doc = self.widget.document()
+        end = QTextCursor(doc)
+        end.movePosition(QTextCursor.End)
+        start = QTextCursor(doc)
+        start.movePosition(QTextCursor.End)
+        for _ in range(self._status_block_count):
+            start.movePosition(QTextCursor.StartOfBlock)
+            if start.position() > 0:
+                start.movePosition(QTextCursor.Left)
+        cursor = QTextCursor(doc)
+        cursor.setPosition(start.position())
+        cursor.setPosition(end.position(), QTextCursor.KeepAnchor)
        cursor.removeSelectedText()
-        self._has_status_line = False
+        self._status_block_count = 0

    def _append_log_line(self, line: str) -> None:
-        status = None
-        if self._has_status_line:
-            status = self.widget.document().lastBlock().text()
-            cursor = QTextCursor(self.widget.document().lastBlock())
-            cursor.select(QTextCursor.BlockUnderCursor)
-            cursor.removeSelectedText()
-            self._has_status_line = False
+        status = self._status_text
+        if status is not None:
+            self.clear_status_line()
        self.widget.appendPlainText(line)
        if status is not None:
            self.set_status_line(status)
@@ -120,6 +120,7 @@ class MeshViewportWidget(ImagePanel):
            mesh_background=self._layout.mesh_background.color,
            contact_arrow_style=self._layout.mesh_contact_arrow,
            flow_arrow_style=self._layout.mesh_flow_arrow,
+            axis_overlay=self._layout.mesh_axis_overlay,
        )
        self._renderer.flow_arrow.set_enabled(self._mesh_state.flow_arrows_enabled)

@@ -41,6 +41,8 @@ class SettingsPanelWidget(QWidget):
    browse_video_requested = pyqtSignal()
    browse_config_requested = pyqtSignal()
    browse_finger_assets_requested = pyqtSignal()
+    browse_csv_dir_requested = pyqtSignal()
+    csv_record_toggled = pyqtSignal(bool)

    def __init__(
        self,
@@ -57,8 +59,8 @@ class SettingsPanelWidget(QWidget):
        self.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)

        root = QVBoxLayout(self)
-        root.setContentsMargins(10, 10, 10, 10)
-        root.setSpacing(10)
+        root.setContentsMargins(8, 8, 8, 8)
+        root.setSpacing(6)

        title = QLabel(layout.settings_panel_title)
        title.setObjectName("settingsTitle")
@@ -125,10 +127,10 @@ class SettingsPanelWidget(QWidget):
        config_row.addWidget(self.btn_browse_config)
        root.addLayout(config_row)

+        # Optional calibration path is kept for future re-enablement but hidden for now.
        self.cal_edit = QLineEdit("")
        self.cal_edit.setPlaceholderText("Optional calibration path")
-        root.addWidget(self._caption("Calibration"))
-        root.addWidget(self.cal_edit)
+        self.cal_edit.hide()

        root.addWidget(self._caption("Finger assets"))
        finger_assets_row = QHBoxLayout()
@@ -143,7 +145,28 @@ class SettingsPanelWidget(QWidget):
        finger_assets_row.addWidget(self.btn_browse_finger_assets)
        root.addLayout(finger_assets_row)

-        root.addSpacing(8)
+        root.addWidget(self._caption("CSV save directory"))
+        csv_dir_row = QHBoxLayout()
+        csv_dir_row.setSpacing(6)
+        self.csv_dir_edit = QLineEdit("./outputs/csv")
+        self.csv_dir_edit.setPlaceholderText("Directory for CSV recordings")
+        csv_dir_row.addWidget(self.csv_dir_edit, 1)
+        self.btn_browse_csv_dir = QPushButton("…")
+        self.btn_browse_csv_dir.setFixedWidth(32)
+        self.btn_browse_csv_dir.setToolTip("Browse CSV save directory")
+        self.btn_browse_csv_dir.clicked.connect(self.browse_csv_dir_requested.emit)
+        csv_dir_row.addWidget(self.btn_browse_csv_dir)
+        root.addLayout(csv_dir_row)
+
+        self.btn_csv_record = QPushButton("Record CSV")
+        self.btn_csv_record.setToolTip("Start writing contact/force CSV")
+        self.btn_csv_record.setEnabled(False)
+        self.btn_csv_record.clicked.connect(self._on_csv_toggle_clicked)
+        root.addWidget(self.btn_csv_record)
+        self._csv_recording = False
+        self._pipeline_running = False
+
+        root.addSpacing(4)
        rotate_row = QHBoxLayout()
        rotate_row.setSpacing(6)
        self.free_rotate_check = QCheckBox("Free rotate 3D view")
@@ -211,6 +234,45 @@ class SettingsPanelWidget(QWidget):
        self.finger_assets_edit.setEnabled(enabled)
        self.btn_browse_finger_assets.setEnabled(enabled)

+    def set_csv_save_dir(self, value: str) -> None:
+        self.csv_dir_edit.setText(value)
+
+    def set_csv_controls_for_pipeline(self, running: bool) -> None:
+        """Update CSV controls from pipeline start/stop."""
+        self._pipeline_running = bool(running)
+        if not self._pipeline_running:
+            self._csv_recording = False
+        self._refresh_csv_controls()
+
+    def set_csv_recording(self, recording: bool) -> None:
+        """Update CSV controls after a successful start/stop of recording."""
+        self._csv_recording = bool(recording) and self._pipeline_running
+        self._refresh_csv_controls()
+
+    def _refresh_csv_controls(self) -> None:
+        self.btn_csv_record.setEnabled(self._pipeline_running)
+        if self._csv_recording:
+            self.btn_csv_record.setText("Stop CSV")
+            self.btn_csv_record.setToolTip("Stop CSV recording")
+            self.btn_csv_record.setProperty("recording", True)
+        else:
+            self.btn_csv_record.setText("Record CSV")
+            self.btn_csv_record.setToolTip("Start writing contact/force CSV")
+            self.btn_csv_record.setProperty("recording", False)
+        style = self.btn_csv_record.style()
+        style.unpolish(self.btn_csv_record)
+        style.polish(self.btn_csv_record)
+        self.btn_csv_record.update()
+
+        dir_enabled = not self._csv_recording
+        self.csv_dir_edit.setEnabled(dir_enabled)
+        self.btn_browse_csv_dir.setEnabled(dir_enabled)
+
+    def _on_csv_toggle_clicked(self) -> None:
+        if not self._pipeline_running:
+            return
+        self.csv_record_toggled.emit(not self._csv_recording)
+
    def _emit_arrow_background_changed(self, _index: int) -> None:
        value = self.arrow_background_combo.currentData()
        if value:
@@ -239,3 +301,6 @@ class SettingsPanelWidget(QWidget):

    def finger_export_dir(self) -> str:
        return self.finger_assets_edit.text().strip()
+
+    def csv_save_dir(self) -> str:
+        return self.csv_dir_edit.text().strip()