stouchinglhh commited on
Commit
d646dd7
·
0 Parent(s):

collector: 同步采集工程 v1.2.1(全部 LFS 模式文件指针化,与 Space 现有文件合并)

Browse files
This view is limited to 50 files because it contains too many changes.   See raw diff
Files changed (50) hide show
  1. .gitattributes +43 -0
  2. README.md +30 -0
  3. collector/.gitattributes +5 -0
  4. collector/.gitignore +57 -0
  5. collector/README.md +731 -0
  6. collector/config/FS-VI80-S80C_3500000262190088_camera_calibration.yaml +29 -0
  7. collector/config/FS-VI80-S80C_3500000262190088_imu.yaml +19 -0
  8. collector/config/__init__.py +6 -0
  9. collector/config/fays_device.json +13 -0
  10. collector/config/i18n.py +536 -0
  11. collector/config/s80m_stereo_calibration.json +60 -0
  12. collector/config/sensors/hand_ble_config.json +165 -0
  13. collector/config/sensors/hand_ble_config_left.json +164 -0
  14. collector/config/settings.py +478 -0
  15. collector/core/__init__.py +0 -0
  16. collector/core/api_client.py +249 -0
  17. collector/core/auto_labeler.py +66 -0
  18. collector/core/ble_engine.py +303 -0
  19. collector/core/calibration.py +147 -0
  20. collector/core/camera.py +676 -0
  21. collector/core/d435_camera.py +571 -0
  22. collector/core/d435_manager.py +149 -0
  23. collector/core/database.py +94 -0
  24. collector/core/depth_codec.py +127 -0
  25. collector/core/depth_reader.py +353 -0
  26. collector/core/device_detector.py +497 -0
  27. collector/core/device_manager.py +148 -0
  28. collector/core/device_naming.py +68 -0
  29. collector/core/egodata_writer.py +1108 -0
  30. collector/core/encoder_probe.py +345 -0
  31. collector/core/exposure_controller.py +80 -0
  32. collector/core/glove_keypoint_solver.py +236 -0
  33. collector/core/glove_usb/__init__.py +22 -0
  34. collector/core/glove_usb/errors.py +53 -0
  35. collector/core/glove_usb/streams.py +385 -0
  36. collector/core/glove_usb/tactile_processing.py +146 -0
  37. collector/core/glove_usb/types.py +349 -0
  38. collector/core/glove_usb/usb_protocol.py +344 -0
  39. collector/core/hand_processor.py +87 -0
  40. collector/core/hand_tracking.py +807 -0
  41. collector/core/helpers.py +1281 -0
  42. collector/core/pipeline.py +866 -0
  43. collector/core/recording_record.py +30 -0
  44. collector/core/recording_repository.py +94 -0
  45. collector/core/render_engine.py +768 -0
  46. collector/core/s80m_manager.py +560 -0
  47. collector/core/sensor_config_dialogs.py +238 -0
  48. collector/core/sensor_hand_config.py +44 -0
  49. collector/core/session_catalog.py +127 -0
  50. collector/core/session_loader.py +112 -0
.gitattributes ADDED
@@ -0,0 +1,43 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ *.7z filter=lfs diff=lfs merge=lfs -text
2
+ *.arrow filter=lfs diff=lfs merge=lfs -text
3
+ *.bin filter=lfs diff=lfs merge=lfs -text
4
+ *.bz2 filter=lfs diff=lfs merge=lfs -text
5
+ *.ckpt filter=lfs diff=lfs merge=lfs -text
6
+ *.ftz filter=lfs diff=lfs merge=lfs -text
7
+ *.gz filter=lfs diff=lfs merge=lfs -text
8
+ *.h5 filter=lfs diff=lfs merge=lfs -text
9
+ *.joblib filter=lfs diff=lfs merge=lfs -text
10
+ *.lfs.* filter=lfs diff=lfs merge=lfs -text
11
+ *.mlmodel filter=lfs diff=lfs merge=lfs -text
12
+ *.model filter=lfs diff=lfs merge=lfs -text
13
+ *.msgpack filter=lfs diff=lfs merge=lfs -text
14
+ *.npy filter=lfs diff=lfs merge=lfs -text
15
+ *.npz filter=lfs diff=lfs merge=lfs -text
16
+ *.onnx filter=lfs diff=lfs merge=lfs -text
17
+ *.ot filter=lfs diff=lfs merge=lfs -text
18
+ *.pb filter=lfs diff=lfs merge=lfs -text
19
+ *.pickle filter=lfs diff=lfs merge=lfs -text
20
+ *.pkl filter=lfs diff=lfs merge=lfs -text
21
+ *.pt filter=lfs diff=lfs merge=lfs -text
22
+ *.pth filter=lfs diff=lfs merge=lfs -text
23
+ *.rar filter=lfs diff=lfs merge=lfs -text
24
+ *.safetensors filter=lfs diff=lfs merge=lfs -text
25
+ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
26
+ *.tar.* filter=lfs diff=lfs merge=lfs -text
27
+ *.tar filter=lfs diff=lfs merge=lfs -text
28
+ *.tflite filter=lfs diff=lfs merge=lfs -text
29
+ *.tgz filter=lfs diff=lfs merge=lfs -text
30
+ *.wasm filter=lfs diff=lfs merge=lfs -text
31
+ *.xz filter=lfs diff=lfs merge=lfs -text
32
+ *.zip filter=lfs diff=lfs merge=lfs -text
33
+ *.zst filter=lfs diff=lfs merge=lfs -text
34
+ *tfevents* filter=lfs diff=lfs merge=lfs -text
35
+
36
+ # collector 目录下的大文件(SDK 二进制/模型)走 LFS
37
+ collector/**/*.rknn filter=lfs diff=lfs merge=lfs -text
38
+ collector/**/*.so filter=lfs diff=lfs merge=lfs -text
39
+ collector/**/*.so.* filter=lfs diff=lfs merge=lfs -text
40
+ collector/**/*.jpg filter=lfs diff=lfs merge=lfs -text
41
+ collector/**/*.png filter=lfs diff=lfs merge=lfs -text
42
+ collector/**/*.bmp filter=lfs diff=lfs merge=lfs -text
43
+ collector/**/*.task filter=lfs diff=lfs merge=lfs -text
README.md ADDED
@@ -0,0 +1,30 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ ---
2
+ title: DataFlowing
3
+ emoji: 🏃
4
+ colorFrom: purple
5
+ colorTo: blue
6
+ sdk: static
7
+ pinned: false
8
+ short_description: A data pipeline for robot learning to LeRobot export
9
+ ---
10
+
11
+ # DataFlowing · 处理端(processor)
12
+
13
+ 本 Space 存放 DataFlowing 项目的**服务端处理代码**,内容来自 GitHub 仓库 [Stouching-robotics/DataFlowing](https://github.com/Stouching-robotics/DataFlowing) 的 `processor/` 目录。
14
+
15
+ ## 目录结构
16
+
17
+ - [`processor/app/`](https://huggingface.co/spaces/Stouching-Robotics/DataFlowing/tree/main/processor/app) — API、存储、工作流、AI 标注、LeRobot/HDF5 导出
18
+ - [`processor/worker/`](https://huggingface.co/spaces/Stouching-Robotics/DataFlowing/tree/main/processor/worker) — 异步处理 worker
19
+ - [`processor/web/`](https://huggingface.co/spaces/Stouching-Robotics/DataFlowing/tree/main/processor/web) — 审核 UI 与 Workflow Studio
20
+ - [`processor/scripts/`](https://huggingface.co/spaces/Stouching-Robotics/DataFlowing/tree/main/processor/scripts) — 部署、迁移与巡检工具
21
+ - [`processor/tests/`](https://huggingface.co/spaces/Stouching-Robotics/DataFlowing/tree/main/processor/tests) — 测试
22
+ - [`processor/examples/`](https://huggingface.co/spaces/Stouching-Robotics/DataFlowing/tree/main/processor/examples) — 脱敏示例会话元数据(LeRobot 格式;视频文件见 GitHub 仓库)
23
+
24
+ ## 链接
25
+
26
+ - GitHub 仓库:https://github.com/Stouching-robotics/DataFlowing
27
+ - 处理端文档:https://github.com/Stouching-robotics/DataFlowing/tree/main/processor
28
+ - 中文说明:https://github.com/Stouching-robotics/DataFlowing/blob/main/README.zh-CN.md
29
+
30
+ > 注意:本 Space 为代码镜像,与 GitHub 手动同步。开发协作请以 GitHub 仓库为准。
collector/.gitattributes ADDED
@@ -0,0 +1,5 @@
 
 
 
 
 
 
1
+ # 换行符规范化:保证 GitLab 下载后 bat 可直接双击运行
2
+ *.bat text eol=crlf
3
+ *.sh text eol=lf
4
+ *.py text eol=lf
5
+ *.md text eol=lf
collector/.gitignore ADDED
@@ -0,0 +1,57 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ __pycache__/
2
+ *.pyc
3
+ *.pyo
4
+ *.egg-info/
5
+ dist/
6
+ # 分发包的**构建模板**要入库(构建产物 dist/ 本身仍忽略)
7
+ !tools/hand_3d_s80c/dist/
8
+ build/
9
+ data/recordings/
10
+ data/*.db
11
+ *.log
12
+ .venv/
13
+ venv/
14
+ venv_lite/
15
+ *.fifo
16
+ *.h5
17
+ *.mp4
18
+ .DS_Store
19
+ *.bak_*
20
+ *.corrupted_*
21
+
22
+ # 备份包/个人配置/生成数据
23
+ *.tar.gz
24
+ *.bak
25
+ *.old
26
+ *.parquet
27
+ .vscode/
28
+ .claude/
29
+ keypoints_output/
30
+ exports/
31
+ tools/weights/
32
+ wheels/ # scripts/pack_wheels.py 生成的离线部署包(不入库,拷给客户用)
33
+ imgui.ini
34
+ .polyscope.ini
35
+ tools/stereo_s80m/offline_slam_output/
36
+ tools/stereo_s80m/hand_benchmark/
37
+ data/*.db-shm
38
+ data/*.db-wal
39
+ tools/demos/stereo_depth_demo/*.bin
40
+ tools/demos/stereo_depth_demo/*.png
41
+ tools/demos/stereo_depth_demo/build/
42
+
43
+ # 本地私有配置(模板见 *.example.json)
44
+ data/server_config.json
45
+ data/device_names.json
46
+ data/tasks.json
47
+
48
+ # 真机标定(本地提取产物,跑 tools/hand_3d_d435/tools/extract_d435_color_calib.py 再生)
49
+ tools/hand_3d_d435/calibration/
50
+ pipeline.db
51
+
52
+ # 手套工具包(第三方 SDK 本地参考,永远不上传)
53
+ stouch_glove_toolkit-Stouch_Glove_Toolkit-beta/
54
+ lite_package/*
55
+ !lite_package/start_lite.bat
56
+ !lite_package/start_lite.sh
57
+ !lite_package/使用说明_lite.md
collector/README.md ADDED
@@ -0,0 +1,731 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # collector — Multimodal Data Acquisition SDK · 多模态数据采集 SDK
2
+
3
+ ![Version](https://img.shields.io/badge/version-1.2.0-blue)
4
+ ![Python](https://img.shields.io/badge/python-3.12-blue)
5
+ ![License](https://img.shields.io/badge/license-TBD-lightgrey)
6
+
7
+
8
+ ## 📚 Navigation · 导航
9
+
10
+ | Section · 章节 | English | 中文 |
11
+ |---|---|---|
12
+ | Features · 功能特性 | [Features](#features) | [功能特性](#功能特性) |
13
+ | Quick Start · 快速开始 | [Quick Start](#quick-start) | [快速开始](#快速开始) |
14
+ | Supported Hardware · 硬件支持 | [Supported Hardware](#supported-hardware) | [硬件支持](#硬件支持) |
15
+ | Data Formats · 数据格式 | [Data Formats](#data-formats) | [数据格式](#数据格式) |
16
+ | Black Glove Keypoints · 黑手套解算 | [Black Glove Keypoints](#black-glove-keypoints) | [黑手套解算](#黑手套解算) |
17
+ | Environment Variables · 环境变量 | [Environment Variables](#environment-variables) | [环境变量](#环境变量) |
18
+ | Directory Structure · 目录结构 | [Directory Structure](#directory-structure) | [目录结构](#目录结构) |
19
+ | Tests · 测试 | [Tests](#tests) | [测试](#测试) |
20
+ | Documentation · 文档 | [Documentation](#documentation) | [文档](#文档) |
21
+ | Privacy & Local Config · 隐私与本地配置 | [Privacy & Local Config](#privacy--local-config) | [隐私与本地配置](#隐私与本地配置) |
22
+ | Third-Party Components & Licensing · 第三方组件与许可 | [Third-Party Components & Licensing](#third-party-components--licensing) | [第三方组件与许可](#第三方组件与许可) |
23
+ | License · 许可证 | [License](#license) | [许可证](#许可证) |
24
+ | Contributing · 贡献 | [Contributing](#contributing) | [贡献](#贡献) |
25
+ | Changelog · 更新记录 | [Changelog](#changelog) | [更新记录](#更新记录) |
26
+
27
+ ---
28
+
29
+ ## English
30
+
31
+ A PyQt5-based multimodal data acquisition SDK + GUI: synchronized recording,
32
+ playback, and HTTP upload across multiple cameras (UVC webcams, Intel
33
+ RealSense D435/D405, S80C / S80M stereo) and BLE data gloves, outputting
34
+ [EgoData](https://github.com/facebookresearch/egodata) /
35
+ [LeRobot v3](https://github.com/huggingface/lerobot) compatible formats for
36
+ robot teleoperation datasets. Ships with 3D hand-keypoint processing
37
+ pipelines and offline SLAM dataset export.
38
+
39
+ **Architecture**: `core/` is the SDK core (capture pipeline, device managers,
40
+ recording/upload/playback logic — depends only on PyQt5.QtCore, never imports
41
+ ui); `ui/` does widget assembly only; `tools/` is a self-contained toolchain
42
+ (never imported by the main program), including standalone demos, model
43
+ weights, and tests.
44
+
45
+ <!-- Screenshot placeholder: main window / 3D keypoint visualization can go here
46
+ ![Main window](docs/images/main_window.png) -->
47
+
48
+ ### Features
49
+
50
+ - **Multi-camera synchronized recording**: per-stream record control
51
+ (normal stop saves / abnormal stop discards) with live recording duration
52
+ - **Draggable multi-view grid layout**, adjustable in playback view too
53
+ - **Device detection panel**: unified UVC / D435 / S80M enumeration, 2 s
54
+ polling, click to display
55
+ - **BLE data gloves**: one per hand, tactile data captured in sync with video
56
+ - **Recording history & playback**: SQLite persistence, compatible with
57
+ EgoData / LeRobot v3 sessions
58
+ - **HTTP upload**: post-recording upload to a self-hosted server, with
59
+ optional auto-sync/delete of local files
60
+ - **Chinese / English UI switching** (i18n)
61
+ - **3D hand keypoints**: D435 RGB-D pipeline
62
+ ([tools/hand_3d_d435/](tools/hand_3d_d435/)), S80C / S80M stereo
63
+ triangulation, MediaPipe bare-hand pipeline, plus a dedicated
64
+ black-glove pipeline (YOLO-World boxes + RTMPose keypoints — see
65
+ [Black Glove Keypoints](#black-glove-keypoints))
66
+ - **Offline SLAM dataset export** (ORB-SLAM compatible formats, with
67
+ validation tooling)
68
+ - **12-bit depth video recording**: single-stream gray HEVC MP4 (log depth
69
+ codes, reversibly decoded to millimetres), unified JET heatmap display
70
+ live and in playback
71
+ - **Direct HEVC recording**: low-bitrate HEVC at record time with
72
+ multi-encoder fallback; the uploader detects HEVC and skips redundant
73
+ re-compression
74
+
75
+ ### Quick Start
76
+
77
+ #### Install dependencies
78
+
79
+ ```bash
80
+ python -m venv venv
81
+ venv/bin/pip install -r requirements.txt
82
+ ```
83
+
84
+ > Optional dependencies such as `pyrealsense2`, `mediapipe`, `torch` are not
85
+ > listed in `requirements.txt` (lazily imported; the corresponding feature is
86
+ > unavailable if missing — one-click installs via `start.bat extras` below).
87
+
88
+ #### Windows one-click deployment (recommended for customers)
89
+
90
+ Double-click **`start.bat`** in the repo root: it installs Python 3.12
91
+ (silent download if absent), creates the venv, installs dependencies, and
92
+ launches the main program; already-deployed machines start instantly. Common
93
+ commands:
94
+
95
+ ```bat
96
+ start.bat deploy and launch (default)
97
+ start.bat reinstall delete venv and reinstall (first resort when broken)
98
+ start.bat extras additionally install mediapipe / pyrealsense2
99
+ start.bat extras-torch additionally install CPU torch (hand-keypoint RTMPose backend)
100
+ start.bat help open the operation guide and troubleshooting doc
101
+ ```
102
+
103
+ > Full operation guide, error-code reference, and intranet offline delivery
104
+ > procedure: [使用说明.md](使用说明.md) / [使用说明_EN.md](使用说明_EN.md)
105
+ > (offline wheel bundles are produced by `python scripts/pack_wheels.py`).
106
+
107
+ #### Launch the main program
108
+
109
+ ```bash
110
+ ./start.sh # Linux one-click deploy (same subcommands: reinstall/extras/...)
111
+ ./run.sh # launch directly when venv already exists
112
+ venv/bin/python main.py # launch directly
113
+ run.bat # launch directly when venv already exists (start.bat deploys first)
114
+ ```
115
+
116
+ No manual config needed on first run: `data/tasks.json` is generated from
117
+ built-in seed tasks; other config files are created the first time the
118
+ corresponding setting is saved (the repo only ships `*.example.json`
119
+ templates).
120
+
121
+ #### 3D hand keypoints (D435)
122
+
123
+ ```bash
124
+ ./tools/hand_3d_d435/run_live_d435.sh # live demo (direct camera)
125
+ ./tools/hand_3d_d435/run_live_d435.sh --replay <session_dir> # replay a session
126
+ ./tools/hand_3d_d435/run_live_d435.sh --glove # black-glove mode (see Black Glove Keypoints)
127
+ ./tools/hand_3d_d435/run_d435.sh <session_dir> # offline pipeline
128
+ ```
129
+
130
+ #### Demos
131
+
132
+ ```bash
133
+ ./tools/demos/run_stereo_depth_demo.sh # S80M depth-engine demo (SDK required, see below)
134
+ venv/bin/python tools/demos/test_stereo_depth_calib.py # stereo depth calibration self-check (SDK required)
135
+ venv/bin/python tools/hand_detection/demo_stereo_hands.py # stereo + MediaPipe hand demo
136
+ ```
137
+
138
+ ### Supported Hardware
139
+
140
+ | Device | Interface | Notes |
141
+ |---|---|---|
142
+ | UVC camera | `/dev/videoN` (OpenCV V4L2) | MJPG pixel format; up to 8 cameras |
143
+ | Intel RealSense D435 / D405 | `pyrealsense2` | RGB + depth dual slots; depth recorded as 12-bit gray HEVC MP4 (log depth codes) + live JET heatmap; built-in stall/framerate watchdog with auto-reconnect; D405 has a dedicated near-range capture profile |
144
+ | S80C / S80M stereo | FaysSense VI Kit SDK (bundled in-repo, incl. FT602 bridge driver) | No SDK install needed; camera profile `STEREO_CAM_FPS` (default 50 fps) decimated to 30 fps recording via wall-clock 1/30 s buckets (burst backfill + empty-bucket watchdog, ~3% empty-bucket rate on healthy recordings); callback frame capture (same as the official GUI); carries hardware nanosecond timestamps and IMU samples; since v1.0.11 the subprocess runs the SDK depth engine → third-tile live depth heatmap + 12-bit gray depth video recording |
145
+ | BLE data gloves | `bleak` | one per hand; parquet column names bound (`right_glove` / `left_glove`) |
146
+
147
+ ### Data Formats
148
+
149
+ Recordings live in a **task-level pooled layout** (v1.1.0+, LeRobot v3
150
+ naming): `data/recordings/<task>/` — the task name doubles as the upload
151
+ "project name", one file group per episode:
152
+
153
+ ```
154
+ data/recordings/<task>/
155
+ ├── videos/chunk-NNN/<slot>/episode-NNN.mp4 # one video per stream per episode
156
+ │ # RGB = mp4; depth = 12-bit gray mp4 (mkv on fallback)
157
+ ├── data/chunk-NNN/episode-NNN.parquet # one parquet per episode (zstd, sparse columns)
158
+ └── meta/
159
+ ├── info.json # task header; format="pooled_episodes_v1" is the discriminator
160
+ ├── stats.json # task-wide stats accumulator (count/mean/std/min/max per column)
161
+ ├── tasks.jsonl # task descriptions (single-line JSONL is contractual)
162
+ └── episodes/chunk-NNN/episode-NNN.parquet # one row per episode (10 columns)
163
+ ```
164
+
165
+ - **Numbering**: episodes are numbered globally from N=1 with
166
+ `chunk = (N-1) // 1000`, `file = (N-1) % 1000` (`chunks_size=1000`,
167
+ declared in info.json); every file of an episode shares the same
168
+ `(chunk, file)`. Abnormally-stopped recordings recycle their number;
169
+ completed episodes never do (in-app deletion is permanent)
170
+ - **Row layout**: each data parquet row = one 30 fps frame with key
171
+ columns (`episode_index` / `frame_index` / `timestamp` / `wall_time` /
172
+ `hardware_ns`) plus sparse observations (`observation.<sensor>` per
173
+ frame, `observation.imu` variable-length sample lists aligned by
174
+ `imu_ts_ns`, `observation.*hand_pose` zero placeholders backfilled by
175
+ post-processing). The full interface contract lives in
176
+ [docs/file_format.md](docs/file_format.md)
177
+ - Device naming follows the EgoData `<location>_<modality>` convention, e.g.
178
+ `head_left_rgb`, `head_right_rgb`, `head_depth`, `right_glove`,
179
+ `right_hand_pose`
180
+ - Depth video: single-stream 12-bit gray HEVC MP4 (log depth codes,
181
+ reversibly decoded to millimetres; falls back to FFV1 MKV when x265 is
182
+ unavailable); playback always renders the unified JET heatmap
183
+ - Recording history & upload queue: SQLite (`data/pipeline.db`)
184
+ - Offline processing output: `keypoints_output/<task>/episode_NNNNNN/`
185
+ (mirrors the pooled key, never written back into the recording
186
+ directory)
187
+
188
+ ### Black Glove Keypoints
189
+
190
+ Bare-hand MediaPipe detection fails on black gloves (4/68 hands
191
+ measured), so gloves are solved with a dedicated YOLO + RTMPose
192
+ pipeline (40/40 on black / grey / any-colour gloves):
193
+
194
+ 1. **Detection box** — open-vocabulary YOLO-World
195
+ (`yolov8m-worldv2.pt`, prompts `hand` / `glove`) or the trained
196
+ single-class yolo11n detector (`best.pt`), hot-switchable at runtime
197
+ 2. **Box tracking** — EMA-smoothed track boxes with dual-threshold
198
+ new-track gating and churn suppression (HandTracker)
199
+ 3. **Keypoints** — RTMPose hand5 (21 points, ONNX via onnxruntime CUDA)
200
+ cropped-box inference; a MediaPipe cropped-box backend is available
201
+ for comparison (hot-switchable at runtime)
202
+ 4. **Stabilization** — per-point confidence weighting; on low confidence
203
+ hold the last output and translate it with the smoothed box motion;
204
+ a hold-escape releases after N consecutive low-confidence frames so
205
+ real new poses (grip, fist) are never frozen out; degradation freeze
206
+ cap and handedness voting
207
+ 5. **3D lift** — D435 RGB-D depth lift or S80C stereo depth engine. On
208
+ S80C the right eye reuses the left eye's smoothed box translated by
209
+ disparity (`x_r = x_l − fx·B/z`) and runs the same stateless pose
210
+ backend; the 2D display stays decoupled from the 3D slot chain
211
+
212
+ Where it runs:
213
+
214
+ | Location | Mode | Notes |
215
+ |---|---|---|
216
+ | Main program post-processing | `HAND_TRACK_MODE=glove` | YOLO box + RTMPose 2D keypoints written back to `keypoints_output/` (92 dim/frame packed); `bare` mode = MediaPipe 2D + 3D world landmarks |
217
+ | D435 live demo | `--glove` | realtime 3D on RGB-D |
218
+ | S80C live demo | `--glove` | realtime 3D on stereo depth |
219
+ | Toolkit | [tools/glove_package/](tools/glove_package/) | annotation, training (`train_detector.py` → `best.pt`), CLIP auto-labeling |
220
+
221
+ ```bash
222
+ ./tools/hand_3d_d435/run_live_d435.sh --glove # D435 live glove mode
223
+ ./tools/hand_3d_s80c/run_live_s80c.sh --glove # S80C live stereo glove mode
224
+ ```
225
+
226
+ Implementation: detection / pose front-ends in
227
+ [tools/hand_detection/](tools/hand_detection/) and
228
+ [tools/glove_package/](tools/glove_package/); post-recording processing
229
+ in [core/hand_tracking.py](core/hand_tracking.py); live demos in
230
+ [tools/hand_3d_d435/](tools/hand_3d_d435/) and
231
+ [tools/hand_3d_s80c/](tools/hand_3d_s80c/).
232
+
233
+ ### Environment Variables
234
+
235
+ | Variable | Required | Description |
236
+ |---|---|---|
237
+ | `FAYSSENSE_SDK_DIR` | S80M demo/diagnostic tools only | Install path of the FaysSense VI Kit SDK (Release directory); those tools exit with an error if unset. The main program's S80C/S80M capture path does not need it (in-repo self-contained libs) |
238
+ | `FFMPEG_BIN` | no | ffmpeg executable override (used by render/video-write tooling) |
239
+ | `VENV_PY` | no | Python interpreter used by launcher scripts, default `venv/bin/python` |
240
+
241
+ ### Directory Structure
242
+
243
+ ```
244
+ collector/
245
+ ├── main.py # program entry (qt-material dark theme)
246
+ ├── start.bat / start.sh # one-click deployment (Windows / Linux)
247
+ ├── run.bat / run.sh # direct launch once deployed
248
+ ├── requirements.txt # required main-program dependencies (start.bat/start.sh self-check matches)
249
+ ├── .gitignore / .gitattributes # repo exclusions / line-ending rules (bat forced CRLF)
250
+ ├── core/ # ★ SDK core (depends only on PyQt5.QtCore and config, never imports ui;
251
+ │ # listed in data-flow order: capture → write → playback/upload)
252
+ │ ├── pipeline.py # recording main loop / pipeline state machine (data-flow hub)
253
+ │ ├── device_manager.py # unified device worker registry + panel toggle dispatch
254
+ │ ├── camera.py # UVC camera capture (CameraWorker)
255
+ │ ├── d435_manager.py # RealSense D435/D405 capture (heatmap/EMA/record write)
256
+ │ ├── s80m_manager.py # S80C/S80M subprocess capture + 50→30 decimation
257
+ │ ├── ble_engine.py # BLE data-glove capture
258
+ │ ├── egodata_writer.py # EgoData / LeRobot v3 recording writer (pooled layout)
259
+ │ ├── depth_codec.py # 12-bit log depth codec (gray12le HEVC video)
260
+ │ ├── encoder_probe.py # HEVC encoder availability probe (nvenc → x265 → x264)
261
+ │ ├── session_catalog.py # session scanning / metadata / fps resolution
262
+ │ ├── session_loader.py # background playback loader (QtCore signals)
263
+ │ ├── session_timeline.py # playback timeline
264
+ │ ├── depth_reader.py # depth video reader (gray12le MP4 / FFV1 MKV / legacy PNG16)
265
+ │ ├── uploader.py # session upload queue
266
+ │ ├── hand_tracking.py # hand-keypoint processing (glove / bare)
267
+ │ ├── helpers.py # session path/duration/size utilities
268
+ │ └── … # support modules: calibration, exposure, naming, SQLite
269
+ │ # history, task polling, rendering (see docs/core.md)
270
+ ├── ui/ # PyQt5 UI assembly
271
+ │ └── main_window.py # main window: slots / record control / panel dispatch (data-flow wiring point)
272
+ ├── config/ # global config + i18n strings + calibration/sensor JSON
273
+ ├── scripts/ # offline processing (process_hands.py) + offline packaging (pack_wheels.py)
274
+ ├── docs/ # per-directory module docs (see below)
275
+ ├── data/ # local config templates + recordings (both gitignored)
276
+ └── tools/ # toolchain (self-contained, never imported by the main program)
277
+ ├── gongsitubiao.png # UI logo (ui/main_window.py, ui/task_page.py)
278
+ ├── stereo_s80m/ # S80M stereo tools (read_stereo_rgb.py = main-program capture subprocess)
279
+ ├── hand_detection/ # YOLO glove detection + MediaPipe bare-hand pipeline
280
+ ├── hand_3d_d435/ # D435 RGB-D 3D hand keypoints (standalone module)
281
+ ├── hand_3d_s80c/ # S80C stereo realtime bare-hand/glove keypoint demo (with self-contained SDK)
282
+ ├── glove_package/ # YOLO-World + RTMPose black-glove toolkit
283
+ ├── fayssense_depth_sdk/ # FaysSense VI Kit depth-engine SDK (proprietary)
284
+ ├── models/ # model weights (MediaPipe hand_landmarker.task)
285
+ ├── weights/ # CLIP and other large weights (gitignored, not distributed)
286
+ ├── demos/ # delivery demos and self-check scripts
287
+ └── tests/ # regression / smoke tests
288
+ ```
289
+
290
+ ### Tests
291
+
292
+ ```bash
293
+ # hardware-free offline tests (QT_QPA_PLATFORM=offscreen)
294
+ venv/bin/python tools/tests/test_playback_multifps.py
295
+ venv/bin/python tools/tests/s80m_signal_regression.py
296
+ venv/bin/python tools/tests/s80m_50fps_decimation_test.py
297
+ venv/bin/python tools/tests/multi_device_registry_test.py
298
+ venv/bin/python tools/tests/exposure_control_test.py
299
+ venv/bin/python tools/tests/test_meta_devices.py
300
+ venv/bin/python tools/tests/test_depth_heatmap.py
301
+ venv/bin/python tools/tests/glove_widget_test.py
302
+ venv/bin/python tools/tests/grid_drag_fps_test.py
303
+ venv/bin/python tools/tests/device_panel_gui_smoke_test.py
304
+ venv/bin/python tools/tests/test_device_detector.py
305
+ # hardware tests require the corresponding device attached: d405_worker_test,
306
+ # d435_e2e_test, d435_gui_smoke_test, mono_regression, d435_playback_test, etc.
307
+ ```
308
+
309
+ ### Documentation
310
+
311
+ Per-module details live in [docs/](docs/) (one page per directory: purpose,
312
+ file inventory, data flow):
313
+
314
+ - [docs/index.md](docs/index.md) — repository overview (entry point of the doc set)
315
+ - [docs/core.md](docs/core.md) — SDK core (pipeline, device managers, recording/upload/playback)
316
+ - [docs/ui.md](docs/ui.md), [docs/config.md](docs/config.md) — UI, configuration
317
+ - [docs/data.md](docs/data.md) — config and data storage layout
318
+ - [docs/scripts.md](docs/scripts.md) — offline processing and deployment scripts
319
+ - [docs/tools.md](docs/tools.md), [docs/demos.md](docs/demos.md) — 3D tools, delivery demos
320
+ - [docs/stereo_s80m.md](docs/stereo_s80m.md), [docs/hand_detection.md](docs/hand_detection.md) — S80M, hand detection
321
+ - [docs/file_format.md](docs/file_format.md) — data file interface contract (authoritative definition of the v1.1.x pooled layout)
322
+
323
+ ### Privacy & Local Config
324
+
325
+ Real config files **never enter the repo**: `data/server_config.json` (may
326
+ contain server address and login credentials), `data/device_names.json`
327
+ (keys contain device serials/MACs), `data/tasks.json`, plus `data/*.db` and
328
+ `data/recordings/` are all excluded by `.gitignore`; apart from
329
+ `data/device_params.json` (factory-default empty config), the repo only
330
+ ships `*.example.json` templates. Do not commit modified real config files.
331
+
332
+ ### Third-Party Components & Licensing
333
+
334
+ - **FaysSense VI Kit SDK**: proprietary software; the main program's
335
+ S80C/S80M capture path uses the in-repo `tools/stereo_s80m/lib` and
336
+ `tools/hand_3d_s80c/third_party` (incl. the FT602 bridge driver
337
+ libft602.so and OpenCV 4.2 dependencies) — a fresh git clone runs with no
338
+ SDK install. `tools/fayssense_depth_sdk/` is an intranet-shared copy (used
339
+ by the S80C demo) and must be purged from history before any public
340
+ open-source release. Standalone demos/diagnostic tools can also point
341
+ `FAYSSENSE_SDK_DIR` at a separately installed SDK
342
+ - **Model weights**: `tools/models/hand_landmarker.task` (MediaPipe) and
343
+ others follow their respective upstream licenses; large weights such as
344
+ CLIP live in `tools/weights/` and are not distributed with the repo.
345
+ Verify upstream license terms before use/redistribution
346
+ - **ffmpeg**: the recorder prefers the static ffmpeg bundled with
347
+ `imageio-ffmpeg`
348
+
349
+ ### License
350
+
351
+ The LICENSE of this repository is to be determined (a LICENSE file will be
352
+ added before release). Third-party components (SDK, model weights, ffmpeg,
353
+ etc.) are governed by their upstream terms.
354
+
355
+ ### Contributing
356
+
357
+ Issues and merge requests are welcome. Development conventions (version
358
+ number defined only in `config/__init__.py`, i18n strings via `tr()`, use
359
+ `object` for large ints in PyQt5 signal params, core never imports ui, etc.)
360
+ are documented in [docs/index.md](docs/index.md#开发约定).
361
+
362
+ ### Changelog
363
+
364
+ - **v1.2.0** — minimal one-click deployment edition (`start_lite.bat/.sh`
365
+ + `venv_lite` whitelist env, ~750 MB): connect devices → record → upload
366
+ only — no login/playback/task page/skeleton solving. Devices: D435
367
+ (1280×720@30 RGB / 848×480@30 depth), one UVC camera (640×480@30
368
+ capture; 640×360@15 preview that auto-switches to the device you open),
369
+ one USB / one BLE glove. Direct x264 (encoder probe skipped).
370
+ `scripts/pack_lite.py` collects the release folder; import-isolation
371
+ guard + hardware-free smoke tests included
372
+ - **v1.1.4** — USB Type-C glove support (STM32 CDC serial engine, 60 fps
373
+ IMU + 16×16 tactile); real-time MANO 21-keypoint solving backfills the
374
+ `hand_pose` placeholder columns (with skeleton rendering in the main UI);
375
+ BLE glove dual-serial config update; single-file PyQt5 viewer demo
376
+ (RGB/depth/tactile/skeleton/IMU panels, live playback, draggable progress
377
+ bar) with tests
378
+ - **v1.1.3** — dual-directory (`videos/` + `data/`) timeline
379
+ reliability: the S80C/S80M 50→30 decimation now uses wall-clock
380
+ 1/30 s buckets with burst backfill (sensor hw-clock jumps no longer
381
+ cause steady frame drops) plus an empty-bucket watchdog (~3%
382
+ empty-bucket rate on healthy recordings, down from 8-15% when
383
+ recording depth). Depth-slot frame drops root-fixed: keep-latest
384
+ depth queue (the old FIFO dropped 35% of engine bursts), x265
385
+ preset=fast, quantization LUT, and alternate-frame engine feed.
386
+ Login & window behavior: closing the startup login now exits the app
387
+ (no more silent guest mode); the upload/playback dialogs get working
388
+ maximize/minimize buttons. Playback: click-on-groove seeking,
389
+ play-after-end restarts, speed-button polish
390
+ - **v1.1.2** — upload and playback dialogs reorganized into two-level
391
+ trees; depth storage switched to 12-bit gray HEVC MP4 (gray12le log
392
+ depth codes, same as LeRobot v3; display and storage now share one code
393
+ scale — the old heatmap near/far params are deprecated; falls back to FFV1
394
+ MKV when x265 is unavailable; readers keep legacy MKV/PNG16 fallback).
395
+ Per-episode meta files renamed `file-` → `episode-` (same numbering;
396
+ legacy chunk shards keep the `file-` prefix). In-app deletion now removes
397
+ files outright (no more `_trash/` staging area)
398
+ - **v1.1.1** — `stats.json` carries its own `count` accumulator per block;
399
+ the `.stats_state.json` sidecar is abolished
400
+ - **v1.1.0** — task-level pooled storage layout (`videos/` / `data/` /
401
+ `meta/` organized as `chunk-NNN/episode-NNN`, one file group per episode);
402
+ one-off migration script for legacy per-session directories
403
+
404
+ ---
405
+
406
+ ## 中文
407
+
408
+ 基于 PyQt5 的多模态数据采集 SDK + GUI:多路相机(UVC 摄像头、Intel
409
+ RealSense D435/D405、S80C / S80M 双目)与 BLE 数据手套的同步录制、回放与
410
+ HTTP 上传,输出 [EgoData](https://github.com/facebookresearch/egodata) /
411
+ [LeRobot v3](https://github.com/huggingface/lerobot) 兼容格式,供机器人
412
+ 遥操作数据集使用。配套手部 3D 关键点处理链路与离线 SLAM 数据集导出。
413
+
414
+ **架构定位**:`core/` 是 SDK 核心(采集管线、设备管理器、录制/上传/回放
415
+ 全部算法口径,只依赖 PyQt5.QtCore,禁止 import ui);`ui/` 只做界面组装;
416
+ `tools/` 是自包含工具链(不被主程序 import),含独立 demo、模型权重、测试。
417
+
418
+ <!-- 截图占位:界面截图、3D 关键点可视化效果图可放在此处
419
+ ![主界面](docs/images/main_window.png) -->
420
+
421
+ ### 功能特性
422
+
423
+ - **多路相机同步录制**:每路独立的录制控制(正常停止保存 / 异常停止丢弃),
424
+ 录制时长实时显示
425
+ - **可拖拽的多画面网格布局**,回放界面同样支持调位与分割条
426
+ - **设备检测面板**:统一枚举 UVC / D435 / S80M 设备,2 秒轮询,点击即显示
427
+ - **BLE 数据手套**:左右手各一只,触觉数据与视频同步采集
428
+ - **录制历史与回放**:SQLite 持久化,兼容 EgoData / LeRobot v3 会话
429
+ - **HTTP 上传**:录制完成后上传至自建服务器,可自动同步/删除本地文件
430
+ - **中英文界面切换**(i18n)
431
+ - **手部 3D 关键点**:D435 RGB-D 管线([tools/hand_3d_d435/](tools/hand_3d_d435/))、
432
+ S80C / S80M 双目三角化、MediaPipe 裸手管线,另有黑手套专用管线
433
+ (YOLO-World 检测框 + RTMPose 关键点,见[黑手套解算](#黑手套解算))
434
+ - **离线 SLAM 数据集导出**(ORB-SLAM 等格式,含校验工具)
435
+ - **12-bit 深度视频录制**:单流灰度 HEVC MP4(对数深度码、可逆解码回毫米),
436
+ 实���与回放统一 JET 热力图显示
437
+ - **录制直出 HEVC**:录制端低码率 HEVC(多编码器自动回退),
438
+ 上传端识别 HEVC 后跳过重复压缩
439
+
440
+ ### 快速开始
441
+
442
+ #### 安装依赖
443
+
444
+ ```bash
445
+ python -m venv venv
446
+ venv/bin/pip install -r requirements.txt
447
+ ```
448
+
449
+ > `pyrealsense2`、`mediapipe`、`torch` 等可选依赖未列入 `requirements.txt`
450
+ > (代码内惰性导入,缺失时对应功能不可用;一键安装见下方 `start.bat extras`)。
451
+
452
+ #### Windows 一键部署(推荐客户使用)
453
+
454
+ 双击根目录 **`start.bat`**:自动安装 Python 3.12(无则静默下载安装)、创建 venv、
455
+ 安装依赖并启动主程序;已部署过则秒开。常用命令:
456
+
457
+ ```bat
458
+ start.bat 部署并启动(默认)
459
+ start.bat reinstall 删除 venv 重装(出问题首选)
460
+ start.bat extras 追加安装 mediapipe / pyrealsense2
461
+ start.bat extras-torch 追加安装 CPU 版 torch(手部关键点 RTMPose 后端)
462
+ start.bat help 打开操作指引与异常排查文档
463
+ ```
464
+
465
+ > 完整操作指引、错误码对照与内网离线交付方式见 [使用说明.md](使用说明.md) /
466
+ > [使用说明_EN.md](使用说明_EN.md)(离线安装包由 `python scripts/pack_wheels.py` 生成)。
467
+
468
+ #### 启动主程序
469
+
470
+ ```bash
471
+ ./start.sh # Linux 一键部署(等价命令:reinstall/extras/...)
472
+ ./run.sh # 已有 venv 时直接启动
473
+ venv/bin/python main.py # 直接启动
474
+ run.bat # 已有 venv 时直接启动(start.bat 会先部署)
475
+ ```
476
+
477
+ 首次运行无需手动建配置:`data/tasks.json` 由内置种子任务生成,其余配置文件
478
+ 在首次保存对应设置时自动创建(仓库只提供 `*.example.json` 模板)。
479
+
480
+ #### 手部 3D 关键点(D435)
481
+
482
+ ```bash
483
+ ./tools/hand_3d_d435/run_live_d435.sh # 实时 demo(直连相机)
484
+ ./tools/hand_3d_d435/run_live_d435.sh --replay <会话目录> # 回放会话
485
+ ./tools/hand_3d_d435/run_live_d435.sh --glove # 黑手套模式(见「黑手套解算」)
486
+ ./tools/hand_3d_d435/run_d435.sh <会话目录> # 离线管线
487
+ ```
488
+
489
+ #### Demos
490
+
491
+ ```bash
492
+ ./tools/demos/run_stereo_depth_demo.sh # S80M 深度引擎 demo(需 SDK,见下)
493
+ venv/bin/python tools/demos/test_stereo_depth_calib.py # 双目深度标定自检(需 SDK)
494
+ venv/bin/python tools/hand_detection/demo_stereo_hands.py # 双目 + MediaPipe 手部 demo
495
+ ```
496
+
497
+ ### 硬件支持
498
+
499
+ | 设备 | 接入方式 | 说明 |
500
+ |---|---|---|
501
+ | UVC 相机 | `/dev/videoN`(OpenCV V4L2) | MJPG 像素格式;最多 8 路 |
502
+ | Intel RealSense D435 / D405 | `pyrealsense2` | RGB + depth 双路槽位;深度以 12-bit 灰度 HEVC MP4 录制(对数深度码)+ 实时 JET 热力图;内置停滞/帧率看门狗自动重连;D405 有独立近距采集配置 |
503
+ | S80C / S80M 双目 | FaysSense VI Kit SDK(仓库自带,含 FT602 桥驱动) | 无需安装 SDK;相机档 `STEREO_CAM_FPS`(默认 50fps)按 wall 时钟 1/30s 桶抽帧录制 30fps(突发补录 + 空桶看门狗,健康录制空桶率 ~3%),回调取帧(官方 GUI 同款);携带硬件纳秒时间戳与 IMU 样本;v1.0.11 起子进程内置 SDK 深度引擎 → 第三格实时深度热力图 + 12-bit 灰度深度视频录制 |
504
+ | BLE 数据手套 | `bleak` | 左右手各一只,parquet 列名绑定(`right_glove` / `left_glove`) |
505
+
506
+ ### 数据格式
507
+
508
+ 录制数据采用**任务级池化布局**(v1.1.0 起,LeRobot v3 命名):
509
+ `data/recordings/<任务>/`——任务名即上传语义的「项目名」,每段 episode
510
+ 一组文件:
511
+
512
+ ```
513
+ data/recordings/<任务>/
514
+ ├── videos/chunk-NNN/<槽名>/episode-NNN.mp4 # 每段每流一个视频
515
+ │ # RGB=mp4;深度=12-bit 灰 mp4(回落 mkv)
516
+ ├── data/chunk-NNN/episode-NNN.parquet # 每段一个 parquet(zstd,稀疏列)
517
+ └── meta/
518
+ ├── info.json # 任务级头部;format="pooled_episodes_v1" 为判别键
519
+ ├── stats.json # 全任务统计累加器(每列 count/mean/std/min/max)
520
+ ├── tasks.jsonl # 任务描述(单行 JSONL 是格式契约)
521
+ └── episodes/chunk-NNN/episode-NNN.parquet # 每段一行元数据(10 列)
522
+ ```
523
+
524
+ - **编号规则**:episode 全局从 N=1 递增,`chunk = (N-1) // 1000`、
525
+ `file = (N-1) % 1000`(`chunks_size=1000`,由 info.json 声明);同一
526
+ episode 的所有文件共用同一 `(chunk, file)`。异常终止的录制回收其编号
527
+ 复用;已完成的编号永不复用(应用内删除即彻底删除)
528
+ - **行布局**:data parquet 一行 = 一个 30fps 帧,键列
529
+ (`episode_index` / `frame_index` / `timestamp` / `wall_time` /
530
+ `hardware_ns`)+ 稀疏观测列(`observation.<传感器>` 逐帧、
531
+ `observation.imu` 变长样本列表按 `imu_ts_ns` 对齐、
532
+ `observation.*hand_pose` 恒写占位零由后处理回填)。完整接口契约见
533
+ [docs/file_format.md](docs/file_format.md)
534
+ - 设备命名遵循 EgoData 标准 `<位置>_<模态>`:如 `head_left_rgb`、
535
+ `head_right_rgb`、`head_depth`、`right_glove`、`right_hand_pose`
536
+ - 深度视频:单流 12-bit 灰度 HEVC MP4(对数深度码,可逆解码回毫米;
537
+ x265 不可用时回落 FFV1 MKV),回放显示统一 JET 热力图
538
+ - 录制历史与上传队列:SQLite(`data/pipeline.db`)
539
+ - 离线处理输出:`keypoints_output/<任务>/episode_NNNNNN/`(镜像池化
540
+ 编号,不写回录制目录)
541
+
542
+ ### 黑手套解算
543
+
544
+ MediaPipe 裸手检测在黑手套上失效(实测 4/68 手),因此黑手套改用专用
545
+ 的 YOLO + RTMPose 管线解算(黑/灰/任意颜色手套实测 40/40):
546
+
547
+ 1. **检测框** — 开放词汇 YOLO-World(`yolov8m-worldv2.pt`,提示词
548
+ `hand` / `glove`)或训练的单类 yolo11n 检测器(`best.pt`),运行中
549
+ 可热切换
550
+ 2. **框跟踪** — HandTracker:EMA 平滑跟踪框、新 track 双阈值门控、
551
+ 碎片框抑制
552
+ 3. **关键点** — RTMPose hand5(21 点,ONNX / onnxruntime CUDA)按框
553
+ 裁剪推理;可切换 MediaPipe 裁剪后端做对比(运行中热切换)
554
+ 4. **稳定层** — 逐点置信度加权;低置信时持出上次输出并按平滑框位移
555
+ 平移补偿;连续 N 帧低置信后放行本轮骨架(握拳/抓取等真实新姿势
556
+ 不被无限冻结);退化冻结上限与手性票仓防抖
557
+ 5. **3D 抬升** — D435 RGB-D 深度抬升或 S80C 双目深度引擎。S80C 上
558
+ 右目共享左目平滑框按视差平移(`x_r = x_l − fx·B/z`)后复用同一
559
+ 无状态 pose 后端;2D 显示与 3D 槽位链解耦
560
+
561
+ 运行位置:
562
+
563
+ | 位置 | 模式 | 说明 |
564
+ |---|---|---|
565
+ | 主程序录后处理 | `HAND_TRACK_MODE=glove` | YOLO 框 + RTMPose 2D 关键点写回 `keypoints_output/`(每帧 92 维打包);`bare` 模式 = MediaPipe 2D + 3D world landmarks |
566
+ | D435 实时 demo | `--glove` | RGB-D 实时 3D |
567
+ | S80C 实时 demo | `--glove` | 双目深度实时 3D |
568
+ | 工具包 | [tools/glove_package/](tools/glove_package/) | 标注、训练(`train_detector.py` → `best.pt`)、CLIP 自动标注 |
569
+
570
+ ```bash
571
+ ./tools/hand_3d_d435/run_live_d435.sh --glove # D435 实时手套模式
572
+ ./tools/hand_3d_s80c/run_live_s80c.sh --glove # S80C 实时双目手套模式
573
+ ```
574
+
575
+ 实现位置:检测/关键点前端在 [tools/hand_detection/](tools/hand_detection/)
576
+ 与 [tools/glove_package/](tools/glove_package/);录后处理在
577
+ [core/hand_tracking.py](core/hand_tracking.py);实时 demo 在
578
+ [tools/hand_3d_d435/](tools/hand_3d_d435/) 与
579
+ [tools/hand_3d_s80c/](tools/hand_3d_s80c/)。
580
+
581
+ ### 环境变量
582
+
583
+ | 变量 | 必需 | 说明 |
584
+ |---|---|---|
585
+ | `FAYSSENSE_SDK_DIR` | 仅 S80M demo/诊断工具 | FaysSense VI Kit SDK 安装路径(Release 目录),未设置时相关工具直接报错退出;主程序 S80C/S80M 采集链路不需要(仓库内自包含) |
586
+ | `FFMPEG_BIN` | 否 | ffmpeg 可执行文件覆盖(渲染/视频写入工具使用) |
587
+ | `VENV_PY` | 否 | launcher 脚本使用的 Python 解释器,默认 `venv/bin/python` |
588
+
589
+ ### 目录结构
590
+
591
+ ```
592
+ collector/
593
+ ├── main.py # 程序入口(qt-material 暗色主题)
594
+ ├── start.bat / start.sh # 一键部署(Windows / Linux)
595
+ ├── run.bat / run.sh # 已部署时直接启动
596
+ ├── requirements.txt # 主程序必需依赖(start.bat/start.sh 依赖自检与之一致)
597
+ ├── .gitignore / .gitattributes # 仓库排除规则 / 换行符规范(bat 强制 CRLF)
598
+ ├── core/ # ★ SDK 核心(仅依赖 PyQt5.QtCore 与 config,禁止 import ui;
599
+ │ # 下列按数据流排序:采集 → 写入 → 回放/上传)
600
+ │ ├── pipeline.py # 录制主循环 / 管线状态机(数据流中枢)
601
+ │ ├── device_manager.py # 统一设备 worker 注册表 + 面板开关分派
602
+ │ ├── camera.py # UVC 相机采集(CameraWorker)
603
+ │ ├── d435_manager.py # RealSense D435/D405 采集(热力图/EMA/录制写入)
604
+ │ ├── s80m_manager.py # S80C/S80M 子进程采集 + 50→30 抽帧
605
+ │ ├── ble_engine.py # BLE 数据手套采集
606
+ │ ├── egodata_writer.py # EgoData / LeRobot v3 录制写入(池化落盘)
607
+ │ ├── depth_codec.py # 12-bit 对数深度码编码(gray12le HEVC 视频)
608
+ │ ├── encoder_probe.py # HEVC 编码器可用性探测(nvenc → x265 → x264)
609
+ │ ├── session_catalog.py # 会话扫描 / 元数据 / 帧率解析
610
+ │ ├── session_loader.py # 回放后台加载器(QtCore 信号)
611
+ │ ├── session_timeline.py # 回放时间线
612
+ │ ├── depth_reader.py # 深度视频读取(gray12le MP4 / FFV1 MKV / 旧 PNG16)
613
+ │ ���── uploader.py # 会话上传队列
614
+ │ ├── hand_tracking.py # 手部关键点处理(手套 / 裸手)
615
+ │ ├── helpers.py # 会话路径/时长/大小等通用工具
616
+ │ └── … # 其余辅助模块:标定、曝光、命名、SQLite 历史、
617
+ │ # 任务轮询、渲染等(详见 docs/core.md)
618
+ ├── ui/ # PyQt5 界面组装
619
+ │ └── main_window.py # 主窗口:槽位/录制控制/面板分派(数据流接线点)
620
+ ├── config/ # 全局配置 + i18n 文案 + 标定/传感器 JSON
621
+ ├── scripts/ # 离线处理(process_hands.py)+ 离线部署打包(pack_wheels.py)
622
+ ├── docs/ # 模块文档(逐目录说明,见下)
623
+ ├── data/ # 本地配置模板 + 录制数据(均 gitignore)
624
+ └── tools/ # 工具链(自包含,不被主程序 import)
625
+ ├── gongsitubiao.png # 界面 logo(ui/main_window.py、ui/task_page.py 引用)
626
+ ├── stereo_s80m/ # S80M 双目工具(read_stereo_rgb.py = 主程序采集子进程)
627
+ ├── hand_detection/ # YOLO 手套检测 + MediaPipe 裸手管线
628
+ ├── hand_3d_d435/ # D435 RGB-D 3D 手部关键点(独立模块)
629
+ ├── hand_3d_s80c/ # S80C 双目实时裸手/手套关键点 demo(含自包含 SDK)
630
+ ├── glove_package/ # YOLO-World + RTMPose 黑手套工具箱
631
+ ├── fayssense_depth_sdk/ # FaysSense VI Kit 深度引擎 SDK(专有)
632
+ ├── models/ # 模型权重(MediaPipe hand_landmarker.task)
633
+ ├── weights/ # CLIP 等大权重(gitignore,不随仓库分发)
634
+ ├── demos/ # 交付版 demo 与自检脚本
635
+ └── tests/ # 回归 / 冒烟测试
636
+ ```
637
+
638
+ ### 测试
639
+
640
+ ```bash
641
+ # 无真机可跑的离线测试(QT_QPA_PLATFORM=offscreen)
642
+ venv/bin/python tools/tests/test_playback_multifps.py
643
+ venv/bin/python tools/tests/s80m_signal_regression.py
644
+ venv/bin/python tools/tests/s80m_50fps_decimation_test.py
645
+ venv/bin/python tools/tests/multi_device_registry_test.py
646
+ venv/bin/python tools/tests/exposure_control_test.py
647
+ venv/bin/python tools/tests/test_meta_devices.py
648
+ venv/bin/python tools/tests/test_depth_heatmap.py
649
+ venv/bin/python tools/tests/glove_widget_test.py
650
+ venv/bin/python tools/tests/grid_drag_fps_test.py
651
+ venv/bin/python tools/tests/device_panel_gui_smoke_test.py
652
+ venv/bin/python tools/tests/test_device_detector.py
653
+ # 真机相关测试需连接对应设备:d405_worker_test、d435_e2e_test、
654
+ # d435_gui_smoke_test、mono_regression、d435_playback_test 等
655
+ ```
656
+
657
+ ### 文档
658
+
659
+ 各模块细节见 [docs/](docs/)(每个目录一篇说明:定位、文件清单、数据流):
660
+
661
+ - [docs/index.md](docs/index.md) — 仓库总览(本文档集的入口)
662
+ - [docs/core.md](docs/core.md) — SDK 核心(管线、设备管理器、录制/上传/回放)
663
+ - [docs/ui.md](docs/ui.md)、[docs/config.md](docs/config.md) — 界面、配置
664
+ - [docs/data.md](docs/data.md) — 配置与数据存储结构
665
+ - [docs/scripts.md](docs/scripts.md) — 离线处理与部署脚本
666
+ - [docs/tools.md](docs/tools.md)、[docs/demos.md](docs/demos.md) — 3D 工具、交付 demo
667
+ - [docs/stereo_s80m.md](docs/stereo_s80m.md)、[docs/hand_detection.md](docs/hand_detection.md) — S80M、手部检测
668
+ - [docs/file_format.md](docs/file_format.md) — 数据文件接口契约(v1.1.x 任务池化布局权威定义)
669
+
670
+ ### 隐私与本地配置
671
+
672
+ 真实配置文件**不进仓库**:`data/server_config.json`(可能含服务器地址与登录凭据)、
673
+ `data/device_names.json`(key 含设备序列号/MAC)、`data/tasks.json` 以及
674
+ `data/*.db`、`data/recordings/` 均被 `.gitignore` 排除;除
675
+ `data/device_params.json`(出厂默认空配置)外,仓库只提供
676
+ `*.example.json` 模板。请勿将修改过的真实配置文件提交。
677
+
678
+ ### 第三方组件与许可
679
+
680
+ - **FaysSense VI Kit SDK**:专有软件;主程序 S80C/S80M 采集链路使用仓库自带的
681
+ `tools/stereo_s80m/lib` 与 `tools/hand_3d_s80c/third_party`(含 FT602 桥驱动
682
+ libft602.so 与 OpenCV 4.2 依赖),git 克隆后无需安装 SDK 即可运行;
683
+ `tools/fayssense_depth_sdk/` 为内网共享副本(S80C demo 用),对外开源发布前
684
+ 需从历史中移除。独立 demo/诊断工具也可经 `FAYSSENSE_SDK_DIR` 指向自行安装的 SDK
685
+ - **模型权重**:`tools/models/hand_landmarker.task`(MediaPipe)等遵循各自上游许可;
686
+ CLIP 等大权重位于 `tools/weights/`,不随仓库分发。使用 / 再分发前请核实上游许可条款
687
+ - **ffmpeg**:录制器优先使用 `imageio-ffmpeg` 捆绑的静态 ffmpeg
688
+
689
+ ### 许可证
690
+
691
+ 本仓库 LICENSE 待定(发布前补充 LICENSE 文件)。第三方组件(SDK、模型权重、
692
+ ffmpeg 等)的许可以其上游条款为准。
693
+
694
+ ### 贡献
695
+
696
+ 欢迎提交 Issue 与 Merge Request。开发约定(版本号唯一定义在 `config/__init__.py`、
697
+ i18n 文案经 `tr()` 翻译、PyQt5 信号参数用 `object` 封送大整数、core 禁止 import ui
698
+ 等)见 [docs/index.md](docs/index.md#开发约定)。
699
+
700
+ ### 更新记录
701
+
702
+ - **v1.2.0** — 新增极简一键部署采集版(`start_lite.bat/.sh` + `venv_lite`
703
+ 白名单依赖,约 750MB):只做连接设备→采集→上传,无登录/回放/任务页/
704
+ 骨架解算。设备:D435(1280×720@30 RGB / 848×480@30 深度)、UVC 摄像头
705
+ 1 台(640×480@30 采集;预览 640×360@15,开哪台自动切哪台)、USB/BLE
706
+ 手套各 1 台。x264 直录(跳过编码器探针)。`scripts/pack_lite.py` 收集
707
+ 发布目录,含导入隔离断言与无硬件冒烟自检
708
+ - **v1.1.4** — 新增 USB Type-C 手套接入(STM32 CDC 串口引擎,60fps IMU +
709
+ 16×16 触觉);录制时实时解算 MANO 21 关键点回填 hand_pose 占位列(含
710
+ 主界面骨架渲染);BLE 手套双序列号配置更新;新增单文件 PyQt5 查看器
711
+ demo(RGB/深度/触觉/骨架/IMU 面板、实时播放与可拖动的进度条)及配套测试
712
+ - **v1.1.3** — 双目录制(`videos/` + `data/` 两树)时间轴可靠性:
713
+ S80C/S80M 50→30 抽帧改 wall 时钟 1/30s 桶 + 突发补录(传感器 hw
714
+ 时钟跳变不再造成稳定缺帧)+ 空桶看门狗(健康录制空桶率 ~3%,带
715
+ 深度录制从 8-15% 降至 ~3%)。深度槽丢帧根治:keep-latest 深度队列
716
+ (旧 FIFO 在引擎突发期丢 35%)、x265 preset=fast、量化查表、
717
+ worker 隔帧喂深度引擎。登录与窗口行为:启动首屏关闭登录即退出
718
+ 应用(不再静默进游客模式);上传/回放对话框的最大化最小化按钮
719
+ 在 GNOME/Mutter 下真正可用。回放体验:点击滑槽直接跳帧、播完再点
720
+ 播放从头重播、倍速按钮文字不再截断
721
+ - **v1.1.2** — 上传与回放对话框改两级树结构;深度存储改 12-bit 灰度
722
+ HEVC MP4(gray12le 对数深度码,与
723
+ LeRobot v3 同款;显示与存储统一码值,旧 heatmap 近/远参数废弃;x265
724
+ 不可用时回落 FFV1 MKV;读取端保留旧 MKV/PNG16 回退)。每段文件名前缀
725
+ `file-` → `episode-`(编号不变,旧分片保留 `file-` 前缀不再重名)。
726
+ 应用内删除改为直接彻底删除(不再产生 `_trash/` 回收区)
727
+ - **v1.1.1** — `stats.json` 自含 `count` 累加器(每块),
728
+ `.stats_state.json` 边车废除
729
+ - **v1.1.0** — 任务级池化存储布局(`videos/` / `data/` / `meta/` 按
730
+ `chunk-NNN/episode-NNN` 组织、每段 episode 一组文件);旧会话目录
731
+ 一次性迁移脚本
collector/config/FS-VI80-S80C_3500000262190088_camera_calibration.yaml ADDED
@@ -0,0 +1,29 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ cam0:
2
+ T_cam_imu:
3
+ - [-0.999881744384766, 0.004449303261936, -0.014725826680660, 0.005998577456921]
4
+ - [-0.005106299184263, -0.998979330062866, 0.044882606714964, 0.004701206926256]
5
+ - [-0.014511099085212, 0.044952493160963, 0.998883724212646, -0.016389895230532]
6
+ - [0.0, 0.0, 0.0, 1.0]
7
+ camera_model: pinhole
8
+ distortion_coeffs: [0.062210198491812, 0.012444571591914, -0.003091567661613, -0.000578491541091]
9
+ distortion_model: equidistant
10
+ intrinsics: [228.609970092773438, 228.666488647460938, 310.380493164062500, 170.066619873046875]
11
+ resolution: [640, 400]
12
+ timeshift_cam_imu: -0.001947268727235
13
+ cam1:
14
+ T_cam_imu:
15
+ - [-0.999790549278259, 0.019221400842071, 0.007029340602458, -0.074173934757710]
16
+ - [-0.018963092938066, -0.999202847480774, 0.035132162272930, 0.004080146551132]
17
+ - [0.007699025794864, 0.034991502761841, 0.999357938766479, -0.015980923548341]
18
+ - [0.0, 0.0, 0.0, 1.0]
19
+ T_cn_cnm1:
20
+ - [0.999654293060303, -0.013781057670712, 0.022393604740500, -0.079738616943359]
21
+ - [0.013997744768858, 0.999856710433960, -0.009548537433147, -0.000860852771439]
22
+ - [-0.022258799523115, 0.009858686476946, 0.999703586101532, 0.000491287559271]
23
+ - [0.0, 0.0, 0.0, 1.0]
24
+ camera_model: pinhole
25
+ distortion_coeffs: [0.054715570062399, 0.025506600737572, -0.013235029764473, 0.001610828214325]
26
+ distortion_model: equidistant
27
+ intrinsics: [228.513122558593750, 228.619979858398438, 315.618072509765625, 201.517837524414062]
28
+ resolution: [640, 400]
29
+ timeshift_cam_imu: -0.001850676140748
collector/config/FS-VI80-S80C_3500000262190088_imu.yaml ADDED
@@ -0,0 +1,19 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ %YAML:1.0
2
+ # Generated by dump_fays_imu_calibration.py at 2026-08-26T09:09:04.671171+00:00
3
+ device:
4
+ model: FS-VI80-S80C
5
+ serial_number: 3500000262190088
6
+ camera_count: 2
7
+ source:
8
+ api: FAYS_VIK_GetCalibrationParam
9
+ physical_usb_path: 2-3.2
10
+ ft_index: null
11
+ stereo_dev_port: /dev/video0
12
+ imu_dev_port: /dev/video2
13
+ usb_speed_mbps: 5000
14
+ imu:
15
+ accelerometer_noise_density: 0.00718758347633413
16
+ accelerometer_random_walk: 7.16249485488661e-05
17
+ gyroscope_noise_density: 0.000806373659162483
18
+ gyroscope_random_walk: 8.2719725151112e-06
19
+ update_rate_hz: 1035
collector/config/__init__.py ADDED
@@ -0,0 +1,6 @@
 
 
 
 
 
 
 
1
+ """config 包 —— 应用配置、国际化与版本号。
2
+
3
+ 版本号集中定义于此:settings.APP_VERSION 及其他模块均从这里取值。
4
+ """
5
+
6
+ __version__ = "1.2.1" # 修复大文件上传被 10 秒误杀(urllib3 发送请求体阶段 socket 超时错用连接超时,服务器慢读停顿超 10s 即 Connection aborted,现已改用读超时窗口);上传对话框关闭后任务在后台继续完成(手动上传并入主窗口共享上传队列,重复提交自动跳过)。
collector/config/fays_device.json ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ {
2
+ "model": "FS-VI80-S80C",
3
+ "serial": "3500000262190088",
4
+ "firmware": "1.2.5",
5
+ "camera_count": 2,
6
+ "imu_count": 1,
7
+ "sdk_version": "3.9.0",
8
+ "stereo_dev_port": "/dev/video0",
9
+ "imu_dev_port": "/dev/video2",
10
+ "physical_usb_path": "2-3.2",
11
+ "usb_speed_mbps": 5000,
12
+ "captured_at": "2026-08-26T09:09:04.671267+00:00"
13
+ }
collector/config/i18n.py ADDED
@@ -0,0 +1,536 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 国际化(i18n)模块 —— 中英文切换支持。
3
+
4
+ 使用方式:
5
+ from config.i18n import tr, lang_manager
6
+ text = tr("开始录制") # 返回当前语言的翻译
7
+ lang_manager.set_language("en") # 切到英文
8
+ lang_manager.language_changed.connect(my_slot) # 监听语言切换
9
+ """
10
+
11
+ from PyQt5.QtCore import QObject, pyqtSignal
12
+
13
+
14
+ # ═══════════════════════════════════════════════════════
15
+ # 翻译字典 { 中文原文: { "en": "English", "zh": "中文" } }
16
+ # ═══════════════════════════════════════════════════════
17
+
18
+ _TRANSLATIONS = {
19
+ # ── 录制记录 ──────────────────────────────────────
20
+ "多路录制": {"en": "Multi-Camera", "zh": "多路录制"},
21
+
22
+ # ── 应用程序 ──────────────────────────────────────
23
+ "DAQ 视频管线": {"en": "DAQ Video Pipeline", "zh": "DAQ 视频管线"},
24
+ "多路摄像机实时监控与录制系统": {"en": "Multi-camera Monitoring & Recording System", "zh": "多路摄像机实时监控与录制系统"},
25
+
26
+ # ── 菜单栏 ────────────────────────────────────────
27
+ "文件(&F)": {"en": "&File", "zh": "文件(&F)"},
28
+ "扫描摄像机": {"en": "Scan Cameras", "zh": "扫描摄像机"},
29
+ "添加摄像机…": {"en": "Add Camera…", "zh": "添加摄像机…"},
30
+ "退出(&X)": {"en": "E&xit", "zh": "退出(&X)"},
31
+ "视图(&V)": {"en": "&View", "zh": "视图(&V)"},
32
+ "清空日志": {"en": "Clear Log", "zh": "清空日志"},
33
+ "刷新历史记录": {"en": "Refresh History", "zh": "刷新历史记录"},
34
+ "重置画面大小": {"en": "Reset Camera Sizes", "zh": "重置画面大小"},
35
+ "帮助(&H)": {"en": "&Help", "zh": "帮助(&H)"},
36
+ "关于": {"en": "About", "zh": "关于"},
37
+ "语言(&L)": {"en": "&Language", "zh": "语言(&L)"},
38
+ "中文": {"en": "中文", "zh": "中文"},
39
+ "English": {"en": "English", "zh": "English"},
40
+
41
+ # ── 工具栏 ────────────────────────────────────────
42
+ "🔍 扫描": {"en": "🔍 Scan", "zh": "🔍 扫描"},
43
+ "+ 添加": {"en": "+ Add", "zh": "+ 添加"},
44
+ "⏺ 全部录制": {"en": "⏺ Record All", "zh": "⏺ 全部录制"},
45
+ "⏹ 完成录制": {"en": "⏹ Finish Recording", "zh": "⏹ 完成录制"},
46
+ "⛔ 异常终止": {"en": "⛔ Abort Recording", "zh": "⛔ 异常终止"},
47
+ "✕ 全部移除": {"en": "✕ Remove All", "zh": "✕ 全部移除"},
48
+ "📂 回放": {"en": "📂 Playback", "zh": "📂 回放"},
49
+ "☁ 上传": {"en": "☁ Upload", "zh": "☁ 上传"},
50
+
51
+ # ── 工具栏提示 ────────────────────────────────────
52
+ "任务名称…": {"en": "Task name…", "zh": "任务名称…"},
53
+ "输入任务名称,录制文件夹将包含此标注": {"en": "Enter a task name to label the recording folder", "zh": "输入任务名称,录制文件夹将包含此标注"},
54
+ "停止录制并永久删除本次录制的全部文件": {"en": "Stop recording and permanently delete all files of this session", "zh": "停止录制并永久删除本次录制的全部文件"},
55
+
56
+ # ── 相机控件 ──────────────────────────────────────
57
+ "● 开始录制": {"en": "● Start Recording", "zh": "● 开始录制"},
58
+ "■ 完成录制": {"en": "■ Stop Recording", "zh": "■ 完成录制"},
59
+ "✕ 异常停止": {"en": "✕ Abort", "zh": "✕ 异常停止"},
60
+ "无信号": {"en": "No Signal", "zh": "无信号"},
61
+ "已��开": {"en": "Disconnected", "zh": "已断开"},
62
+ "等待中…": {"en": "Waiting…", "zh": "等待中…"},
63
+ "FPS": {"en": "FPS", "zh": "FPS"},
64
+
65
+ # ── 状态栏 ────────────────────────────────────────
66
+ "就绪": {"en": "Ready", "zh": "就绪"},
67
+ "摄像机:": {"en": "Cameras:", "zh": "摄像机:"},
68
+ "录制中:": {"en": "Recording:", "zh": "录制中:"},
69
+
70
+ # ── 日志消息 ──────────────────────────────────────
71
+ "DAQ 视频管线已启动。": {"en": "DAQ Video Pipeline started.", "zh": "DAQ 视频管线已启动。"},
72
+ "录制目录:": {"en": "Recording directory:", "zh": "录制目录:"},
73
+ "正在扫描摄像机 (MSMF → DShow → AUTO)…": {"en": "Scanning for cameras (MSMF → DShow → AUTO)…", "zh": "正在扫描摄像机 (MSMF → DShow → AUTO)…"},
74
+ "发现 Camera {},后端: {}": {"en": "Found Camera {} via {}", "zh": "发现 Camera {},后端: {}"},
75
+ "未检测到摄像机。": {"en": "No cameras detected.", "zh": "未检测到摄像机。"},
76
+ "检测到 {} 台摄像机。": {"en": "Detected {} camera(s).", "zh": "检测到 {} 台摄像机。"},
77
+ "摄像机 {} 已添加 ({}):": {"en": "Camera {} added as '{}'.", "zh": "摄像机 {} 已添加 ({}):"},
78
+ "摄像机 '{}' 已移除。": {"en": "Camera '{}' removed.", "zh": "摄像机 '{}' 已移除。"},
79
+ "[{}] 摄像机状态: {}": {"en": "[{}] Camera state: {}", "zh": "[{}] 摄像机状态: {}"},
80
+ "[{}] ▶ 录制开始。": {"en": "[{}] ▶ Recording started.", "zh": "[{}] ▶ 录制开始。"},
81
+ "[{}] ▶ 录制开始 — 编码: {}": {"en": "[{}] ▶ Recording started — codec: {}", "zh": "[{}] ▶ 录制开始 — 编码: {}"},
82
+ " | 任务: {}": {"en": " | Task: {}", "zh": " | 任务: {}"},
83
+ "[{}] ■ 录制完成: {} ({} MB)": {"en": "[{}] ■ Recording completed: {} ({} MB)", "zh": "[{}] ■ 录制完成: {} ({} MB)"},
84
+ "[{}] ✕ 录制已丢弃。": {"en": "[{}] ✕ Recording aborted — file discarded.", "zh": "[{}] ✕ 录制已丢弃。"},
85
+ "[{}] 相机已打开: {}×{} @ {}": {"en": "[{}] Camera opened: {}×{} @ {}", "zh": "[{}] 相机已打开: {}×{} @ {}"},
86
+
87
+ # ── 录制完成消息 ──────────────────────────────────
88
+ "[{}] ■ 录制完成: {}": {"en": "[{}] ■ Recording completed: {}", "zh": "[{}] ■ 录制完成: {}"},
89
+
90
+ # ── 双目摄像机 ────────────────────────────────────
91
+ "正在启动双目摄像机 (S80M)…": {"en": "Starting stereo camera (S80M)…", "zh": "正在启动双目摄像机 (S80M)…"},
92
+ "双目管道已连接,等待帧数据…": {"en": "Stereo pipe connected, waiting for frames…", "zh": "双目管道已连接,等待帧数据…"},
93
+ "双目摄像机已启动": {"en": "Stereo camera started", "zh": "双目摄像机已启动"},
94
+ "双目摄像机画面已开始传输": {"en": "Stereo camera streaming started", "zh": "双目摄像机画面已开始传输"},
95
+ "[双目] 管道断开": {"en": "[Stereo] Pipe broken", "zh": "[双目] 管道断开"},
96
+ "[双目错误] 读取帧异常: {}": {"en": "[Stereo Error] Read exception: {}", "zh": "[双目错误] 读取帧异常: {}"},
97
+ "双目摄像机已断开 (共 {} 帧)": {"en": "Stereo camera disconnected ({} frames)", "zh": "双目摄像机已断开 (共 {} 帧)"},
98
+ "[双目] 子进程已退出,退出码: {}": {"en": "[Stereo] Subprocess exited, code: {}", "zh": "[双目] 子进程已退出,退出码: {}"},
99
+ "[双目 stderr] {}": {"en": "[Stereo stderr] {}", "zh": "[双目 stderr] {}"},
100
+ "[错误] 双目 demo 脚本不存在": {"en": "[Error] Stereo demo script not found", "zh": "[错误] 双目 demo 脚本不存在"},
101
+
102
+ # ── 相机模式切换 ──────────────────────────────────
103
+ "📷 单目摄像机": {"en": "📷 Mono Camera", "zh": "📷 单目摄像机"},
104
+ "👁 双目摄像机 (S80M)": {"en": "👁 Stereo Camera (S80M)", "zh": "👁 双目摄像机 (S80M)"},
105
+ "切换摄像机类型": {"en": "Switch camera type", "zh": "切换摄像机类型"},
106
+ "已切换到:单目摄像机模式": {"en": "Switched to: Mono camera mode", "zh": "已切换到:单目摄像机模式"},
107
+ "已切换到:双目摄像机模式 (S80M)": {"en": "Switched to: Stereo camera mode (S80M)", "zh": "已切换到:双目摄像机模式 (S80M)"},
108
+ "🔭 深度双目 (RealSense)": {"en": "🔭 Depth Stereo (RealSense)", "zh": "🔭 深度双目 (RealSense)"},
109
+ "已切换到:深度双目模式 (RealSense)": {"en": "Switched to: Depth stereo mode (RealSense)", "zh": "已切换到:深度双目模式 (RealSense)"},
110
+ "正在启动深度双目摄像机 ({})…": {"en": "Starting depth stereo camera ({})…", "zh": "正在启动深度双目摄像机 ({})…"},
111
+ "深度双目摄像机已启动: {}": {"en": "Depth stereo camera started: {}", "zh": "深度双目摄像机已启动: {}"},
112
+ "[错误] 未检测到 RealSense 设备": {"en": "[Error] No RealSense device detected", "zh": "[错误] 未检测到 RealSense 设备"},
113
+ "[RealSense 错误] {}": {"en": "[RealSense error] {}", "zh": "[RealSense 错误] {}"},
114
+ "请选择相机模式(单目/双目),然后点击 [扫描] 按钮。": {
115
+ "en": "Please select camera mode (Mono/Stereo), then click [Scan].",
116
+ "zh": "请选择相机模式(单目/双目),然后点击 [扫描] 按钮。",
117
+ },
118
+
119
+ # ── 错误消息 ──────────────────────────────────────
120
+ "[错误]": {"en": "[ERROR]", "zh": "[错误]"},
121
+ "摄像机 {}: 无法打开 (已尝试 MSMF, DShow, AUTO)": {"en": "Camera {}: cannot open (tried MSMF, DShow, AUTO)", "zh": "摄像机 {}: 无法打开 (已尝试 MSMF, DShow, AUTO)"},
122
+ "添加摄像机 {} 失败: {}": {"en": "Failed to add camera {}: {}", "zh": "添加摄像机 {} 失败: {}"},
123
+ "摄像机 {} 读取连续失败 ({} 帧)": {"en": "Camera {}: read failed ({} consecutive)", "zh": "摄像机 {} 读取连续失败 ({} 帧)"},
124
+
125
+ # ── 对话框 ────────────────────────────────────────
126
+ "摄像机检测": {"en": "Camera Detection", "zh": "摄像机检测"},
127
+ "未找到摄像机。\n\n请检查:\n• 摄像机是否已连接且未被其他程序占用\n• 摄像机驱动是否已安装\n• 尝试用索引 0、1、2… 手动添加": {
128
+ "en": "No cameras found.\n\nCheck:\n• Camera is connected and not in use\n• Camera driver is installed\n• Try adding manually with index 0, 1, 2…",
129
+ "zh": "未找到摄像机。\n\n请检查:\n• 摄像机是否已连接且未被其他程序占用\n• 摄像机驱动是否已安装\n• 尝试用索引 0、1、2… 手动添加",
130
+ },
131
+ "警告": {"en": "Warning", "zh": "警告"},
132
+ "摄像机 {} 已存在。": {"en": "Camera {} already added.", "zh": "摄像机 {} 已存在。"},
133
+ "确认": {"en": "Confirm", "zh": "确认"},
134
+ "摄像机 {} 正在录制中,确定要中止并移除?": {"en": "Camera {} is recording. Abort and remove?", "zh": "摄像机 {} 正在录制中,确定要中止并移除?"},
135
+ "移除所有摄像机?": {"en": "Remove all cameras?", "zh": "移除所有摄像机?"},
136
+ "添加摄像机": {"en": "Add Camera", "zh": "添加摄像机"},
137
+ "摄像机索引:": {"en": "Camera index:", "zh": "摄像机索引:"},
138
+
139
+ # ── 面板标题 ──────────────────────────────────────
140
+ "日志": {"en": "Log", "zh": "日志"},
141
+ "录制历史": {"en": "Recording History", "zh": "录制历史"},
142
+ "摄像机": {"en": "Camera", "zh": "摄像机"},
143
+ "编码": {"en": "Codec", "zh": "编码"},
144
+ "文件": {"en": "File", "zh": "文件"},
145
+ "时长": {"en": "Duration", "zh": "时长"},
146
+ "大小": {"en": "Size", "zh": "大小"},
147
+ "状态": {"en": "Status", "zh": "状态"},
148
+ "已完成": {"en": "COMPLETED", "zh": "已完成"},
149
+ "已丢弃": {"en": "ABORTED", "zh": "已丢弃"},
150
+ "已上传": {"en": "Uploaded", "zh": "已上传"},
151
+ "已删除(未上传)": {"en": "DELETED (NOT UPLOADED)", "zh": "已删除(未上传)"},
152
+
153
+ # ── 关于对话框 ────────────────────────────────────
154
+ "关于 DAQ 视频管线": {"en": "About DAQ Video Pipeline", "zh": "关于 DAQ 视频管线"},
155
+ "实时摄像机预览": {"en": "Real-time camera preview", "zh": "实时摄像机预览"},
156
+ "可拖拽调整的画面布局": {"en": "Draggable-resizable camera layout", "zh": "可拖拽调整的画面布局"},
157
+ "支持正常完成和异常停止两种录制模式": {"en": "Normal finish and abort recording modes", "zh": "支持正常完成和异常停止两种录制模式"},
158
+ "录制历史记录追踪": {"en": "Recording history tracking", "zh": "录制历史记录追踪"},
159
+ "中英文界面切换": {"en": "Bilingual UI (Chinese / English)", "zh": "中英文界面切换"},
160
+
161
+ # ── 空状态提示 ────────────────────────────────────
162
+ '尚未检测到摄像机。\n请从左侧设备面板勾选要开启的设备。': {
163
+ "en": "No cameras detected.\nEnable devices from the device panel on the left.",
164
+ 'zh': '尚未检测到摄像机。\n请从左侧设备面板勾选要开启的设备。',
165
+ },
166
+
167
+ # ── 传感器面板 ────────────────────────────────────
168
+ "传感器阵列": {"en": "Sensor Array", "zh": "传感器阵列"},
169
+
170
+ # ── 设备检测面板 ──────────────────────────────────
171
+ "📷 设备检测": {"en": "📷 Device Detection", "zh": "📷 设备检测"},
172
+ "设备列表": {"en": "Device List", "zh": "设备列表"},
173
+ "点击列表中的设备以显示画面": {"en": "Click a device to show its feed", "zh": "点击列表中的设备以显示画面"},
174
+ "插拔自动刷新": {"en": "Auto-refresh on plug/unplug", "zh": "插拔自动刷新"},
175
+ "未检测到设备": {"en": "No devices detected", "zh": "未检测到设备"},
176
+ "已切换设备: {}": {"en": "Switched device: {}", "zh": "已切换设备: {}"},
177
+ "[设备] 已连接: {}": {"en": "[Device] Connected: {}", "zh": "[设备] 已连接: {}"},
178
+ "[设备] 已断开: {}": {"en": "[Device] Disconnected: {}", "zh": "[设备] 已断开: {}"},
179
+ "[设备] {} 已移除。": {"en": "[Device] {} removed.", "zh": "[设备] {} 已移除。"},
180
+ "录制中切换设备将中止当前录制,确定继续?": {
181
+ "en": "Switching device during recording will abort the current recording. Continue?",
182
+ "zh": "录制中切换设备将中止当前录制,确定继续?",
183
+ },
184
+ "USB 摄像头": {"en": "USB Camera", "zh": "USB 摄像头"},
185
+ "[设备] S80M 与 D435 同时连接,SDK 抢占 video0/video2,双目可能无法打开": {
186
+ "en": "[Device] S80M and D435 both connected; SDK takes video0/video2, stereo may fail to open",
187
+ "zh": "[设备] S80M 与 D435 同时连接,SDK 抢占 video0/video2,双目可能无法打开",
188
+ },
189
+ "[设备] 录制中拔线,不自动移除(重插自动恢复): {}": {
190
+ "en": "[Device] Unplugged during recording, not auto-removed (recovers on replug): {}",
191
+ "zh": "[设备] 录制中拔线,不自动移除(重插自动恢复): {}",
192
+ },
193
+
194
+ # ── 设备统一接入(分组 / 开关 / 命名) ──────────────
195
+ "📷 相机": {"en": "📷 Cameras", "zh": "📷 相机"},
196
+ "🧤 手套": {"en": "🧤 Gloves", "zh": "🧤 手套"},
197
+ "🎧 其他蓝牙": {"en": "🎧 Other Bluetooth", "zh": "🎧 其他蓝牙"},
198
+ "🎧 其他蓝牙: {} 台": {"en": "🎧 Other Bluetooth: {} device(s)", "zh": "🎧 其他蓝牙: {} 台"},
199
+ "蓝牙": {"en": "Bluetooth", "zh": "蓝牙"},
200
+ "开关设备以显示画面": {"en": "Toggle a device to show its feed", "zh": "开关设备以显示画面"},
201
+ "双击设备可重命名": {"en": "Double-click a device to rename", "zh": "双击设备可重命名"},
202
+ "重命名设备": {"en": "Rename Device", "zh": "重命名设备"},
203
+ "设备名称:": {"en": "Device name:", "zh": "设备名称:"},
204
+ "录制中不可更改设备": {"en": "Devices cannot be changed while recording", "zh": "录制中不可更改设备"},
205
+ "该设备无可视化数据": {"en": "No visualizable data for this device", "zh": "该设备无可视化数据"},
206
+ "S80M 与 D435 无法同时开启": {"en": "S80M and D435 cannot be enabled at the same time", "zh": "S80M 与 D435 无法同时开启"},
207
+ "S80M 与 D435 抢占视频节点(video0/video2),先开者生���": {
208
+ "en": "S80M and D435 conflict on video nodes (video0/video2); the first one opened wins",
209
+ "zh": "S80M 与 D435 抢占视频节点(video0/video2),先开者生效",
210
+ },
211
+ "[设备] 已开启: {}": {"en": "[Device] Enabled: {}", "zh": "[设备] 已开启: {}"},
212
+ "[设备] 已关闭: {}": {"en": "[Device] Disabled: {}", "zh": "[设备] 已关闭: {}"},
213
+ "[设备] 已重命名: {}": {"en": "[Device] Renamed: {}", "zh": "[设备] 已重命名: {}"},
214
+ "[设备] 开启失败: {}": {"en": "[Device] Failed to enable: {}", "zh": "[设备] 开启失败: {}"},
215
+
216
+ # ── 每设备曝光设置(信息条 ☀ 按钮) ────────────────
217
+ "曝光设置": {"en": "Exposure", "zh": "曝光设置"},
218
+ "自动曝光": {"en": "Auto exposure", "zh": "自动曝光"},
219
+ "拖动滑块即时生效": {"en": "Drag the slider to adjust live", "zh": "拖动滑块即时生效"},
220
+ "恢复默认": {"en": "Reset to default", "zh": "恢复默认"},
221
+ "回到相机最一开始的曝光设置": {"en": "Restore the camera's original exposure settings", "zh": "回到相机最一开始的曝光设置"},
222
+ "录制中不可调整曝光": {"en": "Exposure cannot be adjusted while recording", "zh": "录制中不可调整曝光"},
223
+ "[双目] 曝光下发失败(子进程已退出)": {
224
+ "en": "[Stereo] Exposure command failed (subprocess exited)",
225
+ "zh": "[双目] 曝光下发失败(子进程已退出)",
226
+ },
227
+
228
+ # ── 状态显示 ──────────────────────────────────────
229
+ "● 录制中 ({} 台摄像机)…": {"en": "● Recording ({} camera(s))…", "zh": "● 录制中 ({} 台摄像机)…"},
230
+ "DAQ 视频管线已关闭。": {"en": "DAQ Video Pipeline shut down.", "zh": "DAQ 视频管线已关闭。"},
231
+ "正在扫描摄像机…": {"en": "Scanning for cameras…", "zh": "正在扫描摄像机…"},
232
+ "正在扫描设备…": {"en": "Scanning for devices…", "zh": "正在扫描设备…"},
233
+ "⛔ 录制已异常终止,文件已丢弃。": {"en": "⛔ Recording aborted, files discarded.", "zh": "⛔ 录制已异常终止,文件已丢弃。"},
234
+
235
+ # ── 传感器面板 ────────────────────────────────────
236
+ "传感器阵列 (BLE)": {"en": "Sensor Array (BLE)", "zh": "传感器阵列 (BLE)"},
237
+ "传感器 Right": {"en": "Sensor Right", "zh": "传感器 Right"},
238
+ "传感器 Left": {"en": "Sensor Left", "zh": "传感器 Left"},
239
+ "传感器 {} ({})": {"en": "Sensor {} ({})", "zh": "传感器 {} ({})"},
240
+ "🔍 扫描设备": {"en": "🔍 Scan Device", "zh": "🔍 扫描设备"},
241
+ "选择 BLE 设备": {"en": "Select BLE Device", "zh": "选择 BLE 设备"},
242
+ "🔗 连接": {"en": "🔗 Connect", "zh": "🔗 连接"},
243
+ "未连接": {"en": "Not Connected", "zh": "未连接"},
244
+ "模式:": {"en": "Mode:", "zh": "模式:"},
245
+ "🔥 热力图 (Heatmap)": {"en": "🔥 Heatmap", "zh": "🔥 热力图 (Heatmap)"},
246
+ "📝 轨迹 (Trace)": {"en": "📝 Trace", "zh": "📝 轨迹 (Trace)"},
247
+ "📊 网格 (Grid)": {"en": "📊 Grid", "zh": "📊 网格 (Grid)"},
248
+ "🦾 仿生手掌 (Hand)": {"en": "🦾 Bionic Hand", "zh": "🦾 仿生手掌 (Hand)"},
249
+ "🕸 形变网格 (Deform)": {"en": "🕸 Deform Mesh", "zh": "🕸 形变网格 (Deform)"},
250
+ "🖐 3D 手掌 (3D Hand)": {"en": "🖐 3D Hand", "zh": "🖐 3D 手掌 (3D Hand)"},
251
+ "📐 校准": {"en": "📐 Calibrate", "zh": "📐 校准"},
252
+ "⚙ 配置": {"en": "⚙ Config", "zh": "⚙ 配置"},
253
+ "▸ 降噪参数": {"en": "▸ Denoise Params", "zh": "▸ 降噪参数"},
254
+ "噪声门:": {"en": "Noise Gate:", "zh": "噪声门:"},
255
+ "动态比率:": {"en": "Dynamic Ratio:", "zh": "动态比率:"},
256
+ "空间滤波": {"en": "Spatial Filter", "zh": "空间滤波"},
257
+ "正在扫描…": {"en": "Scanning…", "zh": "正在扫描…"},
258
+ "扫描中…": {"en": "Scanning…", "zh": "扫描中…"},
259
+ "未发现设备": {"en": "No devices found", "zh": "未发现设备"},
260
+ "发现 {} 台设备": {"en": "Found {} device(s)", "zh": "发现 {} 台设备"},
261
+ "断开": {"en": "Disconnect", "zh": "断开"},
262
+ "连接中…": {"en": "Connecting…", "zh": "连接中…"},
263
+ "已连接: {}…": {"en": "Connected: {}…", "zh": "已连接: {}…"},
264
+ "已断开": {"en": "Disconnected", "zh": "已断开"},
265
+ "校准完成": {"en": "Calibration complete", "zh": "校准完成"},
266
+ "校准中… {}%": {"en": "Calibrating… {}%", "zh": "校准中… {}%"},
267
+ "校准中…": {"en": "Calibrating…", "zh": "校准中…"},
268
+ "热力图/轨迹配置": {"en": "Heatmap / Trace Config", "zh": "热力图/轨迹配置"},
269
+ "网格配置": {"en": "Grid Config", "zh": "网格配置"},
270
+ "形变网格配置": {"en": "Deform Mesh Config", "zh": "形变网格配置"},
271
+ "[Sensor] {}": {"en": "[Sensor] {}", "zh": "[Sensor] {}"},
272
+
273
+ # ── 回放对话框 ────────────────────────────────────
274
+ "录制回放": {"en": "Recording Playback", "zh": "录制回放"},
275
+ "录制目录:": {"en": "Recording Directory:", "zh": "录制目录:"},
276
+ "刷新": {"en": "Refresh", "zh": "刷新"},
277
+ "录制会话:": {"en": "Recording Sessions:", "zh": "录制会话:"},
278
+ "传感器数据": {"en": "Sensor Data", "zh": "传感器数据"},
279
+ "显示模式:": {"en": "Display Mode:", "zh": "显示模式:"},
280
+ "选择录制目录": {"en": "Select Recording Directory", "zh": "选择录制目录"},
281
+ "提示": {"en": "Notice", "zh": "提示"},
282
+ "请先勾选要删除的会话": {"en": "Please check sessions to delete first", "zh": "请先勾选要删除的会话"},
283
+ "确认删除": {"en": "Confirm Delete", "zh": "确认删除"},
284
+ "完成": {"en": "Done", "zh": "完成"},
285
+ "⬆ 选中已上传": {"en": "⬆ Select Uploaded", "zh": "⬆ 选中已上传"},
286
+ "仅勾选已上传的会话": {"en": "Select only uploaded sessions", "zh": "仅勾选已上传的会话"},
287
+ "⬇ 选中未上传": {"en": "⬇ Select Not Uploaded", "zh": "⬇ 选中未上传"},
288
+ "仅勾选未上传的会话": {"en": "Select only not-uploaded sessions", "zh": "仅勾选未上传的会话"},
289
+ "🗑 删除选中": {"en": "🗑 Delete Selected", "zh": "🗑 删除选中"},
290
+ "将选中的会话移入回收站": {"en": "Move selected sessions to recycle bin", "zh": "将选中的会话移入回收站"},
291
+ "▶ 播放": {"en": "▶ Play", "zh": "▶ 播放"},
292
+ "⏸ 暂停": {"en": "⏸ Pause", "zh": "⏸ 暂停"},
293
+ "没有找到视频文件": {"en": "No video files found", "zh": "没有找到视频文件"},
294
+ "路": {"en": "ch", "zh": "路"},
295
+ "加载失败: {}": {"en": "Load failed: {}", "zh": "加载失败: {}"},
296
+ "确定要将以下会话移入回收站?\n\n{}\n\n此操作可逆(可从回收站恢复)。": {
297
+ "en": "Move the following sessions to recycle bin?\n\n{}\n\nThis operation is reversible.",
298
+ "zh": "确定要将以下会话移入回收站?\n\n{}\n\n此操作可逆(可从回收站恢复)。",
299
+ },
300
+ "已删除 {}/{} 个会话(已移入回收站)。": {
301
+ "en": "Deleted {}/{} session(s) (moved to recycle bin).",
302
+ "zh": "已删除 {}/{} 个会话(已移入回收站)。",
303
+ },
304
+ "传感器 TS: {} Δ={}ms": {
305
+ "en": "Sensor TS: {} Δ={}ms",
306
+ "zh": "传感器 TS: {} Δ={}ms",
307
+ },
308
+
309
+ # ── 上传对话框 ────────────────────────────────────
310
+ "上传录制数据": {"en": "Upload Recording Data", "zh": "上传录制数据"},
311
+ "服务器配置": {"en": "Server Configuration", "zh": "服务器配置"},
312
+ "服务器地址:": {"en": "Server Address:", "zh": "服务器地址:"},
313
+ "测试连接": {"en": "Test Connection", "zh": "测试连接"},
314
+ "选择要上传的会话": {"en": "Select Sessions to Upload", "zh": "选择要上传的会话"},
315
+ "全选": {"en": "Select All", "zh": "全选"},
316
+ "⬆ 一键上传": {"en": "⬆ Upload", "zh": "⬆ 一键上传"},
317
+ "关闭": {"en": "Close", "zh": "关闭"},
318
+ "请先勾选要上传的会话": {"en": "Please select sessions to upload first", "zh": "请先勾选要上传的会话"},
319
+ "请输入服务器地址": {"en": "Please enter server address", "zh": "请输入服务器地址"},
320
+ "成功": {"en": "Success", "zh": "成功"},
321
+ "服务器连接正常 ✅": {"en": "Server connection OK ✅", "zh": "服务器连接正常 ✅"},
322
+ "失败": {"en": "Failed", "zh": "失败"},
323
+ "无法连接到服务器 ❌": {"en": "Cannot connect to server ❌", "zh": "无法连接到服务器 ❌"},
324
+ "已入队 {} 个任务,开始处理…": {"en": "Queued {} task(s), processing…", "zh": "已入队 {} 个任务,开始处理…"},
325
+ "全部完成": {"en": "All complete", "zh": "全部完成"},
326
+
327
+ # ── 上传开关(自动上传 / 上传后自动删除) ─────────────
328
+ "☁ 自动上传: {}": {"en": "☁ Auto upload: {}", "zh": "☁ 自动上传: {}"},
329
+ "录制完成后自动上传;关闭后需在 ☁ 上传对话框手动上传": {
330
+ "en": "Auto-upload after recording; when off, upload manually from the ☁ Upload dialog",
331
+ "zh": "录制完成后自动上传;关闭后需在 ☁ 上传对话框手动上传",
332
+ },
333
+ "☁ 自动上传未开启,请手动上传: {}": {"en": "☁ Auto upload is off, upload manually: {}", "zh": "☁ 自动上传未开启,请手动上传: {}"},
334
+ "🗑 上传后自动删除: {}": {"en": "🗑 Auto-delete after upload: {}", "zh": "🗑 上传后自动删除: {}"},
335
+ "开": {"en": "ON", "zh": "开"},
336
+ "关": {"en": "OFF", "zh": "关"},
337
+ "上传成功后自动删除该会话的本地文件": {
338
+ "en": "Delete local session files after a successful upload",
339
+ "zh": "上传成功后自动删除该会话的本地文件",
340
+ },
341
+ "☁ 上传完成,本地文件已删除: {}": {"en": "☁ Upload complete, local files deleted: {}", "zh": "☁ 上传完成,本地文件已删除: {}"},
342
+ "☁ 上传完成(本地保留): {}": {"en": "☁ Upload complete (kept locally): {}", "zh": "☁ 上传完成(本地保留): {}"},
343
+ "❌ 上传失败: {}": {"en": "❌ Upload failed: {}", "zh": "❌ 上传失败: {}"},
344
+ "[错误] 上传后自动删除失败: {}": {"en": "[Error] Auto-delete after upload failed: {}", "zh": "[错误] 上传后自动删除失败: {}"},
345
+ "已上传,本地已删": {"en": "Uploaded, deleted locally", "zh": "已上传,本地已删"},
346
+
347
+ # ── 数据查看器 ────────────────────────────────────
348
+ "📂 浏览录制目录": {"en": "📂 Browse Recordings", "zh": "📂 浏览录制目录"},
349
+ "🔄 刷新": {"en": "🔄 Refresh", "zh": "🔄 刷新"},
350
+ "📋 录制会话:": {"en": "📋 Recording Sessions:", "zh": "📋 录制会话:"},
351
+ "选择会话开始回放": {"en": "Select a session to start playback", "zh": "选择会话开始回放"},
352
+ "📹 视频": {"en": "📹 Video", "zh": "📹 视频"},
353
+ "📊 传感器": {"en": "📊 Sensor", "zh": "📊 传感器"},
354
+
355
+ # ── 任务选择页面 ────────────────────────────────────
356
+ "连接": {"en": "Connect", "zh": "连接"},
357
+ "用户名": {"en": "Username", "zh": "用户名"},
358
+ "密码": {"en": "Password", "zh": "密码"},
359
+ "登陆成功": {"en": "Login OK", "zh": "登陆成功"},
360
+ "登陆失败: {}": {"en": "Login failed: {}", "zh": "登陆失败: {}"},
361
+ "后端已连接": {"en": "Backend Connected", "zh": "后端已连接"},
362
+ "后端未连接": {"en": "Backend Disconnected", "zh": "后端未连接"},
363
+ "已切换服务器: {} (用户: {})": {"en": "Switched server: {} (user: {})", "zh": "已切换服务器: {} (用户: {})"},
364
+ "已切换服务器地址: {} (无认证)": {"en": "Switched server: {} (no auth)", "zh": "已切换服务器地址: {} (无认证)"},
365
+ "刷新中…": {"en": "Refreshing…", "zh": "刷新中…"},
366
+ "可用任务": {"en": "Available Tasks", "zh": "可用任务"},
367
+ "任务详情": {"en": "Task Details", "zh": "任务详情"},
368
+ "← 返回任务选择": {"en": "← Back to Task Selection", "zh": "← 返回任务选择"},
369
+ "← 请从左侧列表中选择一个任务": {"en": "← Select a task from the list", "zh": "← 请从左侧列表中选择一个任务"},
370
+ "→ 进入采集": {"en": "→ Start Collection", "zh": "→ 进入采集"},
371
+ "已选择: {}": {"en": "Selected: {}", "zh": "已选择: {}"},
372
+ "已选择任务: {}": {"en": "Task selected: {}", "zh": "已选择任务: {}"},
373
+ "描述:": {"en": "Description:", "zh": "描述:"},
374
+ "任务ID:": {"en": "Task ID:", "zh": "任务ID:"},
375
+ "创建时间:": {"en": "Created:", "zh": "创建时间:"},
376
+ "状态:": {"en": "Status:", "zh": "状态:"},
377
+ "参数:": {"en": "Parameters:", "zh": "参数:"},
378
+ "返回任务选择": {"en": "Back to Task Selection", "zh": "返回任务选择"},
379
+ "待处理": {"en": "Pending", "zh": "待处理"},
380
+ "进行中": {"en": "In Progress", "zh": "进行中"},
381
+ "已取消": {"en": "Cancelled", "zh": "已取消"},
382
+ "未命名任务": {"en": "Unnamed Task", "zh": "未命名任务"},
383
+ "任务名称": {"en": "Task Name", "zh": "任务名称"},
384
+ "进度": {"en": "Progress", "zh": "进度"},
385
+ "发布时间": {"en": "Published", "zh": "发布时间"},
386
+ "暂无可用任务\n请检查后端连接或点击刷新": {
387
+ "en": "No tasks available\nCheck backend connection or click refresh",
388
+ "zh": "暂无可用任务\n请检查后端连接或点击刷新",
389
+ },
390
+ "录制中,确定要返回任务列表?当前录制将被终止。": {
391
+ "en": "Recording in progress. Return to task list? Current recording will be aborted.",
392
+ "zh": "录制中,确定要返回任务列表?当前录制将被终止。",
393
+ },
394
+ "已返回任务选择页面。": {"en": "Returned to task selection page.", "zh": "已返回任务选择页面。"},
395
+ "该任务采集进度已满": {"en": "Task progress is full", "zh": "该任务采集进度已满"},
396
+ "该任务采集进度已满,无法进入": {"en": "Task progress is full, cannot enter", "zh": "该任务采集进度已满,无法进入"},
397
+ "该任务采集进度已满({}/{}),无法开始新的采集。": {
398
+ "en": "Task progress is full ({}/{}), cannot start new collection.",
399
+ "zh": "该任务采集进度已满({}/{}),无法开始新的采集。",
400
+ },
401
+ "☁ 已自动加入上传队列: {}": {"en": "☁ Auto-queued for upload: {}", "zh": "☁ 已自动加入上传队列: {}"},
402
+ "☁ 上传完成: {}": {"en": "☁ Upload complete: {}", "zh": "☁ 上传完成: {}"},
403
+ "第 {}/{} 条": {"en": "#{}/{}", "zh": "第 {}/{} 条"},
404
+ "✅ 第 {}/{} 条上传完成: {}": {"en": "✅ #{}/{} uploaded: {}", "zh": "✅ 第 {}/{} 条上传完成: {}"},
405
+ "✅ 上传完成: {}": {"en": "✅ Uploaded: {}", "zh": "✅ 上传完成: {}"},
406
+ "[上传失败] {}: {}": {"en": "[Upload failed] {}: {}", "zh": "[上传失败] {}: {}"},
407
+ "发布时间:": {"en": "Published:", "zh": "发布时间:"},
408
+ "请在任务选择页面选择一个任务。": {"en": "Please select a task on the task selection page.", "zh": "请在任务选择页面选择一个任务。"},
409
+ "任务进度已更新: {} ({}/{})": {"en": "Task progress updated: {} ({}/{})", "zh": "任务进度已更新: {} ({}/{})"},
410
+ "提示": {"en": "Notice", "zh": "提示"},
411
+ "🗑 删除": {"en": "🗑 Delete", "zh": "🗑 删除"},
412
+ "确定要删除任务 \"{}\" 吗?\n删除后可在 tasks.json 中恢复。": {
413
+ "en": "Delete task \"{}\"?\nIt can be restored in tasks.json.",
414
+ "zh": "确定要删除任务 \"{}\" 吗?\n删除后可在 tasks.json 中恢复。",
415
+ },
416
+ "🗑 删除任务": {"en": "🗑 Delete Task", "zh": "🗑 删除任务"},
417
+ "待采集": {"en": "Pending", "zh": "待采集"},
418
+ "采集完成": {"en": "Completed", "zh": "采集完成"},
419
+ "采集中": {"en": "In Progress", "zh": "采集中"},
420
+ "该任务已采集完成,请更换其他任务": {"en": "Task collection complete, please switch to another task.", "zh": "该任务已采集完成,请更换其他任务"},
421
+
422
+ # ── 登录对话框 / 身份 ───────────────────────────────
423
+ "用户登录": {"en": "User Login", "zh": "用户登录"},
424
+ "登录": {"en": "Login", "zh": "登录"},
425
+ "记住账号": {"en": "Remember account", "zh": "记住账号"},
426
+ "游客登录": {"en": "Guest Login", "zh": "游客登录"},
427
+ "登录中…": {"en": "Logging in…", "zh": "登录中…"},
428
+ "请输入用户名": {"en": "Please enter username", "zh": "请输入用户名"},
429
+ "输入后端派发的账号登录,或点「游客登录」直接进入(仅可见公共任务)。": {
430
+ "en": "Sign in with the account issued by the backend, or click \"Guest Login\" to continue (public tasks only).",
431
+ "zh": "输入后端派发的账号登录,或点「游客登录」直接进入(仅可见公共任务)。",
432
+ },
433
+ "当前身份:": {"en": "Current identity:", "zh": "当前身份:"},
434
+ "切换账号": {"en": "Switch account", "zh": "切换账号"},
435
+ "游客": {"en": "Guest", "zh": "游客"},
436
+ "已登录: {} ({})": {"en": "Logged in: {} ({})", "zh": "已登录: {} ({})"},
437
+ "已进入游客模式(仅可见公共任务)。": {"en": "Entered guest mode (public tasks only).", "zh": "已进入游客模式(仅可见公共任务)。"},
438
+ "登录已过期,请重新登录。": {"en": "Login expired, please sign in again.", "zh": "登录已过期,请重新登录。"},
439
+
440
+ # ── 使用说明窗口 ────────────────────────────────
441
+ "使用说明": {"en": "Usage Guide", "zh": "使用说明"},
442
+ "使用步骤": {"en": "Usage Steps", "zh": "使用步骤"},
443
+ "下次启动不再自动显示(可随时从 帮助→使用说明 重新打开并取消勾选)": {
444
+ "en": "Don't show automatically at startup (reopen anytime via Help → Usage Guide to undo)",
445
+ "zh": "下次启动不再自动显示(可随时从 帮助→使用说明 重新打开并取消勾选)",
446
+ },
447
+
448
+ # ── 手部关键点追踪 ────────────────────────────────
449
+ "🧤 手套追踪": {"en": "🧤 Glove Tracking", "zh": "🧤 手套追踪"},
450
+ "🖐 裸手追踪": {"en": "🖐 Bare Hand", "zh": "🖐 裸手追踪"},
451
+ "✋ 手部叠加": {"en": "✋ Hand Overlay", "zh": "✋ 手部叠加"},
452
+ "🔄 提取关键点": {"en": "🔄 Extract Keypoints", "zh": "🔄 提取关键点"},
453
+ "✋ 处理手部关键点": {"en": "✋ Process Hand Keypoints", "zh": "✋ 处理手部关键点"},
454
+ "对录制完成的视频后台提取手部关键点": {"en": "Extract hand keypoints from recorded video in background", "zh": "对录制完成的视频后台提取手部关键点"},
455
+ "后台处理当前视频,提取手部关键点": {"en": "Process current video in background, extract hand keypoints", "zh": "后台处理当前视频,提取手部关键点"},
456
+ "手部关键点处理正在进行中,请等待完成。": {"en": "Hand keypoint processing in progress, please wait.", "zh": "手部关键点处理正在进行中,请等待完成。"},
457
+ "该录制已有手部关键点数据,要重新处理吗?": {"en": "This recording already has hand keypoint data. Reprocess?", "zh": "该录制已有手部关键点数据,要重新处理吗?"},
458
+ "[手部关键点] 模块不可用(缺少依赖)。": {"en": "[Hand Kpts] Module unavailable (missing dependencies).", "zh": "[手部关键点] 模块不可用(缺少依赖)。"},
459
+ "[手部关键点] 没有找到可处理的录制。": {"en": "[Hand Kpts] No recordable session found.", "zh": "[手部关键点] 没有找到可处理的录制。"},
460
+ "[手部关键点] 会话目录不存在: {}": {"en": "[Hand Kpts] Session directory not found: {}", "zh": "[手部关键点] 会话目录不存在: {}"},
461
+ "[手部关键点] 开始处理: {}": {"en": "[Hand Kpts] Processing: {}", "zh": "[手部关键点] 开始处理: {}"},
462
+ "[手部关键点] {}": {"en": "[Hand Kpts] {}", "zh": "[手部关键点] {}"},
463
+ "✋ 处理手部关键点: {}/{} ({:.0f}%)": {"en": "✋ Processing hand keypoints: {}/{} ({:.0f}%)", "zh": "✋ 处理手部关键点: {}/{} ({:.0f}%)"},
464
+ "[手部关键点] ❌ 处理失败: {}": {"en": "[Hand Kpts] ❌ Processing failed: {}", "zh": "[手部关键点] ❌ 处理失败: {}"},
465
+ "[手部关键点] ✅ 处理完成: {}": {"en": "[Hand Kpts] ✅ Processing complete: {}", "zh": "[手部关键点] ✅ 处理完成: {}"},
466
+ "手部关键点": {"en": "Hand Keypoints", "zh": "手部关键点"},
467
+ "⏳ 处理中…": {"en": "⏳ Processing…", "zh": "⏳ 处理中…"},
468
+ "⏳ {:.0f}%": {"en": "⏳ {:.0f}%", "zh": "⏳ {:.0f}%"},
469
+ "手部关键点模块不可用。": {"en": "Hand keypoint module unavailable.", "zh": "手部关键点模块不可用。"},
470
+ "选择手部追踪模式:黑色手套 / 裸手": {"en": "Select hand tracking mode: Glove / Bare Hand", "zh": "选择手部追踪模式:黑色手套 / 裸手"},
471
+ "选择手部追踪模式": {"en": "Select hand tracking mode", "zh": "选择手部追踪模式"},
472
+ "📊 自动标注": {"en": "📊 Auto Label", "zh": "📊 自动标注"},
473
+ "基于手部关键点数据自动生成手势标签": {"en": "Auto-generate gesture labels from hand keypoint data", "zh": "基于手部关键点数据自动生成手势标签"},
474
+ "[自动标注] 模块不可用。": {"en": "[Auto Label] Module unavailable.", "zh": "[自动标注] 模块不可用。"},
475
+ "[自动标注] 没有找到可处理的录制。": {"en": "[Auto Label] No recordable session found.", "zh": "[自动标注] 没有找到可处理的录制。"},
476
+ "[自动标注] 会话目录不存在: {}": {"en": "[Auto Label] Session directory not found: {}", "zh": "[自动标注] 会话目录不存在: {}"},
477
+ "[自动标注] 请先提取手部关键点。": {"en": "[Auto Label] Please extract hand keypoints first.", "zh": "[自动标注] 请先提取手部关键点。"},
478
+ "[自动标注] 开始标注: {}": {"en": "[Auto Label] Labeling: {}", "zh": "[自动标注] 开始标注: {}"},
479
+ "📊 自动标注: {}/{} ({:.0f}%)": {"en": "📊 Auto labeling: {}/{} ({:.0f}%)", "zh": "📊 自动标注: {}/{} ({:.0f}%)"},
480
+ "[自动标注] ❌ 标注失败: {}": {"en": "[Auto Label] ❌ Labeling failed: {}", "zh": "[自动标注] ❌ 标注失败: {}"},
481
+ "[自动标注] ✅ 标注完成: {}": {"en": "[Auto Label] ✅ Labeling complete: {}", "zh": "[自动标注] ✅ 标注完成: {}"},
482
+ "⏳ 标注中…": {"en": "⏳ Labeling…", "zh": "⏳ 标注中…"},
483
+ }
484
+
485
+
486
+ # ═══════════════════════════════════════════════════════
487
+ # 语言管理器(单例)
488
+ # ═══════════════════════════════════════════════════════
489
+
490
+ class LanguageManager(QObject):
491
+ """管理当前语言,切换时发射信号通知所有监听者刷新界面文字。"""
492
+
493
+ language_changed = pyqtSignal(str) # 参数: "zh" 或 "en"
494
+
495
+ def __init__(self):
496
+ super().__init__()
497
+ self._current = "en" # 默认英文
498
+
499
+ @property
500
+ def current(self) -> str:
501
+ return self._current
502
+
503
+ def set_language(self, lang: str):
504
+ """切换到指定语言并通知所有监听者。"""
505
+ if lang not in ("zh", "en"):
506
+ return
507
+ if lang == self._current:
508
+ return
509
+ self._current = lang
510
+ self.language_changed.emit(lang)
511
+
512
+ def toggle(self):
513
+ """在中英文之间切换。"""
514
+ self.set_language("en" if self._current == "zh" else "zh")
515
+
516
+
517
+ # 全局单例
518
+ lang_manager = LanguageManager()
519
+
520
+
521
+ def tr(text: str, *fmt_args) -> str:
522
+ """
523
+ 翻译函数:根据当前语言返回对应文本。
524
+
525
+ 用法:
526
+ tr("开始录制") → "开始录制" 或 "Start Recording"
527
+ tr("发现 {} 台摄像机。", 2) → "检测到 2 台摄像机。" 或 "Detected 2 camera(s)."
528
+ """
529
+ entry = _TRANSLATIONS.get(text)
530
+ if entry is None:
531
+ result = text
532
+ else:
533
+ result = entry.get(lang_manager.current, text)
534
+ if fmt_args:
535
+ result = result.format(*fmt_args)
536
+ return result
collector/config/s80m_stereo_calibration.json ADDED
@@ -0,0 +1,60 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ {
2
+ "type": "stereo_rgbd_camera",
3
+ "name": "head_stereo",
4
+ "resolution": [
5
+ 1280,
6
+ 800
7
+ ],
8
+ "fps": 25.0,
9
+ "baseline": 0.0797447770264594,
10
+ "left_camera": {
11
+ "intrinsic": [
12
+ 457.2199401855469,
13
+ 457.3329772949219,
14
+ 620.760986328125,
15
+ 340.13323974609375
16
+ ],
17
+ "distortion": [
18
+ 0.062210198491812,
19
+ 0.012444571591914,
20
+ -0.003091567661613,
21
+ -0.000578491541091
22
+ ],
23
+ "distortion_model": "equidistant"
24
+ },
25
+ "right_camera": {
26
+ "intrinsic": [
27
+ 457.0262451171875,
28
+ 457.2399597167969,
29
+ 631.2361450195312,
30
+ 403.0356750488281
31
+ ],
32
+ "distortion": [
33
+ 0.054715570062399,
34
+ 0.025506600737572,
35
+ -0.013235029764473,
36
+ 0.001610828214325
37
+ ],
38
+ "distortion_model": "equidistant"
39
+ },
40
+ "depth_scale": 0.001,
41
+ "cam_imu_timeshift": -0.001947268727235,
42
+ "rotation": [
43
+ 0.999654293060303,
44
+ -0.013781057670712,
45
+ 0.0223936047405,
46
+ 0.013997744768858,
47
+ 0.99985671043396,
48
+ -0.009548537433147,
49
+ -0.022258799523115,
50
+ 0.009858686476946,
51
+ 0.999703586101532
52
+ ],
53
+ "translation": [
54
+ -0.079738616943359,
55
+ -0.000860852771439,
56
+ 0.000491287559271
57
+ ],
58
+ "source": "fays_sdk_static_calib",
59
+ "captured_at": 1786618577.7006745
60
+ }
collector/config/sensors/hand_ble_config.json ADDED
@@ -0,0 +1,165 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ {
2
+ "thumb_tip": {
3
+ "name": "大拇指-指肚",
4
+ "rows": [],
5
+ "cols": [],
6
+ "axis_order": "col_row"
7
+ },
8
+ "thumb_joint": {
9
+ "name": "大拇指-关节",
10
+ "rows": [
11
+ 1,
12
+ 2,
13
+ 3
14
+ ],
15
+ "cols": [
16
+ 14,
17
+ 12,
18
+ 13,
19
+ 15
20
+ ],
21
+ "axis_order": "col_row"
22
+ },
23
+ "thumb_base": {
24
+ "name": "大拇指-指根",
25
+ "rows": [],
26
+ "cols": [],
27
+ "axis_order": "row_col"
28
+ },
29
+ "index_tip": {
30
+ "name": "食指-指肚",
31
+ "rows": [],
32
+ "cols": [],
33
+ "axis_order": "col_row"
34
+ },
35
+ "index_joint": {
36
+ "name": "食指-关节",
37
+ "rows": [
38
+ 4,
39
+ 5,
40
+ 6
41
+ ],
42
+ "cols": [
43
+ 14,
44
+ 12,
45
+ 13,
46
+ 15
47
+ ],
48
+ "axis_order": "col_row"
49
+ },
50
+ "index_base": {
51
+ "name": "食指-指根",
52
+ "rows": [],
53
+ "cols": [],
54
+ "axis_order": "row_col"
55
+ },
56
+ "middle_tip": {
57
+ "name": "中指-指肚",
58
+ "rows": [],
59
+ "cols": [],
60
+ "axis_order": "col_row"
61
+ },
62
+ "middle_joint": {
63
+ "name": "中指-关节",
64
+ "rows": [
65
+ 7,
66
+ 8,
67
+ 9
68
+ ],
69
+ "cols": [
70
+ 14,
71
+ 12,
72
+ 13,
73
+ 15
74
+ ],
75
+ "axis_order": "col_row"
76
+ },
77
+ "middle_base": {
78
+ "name": "中指-指根",
79
+ "rows": [],
80
+ "cols": [],
81
+ "axis_order": "row_col"
82
+ },
83
+ "ring_tip": {
84
+ "name": "无名指-指肚",
85
+ "rows": [],
86
+ "cols": [],
87
+ "axis_order": "col_row"
88
+ },
89
+ "ring_joint": {
90
+ "name": "无名指-关节",
91
+ "rows": [
92
+ 10,
93
+ 11,
94
+ 12
95
+ ],
96
+ "cols": [
97
+ 14,
98
+ 12,
99
+ 13,
100
+ 15
101
+ ],
102
+ "axis_order": "col_row"
103
+ },
104
+ "ring_base": {
105
+ "name": "无名指-指根",
106
+ "rows": [],
107
+ "cols": [],
108
+ "axis_order": "row_col"
109
+ },
110
+ "pinky_tip": {
111
+ "name": "小拇指-指肚",
112
+ "rows": [],
113
+ "cols": [],
114
+ "axis_order": "col_row"
115
+ },
116
+ "pinky_joint": {
117
+ "name": "小拇指-关节",
118
+ "rows": [
119
+ 13,
120
+ 14,
121
+ 15
122
+ ],
123
+ "cols": [
124
+ 14,
125
+ 12,
126
+ 13,
127
+ 15
128
+ ],
129
+ "axis_order": "col_row"
130
+ },
131
+ "pinky_base": {
132
+ "name": "小拇指-指根",
133
+ "rows": [],
134
+ "cols": [],
135
+ "axis_order": "row_col"
136
+ },
137
+ "palm": {
138
+ "name": "手掌心",
139
+ "rows": [
140
+ 1,
141
+ 2,
142
+ 3,
143
+ 4,
144
+ 5,
145
+ 6,
146
+ 7,
147
+ 8,
148
+ 9,
149
+ 10,
150
+ 11,
151
+ 12,
152
+ 13,
153
+ 14,
154
+ 15
155
+ ],
156
+ "cols": [
157
+ 10,
158
+ 9,
159
+ 8,
160
+ 6,
161
+ 4
162
+ ],
163
+ "axis_order": "col_row"
164
+ }
165
+ }
collector/config/sensors/hand_ble_config_left.json ADDED
@@ -0,0 +1,164 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ {
2
+ "thumb_tip": {
3
+ "name": "大拇指-指肚",
4
+ "rows": [],
5
+ "cols": [],
6
+ "axis_order": "col_row"
7
+ },
8
+ "thumb_joint": {
9
+ "name": "大拇指-关节",
10
+ "rows": [
11
+ 15,
12
+ 14,
13
+ 13
14
+ ],
15
+ "cols": [
16
+ 14,
17
+ 12,
18
+ 13,
19
+ 15
20
+ ],
21
+ "axis_order": "col_row"
22
+ },
23
+ "thumb_base": {
24
+ "name": "大拇指-指根",
25
+ "rows": [],
26
+ "cols": [],
27
+ "axis_order": "row_col"
28
+ },
29
+ "index_tip": {
30
+ "name": "食指-指肚",
31
+ "rows": [],
32
+ "cols": [],
33
+ "axis_order": "col_row"
34
+ },
35
+ "index_joint": {
36
+ "name": "食指-关节",
37
+ "rows": [
38
+ 12,
39
+ 11,
40
+ 10
41
+ ],
42
+ "cols": [
43
+ 14,
44
+ 12,
45
+ 13,
46
+ 15
47
+ ],
48
+ "axis_order": "col_row"
49
+ },
50
+ "index_base": {
51
+ "name": "食指-指根",
52
+ "rows": [],
53
+ "cols": [],
54
+ "axis_order": "row_col"
55
+ },
56
+ "middle_tip": {
57
+ "name": "中指-指肚",
58
+ "rows": [],
59
+ "cols": [],
60
+ "axis_order": "col_row"
61
+ },
62
+ "middle_joint": {
63
+ "name": "中指-关节",
64
+ "rows": [
65
+ 9,
66
+ 8,
67
+ 7
68
+ ],
69
+ "cols": [
70
+ 14,
71
+ 12,
72
+ 13,
73
+ 15
74
+ ],
75
+ "axis_order": "col_row"
76
+ },
77
+ "middle_base": {
78
+ "name": "中指-指根",
79
+ "rows": [],
80
+ "cols": [],
81
+ "axis_order": "row_col"
82
+ },
83
+ "ring_tip": {
84
+ "name": "无名指-指肚",
85
+ "rows": [],
86
+ "cols": [],
87
+ "axis_order": "col_row"
88
+ },
89
+ "ring_joint": {
90
+ "name": "无名指-关节",
91
+ "rows": [
92
+ 6,
93
+ 5,
94
+ 4
95
+ ],
96
+ "cols": [
97
+ 14,
98
+ 12,
99
+ 13,
100
+ 15
101
+ ],
102
+ "axis_order": "col_row"
103
+ },
104
+ "ring_base": {
105
+ "name": "无名指-指根",
106
+ "rows": [],
107
+ "cols": [],
108
+ "axis_order": "row_col"
109
+ },
110
+ "pinky_tip": {
111
+ "name": "小拇指-指肚",
112
+ "rows": [],
113
+ "cols": [],
114
+ "axis_order": "col_row"
115
+ },
116
+ "pinky_joint": {
117
+ "name": "小拇指-关节",
118
+ "rows": [
119
+ 3,
120
+ 2,
121
+ 1
122
+ ],
123
+ "cols": [
124
+ 14,
125
+ 12,
126
+ 13,
127
+ 15
128
+ ],
129
+ "axis_order": "col_row"
130
+ },
131
+ "pinky_base": {
132
+ "name": "小拇指-指根",
133
+ "rows": [],
134
+ "cols": [],
135
+ "axis_order": "row_col"
136
+ },
137
+ "palm": {
138
+ "name": "手掌心",
139
+ "rows": [
140
+ 15,
141
+ 14,
142
+ 13,
143
+ 12,
144
+ 11,
145
+ 10,
146
+ 9,
147
+ 8,
148
+ 7,
149
+ 6,
150
+ 5,
151
+ 4,
152
+ 3,
153
+ 2,
154
+ 1
155
+ ],
156
+ "cols": [
157
+ 10,
158
+ 9,
159
+ 8,
160
+ 6
161
+ ],
162
+ "axis_order": "col_row"
163
+ }
164
+ }
collector/config/settings.py ADDED
@@ -0,0 +1,478 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 应用程序全局配置 —— 路径、颜色、相机参数等。
3
+ """
4
+
5
+ import os
6
+ import sys
7
+
8
+ from config import __version__ as APP_VERSION # 版本号唯一定义在 config/__init__.py
9
+
10
+ # ── 路径 ──────────────────────────────────────────────
11
+ BASE_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
12
+ DATA_DIR = os.path.join(BASE_DIR, "data")
13
+ RECORDINGS_DIR = os.path.join(DATA_DIR, "recordings")
14
+ DB_PATH = os.path.join(DATA_DIR, "pipeline.db")
15
+ KEYPOINTS_OUTPUT_DIR = os.path.join(BASE_DIR, "keypoints_output")
16
+
17
+ # ── 应用程序 ──────────────────────────────────────────
18
+ APP_NAME = "EGO数据管线"
19
+ WINDOW_WIDTH = 1600
20
+ WINDOW_HEIGHT = 900
21
+
22
+ # ── 相机默认参数 ──────────────────────────────────────
23
+ DEFAULT_FPS = 30
24
+ if sys.platform == "win32":
25
+ # Windows:DECXIN 驱动对 1280×960「直连」只暴露 5fps 档(UVC 帧率表
26
+ # 怪癖,设 MJPG 也一样)。请求 1280×720 高会命中 30fps 档,实测驱动
27
+ # 协商回 1280×960@30(读回与真实帧尺寸仍为 960 高),画面分辨率不变。
28
+ DEFAULT_RESOLUTION = (1280, 720)
29
+ else:
30
+ DEFAULT_RESOLUTION = (1280, 960) # Linux:UVC 满分辨率(DECXIN);须配合 UVC_FOURCC=MJPG 才能 30fps
31
+ UVC_FOURCC = "MJPG" # UVC 采集像素格式:YUYV 在 1280×960 硬限 5fps
32
+ # 且驱动默认格式就是 YUYV,不显式设 MJPG 会静默掉帧率
33
+ CAMERA_RECONNECT_INTERVAL_MS = 2000 # 断线后重试间隔(毫秒)
34
+ MAX_CAMERAS = 8
35
+
36
+ # ── 录制参数 ──────────────────────────────────────────
37
+ RECORDING_FPS = 30 # 录制帧率
38
+ RECORDING_DIR = RECORDINGS_DIR # 录制文件输出目录
39
+ DEPTH_ENABLED = False # 遗留开关:旧 S80M 视差录制路径(已被 S80M_DEPTH_ENABLED
40
+ # 取代;仅剩 egodata_writer 守卫/测试/文档引用,勿再启用)
41
+
42
+ # ── 录制编码(v1.0.9:录制直出 HEVC,上传免预压)─────
43
+ RECORD_VIDEO_ENCODER = "auto" # "auto" | "nvenc" | "x265" | "x264"
44
+ # auto: hevc_nvenc(可用) → libx265(速度达标) → libx264
45
+ # 显式指定: 跳过速度门槛强制用该编码器;所有候选
46
+ # 二进制都不支持时按 auto 链逐级回退(警告日志)
47
+ RECORD_VIDEO_CRF = 30 # HEVC 目标档(nvenc 用 -cq,x265 用 -crf)
48
+ RECORD_VIDEO_X264_CRF = 23 # x264 回退档(与 v1.0.8 现状一致)
49
+ ENCODER_PROBE_ENABLED = True # 编码器探针开关(离线测试/极低端机可关,关=直接 x264)
50
+ ENCODER_PROBE_FRAME_COUNT = 45 # x265 速度探针每流合成帧数
51
+ ENCODER_PROBE_MAX_STREAMS = 4 # x265 探针并行流数封顶
52
+ ENCODER_PROBE_TIMEOUT_S = 15 # 单个探针子进程超时(秒)
53
+ ENCODER_X265_MIN_FPS_RATIO = 1.5 # x265 采用门槛:实测 fps ≥ 录制帧率 × 该比值
54
+ IMU_PENDING_MAX_SAMPLES = 18000 # 双目 IMU 防丢缓冲上限(~1 分钟 @300Hz)
55
+ DROP_WARN_RATIO = 0.01 # 丢帧提示阈值:丢帧 > 总帧数×该比例 或
56
+ DROP_WARN_MIN_COUNT = 30 # 丢帧 > 该帧数 → 提示换编码器/降分辨率
57
+
58
+ # ── 设备命名规范 (EgoData 标准) ───────────────────────
59
+ # 命名约定: <位置>_<模态>
60
+ # 头戴左目: head_left_rgb
61
+ # 头戴右目: head_right_rgb
62
+ # 头戴深度: head_depth
63
+ # 触觉传感器: left_glove / right_glove
64
+ # 手部关键点: left_hand_pose / right_hand_pose
65
+ #
66
+ CAMERA_LEFT = "head_left_rgb"
67
+ CAMERA_RIGHT = "head_right_rgb"
68
+ CAMERA_DEPTH = "head_depth"
69
+
70
+ def _camera_slot_name(index: int) -> str:
71
+ """OpenCV 设备索引 → EgoData 槽位名。
72
+
73
+ index 0 → head_left_rgb(主相机 / 单目左目)
74
+ index 1 → head_right_rgb(双目右目)
75
+ index 2+ → head_right_rgb_2, head_right_rgb_3, …
76
+ """
77
+ if index == 0:
78
+ return CAMERA_LEFT
79
+ if index == 1:
80
+ return CAMERA_RIGHT
81
+ return f"{CAMERA_RIGHT}_{index}"
82
+
83
+ # 兼容旧命名(供读取旧 LeRobot v3 会话使用)
84
+ CAMERA_PRIMARY_LEGACY = "head_rgb"
85
+ def _camera_slot_name_legacy(index: int) -> str:
86
+ if index == 0:
87
+ return CAMERA_PRIMARY_LEGACY
88
+ return f"{CAMERA_PRIMARY_LEGACY}_{index}"
89
+
90
+ # ── EgoData 输出格式 ─────────────────────────────────
91
+ EPISODE_PREFIX = "episode"
92
+ EPISODE_DIGITS = 6 # episode_000001, episode_000002, …
93
+ DEPTH_FORMAT = "depth_mkv" # 双流 MKV:流0=热力图 h264(默认播放画面),
94
+ # 流1=FFV1 gray16le 无损 uint16 毫米深度。
95
+ # 旧版 png16(PNG 序列)与 raw16 bin 均已弃用——
96
+ # 后者实测体积 6× 且上传被拒收(v1.0.11 回退),
97
+ # 前者 2026-09 被双流 MKV 取代(体积减半+彩色画面)
98
+ DEPTH_SCALE = 0.001 # 深度单位: 像素值 × DEPTH_SCALE = 米
99
+
100
+ # ── 双目标定默认值 ───────────────────────────────────
101
+ STEREO_BASELINE = 0.095 # 基线 (米)
102
+ STEREO_RESOLUTION = (1280, 800) # 双目默认分辨率
103
+ STEREO_FPS = 30 # 双目默认帧率
104
+ STEREO_CAM_FPS = 50 # S80M/S80C 相机档(50=默认,50→30 桶抽帧才录到 30fps;8/31 撕裂根因已由回调取帧修复——官方 GUI 同款回调+深度组合实测无撕裂。25 档保留为回退:25fps 相机下 30 桶只填到 25 帧)
105
+ STEREO_RECORD_FPS = 30 # S80M 录制帧率(相机帧按 wall 时钟 1/30s 桶抽帧,hw 时钟跳变不空桶)
106
+ STEREO_RECORD_MIN_INTERVAL_S = 1.0 / STEREO_RECORD_FPS # 抽帧桶长(秒)
107
+ STEREO_CB_BRIDGE = True # S80M/S80C 取帧用官方 GUI 同款回调(装配完成才交付;shim 缺失/注册失败自动回退轮询)
108
+
109
+ # ── S80M/S80C 深度(SDK 深度引擎,子进程内计算) ─────────────
110
+ S80M_DEPTH_ENABLED = True # S80C 深度开关(关闭→子进程不加 --depth-sdk-dir,协议字节不变)
111
+ S80M_DEPTH_SLOT = "stereo_depth" # 深度槽位(depth/stereo_depth/…,双流 MKV)
112
+ S80M_DEPTH_NEAR_MM = 300 # 热力图固定色标下限 mm(引擎量程 300-10000,中位 ~1.7m)
113
+ S80M_DEPTH_FAR_MM = 3000 # 热力图固定色标上限 mm
114
+ S80M_DEPTH_SMOOTH_K = 3 # 热力图中值滤波核
115
+
116
+ # ── D435 深度双目配置 ───────────────────────────────────
117
+ D435_SLOT_RGB = "d435_rgb" # RGB 彩色槽位(录制为 videos/d435_rgb/…)
118
+ D435_SLOT_DEPTH = "d435_depth" # 深度槽位(depth/d435_depth/…,双流 MKV)
119
+ D435_RESOLUTION = (848, 480) # (width, height),Depth Z16 @30;左右红外仅内部启用
120
+ D435_RGB_RESOLUTION = (1280, 720) # (width, height),RGB 彩色流(D435 标准 720p 全视场)
121
+ D435_FPS = 30
122
+ D435_DEPTH_NEAR_MM = 300 # 实时显示深度范围过滤(毫米,参考 S80M 深度 demo 0.3m)
123
+ D435_DEPTH_FAR_MM = 4000 # 实时显示深度范围过滤(毫米,4.0m)
124
+ D435_PNG_COMPRESSION = 1 # 原始深度 PNG 压缩级(1 最快)
125
+ D435_STALL_TIMEOUT_S = 5 # 流停滞阈值(秒):连续无帧超过该值触发重连
126
+ # (双 RealSense 第二台开流偶发静默停滞,无此
127
+ # 看门狗会无限空转且无任何报错)
128
+ D435_LOW_FPS_WINDOW_S = 10 # 帧率看门狗窗口(秒):窗口内帧数低于期望
129
+ # fps 的 D435_LOW_FPS_FRACTION → 触发重连
130
+ # (半死状态——wait_for_frames 零星出帧不报错
131
+ # 但画面极卡,静默看门狗测不到)
132
+ D435_LOW_FPS_FRACTION = 0.5 # 帧率下限比例(0.5 = 期望帧率的一半)
133
+
134
+ # ── RealSense D400 系列多型号支持 ──────────────────────
135
+ # 同一采集模式(面板按序列号区分具体设备),但不同型号的深度原生
136
+ # 分辨率 / 适用距离不同,按型号给采集配置(resolution 为 (width, height))。
137
+ REALSENSE_PROFILES = {
138
+ "D405": {
139
+ "depth_resolution": (1280, 720), # D405 原生深度分辨率(短距近景相机)
140
+ "rgb_resolution": (1280, 720), # D405 RGB 最高 720p
141
+ "fps": 30,
142
+ "depth_near_mm": 100, # 显示/热力图固定色标(D405 理想工作距 0.07-0.5m)
143
+ "depth_far_mm": 1000,
144
+ "heatmap_smooth_k": 3, # 热力图 3×3 中值(单像素椒盐噪点)
145
+ "heatmap_temporal_alpha": 0.5, # 热力图时域 EMA 权重(帧间抖动;仅可视化)
146
+ },
147
+ }
148
+
149
+ def realsense_profile(model_name: str) -> dict:
150
+ """按设备型号名返回采集配置;未收录型号回落 D435 默认。"""
151
+ for key, prof in REALSENSE_PROFILES.items():
152
+ if key in (model_name or ""):
153
+ return prof
154
+ return {
155
+ "depth_resolution": D435_RESOLUTION,
156
+ "rgb_resolution": D435_RGB_RESOLUTION,
157
+ "fps": D435_FPS,
158
+ "depth_near_mm": D435_DEPTH_NEAR_MM,
159
+ "depth_far_mm": D435_DEPTH_FAR_MM,
160
+ "heatmap_smooth_k": 0,
161
+ "heatmap_temporal_alpha": 0.0,
162
+ }
163
+
164
+ # ── 设备检测面板 ─────────────────────────────────────
165
+ DEVICE_SCAN_MAX_INDEX = 16 # V4L2 sysfs 枚举上限(D435 占 0-5 时 webcam 常落在 6+)
166
+ DEVICE_POLL_INTERVAL_MS = 2000 # 设备列表轮询间隔(毫秒,sysfs 只读扫描)
167
+ REALSENSE_VID = "8086" # Intel vendor ID(排除 RealSense UVC 节点用)
168
+ REALSENSE_PID = "0b07" # D435 的 product ID;其它型号(如 D435i=0b3a)靠驱动 name 含 "RealSense" 兜底
169
+
170
+ # 传感器命名(写入 parquet observation.<name> 列)
171
+ SENSOR_NAMES = [
172
+ "right_glove", # 右手手套传感器(16×16 压力矩阵)
173
+ "left_glove", # 左手手套传感器
174
+ ]
175
+
176
+ # 手部关键点命名(每列 21 关节 × 3 坐标 xyz)
177
+ HAND_POSE_LEFT = "left_hand_pose" # observation.left_hand_pose
178
+ HAND_POSE_RIGHT = "right_hand_pose" # observation.right_hand_pose
179
+ HAND_POSE_DIM = 21 * 3 # 63 维 (x,y,z 关节坐标)
180
+
181
+ # 传感器数据维度(16×16 触觉矩阵展平)
182
+ SENSOR_DIM = 256
183
+
184
+ # ── 显示参数 ──────────────────────────────────────────
185
+ CAMERA_MIRROR_HORIZONTAL = True # 单目相机左右镜像翻转(显示 + 录制)
186
+ DISPLAY_FPS_LIMIT = 30 # GUI 显示帧率上限
187
+ FEED_MIN_WIDTH = 320
188
+ FEED_MIN_HEIGHT = 240
189
+
190
+ # ── 暗色主题颜色(Qt-Material dark_teal) ─────────────
191
+ COLOR_RECORDING = "#EF5350" # 红色 — 录制中
192
+ COLOR_STOPPED = "#66BB6A" # 绿色 — 空闲
193
+ COLOR_ABNORMAL = "#FFA726" # 橙色 — 异常 / 警告
194
+
195
+ # 主背景
196
+ COLOR_BG_MAIN = "#1E1E1E" # 窗口主背景
197
+ COLOR_BG_PANEL = "#252525" # 面板背景
198
+ COLOR_BG_WIDGET = "#2D2D2D" # 控件 / 容器背景
199
+ COLOR_BG_CANVAS = "#1A1A1A" # 画布 / 网格背景
200
+
201
+ # 文字
202
+ COLOR_TEXT_PRIMARY = "#E0E0E0" # 主文字
203
+ COLOR_TEXT_SECONDARY = "#9E9E9E" # 次要文字
204
+ COLOR_TEXT_HINT = "#616161" # 提示文字
205
+
206
+ # 边框与分割线
207
+ COLOR_BORDER = "#424242"
208
+ COLOR_BORDER_STRONG = "#616161"
209
+
210
+ # 按钮颜色
211
+ COLOR_BTN_START = "#43A047" # 开始录制按钮(绿)
212
+ COLOR_BTN_STOP = "#E53935" # 停止录制按钮(红)
213
+ COLOR_BTN_ABORT = "#FF8F00" # 异常停止按钮(橙)
214
+ COLOR_BTN_DEFAULT_BG = "#424242" # 普通按钮背景
215
+ COLOR_BTN_HOVER = "#555555" # 按钮悬停
216
+ COLOR_BTN_DISABLED_BG = "#333333" # 禁用按钮背景
217
+ COLOR_BTN_DISABLED_TEXT = "#616161" # 禁用按钮文字
218
+
219
+ # ── 服务器上传配置 ────────────────────────────────────
220
+ # 出厂默认占位地址;实际地址由用户在页面中填写并持久化到 server_config.json。
221
+ SERVER_URL = "http://127.0.0.1:8000"
222
+ SERVER_CONFIG_FILE = os.path.join(DATA_DIR, "server_config.json")
223
+
224
+ def _load_server_config() -> dict:
225
+ """读取 server_config.json,返回完整字典。"""
226
+ import json
227
+ try:
228
+ if os.path.isfile(SERVER_CONFIG_FILE):
229
+ with open(SERVER_CONFIG_FILE, "r", encoding="utf-8") as f:
230
+ return json.load(f)
231
+ except (json.JSONDecodeError, OSError):
232
+ pass
233
+ return {}
234
+
235
+ def _save_server_config(data: dict):
236
+ """持久化 server_config.json(合并写入,保留未传入的旧字段)。"""
237
+ import json
238
+ current = _load_server_config()
239
+ current.update(data)
240
+ os.makedirs(DATA_DIR, exist_ok=True)
241
+ with open(SERVER_CONFIG_FILE, "w", encoding="utf-8") as f:
242
+ json.dump(current, f, ensure_ascii=False, indent=2)
243
+
244
+ def load_server_url() -> str:
245
+ """读取用户保存的服务器地址,若无则返回出厂默认值。"""
246
+ data = _load_server_config()
247
+ url = data.get("server_url", "").strip()
248
+ return url if url else SERVER_URL
249
+
250
+ def save_server_url(url: str):
251
+ """持久化用户输入的服务器地址。"""
252
+ _save_server_config({"server_url": url.strip()})
253
+
254
+ def load_credentials() -> tuple[str, str]:
255
+ """读取保存的登陆凭据,返回 (username, password)。"""
256
+ data = _load_server_config()
257
+ return data.get("username", ""), data.get("password", "")
258
+
259
+ def save_credentials(username: str, password: str):
260
+ """持久化登陆凭据。"""
261
+ _save_server_config({"username": username.strip(), "password": password})
262
+
263
+ def load_remembered_username() -> str:
264
+ """读取登录对话框「记住账号」保存的用户名(server_config.json)。"""
265
+ return str(_load_server_config().get("remembered_username", "") or "").strip()
266
+
267
+ def save_remembered_username(username: str):
268
+ """持久化「记住账号」的用户名(空串 = 清除)。"""
269
+ _save_server_config({"remembered_username": (username or "").strip()})
270
+
271
+ def load_upload_auto_sync() -> bool:
272
+ """读取"录制完成后自动上传"开关(server_config.json,默认开=沿用历史行为)。"""
273
+ return bool(_load_server_config().get("upload_auto_sync", True))
274
+
275
+ def save_upload_auto_sync(on: bool):
276
+ """持久化"录制完成后自动上传"开关。"""
277
+ _save_server_config({"upload_auto_sync": bool(on)})
278
+
279
+ def load_upload_delete_after() -> bool:
280
+ """读取"上传成功后自动删除本地文件"开关(server_config.json,默认关)。"""
281
+ return bool(_load_server_config().get("upload_delete_after", False))
282
+
283
+ def save_upload_delete_after(on: bool):
284
+ """持久化"上传成功后自动删除本地文件"开关。"""
285
+ _save_server_config({"upload_delete_after": bool(on)})
286
+
287
+ def load_upload_project_id() -> str:
288
+ """读取上传目标项目 ID(server_config.json)。
289
+
290
+ 空字符串 = 不指定,由服务器按会话名自动匹配项目
291
+ (服务器上存在多个同名/近似项目时会返回 409 歧义错误)。
292
+ """
293
+ return str(_load_server_config().get("upload_project_id", "") or "").strip()
294
+
295
+ def save_upload_project_id(project_id: str):
296
+ """持久化上传目标项目 ID(空 = 恢复服务器自动匹配)。"""
297
+ _save_server_config({"upload_project_id": (project_id or "").strip()})
298
+ # ── 设备命名持久化 ────────────────────────────────────
299
+ # key = DeviceInfo.stable_key("uvc:{by-id前缀}" / "d435:{serial}" / "ble:{MAC}" 等)
300
+ # value = 用户命名字符串;data_ble 设备另带 "sensor" 字段绑定 parquet 列名
301
+ # ("right_glove"/"left_glove",按 MAC 持久化,左右手配置各不同)
302
+ DEVICE_NAMES_FILE = os.path.join(DATA_DIR, "device_names.json")
303
+
304
+ def load_device_names() -> dict:
305
+ """读取设备命名表,异常/不存在返回 {}。"""
306
+ try:
307
+ if os.path.isfile(DEVICE_NAMES_FILE):
308
+ import json
309
+ with open(DEVICE_NAMES_FILE, "r", encoding="utf-8") as f:
310
+ data = json.load(f)
311
+ return data if isinstance(data, dict) else {}
312
+ except (json.JSONDecodeError, OSError):
313
+ pass
314
+ return {}
315
+
316
+ def _write_device_names(data: dict):
317
+ import json
318
+ os.makedirs(DATA_DIR, exist_ok=True)
319
+ with open(DEVICE_NAMES_FILE, "w", encoding="utf-8") as f:
320
+ json.dump(data, f, ensure_ascii=False, indent=2)
321
+
322
+ def save_device_name(key: str, name: str, sensor: str = ""):
323
+ """持久化设备命名(merge-write,保留未传入条目)。
324
+
325
+ sensor: 仅 data_ble 用,绑定 parquet 列名("" 表示不动原值)。
326
+ """
327
+ import json
328
+ current = load_device_names()
329
+ # 条目统一规范为 {"name": str, "sensor"?: str};旧版纯字符串自动升级
330
+ entry = current.get(key, "")
331
+ if isinstance(entry, str):
332
+ entry = {"name": entry}
333
+ entry = dict(entry) if entry else {}
334
+ if name:
335
+ entry["name"] = name
336
+ if sensor:
337
+ entry["sensor"] = sensor
338
+ if not entry.get("name") and not entry.get("sensor"):
339
+ return
340
+ current[key] = entry
341
+ _write_device_names(current)
342
+
343
+ def device_name(key: str) -> str:
344
+ """读单个设备命名(兼容 {"name":…} 结构或纯字符串)。"""
345
+ entry = load_device_names().get(key, "")
346
+ return entry["name"] if isinstance(entry, dict) else (entry or "")
347
+
348
+ def device_sensor_role(key: str) -> str:
349
+ """读 data_ble 设备绑定的 parquet 列名(无则空串)。"""
350
+ entry = load_device_names().get(key, "")
351
+ return entry.get("sensor", "") if isinstance(entry, dict) else ""
352
+
353
+ def assign_glove_sensor_role(key: str, prefer: str = "") -> str:
354
+ """为首次连接的未知手套分配下一个空闲 parquet 列名并持久化。
355
+
356
+ 按 MAC 绑定(重连保持);prefer 为期望列名(广播名 'L' → left_glove、
357
+ 'R' → right_glove),空闲则优先占用,否则按 SENSOR_NAMES 顺序取
358
+ 下一个空余名。无空余名时兜底取最后一个。
359
+
360
+ USB (Type-C) 手套例外:prefer 来自工具包注册表(按序列号权威),
361
+ 直接占用该列,并释放其它设备在该列上的陈旧绑定(避免右手套被
362
+ 历史 BLE 绑定挤到 left_glove)。
363
+ """
364
+ role = device_sensor_role(key)
365
+ if role:
366
+ return role
367
+ if prefer and key.startswith("usbglove:"):
368
+ # 硬件权威侧别:抢占对应列,清掉其它键的旧绑定
369
+ current = load_device_names()
370
+ for k, entry in list(current.items()):
371
+ if k != key and isinstance(entry, dict) \
372
+ and entry.get("sensor") == prefer:
373
+ entry["sensor"] = ""
374
+ _write_device_names(current)
375
+ save_device_name(key, device_name(key), sensor=prefer)
376
+ return prefer
377
+ used = {device_sensor_role(k) for k in load_device_names()}
378
+ candidates = [n for n in ([prefer] + SENSOR_NAMES) if n]
379
+ for name in dict.fromkeys(candidates): # 去重保持顺序
380
+ if name not in used:
381
+ save_device_name(key, device_name(key), sensor=name)
382
+ return name
383
+ return SENSOR_NAMES[-1] if SENSOR_NAMES else ""
384
+
385
+ def remove_device_name(key: str):
386
+ """删除设备命名条目。"""
387
+ current = load_device_names()
388
+ if key in current:
389
+ del current[key]
390
+ _write_device_names(current)
391
+
392
+ # ── 每设备相机参数持久化(曝光) ──────────────────────
393
+ # key 与 device_names.json 同为 DeviceInfo.stable_key;
394
+ # value = {"exposure": {"auto": bool, "value": float}}(auto=True 时 value 忽略)
395
+ DEVICE_PARAMS_FILE = os.path.join(DATA_DIR, "device_params.json")
396
+
397
+ def load_device_params() -> dict:
398
+ """读取每设备相机参数表,异常/不存在返回 {}。"""
399
+ try:
400
+ if os.path.isfile(DEVICE_PARAMS_FILE):
401
+ import json
402
+ with open(DEVICE_PARAMS_FILE, "r", encoding="utf-8") as f:
403
+ data = json.load(f)
404
+ return data if isinstance(data, dict) else {}
405
+ except (json.JSONDecodeError, OSError):
406
+ pass
407
+ return {}
408
+
409
+ def _write_device_params(data: dict):
410
+ import json
411
+ os.makedirs(DATA_DIR, exist_ok=True)
412
+ with open(DEVICE_PARAMS_FILE, "w", encoding="utf-8") as f:
413
+ json.dump(data, f, ensure_ascii=False, indent=2)
414
+
415
+ def save_device_exposure(key: str, auto: bool, value: float):
416
+ """持久化单设备曝光(merge-write,保留其他条目/参数)。"""
417
+ current = load_device_params()
418
+ entry = dict(current.get(key) or {})
419
+ entry["exposure"] = {"auto": bool(auto), "value": float(value)}
420
+ current[key] = entry
421
+ _write_device_params(current)
422
+
423
+ def device_exposure(key: str) -> dict:
424
+ """读单设备曝光设置 {"auto": bool, "value": float},无则 None。"""
425
+ entry = load_device_params().get(key) or {}
426
+ exp = entry.get("exposure")
427
+ if isinstance(exp, dict) and "auto" in exp and "value" in exp:
428
+ return {"auto": bool(exp["auto"]), "value": float(exp["value"])}
429
+ return None
430
+
431
+
432
+ def ensure_device_original(key: str, auto: bool, value: float) -> bool:
433
+ """记录设备「最一开始」的曝光基线(首次看到才写,之后永不覆盖)。
434
+
435
+ 恢复默认按钮的基线:无论之后怎么调整,都能回到首次开启设备时
436
+ 读回的原厂曝光状态。返回 True 表示本次为新写入。
437
+ """
438
+ current = load_device_params()
439
+ entry = dict(current.get(key) or {})
440
+ if isinstance(entry.get("original"), dict):
441
+ return False
442
+ entry["original"] = {"auto": bool(auto), "value": float(value)}
443
+ current[key] = entry
444
+ _write_device_params(current)
445
+ return True
446
+
447
+
448
+ def device_original(key: str) -> dict:
449
+ """读设备原始曝光基线 {"auto": bool, "value": float},无则 None。"""
450
+ entry = load_device_params().get(key) or {}
451
+ orig = entry.get("original")
452
+ if isinstance(orig, dict) and "auto" in orig and "value" in orig:
453
+ return {"auto": bool(orig["auto"]), "value": float(orig["value"])}
454
+ return None
455
+
456
+ UPLOAD_ENABLED = True # 是否启用自动上传
457
+ UPLOAD_MAX_CONCURRENT = 1 # 最大并发上传数:串行=1(并发时预压缩临时文件会互相覆盖致视频损坏,且进度无法区分当前是哪条数据)
458
+ UPLOAD_RETRY_MAX = 3 # 最大重试次数
459
+ UPLOAD_AUTO_SYNC = load_upload_auto_sync() # 录制完成后是否自动上传(持久化,启动时从 server_config.json 读取)
460
+ UPLOAD_DELETE_AFTER = load_upload_delete_after() # 上传成功后是否删除本地文件(持久化,启动时从 server_config.json 读取)
461
+ UPLOAD_PRECOMPRESS_VIDEO = True # 上传前是否把 videos 重编码到低码率。开=HEVC(libx265) CRF 档再压(tools/hevc_test 实测 CRF30 体积约原录制件的 7%、SSIM 0.967;大会话必备)。关=原样上传 CRF23 录制品(无二代有损;60min 双目 ~7.6GB,上传带宽/后端超时需能承受)
462
+ UPLOAD_VIDEO_CRF = 30 # HEVC 目标码率档(x265 CRF 越大越小;30 实测体积约 7%;仅 UPLOAD_PRECOMPRESS_VIDEO=True 时生效)
463
+
464
+ # ── 任务服务配置 ──────────────────────────────────────
465
+ TASK_POLL_INTERVAL_MS = 30000 # 任务轮询间隔(毫秒)
466
+ TASK_API_URL = SERVER_URL # 任务 API 地址(复用服务器地址)
467
+ DEVICE_NAME = "EGO_001" # 设备认领名(后端任务分配依据)
468
+
469
+ # ── 手部关键点追踪配置 ──────────────────────────────────
470
+ HAND_TRACK_ENABLED = False # 是否启用录制后手部关键点处理(需 ultralytics)
471
+ HAND_DETECTION_DIR = os.path.join(BASE_DIR, "tools", "hand_detection")
472
+ HAND_DET_MODEL = os.path.join(HAND_DETECTION_DIR, "best.pt") # YOLO 手套检测模型
473
+ HAND_MEDIAPIPE_MODEL = os.path.join(HAND_DETECTION_DIR, "hand_landmarker.task") # MediaPipe 裸手模型
474
+ HAND_TRACK_MODE = "glove" # 追踪模式: "glove"(黑色手套) / "bare"(裸手)
475
+ HAND_DET_DEVICE = "cuda" # 检测器设�� ("cuda" / "cpu")
476
+ HAND_POSE_DEVICE = "cuda" # 关键点设备 ("cuda" / "cpu")
477
+ HAND_TRACK_MAX_HANDS = 2 # 最多追踪手数
478
+ HAND_DATA_DIM = 21 * 2 * 2 + 4 * 2 + 1 # 展平后数据维度
collector/core/__init__.py ADDED
File without changes
collector/core/api_client.py ADDED
@@ -0,0 +1,249 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ HTTP API 客户端 —— 对接 Data Acquisition 服务器。
3
+
4
+ 真实 API:
5
+ POST /api/v1/session/upload — 上传整个 session 的 zip 包
6
+ GET /api/v1/sessions — 查询 sessions 列表
7
+ GET /api/v1/session/{id} — session 详情
8
+ DELETE /api/v1/session/{id} — 删除 session
9
+ GET /api/v1/video/{id}/{cam}/stream — 视频流
10
+ GET /health — 健康检查
11
+ """
12
+
13
+ from __future__ import annotations
14
+ import os
15
+ import time
16
+ from typing import Optional, Callable
17
+
18
+ import requests
19
+ import urllib3
20
+ from urllib3.util import Timeout as _Urllib3Timeout
21
+ from requests.adapters import HTTPAdapter
22
+
23
+
24
+ CONNECT_TIMEOUT = 10
25
+ READ_TIMEOUT = 1800 # 大会话(数 GB)上传 + 服务器解包入库可能耗时数分钟,设 30 分钟
26
+
27
+
28
+ class _PatientSendConnection(urllib3.connection.HTTPConnection):
29
+ """连接子类:请求体发送阶段用读超时窗口,而不是连接超时。
30
+
31
+ urllib3 的 connectionpool 在发请求体前把 conn.timeout 设为
32
+ connect_timeout(本工程 10s),request() 顶层随即用该值对 socket
33
+ 做 settimeout —— 即发送大请求体时 socket 超时实际是"连接超时"。
34
+ 服务器收到大 zip 后一边入库一边慢读(导入队列繁忙时单次 sendall
35
+ 可停顿数秒~数分钟),停顿一超 10s 上传就被误杀为
36
+ ConnectionError(('Connection aborted.', TimeoutError('timed out')))。
37
+ 这里把发送阶段的 socket 超时换成读超时窗口,发送完即恢复原值。
38
+ """
39
+
40
+ def request(self, method, url, body=None, headers=None, *args, **kwargs):
41
+ saved = self.timeout
42
+ # conn.timeout 此时是 urlopen 写入的 connect_timeout(数值)。
43
+ # 非数值(如 Timeout 对象)说明不是这个路径,交给基类处理。
44
+ if saved is not None and not isinstance(saved, _Urllib3Timeout):
45
+ self.timeout = READ_TIMEOUT
46
+ try:
47
+ return super().request(method, url, body=body, headers=headers,
48
+ *args, **kwargs)
49
+ finally:
50
+ self.timeout = saved
51
+
52
+
53
+ class _PatientSendHTTPSConnection(_PatientSendConnection,
54
+ urllib3.connection.HTTPSConnection):
55
+ pass
56
+
57
+
58
+ class _PatientSendHTTPConnectionPool(urllib3.HTTPConnectionPool):
59
+ ConnectionCls = _PatientSendConnection
60
+
61
+
62
+ class _PatientSendHTTPSConnectionPool(urllib3.HTTPSConnectionPool):
63
+ ConnectionCls = _PatientSendHTTPSConnection
64
+
65
+
66
+ class _PatientSendAdapter(HTTPAdapter):
67
+ """把发送超时加长的连接池装入 session。"""
68
+
69
+ def init_poolmanager(self, connections, maxsize, block=False, **pool_kwargs):
70
+ super().init_poolmanager(connections, maxsize, block=block, **pool_kwargs)
71
+ # PoolManager 默认指向模块级 dict(多实例共享),须整体替换实例属性
72
+ self.poolmanager.pool_classes_by_scheme = {
73
+ "http": _PatientSendHTTPConnectionPool,
74
+ "https": _PatientSendHTTPSConnectionPool,
75
+ }
76
+
77
+
78
+ class APIClient:
79
+ """服务器 REST API 客户端。
80
+
81
+ session: 可选复用已有 requests.Session(如 TaskService 的已认证 session)。
82
+ """
83
+
84
+ def __init__(self, base_url: str, session: requests.Session = None):
85
+ self.base_url = base_url.rstrip("/")
86
+ self._own_session = session is None
87
+ self._session = session if session is not None else requests.Session()
88
+ self._session.headers.update({"User-Agent": "DAQ-SDK/1.0"})
89
+ # 大请求体上传时发送阶段的 socket 超时改为读超时窗口,
90
+ # 否则服务器慢读停顿 >10s 会误杀上传(见 _PatientSendConnection)。
91
+ self._session.mount("http://", _PatientSendAdapter())
92
+ self._session.mount("https://", _PatientSendAdapter())
93
+
94
+ def close(self):
95
+ if self._own_session:
96
+ self._session.close()
97
+
98
+ # ── 健康检查 ──────────────────────────────────────
99
+
100
+ def health_check(self) -> bool:
101
+ try:
102
+ r = self._session.get(f"{self.base_url}/health",
103
+ timeout=CONNECT_TIMEOUT)
104
+ return r.status_code == 200
105
+ except requests.RequestException:
106
+ return False
107
+
108
+ # ── 上传 session ──────────────────────────────────
109
+
110
+ def upload_session_zip(self, zip_path: str, session_name: str,
111
+ progress_cb: Optional[Callable[[int, int], None]] = None,
112
+ name: str = "", project_id: str = "",
113
+ episode_index: int = 0
114
+ ) -> dict:
115
+ """
116
+ 上传一个 session 的 zip 包到服务器。
117
+
118
+ Args:
119
+ zip_path: zip 文件路径
120
+ session_name: 会话名称(v1.1.0 池化 = 任务名)
121
+ progress_cb: 进度回调 (uploaded_bytes, total_bytes)
122
+ name: 上传接口的 name 表单字段(会话名;服务器按它解析目标项目,
123
+ 空则回退 session_name)
124
+ project_id: 上传接口的 project_id 表单字段(目标项目 ID;
125
+ 空 = 服务器按名称自动匹配,服务器上有多个同名/近似项目时
126
+ 会返回 409 "Ambiguous project prefix")
127
+ episode_index: v1.1.0 池化表单字段——值为本 episode 的 file 号
128
+ (0 基,与 zip 内 file-NNN 完全一致;真实全局序号 N 在
129
+ parquet 行的 episode_index 列)。旧服务器忽略未知字段,
130
+ 不影响既有上传。
131
+
132
+ Returns:
133
+ {"ok": True, "session_id": "...", "response": {...}}
134
+ 或 {"ok": False, "error": "..."}
135
+ """
136
+ url = f"{self.base_url}/api/v1/session/upload"
137
+ file_size = os.path.getsize(zip_path)
138
+
139
+ # 包装文件对象,在读取时回调进度
140
+ class _ProgressReader:
141
+ def __init__(self, path, cb, total):
142
+ self._f = open(path, "rb")
143
+ self._cb = cb
144
+ self._total = total
145
+ self._read = 0
146
+
147
+ def read(self, size=-1):
148
+ data = self._f.read(size)
149
+ if data:
150
+ self._read += len(data)
151
+ if self._cb:
152
+ self._cb(self._read, self._total)
153
+ return data
154
+
155
+ def close(self):
156
+ self._f.close()
157
+
158
+ reader = _ProgressReader(zip_path, progress_cb, file_size)
159
+
160
+ try:
161
+ r = self._session.post(
162
+ url,
163
+ files={"file": (session_name + ".zip", reader, "application/zip")},
164
+ # 显式传 name/project_id:服务器按 name 解析目标项目,
165
+ # 多个同名/近似项目(含 workflow 别名撞名)时按前缀匹配
166
+ # 会 409 歧义,须用 project_id 消歧。
167
+ data={"name": name or session_name,
168
+ "project_id": project_id or "",
169
+ "episode_index": str(episode_index)},
170
+ timeout=(CONNECT_TIMEOUT, READ_TIMEOUT),
171
+ )
172
+ if r.status_code in (200, 201):
173
+ data = r.json()
174
+ return {"ok": True,
175
+ "session_id": data.get("session_id", ""),
176
+ "response": data}
177
+ if r.status_code == 409:
178
+ # 服务器按名称匹配项目时发现歧义(多项目前缀撞名)
179
+ try:
180
+ detail = r.json().get("detail", "")
181
+ except ValueError:
182
+ detail = r.text
183
+ if "project_id" in str(detail) or "prefix" in str(detail).lower():
184
+ return {"ok": False,
185
+ "ambiguous_project": True,
186
+ "error": "项目名有歧义:服务器上有多个同名/近似项目。"
187
+ "请在 ☁ 上传对话框的『目标项目』中指定项目后重试"}
188
+ return {"ok": False, "error": f"HTTP 409: {r.text[:300]}"}
189
+ return {"ok": False,
190
+ "error": f"HTTP {r.status_code}: {r.text[:300]}"}
191
+ except requests.RequestException as e:
192
+ return {"ok": False, "error": str(e)[:300]}
193
+ finally:
194
+ reader.close()
195
+
196
+ def get_projects(self, limit: int = 100) -> list[dict]:
197
+ """GET /api/v1/projects 项目列表(需已认证 session;失败/未认证返回 [])。
198
+
199
+ 用于上传对话框的「目标项目」下拉框。
200
+ """
201
+ try:
202
+ r = self._session.get(
203
+ f"{self.base_url}/api/v1/projects",
204
+ params={"limit": limit},
205
+ timeout=CONNECT_TIMEOUT,
206
+ )
207
+ if r.status_code == 200:
208
+ return r.json().get("projects", [])
209
+ except requests.RequestException:
210
+ pass
211
+ return []
212
+
213
+ # ── 查询 ──────────────────────────────────────────
214
+
215
+ def get_sessions(self, limit: int = 50) -> list[dict]:
216
+ try:
217
+ r = self._session.get(
218
+ f"{self.base_url}/api/v1/sessions",
219
+ params={"limit": limit},
220
+ timeout=CONNECT_TIMEOUT,
221
+ )
222
+ if r.status_code == 200:
223
+ data = r.json()
224
+ return data.get("sessions", [])
225
+ except requests.RequestException:
226
+ pass
227
+ return []
228
+
229
+ def get_session(self, session_id: str) -> Optional[dict]:
230
+ try:
231
+ r = self._session.get(
232
+ f"{self.base_url}/api/v1/session/{session_id}",
233
+ timeout=CONNECT_TIMEOUT,
234
+ )
235
+ if r.status_code == 200:
236
+ return r.json()
237
+ except requests.RequestException:
238
+ pass
239
+ return None
240
+
241
+ def delete_session(self, session_id: str) -> bool:
242
+ try:
243
+ r = self._session.delete(
244
+ f"{self.base_url}/api/v1/session/{session_id}",
245
+ timeout=CONNECT_TIMEOUT,
246
+ )
247
+ return r.status_code == 200
248
+ except requests.RequestException:
249
+ return False
collector/core/auto_labeler.py ADDED
@@ -0,0 +1,66 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 自动标注器 —— Qt 薄封装,核心逻辑在 core.hand_tracking。
3
+ """
4
+
5
+ from __future__ import annotations
6
+ import threading
7
+ from typing import Optional
8
+
9
+ from PyQt5.QtCore import QObject, pyqtSignal
10
+
11
+ from core.hand_tracking import label_session as _ht_label_session
12
+
13
+
14
+ class AutoLabeler(QObject):
15
+ """基于手部关键点数据的自动标注器。
16
+
17
+ 信号
18
+ ----
19
+ progress(int, int) — (已处理帧数, 总帧数)
20
+ finished(str, str) — (session_path, error_msg)
21
+ """
22
+
23
+ progress = pyqtSignal(int, int)
24
+ finished = pyqtSignal(str, str)
25
+
26
+ def __init__(self, parent=None):
27
+ super().__init__(parent)
28
+ self._running = False
29
+ self._thread: Optional[threading.Thread] = None
30
+
31
+ def label_session(self, session_path: str):
32
+ """启动后台标注一个录制会话。"""
33
+ if self._running:
34
+ return
35
+
36
+ self._running = True
37
+ self._thread = threading.Thread(
38
+ target=self._run, args=(session_path,),
39
+ daemon=True, name="auto-labeler",
40
+ )
41
+ self._thread.start()
42
+
43
+ def cancel(self):
44
+ self._running = False
45
+
46
+ def _run(self, session_path: str):
47
+ def _progress(cur, total):
48
+ self.progress.emit(cur, total)
49
+
50
+ def _cancelled():
51
+ return not self._running
52
+
53
+ result = _ht_label_session(
54
+ session_path=session_path,
55
+ progress_cb=_progress,
56
+ cancel_check=_cancelled,
57
+ )
58
+
59
+ if not self._running:
60
+ return
61
+ if result.get("success"):
62
+ self.finished.emit(session_path, "")
63
+ else:
64
+ self.finished.emit(session_path, result.get("error", "未知错误"))
65
+
66
+ self._running = False
collector/core/ble_engine.py ADDED
@@ -0,0 +1,303 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ BLE 传感器数据引擎 —— 蓝牙连接、数据接收、解析、降噪。
3
+
4
+ 作为 QObject 运行,在独立线程中处理 BLE 异步通信,
5
+ 通过 Qt 信号将处理后的数据帧发送到 UI 线程。
6
+ """
7
+
8
+ import asyncio
9
+ import sys
10
+ import time
11
+ import threading
12
+ import logging
13
+ import numpy as np
14
+ from PyQt5.QtCore import QObject, pyqtSignal, QMutex, QMutexLocker
15
+
16
+ if sys.platform == "win32":
17
+ asyncio.set_event_loop_policy(asyncio.WindowsSelectorEventLoopPolicy())
18
+
19
+ from bleak import BleakClient, BleakScanner
20
+
21
+ # ── 配置常量 ──────────────────────────────────────────
22
+ TX_CHAR_UUID = "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
23
+ MATRIX_ROWS = 16
24
+ MATRIX_COLS = 16
25
+ CALIBRATION_FRAMES = 100
26
+ TARGET_FPS = 100
27
+
28
+ logger = logging.getLogger("SensorBLE")
29
+
30
+
31
+ class SensorBLEEngine(QObject):
32
+ """
33
+ BLE 传感器数据采集引擎。
34
+
35
+ 信号
36
+ ----
37
+ device_found(str, str, int) — 设备名, 地址, RSSI
38
+ scan_complete(list) — 扫描完成,设备列表
39
+ connected(str) — 已连接,设备地址
40
+ disconnected() — 已断开
41
+ data_ready(ndarray) — 处理后的 16×16 数据帧
42
+ fps_updated(float) — 硬件 FPS
43
+ error_occurred(str) — 错误消息
44
+ calibration_progress(int) — 校准进度 (0-100)
45
+ """
46
+
47
+ device_found = pyqtSignal(str, str, int)
48
+ scan_complete = pyqtSignal(list)
49
+ connected = pyqtSignal(str)
50
+ disconnected = pyqtSignal()
51
+ data_ready = pyqtSignal(np.ndarray)
52
+ fps_updated = pyqtSignal(float)
53
+ error_occurred = pyqtSignal(str)
54
+ calibration_progress = pyqtSignal(int)
55
+
56
+ def __init__(self):
57
+ super().__init__()
58
+ self.data_array = np.zeros((MATRIX_ROWS, MATRIX_COLS), dtype=np.float32)
59
+ self.data_lock = threading.Lock()
60
+
61
+ self.raw_buffer = bytearray()
62
+ self.buffer_lock = threading.Lock()
63
+
64
+ self._running = False
65
+ self._scanning = False
66
+ self._mutex = QMutex()
67
+
68
+ # 降噪参数
69
+ self.base_noise_gate = 500
70
+ self.dynamic_noise_ratio = 0.0
71
+ self.temporal_smooth = 0.15
72
+ self.spatial_filter_enabled = True
73
+ self.history_buffer = None
74
+ self.drift_baseline_val = 0
75
+
76
+ # 校准
77
+ self.is_calibrating = True
78
+ self.calibration_buffer = []
79
+ self.baseline_map = np.zeros((MATRIX_ROWS, MATRIX_COLS), dtype=np.float32)
80
+
81
+ # FPS
82
+ self.frame_count = 0
83
+ self.last_fps_time = time.time()
84
+ self.hardware_fps = 0.0
85
+
86
+ # 最新传感器数据的采集时间戳(微秒),由解析线程写入,主线程读取
87
+ self.latest_data_ts_us: int = 0
88
+ self._ts_lock = threading.Lock()
89
+
90
+ # ── 公开接口 ──────────────────────────────────────
91
+
92
+ def start_scan(self):
93
+ """开始扫描 BLE 设备。"""
94
+ with QMutexLocker(self._mutex):
95
+ if self._scanning:
96
+ return
97
+ self._scanning = True
98
+ threading.Thread(target=self._scan_loop, daemon=True).start()
99
+
100
+ def connect_device(self, address: str):
101
+ """连接到指定地址的 BLE 设备。"""
102
+ with QMutexLocker(self._mutex):
103
+ self._running = True
104
+ self._target_mac = address
105
+ threading.Thread(target=self._ble_loop, daemon=True).start()
106
+ threading.Thread(target=self._parse_loop, daemon=True).start()
107
+
108
+ def disconnect(self):
109
+ """断开 BLE 连接。"""
110
+ with QMutexLocker(self._mutex):
111
+ self._running = False
112
+
113
+ def start_calibration(self):
114
+ """开始校准。"""
115
+ with self.data_lock:
116
+ self.is_calibrating = True
117
+ self.calibration_buffer = []
118
+
119
+ def set_noise_gate(self, value: int):
120
+ self.base_noise_gate = value
121
+
122
+ def set_dynamic_ratio(self, value: float):
123
+ self.dynamic_noise_ratio = value
124
+
125
+ def set_spatial_filter(self, enabled: bool):
126
+ self.spatial_filter_enabled = enabled
127
+
128
+ def set_temporal_smooth(self, value: float):
129
+ self.temporal_smooth = value
130
+
131
+ # ── BLE 扫描 ──────────────────────────────────────
132
+
133
+ def _scan_loop(self):
134
+ loop = asyncio.new_event_loop()
135
+ asyncio.set_event_loop(loop)
136
+ try:
137
+ devices = loop.run_until_complete(BleakScanner.discover(timeout=5.0))
138
+ devices.sort(
139
+ key=lambda d: ("Matrix" in (d.name or ""), bool((d.name or "").strip()),
140
+ getattr(d.details, "rssi", -999)),
141
+ reverse=True,
142
+ )
143
+ self.scan_complete.emit(devices)
144
+ except Exception as e:
145
+ self.error_occurred.emit(f"BLE 扫描失败: {e}")
146
+ finally:
147
+ loop.close()
148
+ with QMutexLocker(self._mutex):
149
+ self._scanning = False
150
+
151
+ # ── BLE 通信 ──────────────────────────────────────
152
+
153
+ def _ble_loop(self):
154
+ loop = asyncio.new_event_loop()
155
+ asyncio.set_event_loop(loop)
156
+ loop.run_until_complete(self._ble_main_async())
157
+
158
+ async def _ble_main_async(self):
159
+ while True:
160
+ with QMutexLocker(self._mutex):
161
+ if not self._running:
162
+ break
163
+ try:
164
+ async with BleakClient(self._target_mac) as client:
165
+ self.connected.emit(self._target_mac)
166
+ await client.start_notify(TX_CHAR_UUID, self._ble_notify_callback)
167
+ while client.is_connected:
168
+ with QMutexLocker(self._mutex):
169
+ if not self._running:
170
+ break
171
+ await asyncio.sleep(1)
172
+ except Exception as e:
173
+ # 连接/通知失败原因上抛 UI(否则界面停在"连接中"无从排查)
174
+ self.error_occurred.emit(f"连接失败: {e}")
175
+ self.disconnected.emit()
176
+ with QMutexLocker(self._mutex):
177
+ if not self._running:
178
+ break
179
+ await asyncio.sleep(3)
180
+
181
+ def _ble_notify_callback(self, sender, data):
182
+ with self.buffer_lock:
183
+ self.raw_buffer.extend(data)
184
+
185
+ # ── 数据解析 ──────────────────────────────────────
186
+
187
+ def _parse_loop(self):
188
+ frame_header = b"\xAA\x55"
189
+ frame_tail = b"\xFB\x03"
190
+ data_size = 512
191
+ frame_size = len(frame_header) + data_size + 1 + len(frame_tail)
192
+
193
+ while True:
194
+ with QMutexLocker(self._mutex):
195
+ if not self._running:
196
+ break
197
+
198
+ with self.buffer_lock:
199
+ buffer_len = len(self.raw_buffer)
200
+
201
+ if buffer_len < frame_size:
202
+ time.sleep(0.001)
203
+ continue
204
+
205
+ with self.buffer_lock:
206
+ h_idx = self.raw_buffer.find(frame_header)
207
+ if h_idx == -1:
208
+ del self.raw_buffer[:-1]
209
+ continue
210
+ if h_idx > 0:
211
+ del self.raw_buffer[:h_idx]
212
+ if len(self.raw_buffer) < frame_size:
213
+ continue
214
+
215
+ frame = self.raw_buffer[:frame_size]
216
+ if frame[-2:] == frame_tail:
217
+ d_bytes = frame[2 : 2 + data_size]
218
+ chksum = frame[2 + data_size]
219
+ calc_chk = 0
220
+ for b in d_bytes:
221
+ calc_chk ^= b
222
+ if calc_chk == chksum:
223
+ new_data = (
224
+ np.frombuffer(d_bytes, dtype=np.uint16)
225
+ .reshape((16, 16))
226
+ .astype(np.float32)
227
+ )
228
+ with self.data_lock:
229
+ self.data_array[:] = new_data
230
+ # 记录数据采集时间戳(微秒)
231
+ with self._ts_lock:
232
+ self.latest_data_ts_us = int(time.time() * 1_000_000)
233
+ self.frame_count += 1
234
+ del self.raw_buffer[:frame_size]
235
+ else:
236
+ del self.raw_buffer[:2]
237
+
238
+ curr_time = time.time()
239
+ if curr_time - self.last_fps_time >= 1.0:
240
+ self.hardware_fps = self.frame_count / (
241
+ curr_time - self.last_fps_time
242
+ )
243
+ self.fps_updated.emit(self.hardware_fps)
244
+ self.frame_count = 0
245
+ self.last_fps_time = curr_time
246
+
247
+ # ── 数据处理 ──────────────────────────────────────
248
+
249
+ def process_frame(self) -> tuple:
250
+ """
251
+ 处理一帧数据:校准、降噪、滤波。
252
+ 返回 (processed_data, max_signal)。
253
+ """
254
+ with self.data_lock:
255
+ raw_data = self.data_array.copy()
256
+
257
+ # 校准中
258
+ if self.is_calibrating:
259
+ self.calibration_buffer.append(raw_data)
260
+ progress = min(100, len(self.calibration_buffer) * 100 // CALIBRATION_FRAMES)
261
+ self.calibration_progress.emit(progress)
262
+ if len(self.calibration_buffer) >= CALIBRATION_FRAMES:
263
+ self.baseline_map = np.max(np.stack(self.calibration_buffer), axis=0) + 10
264
+ self.is_calibrating = False
265
+ return None, 0
266
+
267
+ # 时域平滑
268
+ if self.history_buffer is None:
269
+ self.history_buffer = raw_data.copy()
270
+ self.history_buffer = (
271
+ self.history_buffer * self.temporal_smooth
272
+ + raw_data * (1.0 - self.temporal_smooth)
273
+ )
274
+ smoothed = self.history_buffer.copy()
275
+
276
+ # 去基线
277
+ processed = np.maximum(0, smoothed - self.baseline_map)
278
+
279
+ # 漂移补偿
280
+ self.drift_baseline_val = np.percentile(processed, 40)
281
+ if self.drift_baseline_val > 100:
282
+ processed = np.maximum(0, processed - self.drift_baseline_val * 0.8)
283
+
284
+ # 动态噪声门
285
+ current_max = processed.max()
286
+ dynamic_gate = min(current_max * self.dynamic_noise_ratio, 250)
287
+ final_gate = max(self.base_noise_gate, dynamic_gate)
288
+ processed[processed < final_gate] = 0
289
+
290
+ # 空间滤波
291
+ if self.spatial_filter_enabled:
292
+ import cv2
293
+ mask = (processed > 0).astype(np.uint8)
294
+ kernel = np.array([[1, 1, 1], [1, 0, 1], [1, 1, 1]], dtype=np.uint8)
295
+ neighbors = cv2.filter2D(mask, -1, kernel, borderType=cv2.BORDER_CONSTANT)
296
+ noise_floor = np.maximum(self.baseline_map * 2.5, 180)
297
+ isolated_noise = (
298
+ (mask > 0) & (neighbors == 0) & (processed < noise_floor)
299
+ )
300
+ processed[isolated_noise] = 0
301
+
302
+ max_signal = processed.max()
303
+ return processed, max_signal
collector/core/calibration.py ADDED
@@ -0,0 +1,147 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ EgoData 标定数据模型 —— 双目相机内外参 + 深度缩放。
3
+ """
4
+
5
+ from __future__ import annotations
6
+ from dataclasses import dataclass, field
7
+ from typing import List, Optional
8
+ import json
9
+ import numpy as np
10
+
11
+
12
+ @dataclass
13
+ class CameraIntrinsics:
14
+ """单目相机内参。"""
15
+ intrinsic: List[float] # [fx, fy, cx, cy]
16
+ distortion: List[float] = field(default_factory=list) # k1,k2,p1,p2[,k3]
17
+
18
+ @classmethod
19
+ def from_matrix(cls, K: np.ndarray, dist: np.ndarray = None) -> "CameraIntrinsics":
20
+ """从 OpenCV 标定矩阵构建。"""
21
+ fx, fy = float(K[0, 0]), float(K[1, 1])
22
+ cx, cy = float(K[0, 2]), float(K[1, 2])
23
+ d = dist.flatten().tolist() if dist is not None and dist.size > 0 else []
24
+ return cls(intrinsic=[fx, fy, cx, cy], distortion=d)
25
+
26
+ def to_matrix(self) -> np.ndarray:
27
+ """还原为 3×3 相机矩阵。"""
28
+ fx, fy, cx, cy = self.intrinsic
29
+ return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64)
30
+
31
+ def to_dist_array(self) -> np.ndarray:
32
+ """还原为畸变系数数组。"""
33
+ return np.array(self.distortion, dtype=np.float64) if self.distortion else np.zeros(4)
34
+
35
+
36
+ @dataclass
37
+ class StereoCalibration:
38
+ """双目 + 深度相机标定(EgoData 标准)。
39
+
40
+ JSON 格式:
41
+ {
42
+ "type": "stereo_rgbd_camera",
43
+ "name": "head_stereo",
44
+ "resolution": [1280, 800],
45
+ "fps": 25,
46
+ "baseline": 0.095,
47
+ "left_camera": {"intrinsic": [fx, fy, cx, cy], "distortion": []},
48
+ "right_camera": {"intrinsic": [fx, fy, cx, cy], "distortion": []},
49
+ "depth_scale": 0.001,
50
+ "cam_imu_timeshift": -0.0019
51
+ }
52
+ """
53
+ type: str = "stereo_rgbd_camera"
54
+ name: str = "head_stereo"
55
+ resolution: List[int] = field(default_factory=lambda: [1280, 800])
56
+ fps: float = 25.0
57
+ baseline: float = 0.095 # 基线 (米)
58
+ left_camera: CameraIntrinsics = field(
59
+ default_factory=lambda: CameraIntrinsics(intrinsic=[0, 0, 0, 0]))
60
+ right_camera: CameraIntrinsics = field(
61
+ default_factory=lambda: CameraIntrinsics(intrinsic=[0, 0, 0, 0]))
62
+ depth_scale: float = 0.001 # 像素值 × depth_scale = 米
63
+ cam_imu_timeshift: float = -0.0019 # 相机-IMU 时间偏移 (秒), IMU 比相机早
64
+ # 来源: SDK calib.yaml timeshift_cam_imu
65
+ # (cam0=-0.001947, cam1=-0.001851)
66
+ depth_camera: Optional[CameraIntrinsics] = None # 深度相机内参(D435 独有;S80M 无此字段)
67
+ # 注: rs2 畸变系数为 Inverse Brown Conrady
68
+ # 模型,原样存储,消费方需按 rs2 模型处理
69
+
70
+ # ── 序列化 ──────────────────────────────────────
71
+
72
+ def to_dict(self) -> dict:
73
+ d = {
74
+ "type": self.type,
75
+ "name": self.name,
76
+ "resolution": list(self.resolution),
77
+ "fps": self.fps,
78
+ "baseline": self.baseline,
79
+ "left_camera": {
80
+ "intrinsic": list(self.left_camera.intrinsic),
81
+ "distortion": list(self.left_camera.distortion),
82
+ },
83
+ "right_camera": {
84
+ "intrinsic": list(self.right_camera.intrinsic),
85
+ "distortion": list(self.right_camera.distortion),
86
+ },
87
+ "depth_scale": self.depth_scale,
88
+ "cam_imu_timeshift": self.cam_imu_timeshift,
89
+ }
90
+ if self.depth_camera is not None:
91
+ d["depth_camera"] = {
92
+ "intrinsic": list(self.depth_camera.intrinsic),
93
+ "distortion": list(self.depth_camera.distortion),
94
+ }
95
+ return d
96
+
97
+ @classmethod
98
+ def from_dict(cls, d: dict) -> "StereoCalibration":
99
+ def _intrinsics(key: str) -> CameraIntrinsics:
100
+ cam = d.get(key, {})
101
+ return CameraIntrinsics(
102
+ intrinsic=list(cam.get("intrinsic", [0, 0, 0, 0])),
103
+ distortion=list(cam.get("distortion", [])),
104
+ )
105
+ return cls(
106
+ type=d.get("type", "stereo_rgbd_camera"),
107
+ name=d.get("name", "head_stereo"),
108
+ resolution=list(d.get("resolution", [1280, 800])),
109
+ fps=float(d.get("fps", 25.0)),
110
+ baseline=float(d.get("baseline", 0.095)),
111
+ left_camera=_intrinsics("left_camera"),
112
+ right_camera=_intrinsics("right_camera"),
113
+ depth_scale=float(d.get("depth_scale", 0.001)),
114
+ cam_imu_timeshift=float(d.get("cam_imu_timeshift", -0.0019)),
115
+ depth_camera=_intrinsics("depth_camera") if "depth_camera" in d else None,
116
+ )
117
+
118
+ def save(self, path: str):
119
+ with open(path, "w", encoding="utf-8") as f:
120
+ json.dump(self.to_dict(), f, indent=2, ensure_ascii=False)
121
+
122
+ @classmethod
123
+ def load(cls, path: str) -> "StereoCalibration":
124
+ with open(path, "r", encoding="utf-8") as f:
125
+ return cls.from_dict(json.load(f))
126
+
127
+ # ── 工厂方法 ────────────────────────────────────
128
+
129
+ @classmethod
130
+ def from_opencv(cls,
131
+ K_left: np.ndarray, dist_left: np.ndarray,
132
+ K_right: np.ndarray, dist_right: np.ndarray,
133
+ baseline: float = 0.095,
134
+ resolution: tuple = (1280, 800),
135
+ fps: float = 25.0,
136
+ depth_scale: float = 0.001) -> "StereoCalibration":
137
+ """从 OpenCV stereoCalibrate 结果构建。"""
138
+ return cls(
139
+ type="stereo_rgbd_camera",
140
+ name="head_stereo",
141
+ resolution=list(resolution),
142
+ fps=fps,
143
+ baseline=baseline,
144
+ left_camera=CameraIntrinsics.from_matrix(K_left, dist_left),
145
+ right_camera=CameraIntrinsics.from_matrix(K_right, dist_right),
146
+ depth_scale=depth_scale,
147
+ )
collector/core/camera.py ADDED
@@ -0,0 +1,676 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 摄像机采集模块 —— 后台线程采集帧,通过 Qt 信号发送到主线程。
3
+ 所有 VideoCapture 操作在采集线程内执行,确保线程安全。
4
+ """
5
+
6
+ from __future__ import annotations
7
+ import os
8
+ import re
9
+ import time
10
+ import threading
11
+ import queue
12
+ from typing import Optional, Tuple
13
+
14
+ import cv2
15
+ import numpy as np
16
+ from PyQt5.QtCore import QObject, pyqtSignal, QMutex, QMutexLocker
17
+
18
+ from config import settings
19
+
20
+
21
+ class CameraState:
22
+ DISCONNECTED = "disconnected"
23
+ IDLE = "idle"
24
+ RECORDING = "recording"
25
+ ERROR = "error"
26
+
27
+
28
+ if os.name == "nt":
29
+ _BACKENDS = [
30
+ ("DShow", cv2.CAP_DSHOW),
31
+ ("MSMF", cv2.CAP_MSMF),
32
+ ("ANY", cv2.CAP_ANY),
33
+ ]
34
+ else:
35
+ _BACKENDS = [
36
+ ("V4L2", cv2.CAP_V4L2),
37
+ ("FFMPEG", cv2.CAP_FFMPEG), # 兜底:OpenCV 5.0 V4L2 的 obsensor bug 可能导致 UVC 相机无法打开
38
+ ("ANY", cv2.CAP_ANY),
39
+ ]
40
+
41
+
42
+ class _Quiet:
43
+ def __enter__(self):
44
+ self._devnull_fd = os.open(os.devnull, os.O_WRONLY)
45
+ self._saved_fd = os.dup(2)
46
+ os.dup2(self._devnull_fd, 2)
47
+ os.close(self._devnull_fd)
48
+ return self
49
+ def __exit__(self, *_):
50
+ os.dup2(self._saved_fd, 2)
51
+ os.close(self._saved_fd)
52
+ return False
53
+
54
+
55
+ def _quiet():
56
+ return _Quiet()
57
+
58
+
59
+ def _find_persistent_v4l_path(index: int) -> Optional[str]:
60
+ """查找指向 /dev/video{index} 的 by-id 永久路径。
61
+
62
+ 返回 /dev/v4l/by-id/xxx 路径,该路径在设备重新插拔后保持不变,
63
+ 即使 /dev/videoN 编号发生变化。
64
+ """
65
+ target = f"../../video{index}"
66
+ by_id_dir = "/dev/v4l/by-id"
67
+ if not os.path.isdir(by_id_dir):
68
+ return None
69
+ try:
70
+ for entry in os.listdir(by_id_dir):
71
+ entry_path = os.path.join(by_id_dir, entry)
72
+ try:
73
+ if os.readlink(entry_path) == target:
74
+ return entry_path
75
+ except OSError:
76
+ pass
77
+ except OSError:
78
+ pass
79
+ return None
80
+
81
+
82
+ def _scan_all_v4l_by_id_paths() -> list:
83
+ """扫描 /dev/v4l/by-id/ 下所有设备路径,返回按优先级排列的路径列表。"""
84
+ paths = []
85
+ by_id_dir = "/dev/v4l/by-id"
86
+ if not os.path.isdir(by_id_dir):
87
+ return paths
88
+ try:
89
+ for entry in sorted(os.listdir(by_id_dir)):
90
+ entry_path = os.path.join(by_id_dir, entry)
91
+ if os.path.islink(entry_path):
92
+ paths.append(entry_path)
93
+ except OSError:
94
+ pass
95
+ return paths
96
+
97
+
98
+ def _resolve_v4l_index(index: int) -> list:
99
+ """将 /dev/videoN 索引解析为应尝试的设备路径列表。
100
+
101
+ 优先级:
102
+ 1. /dev/v4l/by-id/xxx → /dev/video{index} 的永久路径(不随重新插拔改变)
103
+ 2. 数字索引 /dev/video{index}
104
+ 3. 所有 by-id 路径(仅作为重连兜底:设备可能换了 /dev/videoN 编号)
105
+ """
106
+ paths = []
107
+ persistent = _find_persistent_v4l_path(index)
108
+ if persistent:
109
+ paths.append(persistent)
110
+ paths.append(index)
111
+ return paths
112
+
113
+
114
+ def _try_open_camera(index: int, test_read: bool = False,
115
+ fallback_all_by_id: bool = False) -> Tuple[Optional[cv2.VideoCapture], str]:
116
+ """尝试打开摄像机。
117
+
118
+ Args:
119
+ index: /dev/videoN 数字索引
120
+ test_read: 是否测试读取第一帧
121
+ fallback_all_by_id: 索引失败后是否尝试所有 by-id 路径(用于重连场景)
122
+ """
123
+ # 跳过 FTDI 双目相机设备(需专用 SDK,OpenCV 无法正确捕获)
124
+ if _is_sdk_device(index):
125
+ return None, ""
126
+ # 跳过 RealSense D435 的 UVC 节点(深度走 pyrealsense2 专用通道)
127
+ if _is_realsense_node(index):
128
+ return None, ""
129
+ for device in _resolve_v4l_index(index):
130
+ for name, backend in _BACKENDS:
131
+ try:
132
+ cap = cv2.VideoCapture(device, backend)
133
+ if cap.isOpened():
134
+ if test_read:
135
+ ok, frame = cap.read()
136
+ if not (ok and frame is not None and frame.size > 0):
137
+ cap.release()
138
+ continue
139
+ return cap, name
140
+ except Exception:
141
+ pass
142
+ # 重连兜底:扫描所有 by-id 路径(设备可能换了 /dev/videoN 编号)
143
+ if fallback_all_by_id:
144
+ for p in _scan_all_v4l_by_id_paths():
145
+ # 跳过需专用 SDK / RealSense 的节点,避免 UVC 断线时误开
146
+ try:
147
+ target = os.readlink(p)
148
+ if target.startswith("../../video"):
149
+ n = int(target.split("video")[-1])
150
+ if _is_sdk_device(n) or _is_realsense_node(n):
151
+ continue
152
+ except (OSError, ValueError):
153
+ pass
154
+ for name, backend in _BACKENDS:
155
+ try:
156
+ cap = cv2.VideoCapture(p, backend)
157
+ if cap.isOpened():
158
+ if test_read:
159
+ ok, frame = cap.read()
160
+ if not (ok and frame is not None and frame.size > 0):
161
+ cap.release()
162
+ continue
163
+ return cap, name
164
+ except Exception:
165
+ pass
166
+ return None, ""
167
+
168
+
169
+ def _get_usb_camera_indices() -> set:
170
+ """用 DirectShow 获取 USB 摄像机的索引集合。"""
171
+ usb_indices = set()
172
+ try:
173
+ from pygrabber.dshow_graph import FilterGraph
174
+ devices = FilterGraph().get_input_devices()
175
+ for i, name in enumerate(devices):
176
+ nl = name.lower()
177
+ builtin_kw = ["integrated", "built-in", "builtin", "laptop",
178
+ "thinkpad", "ideapad", "hp ", "dell ", "front",
179
+ "rear", "depth", "ir camera", "3d",
180
+ "user facing", "acer", "lenovo", "asus", "internal"]
181
+ is_builtin = any(kw in nl for kw in builtin_kw)
182
+ is_usb = "usb" in nl
183
+ if is_usb or (not is_builtin):
184
+ usb_indices.add(i)
185
+ except Exception:
186
+ pass
187
+ return usb_indices
188
+
189
+
190
+ def _is_sdk_device(index: int) -> bool:
191
+ """检查 V4L2 设备是否需专用 SDK(FTDI 双目相机),非标准 UVC。"""
192
+ name_path = f"/sys/class/video4linux/video{index}/name"
193
+ try:
194
+ with open(name_path, "r") as f:
195
+ return "FTDI" in f.read()
196
+ except Exception:
197
+ return False
198
+
199
+
200
+ def _usb_vid_pid(video_index: int) -> Optional[Tuple[str, str]]:
201
+ """沿 /sys/class/video4linux/videoN 设备树向上(≤6 层)查找 idVendor/idProduct。
202
+
203
+ 返回 (vid, pid) 小写十六进制串;找不到返回 None。sysfs 只读,无副作用。
204
+ """
205
+ node = os.path.realpath(f"/sys/class/video4linux/video{video_index}")
206
+ for _ in range(6):
207
+ try:
208
+ with open(os.path.join(node, "idVendor")) as f:
209
+ vid = f.read().strip()
210
+ with open(os.path.join(node, "idProduct")) as f:
211
+ pid = f.read().strip()
212
+ if vid and pid:
213
+ return vid.lower(), pid.lower()
214
+ except OSError:
215
+ pass
216
+ parent = os.path.dirname(node)
217
+ if parent == node:
218
+ break
219
+ node = parent
220
+ return None
221
+
222
+
223
+ def _is_realsense_node(index: int) -> bool:
224
+ """检查 V4L2 设备是否为 Intel RealSense 的 UVC 节点。
225
+
226
+ RealSense D400 全家族由 pyrealsense2 专用通道输出,其 UVC 节点不能
227
+ 被 OpenCV 当普通相机使用(否则会与 D435 模式重复列出/抢占设备)。
228
+ 识别: vendor 8086 且(PID 命中 D435 的 0b07 或驱动 name 含
229
+ "RealSense")——D435i 是 0b3a、其余型号 PID 各异,用 name 兜底。
230
+ """
231
+ vp = _usb_vid_pid(index)
232
+ if vp is None or vp[0] != settings.REALSENSE_VID:
233
+ return False
234
+ if vp[1] == settings.REALSENSE_PID:
235
+ return True
236
+ try:
237
+ with open(f"/sys/class/video4linux/video{index}/name") as f:
238
+ return "realsense" in f.read().lower()
239
+ except OSError:
240
+ return False
241
+
242
+
243
+ def _by_id_device_streams() -> dict:
244
+ """解析 /dev/v4l/by-id 下所有条目,返回 {设备前缀: [video_index, ...]}。
245
+
246
+ entry 形如 usb-X_Y_Z-serial-video-index0,去掉末尾 "-video-indexN"
247
+ 后作为物理设备前缀,同一物理设备的多个流聚到同一键下。
248
+ """
249
+ device_streams: dict = {}
250
+ by_id_dir = "/dev/v4l/by-id"
251
+ if not os.path.isdir(by_id_dir):
252
+ return device_streams
253
+ try:
254
+ for entry in os.listdir(by_id_dir):
255
+ entry_path = os.path.join(by_id_dir, entry)
256
+ try:
257
+ target = os.readlink(entry_path)
258
+ if target.startswith("../../video"):
259
+ idx = int(target.split("video")[-1])
260
+ # 提取 USB 设备前缀,去掉末尾的 "-video-indexN"
261
+ parts = entry.rsplit("-video-index", 1)
262
+ prefix = parts[0] if len(parts) == 2 else entry
263
+ device_streams.setdefault(prefix, []).append(idx)
264
+ except (OSError, ValueError):
265
+ pass
266
+ except OSError:
267
+ pass
268
+ return device_streams
269
+
270
+
271
+ def _discover_indices(max_index: int = 8) -> list:
272
+ """发现应尝试打开的设备索引,包含通过 by-id 路径发现的额外索引。
273
+
274
+ 对于同一 USB 物理设备(如 DECXIN 相机的 video0 和 video1 两个流),
275
+ 只保留主视频流(-video-index0),过滤掉辅助流(-video-index1+)。
276
+ 避免同一台相机在页面上出现多个窗口。
277
+ """
278
+ # 基础扫描范围:所有 /dev/videoN(0..max_index-1)
279
+ all_indices = set(range(max_index))
280
+
281
+ device_streams = _by_id_device_streams()
282
+ # 每个物理设备只保留主视频流(-video-index0,即最小索引)
283
+ # 但仅当该设备的多个流 BOTH 都在扫描范围内时才排除辅助流
284
+ for prefix, idx_list in device_streams.items():
285
+ idx_list.sort()
286
+ primary_idx = idx_list[0]
287
+ # 确保主视频流在扫描范围内
288
+ all_indices.add(primary_idx)
289
+ # 只在超过 1 个流时排除辅助流
290
+ if len(idx_list) > 1:
291
+ for idx in idx_list[1:]:
292
+ all_indices.discard(idx)
293
+
294
+ return sorted(all_indices)
295
+
296
+
297
+ def list_v4l_devices(max_index: int = 16) -> list:
298
+ """sysfs 只读枚举 V4L2 设备(不 open、不 test_read,轮询安全)。
299
+
300
+ 以 /dev/v4l/by-id 分组识别物理设备,每个物理设备只保留主视频流
301
+ (最小索引)。返回列表每项:
302
+ {video_index, name, serial, by_id_path, vid, pid, is_sdk}
303
+ name 从 by-id 前缀解码(去 "usb-" 前缀、字段分隔符转空格);
304
+ serial 启发式取前缀最后一段(仅纯字母数字且 ≥8 字符才认作序号)。
305
+ 被占用的设备不会被打开,仍会稳定出现在列表中。
306
+ """
307
+ devices = []
308
+ for prefix, idx_list in sorted(_by_id_device_streams().items()):
309
+ idx_list = sorted(idx_list)
310
+ idx = idx_list[0] # 主视频流
311
+ if idx >= max_index:
312
+ continue
313
+ if prefix.startswith("usb-"):
314
+ # usb-<vendor>_<model>_<serial> → 分段转空格
315
+ segments = [s for s in re.split(r"[-_]", prefix[len("usb-"):]) if s]
316
+ serial = ""
317
+ if segments and segments[-1].isalnum() and len(segments[-1]) >= 8:
318
+ serial = segments[-1] # 序号单独显示,不进名称
319
+ segments = segments[:-1]
320
+ name = " ".join(segments).strip()
321
+ else:
322
+ name, serial = "", ""
323
+ if not name:
324
+ # 非 USB 前缀无法解码,回退 sysfs name 文件
325
+ try:
326
+ with open(f"/sys/class/video4linux/video{idx}/name") as f:
327
+ name = f.read().strip()
328
+ except OSError:
329
+ name = f"video{idx}"
330
+ vp = _usb_vid_pid(idx)
331
+ devices.append({
332
+ "video_index": idx,
333
+ "name": name,
334
+ "serial": serial,
335
+ "by_id_path": _find_persistent_v4l_path(idx),
336
+ "vid": vp[0] if vp else None,
337
+ "pid": vp[1] if vp else None,
338
+ "is_sdk": _is_sdk_device(idx),
339
+ "is_realsense": _is_realsense_node(idx),
340
+ })
341
+ return devices
342
+
343
+
344
+ def detect_cameras(max_index: int = 8) -> list:
345
+ usb_indices = _get_usb_camera_indices()
346
+ with _quiet():
347
+ found = []
348
+ seen_indices = set()
349
+ for idx in _discover_indices(max_index):
350
+ if idx >= max_index * 2: # 只扩展到合理范围
351
+ continue
352
+ if usb_indices and idx not in usb_indices:
353
+ continue
354
+ if _is_sdk_device(idx):
355
+ continue # 跳过 FTDI 双目相机(需专用 SDK)
356
+ if _is_realsense_node(idx):
357
+ continue # 跳过 RealSense UVC 节点(D435 走 SDK 专用通道)
358
+ if idx in seen_indices:
359
+ continue
360
+ cap, backend = _try_open_camera(idx, test_read=True)
361
+ if cap is not None:
362
+ cap.release()
363
+ found.append((idx, backend))
364
+ seen_indices.add(idx)
365
+ return found
366
+
367
+
368
+ class CameraWorker(QObject):
369
+ frame_ready = pyqtSignal(np.ndarray)
370
+ state_changed = pyqtSignal(str)
371
+ error_occurred = pyqtSignal(str)
372
+ fps_updated = pyqtSignal(float)
373
+ camera_opened = pyqtSignal(int, int, str)
374
+
375
+ _RECONNECT_BASE_DELAY = 2.0
376
+ _RECONNECT_MAX_DELAY = 30.0
377
+ _READ_FAIL_THRESHOLD = 120
378
+ _INITIAL_FAIL_THRESHOLD = 30
379
+ _INITIAL_GRACE_PERIOD = 90
380
+
381
+ def __init__(self, camera_index: int = 0, resolution: tuple = None,
382
+ record_queue: queue.Queue = None, on_queue_full=None):
383
+ """Args:
384
+ on_queue_full: 录制队列满丢帧回调(无参;v1.0.9 丢帧统计用,
385
+ 采集线程内调用,必须轻量线程安全)
386
+ """
387
+ super().__init__()
388
+ self.camera_index = camera_index
389
+ # 默认满分辨率 1280×960@30(需 MJPG,见 _open_camera)
390
+ self._requested_res = resolution if resolution is not None else settings.DEFAULT_RESOLUTION
391
+ self._cap: Optional[cv2.VideoCapture] = None
392
+ self._running = False
393
+ self._paused = False
394
+ self._mutex = QMutex()
395
+ self._fps_counter = _FPSCounter()
396
+ self._state = CameraState.DISCONNECTED
397
+ self._actual_res: Tuple[int, int] = (0, 0)
398
+ self._backend_name = ""
399
+ self._thread: Optional[threading.Thread] = None
400
+ # 帧采集时间戳(微秒),在 capture_loop 中随帧一起更新
401
+ self._latest_capture_ts_us: int = 0
402
+ self._ts_lock = QMutex()
403
+ # 录制帧队列 —— B+ 方案:采集线程直接写入,绕过 Qt 信号/主线程
404
+ self._record_queue = record_queue
405
+ self._on_queue_full = on_queue_full # v1.0.9 丢帧统计回调
406
+ # 曝光:UVC 固定自动曝光(正常亮度 + 30fps;实测该相机手动模式
407
+ # 帧率上限 ~26fps)。开启/重连后由采集循环在首帧读出成功后
408
+ # 强制应用(见 _open_camera/_capture_loop)
409
+ self._exp_lock = QMutex()
410
+ self._exp_pending = False
411
+
412
+ @property
413
+ def state(self) -> str:
414
+ return self._state
415
+ @property
416
+ def is_connected(self) -> bool:
417
+ return self._cap is not None and self._cap.isOpened()
418
+ @property
419
+ def resolution(self) -> Tuple[int, int]:
420
+ return self._actual_res if self._actual_res[0] > 0 else settings.DEFAULT_RESOLUTION
421
+ @property
422
+ def latest_capture_ts_us(self) -> int:
423
+ """最新帧的采集时间戳(Unix 微秒),线程安全。"""
424
+ with QMutexLocker(self._ts_lock):
425
+ return self._latest_capture_ts_us
426
+
427
+ def start(self):
428
+ with QMutexLocker(self._mutex):
429
+ if self._running:
430
+ return
431
+ self._running = True
432
+ self._paused = False
433
+ self._thread = threading.Thread(target=self._capture_loop, daemon=True, name=f"cam-{self.camera_index}")
434
+ self._thread.start()
435
+
436
+ def stop(self):
437
+ with QMutexLocker(self._mutex):
438
+ self._running = False
439
+ if self._thread is not None and self._thread.is_alive():
440
+ self._thread.join(timeout=5.0)
441
+ self._thread = None
442
+ self._release_camera()
443
+ self._set_state(CameraState.DISCONNECTED)
444
+
445
+ def pause(self): self._paused = True
446
+ def resume(self): self._paused = False
447
+
448
+ def _set_state(self, state: str):
449
+ if self._state != state:
450
+ self._state = state
451
+ self.state_changed.emit(state)
452
+
453
+ # ── 曝光(UVC 固定自动曝光:开启/重连后强制应用) ──
454
+
455
+ @staticmethod
456
+ def _apply_exposure_to(cap, auto: bool, value: float,
457
+ backend: str = "") -> tuple:
458
+ """把曝光参数写进 VideoCapture。V4L2 下须先切自动/手动再写值,
459
+ 否则手动值被自动模式覆盖。返回 (auto 设置成功, 值设置成功)。
460
+
461
+ V4L2 后端(OpenCV 5 起)对 CAP_PROP_AUTO_EXPOSURE 是菜单值直通:
462
+ 标准相机 0=自动 1=手动,但部分相机(如 DECXIN)菜单布局不同
463
+ (1=手动、3=光圈优先即自动、无 0)。因此按候选顺序尝试,
464
+ 以 set 返回 True 且 get 读回一致为准。Windows 按 OpenCV MSMF
465
+ 约定 1=自动/0=手动 写入;DShow/FFMPEG 不实现该属性(写入无副作用)。
466
+ """
467
+ auto_prop = getattr(cv2, "CAP_PROP_AUTO_EXPOSURE", 21)
468
+ if backend in ("V4L2", "ANY") and os.name != "nt":
469
+ # 自动:0 标准自动 → 3 光圈优先 → 2 快门优先;手动:1 标准手动 → 0 兜底
470
+ candidates = (0, 3, 2) if auto else (1, 0)
471
+ ok_auto = False
472
+ for cand in candidates:
473
+ if cap.set(auto_prop, cand) and cap.get(auto_prop) == cand:
474
+ ok_auto = True
475
+ break
476
+ else:
477
+ # Windows:OpenCV MSMF 约定 1=自动 0=手动(写 0 会把相机锁进
478
+ # 手动曝光,亮度不可控);DShow/FFMPEG 不实现该属性,无副作用。
479
+ ok_auto = cap.set(auto_prop, 1 if auto else 0)
480
+ if auto:
481
+ return ok_auto, True
482
+ ok_val = cap.set(cv2.CAP_PROP_EXPOSURE, max(1.0, float(value)))
483
+ return ok_auto, ok_val
484
+
485
+ def _open_camera(self) -> bool:
486
+ self._release_camera()
487
+ with _quiet():
488
+ cap, backend = _try_open_camera(self.camera_index, fallback_all_by_id=True)
489
+ if cap is None:
490
+ self._set_state(CameraState.ERROR)
491
+ self.error_occurred.emit(f"Camera {self.camera_index}: cannot open")
492
+ return False
493
+ self._cap = cap
494
+ self._backend_name = backend
495
+ # UVC 满分辨率 30fps 必须走 MJPG:YUYV 格式在 1280×960 硬限 5fps,
496
+ # 且驱动默认格式就是 YUYV(只设分辨率会静默掉帧率)。读回不一致
497
+ # 说明该相机不支持 MJPG → 继续用默认格式。
498
+ # FFMPEG 兜底后端不识别 FOURCC/FPS 属性(写了也无效),跳过。
499
+ # 设置顺序按后端拆分(两者要求相反):
500
+ # - Windows DShow/MSMF:每次 SetFormat 都以引脚当前/首选媒体类型为
501
+ # 基础重建,最后一次设置决定最终格式 → FPS → 分辨率 → FOURCC 收尾,
502
+ # 之后不再设任何属性。旧顺序(先 FOURCC 再补 FPS)恰好把 MJPG 打回
503
+ # YUYV(960p 只剩 5fps 档);插上 D435 后驱动协商行为变化才暴露。
504
+ # - Linux V4L2:FOURCC 须在分辨率之前设置,帧率放最后。
505
+ tunable = backend in ("V4L2", "DShow", "MSMF", "ANY")
506
+ want_fourcc = (cv2.VideoWriter_fourcc(*settings.UVC_FOURCC)
507
+ if settings.UVC_FOURCC else 0)
508
+
509
+ def _set_fourcc(quiet: bool = False):
510
+ if not (tunable and want_fourcc):
511
+ return
512
+ try:
513
+ self._cap.set(cv2.CAP_PROP_FOURCC, want_fourcc)
514
+ if not quiet and int(self._cap.get(cv2.CAP_PROP_FOURCC)) != want_fourcc:
515
+ print(f"Camera {self.camera_index}: FOURCC "
516
+ f"{settings.UVC_FOURCC} 不支持,回退默认格式",
517
+ flush=True)
518
+ except Exception:
519
+ pass
520
+
521
+ # 显式请求采集帧率(V4L2 按离散间隔就近选择;DShow/MSMF 设媒体类型
522
+ # 帧率)。不设时驱动取默认间隔——本相机 MJPG 默认 120fps,会白烧
523
+ # CPU 且录制丢帧。
524
+ def _set_fps():
525
+ if not tunable:
526
+ return
527
+ try:
528
+ self._cap.set(cv2.CAP_PROP_FPS, settings.DEFAULT_FPS)
529
+ except Exception:
530
+ pass
531
+
532
+ def _set_resolution():
533
+ if not self._requested_res:
534
+ return
535
+ w, h = self._requested_res
536
+ self._cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
537
+ self._cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
538
+
539
+ if backend in ("DShow", "MSMF"):
540
+ # Windows:FOURCC 收尾,之后不再设任何属性(见上方注释)
541
+ _set_fps()
542
+ _set_resolution()
543
+ _set_fourcc()
544
+ else:
545
+ # Linux V4L2 / ANY 兜底:FOURCC 在前,帧率最后(见上方注释)
546
+ _set_fourcc()
547
+ _set_resolution()
548
+ _set_fps()
549
+ # UVC 固定自动曝光:开启/重连一律回到自动(正常亮度 + 30fps;
550
+ # 实测该相机手动模式帧率上限 ~26fps)。不在此直接写:部分 UVC
551
+ # 相机在 STREAMON(首次 read)时重置/忽略 AE 相关控制,开流前的
552
+ # 写入会丢失 → 标记 pending,由采集循环首帧读出成功后下发。
553
+ with QMutexLocker(self._exp_lock):
554
+ self._exp_pending = True
555
+ actual_w = int(self._cap.get(cv2.CAP_PROP_FRAME_WIDTH))
556
+ actual_h = int(self._cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
557
+ if actual_w <= 0 or actual_h <= 0:
558
+ actual_w, actual_h = 640, 480
559
+ self._actual_res = (actual_w, actual_h)
560
+ # 诊断:打印后端协商结果(Windows 5fps 排查用,控制台可见)
561
+ try:
562
+ fc = int(self._cap.get(cv2.CAP_PROP_FOURCC))
563
+ fc_disp = "".join(chr(c) if 32 <= c < 127 else "?"
564
+ for c in ((fc >> 24) & 0xFF, (fc >> 16) & 0xFF,
565
+ (fc >> 8) & 0xFF, fc & 0xFF))
566
+ fps_rb = float(self._cap.get(cv2.CAP_PROP_FPS))
567
+ except Exception:
568
+ fc_disp, fps_rb = "?", float("nan")
569
+ print(f"Camera {self.camera_index}: backend={backend} "
570
+ f"fourcc={fc_disp} res={actual_w}x{actual_h} "
571
+ f"fps={fps_rb:.1f}", flush=True)
572
+ self._set_state(CameraState.IDLE)
573
+ self.camera_opened.emit(actual_w, actual_h, backend)
574
+ return True
575
+
576
+ def _release_camera(self):
577
+ with QMutexLocker(self._mutex):
578
+ if self._cap is not None:
579
+ try: self._cap.release()
580
+ except Exception: pass
581
+ self._cap = None
582
+
583
+ def _capture_loop(self):
584
+ self._open_camera()
585
+ display_interval = 1.0 / max(settings.DISPLAY_FPS_LIMIT, 1)
586
+ last_emit = 0.0
587
+ consecutive_failures = 0
588
+ consecutive_open_failures = 0
589
+ iteration_count = 0
590
+ while True:
591
+ with QMutexLocker(self._mutex):
592
+ if not self._running:
593
+ break
594
+ cap = self._cap # 局部引用,防止 stop() 并发释放
595
+ if cap is None or not cap.isOpened():
596
+ self._set_state(CameraState.ERROR)
597
+ delay = self._reconnect_delay(consecutive_open_failures)
598
+ time.sleep(delay)
599
+ with QMutexLocker(self._mutex):
600
+ if not self._running: break
601
+ if self._open_camera():
602
+ consecutive_open_failures = 0
603
+ consecutive_failures = 0
604
+ iteration_count = 0
605
+ time.sleep(0.1)
606
+ else:
607
+ consecutive_open_failures += 1
608
+ continue
609
+ iteration_count += 1
610
+ ok, frame = cap.read()
611
+ if not ok or frame is None or frame.size == 0:
612
+ consecutive_failures += 1
613
+ threshold = self._INITIAL_FAIL_THRESHOLD if iteration_count <= self._INITIAL_GRACE_PERIOD else self._READ_FAIL_THRESHOLD
614
+ if consecutive_failures >= threshold:
615
+ self._set_state(CameraState.ERROR)
616
+ self.error_occurred.emit(f"Camera {self.camera_index}: consecutive read failures, reconnecting")
617
+ self._release_camera()
618
+ consecutive_failures = 0
619
+ iteration_count = 0
620
+ time.sleep(0.03)
621
+ continue
622
+ # 强制应用自动曝光。放在首帧读出成功之后:STREAMON 后相机才
623
+ # 可靠接受 AE 控制,开流前的写入会被部分 UVC 相机重置/忽略
624
+ # (重连后画面发暗)。仅在开启/重连后首帧执行一次。
625
+ with QMutexLocker(self._exp_lock):
626
+ pending = self._exp_pending
627
+ self._exp_pending = False
628
+ if pending:
629
+ self._apply_exposure_to(cap, True, 0.0,
630
+ backend=self._backend_name)
631
+ # 帧采集成功,立即记录时间戳(微秒)
632
+ capture_ts_us = int(time.time() * 1_000_000)
633
+ with QMutexLocker(self._ts_lock):
634
+ self._latest_capture_ts_us = capture_ts_us
635
+ consecutive_failures = 0
636
+
637
+ # B+ 方案: 将帧副本直接推入录制队列(采集线程 → 写入线程,无需主线程中转)
638
+ if self._record_queue is not None:
639
+ try:
640
+ self._record_queue.put_nowait(frame.copy())
641
+ except queue.Full:
642
+ # 写入线程消费不及,丢弃旧帧,队列自然保持最新
643
+ if self._on_queue_full is not None:
644
+ try:
645
+ self._on_queue_full()
646
+ except Exception:
647
+ pass # 统计回调失败不影响采集
648
+
649
+ if self._state == CameraState.ERROR:
650
+ self._set_state(CameraState.IDLE)
651
+ self._fps_counter.tick()
652
+ now = time.perf_counter()
653
+ # 阈值留 10% 余量:采集帧率≈显示上限(30fps 采集 vs 30fps 上限)
654
+ # 时,严格等号比较会因帧间隔抖动系统性跳过约一半帧(实测
655
+ # 满分辨率 30fps 采集下显示仅 ~18fps)
656
+ if now - last_emit >= display_interval * 0.9 and not self._paused:
657
+ self.frame_ready.emit(frame.copy())
658
+ self.fps_updated.emit(self._fps_counter.fps)
659
+ last_emit = now
660
+ self._release_camera()
661
+
662
+ @staticmethod
663
+ def _reconnect_delay(consecutive_failures: int) -> float:
664
+ delay = CameraWorker._RECONNECT_BASE_DELAY * (2 ** consecutive_failures)
665
+ return min(delay, CameraWorker._RECONNECT_MAX_DELAY)
666
+
667
+
668
+ class _FPSCounter:
669
+ def __init__(self, window: int = 30):
670
+ self._ticks = []; self._window = window; self.fps = 0.0
671
+ def tick(self):
672
+ now = time.perf_counter(); self._ticks.append(now)
673
+ if len(self._ticks) > self._window: self._ticks.pop(0)
674
+ if len(self._ticks) >= 2:
675
+ elapsed = self._ticks[-1] - self._ticks[0]
676
+ self.fps = (len(self._ticks) - 1) / elapsed if elapsed > 0 else 0.0
collector/core/d435_camera.py ADDED
@@ -0,0 +1,571 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ Intel RealSense D400 系列(D435/D435I/D405…)深度双目采集模块 —— 进程内 QObject worker。
3
+
4
+ D400 系列是稳定 UVC 用户态设备(pyrealsense2 自带 librealsense2 .so + libusb 后端),
5
+ 无需像 S80M 那样隔离到子进程。后台线程跑 rs2.pipeline,对外输出两路
6
+ (RGB 彩色 / 深度),经 Qt 信号跨线程发回主线程,复用
7
+ MainWindow 双目帧消费模式(显示 + 录制外部源链路)。
8
+ 左红外流仅内部启用(时间戳元数据),不对外输出;右红外不再开流
9
+ (基线改从 depth sensor 的 stereo_baseline 选项直读)。
10
+
11
+ 多设备支持:worker 按 serial 锁定具体设备(config.enable_device),
12
+ 面板点击哪台就开哪台;serial 为空时自动取第一台 D400。
13
+
14
+ 信号签名与 MainWindow.stereo_frame_ready 一致:
15
+ frames_ready(str slot_id, np.ndarray frame, int hardware_ns, list imu_samples)
16
+ - "d435_rgb": BGR 三通道彩色图
17
+ - "d435_depth": uint16 毫米深度图(D435/D405 无 IMU,imu_samples 恒为 [])
18
+
19
+ 左红外流仅内部启用(时间戳元数据),不对外输出、不显示、不落盘;
20
+ 右红外不再开流——其唯一用途是算基线,现改从 depth sensor 的
21
+ stereo_baseline 选项直读。双 RealSense 同 hub 共享带宽时多一条
22
+ IR2 会让 D405 掉到 ~19fps(实测,与开启顺序无关),去掉后两台满帧。
23
+ 深度本身由红外双目内部计算,无需暴露红外画面。
24
+
25
+ 用法:
26
+ from core.d435_camera import D435Worker, d435_available, list_d400_devices
27
+ if d435_available():
28
+ worker = D435Worker(width=848, height=480, fps=30, serial="…")
29
+ worker.frames_ready.connect(on_frames)
30
+ worker.start()
31
+ ...
32
+ worker.stop()
33
+
34
+ 亦可独立运行做无头自检:
35
+ python core/d435_camera.py
36
+ """
37
+
38
+ from __future__ import annotations
39
+ import os
40
+ import sys
41
+
42
+ # 脚本直跑(如 python core/d435_camera.py)时 sys.path[0] 是 core/,
43
+ # 需把项目根插入 sys.path 才能 import config —— 仅在直跑场景生效
44
+ if os.path.dirname(os.path.abspath(__file__)) == sys.path[0]:
45
+ sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
46
+
47
+ import threading
48
+ import time
49
+ from collections import deque
50
+ from typing import Optional, List
51
+
52
+ import cv2
53
+ import numpy as np
54
+ from PyQt5.QtCore import QObject, QMutex, QMutexLocker, pyqtSignal
55
+
56
+ from config import settings
57
+
58
+ # pyrealsense2 延迟失败:未安装时本模块仍可 import(d435_available 返回 False)
59
+ try:
60
+ import pyrealsense2 as rs
61
+ except ImportError: # pragma: no cover
62
+ rs = None
63
+
64
+
65
+ # d435_available 结果缓存(探测一次,避免重复打开设备上下文)
66
+ _d435_avail_cache: Optional[bool] = None
67
+
68
+
69
+ def d435_available(force: bool = False) -> bool:
70
+ """检测 pyrealsense2 可用且存在 D400 系列设备。结果模块级缓存。
71
+
72
+ force=True 绕过缓存(热插拔后点击路径的活体复查)。
73
+ """
74
+ global _d435_avail_cache
75
+ if not force and _d435_avail_cache is not None:
76
+ return _d435_avail_cache
77
+ _d435_avail_cache = bool(list_d400_devices())
78
+ return _d435_avail_cache
79
+
80
+
81
+ def list_d400_devices() -> List[tuple]:
82
+ """返回已连接的 D400 系列设备 [(name, serial), …](无设备/未安装 → [])。"""
83
+ out: List[tuple] = []
84
+ if rs is None:
85
+ return out
86
+ try:
87
+ ctx = rs.context()
88
+ for d in ctx.query_devices():
89
+ try:
90
+ if "D400" not in d.get_info(rs.camera_info.product_line):
91
+ continue
92
+ out.append((d.get_info(rs.camera_info.name),
93
+ d.get_info(rs.camera_info.serial_number)))
94
+ except Exception:
95
+ continue
96
+ except Exception:
97
+ pass
98
+ return out
99
+
100
+
101
+ def _frameset_timestamp_us(frames, rs_mod) -> float:
102
+ """帧时间戳逐级回退(µs):
103
+ 1) 深度帧 frame_timestamp 元数据(D435 老固件行为)
104
+ 2) 左红外帧 frame_timestamp 元数据
105
+ 3) 主机时间戳 frames.get_timestamp() × 1000
106
+
107
+ D405 + pyrealsense2 2.58.3 的深度/红外流不支持该元数据
108
+ (supports_frame_metadata=False,无条件读即 RuntimeError → 外层误判
109
+ 设备异常 → 2s 重连死循环)。每级先 supports_frame_metadata 探测再读,
110
+ 读失败继续回退;D405 实测走第 3 级——会话内仍单调递增,
111
+ hardware_ns 语义不变,精度从设备计数器降为主机时钟。
112
+ """
113
+ for getter in (
114
+ lambda: frames.get_depth_frame(),
115
+ lambda: frames.get_infrared_frame(1),
116
+ ):
117
+ try:
118
+ f = getter()
119
+ if (f is not None and f.supports_frame_metadata(
120
+ rs_mod.frame_metadata_value.frame_timestamp)):
121
+ meta = f.get_frame_metadata(
122
+ rs_mod.frame_metadata_value.frame_timestamp)
123
+ if meta is not None:
124
+ return meta
125
+ except Exception:
126
+ continue
127
+ return frames.get_timestamp() * 1000
128
+
129
+
130
+ def _apply_color_exposure(sensor, auto: bool, value: float) -> bool:
131
+ """对 color 传感器应用曝光(µs):auto=True 只开自动曝光;
132
+ 否则先关自动再写手动值。异常返回 False(不打断采集线程)。"""
133
+ try:
134
+ sensor.set_option(rs.option.enable_auto_exposure, 1 if auto else 0)
135
+ if not auto:
136
+ sensor.set_option(rs.option.exposure, float(value))
137
+ return True
138
+ except Exception:
139
+ return False
140
+
141
+
142
+ class D435Worker(QObject):
143
+ """D435 采集 worker:后台线程 rs2.pipeline 三路流,信号回主线程。"""
144
+
145
+ # (slot_id, frame, hw_ns, imu_samples)
146
+ # ★ hw_ns 必须用 object:PyQt5 队列信号把 Python int 按 C++ qint32
147
+ # 封送,超过 2^31(≈2.1s 纳秒)即静默截断为负数(S80M 已有数据受害)
148
+ frames_ready = pyqtSignal(str, np.ndarray, object, list)
149
+ error_occurred = pyqtSignal(str) # 错误消息
150
+ status_changed = pyqtSignal(str) # "running" / "stopped"
151
+
152
+ def __init__(self, width: int = None, height: int = None,
153
+ fps: int = None, parent=None, ts_log: str = None,
154
+ rgb_width: int = None, rgb_height: int = None,
155
+ serial: str = None, model_name: str = None,
156
+ rgb_slot: str = None, depth_slot: str = None,
157
+ exposure: dict = None):
158
+ super().__init__(parent)
159
+ self._width = width if width is not None else settings.D435_RESOLUTION[0]
160
+ self._height = height if height is not None else settings.D435_RESOLUTION[1]
161
+ self._fps = fps if fps is not None else settings.D435_FPS
162
+ self._rgb_width = (rgb_width if rgb_width is not None
163
+ else settings.D435_RGB_RESOLUTION[0])
164
+ self._rgb_height = (rgb_height if rgb_height is not None
165
+ else settings.D435_RGB_RESOLUTION[1])
166
+ self._serial = serial # 目标设备序列号(None → 第一台 D400)
167
+ self._model_name = model_name # 仅用于日志/诊断
168
+ # 槽位名(多设备时由 MainWindow 消歧分配;None 回落 settings 常量)
169
+ self._rgb_slot = rgb_slot or settings.D435_SLOT_RGB
170
+ self._depth_slot = depth_slot or settings.D435_SLOT_DEPTH
171
+ self._depth_unit_factor = 1.0 # 设备深度单位 → mm 换算系数(流启动时探测)
172
+ self._stop_event = threading.Event()
173
+ self._thread: Optional[threading.Thread] = None
174
+ self._calibration = None # StereoCalibration,首帧后可用
175
+ self._frame_count = 0
176
+ self._t0_us: Optional[int] = None # 首个帧计数(µs,重连时重置)
177
+ self._last_us: int = 0 # 上一帧原始计数(解绕用)
178
+ self._wrap_us: int = 0 # 累计回绕补偿(µs)
179
+ self._ts_log = ts_log # 诊断 CSV: i,ft_ms,meta_us(仅调试)
180
+ self._ts_fh = None # 首启失败时 _run 尾部清理仍需存在
181
+ # 曝光设置(主线程只写 pending;采集线程在流启动后应用,
182
+ # 每次重连新建管道须重放最近一次设置)
183
+ self._exp_lock = QMutex()
184
+ self._exp_auto: Optional[bool] = None
185
+ self._exp_value: Optional[float] = None
186
+ self._exp_pending = False
187
+ self._exp_range: Optional[tuple] = None # (min, max) µs,流启动后缓存
188
+ # 「最一开始」曝光基线(开流时读回硬件,应用任何设置之前)
189
+ self._original_exp: Optional[tuple] = None
190
+ if exposure:
191
+ self._exp_auto = bool(exposure.get("auto", True))
192
+ self._exp_value = float(exposure.get("value", 0.0))
193
+ self._exp_pending = True
194
+
195
+ # ── 生命周期 ────────────────────────────────────────
196
+
197
+ def start(self):
198
+ """启动后台采集线程(幂等)。"""
199
+ if self._thread is not None and self._thread.is_alive():
200
+ return
201
+ self._stop_event.clear()
202
+ self._thread = threading.Thread(target=self._run, daemon=True,
203
+ name="d435-capture")
204
+ self._thread.start()
205
+
206
+ def stop(self):
207
+ """停止采集线程(阻塞至多约 3s;wait_for_frames 1s 超时保证退出)。"""
208
+ self._stop_event.set()
209
+ if self._thread is not None:
210
+ self._thread.join(timeout=3.0)
211
+ self._thread = None
212
+ self.status_changed.emit("stopped")
213
+
214
+ # ── 标定(录制开始时由主线程读取) ────────────────────
215
+
216
+ def get_calibration(self):
217
+ """返回首帧后提取的 StereoCalibration;尚未就绪时为 None。"""
218
+ return self._calibration
219
+
220
+ # ── 曝光调节(主线程只写 pending;采集线程应用并缓存量程) ──
221
+
222
+ def set_exposure(self, auto: bool, value: float):
223
+ """设置 color 传感器曝光:auto=True 开自动;否则手动值(µs)。"""
224
+ with QMutexLocker(self._exp_lock):
225
+ self._exp_auto = bool(auto)
226
+ self._exp_value = float(value)
227
+ self._exp_pending = True
228
+
229
+ def exposure_info(self) -> tuple:
230
+ """返回 ((min,max)µs 或 None, auto, value);流未启动时量程 None。"""
231
+ with QMutexLocker(self._exp_lock):
232
+ return (self._exp_range, self._exp_auto, self._exp_value)
233
+
234
+ def original_exposure(self) -> tuple:
235
+ """「最一开始」曝光基线 (auto, value);未捕获返回 None。"""
236
+ with QMutexLocker(self._exp_lock):
237
+ return self._original_exp
238
+
239
+ # ── 后台线程 ────────────────────────────────────────
240
+
241
+ def _run(self):
242
+ if rs is None:
243
+ self.error_occurred.emit("pyrealsense2 未安装")
244
+ return
245
+
246
+ while not self._stop_event.is_set():
247
+ # 每次重连新建管道:复用同一 rs.pipeline 在"流停滞"后 stop→start
248
+ # 无法复位 UVC 通道,同一对象会无限循环"无帧→重连"(实测停滞机
249
+ # 新管道立即恢复出帧——新管道等价于新进程打开设备)
250
+ pipe = rs.pipeline()
251
+ try:
252
+ config = rs.config()
253
+ # 多设备时按序列号锁定目标设备;不指定则用第一台 D400
254
+ if self._serial:
255
+ config.enable_device(self._serial)
256
+ # 对外: RGB 彩色 + 深度两路
257
+ config.enable_stream(rs.stream.color,
258
+ self._rgb_width, self._rgb_height,
259
+ rs.format.rgb8, self._fps)
260
+ config.enable_stream(rs.stream.depth,
261
+ self._width, self._height,
262
+ rs.format.z16, self._fps)
263
+ # 内部: 左红外仅用于时间戳元数据,不对外输出。右红外不开流:
264
+ # 其唯一用途是算基线,现从 stereo_baseline 选项直读——
265
+ # 双 RealSense 同 hub 时多一条 IR2 会把 D405 挤到 ~19fps
266
+ config.enable_stream(rs.stream.infrared, 1,
267
+ self._width, self._height,
268
+ rs.format.y8, self._fps)
269
+ profile = pipe.start(config)
270
+ # 设备深度单位探测(D435=1mm,D405=0.1mm)→ mm 换算系数。
271
+ # 帧统一归一化到 mm 输出:全链路(热力图/无损深度流/metadata
272
+ # "unit:mm"/depth_scale 0.001)都按 mm 约定。
273
+ try:
274
+ dev_unit = float(profile.get_device()
275
+ .first_depth_sensor().get_depth_scale())
276
+ except Exception:
277
+ dev_unit = float(settings.DEPTH_SCALE)
278
+ self._depth_unit_factor = dev_unit / float(settings.DEPTH_SCALE)
279
+ # 曝光:流启动后缓存 color 传感器量程并应用最近设置。
280
+ # 重连 = 新管道 = 新传感器句柄,相机端设置会丢,须每次重放;
281
+ # 消费 pending 同样在本线程完成(主线程只写 pending)。
282
+ try:
283
+ color = profile.get_device().first_color_sensor()
284
+ rng = color.get_option_range(rs.option.exposure)
285
+ with QMutexLocker(self._exp_lock):
286
+ self._exp_range = (float(rng.min), float(rng.max))
287
+ # 「最一开始」曝光基线:应用任何设置前读回硬件
288
+ # 当前值(重连 = 新管道,读的是本次开流的硬件状态)
289
+ try:
290
+ oa = float(color.get_option(
291
+ rs.option.enable_auto_exposure))
292
+ ov = float(color.get_option(rs.option.exposure))
293
+ self._original_exp = (oa > 0.5, ov)
294
+ except Exception:
295
+ self._original_exp = None
296
+ auto, value = self._exp_auto, self._exp_value
297
+ self._exp_pending = False
298
+ if auto is not None:
299
+ _apply_color_exposure(color, auto,
300
+ value if value is not None else 0.0)
301
+ except Exception:
302
+ pass
303
+ if self._calibration is None:
304
+ self._calibration = _build_calibration(
305
+ profile, self._fps, (self._width, self._height))
306
+ # 帧已归一化为 mm,标定里 depth_scale 写输出格式刻度
307
+ # (设备原生单位仅影响内部换算,不进入落盘标定)
308
+ self._calibration.depth_scale = float(settings.DEPTH_SCALE)
309
+ self._t0_us = None # 设备计数随流启动重置,重连后重新归零
310
+ self._last_us = 0
311
+ self._wrap_us = 0
312
+ self._ts_fh = open(self._ts_log, "w") if self._ts_log else None
313
+ if self._ts_fh:
314
+ self._ts_fh.write("i,ft_ms,meta_us\n")
315
+ self.status_changed.emit("running")
316
+
317
+ # 看门狗一: 停滞——连续无帧秒数(双 RealSense 第二台开流偶发
318
+ # 静默停滞——管道已启动但不再出帧,wait_for_frames 持续超时,
319
+ # 无异常可捕获;累计超过阈值则抛给外层走重连)
320
+ # 看门狗二: 帧率——半死状态(零星出帧不报错但画面极卡),
321
+ # 10s 窗口内帧数低于期望一半 → 同样抛给外层走重连
322
+ stall_s = 0
323
+ fps_win: deque = deque() # 帧到达时刻(monotonic 秒)
324
+ fps_win_start: Optional[float] = None # 重连后首个帧时刻
325
+ while not self._stop_event.is_set():
326
+ try:
327
+ frames = pipe.wait_for_frames(timeout_ms=1000)
328
+ except RuntimeError:
329
+ # 超时无帧(拔线初期/流停滞) → 计数,超阈值触发重连
330
+ stall_s += 1
331
+ if stall_s >= settings.D435_STALL_TIMEOUT_S:
332
+ raise RuntimeError(
333
+ f"流停滞({stall_s}s 无帧),触发重连")
334
+ continue
335
+ stall_s = 0
336
+ # 消费 pending 曝光设置(主线程 set_exposure 下发)
337
+ with QMutexLocker(self._exp_lock):
338
+ if self._exp_pending and self._exp_auto is not None:
339
+ auto, value = self._exp_auto, self._exp_value
340
+ self._exp_pending = False
341
+ else:
342
+ auto = None
343
+ if auto is not None:
344
+ try:
345
+ color = profile.get_device().first_color_sensor()
346
+ _apply_color_exposure(
347
+ color, auto, value if value is not None else 0.0)
348
+ except Exception:
349
+ pass
350
+ now_s = time.monotonic()
351
+ fps_win.append(now_s)
352
+ while (fps_win
353
+ and now_s - fps_win[0] > settings.D435_LOW_FPS_WINDOW_S):
354
+ fps_win.popleft()
355
+ if fps_win_start is None:
356
+ fps_win_start = now_s
357
+ elif (now_s - fps_win_start >= settings.D435_LOW_FPS_WINDOW_S
358
+ and len(fps_win) < (settings.D435_LOW_FPS_FRACTION
359
+ * self._fps
360
+ * settings.D435_LOW_FPS_WINDOW_S)):
361
+ raise RuntimeError(
362
+ f"帧率过低({len(fps_win)}帧/"
363
+ f"{settings.D435_LOW_FPS_WINDOW_S}s,期望≥"
364
+ f"{settings.D435_LOW_FPS_FRACTION * self._fps:.0f}fps),"
365
+ f"触发重连")
366
+
367
+ # 帧时间戳逐级回退(深度→红外→主机时钟,见
368
+ # _frameset_timestamp_us)。设备计数为 32 位 µs 计数器
369
+ # (2^32µs ≈ 71.6min 才回绕),软件解绕 + 首帧归零 → 会话内
370
+ # 单调递增 hardware_ns(与 S80M 会话相对时钟语义一致)。
371
+ # 注: 排查记录里疑似"每 2.1s 回绕"实为 Qt 信号 qint32
372
+ # 截断假象(>2^31ns 翻转,已改用 object 参数修复)。
373
+ meta_us = _frameset_timestamp_us(frames, rs)
374
+ raw_us = int(meta_us)
375
+ if self._t0_us is None:
376
+ self._t0_us = raw_us
377
+ self._last_us = raw_us
378
+ hw_ns = 0
379
+ else:
380
+ # 解绕: 下跌超过半程(2^31µs) → 补一个整程(2^32µs)
381
+ if raw_us < self._last_us - (1 << 31):
382
+ self._wrap_us += 1 << 32
383
+ self._last_us = raw_us
384
+ hw_ns = (raw_us + self._wrap_us - self._t0_us) * 1000
385
+ if self._ts_fh:
386
+ self._ts_fh.write(
387
+ f"{self._frame_count},{frames.get_timestamp()},"
388
+ f"{int(meta_us)}\n")
389
+ self._ts_fh.flush()
390
+
391
+ # 显式 copy:rs2 帧缓冲由 C++ 持有,frameset 一释放即失效;
392
+ # 深度帧经排队信号延迟消费,必须脱离底层缓冲(848×480×2≈0.8MB)
393
+ depth = np.asanyarray(
394
+ frames.get_depth_frame().get_data()).copy() # 设备原生单位
395
+ if self._depth_unit_factor != 1.0:
396
+ # 归一化到 mm(D405 原生 0.1mm/单位;×0.1 不会溢出 uint16)
397
+ depth = (depth.astype(np.float32)
398
+ * self._depth_unit_factor + 0.5).astype(np.uint16)
399
+
400
+ # RGB8 → BGR 三通道(显示 + MP4 录制链路要求 BGR24)
401
+ rgb = np.asanyarray(frames.get_color_frame().get_data())
402
+ rgb_bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR)
403
+
404
+ self.frames_ready.emit(self._rgb_slot,
405
+ rgb_bgr, hw_ns, [])
406
+ self.frames_ready.emit(self._depth_slot,
407
+ depth, hw_ns, [])
408
+ self._frame_count += 1
409
+
410
+ except RuntimeError as e:
411
+ # 拔线 / 设备异常 → 每 2s 重连(仿 CameraWorker 重连策略)
412
+ if not self._stop_event.is_set():
413
+ self.error_occurred.emit(
414
+ f"RealSense {self._model_name or ''} "
415
+ f"{self._serial or ''} 设备异常,2s 后重连: {e}".replace(" ", " "))
416
+ self.status_changed.emit("error")
417
+ try:
418
+ pipe.stop()
419
+ except Exception:
420
+ pass
421
+ self._stop_event.wait(settings.CAMERA_RECONNECT_INTERVAL_MS / 1000.0)
422
+ except Exception as e:
423
+ if not self._stop_event.is_set():
424
+ self.error_occurred.emit(f"RealSense 采集异常: {e}")
425
+ try:
426
+ pipe.stop()
427
+ except Exception:
428
+ pass
429
+ self._stop_event.wait(settings.CAMERA_RECONNECT_INTERVAL_MS / 1000.0)
430
+
431
+ try:
432
+ pipe.stop()
433
+ except Exception:
434
+ pass
435
+ if self._ts_fh:
436
+ try:
437
+ self._ts_fh.close()
438
+ except Exception:
439
+ pass
440
+ self._ts_fh = None
441
+
442
+
443
+ # ── 标定提取 ───────────────────────────────────────────
444
+
445
+ def _build_calibration(profile, fps: int, resolution: tuple):
446
+ """从 rs2 pipeline profile 提取左右红外内参/深度内参/基线 → StereoCalibration。"""
447
+ from core.calibration import StereoCalibration, CameraIntrinsics
448
+
449
+ def _ir_intrinsics(idx: int) -> CameraIntrinsics:
450
+ try:
451
+ prof = profile.get_stream(rs.stream.infrared, idx)
452
+ except RuntimeError:
453
+ prof = None # 未启用的流: get_stream 抛异常而非返回 None
454
+ if prof is None:
455
+ # IR2 已不开流:右目与左目是同一传感器模组、几何一致,
456
+ # 内参镜像左目(基线另从 stereo_baseline 选项读取)
457
+ return _ir_intrinsics(1)
458
+ intr = prof.as_video_stream_profile().get_intrinsics()
459
+ return CameraIntrinsics(
460
+ intrinsic=[intr.fx, intr.fy, intr.ppx, intr.ppy],
461
+ distortion=list(intr.coeffs), # Inverse Brown Conrady 系数,原样存储
462
+ )
463
+
464
+ def _depth_intrinsics() -> CameraIntrinsics:
465
+ prof = profile.get_stream(rs.stream.depth)
466
+ intr = prof.as_video_stream_profile().get_intrinsics()
467
+ return CameraIntrinsics(
468
+ intrinsic=[intr.fx, intr.fy, intr.ppx, intr.ppy],
469
+ distortion=list(intr.coeffs),
470
+ )
471
+
472
+ def _baseline() -> float:
473
+ # 直读 depth sensor 的 stereo_baseline 选项(单位 mm→m),无需 IR2 流。
474
+ # 与旧 IR1→IR2 外参算法实测一致(D435 50.15mm / D405 17.98mm)
475
+ try:
476
+ return float(profile.get_device().first_depth_sensor()
477
+ .get_option(rs.option.stereo_baseline)) / 1000.0
478
+ except Exception:
479
+ # 无此选项的罕见设备:IR2 若仍在流中则回落外参算法
480
+ # (未启用时 get_stream 抛异常而非返回 None)
481
+ try:
482
+ ir2 = profile.get_stream(rs.stream.infrared, 2)
483
+ except RuntimeError:
484
+ ir2 = None
485
+ if ir2 is not None:
486
+ ir1 = profile.get_stream(
487
+ rs.stream.infrared, 1).as_video_stream_profile()
488
+ trans = ir1.get_extrinsics_to(
489
+ ir2.as_video_stream_profile()).translation
490
+ return float(np.linalg.norm(trans)) # D435 约 0.050 m
491
+ return float(settings.STEREO_BASELINE)
492
+
493
+ def _depth_scale() -> float:
494
+ try:
495
+ sensor = profile.get_device().first_depth_sensor()
496
+ return float(sensor.get_depth_scale())
497
+ except Exception:
498
+ return float(settings.DEPTH_SCALE)
499
+
500
+ return StereoCalibration(
501
+ resolution=[resolution[0], resolution[1]],
502
+ fps=float(fps),
503
+ baseline=_baseline(),
504
+ left_camera=_ir_intrinsics(1),
505
+ right_camera=_ir_intrinsics(2),
506
+ depth_camera=_depth_intrinsics(),
507
+ depth_scale=_depth_scale(),
508
+ cam_imu_timeshift=0.0, # D435 无 IMU
509
+ )
510
+
511
+
512
+ # ── 无头自检 ───────────────────────────────────────────
513
+
514
+ if __name__ == "__main__":
515
+ import sys
516
+ import os
517
+ # 脚本直跑时把项目根加入 sys.path(config/core 均为包)
518
+ sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
519
+ from PyQt5.QtCore import QCoreApplication
520
+
521
+ app = QCoreApplication(sys.argv)
522
+
523
+ if not d435_available():
524
+ print("未检测到 D435 设备(pyrealsense2 或设备缺失)")
525
+ sys.exit(1)
526
+
527
+ worker = D435Worker()
528
+ stats = {"t0": time.time(), "count": 0, "saved": False,
529
+ "sample_dir": "/tmp/d435_selftest"}
530
+
531
+ def on_frames(slot_id, frame, hw_ns, imu_samples):
532
+ stats["count"] += 1
533
+ # 无头自检不弹窗口(cv2.imshow 与 PyQt5 同进程会冲突);
534
+ # 各抽一帧存 /tmp 供人工查看
535
+ if not stats["saved"]:
536
+ os.makedirs(stats["sample_dir"], exist_ok=True)
537
+ if slot_id == settings.D435_SLOT_DEPTH:
538
+ cv2.imwrite(os.path.join(stats["sample_dir"], "depth.png"), frame)
539
+ else:
540
+ cv2.imwrite(os.path.join(stats["sample_dir"],
541
+ f"{slot_id}.png"), frame)
542
+ if os.path.isfile(os.path.join(stats["sample_dir"], "depth.png")) \
543
+ and os.path.isfile(os.path.join(stats["sample_dir"],
544
+ f"{settings.D435_SLOT_RGB}.png")):
545
+ stats["saved"] = True
546
+
547
+ worker.frames_ready.connect(on_frames)
548
+ worker.error_occurred.connect(lambda m: print("[error]", m))
549
+ worker.start()
550
+
551
+ print(f"D435 自检: RGB {worker._rgb_width}x{worker._rgb_height} + "
552
+ f"深度 {worker._width}x{worker._height}@{worker._fps}, 运行 5s...")
553
+ t_end = time.time() + 5.0
554
+ while time.time() < t_end:
555
+ app.processEvents()
556
+ time.sleep(0.01)
557
+
558
+ elapsed = time.time() - stats["t0"]
559
+ print(f" 帧率: {stats['count'] / max(elapsed, 1e-6):.1f} 信号/秒")
560
+ print(f" 抽样帧: {stats['sample_dir']}/ (depth.png, d435_rgb.png)")
561
+
562
+ calib = worker.get_calibration()
563
+ if calib is not None:
564
+ print("── 标定 ──")
565
+ print(f" baseline = {calib.baseline:.6f} m")
566
+ print(f" depth_scale = {calib.depth_scale}")
567
+ print(f" left IR 内参 = {calib.left_camera.intrinsic}")
568
+ print(f" right IR 内参 = {calib.right_camera.intrinsic}")
569
+ print(f" depth 内参 = {calib.depth_camera.intrinsic}")
570
+
571
+ worker.stop()
collector/core/d435_manager.py ADDED
@@ -0,0 +1,149 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ D435 深度双目设备管理器 —— worker 生命周期与帧处理口径
3
+ (QtCore QObject + 信号,不依赖 QtWidgets)。
4
+
5
+ - D435DeviceManager 每台一个条目:D435Worker 创建/接线/停止 +
6
+ 帧处理口径(calib 首帧注入、深度热力图/EMA、
7
+ 录制写入)
8
+ - 帧经 frames_ready(slot, ndarray, object, object, str) 交主线程
9
+
10
+ worker 类由调用方传入(离线测试在 ui.main_window 模块 patch D435Worker
11
+ 替身仍生效);hw_ns 用 object 封装(PyQt5 qint32 封送截断坑,同
12
+ core.s80m_manager 口径)。
13
+ """
14
+
15
+ from __future__ import annotations
16
+
17
+ from functools import partial
18
+
19
+ import cv2
20
+ import numpy as np
21
+
22
+ from PyQt5.QtCore import QObject, pyqtSignal
23
+
24
+ from config.i18n import tr
25
+ from core.depth_codec import depth_to_heatmap_bgr
26
+ from core.stereo_depth import DepthHeatmapSmoother
27
+
28
+
29
+ class D435DeviceManager(QObject):
30
+ """D435 深度双目采集 worker 管理(每台一个条目)。
31
+
32
+ 信号
33
+ ----
34
+ frames_ready(str, np.ndarray, object, object, str)
35
+ — (slot_id, frame, hw_ns, imu_samples, dev_key),采集线程 → 主线程
36
+ log(str)
37
+ — 跨线程日志
38
+ """
39
+
40
+ frames_ready = pyqtSignal(str, np.ndarray, object, object, str)
41
+ log = pyqtSignal(str)
42
+
43
+ def __init__(self, pipeline, parent=None):
44
+ super().__init__(parent)
45
+ self._pipeline = pipeline
46
+
47
+ @staticmethod
48
+ def new_entry(label: str, serial: str, rgb_slot: str, depth_slot: str,
49
+ near_mm: float, far_mm: float, smooth_k: int,
50
+ temporal_alpha: float) -> dict:
51
+ """注册表条目骨架(worker 由 spawn 填充,形状与旧主窗口口径一致)。"""
52
+ return {
53
+ "kind": "d435",
54
+ "slots": [rgb_slot, depth_slot],
55
+ "label": label,
56
+ "serial": serial,
57
+ "rgb_slot": rgb_slot,
58
+ "depth_slot": depth_slot,
59
+ "worker": None,
60
+ "calib_sent": False,
61
+ "near_mm": near_mm,
62
+ "far_mm": far_mm,
63
+ "smooth_k": smooth_k,
64
+ "temporal_alpha": temporal_alpha,
65
+ "heat_smoother": None,
66
+ }
67
+
68
+ def spawn(self, dev_key: str, entry: dict, model_name: str,
69
+ profile: dict, exposure, worker_cls) -> None:
70
+ """创建 D435Worker 并接线(帧/错误信号 → manager 信号 → 主窗口)。
71
+
72
+ 进程内采集 worker(无 IMU;深度与左红外同 imager 出厂对齐)。
73
+ 持久化曝光随 worker 启动应用(流启动后由采集线程消费)。
74
+ 信号连接在 worker.start() 之前完成。
75
+ """
76
+ worker = worker_cls(
77
+ width=profile["depth_resolution"][0],
78
+ height=profile["depth_resolution"][1],
79
+ fps=profile["fps"],
80
+ rgb_width=profile["rgb_resolution"][0],
81
+ rgb_height=profile["rgb_resolution"][1],
82
+ serial=entry["serial"], model_name=model_name,
83
+ parent=self,
84
+ rgb_slot=entry["rgb_slot"], depth_slot=entry["depth_slot"],
85
+ exposure=exposure)
86
+ worker.frames_ready.connect(
87
+ partial(self._relay_frames, dev_key=dev_key))
88
+ worker.error_occurred.connect(
89
+ lambda m: self.log.emit(tr("[RealSense 错误] {}", m)))
90
+ worker.start()
91
+ entry["worker"] = worker
92
+
93
+ def _relay_frames(self, slot_id: str, frame, hardware_ns=0,
94
+ imu_samples=None, dev_key: str = None):
95
+ self.frames_ready.emit(slot_id, frame, hardware_ns,
96
+ imu_samples, dev_key)
97
+
98
+ def process_frame(self, entry: dict, slot_id: str, frame,
99
+ hardware_ns: int = 0, dev_key: str = None) -> tuple:
100
+ """D435 帧处理口径 → 返回 (display_frame, is_depth)。
101
+
102
+ calib 首帧注入(录制开始时写 calibration/head_stereo.json);
103
+ 深度槽:规范码值 JET(core.depth_codec 量化域色标,与存储
104
+ 12-bit 灰度视频同构——near/far 线性色标已废弃;EMA 按型号
105
+ 配置,仅显示降噪,存储码值不受影响)+ 录制写原始 uint16
106
+ 毫米深度(12-bit 灰度 MP4:gray12le 对数码,管线内量化);
107
+ RGB 槽:原帧 + 录制写外部帧源。UI 侧只做 set_frame 与 FPS
108
+ 计数。
109
+ """
110
+ # 首帧后把标定送入管线(录制开始时写 calibration/head_stereo.json)
111
+ if not entry["calib_sent"]:
112
+ calib = entry["worker"].get_calibration()
113
+ if calib is not None:
114
+ self._pipeline.set_device_calibration(dev_key, calib)
115
+ entry["calib_sent"] = True
116
+
117
+ if slot_id == entry["depth_slot"]:
118
+ # 规范码值 JET(与存储 12-bit 灰度视频同构,见 depth_codec);
119
+ # 可选中值预滤(仅显示降噪,不进入存储码值)
120
+ disp = frame
121
+ if entry["smooth_k"] and entry["smooth_k"] > 1:
122
+ disp = cv2.medianBlur(frame, entry["smooth_k"])
123
+ heat = depth_to_heatmap_bgr(disp)
124
+ # 时域 EMA(按型号配置;仅可视化,存储码值不受影响)
125
+ if entry["temporal_alpha"] > 0:
126
+ if entry["heat_smoother"] is None:
127
+ entry["heat_smoother"] = DepthHeatmapSmoother(
128
+ entry["temporal_alpha"])
129
+ heat = entry["heat_smoother"].update(heat)
130
+ # 录制:原始 uint16 毫米深度 → 12-bit 灰度 MP4(gray12le
131
+ # 对数码,量化在管线内完成)
132
+ if self._pipeline.is_recording:
133
+ self._pipeline.write_depth(frame, depth_slot=slot_id)
134
+ return heat, True
135
+ if self._pipeline.is_recording:
136
+ self._pipeline.write_external_frame(
137
+ slot_id, frame.copy(),
138
+ hardware_ns=hardware_ns, imu_samples=None)
139
+ return frame, False
140
+
141
+ def close(self, entry: dict):
142
+ """停止 D435 worker(信号先断,防迟到信号进槽)。"""
143
+ worker = entry["worker"]
144
+ try:
145
+ worker.frames_ready.disconnect()
146
+ except Exception:
147
+ pass
148
+ worker.stop()
149
+ worker.deleteLater()
collector/core/database.py ADDED
@@ -0,0 +1,94 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ SQLite 数据库管理器 —— 线程安全的单例模式,管理录制历史记录表。
3
+ """
4
+
5
+ import sqlite3
6
+ import os
7
+ import threading
8
+ from config.settings import DB_PATH, DATA_DIR
9
+
10
+
11
+ class Database:
12
+ """简单的 SQLite 封装,含建表逻辑和线程本地连接。"""
13
+
14
+ _local = threading.local()
15
+
16
+ def __init__(self, db_path: str = DB_PATH):
17
+ self._db_path = db_path
18
+
19
+ @property
20
+ def conn(self) -> sqlite3.Connection:
21
+ """获取当前线程的数据库连接(自动创建)。"""
22
+ if not hasattr(self._local, "conn") or self._local.conn is None:
23
+ os.makedirs(os.path.dirname(self._db_path), exist_ok=True)
24
+ c = sqlite3.connect(self._db_path)
25
+ c.row_factory = sqlite3.Row
26
+ c.execute("PRAGMA journal_mode=WAL")
27
+ c.execute("PRAGMA foreign_keys=ON")
28
+ self._local.conn = c
29
+ return self._local.conn
30
+
31
+ def close(self):
32
+ """关闭当前线程的数据库连接。"""
33
+ if hasattr(self._local, "conn") and self._local.conn:
34
+ self._local.conn.close()
35
+ self._local.conn = None
36
+
37
+ # ── 建表 SQL ──────────────────────────────────────
38
+
39
+ SCHEMA_SQL = """
40
+ CREATE TABLE IF NOT EXISTS recording (
41
+ id TEXT PRIMARY KEY,
42
+ camera_index INTEGER NOT NULL,
43
+ camera_name TEXT NOT NULL,
44
+ file_path TEXT,
45
+ episode_index INTEGER DEFAULT 0, -- v1.1.0 池化:任务目录内的全局 episode 序号
46
+ file_size_mb REAL DEFAULT 0,
47
+ duration_sec REAL DEFAULT 0,
48
+ resolution_w INTEGER DEFAULT 0,
49
+ resolution_h INTEGER DEFAULT 0,
50
+ status TEXT NOT NULL DEFAULT 'completed', -- completed | uploaded | aborted | deleted | uploaded_deleted
51
+ started_at TEXT NOT NULL,
52
+ finished_at TEXT NOT NULL
53
+ );
54
+
55
+ CREATE INDEX IF NOT EXISTS idx_recording_camera ON recording(camera_index);
56
+ CREATE INDEX IF NOT EXISTS idx_recording_date ON recording(started_at);
57
+
58
+ CREATE TABLE IF NOT EXISTS upload_task (
59
+ id TEXT PRIMARY KEY,
60
+ session_path TEXT NOT NULL,
61
+ session_name TEXT NOT NULL,
62
+ episode_index INTEGER DEFAULT 0, -- v1.1.0 池化:全局 episode 序号
63
+ status TEXT NOT NULL DEFAULT 'pending', -- pending | uploading | completed | failed | skipped
64
+ progress REAL DEFAULT 0.0,
65
+ retry_count INTEGER DEFAULT 0,
66
+ server_url TEXT NOT NULL,
67
+ server_session_id TEXT DEFAULT '',
68
+ error_message TEXT DEFAULT '',
69
+ created_at TEXT NOT NULL,
70
+ updated_at TEXT NOT NULL
71
+ );
72
+
73
+ CREATE INDEX IF NOT EXISTS idx_upload_status ON upload_task(status);
74
+ CREATE INDEX IF NOT EXISTS idx_upload_session ON upload_task(session_path);
75
+ """
76
+
77
+ def init_schema(self):
78
+ """执行建表语句,并做既有库的列迁移(向后兼容)。"""
79
+ self.conn.executescript(self.SCHEMA_SQL)
80
+ # CREATE TABLE IF NOT EXISTS 不给既有表加列,需 ALTER 迁移
81
+ self._ensure_column("recording", "episode_index", "INTEGER DEFAULT 0")
82
+ self._ensure_column("upload_task", "episode_index", "INTEGER DEFAULT 0")
83
+ self.conn.commit()
84
+
85
+ def _ensure_column(self, table: str, column: str, decl: str):
86
+ """表缺列时 ALTER TABLE ADD COLUMN(幂等)。"""
87
+ cols = {r[1] for r in self.conn.execute(f"PRAGMA table_info({table})")}
88
+ if column not in cols:
89
+ self.conn.execute(
90
+ f"ALTER TABLE {table} ADD COLUMN {column} {decl}")
91
+
92
+
93
+ # 模块级单例
94
+ db = Database()
collector/core/depth_codec.py ADDED
@@ -0,0 +1,127 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """深度 12-bit 灰度视频编码(v1.1.2:gray12le HEVC MP4,lerobot v3 同款)。
2
+
3
+ 存储格式: 单视频轨 hevc (Rext) gray12le(12-bit 单平面灰度)——
4
+ Y 平面 = 对数深度码,量化域 [DEPTH_MIN_MM, DEPTH_MAX_MM] 对数映射到
5
+ [0, DEPTH_QMAX]。播放器直接打开显示为灰度视频;读取端 gray12le 解码
6
+ 后反量化回 uint16 毫米。文件为 .mp4(hvc1 容器标记)。
7
+
8
+ 显示对齐: 所有实时/回放深度画面一律走 depth_to_heatmap_bgr()
9
+ (毫米 → 码值 → JET),与存储数据同构——颜色与码值一一对应,
10
+ 不再有 near/far 线性色标(log 域色标对手部近场场景区分度更好)。
11
+
12
+ 精度账(实测,848×480 真实深度):
13
+ 量化自身: 每 LSB 相对误差 ln(50)/4095 ≈ 0.096%(0.1mm@0.1m,
14
+ 1mm@1m),mean≈0.34mm——低于 D435 传感器噪声 ±3-5mm
15
+ 编码 qp=6: 码误差 mean≈2.4/4095(近场深度误差 mean≈1.2mm)
16
+ 体积: ~42.6KB/帧(vs FFV1 无损 77KB/帧、PNG16 168KB/帧)
17
+ 编码速度: x265 preset=fast 实测 44fps@1280×800 纯噪声下界(真实
18
+ 深度更快;medium 仅 35fps,负载下会反压写线程)
19
+
20
+ 编码注意:
21
+ - 必须不带 -profile:v(main12 与 4:0:0 不兼容,去掉后 x265 自动
22
+ 落 hevc Rext 单平面)
23
+ - 必须 range=full(缺省有限域换算会把 600→402 之类毁掉码值)
24
+ - CRF 模式禁用(x265 感知优化把深度噪声当纹理抹掉,近场误差
25
+ 3-5×);CQP qp=6 近无损
26
+ - nvenc 不支持 12-bit(p016 族是 10-bit 数据装 16-bit 容器)
27
+ """
28
+ from __future__ import annotations
29
+
30
+ import math
31
+
32
+ import cv2
33
+ import numpy as np
34
+
35
+ # ── 量化域常量 ─────────────────────────────────────────
36
+ DEPTH_MIN_MM = 100.0 # 量化域下限(毫米);低于此值裁剪为码 0。
37
+ # D405 最近工作距 ~70mm,0.1m 覆盖手部工作域
38
+ DEPTH_MAX_MM = 5000.0 # 量化域上限(毫米);D435 有效量程 ~4m 留余量
39
+ DEPTH_QMAX = 4095 # 12-bit 码值上限
40
+ DEPTH_QP = 6 # x265 近无损 CQP 档(CRF 模式禁用,见模块注释)
41
+
42
+ _LOG_LO = math.log(DEPTH_MIN_MM)
43
+ _LOG_SPAN = math.log(DEPTH_MAX_MM) - _LOG_LO
44
+ _LOG_STEP = DEPTH_QMAX / _LOG_SPAN # 每 LSB 的对数步长
45
+
46
+
47
+ _QUANTIZE_LUT: np.ndarray | None = None # 65536 项 mm→码 LUT(惰性构建)
48
+
49
+
50
+ def _quantize_lut() -> np.ndarray:
51
+ """mm 值 0..65535 → 12-bit 码值查表(与 float64 公式逐位一致)。
52
+
53
+ 写线程每帧 3.0ms 的 np.log(1280×800 float64)降到 ~0.4ms
54
+ 查表——2026-09-03 写线程时间轴漂移残余修复(preset fast 已把
55
+ 漂移 1.74s→0.71s,LUT 再省 ~2.6ms/拍)。
56
+ """
57
+ global _QUANTIZE_LUT
58
+ if _QUANTIZE_LUT is None:
59
+ lut = np.zeros(65536, dtype="<u2")
60
+ mm = np.arange(1, 65536, dtype=np.float64)
61
+ lut[1:] = np.clip(
62
+ np.rint((np.log(mm) - _LOG_LO) * _LOG_STEP),
63
+ 0, DEPTH_QMAX).astype("<u2")
64
+ _QUANTIZE_LUT = lut
65
+ return _QUANTIZE_LUT
66
+
67
+
68
+ def quantize_depth(depth_mm: np.ndarray) -> np.ndarray:
69
+ """uint16 毫米深度 → 12-bit 对数码(uint16 存储,值域 0..4095)。
70
+
71
+ 无效值(=0)映射为码 0;低于 DEPTH_MIN_MM 裁剪为码 0,
72
+ 高于 DEPTH_MAX_MM 饱和为 DEPTH_QMAX。uint16 输入走查表快路径
73
+ (与 float64 公式逐位一致);非 uint16 输入回退原公式。
74
+ """
75
+ mm = np.asarray(depth_mm)
76
+ if mm.dtype == np.uint16:
77
+ return np.take(_quantize_lut(), mm)
78
+ mm = mm.astype(np.float64)
79
+ valid = mm > 0
80
+ if not valid.any():
81
+ return np.zeros(mm.shape, dtype="<u2")
82
+ codes = np.empty(mm.shape, dtype="<u2")
83
+ codes[~valid] = 0
84
+ codes[valid] = np.clip(
85
+ np.rint((np.log(mm[valid]) - _LOG_LO) * _LOG_STEP),
86
+ 0, DEPTH_QMAX).astype("<u2")
87
+ return codes
88
+
89
+
90
+ def dequantize_depth(codes: np.ndarray) -> np.ndarray:
91
+ """12-bit 对数码 → uint16 毫米深度(反量化,四舍五入)。"""
92
+ c = np.asarray(codes, dtype=np.float64)
93
+ return np.rint(np.exp(_LOG_LO + c / _LOG_STEP)).astype("<u2")
94
+
95
+
96
+ def codes_to_heatmap_bgr(codes: np.ndarray) -> np.ndarray:
97
+ """12-bit 深度码 → BGR 热力图(规范显示口径:码值 → JET)。
98
+
99
+ 所有实时/回放深度画面的统一颜色映射;码 0(无效/低于量化域下限)
100
+ 显示为 JET(0) 深蓝,不置黑(与旧 depth_to_heatmap 口径一致)。
101
+ """
102
+ # 注意:codes 是 uint16,直接 *255 会按 u2 回绕(码 >257 全错),
103
+ # 必须先升 int32 再乘(实测 1799*255 回绕后 c8 从 112 错成 16)。
104
+ c8 = ((np.clip(codes, 0, DEPTH_QMAX).astype(np.int32) * 255)
105
+ // DEPTH_QMAX).astype(np.uint8)
106
+ return cv2.applyColorMap(c8, cv2.COLORMAP_JET)
107
+
108
+
109
+ def depth_to_heatmap_bgr(depth_mm: np.ndarray) -> np.ndarray:
110
+ """uint16 毫米深度 → BGR 热力图(量化 → ���范 JET,与存储同构)。"""
111
+ return codes_to_heatmap_bgr(quantize_depth(depth_mm))
112
+
113
+
114
+ def depth_video_encoder_args() -> list:
115
+ """深度视频编码参数(不含 -c:v):gray12le → libx265 qp=6 近无损。
116
+
117
+ 必须不带 -profile:v(main12 与 4:0:0 不兼容,去掉后 x265 自动
118
+ 落 hevc Rext 单平面)。range=full 保 12-bit 码值不被有限域换算
119
+ 破坏(实测缺省会把 600→402)。preset=fast:实测 1280×800 纯噪声
120
+ 下界 44fps(medium 仅 35fps),机器全负载(双彩色流+GUI+USB)时
121
+ 编码器不跌破 30fps 反压写线程——2026-09-03 实机写线程时间轴
122
+ 漂移 1.7s/20s 的根因修复;精度代价实测 +0.26 码值(≈0.15mm@600mm),
123
+ 可忽略(转码对比见 session 验证)。
124
+ """
125
+ return ["-pix_fmt", "gray12le", "-tag:v", "hvc1",
126
+ "-preset", "fast",
127
+ "-x265-params", f"qp={DEPTH_QP}:range=full"]
collector/core/depth_reader.py ADDED
@@ -0,0 +1,353 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """深度数据读取(v1.1.2 12-bit 灰度 MP4 + 旧 MKV/png16 回退)。
2
+
3
+ 新格式 depth/<slot>/<slot>.mp4:单视频轨 hevc (Rext) gray12le ——
4
+ 12-bit 对数深度码(core.depth_codec 量化域),ffmpeg CLI 解
5
+ gray12le 后反量化回 uint16 毫米。
6
+ 旧格式回退:
7
+ depth/<slot>/<slot>.mkv v1.0.14 双流(流0 热力图 h264 + 流1
8
+ FFV1 gray16le 无损 uint16 毫米);
9
+ 迁移合成件 = 单流 FFV1(流0)。
10
+ depth/<slot>/*.png v1.0.13 及以前 PNG16 序列。
11
+
12
+ cv2 的 CAP_PROP_STREAM_INDEX 对 ffmpeg 后端不可靠(实测不生效),
13
+ 数据轨统一走 ffmpeg CLI 解 rawvideo。
14
+ """
15
+
16
+ from __future__ import annotations
17
+ import glob
18
+ import os
19
+ import re
20
+ import subprocess
21
+ from typing import Iterator, Optional
22
+
23
+ import cv2
24
+ import numpy as np
25
+
26
+ from core.depth_codec import codes_to_heatmap_bgr, dequantize_depth
27
+ from core.encoder_probe import list_working_ffmpegs
28
+ from core.helpers import (egodata_depth_path, egodata_depth_video_path,
29
+ episode_video_files)
30
+
31
+
32
+ def _get_ffmpeg() -> Optional[str]:
33
+ bins = list_working_ffmpegs()
34
+ return bins[0] if bins else None
35
+
36
+
37
+ def has_depth_mkv(episode_dir: str, depth_name: str) -> bool:
38
+ """该槽是否有视频深度(12-bit 灰 MP4 / 双流 MKV 任一)。"""
39
+ return os.path.isfile(egodata_depth_video_path(episode_dir, depth_name))
40
+
41
+
42
+ def iter_depth_frames(episode_dir: str, depth_name: str) -> Iterator[np.ndarray]:
43
+ """顺序产出 uint16 毫米深度帧((H, W) numpy 数组)。
44
+
45
+ 新格式:12-bit 灰度 MP4(gray12le 解码 + 反量化);
46
+ 旧格式:双流/单流 FFV1 MKV(uint16 毫米直出);
47
+ 更旧:depth/<slot>/000001.png 序列(v1.0.13 及以前)。
48
+ """
49
+ path = egodata_depth_video_path(episode_dir, depth_name)
50
+ if os.path.isfile(path):
51
+ if path.lower().endswith(".mp4"):
52
+ yield from _iter_mp4_depth_frames_path(path)
53
+ else:
54
+ yield from _iter_mkv_depth_frames_path(path)
55
+ return
56
+ yield from _iter_png16_frames(episode_dir, depth_name)
57
+
58
+
59
+ def _ffv1_stream_index(ffmpeg: str, mkv_path: str) -> Optional[int]:
60
+ """探测 MKV 里 FFV1 无损深度轨的流号(stderr 解析,无则 None)。
61
+
62
+ v1.0.14 双流 = 流0 热力图 h264 + 流1 FFV1;迁移合成件 = 单流 FFV1
63
+ (流0)。解码前探测一次,两种布局都命中。
64
+ """
65
+ try:
66
+ r = subprocess.run([ffmpeg, "-hide_banner", "-i", mkv_path],
67
+ capture_output=True, timeout=30)
68
+ except Exception:
69
+ return None
70
+ text = (r.stderr or b"").decode("utf-8", "ignore")
71
+ for line in text.splitlines():
72
+ m = re.match(r"\s*Stream #0:(\d+)(?:\([^)]*\))?:\s*Video:\s*ffv1", line)
73
+ if m:
74
+ return int(m.group(1))
75
+ return None
76
+
77
+
78
+ def _iter_mkv_depth_frames_path(mkv_path: str) -> Iterator[np.ndarray]:
79
+ """ffmpeg 流式解 MKV 的 FFV1 无损轨(gray16le → uint16 毫米帧)。"""
80
+ ffmpeg = _get_ffmpeg()
81
+ if not ffmpeg:
82
+ return
83
+ cap = cv2.VideoCapture(mkv_path)
84
+ if not cap.isOpened():
85
+ cap.release()
86
+ return
87
+ w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
88
+ h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
89
+ cap.release()
90
+ if w <= 0 or h <= 0:
91
+ return
92
+ # FFV1 轨流号探测:双流件(v1.0.14)= 1,迁移单流件 = 0;探测失败
93
+ # 回退 0:1(v1.0.14 双流默认)。
94
+ idx = _ffv1_stream_index(ffmpeg, mkv_path)
95
+ stream = f"0:{idx}" if idx is not None else "0:1"
96
+ proc = subprocess.Popen(
97
+ [ffmpeg, "-hide_banner", "-i", mkv_path,
98
+ "-map", stream, "-f", "rawvideo", "-pix_fmt", "gray16le", "-"],
99
+ stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
100
+ frame_bytes = w * h * 2
101
+ buf = b""
102
+ while proc.poll() is None:
103
+ chunk = proc.stdout.read(65536)
104
+ if not chunk:
105
+ break
106
+ buf += chunk
107
+ while len(buf) >= frame_bytes:
108
+ yield np.frombuffer(buf[:frame_bytes], "<u2").reshape(h, w).copy()
109
+ buf = buf[frame_bytes:]
110
+ proc.stdout.close()
111
+ proc.wait()
112
+
113
+
114
+ def _iter_mp4_depth_frames_path(mp4_path: str) -> Iterator[np.ndarray]:
115
+ """ffmpeg 流式解 12-bit 灰度 MP4(gray12le → 反量化 → uint16 毫米)。
116
+
117
+ 探测容器内流是否为 gray12le(v1.0.14 合封失败回退产生的 8-bit
118
+ 热力图 mp4 无数据轨,产出空帧序列);其余 HEVC 位深不回退(
119
+ 写端只有 12-bit 一条路)。
120
+ """
121
+ if not _mp4_is_gray12(mp4_path):
122
+ return
123
+ ffmpeg = _get_ffmpeg()
124
+ if not ffmpeg:
125
+ return
126
+ cap = cv2.VideoCapture(mp4_path)
127
+ if not cap.isOpened():
128
+ cap.release()
129
+ return
130
+ w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
131
+ h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
132
+ cap.release()
133
+ if w <= 0 or h <= 0:
134
+ return
135
+ proc = subprocess.Popen(
136
+ [ffmpeg, "-hide_banner", "-i", mp4_path,
137
+ "-f", "rawvideo", "-pix_fmt", "gray12le", "-"],
138
+ stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
139
+ frame_bytes = w * h * 2
140
+ buf = b""
141
+ while proc.poll() is None:
142
+ chunk = proc.stdout.read(65536)
143
+ if not chunk:
144
+ break
145
+ buf += chunk
146
+ while len(buf) >= frame_bytes:
147
+ codes = np.frombuffer(buf[:frame_bytes], "<u2").reshape(h, w)
148
+ buf = buf[frame_bytes:]
149
+ yield dequantize_depth(codes)
150
+ proc.stdout.close()
151
+ proc.wait()
152
+
153
+
154
+ class Gray12DepthVideo:
155
+ """12-bit 灰度 MP4 随机访问读取(回放用)。
156
+
157
+ cv2 直读 gray12le 的 8-bit 转换不可靠(实测与规范码值映射
158
+ mean 差 169/255),像素一律走 ffmpeg CLI 解 gray12le →
159
+ codes_to_heatmap_bgr(规范 JET,与实时显示同构)。
160
+ 小步顺序读续用当前流;随机大跳用 -ss 输入快进(从最近关键帧
161
+ 解到目标,keyint~250 最坏数百帧,亚秒级)。
162
+ """
163
+
164
+ def __init__(self, path: str, width: int, height: int,
165
+ fps: float, total: int):
166
+ self.path = path
167
+ self.width = int(width)
168
+ self.height = int(height)
169
+ self.fps = float(fps) if fps > 0 else 30.0
170
+ self.total = max(1, int(total))
171
+ self._proc: Optional[subprocess.Popen] = None
172
+ self._buf = b""
173
+ self._next_idx = 0
174
+ self._frame_bytes = self.width * self.height * 2
175
+
176
+ @classmethod
177
+ def from_path(cls, mp4_path: str) -> Optional["Gray12DepthVideo"]:
178
+ """gray12le 且可探测尺寸/帧率的返回实例,否则 None。"""
179
+ if not _mp4_is_gray12(mp4_path):
180
+ return None
181
+ w = h = 0
182
+ fps = 0.0
183
+ total = 0
184
+ try:
185
+ cap = cv2.VideoCapture(mp4_path)
186
+ if cap.isOpened():
187
+ w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
188
+ h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
189
+ fps = float(cap.get(cv2.CAP_PROP_FPS) or 0)
190
+ total = int(cap.get(cv2.CAP_PROP_FRAME_COUNT) or 0)
191
+ cap.release()
192
+ except Exception:
193
+ pass
194
+ if w > 0 and h > 0:
195
+ return cls(mp4_path, w, h, fps, total)
196
+ # cv2 探不开(灰度 12-bit 后端缺省)→ ffmpeg -i stderr 兜底
197
+ ffmpeg = _get_ffmpeg()
198
+ if not ffmpeg:
199
+ return None
200
+ try:
201
+ r = subprocess.run([ffmpeg, "-hide_banner", "-i", mp4_path],
202
+ capture_output=True, timeout=30)
203
+ except Exception:
204
+ return None
205
+ text = (r.stderr or b"").decode("utf-8", "ignore")
206
+ m = re.search(r"gray12le[^\n]*?(\d{3,5})x(\d{3,5})[^\n]*?([\d.]+)\s+fps",
207
+ text)
208
+ if not m:
209
+ return None
210
+ w, h = int(m.group(1)), int(m.group(2))
211
+ fps = float(m.group(3))
212
+ dm = re.search(r"Duration:\s*(\d+):(\d+):([\d.]+)", text)
213
+ if dm and fps > 0:
214
+ total = int((int(dm.group(1)) * 3600 + int(dm.group(2)) * 60
215
+ + float(dm.group(3))) * fps)
216
+ return cls(mp4_path, w, h, fps, total)
217
+
218
+ def read(self, idx: int) -> Optional[np.ndarray]:
219
+ """读取帧 idx(0-based)→ BGR 热力图;越界/EOF 返回 None。"""
220
+ if idx >= self.total:
221
+ return None
222
+ if self._proc is None or self._proc.poll() is not None:
223
+ self._open(idx)
224
+ while self._next_idx < idx:
225
+ if self._pull() is None:
226
+ return None
227
+ return self._pull()
228
+
229
+ def close(self) -> None:
230
+ if self._proc is not None:
231
+ try:
232
+ self._proc.kill()
233
+ except Exception:
234
+ pass
235
+ try:
236
+ self._proc.wait(timeout=3)
237
+ except Exception:
238
+ pass
239
+ self._proc = None
240
+ self._buf = b""
241
+
242
+ def _open(self, idx: int) -> None:
243
+ self.close()
244
+ ffmpeg = _get_ffmpeg()
245
+ if not ffmpeg:
246
+ return
247
+ cmd = [ffmpeg, "-hide_banner", "-nostdin"]
248
+ if idx > 0:
249
+ # -ss 输入快进:从 ss 前最近关键帧解码并丢弃;-0.5/fps 余量
250
+ # 防 PTS 舍入把 idx-1 带出来(均匀 PTS 下首输出即帧 idx)
251
+ ss = max(0.0, (idx - 0.5) / self.fps)
252
+ cmd += ["-ss", f"{ss:.6f}"]
253
+ cmd += ["-i", self.path, "-f", "rawvideo",
254
+ "-pix_fmt", "gray12le", "-"]
255
+ try:
256
+ self._proc = subprocess.Popen(
257
+ cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
258
+ except Exception:
259
+ self._proc = None
260
+ return
261
+ self._next_idx = idx
262
+
263
+ def _pull(self) -> Optional[np.ndarray]:
264
+ if self._proc is None:
265
+ return None
266
+ try:
267
+ while len(self._buf) < self._frame_bytes:
268
+ chunk = self._proc.stdout.read(65536)
269
+ if not chunk:
270
+ return None # EOF(流结束/解码失败)
271
+ self._buf += chunk
272
+ raw = self._buf[:self._frame_bytes]
273
+ self._buf = self._buf[self._frame_bytes:]
274
+ self._next_idx += 1
275
+ except Exception:
276
+ return None
277
+ codes = np.frombuffer(raw, "<u2").reshape(self.height, self.width)
278
+ return codes_to_heatmap_bgr(codes)
279
+
280
+
281
+ def _mp4_is_gray12(mp4_path: str) -> bool:
282
+ """容器内视频流是否为 gray12le(stderr 解析;探测失败保守 False)。"""
283
+ ffmpeg = _get_ffmpeg()
284
+ if not ffmpeg:
285
+ return False
286
+ try:
287
+ r = subprocess.run([ffmpeg, "-hide_banner", "-i", mp4_path],
288
+ capture_output=True, timeout=30)
289
+ except Exception:
290
+ return False
291
+ text = (r.stderr or b"").decode("utf-8", "ignore")
292
+ return bool(re.search(r"Video:\s*[^\n]*gray12le", text))
293
+
294
+
295
+ def _iter_png16_frames(episode_dir: str, depth_name: str) -> Iterator[np.ndarray]:
296
+ pattern = os.path.join(episode_dir, "depth", depth_name, "*.png")
297
+ for path in sorted(glob.glob(pattern)):
298
+ yield cv2.imread(path, cv2.IMREAD_UNCHANGED)
299
+
300
+
301
+ def read_depth_frame(episode_dir: str, depth_name: str,
302
+ frame_index: int) -> Optional[np.ndarray]:
303
+ """读取单帧(1-based 序号,与 RGB 帧号对齐);越界/缺失返回 None。
304
+
305
+ 新格式下为 O(N) 顺序解到目标帧(FFV1 无随机访问捷径);
306
+ 批量遍历请直接用 iter_depth_frames。
307
+ """
308
+ for i, frame in enumerate(iter_depth_frames(episode_dir, depth_name)):
309
+ if i == frame_index - 1:
310
+ return frame
311
+ return None
312
+
313
+
314
+ # ═══════════════════════════════════════════════════════
315
+ # 池化布局(v1.1.0):(task_dir, depth_key, episode_index) 键控
316
+ # ═══════════════════════════════════════════════════════
317
+
318
+ def pooled_depth_mkv_path(task_dir: str, depth_key: str,
319
+ episode_index: int) -> Optional[str]:
320
+ """episode N 的深度文件路径(videos/chunk-{c}/<depth_key>/episode-{f}.{ext})。
321
+
322
+ 新录制 = 12-bit 灰度 mp4(v1.1.2 起);历史 = mkv(v1.0.14 双流
323
+ FFV1 / 迁移合成单流 FFV1)。无视频深度返回 None。
324
+ """
325
+ path = episode_video_files(task_dir, episode_index).get(depth_key)
326
+ if path and os.path.splitext(path)[1].lower() in (".mp4", ".mkv"):
327
+ return path
328
+ return None
329
+
330
+
331
+ def iter_pooled_depth_frames(task_dir: str, depth_key: str,
332
+ episode_index: int) -> Iterator[np.ndarray]:
333
+ """顺序产出 episode N 该深度槽的 uint16 毫米帧;无文件产出空。"""
334
+ path = pooled_depth_mkv_path(task_dir, depth_key, episode_index)
335
+ if not path:
336
+ return
337
+ if path.lower().endswith(".mp4"):
338
+ yield from _iter_mp4_depth_frames_path(path)
339
+ else:
340
+ yield from _iter_mkv_depth_frames_path(path)
341
+
342
+
343
+ def read_pooled_depth_frame(task_dir: str, depth_key: str, episode_index: int,
344
+ frame_index: int) -> Optional[np.ndarray]:
345
+ """池化单帧读取(1-based 序号,与 RGB 帧号对齐);越界/缺失返回 None。
346
+
347
+ O(N) 顺序解到目标帧;批量遍历请直接用 iter_pooled_depth_frames。
348
+ """
349
+ for i, frame in enumerate(iter_pooled_depth_frames(
350
+ task_dir, depth_key, episode_index)):
351
+ if i == frame_index - 1:
352
+ return frame
353
+ return None
collector/core/device_detector.py ADDED
@@ -0,0 +1,497 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 设备检测模块 —— 统一枚举已连接设备(UVC + RealSense D400 + S80M + 蓝牙)。
3
+
4
+ 列表轮询走 sysfs 只读扫描(不 open 设备、不 test_read)——轮询 <5ms、
5
+ 录制中安全、被占用的相机不会从列表闪烁消失;open 测试只发生在开关打开后
6
+ (CameraWorker / D435Worker / S80M 子进程 / SensorBLEEngine 各自带重连)。
7
+
8
+ 蓝牙两个来源:bluetoothctl 已配对列表(快、只读、每轮可用)+ bleak 主动
9
+ 发现(慢 ~5s,节流缓存;手套连接中建议 set_ble_scan_suppressed(True) 抑制
10
+ 扫描,避免挤占 BLE 数据吞吐)。
11
+
12
+ 用法:
13
+ from core.device_detector import DeviceScanner, detect_devices
14
+
15
+ scanner = DeviceScanner()
16
+ scanner.scan_finished.connect(on_devices) # list[DeviceInfo]
17
+ scanner.request_scan() # 后台线程扫描
18
+ """
19
+
20
+ from __future__ import annotations
21
+ import os
22
+ import re
23
+ import subprocess
24
+ import threading
25
+ import time
26
+ from dataclasses import dataclass, field
27
+ from typing import Optional, List
28
+
29
+ from PyQt5.QtCore import QObject, pyqtSignal
30
+
31
+ from config import settings
32
+ from core.camera import list_v4l_devices, _is_sdk_device
33
+
34
+
35
+ @dataclass
36
+ class DeviceInfo:
37
+ """已连接设备的一条描述(跨线程经信号传回主线程)。"""
38
+ key: str # "uvc:{by-id前缀或索引}" | "d435:{rs serial}" | "s80m:ftdi" | "ble:{MAC}" | "usbglove:{serial}"
39
+ kind: str # "uvc" | "d435" | "s80m" | "data_ble" | "ble" | "usb_glove"
40
+ display_name: str # 设备内部命名(by-id 解码 / rs 权威名 / 蓝牙广播名 / USB 手套)
41
+ serial: str = "" # 设备序号(无则为空串)
42
+ video_index: int = -1 # UVC 设备的 /dev/videoN 索引(其它为 -1)
43
+ by_id_path: Optional[str] = None # /dev/v4l/by-id 永久路径(有则填)
44
+ backend: str = "" # 打开后端(开关打开后由 CameraWorker 决定,枚举时为空)
45
+ address: str = "" # BLE MAC 地址 / USB 手套串口路径(非 BLE 为空串)
46
+ rssi: int = 0 # BLE 信号强度(排序用)
47
+ user_name: str = "" # 用户命名(枚举后由 MainWindow 从 device_names.json 填充)
48
+
49
+ @property
50
+ def stable_key(self) -> str:
51
+ """持久化用 key:跨插拔稳定(=key)。"""
52
+ return self.key
53
+
54
+ @property
55
+ def group(self) -> str:
56
+ """面板分组: "camera" | "glove" | "other_ble"。"""
57
+ if self.kind in ("data_ble", "usb_glove"):
58
+ return "glove"
59
+ if self.kind == "ble":
60
+ return "other_ble"
61
+ return "camera"
62
+
63
+ @property
64
+ def label(self) -> str:
65
+ """列表/画面叠加显示名:用户命名优先,回落内部命名。"""
66
+ return self.user_name or self.display_name
67
+
68
+
69
+ def _parse_by_id_entry(entry: str) -> Optional[dict]:
70
+ """解析 /dev/v4l/by-id 条目名 → {prefix, serial, index}。
71
+
72
+ 形如 usb-X_Y_Z-serial-video-index0;serial 为启发式(最后一段、
73
+ 纯字母数字且 ≥8 字符),无法解析返回 None。
74
+ """
75
+ if not entry:
76
+ return None
77
+ parts = entry.rsplit("-video-index", 1)
78
+ prefix = parts[0] if len(parts) == 2 else entry
79
+ index = int(parts[1]) if (len(parts) == 2 and parts[1].isdigit()) else None
80
+ serial = ""
81
+ if prefix.startswith("usb-"):
82
+ segments = [s for s in re.split(r"[-_]", prefix[len("usb-"):]) if s]
83
+ if segments and segments[-1].isalnum() and len(segments[-1]) >= 8:
84
+ serial = segments[-1]
85
+ return {"prefix": prefix, "serial": serial, "index": index}
86
+
87
+
88
+ _REALSENSE_NAME_KW = ("realsense",)
89
+
90
+
91
+ def _is_realsense_name(name: str) -> bool:
92
+ """Windows DShow 设备名命中 RealSense 关键词(D400 全家族走
93
+ pyrealsense2 专用通道,其 UVC 节点不能当普通相机列出)。"""
94
+ nl = (name or "").lower()
95
+ return any(kw in nl for kw in _REALSENSE_NAME_KW)
96
+
97
+
98
+ def _list_uvc_devices_windows(max_index: int) -> List[DeviceInfo]:
99
+ """Windows UVC 枚举:pygrabber DirectShow 设备名。
100
+
101
+ 索引与 OpenCV CAP_DSHOW 顺序一致(两者都枚举同一个 DirectShow
102
+ 视频输入设备类),面板 video_index 可直接用于开关打开。
103
+ pygrabber 缺失时退化按索引 DShow open 探测(只验证可打开不读帧)。
104
+ 注意:Windows 无 by-id/sysfs,key 用索引(重插拔索引可能变化)。
105
+ """
106
+ infos: List[DeviceInfo] = []
107
+ names = None
108
+ try:
109
+ from pygrabber.dshow_graph import FilterGraph
110
+ names = FilterGraph().get_input_devices()
111
+ except Exception:
112
+ names = None
113
+ if names is not None:
114
+ for i, name in enumerate(names):
115
+ if i >= max_index:
116
+ break
117
+ if _is_realsense_name(name):
118
+ continue
119
+ if not (name or "").strip():
120
+ name = f"USB Camera {i}"
121
+ infos.append(DeviceInfo(
122
+ key=f"uvc:{i}",
123
+ kind="uvc",
124
+ display_name=name.strip(),
125
+ video_index=i,
126
+ ))
127
+ return infos
128
+ # 兜底:pygrabber 未安装 → DShow 索引探测(只开不读,快速释放)
129
+ import cv2
130
+ for i in range(max_index):
131
+ try:
132
+ cap = cv2.VideoCapture(i, cv2.CAP_DSHOW)
133
+ ok = cap.isOpened()
134
+ cap.release()
135
+ except Exception:
136
+ ok = False
137
+ if ok:
138
+ infos.append(DeviceInfo(
139
+ key=f"uvc:{i}",
140
+ kind="uvc",
141
+ display_name=f"USB Camera {i}",
142
+ video_index=i,
143
+ ))
144
+ return infos
145
+
146
+
147
+ def _list_uvc_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
148
+ """UVC 网络摄像头(排除 RealSense UVC 节点与 FTDI SDK 设备)。
149
+
150
+ Linux: sysfs 只读枚举(/dev/v4l/by-id 分组,轮询安全)。
151
+ Windows: pygrabber DirectShow 枚举(见 _list_uvc_devices_windows)。
152
+ """
153
+ if os.name == "nt":
154
+ return _list_uvc_devices_windows(max_index)
155
+ infos: List[DeviceInfo] = []
156
+ for d in list_v4l_devices(max_index):
157
+ if d.get("is_sdk"):
158
+ continue
159
+ if d.get("is_realsense"):
160
+ continue
161
+ # key 用 by-id 前缀(跨插拔稳定);无 by-id 时退化为索引
162
+ prefix = str(d["video_index"])
163
+ if d.get("by_id_path"):
164
+ prefix = os.path.basename(d["by_id_path"]).rsplit("-video-index", 1)[0]
165
+ infos.append(DeviceInfo(
166
+ key=f"uvc:{prefix}",
167
+ kind="uvc",
168
+ display_name=d["name"] or f"USB Camera {d['video_index']}",
169
+ serial=d.get("serial", ""),
170
+ video_index=d["video_index"],
171
+ by_id_path=d.get("by_id_path"),
172
+ ))
173
+ return infos
174
+
175
+
176
+ def _list_d435_devices() -> List[DeviceInfo]:
177
+ """pyrealsense2 权威枚举(名称 + 序列号);无设备/未安装返回 []。"""
178
+ try:
179
+ import pyrealsense2 as rs
180
+ ctx = rs.context()
181
+ infos: List[DeviceInfo] = []
182
+ for dev in ctx.query_devices():
183
+ try:
184
+ if "D400" not in dev.get_info(rs.camera_info.product_line):
185
+ continue
186
+ name = dev.get_info(rs.camera_info.name)
187
+ serial = dev.get_info(rs.camera_info.serial_number)
188
+ infos.append(DeviceInfo(
189
+ key=f"d435:{serial}",
190
+ kind="d435",
191
+ display_name=name or "Intel RealSense D435",
192
+ serial=serial,
193
+ ))
194
+ except Exception:
195
+ continue
196
+ return infos
197
+ except Exception:
198
+ return []
199
+
200
+
201
+ def _list_s80m_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
202
+ """FTDI 命中一次即返回单条 S80M 条目(SDK 配置写死 video0/video2)。
203
+
204
+ 序号尝试从 by-id 条目解析(FT602 通常无序号 → 空串)。
205
+ """
206
+ for i in range(max_index):
207
+ if not _is_sdk_device(i):
208
+ continue
209
+ serial, by_id_path = "", None
210
+ by_id_dir = "/dev/v4l/by-id"
211
+ if os.path.isdir(by_id_dir):
212
+ for entry in sorted(os.listdir(by_id_dir)):
213
+ p = os.path.join(by_id_dir, entry)
214
+ try:
215
+ if os.readlink(p) == f"../../video{i}":
216
+ parsed = _parse_by_id_entry(entry)
217
+ if parsed:
218
+ serial = parsed.get("serial", "")
219
+ by_id_path = p
220
+ break
221
+ except OSError:
222
+ continue
223
+ return [DeviceInfo(
224
+ key="s80m:ftdi",
225
+ kind="s80m",
226
+ display_name="FaysSense S80M",
227
+ serial=serial,
228
+ video_index=i,
229
+ by_id_path=by_id_path,
230
+ )]
231
+ return []
232
+
233
+
234
+ # ── 蓝牙枚举 ──────────────────────────────────────────
235
+ BLE_DISCOVERY_INTERVAL_S = 20.0 # bleak 主动发现节流间隔
236
+ BLE_DISCOVERY_TIMEOUT_S = 5.0 # 单次 discover 超时
237
+ _ble_discovery_cache = {"ts": 0.0, "devices": []} # [(name, mac, rssi)]
238
+ _ble_discovery_lock = threading.Lock()
239
+ _ble_scan_suppressed = False # 手套连接中由 MainWindow 置 True
240
+
241
+
242
+ def set_ble_scan_suppressed(on: bool):
243
+ """抑制 bleak 主动发现(手套连接中防扫描挤占数据吞吐)。
244
+
245
+ 只影响主动发现;bluetoothctl 已配对列表是只读系统调用,不受影响。
246
+ """
247
+ global _ble_scan_suppressed
248
+ _ble_scan_suppressed = bool(on)
249
+
250
+
251
+ def _mac_norm(mac: str) -> str:
252
+ """MAC 归一化:大写、冒号分隔(bluetoothctl 与 bleak 格式统一)。"""
253
+ return re.sub(r"[^0-9A-Fa-f]", ":", mac or "").replace("::", ":").strip(":").upper()
254
+
255
+
256
+ def _ble_discover() -> List[tuple]:
257
+ """bleak 主动发现(节流缓存;线程内调用,5s 阻塞)。"""
258
+ now = time.monotonic()
259
+ with _ble_discovery_lock:
260
+ if now - _ble_discovery_cache["ts"] <= BLE_DISCOVERY_INTERVAL_S:
261
+ return list(_ble_discovery_cache["devices"])
262
+ _ble_discovery_cache["ts"] = now
263
+ result: List[tuple] = []
264
+ try:
265
+ import asyncio
266
+ from bleak import BleakScanner
267
+ loop = asyncio.new_event_loop()
268
+ try:
269
+ found = loop.run_until_complete(
270
+ BleakScanner.discover(timeout=BLE_DISCOVERY_TIMEOUT_S))
271
+ for d in found:
272
+ rssi = None
273
+ try:
274
+ rssi = d.rssi
275
+ except Exception:
276
+ rssi = getattr(getattr(d, "details", None), "rssi", None)
277
+ result.append((d.name or "", _mac_norm(d.address),
278
+ int(rssi) if rssi is not None else -999))
279
+ finally:
280
+ loop.close()
281
+ except Exception:
282
+ result = []
283
+ with _ble_discovery_lock:
284
+ _ble_discovery_cache["devices"] = result
285
+ return list(result)
286
+
287
+
288
+ def _bluetoothctl_paired() -> dict:
289
+ """bluetoothctl devices 已配对列表(只读;不可用返回 {})。"""
290
+ paired: dict = {}
291
+ try:
292
+ out = subprocess.run(["bluetoothctl", "devices"], capture_output=True,
293
+ text=True, timeout=5)
294
+ for line in out.stdout.splitlines():
295
+ m = re.match(r"Device\s+(\S+)\s+(.+)$", line.strip())
296
+ if m:
297
+ paired[_mac_norm(m.group(1))] = m.group(2).strip()
298
+ except Exception:
299
+ pass
300
+ return paired
301
+
302
+
303
+ _GLOVE_NAMES = {"l", "r", "left", "right", "l_glove", "r_glove",
304
+ "left_glove", "right_glove"}
305
+
306
+
307
+ def _is_glove_name(name: str) -> bool:
308
+ """广播名判手套:历史 "Matrix…" 或单字母 L/R(现役手套固件)。"""
309
+ n = (name or "").strip().lower()
310
+ return "matrix" in n or n in _GLOVE_NAMES
311
+
312
+
313
+ def _list_ble_devices() -> List[DeviceInfo]:
314
+ """蓝牙设备:bluetoothctl 已配对 + bleak 主动发现,按 MAC 去重合并。
315
+
316
+ 广播名含 "Matrix"/"L"/"R" 判为手套(data_ble),其余为普通 BLE
317
+ (other_ble);device_names.json 里已绑定 sensor 列的 MAC 永远按手套
318
+ 对待(连过一次即持久化,改名/空名不丢)。手套连接中
319
+ (_ble_scan_suppressed)跳过主动发现,防扫描挤占数据吞吐。
320
+ """
321
+ merged: dict = {}
322
+ for mac, name in _bluetoothctl_paired().items():
323
+ merged[_mac_norm(mac)] = {"name": name, "rssi": 0}
324
+ if not _ble_scan_suppressed:
325
+ for name, mac, rssi in _ble_discover():
326
+ mac = _mac_norm(mac)
327
+ if mac in merged:
328
+ # 配对名优先;无配对名时用广播名
329
+ if not merged[mac]["name"]:
330
+ merged[mac]["name"] = name
331
+ else:
332
+ merged[mac] = {"name": name, "rssi": rssi}
333
+ infos: List[DeviceInfo] = []
334
+ for mac, d in merged.items():
335
+ name = d["name"] or "BLE Device"
336
+ is_glove = _is_glove_name(name) or bool(
337
+ settings.device_sensor_role(f"ble:{mac}"))
338
+ infos.append(DeviceInfo(
339
+ key=f"ble:{mac}",
340
+ kind="data_ble" if is_glove else "ble",
341
+ display_name=name,
342
+ serial=mac,
343
+ address=mac,
344
+ rssi=d["rssi"],
345
+ ))
346
+ return infos
347
+
348
+
349
+ def detect_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
350
+ """五段枚举(UVC + D435 + S80M + BLE + USB 手套),各自容错,整体不崩。"""
351
+ devices: List[DeviceInfo] = []
352
+ try:
353
+ devices += _list_uvc_devices(max_index)
354
+ except Exception:
355
+ pass
356
+ try:
357
+ devices += _list_d435_devices()
358
+ except Exception:
359
+ pass
360
+ try:
361
+ devices += _list_s80m_devices(max_index)
362
+ except Exception:
363
+ pass
364
+ try:
365
+ devices += _list_ble_devices()
366
+ except Exception:
367
+ pass
368
+ try:
369
+ devices += _list_usb_glove_devices()
370
+ except Exception:
371
+ pass
372
+ return devices
373
+
374
+
375
+ # ── USB (Type-C) 手套 ──────────────────────────────────
376
+
377
+ _GLOVE_USB_VID = 0x0483
378
+ _GLOVE_USB_PID = 0x5740
379
+ _GLOVE_SIDE_NAMES = {"left_glove": "USB 手套·左手",
380
+ "right_glove": "USB 手套·右手"}
381
+ _glove_side_cache: Optional[dict] = None
382
+ _glove_side_cache_time = 0.0
383
+
384
+
385
+ def _glove_side_by_serial() -> dict:
386
+ """读工具包 glove_devices.json → {usb_serial小写: "left_glove"/"right_glove"}。
387
+
388
+ 优先项目根下同级的 stouch_glove_toolkit* 目录,其次项目根自身;
389
+ 文件缺失/解析失败返回 {}(左右手仍可经首连分配后持久化)。
390
+ 除 left/right 的 usb_serial 外,还合并可选的 extra_usb_serials
391
+ 映射({序列号: "left_glove"/"right_glove"})——支持同一手侧多只
392
+ 手套(换机/固件序列号变更后旧号仍能认出来)。
393
+ 结果缓存 10s,避免 2s 轮询反复读盘。
394
+ """
395
+ global _glove_side_cache, _glove_side_cache_time
396
+ now = time.monotonic()
397
+ if _glove_side_cache is not None and now - _glove_side_cache_time < 10.0:
398
+ return _glove_side_cache
399
+ result: dict = {}
400
+ root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
401
+ candidates = [os.path.join(root, "glove_devices.json")]
402
+ try:
403
+ candidates += sorted(
404
+ os.path.join(root, name, "glove_devices.json")
405
+ for name in os.listdir(root)
406
+ if name.lower().startswith("stouch_glove_toolkit")
407
+ )
408
+ except OSError:
409
+ pass
410
+ import json
411
+ for path in candidates:
412
+ try:
413
+ with open(path, "r", encoding="utf-8") as f:
414
+ data = json.load(f)
415
+ except (OSError, ValueError):
416
+ continue
417
+ for side, role in (("left", "left_glove"), ("right", "right_glove")):
418
+ serial = (data.get(side) or {}).get("usb_serial", "")
419
+ if serial:
420
+ result[str(serial).strip().lower()] = role
421
+ for serial, role in (data.get("extra_usb_serials") or {}).items():
422
+ if role in ("left_glove", "right_glove") and serial:
423
+ result.setdefault(str(serial).strip().lower(), role)
424
+ if result:
425
+ break
426
+ _glove_side_cache = result
427
+ _glove_side_cache_time = now
428
+ return result
429
+
430
+
431
+ def usb_glove_prefer_side(serial: str) -> str:
432
+ """USB 手套序列号 → 期望传感器列名(未知返回空串)。"""
433
+ return _glove_side_by_serial().get((serial or "").strip().lower(), "")
434
+
435
+
436
+ def _list_usb_glove_devices() -> List[DeviceInfo]:
437
+ """枚举 STM32 USB CDC 手套(0483:5740,串口工具按 VID/PID 过滤)。"""
438
+ try:
439
+ import serial.tools.list_ports
440
+ except ImportError:
441
+ return []
442
+ side_map = _glove_side_by_serial()
443
+ infos: List[DeviceInfo] = []
444
+ for port in serial.tools.list_ports.comports():
445
+ if port.vid != _GLOVE_USB_VID or port.pid != _GLOVE_USB_PID:
446
+ continue
447
+ serial = (port.serial_number or "").strip()
448
+ key = f"usbglove:{serial}" if serial else f"usbglove:{port.device}"
449
+ side = side_map.get(serial.lower(), "")
450
+ infos.append(DeviceInfo(
451
+ key=key,
452
+ kind="usb_glove",
453
+ display_name=_GLOVE_SIDE_NAMES.get(side, "USB 手套"),
454
+ serial=serial,
455
+ address=port.device,
456
+ ))
457
+ return infos
458
+
459
+
460
+ class DeviceScanner(QObject):
461
+ """后台线程扫描设备(sysfs 只读),经排队信号回主线程。
462
+
463
+ 带 _busy 守卫:扫描未完成时 request_scan 直接返回,防止轮询堆积。
464
+ """
465
+
466
+ scan_finished = pyqtSignal(list) # list[DeviceInfo]
467
+
468
+ def __init__(self, parent=None, max_index: int = None):
469
+ super().__init__(parent)
470
+ self._max_index = (max_index if max_index is not None
471
+ else settings.DEVICE_SCAN_MAX_INDEX)
472
+ self._busy = False
473
+ self._busy_lock = threading.Lock()
474
+ self._stop = False
475
+
476
+ def request_scan(self):
477
+ """请求一次扫描;进行中则忽略(防轮询堆积)。"""
478
+ with self._busy_lock:
479
+ if self._busy or self._stop:
480
+ return
481
+ self._busy = True
482
+ threading.Thread(target=self._run_scan, daemon=True,
483
+ name="device-scan").start()
484
+
485
+ def _run_scan(self):
486
+ try:
487
+ devices = detect_devices(self._max_index)
488
+ if not self._stop:
489
+ self.scan_finished.emit(devices)
490
+ finally:
491
+ with self._busy_lock:
492
+ self._busy = False
493
+
494
+ def stop(self):
495
+ """停止接受新扫描(在途扫描完成后自然退出)。"""
496
+ with self._busy_lock:
497
+ self._stop = True
collector/core/device_manager.py ADDED
@@ -0,0 +1,148 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 统一设备 worker 注册表 + 面板开关分派 + 录制元数据口径(纯 Python,
3
+ 零 Qt 依赖)。
4
+
5
+ 具体开启/关闭动作(建槽、弹窗、widget 接线)留在 UI 侧回调,本模块
6
+ 只做核心口径:
7
+ - DeviceManager 注册表容器:条目骨架构造 + 查询 + 元数据/抽帧状态
8
+ - dispatch_toggle 面板开关分派(录制锁双保险 + kind 路由)
9
+ - build_device_meta / reset_s80m_record_state / teardown_all
10
+ 注册表级纯口径(离线测试直接注入假条目即可)
11
+
12
+ 注册表条目形状(与旧主窗口 _workers 口径一致):
13
+ kind ∈ {"uvc", "d435", "s80m", "data_ble", "ble"}
14
+ d435/s80m 条目由各自 manager 的 new_entry 构造
15
+ (core.d435_manager / core.s80m_manager),uvc/ble/glove 骨架由本类构造。
16
+ """
17
+
18
+ from __future__ import annotations
19
+
20
+ from config import settings
21
+
22
+
23
+ class DeviceManager:
24
+ """统一设备 worker 注册表(多路并发核心)。
25
+
26
+ entries: dev_key → entry dict(主窗口 _workers 直接引用同一 dict,
27
+ 离线测试注入假条目沿用同一形状)。UI 侧负责把条目塞进注册表,
28
+ 本类只提供骨架构造、查询与录制口径。
29
+ """
30
+
31
+ def __init__(self):
32
+ self.entries: dict = {}
33
+
34
+ # ── 条目骨架 ──
35
+ @staticmethod
36
+ def uvc_entry(slots, label) -> dict:
37
+ """UVC 条目(无专属字段,槽位即状态)。"""
38
+ return {"kind": "uvc", "slots": list(slots), "label": label}
39
+
40
+ @staticmethod
41
+ def ble_entry(slot: str, label: str) -> dict:
42
+ """无数据蓝牙(耳机类)占位条目。"""
43
+ return {"kind": "ble", "slots": [slot], "label": label}
44
+
45
+ @staticmethod
46
+ def glove_entry(slot: str, role: str, glove_widget, label: str) -> dict:
47
+ """数据手套条目:传感器列名 + 画面 widget(close 时停 BLE/撤画面)。"""
48
+ return {"kind": "data_ble", "slots": [slot],
49
+ "sensor_column": role, "glove": glove_widget,
50
+ "label": label}
51
+
52
+ # ── 查询 ──
53
+ def get(self, dev_key: str) -> dict | None:
54
+ return self.entries.get(dev_key)
55
+
56
+ def has_kind(self, kind: str) -> bool:
57
+ return any(e["kind"] == kind for e in self.entries.values())
58
+
59
+ def has_serial(self, kind: str, serial: str) -> bool:
60
+ """按 serial 查重(d435 同机重复开关检测)。"""
61
+ return any(e["kind"] == kind and e.get("serial") == serial
62
+ for e in self.entries.values())
63
+
64
+ # ── 录制口径 ──
65
+ def build_device_meta(self) -> list:
66
+ """按注册表构建录制设备信息(写入 meta 的 devices 段)。"""
67
+ return build_device_meta(self.entries)
68
+
69
+ def reset_s80m_record_state(self):
70
+ """录制起止时重置 50→30 抽帧状态(桶号 + 待写 IMU 缓冲)。"""
71
+ reset_s80m_record_state(self.entries)
72
+
73
+ def teardown_all(self, close_fns: dict) -> None:
74
+ """关闭全部已开启设备(遍历注册表分类清理,未知 kind 兜底弹出)。"""
75
+ teardown_all(self.entries, close_fns)
76
+
77
+
78
+ def build_device_meta(entries: dict) -> list:
79
+ """按注册表构建录制设备信息(写入 meta 的 devices 段)。
80
+
81
+ 手套:无画面槽位,附 parquet 传感器列名;
82
+ 无数据蓝牙(ble)不进 devices 段。
83
+ """
84
+ meta = []
85
+ for key, e in entries.items():
86
+ if e["kind"] == "data_ble":
87
+ meta.append({
88
+ "key": key, "kind": e["kind"],
89
+ "name": settings.device_name(key) or e.get("label", ""),
90
+ "slots": [],
91
+ "sensor_column": e.get("sensor_column", "")})
92
+ continue
93
+ if e["kind"] not in ("uvc", "d435", "s80m"):
94
+ continue
95
+ d = {"key": key, "kind": e["kind"],
96
+ "name": settings.device_name(key) or e.get("label", ""),
97
+ "slots": list(e.get("slots", []))}
98
+ if e.get("serial"):
99
+ d["serial"] = str(e["serial"])
100
+ meta.append(d)
101
+ return meta
102
+
103
+
104
+ def reset_s80m_record_state(entries: dict) -> None:
105
+ """录制起止时重置 50→30 抽帧状态(桶号 + 待写 IMU 缓冲 + 空桶统计)。"""
106
+ for e in entries.values():
107
+ if e["kind"] == "s80m":
108
+ e["last_bucket"] = {}
109
+ e["pending_imu"] = []
110
+ e["drop_watch"] = {"last_mono_ns": None, "dropped": 0,
111
+ "elapsed": 0, "alerted": False}
112
+
113
+
114
+ def teardown_all(entries: dict, close_fns: dict) -> None:
115
+ """关闭全部已开启设备(遍历注册表分类清理,未知 kind 兜底弹出)。
116
+
117
+ s80m/d435 的关闭回调会自行 unregister(pop),故遍历键副本。
118
+ """
119
+ for key in list(entries):
120
+ entry = entries.get(key)
121
+ if not entry:
122
+ continue
123
+ fn = (close_fns or {}).get(entry["kind"])
124
+ if fn:
125
+ fn(key)
126
+ else:
127
+ entries.pop(key, None)
128
+ entries.clear()
129
+
130
+
131
+ def dispatch_toggle(dev, on: bool, is_recording: bool,
132
+ open_fns: dict, close_fns: dict) -> bool:
133
+ """面板开关分派口径(录制锁双保险 + kind 路由)。
134
+
135
+ open_fns / close_fns: kind → 可调用(具体动作在 UI 侧,
136
+ open 接收 dev 并返回是否成功,close 接收 dev_key)。
137
+ 返回 opened:打开成功 True;录制中/打开失败/缺回调 False
138
+ (UI 侧按 on 区分「回退勾选」与「关闭路径」)。
139
+ """
140
+ if on:
141
+ if is_recording:
142
+ return False # 面板已锁死,双保险:UI 侧回退勾选
143
+ fn = (open_fns or {}).get(dev.kind)
144
+ return bool(fn(dev)) if fn else False
145
+ fn = (close_fns or {}).get(dev.kind)
146
+ if fn:
147
+ fn(dev.key)
148
+ return False
collector/core/device_naming.py ADDED
@@ -0,0 +1,68 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 设备命名辅助 —— 型号短名、槽名清洗、曝光基线归一化与槽名分配(零 Qt 依赖)。
3
+
4
+ 供主窗口设备接入链路(D435 槽名分配 / 曝光对话框基线)与离线测试共用同一套口径:
5
+ - realsense_short 型号名 → 短名("RealSense D405" → "D405")
6
+ - slot_base 用户命名 → 槽名前缀(文件系统安全 + "depth" 后缀约定不破坏)
7
+ - normalize_original 各种来源的「最一开始」曝光基线 → (auto, value) 二元组
8
+ - allocate_slot_names RealSense RGB/深度槽名对分配(同前缀多台编号追加)
9
+ """
10
+
11
+ from __future__ import annotations
12
+
13
+ import re
14
+
15
+
16
+ def realsense_short(model_name: str) -> str:
17
+ """型号名 → 短名("RealSense D405" → "D405";无型号段回落 "RealSense")。"""
18
+ m = re.search(r"D\d{3}", model_name or "")
19
+ return m.group(0) if m else "RealSense"
20
+
21
+
22
+ def slot_base(name: str, fallback: str) -> str:
23
+ """用户命名 → 槽名前缀(仅保留字母/数字/下划线/中文;空或全非法回落)。
24
+
25
+ GUI 命名可含空格、emoji 等,槽名要作为文件夹名与 info.json 键,
26
+ 须保证文件系统安全且不破坏 "depth" 后缀约定。
27
+ """
28
+ clean = re.sub(r"[^0-9A-Za-z_一-鿿]+", "_",
29
+ (name or "").strip()).strip("_")
30
+ return clean or fallback
31
+
32
+
33
+ def normalize_original(orig) -> tuple | None:
34
+ """「最一开始」曝光基线归一化为 (auto, value) 元组;无/坏数据返回 None。
35
+
36
+ settings.device_original 返回 dict,worker.original_exposure 返回
37
+ tuple,S80M entry 存 tuple —— 统一在此收口成对话框要的二元组。
38
+ """
39
+ if isinstance(orig, dict) and "auto" in orig and "value" in orig:
40
+ return (bool(orig["auto"]), float(orig["value"]))
41
+ if (isinstance(orig, (tuple, list)) and len(orig) == 2
42
+ and orig[0] is not None and orig[1] is not None):
43
+ return (bool(orig[0]), float(orig[1]))
44
+ return None
45
+
46
+
47
+ def allocate_slot_names(user_name: str, model_name: str,
48
+ occupied_rgb_slots: list[str]) -> tuple[str, str]:
49
+ """分配 RealSense RGB/深度槽名对,同前缀多台编号追加。
50
+
51
+ RGB 槽 = 前缀 + "_rgb";深度槽 = 前缀本身若已以 "_depth" 结尾
52
+ (用户命名 "D405_depth" → depth 文件夹恰为用户名原样),否则补
53
+ "_depth"(保持回放/录制链路 "depth" 后缀约定)。同前缀多台编号
54
+ 追加在槽名之后(d435_depth_2 经典式)。首台未命名 D435 仍得
55
+ d435_rgb/d435_depth 经典名。
56
+
57
+ Args:
58
+ user_name: GUI 用户命名(device_names.json,可空回落型号)
59
+ model_name: RealSense 显示名(如 "RealSense D405")
60
+ occupied_rgb_slots: 已占用 RGB 槽名列表(用于同前缀计数编号)
61
+ """
62
+ base = slot_base(user_name, realsense_short(model_name).lower())
63
+ n = sum(1 for s in occupied_rgb_slots if s.startswith(f"{base}_rgb"))
64
+ rgb_slot = f"{base}_rgb" if n == 0 else f"{base}_rgb_{n + 1}"
65
+ depth_slot = base if base.endswith("_depth") else f"{base}_depth"
66
+ if n > 0:
67
+ depth_slot = f"{depth_slot}_{n + 1}"
68
+ return rgb_slot, depth_slot
collector/core/egodata_writer.py ADDED
@@ -0,0 +1,1108 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ EgoData 格式录制器 —— 多模态机器人遥操作数据采集。
3
+
4
+ 目录布局(v1.1.0 任务级池化,LeRobot v3 命名,见 docs/data.md):
5
+ <output_dir>/<task_tag>/
6
+ ├── videos/chunk-{c:03d}/<image_key>/episode-{f:03d}.{ext}
7
+ │ # 每 episode 每流一文件;RGB=mp4;深度=12-bit
8
+ │ # 灰度 MP4(hevc Rext gray12le,对数深度码,
9
+ │ # 见 core/depth_codec.py;x265 无 12-bit 时
10
+ │ # 回落 FFV1 gray16le 无损 MKV)
11
+ ├── data/chunk-{c:03d}/episode-{f:03d}.parquet
12
+ │ # 每 episode 一个,zstd,稀疏列(有数据才写)
13
+ └── meta/
14
+ ├── info.json # 任务级 schema/fps/版本/路径模板/calibration
15
+ ├── stats.json # 任务级全局统计,每块 {count, mean, std,
16
+ │ # min, max}——count 可反推 sum/sum_sq,
17
+ │ # 文件自身即增量累加器(v1.1.1,无边车)
18
+ ├── tasks.jsonl
19
+ └── episodes/chunk-{c:03d}/episode-{f:03d}.parquet
20
+ # 每 episode 一个文件(单行),与 data/videos
21
+ # 同编号;录制结束原子写,重录覆盖
22
+ """
23
+
24
+ from __future__ import annotations
25
+ import os
26
+ import json
27
+ import time
28
+ import subprocess
29
+ from typing import Optional, Dict, List
30
+
31
+ import cv2
32
+ import numpy as np
33
+ import pyarrow as pa
34
+ import pyarrow.parquet as pq
35
+ from PyQt5.QtCore import QObject, pyqtSignal, QMutex, QMutexLocker
36
+
37
+ from config import settings
38
+ from core.encoder_probe import (select_encoder, list_working_ffmpegs,
39
+ find_depth_12bit_ffmpeg)
40
+ from core.depth_codec import quantize_depth, depth_video_encoder_args
41
+ from core.calibration import StereoCalibration
42
+ from core.helpers import (
43
+ task_dir_of, episode_chunk_file, POOLED_CHUNK_SIZE,
44
+ pooled_video_path, pooled_video_dir,
45
+ pooled_data_parquet_path, pooled_episodes_path,
46
+ _legacy_episodes_shard_path,
47
+ pooled_info_path, pooled_stats_path, pooled_tasks_jsonl_path,
48
+ load_stats_acc, merge_stat_block, acc_to_stats_json, recalc_stats,
49
+ list_task_episodes, next_pooled_episode_index,
50
+ mark_recycled_episode, clear_recycled_episode,
51
+ )
52
+
53
+ # 跨进程文件锁(POSIX);Windows 无 fcntl 时退化为无锁(单机假设,
54
+ # 多机共享任务目录须为 POSIX,见 docs/data.md 并发说明)
55
+ try:
56
+ import fcntl as _fcntl
57
+ except ImportError: # pragma: no cover
58
+ _fcntl = None
59
+
60
+
61
+ # ── 全局 ffmpeg ────────────────────────────────────────
62
+
63
+ def _get_ffmpeg() -> Optional[str]:
64
+ """解析可用的 ffmpeg 可执行文件(v1.0.9 起委托 core.encoder_probe)。
65
+
66
+ 优先使用 imageio_ffmpeg 自带的静态二进制(无动态库依赖,最可靠);
67
+ PATH 中的 ffmpeg(如 conda 版)必须通过 -version 自检才采用 ——
68
+ conda 的 ffmpeg 曾因 libopenvino 符号缺失启动即崩,导致视频静默丢失。
69
+ 候选列表进程内缓存(encoder_probe.list_working_ffmpegs)。
70
+ """
71
+ bins = list_working_ffmpegs()
72
+ return bins[0] if bins else None
73
+
74
+
75
+ # ── Parquet 模式 (LeRobot v3 兼容) ──────────────────────
76
+
77
+ # meta/episodes 分片行:既有 5 列 + v1.1.0 新增(JSON 序列化字符串列)
78
+ _EPISODE_COLUMNS = [
79
+ ("episode_index", pa.int64(), 0),
80
+ ("task_index", pa.int64(), 0),
81
+ ("start_frame_index", pa.int64(), 0),
82
+ ("end_frame_index", pa.int64(), 0),
83
+ ("length", pa.int64(), 0),
84
+ ("created_at", pa.float64(), 0.0),
85
+ ("duration_sec", pa.float64(), 0.0),
86
+ ("drop_stats", pa.string(), "{}"),
87
+ ("video_codec", pa.string(), "{}"),
88
+ ("calibration", pa.string(), "{}"),
89
+ ]
90
+ _EPISODE_SCHEMA = pa.schema([(n, t) for n, t, _d in _EPISODE_COLUMNS])
91
+
92
+
93
+ def _episode_rows_table(rows: List[dict]) -> pa.Table:
94
+ """episode 行列表 → 分片表(缺失键填默认值)。"""
95
+ arrs = {}
96
+ for name, typ, default in _EPISODE_COLUMNS:
97
+ arrs[name] = pa.array([r.get(name, default) for r in rows], type=typ)
98
+ return pa.table(arrs, schema=_EPISODE_SCHEMA)
99
+
100
+
101
+ def _read_episode_rows(path: str) -> List[dict]:
102
+ """读 episodes 文件为行 dict 列表;文件缺失/损坏返回 []。"""
103
+ if not os.path.isfile(path) or os.path.getsize(path) == 0:
104
+ return []
105
+ try:
106
+ return pq.read_table(path).to_pylist()
107
+ except Exception:
108
+ return []
109
+
110
+
111
+ # ═══════════════════════════════════════════════════════
112
+ # EgoData 录制器
113
+ # ══════════════════════════════════���════════════════════
114
+
115
+ class EgoDataWriter(QObject):
116
+ """EgoData 格式录制器 —— 任务级池化布局(v1.1.0)。
117
+
118
+ 用法:
119
+ w = EgoDataWriter()
120
+ w.start_episode(output_dir, cameras, fps=30,
121
+ sensors=["right_glove", "left_glove"],
122
+ task_name="grasp_cup")
123
+ w.write_video_frame("head_left_rgb", bgr_frame)
124
+ w.write_depth_frame(frame_index, depth_uint16)
125
+ w.write_frame_row(frame_index, timestamp_s,
126
+ sensors={"right_glove": data})
127
+ w.end_episode()
128
+ """
129
+
130
+ episode_started = pyqtSignal(str)
131
+ episode_finished = pyqtSignal(str)
132
+ error_occurred = pyqtSignal(str)
133
+ log_occurred = pyqtSignal(str) # 编码器探测等录制期日志(v1.0.9)
134
+
135
+ def __init__(self):
136
+ super().__init__()
137
+ self._task_dir: str = ""
138
+ self._episode_index: int = 0
139
+ self._ffmpeg_procs: Dict[str, subprocess.Popen] = {}
140
+ self._camera_dims: Dict[str, tuple] = {}
141
+ self._camera_fps: Dict[str, float] = {}
142
+ self._fps: float = 30.0
143
+ self._frame_count = 0
144
+ self._max_frame_idx = 0 # 单路最大帧号(用于时长计算)
145
+ self._episode_start_s: float = 0.0
146
+ self._episode_created_at: float = 0.0
147
+ self._rows: List[dict] = []
148
+ self._mutex = QMutex()
149
+ self._sensor_names: List[str] = []
150
+ self._sensor_dim: int = settings.SENSOR_DIM
151
+ self._stats: Dict[str, dict] = {}
152
+ self._last_task: str = ""
153
+ self._device_ids: List[str] = []
154
+ self._devices: List[dict] = []
155
+ self._calibrations: Dict[str, object] = {}
156
+ self._calib_dict: dict = {} # 本 episode 标定(info.json/episodes 行)
157
+ # 稀疏列跟踪:只落盘本 episode 实际有数据的列
158
+ self._present_sensors: set = set()
159
+ self._present_imu: bool = False
160
+ self._present_glove_imu: set = set() # USB 手套 IMU 稀疏列(按传感器名)
161
+ # 深度多槽位(D435 第 n 台 = d435_depth[_n];S80M 传统路径兜底
162
+ # settings.CAMERA_DEPTH):槽位键 → depth image_key
163
+ self._depth_slots: Dict[str, str] = {}
164
+ self._depth_dirs: Dict[str, str] = {} # 每槽深度 image_key 目录(惰性创建)
165
+ self._depth_final_ext: Dict[str, str] = {} # 每槽最终扩展名(12-bit 灰 mp4;回落 mkv)
166
+ self._depth_12bit: Dict[str, bool] = {} # 每槽是否 12-bit 灰度编码(否则 FFV1 无损)
167
+ self._depth_proc_started: Dict[str, bool] = {} # 每槽深度 ffmpeg 是否已启动
168
+ self._depth_enabled: bool = False # 业务开关:是否录制深度
169
+ self._encoder_choice = None # 本会话编码器选择(v1.0.9)
170
+ self._drop_stats: Dict[str, int] = {} # 丢帧统计(pipeline 注入)
171
+ # abort 清理清单:本 episode 落盘的最终文件 + 创建的目录
172
+ self._created_files: List[str] = []
173
+ self._created_dirs: List[str] = []
174
+
175
+ # ── 对外属性 ──────────────────────────────────────
176
+
177
+ @property
178
+ def task_dir(self) -> str:
179
+ """本 episode 归属的任务目录(池化布局根)。"""
180
+ return self._task_dir
181
+
182
+ @property
183
+ def episode_index(self) -> int:
184
+ """本 episode 的全局序号(1 起)。"""
185
+ return self._episode_index
186
+
187
+ # ── Episode ────────────────────────────────────────
188
+
189
+ def start_episode(self, output_dir: str, cameras: Dict[str, tuple],
190
+ fps: float = 30,
191
+ sensors: Optional[List[str]] = None,
192
+ task_name: str = "",
193
+ device_ids: Optional[List[str]] = None,
194
+ calibration: Optional[StereoCalibration] = None,
195
+ camera_fps: Optional[Dict[str, float]] = None,
196
+ depth_enabled: Optional[bool] = None,
197
+ heatmap_near_mm: float = 0.0,
198
+ heatmap_far_mm: float = 0.0,
199
+ heatmap_smooth_k: int = 0,
200
+ heatmap_temporal_alpha: float = 0.0,
201
+ depth_heatmaps: Optional[Dict[str, dict]] = None,
202
+ depth_slots: Optional[List[str]] = None,
203
+ devices: Optional[List[dict]] = None,
204
+ calibrations: Optional[Dict[str, object]] = None,
205
+ batch_index: int = 0) -> bool:
206
+ """创建池化任务目录并启动 ffmpeg(直接写最终路径)。
207
+
208
+ Args:
209
+ output_dir: 录制根目录
210
+ cameras: {camera_name: (height, width)},名称如 head_left_rgb
211
+ fps: 默认录制帧率(未在 camera_fps 中指定的摄像机使用)
212
+ sensors: 传感器名称列表
213
+ task_name: 任务标注
214
+ device_ids: 设备 ID 列表(用于 status 列)
215
+ calibration: 双目标定数据(旧单值路径)
216
+ camera_fps: 每路摄像机的独立帧率(用于 ffmpeg -r)
217
+ depth_enabled: 深度录制开关;None 时回退 settings.DEPTH_ENABLED
218
+ heatmap_near_mm/far_mm: 已废弃(v1.1.2 起显示与存储统一为
219
+ core.depth_codec 规范码值 JET,色标固定对数域),仅保留
220
+ 签名兼容
221
+ depth_heatmaps: 已废弃(同上),仅保留签名兼容
222
+ depth_slots: 显式深度槽位列表(pipeline 中经 set_depth_camera
223
+ 注册的真实深度设备)。空/None → S80M 传统路径兜底单槽
224
+ devices: 录制设备信息(写入 info.json 的 devices 段):
225
+ [{"key","kind","name","serial","slots"}]
226
+ calibrations: device_key → 标定(多路);存 info.json 任务级
227
+ 最新 + episodes 行,不落盘 calibration/ 目录
228
+ batch_index: 任务进度序号(录制完成次数 + 1);>0 时按进度
229
+ 递增(与文件删除无关),0 时退化为目录扫描取最大 + 1
230
+ """
231
+ ffmpeg = _get_ffmpeg()
232
+ if not ffmpeg:
233
+ self.error_occurred.emit("ffmpeg 未找到"); return False
234
+
235
+ self._fps = fps
236
+ self._camera_dims = dict(cameras)
237
+ self._camera_fps = dict(camera_fps) if camera_fps else {}
238
+ self._sensor_names = list(sensors) if sensors else []
239
+ self._sensor_dim = settings.SENSOR_DIM
240
+ self._device_ids = list(device_ids) if device_ids else []
241
+ self._calibrations = dict(calibrations) if calibrations else {}
242
+ self._devices = list(devices) if devices else []
243
+ self._calib_dict = self._plan_calibration_dict(calibration)
244
+
245
+ # 任务目录: <output_dir>/<task_tag>/,episode 序号全局递增
246
+ self._task_dir = task_dir_of(output_dir, task_name)
247
+ self._episode_index = next_pooled_episode_index(self._task_dir,
248
+ batch_index)
249
+ cidx, _fidx = episode_chunk_file(self._episode_index)
250
+
251
+ # 深度配置:以显式注册的深度设备(pipeline.set_depth_camera →
252
+ # depth_slots)为准,不用槽名是否含 "depth" 猜测——D435/D405 等
253
+ # 槽名可被消歧编号(d435_depth_2),型号与槽名都不该决定是否落盘。
254
+ depth_requested = (depth_enabled if depth_enabled is not None
255
+ else settings.DEPTH_ENABLED)
256
+ if depth_slots:
257
+ self._depth_slots = {dn: dn for dn in depth_slots if dn}
258
+ else:
259
+ self._depth_slots = {settings.CAMERA_DEPTH: settings.CAMERA_DEPTH}
260
+ # 显式注册的深度设备打开即录(与旧语义一致:D435/D405 不受
261
+ # settings.DEPTH_ENABLED 门控——该开关只作用于 S80M 视差路径)
262
+ self._depth_enabled = depth_requested and bool(depth_slots)
263
+ self._depth_proc_started = {}
264
+ self._depth_dirs = {}
265
+ self._depth_12bit = {}
266
+ # 深度存 12-bit 灰度 MP4(heatmap_near_mm/far_mm/depth_heatmaps
267
+ # 参数自 v1.1.2 起不再作用于落盘:显示与存储统一为
268
+ # core.depth_codec 的规范码值 JET,色标固定对数域)
269
+ self._depth_final_ext = {slot: "mp4" for slot in self._depth_slots}
270
+
271
+ # ★ v1.0.9 编码器自动选择:本会话 RGB 流数/最大分辨率驱动探针。
272
+ # 本函数运行在 _start_async 后台线程(写盘线程在其返回后才启动),
273
+ # 探针阻塞不占 UI 节拍;结果进程内缓存,同参数不重探。深度热力图
274
+ # 惰性启动时复用同一选择(同编码器同档位)。
275
+ rgb_cams = {n: d for n, d in cameras.items()
276
+ if n not in self._depth_slots}
277
+ if rgb_cams:
278
+ max_h, max_w = max(rgb_cams.values(),
279
+ key=lambda hw: hw[0] * hw[1])
280
+ n_streams = len(rgb_cams)
281
+ else:
282
+ max_h, max_w = (max(cameras.values(),
283
+ key=lambda hw: hw[0] * hw[1])
284
+ if cameras else (0, 0))
285
+ n_streams = 1
286
+ if max_w <= 0 or max_h <= 0:
287
+ self.error_occurred.emit("无有效视频流,无法启动录制"); return False
288
+ prefer = settings.RECORD_VIDEO_ENCODER if settings.ENCODER_PROBE_ENABLED \
289
+ else "x264"
290
+ choice = select_encoder(prefer, max_w, max_h, fps, n_streams,
291
+ log=self.log_occurred.emit)
292
+ if choice is None:
293
+ self.error_occurred.emit("未找到可用视频编码器 (ffmpeg -encoders)")
294
+ return False
295
+ self._encoder_choice = choice
296
+
297
+ # 目录:meta/episodes chunk + data chunk + 每路视频 image_key 目录
298
+ # (关键点数据独立存于 keypoints_output/,此处不创建)
299
+ self._created_files = []
300
+ self._created_dirs = []
301
+ dirs = [
302
+ os.path.join("meta", "episodes", f"chunk-{cidx:03d}"),
303
+ os.path.join("data", f"chunk-{cidx:03d}"),
304
+ ]
305
+ for cam_name in cameras:
306
+ if cam_name in self._depth_slots:
307
+ continue
308
+ vdir = pooled_video_dir(self._task_dir, cam_name,
309
+ self._episode_index)
310
+ dirs.append(vdir)
311
+ self._created_files.append(
312
+ pooled_video_path(self._task_dir, cam_name,
313
+ self._episode_index))
314
+ for sub in dirs:
315
+ d = os.path.join(self._task_dir, sub)
316
+ try:
317
+ os.makedirs(d, exist_ok=True)
318
+ except OSError as e:
319
+ self.error_occurred.emit(str(e)); return False
320
+ self._created_dirs.append(d)
321
+
322
+ # 启动 ffmpeg(仅 RGB 相机),每路使用独立帧率;
323
+ # 编码器取本会话选择(v1.0.9 动态化,见上方 select_encoder)
324
+ for cam_name, (h, w) in cameras.items():
325
+ if cam_name in self._depth_slots:
326
+ continue # 深度不走 MP4(显式注册驱动,不猜槽名)
327
+ cam_r = self._camera_fps.get(cam_name, fps)
328
+ video_path = pooled_video_path(self._task_dir, cam_name,
329
+ self._episode_index)
330
+ cmd = [
331
+ choice.ffmpeg, "-y", "-f", "rawvideo", "-pix_fmt", "bgr24",
332
+ "-s", f"{w}x{h}", "-r", str(cam_r), "-i", "-",
333
+ "-c:v", choice.encoder, *choice.args,
334
+ "-pix_fmt", "yuv420p", "-f", "mp4", video_path,
335
+ ]
336
+ try:
337
+ self._ffmpeg_procs[cam_name] = subprocess.Popen(
338
+ cmd, stdin=subprocess.PIPE,
339
+ stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
340
+ )
341
+ except Exception as e:
342
+ self.error_occurred.emit(str(e)); return False
343
+
344
+ self._episode_start_s = time.time()
345
+ self._episode_created_at = time.time()
346
+ self._frame_count = 0
347
+ self._max_frame_idx = 0
348
+ self._rows.clear()
349
+ self._present_sensors = set()
350
+ self._present_imu = False
351
+ self._present_glove_imu = set()
352
+ self._reset_stats()
353
+ self._last_task = task_name
354
+
355
+ self.episode_started.emit(self._task_dir)
356
+ return True
357
+
358
+ # ── 标定规划 ───────────────────────────────────────
359
+
360
+ def _plan_calibration_dict(self, legacy_calib) -> dict:
361
+ """规划标定 dict(存 info.json/episodes 行,不落盘 calibration/)。
362
+
363
+ 顺序由 devices 列表决定(calibrations 按 device_key 取);旧单值
364
+ calibration 参数写 "head_stereo" 键;全都缺时仍写默认
365
+ head_stereo(保持既有行为)。首台双目型设备同时以 "head_stereo"
366
+ 键保存(服务器/回放/stereo_triangulate 兼容路径)。
367
+ """
368
+ ordered = [(d.get("key", ""), d) for d in self._devices]
369
+ if not ordered or not self._calibrations:
370
+ c = legacy_calib if legacy_calib is not None else StereoCalibration()
371
+ return {"head_stereo": c.to_dict()}
372
+ out: Dict[str, dict] = {}
373
+ first_stereo = True
374
+ for dev_key, d in ordered:
375
+ c = self._calibrations.get(dev_key)
376
+ if c is None or not isinstance(c, StereoCalibration):
377
+ continue
378
+ if first_stereo:
379
+ out["head_stereo"] = c.to_dict()
380
+ first_stereo = False
381
+ else:
382
+ out[f"{self._device_slot_prefix(d)}_calibration"] = c.to_dict()
383
+ if not out:
384
+ out["head_stereo"] = StereoCalibration().to_dict()
385
+ return out
386
+
387
+ @staticmethod
388
+ def _device_slot_prefix(device: dict) -> str:
389
+ """设备槽前缀: 首槽去掉 "_N" 消歧编号(d435_rgb_2 → d435_rgb)。"""
390
+ slots = device.get("slots") or []
391
+ slot = slots[0] if slots else (device.get("key") or "device")
392
+ import re as _re
393
+ return _re.sub(r"_\d+$", "", str(slot))
394
+
395
+ def _video_codec_meta(self) -> dict:
396
+ """本会话视频编码信息(episodes 行 video_codec 段)。"""
397
+ c = self._encoder_choice
398
+ if c is None:
399
+ meta = {"encoder": "libx264", "codec": "H.264",
400
+ "crf": settings.RECORD_VIDEO_X264_CRF,
401
+ "selected_by": settings.RECORD_VIDEO_ENCODER}
402
+ else:
403
+ meta = {"encoder": c.encoder, "codec": c.codec, "crf": c.crf,
404
+ "ffmpeg": c.ffmpeg, "selected_by": c.selected_by,
405
+ "probe": c.probe}
406
+ # 深度槽编码(12-bit 灰度 HEVC 或 FFV1 回落),按槽记录
407
+ depth_meta = {}
408
+ for slot in self._depth_slots:
409
+ if self._depth_12bit.get(slot):
410
+ depth_meta[slot] = {"codec": "HEVC (Rext) gray12le",
411
+ "qp": 6, "format": "12bit log codes"}
412
+ else:
413
+ depth_meta[slot] = {"codec": "FFV1 gray16le",
414
+ "format": "uint16 mm"}
415
+ if depth_meta:
416
+ meta["depth"] = depth_meta
417
+ return meta
418
+
419
+ # ── 视频帧写入 ─────────────────────────────────────
420
+
421
+ def write_video_frame(self, camera_name: str, frame: np.ndarray,
422
+ flip_vertical: bool = True):
423
+ """写入一帧 BGR 到对应摄像机的 ffmpeg 管道。
424
+
425
+ Args:
426
+ camera_name: 摄像机标识
427
+ frame: BGR numpy array
428
+ flip_vertical: 是否上下翻转(默认 True,单目 CameraSlot 路径使用;
429
+ 双目显示帧路径传入 False,因为已在 _on_stereo_frame 完成翻转)
430
+ """
431
+ proc = self._ffmpeg_procs.get(camera_name)
432
+ if proc and proc.stdin and not proc.stdin.closed:
433
+ try:
434
+ if flip_vertical:
435
+ frame = cv2.flip(frame, 0)
436
+ proc.stdin.write(frame.tobytes())
437
+ except (BrokenPipeError, OSError):
438
+ self.error_occurred.emit(f"ffmpeg pipe broken ({camera_name})")
439
+
440
+ # ── 深度帧写入 ─────────────────────────────────────
441
+
442
+ def _resolve_depth_slot(self, depth_slot: str) -> str:
443
+ """把调用方深度槽名映射到本 episode 的落盘槽名。
444
+
445
+ 规则: 槽名精确匹配 → 原样;否则若只有单一深度槽(S80M 传统路径
446
+ 的 "stereo_left" 名义槽)→ 回落到该槽;多槽且无匹配 → 原样返回,
447
+ 由调用方发现无 ffmpeg proc 时自然跳过。
448
+ """
449
+ if depth_slot in self._depth_slots:
450
+ return depth_slot
451
+ if len(self._depth_slots) == 1:
452
+ return next(iter(self._depth_slots))
453
+ return depth_slot
454
+
455
+ def write_depth_frame(self, frame_index: int, depth_frame: np.ndarray,
456
+ depth_slot: str = ""):
457
+ """写入一帧深度图 —— 12-bit 灰度 MP4(v1.1.2,lerobot v3 同款)。
458
+
459
+ 单条 ffmpeg 管道:gray12le → libx265 qp=6 近无损(hevc Rext
460
+ 单平面,12-bit 对数深度码,见 core/depth_codec.py),直接写
461
+ 最终路径 videos/chunk-NNN/<slot>/file-NNN.mp4(单流无需合封)。
462
+ x265 无 12-bit 灰度能力时回落 FFV1 gray16le 无损 MKV(旧格式,
463
+ uint16 毫米原值)。
464
+
465
+ 帧节拍:调用方(pipeline)按主槽位帧节拍调用本函数,深度源缺帧时
466
+ 重复最近帧——深度帧 i 与 RGB 帧 i 严格对齐。
467
+
468
+ Args:
469
+ frame_index: 帧序号(与 RGB 严格对应;仅存档签名,未参与写入)
470
+ depth_frame: uint16 numpy array (H, W),单位毫米
471
+ depth_slot: 深度槽名(多深度相机时区分落盘目录;
472
+ 空/未注册时按单槽回落,多槽无匹配则跳过)
473
+ """
474
+ if not settings.DEPTH_ENABLED and not self._depth_enabled:
475
+ return # 深度录制已关闭,直接跳过
476
+ slot = self._resolve_depth_slot(depth_slot)
477
+ if slot not in self._depth_slots:
478
+ return # 多槽下无匹配(如 stereo_left 名义帧)→ 不落盘
479
+ if not self._depth_proc_started.get(slot):
480
+ # 首次调用:创建目录 + 启动单流深度 ffmpeg(12-bit 优先)
481
+ self._depth_dirs[slot] = pooled_video_dir(
482
+ self._task_dir, slot, self._episode_index)
483
+ try:
484
+ os.makedirs(self._depth_dirs[slot], exist_ok=True)
485
+ except OSError:
486
+ pass
487
+ self._created_dirs.append(self._depth_dirs[slot])
488
+
489
+ h, w = depth_frame.shape[:2]
490
+ ffmpeg = _get_ffmpeg()
491
+ if ffmpeg:
492
+ depth_r = (self._camera_fps.get(slot)
493
+ or self._camera_fps.get("stereo_left", self._fps))
494
+ d12 = find_depth_12bit_ffmpeg()
495
+ if d12:
496
+ # 12-bit 对数深度码 → gray12le HEVC(直接写最终路径)
497
+ final = pooled_video_path(self._task_dir, slot,
498
+ self._episode_index, "mp4")
499
+ cmd = [
500
+ d12, "-y", "-f", "rawvideo", "-pix_fmt", "gray12le",
501
+ "-s", f"{w}x{h}", "-r", str(depth_r), "-i", "-",
502
+ "-c:v", "libx265", *depth_video_encoder_args(), final,
503
+ ]
504
+ self._depth_12bit[slot] = True
505
+ else:
506
+ # 回落:FFV1 gray16le 无损 uint16 毫米(旧格式 MKV)
507
+ final = pooled_video_path(self._task_dir, slot,
508
+ self._episode_index, "mkv")
509
+ self._depth_final_ext[slot] = "mkv"
510
+ cmd = [
511
+ ffmpeg, "-y", "-f", "rawvideo", "-pix_fmt", "gray16le",
512
+ "-s", f"{w}x{h}", "-r", str(depth_r), "-i", "-",
513
+ "-c:v", "ffv1", "-level", "3",
514
+ "-pix_fmt", "gray16le", final,
515
+ ]
516
+ self._depth_12bit[slot] = False
517
+ try:
518
+ self._ffmpeg_procs[slot] = subprocess.Popen(
519
+ cmd, stdin=subprocess.PIPE,
520
+ stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
521
+ )
522
+ self._created_files.append(final)
523
+ except Exception as e:
524
+ self.error_occurred.emit(f"depth ffmpeg failed: {e}")
525
+
526
+ self._depth_proc_started[slot] = True
527
+
528
+ proc = self._ffmpeg_procs.get(slot)
529
+ if not proc or not proc.stdin or proc.stdin.closed:
530
+ return
531
+ if self._depth_12bit.get(slot):
532
+ payload = np.ascontiguousarray(
533
+ quantize_depth(depth_frame), dtype="<u2").tobytes()
534
+ else:
535
+ payload = np.ascontiguousarray(depth_frame, dtype="<u2").tobytes()
536
+ try:
537
+ proc.stdin.write(payload)
538
+ except (BrokenPipeError, OSError):
539
+ pass
540
+
541
+ # ── 数据行写入 (LeRobot v3 兼容 Parquet) ────────────
542
+
543
+ def write_frame_row(self, frame_index: int, timestamp_s: float,
544
+ sensors: Optional[Dict[str, np.ndarray]] = None,
545
+ connection_status: Optional[Dict[str, str]] = None,
546
+ hardware_ns: int = 0,
547
+ imu_samples: Optional[List] = None,
548
+ glove_imu: Optional[Dict[str, tuple]] = None,
549
+ glove_kpts: Optional[Dict[str, np.ndarray]] = None):
550
+ """写入一行到 data parquet 缓冲区。
551
+
552
+ Args:
553
+ frame_index: 帧序号(与视频帧一致)
554
+ timestamp_s: episode 起始相对秒(f32 墙钟)
555
+ sensors: {传感器名: 16×16 压力数据},缺失帧填零
556
+ connection_status: {device_id: "connected"/"disconnected"}
557
+ hardware_ns: 帧的 SDK 硬件纳秒时间戳(双目相机,与 IMU 同源时钟;
558
+ 单目/传感器路径为 0)
559
+ imu_samples: 本帧窗口内采集的 IMU 样本列表
560
+ [(ts_ns, gx, gy, gz, ax, ay, az), ...]
561
+ 仅双目 stereo_left 帧携带(左右目共享同一份样本)
562
+ glove_imu: {传感器名: (quats 64×f32, valid 16×f32)}
563
+ USB 手套 IMU 四元数快照(有数据才写行键,
564
+ 稀疏列按 self._present_glove_imu 落盘)
565
+ glove_kpts: {传感器名: 63×f32 骨架关键点(21×3 米)}
566
+ USB 手套骨架快照,回填恒写的
567
+ observation.{left,right}_hand_pose 占位列
568
+ """
569
+ sensors = sensors or {}
570
+ row = {
571
+ "episode_index": self._episode_index,
572
+ "frame_index": frame_index,
573
+ "timestamp": np.float32(timestamp_s),
574
+ "task_index": 0,
575
+ "wall_time": float(time.time()), # 绝对 Unix 秒(宿主锚定)
576
+ "hardware_ns": int(hardware_ns or 0),
577
+ "action": [0.0],
578
+ }
579
+ for sn in self._sensor_names:
580
+ data = sensors.get(sn)
581
+ if data is None:
582
+ data = np.zeros(self._sensor_dim, dtype=np.float32)
583
+ else:
584
+ data = data.astype(np.float32).ravel()[:self._sensor_dim]
585
+ self._present_sensors.add(sn) # 稀疏列:有真实数据才落盘
586
+ row[f"observation.{sn}"] = data.tolist()
587
+ self._update_stats(sn, data)
588
+
589
+ # IMU: 样本列表 → 嵌套列 (ts 单独一列 int64 保证纳秒精度);
590
+ # 稀疏:仅本 episode 有 IMU 样本时才写这两列
591
+ imu_samples = imu_samples or []
592
+ if imu_samples:
593
+ self._present_imu = True
594
+ row["imu_ts_ns"] = [int(s[0]) for s in imu_samples]
595
+ row["observation.imu"] = [
596
+ [float(v) for v in s[1:7]] for s in imu_samples
597
+ ]
598
+ self._update_imu_stats(row["observation.imu"])
599
+
600
+ # USB 手套 IMU:四元数 (16×4) + 有效掩码 (16),每传感器两列,
601
+ # 稀疏契约同触觉列(本 episode 出现过的传感器才建列)
602
+ for sn, (quats, valid) in (glove_imu or {}).items():
603
+ if sn not in self._sensor_names:
604
+ continue
605
+ row[f"observation.{sn}_imu_quat"] = [
606
+ float(v) for v in np.asarray(quats, dtype=np.float32
607
+ ).ravel()[:64]]
608
+ row[f"observation.{sn}_imu_valid"] = [
609
+ float(v) for v in np.asarray(valid, dtype=np.float32
610
+ ).ravel()[:16]]
611
+ self._present_glove_imu.add(sn)
612
+
613
+ # 恒写的 hand_pose 占位列(无骨架数据时保持全零)
614
+ row[f"observation.{settings.HAND_POSE_LEFT}"] = \
615
+ [0.0] * settings.HAND_POSE_DIM
616
+ row[f"observation.{settings.HAND_POSE_RIGHT}"] = \
617
+ [0.0] * settings.HAND_POSE_DIM
618
+
619
+ # 手套骨架关键点(工具包解算)→ 回填上述占位列。
620
+ # 传感器名 left_glove/right_glove → left/right_hand_pose;
621
+ # 无骨架快照的帧保持零占位(旧录制读取端统一按全零处理)
622
+ for sn, kpts in (glove_kpts or {}).items():
623
+ side = sn.split("_")[0]
624
+ if side not in ("left", "right"):
625
+ continue
626
+ k = np.asarray(kpts, dtype=np.float32).ravel()[:settings.HAND_POSE_DIM]
627
+ if k.size != settings.HAND_POSE_DIM or not np.isfinite(k).all():
628
+ continue
629
+ row[f"observation.{side}_hand_pose"] = [
630
+ float(v) for v in k]
631
+
632
+ cs = connection_status or {}
633
+ for did in self._device_ids:
634
+ row[f"status.{did}"] = cs.get(did, "connected")
635
+
636
+ with QMutexLocker(self._mutex):
637
+ self._rows.append(row)
638
+ self._frame_count += 1
639
+ if frame_index + 1 > self._max_frame_idx:
640
+ self._max_frame_idx = frame_index + 1
641
+
642
+ # ── 结束 ──────────────────────────────────────────
643
+
644
+ def end_episode(self):
645
+ """关闭 ffmpeg,写出全部文件(顺序见 docs/data.md 写入流程)。"""
646
+ self._close_ffmpeg()
647
+ self._write_data_parquet()
648
+ self._append_episode_row()
649
+ self._write_info_json()
650
+ self._merge_stats()
651
+ self._write_tasks_jsonl()
652
+ clear_recycled_episode(self._task_dir)
653
+ self.episode_finished.emit(self._task_dir)
654
+
655
+ @property
656
+ def encoder_label(self) -> str:
657
+ """本会话视频编码的 UI 显示名(pipeline 录制状态栏用)。"""
658
+ c = self._encoder_choice
659
+ return c.label if c is not None else "H.264/MP4"
660
+
661
+ def set_drop_stats(self, stats: Dict[str, int]) -> None:
662
+ """注入录制丢帧统计(pipeline 在 end_episode 前调用)。"""
663
+ self._drop_stats = dict(stats or {})
664
+
665
+ def abort_episode(self):
666
+ """丢弃本 episode 的全部数据(文件级清理)。
667
+
668
+ abort 只发生在录制期(end_episode 之前),本 episode 未写 data
669
+ parquet / episodes 行 / stats 合并;只需删除 ffmpeg 已落盘的视频
670
+ 终稿与深度临时件,并回滚空目录。防御性回滚 episodes 行(若存在)。
671
+
672
+ 异常终止不占号:标记本段序号可复用,下次录制优先取回
673
+ (next_pooled_episode_index 读到标记即复用;正常完成时清除)。
674
+ """
675
+ if getattr(self, "_episode_index", 0) > 0 and getattr(
676
+ self, "_task_dir", ""):
677
+ mark_recycled_episode(self._task_dir, self._episode_index)
678
+ self._close_ffmpeg()
679
+ for p in list(self._created_files):
680
+ try:
681
+ if os.path.isfile(p):
682
+ os.remove(p)
683
+ except OSError:
684
+ pass
685
+ # 防御性:episodes 行回滚(正常流程不可达)
686
+ self._rollback_episode_row()
687
+ # 空目录回滚(深到浅,只删空目录;共享目录自然保留)
688
+ for d in reversed(sorted(self._created_dirs, key=len)):
689
+ try:
690
+ os.rmdir(d)
691
+ except OSError:
692
+ pass
693
+ try:
694
+ os.rmdir(self._task_dir)
695
+ except OSError:
696
+ pass
697
+ self.episode_finished.emit("")
698
+
699
+ def _rollback_episode_row(self):
700
+ """移除本 episode 的 episodes 元数据(每段一文件 → os.remove;
701
+ 旧分片回退 → 读-改-写删行,保证 episode_row(N) 回退后返回 {})。
702
+ N=1 时分片与每段文件同路径,先按内容判每段文件才删。"""
703
+ path = pooled_episodes_path(self._task_dir, self._episode_index)
704
+ with self._task_lock():
705
+ removed_path = False
706
+ if os.path.isfile(path):
707
+ rows = _read_episode_rows(path)
708
+ if len(rows) <= 1 and (not rows or rows[0].get(
709
+ "episode_index") == self._episode_index):
710
+ try:
711
+ os.remove(path)
712
+ except OSError:
713
+ pass
714
+ removed_path = True
715
+ cidx, _fidx = episode_chunk_file(self._episode_index)
716
+ legacy = _legacy_episodes_shard_path(self._task_dir, cidx)
717
+ if os.path.isfile(legacy) and not (removed_path
718
+ and legacy == path):
719
+ rows = _read_episode_rows(legacy)
720
+ kept = [r for r in rows
721
+ if r.get("episode_index") != self._episode_index]
722
+ if len(kept) != len(rows):
723
+ if kept:
724
+ _atomic_write_parquet(_episode_rows_table(kept), legacy)
725
+ else:
726
+ try:
727
+ os.remove(legacy)
728
+ except OSError:
729
+ pass
730
+
731
+ # ── 内部 ──────────────────────────────────────────
732
+
733
+ def _close_ffmpeg(self):
734
+ for proc in list(self._ffmpeg_procs.values()):
735
+ try:
736
+ if proc.stdin: proc.stdin.close()
737
+ except Exception:
738
+ pass
739
+ try:
740
+ proc.wait(timeout=5)
741
+ except subprocess.TimeoutExpired:
742
+ proc.kill(); proc.wait()
743
+ self._ffmpeg_procs.clear()
744
+
745
+ def _write_data_parquet(self):
746
+ """写入本 episode 的 data parquet(单文件,稀疏列,zstd)。
747
+
748
+ 键列恒有(episode_index/frame_index/timestamp/task_index/
749
+ wall_time/hardware_ns/action/手部占位列/status 列);观测列稀疏:
750
+ 只写本 episode 实际有数据的传感器与 IMU 列。
751
+ """
752
+ with QMutexLocker(self._mutex):
753
+ if not self._rows:
754
+ return
755
+ rows = self._rows[:]
756
+ self._rows.clear()
757
+
758
+ cols = {
759
+ "episode_index": pa.array([r["episode_index"] for r in rows],
760
+ pa.int64()),
761
+ "frame_index": pa.array([r["frame_index"] for r in rows],
762
+ pa.int64()),
763
+ "timestamp": pa.array([r["timestamp"] for r in rows],
764
+ pa.float32()),
765
+ "task_index": pa.array([r["task_index"] for r in rows],
766
+ pa.int64()),
767
+ "wall_time": pa.array([r["wall_time"] for r in rows],
768
+ pa.float64()),
769
+ "hardware_ns": pa.array([r["hardware_ns"] for r in rows],
770
+ pa.int64()),
771
+ "action": pa.array([r["action"] for r in rows],
772
+ pa.list_(pa.float32(), 1)),
773
+ }
774
+ # 稀疏传感器列(有真实数据的才写)
775
+ for sn in sorted(self._present_sensors):
776
+ name = f"observation.{sn}"
777
+ cols[name] = pa.array(
778
+ [r.get(name, [0.0] * self._sensor_dim) for r in rows],
779
+ pa.list_(pa.float32(), self._sensor_dim),
780
+ )
781
+ # 稀疏 IMU 列
782
+ if self._present_imu:
783
+ cols["imu_ts_ns"] = pa.array(
784
+ [r.get("imu_ts_ns", []) for r in rows], pa.list_(pa.int64()))
785
+ cols["observation.imu"] = pa.array(
786
+ [r.get("observation.imu", []) for r in rows],
787
+ pa.list_(pa.list_(pa.float32(), 6)))
788
+ # 稀疏 USB 手套 IMU 列(四元数 64 维 + 有效掩码 16 维,每传感器一对)
789
+ for sn in sorted(self._present_glove_imu):
790
+ name_q = f"observation.{sn}_imu_quat"
791
+ name_v = f"observation.{sn}_imu_valid"
792
+ cols[name_q] = pa.array(
793
+ [r.get(name_q, [0.0] * 64) for r in rows],
794
+ pa.list_(pa.float32(), 64))
795
+ cols[name_v] = pa.array(
796
+ [r.get(name_v, [0.0] * 16) for r in rows],
797
+ pa.list_(pa.float32(), 16))
798
+ # 手部关键点占位列(后处理回填,恒写)
799
+ for pose_name in [settings.HAND_POSE_LEFT, settings.HAND_POSE_RIGHT]:
800
+ name = f"observation.{pose_name}"
801
+ cols[name] = pa.array(
802
+ [r[name] for r in rows],
803
+ pa.list_(pa.float32(), settings.HAND_POSE_DIM),
804
+ )
805
+ for did in self._device_ids:
806
+ name = f"status.{did}"
807
+ cols[name] = pa.array(
808
+ [r.get(name, "connected") for r in rows], pa.string())
809
+
810
+ table = pa.table(cols)
811
+ path = pooled_data_parquet_path(self._task_dir, self._episode_index)
812
+ os.makedirs(os.path.dirname(path), exist_ok=True)
813
+ _atomic_write_parquet(table, path)
814
+
815
+ def _append_episode_row(self):
816
+ """meta/episodes/chunk-NNN/episode-NNN.parquet 每段一个文件、单行原子写。
817
+
818
+ 与 data/videos 同编号(episode-000 = episode 1):每采一条新任务
819
+ 就多一个 episode 文件。重录同 episode_index = 覆盖该文件。旧分片
820
+ (每 chunk 一个多行文件)里的同号行一并删除,避免双份。
821
+ 跨进程安全:fcntl.flock 锁 meta/episodes/.lock(Windows 无
822
+ fcntl 退化为无锁,多机共享须 POSIX);写入走临时文件 +
823
+ os.replace 原子替换。
824
+ """
825
+ path = pooled_episodes_path(self._task_dir, self._episode_index)
826
+ os.makedirs(os.path.dirname(path), exist_ok=True)
827
+ row = {
828
+ "episode_index": self._episode_index,
829
+ "task_index": 0,
830
+ "start_frame_index": 0,
831
+ "end_frame_index": max(0, self._max_frame_idx - 1),
832
+ "length": self._max_frame_idx,
833
+ "created_at": float(self._episode_created_at),
834
+ "duration_sec": float(self._max_frame_idx / max(self._fps, 1.0)),
835
+ "drop_stats": json.dumps(self._drop_stats or {},
836
+ ensure_ascii=False),
837
+ "video_codec": json.dumps(self._video_codec_meta(),
838
+ ensure_ascii=False),
839
+ "calibration": json.dumps(self._calib_dict, ensure_ascii=False),
840
+ }
841
+ with self._task_lock():
842
+ _atomic_write_parquet(_episode_rows_table([row]), path)
843
+ # 旧分片里若有同号行则删掉(避免 episode_row 回退到旧记录)
844
+ cidx, _fidx = episode_chunk_file(self._episode_index)
845
+ legacy = _legacy_episodes_shard_path(self._task_dir, cidx)
846
+ if legacy != path and os.path.isfile(legacy):
847
+ rows = _read_episode_rows(legacy)
848
+ kept = [r for r in rows
849
+ if r.get("episode_index") != self._episode_index]
850
+ if len(kept) != len(rows):
851
+ if kept:
852
+ _atomic_write_parquet(_episode_rows_table(kept),
853
+ legacy)
854
+ else:
855
+ try:
856
+ os.remove(legacy)
857
+ except OSError:
858
+ pass
859
+
860
+ def _task_lock(self):
861
+ """任务级跨进程锁(meta/episodes/.lock);上下文管理器。"""
862
+ lock_path = os.path.join(self._task_dir, "meta", "episodes", ".lock")
863
+ os.makedirs(os.path.dirname(lock_path), exist_ok=True)
864
+ fh = open(lock_path, "a+")
865
+ if _fcntl is not None:
866
+ _fcntl.flock(fh.fileno(), _fcntl.LOCK_EX)
867
+ return _LockedFile(fh)
868
+
869
+ def _write_info_json(self):
870
+ """任务级 meta/info.json 读-改-写。
871
+
872
+ 服务器上传严格依赖既有字段名/类型/结构(features/cameras/
873
+ devices/device_names/sensors/sensor_dim/fps/video/task_name/
874
+ codebase_version/created_at),v1.1.0 新增 format/chunks_size/
875
+ data_path/video_path/episodes_path/video_extensions/calibration/
876
+ total_episodes/app_version。既有键保留合并(其他机器/旧版本写入
877
+ 的不丢);features/cameras/video_extensions 按并集合并(key 一旦
878
+ 存在不因后续 episode 缺席而丢失),值以最新 episode 为准。
879
+ """
880
+ # cameras: 必须是 dict 格式 {key: {height, width, fps}},不能用 list
881
+ # 深度槽过滤以显式注册为准,不猜槽名
882
+ cameras_dict = {}
883
+ for name, (h, w) in self._camera_dims.items():
884
+ if name in self._depth_slots:
885
+ continue
886
+ entry = {"height": h, "width": w}
887
+ cam_fps = self._camera_fps.get(name, self._fps)
888
+ if cam_fps != self._fps:
889
+ entry["fps"] = cam_fps
890
+ cameras_dict[name] = entry
891
+
892
+ # features: shape 必须是 2D [16, 16],不是 1D [256];
893
+ # 只声明本 episode 实际存在的列(稀疏契约)
894
+ features = {}
895
+ for sn in sorted(self._present_sensors):
896
+ features[f"observation.{sn}"] = {
897
+ "dtype": "float32",
898
+ "shape": [16, 16],
899
+ }
900
+ # 双目 IMU: 每帧窗口内变长样本序列, 每样本 6 轴 (gx,gy,gz,ax,ay,az)
901
+ if self._present_imu:
902
+ features["observation.imu"] = {"dtype": "float32", "shape": [6]}
903
+ # USB 手套 IMU: 每帧 16×4 XYZW 四元数 + 16 维有效掩码
904
+ for sn in sorted(self._present_glove_imu):
905
+ features[f"observation.{sn}_imu_quat"] = {
906
+ "dtype": "float32", "shape": [16, 4]}
907
+ features[f"observation.{sn}_imu_valid"] = {
908
+ "dtype": "float32", "shape": [16]}
909
+ # 手部骨架关键点(21×3 米,恒写列;无骨架数据时为全零占位)
910
+ features[f"observation.{settings.HAND_POSE_LEFT}"] = {
911
+ "dtype": "float32", "shape": [21, 3]}
912
+ features[f"observation.{settings.HAND_POSE_RIGHT}"] = {
913
+ "dtype": "float32", "shape": [21, 3]}
914
+ features["action"] = {"dtype": "float32", "shape": [1]}
915
+
916
+ # devices 紧凑段 + device_names(槽位 → 用户命名)——只加字段,
917
+ # 既有键名/类型一律不动(服务器严格依赖)
918
+ devices_compact = [{
919
+ "key": d.get("key", ""), "kind": d.get("kind", ""),
920
+ "name": d.get("name", ""), "slots": list(d.get("slots") or []),
921
+ } for d in self._devices]
922
+ device_names = {}
923
+ for d in self._devices:
924
+ if d.get("name"):
925
+ for slot in (d.get("slots") or []):
926
+ device_names[slot] = d["name"]
927
+
928
+ # video_extensions: 每 image_key 的文件扩展名(RGB=mp4;深度=mp4
929
+ # 12-bit 灰度,x265 无能力回落 mkv)。并集合并,key 不丢。
930
+ video_extensions = {
931
+ name: "mp4" for name in cameras_dict
932
+ }
933
+ for slot in self._depth_slots:
934
+ video_extensions[slot] = self._depth_final_ext.get(slot, "mp4")
935
+
936
+ base = {}
937
+ path = pooled_info_path(self._task_dir)
938
+ if os.path.isfile(path):
939
+ try:
940
+ with open(path, "r", encoding="utf-8") as f:
941
+ base = json.load(f)
942
+ except (OSError, json.JSONDecodeError):
943
+ base = {}
944
+ if not isinstance(base, dict):
945
+ base = {}
946
+
947
+ merged_features = dict(base.get("features") or {})
948
+ merged_features.update(features)
949
+ merged_cameras = dict(base.get("cameras") or {})
950
+ merged_cameras.update(cameras_dict)
951
+ merged_exts = dict(base.get("video_extensions") or {})
952
+ merged_exts.update(video_extensions)
953
+
954
+ info = dict(base)
955
+ info.update({
956
+ "format": "pooled_episodes_v1",
957
+ "chunks_size": POOLED_CHUNK_SIZE,
958
+ "data_path": "data/chunk-{c:03d}/episode-{f:03d}.parquet",
959
+ "video_path": "videos/chunk-{c:03d}/{image_key}/episode-{f:03d}.{ext}",
960
+ "episodes_path": "meta/episodes/chunk-{c:03d}/episode-{f:03d}.parquet",
961
+ "codebase_version": "v3.0",
962
+ "app_version": settings.APP_VERSION,
963
+ "fps": self._fps,
964
+ "video": len(cameras_dict) > 0,
965
+ "task_name": self._last_task or "",
966
+ "features": merged_features,
967
+ "cameras": merged_cameras,
968
+ "devices": devices_compact,
969
+ "device_names": device_names,
970
+ "sensors": sorted(self._present_sensors),
971
+ "sensor_dim": self._sensor_dim,
972
+ "created_at": time.time(),
973
+ "calibration": self._calib_dict, # 任务级最新标定
974
+ "total_episodes": len(list_task_episodes(self._task_dir)),
975
+ "video_extensions": merged_exts,
976
+ })
977
+ self._atomic_write_json(path, info)
978
+
979
+ def _merge_stats(self):
980
+ """把本 episode 统计合并进任务级 stats.json(v1.1.1 自含累加器)。
981
+
982
+ stats.json 每块 {count, mean, std, min, max}——count 可反推
983
+ sum/sum_sq,因此文件自身就是增量累加器,无需 .stats_state 边车。
984
+ 旧格式(无 count)或损坏 → recalc_stats 全量重扫重建一次。
985
+ 只统计实际存在的列(present 传感器 + present IMU);
986
+ action 恒为占位(mean 0/std 1,与旧格式一致)。
987
+ """
988
+ acc, need_recalc = load_stats_acc(self._task_dir)
989
+ if need_recalc:
990
+ acc = recalc_stats(self._task_dir)
991
+
992
+ for sn in sorted(self._present_sensors):
993
+ merge_stat_block(acc, f"observation.{sn}",
994
+ self._stats.get(sn), dim=self._sensor_dim)
995
+ if self._present_imu:
996
+ merge_stat_block(acc, "observation.imu",
997
+ self._stats.get("imu"), dim=6)
998
+
999
+ self._atomic_write_json(pooled_stats_path(self._task_dir),
1000
+ acc_to_stats_json(acc))
1001
+
1002
+ def _write_tasks_jsonl(self):
1003
+ """任务级 meta/tasks.jsonl(不存在才建,服务器依赖此文件识别任务)。"""
1004
+ path = pooled_tasks_jsonl_path(self._task_dir)
1005
+ if os.path.isfile(path):
1006
+ return
1007
+ with open(path, "w", encoding="utf-8") as f:
1008
+ json.dump({
1009
+ "task_index": 0,
1010
+ "task": self._last_task or "default recording",
1011
+ }, f, ensure_ascii=False)
1012
+ f.write("\n")
1013
+
1014
+ # ── 原子写入工具 ──────────────────────────────────
1015
+
1016
+ def _atomic_write_json(self, path: str, data: dict):
1017
+ """JSON 原子写:临时文件 + os.replace(同盘保证原子)。"""
1018
+ os.makedirs(os.path.dirname(path), exist_ok=True)
1019
+ tmp = os.path.join(os.path.dirname(path),
1020
+ f".{os.path.basename(path)}.tmp")
1021
+ with open(tmp, "w", encoding="utf-8") as f:
1022
+ json.dump(data, f, indent=2, ensure_ascii=False)
1023
+ os.replace(tmp, path)
1024
+
1025
+ # ── 统计量 ────────────────────────────────────────
1026
+
1027
+ def _reset_stats(self):
1028
+ self._stats.clear()
1029
+
1030
+ def _ensure_stats(self, sensor_name: str):
1031
+ if sensor_name not in self._stats:
1032
+ self._stats[sensor_name] = {
1033
+ "sum": np.zeros(self._sensor_dim, dtype=np.float64),
1034
+ "sum_sq": np.zeros(self._sensor_dim, dtype=np.float64),
1035
+ "min": np.full(self._sensor_dim, np.inf, dtype=np.float64),
1036
+ "max": np.full(self._sensor_dim, -np.inf, dtype=np.float64),
1037
+ "count": 0,
1038
+ }
1039
+
1040
+ def _update_stats(self, sensor_name: str, arr: np.ndarray):
1041
+ self._ensure_stats(sensor_name)
1042
+ st = self._stats[sensor_name]
1043
+ a = arr.astype(np.float64)
1044
+ st["sum"] += a
1045
+ st["sum_sq"] += a * a
1046
+ st["min"] = np.minimum(st["min"], a)
1047
+ st["max"] = np.maximum(st["max"], a)
1048
+ st["count"] += 1
1049
+
1050
+ # ── IMU 统计(样本级, 6 轴, 区别于传感器的 256 维帧级统计)──
1051
+
1052
+ def _ensure_imu_stats(self):
1053
+ if "imu" not in self._stats:
1054
+ self._stats["imu"] = {
1055
+ "sum": np.zeros(6, dtype=np.float64),
1056
+ "sum_sq": np.zeros(6, dtype=np.float64),
1057
+ "min": np.full(6, np.inf, dtype=np.float64),
1058
+ "max": np.full(6, -np.inf, dtype=np.float64),
1059
+ "count": 0,
1060
+ }
1061
+
1062
+ def _update_imu_stats(self, samples: List[List[float]]):
1063
+ """按 IMU 样本累积 6 轴统计(count = 样本总数)。
1064
+
1065
+ samples: 本帧窗口的样本列表, 每样本 [gx, gy, gz, ax, ay, az]
1066
+ """
1067
+ if not samples:
1068
+ return
1069
+ self._ensure_imu_stats()
1070
+ st = self._stats["imu"]
1071
+ a = np.asarray(samples, dtype=np.float64).reshape(-1, 6)
1072
+ st["sum"] += a.sum(axis=0)
1073
+ st["sum_sq"] += np.square(a).sum(axis=0)
1074
+ st["min"] = np.minimum(st["min"], a.min(axis=0))
1075
+ st["max"] = np.maximum(st["max"], a.max(axis=0))
1076
+ st["count"] += len(samples)
1077
+
1078
+
1079
+ # ── 模块级工具 ────────────────────────────────────────
1080
+
1081
+ class _LockedFile:
1082
+ """flock 锁句柄上下文(释放锁 + 关闭文件)。"""
1083
+
1084
+ def __init__(self, fh):
1085
+ self._fh = fh
1086
+
1087
+ def __enter__(self):
1088
+ return self._fh
1089
+
1090
+ def __exit__(self, *exc):
1091
+ try:
1092
+ if _fcntl is not None:
1093
+ _fcntl.flock(self._fh.fileno(), _fcntl.LOCK_UN)
1094
+ except Exception:
1095
+ pass
1096
+ try:
1097
+ self._fh.close()
1098
+ except Exception:
1099
+ pass
1100
+ return False
1101
+
1102
+
1103
+ def _atomic_write_parquet(table: pa.Table, path: str):
1104
+ """Parquet 原子写:临时文件 + os.replace(同盘保证原子)。"""
1105
+ tmp = os.path.join(os.path.dirname(path),
1106
+ f".{os.path.basename(path)}.tmp")
1107
+ pq.write_table(table, tmp, compression="zstd")
1108
+ os.replace(tmp, path)
collector/core/encoder_probe.py ADDED
@@ -0,0 +1,345 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 编码器探针 —— 录制编码器自动选择(v1.0.9:录制直出 HEVC)。
3
+
4
+ 纯函数模块(无 Qt 依赖),供 EgoDataWriter 在录制启动前调用(发生在
5
+ pipeline 的 _start_async 后台线程内,写盘线程在其返回后才启动,探针
6
+ 不卡 UI、不影响写入节奏):
7
+ - 候选 ffmpeg 二进制枚举(imageio 静态 → PATH,-version 自检)
8
+ - 编码器能力探测(-encoders)与速度探针(null muxer 微型编码)
9
+ - 编码器选择(auto: hevc_nvenc → libx265 速度达标 → libx264 回退)
10
+ 与参数映射
11
+
12
+ 选择结果进程内缓存(同一进程内机器不变不重探;负结果也缓存)。探针
13
+ 同步执行、最坏 ~10-20s;所有探针异常按「该编码器不可用」降级处理。
14
+ """
15
+ from __future__ import annotations
16
+
17
+ import dataclasses
18
+ import shutil
19
+ import subprocess
20
+ import threading
21
+ import time
22
+ from typing import Callable, Dict, List, Optional
23
+
24
+ from config import settings
25
+
26
+ # ── 进程内缓存 ────────────────────────────────────────
27
+ _FFMPEG_CACHE: Optional[List[str]] = None
28
+ _ENCODER_CACHE: Dict[tuple, bool] = {}
29
+ _SELECT_CACHE: Dict[tuple, Optional["EncoderChoice"]] = {}
30
+
31
+
32
+ @dataclasses.dataclass
33
+ class EncoderChoice:
34
+ """一次编码器选择的结果。"""
35
+ encoder: str # "hevc_nvenc" | "libx265" | "libx264"
36
+ codec: str # "HEVC" | "H.264"
37
+ crf: int # 本选择实际用的档位(x264 用回退档)
38
+ args: list # ffmpeg 编码参数(不含 -c:v)
39
+ ffmpeg: str # 采用的 ffmpeg 二进制
40
+ label: str # UI 显示名,如 "HEVC (hevc_nvenc)"
41
+ probe: dict # 探针明细(写 metadata.json)
42
+ selected_by: str # settings.RECORD_VIDEO_ENCODER 原值
43
+
44
+
45
+ # ═══════════════════════════════════════════════════════
46
+ # ffmpeg 二进制枚举 / 编码器能力
47
+ # ═══════════════════════════════════════════════════════
48
+
49
+ def list_working_ffmpegs() -> List[str]:
50
+ """候选 ffmpeg 二进制(imageio 静态 → PATH),经 -version 自检。
51
+
52
+ 全部自检失败时,仍把 imageio 静态二进制作为最后兜底返回(与旧
53
+ _get_ffmpeg 口径一致:它已在录制段被证明可用)。
54
+ """
55
+ global _FFMPEG_CACHE
56
+ if _FFMPEG_CACHE is not None:
57
+ return list(_FFMPEG_CACHE)
58
+ candidates: List[str] = []
59
+ imageio_exe = None
60
+ try:
61
+ import imageio_ffmpeg
62
+ imageio_exe = imageio_ffmpeg.get_ffmpeg_exe()
63
+ candidates.append(imageio_exe)
64
+ except Exception:
65
+ pass
66
+ for name in ("ffmpeg", "ffmpeg.exe"):
67
+ p = shutil.which(name)
68
+ if p and p not in candidates:
69
+ candidates.append(p)
70
+ working = []
71
+ for exe in candidates:
72
+ try:
73
+ r = subprocess.run([exe, "-version"], capture_output=True,
74
+ timeout=10)
75
+ if r.returncode == 0:
76
+ working.append(exe)
77
+ except Exception:
78
+ continue
79
+ if not working and imageio_exe:
80
+ working.append(imageio_exe) # 兜底:录段历史证明可用
81
+ _FFMPEG_CACHE = working
82
+ return list(working)
83
+
84
+
85
+ def ffmpeg_has_encoder(ffmpeg: str, encoder: str) -> bool:
86
+ """-encoders 输出里是否含该编码器(结果缓存)。"""
87
+ key = (ffmpeg, encoder)
88
+ if key in _ENCODER_CACHE:
89
+ return _ENCODER_CACHE[key]
90
+ found = False
91
+ try:
92
+ r = subprocess.run([ffmpeg, "-hide_banner", "-encoders"],
93
+ capture_output=True, timeout=30)
94
+ text = ((r.stdout or b"") + (r.stderr or b"")).decode("utf-8", "ignore")
95
+ found = encoder in text.split()
96
+ except Exception:
97
+ found = False
98
+ _ENCODER_CACHE[key] = found
99
+ return found
100
+
101
+
102
+ # ═══════════════════════════════════════════════════════
103
+ # 参数映射 / 微型探针
104
+ # ═══════════════════════════════════════════════════════
105
+
106
+ def encoder_args(encoder: str, crf: Optional[int] = None) -> List[str]:
107
+ """编码器参数映射(HEVC 一律带 hvc1 容器标记)。"""
108
+ if crf is None:
109
+ crf = settings.RECORD_VIDEO_CRF
110
+ table = {
111
+ "hevc_nvenc": ["-rc", "vbr", "-cq", str(crf), "-preset", "p4",
112
+ "-tag:v", "hvc1"],
113
+ "libx265": ["-crf", str(crf), "-preset", "veryfast", "-tag:v", "hvc1"],
114
+ "libx264": ["-preset", "ultrafast",
115
+ "-crf", str(settings.RECORD_VIDEO_X264_CRF)],
116
+ }
117
+ return list(table[encoder])
118
+
119
+
120
+ def _kill(p: subprocess.Popen) -> None:
121
+ try:
122
+ p.kill()
123
+ except Exception:
124
+ pass
125
+ try:
126
+ p.wait(timeout=3)
127
+ except Exception:
128
+ pass
129
+
130
+
131
+ def _feed_raw(proc: subprocess.Popen, frame_bytes: bytes, frames: int) -> None:
132
+ try:
133
+ for _ in range(frames):
134
+ proc.stdin.write(frame_bytes)
135
+ proc.stdin.flush()
136
+ finally:
137
+ try:
138
+ proc.stdin.close()
139
+ except Exception:
140
+ pass
141
+
142
+
143
+ def probe_nvenc(ffmpeg: str, width: int, height: int, fps: float,
144
+ crf: Optional[int] = None, timeout: int = 10) -> bool:
145
+ """NVENC 微型探针:~1s 编码(null muxer)确认 GPU 可用。"""
146
+ frames = max(3, int(fps))
147
+ cmd = [ffmpeg, "-y", "-f", "rawvideo", "-pix_fmt", "bgr24",
148
+ "-s", f"{width}x{height}", "-r", str(fps), "-i", "-",
149
+ "-frames:v", str(frames),
150
+ "-c:v", "hevc_nvenc", *encoder_args("hevc_nvenc", crf),
151
+ "-f", "null", "-"]
152
+ try:
153
+ p = subprocess.Popen(cmd, stdin=subprocess.PIPE,
154
+ stdout=subprocess.DEVNULL,
155
+ stderr=subprocess.DEVNULL)
156
+ _feed_raw(p, bytes(width * height * 3), frames)
157
+ return p.wait(timeout=timeout) == 0
158
+ except Exception:
159
+ return False
160
+
161
+
162
+ def probe_x265(ffmpeg: str, width: int, height: int, fps: float,
163
+ n_streams: int, frames: Optional[int] = None,
164
+ timeout: Optional[int] = None) -> dict:
165
+ """x265 速度探针:并行 n 流各编 frames 帧 → null,返回每流实测 fps。
166
+
167
+ n_streams = 本会话视频流数(封顶 ENCODER_PROBE_MAX_STREAMS)。全部
168
+ 流并行跑满同一墙钟,每流 fps = frames / 总耗时 —— 与写盘线程每 tick
169
+ 串行写全部槽的背压口径近似一致。合成帧为全零帧(编码最快),1.5×
170
+ 门槛余量即为此乐观偏差所设。
171
+ """
172
+ if frames is None:
173
+ frames = settings.ENCODER_PROBE_FRAME_COUNT
174
+ if timeout is None:
175
+ timeout = settings.ENCODER_PROBE_TIMEOUT_S
176
+ n = max(1, min(int(n_streams), settings.ENCODER_PROBE_MAX_STREAMS))
177
+ frame_bytes = bytes(width * height * 3)
178
+ cmd_base = [ffmpeg, "-y", "-f", "rawvideo", "-pix_fmt", "bgr24",
179
+ "-s", f"{width}x{height}", "-r", str(fps), "-i", "-",
180
+ "-frames:v", str(frames),
181
+ "-c:v", "libx265", *encoder_args("libx265"),
182
+ "-f", "null", "-"]
183
+ procs: List[subprocess.Popen] = []
184
+ start = time.monotonic()
185
+ ok = True
186
+ try:
187
+ for _ in range(n):
188
+ procs.append(subprocess.Popen(
189
+ cmd_base, stdin=subprocess.PIPE, stdout=subprocess.DEVNULL,
190
+ stderr=subprocess.DEVNULL))
191
+ feeders = [threading.Thread(target=_feed_raw, args=(p, frame_bytes, frames),
192
+ daemon=True) for p in procs]
193
+ for t in feeders:
194
+ t.start()
195
+ for p in procs:
196
+ p.wait(timeout=timeout)
197
+ except subprocess.TimeoutExpired:
198
+ ok = False
199
+ for p in procs:
200
+ if p.poll() is None:
201
+ _kill(p)
202
+ ok = False
203
+ elif p.returncode != 0:
204
+ ok = False
205
+ elapsed = time.monotonic() - start
206
+ if not ok or elapsed <= 0:
207
+ return {"per_stream_fps": [], "min_fps": 0.0, "passed": False,
208
+ "streams": n, "frames": frames, "elapsed_s": round(elapsed, 2)}
209
+ per = [frames / elapsed] * n
210
+ min_fps = min(per)
211
+ threshold = fps * settings.ENCODER_X265_MIN_FPS_RATIO
212
+ return {"per_stream_fps": [round(x, 1) for x in per],
213
+ "min_fps": round(min_fps, 1), "passed": min_fps >= threshold,
214
+ "threshold_fps": round(threshold, 1),
215
+ "streams": n, "frames": frames, "elapsed_s": round(elapsed, 2)}
216
+
217
+
218
+ def probe_depth_12bit(ffmpeg: str, timeout: int = 8) -> bool:
219
+ """深度 12-bit 灰度编码能力探针:gray12le → libx265 → null。
220
+
221
+ x265 版本/构建差异下 gray12le(4:0:0,hevc Rext)未必可用;
222
+ 单帧微型编码(32×32 → null)确认能力,结果进程内缓存。
223
+ 失败时调用方回落 FFV1 gray16le MKV(无损 16-bit 旧格式)。
224
+ """
225
+ key = (ffmpeg, "depth12")
226
+ if key in _ENCODER_CACHE:
227
+ return _ENCODER_CACHE[key]
228
+ ok = False
229
+ try:
230
+ p = subprocess.Popen(
231
+ [ffmpeg, "-y", "-f", "rawvideo", "-pix_fmt", "gray12le",
232
+ "-s", "32x32", "-r", "30", "-i", "-",
233
+ "-frames:v", "1", "-c:v", "libx265",
234
+ "-x265-params", "qp=6:range=full", "-f", "null", "-"],
235
+ stdin=subprocess.PIPE, stdout=subprocess.DEVNULL,
236
+ stderr=subprocess.DEVNULL)
237
+ _feed_raw(p, bytes(32 * 32 * 2), 1)
238
+ ok = p.wait(timeout=timeout) == 0
239
+ except Exception:
240
+ ok = False
241
+ _ENCODER_CACHE[key] = ok
242
+ return ok
243
+
244
+
245
+ def find_depth_12bit_ffmpeg() -> Optional[str]:
246
+ """候选二进制中第一个支持 gray12le 12-bit 编码的;全不支持返回 None。"""
247
+ return next((b for b in list_working_ffmpegs() if probe_depth_12bit(b)),
248
+ None)
249
+
250
+
251
+ # ═══════════════════════════════════════════════════════
252
+ # 选择入口
253
+ # ═══════════════════════════════════════════════════════
254
+
255
+ def select_encoder(prefer: str, width: int, height: int, fps: float,
256
+ n_streams: int, log: Optional[Callable[[str], None]] = None
257
+ ) -> Optional[EncoderChoice]:
258
+ """选择录制编码器。返回 EncoderChoice,全链失败返回 None。
259
+
260
+ prefer: settings.RECORD_VIDEO_ENCODER("auto"|"nvenc"|"x265"|"x264")
261
+ auto 链: hevc_nvenc(探针可用) → libx265(速度达标) → libx264(兜底)
262
+ 显式指定: 跳过速度门槛强制用该编码器;二进制不支持/探针失败时按
263
+ auto 链回退并播报原因。
264
+ """
265
+ if log is None:
266
+ log = lambda msg: None
267
+ cache_key = (prefer, width, height, fps,
268
+ min(int(n_streams), settings.ENCODER_PROBE_MAX_STREAMS))
269
+ if cache_key in _SELECT_CACHE:
270
+ return _SELECT_CACHE[cache_key]
271
+ bins = list_working_ffmpegs()
272
+ if not bins:
273
+ log("[编码器] 未找到可用 ffmpeg 二进制")
274
+ _SELECT_CACHE[cache_key] = None
275
+ return None
276
+
277
+ def find_with(enc: str) -> Optional[str]:
278
+ return next((b for b in bins if ffmpeg_has_encoder(b, enc)), None)
279
+
280
+ def make(enc: str, ffmpeg: str, probe: Optional[dict] = None) -> EncoderChoice:
281
+ is_hevc = enc != "libx264"
282
+ crf = settings.RECORD_VIDEO_CRF if is_hevc else settings.RECORD_VIDEO_X264_CRF
283
+ return EncoderChoice(
284
+ encoder=enc,
285
+ codec="HEVC" if is_hevc else "H.264",
286
+ crf=crf,
287
+ args=encoder_args(enc),
288
+ ffmpeg=ffmpeg,
289
+ label=("HEVC (%s)" % enc) if is_hevc else "H.264 (libx264)",
290
+ probe=probe or {},
291
+ selected_by=prefer)
292
+
293
+ def auto_chain(why: Optional[str] = None) -> Optional[EncoderChoice]:
294
+ if why:
295
+ log(f"[编码器] {why},按 auto 链回退")
296
+ exe = find_with("hevc_nvenc")
297
+ if exe and probe_nvenc(exe, width, height, fps,
298
+ settings.RECORD_VIDEO_CRF):
299
+ log("[编码器] hevc_nvenc 探针通过 → HEVC 硬件编码")
300
+ return make("hevc_nvenc", exe, {"kind": "nvenc", "ok": True})
301
+ if exe:
302
+ log("[编码器] hevc_nvenc 探针失败(GPU 不可用),跳过")
303
+ exe = find_with("libx265")
304
+ if exe:
305
+ probe = probe_x265(exe, width, height, fps, n_streams)
306
+ if probe["passed"]:
307
+ log(f"[编码器] libx265 实测 {probe['min_fps']:.0f} fps/流 "
308
+ f"(门槛 {probe['threshold_fps']:.0f}) → HEVC 软编")
309
+ return make("libx265", exe, probe)
310
+ log(f"[编码器] libx265 实测 {probe['min_fps']:.0f} fps/流 "
311
+ f"(门槛 {probe['threshold_fps']:.0f}) → 不达标")
312
+ exe = find_with("libx264")
313
+ if exe:
314
+ log(f"[编码器] 回退 H.264 (libx264 ultrafast, "
315
+ f"CRF{settings.RECORD_VIDEO_X264_CRF})")
316
+ return make("libx264", exe)
317
+ return None
318
+
319
+ if prefer == "nvenc":
320
+ exe = find_with("hevc_nvenc")
321
+ if exe and probe_nvenc(exe, width, height, fps,
322
+ settings.RECORD_VIDEO_CRF):
323
+ log("[编码器] 显式 nvenc:hevc_nvenc 探针通过")
324
+ choice = make("hevc_nvenc", exe, {"kind": "nvenc", "ok": True})
325
+ else:
326
+ choice = auto_chain("显式 nvenc 但 hevc_nvenc 探针失败")
327
+ elif prefer == "x265":
328
+ exe = find_with("libx265")
329
+ if exe:
330
+ log("[编码器] 显式 x265:跳过速度门槛,强制 HEVC 软编")
331
+ choice = make("libx265", exe)
332
+ else:
333
+ choice = auto_chain("显式 x265 但无二进制支持 libx265")
334
+ elif prefer == "x264":
335
+ exe = find_with("libx264")
336
+ if exe:
337
+ log("[编码器] 显式 x264:跳过探针,直接 H.264")
338
+ choice = make("libx264", exe)
339
+ else:
340
+ choice = auto_chain("显式 x264 但无二进制支持 libx264")
341
+ else: # auto
342
+ choice = auto_chain()
343
+
344
+ _SELECT_CACHE[cache_key] = choice
345
+ return choice
collector/core/exposure_controller.py ADDED
@@ -0,0 +1,80 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 每设备曝光控制 —— ☀ 对话框参数解析与下发/持久化口径(零 Qt 依赖)。
3
+
4
+ 供主窗口曝光对话框入口与离线测试共用同一套口径:
5
+ - exposure_dialog_params 按设备类型解析对话框上下文(量程/自动/当前值/
6
+ 恢复默认基线 + 首见基线落盘锁定);UVC 返回 None
7
+ - apply_exposure 按设备类型下发并持久化(下次开启自动应用)
8
+ """
9
+
10
+ from __future__ import annotations
11
+
12
+ from config import settings
13
+ from core.device_naming import normalize_original
14
+
15
+
16
+ def exposure_dialog_params(kind: str, entry: dict,
17
+ dev_key: str) -> dict | None:
18
+ """☀ 曝光对话框参数解析(对话框只做 0..1000 刻度映射)。
19
+
20
+ 量程/单位语义:
21
+ D435 → µs,流启动后读真实 rs.option.exposure 量程
22
+ S80M → SDK 曝光 1.0~885.0(与 yaml stereo_init_exposure 同单位)
23
+ UVC 无曝光入口(固定自动曝光,保证正常亮度 + 30fps)→ 返回 None。
24
+
25
+ 「恢复默认」基线(original):优先持久化的首见基线
26
+ (settings.device_original,首次开启设备时锁定),无则用
27
+ worker 本次开流读回的硬件状态,并在对话框弹出时落盘锁定
28
+ (之后 worker 读回值不再覆盖)。
29
+ """
30
+ if kind == "uvc":
31
+ return None
32
+ label = entry["label"]
33
+ if kind == "d435":
34
+ rng, auto, value = entry["worker"].exposure_info()
35
+ if rng is None:
36
+ rng = (1.0, 66000.0) # 流未启动兜底量程(µs)
37
+ if auto is None:
38
+ auto = True
39
+ if value is None:
40
+ value = rng[1] / 2.0
41
+ orig = settings.device_original(dev_key)
42
+ if orig is None:
43
+ orig = entry["worker"].original_exposure()
44
+ decimals = 0
45
+ elif kind == "s80m":
46
+ exp = settings.device_exposure(dev_key)
47
+ auto = True if exp is None else exp["auto"]
48
+ value = 400.0 if exp is None else exp["value"]
49
+ orig = settings.device_original(dev_key)
50
+ if orig is None:
51
+ orig = entry.get("original_exp")
52
+ rng = (1.0, 885.0)
53
+ decimals = 1
54
+ else:
55
+ return None
56
+
57
+ orig = normalize_original(orig)
58
+ if orig is not None:
59
+ settings.ensure_device_original(dev_key, orig[0], orig[1])
60
+ return {"label": label, "rng": rng, "value": float(value),
61
+ "auto": bool(auto), "decimals": decimals, "original": orig}
62
+
63
+
64
+ def apply_exposure(dev_key: str, entry: dict, auto: bool, value: float,
65
+ send_s80m=None) -> None:
66
+ """按设备类型下发曝光并持久化(下次开启自动应用)。
67
+
68
+ UVC 无曝光入口(固定自动曝光),不会进入此函数。
69
+ S80M 的 stdin 行协议由 send_s80m(entry, auto, value) 回调提供
70
+ (见 core.s80m_manager.S80MDeviceManager.send_exposure)。
71
+ """
72
+ kind = entry["kind"]
73
+ if kind == "d435":
74
+ entry["worker"].set_exposure(auto, value)
75
+ elif kind == "s80m":
76
+ if send_s80m is not None:
77
+ send_s80m(entry, auto, value)
78
+ else:
79
+ return
80
+ settings.save_device_exposure(dev_key, auto, value)
collector/core/glove_keypoint_solver.py ADDED
@@ -0,0 +1,236 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ #!/usr/bin/env python3
2
+ # -*- coding: utf-8 -*-
3
+ """手套骨架解算封装:16×BNO055 四元数 → MANO 21 关键点(米)。
4
+
5
+ 复用手套工具包(stouch_glove_toolkit*,与 glove_devices.json 同级)的
6
+ HandSolver:运行时懒导入,工具包目录缺失 / 导入或构造失败时
7
+ available()=False,调用方跳过骨架列即可(触觉/IMU 录制不受影响)。
8
+
9
+ present_mask 近似取 valid_mask:本工程传输层不区分"物理在位"位域,
10
+ 缺失传感器必为无效样本(valid=False),工具链按有效样本求解。
11
+ """
12
+ import json
13
+ import os
14
+ import sys
15
+ import time
16
+
17
+ import numpy as np
18
+
19
+ # 进程内共享的懒导入结果(首次 import scipy 链需要 1s 左右)
20
+ _HAND_SOLVER_CLS = None
21
+ _RAW_FRAME_CLS = None
22
+ _IMPORT_ERROR = None
23
+
24
+ # 工具包标定选择器拒绝的标定类型(与 apps/gui/calibration_selector.py
25
+ # 的 discover_calibration_files 过滤口径一致)
26
+ _DIRECT_FK_KINEMATICS = {"direct_fk", "fitted_direct_fk"}
27
+ _DIRECT_FK_TOOLS = {"direct_fk_calibrate_cli", "hand_fk_fit_cli"}
28
+
29
+
30
+ def _calibration_ok(path: str, side: str) -> bool:
31
+ """标定文件是否可用于该手侧(载荷解析 + 过滤口径)。"""
32
+ try:
33
+ with open(path, "r", encoding="utf-8") as f:
34
+ payload = json.load(f)
35
+ except (OSError, ValueError):
36
+ return False
37
+ if str(payload.get("side") or "").lower() != side:
38
+ return False
39
+ if not isinstance(payload.get("param_inst_calib"), dict):
40
+ return False
41
+ if payload.get("kinematics") in _DIRECT_FK_KINEMATICS:
42
+ return False
43
+ return str(payload.get("tool") or "") not in _DIRECT_FK_TOOLS
44
+
45
+
46
+ def _calibration_ts(path: str) -> float:
47
+ """标定生成时间(payload generated_at → epoch;解析失败用文件 mtime)。"""
48
+ try:
49
+ with open(path, "r", encoding="utf-8") as f:
50
+ payload = json.load(f)
51
+ ts = payload.get("generated_at")
52
+ if ts:
53
+ from datetime import datetime
54
+ return datetime.fromisoformat(str(ts)).timestamp()
55
+ except (OSError, ValueError, TypeError, ImportError):
56
+ pass
57
+ try:
58
+ return os.path.getmtime(path)
59
+ except OSError:
60
+ return 0.0
61
+
62
+
63
+ def pick_calibration(toolkit_dir: str, side: str) -> str:
64
+ """该手侧要用的标定 JSON 路径;全无可用时返回 ""。
65
+
66
+ 优先级(对应「连接手套后先用标定 GUI 做标定」的工作流:
67
+ GUI 每次保存带时间戳的新文件 → 该手套最新的标定自动被采用):
68
+ 1) calibration/ 下按 generated_at 最新的该侧标定(跳过
69
+ _default 与 2d 标定 —— 口径见 calibration_selector)
70
+ 2) calibration/imu_calibration_{side}_default.json(随包默认标定)
71
+
72
+ 注意: 标定文件只按手侧区分、不含 USB 序列号 —— 换用新手套后
73
+ 若未重新标定,会沿用旧手套的标定(日志会标明实际采用的文件)。
74
+ """
75
+ cal_dir = os.path.join(toolkit_dir, "calibration")
76
+ if not os.path.isdir(cal_dir):
77
+ return ""
78
+ best, best_ts = "", 0.0
79
+ try:
80
+ names = sorted(os.listdir(cal_dir))
81
+ except OSError:
82
+ names = []
83
+ for name in names:
84
+ if not name.lower().endswith(".json"):
85
+ continue
86
+ if name == f"imu_calibration_{side}_default.json":
87
+ continue
88
+ if "2d" in name.lower():
89
+ continue
90
+ path = os.path.join(cal_dir, name)
91
+ if not _calibration_ok(path, side):
92
+ continue
93
+ ts = _calibration_ts(path)
94
+ if ts > best_ts:
95
+ best, best_ts = path, ts
96
+ if best:
97
+ return best
98
+ dflt = os.path.join(cal_dir, f"imu_calibration_{side}_default.json")
99
+ return dflt if _calibration_ok(dflt, side) else ""
100
+
101
+
102
+ def find_toolkit_dir() -> str:
103
+ """项目根同级的 stouch_glove_toolkit* 目录(照 device_detector 的查找模式)。"""
104
+ root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
105
+ try:
106
+ for name in sorted(os.listdir(root)):
107
+ if name.lower().startswith("stouch_glove_toolkit"):
108
+ path = os.path.join(root, name)
109
+ if os.path.isdir(path):
110
+ return path
111
+ except OSError:
112
+ pass
113
+ return ""
114
+
115
+
116
+ def _ensure_imported() -> str:
117
+ """确保 toolkit 模块已导入;返回错误描述(成功返回 "")。"""
118
+ global _HAND_SOLVER_CLS, _RAW_FRAME_CLS, _IMPORT_ERROR
119
+ if _HAND_SOLVER_CLS is not None:
120
+ return ""
121
+ if _IMPORT_ERROR is not None:
122
+ return _IMPORT_ERROR
123
+ toolkit_dir = find_toolkit_dir()
124
+ if not toolkit_dir:
125
+ _IMPORT_ERROR = "未找到工具包目录(项目根同级 stouch_glove_toolkit*)"
126
+ return _IMPORT_ERROR
127
+ try:
128
+ if toolkit_dir not in sys.path:
129
+ sys.path.insert(0, toolkit_dir)
130
+ from glove_sdk.interfaces.solver import HandSolver # noqa: F401
131
+ from glove_sdk.types import RawImuFrame # noqa: F401
132
+ _HAND_SOLVER_CLS = HandSolver
133
+ _RAW_FRAME_CLS = RawImuFrame
134
+ return ""
135
+ except Exception as exc: # 缺依赖/版本不兼容等,全部降级
136
+ _IMPORT_ERROR = f"{type(exc).__name__}: {exc}"
137
+ return _IMPORT_ERROR
138
+
139
+
140
+ class GloveKeypointSolver:
141
+ """单只手套的 21 关键点解算器(GloveDataPump 在连接时创建)。
142
+
143
+ Args:
144
+ side: "left" / "right"(决定标定文件,见 pick_calibration:优先该
145
+ 手套自己的标定,其次最新标定,最后随包默认标定)
146
+ on_error: 工具包不可用/初始化失败时回调一次(接主窗口日志)
147
+ on_warmup: 解算 warmup 完成时回调一次(骨架列开始有有效数据)
148
+ """
149
+
150
+ def __init__(self, side: str, on_error=None, on_warmup=None):
151
+ self.side = side if side in ("left", "right") else "right"
152
+ self._on_error = on_error or (lambda msg: None)
153
+ self._on_warmup = on_warmup or (lambda: None)
154
+ self._solver = None
155
+ self._warm = False
156
+ self._seq = 0
157
+ self.calibration_name = "" # 实际采用的标定文件名(日志用)
158
+ self._load()
159
+
160
+ # ── 初始化 ──────────────────────────────────────
161
+
162
+ def _load(self):
163
+ err = _ensure_imported()
164
+ if err:
165
+ self._on_error(f"骨架解算不可用: {err}")
166
+ return
167
+ toolkit_dir = find_toolkit_dir()
168
+ calibration = pick_calibration(toolkit_dir, self.side)
169
+ if not calibration:
170
+ self._on_error(f"骨架解算不可用: 无 {self.side} 手标定文件")
171
+ return
172
+ self.calibration_name = os.path.basename(calibration)
173
+ geometry = os.path.join(
174
+ toolkit_dir, "assets", "hand_geometry",
175
+ "hand_measured_runtime_v1.json")
176
+ try:
177
+ self._solver = _HAND_SOLVER_CLS(self.side, calibration, geometry)
178
+ except Exception as exc:
179
+ self._solver = None
180
+ self._on_error(f"骨架解算初始化失败: {type(exc).__name__}: {exc}")
181
+
182
+ def available(self) -> bool:
183
+ return self._solver is not None
184
+
185
+ @property
186
+ def warmup_completed(self) -> bool:
187
+ return self._warm
188
+
189
+ # ── 解算 ────────────────────────────────────────
190
+
191
+ def process(self, quats, valid, device_ts_us: int = 0):
192
+ """→ (21,3) float32 关键点(米);未就绪/未 warmup/异常返回 None。
193
+
194
+ Args:
195
+ quats: 64×float32 展平的 16×4 XYZW 四元数
196
+ valid: 16×bool/float32 有效掩码(展平)
197
+ device_ts_us: 帧的设备时间戳(微秒),0 时用宿主时钟
198
+ """
199
+ if self._solver is None:
200
+ return None
201
+ try:
202
+ q = np.asarray(quats, dtype=np.float64).reshape(16, 4)
203
+ v = np.asarray(valid).reshape(16) > 0.5
204
+ except ValueError:
205
+ return None
206
+ if not np.isfinite(q).all():
207
+ return None
208
+ self._seq += 1
209
+ host_us = time.time_ns() // 1_000
210
+ frame = _RAW_FRAME_CLS(
211
+ sequence=self._seq,
212
+ # 无设备时间戳时用宿主时钟占位(恒等映射,避免时间轴跳变)
213
+ device_timestamp_us=int(device_ts_us) if device_ts_us else host_us,
214
+ host_timestamp_us=host_us,
215
+ quaternions_xyzw=q,
216
+ present_mask=v,
217
+ valid_mask=v,
218
+ )
219
+ try:
220
+ kf = self._solver.process(frame)
221
+ except Exception as exc:
222
+ if self._warm:
223
+ return None
224
+ self._on_error(f"骨架解算异常: {type(exc).__name__}: {exc}")
225
+ return None
226
+ if not self._warm:
227
+ details = getattr(getattr(kf, "status", None), "details", {}) or {}
228
+ self._warm = bool(details.get("warmup_completed", False))
229
+ if self._warm:
230
+ self._on_warmup()
231
+ if not self._warm:
232
+ return None
233
+ joints = np.asarray(kf.joints_m, dtype=np.float32)
234
+ if joints.shape != (21, 3) or not np.isfinite(joints).all():
235
+ return None
236
+ return joints
collector/core/glove_usb/__init__.py ADDED
@@ -0,0 +1,22 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Stouch STM32 手套 USB CDC 协议栈(自工具包移植的明文子集)。
2
+
3
+ 只依赖 numpy + pyserial,无 PyArmor/许可证负担。来源:
4
+ stouch_glove_toolkit-Stouch_Glove_Toolkit-beta 的 glove_io/ + glove_sdk/
5
+ 明文模块(errors/types/usb_protocol/streams/tactile_processing),
6
+ 导入路径改为 core.glove_usb.*,其余代码与上游保持一致。
7
+ """
8
+
9
+ from core.glove_usb.streams import RawImuStream, SensorStream, TactileStream
10
+ from core.glove_usb.tactile_processing import TactilePreprocessor
11
+ from core.glove_usb.types import RawImuFrame, TactileFrame
12
+ from core.glove_usb.usb_protocol import (
13
+ STM32_USB_PID,
14
+ STM32_USB_VID,
15
+ find_stm32_cdc_port,
16
+ )
17
+
18
+ __all__ = [
19
+ "RawImuStream", "SensorStream", "TactileStream",
20
+ "TactilePreprocessor", "RawImuFrame", "TactileFrame",
21
+ "STM32_USB_PID", "STM32_USB_VID", "find_stm32_cdc_port",
22
+ ]
collector/core/glove_usb/errors.py ADDED
@@ -0,0 +1,53 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Stable public exception hierarchy for the modular glove SDK."""
2
+
3
+
4
+ class GloveSdkError(RuntimeError):
5
+ """Base class for all supported SDK errors."""
6
+
7
+
8
+ class DeviceNotFoundError(GloveSdkError):
9
+ pass
10
+
11
+
12
+ class DeviceBindingError(GloveSdkError):
13
+ pass
14
+
15
+
16
+ class DeviceBusyError(GloveSdkError):
17
+ pass
18
+
19
+
20
+ class StreamError(GloveSdkError):
21
+ pass
22
+
23
+
24
+ class StreamTimeoutError(StreamError):
25
+ pass
26
+
27
+
28
+ class StreamClosedError(StreamError):
29
+ pass
30
+
31
+
32
+ class CalibrationError(GloveSdkError):
33
+ pass
34
+
35
+
36
+ class CalibrationQualityError(CalibrationError):
37
+ pass
38
+
39
+
40
+ class CalibrationCompatibilityError(CalibrationError):
41
+ pass
42
+
43
+
44
+ class RecordingError(GloveSdkError):
45
+ pass
46
+
47
+
48
+ class ReplayError(GloveSdkError):
49
+ pass
50
+
51
+
52
+ class UnsupportedPlatformError(GloveSdkError):
53
+ pass
collector/core/glove_usb/streams.py ADDED
@@ -0,0 +1,385 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Plaintext STM32 USB acquisition used by the public sensor interfaces.
2
+
3
+ The transport only validates and decodes the firmware wire format. It does
4
+ not remap sensors, correct axes, filter orientations, calibrate a glove, or
5
+ solve hand keypoints. Those operations belong to the protected core behind
6
+ the public 21-keypoint interface.
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ from queue import Empty, Full, Queue
12
+ import threading
13
+ import time
14
+ from typing import Callable, Iterator
15
+
16
+ import numpy as np
17
+
18
+ from core.glove_usb.usb_protocol import (
19
+ STM32_USB_PID,
20
+ STM32_USB_VID,
21
+ is_supported_version,
22
+ USB_TYPE_ADC_MATRIX,
23
+ USB_TYPE_IMU_Q14,
24
+ USB_TYPE_STATUS,
25
+ UsbCdcFrameParser,
26
+ decode_adc_matrix_payload,
27
+ decode_imu_q14_payload,
28
+ find_stm32_cdc_port,
29
+ plausible_quaternion_mask,
30
+ )
31
+ from core.glove_usb.errors import StreamClosedError, StreamTimeoutError
32
+ from core.glove_usb.types import RawImuFrame, TactileFrame
33
+
34
+
35
+ StatusCallback = Callable[[str], None]
36
+
37
+
38
+ def _put_latest(queue: Queue, value) -> None:
39
+ """Put a value without allowing a slow consumer to block acquisition."""
40
+
41
+ while True:
42
+ try:
43
+ queue.put_nowait(value)
44
+ return
45
+ except Full:
46
+ try:
47
+ queue.get_nowait()
48
+ except Empty:
49
+ return
50
+
51
+
52
+ def _drain(queue: Queue, maximum: int | None = None) -> list:
53
+ values = []
54
+ limit = None if maximum is None else max(0, int(maximum))
55
+ while limit is None or len(values) < limit:
56
+ try:
57
+ values.append(queue.get_nowait())
58
+ except Empty:
59
+ break
60
+ return values
61
+
62
+
63
+ class _UsbSensorTransport:
64
+ """One serial owner that multiplexes raw IMU and tactile frames."""
65
+
66
+ def __init__(
67
+ self,
68
+ serial_port: str | None,
69
+ usb_vid: int,
70
+ usb_pid: int,
71
+ imu_queue_size: int,
72
+ tactile_queue_size: int,
73
+ status_callback: StatusCallback | None,
74
+ ):
75
+ self.requested_port = serial_port
76
+ self.usb_vid = int(usb_vid)
77
+ self.usb_pid = int(usb_pid)
78
+ self.imu_queue: Queue[RawImuFrame] = Queue(
79
+ maxsize=max(2, int(imu_queue_size)))
80
+ self.tactile_queue: Queue[TactileFrame] = Queue(
81
+ maxsize=max(2, int(tactile_queue_size)))
82
+ self.status_callback = status_callback
83
+ self.stop_event = threading.Event()
84
+ self.thread: threading.Thread | None = None
85
+ self.active_port: str | None = None
86
+ self.connected = False
87
+ self.last_error: str | None = None
88
+ self.last_status: str = "idle"
89
+ self._state_lock = threading.RLock()
90
+
91
+ def _set_status(self, value: str, error: str | None = None) -> None:
92
+ callback = None
93
+ with self._state_lock:
94
+ changed = value != self.last_status or error != self.last_error
95
+ self.last_status = value
96
+ self.last_error = error
97
+ if changed:
98
+ callback = self.status_callback
99
+ if callback is not None:
100
+ callback(value)
101
+
102
+ def start(self) -> None:
103
+ if self.thread is not None and self.thread.is_alive():
104
+ return
105
+ self.stop_event.clear()
106
+ self.thread = threading.Thread(
107
+ target=self._run,
108
+ name="stouch-usb-sensor",
109
+ daemon=True,
110
+ )
111
+ self.thread.start()
112
+
113
+ def stop(self, timeout_s: float = 3.0) -> None:
114
+ self.stop_event.set()
115
+ thread = self.thread
116
+ if thread is not None and thread.is_alive():
117
+ thread.join(timeout=max(0.0, float(timeout_s)))
118
+ with self._state_lock:
119
+ self.connected = False
120
+ self._set_status("stopped")
121
+
122
+ def _run(self) -> None:
123
+ try:
124
+ import serial
125
+ except ImportError as exc:
126
+ self._set_status("error", "pyserial is not installed")
127
+ return
128
+
129
+ parser = UsbCdcFrameParser()
130
+ serial_handle = None
131
+ while not self.stop_event.is_set():
132
+ if serial_handle is None:
133
+ try:
134
+ port = self.requested_port or find_stm32_cdc_port(
135
+ self.usb_vid, self.usb_pid)
136
+ if not port:
137
+ self._set_status(
138
+ "waiting",
139
+ f"STM32 USB CDC {self.usb_vid:04X}:{self.usb_pid:04X} not found",
140
+ )
141
+ self.stop_event.wait(0.5)
142
+ continue
143
+ serial_handle = serial.Serial(
144
+ port, baudrate=115200, timeout=0.2)
145
+ parser.reset()
146
+ with self._state_lock:
147
+ self.active_port = str(port)
148
+ self.connected = True
149
+ self._set_status("connected", None)
150
+ except (OSError, serial.SerialException) as exc:
151
+ with self._state_lock:
152
+ self.connected = False
153
+ self._set_status("waiting", f"cannot open serial port: {exc}")
154
+ serial_handle = None
155
+ self.stop_event.wait(0.5)
156
+ continue
157
+
158
+ try:
159
+ data = serial_handle.read(4096)
160
+ except (OSError, serial.SerialException) as exc:
161
+ self._set_status("disconnected", f"serial read failed: {exc}")
162
+ with self._state_lock:
163
+ self.connected = False
164
+ try:
165
+ serial_handle.close()
166
+ except Exception:
167
+ pass
168
+ serial_handle = None
169
+ self.stop_event.wait(0.2)
170
+ continue
171
+
172
+ if not data:
173
+ continue
174
+ for wire_frame in parser.feed(data):
175
+ host_timestamp_us = time.time_ns() // 1_000
176
+ if not is_supported_version(wire_frame.version):
177
+ self._set_status(
178
+ "error",
179
+ f"unsupported USB protocol version {wire_frame.version}",
180
+ )
181
+ continue
182
+ try:
183
+ if wire_frame.message_type == USB_TYPE_IMU_Q14:
184
+ quaternions = decode_imu_q14_payload(wire_frame.payload)
185
+ present = np.asarray([
186
+ bool(wire_frame.flags & (1 << index))
187
+ for index in range(16)
188
+ ], dtype=bool)
189
+ plausible = plausible_quaternion_mask(quaternions)
190
+ _put_latest(self.imu_queue, RawImuFrame(
191
+ sequence=int(wire_frame.sequence),
192
+ device_timestamp_us=int(wire_frame.timestamp_us),
193
+ host_timestamp_us=int(host_timestamp_us),
194
+ quaternions_xyzw=quaternions,
195
+ present_mask=present,
196
+ valid_mask=present & plausible,
197
+ ))
198
+ elif wire_frame.message_type == USB_TYPE_ADC_MATRIX:
199
+ matrix = decode_adc_matrix_payload(wire_frame.payload)
200
+ _put_latest(self.tactile_queue, TactileFrame(
201
+ sequence=int(matrix.sequence),
202
+ timestamp_us=int(matrix.scan_time_us),
203
+ samples=matrix.samples,
204
+ processed=False,
205
+ ))
206
+ elif wire_frame.message_type == USB_TYPE_STATUS:
207
+ status = wire_frame.payload.decode(
208
+ "utf-8", errors="replace").strip()
209
+ if status:
210
+ self._set_status(f"device: {status}", None)
211
+ except ValueError as exc:
212
+ self._set_status("error", f"invalid USB payload: {exc}")
213
+
214
+ if serial_handle is not None:
215
+ try:
216
+ serial_handle.close()
217
+ except Exception:
218
+ pass
219
+ with self._state_lock:
220
+ self.connected = False
221
+
222
+
223
+ class RawImuStream:
224
+ """Public physical-channel IMU stream.
225
+
226
+ Frames are exactly the decoded Q14 values in STM32 physical channel order.
227
+ No sensor mapping, axis conversion, smoothing, calibration, or FK is
228
+ applied. Use :class:`glove_sdk.HandSolver.process` for 21-keypoint output.
229
+ """
230
+
231
+ def __init__(
232
+ self,
233
+ serial_port: str | None = None,
234
+ *,
235
+ usb_vid: int = STM32_USB_VID,
236
+ usb_pid: int = STM32_USB_PID,
237
+ buffer_size: int = 256,
238
+ tactile_buffer_size: int = 2048,
239
+ status_callback: StatusCallback | None = None,
240
+ _transport: _UsbSensorTransport | None = None,
241
+ ):
242
+ self._transport = _transport or _UsbSensorTransport(
243
+ serial_port,
244
+ usb_vid,
245
+ usb_pid,
246
+ buffer_size,
247
+ tactile_buffer_size,
248
+ status_callback,
249
+ )
250
+
251
+ def start(self) -> "RawImuStream":
252
+ self._transport.start()
253
+ return self
254
+
255
+ def stop(self, timeout_s: float = 3.0) -> None:
256
+ self._transport.stop(timeout_s)
257
+
258
+ close = stop
259
+
260
+ def __enter__(self) -> "RawImuStream":
261
+ return self.start()
262
+
263
+ def __exit__(self, *exc) -> None:
264
+ self.stop()
265
+
266
+ @property
267
+ def connected(self) -> bool:
268
+ return self._transport.connected
269
+
270
+ @property
271
+ def port(self) -> str | None:
272
+ return self._transport.active_port
273
+
274
+ @property
275
+ def status(self) -> str:
276
+ return self._transport.last_status
277
+
278
+ @property
279
+ def last_error(self) -> str | None:
280
+ return self._transport.last_error
281
+
282
+ def read(self, timeout: float | None = None) -> RawImuFrame:
283
+ self.start()
284
+ try:
285
+ return self._transport.imu_queue.get(timeout=timeout)
286
+ except Empty as exc:
287
+ if self._transport.stop_event.is_set():
288
+ raise StreamClosedError("raw IMU stream is closed") from exc
289
+ raise StreamTimeoutError("timed out waiting for a raw IMU frame") from exc
290
+
291
+ def poll(self, maximum: int | None = None) -> list[RawImuFrame]:
292
+ """Return all currently buffered frames without blocking."""
293
+
294
+ return _drain(self._transport.imu_queue, maximum)
295
+
296
+ def frames(self, timeout: float = 0.25) -> Iterator[RawImuFrame]:
297
+ self.start()
298
+ while not self._transport.stop_event.is_set():
299
+ try:
300
+ yield self.read(timeout=timeout)
301
+ except StreamTimeoutError:
302
+ continue
303
+
304
+ def tactile_stream(self) -> "TactileStream":
305
+ """Return a tactile interface sharing this stream's serial handle."""
306
+
307
+ return TactileStream(imu=self)
308
+
309
+ def poll_tactile(self, maximum: int | None = None) -> list[TactileFrame]:
310
+ return _drain(self._transport.tactile_queue, maximum)
311
+
312
+
313
+ class TactileStream:
314
+ """Public raw 16x16 tactile stream.
315
+
316
+ Pass ``imu=raw_stream`` to share the same USB connection. A standalone
317
+ tactile stream owns its transport and discards unconsumed IMU frames.
318
+ """
319
+
320
+ def __init__(
321
+ self,
322
+ imu: RawImuStream | None = None,
323
+ *,
324
+ serial_port: str | None = None,
325
+ usb_vid: int = STM32_USB_VID,
326
+ usb_pid: int = STM32_USB_PID,
327
+ buffer_size: int = 2048,
328
+ status_callback: StatusCallback | None = None,
329
+ ):
330
+ self._owner = imu is None
331
+ self._imu = imu or RawImuStream(
332
+ serial_port,
333
+ usb_vid=usb_vid,
334
+ usb_pid=usb_pid,
335
+ buffer_size=16,
336
+ tactile_buffer_size=buffer_size,
337
+ status_callback=status_callback,
338
+ )
339
+
340
+ def start(self) -> "TactileStream":
341
+ self._imu.start()
342
+ return self
343
+
344
+ def stop(self, timeout_s: float = 3.0) -> None:
345
+ if self._owner:
346
+ self._imu.stop(timeout_s)
347
+
348
+ close = stop
349
+
350
+ def __enter__(self) -> "TactileStream":
351
+ return self.start()
352
+
353
+ def __exit__(self, *exc) -> None:
354
+ self.stop()
355
+
356
+ @property
357
+ def connected(self) -> bool:
358
+ return self._imu.connected
359
+
360
+ def read(self, timeout: float | None = None) -> TactileFrame:
361
+ self.start()
362
+ try:
363
+ return self._imu._transport.tactile_queue.get(timeout=timeout)
364
+ except Empty as exc:
365
+ if self._imu._transport.stop_event.is_set():
366
+ raise StreamClosedError("tactile stream is closed") from exc
367
+ raise StreamTimeoutError("timed out waiting for a tactile frame") from exc
368
+
369
+ def poll(self, maximum: int | None = None) -> list[TactileFrame]:
370
+ return self._imu.poll_tactile(maximum)
371
+
372
+ def frames(self, timeout: float = 0.25) -> Iterator[TactileFrame]:
373
+ self.start()
374
+ while not self._imu._transport.stop_event.is_set():
375
+ try:
376
+ yield self.read(timeout=timeout)
377
+ except StreamTimeoutError:
378
+ continue
379
+
380
+
381
+ # Explicit name for callers that want one object for both sensor families.
382
+ SensorStream = RawImuStream
383
+
384
+
385
+ __all__ = ["RawImuStream", "SensorStream", "TactileStream"]
collector/core/glove_usb/tactile_processing.py ADDED
@@ -0,0 +1,146 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Denoising pipeline for raw tactile frames, shared by the demo and hand GUIs.
2
+
3
+ This logic originally lived only in pc/tactile_hand_gui.py; the demo had its own
4
+ simplified version (a single-frame baseline plus one hard threshold), so the two
5
+ behaviors diverged. Extracted into one shared implementation so a change in one
6
+ place updates both, and they can no longer drift apart.
7
+
8
+ Pipeline order (line-by-line consistent with the hand implementation):
9
+
10
+ 1. Temporal smoothing history = history*α + raw*(1-α) removes frame-to-frame jitter
11
+ 2. Baseline subtraction max(0, history - baseline_map) per-cell zero level
12
+ 3. Drift removal 40th percentile > 100 → subtract 0.8x of it; keeps the noise floor down on global rises
13
+ 4. Noise gate zero below max(base_gate, min(peak*ratio, 250))
14
+ 5. Spatial filter zero isolated cells whose 8 neighbors are all 0 and whose value is below noise_floor
15
+
16
+ The baseline map is computed as [per-cell max over N frames + 10], which is more
17
+ conservative than the mean — the mean would fold occasional noise spikes into the signal.
18
+ """
19
+
20
+ import numpy as np
21
+
22
+ MATRIX_ROWS = 16
23
+ MATRIX_COLS = 16
24
+
25
+ # Cap on the dynamic gate. When the peak is large, the gate must not rise unboundedly, otherwise light presses get "drowned out" by heavy ones.
26
+ DYNAMIC_GATE_CAP = 250.0
27
+ # Lower bound for isolated-cell detection: cells below this are noise; a strong isolated cell is a real signal
28
+ ISOLATED_NOISE_FLOOR = 180.0
29
+ ISOLATED_BASELINE_SCALE = 2.5
30
+
31
+
32
+ class TactilePreprocessor:
33
+ """Process a raw (16,16) ADC frame into a usable pressure map.
34
+
35
+ :param base_gate: fixed noise gate (in ADC counts after baseline subtraction)
36
+ :param temporal_smooth: temporal smoothing factor α; larger is smoother but slower to react
37
+ :param dynamic_noise_ratio: dynamic gate = current peak × this value, combined with base_gate via max()
38
+ :param spatial_filter: whether to remove isolated cells
39
+ :param calibration_frames: how many frames to sample during auto zeroing
40
+ :param bypass_gates: when True, skip steps 4 and 5 (hand's --debug-view)
41
+ """
42
+
43
+ def __init__(self,
44
+ base_gate: float = 500.0,
45
+ temporal_smooth: float = 0.15,
46
+ dynamic_noise_ratio: float = 0.0,
47
+ spatial_filter: bool = True,
48
+ calibration_frames: int = 30,
49
+ bypass_gates: bool = False):
50
+ self.base_gate = float(base_gate)
51
+ self.temporal_smooth = float(temporal_smooth)
52
+ self.dynamic_noise_ratio = float(dynamic_noise_ratio)
53
+ self.spatial_filter = bool(spatial_filter)
54
+ self.calibration_frames = int(calibration_frames)
55
+ self.bypass_gates = bool(bypass_gates)
56
+
57
+ self.baseline_map = np.zeros((MATRIX_ROWS, MATRIX_COLS), dtype=np.float32)
58
+ self.history_buffer: np.ndarray | None = None
59
+ self.drift_baseline_val = 0.0
60
+ self.is_calibrating = True
61
+ self._calibration_buffer: list[np.ndarray] = []
62
+
63
+ # ---- Baseline ----
64
+
65
+ def start_calibration(self, frames: int | None = None):
66
+ """Restart zeroing. It ends automatically once frames frames have been collected; process() returns None meanwhile."""
67
+ if frames is not None:
68
+ self.calibration_frames = int(frames)
69
+ self._calibration_buffer.clear()
70
+ self.is_calibrating = True
71
+
72
+ def set_baseline(self, baseline_map: np.ndarray):
73
+ """Load a baseline map directly (e.g. restored from disk), skipping acquisition."""
74
+ self.baseline_map = np.asarray(baseline_map, dtype=np.float32).reshape(
75
+ MATRIX_ROWS, MATRIX_COLS).copy()
76
+ self.is_calibrating = False
77
+ self._calibration_buffer.clear()
78
+
79
+ @property
80
+ def calibration_progress(self) -> tuple[int, int]:
81
+ return len(self._calibration_buffer), self.calibration_frames
82
+
83
+ # ---- Main pipeline ----
84
+
85
+ def process(self, raw_data: np.ndarray) -> tuple[np.ndarray | None, float]:
86
+ """→ (processed, peak); returns (None, 0.0) while zeroing is in progress."""
87
+ raw_data = np.asarray(raw_data, dtype=np.float32).reshape(MATRIX_ROWS, MATRIX_COLS)
88
+
89
+ if self.is_calibrating:
90
+ self._calibration_buffer.append(raw_data.copy())
91
+ if len(self._calibration_buffer) >= self.calibration_frames:
92
+ # Per-cell max + 10, more conservative than the mean; noise spikes are not counted as signal
93
+ self.baseline_map = np.max(
94
+ np.stack(self._calibration_buffer), axis=0) + 10
95
+ self.is_calibrating = False
96
+ self._calibration_buffer.clear()
97
+ return None, 0.0
98
+
99
+ # 1. Temporal smoothing
100
+ if self.history_buffer is None:
101
+ self.history_buffer = raw_data.copy()
102
+ self.history_buffer = (self.history_buffer * self.temporal_smooth
103
+ + raw_data * (1.0 - self.temporal_smooth))
104
+
105
+ # 2. Baseline subtraction
106
+ processed = np.maximum(0.0, self.history_buffer - self.baseline_map)
107
+
108
+ # 3. Drift removal: when the whole frame rises together (temperature/power), the 40th percentile rises too
109
+ self.drift_baseline_val = float(np.percentile(processed, 40))
110
+ if self.drift_baseline_val > 100:
111
+ processed = np.maximum(0.0, processed - self.drift_baseline_val * 0.8)
112
+
113
+ if not self.bypass_gates:
114
+ # 4. Noise gate
115
+ dynamic_gate = min(processed.max() * self.dynamic_noise_ratio, DYNAMIC_GATE_CAP)
116
+ final_gate = max(self.base_gate, dynamic_gate)
117
+ processed[processed < final_gate] = 0.0
118
+
119
+ # 5. Isolated-cell removal. All 8 neighbors being 0 means no adjacent cell is
120
+ # pressed at the same time; a real finger contact always covers more than one cell.
121
+ if self.spatial_filter:
122
+ mask = (processed > 0).astype(np.uint8)
123
+ neighbors = _count_neighbors(mask)
124
+ noise_floor = np.maximum(self.baseline_map * ISOLATED_BASELINE_SCALE,
125
+ ISOLATED_NOISE_FLOOR)
126
+ isolated = (mask > 0) & (neighbors == 0) & (processed < noise_floor)
127
+ processed[isolated] = 0.0
128
+
129
+ return processed, float(processed.max())
130
+
131
+
132
+ def _count_neighbors(mask: np.ndarray) -> np.ndarray:
133
+ """Count 8-neighbors, padding the border with 0.
134
+
135
+ Equivalent to cv2.filter2D(mask, -1, 8-neighbor kernel, BORDER_CONSTANT),
136
+ but without adding an opencv dependency — the demo side (polyscope) doesn't need it.
137
+ """
138
+ padded = np.zeros((mask.shape[0] + 2, mask.shape[1] + 2), dtype=np.int32)
139
+ padded[1:-1, 1:-1] = mask
140
+ out = np.zeros_like(mask, dtype=np.int32)
141
+ for dr in (-1, 0, 1):
142
+ for dc in (-1, 0, 1):
143
+ if dr == 0 and dc == 0:
144
+ continue
145
+ out += padded[1 + dr:1 + dr + mask.shape[0], 1 + dc:1 + dc + mask.shape[1]]
146
+ return out
collector/core/glove_usb/types.py ADDED
@@ -0,0 +1,349 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """Versioned public data contracts used by every SDK interface."""
2
+
3
+ from __future__ import annotations
4
+
5
+ from dataclasses import dataclass, field
6
+ from pathlib import Path
7
+ from typing import Any, Literal
8
+
9
+ import numpy as np
10
+
11
+
12
+ Side = Literal["left", "right"]
13
+
14
+
15
+ def _side(value: str) -> Side:
16
+ normalized = str(value).strip().lower()
17
+ if normalized not in ("left", "right"):
18
+ raise ValueError(f"side must be left or right, got {value!r}")
19
+ return normalized # type: ignore[return-value]
20
+
21
+
22
+ def _array(value, shape, dtype, name):
23
+ result = np.asarray(value, dtype=dtype).reshape(shape).copy()
24
+ if not np.all(np.isfinite(result)) and result.dtype.kind == "f":
25
+ # NaN is meaningful for sensor age/tactile absence, so callers decide
26
+ # whether it is acceptable. Shape and ownership are enforced here.
27
+ pass
28
+ return result
29
+
30
+
31
+ @dataclass(frozen=True)
32
+ class DeviceInfo:
33
+ device: str
34
+ serial_number: str
35
+ vid: int
36
+ pid: int
37
+ location: str = ""
38
+ bound_side: Side | None = None
39
+
40
+
41
+ @dataclass(frozen=True)
42
+ class DeviceBindings:
43
+ left_serial: str
44
+ right_serial: str
45
+ left_port: str | None = None
46
+ right_port: str | None = None
47
+
48
+
49
+ @dataclass(frozen=True)
50
+ class GloveConfig:
51
+ side: Side
52
+ calibration: Path | str
53
+ serial_number: str | None = None
54
+ registry: Path | str | None = None
55
+ sample_rate_hz: int = 80
56
+ include_tactile: bool = True
57
+ smoothing_ms: float = 35.0
58
+ recording_root: Path | str | None = None
59
+
60
+ def __post_init__(self):
61
+ object.__setattr__(self, "side", _side(self.side))
62
+ object.__setattr__(self, "calibration", Path(self.calibration))
63
+ if self.registry is not None:
64
+ object.__setattr__(self, "registry", Path(self.registry))
65
+ if self.recording_root is not None:
66
+ object.__setattr__(self, "recording_root", Path(self.recording_root))
67
+ if int(self.sample_rate_hz) <= 0:
68
+ raise ValueError("sample_rate_hz must be positive")
69
+ if float(self.smoothing_ms) < 0.0:
70
+ raise ValueError("smoothing_ms must not be negative")
71
+
72
+
73
+ @dataclass(frozen=True)
74
+ class BimanualConfig:
75
+ left: GloveConfig
76
+ right: GloveConfig
77
+ output_root: Path | str | None = None
78
+ display_separation_m: float = 0.30
79
+
80
+ def __post_init__(self):
81
+ if self.left.side != "left" or self.right.side != "right":
82
+ raise ValueError("BimanualConfig requires left and right GloveConfig objects")
83
+ if self.output_root is not None:
84
+ object.__setattr__(self, "output_root", Path(self.output_root))
85
+ if float(self.display_separation_m) <= 0.0:
86
+ raise ValueError("display_separation_m must be positive")
87
+
88
+
89
+ @dataclass(frozen=True)
90
+ class DeviceHealth:
91
+ connected: bool
92
+ ready_imu_count: int
93
+ missing_imus: tuple[str, ...] = ()
94
+ stream_fps: float = 0.0
95
+ message: str = ""
96
+ last_error: str | None = None
97
+
98
+
99
+ @dataclass(frozen=True)
100
+ class BimanualHealth:
101
+ left: DeviceHealth
102
+ right: DeviceHealth
103
+ synchronization_error_ms: float | None = None
104
+
105
+ @property
106
+ def connected(self) -> bool:
107
+ return self.left.connected and self.right.connected
108
+
109
+
110
+ @dataclass(frozen=True)
111
+ class HandStatus:
112
+ safety_intervened: bool = False
113
+ active_contact: str | None = None
114
+ kinematics: str = ""
115
+ details: dict[str, Any] = field(default_factory=dict)
116
+
117
+
118
+ @dataclass(frozen=True)
119
+ class RawImuFrame:
120
+ """One decoded STM32 IMU packet in physical channel order.
121
+
122
+ The quaternion order is XYZW. No physical-to-hand remapping, axis
123
+ conversion, filtering, calibration, or hand-pose solving has been applied.
124
+ """
125
+
126
+ sequence: int
127
+ device_timestamp_us: int
128
+ host_timestamp_us: int
129
+ quaternions_xyzw: np.ndarray
130
+ present_mask: np.ndarray
131
+ valid_mask: np.ndarray
132
+
133
+ def __post_init__(self):
134
+ object.__setattr__(self, "quaternions_xyzw", _array(
135
+ self.quaternions_xyzw, (16, 4), np.float64,
136
+ "quaternions_xyzw"))
137
+ object.__setattr__(self, "present_mask", _array(
138
+ self.present_mask, (16,), bool, "present_mask"))
139
+ object.__setattr__(self, "valid_mask", _array(
140
+ self.valid_mask, (16,), bool, "valid_mask"))
141
+
142
+
143
+ @dataclass(frozen=True)
144
+ class ImuFrame:
145
+ sequence: int
146
+ timestamp_us: int
147
+ quaternions_xyzw: np.ndarray
148
+ valid_mask: np.ndarray
149
+ sensor_age_s: np.ndarray
150
+ rejected_mask: np.ndarray
151
+
152
+ def __post_init__(self):
153
+ object.__setattr__(self, "quaternions_xyzw", _array(
154
+ self.quaternions_xyzw, (16, 4), np.float64, "quaternions_xyzw"))
155
+ object.__setattr__(self, "valid_mask", _array(
156
+ self.valid_mask, (16,), bool, "valid_mask"))
157
+ object.__setattr__(self, "sensor_age_s", _array(
158
+ self.sensor_age_s, (16,), np.float32, "sensor_age_s"))
159
+ object.__setattr__(self, "rejected_mask", _array(
160
+ self.rejected_mask, (16,), bool, "rejected_mask"))
161
+
162
+
163
+ @dataclass(frozen=True)
164
+ class TactileFrame:
165
+ sequence: int
166
+ timestamp_us: int
167
+ samples: np.ndarray
168
+ processed: bool = False
169
+
170
+ def __post_init__(self):
171
+ object.__setattr__(self, "samples", _array(
172
+ self.samples, (16, 16), np.float32, "samples"))
173
+
174
+ @property
175
+ def scan_time_us(self) -> int:
176
+ """Firmware scan-time field retained as a descriptive alias."""
177
+
178
+ return self.timestamp_us
179
+
180
+
181
+ @dataclass(frozen=True)
182
+ class SolvedHandFrame:
183
+ side: Side
184
+ joints_m: np.ndarray
185
+ status: HandStatus
186
+
187
+ def __post_init__(self):
188
+ object.__setattr__(self, "side", _side(self.side))
189
+ object.__setattr__(self, "joints_m", _array(
190
+ self.joints_m, (21, 3), np.float32, "joints_m"))
191
+
192
+
193
+ @dataclass(frozen=True)
194
+ class KeypointFrame:
195
+ """Versioned output of the protected raw-IMU to 21-keypoint pipeline."""
196
+
197
+ side: Side
198
+ sequence: int
199
+ timestamp_us: int
200
+ joints_m: np.ndarray
201
+ imu_xyzw: np.ndarray
202
+ valid_mask: np.ndarray
203
+ sensor_age_s: np.ndarray
204
+ rejected_mask: np.ndarray
205
+ status: HandStatus
206
+
207
+ def __post_init__(self):
208
+ object.__setattr__(self, "side", _side(self.side))
209
+ object.__setattr__(self, "joints_m", _array(
210
+ self.joints_m, (21, 3), np.float32, "joints_m"))
211
+ object.__setattr__(self, "imu_xyzw", _array(
212
+ self.imu_xyzw, (16, 4), np.float64, "imu_xyzw"))
213
+ object.__setattr__(self, "valid_mask", _array(
214
+ self.valid_mask, (16,), bool, "valid_mask"))
215
+ object.__setattr__(self, "sensor_age_s", _array(
216
+ self.sensor_age_s, (16,), np.float32, "sensor_age_s"))
217
+ object.__setattr__(self, "rejected_mask", _array(
218
+ self.rejected_mask, (16,), bool, "rejected_mask"))
219
+
220
+ @property
221
+ def keypoints_21_m(self) -> np.ndarray:
222
+ """Descriptive alias for :attr:`joints_m`."""
223
+
224
+ return self.joints_m.copy()
225
+
226
+
227
+ @dataclass(frozen=True)
228
+ class HandFrame:
229
+ side: Side
230
+ sequence: int
231
+ timestamp_us: int
232
+ imu_xyzw: np.ndarray
233
+ imu_valid: np.ndarray
234
+ sensor_age_s: np.ndarray
235
+ joints_raw_m: np.ndarray
236
+ joints_smoothed_m: np.ndarray
237
+ tactile: np.ndarray | None
238
+ status: HandStatus
239
+ raw_imu_xyzw: np.ndarray | None = None
240
+ raw_imu_present: np.ndarray | None = None
241
+ raw_imu_valid: np.ndarray | None = None
242
+ raw_device_timestamp_us: int | None = None
243
+ tactile_raw: np.ndarray | None = None
244
+
245
+ def __post_init__(self):
246
+ object.__setattr__(self, "side", _side(self.side))
247
+ object.__setattr__(self, "imu_xyzw", _array(
248
+ self.imu_xyzw, (16, 4), np.float64, "imu_xyzw"))
249
+ object.__setattr__(self, "imu_valid", _array(
250
+ self.imu_valid, (16,), bool, "imu_valid"))
251
+ object.__setattr__(self, "sensor_age_s", _array(
252
+ self.sensor_age_s, (16,), np.float32, "sensor_age_s"))
253
+ object.__setattr__(self, "joints_raw_m", _array(
254
+ self.joints_raw_m, (21, 3), np.float32, "joints_raw_m"))
255
+ object.__setattr__(self, "joints_smoothed_m", _array(
256
+ self.joints_smoothed_m, (21, 3), np.float32, "joints_smoothed_m"))
257
+ if self.tactile is not None:
258
+ object.__setattr__(self, "tactile", _array(
259
+ self.tactile, (16, 16), np.float32, "tactile"))
260
+ if self.raw_imu_xyzw is not None:
261
+ object.__setattr__(self, "raw_imu_xyzw", _array(
262
+ self.raw_imu_xyzw, (16, 4), np.float64, "raw_imu_xyzw"))
263
+ if self.raw_imu_present is not None:
264
+ object.__setattr__(self, "raw_imu_present", _array(
265
+ self.raw_imu_present, (16,), bool, "raw_imu_present"))
266
+ if self.raw_imu_valid is not None:
267
+ object.__setattr__(self, "raw_imu_valid", _array(
268
+ self.raw_imu_valid, (16,), bool, "raw_imu_valid"))
269
+ if self.tactile_raw is not None:
270
+ object.__setattr__(self, "tactile_raw", _array(
271
+ self.tactile_raw, (16, 16), np.float32, "tactile_raw"))
272
+
273
+
274
+ @dataclass(frozen=True)
275
+ class BimanualFrame:
276
+ sequence: int
277
+ timestamp_us: int
278
+ left: HandFrame
279
+ right: HandFrame
280
+ synchronization_error_ms: float
281
+
282
+
283
+ @dataclass(frozen=True)
284
+ class CalibrationStep:
285
+ id: str
286
+ title: str
287
+ action: Literal["capture", "compute"]
288
+ stage: Literal["root", "installation", "contact"]
289
+
290
+
291
+ @dataclass(frozen=True)
292
+ class CalibrationStepResult:
293
+ step_id: str
294
+ ok: bool
295
+ message: str
296
+
297
+
298
+ @dataclass(frozen=True)
299
+ class CalibrationProgress:
300
+ current_index: int
301
+ total_steps: int
302
+ completed_step_ids: tuple[str, ...]
303
+ complete: bool
304
+
305
+
306
+ @dataclass(frozen=True)
307
+ class RecordingStatus:
308
+ state: Literal["disabled", "idle", "recording", "saved"]
309
+ take_index: int
310
+ frame_count: int
311
+ output_path: Path | None = None
312
+
313
+
314
+ @dataclass(frozen=True)
315
+ class RecordingResult:
316
+ parquet_path: Path | None
317
+ session_path: Path | None
318
+ take_index: int
319
+ frame_count: int
320
+ duration_s: float
321
+ sample_fps: float
322
+
323
+
324
+ @dataclass(frozen=True)
325
+ class RecordingInfo:
326
+ parquet_path: Path
327
+ session_path: Path | None
328
+ mode: str
329
+ sides: tuple[Side, ...]
330
+ frame_count: int
331
+ sample_fps: float
332
+ has_imu: bool
333
+ has_tactile: bool
334
+ tactile_valid_frames: int = 0
335
+ tactile_nonzero_samples: int = 0
336
+ has_raw_imu: bool = False
337
+ has_raw_tactile: bool = False
338
+
339
+ @property
340
+ def has_tactile_signal(self) -> bool:
341
+ return self.tactile_nonzero_samples > 0
342
+
343
+
344
+ @dataclass(frozen=True)
345
+ class ReplayResult:
346
+ input_path: Path
347
+ output_path: Path
348
+ frame_count: int
349
+ sample_fps: float
collector/core/glove_usb/usb_protocol.py ADDED
@@ -0,0 +1,344 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """STM32 glove USB CDC framing and IMU payload decoding.
2
+
3
+ The firmware multiplexes status, IMU and pressure-matrix messages on one CDC
4
+ byte stream. This module deliberately owns only the wire protocol; serial-port
5
+ opening and MANO processing remain in :mod:`modules.hand.hand`.
6
+ """
7
+
8
+ from __future__ import annotations
9
+
10
+ from dataclasses import dataclass
11
+ import struct
12
+ from typing import Iterable
13
+
14
+ import numpy as np
15
+
16
+
17
+ STM32_USB_VID = 0x0483
18
+ STM32_USB_PID = 0x5740
19
+
20
+ USB_MAGIC = b"\xA5\x5A"
21
+ # Firmware v1.2.2 is the first release described by
22
+ # ``流程图与数据帧格式_v1.2.2.pdf``. It changed the header to carry the
23
+ # complete ``major.minor.patch`` version and moved the payload length/flags to
24
+ # offsets 14/16. Later firmware keeps this wire format, so the comparison is
25
+ # deliberately semantic rather than a hard-coded version string.
26
+ USB_NEW_FORMAT_FROM = (1, 2, 2)
27
+ USB_HEADER_SIZE = 18
28
+ USB_CRC_SIZE = 2
29
+ USB_MAX_PAYLOAD = 1100
30
+
31
+ USB_TYPE_STATUS = 0x01
32
+ USB_TYPE_IMU_Q14 = 0x02
33
+ USB_TYPE_ADC_MATRIX = 0x03
34
+
35
+ TOTAL_IMU_COUNT = 16
36
+ IMU_COMPONENT_COUNT = 4
37
+ IMU_Q14_PAYLOAD_SIZE = TOTAL_IMU_COUNT * IMU_COMPONENT_COUNT * 2
38
+ Q14_SCALE = 1.0 / 16384.0
39
+ ADC_MATRIX_ROWS = 16
40
+ ADC_MATRIX_COLS = 16
41
+ ADC_MATRIX_META_SIZE = 12
42
+ ADC_MATRIX_PAYLOAD_SIZE = (
43
+ ADC_MATRIX_META_SIZE + ADC_MATRIX_ROWS * ADC_MATRIX_COLS * 2)
44
+
45
+ def parse_firmware_version(version: str | tuple[int, ...] | list[int]) -> tuple[int, ...] | None:
46
+ """Return numeric firmware components, or ``None`` for malformed input."""
47
+
48
+ try:
49
+ if isinstance(version, str):
50
+ components = tuple(int(part.strip()) for part in version.split("."))
51
+ else:
52
+ components = tuple(int(part) for part in version)
53
+ except (TypeError, ValueError):
54
+ return None
55
+ return components if len(components) == 3 and all(value >= 0 for value in components) else None
56
+
57
+
58
+ def is_supported_version(version: str | tuple[int, ...] | list[int]) -> bool:
59
+ """Return whether ``version`` uses the current 18-byte-header format.
60
+
61
+ The PDF documents v1.2.2 itself as the current format. Accepting it as
62
+ well as all newer versions is important when a board is flashed with the
63
+ documented release; future patch/minor releases remain compatible.
64
+ """
65
+
66
+ components = parse_firmware_version(version)
67
+ return components is not None and components >= USB_NEW_FORMAT_FROM
68
+
69
+
70
+ # The v1.2.2 firmware emits joint order (wrist, thumb, index, middle, ring,
71
+ # pinky). Keep the v1.0 physical-channel maps here so old JSON configurations
72
+ # continue to work: ``channel_to_hand_for_firmware`` composes them with the
73
+ # firmware's output permutation. New configurations use the resulting map
74
+ # directly, which is the same for both hand sides.
75
+ LEGACY_CHANNEL_TO_HAND_BY_SIDE = {
76
+ "left": (
77
+ 14, 13, 8, 15,
78
+ 2, 7, 9, 1,
79
+ 4, 3, 11, 0,
80
+ 5, 12, 10, 6,
81
+ ),
82
+ "right": (
83
+ 8, 7, 14, 9,
84
+ 11, 13, 15, 10,
85
+ 4, 12, 2, 0,
86
+ 5, 3, 1, 6,
87
+ ),
88
+ }
89
+
90
+ # USB output slot -> pre-v1.2.2 firmware slot. These are the ORDER_L/R
91
+ # tables shown in section A.3 of the supplied PDF.
92
+ FIRMWARE_OUTPUT_ORDER_BY_SIDE = {
93
+ "left": (11, 1, 0, 3, 7, 4, 9, 8, 12, 15, 14, 10, 13, 5, 2, 6),
94
+ "right": (11, 5, 2, 6, 14, 10, 13, 8, 12, 15, 7, 4, 9, 1, 0, 3),
95
+ }
96
+
97
+ NEW_FIRMWARE_CHANNEL_TO_HAND_BY_SIDE = {
98
+ side: tuple(
99
+ LEGACY_CHANNEL_TO_HAND_BY_SIDE[side][source]
100
+ for source in FIRMWARE_OUTPUT_ORDER_BY_SIDE[side]
101
+ )
102
+ for side in FIRMWARE_OUTPUT_ORDER_BY_SIDE
103
+ }
104
+
105
+ DEFAULT_CHANNEL_TO_HAND = NEW_FIRMWARE_CHANNEL_TO_HAND_BY_SIDE["right"]
106
+
107
+
108
+ @dataclass(frozen=True)
109
+ class UsbCdcFrame:
110
+ """One CRC-validated frame extracted from the CDC byte stream."""
111
+
112
+ version: str
113
+ message_type: int
114
+ sequence: int
115
+ timestamp_us: int
116
+ flags: int
117
+ payload: bytes
118
+
119
+ @property
120
+ def firmware_version(self) -> str:
121
+ """Compatibility alias used by older SDK callers."""
122
+
123
+ return self.version
124
+
125
+
126
+ @dataclass(frozen=True)
127
+ class UsbAdcMatrixFrame:
128
+ """Decoded STM32 16x16 tactile matrix payload."""
129
+
130
+ sequence: int
131
+ scan_time_us: int
132
+ samples: np.ndarray
133
+
134
+
135
+ def crc16_ccitt_false(data: bytes | bytearray | memoryview) -> int:
136
+ """Return CRC16-CCITT-FALSE (poly 0x1021, init 0xFFFF)."""
137
+
138
+ crc = 0xFFFF
139
+ for value in data:
140
+ crc ^= int(value) << 8
141
+ for _ in range(8):
142
+ if crc & 0x8000:
143
+ crc = ((crc << 1) ^ 0x1021) & 0xFFFF
144
+ else:
145
+ crc = (crc << 1) & 0xFFFF
146
+ return crc
147
+
148
+
149
+ class UsbCdcFrameParser:
150
+ """Incrementally recover framed messages from arbitrary serial chunks."""
151
+
152
+ def __init__(self):
153
+ self.buffer = bytearray()
154
+ self.discarded_bytes = 0
155
+ self.crc_errors = 0
156
+ self.length_errors = 0
157
+
158
+ def reset(self) -> None:
159
+ self.buffer.clear()
160
+
161
+ def feed(self, data: bytes | bytearray | memoryview) -> list[UsbCdcFrame]:
162
+ if data:
163
+ self.buffer.extend(data)
164
+
165
+ frames: list[UsbCdcFrame] = []
166
+ while True:
167
+ magic_index = self.buffer.find(USB_MAGIC)
168
+ if magic_index < 0:
169
+ # Keep a trailing 0xA5 because it may be the first byte of a
170
+ # magic sequence split across two serial reads.
171
+ keep = 1 if self.buffer.endswith(USB_MAGIC[:1]) else 0
172
+ discard = len(self.buffer) - keep
173
+ if discard > 0:
174
+ del self.buffer[:discard]
175
+ self.discarded_bytes += discard
176
+ break
177
+
178
+ if magic_index > 0:
179
+ del self.buffer[:magic_index]
180
+ self.discarded_bytes += magic_index
181
+
182
+ if len(self.buffer) < USB_HEADER_SIZE:
183
+ break
184
+
185
+ payload_length = int.from_bytes(self.buffer[14:16], "little")
186
+ if payload_length > USB_MAX_PAYLOAD:
187
+ del self.buffer[0]
188
+ self.discarded_bytes += 1
189
+ self.length_errors += 1
190
+ continue
191
+
192
+ total_length = USB_HEADER_SIZE + payload_length + USB_CRC_SIZE
193
+ if len(self.buffer) < total_length:
194
+ break
195
+
196
+ crc_offset = USB_HEADER_SIZE + payload_length
197
+ expected_crc = int.from_bytes(
198
+ self.buffer[crc_offset:crc_offset + USB_CRC_SIZE], "little")
199
+ actual_crc = crc16_ccitt_false(self.buffer[:crc_offset])
200
+ if actual_crc != expected_crc:
201
+ del self.buffer[0]
202
+ self.discarded_bytes += 1
203
+ self.crc_errors += 1
204
+ continue
205
+
206
+ frames.append(UsbCdcFrame(
207
+ version=".".join(
208
+ str(value) for value in self.buffer[2:5]),
209
+ message_type=int(self.buffer[5]),
210
+ sequence=int.from_bytes(self.buffer[6:10], "little"),
211
+ timestamp_us=int.from_bytes(self.buffer[10:14], "little"),
212
+ flags=int.from_bytes(self.buffer[16:18], "little"),
213
+ payload=bytes(self.buffer[USB_HEADER_SIZE:crc_offset]),
214
+ ))
215
+ del self.buffer[:total_length]
216
+
217
+ return frames
218
+
219
+
220
+ def validate_channel_to_hand(mapping: Iterable[int]) -> tuple[int, ...]:
221
+ """Validate and freeze a physical-channel to MANO-joint permutation."""
222
+
223
+ result = tuple(int(value) for value in mapping)
224
+ if len(result) != TOTAL_IMU_COUNT:
225
+ raise ValueError("channel_to_hand must contain exactly 16 entries")
226
+ if sorted(result) != list(range(TOTAL_IMU_COUNT)):
227
+ raise ValueError("channel_to_hand must be a permutation of 0..15")
228
+ return result
229
+
230
+
231
+ def channel_to_hand_for_firmware(mapping: Iterable[int]) -> tuple[int, ...]:
232
+ """Convert a persisted v1.0 map to the v1.2.2 joint-order map.
233
+
234
+ A map already written for the new payload is returned unchanged. This
235
+ lets users upgrade the SDK without invalidating existing calibration JSON.
236
+ """
237
+
238
+ normalized = validate_channel_to_hand(mapping)
239
+ for side, legacy_mapping in LEGACY_CHANNEL_TO_HAND_BY_SIDE.items():
240
+ if normalized == legacy_mapping:
241
+ return NEW_FIRMWARE_CHANNEL_TO_HAND_BY_SIDE[side]
242
+ return normalized
243
+
244
+
245
+ def decode_imu_q14_payload(payload: bytes) -> np.ndarray:
246
+ """Decode the fixed 128-byte payload into SciPy-order XYZW quaternions."""
247
+
248
+ if len(payload) != IMU_Q14_PAYLOAD_SIZE:
249
+ raise ValueError(
250
+ f"STM32 IMU payload must be {IMU_Q14_PAYLOAD_SIZE} bytes, "
251
+ f"got {len(payload)}")
252
+
253
+ raw = np.asarray(
254
+ struct.unpack("<64h", payload), dtype=np.float64
255
+ ).reshape(TOTAL_IMU_COUNT, IMU_COMPONENT_COUNT)
256
+ wxyz = raw * Q14_SCALE
257
+ return wxyz[:, [1, 2, 3, 0]]
258
+
259
+
260
+ def decode_adc_matrix_payload(payload: bytes) -> UsbAdcMatrixFrame:
261
+ """Decode the STM32 12-byte metadata plus 256 little-endian ADC values."""
262
+
263
+ if len(payload) != ADC_MATRIX_PAYLOAD_SIZE:
264
+ raise ValueError(
265
+ f"STM32 ADC matrix payload must be {ADC_MATRIX_PAYLOAD_SIZE} bytes, "
266
+ f"got {len(payload)}")
267
+ rows, cols, adc_bits, order = payload[:4]
268
+ if (rows, cols, adc_bits, order) != (
269
+ ADC_MATRIX_ROWS, ADC_MATRIX_COLS, 12, 0):
270
+ raise ValueError(
271
+ "unsupported STM32 ADC matrix metadata: "
272
+ f"rows={rows}, cols={cols}, bits={adc_bits}, order={order}")
273
+ sequence = int.from_bytes(payload[4:8], "little")
274
+ scan_time_us = int.from_bytes(payload[8:12], "little")
275
+ samples = np.frombuffer(
276
+ payload, dtype="<u2", offset=ADC_MATRIX_META_SIZE,
277
+ count=ADC_MATRIX_ROWS * ADC_MATRIX_COLS,
278
+ ).reshape(ADC_MATRIX_ROWS, ADC_MATRIX_COLS).astype(np.float32)
279
+ return UsbAdcMatrixFrame(
280
+ sequence=sequence,
281
+ scan_time_us=scan_time_us,
282
+ samples=samples,
283
+ )
284
+
285
+
286
+ def plausible_quaternion_mask(quaternions_xyzw: np.ndarray) -> np.ndarray:
287
+ """Apply the same broad norm gate used by the ESP runtime."""
288
+
289
+ quaternions = np.asarray(quaternions_xyzw, dtype=float)
290
+ if quaternions.shape != (TOTAL_IMU_COUNT, IMU_COMPONENT_COUNT):
291
+ raise ValueError("quaternions must have shape (16, 4)")
292
+ norm_squared = np.sum(quaternions * quaternions, axis=1)
293
+ return (
294
+ np.all(np.isfinite(quaternions), axis=1)
295
+ & (norm_squared > 0.25)
296
+ & (norm_squared < 2.25)
297
+ )
298
+
299
+
300
+ def remap_physical_to_hand(
301
+ quaternions_xyzw: np.ndarray,
302
+ physical_valid_mask: int,
303
+ channel_to_hand: Iterable[int] = DEFAULT_CHANNEL_TO_HAND,
304
+ ) -> tuple[np.ndarray, np.ndarray]:
305
+ """Reorder firmware joint-order slots and valid bits into MANO joint order."""
306
+
307
+ quaternions = np.asarray(quaternions_xyzw, dtype=float)
308
+ if quaternions.shape != (TOTAL_IMU_COUNT, IMU_COMPONENT_COUNT):
309
+ raise ValueError("quaternions must have shape (16, 4)")
310
+ mapping = channel_to_hand_for_firmware(channel_to_hand)
311
+
312
+ mano_quaternions = np.tile(
313
+ np.array([0.0, 0.0, 0.0, 1.0]), (TOTAL_IMU_COUNT, 1))
314
+ mano_valid = np.zeros(TOTAL_IMU_COUNT, dtype=bool)
315
+ plausible = plausible_quaternion_mask(quaternions)
316
+
317
+ for physical_channel, mano_joint in enumerate(mapping):
318
+ mano_quaternions[mano_joint] = quaternions[physical_channel]
319
+ mano_valid[mano_joint] = bool(
320
+ physical_valid_mask & (1 << physical_channel)) and plausible[physical_channel]
321
+
322
+ return mano_quaternions, mano_valid
323
+
324
+
325
+ def find_stm32_cdc_port(vid: int = STM32_USB_VID, pid: int = STM32_USB_PID) -> str | None:
326
+ """Return the first matching STM32 CDC port, or ``None`` when absent."""
327
+
328
+ import serial.tools.list_ports
329
+
330
+ for port in serial.tools.list_ports.comports():
331
+ if port.vid == int(vid) and port.pid == int(pid):
332
+ return port.device
333
+ return None
334
+
335
+
336
+ def list_serial_ports() -> list[tuple[str, str, int | None, int | None]]:
337
+ """Return serial-port details for CLI diagnostics."""
338
+
339
+ import serial.tools.list_ports
340
+
341
+ return [
342
+ (port.device, port.description, port.vid, port.pid)
343
+ for port in serial.tools.list_ports.comports()
344
+ ]
collector/core/hand_processor.py ADDED
@@ -0,0 +1,87 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 录制会话手部关键点后处理 —— Qt 薄封装,核心逻辑在 core.hand_tracking。
3
+ """
4
+
5
+ from __future__ import annotations
6
+ import threading
7
+ from typing import Optional
8
+
9
+ from PyQt5.QtCore import QObject, pyqtSignal
10
+
11
+ from core.hand_tracking import process_session as _ht_process_session
12
+
13
+
14
+ class SessionHandProcessor(QObject):
15
+ """对已录制的会话视频逐帧跑手部关键点推理,结果写入 parquet。
16
+
17
+ 信号
18
+ ----
19
+ progress(int, int) — (当前帧, 总帧数)
20
+ finished(str, str) — (session_path, error_msg)
21
+ error_msg 为空表示成功
22
+ status_changed(str) — 状态变化通知
23
+ """
24
+
25
+ progress = pyqtSignal(int, int)
26
+ finished = pyqtSignal(str, str)
27
+ status_changed = pyqtSignal(str)
28
+
29
+ def __init__(self, parent=None):
30
+ super().__init__(parent)
31
+ self._running = False
32
+ self._thread: Optional[threading.Thread] = None
33
+
34
+ def process_session(self, session_path: str, detector: str = "",
35
+ det_device: str = "cuda", pose_device: str = "cuda",
36
+ mode: str = ""):
37
+ """启动后台处理一个录制会话。"""
38
+ if self._running:
39
+ return
40
+
41
+ self._track_mode = mode
42
+ det_path = detector
43
+ self._det_device = det_device
44
+ self._pose_device = pose_device
45
+
46
+ self._running = True
47
+ self._thread = threading.Thread(
48
+ target=self._process, args=(session_path, det_path),
49
+ daemon=True, name="hand-processor",
50
+ )
51
+ self._thread.start()
52
+
53
+ def cancel(self):
54
+ """取消当前处理任务。"""
55
+ self._running = False
56
+
57
+ # ── 后台 ──────────────────────────────────────────
58
+
59
+ def _process(self, session_path: str, det_path: str):
60
+ def _progress(cur, total):
61
+ self.progress.emit(cur, total)
62
+
63
+ def _status(msg):
64
+ self.status_changed.emit(msg)
65
+
66
+ def _cancelled():
67
+ return not self._running
68
+
69
+ result = _ht_process_session(
70
+ session_path=session_path,
71
+ mode=getattr(self, '_track_mode', ''),
72
+ detector=det_path or '',
73
+ det_device=getattr(self, '_det_device', 'cuda'),
74
+ pose_device=getattr(self, '_pose_device', 'cuda'),
75
+ progress_cb=_progress,
76
+ status_cb=_status,
77
+ cancel_check=_cancelled,
78
+ )
79
+
80
+ if not self._running:
81
+ self.status_changed.emit("cancelled")
82
+ elif result.get("success"):
83
+ self.finished.emit(session_path, "")
84
+ else:
85
+ self.finished.emit(session_path, result.get("error", "未知错误"))
86
+
87
+ self._running = False
collector/core/hand_tracking.py ADDED
@@ -0,0 +1,807 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 手部关键点后处理独立模块 —— 零 Qt 依赖,纯函数式 API。
3
+
4
+ 支持两种追踪模式:
5
+ - "glove": YOLO 检测黑色手套 + RTMPose 关键点 (仅 2D)
6
+ - "bare": MediaPipe 裸手追踪 (2D + 3D world_landmarks)
7
+
8
+ bare 模式下额外保存 3D 世界坐标到 keypoints/hand_pose_3d/,
9
+ 供下游 3D 可视化与数据集导出使用。
10
+
11
+ 用法:
12
+ from core.hand_tracking import process_session, load_hand_kpts, draw_kpts_overlay
13
+
14
+ result = process_session("/path/to/session", mode="bare")
15
+ kpts = load_hand_kpts("/path/to/session")
16
+ """
17
+
18
+ from __future__ import annotations
19
+ import os
20
+ import sys
21
+ import json
22
+ import time
23
+ from typing import Optional, Callable
24
+
25
+ import numpy as np
26
+ import cv2
27
+ import pyarrow as pa
28
+ import pyarrow.parquet as pq
29
+
30
+ from config import settings
31
+ from core.helpers import (
32
+ hand_kpts_parquet_path, hand_3d_parquet_path, auto_labels_parquet_path,
33
+ data_parquet_path, video_mp4_path, detect_session_format,
34
+ egodata_video_path, egodata_metadata_path,
35
+ _session_kpts_hand_kpts_path, _session_kpts_hand_3d_path,
36
+ _session_kpts_auto_labels_path,
37
+ _legacy_hand_kpts_path, _legacy_hand_3d_path, _legacy_auto_labels_path,
38
+ )
39
+
40
+ # ── 将本地 tools/hand_detection 目录加入 sys.path ────────────
41
+ _HAND_DET_DIR = os.path.normpath(
42
+ os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),
43
+ "tools", "hand_detection"))
44
+ _HAND_DET_DIR = os.path.abspath(_HAND_DET_DIR)
45
+ if _HAND_DET_DIR not in sys.path and os.path.isdir(_HAND_DET_DIR):
46
+ sys.path.insert(0, _HAND_DET_DIR)
47
+
48
+ # ── 常量 ────────────────────────────────────────────────
49
+
50
+ PER_HAND_DIM = 21 * 2 + 4 # 46: 21点×2坐标 + 4框坐标
51
+ MAX_HANDS = 2
52
+ TOTAL_DIM = PER_HAND_DIM * MAX_HANDS # 92
53
+
54
+ PER_HAND_3D_DIM = 21 * 3 # 63: xyz per landmark
55
+
56
+ # ── Parquet Schema ──────────────────────────────────────
57
+
58
+ _HAND_KPTS_SCHEMA = pa.schema([
59
+ ("frame_index", pa.int32()),
60
+ ("num_hands", pa.int32()),
61
+ ("hand_data", pa.list_(pa.float32(), TOTAL_DIM)),
62
+ ("track_ids", pa.list_(pa.int32(), MAX_HANDS)),
63
+ ])
64
+
65
+ _HAND_3D_SCHEMA = pa.schema([
66
+ ("frame_index", pa.int32()),
67
+ ("hand_0_present", pa.bool_()),
68
+ ("hand_0_landmarks_3d", pa.list_(pa.float32(), PER_HAND_3D_DIM)),
69
+ ("hand_0_label", pa.string()),
70
+ ("hand_1_present", pa.bool_()),
71
+ ("hand_1_landmarks_3d", pa.list_(pa.float32(), PER_HAND_3D_DIM)),
72
+ ("hand_1_label", pa.string()),
73
+ ])
74
+
75
+ _AUTO_LABELS_SCHEMA = pa.schema([
76
+ ("frame_index", pa.int32()),
77
+ ("hand_0_gesture", pa.string()),
78
+ ("hand_0_extended", pa.list_(pa.string())),
79
+ ("hand_0_extended_count", pa.int32()),
80
+ ("hand_0_fist", pa.bool_()),
81
+ ("hand_0_pinch", pa.bool_()),
82
+ ("hand_0_center_x", pa.float32()),
83
+ ("hand_0_center_y", pa.float32()),
84
+ ("hand_0_motion", pa.float32()),
85
+ ("hand_1_gesture", pa.string()),
86
+ ("hand_1_extended", pa.list_(pa.string())),
87
+ ("hand_1_extended_count", pa.int32()),
88
+ ("hand_1_fist", pa.bool_()),
89
+ ("hand_1_pinch", pa.bool_()),
90
+ ("hand_1_motion", pa.float32()),
91
+ ("two_hand_distance", pa.float32()),
92
+ ("contact", pa.string()),
93
+ ])
94
+
95
+ # ── 延迟导入(重型依赖仅处理时加载) ───────────────────
96
+
97
+ _HandPipeline = None
98
+ _hand_common = None
99
+
100
+
101
+ def _lazy_import_pipeline():
102
+ """导入 HandPipeline(含 ultralytics/torch/CUDA)。"""
103
+ global _HandPipeline
104
+ if _HandPipeline is None:
105
+ try:
106
+ from hand_pipeline import HandPipeline as _HP
107
+ _HandPipeline = _HP
108
+ except ImportError as e:
109
+ raise ImportError(
110
+ f"无法导入 glove 模式依赖 (hand_pipeline): {e}\n"
111
+ "请确保已安装: pip install ultralytics torch rtmlib"
112
+ ) from e
113
+ return _HandPipeline
114
+
115
+
116
+ def _lazy_import_common():
117
+ """导入 hand_common(纯 numpy/cv2)。"""
118
+ global _hand_common
119
+ if _hand_common is None:
120
+ try:
121
+ from hand_common import draw_hand, compute_joint_angles, count_extended_fingers, FINGERS
122
+ _hand_common = {
123
+ "draw_hand": draw_hand,
124
+ "compute_joint_angles": compute_joint_angles,
125
+ "count_extended_fingers": count_extended_fingers,
126
+ "FINGERS": FINGERS,
127
+ }
128
+ except ImportError as e:
129
+ raise ImportError(
130
+ f"无法导入 hand_common: {e}\n"
131
+ "请确保 tools/hand_detection/ 目录存在且包含 hand_common.py"
132
+ ) from e
133
+ return _hand_common
134
+
135
+
136
+ # ═════════════��═════════════════════════════════════════════
137
+ # 数据打包
138
+ # ═══════════════════════════════════════════════════════════
139
+
140
+ def _pack_hand_data(boxes, kpts, track_ids) -> np.ndarray:
141
+ """将一帧手部结果打包为固定长度 float32 数组 (TOTAL_DIM,)。"""
142
+ arr = np.zeros(TOTAL_DIM, dtype=np.float32)
143
+ num = min(len(boxes), MAX_HANDS)
144
+ for h in range(num):
145
+ off = h * PER_HAND_DIM
146
+ pts = kpts[h] if kpts is not None and h < len(kpts) else np.zeros((21, 2))
147
+ for j in range(min(21, len(pts))):
148
+ arr[off + j * 2] = float(pts[j][0])
149
+ arr[off + j * 2 + 1] = float(pts[j][1])
150
+ box = boxes[h] if h < len(boxes) else [0, 0, 0, 0]
151
+ for j in range(4):
152
+ arr[off + 42 + j] = float(box[j])
153
+ return arr
154
+
155
+
156
+ def _unpack_hand_data(arr: np.ndarray) -> tuple:
157
+ """从打包数组恢复 (boxes, kpts, num_hands)。"""
158
+ num = 0; boxes = []; kpts = []
159
+ for h in range(MAX_HANDS):
160
+ off = h * PER_HAND_DIM
161
+ box = [float(arr[off + 42 + j]) for j in range(4)]
162
+ if box[2] > 1 and box[3] > 1:
163
+ pts = np.array([[float(arr[off + j * 2]),
164
+ float(arr[off + j * 2 + 1])] for j in range(21)],
165
+ dtype=np.float32)
166
+ boxes.append(box); kpts.append(pts); num += 1
167
+ return boxes, kpts, num
168
+
169
+
170
+ def _pack_hand_3d_data(world_landmarks_list) -> np.ndarray:
171
+ """将单帧 3D 数据打包为 (PER_HAND_3D_DIM,) float32。"""
172
+ arr = np.zeros(PER_HAND_3D_DIM, dtype=np.float32)
173
+ wl = world_landmarks_list[0] if world_landmarks_list else None
174
+ if wl is not None:
175
+ flat = np.array(wl, dtype=np.float32).ravel()[:PER_HAND_3D_DIM]
176
+ arr[:len(flat)] = flat
177
+ return arr
178
+
179
+
180
+ # ═══════════════════════════════════════════════════════════
181
+ # 关键点叠加绘制
182
+ # ═══════════════════════════════════════════════════════════
183
+
184
+ def draw_kpts_overlay(frame: np.ndarray, data: np.ndarray,
185
+ track_ids: list = None) -> np.ndarray:
186
+ """在 BGR 帧上叠加手部关键点。"""
187
+ hc = _lazy_import_common()
188
+ overlay = frame.copy()
189
+ boxes, kpts, num = _unpack_hand_data(data)
190
+ if num == 0:
191
+ return overlay
192
+ for i, (box, pts) in enumerate(zip(boxes, kpts)):
193
+ x1, y1, x2, y2 = [int(v) for v in box]
194
+ cv2.rectangle(overlay, (x1, y1), (x2, y2), (0, 220, 220), 2, cv2.LINE_AA)
195
+ tid = track_ids[i] if track_ids and i < len(track_ids) else i
196
+ label = f"Hand #{tid}"
197
+ (tw, th), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)
198
+ cv2.rectangle(overlay, (x1, y1 - th - 6), (x1 + tw + 6, y1), (0, 200, 200), -1)
199
+ cv2.putText(overlay, label, (x1 + 3, y1 - 3),
200
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2, cv2.LINE_AA)
201
+ hc["draw_hand"](overlay, pts, angles=None, show_angles=False,
202
+ kpt_scores=None, thr=0.0)
203
+ try:
204
+ angles = hc["compute_joint_angles"](pts)
205
+ extended = hc["count_extended_fingers"](angles)
206
+ ext_text = "open: " + ",".join(extended) if extended else "fist"
207
+ cv2.putText(overlay, ext_text, (x1, y2 + 18),
208
+ cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 120), 1, cv2.LINE_AA)
209
+ except Exception:
210
+ pass
211
+ return overlay
212
+
213
+
214
+ # ═══════════════════════════════════════════════════════════
215
+ # 手势分类
216
+ # ═══════════════════════════════════════════════════════════
217
+
218
+ PINCH_THRESH = 30.0
219
+ TWO_HAND_TOUCH_THRESH = 80.0
220
+
221
+
222
+ def classify_gesture(extended: list, thumb_tip_pt: np.ndarray,
223
+ index_tip_pt: np.ndarray) -> dict:
224
+ """根据伸直手指 + 指尖距离分类手势。"""
225
+ n = len(extended)
226
+ pinch = (np.linalg.norm(thumb_tip_pt - index_tip_pt) < PINCH_THRESH)
227
+ if n == 0:
228
+ gesture, fist = "fist", True
229
+ elif n >= 4:
230
+ gesture, fist = ("open" if not pinch else "ok"), False
231
+ elif n == 1:
232
+ if "Index" in extended: gesture, fist = "point", False
233
+ elif "Thumb" in extended: gesture, fist = "hook", False
234
+ else: gesture, fist = "unknown", False
235
+ elif pinch and n == 3:
236
+ gesture, fist = "ok", False
237
+ elif pinch:
238
+ gesture, fist = "pinch", False
239
+ elif n in (2, 3):
240
+ gesture, fist = "grab", False
241
+ else:
242
+ gesture, fist = "unknown", False
243
+ return {"gesture": gesture, "pinch": bool(pinch), "fist": fist,
244
+ "extended_count": n}
245
+
246
+
247
+ def _thumb_tip(kpts: np.ndarray) -> np.ndarray:
248
+ return kpts[4] if len(kpts) > 4 else np.zeros(2)
249
+
250
+
251
+ def _index_tip(kpts: np.ndarray) -> np.ndarray:
252
+ return kpts[8] if len(kpts) > 8 else np.zeros(2)
253
+
254
+
255
+ def _hand_center(kpts: np.ndarray) -> np.ndarray:
256
+ wrist = kpts[0] if len(kpts) > 0 else np.zeros(2)
257
+ middle_mcp = kpts[9] if len(kpts) > 9 else np.zeros(2)
258
+ return (wrist + middle_mcp) / 2.0
259
+
260
+
261
+ def _compute_contact(motion_0: float, motion_1: float,
262
+ sensors: Optional[dict]) -> str:
263
+ max_p = 0.0
264
+ if sensors:
265
+ for arr in sensors.values():
266
+ if arr is not None and len(arr) > 0:
267
+ max_p = max(max_p, float(arr.max()))
268
+ motion = max(motion_0, motion_1) if motion_0 >= 0 else motion_0
269
+ if max_p > 500 and motion > 15:
270
+ return "grasping"
271
+ elif max_p > 500:
272
+ return "holding"
273
+ elif motion > 10:
274
+ return "reaching"
275
+ elif max_p > 0:
276
+ return "resting"
277
+ return "none"
278
+
279
+
280
+ # ═══════════════════════════════════════════════════════════
281
+ # 主入口: 批量处理一个会话
282
+ # ═══════════════════════════════════════════════════════════
283
+
284
+ def process_session(session_path: str,
285
+ mode: str = "",
286
+ detector: str = "",
287
+ det_device: str = "cuda",
288
+ pose_device: str = "cuda",
289
+ progress_cb: Optional[Callable[[int, int], None]] = None,
290
+ status_cb: Optional[Callable[[str], None]] = None,
291
+ cancel_check: Optional[Callable[[], bool]] = None,
292
+ ) -> dict:
293
+ """对已录制会话逐帧提取手部关键点,写入 parquet。
294
+
295
+ bare 模式额外保存 3D world_landmarks 到独立 parquet。
296
+
297
+ Returns:
298
+ {"success": True, "frames": N, "elapsed": s, "fps": float,
299
+ "mode": str, "session_path": str}
300
+ 或 {"success": False, "error": msg}
301
+ """
302
+ if not os.path.isdir(session_path):
303
+ return {"success": False, "error": f"会话目录不存在: {session_path}"}
304
+
305
+ mode = mode or settings.HAND_TRACK_MODE
306
+ det_path = detector or settings.HAND_DET_MODEL
307
+ det_device = det_device or settings.HAND_DET_DEVICE
308
+ pose_device = pose_device or settings.HAND_POSE_DEVICE
309
+
310
+ out_path = hand_kpts_parquet_path(session_path)
311
+ out_3d_path = hand_3d_parquet_path(session_path)
312
+ t_start = time.perf_counter()
313
+
314
+ try:
315
+ # ── 加载管线 ──────────────────────────────────
316
+ _is_mediapipe = False
317
+ if mode == "bare":
318
+ if status_cb: status_cb("loading (MediaPipe bare hand)")
319
+ _mp_model_src = settings.HAND_MEDIAPIPE_MODEL
320
+ _mp_cache = os.path.join(os.path.expanduser("~"), ".cache",
321
+ "hand_landmarker.task")
322
+ if not os.path.isfile(_mp_cache) or \
323
+ os.path.getsize(_mp_cache) != os.path.getsize(_mp_model_src):
324
+ os.makedirs(os.path.dirname(_mp_cache), exist_ok=True)
325
+ import shutil
326
+ shutil.copy2(_mp_model_src, _mp_cache)
327
+ from hand_pipeline_mediapipe import MediaPipeHandPipeline
328
+ pipeline = MediaPipeHandPipeline(
329
+ model_path=_mp_cache, num_hands=MAX_HANDS, mirror=False)
330
+ _is_mediapipe = True
331
+ else:
332
+ _lazy_import_pipeline()
333
+ try:
334
+ import torch
335
+ if not torch.cuda.is_available():
336
+ if det_device == "cuda": det_device = "cpu"
337
+ if pose_device == "cuda": pose_device = "cpu"
338
+ except Exception:
339
+ det_device = "cpu"; pose_device = "cpu"
340
+ pipeline = _HandPipeline(
341
+ detector=det_path, det_device=det_device,
342
+ pose_device=pose_device, max_hands=MAX_HANDS)
343
+ _is_mediapipe = False
344
+
345
+ if status_cb: status_cb("processing")
346
+
347
+ # ── 找视频 ────────────────────────────────────
348
+ fmt = detect_session_format(session_path)
349
+ cameras = {}
350
+ if fmt == "egodata":
351
+ meta_path = egodata_metadata_path(session_path)
352
+ else:
353
+ meta_path = os.path.join(session_path, "meta", "info.json")
354
+ if os.path.isfile(meta_path):
355
+ with open(meta_path, "r", encoding="utf-8") as f:
356
+ cameras = json.load(f).get("cameras", {})
357
+
358
+ # 为 EgoData 格式构造视频路径(cameras 元数据不含 "video" 键)
359
+ if fmt == "egodata" and cameras:
360
+ for cam_name in list(cameras.keys()):
361
+ # 优先 EgoData 标准路径, 回退 flat videos/<cam>.mp4
362
+ for vp in [egodata_video_path(session_path, cam_name),
363
+ os.path.join(session_path, "videos", f"{cam_name}.mp4")]:
364
+ if os.path.isfile(vp):
365
+ cameras[cam_name]["video"] = vp
366
+ break
367
+
368
+ if not cameras:
369
+ vdir = os.path.join(session_path, "videos")
370
+ if os.path.isdir(vdir):
371
+ for cam_name in sorted(os.listdir(vdir)):
372
+ if fmt == "egodata":
373
+ mp4 = egodata_video_path(session_path, cam_name)
374
+ flat = os.path.join(session_path, "videos", f"{cam_name}.mp4")
375
+ if os.path.isfile(mp4):
376
+ cameras[cam_name] = {"video": mp4}
377
+ elif cam_name.endswith(".mp4") and os.path.isfile(flat):
378
+ base = cam_name[:-4]
379
+ cameras[base] = {"video": flat}
380
+ else:
381
+ mp4 = video_mp4_path(session_path, cam_name)
382
+ if os.path.isfile(mp4):
383
+ cameras[cam_name] = {"video": mp4}
384
+ else:
385
+ old = os.path.join(vdir, cam_name, "chunk_000000.mp4")
386
+ if os.path.isfile(old):
387
+ cameras[cam_name] = {"video": old}
388
+
389
+ if not cameras:
390
+ return {"success": False, "error": "未找到视频文件"}
391
+
392
+ # 只保留有视频文件的 RGB 摄像机(排除 depth)
393
+ _rgb_cameras = {}
394
+ for cam_id, cam_info in cameras.items():
395
+ if isinstance(cam_info, dict) and cam_info.get("type") == "depth":
396
+ continue
397
+ if cam_id.lower().endswith("_depth"):
398
+ continue
399
+ vp = cam_info.get("video", "")
400
+ if os.path.isfile(vp):
401
+ _rgb_cameras[cam_id] = cam_info
402
+ cameras = _rgb_cameras
403
+
404
+ if not cameras:
405
+ return {"success": False, "error": "未找到可播放的视频文件"}
406
+
407
+ first_cam = list(cameras.keys())[0]
408
+ video_path = cameras[first_cam].get("video", "")
409
+
410
+ cap = cv2.VideoCapture(video_path)
411
+ if not cap.isOpened():
412
+ return {"success": False, "error": f"无法打开视频: {video_path}"}
413
+
414
+ total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
415
+ if total_frames <= 0:
416
+ total_frames = 1
417
+
418
+ # ── 逐帧处理 ──────────────────────────────────
419
+ rows = []
420
+ rows_3d = []
421
+ frame_idx = 0
422
+
423
+ while True:
424
+ if cancel_check and cancel_check():
425
+ cap.release()
426
+ return {"success": False, "error": "cancelled"}
427
+
428
+ ok, frame = cap.read()
429
+ if not ok:
430
+ break
431
+
432
+ if _is_mediapipe:
433
+ result = pipeline.process(frame)
434
+ boxes, kpts_list, tids_raw = [], [], []
435
+ world_3d_per_hand = []
436
+ handedness = []
437
+ for i, hand in enumerate(result.hands):
438
+ lms = hand.landmarks # (21, 2)
439
+ x1, y1 = lms.min(axis=0); x2, y2 = lms.max(axis=0)
440
+ boxes.append([float(x1), float(y1), float(x2), float(y2)])
441
+ kpts_list.append(lms)
442
+ tids_raw.append(i)
443
+ wl = getattr(hand, "world_landmarks", None)
444
+ world_3d_per_hand.append(wl)
445
+ hlabel = getattr(hand, "label", None) or getattr(hand, "handedness", "Unknown")
446
+ handedness.append(str(hlabel))
447
+ kpts = np.array(kpts_list, dtype=np.float32) if kpts_list \
448
+ else np.zeros((0, 21, 2), dtype=np.float32)
449
+ track_ids = tids_raw
450
+ else:
451
+ boxes, kpts, scores, track_ids = pipeline.process(frame)
452
+ if kpts is None: kpts = np.zeros((0, 21, 2), dtype=np.float32)
453
+ world_3d_per_hand = [None] * len(boxes)
454
+ handedness = ["Unknown"] * len(boxes)
455
+
456
+ # 2D 打包
457
+ packed = _pack_hand_data(boxes, kpts, track_ids)
458
+ num = min(len(boxes), MAX_HANDS)
459
+ tids = list(track_ids[:MAX_HANDS]) if track_ids else []
460
+ tids += [0] * (MAX_HANDS - len(tids))
461
+ rows.append({
462
+ "frame_index": frame_idx,
463
+ "num_hands": num,
464
+ "hand_data": packed.tolist(),
465
+ "track_ids": tids,
466
+ })
467
+
468
+ # 3D 打包 (bare 模式)
469
+ if _is_mediapipe:
470
+ hl = handedness if handedness else []
471
+ wl = world_3d_per_hand if world_3d_per_hand else []
472
+ row_3d = {
473
+ "frame_index": frame_idx,
474
+ "hand_0_present": len(boxes) >= 1,
475
+ "hand_0_landmarks_3d": _pack_hand_3d_data(wl[0:1]).tolist(),
476
+ "hand_0_label": hl[0] if hl else "Unknown",
477
+ "hand_1_present": len(boxes) >= 2,
478
+ "hand_1_landmarks_3d": _pack_hand_3d_data(wl[1:2]).tolist(),
479
+ "hand_1_label": hl[1] if len(hl) >= 2 else "Unknown",
480
+ }
481
+ rows_3d.append(row_3d)
482
+
483
+ frame_idx += 1
484
+ if progress_cb and frame_idx % 10 == 0:
485
+ progress_cb(frame_idx, total_frames)
486
+
487
+ cap.release()
488
+
489
+ if cancel_check and cancel_check():
490
+ return {"success": False, "error": "cancelled"}
491
+
492
+ # ── 写入 2D parquet ───────────────────────────
493
+ if rows:
494
+ table = pa.table({
495
+ "frame_index": pa.array([r["frame_index"] for r in rows], pa.int32()),
496
+ "num_hands": pa.array([r["num_hands"] for r in rows], pa.int32()),
497
+ "hand_data": pa.array([r["hand_data"] for r in rows],
498
+ pa.list_(pa.float32(), TOTAL_DIM)),
499
+ "track_ids": pa.array([r["track_ids"] for r in rows],
500
+ pa.list_(pa.int32(), MAX_HANDS)),
501
+ }, schema=_HAND_KPTS_SCHEMA)
502
+ os.makedirs(os.path.dirname(out_path), exist_ok=True)
503
+ pq.write_table(table, out_path, compression="zstd")
504
+
505
+ # ── 写入 3D parquet (bare 模式) ───────────────
506
+ if rows_3d:
507
+ table_3d = pa.table({
508
+ "frame_index": pa.array([r["frame_index"] for r in rows_3d], pa.int32()),
509
+ "hand_0_present": pa.array([r["hand_0_present"] for r in rows_3d], pa.bool_()),
510
+ "hand_0_landmarks_3d": pa.array([r["hand_0_landmarks_3d"] for r in rows_3d],
511
+ pa.list_(pa.float32(), PER_HAND_3D_DIM)),
512
+ "hand_0_label": pa.array([r["hand_0_label"] for r in rows_3d], pa.string()),
513
+ "hand_1_present": pa.array([r["hand_1_present"] for r in rows_3d], pa.bool_()),
514
+ "hand_1_landmarks_3d": pa.array([r["hand_1_landmarks_3d"] for r in rows_3d],
515
+ pa.list_(pa.float32(), PER_HAND_3D_DIM)),
516
+ "hand_1_label": pa.array([r["hand_1_label"] for r in rows_3d], pa.string()),
517
+ }, schema=_HAND_3D_SCHEMA)
518
+ os.makedirs(os.path.dirname(out_3d_path), exist_ok=True)
519
+ pq.write_table(table_3d, out_3d_path, compression="zstd")
520
+
521
+ elapsed = time.perf_counter() - t_start
522
+ result = {
523
+ "success": True, "frames": frame_idx, "elapsed": elapsed,
524
+ "fps": frame_idx / elapsed if elapsed > 0 else 0,
525
+ "mode": mode, "session_path": session_path,
526
+ }
527
+ if status_cb:
528
+ status_cb(f"done ({frame_idx} frames in {elapsed:.1f}s, "
529
+ f"{frame_idx/elapsed:.1f} fps)")
530
+ if progress_cb:
531
+ progress_cb(frame_idx, total_frames)
532
+ return result
533
+
534
+ except Exception as e:
535
+ import traceback
536
+ traceback.print_exc()
537
+ return {"success": False, "error": str(e)}
538
+
539
+
540
+ # ═══════════════════════════════════════════════════════════
541
+ # 自动标注
542
+ # ═══════════════════════════════════════════════════════════
543
+
544
+ MIN_SEGMENT_FRAMES = 10
545
+
546
+
547
+ def label_session(session_path: str,
548
+ progress_cb: Optional[Callable[[int, int], None]] = None,
549
+ cancel_check: Optional[Callable[[], bool]] = None,
550
+ ) -> dict:
551
+ """对手部关键点数据逐帧计算手势标签,写入 auto_labels.parquet + auto_actions.jsonl。
552
+
553
+ Returns:
554
+ {"success": True/False, "frames": N, "error": ""}
555
+ """
556
+ t_start = time.perf_counter()
557
+
558
+ try:
559
+ # 1. 加载手部关键点
560
+ hand_kpts = load_hand_kpts(session_path)
561
+ if not hand_kpts:
562
+ return {"success": False, "error": "手部关键点数据不存在,请先提取关键点"}
563
+
564
+ # 2. 加载传感器数据
565
+ sensor_data = None
566
+ sensor_path = data_parquet_path(session_path)
567
+ if os.path.isfile(sensor_path):
568
+ try:
569
+ sensor_rows = pq.read_table(sensor_path).to_pylist()
570
+ sensor_data = {}
571
+ for sn in settings.SENSOR_NAMES:
572
+ sensor_data[sn] = {}
573
+ for r in sensor_rows:
574
+ fi = r["frame_index"]
575
+ for sn in settings.SENSOR_NAMES:
576
+ col = f"observation.{sn}"
577
+ val = r.get(col)
578
+ if val is not None:
579
+ sensor_data[sn][fi] = np.array(val, dtype=np.float32)
580
+ except Exception:
581
+ sensor_data = None
582
+
583
+ # 3. 逐帧标注
584
+ _lazy_import_common()
585
+ frame_indices = sorted(hand_kpts.keys())
586
+ total = len(frame_indices)
587
+ rows = []
588
+ prev_centers = [None, None]
589
+
590
+ for idx, fi in enumerate(frame_indices):
591
+ if cancel_check and cancel_check():
592
+ return {"success": False, "error": "cancelled"}
593
+
594
+ kpt = hand_kpts[fi]
595
+ boxes, kpts_list, num = _unpack_hand_data(kpt["hand_data"])
596
+ row = {"frame_index": fi}
597
+
598
+ for h in range(2):
599
+ prefix = f"hand_{h}"
600
+ if h < num and h < len(kpts_list):
601
+ pts = kpts_list[h]
602
+ tt = _thumb_tip(pts)
603
+ it = _index_tip(pts)
604
+ hc_ = _lazy_import_common()
605
+ angles = hc_["compute_joint_angles"](pts)
606
+ extended = hc_["count_extended_fingers"](angles)
607
+ result = classify_gesture(extended, tt, it)
608
+
609
+ center = _hand_center(pts)
610
+ motion = 0.0
611
+ if prev_centers[h] is not None:
612
+ motion = float(np.linalg.norm(center - prev_centers[h]))
613
+ prev_centers[h] = center
614
+
615
+ row[f"{prefix}_gesture"] = result["gesture"]
616
+ row[f"{prefix}_extended"] = extended
617
+ row[f"{prefix}_extended_count"] = result["extended_count"]
618
+ row[f"{prefix}_fist"] = result["fist"]
619
+ row[f"{prefix}_pinch"] = result["pinch"]
620
+ row[f"{prefix}_center_x"] = float(center[0])
621
+ row[f"{prefix}_center_y"] = float(center[1])
622
+ row[f"{prefix}_motion"] = motion
623
+ else:
624
+ for key in ["gesture", "extended", "extended_count",
625
+ "fist", "pinch", "center_x", "center_y", "motion"]:
626
+ row[f"{prefix}_{key}"] = {
627
+ "gesture": "none", "extended": [], "extended_count": 0,
628
+ "fist": False, "pinch": False,
629
+ "center_x": -1.0, "center_y": -1.0, "motion": -1.0,
630
+ }[key]
631
+
632
+ # 两手距离
633
+ if num >= 2:
634
+ c0 = _hand_center(kpts_list[0])
635
+ c1 = _hand_center(kpts_list[1])
636
+ row["two_hand_distance"] = float(np.linalg.norm(c0 - c1))
637
+ else:
638
+ row["two_hand_distance"] = -1.0
639
+
640
+ # 接触状态
641
+ cur_sensors = None
642
+ if sensor_data is not None:
643
+ cur_sensors = {}
644
+ for sn in settings.SENSOR_NAMES:
645
+ arr = sensor_data[sn].get(fi)
646
+ if arr is not None:
647
+ cur_sensors[sn] = arr
648
+ row["contact"] = _compute_contact(
649
+ row["hand_0_motion"], row["hand_1_motion"], cur_sensors)
650
+
651
+ rows.append(row)
652
+
653
+ if progress_cb and idx % 100 == 0:
654
+ progress_cb(idx, total)
655
+
656
+ if cancel_check and cancel_check():
657
+ return {"success": False, "error": "cancelled"}
658
+
659
+ # 4. 写入 parquet
660
+ if rows:
661
+ table = pa.Table.from_pylist(rows, schema=_AUTO_LABELS_SCHEMA)
662
+ out_path = auto_labels_parquet_path(session_path)
663
+ os.makedirs(os.path.dirname(out_path), exist_ok=True)
664
+ pq.write_table(table, out_path, compression="zstd")
665
+
666
+ # 5. 时序分割 → jsonl
667
+ from core.helpers import _keypoints_session_dir
668
+ actions_path = os.path.join(_keypoints_session_dir(session_path),
669
+ "auto_actions.jsonl")
670
+ os.makedirs(os.path.dirname(actions_path), exist_ok=True)
671
+ with open(actions_path, "w", encoding="utf-8") as f:
672
+ for action in _segment_actions(rows):
673
+ f.write(json.dumps(action, ensure_ascii=False) + "\n")
674
+
675
+ elapsed = time.perf_counter() - t_start
676
+ if progress_cb:
677
+ progress_cb(total, total)
678
+ return {"success": True, "frames": total, "elapsed": elapsed}
679
+
680
+ except Exception as e:
681
+ import traceback
682
+ traceback.print_exc()
683
+ return {"success": False, "error": str(e)}
684
+
685
+
686
+ def _segment_actions(rows: list) -> list:
687
+ """帧级标签 → 动作片段列表。"""
688
+ if not rows:
689
+ return []
690
+ segments = []
691
+ cur_label = rows[0]["hand_0_gesture"]
692
+ cur_start = 0
693
+ for i in range(1, len(rows)):
694
+ label = rows[i]["hand_0_gesture"]
695
+ if label != cur_label:
696
+ segments.append({"start": cur_start, "end": i - 1, "label": cur_label})
697
+ cur_label = label
698
+ cur_start = i
699
+ segments.append({"start": cur_start, "end": len(rows) - 1, "label": cur_label})
700
+
701
+ for seg in segments:
702
+ seg["length"] = seg["end"] - seg["start"] + 1
703
+
704
+ merged = []
705
+ i = 0
706
+ while i < len(segments):
707
+ seg = segments[i]
708
+ if seg["length"] < MIN_SEGMENT_FRAMES and len(merged) > 0:
709
+ merged[-1]["end"] = seg["end"]
710
+ merged[-1]["length"] = merged[-1]["end"] - merged[-1]["start"] + 1
711
+ else:
712
+ merged.append(dict(seg))
713
+ i += 1
714
+
715
+ actions = []
716
+ for seg in merged:
717
+ seg_rows = rows[seg["start"]:seg["end"] + 1]
718
+ known = sum(1 for r in seg_rows
719
+ if r.get("hand_0_gesture", "unknown") != "unknown")
720
+ conf = known / max(len(seg_rows), 1)
721
+ actions.append({
722
+ "start_frame": seg["start"],
723
+ "end_frame": seg["end"],
724
+ "label": seg["label"],
725
+ "dominant_hand": 0,
726
+ "confidence": round(conf, 3),
727
+ })
728
+ return actions
729
+
730
+
731
+ # ═══════════════════════════════════════════════════════════
732
+ # 加载函数
733
+ # ═══════════════════════════════════════════════════════════
734
+
735
+ def load_hand_kpts(session_path: str) -> Optional[dict]:
736
+ """加载 2D 手部关键点,返回 {frame_index: {hand_data, num_hands, track_ids}}。
737
+
738
+ 读取顺序: keypoints_output/ → session/keypoints/ → session/annotations/
739
+ """
740
+ for path_fn in [hand_kpts_parquet_path, _session_kpts_hand_kpts_path,
741
+ _legacy_hand_kpts_path]:
742
+ kpts_path = path_fn(session_path)
743
+ if os.path.isfile(kpts_path):
744
+ break
745
+ else:
746
+ return None
747
+ table = pq.read_table(kpts_path)
748
+ rows = table.to_pylist()
749
+ return {r["frame_index"]: {
750
+ "hand_data": np.array(r["hand_data"], dtype=np.float32),
751
+ "num_hands": r["num_hands"],
752
+ "track_ids": r["track_ids"],
753
+ } for r in rows}
754
+
755
+
756
+ def load_hand_kpts_pooled(task_dir: str, episode_index: int) -> Optional[dict]:
757
+ """池化布局加载 2D 手部关键点(v1.1.0)。
758
+
759
+ 镜像目录 keypoints_output/<task>/episode_{N:06d}/hand_pose/chunk-000.parquet;
760
+ 返回结构与 load_hand_kpts 相同:{frame_index: {hand_data, num_hands,
761
+ track_ids}};无数据返回 None。
762
+ """
763
+ from core.helpers import episode_hand_kpts_parquet_path
764
+ kpts_path = episode_hand_kpts_parquet_path(task_dir, episode_index)
765
+ if not os.path.isfile(kpts_path):
766
+ return None
767
+ table = pq.read_table(kpts_path)
768
+ rows = table.to_pylist()
769
+ return {r["frame_index"]: {
770
+ "hand_data": np.array(r["hand_data"], dtype=np.float32),
771
+ "num_hands": r["num_hands"],
772
+ "track_ids": r["track_ids"],
773
+ } for r in rows}
774
+
775
+
776
+ def load_hand_3d(session_path: str) -> Optional[dict]:
777
+ """加载 3D 手部 landmarks,返回 {frame_index: {...}}。
778
+
779
+ 读取顺序: keypoints_output/ → session/keypoints/ → session/annotations/
780
+ """
781
+ for path_fn in [hand_3d_parquet_path, _session_kpts_hand_3d_path,
782
+ _legacy_hand_3d_path]:
783
+ path = path_fn(session_path)
784
+ if os.path.isfile(path):
785
+ break
786
+ else:
787
+ return None
788
+ table = pq.read_table(path)
789
+ rows = table.to_pylist()
790
+ return {r["frame_index"]: r for r in rows}
791
+
792
+
793
+ def load_auto_labels(session_path: str) -> Optional[dict]:
794
+ """加载自动标注,返回 {frame_index: row}。
795
+
796
+ 读取顺序: keypoints_output/ → session/keypoints/ → session/annotations/
797
+ """
798
+ for path_fn in [auto_labels_parquet_path, _session_kpts_auto_labels_path,
799
+ _legacy_auto_labels_path]:
800
+ path = path_fn(session_path)
801
+ if os.path.isfile(path):
802
+ break
803
+ else:
804
+ return None
805
+ table = pq.read_table(path)
806
+ rows = table.to_pylist()
807
+ return {r["frame_index"]: r for r in rows}
collector/core/helpers.py ADDED
@@ -0,0 +1,1281 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 通用工具函数 —— ID 生成、时间戳、时长格式化。
3
+ """
4
+
5
+ import os
6
+ import json
7
+ import re
8
+ import uuid
9
+ import ctypes
10
+ from datetime import datetime
11
+
12
+
13
+ def new_id() -> str:
14
+ """生成一个 12 位十六进制短 ID。"""
15
+ return uuid.uuid4().hex[:12]
16
+
17
+
18
+ def utcnow() -> str:
19
+ """返回 ISO-8601 格式的 UTC 时间戳字符串。"""
20
+ return datetime.utcnow().strftime("%Y-%m-%dT%H:%M:%SZ")
21
+
22
+
23
+ def send_to_recycle_bin(path: str) -> bool:
24
+ """将文件或文件夹移入回收站。
25
+
26
+ Windows: SHFileOperationW (FOF_ALLOWUNDO → 回收站)
27
+ Linux: gio trash(GNOME/GLib 自带);无 gio 时手动移入
28
+ ~/.local/share/Trash(GNOME 标准回收站)。
29
+
30
+ 返回 True 表示成功,False 表示失败。
31
+ """
32
+ if not os.path.exists(path):
33
+ return False
34
+
35
+ # ── Linux: gio trash(GNOME 桌面自带,移入桌面回收站)──
36
+ if os.name != "nt":
37
+ import shutil
38
+ import subprocess
39
+ gio = shutil.which("gio")
40
+ if gio:
41
+ try:
42
+ ret = subprocess.run([gio, "trash", path],
43
+ capture_output=True, timeout=30)
44
+ if ret.returncode == 0:
45
+ return True
46
+ except Exception:
47
+ pass
48
+ # fallback: 手动移入 GNOME 回收站目录(记录原始路径以便恢复)
49
+ trash_dir = os.path.expanduser("~/.local/share/Trash")
50
+ try:
51
+ files_dir = os.path.join(trash_dir, "files")
52
+ info_dir = os.path.join(trash_dir, "info")
53
+ os.makedirs(files_dir, exist_ok=True)
54
+ os.makedirs(info_dir, exist_ok=True)
55
+ base = os.path.basename(path.rstrip("/")) or "item"
56
+ dest = os.path.join(files_dir, base)
57
+ if os.path.exists(dest): # 重名时加后缀
58
+ stem, ext = os.path.splitext(base)
59
+ i = 1
60
+ while os.path.exists(dest):
61
+ dest = os.path.join(files_dir, f"{stem}_{i}{ext}")
62
+ i += 1
63
+ os.rename(path, dest)
64
+ import time
65
+ with open(os.path.join(info_dir, os.path.basename(dest) + ".trashinfo"), "w") as f:
66
+ f.write("[Trash Info]\n")
67
+ f.write(f"Path={path}\n")
68
+ f.write(f"DeletionDate={time.strftime('%Y-%m-%dT%H:%M:%S')}\n")
69
+ return True
70
+ except Exception:
71
+ return False
72
+
73
+ # ── Windows: SHFileOperationW ──
74
+ try:
75
+ # SHFileOperationW 需要双 null 结尾的宽字符串
76
+ from ctypes import wintypes
77
+ SHF = ctypes.windll.shell32.SHFileOperationW
78
+
79
+ class SHFILEOPSTRUCTW(ctypes.Structure):
80
+ _fields_ = [
81
+ ("hwnd", wintypes.HWND),
82
+ ("wFunc", wintypes.UINT),
83
+ ("pFrom", wintypes.LPCWSTR),
84
+ ("pTo", wintypes.LPCWSTR),
85
+ ("fFlags", wintypes.WORD),
86
+ ("fAnyOperationsAborted", wintypes.BOOL),
87
+ ("hNameMappings", wintypes.LPVOID),
88
+ ("lpszProgressTitle", wintypes.LPCWSTR),
89
+ ]
90
+
91
+ FO_DELETE = 3
92
+ FOF_ALLOWUNDO = 0x40
93
+ FOF_NOCONFIRMATION = 0x10
94
+ FOF_SILENT = 0x04
95
+
96
+ fileop = SHFILEOPSTRUCTW()
97
+ fileop.wFunc = FO_DELETE
98
+ fileop.pFrom = path + "\0" # null 结尾(ctypes 自动处理宽字符)
99
+ fileop.fFlags = FOF_ALLOWUNDO | FOF_NOCONFIRMATION
100
+
101
+ result = SHF(ctypes.byref(fileop))
102
+ return result == 0
103
+ except Exception:
104
+ return False
105
+
106
+
107
+ def format_duration(seconds: float) -> str:
108
+ """将秒数格式化为 HH:MM:SS 字符串。"""
109
+ h = int(seconds // 3600)
110
+ m = int((seconds % 3600) // 60)
111
+ s = int(seconds % 60)
112
+ return f"{h:02d}:{m:02d}:{s:02d}"
113
+
114
+
115
+ # ── EgoData 会话摘要 ──────────────────────────────
116
+
117
+ def session_size_mb(session_dir: str) -> float:
118
+ """遍历会话目录累加所有文件大小,返回 MB。目录不存在/出错返回 0。"""
119
+ if not session_dir or not os.path.isdir(session_dir):
120
+ return 0.0
121
+ total = 0
122
+ try:
123
+ for root, _dirs, files in os.walk(session_dir):
124
+ for fn in files:
125
+ try:
126
+ total += os.path.getsize(os.path.join(root, fn))
127
+ except OSError:
128
+ pass
129
+ except OSError:
130
+ return 0.0
131
+ return total / (1024.0 * 1024.0)
132
+
133
+
134
+ def read_metadata(session_dir: str) -> dict:
135
+ """读 EgoData metadata.json,任何失败返回 {}。"""
136
+ try:
137
+ p = egodata_metadata_path(session_dir)
138
+ if os.path.isfile(p):
139
+ with open(p, "r", encoding="utf-8") as f:
140
+ return json.load(f)
141
+ except (OSError, json.JSONDecodeError):
142
+ pass
143
+ return {}
144
+
145
+
146
+ def _slot_base(name: str, all_names) -> str:
147
+ """流名 → 设备基名(legacy 会话无 devices 段��的启发式)。
148
+
149
+ 兼容三种命名惯例:
150
+ xxx_rgb → xxx
151
+ xxx_depth → xxx(同组存在 xxx_rgb 时)
152
+ xxx_depth → xxx_depth(同组存在 xxx_depth_rgb,即设备本身被命名为
153
+ xxx_depth,如 D435_depth)
154
+ 同前缀多台设备的 _N 编号后缀(settings 自动追加)保留在基名上:
155
+ d435_rgb_2 / d435_depth_2 → d435_2
156
+ 其余原样返回(stereo_left/stereo_right 等)。
157
+ """
158
+ m = re.match(r"^(.*)_(\d+)$", name)
159
+ core, num = (m.group(1), m.group(2)) if m else (name, "")
160
+ if core.endswith("_rgb"):
161
+ base = core[:-4]
162
+ elif core.endswith("_depth"):
163
+ if core + "_rgb" in all_names:
164
+ base = core
165
+ elif core[:-6] + "_rgb" in all_names:
166
+ base = core[:-6]
167
+ else:
168
+ base = core
169
+ else:
170
+ base = core
171
+ return base + ("_" + num if num else "")
172
+
173
+
174
+ def _sub_name(slot: str, dev_name: str, slots: list) -> str:
175
+ """槽名 → 子画面名:优先去设备名前缀,其次去槽名公共前缀。
176
+
177
+ slot == dev_name 的惯例是深度槽以设备本身命名(如 D435_depth),
178
+ 子画面名即 "depth"。
179
+ """
180
+ if slot == dev_name:
181
+ return "depth"
182
+ if dev_name and slot.startswith(dev_name + "_"):
183
+ return slot[len(dev_name) + 1:]
184
+ if len(slots) > 1:
185
+ prefix = os.path.commonprefix(slots)
186
+ if prefix:
187
+ return slot[len(prefix):].lstrip("_")
188
+ return slot
189
+
190
+
191
+ def _device_display_names(meta: dict) -> list:
192
+ """录制历史"摄像机"列文本:按设备聚合,不把子画面平铺当相机。
193
+
194
+ D435 一台设备出 rgb+depth 两路流 → "D435_depth (rgb, depth)";
195
+ 双目 S80M → "FaysSense S80M (left, right)";单流设备只显示设备名。
196
+ 有 devices 段按设备条目聚合(跳过 data_ble 手套/无槽设备);
197
+ legacy 会话无 devices 段时按 _slot_base 对 cameras 键分组。
198
+ """
199
+ devices = meta.get("devices") or []
200
+ groups = [] # [(设备名, [槽名...])]
201
+ if devices:
202
+ for d in devices:
203
+ slots = list(d.get("slots") or [])
204
+ if not slots or d.get("kind") == "data_ble":
205
+ continue
206
+ name = d.get("name") or ""
207
+ if not name:
208
+ if len(slots) > 1:
209
+ prefix = os.path.commonprefix(slots).rstrip("_")
210
+ name = prefix or _slot_base(slots[0], set(slots))
211
+ else:
212
+ name = _slot_base(slots[0], set(slots))
213
+ groups.append((name, slots))
214
+ else:
215
+ cam_names = list((meta.get("cameras") or {}).keys())
216
+ by_base = {}
217
+ for n in cam_names:
218
+ by_base.setdefault(_slot_base(n, set(cam_names)), []).append(n)
219
+ groups = list(by_base.items())
220
+ disp = []
221
+ for name, slots in groups:
222
+ subs = [s for s in (_sub_name(s, name, slots) for s in slots) if s]
223
+ if len(subs) >= 2:
224
+ disp.append(f"{name} ({', '.join(subs)})")
225
+ else:
226
+ disp.append(name)
227
+ return disp
228
+
229
+
230
+ def session_summary(session_dir: str,
231
+ frames_by_cam: dict,
232
+ episode_index: int = 0) -> tuple:
233
+ """会话展示摘要。返回 (camera_list, duration_sec, size_mb)。
234
+
235
+ episode_index > 0 且目录为池化布局时走任务级口径:
236
+ - camera_list: info.json devices 段按设备聚合
237
+ - duration_sec: episodes 每段文件行 duration_sec
238
+ - size_mb: episode_size_mb(task_dir, N)(只统计本 episode 文件组)
239
+ 否则走旧会话目录口径(历史自愈/未迁移残留):
240
+ - camera_list: 按设备聚合的显示名(D435 一台设备出 rgb+depth 两路
241
+ 子画面 → "D435_depth (rgb, depth)",见 _device_display_names);
242
+ metadata 缺失时用 frames_by_cam 中有帧的键(sorted);
243
+ 两者皆空返回 ""(调用方兜底)
244
+ - duration_sec: max(每相机 帧数/fps);每相机 fps 优先
245
+ cameras[cam].fps(egodata_writer 只在异于全局时写入),缺省
246
+ metadata["fps"],再缺省 settings.RECORDING_FPS;帧数为 0 的相机
247
+ 贡献 0;帧数全部缺失(旧记录自愈)时降级 timestamps.json 最后一
248
+ 帧相对时间,再降级 total_frames / 最小 fps(近似)
249
+ - size_mb: session_size_mb(session_dir)
250
+ """
251
+ if episode_index and detect_session_format(session_dir) == "pooled":
252
+ info = {}
253
+ try:
254
+ with open(pooled_info_path(session_dir), "r",
255
+ encoding="utf-8") as f:
256
+ info = json.load(f)
257
+ except (OSError, json.JSONDecodeError):
258
+ info = {}
259
+ row = episode_row(session_dir, episode_index)
260
+ duration = float(row.get("duration_sec") or 0.0)
261
+ display = ", ".join(_device_display_names(info)) if info else ""
262
+ if not display:
263
+ # 无 devices 段兜底:本 episode 实际存在的视频 key
264
+ display = ", ".join(
265
+ sorted(episode_video_files(session_dir, episode_index)))
266
+ return display, duration, episode_size_mb(session_dir, episode_index)
267
+
268
+ meta = read_metadata(session_dir)
269
+ cam_names = list((meta.get("cameras") or {}).keys())
270
+ if not cam_names:
271
+ cam_names = sorted(k for k, v in (frames_by_cam or {}).items() if v > 0)
272
+ fps_list = []
273
+ for cam in cam_names:
274
+ info = (meta.get("cameras") or {}).get(cam) or {}
275
+ fps = float(info.get("fps") or meta.get("fps") or _settings.RECORDING_FPS)
276
+ frames = float((frames_by_cam or {}).get(cam, 0))
277
+ if frames > 0:
278
+ fps_list.append(frames / max(fps, 1.0))
279
+ duration = max(fps_list) if fps_list else 0.0
280
+ if duration == 0.0 and cam_names:
281
+ duration = _timestamps_duration(session_dir, meta, cam_names)
282
+ display = ", ".join(_device_display_names(meta)) if meta else ""
283
+ if not display:
284
+ display = ", ".join(cam_names) # 无 cameras 元数据 → 旧行为
285
+ return display, duration, session_size_mb(session_dir)
286
+
287
+
288
+ def _timestamps_duration(session_dir: str, meta: dict, cam_names: list) -> float:
289
+ """timestamps.json 降级补算时长:最后一条相对时间(精确),
290
+ 再降级 total_frames / 最小 fps(近似,同 playback 的 _get_effective_fps 思路)。"""
291
+ try:
292
+ p = egodata_timestamps_path(session_dir)
293
+ if not os.path.isfile(p):
294
+ return 0.0
295
+ with open(p, "r", encoding="utf-8") as f:
296
+ data = json.load(f)
297
+ ts_list = data.get("timestamps") or []
298
+ if ts_list and isinstance(ts_list[-1], dict):
299
+ last_ts = ts_list[-1].get("timestamp")
300
+ if isinstance(last_ts, (int, float)) and last_ts > 0:
301
+ return float(last_ts)
302
+ total = data.get("total_frames")
303
+ if isinstance(total, (int, float)) and total > 0:
304
+ fps_values = []
305
+ for name in cam_names:
306
+ info = (meta.get("cameras") or {}).get(name) or {}
307
+ fps_values.append(float(info.get("fps") or meta.get("fps")
308
+ or _settings.RECORDING_FPS))
309
+ fps = min(fps_values) if fps_values else float(_settings.RECORDING_FPS)
310
+ return float(total) / max(fps, 1.0)
311
+ except (OSError, ValueError, TypeError, json.JSONDecodeError):
312
+ pass
313
+ return 0.0
314
+
315
+
316
+ def format_size_mb(mb: float) -> str:
317
+ """大小列文本: >0 显示 "12.3 MB",0/缺失显示 "-"。"""
318
+ if not mb or mb <= 0:
319
+ return "-"
320
+ return f"{mb:.1f} MB"
321
+
322
+
323
+ def timestamp_filename(prefix: str = "rec", ext: str = ".avi") -> str:
324
+ """生成带时间戳的文件名,例如 rec_20260804_143052.avi"""
325
+ stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
326
+ return f"{prefix}_{stamp}{ext}"
327
+
328
+
329
+ def _sanitize_tag(raw: str) -> str:
330
+ """清洗标签名,保留字母数字、中文和下划线。"""
331
+ tag = raw.strip() if raw else "session"
332
+ return "".join(c if c.isalnum() or c in "_一-鿿" else "_" for c in tag)
333
+
334
+
335
+ def task_tag(task_name: str = "") -> str:
336
+ """生成任务分类标签(用于子文件夹名)。
337
+
338
+ Args:
339
+ task_name: 任务标注(如 "grasp_cup"),为空时返回 "session"
340
+
341
+ Returns:
342
+ 清洗后的标签名,例如 "grasp_cup"、"test00"、"session"
343
+ """
344
+ return _sanitize_tag(task_name)
345
+
346
+
347
+ def session_dirname(task_name: str = "", batch_index: int = 0) -> str:
348
+ """生成会话目录名。
349
+
350
+ Args:
351
+ task_name: 任务标注(如 "grasp_cup"),为空时默认用 "session"
352
+
353
+ Returns:
354
+ 目录名字符串,例如 "grasp_cup_20260805_195046" 或 "session_20260805_195046"
355
+ """
356
+ tag = _sanitize_tag(task_name)
357
+ stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
358
+ if batch_index > 0:
359
+ return f"{tag}_{batch_index:04d}_{stamp}"
360
+ return f"{tag}_{stamp}"
361
+
362
+
363
+ # ═══════════════════════════════════════════════════════════
364
+ # LeRobot v3 分块存储路径工具
365
+ # ═══════════════════════════════════════════════════════════
366
+
367
+ def chunk_dir(chunk_index: int = 0) -> str:
368
+ """chunk 目录名: chunk-000"""
369
+ return f"chunk-{chunk_index:03d}"
370
+
371
+
372
+ def chunk_file(chunk_index: int = 0, name: str = "file") -> str:
373
+ """chunk 内文件名: file-000.parquet / file-000.mp4"""
374
+ return f"{name}-{chunk_index:03d}"
375
+
376
+
377
+ def data_parquet_path(session_dir: str, chunk_index: int = 0) -> str:
378
+ """data/chunk-000/file-000.parquet"""
379
+ return os.path.join(session_dir, "data", chunk_dir(chunk_index),
380
+ chunk_file(chunk_index) + ".parquet")
381
+
382
+
383
+ def episode_parquet_path(session_dir: str, chunk_index: int = 0) -> str:
384
+ """meta/episodes/chunk-000/file-000.parquet"""
385
+ return os.path.join(session_dir, "meta", "episodes", chunk_dir(chunk_index),
386
+ chunk_file(chunk_index) + ".parquet")
387
+
388
+
389
+ def video_mp4_path(session_dir: str, camera_key: str, chunk_index: int = 0) -> str:
390
+ """videos/<camera_key>/chunk-000.mp4"""
391
+ return os.path.join(session_dir, "videos", camera_key,
392
+ f"{chunk_dir(chunk_index)}.mp4")
393
+
394
+
395
+ def tasks_jsonl_path(session_dir: str) -> str:
396
+ """meta/tasks.jsonl"""
397
+ return os.path.join(session_dir, "meta", "tasks.jsonl")
398
+
399
+
400
+ # ═══════════════════════════════════════════════════════════
401
+ # 池化任务布局路径工具(v1.1.0)
402
+ # ═══════════════════════════════════════════════════════════
403
+
404
+ POOLED_CHUNK_SIZE = 1000
405
+
406
+
407
+ def episode_chunk_file(episode_index: int) -> tuple:
408
+ """全局 episode 序号 N(1 起)→ (chunk_index, file_index)。
409
+
410
+ 每 chunk 1000 个 episode,episode 文件号三位零起:
411
+ N=1 → (0, 0) chunk-000/episode-000
412
+ N=1000 → (0, 999)
413
+ N=1001 → (1, 0) chunk-001/episode-000
414
+ """
415
+ n = max(int(episode_index) - 1, 0)
416
+ return n // POOLED_CHUNK_SIZE, n % POOLED_CHUNK_SIZE
417
+
418
+
419
+ def pooled_file_stem(file_index: int, ext: str = "") -> str:
420
+ """chunk 内文件名: episode-000.parquet / episode-000.mp4(v1.1.2 起)"""
421
+ return f"episode-{file_index:03d}{ext}"
422
+
423
+
424
+ def episode_file_suffix(episode_index: int) -> int:
425
+ """episode N 的 episode 文件号(0 基):episode-000 = 第 1 段(= 本地文件后缀)。
426
+
427
+ 上传/回放界面的任务名后缀与上传表单 episode_index 均用此值,
428
+ 与本地 episode-NNN 完全对齐;真实全局序号 N 始终在 parquet 行内。
429
+ """
430
+ return episode_chunk_file(episode_index)[1]
431
+
432
+
433
+ def task_dir_of(base_dir: str, task_name: str) -> str:
434
+ """池化任务目录: <base_dir>/<task_tag>(目录名与旧任务目录一致)。"""
435
+ return os.path.join(base_dir, task_tag(task_name))
436
+
437
+
438
+ def pooled_video_path(task_dir: str, image_key: str, episode_index: int,
439
+ ext: str = "mp4") -> str:
440
+ """videos/chunk-NNN/<image_key>/episode-NNN.<ext>(每 episode 每流一文件)"""
441
+ c, f = episode_chunk_file(episode_index)
442
+ return os.path.join(task_dir, "videos", chunk_dir(c), image_key,
443
+ pooled_file_stem(f, f".{ext}"))
444
+
445
+
446
+ def pooled_video_dir(task_dir: str, image_key: str, episode_index: int) -> str:
447
+ """videos/chunk-NNN/<image_key>/ 目录路径"""
448
+ c, _ = episode_chunk_file(episode_index)
449
+ return os.path.join(task_dir, "videos", chunk_dir(c), image_key)
450
+
451
+
452
+ def pooled_data_parquet_path(task_dir: str, episode_index: int) -> str:
453
+ """data/chunk-NNN/episode-NNN.parquet(每 episode 一个,稀疏列)"""
454
+ c, f = episode_chunk_file(episode_index)
455
+ return os.path.join(task_dir, "data", chunk_dir(c),
456
+ pooled_file_stem(f, ".parquet"))
457
+
458
+
459
+ def pooled_episodes_path(task_dir: str, episode_index: int) -> str:
460
+ """meta/episodes/chunk-NNN/episode-NNN.parquet(每 episode 一个文件,
461
+ 与 data/videos 同编号:episode-000 = episode 1;单行 10 列)。"""
462
+ c, f = episode_chunk_file(episode_index)
463
+ return os.path.join(task_dir, "meta", "episodes", chunk_dir(c),
464
+ pooled_file_stem(f, ".parquet"))
465
+
466
+
467
+ def _legacy_episodes_shard_path(task_dir: str, chunk_index: int) -> str:
468
+ """v1.1.0 旧布局的 episodes 分片路径(每 chunk 一个 ≤1000 行文件,
469
+ 文件名 = chunk 号,即 file-{chunk:03d}.parquet——v1.1.2 每段文件改
470
+ episode- 前缀后两者不再重名)。仅作旧数据回退读取/删行用。"""
471
+ return os.path.join(task_dir, "meta", "episodes", chunk_dir(chunk_index),
472
+ chunk_file(chunk_index) + ".parquet")
473
+
474
+
475
+ def pooled_info_path(task_dir: str) -> str:
476
+ """meta/info.json(任务级)"""
477
+ return os.path.join(task_dir, "meta", "info.json")
478
+
479
+
480
+ def pooled_stats_path(task_dir: str) -> str:
481
+ """meta/stats.json(任务级全局统计,块含 count/mean/std/min/max)。
482
+
483
+ v1.1.1 起 stats.json 自身即增量累加器(count 可反推 sum/sum_sq),
484
+ 不再有 .stats_state.json 边车。
485
+ """
486
+ return os.path.join(task_dir, "meta", "stats.json")
487
+
488
+
489
+ def pooled_tasks_jsonl_path(task_dir: str) -> str:
490
+ """meta/tasks.jsonl(任务级)"""
491
+ return os.path.join(task_dir, "meta", "tasks.jsonl")
492
+
493
+
494
+ def list_task_episodes(task_dir: str) -> list:
495
+ """扫描池化任务目录,返回已存在的全局 episode 序号列表(升序)。
496
+
497
+ 依据 data/ 与 videos/ 下的 episode-NNN 文件组(episodes 行可能因
498
+ abort 回滚而缺失,数据文件才是权威)。任务目录不存在返回 []。
499
+ """
500
+ if not os.path.isdir(task_dir):
501
+ return []
502
+ ns = set()
503
+ for sub in ("data", "videos"):
504
+ root = os.path.join(task_dir, sub)
505
+ if not os.path.isdir(root):
506
+ continue
507
+ for cname in os.listdir(root):
508
+ m = re.match(r"^chunk-(\d+)$", cname)
509
+ if not m:
510
+ continue
511
+ cdir = os.path.join(root, cname)
512
+ if not os.path.isdir(cdir):
513
+ continue
514
+ chunk = int(m.group(1))
515
+ for entry in os.listdir(cdir):
516
+ p = os.path.join(cdir, entry)
517
+ if os.path.isdir(p): # videos: image_key 目录
518
+ for fn in os.listdir(p):
519
+ fm = re.match(r"^episode-(\d+)\.", fn)
520
+ if fm:
521
+ ns.add(chunk * POOLED_CHUNK_SIZE
522
+ + int(fm.group(1)) + 1)
523
+ else: # data: 直接 episode-NNN.parquet
524
+ fm = re.match(r"^episode-(\d+)\.parquet$", entry)
525
+ if fm:
526
+ ns.add(chunk * POOLED_CHUNK_SIZE
527
+ + int(fm.group(1)) + 1)
528
+ return sorted(ns)
529
+
530
+
531
+ def recycled_episode_path(task_dir: str) -> str:
532
+ """异常终止回退标记文件路径(meta/recycled_episode.json)。"""
533
+ return os.path.join(task_dir, "meta", "recycled_episode.json")
534
+
535
+
536
+ def read_recycled_episode(task_dir: str) -> int:
537
+ """读取上次异常终止释放的 episode 序号;无标记/损坏返回 0。"""
538
+ try:
539
+ with open(recycled_episode_path(task_dir), "r",
540
+ encoding="utf-8") as f:
541
+ data = json.load(f)
542
+ n = int(data.get("episode_index", 0) or 0)
543
+ return n if n > 0 else 0
544
+ except (OSError, ValueError, TypeError):
545
+ return 0
546
+
547
+
548
+ def mark_recycled_episode(task_dir: str, episode_index: int) -> None:
549
+ """异常终止后标记该序号可复用(下次录制优先取回——不占号语义)。
550
+
551
+ 原子写(临时件 + os.replace);episode_index <= 0 视为清除。
552
+ 标记由本段正常完成(writer.end_episode)时清除。
553
+ """
554
+ path = recycled_episode_path(task_dir)
555
+ if episode_index <= 0:
556
+ clear_recycled_episode(task_dir)
557
+ return
558
+ try:
559
+ os.makedirs(os.path.dirname(path), exist_ok=True)
560
+ tmp = f"{path}.tmp{os.getpid()}"
561
+ with open(tmp, "w", encoding="utf-8") as f:
562
+ json.dump({"episode_index": int(episode_index),
563
+ "freed_at": utcnow()}, f)
564
+ os.replace(tmp, path)
565
+ except OSError:
566
+ pass
567
+
568
+
569
+ def clear_recycled_episode(task_dir: str) -> None:
570
+ """录制正常完成后清除回退标记(该号已被本段重新占住)。"""
571
+ try:
572
+ os.remove(recycled_episode_path(task_dir))
573
+ except OSError:
574
+ pass
575
+
576
+
577
+ def next_pooled_episode_index(task_dir: str, batch_index: int = 0) -> int:
578
+ """返回任务的下一个全局 episode 序号。
579
+
580
+ 权威 = 任务进度序号 batch_index(录制完成次数 + 1,与本地文件删除
581
+ 无关——上传后自动删除不会让序号回退)与目录扫描 max + 1 取大
582
+ (防与现存文件组重名)。batch_index <= 0 退化为纯目录扫描。
583
+
584
+ 异常终止回退(不占号):上次中止写下的 recycled 标记若其号未被
585
+ 文件组占用,优先复用该号——batch_index 水位跑在前面也
586
+ 不跳号;号已被占(跨机共享目录/遗留)时自动放弃标记并走常规取号。
587
+ """
588
+ existing = list_task_episodes(task_dir)
589
+ recycled = read_recycled_episode(task_dir)
590
+ if recycled:
591
+ if recycled not in existing:
592
+ return recycled
593
+ clear_recycled_episode(task_dir)
594
+ scan_next = (existing[-1] + 1) if existing else 1
595
+ if batch_index > 0:
596
+ return max(batch_index, scan_next)
597
+ return scan_next
598
+
599
+
600
+ def episode_video_files(task_dir: str, episode_index: int) -> dict:
601
+ """episode N 的视频文件组 {image_key: 文件路径}。
602
+
603
+ key 可跨 episode 改名(同一物理相机不同会话槽名不同),以本 episode
604
+ 实际存在的文件为准,不按任务级 video_extensions 推断。
605
+ """
606
+ c, f = episode_chunk_file(episode_index)
607
+ stem = pooled_file_stem(f, "")
608
+ out = {}
609
+ vroot = os.path.join(task_dir, "videos", chunk_dir(c))
610
+ if os.path.isdir(vroot):
611
+ for key in sorted(os.listdir(vroot)):
612
+ kd = os.path.join(vroot, key)
613
+ if not os.path.isdir(kd):
614
+ continue
615
+ for fn in sorted(os.listdir(kd)):
616
+ if fn.startswith(stem + "."):
617
+ out[key] = os.path.join(kd, fn)
618
+ return out
619
+
620
+
621
+ def episode_size_mb(task_dir: str, episode_index: int) -> float:
622
+ """episode N 的文件组总大小(MB;视频 + data parquet)。"""
623
+ total = 0
624
+ for p in episode_video_files(task_dir, episode_index).values():
625
+ try:
626
+ total += os.path.getsize(p)
627
+ except OSError:
628
+ pass
629
+ dp = pooled_data_parquet_path(task_dir, episode_index)
630
+ if os.path.isfile(dp):
631
+ try:
632
+ total += os.path.getsize(dp)
633
+ except OSError:
634
+ pass
635
+ return total / (1024.0 * 1024.0)
636
+
637
+
638
+ def episode_row(task_dir: str, episode_index: int) -> dict:
639
+ """episode N 的元数据行;不存在返回 {}。
640
+
641
+ 新布局:每段一个文件(pooled_episodes_path 直读)。
642
+ 旧布局回退:v1.1.0 分片(每 chunk 一个 ≤1000 行文件,文件名 =
643
+ chunk 号)里按 episode_index 列过滤。
644
+ """
645
+ import pyarrow.parquet as pq
646
+ p = pooled_episodes_path(task_dir, episode_index)
647
+ if os.path.isfile(p):
648
+ try:
649
+ for r in pq.read_table(p).to_pylist():
650
+ if r.get("episode_index") == episode_index:
651
+ return r
652
+ except Exception:
653
+ pass
654
+ # 旧分片回退(分片名 = chunk 号 file-{c:03d},v1.1.2 起与每段文件
655
+ # episode-{f:03d} 不再重名)
656
+ c, _ = episode_chunk_file(episode_index)
657
+ legacy = _legacy_episodes_shard_path(task_dir, c)
658
+ if legacy != p and os.path.isfile(legacy):
659
+ try:
660
+ for r in pq.read_table(legacy).to_pylist():
661
+ if r.get("episode_index") == episode_index:
662
+ return r
663
+ except Exception:
664
+ pass
665
+ return {}
666
+
667
+
668
+ def episode_refs(base_dir: str) -> list:
669
+ """扫描全部池化任务的 episode 列表(读侧唯一枚举入口)。
670
+
671
+ Returns:
672
+ [{"task", "task_dir", "episode_index", "name"}, ...]
673
+ name 后缀 = episode 文件号(0 基,与本地 episode-NNN 对齐);按名称倒序
674
+ (最新任务/最新序号在前)。旧格式会话目录不再列出
675
+ (v1.1.0 读侧只认池化布局)。
676
+ """
677
+ out = []
678
+ if not os.path.isdir(base_dir):
679
+ return out
680
+ for task in sorted(os.listdir(base_dir)):
681
+ task_dir = os.path.join(base_dir, task)
682
+ if not os.path.isdir(task_dir) or task.startswith((".", "_")):
683
+ continue
684
+ if detect_session_format(task_dir) != "pooled":
685
+ continue
686
+ for n in list_task_episodes(task_dir):
687
+ out.append({"task": task, "task_dir": task_dir,
688
+ "episode_index": n,
689
+ "name": f"{task}_ep{episode_file_suffix(n):06d}"})
690
+ return sorted(out, key=lambda s: s["name"], reverse=True)
691
+
692
+
693
+ # ═══════════════════════════════════════════════════════
694
+ # 任务级统计(v1.1.1:stats.json 自含累加器,无 .stats_state 边车)
695
+ # ═══════════════════════════════════════════════════════
696
+
697
+ def stat_block_to_sum(blk: dict) -> dict:
698
+ """统计块归一为 sum-form {count, sum, sum_sq, min, max}。
699
+
700
+ 兼容两种入参:writer/迁移产出的 sum-form;stats.json 的 json-form
701
+ {count, mean, std, min, max}(按 mean/std/count 反推 sum/sum_sq——
702
+ 这是 stats.json 能当累加器的基础)。
703
+ """
704
+ import numpy as np
705
+ n = int(blk.get("count", 0) or 0)
706
+ if "sum" in blk:
707
+ s = np.asarray(blk.get("sum", []), dtype=np.float64)
708
+ sq = np.asarray(blk.get("sum_sq", []), dtype=np.float64)
709
+ dim = len(s)
710
+ elif "mean" in blk:
711
+ m = np.asarray(blk.get("mean", []), dtype=np.float64)
712
+ sd = np.asarray(blk.get("std", []), dtype=np.float64)
713
+ dim = len(m)
714
+ s = m * n
715
+ sq = (sd * sd + m * m) * n
716
+ else:
717
+ return {"count": 0, "sum": [], "sum_sq": [], "min": [], "max": []}
718
+ mn = blk.get("min")
719
+ mx = blk.get("max")
720
+ if mn is None or len(mn) != dim:
721
+ mn = [float("inf")] * dim
722
+ if mx is None or len(mx) != dim:
723
+ mx = [float("-inf")] * dim
724
+ return {"count": n, "sum": s.tolist(), "sum_sq": sq.tolist(),
725
+ "min": [float(v) for v in mn], "max": [float(v) for v in mx]}
726
+
727
+
728
+ def merge_stat_block(acc: dict, key: str, blk: dict, dim: int) -> None:
729
+ """把新统计块合并进累加器(sum-form;count<=0 跳过)。
730
+
731
+ blk 可为 sum-form(writer 的 np 数组块/迁移块)或 json-form;
732
+ acc 中旧块按 sum-form 读取,无则从零起。
733
+ """
734
+ if not blk or int(blk.get("count", 0) or 0) <= 0:
735
+ return
736
+ import numpy as np
737
+ new = stat_block_to_sum(blk)
738
+ n1 = int(new["count"])
739
+ old = acc.get(key)
740
+ if isinstance(old, dict) and int(old.get("count", 0) or 0) > 0:
741
+ old = stat_block_to_sum(old)
742
+ n0 = int(old["count"])
743
+ s = (np.asarray(old["sum"], dtype=np.float64)
744
+ + np.asarray(new["sum"], dtype=np.float64))
745
+ sq = (np.asarray(old["sum_sq"], dtype=np.float64)
746
+ + np.asarray(new["sum_sq"], dtype=np.float64))
747
+ mn = np.minimum(np.asarray(old["min"], dtype=np.float64),
748
+ np.asarray(new["min"], dtype=np.float64))
749
+ mx = np.maximum(np.asarray(old["max"], dtype=np.float64),
750
+ np.asarray(new["max"], dtype=np.float64))
751
+ else:
752
+ n0 = 0
753
+ s = np.asarray(new["sum"], dtype=np.float64)
754
+ sq = np.asarray(new["sum_sq"], dtype=np.float64)
755
+ mn = np.asarray(new["min"], dtype=np.float64)
756
+ mx = np.asarray(new["max"], dtype=np.float64)
757
+ acc[key] = {"count": n0 + n1, "sum": s.tolist(), "sum_sq": sq.tolist(),
758
+ "min": mn.tolist(), "max": mx.tolist()}
759
+
760
+
761
+ def acc_to_stats_json(acc: dict) -> dict:
762
+ """sum-form 累加器 → stats.json 内容:每块 {count, mean, std, min, max}。
763
+
764
+ count=0 的块写占位(mean 0/std 1,与旧格式空块语义一致);
765
+ action 恒为占位块(count 0,对外只表示列形状,不参与统计)。
766
+ """
767
+ import numpy as np
768
+ stats: dict = {}
769
+ for key, blk in acc.items():
770
+ if not isinstance(blk, dict):
771
+ continue
772
+ sblk = stat_block_to_sum(blk)
773
+ n = int(sblk["count"])
774
+ dim = len(sblk["sum"])
775
+ if n <= 0 or dim == 0:
776
+ stats[key] = {"count": 0, "mean": [0.0] * dim, "std": [1.0] * dim,
777
+ "min": [0.0] * dim, "max": [0.0] * dim}
778
+ continue
779
+ s = np.asarray(sblk["sum"], dtype=np.float64)
780
+ sq = np.asarray(sblk["sum_sq"], dtype=np.float64)
781
+ mean = s / n
782
+ var = np.maximum(sq / n - mean * mean, 0.0)
783
+ stats[key] = {"count": n, "mean": mean.tolist(),
784
+ "std": np.sqrt(var).tolist(),
785
+ "min": sblk["min"], "max": sblk["max"]}
786
+ stats["action"] = {"count": 0, "mean": [0.0], "std": [1.0],
787
+ "min": [0.0], "max": [0.0]}
788
+ return stats
789
+
790
+
791
+ def load_stats_acc(task_dir: str) -> tuple:
792
+ """读任务 stats.json 归一为 sum-form 累加器。
793
+
794
+ 返回 (acc, need_recalc):文件缺失/损坏 → ({}, False);任一块缺 count
795
+ (旧格式,无法增量合并)→ ({}, True),调用方应 recalc_stats 重建。
796
+ """
797
+ path = pooled_stats_path(task_dir)
798
+ if not os.path.isfile(path):
799
+ return {}, False
800
+ try:
801
+ with open(path, "r", encoding="utf-8") as f:
802
+ raw = json.load(f)
803
+ except (OSError, json.JSONDecodeError):
804
+ return {}, False
805
+ if not isinstance(raw, dict):
806
+ return {}, False
807
+ acc = {}
808
+ for key, blk in raw.items():
809
+ if not isinstance(blk, dict):
810
+ continue
811
+ if "count" not in blk:
812
+ return {}, True # 旧格式(无 count)→ 全量重算一次
813
+ acc[key] = stat_block_to_sum(blk)
814
+ return acc, False
815
+
816
+
817
+ def recalc_stats(task_dir: str) -> dict:
818
+ """全量重扫任务 data parquet,重建 stats.json(自含 count),返回 sum-form acc。
819
+
820
+ 只统计实际存在的观测列:observation.*(帧级,逐行计数,与写入器同口径)
821
+ 与 observation.imu(样本级 6 轴);observation.*hand_pose 为恒写占位
822
+ 零列(后处理回填),跳过不计(与写入器增量口径一致,且避免 std=0
823
+ 块毒化归一化)。旧格式 stats.json 迁移、删边车后的重建兜底都用它。
824
+ 无数据列时产出仅含 action 占位块。
825
+ """
826
+ import numpy as np
827
+ import pyarrow.parquet as pq
828
+ acc: dict = {}
829
+ for ep in list_task_episodes(task_dir):
830
+ p = pooled_data_parquet_path(task_dir, ep)
831
+ if not os.path.isfile(p):
832
+ continue
833
+ try:
834
+ tbl = pq.read_table(p)
835
+ except Exception:
836
+ continue
837
+ for col in tbl.column_names:
838
+ if not col.startswith("observation."):
839
+ continue
840
+ if col.endswith("hand_pose"):
841
+ continue # 恒写占位零列,后处理回填,不计统计
842
+ try:
843
+ if col == "observation.imu":
844
+ samples = [np.asarray(v, dtype=np.float64)
845
+ for v in tbl.column(col).to_pylist() if v]
846
+ if not samples:
847
+ continue
848
+ arr = np.concatenate([a.reshape(-1, 6) for a in samples])
849
+ else:
850
+ vals = [np.asarray(v, dtype=np.float64)
851
+ for v in tbl.column(col).to_pylist()
852
+ if v is not None]
853
+ if not vals:
854
+ continue
855
+ arr = np.stack(vals)
856
+ except (TypeError, ValueError):
857
+ continue
858
+ merge_stat_block(acc, col, {
859
+ "count": int(arr.shape[0]),
860
+ "sum": arr.sum(axis=0).tolist(),
861
+ "sum_sq": (arr * arr).sum(axis=0).tolist(),
862
+ "min": arr.min(axis=0).tolist(),
863
+ "max": arr.max(axis=0).tolist(),
864
+ }, arr.shape[1])
865
+ # 原子写(indent=2:stats.json 是给人读的契约文件)
866
+ path = pooled_stats_path(task_dir)
867
+ os.makedirs(os.path.dirname(path), exist_ok=True)
868
+ tmp = path + ".tmp"
869
+ with open(tmp, "w", encoding="utf-8") as f:
870
+ json.dump(acc_to_stats_json(acc), f, indent=2, ensure_ascii=False)
871
+ os.replace(tmp, path)
872
+ return acc
873
+
874
+
875
+ def _rmdir_empty(path: str) -> bool:
876
+ """删除空目录(自底向上连带空父目录);非空/不存在返回 False。"""
877
+ if not os.path.isdir(path):
878
+ return False
879
+ try:
880
+ os.rmdir(path)
881
+ return True
882
+ except OSError:
883
+ return False
884
+
885
+
886
+ def delete_pooled_episode(task_dir: str, episode_index: int) -> bool:
887
+ """彻底删除 episode N 的文件组(用户裁决:GUI 删除不走回收区)。
888
+
889
+ - videos/<key>/episode-{f}.{ext} 各流文件
890
+ - data/chunk-{c}/episode-{f}.parquet
891
+ - episodes 每段文件直接删;旧分片回退=flock 读-改-写删行(与 writer
892
+ 同口径)
893
+ 统计不回退(stats/total_episodes 保持,与进度语义一致)。
894
+
895
+ 幂等:文件组已不存在也返回 True(视为已删)。
896
+ """
897
+ files = episode_video_files(task_dir, episode_index)
898
+ for key, src in files.items():
899
+ try:
900
+ os.remove(src)
901
+ except OSError:
902
+ pass
903
+ _rmdir_empty(os.path.dirname(src))
904
+ dp = pooled_data_parquet_path(task_dir, episode_index)
905
+ if os.path.isfile(dp):
906
+ try:
907
+ os.remove(dp)
908
+ except OSError:
909
+ pass
910
+ _rmdir_empty(os.path.dirname(dp))
911
+
912
+ # episodes 元数据:新布局=每段一个文件 → 直接删;旧分片回退=删行
913
+ from core.egodata_writer import (_read_episode_rows, _episode_rows_table,
914
+ _atomic_write_parquet)
915
+ cidx, fidx = episode_chunk_file(episode_index)
916
+ ep_file = pooled_episodes_path(task_dir, episode_index)
917
+ handled = False
918
+ if os.path.isfile(ep_file):
919
+ try:
920
+ rows = _read_episode_rows(ep_file)
921
+ except Exception:
922
+ rows = []
923
+ if len(rows) == 1 and rows[0].get("episode_index") == episode_index:
924
+ # 单行且正是 N → 直接删(含旧分片只剩该行的退化情形)
925
+ try:
926
+ os.remove(ep_file)
927
+ handled = True
928
+ except OSError:
929
+ pass
930
+ _rmdir_empty(os.path.dirname(ep_file))
931
+ if not handled:
932
+ # 旧分片(每 chunk 一个多行文件)→ flock 读-改-写删行
933
+ legacy = _legacy_episodes_shard_path(task_dir, cidx)
934
+ if os.path.isfile(legacy):
935
+ try:
936
+ import fcntl as _fcntl
937
+ except ImportError:
938
+ _fcntl = None
939
+ lock_dir = os.path.join(task_dir, "meta", "episodes")
940
+ os.makedirs(lock_dir, exist_ok=True)
941
+ fh = open(os.path.join(lock_dir, ".lock"), "a+")
942
+ try:
943
+ if _fcntl is not None:
944
+ _fcntl.flock(fh.fileno(), _fcntl.LOCK_EX)
945
+ rows = [r for r in _read_episode_rows(legacy)
946
+ if r.get("episode_index") != episode_index]
947
+ _atomic_write_parquet(_episode_rows_table(rows), legacy)
948
+ finally:
949
+ if _fcntl is not None:
950
+ _fcntl.flock(fh.fileno(), _fcntl.LOCK_UN)
951
+ fh.close()
952
+ return True
953
+
954
+
955
+ # ═══════════════════════════════════════════════════════════
956
+ # 关键点数据路径 —— 独立输出到 keypoints_output/,与 data/ 镜像结构
957
+ # ═══════════════════════════════════════════════════════════
958
+
959
+ from config import settings as _settings
960
+
961
+
962
+ def _keypoints_session_dir(session_dir: str) -> str:
963
+ """将录制 session 路径映射到 keypoints_output 下的镜像目录。
964
+
965
+ data/recordings/Test005/Test005_000024
966
+ → keypoints_output/Test005/Test005_000024
967
+ """
968
+ recordings_root = os.path.normpath(_settings.RECORDING_DIR)
969
+ sd = os.path.normpath(session_dir)
970
+ if sd.startswith(recordings_root + os.sep):
971
+ rel = os.path.relpath(sd, recordings_root)
972
+ else:
973
+ parts = sd.replace(os.sep, "/").rstrip("/").split("/")
974
+ rel = os.path.join(parts[-2], parts[-1]) if len(parts) >= 2 else parts[-1]
975
+ return os.path.join(_settings.KEYPOINTS_OUTPUT_DIR, rel)
976
+
977
+
978
+ def episode_keypoints_dir(task_dir: str, episode_index: int) -> str:
979
+ """池化 episode 的 keypoints 镜像目录。
980
+
981
+ data/recordings/<task> + N → keypoints_output/<task>/episode_{N:06d}
982
+ """
983
+ task = os.path.basename(os.path.normpath(task_dir))
984
+ return os.path.join(_settings.KEYPOINTS_OUTPUT_DIR, task,
985
+ f"episode_{episode_index:06d}")
986
+
987
+
988
+ def episode_keypoints_video_dir(task_dir: str, episode_index: int) -> str:
989
+ """keypoints_output/<task>/episode_{N:06d}/videos/"""
990
+ return os.path.join(episode_keypoints_dir(task_dir, episode_index), "videos")
991
+
992
+
993
+ def episode_hand_kpts_parquet_path(task_dir: str, episode_index: int) -> str:
994
+ """keypoints_output/<task>/episode_{N:06d}/hand_pose/chunk-000.parquet"""
995
+ return os.path.join(episode_keypoints_dir(task_dir, episode_index),
996
+ "hand_pose", f"{chunk_dir(0)}.parquet")
997
+
998
+
999
+ def episode_auto_labels_parquet_path(task_dir: str, episode_index: int) -> str:
1000
+ """keypoints_output/<task>/episode_{N:06d}/auto_labels/auto_labels.parquet"""
1001
+ return os.path.join(episode_keypoints_dir(task_dir, episode_index),
1002
+ "auto_labels", "auto_labels.parquet")
1003
+
1004
+
1005
+ def episode_hand_3d_parquet_path(task_dir: str, episode_index: int) -> str:
1006
+ """keypoints_output/<task>/episode_{N:06d}/hand_pose_3d/chunk-000.parquet"""
1007
+ return os.path.join(episode_keypoints_dir(task_dir, episode_index),
1008
+ "hand_pose_3d", f"{chunk_dir(0)}.parquet")
1009
+
1010
+
1011
+ def keypoints_video_dir(session_dir: str) -> str:
1012
+ """keypoints_output/<project>/<session>/videos/"""
1013
+ return os.path.join(_keypoints_session_dir(session_dir), "videos")
1014
+
1015
+
1016
+ def hand_kpts_parquet_path(session_dir: str) -> str:
1017
+ """keypoints_output/<project>/<session>/hand_pose/chunk-000.parquet"""
1018
+ return os.path.join(_keypoints_session_dir(session_dir), "hand_pose",
1019
+ f"{chunk_dir(0)}.parquet")
1020
+
1021
+
1022
+ def auto_labels_parquet_path(session_dir: str) -> str:
1023
+ """keypoints_output/<project>/<session>/auto_labels/auto_labels.parquet"""
1024
+ return os.path.join(_keypoints_session_dir(session_dir), "auto_labels",
1025
+ "auto_labels.parquet")
1026
+
1027
+
1028
+ def hand_3d_parquet_path(session_dir: str) -> str:
1029
+ """keypoints_output/<project>/<session>/hand_pose_3d/chunk-000.parquet"""
1030
+ return os.path.join(_keypoints_session_dir(session_dir), "hand_pose_3d",
1031
+ f"{chunk_dir(0)}.parquet")
1032
+
1033
+
1034
+ # ── 回退路径 1: session 目录内的 keypoints/ ──────────────
1035
+
1036
+ def _session_kpts_hand_kpts_path(session_dir: str) -> str:
1037
+ return os.path.join(session_dir, "keypoints", "hand_pose",
1038
+ f"{chunk_dir(0)}.parquet")
1039
+
1040
+ def _session_kpts_hand_3d_path(session_dir: str) -> str:
1041
+ return os.path.join(session_dir, "keypoints", "hand_pose_3d",
1042
+ f"{chunk_dir(0)}.parquet")
1043
+
1044
+ def _session_kpts_auto_labels_path(session_dir: str) -> str:
1045
+ return os.path.join(session_dir, "keypoints", "auto_labels",
1046
+ "auto_labels.parquet")
1047
+
1048
+ # ── 回退路径 2: session 目录内的 annotations/(最旧版兼容)──
1049
+
1050
+ def _legacy_hand_kpts_path(session_dir: str) -> str:
1051
+ return os.path.join(session_dir, "annotations", "hand_pose",
1052
+ f"{chunk_dir(0)}.parquet")
1053
+
1054
+ def _legacy_hand_3d_path(session_dir: str) -> str:
1055
+ return os.path.join(session_dir, "annotations", "hand_pose_3d",
1056
+ f"{chunk_dir(0)}.parquet")
1057
+
1058
+ def _legacy_auto_labels_path(session_dir: str) -> str:
1059
+ return os.path.join(session_dir, "annotations", "mmpose", "auto_labels.parquet")
1060
+
1061
+
1062
+ def list_all_sessions(base_dir: str) -> list:
1063
+ """扫描录制根目录,返回全部池化 episode(v1.1.0 读侧唯一枚举入口)。
1064
+
1065
+ base_dir 是录制根目录(如 RECORDING_DIR)。
1066
+ 旧格式会话目录不再列出——迁移后只认任务级池化布局。
1067
+
1068
+ 返回: [(task_dir, episode_index, display_name), ...] 按名称倒序,
1069
+ display_name 后缀 = episode 文件号(0 基,与本地 episode-NNN 对齐)。
1070
+ """
1071
+ return [(r["task_dir"], r["episode_index"], r["name"])
1072
+ for r in episode_refs(base_dir)]
1073
+
1074
+
1075
+ # ═══════════════════════════════════════════════════════════
1076
+ # EgoData 格式路径工具
1077
+ # ═══════════════════════════════════════════════════════════
1078
+
1079
+ def episode_dirname(episode_index: int = 1, task_name: str = "",
1080
+ digits: int = 6) -> str:
1081
+ """EgoData episode 目录名。
1082
+
1083
+ 有 task_name 时: Chew_gum_000001
1084
+ 无 task_name 时: episode_000001(兼容旧命名)
1085
+ """
1086
+ from config import settings
1087
+ d = digits if digits else settings.EPISODE_DIGITS
1088
+ if task_name:
1089
+ prefix = _sanitize_tag(task_name)
1090
+ else:
1091
+ prefix = settings.EPISODE_PREFIX
1092
+ return f"{prefix}_{episode_index:0{d}d}"
1093
+
1094
+
1095
+ def next_episode_index(base_dir: str, task_name: str = "",
1096
+ batch_index: int = 0) -> int:
1097
+ """返回下一个 episode 索引。
1098
+
1099
+ 优先取任务进度序号 batch_index(录制完成次数 + 1,与本地文件是否
1100
+ 删除无关——上传后自动删除不会让序号回退);同时至少大于目录扫描出
1101
+ 的最大索引,防止与现存目录重名(删除失败/人工留存等场景)。
1102
+ batch_index <= 0 时退化为纯目录扫描(旧行为)。
1103
+
1104
+ 兼容新命名 (<tag>_NNNNNN) 和旧命名 (episode_NNNNNN)。
1105
+ """
1106
+ import re
1107
+ from config import settings
1108
+
1109
+ tag = task_tag(task_name) if task_name else ""
1110
+
1111
+ # 构建匹配模式:新格式 (task_tag_NNN) + 旧格式 (episode_NNN) 兼容
1112
+ patterns = []
1113
+ if tag:
1114
+ patterns.append(re.compile(rf"^{re.escape(tag)}_(\d+)$"))
1115
+ patterns.append(re.compile(rf"^{settings.EPISODE_PREFIX}_(\d+)$"))
1116
+
1117
+ max_idx = 0
1118
+ search_dirs = []
1119
+ if tag:
1120
+ tagged = os.path.join(base_dir, tag)
1121
+ if os.path.isdir(tagged):
1122
+ search_dirs.append(tagged)
1123
+ search_dirs.append(base_dir)
1124
+
1125
+ for sd in search_dirs:
1126
+ if not os.path.isdir(sd):
1127
+ continue
1128
+ for entry in os.listdir(sd):
1129
+ for pat in patterns:
1130
+ m = pat.match(entry)
1131
+ if m and os.path.isdir(os.path.join(sd, entry)):
1132
+ idx = int(m.group(1))
1133
+ if idx > max_idx:
1134
+ max_idx = idx
1135
+ scan_next = max_idx + 1
1136
+ if batch_index > 0:
1137
+ return max(batch_index, scan_next)
1138
+ return scan_next
1139
+
1140
+
1141
+ # ── EgoData 目录内路径 ──────────────────────────────
1142
+
1143
+ def egodata_video_path(episode_dir: str, camera_name: str) -> str:
1144
+ """videos/<base_cam>/chunk-0000/<camera_name>.mp4
1145
+
1146
+ _aux 后缀的摄像头归入其主摄像头文件夹:
1147
+ stereo_left_aux → videos/stereo_left/chunk-0000/stereo_left_aux.mp4
1148
+ """
1149
+ base_cam = camera_name[:-4] if camera_name.endswith("_aux") else camera_name
1150
+ return os.path.join(episode_dir, "videos", base_cam, "chunk-0000",
1151
+ f"{camera_name}.mp4")
1152
+
1153
+
1154
+ def egodata_video_dir(episode_dir: str, camera_name: str) -> str:
1155
+ """videos/<base_cam>/chunk-0000/ 目录路径。"""
1156
+ base_cam = camera_name[:-4] if camera_name.endswith("_aux") else camera_name
1157
+ return os.path.join(episode_dir, "videos", base_cam, "chunk-0000")
1158
+
1159
+
1160
+ def egodata_depth_path(episode_dir: str, depth_name: str, frame_index: int) -> str:
1161
+ """episode_dir/depth/head_depth/000001.png(PNG 16-bit grayscale,uint16 毫米)
1162
+
1163
+ 旧版 png16 格式(v1.0.13 及以前),保留给历史会话读取回退。
1164
+ """
1165
+ return os.path.join(episode_dir, "depth", depth_name,
1166
+ f"{frame_index:06d}.png")
1167
+
1168
+
1169
+ def egodata_depth_video_path(episode_dir: str, depth_name: str) -> str:
1170
+ """episode_dir/depth/<slot>/<slot>.mp4(12-bit 灰度 HEVC,v1.1.2 起)
1171
+
1172
+ 单视频轨 hevc (Rext) gray12le,12-bit 对数深度码(见
1173
+ core/depth_codec.py)。旧格式回退 <slot>.mkv(v1.0.14 双流
1174
+ FFV1);哪个存在返回哪个,都无则按 mp4 返回。
1175
+ """
1176
+ base = os.path.join(episode_dir, "depth", depth_name, depth_name)
1177
+ for ext in (".mp4", ".mkv"):
1178
+ path = base + ext
1179
+ if os.path.isfile(path):
1180
+ return path
1181
+ return base + ".mp4"
1182
+
1183
+
1184
+ def egodata_image_path(episode_dir: str, camera_name: str, frame_index: int) -> str:
1185
+ """episode_dir/images/head_left_rgb/000001.jpg"""
1186
+ return os.path.join(episode_dir, "images", camera_name,
1187
+ f"{frame_index:06d}.jpg")
1188
+
1189
+
1190
+ def egodata_calibration_path(episode_dir: str,
1191
+ calib_name: str = "head_stereo") -> str:
1192
+ """episode_dir/calibration/head_stereo.json"""
1193
+ return os.path.join(episode_dir, "calibration", f"{calib_name}.json")
1194
+
1195
+
1196
+ def egodata_metadata_path(episode_dir: str) -> str:
1197
+ """episode_dir/metadata.json"""
1198
+ return os.path.join(episode_dir, "metadata.json")
1199
+
1200
+
1201
+ def egodata_timestamps_path(episode_dir: str) -> str:
1202
+ """episode_dir/timestamps.json"""
1203
+ return os.path.join(episode_dir, "timestamps.json")
1204
+
1205
+
1206
+ def egodata_sensor_data_dir(episode_dir: str, sensor_name: str) -> str:
1207
+ """data/<sensor_name>/chunk-0000/ 目录路径。"""
1208
+ return os.path.join(episode_dir, "data", sensor_name, "chunk-0000")
1209
+
1210
+
1211
+ def egodata_sensor_parquet_path(episode_dir: str, sensor_name: str) -> str:
1212
+ """data/<sensor_name>/chunk-0000/chunk_000000.parquet"""
1213
+ return os.path.join(episode_dir, "data", sensor_name, "chunk-0000",
1214
+ "chunk_000000.parquet")
1215
+
1216
+
1217
+ # ── 格式检测 ────────────────────────────────────────
1218
+
1219
+ def detect_session_format(session_dir: str) -> str:
1220
+ """检测会话目录格式。
1221
+
1222
+ Returns:
1223
+ "pooled" — v1.1.0 任务池化布局(meta/info.json format 字段)
1224
+ "egodata" — 有 metadata.json
1225
+ "lerobot_v3" — 有 meta/info.json(旧 LeRobot v3 会话目录)
1226
+ "unknown" — 无法识别
1227
+ """
1228
+ info_path = os.path.join(session_dir, "meta", "info.json")
1229
+ if os.path.isfile(info_path):
1230
+ try:
1231
+ with open(info_path, "r", encoding="utf-8") as f:
1232
+ info = json.load(f)
1233
+ if isinstance(info, dict) and \
1234
+ info.get("format") == "pooled_episodes_v1":
1235
+ return "pooled"
1236
+ except (OSError, json.JSONDecodeError):
1237
+ pass
1238
+ return "lerobot_v3"
1239
+ if os.path.isfile(os.path.join(session_dir, "metadata.json")):
1240
+ return "egodata"
1241
+ return "unknown"
1242
+
1243
+
1244
+ # ── 扩展 list_all_sessions 兼容 EgoData ──────────────
1245
+
1246
+ def list_all_egodata_sessions(base_dir: str) -> list:
1247
+ """扫描全部 EgoData episode 目录。
1248
+
1249
+ 目录结构: base_dir/<task_tag>/episode_000001/
1250
+
1251
+ Returns: [(ep_path, ep_name, tag_name), ...] 按名称倒序
1252
+ """
1253
+ import re
1254
+ from config import settings
1255
+
1256
+ sessions = []
1257
+ if not os.path.isdir(base_dir):
1258
+ return sessions
1259
+
1260
+ d = settings.EPISODE_DIGITS
1261
+ # 匹配: 任意前缀_NNNNNN (新任务命名) 或 episode_NNNNNN (旧兼容)
1262
+ pattern = re.compile(rf"^(\w+)_(\d{{{d}}})$")
1263
+
1264
+ for entry in sorted(os.listdir(base_dir)):
1265
+ entry_path = os.path.join(base_dir, entry)
1266
+ if not os.path.isdir(entry_path):
1267
+ continue
1268
+
1269
+ # EgoData: 本身是 episode 目录(有 metadata.json)
1270
+ if pattern.match(entry) and os.path.isfile(
1271
+ os.path.join(entry_path, "metadata.json")):
1272
+ sessions.append((entry_path, entry, entry.split("_")[0]))
1273
+ else:
1274
+ # 任务子文件夹
1275
+ for sub in sorted(os.listdir(entry_path)):
1276
+ sub_path = os.path.join(entry_path, sub)
1277
+ if pattern.match(sub) and os.path.isfile(
1278
+ os.path.join(sub_path, "metadata.json")):
1279
+ sessions.append((sub_path, sub, entry))
1280
+
1281
+ return sorted(sessions, key=lambda s: s[1], reverse=True)
collector/core/pipeline.py ADDED
@@ -0,0 +1,866 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 摄像机管线 —— 管理多路摄像机和共享录制会话。
3
+ 所有摄像机录制到同一个 episode(v1.1.0 任务池化布局:每路一个
4
+ videos/chunk-NNN/<key>/file-NNN 视频 + 一个 data parquet)。
5
+ """
6
+
7
+ from __future__ import annotations
8
+ import time
9
+ import threading
10
+ import queue
11
+ from typing import Callable, Dict, List, Optional
12
+
13
+ import numpy as np
14
+ from PyQt5.QtCore import QObject, pyqtSignal, QTimer
15
+
16
+ from config import settings
17
+ from core.camera import CameraWorker, CameraState
18
+ from core.egodata_writer import EgoDataWriter
19
+
20
+
21
+ class DropStats:
22
+ """录制丢帧统计(v1.0.9:编码背压可见化)。
23
+
24
+ 线程安全计数器 + 快照。计数路径:采集线程/主线程的 put_nowait 满
25
+ 分支(写入线程只读快照)。
26
+ """
27
+
28
+ def __init__(self):
29
+ self._lock = threading.Lock()
30
+ self._counts: Dict[str, int] = {}
31
+
32
+ def inc(self, key: str, n: int = 1):
33
+ with self._lock:
34
+ self._counts[key] = self._counts.get(key, 0) + n
35
+
36
+ def clear(self):
37
+ with self._lock:
38
+ self._counts.clear()
39
+
40
+ def snapshot(self) -> Dict[str, int]:
41
+ with self._lock:
42
+ return dict(self._counts)
43
+
44
+
45
+ class CameraSlot(QObject):
46
+ """
47
+ 一个摄像机槽位 —— 相机采集 + 帧缓冲。
48
+
49
+ 信号
50
+ ----
51
+ frame_ready(str, ndarray) — slot_id, BGR帧(用于 UI 显示)
52
+ state_changed(str, str) — slot_id, 新状态
53
+ error_occurred(str, str) — slot_id, 错误消息
54
+ """
55
+
56
+ frame_ready = pyqtSignal(str, object)
57
+ state_changed = pyqtSignal(str, str)
58
+ error_occurred = pyqtSignal(str, str)
59
+
60
+ def __init__(self, slot_id: str, camera_index: int,
61
+ on_queue_full: Optional[Callable[[], None]] = None):
62
+ super().__init__()
63
+ self.slot_id = slot_id
64
+ self.camera_index = camera_index
65
+ self.camera_name = f"Camera {camera_index}"
66
+
67
+ # B+ 方案: 帧队列,采集线程直接写入,录制线程消费
68
+ # maxsize=3 保证队列里始终是最新的帧,避免积压
69
+ self.frame_queue = queue.Queue(maxsize=3)
70
+ self.camera = CameraWorker(camera_index, record_queue=self.frame_queue,
71
+ on_queue_full=on_queue_full)
72
+
73
+ self.camera.frame_ready.connect(self._on_frame)
74
+ self.camera.state_changed.connect(self._on_camera_state)
75
+ self.camera.error_occurred.connect(
76
+ lambda msg: self.error_occurred.emit(self.slot_id, msg)
77
+ )
78
+
79
+ def start_camera(self):
80
+ self.camera.start()
81
+
82
+ def stop_camera(self):
83
+ self.camera.stop()
84
+
85
+ @property
86
+ def camera_state(self) -> str:
87
+ return self.camera.state
88
+
89
+ def _on_frame(self, frame):
90
+ # 转发给 UI 显示(录制帧由 frame_queue 独立传递,不受此信号影响)
91
+ self.frame_ready.emit(self.slot_id, frame)
92
+
93
+ def _on_camera_state(self, state: str):
94
+ self.state_changed.emit(self.slot_id, state)
95
+
96
+
97
+ class CameraPipeline(QObject):
98
+ """
99
+ 管理一组 CameraSlot + 共享 LeRobot 录制会话。
100
+
101
+ 所有摄像机录制到同一个会话目录,保证 MP4 和 Parquet 完整。
102
+ """
103
+
104
+ slot_added = pyqtSignal(str)
105
+ slot_removed = pyqtSignal(str)
106
+ session_changed = pyqtSignal(object) # VideoRecorder | None
107
+ recording_started = pyqtSignal(str) # slot_id
108
+ recording_finished = pyqtSignal(str, str) # slot_id, task_dir(v1.1.0 池化)
109
+ recording_aborted = pyqtSignal(str) # slot_id
110
+ duration_changed = pyqtSignal(str, float)
111
+ error_occurred = pyqtSignal(str, str)
112
+ state_changed = pyqtSignal(str, str)
113
+ recording_log = pyqtSignal(str) # v1.0.9 编码器探测等录制期日志
114
+
115
+ def __init__(self, output_dir: str = None):
116
+ super().__init__()
117
+ self._output_dir = output_dir or settings.RECORDING_DIR
118
+ self._slots: Dict[str, CameraSlot] = {}
119
+
120
+ # 共享录制状态
121
+ self._writer: Optional[EgoDataWriter] = None
122
+ self._recording = False
123
+ self._frame_count = 0 # 全局帧序号(跨摄像机递增)
124
+ self._per_cam_frame: Dict[str, int] = {} # 每路摄像机的独立帧序号
125
+ self._last_recording_frames: Dict[str, int] = {} # 上一轮录制的每相机帧数快照
126
+ self._start_time: float = 0.0
127
+ self._episode_start_s: float = 0.0 # episode 起始 Unix 秒(用于相对时间戳)
128
+ self._timer_running = False
129
+ self._codec_name = ""
130
+ self._session_path: Optional[str] = None
131
+ self._last_episode_index: int = 0 # v1.1.0:上一轮录制的全局 episode 序号
132
+ self._recording_slot: Optional[str] = None
133
+
134
+ # v1.0.9 丢帧统计与 IMU 防丢
135
+ self._drop_stats = DropStats()
136
+ self._last_drop_stats: Dict[str, int] = {}
137
+ self._pending_imu: Dict[str, list] = {} # slot → 队列满暂存的 IMU 样本
138
+ self._pending_imu_lock = threading.Lock()
139
+ self._imu_overflow_count = 0 # 防丢缓冲超限次数(每 episode 告警一次)
140
+
141
+ # 已注册的传感器名称列表(决定 parquet 中 observation.<name> 列)
142
+ self._sensor_names: List[str] = []
143
+ self._device_meta: List[dict] = [] # start_recording(device_meta=…) 透传
144
+
145
+ # B+ 方案: 传感器数据通过队列传递(主线程 → 写入线程),线程安全
146
+ # 队列元素: (sensor_name, data_array) 元组
147
+ self._sensor_queue: queue.Queue = queue.Queue(maxsize=10)
148
+
149
+ # USB 手套 IMU 四元数:sensor_name → (quats 64×f32, valid 16×f32)
150
+ # 最新值覆盖式快照,写入线程每帧取走后清空(与传感器队列同节奏)
151
+ self._glove_imu: Dict[str, tuple] = {}
152
+ self._glove_imu_lock = threading.Lock()
153
+ # USB 手套骨架关键点:sensor_name → 63×f32(21×3 米),同节奏快照
154
+ self._glove_kpts: Dict[str, np.ndarray] = {}
155
+ self._glove_kpts_lock = threading.Lock()
156
+
157
+ # 外部帧源(如双目子进程),不经过 CameraWorker
158
+ # 帧通过队列传入写入线程,以固定帧率均匀消费,避免 GIL 导致的写入抖动
159
+ # _external_queues[slot_id] = queue.Queue(maxsize=2)
160
+ # _external_dims[slot_id] = (height, width)
161
+ # _external_fps[slot_id] = fps
162
+ self._external_queues: Dict[str, queue.Queue] = {}
163
+ self._external_dims: Dict[str, tuple] = {}
164
+ self._external_fps: Dict[str, float] = {}
165
+
166
+ # B+ 方案: 独立写入线程,用 time.perf_counter() 精确控制 30fps
167
+ self._write_thread: Optional[threading.Thread] = None
168
+
169
+ # 录制时长更新定时器(主线程,仅用于 UI 显示)
170
+ self._duration_timer = QTimer(self)
171
+ self._duration_timer.timeout.connect(self._tick_duration)
172
+ self._duration_timer.setInterval(250)
173
+
174
+ # 深度帧队列(双目深度计算 → 写入线程);按深度槽位名分列
175
+ # (S80M 传统路径用 "stereo_left" 作为默认键)
176
+ self._depth_queues: Dict[str, queue.Queue] = {}
177
+ self._depth_frame_idx: Dict[str, int] = {}
178
+
179
+ # 深度伪相机元数据(D435 等原生深度相机,多路并存):
180
+ # _depth_cameras[depth槽名] = {"resolution": (h,w), "fps": f,
181
+ # "master_slot": 主槽位,
182
+ # "heatmap_near_mm"/"heatmap_far_mm"/"heatmap_smooth_k"/
183
+ # "heatmap_temporal_alpha": …}
184
+ # 名字含 "depth" → EgoDataWriter 自动标 type:depth 并跳过视频目录创建
185
+ self._depth_cameras: Dict[str, dict] = {}
186
+ # 最近深度帧(深度源低于录制帧率时补拍节拍:重复写双流使
187
+ # 深度 MKV 帧 i 与 RGB 帧 i 对齐;FFV1 对重复帧增量≈0)
188
+ self._last_depth_frames: Dict[str, np.ndarray] = {}
189
+ # 深度帧随哪路主槽位同步落盘(S80M 默认 stereo_left,D435 为 d435_rgb)
190
+ self._depth_master_slot: str = "stereo_left"
191
+ # 外部帧源标定(D435 运行时从 SDK 提取,录制开始时写入 calibration/)
192
+ self._external_calibration = None
193
+ self._external_calibrations: dict = {} # device_key → 标定(多路)
194
+
195
+ # 录制事件日志(连接/断开标记)
196
+ self._device_status: Dict[str, str] = {}
197
+
198
+ def record_event(self, device: str, event_type: str, message: str = ""):
199
+ """记录一个带时间戳的设备事件并更新连接状态。
200
+
201
+ CameraPipeline 写入线程会在每帧 parquet 数据中写入 status.<device> 列。
202
+
203
+ Args:
204
+ device: 设备标识(如 cam_001, sensors_right)
205
+ event_type: 事件类型(connected, disconnected)
206
+ message: 附加描述
207
+ """
208
+ status = "connected" if event_type == "connected" else "disconnected"
209
+ self._device_status[device] = status
210
+
211
+ # ── 槽位管理 ──────────────────────────────────────
212
+
213
+ def add_camera(self, slot_id: str, camera_index: int) -> CameraSlot:
214
+ if slot_id in self._slots:
215
+ raise ValueError(f"槽位 {slot_id} 已存在")
216
+ slot = CameraSlot(slot_id, camera_index,
217
+ on_queue_full=lambda: self._drop_stats.inc(
218
+ f"uvc:{slot_id}"))
219
+ self._slots[slot_id] = slot
220
+ slot.start_camera()
221
+ self.slot_added.emit(slot_id)
222
+ return slot
223
+
224
+ def remove_camera(self, slot_id: str):
225
+ slot = self._slots.pop(slot_id, None)
226
+ if slot:
227
+ if self._recording:
228
+ self.abort_recording(slot_id)
229
+ slot.stop_camera()
230
+ slot.blockSignals(True)
231
+ slot.deleteLater()
232
+ self.slot_removed.emit(slot_id)
233
+
234
+ def get_slot(self, slot_id: str) -> Optional[CameraSlot]:
235
+ return self._slots.get(slot_id)
236
+
237
+ def slot_ids(self):
238
+ return list(self._slots.keys())
239
+
240
+ def slot_count(self) -> int:
241
+ return len(self._slots)
242
+
243
+ def remove_all(self):
244
+ for sid in list(self._slots.keys()):
245
+ self.remove_camera(sid)
246
+
247
+ @property
248
+ def is_recording(self) -> bool:
249
+ return self._recording
250
+
251
+ # ── 外部帧源(双目子进程等) ──────────────────────
252
+
253
+ def register_external_source(self, slot_id: str, resolution: tuple,
254
+ fps: float = 25.0):
255
+ """注册一个外部帧源(不创建 CameraWorker)。
256
+
257
+ 外部帧源通过队列将帧传入写入线程,以固定帧率均匀消费。
258
+ 每路外部源可使用独立 fps,ffmpeg 用该值设置输出帧率。
259
+
260
+ Args:
261
+ slot_id: 槽位标识(如 "stereo_left")
262
+ resolution: (height, width)
263
+ fps: 该摄像机的原生帧率(用于 ffmpeg -r 参数)
264
+ """
265
+ # 深缓冲(≈1s):写线程/编码器子进程启动、系统卡顿期间吸收
266
+ # 抽帧后的积压帧,避免 put_nowait 丢帧;写线程按 30fps 均匀
267
+ # 消费,缓冲只延后写入不丢内容(峰值内存 ~90MB/槽,可控)
268
+ self._external_queues[slot_id] = queue.Queue(maxsize=30)
269
+ self._external_dims[slot_id] = resolution
270
+ self._external_fps[slot_id] = fps
271
+
272
+ def unregister_external_source(self, slot_id: str):
273
+ """移除外部帧源。"""
274
+ self._external_queues.pop(slot_id, None)
275
+ self._external_dims.pop(slot_id, None)
276
+ self._external_fps.pop(slot_id, None)
277
+
278
+ def write_external_frame(self, slot_id: str, frame: np.ndarray,
279
+ hardware_ns: int = 0,
280
+ imu_samples: Optional[List] = None):
281
+ """将外部帧推入录制队列(线程安全,非阻塞)。
282
+
283
+ 帧由写入线程以固定帧率均匀消费,保证输出视频无抖动。
284
+
285
+ Args:
286
+ hardware_ns: 帧的 SDK 硬件纳秒时间戳(双目相机,与 IMU 同源)
287
+ imu_samples: 本帧窗口内的 IMU 样本 [(ts_ns, gx,gy,gz, ax,ay,az), ...]
288
+ """
289
+ q = self._external_queues.get(slot_id)
290
+ if q is not None and self._recording:
291
+ try:
292
+ q.put_nowait((frame, hardware_ns, imu_samples))
293
+ except queue.Full:
294
+ # ★ v1.0.9 丢帧保 IMU:帧+hardware_ns 成对丢弃(视频帧与
295
+ # parquet 行同步少一行,序号对齐不受影响),但 stereo_left
296
+ # 的 IMU 批次转入防丢缓冲,随后续帧按时间戳挂靠落盘 ——
297
+ # 丢帧只损失视频帧,不损失 IMU 样本
298
+ self._drop_stats.inc(f"ext:{slot_id}")
299
+ if slot_id == "stereo_left" and imu_samples:
300
+ with self._pending_imu_lock:
301
+ buf = self._pending_imu.setdefault(slot_id, [])
302
+ buf.extend(imu_samples)
303
+ overflow = (len(buf)
304
+ - settings.IMU_PENDING_MAX_SAMPLES)
305
+ if overflow > 0:
306
+ del buf[:overflow]
307
+ self._imu_overflow_count += 1
308
+ if self._imu_overflow_count == 1:
309
+ self.recording_log.emit(
310
+ "[警告] IMU 防丢缓冲超限,最旧样本被丢弃")
311
+
312
+ def write_depth(self, depth_frame: np.ndarray,
313
+ depth_slot: str = "stereo_left"):
314
+ """将深度帧推入录制队列(线程安全,非阻塞,keep-latest)。
315
+
316
+ 深度图(S80M 视差 / D435 原生毫米深度,均为 uint16)通过此方法传入,
317
+ 写入线程在消费该路深度主槽位帧时同步写入。深度帧语义是
318
+ "每帧 RGB 配最新深度":入队前清掉未消费旧帧——旧 FIFO 丢新帧
319
+ 口径在实测 ~27fps 突发产出下丢 35%(突发期队列必然溢出),
320
+ 被覆盖的旧帧下游本来就不消费,最新帧覆盖无损失。
321
+
322
+ Args:
323
+ depth_frame: uint16 numpy array (H, W),视差图(值×16)或毫米深度图
324
+ depth_slot: 深度槽位名(默认 "stereo_left" 兼容 S80M 传统路径)
325
+ """
326
+ if not self._recording:
327
+ return
328
+ # 兼容旧单深度调用(如 d435_e2e_test 的 write_depth(frame)):
329
+ # 名义槽未注册而恰好只声明了一路深度相机时,自动落到该路
330
+ if depth_slot not in self._depth_cameras \
331
+ and len(self._depth_cameras) == 1:
332
+ depth_slot = next(iter(self._depth_cameras))
333
+ q = self._depth_queues.setdefault(depth_slot,
334
+ queue.Queue(maxsize=3))
335
+ # keep-latest:深度引擎实测 ~27fps 突发产出(burst 期可达 ~50fps),
336
+ # 与写线程 30fps 消费名义相等——FIFO+丢新帧必然溢出(实机 165/478
337
+ # = 35%);最新帧覆盖旧帧后突发期无丢帧、每拍都配到最新深度,
338
+ # 引擎停顿期的补拍(重复最近帧)路径不受影响
339
+ try:
340
+ while True:
341
+ q.get_nowait()
342
+ except queue.Empty:
343
+ pass
344
+ q.put_nowait(depth_frame)
345
+
346
+ # ── 深度伪相机(D435 等原生深度设备) ──────────────
347
+
348
+ def set_depth_camera(self, name: str, resolution: tuple,
349
+ fps: float, master_slot: str,
350
+ heatmap_near_mm: float = 0.0,
351
+ heatmap_far_mm: float = 0.0,
352
+ heatmap_smooth_k: int = 0,
353
+ heatmap_temporal_alpha: float = 0.0):
354
+ """声明一个元数据深度相机(不注册外部队列、不建视频目录;多路并存)。
355
+
356
+ 名字含 "depth" 的相机会被 EgoDataWriter 自动跳过视频创建,
357
+ 但进入 metadata.json(type:depth/unit:mm)并启用 write_depth
358
+ 12-bit 灰度 MP4 落盘(gray12le 对数码,编码器不支持时回落
359
+ FFV1 灰度 MKV 无损 uint16 毫米)。
360
+
361
+ Args:
362
+ name: 深度槽位名(如 "d435_depth")
363
+ resolution: (height, width)
364
+ fps: 深度帧率
365
+ master_slot: 深度帧随哪路主槽位同步落盘(如 "d435_rgb")
366
+ heatmap_near_mm: 热力图色标下限(毫米);0 表示帧内自适应
367
+ heatmap_far_mm: 热力图色标上限(毫米);0 表示帧内自适应
368
+ near/far 同给时是**真固定色标**(线性映射,帧间颜色
369
+ 完全一致),无效值/近端→JET(0) 深蓝、超远→红饱和
370
+ (demo 口径,不置黑);否则每帧重配(会闪)
371
+ heatmap_smooth_k: 热力图中值滤波核(奇数,0 关闭)
372
+ heatmap_temporal_alpha: 热力图时域 EMA 权重(0 关闭;
373
+ 仅影响可视化热力图流,无损深度流不受影响)
374
+ """
375
+ self._depth_cameras[name] = {
376
+ "resolution": resolution,
377
+ "fps": fps,
378
+ "master_slot": master_slot,
379
+ "heatmap_near_mm": heatmap_near_mm,
380
+ "heatmap_far_mm": heatmap_far_mm,
381
+ "heatmap_smooth_k": int(heatmap_smooth_k),
382
+ "heatmap_temporal_alpha": float(heatmap_temporal_alpha),
383
+ }
384
+ self._depth_master_slot = master_slot
385
+
386
+ def clear_depth_camera(self, name: str = None):
387
+ """清除深度伪相机(name=None 清全部),S80M 默认主槽位兜底。"""
388
+ if name is None:
389
+ self._depth_cameras.clear()
390
+ else:
391
+ self._depth_cameras.pop(name, None)
392
+ self._depth_master_slot = "stereo_left"
393
+
394
+ def set_external_calibration(self, calib):
395
+ """兼容旧调用:设置外部帧源标定(单设备场景 → head_stereo.json)。"""
396
+ self._external_calibration = calib
397
+ self._external_calibrations["_default"] = calib
398
+
399
+ def set_device_calibration(self, device_key: str, calib):
400
+ """按设备注册标定(多路场景)。录制开始时:
401
+ 首台双目型设备 → calibration/head_stereo.json(服务器/回放依赖),
402
+ 其余 → calibration/{slot前缀}_calibration.json。
403
+ """
404
+ self._external_calibrations[device_key] = calib
405
+
406
+ def clear_device_calibration(self, device_key: str):
407
+ """设备关闭时清除其标定,避免下个会话串台。"""
408
+ self._external_calibrations.pop(device_key, None)
409
+
410
+ # ── 设备注册 ──────────────────────────────────────
411
+
412
+ def register_sensors(self, sensor_names: List[str]):
413
+ """注册传感器设备名称。
414
+
415
+ 必须在 start_recording 之前调用。传感器名称决定 parquet 中
416
+ observation.<name> 列名,如 "right_glove", "left_glove"。
417
+ """
418
+ self._sensor_names = list(sensor_names)
419
+
420
+ def register_sensor(self, sensor_name: str):
421
+ """增量注册单个传感器(手套开关 ON 时实时调用,下次录制生效)。"""
422
+ if sensor_name not in self._sensor_names:
423
+ self._sensor_names.append(sensor_name)
424
+
425
+ def unregister_sensor(self, sensor_name: str):
426
+ """注销单个传感器(手套开关 OFF)。"""
427
+ if sensor_name in self._sensor_names:
428
+ self._sensor_names.remove(sensor_name)
429
+
430
+ # ── 录制控制 ──────────────────────────────────────
431
+
432
+ def start_recording(self, slot_id: str, task_name: str = "",
433
+ batch_index: int = 0,
434
+ device_meta: Optional[List[dict]] = None):
435
+ """全部摄像机开始录制到同一个 LeRobot 会话。
436
+
437
+ Args:
438
+ slot_id: 触发录制的摄像机槽位 ID
439
+ task_name: 任务标注(如 "grasp_cup"),会嵌入目录名
440
+ batch_index: 任务进度序号(录制完成次数 + 1),用于 episode
441
+ 目录序号命名(与本地文件删除无关)
442
+ device_meta: 录制设备信息列表(写入 metadata.json / info.json
443
+ devices 段): [{"key","kind","name","serial","slots"}]
444
+ """
445
+ if self._recording or (not self._slots and not self._external_queues):
446
+ return
447
+ # 先设标志防重复触发,再后台初始化
448
+ self._recording_slot = slot_id
449
+ self._recording = True
450
+ self._device_status.clear() # 清空上一轮的状态
451
+ self._start_time = time.monotonic()
452
+ self._timer_running = True
453
+ self._duration_timer.start()
454
+ self._task_name = task_name
455
+ self._batch_index = batch_index
456
+ self._device_meta = list(device_meta) if device_meta else []
457
+ threading.Thread(target=self._start_async, daemon=True).start()
458
+
459
+ def _start_async(self):
460
+ """后台线程:创建 EgoData episode 并启动独立写入线程。"""
461
+ # 收集所有摄像机尺寸(含外部帧源如双目)
462
+ cameras = {}
463
+ camera_fps = {} # per-camera FPS(外部源用原生帧率,其余用录制帧率)
464
+ default_fps = float(settings.RECORDING_FPS)
465
+ for sid, sl in self._slots.items():
466
+ w, h = sl.camera.resolution # resolution = (width, height)
467
+ cameras[sid] = (h, w) # EgoData: (height, width)
468
+ camera_fps[sid] = default_fps
469
+ for sid, (h, w) in self._external_dims.items():
470
+ cameras[sid] = (h, w)
471
+ camera_fps[sid] = self._external_fps.get(sid, default_fps)
472
+ # 深度伪相机(D435 等):只进 metadata,不建视频目录/MP4;多路并存
473
+ for dname, dconf in self._depth_cameras.items():
474
+ dh, dw = dconf["resolution"]
475
+ cameras[dname] = (dh, dw)
476
+ camera_fps[dname] = dconf["fps"]
477
+
478
+ w = EgoDataWriter()
479
+ # 探针日志在 start_episode 内 emit → connect 必须在调用之前
480
+ w.log_occurred.connect(self.recording_log)
481
+ all_device_ids = list(self._slots.keys()) + list(self._sensor_names)
482
+ # 设备标定(多路):set_device_calibration 注册的按 key 传入;
483
+ # 旧 set_external_calibration 单值路径存 "_default" 键
484
+ calibrations = dict(self._external_calibrations)
485
+ legacy_calib = self._external_calibration if calibrations else None
486
+ ok = w.start_episode(self._output_dir, cameras, default_fps,
487
+ sensors=self._sensor_names,
488
+ task_name=getattr(self, '_task_name', ''),
489
+ device_ids=all_device_ids,
490
+ calibration=legacy_calib,
491
+ camera_fps=camera_fps,
492
+ devices=getattr(self, '_device_meta', []),
493
+ calibrations=calibrations,
494
+ # 任务进度序号:按录制完成次数命名(与文件
495
+ # 删除无关);0 时 writer 退化为目录扫描
496
+ batch_index=getattr(self, '_batch_index', 0),
497
+ # 与旧语义一致:显式注册的深度设备(如 D435/D405)
498
+ # 打开即录深度;settings.DEPTH_ENABLED 只门控
499
+ # S80M 视差路径(无注册槽的兜底单槽)
500
+ depth_enabled=bool(self._depth_cameras),
501
+ depth_heatmaps={
502
+ dn: {"near_mm": dc["heatmap_near_mm"],
503
+ "far_mm": dc["heatmap_far_mm"],
504
+ "smooth_k": dc["heatmap_smooth_k"],
505
+ "temporal_alpha": dc["heatmap_temporal_alpha"]}
506
+ for dn, dc in self._depth_cameras.items()},
507
+ # 深度槽以显式注册为准(set_depth_camera),
508
+ # 不由槽名是否含 "depth" 猜测
509
+ depth_slots=list(self._depth_cameras.keys()))
510
+ if not ok:
511
+ self._recording = False
512
+ self._timer_running = False
513
+ self.error_occurred.emit("", "ffmpeg 启动失败")
514
+ return
515
+
516
+ self._writer = w
517
+ self._session_path = w.task_dir # v1.1.0:任务目录(池化布局根)
518
+ self._last_episode_index = w.episode_index
519
+ # v1.0.9 编码器名动态化(如 "HEVC (libx265)")
520
+ self._codec_name = f"{w.encoder_label} | EgoData + LeRobot v3 Parquet"
521
+ # 丢帧统计/IMU 防丢:每 episode 重置
522
+ self._drop_stats.clear()
523
+ self._pending_imu.clear()
524
+ with self._glove_imu_lock:
525
+ self._glove_imu.clear()
526
+ with self._glove_kpts_lock:
527
+ self._glove_kpts.clear()
528
+ self._imu_overflow_count = 0
529
+ self._frame_count = 0
530
+ self._per_cam_frame = {sid: 0 for sid in self._slots}
531
+ self._depth_frame_idx = {}
532
+ self._last_depth_frames = {}
533
+ self._episode_start_s = time.time()
534
+
535
+ # 清空录制前残留的旧帧和传感器数据
536
+ for sid, sl in self._slots.items():
537
+ while not sl.frame_queue.empty():
538
+ try: sl.frame_queue.get_nowait()
539
+ except queue.Empty: break
540
+ while not self._sensor_queue.empty():
541
+ try: self._sensor_queue.get_nowait()
542
+ except queue.Empty: break
543
+
544
+ # B+ 方案: 启动独立写入线程(精确 30fps,完全不受主线程 UI 影响)
545
+ self._write_thread = threading.Thread(
546
+ target=self._write_loop, daemon=True, name="frame-writer"
547
+ )
548
+ self._write_thread.start()
549
+
550
+ self.recording_started.emit(self._recording_slot)
551
+ self.session_changed.emit(self)
552
+
553
+ def _write_loop(self):
554
+ """独立写入线程 —— 用 time.perf_counter() 精确按 30fps 写入帧。"""
555
+ frame_interval = 1.0 / settings.RECORDING_FPS # ~0.0333s
556
+ next_write = time.perf_counter()
557
+
558
+ while self._recording and self._writer is not None:
559
+ now = time.perf_counter()
560
+
561
+ # 精确等待到下一帧时刻
562
+ sleep_for = next_write - now
563
+ if sleep_for > 0.001:
564
+ # sleep 到目标时间前 ~1ms,然后用 busy-wait 补齐精度
565
+ time.sleep(sleep_for - 0.001)
566
+ while time.perf_counter() < next_write:
567
+ pass
568
+ elif sleep_for < -frame_interval:
569
+ # 落后超过一帧(系统卡顿),重置时钟防止帧爆发
570
+ next_write = now + frame_interval
571
+
572
+ # 推进到下一帧时刻(用 += 防止累积误差)
573
+ next_write += frame_interval
574
+
575
+ try:
576
+ # 每帧先排空传感器队列,取最新数据(所有相机共享)
577
+ sensor_data: Dict[str, np.ndarray] = {}
578
+ while True:
579
+ try:
580
+ s_name, s_data = self._sensor_queue.get_nowait()
581
+ sensor_data[s_name] = s_data
582
+ except queue.Empty:
583
+ break
584
+ # 手套 IMU/骨架快照随本帧行写入(USB 手套;空 dict 无影响)
585
+ glove_imu = self._pop_glove_imu()
586
+ glove_kpts = self._pop_glove_keypoints()
587
+
588
+ # 处理 CameraSlot 队列(排空取最新帧)
589
+ for sid, sl in self._slots.items():
590
+ try:
591
+ frame = sl.frame_queue.get_nowait()
592
+ except queue.Empty:
593
+ continue
594
+ while True:
595
+ try:
596
+ frame = sl.frame_queue.get_nowait()
597
+ except queue.Empty:
598
+ break
599
+
600
+ # 单目相机左右镜像翻转(录制时写入 MP4)
601
+ if getattr(settings, 'CAMERA_MIRROR_HORIZONTAL', False):
602
+ frame = np.flip(frame, axis=1)
603
+
604
+ self._write_one_frame(sid, frame, sensor_data,
605
+ glove_imu=glove_imu,
606
+ glove_kpts=glove_kpts)
607
+
608
+ # 处理外部帧源队列(取一帧不排空,保证输出均匀无抖动)
609
+ # 双目帧已在 _on_stereo_frame 完成垂直翻转,此处不再重复翻转
610
+ for sid, q in self._external_queues.items():
611
+ try:
612
+ item = q.get_nowait()
613
+ except queue.Empty:
614
+ continue
615
+ if isinstance(item, tuple) and len(item) == 3:
616
+ frame, hw_ns, imu_s = item
617
+ else: # 兼容旧格式(仅帧)
618
+ frame, hw_ns, imu_s = item, 0, None
619
+ # IMU 样本只随 stereo_left 写入(左右目共享同一份,
620
+ # 避免 data/imu/ 出现重复行)
621
+ if sid != "stereo_left":
622
+ imu_s = None
623
+ else:
624
+ # v1.0.9:队列满时暂存的 IMU 批次随后续帧挂靠。
625
+ # 被丢帧的 IMU 晚于队内旧帧,只挂靠 ts ≤ 本帧 hw_ns
626
+ # 的样本(旧样本在前,跨行时序单调),其余留在缓冲
627
+ # 等更晚的帧;hw_ns=0 无时序信息则全量挂靠。
628
+ with self._pending_imu_lock:
629
+ pending = self._pending_imu.pop(sid, None)
630
+ if pending:
631
+ if hw_ns and hw_ns > 0:
632
+ due = [s for s in pending if s[0] <= hw_ns]
633
+ rest = [s for s in pending if s[0] > hw_ns]
634
+ if rest:
635
+ with self._pending_imu_lock:
636
+ self._pending_imu.setdefault(
637
+ sid, []).extend(rest)
638
+ pending = due
639
+ if pending:
640
+ imu_s = list(pending) + list(imu_s or [])
641
+ self._write_one_frame(sid, frame, sensor_data,
642
+ flip_vertical=False,
643
+ hardware_ns=hw_ns,
644
+ imu_samples=imu_s,
645
+ glove_imu=glove_imu,
646
+ glove_kpts=glove_kpts)
647
+
648
+ except Exception:
649
+ pass
650
+
651
+ def _write_one_frame(self, sid: str, frame: np.ndarray,
652
+ sensor_data: Dict[str, np.ndarray],
653
+ flip_vertical: bool = True,
654
+ hardware_ns: int = 0,
655
+ imu_samples: Optional[List] = None,
656
+ glove_imu: Optional[Dict[str, tuple]] = None,
657
+ glove_kpts: Optional[Dict[str, np.ndarray]] = None):
658
+ """写入单帧到 MP4 + Parquet。
659
+
660
+ Args:
661
+ flip_vertical: 是否在 write_video_frame 中上下翻转。
662
+ 单目 CameraSlot 路径为 True(默认);
663
+ 双目外部帧路径为 False(已在 _on_stereo_frame 完成翻转)。
664
+ hardware_ns: SDK 硬件纳秒时间戳(双目相机帧)
665
+ imu_samples: 本帧窗口的 IMU 样本列表(双目,随 stereo_left 携带)
666
+ glove_imu: {传感器名: (quats 64×f32, valid 16×f32)} USB 手套 IMU 快照
667
+ glove_kpts: {传感器名: 63×f32 骨架关键点} USB 手套骨架快照
668
+ """
669
+ cam_frame_idx = self._per_cam_frame.get(sid, 0)
670
+ rel_ts = time.time() - self._episode_start_s
671
+ self._writer.write_video_frame(sid, frame, flip_vertical=flip_vertical)
672
+ self._writer.write_frame_row(
673
+ cam_frame_idx, rel_ts,
674
+ sensors=sensor_data if sensor_data else None,
675
+ connection_status=self._device_status,
676
+ hardware_ns=hardware_ns,
677
+ imu_samples=imu_samples,
678
+ glove_imu=glove_imu,
679
+ glove_kpts=glove_kpts,
680
+ )
681
+ self._per_cam_frame[sid] = cam_frame_idx + 1
682
+ self._frame_count += 1
683
+
684
+ # 深度帧:每路深度相机随自己的主槽位同步写入
685
+ # (S80M: stereo_left;D435 第 n 台: d435_rgb[_n])
686
+ depth_targets = dict(self._depth_cameras)
687
+ if not depth_targets:
688
+ # S80M 传统路径:无深度伪相机注册,默认随 stereo_left 落盘
689
+ depth_targets = {"stereo_left": {
690
+ "master_slot": "stereo_left"}}
691
+ for dname, dconf in depth_targets.items():
692
+ if sid != dconf["master_slot"]:
693
+ continue
694
+ try:
695
+ depth_frame = self._depth_queues.get(dname,
696
+ queue.Queue()).get_nowait()
697
+ fresh = True
698
+ except queue.Empty:
699
+ # 深度引擎停顿期队列空(实测产出 ~27fps 突发,低于录制
700
+ # 帧率,见 write_depth keep-latest):重复最近帧补满节拍,
701
+ # 深度视频帧 i 与 RGB 帧 i 严格对齐(12-bit MP4 与 FFV1
702
+ # 回退同口径);编码器对重复帧增量≈0
703
+ depth_frame = self._last_depth_frames.get(dname)
704
+ if depth_frame is None:
705
+ continue
706
+ fresh = False
707
+ if fresh:
708
+ idx = self._depth_frame_idx.get(dname, 0)
709
+ self._last_depth_frames[dname] = depth_frame
710
+ self._depth_frame_idx[dname] = idx + 1
711
+ else:
712
+ idx = 0
713
+ self._writer.write_depth_frame(idx, depth_frame, depth_slot=dname)
714
+
715
+ def write_sensor(self, data, capture_ts_us: int = 0,
716
+ sensor_name: str = ""):
717
+ """将传感器数据通过队列传给写入线程(线程安全)。
718
+
719
+ Args:
720
+ data: 16×16 float32 压力矩阵
721
+ capture_ts_us: 保留参数(兼容),当前不使用
722
+ sensor_name: 传感器设备名(如 "sensors_right"),
723
+ 为空时默认用 _sensor_names 的第一个
724
+ """
725
+ if self._writer is not None and self._recording:
726
+ if not sensor_name and self._sensor_names:
727
+ sensor_name = self._sensor_names[0]
728
+ if not sensor_name:
729
+ return
730
+ try:
731
+ self._sensor_queue.put_nowait(
732
+ (sensor_name, data.astype(np.float32).ravel())
733
+ )
734
+ except queue.Full:
735
+ self._drop_stats.inc("sensor_queue") # 消费不及,丢弃旧数据
736
+
737
+ def write_glove_imu(self, sensor_name: str, quats, valid):
738
+ """USB 手套 IMU 四元数快照(主线程 → 写入线程,最新值覆盖)。
739
+
740
+ Args:
741
+ sensor_name: 传感器列名(如 "right_glove")
742
+ quats: 16×4 XYZW 四元数(64×float32 展平)
743
+ valid: 16×bool 有效掩码(展平)
744
+ """
745
+ if self._writer is None or not self._recording or not sensor_name:
746
+ return
747
+ q = np.asarray(quats, dtype=np.float32).ravel()[:64]
748
+ v = np.asarray(valid, dtype=np.float32).ravel()[:16]
749
+ if q.size != 64 or v.size != 16:
750
+ return
751
+ with self._glove_imu_lock:
752
+ self._glove_imu[sensor_name] = (q, v)
753
+
754
+ def _pop_glove_imu(self) -> Dict[str, tuple]:
755
+ """取走并清空本帧窗口内的手套 IMU 快照(写入线程调用)。"""
756
+ with self._glove_imu_lock:
757
+ snapshot = dict(self._glove_imu)
758
+ self._glove_imu.clear()
759
+ return snapshot
760
+
761
+ def write_glove_keypoints(self, sensor_name: str, kpts):
762
+ """USB 手套骨架关键点快照(主线程 → 写入线程,最新值覆盖)。
763
+
764
+ Args:
765
+ sensor_name: 传感器列名(如 "left_glove")
766
+ kpts: 21×3 关节坐标(米,float32,展平 63 维)
767
+ """
768
+ if self._writer is None or not self._recording or not sensor_name:
769
+ return
770
+ k = np.asarray(kpts, dtype=np.float32).ravel()[:63]
771
+ if k.size != 63 or not np.isfinite(k).all():
772
+ return
773
+ with self._glove_kpts_lock:
774
+ self._glove_kpts[sensor_name] = k
775
+
776
+ def _pop_glove_keypoints(self) -> Dict[str, np.ndarray]:
777
+ """取走并清空本帧窗口内的手套骨架快照(写入线程调用)。"""
778
+ with self._glove_kpts_lock:
779
+ snapshot = dict(self._glove_kpts)
780
+ self._glove_kpts.clear()
781
+ return snapshot
782
+
783
+ @property
784
+ def last_recording_frames(self) -> Dict[str, int]:
785
+ """上一轮录制的每相机帧数(快照副本,用于录制完成回调的统计)。"""
786
+ return dict(self._last_recording_frames)
787
+
788
+ @property
789
+ def last_drop_stats(self) -> Dict[str, int]:
790
+ """上一轮录制的丢帧统计(快照副本;含 imu_overflow 键)。"""
791
+ return dict(self._last_drop_stats)
792
+
793
+ @property
794
+ def last_episode_index(self) -> int:
795
+ """上一轮录制的全局 episode 序号(v1.1.0 池化布局;1 起)。"""
796
+ return self._last_episode_index
797
+
798
+ def finish_recording(self, slot_id: str):
799
+ """正常停止——最终化共享会话。"""
800
+ if not self._recording:
801
+ return
802
+ self._recording = False
803
+ self._timer_running = False
804
+ self._duration_timer.stop()
805
+
806
+ # 等待写入线程退出
807
+ if self._write_thread is not None and self._write_thread.is_alive():
808
+ self._write_thread.join(timeout=3.0)
809
+ self._write_thread = None
810
+
811
+ # 快照每相机帧数:_finish_async 在后台线程 emit 回调,
812
+ # 用户可能抢在回调前开始新一轮录制重置 _per_cam_frame
813
+ self._last_recording_frames = dict(self._per_cam_frame)
814
+
815
+ # v1.0.9 丢帧统计快照(含 IMU 溢出次数):供录制完成回调提示与
816
+ # writer 元数据回写;在 _finish_async(end_episode)之前注入
817
+ self._last_drop_stats = self._drop_stats.snapshot()
818
+ self._last_drop_stats["imu_overflow"] = self._imu_overflow_count
819
+ if self._writer is not None:
820
+ self._writer.set_drop_stats(self._last_drop_stats)
821
+
822
+ path = self._session_path
823
+ threading.Thread(target=self._finish_async, args=(path,), daemon=True).start()
824
+
825
+ def _finish_async(self, path: str):
826
+ if self._writer:
827
+ self._writer.end_episode()
828
+ self._writer = None
829
+ self._session_path = None
830
+ self.session_changed.emit(None)
831
+ if path:
832
+ self.recording_finished.emit(self._recording_slot or "", path)
833
+
834
+ def abort_recording(self, slot_id: str):
835
+ """异常停止——丢弃共享会话。"""
836
+ if not self._recording:
837
+ return
838
+ self._recording = False
839
+ self._timer_running = False
840
+ self._duration_timer.stop()
841
+
842
+ # 等待写入线程退出
843
+ if self._write_thread is not None and self._write_thread.is_alive():
844
+ self._write_thread.join(timeout=3.0)
845
+ self._write_thread = None
846
+
847
+ threading.Thread(target=self._abort_async, daemon=True).start()
848
+
849
+ def _abort_async(self):
850
+ if self._writer:
851
+ self._writer.abort_episode()
852
+ self._writer = None
853
+ self._session_path = None
854
+ self.session_changed.emit(None)
855
+ self.recording_aborted.emit(self._recording_slot or "")
856
+
857
+ @property
858
+ def elapsed(self) -> float:
859
+ if not self._recording:
860
+ return 0.0
861
+ return time.monotonic() - self._start_time
862
+
863
+ def _tick_duration(self):
864
+ """QTimer 回调 —— 主线程执行,无跨线程问题。"""
865
+ if self._recording:
866
+ self.duration_changed.emit(self._recording_slot or "", self.elapsed)
collector/core/recording_record.py ADDED
@@ -0,0 +1,30 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 数据模型 —— 录制记录的数据类定义。
3
+ """
4
+
5
+ from __future__ import annotations
6
+ from dataclasses import dataclass, field
7
+ from core.helpers import new_id, utcnow
8
+
9
+
10
+ @dataclass
11
+ class RecordingRecord:
12
+ """录制历史表中的一条记录。"""
13
+
14
+ id: str = field(default_factory=new_id)
15
+ camera_index: int = 0
16
+ camera_name: str = ""
17
+ file_path: str = "" # v1.1.0 池化:任务目录(一行 = 一个 episode)
18
+ episode_index: int = 0 # v1.1.0 池化:全局 episode 序号(1 起)
19
+ file_size_mb: float = 0.0 # 文件大小(MB)
20
+ duration_sec: float = 0.0 # 录制时长(秒)
21
+ resolution_w: int = 0
22
+ resolution_h: int = 0
23
+ status: str = "completed" # "completed"(已完成)| "aborted"(已丢弃)| "uploaded_deleted"(已上传,本地已删)| "deleted"(已删除,未上传)
24
+ started_at: str = field(default_factory=utcnow)
25
+ finished_at: str = field(default_factory=utcnow)
26
+
27
+ @classmethod
28
+ def from_row(cls, row) -> RecordingRecord:
29
+ """从 sqlite3.Row 构造实例。"""
30
+ return cls(**dict(row))
collector/core/recording_repository.py ADDED
@@ -0,0 +1,94 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 数据仓库层 —— 录制记录的增删改查操作。
3
+ """
4
+
5
+ from typing import List, Optional
6
+ from core.database import db
7
+ from core.recording_record import RecordingRecord
8
+
9
+
10
+ class RecordingRepo:
11
+ """录制记录的 CRUD 仓库。"""
12
+
13
+ @staticmethod
14
+ def save(r: RecordingRecord):
15
+ """保存(插入或替换)一条录制记录。"""
16
+ db.conn.execute(
17
+ """INSERT OR REPLACE INTO recording
18
+ (id, camera_index, camera_name, file_path, episode_index,
19
+ file_size_mb, duration_sec, resolution_w, resolution_h,
20
+ status, started_at, finished_at)
21
+ VALUES (?,?,?,?,?,?,?,?,?,?,?,?)""",
22
+ (r.id, r.camera_index, r.camera_name, r.file_path,
23
+ r.episode_index, r.file_size_mb, r.duration_sec,
24
+ r.resolution_w, r.resolution_h, r.status,
25
+ r.started_at, r.finished_at),
26
+ )
27
+ db.conn.commit()
28
+
29
+ @staticmethod
30
+ def list_all(limit: int = 100) -> List[RecordingRecord]:
31
+ """按时间倒序返回最近 N 条录制记录。"""
32
+ rows = db.conn.execute(
33
+ "SELECT * FROM recording ORDER BY started_at DESC LIMIT ?", (limit,)
34
+ ).fetchall()
35
+ return [RecordingRecord.from_row(r) for r in rows]
36
+
37
+ @staticmethod
38
+ def list_by_camera(camera_index: int, limit: int = 50) -> List[RecordingRecord]:
39
+ """按摄像机索引筛选录制记录。"""
40
+ rows = db.conn.execute(
41
+ "SELECT * FROM recording WHERE camera_index=? ORDER BY started_at DESC LIMIT ?",
42
+ (camera_index, limit),
43
+ ).fetchall()
44
+ return [RecordingRecord.from_row(r) for r in rows]
45
+
46
+ @staticmethod
47
+ def delete(record_id: str):
48
+ """删除指定记录。"""
49
+ db.conn.execute("DELETE FROM recording WHERE id=?", (record_id,))
50
+ db.conn.commit()
51
+
52
+ @staticmethod
53
+ def _where(file_path: str, episode_index):
54
+ """匹配条件:v1.1.0 起按 (file_path, episode_index) 定位一个 episode;
55
+ episode_index 为 None 时退回旧路径匹配(历史行/旧调用方兼容)。"""
56
+ if episode_index is None:
57
+ return "file_path=?", (file_path,)
58
+ return "file_path=? AND episode_index=?", (file_path, episode_index)
59
+
60
+ @staticmethod
61
+ def mark_uploaded(file_path: str, episode_index=None) -> int:
62
+ """把指定 episode 的录制行状态改为 uploaded(上传成功、本地保留)。
63
+
64
+ 行保留(历史可查"已上传"),返回受影响行数。
65
+ """
66
+ cond, args = RecordingRepo._where(file_path, episode_index)
67
+ cur = db.conn.execute(
68
+ f"UPDATE recording SET status='uploaded' WHERE {cond}", args)
69
+ db.conn.commit()
70
+ return cur.rowcount
71
+
72
+ @staticmethod
73
+ def mark_uploaded_deleted(file_path: str, episode_index=None) -> int:
74
+ """把指定 episode 的录制行状态改为 uploaded_deleted(上传成功后本地已删)。
75
+
76
+ 行保留(用户要求历史可查"已上传,本地已删"),返回受影响行数。
77
+ """
78
+ cond, args = RecordingRepo._where(file_path, episode_index)
79
+ cur = db.conn.execute(
80
+ f"UPDATE recording SET status='uploaded_deleted' WHERE {cond}", args)
81
+ db.conn.commit()
82
+ return cur.rowcount
83
+
84
+ @staticmethod
85
+ def mark_deleted(file_path: str, episode_index=None) -> int:
86
+ """把指定 episode 的录制行状态改为 deleted(本地已删、未上传)。
87
+
88
+ 行保留(历史可查"已删除,未上传"),返回受影响行数。
89
+ """
90
+ cond, args = RecordingRepo._where(file_path, episode_index)
91
+ cur = db.conn.execute(
92
+ f"UPDATE recording SET status='deleted' WHERE {cond}", args)
93
+ db.conn.commit()
94
+ return cur.rowcount
collector/core/render_engine.py ADDED
@@ -0,0 +1,768 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 传感器数据渲染引擎 —— 5 种可视化模式。
3
+
4
+ 所有渲染函数接收 (processed_data, max_signal, config, window_size)
5
+ 返回 BGR 格式的 numpy 帧数组,由 UI 层转为 QPixmap 显示。
6
+ """
7
+
8
+ import json
9
+ import os
10
+ import numpy as np
11
+ import cv2
12
+
13
+ from config import settings
14
+
15
+ # ── 常量 ──────────────────────────────────────────────
16
+ MATRIX_ROWS = 16
17
+ MATRIX_COLS = 16
18
+ VIRIDIS_LUT: np.ndarray = None
19
+
20
+ # ── 配置路径 ──────────────────────────────────────────
21
+ # 仿生手掌(左/右手)映射配置。原定义在 sensors.sensor_panel,
22
+ # 迁到这里:glove_widget / playback_dialog 需要路径但不应引入 bleak。
23
+ CONFIG_DIR = os.path.join(settings.BASE_DIR, "config", "sensors")
24
+ CONFIG_FILE = os.path.join(CONFIG_DIR, "hand_ble_config.json")
25
+ CONFIG_FILE_LEFT = os.path.join(CONFIG_DIR, "hand_ble_config_left.json")
26
+
27
+
28
+ def _get_viridis_lut() -> np.ndarray:
29
+ """延迟初始化 Viridis 颜色查找表。"""
30
+ global VIRIDIS_LUT
31
+ if VIRIDIS_LUT is None:
32
+ lut = np.zeros((256, 1, 3), dtype=np.uint8)
33
+ for i in range(256):
34
+ lut[i, 0, 0] = i
35
+ VIRIDIS_LUT = cv2.applyColorMap(lut, cv2.COLORMAP_VIRIDIS)
36
+ return VIRIDIS_LUT
37
+
38
+
39
+ # ── HUD 绘制 ──────────────────────────────────────────
40
+
41
+ def _draw_hud(frame: np.ndarray, lines: list, color=(255, 255, 255)):
42
+ """在帧左上角绘制半透明 HUD 信息栏。"""
43
+ font, scale, thick, lh, pad = cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1, 22, 10
44
+ max_w = (
45
+ max(cv2.getTextSize(l, font, scale, thick)[0][0] for l in lines)
46
+ if lines else 0
47
+ )
48
+ overlay = frame.copy()
49
+ cv2.rectangle(
50
+ overlay, (0, 0), (max_w + pad * 2, len(lines) * lh + pad * 2),
51
+ (0, 0, 0), -1,
52
+ )
53
+ cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame)
54
+ for i, line in enumerate(lines):
55
+ cv2.putText(
56
+ frame, line, (pad, pad + lh - 5 + i * lh),
57
+ font, scale, color, thick, cv2.LINE_AA,
58
+ )
59
+
60
+
61
+ # ── 配置加载 ──────────────────────────────────────────
62
+
63
+ def _load_json(path, default):
64
+ if os.path.exists(path):
65
+ try:
66
+ with open(path, "r", encoding="utf-8") as f:
67
+ loaded = json.load(f)
68
+ if isinstance(default, dict):
69
+ default.update(loaded)
70
+ return default
71
+ return loaded
72
+ except Exception:
73
+ pass
74
+ return default
75
+
76
+
77
+ # ═══════════════════════════════════════════════════════
78
+ # 模式 1: 热力图
79
+ # ═══════════════════════════════════════════════════════
80
+
81
+ def render_heatmap(
82
+ processed: np.ndarray,
83
+ max_signal: float,
84
+ config: dict,
85
+ window_size: tuple,
86
+ current_vmax: float,
87
+ fps: float,
88
+ noise_gate: int,
89
+ dyn_ratio: float,
90
+ spatial_on: bool,
91
+ ) -> tuple:
92
+ """返回 (frame, new_vmax)。"""
93
+ ww, wh = window_size
94
+ rows = config.get("rows", list(range(16)))
95
+ cols = config.get("cols", list(range(16)))
96
+ order = config.get("axis_order", "row_col")
97
+
98
+ # 增强参数(来自 config,向后兼容)
99
+ subpixel = config.get("subpixel", True)
100
+ gamma = config.get("gamma", 1.0)
101
+ blur_ksize = config.get("blur", 5)
102
+
103
+ nr, nc = len(rows), len(cols)
104
+ if nr == 0 or nc == 0:
105
+ frame = np.zeros((wh, ww, 3), dtype=np.uint8)
106
+ _draw_hud(frame, ["Press [M] to select Heatmap Rows/Cols!"], (0, 0, 255))
107
+ return frame, current_vmax
108
+
109
+ # 提取子矩阵
110
+ sub = processed[np.ix_(rows, cols)]
111
+ if order != "row_col":
112
+ sub = sub.T
113
+
114
+ # ── Subpixel 超分 ──────────────────────────────
115
+ if subpixel and sub.shape[0] >= 2 and sub.shape[1] >= 2:
116
+ H0, W0 = sub.shape
117
+ shrink = 1.5
118
+ padded = np.pad(sub, 1, mode='constant')
119
+ top = padded[0:H0, 1:W0 + 1]
120
+ bottom = padded[2:H0 + 2, 1:W0 + 1]
121
+ left = padded[1:H0 + 1, 0:W0]
122
+ right = padded[1:H0 + 1, 2:W0 + 2]
123
+ sum_x = left + right + sub + 1e-6
124
+ sum_y = top + bottom + sub + 1e-6
125
+ dx = np.clip((right - left) / sum_x * shrink, -1, 1)
126
+ dy = np.clip((bottom - top) / sum_y * shrink, -1, 1)
127
+ super_res = np.zeros((H0 * 2, W0 * 2), dtype=np.float32)
128
+ super_res[0::2, 0::2] = sub * np.maximum(0, 1.0 - dx - dy)
129
+ super_res[0::2, 1::2] = sub * np.maximum(0, 1.0 + dx - dy)
130
+ super_res[1::2, 0::2] = sub * np.maximum(0, 1.0 - dx + dy)
131
+ super_res[1::2, 1::2] = sub * np.maximum(0, 1.0 + dx + dy)
132
+ sub = super_res
133
+
134
+ H, W = sub.shape
135
+ scale_w = (ww - 200) / W
136
+ scale_h = (wh - 120) / H
137
+ scale = min(scale_w, scale_h)
138
+ target_w = int(W * scale)
139
+ target_h = int(H * scale)
140
+
141
+ resized = cv2.resize(sub, (target_w, target_h), interpolation=cv2.INTER_CUBIC)
142
+ if blur_ksize > 0:
143
+ k = blur_ksize if blur_ksize % 2 == 1 else blur_ksize + 1
144
+ resized = cv2.GaussianBlur(resized, (k, k), 0)
145
+
146
+ # 平滑 vmax
147
+ vmax_cand = max(5000, np.percentile(resized, 99.7))
148
+ new_vmax = current_vmax * 0.95 + vmax_cand * 0.05 if vmax_cand > current_vmax else vmax_cand
149
+
150
+ # ── Gamma 校正 ────────────────────────────────
151
+ norm = np.clip(resized / new_vmax, 0, 1) if new_vmax > 0 else resized
152
+ if gamma != 1.0:
153
+ norm = np.power(norm, gamma)
154
+ img_8u = (norm * 255).astype(np.uint8)
155
+ color = cv2.applyColorMap(img_8u, cv2.COLORMAP_VIRIDIS)
156
+
157
+ frame = np.zeros((wh, ww, 3), dtype=np.uint8)
158
+ sx = (ww - target_w) // 2
159
+ sy = (wh - target_h) // 2 + 20
160
+ frame[sy:sy + target_h, sx:sx + target_w] = color
161
+
162
+ _draw_hud(frame, [
163
+ f"Heatmap | FPS:{fps:.1f} | Max:{int(max_signal)}",
164
+ f"Gate:{noise_gate} | Dyn:{dyn_ratio:.2f} | Filter:{'ON' if spatial_on else 'OFF'}",
165
+ f"SP:{'ON' if subpixel else 'OFF'} G:{gamma:.1f} B:{blur_ksize} | [M] Config",
166
+ ])
167
+ return frame, new_vmax
168
+
169
+
170
+ # ═══════════════════════════════════════════════════════
171
+ # 模式 2: 轨迹模式
172
+ # ═══════════════════════════════════════════════════════
173
+
174
+ _trace_canvas = None
175
+
176
+ def render_trace(
177
+ processed: np.ndarray,
178
+ max_signal: float,
179
+ config: dict,
180
+ window_size: tuple,
181
+ current_vmax: float,
182
+ fps: float,
183
+ noise_gate: int,
184
+ dyn_ratio: float,
185
+ spatial_on: bool,
186
+ ) -> tuple:
187
+ """轨迹模式:叠加历史数据。"""
188
+ global _trace_canvas
189
+ ww, wh = window_size
190
+ rows = config.get("rows", list(range(16)))
191
+ cols = config.get("cols", list(range(16)))
192
+ order = config.get("axis_order", "row_col")
193
+
194
+ # 增强参数
195
+ subpixel = config.get("subpixel", True)
196
+ gamma = config.get("gamma", 1.0)
197
+ blur_ksize = config.get("blur", 5)
198
+
199
+ nr, nc = len(rows), len(cols)
200
+ if nr == 0 or nc == 0:
201
+ frame = np.zeros((wh, ww, 3), dtype=np.uint8)
202
+ _draw_hud(frame, ["Press hotkey to config"], (0, 0, 255))
203
+ return frame, current_vmax, False
204
+
205
+ sub = processed[np.ix_(rows, cols)]
206
+ if order != "row_col":
207
+ sub = sub.T
208
+
209
+ # ── Subpixel 超分 ──────────────────────────────
210
+ if subpixel and sub.shape[0] >= 2 and sub.shape[1] >= 2:
211
+ H0, W0 = sub.shape
212
+ shrink = 1.5
213
+ padded = np.pad(sub, 1, mode='constant')
214
+ top = padded[0:H0, 1:W0 + 1]
215
+ bottom = padded[2:H0 + 2, 1:W0 + 1]
216
+ left = padded[1:H0 + 1, 0:W0]
217
+ right = padded[1:H0 + 1, 2:W0 + 2]
218
+ sum_x = left + right + sub + 1e-6
219
+ sum_y = top + bottom + sub + 1e-6
220
+ dx = np.clip((right - left) / sum_x * shrink, -1, 1)
221
+ dy = np.clip((bottom - top) / sum_y * shrink, -1, 1)
222
+ super_res = np.zeros((H0 * 2, W0 * 2), dtype=np.float32)
223
+ super_res[0::2, 0::2] = sub * np.maximum(0, 1.0 - dx - dy)
224
+ super_res[0::2, 1::2] = sub * np.maximum(0, 1.0 + dx - dy)
225
+ super_res[1::2, 0::2] = sub * np.maximum(0, 1.0 - dx + dy)
226
+ super_res[1::2, 1::2] = sub * np.maximum(0, 1.0 + dx + dy)
227
+ sub = super_res
228
+
229
+ if _trace_canvas is None or _trace_canvas.shape != sub.shape:
230
+ _trace_canvas = np.zeros_like(sub)
231
+ _trace_canvas = np.maximum(_trace_canvas, sub)
232
+ display = _trace_canvas
233
+
234
+ H, W = display.shape
235
+ scale_w = (ww - 200) / W
236
+ scale_h = (wh - 120) / H
237
+ scale = min(scale_w, scale_h)
238
+ tw, th = int(W * scale), int(H * scale)
239
+
240
+ resized = cv2.resize(display, (tw, th), interpolation=cv2.INTER_CUBIC)
241
+ if blur_ksize > 0:
242
+ k = blur_ksize if blur_ksize % 2 == 1 else blur_ksize + 1
243
+ resized = cv2.GaussianBlur(resized, (k, k), 0)
244
+
245
+ vmax_cand = max(5000, np.percentile(resized, 99.7))
246
+ new_vmax = current_vmax * 0.95 + vmax_cand * 0.05 if vmax_cand > current_vmax else vmax_cand
247
+
248
+ norm = np.clip(resized / new_vmax, 0, 1) if new_vmax > 0 else resized
249
+ if gamma != 1.0:
250
+ norm = np.power(norm, gamma)
251
+ img_8u = (norm * 255).astype(np.uint8)
252
+ color = cv2.applyColorMap(img_8u, cv2.COLORMAP_VIRIDIS)
253
+
254
+ frame = np.zeros((wh, ww, 3), dtype=np.uint8)
255
+ sx, sy = (ww - tw) // 2, (wh - th) // 2 + 20
256
+ frame[sy:sy + th, sx:sx + tw] = color
257
+
258
+ _draw_hud(frame, [
259
+ f"Trace | FPS:{fps:.1f} | Max:{int(max_signal)}",
260
+ f"Gate:{noise_gate} | Dyn:{dyn_ratio:.2f} | Filter:{'ON' if spatial_on else 'OFF'}",
261
+ f"SP:{'ON' if subpixel else 'OFF'} G:{gamma:.1f} B:{blur_ksize} | [X] Clear | [M] Config",
262
+ ])
263
+ return frame, new_vmax, True
264
+
265
+ def clear_trace_canvas():
266
+ global _trace_canvas
267
+ _trace_canvas = None
268
+
269
+
270
+ # ═══════════════════════════════════════════════════════
271
+ # 模式 3: 网格数据
272
+ # ═══════════════════════════════════════════════════════
273
+
274
+ def render_grid(
275
+ processed: np.ndarray,
276
+ max_signal: float,
277
+ config: dict,
278
+ window_size: tuple,
279
+ fps: float,
280
+ ) -> np.ndarray:
281
+ """网格模式:每个单元格显示数值。"""
282
+ ww, wh = window_size
283
+ rows = config.get("rows", list(range(16)))
284
+ cols = config.get("cols", list(range(16)))
285
+ order = config.get("axis_order", "row_col")
286
+
287
+ nr, nc = len(rows), len(cols)
288
+ visual_rows = nr if order == "row_col" else nc
289
+ visual_cols = nc if order == "row_col" else nr
290
+
291
+ if nr == 0 or nc == 0:
292
+ frame = np.zeros((wh, ww, 3), dtype=np.uint8)
293
+ _draw_hud(frame, ["Press hotkey to config"], (0, 0, 255))
294
+ return frame
295
+
296
+ cell_size = min((ww - 100) / max(1, visual_cols), (wh - 120) / max(1, visual_rows))
297
+ cell_size = max(5.0, min(80.0, cell_size))
298
+
299
+ grid_w = int(visual_cols * cell_size)
300
+ grid_h = int(visual_rows * cell_size)
301
+ sx = (ww - grid_w) // 2
302
+ sy = (wh - grid_h) // 2 + 30
303
+
304
+ frame = np.zeros((wh, ww, 3), dtype=np.uint8)
305
+ grid_vmax = max(5000.0, max_signal)
306
+
307
+ for vi in range(visual_rows):
308
+ for vj in range(visual_cols):
309
+ if order == "row_col":
310
+ r_idx, c_idx = rows[vi], cols[vj]
311
+ else:
312
+ r_idx, c_idx = rows[vj], cols[vi]
313
+ if r_idx >= MATRIX_ROWS or c_idx >= MATRIX_COLS:
314
+ continue
315
+
316
+ val = int(processed[r_idx, c_idx])
317
+ x1 = int(sx + vj * cell_size)
318
+ y1 = int(sy + vi * cell_size)
319
+ x2 = int(sx + (vj + 1) * cell_size)
320
+ y2 = int(sy + (vi + 1) * cell_size)
321
+
322
+ if val > 0:
323
+ blue = min(255, int((val / grid_vmax) * 255))
324
+ cv2.rectangle(frame, (x1, y1), (x2, y2), (blue, 0, 0), -1)
325
+ cv2.rectangle(frame, (x1, y1), (x2, y2), (50, 50, 50), 1)
326
+
327
+ if val > 0 and cell_size > 15:
328
+ fs = max(0.3, cell_size / 60.0)
329
+ text = str(val)
330
+ (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_PLAIN, fs, 1)
331
+ if tw > cell_size * 0.9:
332
+ fs *= (cell_size * 0.9 / tw)
333
+ (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_PLAIN, fs, 1)
334
+ cv2.putText(
335
+ frame, text,
336
+ (x1 + (int(cell_size) - tw) // 2, y1 + (int(cell_size) + th) // 2),
337
+ cv2.FONT_HERSHEY_PLAIN, fs, (255, 255, 255), 1, cv2.LINE_AA,
338
+ )
339
+
340
+ _draw_hud(frame, [
341
+ f"Grid | FPS:{fps:.1f} | Max:{int(max_signal)}",
342
+ "[M] Config Matrix",
343
+ ])
344
+ return frame
345
+
346
+
347
+ # ═══════════════════════════════════════════════════════
348
+ # 模式 4: 仿生手掌
349
+ # ═══════════════════════════════════════════════════════
350
+
351
+ # 手部锚点坐标(相对于窗口)
352
+ HAND_ANCHORS = {
353
+ "thumb_tip": (350, 280), "thumb_joint": (420, 360), "thumb_base": (480, 420),
354
+ "index_tip": (510, 120), "index_joint": (540, 240), "index_base": (560, 340),
355
+ "middle_tip": (640, 80), "middle_joint": (640, 200), "middle_base": (640, 320),
356
+ "ring_tip": (770, 120), "ring_joint": (740, 240), "ring_base": (720, 340),
357
+ "pinky_tip": (930, 280), "pinky_joint": (860, 360), "pinky_base": (800, 420),
358
+ "palm": (640, 480),
359
+ }
360
+ WRIST_ANCHOR = (640, 600)
361
+
362
+ # 默认手部配置(16×16 传感器映射到手指区域)
363
+ # 右手实测(2026-09-04 录制逐格核对):行 = 手指,拇指 1-3 … 小指
364
+ # 13-15(行 0 为空),即旧配置 [0-2…12-14] 整体偏高一行的修正;
365
+ # 列 12-15 = 指骨 [14,12,13,15] 根→尖;掌心行 1-15 x 传感列 [10,9,8,6,4]。
366
+ DEFAULT_HAND = {
367
+ "thumb_joint": {"rows": [1, 2, 3], "cols": [14, 12, 13, 15], "axis_order": "col_row"},
368
+ "index_joint": {"rows": [4, 5, 6], "cols": [14, 12, 13, 15], "axis_order": "col_row"},
369
+ "middle_joint": {"rows": [7, 8, 9], "cols": [14, 12, 13, 15], "axis_order": "col_row"},
370
+ "ring_joint": {"rows": [10, 11, 12], "cols": [14, 12, 13, 15], "axis_order": "col_row"},
371
+ "pinky_joint": {"rows": [13, 14, 15], "cols": [14, 12, 13, 15], "axis_order": "col_row"},
372
+ "palm": {"rows": list(range(1, 16)), "cols": [10, 9, 8, 6, 4], "axis_order": "col_row"},
373
+ }
374
+ for k in DEFAULT_HAND:
375
+ if k != "palm":
376
+ DEFAULT_HAND[k]["name"] = k
377
+
378
+
379
+ def _scale_point(x: float, y: float, sx: float, sy: float) -> tuple:
380
+ """按缩放因子缩放坐标点。"""
381
+ return (int(x * sx), int(y * sy))
382
+
383
+
384
+ def render_hand(
385
+ processed: np.ndarray,
386
+ max_signal: float,
387
+ config: dict,
388
+ window_size: tuple,
389
+ current_vmax: float,
390
+ fps: float,
391
+ noise_gate: int,
392
+ dyn_ratio: float,
393
+ spatial_on: bool,
394
+ drift: float,
395
+ ) -> tuple:
396
+ """仿生手掌映射模式。
397
+
398
+ 硬编码锚点坐标基于 1280×720 画布设计,运行时根据实际 window_size
399
+ 等比缩放,适配任意尺寸(如回放面板的 640×400)。
400
+ """
401
+ ww, wh = window_size
402
+
403
+ # ── 计算缩放因子(锚点基于 1280×720 设计) ──────────
404
+ NOM_W, NOM_H = 1280.0, 720.0
405
+ sx = ww / NOM_W
406
+ sy = wh / NOM_H
407
+
408
+ frame = np.full((wh, ww, 3), 15, dtype=np.uint8)
409
+ lut = _get_viridis_lut()
410
+
411
+ # ── 缩放后的锚点 ────────────────────────────────────
412
+ def _sp(x, y):
413
+ return _scale_point(x, y, sx, sy)
414
+
415
+ wrist = _sp(*WRIST_ANCHOR)
416
+ scaled_anchors = {k: _sp(*v) for k, v in HAND_ANCHORS.items()}
417
+
418
+ # ── 缩放线宽和圆半径 ──────────────────────────────
419
+ lw_scale = min(sx, sy)
420
+ _lw = lambda v: max(1, int(v * lw_scale))
421
+ _cr = lambda v: max(1, int(v * lw_scale))
422
+
423
+ # 画骨骼框架
424
+ line_c, joint_c = (60, 60, 60), (80, 80, 80)
425
+ cv2.line(frame, wrist, scaled_anchors["palm"], line_c, _lw(6), cv2.LINE_AA)
426
+ for fg in ["thumb", "index", "middle", "ring", "pinky"]:
427
+ b = scaled_anchors[f"{fg}_base"]
428
+ j = scaled_anchors[f"{fg}_joint"]
429
+ t = scaled_anchors[f"{fg}_tip"]
430
+ cv2.line(frame, scaled_anchors["palm"], b, line_c, _lw(5), cv2.LINE_AA)
431
+ cv2.line(frame, b, j, line_c, _lw(4), cv2.LINE_AA)
432
+ cv2.line(frame, j, t, line_c, _lw(3), cv2.LINE_AA)
433
+ cv2.circle(frame, b, _cr(10), joint_c, -1, cv2.LINE_AA)
434
+ cv2.circle(frame, j, _cr(8), joint_c, -1, cv2.LINE_AA)
435
+ cv2.circle(frame, t, _cr(6), joint_c, -1, cv2.LINE_AA)
436
+ cv2.circle(frame, scaled_anchors["palm"], _cr(16), joint_c, -1, cv2.LINE_AA)
437
+
438
+ # 平滑 vmax
439
+ vmax = max(max_signal, 5000)
440
+ new_vmax = current_vmax * 0.95 + vmax * 0.05 if vmax < current_vmax else vmax
441
+
442
+ CELL = max(1, int(14 * min(sx, sy)))
443
+ for part_key, cfg in config.items():
444
+ if part_key not in scaled_anchors:
445
+ continue
446
+ ax, ay = scaled_anchors[part_key]
447
+ rows = cfg.get("rows", [])
448
+ cols = cfg.get("cols", [])
449
+ order = cfg.get("axis_order", "row_col")
450
+ if not rows or not cols:
451
+ continue
452
+
453
+ nrr, ncc = len(rows), len(cols)
454
+ w = ncc * CELL if order == "row_col" else nrr * CELL
455
+ h = nrr * CELL if order == "row_col" else ncc * CELL
456
+ cx, cy = ax - w // 2, ay - h // 2
457
+
458
+ for i, r in enumerate(rows):
459
+ for j, c in enumerate(cols):
460
+ if r >= MATRIX_ROWS or c >= MATRIX_COLS:
461
+ continue
462
+ val = processed[r, c]
463
+ x1 = cx + (j if order == "row_col" else i) * CELL
464
+ y1 = cy + (i if order == "row_col" else j) * CELL
465
+ x2, y2 = x1 + CELL, y1 + CELL
466
+
467
+ if val > 0:
468
+ idx = int(min(255, (val / new_vmax) * 255))
469
+ b, g, rr = lut[idx, 0]
470
+ cv2.rectangle(frame, (x1, y1), (x2, y2), (int(b), int(g), int(rr)), -1)
471
+ # 单元格内显示数据值
472
+ if CELL > 14:
473
+ vtext = str(int(val))
474
+ fs = 0.4
475
+ (tw, th), _ = cv2.getTextSize(vtext, cv2.FONT_HERSHEY_PLAIN, fs, 1)
476
+ tc = (0, 0, 0) if (0.299 * rr + 0.587 * g + 0.114 * b) > 140 else (255, 255, 255)
477
+ cv2.putText(frame, vtext,
478
+ (x1 + (CELL - tw) // 2, y1 + (CELL + th) // 2),
479
+ cv2.FONT_HERSHEY_PLAIN, fs, tc, 1, cv2.LINE_AA)
480
+ cv2.rectangle(frame, (x1, y1), (x2, y2), (100, 100, 100), 1)
481
+
482
+ _draw_hud(frame, [
483
+ f"Bionic Hand | FPS:{fps:.1f} | Max:{int(max_signal)} | Drift:{int(drift)}",
484
+ f"Gate:{noise_gate} | Dyn:{dyn_ratio:.2f} | Filter:{'ON' if spatial_on else 'OFF'}",
485
+ "[M] Config | [C] Calibrate",
486
+ ])
487
+ return frame, new_vmax
488
+
489
+
490
+ # ═══════════════════════════════════════════════════════
491
+ # 模式 5: 拓扑形变
492
+ # ═══════════════════════════════════════════════════════
493
+
494
+ class DeformMeshState:
495
+ """模式5 的交互状态。"""
496
+
497
+ def __init__(self):
498
+ self.holes = []
499
+ self.flip_x = False
500
+ self.flip_y = False
501
+ self.deform_strength = 1.0
502
+ self.cached_x = None
503
+ self.cached_y = None
504
+ self.cache_valid = False
505
+ # 绘制中
506
+ self.drawing = False
507
+ self.draw_start = (0, 0)
508
+ self.draw_end = (0, 0)
509
+
510
+
511
+ def _update_mesh_cache(state: DeformMeshState, rows, cols, order, ww, wh):
512
+ """更新形变网格缓存。"""
513
+ nr, nc = len(rows), len(cols)
514
+ if nr == 0 or nc == 0:
515
+ state.cached_x = state.cached_y = None
516
+ return
517
+
518
+ CELL = 26
519
+ tw = nc * CELL if order == "row_col" else nr * CELL
520
+ th = nr * CELL if order == "row_col" else nc * CELL
521
+ sx = (ww - tw) // 2
522
+ sy = (wh - th) // 2 + 30
523
+
524
+ jj, ii = np.meshgrid(np.arange(nc), np.arange(nr))
525
+ if order == "row_col":
526
+ mx = sx + jj * CELL
527
+ my = sy + ii * CELL
528
+ else:
529
+ mx = sx + ii * CELL
530
+ my = sy + jj * CELL
531
+
532
+ if state.flip_x:
533
+ mx, my = np.fliplr(mx), np.fliplr(my)
534
+ if state.flip_y:
535
+ mx, my = np.flipud(mx), np.flipud(my)
536
+
537
+ # 应用孔洞形变
538
+ holes = list(state.holes)
539
+ if state.drawing:
540
+ cx1, cy1 = state.draw_start
541
+ cx2, cy2 = state.draw_end
542
+ rx, ry = max(1, abs(cx2 - cx1)), max(1, abs(cy2 - cy1))
543
+ if rx > 5 and ry > 5:
544
+ holes.append({"cx": cx1, "cy": cy1, "rx": rx, "ry": ry})
545
+
546
+ for h in holes:
547
+ cx, cy, rx, ry = h["cx"], h["cy"], h["rx"], h["ry"]
548
+ vx, vy = mx - cx, my - cy
549
+ d = np.sqrt((vx / rx) ** 2 + (vy / ry) ** 2) + 1e-6
550
+ target_d = d + state.deform_strength * np.exp(-d * 1.5)
551
+ scale = target_d / d
552
+ mx = cx + vx * scale
553
+ my = cy + vy * scale
554
+
555
+ state.cached_x, state.cached_y = mx, my
556
+ state.cache_valid = True
557
+
558
+
559
+ def render_deform_mesh(
560
+ processed: np.ndarray,
561
+ max_signal: float,
562
+ config: dict,
563
+ window_size: tuple,
564
+ current_vmax: float,
565
+ fps: float,
566
+ state: DeformMeshState,
567
+ ) -> tuple:
568
+ """拓扑形变模式。"""
569
+ ww, wh = window_size
570
+ frame = np.full((wh, ww, 3), 15, dtype=np.uint8)
571
+ lut = _get_viridis_lut()
572
+
573
+ vmax = max(5000, max_signal)
574
+ new_vmax = current_vmax * 0.95 + vmax * 0.05 if vmax < current_vmax else vmax
575
+
576
+ rows = config.get("rows", list(range(16)))
577
+ cols = config.get("cols", list(range(16)))
578
+ order = config.get("axis_order", "row_col")
579
+
580
+ if not state.cache_valid:
581
+ _update_mesh_cache(state, rows, cols, order, ww, wh)
582
+
583
+ mx, my = state.cached_x, state.cached_y
584
+ if mx is None:
585
+ _draw_hud(frame, ["Press [M] to select Matrix Rows/Cols!"], (0, 0, 255))
586
+ return frame, new_vmax
587
+
588
+ nr, nc = len(rows), len(cols)
589
+
590
+ # 画网格线
591
+ pts = np.stack([mx, my], axis=-1).astype(np.int32)
592
+ cv2.polylines(frame, pts, False, (60, 60, 60), 1, cv2.LINE_AA)
593
+ pts_t = np.ascontiguousarray(np.transpose(pts, (1, 0, 2)))
594
+ cv2.polylines(frame, pts_t, False, (60, 60, 60), 1, cv2.LINE_AA)
595
+
596
+ # 画孔洞
597
+ holes = list(state.holes)
598
+ if state.drawing:
599
+ cx1, cy1 = state.draw_start
600
+ cx2, cy2 = state.draw_end
601
+ rx, ry = max(1, abs(cx2 - cx1)), max(1, abs(cy2 - cy1))
602
+ if rx > 5 and ry > 5:
603
+ holes.append({"cx": cx1, "cy": cy1, "rx": rx, "ry": ry})
604
+ for h in holes:
605
+ cv2.ellipse(frame, (h["cx"], h["cy"]), (h["rx"], h["ry"]),
606
+ 0, 0, 360, (90, 90, 90), 2, cv2.LINE_AA)
607
+
608
+ # 画数据点
609
+ for i in range(nr):
610
+ for j in range(nc):
611
+ ri, cj = rows[i], cols[j]
612
+ if ri >= MATRIX_ROWS or cj >= MATRIX_COLS:
613
+ continue
614
+ val = processed[ri, cj]
615
+ px, py = int(mx[i, j]), int(my[i, j])
616
+ if val > 0:
617
+ idx = int(min(255, (val / new_vmax) * 255))
618
+ b, g, rr = lut[idx, 0]
619
+ cv2.circle(frame, (px, py), 10, (int(b), int(g), int(rr)), -1, cv2.LINE_AA)
620
+ else:
621
+ cv2.circle(frame, (px, py), 3, (80, 80, 80), -1, cv2.LINE_AA)
622
+
623
+ s = state.deform_strength
624
+ _draw_hud(frame, [
625
+ f"Deform Mesh | FPS:{fps:.1f} | Max:{int(max_signal)} | Strength:{s:.1f}",
626
+ f"Flip X:{state.flip_x} | Flip Y:{state.flip_y}",
627
+ "[Drag] Draw Hole | [R] Clear | [U/I] Flip | [O/P] Strength | [M] Config",
628
+ ])
629
+ return frame, new_vmax
630
+
631
+
632
+ # ═══════════════════════════════════════════════════════
633
+ # 手部骨架渲染(MANO 21 关键点 → 3/4 视角透视画面)
634
+ #
635
+ # 移植自手套工具包 apps/rendering/replay.py 的 Camera/_grid/
636
+ # _draw_hand(画布尺寸参数化)。tools/demos/pooled_viewer_demo
637
+ # 另有同口径自包含副本(demo 不导入 core,两边保持同步即可)。
638
+ # ═════════════════════════════════════��═════════════════
639
+
640
+ # MANO 21 关键点骨骼连接 + 分组: 0 拇指 1 食指 2 中指 3 无名指 4 小指 5 掌骨
641
+ SKELETON_BONES = [
642
+ (0, 1, 0), (1, 2, 0), (2, 3, 0), (3, 4, 0), # thumb
643
+ (0, 5, 1), (5, 6, 1), (6, 7, 1), (7, 8, 1), # index
644
+ (0, 9, 2), (9, 10, 2), (10, 11, 2), (11, 12, 2), # middle
645
+ (0, 13, 3), (13, 14, 3), (14, 15, 3), (15, 16, 3), # ring
646
+ (0, 17, 4), (17, 18, 4), (18, 19, 4), (19, 20, 4), # pinky
647
+ (5, 9, 5), (9, 13, 5), (13, 17, 5), # metacarpal
648
+ ]
649
+ # 每根手指分组颜色(BGR)
650
+ SKELETON_FINGER_BGR = [
651
+ (60, 80, 255), # 0 拇指 红
652
+ (60, 255, 255), # 1 食指 黄
653
+ (60, 220, 60), # 2 中指 绿
654
+ (255, 210, 60), # 3 无名指 青
655
+ (255, 120, 255), # 4 小指 紫
656
+ (210, 210, 210), # 5 掌骨 白
657
+ ]
658
+
659
+ _SKELETON_FOV_DEG = 50.0
660
+
661
+
662
+ class SkeletonCamera:
663
+ """球坐标相机 + 透视投影(照工具包 replay.py 的 Camera,画布尺寸参数化)。"""
664
+
665
+ def __init__(self, yaw_deg, elev_deg, dist, img_w, img_h, roll_deg=0.0):
666
+ yaw, elev = np.deg2rad(yaw_deg), np.deg2rad(elev_deg)
667
+ roll = np.deg2rad(roll_deg)
668
+ cp = np.array([dist * np.cos(elev) * np.sin(yaw),
669
+ dist * np.sin(elev),
670
+ dist * np.cos(elev) * np.cos(yaw)])
671
+ self.pos = cp
672
+ self.fwd = -cp / np.linalg.norm(cp)
673
+ self.right = np.cross(self.fwd, [0, 1, 0])
674
+ self.right /= np.linalg.norm(self.right)
675
+ self.up = np.cross(self.right, self.fwd)
676
+ cos_r, sin_r = np.cos(roll), np.sin(roll)
677
+ r0, u0 = self.right, self.up
678
+ self.right = r0 * cos_r + u0 * sin_r
679
+ self.up = -r0 * sin_r + u0 * cos_r
680
+ self.w, self.h = img_w, img_h
681
+ self.f = (self.h / 2) / np.tan(np.deg2rad(_SKELETON_FOV_DEG) / 2)
682
+
683
+ def project(self, pts3d):
684
+ """(N,3) → (N,2) 图像坐标 + (N,) 深度(相机前方为正)。"""
685
+ v = pts3d - self.pos
686
+ z = v @ self.fwd
687
+ x = v @ self.right
688
+ y = v @ self.up
689
+ u = self.f * x / z + self.w / 2
690
+ vv = self.h / 2 - self.f * y / z
691
+ return np.stack([u, vv], -1), z
692
+
693
+
694
+ def fit_skeleton_dist(kpts):
695
+ """按手部空间尺度定相机距离(照工具包 replay.py)。"""
696
+ v = kpts[np.isfinite(kpts).all(axis=-1)]
697
+ if len(v) == 0:
698
+ return 0.5
699
+ r = float(np.abs(v).max())
700
+ return max(0.25, r * 2.6 + 0.1)
701
+
702
+
703
+ def _skeleton_bg(w, h):
704
+ img = np.full((h, w, 3), 12, np.uint8)
705
+ for y in range(h):
706
+ img[y] = np.full(3, 10 + int(10 * y / h), np.uint8)
707
+ return img
708
+
709
+
710
+ def _skeleton_grid(img, cam, r):
711
+ """z=0 平面网格 + 三色坐标轴(照工具包 replay.py 的 _grid)。"""
712
+ x = [(i / 6) * r for i in range(-6, 7)]
713
+ lines = ([([v, -r, 0.0], [v, r, 0.0]) for v in x]
714
+ + [([-r, v, 0.0], [r, v, 0.0]) for v in x])
715
+ for p0, p1 in lines:
716
+ pts, z = cam.project(np.array([p0, p1], np.float32))
717
+ if (z > 0).all():
718
+ cv2.line(img, tuple(pts[0].astype(int)), tuple(pts[1].astype(int)),
719
+ (40, 44, 52), 1, cv2.LINE_AA)
720
+ origin = np.zeros((3,), np.float32)
721
+ for axis, col in [(np.array([r, 0.0, 0.0]), (40, 60, 255)), # X 红
722
+ (np.array([0.0, r, 0.0]), (40, 255, 60)), # Y 绿
723
+ (np.array([0.0, 0.0, r]), (255, 60, 40))]: # Z 蓝
724
+ pts, z = cam.project(np.stack([origin, axis]))
725
+ if (z > 0).all():
726
+ cv2.line(img, tuple(pts[0].astype(int)), tuple(pts[1].astype(int)),
727
+ col, 2, cv2.LINE_AA)
728
+
729
+
730
+ def _draw_skeleton_hand(img, cam, kpts):
731
+ """深度着色的骨骼连线 + 白色关节点(照工具包 replay.py 的 _draw_hand)。"""
732
+ pts, z = cam.project(kpts)
733
+ vis = np.isfinite(kpts).all(axis=1) & (z > 0)
734
+ # 近亮远暗的深度着色;手指比掌骨略亮
735
+ zmin, zmax = 0.05, 0.6
736
+ lum = np.clip((zmax - z) / (zmax - zmin), 0.35, 1.0)
737
+ for a, b, g in SKELETON_BONES:
738
+ if vis[a] and vis[b]:
739
+ k = 0.5 * (lum[a] + lum[b])
740
+ col = tuple(int(c * k) for c in SKELETON_FINGER_BGR[g])
741
+ cv2.line(img, tuple(pts[a].astype(int)), tuple(pts[b].astype(int)),
742
+ col, 3, cv2.LINE_AA)
743
+ for p, ok in zip(pts, vis):
744
+ if ok:
745
+ cv2.circle(img, tuple(p.astype(int)), 2, (255, 255, 255), -1,
746
+ cv2.LINE_AA)
747
+
748
+
749
+ def render_skeleton(kpts, w=640, h=420, dist=None, label=""):
750
+ """单手骨架面板(固定 3/4 视角;dist 传入为帧间防抖后的距离)。
751
+
752
+ Args:
753
+ kpts: (21,3) float 关键点(米)
754
+ dist: 相机距离(None → 按当前帧 fit_skeleton_dist)
755
+ Returns:
756
+ BGR 画布
757
+ """
758
+ if dist is None:
759
+ dist = fit_skeleton_dist(kpts)
760
+ img = _skeleton_bg(w, h)
761
+ cam = SkeletonCamera(yaw_deg=-25.0, elev_deg=15.0, dist=dist,
762
+ img_w=w, img_h=h)
763
+ _skeleton_grid(img, cam, fit_skeleton_dist(kpts) * 0.5)
764
+ _draw_skeleton_hand(img, cam, kpts)
765
+ if label:
766
+ cv2.putText(img, label, (12, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
767
+ (240, 240, 240), 2, cv2.LINE_AA)
768
+ return img
collector/core/s80m_manager.py ADDED
@@ -0,0 +1,560 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ S80M 双目设备管理器 —— 子进程生命周期、stdout 帧管道读取、曝光协议与
3
+ 50→30 抽帧口径(QtCore QObject + 信号,不依赖 QtWidgets)。
4
+
5
+ 自包含双目模块:tools/stereo_s80m/ 内含 read_stereo_rgb.py +
6
+ libfays_vikit.so (3.9.0) + fays_vikit.yaml,与外部 SDK 目录完全独立——
7
+ 外部目录 git 更新、跑官方 demo 均不影响 DAQ 程序。
8
+
9
+ - S80MDeviceManager 每台一个条目:Popen 子进程 / stdin 曝光通道 /
10
+ watchdog / stdout 帧读取线程 / 临时 50fps yaml
11
+ - frame_record_decision 录制 50→30 抽帧纯口径(wall 时钟每 1/30s 桶保留首帧,突发补录)
12
+ - s80m_drop_watch 录制期空桶统计纯口径(wall 时钟,超阈值由 UI 告警)
13
+ - load_default_exposure 「恢复默认」基线(yaml stereo_init_exposure)
14
+
15
+ 帧信号 frame_ready(str, ndarray, object, list) 与深度信号
16
+ depth_ready(str, ndarray, object) 的 hw_ns 必须用 object 封装:PyQt5
17
+ 队列信号把 Python int 按 C++ qint32 封送,超过 2^31 (≈2.1s 纳秒)
18
+ 即静默截断为负数,录制数据受害。
19
+ """
20
+
21
+ from __future__ import annotations
22
+
23
+ import os
24
+ import sys
25
+ import struct
26
+ import shutil
27
+ import tempfile
28
+ import threading
29
+ import subprocess
30
+
31
+ import numpy as np
32
+ import cv2
33
+
34
+ from PyQt5.QtCore import QObject, pyqtSignal, QTimer
35
+
36
+ from config import settings
37
+ from config.i18n import tr
38
+
39
+ # ★ 自包含双目模块:tools/stereo_s80m/(路径锚定仓库根,不随 cwd 漂移)
40
+ SDK_ROOT = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))),
41
+ "tools", "stereo_s80m")
42
+ STEREO_DEMO = os.path.join(SDK_ROOT, "read_stereo_rgb.py")
43
+ STEREO_AVAILABLE = os.path.isfile(STEREO_DEMO)
44
+ STEREO_CFG_25 = os.path.join(SDK_ROOT, "config", "fays_vikit.yaml")
45
+ STEREO_CFG_50 = os.path.join(SDK_ROOT, "config", "fays_vikit_50fps.yaml")
46
+ # 相机档(settings.STEREO_CAM_FPS):50=默认,50→30 桶抽帧才有真 30fps
47
+ # 录制(回调取帧已根治 SDK 交付撕裂,官方 GUI 同款组合);25=回退档
48
+ #(25fps 相机下 30 桶只填到 25 帧,录制内容等效 25fps)
49
+ STEREO_CFG = STEREO_CFG_50 if settings.STEREO_CAM_FPS >= 50 else STEREO_CFG_25
50
+
51
+ # S80C 深度:SDK 深度引擎自包含包(third_party/ 内 vikit 3.9.0 +
52
+ # 深度库 + OpenCV 4.2 + 深度配置/model,与 hand_3d_s80c 共用)
53
+ DEPTH_SDK_ROOT = os.path.join(os.path.dirname(os.path.dirname(
54
+ os.path.abspath(__file__))), "tools", "hand_3d_s80c", "third_party")
55
+
56
+ # 回调取帧桥接(官方 GUI 同款消费方案;shim 在 S80C 自包含包内)。
57
+ # 轮询 GetStereoFrames + 深度引擎绑定叠加会在交付帧内留下水平错位带
58
+ #(8/31 主程序录制回归:25fps 档降缝率但不根治)——回调在 SDK 装配
59
+ # 完成后才触发、帧必完整。settings.STEREO_CB_BRIDGE 关掉即回轮询;
60
+ # shim 缺失/注册失败子进程也自动回退(协议不变)。
61
+ CB_BRIDGE_LIB = os.path.join(DEPTH_SDK_ROOT, "cb_bridge",
62
+ "libfays_cb_bridge.so")
63
+
64
+
65
+ def s80m_depth_available() -> bool:
66
+ """S80C 深度可用性:开关开 + 深度包关键文件齐备。
67
+
68
+ 关闭(settings.S80M_DEPTH_ENABLED=False)→ spawn 不传 --depth-sdk-dir,
69
+ 子进程协议无深度块、零深度 CPU 开销(与旧版逐字节一致)。
70
+ """
71
+ if not settings.S80M_DEPTH_ENABLED:
72
+ return False
73
+ return all(os.path.isfile(p) for p in (
74
+ os.path.join(DEPTH_SDK_ROOT, "lib", "libfays_vikit.so"),
75
+ os.path.join(DEPTH_SDK_ROOT, "lib", "libfayssense_aikit_depth.so"),
76
+ os.path.join(DEPTH_SDK_ROOT, "config", "stereo_depth.yaml"),
77
+ os.path.join(DEPTH_SDK_ROOT, "config", "models",
78
+ "rk3588", "stereo_s_general.rknn"),
79
+ )) and os.path.isdir(os.path.join(DEPTH_SDK_ROOT, "opencv4.2", "lib406"))
80
+
81
+
82
+ def load_default_exposure() -> tuple:
83
+ """「恢复默认」基线:SDK 无曝光读回接口,出厂值即 yaml
84
+ stereo_init_exposure(-1 = 自动曝光,1.0~885.0 = 手动)。"""
85
+ try:
86
+ import yaml as _yaml
87
+ with open(STEREO_CFG, encoding="utf-8") as _f:
88
+ _si = float(_yaml.safe_load(_f).get(
89
+ "stereo_init_exposure", 400.0))
90
+ return (_si < 0, 400.0 if _si < 0 else _si)
91
+ except Exception:
92
+ return (False, 400.0)
93
+
94
+
95
+ def frame_record_decision(entry: dict | None, slot_id: str,
96
+ hardware_ns: int, mono_ns: int,
97
+ imu_samples: list) -> tuple:
98
+ """相机帧 / 30fps 采集抽帧:wall 时钟每 1/30s 桶保留首帧,突发补录。
99
+
100
+ 正常节奏(帧距 20ms)按 wall 桶取每桶首帧:桶号用调用方到达时刻
101
+ mono_ns(time.monotonic_ns,与视频 PTS 同源)而非 hardware_ns——
102
+ 传感器 hw 时钟会跳变(实测 400ms),按 hw 桶会把跳变区间整段判成
103
+ 空桶 → 30fps 抽帧每次稳定缺帧;wall 口径下 hw 跳变不影响抽帧率。
104
+
105
+ 突发补录:主进程卡顿(编码器子进程启动/GIL 争抢)期间帧堆积在
106
+ 队列里,恢复后集中处理时 mono 间隔 <10ms 且落在同一 wall 桶——
107
+ 这些帧的真实时间差在 hw 上仍逐桶推进,按 hw 桶号 ≥1 补录;正常
108
+ 20ms 节奏的同桶第二帧 mono 间隔 ≥10ms,不受影响,输出恒 30fps。
109
+ 补录帧由写线程 30fps 均匀消费(配合深外部队列),视频时间轴延后
110
+ 但不出空洞。
111
+
112
+ 左右目同 ts 天然同步;hw_ns==0 兜底全录(无时间戳可依)。被抽掉帧
113
+ 携带的 IMU 块累积到下一帧(stereo_left 主槽),不丢样本。entry 至少
114
+ 带 last_bucket / pending_imu 键(由 S80MDeviceManager.new_entry 初始化,
115
+ 离线测试假条目同款形状)。返回 (record, imu_batch)。
116
+ """
117
+ record = True
118
+ imu_batch = imu_samples
119
+ if entry is not None and hardware_ns > 0:
120
+ interval_ns = int(settings.STEREO_RECORD_MIN_INTERVAL_S * 1e9)
121
+ wall_b = mono_ns // interval_ns
122
+ hw_b = hardware_ns // interval_ns
123
+ prev = entry["last_bucket"].get(slot_id)
124
+ if prev is not None:
125
+ if wall_b == prev["wall"]:
126
+ # 同 wall 桶:突发补录(mono 间隔 <10ms = 队列积压集中
127
+ # 处理,帧真实间隔看 hw 桶推进);正常节奏同桶第二帧
128
+ # (mono 间隔 ≈20ms)跳过
129
+ record = (mono_ns - prev["mono"] < _BURST_MONO_GAP_NS
130
+ and hw_b >= prev["hw"] + 1)
131
+ else:
132
+ record = True
133
+ if record:
134
+ entry["last_bucket"][slot_id] = {
135
+ "wall": wall_b, "hw": hw_b, "mono": mono_ns}
136
+ if slot_id == "stereo_left":
137
+ if record:
138
+ if entry["pending_imu"]:
139
+ imu_batch = entry["pending_imu"] + (imu_samples or [])
140
+ entry["pending_imu"] = []
141
+ else:
142
+ entry["pending_imu"].extend(imu_samples or [])
143
+ return record, imu_batch
144
+
145
+
146
+ # 突发补录判定:同 wall 桶内 mono 间隔低于此值视为队列积压集中处理
147
+ # (正常 50fps 节奏帧距 20ms;Qt 队列事件连续处理间隔亚毫秒级)
148
+ _BURST_MONO_GAP_NS = 10_000_000
149
+
150
+ # 空桶告警阈值:录制期空桶率超过此值弹一次告警(8/28 健康录制 ~2%;
151
+ # 9/2 三槽齐录实测 ~15%,视频 PTS 与数据时间轴错位 ~1.2s)
152
+ STEREO_DROP_ALERT_RATE = 0.10
153
+
154
+
155
+ def s80m_drop_watch(entry: dict | None, mono_ns: int,
156
+ interval_ns: int) -> tuple:
157
+ """录制期空桶统计(stereo_left 主槽口径,纯函数可离线测)。
158
+
159
+ 用 wall 时钟(time.monotonic_ns,与 data 帧时间戳同源)而非
160
+ hardware_ns:传感器时钟跳变(实测 400ms)按 hw 桶统计会误计
161
+ 为丢桶,wall 口径与"视频 PTS 均匀 30fps、数据时间轴被拉长"
162
+ 的错位症状一致。每帧推进:跨过的桶数 elapsed=⌊gap/interval⌋,
163
+ 跨 k≥2 桶即计 k-1 个空桶;间隔 20ms 的同桶弃帧自然不误计。
164
+ hw_ns==0 兜底全录时帧距 20ms 跨 0 桶,统计恒 0 不告警。
165
+
166
+ entry 需带 drop_watch 键(new_entry 初始化、录制起止由
167
+ device_manager.reset_s80m_record_state 重置)。返回
168
+ (本帧新增空桶, 累计空桶, 累计桶数);entry 为 None 返回 (0, 0, 0)。
169
+ """
170
+ if entry is None:
171
+ return (0, 0, 0)
172
+ w = entry["drop_watch"]
173
+ last = w["last_mono_ns"]
174
+ new_drops = 0
175
+ if last is not None and mono_ns > last:
176
+ elapsed = (mono_ns - last) // interval_ns
177
+ if elapsed >= 2:
178
+ new_drops = elapsed - 1
179
+ w["dropped"] += new_drops
180
+ w["elapsed"] += elapsed
181
+ w["last_mono_ns"] = mono_ns
182
+ return (new_drops, w["dropped"], w["elapsed"])
183
+
184
+
185
+ class S80MDeviceManager(QObject):
186
+ """S80M 双目子进程生命周期管理(每台一个条目)。
187
+
188
+ 信号
189
+ ----
190
+ frame_ready(str, np.ndarray, object, list)
191
+ — (slot_id, frame, hw_ns, imu_samples),reader 线程 → 主线程
192
+ depth_ready(str, np.ndarray, object)
193
+ — (depth_slot, depth_uint16_mm, hw_ns),子进程 SDK 深度引擎输出
194
+ (--depth-sdk-dir 模式;hw_ns 同帧 SDK 硬件纳秒时钟)
195
+ device_closed(str)
196
+ — dev_key,进程/文件/线程清理完毕;UI 侧注销管线源、拆槽、
197
+ 清标定由调用方槽执行(顺序与原 _close_s80m 一致)
198
+ log(str)
199
+ — 跨线程日志
200
+
201
+ reader 线程与 watchdog 不触碰 UI;主线程创建、QObject 亲和性在主线程。
202
+ """
203
+
204
+ frame_ready = pyqtSignal(str, np.ndarray, object, list)
205
+ depth_ready = pyqtSignal(str, np.ndarray, object)
206
+ device_closed = pyqtSignal(str)
207
+ log = pyqtSignal(str)
208
+
209
+ def __init__(self, pipeline, parent=None):
210
+ super().__init__(parent)
211
+ self._pipeline = pipeline
212
+ self._entries: dict = {} # dev_key → 注册表条目(与主窗口共享同一 dict)
213
+ self.shutting_down = False
214
+
215
+ @staticmethod
216
+ def new_entry(label: str) -> dict:
217
+ """注册表条目骨架(kind/slots/label 由主窗口注册,进程字段由 spawn 填充)。"""
218
+ return {
219
+ "kind": "s80m",
220
+ # 深度可用时设备 meta 多记录一路深度槽(spawn 后真实生效)
221
+ "slots": ["stereo_left", "stereo_right"]
222
+ + (["stereo_depth"] if s80m_depth_available() else []),
223
+ "label": label,
224
+ "proc": None,
225
+ "stdin": None, # 曝光命令通道(close 时关闭)
226
+ "stderr_file": None,
227
+ "config_file": None,
228
+ "original_exp": None, # 恢复默认基线(yaml 出厂值)
229
+ "watchdog": None,
230
+ "reader_thread": None,
231
+ "depth_active": False, # spawn 后填充:子进程协议带深度块
232
+ "last_bucket": {}, # 50→30 抽帧:每槽最近保留帧 {wall,hw,mono}(突发补录口径)
233
+ "pending_imu": [], # 被抽掉帧的 IMU 块缓冲(随下一帧落盘)
234
+ "drop_watch": { # 录制期空桶统计(wall 时钟,UI 告警用)
235
+ "last_mono_ns": None,
236
+ "dropped": 0, # 累计空桶数
237
+ "elapsed": 0, # 累计跨过的桶数(= 已填桶 + 空桶)
238
+ "alerted": False, # 本段已弹过一次告警
239
+ },
240
+ }
241
+
242
+ def spawn(self, dev_key: str, entry: dict) -> bool:
243
+ """启动子进程 + watchdog + reader 线程,填充 entry 进程字段。
244
+
245
+ 用户已在 video 组,无需 sudo(sudo 在 no-new-privileges 环境下会
246
+ 直接失败)。stdout 管道传帧数据(--pipe -),避免 FIFO 文件权限
247
+ 问题(AppArmor)。
248
+ """
249
+ if not STEREO_AVAILABLE:
250
+ return False
251
+
252
+ # ★ 相机档 / 30fps 采集:SDK 跑 STEREO_CAM_FPS 档 yaml 副本(tempfile
253
+ # 拷贝,仓库内原文件不动,关闭时清理),录制按 wall 时钟 1/30s 桶抽帧
254
+ cfg_tmp = None
255
+ cfg_args = []
256
+ if os.path.isfile(STEREO_CFG):
257
+ cfg_tmp = tempfile.mktemp(suffix="_s80m_cam.yaml")
258
+ shutil.copyfile(STEREO_CFG, cfg_tmp)
259
+ cfg_args = ["--config", cfg_tmp]
260
+ entry["original_exp"] = load_default_exposure()
261
+
262
+ stereo_stderr = tempfile.mktemp(suffix="_s80m_stderr.log")
263
+ # 深度模式:cmd 追加 --depth-sdk-dir + env 注入 lib406
264
+ # (深度库依赖链用裸 SONAME 必须靠动态链接器搜索路径解析;
265
+ # venv cv2 走自身 rpath 绑自家 OpenCV,不受影响)
266
+ depth_args = []
267
+ spawn_env = None
268
+ if s80m_depth_available():
269
+ depth_args = ["--depth-sdk-dir", DEPTH_SDK_ROOT]
270
+ _lib406 = os.path.join(DEPTH_SDK_ROOT, "opencv4.2", "lib406")
271
+ spawn_env = dict(os.environ)
272
+ spawn_env["LD_LIBRARY_PATH"] = _lib406 + ":" + \
273
+ spawn_env.get("LD_LIBRARY_PATH", "")
274
+ entry["depth_active"] = bool(depth_args)
275
+ # 回调取帧(官方 GUI 同款;开关/shim 齐备才传,子进程另有回退)
276
+ cb_args = (["--cb-bridge"]
277
+ if settings.STEREO_CB_BRIDGE and os.path.isfile(CB_BRIDGE_LIB)
278
+ else [])
279
+ # stdin 管道 = 曝光控制通道(行协议 "SET_EXPOSURE <float>",
280
+ # SDK 运行时生效无需重启;子进程句柄创建完成前命令先积在管道缓冲)
281
+ proc = subprocess.Popen(
282
+ [sys.executable, STEREO_DEMO, "--pipe", "-"] + cfg_args
283
+ + depth_args + cb_args,
284
+ cwd=SDK_ROOT,
285
+ stdin=subprocess.PIPE,
286
+ stdout=subprocess.PIPE,
287
+ stderr=open(stereo_stderr, "w"),
288
+ env=spawn_env,
289
+ )
290
+ entry.update(proc=proc, stdin=proc.stdin,
291
+ stderr_file=stereo_stderr, config_file=cfg_tmp)
292
+ self._entries[dev_key] = entry
293
+
294
+ # 监控子进程退出(定时检查)
295
+ watchdog = QTimer(self)
296
+ watchdog.setInterval(1000)
297
+ watchdog.timeout.connect(
298
+ lambda key=dev_key: self.check_alive(key))
299
+ watchdog.start()
300
+ entry["watchdog"] = watchdog
301
+
302
+ # 启动帧读取线程
303
+ reader = threading.Thread(
304
+ target=self.read_pipe, args=(dev_key,), daemon=True)
305
+ entry["reader_thread"] = reader
306
+ reader.start()
307
+ return True
308
+
309
+ def read_pipe(self, dev_key: str):
310
+ """从子进程 stdout 管道读取双目帧,经 frame_ready 信号发主线程。"""
311
+ entry = self._entries.get(dev_key)
312
+ if not entry or entry["kind"] != "s80m":
313
+ return
314
+ fifo = entry["proc"].stdout # subprocess.PIPE 的读端
315
+ self.log.emit(tr("双目管道已连接,等待帧数据…"))
316
+
317
+ frame_count = 0
318
+ while entry["proc"] and entry["proc"].poll() is None:
319
+ if self.shutting_down:
320
+ break
321
+ try:
322
+ # 帧格式:
323
+ # [4B left_len][8B left_ts_ns][left_jpg]
324
+ # [4B right_len][8B right_ts_ns][right_jpg]
325
+ # [4B imu_count][imu_count × (8B ts_ns + 8B×6 gyro/acc)]
326
+ # [4B depth_len] ← 仅 depth_active
327
+ # >0: [8B depth_ts][4B w][4B h][w*h*2 字节 uint16 毫米]
328
+ # 时间戳均为 SDK 硬件纳秒时钟(帧/IMU 同源,SLAM 对齐用)
329
+ header = fifo.read(4)
330
+ if len(header) < 4:
331
+ break
332
+ left_len = struct.unpack(">I", header)[0]
333
+ # 帧头自检:0 或超 8MB 均属管道流错位(如子进程打印进入
334
+ # 管道),防巨额分配/空帧 imdecode 抛异常;正常 1280×800
335
+ # JPEG 远小于 8MB
336
+ if not (0 < left_len <= 8 * 1024 * 1024):
337
+ self.log.emit(tr(
338
+ "[双目错误] 帧头异常 left_len={},管道流错位", left_len))
339
+ break
340
+ ts_raw = fifo.read(8)
341
+ if len(ts_raw) < 8:
342
+ break
343
+ left_ts = struct.unpack(">Q", ts_raw)[0]
344
+ left_data = fifo.read(left_len)
345
+ if len(left_data) < left_len:
346
+ break
347
+
348
+ header = fifo.read(4)
349
+ if len(header) < 4:
350
+ break
351
+ right_len = struct.unpack(">I", header)[0]
352
+ if not (0 < right_len <= 8 * 1024 * 1024):
353
+ self.log.emit(tr(
354
+ "[双目错误] 帧头异常 right_len={},管道流错位", right_len))
355
+ break
356
+ ts_raw = fifo.read(8)
357
+ if len(ts_raw) < 8:
358
+ break
359
+ right_ts = struct.unpack(">Q", ts_raw)[0]
360
+ right_data = fifo.read(right_len)
361
+ if len(right_data) < right_len:
362
+ break
363
+
364
+ # IMU 样本块
365
+ hdr = fifo.read(4)
366
+ if len(hdr) < 4:
367
+ break
368
+ imu_count = struct.unpack(">I", hdr)[0]
369
+ imu_samples = []
370
+ for _ in range(imu_count):
371
+ rec = fifo.read(56)
372
+ if len(rec) < 56:
373
+ break
374
+ imu_samples.append(struct.unpack(">Q6d", rec))
375
+ if len(imu_samples) < imu_count:
376
+ break
377
+
378
+ # 深度块(子进程 SDK 深度引擎输出;每帧必写保证解析确定性。
379
+ # 尺寸校验防协议错位;校验失败仍已消费 payload 保持同步)
380
+ if entry.get("depth_active"):
381
+ hdr = fifo.read(4)
382
+ if len(hdr) < 4:
383
+ break
384
+ depth_len = struct.unpack(">I", hdr)[0]
385
+ if depth_len > 0:
386
+ if depth_len > 64 * 1024 * 1024: # 防坏流巨额分配
387
+ break
388
+ dh = fifo.read(16) # >QII = 8+4+4
389
+ if len(dh) < 16:
390
+ break
391
+ # unpack_from(而非 unpack(dh):本机 Python 严格版
392
+ # 要求 buffer 恰好 == calcsize,16=16 亦可,但
393
+ # unpack_from 语义更稳)。注意此处必须 16 字节:
394
+ # 写端 struct.pack(">QII")=16;多读 4 字节会把
395
+ # 下一帧帧头吃进本块(实测全流错位→0 帧断连)
396
+ depth_ts, dw, dh2 = struct.unpack_from(">QII", dh)
397
+ payload = fifo.read(depth_len)
398
+ if len(payload) < depth_len:
399
+ break
400
+ if depth_len == dw * dh2 * 2 \
401
+ and 0 < dw <= 4096 and 0 < dh2 <= 4096:
402
+ depth = np.frombuffer(payload, np.uint16) \
403
+ .reshape(dh2, dw)
404
+ self.depth_ready.emit(
405
+ settings.S80M_DEPTH_SLOT, depth, depth_ts)
406
+ else:
407
+ self.log.emit(tr(
408
+ "[双目] 深度块尺寸不符 w={} h={} len={},跳过",
409
+ dw, dh2, depth_len))
410
+
411
+ # JPEG 解码(cv2 5.0.0 起空/坏缓冲直接抛 cv2.error 而非
412
+ # 返回 None;异常按流损坏处理,交由下方异常出口收尾)
413
+ try:
414
+ left = cv2.imdecode(np.frombuffer(left_data, np.uint8),
415
+ cv2.IMREAD_COLOR)
416
+ right = cv2.imdecode(np.frombuffer(right_data, np.uint8),
417
+ cv2.IMREAD_COLOR)
418
+ except cv2.error as e:
419
+ self.log.emit(tr("[双目错误] JPEG 解码失败: {}", e))
420
+ break
421
+ if left is None or right is None:
422
+ continue
423
+
424
+ # ★ S80M 子进程直接输出左右两个完整单目画面 (1280×800)
425
+ # ★ 镜头方向已由 SDK config (left_cam_rotate_180 /
426
+ # right_cam_rotate_180 / stereo_swap_lr) 处理,此处不再翻转
427
+ # ★ 显示翻转由主线程 _on_stereo_frame 统一执行(与单目一致)
428
+ self.frame_ready.emit("stereo_left", left, left_ts, imu_samples)
429
+ self.frame_ready.emit("stereo_right", right, right_ts, imu_samples)
430
+
431
+ frame_count += 1
432
+ if frame_count == 1:
433
+ self.log.emit(tr("双目摄像机画面已开始传输"))
434
+
435
+ except BrokenPipeError:
436
+ if not self.shutting_down:
437
+ self.log.emit(tr("[双目] 管道断开"))
438
+ break
439
+ except Exception as e:
440
+ if not self.shutting_down:
441
+ self.log.emit(tr("[双目错误] 读取帧异常: {}", e))
442
+ break
443
+
444
+ if not self.shutting_down:
445
+ # 异常退出(流错位/解码失败/EOF)时确保子进程终止:SIGTERM
446
+ # 走脚本内优雅退出(finally → _fn_destroy 释放 SDK),避免
447
+ # 半死子进程占着相机不死。正常 close() 路径已 terminate,
448
+ # poll() 非 None 不会误杀。
449
+ proc = entry.get("proc")
450
+ if proc and proc.poll() is None:
451
+ try:
452
+ proc.terminate()
453
+ proc.wait(timeout=3)
454
+ except Exception:
455
+ try:
456
+ proc.kill()
457
+ except Exception:
458
+ pass
459
+ self.log.emit(tr("双目摄像机已断开 (共 {} 帧)", frame_count))
460
+
461
+ def check_alive(self, dev_key: str):
462
+ """定时检查双目子进程是否仍在运行(watchdog 槽)。"""
463
+ entry = self._entries.get(dev_key)
464
+ if not entry or entry["kind"] != "s80m":
465
+ return
466
+ ret = entry["proc"].poll()
467
+ if ret is not None:
468
+ entry["watchdog"].stop()
469
+ self.log.emit(tr("[双目] 子进程已退出,退出码: {}", ret))
470
+ # 读取 stderr 末尾(最后 500 字符)
471
+ err_file = entry["stderr_file"]
472
+ if err_file and os.path.isfile(err_file):
473
+ try:
474
+ with open(err_file, "r") as f:
475
+ f.seek(0, os.SEEK_END)
476
+ size = f.tell()
477
+ f.seek(max(0, size - 500))
478
+ tail = f.read()
479
+ if tail.strip():
480
+ self.log.emit(tr("[双目 stderr] {}", tail.strip()))
481
+ except Exception:
482
+ pass
483
+
484
+ def close(self, dev_key: str):
485
+ """停止 S80M 子进程并清理进程侧资源,完毕 emit device_closed。
486
+
487
+ (主窗口注册表条目由调用方弹出;管线注销/拆槽/清标定在
488
+ device_closed 槽中执行,整体顺序与原 _close_s80m 一致。)
489
+ """
490
+ entry = self._entries.pop(dev_key, None)
491
+ if not entry or entry["kind"] != "s80m":
492
+ return
493
+ # 1. 停止 watchdog 定时器
494
+ wd = entry.get("watchdog")
495
+ if wd:
496
+ wd.stop()
497
+ wd.deleteLater()
498
+
499
+ # 2. 先关曝光命令通道(子进程 stdin 线程随 EOF 退出),再终止子进程
500
+ stdin = entry.get("stdin")
501
+ if stdin:
502
+ try:
503
+ stdin.close()
504
+ except Exception:
505
+ pass
506
+ proc = entry["proc"]
507
+ try:
508
+ proc.terminate()
509
+ proc.wait(timeout=3)
510
+ except Exception:
511
+ try:
512
+ proc.kill()
513
+ proc.wait(timeout=2)
514
+ except Exception:
515
+ pass
516
+ # 关闭 stdout 管道 → 唤醒 reader 线程的阻塞 read()
517
+ if proc.stdout:
518
+ try:
519
+ proc.stdout.close()
520
+ except Exception:
521
+ pass
522
+
523
+ # 3. 等待 reader 线程退出(管道已关闭,不再阻塞)
524
+ reader = entry.get("reader_thread")
525
+ if reader and reader.is_alive():
526
+ reader.join(timeout=3.0)
527
+
528
+ # 4. 清理 stderr 日志文件 + 50fps yaml 临时副本
529
+ err_file = entry.get("stderr_file")
530
+ if err_file and os.path.isfile(err_file):
531
+ try:
532
+ os.unlink(err_file)
533
+ except Exception:
534
+ pass
535
+ cfg_tmp = entry.get("config_file")
536
+ if cfg_tmp and os.path.isfile(cfg_tmp):
537
+ try:
538
+ os.unlink(cfg_tmp)
539
+ except Exception:
540
+ pass
541
+
542
+ self.device_closed.emit(dev_key)
543
+
544
+ def send_exposure(self, entry: dict, auto: bool, value: float):
545
+ """向 S80M 子进程下发曝光(stdin 行协议;-1.0 = 自动曝光)。
546
+
547
+ entry 由调用方按注册表取到(主窗口注册表与离线测试假条目同款
548
+ 形状),行协议口径集中于此。
549
+ """
550
+ if not entry:
551
+ return
552
+ stdin = entry.get("stdin")
553
+ if stdin is None:
554
+ return
555
+ v = -1.0 if auto else float(value)
556
+ try:
557
+ stdin.write(f"SET_EXPOSURE {v}\n".encode())
558
+ stdin.flush()
559
+ except (BrokenPipeError, OSError):
560
+ self.log.emit(tr("[双目] 曝光下发失败(子进程已退出)"))
collector/core/sensor_config_dialogs.py ADDED
@@ -0,0 +1,238 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 传感器配置对话框 —— PyQt5 实现的矩阵选择和手掌配置界面。
3
+ """
4
+
5
+ import json
6
+ import os
7
+ from PyQt5.QtCore import Qt
8
+ from PyQt5.QtWidgets import (
9
+ QDialog, QVBoxLayout, QHBoxLayout, QLabel, QPushButton,
10
+ QGroupBox, QRadioButton, QScrollArea, QWidget, QButtonGroup,
11
+ QDialogButtonBox, QGridLayout,
12
+ )
13
+
14
+
15
+ # ═══════════════════════════════════════════════════════
16
+ # 通用矩阵行列选择对话框
17
+ # ═══════════════════════════════════════════════════════
18
+
19
+ class MatrixConfigDialog(QDialog):
20
+ """通用矩阵行列配置 —— 按钮点击选择行/列序列。"""
21
+
22
+ def __init__(self, parent, title: str, config: dict):
23
+ super().__init__(parent)
24
+ self.setWindowTitle(title)
25
+ self.setMinimumSize(600, 480)
26
+
27
+ self._rows = list(config.get("rows", []))
28
+ self._cols = list(config.get("cols", []))
29
+ self._order = config.get("axis_order", "row_col")
30
+
31
+ self._setup_ui()
32
+
33
+ def _setup_ui(self):
34
+ layout = QVBoxLayout(self)
35
+
36
+ # 映射方向
37
+ order_group = QGroupBox("映射渲染方向 (Axis Mapping)")
38
+ order_layout = QHBoxLayout(order_group)
39
+ bg = QButtonGroup(self)
40
+ self._rb_rowcol = QRadioButton("先行后列 (横向列, 纵向行)")
41
+ self._rb_colrow = QRadioButton("先列后行 (横向行, 纵向列)")
42
+ bg.addButton(self._rb_rowcol)
43
+ bg.addButton(self._rb_colrow)
44
+ if self._order == "row_col":
45
+ self._rb_rowcol.setChecked(True)
46
+ else:
47
+ self._rb_colrow.setChecked(True)
48
+ order_layout.addWidget(self._rb_rowcol)
49
+ order_layout.addWidget(self._rb_colrow)
50
+ layout.addWidget(order_group)
51
+
52
+ # 行列选择
53
+ lists_layout = QHBoxLayout()
54
+
55
+ # 行选择
56
+ row_group = QGroupBox("行 (Rows 00-15)")
57
+ row_vbox = QVBoxLayout(row_group)
58
+ self._row_label = QLabel()
59
+ self._row_label.setStyleSheet("color:#1565C0; font-weight:bold;")
60
+ self._row_label.setWordWrap(True)
61
+ row_vbox.addWidget(self._row_label)
62
+
63
+ row_grid = QGridLayout()
64
+ for i in range(16):
65
+ btn = QPushButton(f"{i:02d}")
66
+ btn.setFixedSize(36, 28)
67
+ btn.clicked.connect(lambda checked, x=i: self._add_row(x))
68
+ row_grid.addWidget(btn, i // 4, i % 4)
69
+ row_vbox.addLayout(row_grid)
70
+
71
+ row_btn_layout = QHBoxLayout()
72
+ row_btn_layout.addWidget(QPushButton("撤销", clicked=self._undo_row))
73
+ row_btn_layout.addWidget(QPushButton("清空", clicked=self._clear_row))
74
+ row_vbox.addLayout(row_btn_layout)
75
+ lists_layout.addWidget(row_group)
76
+
77
+ # 列选择
78
+ col_group = QGroupBox("列 (Cols 00-15)")
79
+ col_vbox = QVBoxLayout(col_group)
80
+ self._col_label = QLabel()
81
+ self._col_label.setStyleSheet("color:#1565C0; font-weight:bold;")
82
+ self._col_label.setWordWrap(True)
83
+ col_vbox.addWidget(self._col_label)
84
+
85
+ col_grid = QGridLayout()
86
+ for i in range(16):
87
+ btn = QPushButton(f"{i:02d}")
88
+ btn.setFixedSize(36, 28)
89
+ btn.clicked.connect(lambda checked, x=i: self._add_col(x))
90
+ col_grid.addWidget(btn, i // 4, i % 4)
91
+ col_vbox.addLayout(col_grid)
92
+
93
+ col_btn_layout = QHBoxLayout()
94
+ col_btn_layout.addWidget(QPushButton("撤销", clicked=self._undo_col))
95
+ col_btn_layout.addWidget(QPushButton("清空", clicked=self._clear_col))
96
+ col_vbox.addLayout(col_btn_layout)
97
+ lists_layout.addWidget(col_group)
98
+
99
+ layout.addLayout(lists_layout)
100
+
101
+ # 确认按钮
102
+ btns = QDialogButtonBox(
103
+ QDialogButtonBox.Ok | QDialogButtonBox.Cancel
104
+ )
105
+ btns.accepted.connect(self.accept)
106
+ btns.rejected.connect(self.reject)
107
+ layout.addWidget(btns)
108
+
109
+ self._update_labels()
110
+
111
+ def _add_row(self, val):
112
+ if val not in self._rows:
113
+ self._rows.append(val)
114
+ self._update_labels()
115
+
116
+ def _undo_row(self):
117
+ if self._rows:
118
+ self._rows.pop()
119
+ self._update_labels()
120
+
121
+ def _clear_row(self):
122
+ self._rows.clear()
123
+ self._update_labels()
124
+
125
+ def _add_col(self, val):
126
+ if val not in self._cols:
127
+ self._cols.append(val)
128
+ self._update_labels()
129
+
130
+ def _undo_col(self):
131
+ if self._cols:
132
+ self._cols.pop()
133
+ self._update_labels()
134
+
135
+ def _clear_col(self):
136
+ self._cols.clear()
137
+ self._update_labels()
138
+
139
+ def _update_labels(self):
140
+ r_str = " → ".join(map(str, self._rows)) if self._rows else "(无)"
141
+ c_str = " → ".join(map(str, self._cols)) if self._cols else "(无)"
142
+ self._row_label.setText(f"已��行:\n{r_str}")
143
+ self._col_label.setText(f"已选列:\n{c_str}")
144
+
145
+ def get_config(self) -> dict:
146
+ order = "row_col" if self._rb_rowcol.isChecked() else "col_row"
147
+ return {
148
+ "rows": list(self._rows),
149
+ "cols": list(self._cols),
150
+ "axis_order": order,
151
+ }
152
+
153
+
154
+ # ═══════════════════════════════════════════════════════
155
+ # 仿生手掌配置对话框
156
+ # ═══════════════════════════════════════════════════════
157
+
158
+ class HandConfigDialog(QDialog):
159
+ """仿生手掌 —— 各手指区域的矩阵配置。"""
160
+
161
+ def __init__(self, parent, config: dict):
162
+ super().__init__(parent)
163
+ self.setWindowTitle("仿生手掌映射配置")
164
+ self.setMinimumSize(650, 600)
165
+ self._config = {k: dict(v) for k, v in config.items()}
166
+ self._labels = {}
167
+ self._setup_ui()
168
+
169
+ def _setup_ui(self):
170
+ layout = QVBoxLayout(self)
171
+ layout.addWidget(QLabel("🦾 仿生手掌模式 - 矩阵配置"))
172
+ layout.addWidget(QLabel('点击"选择"为每个部位配置传感器行列映射'))
173
+
174
+ scroll = QScrollArea()
175
+ scroll.setWidgetResizable(True)
176
+ container = QWidget()
177
+ container_layout = QVBoxLayout(container)
178
+
179
+ for key, item in self._config.items():
180
+ name = item.get("name", key)
181
+ row_widget = QWidget()
182
+ row_layout = QHBoxLayout(row_widget)
183
+ row_layout.setContentsMargins(0, 2, 0, 2)
184
+
185
+ row_layout.addWidget(QLabel(name))
186
+
187
+ label = QLabel(self._format_cfg(item))
188
+ label.setStyleSheet("color:#1565C0;")
189
+ label.setMinimumWidth(300)
190
+ self._labels[key] = label
191
+ row_layout.addWidget(label, 1)
192
+
193
+ btn = QPushButton("选择")
194
+ btn.clicked.connect(
195
+ lambda checked, k=key: self._open_sub_config(k)
196
+ )
197
+ row_layout.addWidget(btn)
198
+
199
+ container_layout.addWidget(row_widget)
200
+
201
+ scroll.setWidget(container)
202
+ layout.addWidget(scroll, 1)
203
+
204
+ btns = QDialogButtonBox(
205
+ QDialogButtonBox.Ok | QDialogButtonBox.Cancel
206
+ )
207
+ btns.accepted.connect(self.accept)
208
+ btns.rejected.connect(self.reject)
209
+ layout.addWidget(btns)
210
+
211
+ def _format_cfg(self, item):
212
+ rows = item.get("rows", [])
213
+ cols = item.get("cols", [])
214
+ order = item.get("axis_order", "row_col")
215
+ r_str = ",".join(map(str, rows)) if rows else "无"
216
+ c_str = ",".join(map(str, cols)) if cols else "无"
217
+ dir_str = "先行后列" if order == "row_col" else "先列后行"
218
+ return f"[{dir_str}] 行:[{r_str}] 列:[{c_str}]"
219
+
220
+ def _open_sub_config(self, key):
221
+ item = self._config[key]
222
+ sub_cfg = {
223
+ "rows": list(item.get("rows", [])),
224
+ "cols": list(item.get("cols", [])),
225
+ "axis_order": item.get("axis_order", "row_col"),
226
+ }
227
+ dlg = MatrixConfigDialog(
228
+ self, f"配置 - {item.get('name', key)}", sub_cfg
229
+ )
230
+ if dlg.exec_() == dlg.Accepted:
231
+ new_cfg = dlg.get_config()
232
+ item["rows"] = new_cfg["rows"]
233
+ item["cols"] = new_cfg["cols"]
234
+ item["axis_order"] = new_cfg["axis_order"]
235
+ self._labels[key].setText(self._format_cfg(item))
236
+
237
+ def get_config(self) -> dict:
238
+ return self._config
collector/core/sensor_hand_config.py ADDED
@@ -0,0 +1,44 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 传感器(BLE 手套)仿生手掌配置与有效性过滤 —— 零 Qt 依赖,纯函数。
3
+
4
+ 供回放对话框与手套面板共用同一套口径:
5
+ - load_sensor_hand_config 按传感器角色加载仿生手掌映射(左/右手套不同)
6
+ - valid_sensor_names 只保留时间线中确实存在 16×16 压力矩阵的传感器列
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ from core.render_engine import (
12
+ DEFAULT_HAND, _load_json, CONFIG_FILE, CONFIG_FILE_LEFT,
13
+ )
14
+
15
+
16
+ def load_sensor_hand_config(sensor_name: str) -> dict:
17
+ """按传感器角色加载仿生手掌映射配置(左/右手套不同)。"""
18
+ config_file = (CONFIG_FILE_LEFT
19
+ if sensor_name == "left_glove" else CONFIG_FILE)
20
+ cfg = {k: dict(v) for k, v in DEFAULT_HAND.items()}
21
+ for k, v in _load_json(config_file, {}).items():
22
+ if k in cfg:
23
+ cfg[k].update(v)
24
+ else:
25
+ cfg[k] = v # 左传感器配置文件有 DEFAULT_HAND 之外的部位
26
+ return cfg
27
+
28
+
29
+ def valid_sensor_names(timeline, names: list) -> list:
30
+ """只保留时间线中确实存在 16×16 压力矩阵(256 宽)的传感器列。
31
+
32
+ 无 sensors 键的会话会从 features 推断(observation.imu 等
33
+ 非手套特征会被误判成传感器),按实际列宽过滤掉,避免出现
34
+ 永远"无信号"的幽灵传感器格。
35
+ """
36
+ if timeline is None:
37
+ return []
38
+ out = []
39
+ for n in names:
40
+ data = timeline.obs.get(f"observation.{n}")
41
+ if (data is not None and getattr(data, "ndim", 0) == 2
42
+ and data.shape[1] == 256):
43
+ out.append(n)
44
+ return out
collector/core/session_catalog.py ADDED
@@ -0,0 +1,127 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 录制数据目录 —— 扫描、元数据读取与主时钟帧率解析(零 Qt 依赖,纯函数)。
3
+
4
+ 供回放对话框与录制历史刷新共用:
5
+ - get_effective_fps 任务元数据 → 回放主时钟帧率
6
+ - list_sessions 扫描录制根目录,返回全部 episode(池化布局)
7
+ - load_session_meta 任务元数据读取(格式探测 / info / fps / 传感器列名)
8
+
9
+ v1.1.0 起读侧只认任务级池化布局(meta/info.json format=="pooled_episodes_v1"),
10
+ 键为 (task_dir, episode_index);旧格式会话目录不再列出。
11
+ """
12
+
13
+ from __future__ import annotations
14
+
15
+ import os
16
+ import json
17
+
18
+ from config import settings
19
+ from core.helpers import (
20
+ episode_refs, detect_session_format,
21
+ pooled_info_path, episode_row, episode_video_files,
22
+ )
23
+
24
+
25
+ def get_effective_fps(info: dict) -> float:
26
+ """从任务元数据中提取回放主时钟帧率(每路摄像机帧率的最大值)。
27
+
28
+ 主时钟取最高帧率保证播放平滑;低帧率路按 _cam_fps 逐路独立
29
+ seek 按比例抽帧。兼容旧数据:无 per-camera fps 时通过摄像机
30
+ 命名检测双目模式。
31
+ """
32
+ cameras = info.get("cameras", {})
33
+ cam_fps_vals = []
34
+ for cam_name, cam_info in cameras.items():
35
+ if isinstance(cam_info, dict):
36
+ f = cam_info.get("fps")
37
+ if f and f > 0:
38
+ cam_fps_vals.append(float(f))
39
+
40
+ if cam_fps_vals:
41
+ # 有 per-camera fps:取最大值作为主时钟
42
+ return max(cam_fps_vals)
43
+
44
+ # 兼容旧数据:无 per-camera fps 时,按摄像机名称推断
45
+ cam_names = [k.lower() for k in cameras.keys()]
46
+ is_stereo = any("stereo" in cn for cn in cam_names)
47
+ if is_stereo:
48
+ return float(settings.STEREO_FPS)
49
+
50
+ global_fps = info.get("fps", 30)
51
+ return float(global_fps) if global_fps > 0 else 30.0
52
+
53
+
54
+ def load_session_meta(task_dir: str, episode_index: int = 0) -> dict:
55
+ """同步读取任务元数据(极快,主线程安全)。
56
+
57
+ 返回 {"fmt", "info", "fps", "sensor_names", "episode_index"}:
58
+ - fmt "pooled"(池化布局)或旧格式名
59
+ - episode_index 传入的 episode 序号(0 表示未指定,仅任务级元数据)
60
+ - sensor_names 从 info["sensors"] 读;旧格式从 features 推断;
61
+ 最旧格式回退 ["state"]
62
+ """
63
+ fmt = detect_session_format(task_dir)
64
+
65
+ if fmt == "pooled":
66
+ mp = pooled_info_path(task_dir)
67
+ else:
68
+ mp = os.path.join(task_dir, "meta", "info.json")
69
+ info = {}
70
+ if os.path.isfile(mp):
71
+ with open(mp, "r", encoding="utf-8") as f:
72
+ info = json.load(f)
73
+
74
+ sensor_names = info.get("sensors", [])
75
+ if not sensor_names:
76
+ # 兼容旧格式:尝试从 features 键推断
77
+ features = info.get("features", {})
78
+ sensor_names = [
79
+ k.replace("observation.", "") for k in features
80
+ if k.startswith("observation.")
81
+ ]
82
+ if not sensor_names:
83
+ sensor_names = ["state"] # 最旧格式回退
84
+
85
+ return {
86
+ "fmt": fmt,
87
+ "info": info,
88
+ "fps": get_effective_fps(info),
89
+ "sensor_names": sensor_names,
90
+ "episode_index": episode_index,
91
+ }
92
+
93
+
94
+ def list_sessions(directory: str) -> list:
95
+ """扫描录制根目录,返回全部 episode(按名称倒序)。
96
+
97
+ 每项 {"name", "path", "tag", "episode_index", "info", "duration", "fps"}:
98
+ - path = 任务目录(task_dir)
99
+ - episode_index = episode 序号(池化文件组的 N)
100
+ - duration = episodes 行的 duration_sec(秒)
101
+ """
102
+ sessions = []
103
+ for ref in episode_refs(directory):
104
+ task_dir = ref["task_dir"]
105
+ episode_index = ref["episode_index"]
106
+ meta = load_session_meta(task_dir, episode_index)
107
+ row = episode_row(task_dir, episode_index)
108
+ sessions.append({
109
+ "name": ref["name"], "path": task_dir, "tag": ref["task"],
110
+ "episode_index": episode_index,
111
+ "info": meta["info"],
112
+ "duration": row.get("duration_sec", 0) or 0,
113
+ "fps": meta["fps"],
114
+ "cams": len(episode_video_files(task_dir, episode_index)),
115
+ })
116
+ return sessions
117
+
118
+
119
+ def list_recordings(base_dir: str) -> list[dict]:
120
+ """扫描录制根目录,返回全部 episode(上传对话框口径)。
121
+
122
+ 每项 {"name", "path", "tag", "episode_index"}(轻量版 list_sessions,
123
+ 不读元数据)。
124
+ """
125
+ return [{"name": r["name"], "path": r["task_dir"], "tag": r["task"],
126
+ "episode_index": r["episode_index"]}
127
+ for r in episode_refs(base_dir)]
collector/core/session_loader.py ADDED
@@ -0,0 +1,112 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ 会话后台加载器 —— 会话时间线与手部关键点的 parquet 读取全部在 worker 线程。
3
+
4
+ 与 UI 无关(QtCore QObject + 信号,不依赖 QtWidgets):
5
+ - SessionLoader 后台加载(gen 防过期协议由调用方维护)
6
+ - 可选手部关键点经 core.hand_tracking 惰性导入
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ import threading
12
+
13
+ from PyQt5.QtCore import QObject, pyqtSignal
14
+
15
+ from core.session_timeline import load_timeline, SensorTimeline
16
+
17
+ # 手部关键点(可选模块;core.hand_tracking 引入 ultralytics/torch 等重依赖)
18
+ try:
19
+ from core.hand_tracking import load_hand_kpts as _load_hand_kpts
20
+ from core.hand_tracking import load_hand_kpts_pooled as _load_hand_kpts_pooled
21
+ _HAND_KPTS_AVAILABLE = True
22
+ except ImportError:
23
+ _load_hand_kpts = None
24
+ _load_hand_kpts_pooled = None
25
+ _HAND_KPTS_AVAILABLE = False
26
+
27
+
28
+ class SessionLoader(QObject):
29
+ """后台加载会话时间线与手部关键点(parquet 读取全部在 worker 线程)。
30
+
31
+ 信号
32
+ ----
33
+ finished(int, object) — (gen, {"timeline": SensorTimeline, "hand_kpts": dict})
34
+ payload 按引用传递,worker emit 后不再触碰
35
+ failed(int, str) — (gen, error)
36
+
37
+ VideoCapture 不在 worker 中创建/读取(OpenCV FFmpeg 后端不承诺跨线程
38
+ 安全),视频打开与播放全部留在调用方主线程。
39
+ """
40
+
41
+ finished = pyqtSignal(int, object)
42
+ failed = pyqtSignal(int, str)
43
+
44
+ def __init__(self, parent=None):
45
+ super().__init__(parent)
46
+ self._running = False
47
+ self._pending = None # 排队的下一次加载请求(最新覆盖旧的)
48
+ self._lock = threading.Lock()
49
+ self._thread: threading.Thread = None
50
+
51
+ def start(self, gen: int, session_dir: str, sensor_names: list[str],
52
+ load_kpts: bool = False, load_timeline: bool = True,
53
+ episode_index: int = 0):
54
+ """启动后台加载。
55
+
56
+ 已有加载在跑时把请求排队(新请求覆盖旧请求——过期结果本就会被
57
+ gen 丢弃,只跑最新一次即可)。
58
+
59
+ load_timeline=False → kpts-only 模式(手部处理完成后重载关键点)。
60
+ episode_index > 0 时按池化布局读本 episode 的文件组。
61
+ """
62
+ with self._lock:
63
+ if self._running:
64
+ self._pending = (gen, session_dir, sensor_names,
65
+ load_kpts, load_timeline, episode_index)
66
+ return
67
+ self._running = True
68
+ self._spawn(gen, session_dir, sensor_names, load_kpts, load_timeline,
69
+ episode_index)
70
+
71
+ def is_running(self) -> bool:
72
+ return self._running
73
+
74
+ def _spawn(self, gen, session_dir, sensor_names, load_kpts, load_timeline,
75
+ episode_index):
76
+ self._thread = threading.Thread(
77
+ target=self._run,
78
+ args=(gen, session_dir, sensor_names, load_kpts, load_timeline,
79
+ episode_index),
80
+ daemon=True, name="session-loader",
81
+ )
82
+ self._thread.start()
83
+
84
+ # ── 后台 ──────────────────────────────────────────
85
+
86
+ def _run(self, gen: int, session_dir: str, sensor_names: list[str],
87
+ load_kpts: bool, want_timeline: bool, episode_index: int):
88
+ try:
89
+ timeline = None
90
+ if want_timeline:
91
+ timeline = load_timeline(session_dir, sensor_names,
92
+ episode_index)
93
+ hand_kpts = {}
94
+ if load_kpts and _HAND_KPTS_AVAILABLE:
95
+ if episode_index > 0 and _load_hand_kpts_pooled:
96
+ # 池化布局:镜像目录 keypoints_output/<task>/episode_NNNNNN/
97
+ hand_kpts = (_load_hand_kpts_pooled(
98
+ session_dir, episode_index) or {})
99
+ elif _load_hand_kpts:
100
+ hand_kpts = _load_hand_kpts(session_dir) or {}
101
+ self.finished.emit(gen, {"timeline": timeline, "hand_kpts": hand_kpts})
102
+ except Exception as e:
103
+ self.failed.emit(gen, str(e)[:300])
104
+ finally:
105
+ with self._lock:
106
+ self._running = False
107
+ pending = self._pending
108
+ self._pending = None
109
+ if pending is not None:
110
+ self._running = True
111
+ if pending is not None:
112
+ self._spawn(*pending)