Spaces:
Running
Running
stouchinglhh commited on
Commit ·
af37f5b
1
Parent(s): 95f5290
collector: 更新采集工程至 v1.3.0(UMI 夹爪全链接入:Fays S80M 双目 SLAM 位姿/轨迹 + Sightac 左右触觉与 250×250 力矩阵 + DECXIN RGB;双臂双夹爪同录,rig1 旧契约不变 rig2 加前缀;两 rig 独立 CPU 亲和分区;SLAM 位姿坐标约定修正 X=右/Y=正对/Z=上;左目显示稳 15fps 且 raw 流断线自动重连;轨迹并入 episode parquet 列)
Browse filesThis view is limited to 50 files because it contains too many changes. See raw diff
- collector/.gitignore +7 -0
- collector/README.md +36 -1
- collector/config/__init__.py +1 -1
- collector/config/i18n.py +27 -0
- collector/config/settings.py +19 -0
- collector/core/api_client.py +64 -5
- collector/core/device_detector.py +176 -29
- collector/core/device_manager.py +16 -3
- collector/core/egodata_writer.py +71 -1
- collector/core/gripper/__init__.py +15 -0
- collector/core/gripper/affinity.py +118 -0
- collector/core/gripper/bridge.py +811 -0
- collector/core/gripper/control/__init__.py +19 -0
- collector/core/gripper/control/grip_state.py +233 -0
- collector/core/gripper/devices/__init__.py +37 -0
- collector/core/gripper/devices/gripper_connection.py +717 -0
- collector/core/gripper/devices/gripper_serial.py +181 -0
- collector/core/gripper/devices/relative_usb_topology.py +455 -0
- collector/core/gripper/devices/serial_command_worker.py +294 -0
- collector/core/gripper/devices/tactile_discovery.py +1707 -0
- collector/core/gripper/devices/tactile_process_manager.py +0 -0
- collector/core/gripper/devices/usb_camera_ipc.py +255 -0
- collector/core/gripper/devices/usb_camera_sets.py +81 -0
- collector/core/gripper/devices/uvc_camera_service.py +782 -0
- collector/core/gripper/fays_runtime.py +1638 -0
- collector/core/gripper/fays_serial_probe.py +246 -0
- collector/core/gripper/fays_single.py +334 -0
- collector/core/gripper/paths.py +164 -0
- collector/core/gripper/recording/__init__.py +7 -0
- collector/core/gripper/recording/fays_raw_client.py +314 -0
- collector/core/gripper/runtime/__init__.py +7 -0
- collector/core/gripper/runtime/cpu_policy.py +777 -0
- collector/core/gripper/runtime/device_access.py +66 -0
- collector/core/gripper/runtime/interruptible_queue.py +75 -0
- collector/core/gripper/runtime/thread_affinity.py +677 -0
- collector/core/gripper/sightac_sdk/API_VERSIONS.md +27 -0
- collector/core/gripper/sightac_sdk/api_new/__init__.py +3 -0
- collector/core/gripper/sightac_sdk/api_new/libSonixCamera.so +3 -0
- collector/core/gripper/sightac_sdk/api_new/parameters.yaml +80 -0
- collector/core/gripper/sightac_sdk/api_new/platform_utils.py +3 -0
- collector/core/gripper/sightac_sdk/api_new/sensor_function.py +0 -0
- collector/core/gripper/sightac_sdk/api_new/sensor_gripper.py +3 -0
- collector/core/gripper/sightac_sdk/api_new/touch_sensor.py +0 -0
- collector/core/gripper/sightac_sdk/api_new/touch_sensor_base.py +0 -0
- collector/core/gripper/sightac_sdk/api_new/xu_camera.py +0 -0
- collector/core/gripper/sightac_sdk/pyarmor_runtime_000000/__init__.py +2 -0
- collector/core/gripper/sightac_sdk/pyarmor_runtime_000000/pyarmor_runtime.so +3 -0
- collector/core/gripper/slam/__init__.py +12 -0
- collector/core/gripper/slam/process_controller.py +1157 -0
- collector/core/gripper/slam/protocol.py +440 -0
collector/.gitignore
CHANGED
|
@@ -51,6 +51,13 @@ 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
|
|
|
|
| 51 |
|
| 52 |
# 手套工具包(第三方 SDK 本地参考,永远不上传)
|
| 53 |
stouch_glove_toolkit-Stouch_Glove_Toolkit-beta/
|
| 54 |
+
|
| 55 |
+
# 夹爪上位机程序及原生资源(同手套工具包策略,整个目录不上传)
|
| 56 |
+
online/
|
| 57 |
+
# 夹爪随包交付的原生资源(从 online/ 镜像而来:厂商 SDK/二进制/ORBvoc.txt 共 785MB,
|
| 58 |
+
# ORBvoc.txt 单文件 138MB 超 GitHub 100MB 限制,且为第三方二进制,不入库)
|
| 59 |
+
# 触觉 Sightac SDK 例外:core/gripper/sightac_sdk/ 已 pyarmor 加密,随包入库
|
| 60 |
+
core/gripper/native/
|
| 61 |
lite_package/*
|
| 62 |
!lite_package/start_lite.bat
|
| 63 |
!lite_package/start_lite.sh
|
collector/README.md
CHANGED
|
@@ -1,6 +1,6 @@
|
|
| 1 |
# collector — Multimodal Data Acquisition SDK · 多模态数据采集 SDK
|
| 2 |
|
| 3 |
-

|
| 5 |

|
| 6 |
|
|
@@ -361,6 +361,27 @@ 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
|
|
@@ -699,6 +720,20 @@ i18n 文案经 `tr()` 翻译、PyQt5 信号参数用 `object` 封送大整数、
|
|
| 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 摄像头
|
|
|
|
| 1 |
# collector — Multimodal Data Acquisition SDK · 多模态数据采集 SDK
|
| 2 |
|
| 3 |
+

|
| 4 |

|
| 5 |

|
| 6 |
|
|
|
|
| 361 |
|
| 362 |
### Changelog
|
| 363 |
|
| 364 |
+
- **v1.3.0** — UMI gripper support (Fays S80M stereo SLAM pose/trajectory
|
| 365 |
+
+ Sightac left/right tactile force with 250×250 force matrix + DECXIN
|
| 366 |
+
RGB; gripper group in the device panel, hidden when native resources
|
| 367 |
+
are absent), dual-gripper recording (rig1 keeps the legacy slot/column
|
| 368 |
+
contract, rig2 is prefixed `gripper_2_`; per-rig CPU affinity
|
| 369 |
+
partitions with zero shared physical cores + dedicated raw-stream core
|
| 370 |
+
+ 30 fps empty-bucket watchdog). SLAM pose convention corrected for
|
| 371 |
+
the new ORB core (X=right / Y=forward / Z=up, pose relativized to the
|
| 372 |
+
origin — no first-frame jump). Stereo preview steady at 15 fps with
|
| 373 |
+
automatic raw-stream reconnect; trajectory merged into the episode
|
| 374 |
+
parquet instead of a txt sidecar. The gripper no longer depends on
|
| 375 |
+
`online/`: native resources mirrored to `core/gripper/native/` (not
|
| 376 |
+
committed), Sightac SDK shipped pyarmor-encrypted,
|
| 377 |
+
`tools/import_gripper_calibration.py` imports per-serial calibration
|
| 378 |
+
for a new gripper. Multiple S80M cameras distinguished by serial /
|
| 379 |
+
USB topology path
|
| 380 |
+
- **v1.2.1** — large-file upload no longer killed by the 10 s timeout
|
| 381 |
+
(the send-body phase used the connect timeout; now uses a read-timeout
|
| 382 |
+
window); closing the upload dialog lets the task finish in the
|
| 383 |
+
background (manual uploads share the main window queue, duplicates
|
| 384 |
+
skipped)
|
| 385 |
- **v1.2.0** — minimal one-click deployment edition (`start_lite.bat/.sh`
|
| 386 |
+ `venv_lite` whitelist env, ~750 MB): connect devices → record → upload
|
| 387 |
only — no login/playback/task page/skeleton solving. Devices: D435
|
|
|
|
| 720 |
|
| 721 |
### 更新记录
|
| 722 |
|
| 723 |
+
- **v1.3.0** — 新增 UMI 夹爪全链接入(Fays S80M 双目 SLAM 位姿/轨迹 +
|
| 724 |
+
Sightac 左右触觉力与 250×250 力矩阵 + DECXIN RGB;设备面板夹爪分组,
|
| 725 |
+
原生资源缺失时自动隐藏),支持双臂双夹爪同录(rig1 旧槽位/数据列契约
|
| 726 |
+
不变,rig2 加 `gripper_2_` 前缀;两 rig 独立 CPU 亲和分区零共享物理核 +
|
| 727 |
+
raw 流专用核 + 30fps 空桶看门狗)。SLAM 位姿坐标约定按新 ORB 核修正
|
| 728 |
+
(X=右 / Y=正对 / Z=上,原点后姿态相对化,起始不跳变)。左目显示稳
|
| 729 |
+
15fps(奇偶抽帧)且 raw 流断线自动重连;轨迹并入 episode parquet 列,
|
| 730 |
+
不再落 txt 侧车。夹爪脱离 `online/` 自持:原生资源镜像到
|
| 731 |
+
`core/gripper/native/`(不入库),Sightac SDK pyarmor 加密随包,
|
| 732 |
+
新增 `tools/import_gripper_calibration.py` 搬入新夹爪 per-serial 标定。
|
| 733 |
+
多台 S80M 按序列号 / USB 拓扑路径区分
|
| 734 |
+
- **v1.2.1** — 修复大文件上传被 10 秒误杀(发送阶段 socket 超时错用连接
|
| 735 |
+
超时,现改用读超时窗口);上传对话框关闭后任务在后台继续完成(手动上传
|
| 736 |
+
并入主窗口共享队列,重复提交自动跳过)
|
| 737 |
- **v1.2.0** — 新增极简一键部署采集版(`start_lite.bat/.sh` + `venv_lite`
|
| 738 |
白名单依赖,约 750MB):只做连接设备→采集→上传,无登录/回放/任务页/
|
| 739 |
骨架解算。设备:D435(1280×720@30 RGB / 848×480@30 深度)、UVC 摄像头
|
collector/config/__init__.py
CHANGED
|
@@ -3,4 +3,4 @@
|
|
| 3 |
版本号集中定义于此:settings.APP_VERSION 及其他模块均从这里取值。
|
| 4 |
"""
|
| 5 |
|
| 6 |
-
__version__ = "1.
|
|
|
|
| 3 |
版本号集中定义于此:settings.APP_VERSION 及其他模块均从这里取值。
|
| 4 |
"""
|
| 5 |
|
| 6 |
+
__version__ = "1.3.0" # 新增 UMI 夹爪全链接入(Fays S80M 双目 SLAM 位姿/轨迹 + Sightac 左右触觉力与 250×250 力矩阵 + DECXIN RGB;面板夹爪分组、资源缺失自动隐藏),支持双臂双夹爪同录(rig1 旧槽位/数据列契约不变,rig2 加 gripper_2_ 前缀;两 rig 独立 CPU 亲和分区零共享物理核 + raw 流专用核 + 30fps 空桶看门狗);SLAM 位姿坐标约定按新 ORB 核修正(X=右/Y=正对/Z=上,原点后姿态相对化,起始不跳变);左目显示稳 15fps(奇偶抽帧)且 raw 流断线自动重连;轨迹并入 episode parquet 列不再落 txt 侧车;夹爪脱离 online/ 自持(原生资源镜像 core/gripper/native/ 不入库、Sightac SDK pyarmor 加密随包、新增 tools/import_gripper_calibration.py 接入新夹爪标定);多台 S80M 按序列号/USB 拓扑路径区分(--device-serial/--device-path)。
|
collector/config/i18n.py
CHANGED
|
@@ -404,6 +404,33 @@ _TRANSLATIONS = {
|
|
| 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": "任务进度已更新: {} ({}/{})"},
|
|
|
|
| 404 |
"✅ 第 {}/{} 条上传完成: {}": {"en": "✅ #{}/{} uploaded: {}", "zh": "✅ 第 {}/{} 条上传完成: {}"},
|
| 405 |
"✅ 上传完成: {}": {"en": "✅ Uploaded: {}", "zh": "✅ 上传完成: {}"},
|
| 406 |
"[上传失败] {}: {}": {"en": "[Upload failed] {}: {}", "zh": "[上传失败] {}: {}"},
|
| 407 |
+
|
| 408 |
+
# ── 上传硬化(进度可见 / 续传 / 清理 / 退出确认)──────
|
| 409 |
+
"⏳ {}(上传中 {}% · {})": {"en": "⏳ {} (uploading {}% · {})", "zh": "⏳ {}(上传中 {}% · {})"},
|
| 410 |
+
"⏳ {}(排队中)": {"en": "⏳ {} (queued)", "zh": "⏳ {}(排队中)"},
|
| 411 |
+
"☁ 开始上传: {}": {"en": "☁ Upload started: {}", "zh": "☁ 开始上传: {}"},
|
| 412 |
+
"☁ [{}] {}": {"en": "☁ [{}] {}", "zh": "☁ [{}] {}"},
|
| 413 |
+
"☁ [{}] 上传中… {}%": {"en": "☁ [{}] uploading… {}%", "zh": "☁ [{}] 上传中… {}%"},
|
| 414 |
+
"☁ 恢复未完成的上传: {}": {"en": "☁ Resuming unfinished upload: {}", "zh": "☁ 恢复未完成的上传: {}"},
|
| 415 |
+
"☁ 已恢复 {} 个未完成的上传任务": {"en": "☁ Resumed {} unfinished upload task(s)", "zh": "☁ 已恢复 {} 个未完成的上传任务"},
|
| 416 |
+
"☁ 跳过恢复 {}: {}": {"en": "☁ Skipped resuming {}: {}", "zh": "☁ 跳过恢复 {}: {}"},
|
| 417 |
+
"☁ {} 已在服务器上,无需重传: {}": {"en": "☁ {} is already on the server, no re-upload needed: {}", "zh": "☁ {} 已在服务器上,无需重传: {}"},
|
| 418 |
+
"🧹 已清理上传残留临时文件 {} 个": {"en": "🧹 Cleaned up {} leftover upload temp file(s)", "zh": "🧹 已清理上传残留临时文件 {} 个"},
|
| 419 |
+
"本地文件不存在": {"en": "local files are gone", "zh": "本地文件不存在"},
|
| 420 |
+
"服务器地址已变更": {"en": "server address changed", "zh": "服务器地址已变更"},
|
| 421 |
+
"已上传完成": {"en": "already uploaded", "zh": "已上传完成"},
|
| 422 |
+
"已在队列中": {"en": "already queued", "zh": "已在队列中"},
|
| 423 |
+
"上次进程中断前已入库": {"en": "already stored before the last shutdown", "zh": "上次进程中断前已入库"},
|
| 424 |
+
"POST 返回失败但服务器已入库": {"en": "POST failed but the server already stored it", "zh": "POST 返回失败但服务器已入库"},
|
| 425 |
+
"上传中断但服务器已入库": {"en": "upload interrupted but the server already stored it", "zh": "上传中断但服务器已入库"},
|
| 426 |
+
"服务器上已存在本次会话,按成功处理({})": {
|
| 427 |
+
"en": "This session already exists on the server; treated as success ({})",
|
| 428 |
+
"zh": "服务器上已存在本次会话,按成功处理({})",
|
| 429 |
+
},
|
| 430 |
+
"有 {} 个上传任务未完成,确定退出?未完成的任务将在下次启动时自动恢复。": {
|
| 431 |
+
"en": "{} upload task(s) still unfinished. Quit anyway?\nThey will be resumed automatically on the next start.",
|
| 432 |
+
"zh": "有 {} 个上传任务未完成,确定退出?未完成的任务将在下次启动时自动恢复。",
|
| 433 |
+
},
|
| 434 |
"发布时间:": {"en": "Published:", "zh": "发布时间:"},
|
| 435 |
"请在任务选择页面选择一个任务。": {"en": "Please select a task on the task selection page.", "zh": "请在任务选择页面选择一个任务。"},
|
| 436 |
"任务进度已更新: {} ({}/{})": {"en": "Task progress updated: {} ({}/{})", "zh": "任务进度已更新: {} ({}/{})"},
|
collector/config/settings.py
CHANGED
|
@@ -11,6 +11,7 @@ from config import __version__ as APP_VERSION # 版本号唯一定义在 confi
|
|
| 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 |
|
|
@@ -460,6 +461,12 @@ UPLOAD_AUTO_SYNC = load_upload_auto_sync() # 录制完成后是否自动上
|
|
| 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 # 任务轮询间隔(毫秒)
|
|
@@ -476,3 +483,15 @@ HAND_DET_DEVICE = "cuda" # 检测器设备 ("cuda" / "cp
|
|
| 476 |
HAND_POSE_DEVICE = "cuda" # 关键点设备 ("cuda" / "cpu")
|
| 477 |
HAND_TRACK_MAX_HANDS = 2 # 最多追踪手数
|
| 478 |
HAND_DATA_DIM = 21 * 2 * 2 + 4 * 2 + 1 # 展平后数据维度
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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 |
+
LOGS_DIR = os.path.join(BASE_DIR, "logs")
|
| 15 |
DB_PATH = os.path.join(DATA_DIR, "pipeline.db")
|
| 16 |
KEYPOINTS_OUTPUT_DIR = os.path.join(BASE_DIR, "keypoints_output")
|
| 17 |
|
|
|
|
| 461 |
UPLOAD_DELETE_AFTER = load_upload_delete_after() # 上传成功后是否删除本地文件(持久化,启动时从 server_config.json 读取)
|
| 462 |
UPLOAD_PRECOMPRESS_VIDEO = True # 上传前是否把 videos 重编码到低码率。开=HEVC(libx265) CRF 档再压(tools/hevc_test 实测 CRF30 体积约原录制件的 7%、SSIM 0.967;大会话必备)。关=原样上传 CRF23 录制品(无二代有损;60min 双目 ~7.6GB,上传带宽/后端超时需能承受)
|
| 463 |
UPLOAD_VIDEO_CRF = 30 # HEVC 目标码率档(x265 CRF 越大越小;30 实测体积约 7%;仅 UPLOAD_PRECOMPRESS_VIDEO=True 时生效)
|
| 464 |
+
UPLOAD_SESSION_SNAPSHOT_LIMIT = 200 # 上传前后拉取的会话列表条数("POST 是否已入库"的判据;服务器现有 ~26 条)
|
| 465 |
+
UPLOAD_RESUME_MAX_AGE_HOURS = 24 # 启动续传窗口:更早的未完成上传不再自动重传
|
| 466 |
+
UPLOAD_RESUME_SKEW_TOL_S = 60.0 # 续传判定容忍的客户端/服务器时钟差(秒)
|
| 467 |
+
UPLOAD_TMP_SWEEP_AGE_HOURS = 24 # 启动清理残留上传临时文件的年龄门槛(小时)
|
| 468 |
+
UPLOAD_INFLIGHT_REFRESH_MS = 1500 # 上传对话框 ⏳ 状态刷新间隔(毫秒)
|
| 469 |
+
UPLOAD_LOG_THROTTLE_S = 10.0 # 主窗口上传进度日志节流(秒)
|
| 470 |
|
| 471 |
# ── 任务服务配置 ──────────────────────────────────────
|
| 472 |
TASK_POLL_INTERVAL_MS = 30000 # 任务轮询间隔(毫秒)
|
|
|
|
| 483 |
HAND_POSE_DEVICE = "cuda" # 关键点设备 ("cuda" / "cpu")
|
| 484 |
HAND_TRACK_MAX_HANDS = 2 # 最多追踪手数
|
| 485 |
HAND_DATA_DIM = 21 * 2 * 2 + 4 * 2 + 1 # 展平后数据维度
|
| 486 |
+
|
| 487 |
+
# ── UMI 夹爪(core/gripper 包;原生资源随包在 core/gripper/native/,
|
| 488 |
+
# 缺失时设备隐藏)─
|
| 489 |
+
GRIPPER_SLOT_STEREO_L = "gripper_stereo_left" # s80m 左目(桥接 raw 流,仅显示不录制)
|
| 490 |
+
GRIPPER_SLOT_RGB = "gripper_rgb" # DECXIN 1280×960@30
|
| 491 |
+
GRIPPER_SLOT_POSE = "gripper_pose" # SLAM 轨迹视图槽
|
| 492 |
+
GRIPPER_SLOT_FORCE_L = "gripper_force_left" # Sightac 左(热力图+力曲线)
|
| 493 |
+
GRIPPER_SLOT_FORCE_R = "gripper_force_right" # Sightac 右(热力图+力曲线)
|
| 494 |
+
GRIPPER_STEREO_DISPLAY_FPS = 15.0 # 左目显示节流(SLAM 取帧在桥接进程内保持 30fps)
|
| 495 |
+
GRIPPER_SLAM_READY_TIMEOUT_S = 45.0 # wait_sdk_ready 上限(SDK 运行时重读标定)
|
| 496 |
+
GRIPPER_FORCE_MATRIX_DIM = 250 # 触觉力矩阵边长(250×250×3 int16 行差分)
|
| 497 |
+
GRIPPER_TACTILE_MATRIX_MATERIALIZE = False # 矩阵逐帧拷回主进程开关(默认子进程直写)
|
collector/core/api_client.py
CHANGED
|
@@ -12,18 +12,46 @@ HTTP API 客户端 —— 对接 Data Acquisition 服务器。
|
|
| 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 |
"""连接子类:请求体发送阶段用读超时窗口,而不是连接超时。
|
|
@@ -49,6 +77,22 @@ class _PatientSendConnection(urllib3.connection.HTTPConnection):
|
|
| 49 |
finally:
|
| 50 |
self.timeout = saved
|
| 51 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 52 |
|
| 53 |
class _PatientSendHTTPSConnection(_PatientSendConnection,
|
| 54 |
urllib3.connection.HTTPSConnection):
|
|
@@ -67,6 +111,9 @@ 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 = {
|
|
@@ -88,8 +135,9 @@ class APIClient:
|
|
| 88 |
self._session.headers.update({"User-Agent": "DAQ-SDK/1.0"})
|
| 89 |
# 大请求体上传时发送阶段的 socket 超时改为读超时窗口,
|
| 90 |
# 否则服务器慢读停顿 >10s 会误杀上传(见 _PatientSendConnection)。
|
| 91 |
-
|
| 92 |
-
self._session.mount("
|
|
|
|
| 93 |
|
| 94 |
def close(self):
|
| 95 |
if self._own_session:
|
|
@@ -212,7 +260,14 @@ class APIClient:
|
|
| 212 |
|
| 213 |
# ── 查询 ──────────────────────────────────────────
|
| 214 |
|
| 215 |
-
def
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 216 |
try:
|
| 217 |
r = self._session.get(
|
| 218 |
f"{self.base_url}/api/v1/sessions",
|
|
@@ -221,10 +276,14 @@ class APIClient:
|
|
| 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:
|
|
|
|
| 12 |
|
| 13 |
from __future__ import annotations
|
| 14 |
import os
|
| 15 |
+
import socket
|
| 16 |
import time
|
| 17 |
from typing import Optional, Callable
|
| 18 |
|
| 19 |
import requests
|
| 20 |
import urllib3
|
| 21 |
from urllib3.util import Timeout as _Urllib3Timeout
|
| 22 |
+
from urllib3.util.retry import Retry as _Retry
|
| 23 |
from requests.adapters import HTTPAdapter
|
| 24 |
|
| 25 |
|
| 26 |
CONNECT_TIMEOUT = 10
|
| 27 |
READ_TIMEOUT = 1800 # 大会话(数 GB)上传 + 服务器解包入库可能耗时数分钟,设 30 分钟
|
| 28 |
|
| 29 |
+
# TCP 保活:对端(服务器/中间网络)静默消失时,阻塞的 recv 不会自己醒。
|
| 30 |
+
# 没有保活时 Linux 默认 2 小时才报错——上传线程会静默挂死(2026-09-09
|
| 31 |
+
# episode-012 事故)。60+15×4 ≈ 120s 内探测到死连接并抛 ConnectionError。
|
| 32 |
+
_KEEPALIVE_IDLE_S, _KEEPALIVE_INTVL_S, _KEEPALIVE_CNT = 60, 15, 4
|
| 33 |
+
_TCP_LEVEL = getattr(socket, "IPPROTO_TCP", 6)
|
| 34 |
+
_KEEPALIVE_OPTS = [
|
| 35 |
+
(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1),
|
| 36 |
+
(_TCP_LEVEL, getattr(socket, "TCP_KEEPIDLE", None), _KEEPALIVE_IDLE_S), # Linux
|
| 37 |
+
(_TCP_LEVEL, getattr(socket, "TCP_KEEPALIVE", None), _KEEPALIVE_IDLE_S), # macOS 别名
|
| 38 |
+
(_TCP_LEVEL, getattr(socket, "TCP_KEEPINTVL", None), _KEEPALIVE_INTVL_S),
|
| 39 |
+
(_TCP_LEVEL, getattr(socket, "TCP_KEEPCNT", None), _KEEPALIVE_CNT),
|
| 40 |
+
]
|
| 41 |
+
# 常量在本平台不存在的项直接丢掉:Windows 旧版 SDK 无 TCP_KEEPIDLE/
|
| 42 |
+
# TCP_KEEPINTVL/TCP_KEEPCNT,退化为仅 SO_KEEPALIVE(系统默认 2 小时)。
|
| 43 |
+
_SOCKET_OPTIONS = [o for o in _KEEPALIVE_OPTS if o[1] is not None]
|
| 44 |
+
|
| 45 |
+
# 只重试「连接阶段」错误:DNS 解析失败/连接被拒/建连超时——这些都在
|
| 46 |
+
# 请求体写出任何字节之前抛出,重试不会重复发送(POST 不在 urllib3 的
|
| 47 |
+
# 默认重试方法里,必须显式放行)。read=0/status=0:一旦开始发送或收到
|
| 48 |
+
# 响应,绝不自动重试,交由 UploadManager 的「查服务器再决定」逻辑处理。
|
| 49 |
+
_UPLOAD_RETRY = _Retry(
|
| 50 |
+
total=2, connect=2, read=0, status=0, redirect=0,
|
| 51 |
+
allowed_methods={"POST"}, backoff_factor=0,
|
| 52 |
+
respect_retry_after_header=False, raise_on_status=False,
|
| 53 |
+
)
|
| 54 |
+
|
| 55 |
|
| 56 |
class _PatientSendConnection(urllib3.connection.HTTPConnection):
|
| 57 |
"""连接子类:请求体发送阶段用读超时窗口,而不是连接超时。
|
|
|
|
| 77 |
finally:
|
| 78 |
self.timeout = saved
|
| 79 |
|
| 80 |
+
def connect(self):
|
| 81 |
+
"""建连阶段固定用 CONNECT_TIMEOUT。
|
| 82 |
+
|
| 83 |
+
request() 已把 self.timeout 抬到 READ_TIMEOUT,新建连接若照它
|
| 84 |
+
建 TCP,则"连接超时"实际是 30 分钟(服务器不响应 SYN 时线程
|
| 85 |
+
静默挂死)。这里临时换回连接窗口,建连完成即恢复,发送阶段
|
| 86 |
+
的宽窗口不受影响。
|
| 87 |
+
"""
|
| 88 |
+
saved = self.timeout
|
| 89 |
+
if isinstance(saved, (int, float)) and saved != CONNECT_TIMEOUT:
|
| 90 |
+
self.timeout = CONNECT_TIMEOUT
|
| 91 |
+
try:
|
| 92 |
+
super().connect()
|
| 93 |
+
finally:
|
| 94 |
+
self.timeout = saved
|
| 95 |
+
|
| 96 |
|
| 97 |
class _PatientSendHTTPSConnection(_PatientSendConnection,
|
| 98 |
urllib3.connection.HTTPSConnection):
|
|
|
|
| 111 |
"""把发送超时加长的连接池装入 session。"""
|
| 112 |
|
| 113 |
def init_poolmanager(self, connections, maxsize, block=False, **pool_kwargs):
|
| 114 |
+
# socket_options 只能经连接池的 **conn_kw 传到 HTTPConnection,
|
| 115 |
+
# 由这里统一注入(http/https 共用本类,一处生效)。
|
| 116 |
+
pool_kwargs.setdefault("socket_options", _SOCKET_OPTIONS)
|
| 117 |
super().init_poolmanager(connections, maxsize, block=block, **pool_kwargs)
|
| 118 |
# PoolManager 默认指向模块级 dict(多实例共享),须整体替换实例属性
|
| 119 |
self.poolmanager.pool_classes_by_scheme = {
|
|
|
|
| 135 |
self._session.headers.update({"User-Agent": "DAQ-SDK/1.0"})
|
| 136 |
# 大请求体上传时发送阶段的 socket 超时改为读超时窗口,
|
| 137 |
# 否则服务器慢读停顿 >10s 会误杀上传(见 _PatientSendConnection)。
|
| 138 |
+
# max_retries 只放行建连阶段错误(见 _UPLOAD_RETRY)。
|
| 139 |
+
self._session.mount("http://", _PatientSendAdapter(max_retries=_UPLOAD_RETRY))
|
| 140 |
+
self._session.mount("https://", _PatientSendAdapter(max_retries=_UPLOAD_RETRY))
|
| 141 |
|
| 142 |
def close(self):
|
| 143 |
if self._own_session:
|
|
|
|
| 260 |
|
| 261 |
# ── 查询 ──────────────────────────────────────────
|
| 262 |
|
| 263 |
+
def try_get_sessions(self, limit: int = 50) -> Optional[list[dict]]:
|
| 264 |
+
"""GET /api/v1/sessions;**失败返回 None**。
|
| 265 |
+
|
| 266 |
+
与 get_sessions 的区别:None = 查询失败(网络/认证/非 200),
|
| 267 |
+
而 [] = 服务器上确实一个会话都没有。上传前的快照必须能区分
|
| 268 |
+
这两种情况,否则查询失败会被误判成"服务器是空的",POST 失败
|
| 269 |
+
后任何一个同名会话都会被当成"新出现"而误报成功。
|
| 270 |
+
"""
|
| 271 |
try:
|
| 272 |
r = self._session.get(
|
| 273 |
f"{self.base_url}/api/v1/sessions",
|
|
|
|
| 276 |
)
|
| 277 |
if r.status_code == 200:
|
| 278 |
data = r.json()
|
| 279 |
+
return data.get("sessions", []) or []
|
| 280 |
except requests.RequestException:
|
| 281 |
pass
|
| 282 |
+
return None
|
| 283 |
+
|
| 284 |
+
def get_sessions(self, limit: int = 50) -> list[dict]:
|
| 285 |
+
sessions = self.try_get_sessions(limit)
|
| 286 |
+
return sessions if sessions is not None else []
|
| 287 |
|
| 288 |
def get_session(self, session_id: str) -> Optional[dict]:
|
| 289 |
try:
|
collector/core/device_detector.py
CHANGED
|
@@ -35,14 +35,15 @@ from core.camera import list_v4l_devices, _is_sdk_device
|
|
| 35 |
@dataclass
|
| 36 |
class DeviceInfo:
|
| 37 |
"""已连接设备的一条描述(跨线程经信号传回主线程)。"""
|
| 38 |
-
key: str # "uvc:{by-id前缀或索引}" | "d435:{rs serial}" | "s80m:
|
| 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 |
|
|
@@ -53,9 +54,11 @@ class DeviceInfo:
|
|
| 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"
|
|
@@ -145,10 +148,11 @@ def _list_uvc_devices_windows(max_index: int) -> List[DeviceInfo]:
|
|
| 145 |
|
| 146 |
|
| 147 |
def _list_uvc_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
|
| 148 |
-
"""UVC 网络摄像头(排除 RealSense UVC 节点
|
| 149 |
|
| 150 |
-
|
| 151 |
-
|
|
|
|
| 152 |
"""
|
| 153 |
if os.name == "nt":
|
| 154 |
return _list_uvc_devices_windows(max_index)
|
|
@@ -158,6 +162,8 @@ def _list_uvc_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[D
|
|
| 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"):
|
|
@@ -198,35 +204,110 @@ def _list_d435_devices() -> List[DeviceInfo]:
|
|
| 198 |
return []
|
| 199 |
|
| 200 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 201 |
def _list_s80m_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
|
| 202 |
-
"""FTDI 命中一
|
| 203 |
|
| 204 |
-
序号
|
|
|
|
|
|
|
|
|
|
| 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=
|
| 228 |
video_index=i,
|
| 229 |
-
by_id_path=
|
| 230 |
)]
|
| 231 |
return []
|
| 232 |
|
|
@@ -347,20 +428,28 @@ def _list_ble_devices() -> List[DeviceInfo]:
|
|
| 347 |
|
| 348 |
|
| 349 |
def detect_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
|
| 350 |
-
"""
|
| 351 |
devices: List[DeviceInfo] = []
|
|
|
|
| 352 |
try:
|
| 353 |
-
|
| 354 |
except Exception:
|
| 355 |
-
|
| 356 |
try:
|
| 357 |
-
devices +=
|
| 358 |
except Exception:
|
| 359 |
pass
|
| 360 |
try:
|
| 361 |
-
devices +=
|
| 362 |
except Exception:
|
| 363 |
pass
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 364 |
try:
|
| 365 |
devices += _list_ble_devices()
|
| 366 |
except Exception:
|
|
@@ -369,6 +458,7 @@ def detect_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[Devi
|
|
| 369 |
devices += _list_usb_glove_devices()
|
| 370 |
except Exception:
|
| 371 |
pass
|
|
|
|
| 372 |
return devices
|
| 373 |
|
| 374 |
|
|
@@ -457,6 +547,63 @@ def _list_usb_glove_devices() -> List[DeviceInfo]:
|
|
| 457 |
return infos
|
| 458 |
|
| 459 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 460 |
class DeviceScanner(QObject):
|
| 461 |
"""后台线程扫描设备(sysfs 只读),经排队信号回主线程。
|
| 462 |
|
|
|
|
| 35 |
@dataclass
|
| 36 |
class DeviceInfo:
|
| 37 |
"""已连接设备的一条描述(跨线程经信号传回主线程)。"""
|
| 38 |
+
key: str # "uvc:{by-id前缀或索引}" | "d435:{rs serial}" | "s80m:{sn}" | "ble:{MAC}" | "usbglove:{serial}" | "gripper:{serial}"
|
| 39 |
+
kind: str # "uvc" | "d435" | "s80m" | "data_ble" | "ble" | "usb_glove" | "gripper"
|
| 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 |
+
usb_path: str = "" # S80 相机的 USB 设备拓扑路径(如 1-3.4.1,spawn 选相机用;其余为空)
|
| 47 |
rssi: int = 0 # BLE 信号强度(排序用)
|
| 48 |
user_name: str = "" # 用户命名(枚举后由 MainWindow 从 device_names.json 填充)
|
| 49 |
|
|
|
|
| 54 |
|
| 55 |
@property
|
| 56 |
def group(self) -> str:
|
| 57 |
+
"""面板分组: "camera" | "glove" | "gripper" | "other_ble"。"""
|
| 58 |
if self.kind in ("data_ble", "usb_glove"):
|
| 59 |
return "glove"
|
| 60 |
+
if self.kind == "gripper":
|
| 61 |
+
return "gripper"
|
| 62 |
if self.kind == "ble":
|
| 63 |
return "other_ble"
|
| 64 |
return "camera"
|
|
|
|
| 148 |
|
| 149 |
|
| 150 |
def _list_uvc_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
|
| 151 |
+
"""UVC 网络摄像头(排除 RealSense UVC 节点、FTDI SDK 设备与夹爪组件)。
|
| 152 |
|
| 153 |
+
夹爪组件的 Sightac/DECXIN/FTDI 相机由 libuvc 服务或 Fays 桥接独占,
|
| 154 |
+
绝不作为通用 UVC 相机出现。Linux: sysfs 只读枚举(/dev/v4l/by-id
|
| 155 |
+
分组,轮询安全)。Windows: pygrabber DirectShow 枚举。
|
| 156 |
"""
|
| 157 |
if os.name == "nt":
|
| 158 |
return _list_uvc_devices_windows(max_index)
|
|
|
|
| 162 |
continue
|
| 163 |
if d.get("is_realsense"):
|
| 164 |
continue
|
| 165 |
+
if _is_gripper_component_camera(d):
|
| 166 |
+
continue
|
| 167 |
# key 用 by-id 前缀(跨插拔稳定);无 by-id 时退化为索引
|
| 168 |
prefix = str(d["video_index"])
|
| 169 |
if d.get("by_id_path"):
|
|
|
|
| 204 |
return []
|
| 205 |
|
| 206 |
|
| 207 |
+
def _ftdi_camera_groups() -> List[dict]:
|
| 208 |
+
"""sysfs 只读扫描 FTDI Superspeed Video Bridge 节点 → 按物理相机分组。
|
| 209 |
+
|
| 210 |
+
每台 S80 相机 = 一个 FTDI USB 设备(接口 1.0 = 双目对、1.2 = IMU),
|
| 211 |
+
多台接入时按 USB 设备拓扑路径分开。返回每台
|
| 212 |
+
{"usb_path", "serial", "stereo_index"}(按节点名排序,不 open 设备,
|
| 213 |
+
轮询安全);无 FTDI 返回 []。
|
| 214 |
+
"""
|
| 215 |
+
v4l_root = "/sys/class/video4linux"
|
| 216 |
+
if not os.path.isdir(v4l_root):
|
| 217 |
+
return []
|
| 218 |
+
groups: dict = {}
|
| 219 |
+
order: List[str] = []
|
| 220 |
+
for name in sorted(os.listdir(v4l_root)):
|
| 221 |
+
vp = os.path.join(v4l_root, name)
|
| 222 |
+
try:
|
| 223 |
+
with open(os.path.join(vp, "name"), encoding="utf-8") as f:
|
| 224 |
+
if "FTDI Superspeed Video Bridge" not in f.read().strip():
|
| 225 |
+
continue
|
| 226 |
+
# device 符号链接指向 USB 接口(…/1-3.4.1:1.0),basename 即
|
| 227 |
+
# 接口名;父级 1-3.4.1 为 USB 设备节点,serial 文件在其下
|
| 228 |
+
iface = os.path.basename(os.readlink(os.path.join(vp, "device")))
|
| 229 |
+
except OSError:
|
| 230 |
+
continue
|
| 231 |
+
if not re.match(r".+:(\d+\.\d+)$", iface):
|
| 232 |
+
continue
|
| 233 |
+
usb_path = iface.rsplit(":", 1)[0]
|
| 234 |
+
serial = ""
|
| 235 |
+
try:
|
| 236 |
+
with open(os.path.join("/sys/bus/usb/devices", usb_path, "serial"),
|
| 237 |
+
encoding="utf-8") as f:
|
| 238 |
+
serial = f.read().strip()
|
| 239 |
+
except OSError:
|
| 240 |
+
pass
|
| 241 |
+
idx = int(name[5:]) if name.startswith("video") and name[5:].isdigit() else -1
|
| 242 |
+
if usb_path not in groups:
|
| 243 |
+
groups[usb_path] = {"usb_path": usb_path, "serial": serial,
|
| 244 |
+
"stereo_index": -1}
|
| 245 |
+
order.append(usb_path)
|
| 246 |
+
# 双目对取接口 1.0 的第一个节点(与 read_stereo_rgb.py 口径一致)
|
| 247 |
+
if iface.endswith(":1.0") and groups[usb_path]["stereo_index"] < 0:
|
| 248 |
+
groups[usb_path]["stereo_index"] = idx
|
| 249 |
+
return [groups[p] for p in order]
|
| 250 |
+
|
| 251 |
+
|
| 252 |
+
def _find_s80m_by_id(video_index: int) -> Optional[str]:
|
| 253 |
+
"""S80M 双目节点的 /dev/v4l/by-id 永久路径(无则 None)。"""
|
| 254 |
+
by_id_dir = "/dev/v4l/by-id"
|
| 255 |
+
if not os.path.isdir(by_id_dir):
|
| 256 |
+
return None
|
| 257 |
+
for entry in sorted(os.listdir(by_id_dir)):
|
| 258 |
+
p = os.path.join(by_id_dir, entry)
|
| 259 |
+
try:
|
| 260 |
+
if os.readlink(p) == f"../../video{video_index}":
|
| 261 |
+
return p
|
| 262 |
+
except OSError:
|
| 263 |
+
continue
|
| 264 |
+
return None
|
| 265 |
+
|
| 266 |
+
|
| 267 |
def _list_s80m_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
|
| 268 |
+
"""每台 FTDI 命中一条 S80M 条目(多台相机按 USB 序列号区分)。
|
| 269 |
|
| 270 |
+
序列号从 USB 设备节点 serial 文件读取(FTDI 出厂默认 000000000001,
|
| 271 |
+
可能多台同号 → key 追加 USB 拓扑路径兜底唯一);无序列号时同样以
|
| 272 |
+
USB 路径兜底。usb_path 供 spawn 传 --device-path 精确选中该相机。
|
| 273 |
+
sysfs 扫描无果时退回旧版 _is_sdk_device 单条逻辑(老环境兜底)。
|
| 274 |
"""
|
| 275 |
+
cams = _ftdi_camera_groups()
|
| 276 |
+
if cams:
|
| 277 |
+
# 同序列号多台 → key 用 "s80m:{sn}@{usb_path}" 兜底唯一
|
| 278 |
+
seen: dict = {}
|
| 279 |
+
for c in cams:
|
| 280 |
+
seen[c["serial"]] = seen.get(c["serial"], 0) + 1
|
| 281 |
+
infos: List[DeviceInfo] = []
|
| 282 |
+
for c in cams:
|
| 283 |
+
if not (0 <= c["stereo_index"] < max_index):
|
| 284 |
+
continue
|
| 285 |
+
sn, path = c["serial"], c["usb_path"]
|
| 286 |
+
if sn and seen[sn] == 1:
|
| 287 |
+
key = f"s80m:{sn}"
|
| 288 |
+
else:
|
| 289 |
+
key = f"s80m:{sn}@{path}" if sn else f"s80m:usb-{path}"
|
| 290 |
+
infos.append(DeviceInfo(
|
| 291 |
+
key=key,
|
| 292 |
+
kind="s80m",
|
| 293 |
+
display_name=f"FaysSense S80M ({sn or path})",
|
| 294 |
+
serial=sn,
|
| 295 |
+
video_index=c["stereo_index"],
|
| 296 |
+
by_id_path=_find_s80m_by_id(c["stereo_index"]),
|
| 297 |
+
usb_path=path,
|
| 298 |
+
))
|
| 299 |
+
return infos
|
| 300 |
+
# 兜底:sysfs 名称不符的老环境退回旧版单条逻辑(key 保持 s80m:ftdi)
|
| 301 |
for i in range(max_index):
|
| 302 |
if not _is_sdk_device(i):
|
| 303 |
continue
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 304 |
return [DeviceInfo(
|
| 305 |
key="s80m:ftdi",
|
| 306 |
kind="s80m",
|
| 307 |
display_name="FaysSense S80M",
|
| 308 |
+
serial="",
|
| 309 |
video_index=i,
|
| 310 |
+
by_id_path=_find_s80m_by_id(i),
|
| 311 |
)]
|
| 312 |
return []
|
| 313 |
|
|
|
|
| 428 |
|
| 429 |
|
| 430 |
def detect_devices(max_index: int = settings.DEVICE_SCAN_MAX_INDEX) -> List[DeviceInfo]:
|
| 431 |
+
"""六段枚举(UVC + D435 + S80M + BLE + USB 手套 + UMI 夹爪),各自容错。"""
|
| 432 |
devices: List[DeviceInfo] = []
|
| 433 |
+
gripper_devices: List[DeviceInfo] = []
|
| 434 |
try:
|
| 435 |
+
gripper_devices = _list_gripper_devices()
|
| 436 |
except Exception:
|
| 437 |
+
gripper_devices = []
|
| 438 |
try:
|
| 439 |
+
devices += _list_uvc_devices(max_index)
|
| 440 |
except Exception:
|
| 441 |
pass
|
| 442 |
try:
|
| 443 |
+
devices += _list_d435_devices()
|
| 444 |
except Exception:
|
| 445 |
pass
|
| 446 |
+
if not gripper_devices:
|
| 447 |
+
# 单设备架设:夹爪在场时其 s80m 只经 ORB 桥接进程打开,
|
| 448 |
+
# 隐藏通用 S80M 条目,避免同一 FT602 被两个通道双开。
|
| 449 |
+
try:
|
| 450 |
+
devices += _list_s80m_devices(max_index)
|
| 451 |
+
except Exception:
|
| 452 |
+
pass
|
| 453 |
try:
|
| 454 |
devices += _list_ble_devices()
|
| 455 |
except Exception:
|
|
|
|
| 458 |
devices += _list_usb_glove_devices()
|
| 459 |
except Exception:
|
| 460 |
pass
|
| 461 |
+
devices += gripper_devices
|
| 462 |
return devices
|
| 463 |
|
| 464 |
|
|
|
|
| 547 |
return infos
|
| 548 |
|
| 549 |
|
| 550 |
+
# ── UMI 夹爪 ──────────────────────────────────────────
|
| 551 |
+
|
| 552 |
+
_GRIPPER_USB_VID = 0x303A # Espressif ESP32-S3(CDC-ACM 控制板)
|
| 553 |
+
_GRIPPER_USB_PID = 0x1001
|
| 554 |
+
# 夹爪组件相机(Sightac 触觉 ×2 / DECXIN RGB / Fays FT602),libuvc 服务与
|
| 555 |
+
# Fays 桥接独占,绝不进通用 UVC 列表
|
| 556 |
+
_GRIPPER_COMPONENT_UVC = {("0c45", "636f"), ("1bcf", "2d4f"), ("0403", "602e")}
|
| 557 |
+
|
| 558 |
+
|
| 559 |
+
def _is_gripper_component_camera(d: dict) -> bool:
|
| 560 |
+
"""按 VID/PID 识别夹爪组件相机;VID/PID 缺失时用 by-id 字符串兜底。"""
|
| 561 |
+
vid = str(d.get("vid") or "").lower()
|
| 562 |
+
pid = str(d.get("pid") or "").lower()
|
| 563 |
+
if vid and pid:
|
| 564 |
+
return (vid, pid) in _GRIPPER_COMPONENT_UVC
|
| 565 |
+
by_id = (d.get("by_id_path") or "").lower()
|
| 566 |
+
name = (d.get("name") or "").lower()
|
| 567 |
+
return any(
|
| 568 |
+
token in by_id or token in name
|
| 569 |
+
for token in ("0c45_636f", "1bcf_2d4f", "0403_602e",
|
| 570 |
+
"sightac", "decxin", "ftdi superspeed")
|
| 571 |
+
)
|
| 572 |
+
|
| 573 |
+
|
| 574 |
+
def _list_gripper_devices() -> List[DeviceInfo]:
|
| 575 |
+
"""枚举 UMI 夹爪控制板(303A:1001);原生资源缺失时隐藏整类。
|
| 576 |
+
|
| 577 |
+
资源缺失(core/gripper/native/ 未随包交付)返回 [],夹爪条目与组件
|
| 578 |
+
相机排除同时消失,主程序退化为纯手套模式(P0 验证:改名 native/
|
| 579 |
+
后条目消失)。
|
| 580 |
+
"""
|
| 581 |
+
try:
|
| 582 |
+
from core.gripper import paths
|
| 583 |
+
if not paths.gripper_resources_available():
|
| 584 |
+
return []
|
| 585 |
+
except Exception:
|
| 586 |
+
return []
|
| 587 |
+
try:
|
| 588 |
+
import serial.tools.list_ports
|
| 589 |
+
except ImportError:
|
| 590 |
+
return []
|
| 591 |
+
infos: List[DeviceInfo] = []
|
| 592 |
+
for port in serial.tools.list_ports.comports():
|
| 593 |
+
if port.vid != _GRIPPER_USB_VID or port.pid != _GRIPPER_USB_PID:
|
| 594 |
+
continue
|
| 595 |
+
serial = (port.serial_number or "").strip()
|
| 596 |
+
key = f"gripper:{serial}" if serial else f"gripper:{port.device}"
|
| 597 |
+
infos.append(DeviceInfo(
|
| 598 |
+
key=key,
|
| 599 |
+
kind="gripper",
|
| 600 |
+
display_name="UMI 夹爪",
|
| 601 |
+
serial=serial,
|
| 602 |
+
address=port.device,
|
| 603 |
+
))
|
| 604 |
+
return infos
|
| 605 |
+
|
| 606 |
+
|
| 607 |
class DeviceScanner(QObject):
|
| 608 |
"""后台线程扫描设备(sysfs 只读),经排队信号回主线程。
|
| 609 |
|
collector/core/device_manager.py
CHANGED
|
@@ -10,9 +10,10 @@
|
|
| 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
|
|
@@ -49,6 +50,13 @@ class DeviceManager:
|
|
| 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)
|
|
@@ -90,13 +98,18 @@ def build_device_meta(entries: dict) -> list:
|
|
| 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 |
|
|
|
|
| 10 |
注册表级纯口径(离线测试直接注入假条目即可)
|
| 11 |
|
| 12 |
注册表条目形状(与旧主窗口 _workers 口径一致):
|
| 13 |
+
kind ∈ {"uvc", "d435", "s80m", "data_ble", "ble", "gripper"}
|
| 14 |
d435/s80m 条目由各自 manager 的 new_entry 构造
|
| 15 |
+
(core.d435_manager / core.s80m_manager),uvc/ble/glove/gripper
|
| 16 |
+
骨架由本类构造。
|
| 17 |
"""
|
| 18 |
|
| 19 |
from __future__ import annotations
|
|
|
|
| 50 |
"sensor_column": role, "glove": glove_widget,
|
| 51 |
"label": label}
|
| 52 |
|
| 53 |
+
@staticmethod
|
| 54 |
+
def gripper_entry(slots, label: str, serial: str = "") -> dict:
|
| 55 |
+
"""UMI 夹爪条目:6 个网格槽 + ESP 序列号(fays_serial 开链后回填)。"""
|
| 56 |
+
return {"kind": "gripper", "slots": list(slots),
|
| 57 |
+
"label": label, "serial": serial,
|
| 58 |
+
"fays_serial": "", "sensor_columns": []}
|
| 59 |
+
|
| 60 |
# ── 查询 ──
|
| 61 |
def get(self, dev_key: str) -> dict | None:
|
| 62 |
return self.entries.get(dev_key)
|
|
|
|
| 98 |
"slots": [],
|
| 99 |
"sensor_column": e.get("sensor_column", "")})
|
| 100 |
continue
|
| 101 |
+
if e["kind"] not in ("uvc", "d435", "s80m", "gripper"):
|
| 102 |
continue
|
| 103 |
d = {"key": key, "kind": e["kind"],
|
| 104 |
"name": settings.device_name(key) or e.get("label", ""),
|
| 105 |
"slots": list(e.get("slots", []))}
|
| 106 |
if e.get("serial"):
|
| 107 |
d["serial"] = str(e["serial"])
|
| 108 |
+
if e["kind"] == "gripper":
|
| 109 |
+
if e.get("fays_serial"):
|
| 110 |
+
d["fays_serial"] = str(e["fays_serial"])
|
| 111 |
+
if e.get("sensor_columns"):
|
| 112 |
+
d["sensor_columns"] = list(e["sensor_columns"])
|
| 113 |
meta.append(d)
|
| 114 |
return meta
|
| 115 |
|
collector/core/egodata_writer.py
CHANGED
|
@@ -158,6 +158,7 @@ class EgoDataWriter(QObject):
|
|
| 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] = {}
|
|
@@ -349,6 +350,7 @@ class EgoDataWriter(QObject):
|
|
| 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 |
|
|
@@ -546,7 +548,8 @@ class EgoDataWriter(QObject):
|
|
| 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:
|
|
@@ -565,6 +568,12 @@ class EgoDataWriter(QObject):
|
|
| 565 |
glove_kpts: {传感器名: 63×f32 骨架关键点(21×3 米)}
|
| 566 |
USB 手套骨架快照,回填恒写的
|
| 567 |
observation.{left,right}_hand_pose 占位列
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 568 |
"""
|
| 569 |
sensors = sensors or {}
|
| 570 |
row = {
|
|
@@ -629,6 +638,32 @@ class EgoDataWriter(QObject):
|
|
| 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")
|
|
@@ -795,6 +830,23 @@ class EgoDataWriter(QObject):
|
|
| 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}"
|
|
@@ -906,6 +958,24 @@ class EgoDataWriter(QObject):
|
|
| 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]}
|
|
|
|
| 158 |
self._present_sensors: set = set()
|
| 159 |
self._present_imu: bool = False
|
| 160 |
self._present_glove_imu: set = set() # USB 手套 IMU 稀疏列(按传感器名)
|
| 161 |
+
self._present_gripper: set = set() # UMI 夹爪稀疏列(slam_pose/state/force/matrix)
|
| 162 |
# 深度多槽位(D435 第 n 台 = d435_depth[_n];S80M 传统路径兜底
|
| 163 |
# settings.CAMERA_DEPTH):槽位键 → depth image_key
|
| 164 |
self._depth_slots: Dict[str, str] = {}
|
|
|
|
| 350 |
self._present_sensors = set()
|
| 351 |
self._present_imu = False
|
| 352 |
self._present_glove_imu = set()
|
| 353 |
+
self._present_gripper = set()
|
| 354 |
self._reset_stats()
|
| 355 |
self._last_task = task_name
|
| 356 |
|
|
|
|
| 548 |
hardware_ns: int = 0,
|
| 549 |
imu_samples: Optional[List] = None,
|
| 550 |
glove_imu: Optional[Dict[str, tuple]] = None,
|
| 551 |
+
glove_kpts: Optional[Dict[str, np.ndarray]] = None,
|
| 552 |
+
gripper: Optional[Dict] = None):
|
| 553 |
"""写入一行到 data parquet 缓冲区。
|
| 554 |
|
| 555 |
Args:
|
|
|
|
| 568 |
glove_kpts: {传感器名: 63×f32 骨架关键点(21×3 米)}
|
| 569 |
USB 手套骨架快照,回填恒写的
|
| 570 |
observation.{left,right}_hand_pose 占位列
|
| 571 |
+
gripper: UMI 夹爪稀疏快照(P3/P4),键值:
|
| 572 |
+
slam_pose [x,y,z,qx,qy,qz,qw]
|
| 573 |
+
gripper_state [open_pct, gripped, fz_mn]
|
| 574 |
+
gripper_{left,right}_force [fx,fy,fz] mN
|
| 575 |
+
gripper_{left,right}_force_matrix int16 行差分
|
| 576 |
+
量化变长列表(P4 泵线程预编码,无新样本不写键)
|
| 577 |
"""
|
| 578 |
sensors = sensors or {}
|
| 579 |
row = {
|
|
|
|
| 638 |
row[f"observation.{side}_hand_pose"] = [
|
| 639 |
float(v) for v in k]
|
| 640 |
|
| 641 |
+
# UMI 夹爪稀疏列(P3/P4):本 episode 出现过的列才建列。
|
| 642 |
+
# 键按后缀定类型(_slam_pose=7f32 / _state/_force=3f32 /
|
| 643 |
+
# _force_matrix=int16 行差分变长,P4 泵线程预编码后原样落盘);
|
| 644 |
+
# 双夹爪 rig2 带 "gripper_2_" 前缀,rig1 旧键(slam_pose /
|
| 645 |
+
# gripper_state / gripper_left_force…)契约不变。无新样本不写键。
|
| 646 |
+
for key, value in (gripper or {}).items():
|
| 647 |
+
if value is None:
|
| 648 |
+
continue
|
| 649 |
+
if key.endswith("_force_matrix"):
|
| 650 |
+
row[f"observation.{key}"] = value
|
| 651 |
+
elif key.endswith("slam_trajectory"):
|
| 652 |
+
# 本帧窗口内全部轨迹点的扁平 8 值列表
|
| 653 |
+
# [t,x,y,z,qx,qy,qz,qw]×N(float64 原样落盘)
|
| 654 |
+
row[f"observation.{key}"] = value
|
| 655 |
+
elif key.endswith("slam_pose"):
|
| 656 |
+
# 后缀不带下划线:rig1 旧键是裸 "slam_pose"(无前缀),
|
| 657 |
+
# rig2 为 "gripper_2_slam_pose"
|
| 658 |
+
row[f"observation.{key}"] = [
|
| 659 |
+
float(v) for v in value[:7]]
|
| 660 |
+
elif key.endswith("_state") or key.endswith("_force"):
|
| 661 |
+
row[f"observation.{key}"] = [
|
| 662 |
+
float(v) for v in value[:3]]
|
| 663 |
+
else:
|
| 664 |
+
continue # 未知键不落盘,防静默错型
|
| 665 |
+
self._present_gripper.add(key)
|
| 666 |
+
|
| 667 |
cs = connection_status or {}
|
| 668 |
for did in self._device_ids:
|
| 669 |
row[f"status.{did}"] = cs.get(did, "connected")
|
|
|
|
| 830 |
cols[name_v] = pa.array(
|
| 831 |
[r.get(name_v, [0.0] * 16) for r in rows],
|
| 832 |
pa.list_(pa.float32(), 16))
|
| 833 |
+
# 稀疏 UMI 夹爪列(slam_pose/state/force 定长;matrix 变长 int16)
|
| 834 |
+
for key in sorted(self._present_gripper):
|
| 835 |
+
name = f"observation.{key}"
|
| 836 |
+
if key.endswith("_force_matrix"):
|
| 837 |
+
cols[name] = pa.array(
|
| 838 |
+
[r.get(name, []) for r in rows], pa.list_(pa.int16()))
|
| 839 |
+
elif key.endswith("slam_trajectory"):
|
| 840 |
+
cols[name] = pa.array(
|
| 841 |
+
[r.get(name, []) for r in rows], pa.list_(pa.float64()))
|
| 842 |
+
elif key.endswith("slam_pose"):
|
| 843 |
+
cols[name] = pa.array(
|
| 844 |
+
[r.get(name, [0.0] * 7) for r in rows],
|
| 845 |
+
pa.list_(pa.float32(), 7))
|
| 846 |
+
else: # _state / _force
|
| 847 |
+
cols[name] = pa.array(
|
| 848 |
+
[r.get(name, [0.0] * 3) for r in rows],
|
| 849 |
+
pa.list_(pa.float32(), 3))
|
| 850 |
# 手部关键点占位列(后处理回填,恒写)
|
| 851 |
for pose_name in [settings.HAND_POSE_LEFT, settings.HAND_POSE_RIGHT]:
|
| 852 |
name = f"observation.{pose_name}"
|
|
|
|
| 958 |
"dtype": "float32", "shape": [16, 4]}
|
| 959 |
features[f"observation.{sn}_imu_valid"] = {
|
| 960 |
"dtype": "float32", "shape": [16]}
|
| 961 |
+
# UMI 夹爪(P3/P4):定长 f32 列 + 变长 int16 行差分量化的力矩阵
|
| 962 |
+
for key in sorted(self._present_gripper):
|
| 963 |
+
if key.endswith("_force_matrix"):
|
| 964 |
+
features[f"observation.{key}"] = {
|
| 965 |
+
"dtype": "int16",
|
| 966 |
+
"shape": [settings.GRIPPER_FORCE_MATRIX_DIM] * 2 + [3],
|
| 967 |
+
"encoding": "row_diff_quantized",
|
| 968 |
+
}
|
| 969 |
+
elif key.endswith("slam_trajectory"):
|
| 970 |
+
features[f"observation.{key}"] = {
|
| 971 |
+
"dtype": "float64", "shape": [8],
|
| 972 |
+
"encoding": "flat_points_8_txyz_qxyzw"}
|
| 973 |
+
elif key.endswith("slam_pose"):
|
| 974 |
+
features[f"observation.{key}"] = {
|
| 975 |
+
"dtype": "float32", "shape": [7]}
|
| 976 |
+
else: # _state / _force
|
| 977 |
+
features[f"observation.{key}"] = {
|
| 978 |
+
"dtype": "float32", "shape": [3]}
|
| 979 |
# 手部骨架关键点(21×3 米,恒写列;无骨架数据时为全零占位)
|
| 980 |
features[f"observation.{settings.HAND_POSE_LEFT}"] = {
|
| 981 |
"dtype": "float32", "shape": [21, 3]}
|
collector/core/gripper/__init__.py
ADDED
|
@@ -0,0 +1,15 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""UMI 夹爪 rig 集成包(从 online/gripper_version1 分叉)。
|
| 2 |
+
|
| 3 |
+
原生二进制/SDK/标定随包交付在 core/gripper/native/,触觉 Sightac SDK 在
|
| 4 |
+
core/gripper/sightac_sdk/(pyarmor 加密);本包导入时先把 fays_runtime 的
|
| 5 |
+
路径常量补丁到 native/ 下(见 paths.patch_fays_runtime_constants)。资源
|
| 6 |
+
缺失时主程序通过 paths.gripper_resources_available() 隐藏夹爪设备条目。
|
| 7 |
+
"""
|
| 8 |
+
|
| 9 |
+
from __future__ import annotations
|
| 10 |
+
|
| 11 |
+
from core.gripper import paths
|
| 12 |
+
|
| 13 |
+
paths.patch_fays_runtime_constants()
|
| 14 |
+
|
| 15 |
+
__all__ = ["paths"]
|
collector/core/gripper/affinity.py
ADDED
|
@@ -0,0 +1,118 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""夹爪 CPU 预留(P5 补丁):主进程掩码收窄 + raw 流接收线程专用核。
|
| 2 |
+
|
| 3 |
+
P5 只绑了 native SLAM 进程与触觉进程的线程;主进程(GUI、力矩阵泵、
|
| 4 |
+
x265 编码子进程、各采集泵)的线程仍可漂到 SLAM 分区核上抢占
|
| 5 |
+
fays_input 抓帧线程 → raw 流突发空桶、显示 <15fps、SLAM 取帧 <30fps。
|
| 6 |
+
(后续探针实验定论:损耗主因不是主进程争用,而是 rig1 任务集含毒核
|
| 7 |
+
6——见 bridge.py 分区表注释;本模块掩码收窄仍保留,防止主进程线程落
|
| 8 |
+
SLAM 分区核或毒核。)
|
| 9 |
+
|
| 10 |
+
本模块职责:
|
| 11 |
+
- reserve(rig_index):主线程掩码 = 当前掩码 − 该 rig SLAM 分区 −
|
| 12 |
+
raw 专用核。之后创建的线程与录制开始时 fork 的 ffmpeg 子进程
|
| 13 |
+
(start_episode 在录制开始时由继承掩码的线程调用)继承受限掩码,
|
| 14 |
+
不再与 SLAM 分区抢核。
|
| 15 |
+
- release(rig_index):退还该 rig 预留;全部 rig 关闭后恢复完整掩码。
|
| 16 |
+
- raw_cpu(rig_index):raw 流接收线程专用核(SMT 兄弟核,与轻载
|
| 17 |
+
触觉核共享物理核;接收线程负载极轻,只求不被重线程抢占)。
|
| 18 |
+
- pin_raw_thread(cpu):raw 接收线程启动时自绑(线程级掩码独立于
|
| 19 |
+
继承掩码,可扩回专用核)。
|
| 20 |
+
|
| 21 |
+
机器 <24 逻辑 CPU 或掩码不含所需核 → 不预留,回退 P5 原状(
|
| 22 |
+
bridge._select_cpu_partition 同款回退口径)。
|
| 23 |
+
"""
|
| 24 |
+
|
| 25 |
+
import os
|
| 26 |
+
|
| 27 |
+
# SLAM 分区(与 bridge.py 常量一致;独立重复一份避免 import 环)。
|
| 28 |
+
# rig1 任务集已改为 {4,5,7,8,9}:核 6 因 SMT 兄弟 18 挂 xhci 摄像头
|
| 29 |
+
# 中断风暴(流式期间 3000+/s)被整体移出任务集——主进程线程也不得落
|
| 30 |
+
# 该核(风暴会拖慢任何实时性敏感的线程),故预留集合仍含 6。
|
| 31 |
+
_SLAM_RIG1 = frozenset({4, 5, 6, 7, 8, 9})
|
| 32 |
+
_SLAM_RIG2 = frozenset({10, 11, 13, 15, 22, 23})
|
| 33 |
+
# raw 专用核:物理核 2(rig1 触觉右)/ 物理核 3(rig2 触觉右)的
|
| 34 |
+
# SMT 兄弟(12+2=14、12+3=15)——触觉线程约 40% 单核,raw 接收
|
| 35 |
+
# 线程负载极轻,共享物理核无碍
|
| 36 |
+
_RAW_CPU_RIG1 = 14
|
| 37 |
+
_RAW_CPU_RIG2 = 15
|
| 38 |
+
|
| 39 |
+
_state = {
|
| 40 |
+
"baseline": None, # 首次预留前的主线程完整掩码(恢复基准)
|
| 41 |
+
"reserved": set(), # 当前已预留的 CPU 集合(多 rig 并集)
|
| 42 |
+
}
|
| 43 |
+
|
| 44 |
+
|
| 45 |
+
def _all_cpus():
|
| 46 |
+
try:
|
| 47 |
+
return {int(cpu) for cpu in os.sched_getaffinity(0)}
|
| 48 |
+
except (OSError, AttributeError):
|
| 49 |
+
return None
|
| 50 |
+
|
| 51 |
+
|
| 52 |
+
def supported() -> bool:
|
| 53 |
+
"""本机是否支持预留(>=24 逻辑 CPU 且完整掩码含全部所需核)。
|
| 54 |
+
|
| 55 |
+
必须按 baseline(首次预留前的完整掩码)判断:首台 rig 预留后
|
| 56 |
+
主线程当前掩码已收窄,若按当前掩码判断,第二台 rig 的预留会
|
| 57 |
+
因专用核已不在当前掩码里而被误判为不支持。"""
|
| 58 |
+
cpus = _state["baseline"] if _state["baseline"] is not None else _all_cpus()
|
| 59 |
+
if cpus is None or len(cpus) < 24:
|
| 60 |
+
return False
|
| 61 |
+
return (_SLAM_RIG1 <= cpus and _SLAM_RIG2 <= cpus
|
| 62 |
+
and _RAW_CPU_RIG1 in cpus and _RAW_CPU_RIG2 in cpus)
|
| 63 |
+
|
| 64 |
+
|
| 65 |
+
def raw_cpu(rig_index: int) -> int:
|
| 66 |
+
"""rig 序号(1 起)→ raw 流接收线程专用核。"""
|
| 67 |
+
return _RAW_CPU_RIG2 if rig_index >= 2 else _RAW_CPU_RIG1
|
| 68 |
+
|
| 69 |
+
|
| 70 |
+
def pin_raw_thread(cpu: int) -> bool:
|
| 71 |
+
"""raw 流接收线程内自绑专用核;失败返回 False(调用方记日志)。"""
|
| 72 |
+
try:
|
| 73 |
+
os.sched_setaffinity(0, {int(cpu)})
|
| 74 |
+
return True
|
| 75 |
+
except (OSError, AttributeError):
|
| 76 |
+
return False
|
| 77 |
+
|
| 78 |
+
|
| 79 |
+
def _apply_mask(logger):
|
| 80 |
+
if _state["baseline"] is None or logger is None:
|
| 81 |
+
return
|
| 82 |
+
mask = _state["baseline"] - _state["reserved"]
|
| 83 |
+
try:
|
| 84 |
+
os.sched_setaffinity(0, mask)
|
| 85 |
+
except (OSError, AttributeError):
|
| 86 |
+
return
|
| 87 |
+
logger(f"[CPU] 主进程掩码收窄至 {sorted(mask)}"
|
| 88 |
+
f"(预留 {sorted(_state['reserved'])})")
|
| 89 |
+
|
| 90 |
+
|
| 91 |
+
def reserve(rig_index: int, logger=None):
|
| 92 |
+
"""打开 rig 时预留其 SLAM 分区 + raw 专用核(主线程调用)。"""
|
| 93 |
+
if _state["baseline"] is None:
|
| 94 |
+
_state["baseline"] = _all_cpus()
|
| 95 |
+
if not supported():
|
| 96 |
+
return None
|
| 97 |
+
rig_set = _SLAM_RIG2 if rig_index >= 2 else _SLAM_RIG1
|
| 98 |
+
_state["reserved"] |= rig_set | {raw_cpu(rig_index)}
|
| 99 |
+
_apply_mask(logger)
|
| 100 |
+
return sorted(_state["reserved"])
|
| 101 |
+
|
| 102 |
+
|
| 103 |
+
def release(rig_index: int, logger=None):
|
| 104 |
+
"""关闭 rig 时退还预留;全部关闭后恢复完整掩码(主线程调用)。"""
|
| 105 |
+
if _state["baseline"] is None:
|
| 106 |
+
return
|
| 107 |
+
rig_set = _SLAM_RIG2 if rig_index >= 2 else _SLAM_RIG1
|
| 108 |
+
_state["reserved"] -= rig_set | {raw_cpu(rig_index)}
|
| 109 |
+
if _state["reserved"]:
|
| 110 |
+
_apply_mask(logger)
|
| 111 |
+
return
|
| 112 |
+
try:
|
| 113 |
+
os.sched_setaffinity(0, _state["baseline"])
|
| 114 |
+
except (OSError, AttributeError):
|
| 115 |
+
pass
|
| 116 |
+
_state["baseline"] = None
|
| 117 |
+
if logger:
|
| 118 |
+
logger("[CPU] 夹爪预留全部释放,主进程掩码恢复")
|
collector/core/gripper/bridge.py
ADDED
|
@@ -0,0 +1,811 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""夹爪 rig 与主程序的 Qt 桥(后台线程采集 → 队列信号回主线程)。
|
| 2 |
+
|
| 3 |
+
各采集线程(SLAM stdout、raw 双目流、RGB 读取、触觉结果泵)只向本对象的
|
| 4 |
+
内部事件队列投递;由**唯一**的 emitter 线程串行调用 pyqtSignal.emit ——
|
| 5 |
+
多线程并发 emit 在 PyQt5 下存在竞态(实测 RGB 线程与主线程同时 emit
|
| 6 |
+
触发段错误),因此所有信号出口收敛到一个线程。主窗口用队列连接把信号
|
| 7 |
+
调度回 GUI 线程。open/close 由主窗口的 _open_gripper/_close_gripper 按
|
| 8 |
+
生命周期顺序调用(见 main_window)。
|
| 9 |
+
|
| 10 |
+
帧类事件(rgb/tactile/stereo_left)走 latest-wins 单槽:
|
| 11 |
+
GUI 卡顿时只丢旧帧不积压内存;控制类事件(log/error/opened/closed/
|
| 12 |
+
pose/state)入队列保序。触觉录制数据不走本桥信号(P4 的 record_queue
|
| 13 |
+
直连泵线程)。
|
| 14 |
+
|
| 15 |
+
P2 已接入:UvcCameraServiceManager 相机组选择 → Sightac Flash 参数缓存 →
|
| 16 |
+
DECXIN RGB 常驻读取线程 → 触觉输入/计算双进程(spawn)。
|
| 17 |
+
P3 已接入:SingleFaysLease.acquire 全链(USB3 速度校验/官方探针/flock)→
|
| 18 |
+
SlamProcessController(wait_sdk_ready 等 [FAYS-CALIB] 标定标记)→
|
| 19 |
+
FaysRawStreamClient(双目分包拆分,只取左目投 UI;IMU 原始数据不外送)。
|
| 20 |
+
P4 已接入:GripperSerial(? 握手 + C/I/F 信号,失败不拆已就绪链路)、
|
| 21 |
+
GripState(10ms 力检查循环 + 300mN 锁存)、力矩阵最新值(录制泵线程
|
| 22 |
+
30ms 轮询 matrix_seq 取走,int16 行差分编码在泵线程完成)。
|
| 23 |
+
open() 在独立线程执行(相机服务启动/首帧重试/触觉 warmup/SLAM 初始化
|
| 24 |
+
都是秒级阻塞),ready/error 经信号回主线程。
|
| 25 |
+
"""
|
| 26 |
+
|
| 27 |
+
from __future__ import annotations
|
| 28 |
+
|
| 29 |
+
import os
|
| 30 |
+
import queue
|
| 31 |
+
import threading
|
| 32 |
+
import time
|
| 33 |
+
|
| 34 |
+
import cv2
|
| 35 |
+
import numpy as np
|
| 36 |
+
from PyQt5.QtCore import QObject, pyqtSignal
|
| 37 |
+
|
| 38 |
+
from config import settings
|
| 39 |
+
from core.s80m_manager import s80m_drop_watch, STEREO_DROP_ALERT_RATE
|
| 40 |
+
from core.gripper import affinity, paths
|
| 41 |
+
from core.gripper.fays_runtime import build_fays_runtime_env
|
| 42 |
+
from core.gripper.fays_single import SingleFaysLease
|
| 43 |
+
from core.gripper.control.grip_state import GripState
|
| 44 |
+
from core.gripper.devices.gripper_connection import (
|
| 45 |
+
ConnectionState,
|
| 46 |
+
GripperConnectionController,
|
| 47 |
+
)
|
| 48 |
+
from core.gripper.devices.gripper_serial import GripperSerial
|
| 49 |
+
from core.gripper.devices.serial_command_worker import SerialCommandWorker
|
| 50 |
+
from core.gripper.devices.tactile_discovery import TactileDiscovery
|
| 51 |
+
from core.gripper.devices.tactile_process_manager import (
|
| 52 |
+
TactileProcessManager,
|
| 53 |
+
TactileState,
|
| 54 |
+
)
|
| 55 |
+
from core.gripper.devices.uvc_camera_service import UvcCameraServiceManager
|
| 56 |
+
from core.gripper.recording.fays_raw_client import FaysRawStreamClient
|
| 57 |
+
from core.gripper.slam.process_controller import (
|
| 58 |
+
SlamProcessController,
|
| 59 |
+
SlamState,
|
| 60 |
+
)
|
| 61 |
+
|
| 62 |
+
# ── P5 双 rig CPU 分区(本机 Ryzen 9900X:12 物理核 × SMT,兄弟对
|
| 63 |
+
# (0,12)…(11,23))。rig1:触觉 0/2 + SLAM 任务集 {4,5,7,8,9}——
|
| 64 |
+
# 物理核 6 不得进入:其 SMT 兄弟 18 挂着 xhci 摄像头中断风暴
|
| 65 |
+
# (流式期间 3000+/s),热角色落在 6 上会拖垮双目取帧(实测
|
| 66 |
+
# left_orb 在 6 时空桶 23-29%,移出后 0.0%)。background
|
| 67 |
+
# (local-map 线程,40-90% 单核负载)必须独享核 9——与 track
|
| 68 |
+
# 共享同一核会令 SLAM 首 pose 无法出现(复现 2/2)。input 与
|
| 69 |
+
# track 均轻载,共享核 4。rig2 独立分区:触觉 1/3 + SLAM
|
| 70 |
+
# 任务集 {10,11,13,15,22,23}——两 rig 零物理核共享。机器 <24
|
| 71 |
+
# 逻辑 CPU(或进程亲和范围不含 rig2 分区)时回退 rig1 表共享
|
| 72 |
+
# (见 _select_cpu_partition)。 ──
|
| 73 |
+
_TACTILE_CPU_MAP_RIG1 = {"left": 0, "right": 2}
|
| 74 |
+
_TACTILE_CPU_MAP_RIG2 = {"left": 1, "right": 3}
|
| 75 |
+
_SLAM_CPU_ROLES_RIG1 = {
|
| 76 |
+
"fays_input": (4,),
|
| 77 |
+
"fays_prepare": (5,),
|
| 78 |
+
"fays_left_orb": (7,),
|
| 79 |
+
"fays_right_orb": (8,),
|
| 80 |
+
"fays_track": (4,),
|
| 81 |
+
"fays_background": (9,),
|
| 82 |
+
}
|
| 83 |
+
_SLAM_CPU_ROLES_RIG2 = {
|
| 84 |
+
"fays_input": (10,),
|
| 85 |
+
"fays_prepare": (22,),
|
| 86 |
+
"fays_left_orb": (13,),
|
| 87 |
+
"fays_right_orb": (15,),
|
| 88 |
+
"fays_track": (11,),
|
| 89 |
+
"fays_background": (23,),
|
| 90 |
+
}
|
| 91 |
+
# rig2 分区所需的全部逻辑 CPU(触觉 + SLAM 六角色 + taskset 启动域)
|
| 92 |
+
_RIG2_PARTITION_CPUS = frozenset({
|
| 93 |
+
1, 3, 10, 11, 13, 15, 22, 23,
|
| 94 |
+
})
|
| 95 |
+
|
| 96 |
+
|
| 97 |
+
def _select_cpu_partition(rig_index: int) -> tuple:
|
| 98 |
+
"""按 rig 序号选 CPU 分区,返回 (tactile_cpu_map, slam_cpu_roles, 说明)。
|
| 99 |
+
|
| 100 |
+
亲和范围不含 rig2 分区(如少于 24 逻辑 CPU 的机器)时回退 rig1 表:
|
| 101 |
+
两 rig 同表共享,功能不受影响,仅性能退回共享(待 probe 实测)。
|
| 102 |
+
"""
|
| 103 |
+
if rig_index >= 2:
|
| 104 |
+
try:
|
| 105 |
+
allowed = {
|
| 106 |
+
int(cpu) for cpu in os.sched_getaffinity(0)
|
| 107 |
+
}
|
| 108 |
+
except (OSError, AttributeError):
|
| 109 |
+
allowed = None
|
| 110 |
+
if allowed is not None and _RIG2_PARTITION_CPUS <= allowed:
|
| 111 |
+
return (dict(_TACTILE_CPU_MAP_RIG2),
|
| 112 |
+
dict(_SLAM_CPU_ROLES_RIG2), None)
|
| 113 |
+
return (dict(_TACTILE_CPU_MAP_RIG1),
|
| 114 |
+
dict(_SLAM_CPU_ROLES_RIG1),
|
| 115 |
+
"[Gripper] CPU 亲和范围不含 rig2 分区 "
|
| 116 |
+
f"{sorted(_RIG2_PARTITION_CPUS)},rig2 退回 rig1 核表共享"
|
| 117 |
+
"(双 SLAM 同表,性能可能受资源竞争影响)")
|
| 118 |
+
return (dict(_TACTILE_CPU_MAP_RIG1),
|
| 119 |
+
dict(_SLAM_CPU_ROLES_RIG1), None)
|
| 120 |
+
|
| 121 |
+
|
| 122 |
+
# 轨迹滚动窗口(与上位机 PoseState 2000 点上限一致)
|
| 123 |
+
# 显示轨迹窗口上限。GUI 用锚定索引 0 的固定网格降采样(见 ui/pose_view_qt.py),
|
| 124 |
+
# 依赖窗口头部不滑动——窗口一旦滚动,网格顶点索引会整体错位导致全线漂移。
|
| 125 |
+
# 60000 点 @30fps ≈ 33 分钟,典型会话不会触发;存储侧轨迹由 SLAM 进程独立写盘,
|
| 126 |
+
# 与此显示窗口无关。
|
| 127 |
+
TRAJECTORY_POINT_CAP = 60000
|
| 128 |
+
|
| 129 |
+
# 队列事件名 → 信号属性
|
| 130 |
+
_CONTROL_SIGNALS = {
|
| 131 |
+
"log": "log",
|
| 132 |
+
"error": "error",
|
| 133 |
+
"opened": "opened",
|
| 134 |
+
"closed": "closed",
|
| 135 |
+
"pose": "pose_ready",
|
| 136 |
+
"gripper_state": "gripper_state_ready",
|
| 137 |
+
}
|
| 138 |
+
|
| 139 |
+
# 帧类信号在 emitter 里的派发方式(slot 名 = 事件 kind;
|
| 140 |
+
# 右目不投递:只显示左目,SLAM 解算在桥接进程内部完成)
|
| 141 |
+
_FRAME_EMITTERS = {
|
| 142 |
+
"rgb": lambda self_, payload: self_.rgb_frame_ready.emit(*payload),
|
| 143 |
+
"stereo_left": lambda self_, payload: (
|
| 144 |
+
self_.stereo_frame_ready.emit(self_._stereo_slot, *payload)),
|
| 145 |
+
"tactile": lambda self_, payload: self_.tactile_ready.emit(*payload),
|
| 146 |
+
}
|
| 147 |
+
|
| 148 |
+
|
| 149 |
+
class _SerialLifecycleAdapter:
|
| 150 |
+
"""传感器链路已由 bridge 先行启动;串口连接器只做状态适配。"""
|
| 151 |
+
|
| 152 |
+
def prepare_connect(self):
|
| 153 |
+
return None
|
| 154 |
+
|
| 155 |
+
def connected(self):
|
| 156 |
+
return None
|
| 157 |
+
|
| 158 |
+
def connect_failed(self):
|
| 159 |
+
return None
|
| 160 |
+
|
| 161 |
+
def disconnect(self):
|
| 162 |
+
return None
|
| 163 |
+
|
| 164 |
+
def verify_prepared_control_port(self, device_path):
|
| 165 |
+
return None
|
| 166 |
+
|
| 167 |
+
|
| 168 |
+
class GripperBridge(QObject):
|
| 169 |
+
"""One open gripper session; owns workers, emits events."""
|
| 170 |
+
|
| 171 |
+
stereo_frame_ready = pyqtSignal(str, object, int)
|
| 172 |
+
# slot("gripper_stereo_left"), bgr ndarray, host_monotonic_ns
|
| 173 |
+
rgb_frame_ready = pyqtSignal(object, int) # bgr ndarray, host_monotonic_ns
|
| 174 |
+
tactile_ready = pyqtSignal(str, object, object, object)
|
| 175 |
+
# side("left"|"right"), heatmap(320x240x3 RGB,worker 已 BGR→RGB),
|
| 176 |
+
# force(fx,fy,fz mN),
|
| 177 |
+
# force_matrix(250x250x3 float32) 或 None(矩阵回传关闭时)
|
| 178 |
+
pose_ready = pyqtSignal(object, object, object, object)
|
| 179 |
+
# pos(x,y,z), quat(qx,qy,qz,qw), trajectory(位置点不可变元组,新点追加才重建),
|
| 180 |
+
# timestamp(SLAM 秒,与轨迹 txt 第一列同源——轨迹并入 episode parquet 后
|
| 181 |
+
# 每点 8 值 [t,x,y,z,qx,qy,qz,qw] 需要它)
|
| 182 |
+
gripper_state_ready = pyqtSignal(object) # {pct,gripped,fz,...}
|
| 183 |
+
opened = pyqtSignal() # 相机组 + 触觉 + SLAM 链路就绪
|
| 184 |
+
log = pyqtSignal(str)
|
| 185 |
+
error = pyqtSignal(str)
|
| 186 |
+
closed = pyqtSignal()
|
| 187 |
+
|
| 188 |
+
def __init__(self, parent=None, stereo_slot="gripper_stereo_left",
|
| 189 |
+
rig_index=1):
|
| 190 |
+
super().__init__(parent)
|
| 191 |
+
# 双夹爪:每台 bridge 发自己的双目槽名(rig1 保持旧名
|
| 192 |
+
# "gripper_stereo_left",rig2 为 "gripper_2_stereo_left"),
|
| 193 |
+
# 主窗口按 dev.key 路由到各自条目。
|
| 194 |
+
self._stereo_slot = stereo_slot
|
| 195 |
+
# P5 双 rig CPU 分区:rig1 旧核表 / rig2 独立物理核分区
|
| 196 |
+
# (亲和范围不足时回退 rig1 表共享,见 _select_cpu_partition)。
|
| 197 |
+
# cpu_note 等 emitter 队列就绪后再发(_log 依赖 _events)
|
| 198 |
+
self._rig_index = int(rig_index)
|
| 199 |
+
self._tactile_cpu_map, self._slam_cpu_roles, self._cpu_note = (
|
| 200 |
+
_select_cpu_partition(self._rig_index))
|
| 201 |
+
# P5 双目取帧空桶看门狗(30fps 口径,复用主程序 s80m_drop_watch):
|
| 202 |
+
# raw 双目包按 wall 时钟 1/30s 桶计数,空桶率超阈值告警一次;
|
| 203 |
+
# 录制开始由 main_window 重置(与 s80m 口径一致)
|
| 204 |
+
self._stereo_drop_watch = {
|
| 205 |
+
"last_mono_ns": None,
|
| 206 |
+
"dropped": 0,
|
| 207 |
+
"elapsed": 0,
|
| 208 |
+
"alerted": False,
|
| 209 |
+
}
|
| 210 |
+
self._lock = threading.RLock()
|
| 211 |
+
self._stop = threading.Event()
|
| 212 |
+
self._open_thread = None
|
| 213 |
+
self._rgb_thread = None
|
| 214 |
+
self._uvc = None
|
| 215 |
+
self._reservation = None
|
| 216 |
+
self._tactile = None
|
| 217 |
+
self._rgb_camera = None
|
| 218 |
+
self._lease = None
|
| 219 |
+
self._slam = None
|
| 220 |
+
self._raw_client = None
|
| 221 |
+
self._opened = False
|
| 222 |
+
# 双目 raw 流拆包状态(首包判定模式,open 期间有效)
|
| 223 |
+
self._stereo_mode = None # "side_by_side" | "paired_packets"
|
| 224 |
+
self._stereo_pending = None
|
| 225 |
+
# P4 串口/夹爪状态(heartbeat 与 grip 循环线程写,信号出)
|
| 226 |
+
self._serial = None
|
| 227 |
+
self._serial_worker = None
|
| 228 |
+
self._serial_controller = None
|
| 229 |
+
self._grip_state = GripState()
|
| 230 |
+
self._latest_force = {
|
| 231 |
+
"left": (0.0, 0.0, 0.0),
|
| 232 |
+
"right": (0.0, 0.0, 0.0),
|
| 233 |
+
}
|
| 234 |
+
self._last_state_payload = None
|
| 235 |
+
self._last_state_post = 0.0
|
| 236 |
+
# P4 力矩阵最新值 + 序号(录制泵线程轮询,序号变化才取走)
|
| 237 |
+
self._matrix_lock = threading.Lock()
|
| 238 |
+
self._latest_matrix = {"left": None, "right": None}
|
| 239 |
+
self._matrix_seq = {"left": 0, "right": 0}
|
| 240 |
+
# SLAM 轨迹:有效位姿逐点累积(滚动窗口,恒存不可变元组快照)
|
| 241 |
+
self._trajectory_points = []
|
| 242 |
+
self._trajectory_cache = ()
|
| 243 |
+
self._matrix_first_logged = set()
|
| 244 |
+
# 事件收敛:唯一 emitter 线程串行 emit(多线程并发 emit 会段错误)
|
| 245 |
+
self._events = queue.Queue()
|
| 246 |
+
self._frame_lock = threading.Lock()
|
| 247 |
+
self._frame_slots = {}
|
| 248 |
+
self._frame_pending = set()
|
| 249 |
+
self._emitter = threading.Thread(
|
| 250 |
+
target=self._emit_loop,
|
| 251 |
+
name="gripper-bridge-emit",
|
| 252 |
+
daemon=True,
|
| 253 |
+
)
|
| 254 |
+
self._emitter.start()
|
| 255 |
+
if self._cpu_note:
|
| 256 |
+
self._log(self._cpu_note)
|
| 257 |
+
|
| 258 |
+
# ---- 事件投递(任意线程可调) ----
|
| 259 |
+
def _post(self, name, *args):
|
| 260 |
+
self._events.put((name, args))
|
| 261 |
+
|
| 262 |
+
def _post_frame(self, kind, *payload):
|
| 263 |
+
"""latest-wins 帧槽:GUI 卡顿时丢旧帧不积压。"""
|
| 264 |
+
with self._frame_lock:
|
| 265 |
+
self._frame_slots[kind] = payload
|
| 266 |
+
if kind in self._frame_pending:
|
| 267 |
+
return
|
| 268 |
+
self._frame_pending.add(kind)
|
| 269 |
+
self._events.put(("frame", (kind,)))
|
| 270 |
+
|
| 271 |
+
def _emit_loop(self):
|
| 272 |
+
while True:
|
| 273 |
+
name, args = self._events.get()
|
| 274 |
+
if name == "_stop":
|
| 275 |
+
return
|
| 276 |
+
if name == "frame":
|
| 277 |
+
kind = args[0]
|
| 278 |
+
with self._frame_lock:
|
| 279 |
+
payload = self._frame_slots.get(kind)
|
| 280 |
+
self._frame_slots[kind] = None
|
| 281 |
+
self._frame_pending.discard(kind)
|
| 282 |
+
if payload is None:
|
| 283 |
+
continue
|
| 284 |
+
emitter = _FRAME_EMITTERS.get(kind)
|
| 285 |
+
if emitter is not None:
|
| 286 |
+
emitter(self, payload)
|
| 287 |
+
continue
|
| 288 |
+
attr = _CONTROL_SIGNALS.get(name)
|
| 289 |
+
if attr is not None:
|
| 290 |
+
getattr(self, attr).emit(*args)
|
| 291 |
+
|
| 292 |
+
# ---- 生命周期 ----
|
| 293 |
+
def open(self, esp_serial):
|
| 294 |
+
with self._lock:
|
| 295 |
+
if self._open_thread is not None:
|
| 296 |
+
raise RuntimeError("GripperBridge.open 已在运行")
|
| 297 |
+
if not self._emitter.is_alive():
|
| 298 |
+
raise RuntimeError("GripperBridge 已关闭,不可复用")
|
| 299 |
+
self._stop.clear()
|
| 300 |
+
self._opened = False
|
| 301 |
+
self._trajectory_path = None
|
| 302 |
+
self._grip_state.reset_latch()
|
| 303 |
+
self._latest_force = {
|
| 304 |
+
"left": (0.0, 0.0, 0.0),
|
| 305 |
+
"right": (0.0, 0.0, 0.0),
|
| 306 |
+
}
|
| 307 |
+
with self._matrix_lock:
|
| 308 |
+
self._latest_matrix = {"left": None, "right": None}
|
| 309 |
+
self._matrix_seq = {"left": 0, "right": 0}
|
| 310 |
+
self._trajectory_points = []
|
| 311 |
+
self._trajectory_cache = ()
|
| 312 |
+
self._matrix_first_logged = set()
|
| 313 |
+
self._stereo_drop_watch = {
|
| 314 |
+
"last_mono_ns": None,
|
| 315 |
+
"dropped": 0,
|
| 316 |
+
"elapsed": 0,
|
| 317 |
+
"alerted": False,
|
| 318 |
+
}
|
| 319 |
+
thread = threading.Thread(
|
| 320 |
+
target=self._open_run,
|
| 321 |
+
args=(str(esp_serial),),
|
| 322 |
+
name="gripper-bridge-open",
|
| 323 |
+
daemon=False,
|
| 324 |
+
)
|
| 325 |
+
self._open_thread = thread
|
| 326 |
+
thread.start()
|
| 327 |
+
|
| 328 |
+
def _log(self, msg):
|
| 329 |
+
self._post("log", msg)
|
| 330 |
+
|
| 331 |
+
def _open_run(self, esp_serial):
|
| 332 |
+
try:
|
| 333 |
+
# P3 全链租约:ESP 唯一匹配 → rig 内唯一 S80M → USB3 速度校验
|
| 334 |
+
# → 官方 SDK 探针读序列号 → flock → materialize 运行时 yaml
|
| 335 |
+
self._lease = SingleFaysLease(logger=self._log)
|
| 336 |
+
selected = self._lease.acquire(esp_serial)
|
| 337 |
+
# 轨迹 txt 由桥接进程直写;录制结束后 main_window 拷贝到
|
| 338 |
+
# meta/(P4)。open() 置 None 防旧会话残留。
|
| 339 |
+
self._trajectory_path = selected["trajectory"]
|
| 340 |
+
assignment = {
|
| 341 |
+
"esp32": {"serial": str(selected["esp_serial"])},
|
| 342 |
+
"fays": {
|
| 343 |
+
"product_serial": selected["product_serial"],
|
| 344 |
+
"physical_usb_path": selected["physical_usb_path"],
|
| 345 |
+
},
|
| 346 |
+
}
|
| 347 |
+
self._log(
|
| 348 |
+
"[Gripper] Fays 租约已持有: serial={} "
|
| 349 |
+
"usb={:g}M".format(
|
| 350 |
+
selected["product_serial"],
|
| 351 |
+
selected["usb_speed_mbps"]))
|
| 352 |
+
|
| 353 |
+
self._uvc = UvcCameraServiceManager(logger=self._log)
|
| 354 |
+
self._reservation = self._uvc.select(lambda: assignment)
|
| 355 |
+
discovery = TactileDiscovery(
|
| 356 |
+
sightac_root=paths.SIGHTAC_SDK_ROOT,
|
| 357 |
+
logger=self._log,
|
| 358 |
+
)
|
| 359 |
+
# Sightac Flash 参数入缓存:TactileProcessManager 的
|
| 360 |
+
# _resolve_inputs 依赖 cache_snapshot(),必须先于 start
|
| 361 |
+
for side in ("left", "right"):
|
| 362 |
+
camera = self._reservation.camera(side)
|
| 363 |
+
discovery.cache_external_flash_params(
|
| 364 |
+
camera.node.video_index, camera.flash_params)
|
| 365 |
+
|
| 366 |
+
# DECXIN RGB:claim 内置首帧重试;成功即启动常驻读取线程
|
| 367 |
+
decxin_camera = self._reservation.camera("decxin")
|
| 368 |
+
ipc_camera, first_frame, actual_format = decxin_camera.claim()
|
| 369 |
+
self._rgb_camera = ipc_camera
|
| 370 |
+
self._log(
|
| 371 |
+
"[Gripper] DECXIN RGB 就绪: {}x{}@{}fps {}".format(
|
| 372 |
+
actual_format[0], actual_format[1],
|
| 373 |
+
actual_format[2], actual_format[3]))
|
| 374 |
+
self._post_frame("rgb", first_frame, time.monotonic_ns())
|
| 375 |
+
self._rgb_thread = threading.Thread(
|
| 376 |
+
target=self._rgb_run,
|
| 377 |
+
name="gripper-rgb-read",
|
| 378 |
+
daemon=True,
|
| 379 |
+
)
|
| 380 |
+
self._rgb_thread.start()
|
| 381 |
+
|
| 382 |
+
# 触觉输入/计算双进程(spawn,父进程不建 TouchSensor;
|
| 383 |
+
# materialize_force_matrix:力矩阵回传主进程,P4 泵线程落盘)
|
| 384 |
+
self._tactile = TactileProcessManager(
|
| 385 |
+
TactileState(),
|
| 386 |
+
discovery,
|
| 387 |
+
cpu_map=dict(self._tactile_cpu_map),
|
| 388 |
+
materialize_force_matrix=True,
|
| 389 |
+
publish_result=self._on_tactile_result,
|
| 390 |
+
logger=self._log,
|
| 391 |
+
)
|
| 392 |
+
self._tactile.start_connected(
|
| 393 |
+
left=self._reservation.camera("left"),
|
| 394 |
+
right=self._reservation.camera("right"),
|
| 395 |
+
asynchronous=True,
|
| 396 |
+
)
|
| 397 |
+
if not self._tactile.wait_ready(
|
| 398 |
+
timeout=self._tactile.PROCESS_READY_TIMEOUT):
|
| 399 |
+
snapshot = self._tactile.snapshot()
|
| 400 |
+
details = "; ".join(
|
| 401 |
+
f"{side}={getattr(snapshot, side).error}"
|
| 402 |
+
for side in ("left", "right")
|
| 403 |
+
if getattr(snapshot, side).error)
|
| 404 |
+
raise RuntimeError(
|
| 405 |
+
"触觉双进程未就绪: " + (details or "超时"))
|
| 406 |
+
|
| 407 |
+
# SLAM 桥接进程(stdbuf+taskset 包官方 ORB 二进制,stdout
|
| 408 |
+
# 逐行协议;wait_sdk_ready 等 [FAYS-CALIB] 标定标记)
|
| 409 |
+
ipc_dir = selected["ipc_dir"]
|
| 410 |
+
|
| 411 |
+
def slam_runtime_env():
|
| 412 |
+
# 原生二进制按 KSQ_FAYS_IPC_DIR 定位 raw 流 socket/位姿
|
| 413 |
+
# JSON(默认 /dev/shm,会与租约的实例目录错位);
|
| 414 |
+
# 原程序由 FaysInstanceManager.runtime_environment() 注入
|
| 415 |
+
env = build_fays_runtime_env()
|
| 416 |
+
env["KSQ_FAYS_IPC_DIR"] = ipc_dir
|
| 417 |
+
env["KSQ_FAYS_CALIBRATION_SERIAL"] = (
|
| 418 |
+
selected["product_serial"])
|
| 419 |
+
return env
|
| 420 |
+
|
| 421 |
+
slam = SlamProcessController(
|
| 422 |
+
SlamState(),
|
| 423 |
+
executable=selected["orb_binary"],
|
| 424 |
+
vocabulary=paths.ORB_VOCABULARY,
|
| 425 |
+
settings=selected["orb_yaml"],
|
| 426 |
+
device_config=selected["runtime_config"],
|
| 427 |
+
trajectory=selected["trajectory"],
|
| 428 |
+
work_dir=selected["work_dir"],
|
| 429 |
+
native_log_path=(
|
| 430 |
+
f"{selected['work_dir']}/slam_stdout.log"),
|
| 431 |
+
runtime_env_factory=slam_runtime_env,
|
| 432 |
+
cpu_roles=dict(self._slam_cpu_roles),
|
| 433 |
+
on_pose=self._on_slam_pose,
|
| 434 |
+
log=self._log,
|
| 435 |
+
pose_paths=(
|
| 436 |
+
f"{ipc_dir}/orb_pose.json.tmp",
|
| 437 |
+
f"{ipc_dir}/orb_pose.json",
|
| 438 |
+
),
|
| 439 |
+
)
|
| 440 |
+
slam.start()
|
| 441 |
+
self._slam = slam
|
| 442 |
+
self._log("[Gripper] SLAM 桥接进程已启动,等待 SDK 标定…")
|
| 443 |
+
if not slam.wait_sdk_ready(
|
| 444 |
+
timeout=settings.GRIPPER_SLAM_READY_TIMEOUT_S):
|
| 445 |
+
raise RuntimeError(
|
| 446 |
+
"SLAM SDK 初始化超时(未等到 [FAYS-CALIB] 标定标记;"
|
| 447 |
+
"确认夹爪 USB3 头插在 USB3 口)")
|
| 448 |
+
|
| 449 |
+
# 原始双目/IMU 流(AF_UNIX):双目分包拆分 + IMU 批随左目
|
| 450 |
+
# raw_cpu:接收线程专用 SMT 核(P5 补丁,见 core/gripper/
|
| 451 |
+
# affinity.py)——保证 native 30fps 取帧不因接收侧丢包而降频
|
| 452 |
+
raw_cpu = affinity.raw_cpu(self._rig_index)
|
| 453 |
+
raw_client = FaysRawStreamClient(
|
| 454 |
+
f"{ipc_dir}/orb_raw_stream.sock",
|
| 455 |
+
on_stereo=self._on_stereo_packet,
|
| 456 |
+
logger=self._log,
|
| 457 |
+
on_error=lambda message: self._log(
|
| 458 |
+
f"[Gripper-Raw] {message}"),
|
| 459 |
+
on_reconnected=self._on_raw_reconnected,
|
| 460 |
+
raw_cpu=raw_cpu,
|
| 461 |
+
)
|
| 462 |
+
raw_client.start()
|
| 463 |
+
self._raw_client = raw_client
|
| 464 |
+
|
| 465 |
+
# 串口(P4):8 次 ? 握手 + C/I/F 信号;heartbeat/grip 循环
|
| 466 |
+
# 自带线程。所有传感器链路已先行就绪,lifecycle 只做占位
|
| 467 |
+
# 适配;握手失败不拆已就绪链路(日志可见,触觉/SLAM 保持)。
|
| 468 |
+
tty = str(selected.get("esp_tty") or "").strip()
|
| 469 |
+
if tty:
|
| 470 |
+
serial_owner = GripperSerial(logger=self._log)
|
| 471 |
+
worker = SerialCommandWorker(serial_owner.send)
|
| 472 |
+
worker.start()
|
| 473 |
+
controller = GripperConnectionController(
|
| 474 |
+
ConnectionState(),
|
| 475 |
+
serial_owner,
|
| 476 |
+
worker,
|
| 477 |
+
_SerialLifecycleAdapter(),
|
| 478 |
+
on_board_update=self._on_board_update,
|
| 479 |
+
on_grip_check=self._on_grip_check,
|
| 480 |
+
)
|
| 481 |
+
self._serial = serial_owner
|
| 482 |
+
self._serial_worker = worker
|
| 483 |
+
self._serial_controller = controller
|
| 484 |
+
if not controller.connect_sync(tty):
|
| 485 |
+
self._log(
|
| 486 |
+
"[Gripper] ESP32 串口连接失败: {} "
|
| 487 |
+
"(相机/触觉/SLAM 链路保持)".format(
|
| 488 |
+
serial_owner.last_error
|
| 489 |
+
or "handshake timeout"))
|
| 490 |
+
else:
|
| 491 |
+
self._log("[Gripper] 未找到 ESP32 串口,跳过夹爪控制")
|
| 492 |
+
|
| 493 |
+
if self._stop.is_set():
|
| 494 |
+
# 启动期间被 close():回收后不再发 opened
|
| 495 |
+
self._teardown()
|
| 496 |
+
return
|
| 497 |
+
self._opened = True
|
| 498 |
+
self._log("[Gripper] 相机 + 触觉 + SLAM 链路就绪")
|
| 499 |
+
self._post("opened")
|
| 500 |
+
except Exception as exc:
|
| 501 |
+
if self._stop.is_set():
|
| 502 |
+
# close() 路径触发(如 wait_ready 期间被回收):静默退出
|
| 503 |
+
return
|
| 504 |
+
self._log("[Gripper] 启动失败: {}".format(exc))
|
| 505 |
+
self._teardown()
|
| 506 |
+
self._post("error", str(exc))
|
| 507 |
+
|
| 508 |
+
def _rgb_run(self):
|
| 509 |
+
while not self._stop.is_set():
|
| 510 |
+
ok, frame = self._rgb_camera.read()
|
| 511 |
+
if not ok:
|
| 512 |
+
if not self._stop.is_set():
|
| 513 |
+
time.sleep(0.005)
|
| 514 |
+
continue
|
| 515 |
+
self._post_frame("rgb", frame, time.monotonic_ns())
|
| 516 |
+
|
| 517 |
+
def _on_tactile_result(self, side, heatmap, force, force_matrix):
|
| 518 |
+
if side in ("left", "right"):
|
| 519 |
+
try:
|
| 520 |
+
self._latest_force[side] = tuple(
|
| 521 |
+
float(value) for value in force[:3])
|
| 522 |
+
except (TypeError, ValueError, IndexError):
|
| 523 |
+
pass
|
| 524 |
+
if force_matrix is not None:
|
| 525 |
+
with self._matrix_lock:
|
| 526 |
+
self._latest_matrix[side] = force_matrix
|
| 527 |
+
self._matrix_seq[side] += 1
|
| 528 |
+
if side not in self._matrix_first_logged:
|
| 529 |
+
self._matrix_first_logged.add(side)
|
| 530 |
+
self._log(
|
| 531 |
+
"[Gripper-Tactile] {} 侧力矩阵首帧回传 shape={}".format(
|
| 532 |
+
side, force_matrix.shape))
|
| 533 |
+
self._post_frame("tactile", side, heatmap, force, force_matrix)
|
| 534 |
+
|
| 535 |
+
# ---- P4 串口/夹爪状态(heartbeat 与 grip 循环线程调用) ----
|
| 536 |
+
def _on_board_update(self, snapshot):
|
| 537 |
+
"""串口 STATE 响应 → GripState 板字段。"""
|
| 538 |
+
try:
|
| 539 |
+
self._grip_state.update_firmware_state(
|
| 540 |
+
dict(snapshot.board_fields or {}),
|
| 541 |
+
updated=snapshot.updated)
|
| 542 |
+
except Exception:
|
| 543 |
+
pass
|
| 544 |
+
self._publish_grip_state()
|
| 545 |
+
|
| 546 |
+
def _on_grip_check(self):
|
| 547 |
+
"""10ms 力检查循环:双路 Fz 阈值 → 锁存 → 状态事件。"""
|
| 548 |
+
event = self._grip_state.evaluate_forces(
|
| 549 |
+
self._latest_force["left"], self._latest_force["right"])
|
| 550 |
+
self._publish_grip_state(event=event)
|
| 551 |
+
|
| 552 |
+
def _publish_grip_state(self, event=None):
|
| 553 |
+
snap = self._grip_state.snapshot()
|
| 554 |
+
left = self._latest_force["left"]
|
| 555 |
+
right = self._latest_force["right"]
|
| 556 |
+
try:
|
| 557 |
+
fz = max(float(left[2]), float(right[2]))
|
| 558 |
+
except (TypeError, ValueError, IndexError):
|
| 559 |
+
fz = 0.0
|
| 560 |
+
payload = {
|
| 561 |
+
"pct": snap.percent,
|
| 562 |
+
"gripped": snap.grip_latched,
|
| 563 |
+
"fz": fz,
|
| 564 |
+
"event": str(event or ""),
|
| 565 |
+
"board": dict(snap.board_state),
|
| 566 |
+
}
|
| 567 |
+
now = time.monotonic()
|
| 568 |
+
if (payload != self._last_state_payload
|
| 569 |
+
or now - self._last_state_post >= 0.1):
|
| 570 |
+
self._last_state_payload = payload
|
| 571 |
+
self._last_state_post = now
|
| 572 |
+
self._post("gripper_state", payload)
|
| 573 |
+
|
| 574 |
+
def matrix_seq(self, side):
|
| 575 |
+
with self._matrix_lock:
|
| 576 |
+
return self._matrix_seq.get(side, 0)
|
| 577 |
+
|
| 578 |
+
def latest_matrix(self, side):
|
| 579 |
+
with self._matrix_lock:
|
| 580 |
+
return self._latest_matrix.get(side)
|
| 581 |
+
|
| 582 |
+
def _on_slam_pose(self, pose):
|
| 583 |
+
"""SlamProcessController 工作线程回调:位姿进队列保序。
|
| 584 |
+
|
| 585 |
+
原协议用 None 表示"无位姿/进程停止"(start() 的首个 stop()
|
| 586 |
+
与进程退出都会回调一次),桥接直接丢弃。
|
| 587 |
+
轨迹在此累积(与上位机 PoseState 同款滚动窗口),新点追加才
|
| 588 |
+
重建不可变元组——UI 侧按对象同一性省去无变化重绘。
|
| 589 |
+
"""
|
| 590 |
+
if pose is None:
|
| 591 |
+
return
|
| 592 |
+
self._trajectory_points.append(tuple(pose.position))
|
| 593 |
+
if len(self._trajectory_points) > TRAJECTORY_POINT_CAP:
|
| 594 |
+
self._trajectory_points = (
|
| 595 |
+
self._trajectory_points[-TRAJECTORY_POINT_CAP:])
|
| 596 |
+
self._trajectory_cache = tuple(self._trajectory_points)
|
| 597 |
+
self._post("pose", tuple(pose.position), tuple(pose.rotation),
|
| 598 |
+
self._trajectory_cache, pose.timestamp)
|
| 599 |
+
|
| 600 |
+
def _on_stereo_packet(
|
| 601 |
+
self, payload, sensor_ts_ns, host_mono_ns, sequence,
|
| 602 |
+
width, height, channels, step, encoding,
|
| 603 |
+
):
|
| 604 |
+
"""raw 流双目包:首包判定拆分模式,只取左目转 BGR 投帧槽。
|
| 605 |
+
|
| 606 |
+
右目不投递(主程序只显示左目;SLAM 解算在桥接进程内部完成,
|
| 607 |
+
左右目原始视频与 IMU 均不落盘,只存 SLAM 位姿/轨迹)。
|
| 608 |
+
模式一 side_by_side:单包宽 >= 3×高(左右拼合,如 1280×400),
|
| 609 |
+
取左半。
|
| 610 |
+
模式二 vertical_stacked:单包高 > 宽(上下拼合,实测 640×800
|
| 611 |
+
= 2×640×400),取上半(上半→左目,真机核对后定)。
|
| 612 |
+
模式三 paired_packets:单包即单目(如 640×400),相邻两包配对
|
| 613 |
+
(先左后右为暂定约定),右目包直接丢弃。
|
| 614 |
+
|
| 615 |
+
P5 每个 raw 包(含各模式)先过 30fps 空桶看门狗:SLAM 进程被
|
| 616 |
+
资源竞争拖慢时包以「空洞+突发」到达,wall 桶统计即可检出。
|
| 617 |
+
"""
|
| 618 |
+
self._stereo_drop_tick(host_mono_ns)
|
| 619 |
+
if self._stereo_mode is None:
|
| 620 |
+
if width >= height * 3:
|
| 621 |
+
self._stereo_mode = "side_by_side"
|
| 622 |
+
elif height > width:
|
| 623 |
+
self._stereo_mode = "vertical_stacked"
|
| 624 |
+
else:
|
| 625 |
+
self._stereo_mode = "paired_packets"
|
| 626 |
+
self._log(
|
| 627 |
+
"[Gripper-Raw] 双目包模式判定: {} ({}x{}x{})".format(
|
| 628 |
+
self._stereo_mode, width, height, channels))
|
| 629 |
+
buffer = np.frombuffer(
|
| 630 |
+
payload, dtype=np.uint8,
|
| 631 |
+
count=step * height,
|
| 632 |
+
).reshape(height, step)
|
| 633 |
+
if channels == 1:
|
| 634 |
+
frame = cv2.cvtColor(
|
| 635 |
+
buffer[:, :width].reshape(height, width),
|
| 636 |
+
cv2.COLOR_GRAY2BGR)
|
| 637 |
+
else:
|
| 638 |
+
frame = buffer[:, :width * channels].reshape(
|
| 639 |
+
height, width, channels).copy()
|
| 640 |
+
if self._stereo_mode == "side_by_side":
|
| 641 |
+
half = width // 2
|
| 642 |
+
self._post_frame("stereo_left", frame[:, :half].copy(),
|
| 643 |
+
host_mono_ns)
|
| 644 |
+
return
|
| 645 |
+
if self._stereo_mode == "vertical_stacked":
|
| 646 |
+
half = height // 2
|
| 647 |
+
self._post_frame("stereo_left", frame[:half].copy(),
|
| 648 |
+
host_mono_ns)
|
| 649 |
+
return
|
| 650 |
+
pending = self._stereo_pending
|
| 651 |
+
if pending is None:
|
| 652 |
+
self._stereo_pending = (frame, host_mono_ns)
|
| 653 |
+
return
|
| 654 |
+
left, left_ns = pending
|
| 655 |
+
self._stereo_pending = None
|
| 656 |
+
self._post_frame("stereo_left", left, left_ns)
|
| 657 |
+
|
| 658 |
+
def _stereo_drop_tick(self, mono_ns):
|
| 659 |
+
"""双目取帧 30fps 空桶看门狗(口径复用主程序 s80m_drop_watch)。
|
| 660 |
+
|
| 661 |
+
wall 时钟 1/30s 桶计数:SLAM 进程取帧掉速时包间隔拉大,跨 k≥2
|
| 662 |
+
桶计 k-1 个空桶。累计 ~3 秒样本后空桶率超
|
| 663 |
+
STEREO_DROP_ALERT_RATE 记一次日志告警(每段录制重置一次,
|
| 664 |
+
与 s80m 告警同口径;日志面板可见,不弹窗打扰)。
|
| 665 |
+
"""
|
| 666 |
+
interval_ns = int(settings.STEREO_RECORD_MIN_INTERVAL_S * 1e9)
|
| 667 |
+
# s80m_drop_watch 的 entry 契约带 drop_watch 键(与 s80m 注册表
|
| 668 |
+
# 条目同形状),这里按同形状包一层,纯函数口径原样复用
|
| 669 |
+
_, dropped, elapsed = s80m_drop_watch(
|
| 670 |
+
{"drop_watch": self._stereo_drop_watch},
|
| 671 |
+
mono_ns, interval_ns)
|
| 672 |
+
w = self._stereo_drop_watch
|
| 673 |
+
if w["alerted"] or elapsed < 90:
|
| 674 |
+
return
|
| 675 |
+
rate = dropped / elapsed
|
| 676 |
+
if rate < STEREO_DROP_ALERT_RATE:
|
| 677 |
+
return
|
| 678 |
+
w["alerted"] = True
|
| 679 |
+
self._log(
|
| 680 |
+
"[夹爪-SLAM告警] 双目取帧不稳定:近 3 秒空桶率 {:.0f}%"
|
| 681 |
+
"({} 空桶 / {} 桶)——30fps 采集受影响,SLAM 轨迹将卡顿;"
|
| 682 |
+
"若持续偏高请检查 USB 带宽与 CPU 争用".format(
|
| 683 |
+
rate * 100, dropped, elapsed))
|
| 684 |
+
|
| 685 |
+
def stereo_drop_snapshot(self):
|
| 686 |
+
"""(累计空桶, 累计桶数)——录制结束汇总口径,与主程序
|
| 687 |
+
_s80m_drop_summary 一致。"""
|
| 688 |
+
w = self._stereo_drop_watch
|
| 689 |
+
return (w["dropped"], w["elapsed"])
|
| 690 |
+
|
| 691 |
+
def reset_stereo_drop_watch(self):
|
| 692 |
+
"""录制开始重置空桶统计(告警放行下一段录制)。"""
|
| 693 |
+
self._stereo_drop_watch = {
|
| 694 |
+
"last_mono_ns": None,
|
| 695 |
+
"dropped": 0,
|
| 696 |
+
"elapsed": 0,
|
| 697 |
+
"alerted": False,
|
| 698 |
+
}
|
| 699 |
+
|
| 700 |
+
def clear_trajectory(self):
|
| 701 |
+
"""录制开始清空 GUI 轨迹滚动窗口(只显示本段录制的点)。
|
| 702 |
+
|
| 703 |
+
parquet 的 slam_trajectory 列由 writer 在录制期独立累积
|
| 704 |
+
(见 pipeline.write_slam_pose),不受此窗口影响;这里只管
|
| 705 |
+
显示侧。UI 侧配合 pose_view.reset() 同步清空渲染缓存。
|
| 706 |
+
"""
|
| 707 |
+
self._trajectory_points = []
|
| 708 |
+
self._trajectory_cache = ()
|
| 709 |
+
|
| 710 |
+
def _on_raw_reconnected(self):
|
| 711 |
+
"""raw 客户端断线重连成功回调。
|
| 712 |
+
|
| 713 |
+
断连窗口(服务端原始队列溢出主动断开,通常是录制停止时
|
| 714 |
+
主进程瞬时卡顿所致)的空桶属于显示侧问题,不计入取帧质量:
|
| 715 |
+
清掉残留的 paired 待配对帧(断点后流序重来),并重置空桶
|
| 716 |
+
看门狗统计。
|
| 717 |
+
"""
|
| 718 |
+
self._stereo_pending = None
|
| 719 |
+
self.reset_stereo_drop_watch()
|
| 720 |
+
|
| 721 |
+
def _teardown(self):
|
| 722 |
+
"""逆序回收:串口 → raw 流 → SLAM 进程 → RGB 线程 → 触觉子进程
|
| 723 |
+
→ 相机租约 → 服务进程 → Fays 租约。"""
|
| 724 |
+
with self._lock:
|
| 725 |
+
self._stop.set()
|
| 726 |
+
serial_controller = self._serial_controller
|
| 727 |
+
self._serial_controller = None
|
| 728 |
+
self._serial_worker = None
|
| 729 |
+
self._serial = None
|
| 730 |
+
raw_client = self._raw_client
|
| 731 |
+
self._raw_client = None
|
| 732 |
+
slam = self._slam
|
| 733 |
+
self._slam = None
|
| 734 |
+
rgb_thread = self._rgb_thread
|
| 735 |
+
self._rgb_thread = None
|
| 736 |
+
rgb_camera = self._rgb_camera
|
| 737 |
+
self._rgb_camera = None
|
| 738 |
+
tactile = self._tactile
|
| 739 |
+
self._tactile = None
|
| 740 |
+
reservation = self._reservation
|
| 741 |
+
self._reservation = None
|
| 742 |
+
uvc = self._uvc
|
| 743 |
+
self._uvc = None
|
| 744 |
+
lease = self._lease
|
| 745 |
+
self._lease = None
|
| 746 |
+
if serial_controller is not None:
|
| 747 |
+
try:
|
| 748 |
+
serial_controller.close()
|
| 749 |
+
except Exception:
|
| 750 |
+
pass
|
| 751 |
+
if raw_client is not None:
|
| 752 |
+
try:
|
| 753 |
+
raw_client.stop()
|
| 754 |
+
except Exception:
|
| 755 |
+
pass
|
| 756 |
+
if slam is not None:
|
| 757 |
+
try:
|
| 758 |
+
slam.stop(join=False)
|
| 759 |
+
except Exception:
|
| 760 |
+
pass
|
| 761 |
+
if rgb_thread is not None and rgb_thread.is_alive():
|
| 762 |
+
rgb_thread.join(timeout=3.0)
|
| 763 |
+
if rgb_camera is not None:
|
| 764 |
+
try:
|
| 765 |
+
rgb_camera.release()
|
| 766 |
+
except Exception:
|
| 767 |
+
pass
|
| 768 |
+
if tactile is not None:
|
| 769 |
+
try:
|
| 770 |
+
tactile.stop()
|
| 771 |
+
except Exception:
|
| 772 |
+
pass
|
| 773 |
+
if reservation is not None:
|
| 774 |
+
try:
|
| 775 |
+
reservation.release_unclaimed()
|
| 776 |
+
except Exception:
|
| 777 |
+
pass
|
| 778 |
+
if uvc is not None:
|
| 779 |
+
try:
|
| 780 |
+
uvc.stop()
|
| 781 |
+
except Exception:
|
| 782 |
+
pass
|
| 783 |
+
if lease is not None:
|
| 784 |
+
try:
|
| 785 |
+
lease.release()
|
| 786 |
+
except Exception:
|
| 787 |
+
pass
|
| 788 |
+
|
| 789 |
+
def close(self):
|
| 790 |
+
with self._lock:
|
| 791 |
+
if self._open_thread is None and not self._opened:
|
| 792 |
+
return
|
| 793 |
+
self._open_thread = None
|
| 794 |
+
self._teardown()
|
| 795 |
+
self._opened = False
|
| 796 |
+
self._post("closed")
|
| 797 |
+
# 哨兵事件让 emitter 线程优雅退出:守护线程若挂在
|
| 798 |
+
# queue.get() 上,解释器退出时仍在写 stderr 会核心转储
|
| 799 |
+
self._events.put(("_stop", ()))
|
| 800 |
+
emitter = self._emitter
|
| 801 |
+
if emitter is not threading.current_thread():
|
| 802 |
+
emitter.join(timeout=2.0)
|
| 803 |
+
|
| 804 |
+
@property
|
| 805 |
+
def running(self):
|
| 806 |
+
return self._opened
|
| 807 |
+
|
| 808 |
+
@property
|
| 809 |
+
def trajectory_path(self):
|
| 810 |
+
"""桥接进程直写的轨迹 txt 路径(open 成功后才非 None)。"""
|
| 811 |
+
return self._trajectory_path
|
collector/core/gripper/control/__init__.py
ADDED
|
@@ -0,0 +1,19 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""夹爪软件判定(机械臂遥操控制已随 teleop 录制一并移除)。"""
|
| 2 |
+
|
| 3 |
+
from .grip_state import (
|
| 4 |
+
GripEvent,
|
| 5 |
+
GripSnapshot,
|
| 6 |
+
GripState,
|
| 7 |
+
clamp_percent,
|
| 8 |
+
firmware_percent,
|
| 9 |
+
normalize_fz_threshold,
|
| 10 |
+
)
|
| 11 |
+
|
| 12 |
+
__all__ = [
|
| 13 |
+
"GripEvent",
|
| 14 |
+
"GripSnapshot",
|
| 15 |
+
"GripState",
|
| 16 |
+
"clamp_percent",
|
| 17 |
+
"firmware_percent",
|
| 18 |
+
"normalize_fz_threshold",
|
| 19 |
+
]
|
collector/core/gripper/control/grip_state.py
ADDED
|
@@ -0,0 +1,233 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""夹爪板状态、百分比与双路 Fz 锁存的唯一状态源。"""
|
| 2 |
+
|
| 3 |
+
from __future__ import annotations
|
| 4 |
+
|
| 5 |
+
from dataclasses import dataclass
|
| 6 |
+
from enum import Enum
|
| 7 |
+
import math
|
| 8 |
+
import threading
|
| 9 |
+
from types import MappingProxyType
|
| 10 |
+
from typing import Mapping, Optional, Sequence, Tuple
|
| 11 |
+
|
| 12 |
+
|
| 13 |
+
DEFAULT_FZ_THRESHOLD_MN = 300
|
| 14 |
+
MIN_FZ_THRESHOLD_MN = 10
|
| 15 |
+
MAX_FZ_THRESHOLD_MN = 5000
|
| 16 |
+
RELEASE_PERCENT_THRESHOLD = 20.0
|
| 17 |
+
RELEASE_CONFIRMATION_COUNT = 3
|
| 18 |
+
ACTIVE_BOARD_STATES = frozenset({"1", "2"})
|
| 19 |
+
|
| 20 |
+
|
| 21 |
+
class GripEvent(str, Enum):
|
| 22 |
+
NONE = "none"
|
| 23 |
+
TRIGGERED = "triggered"
|
| 24 |
+
RELEASED = "released"
|
| 25 |
+
|
| 26 |
+
|
| 27 |
+
@dataclass(frozen=True)
|
| 28 |
+
class GripSnapshot:
|
| 29 |
+
"""供 UI/录制读取的不可变夹爪快照。"""
|
| 30 |
+
|
| 31 |
+
board_state: Mapping[str, str]
|
| 32 |
+
actual: str
|
| 33 |
+
as5600: Mapping[str, str]
|
| 34 |
+
raw_state: str
|
| 35 |
+
raw_actual: str
|
| 36 |
+
raw_as5600: str
|
| 37 |
+
updated: str
|
| 38 |
+
fz_threshold_mn: int
|
| 39 |
+
grip_latched: bool
|
| 40 |
+
release_count: int
|
| 41 |
+
percent: Optional[float]
|
| 42 |
+
raw_angle: Optional[str]
|
| 43 |
+
|
| 44 |
+
|
| 45 |
+
def _freeze_string_mapping(
|
| 46 |
+
values: Optional[Mapping[object, object]],
|
| 47 |
+
) -> Mapping[str, str]:
|
| 48 |
+
return MappingProxyType({
|
| 49 |
+
str(key): str(value) for key, value in (values or {}).items()
|
| 50 |
+
})
|
| 51 |
+
|
| 52 |
+
|
| 53 |
+
def normalize_fz_threshold(value: object) -> int:
|
| 54 |
+
"""按原 UI 契约把阈值限制到 10–5000 mN。"""
|
| 55 |
+
|
| 56 |
+
number = float(value)
|
| 57 |
+
if not math.isfinite(number):
|
| 58 |
+
raise ValueError("Fz threshold must be finite")
|
| 59 |
+
return max(
|
| 60 |
+
MIN_FZ_THRESHOLD_MN,
|
| 61 |
+
min(MAX_FZ_THRESHOLD_MN, int(number)),
|
| 62 |
+
)
|
| 63 |
+
|
| 64 |
+
|
| 65 |
+
def clamp_percent(value: object) -> float:
|
| 66 |
+
number = float(value)
|
| 67 |
+
if not math.isfinite(number):
|
| 68 |
+
raise ValueError("grip percent must be finite")
|
| 69 |
+
return max(0.0, min(100.0, number))
|
| 70 |
+
|
| 71 |
+
|
| 72 |
+
def firmware_percent(
|
| 73 |
+
board_state: Mapping[str, object],
|
| 74 |
+
) -> Tuple[Optional[float], bool, Optional[str]]:
|
| 75 |
+
"""读取固件 ``PCT/GRIP/RAW``,保持原三元组返回契约。"""
|
| 76 |
+
|
| 77 |
+
try:
|
| 78 |
+
percent = clamp_percent(board_state.get("PCT", 0))
|
| 79 |
+
except (TypeError, ValueError):
|
| 80 |
+
return None, False, None
|
| 81 |
+
gripped = str(board_state.get("GRIP", "0")) == "1"
|
| 82 |
+
raw_value = board_state.get("RAW")
|
| 83 |
+
raw_angle = None if raw_value is None else str(raw_value)
|
| 84 |
+
return percent, gripped, raw_angle
|
| 85 |
+
|
| 86 |
+
|
| 87 |
+
def _force_z(force: Optional[Sequence[object]]) -> float:
|
| 88 |
+
if force is None or len(force) < 3:
|
| 89 |
+
return 0.0
|
| 90 |
+
try:
|
| 91 |
+
value = float(force[2])
|
| 92 |
+
except (TypeError, ValueError, OverflowError):
|
| 93 |
+
return 0.0
|
| 94 |
+
return value if math.isfinite(value) else 0.0
|
| 95 |
+
|
| 96 |
+
|
| 97 |
+
class GripState:
|
| 98 |
+
"""唯一写入 board/Fz 锁存状态的线程安全控制对象。
|
| 99 |
+
|
| 100 |
+
该对象内部保存可变状态;消费者只能通过 :meth:`snapshot` 取得深拷贝并
|
| 101 |
+
冻结后的视图,不暴露入口字段转发器。
|
| 102 |
+
"""
|
| 103 |
+
|
| 104 |
+
def __init__(self, fz_threshold_mn: int = DEFAULT_FZ_THRESHOLD_MN) -> None:
|
| 105 |
+
self._lock = threading.RLock()
|
| 106 |
+
self._board_state: dict[str, str] = {}
|
| 107 |
+
self._actual = "--"
|
| 108 |
+
self._as5600: dict[str, str] = {}
|
| 109 |
+
self._raw_state = "--"
|
| 110 |
+
self._raw_actual = "--"
|
| 111 |
+
self._raw_as5600 = "--"
|
| 112 |
+
self._updated = "--"
|
| 113 |
+
self._fz_threshold_mn = normalize_fz_threshold(fz_threshold_mn)
|
| 114 |
+
self._grip_latched = False
|
| 115 |
+
self._release_count = 0
|
| 116 |
+
|
| 117 |
+
def snapshot(self) -> GripSnapshot:
|
| 118 |
+
with self._lock:
|
| 119 |
+
percent, _gripped, raw_angle = firmware_percent(
|
| 120 |
+
self._board_state)
|
| 121 |
+
return GripSnapshot(
|
| 122 |
+
board_state=_freeze_string_mapping(self._board_state),
|
| 123 |
+
actual=self._actual,
|
| 124 |
+
as5600=_freeze_string_mapping(self._as5600),
|
| 125 |
+
raw_state=self._raw_state,
|
| 126 |
+
raw_actual=self._raw_actual,
|
| 127 |
+
raw_as5600=self._raw_as5600,
|
| 128 |
+
updated=self._updated,
|
| 129 |
+
fz_threshold_mn=self._fz_threshold_mn,
|
| 130 |
+
grip_latched=self._grip_latched,
|
| 131 |
+
release_count=self._release_count,
|
| 132 |
+
percent=percent,
|
| 133 |
+
raw_angle=raw_angle,
|
| 134 |
+
)
|
| 135 |
+
|
| 136 |
+
def replace_board_snapshot(
|
| 137 |
+
self,
|
| 138 |
+
board_snapshot: Mapping[str, object],
|
| 139 |
+
) -> GripSnapshot:
|
| 140 |
+
"""原子接收串口解析后的完整 board 快照。"""
|
| 141 |
+
|
| 142 |
+
nested = board_snapshot.get("state", {})
|
| 143 |
+
state_values = nested if isinstance(nested, Mapping) else {}
|
| 144 |
+
as5600 = board_snapshot.get("as5600", {})
|
| 145 |
+
as5600_values = as5600 if isinstance(as5600, Mapping) else {}
|
| 146 |
+
with self._lock:
|
| 147 |
+
self._board_state = {
|
| 148 |
+
str(key): str(value) for key, value in state_values.items()
|
| 149 |
+
}
|
| 150 |
+
if self._grip_latched:
|
| 151 |
+
self._board_state["GRIP"] = "1"
|
| 152 |
+
self._actual = str(board_snapshot.get("actual", "--"))
|
| 153 |
+
self._as5600 = {
|
| 154 |
+
str(key): str(value) for key, value in as5600_values.items()
|
| 155 |
+
}
|
| 156 |
+
self._raw_state = str(board_snapshot.get("raw_state", "--"))
|
| 157 |
+
self._raw_actual = str(board_snapshot.get("raw_actual", "--"))
|
| 158 |
+
self._raw_as5600 = str(board_snapshot.get("raw_as5600", "--"))
|
| 159 |
+
self._updated = str(board_snapshot.get("updated", "--"))
|
| 160 |
+
return self.snapshot()
|
| 161 |
+
|
| 162 |
+
def update_firmware_state(
|
| 163 |
+
self,
|
| 164 |
+
values: Mapping[str, object],
|
| 165 |
+
*,
|
| 166 |
+
updated: Optional[object] = None,
|
| 167 |
+
) -> GripSnapshot:
|
| 168 |
+
"""只替换固件 ``STATE`` 字段,供串口状态循环使用。"""
|
| 169 |
+
|
| 170 |
+
with self._lock:
|
| 171 |
+
self._board_state = {
|
| 172 |
+
str(key): str(value) for key, value in values.items()
|
| 173 |
+
}
|
| 174 |
+
if self._grip_latched:
|
| 175 |
+
self._board_state["GRIP"] = "1"
|
| 176 |
+
if updated is not None:
|
| 177 |
+
self._updated = str(updated)
|
| 178 |
+
return self.snapshot()
|
| 179 |
+
|
| 180 |
+
def set_fz_threshold(self, value: object) -> int:
|
| 181 |
+
threshold = normalize_fz_threshold(value)
|
| 182 |
+
with self._lock:
|
| 183 |
+
self._fz_threshold_mn = threshold
|
| 184 |
+
return threshold
|
| 185 |
+
|
| 186 |
+
def evaluate_forces(
|
| 187 |
+
self,
|
| 188 |
+
left_force: Optional[Sequence[object]],
|
| 189 |
+
right_force: Optional[Sequence[object]],
|
| 190 |
+
) -> GripEvent:
|
| 191 |
+
"""应用双路 Fz 阈值、锁存和连续三次张开释放滞后。"""
|
| 192 |
+
|
| 193 |
+
left_fz = _force_z(left_force)
|
| 194 |
+
right_fz = _force_z(right_force)
|
| 195 |
+
with self._lock:
|
| 196 |
+
state_code = self._board_state.get("ST", "0")
|
| 197 |
+
if state_code not in ACTIVE_BOARD_STATES:
|
| 198 |
+
return GripEvent.NONE
|
| 199 |
+
try:
|
| 200 |
+
percent = clamp_percent(
|
| 201 |
+
self._board_state.get("PCT", "0"))
|
| 202 |
+
except (TypeError, ValueError):
|
| 203 |
+
return GripEvent.NONE
|
| 204 |
+
|
| 205 |
+
if self._grip_latched:
|
| 206 |
+
if percent < RELEASE_PERCENT_THRESHOLD:
|
| 207 |
+
self._release_count += 1
|
| 208 |
+
if self._release_count >= RELEASE_CONFIRMATION_COUNT:
|
| 209 |
+
self._grip_latched = False
|
| 210 |
+
self._release_count = 0
|
| 211 |
+
self._board_state.pop("GRIP", None)
|
| 212 |
+
return GripEvent.RELEASED
|
| 213 |
+
else:
|
| 214 |
+
self._release_count = 0
|
| 215 |
+
return GripEvent.NONE
|
| 216 |
+
|
| 217 |
+
has_force = (
|
| 218 |
+
left_fz >= self._fz_threshold_mn
|
| 219 |
+
or right_fz >= self._fz_threshold_mn
|
| 220 |
+
)
|
| 221 |
+
if not has_force:
|
| 222 |
+
return GripEvent.NONE
|
| 223 |
+
|
| 224 |
+
self._grip_latched = True
|
| 225 |
+
self._release_count = 0
|
| 226 |
+
self._board_state["GRIP"] = "1"
|
| 227 |
+
return GripEvent.TRIGGERED
|
| 228 |
+
|
| 229 |
+
def reset_latch(self) -> None:
|
| 230 |
+
with self._lock:
|
| 231 |
+
self._grip_latched = False
|
| 232 |
+
self._release_count = 0
|
| 233 |
+
self._board_state.pop("GRIP", None)
|
collector/core/gripper/devices/__init__.py
ADDED
|
@@ -0,0 +1,37 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""KSQ 夹爪硬件适配器。
|
| 2 |
+
|
| 3 |
+
设备模块不持有应用或 Tk 对象。串口、Sightac 句柄和触觉计算子进程分别只有
|
| 4 |
+
这里声明的控制器能够创建和关闭,其他子系统只读取不可变快照或调用公开 API。
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from .gripper_connection import (
|
| 8 |
+
ConnectionSnapshot,
|
| 9 |
+
ConnectionState,
|
| 10 |
+
GripperConnectionController,
|
| 11 |
+
)
|
| 12 |
+
from .gripper_serial import GripperSerial
|
| 13 |
+
from .serial_command_worker import (
|
| 14 |
+
SerialCommandCancelled,
|
| 15 |
+
SerialCommandWorker,
|
| 16 |
+
)
|
| 17 |
+
from .tactile_discovery import TactileDiscovery
|
| 18 |
+
from .tactile_process_manager import (
|
| 19 |
+
TactileProcessBridge,
|
| 20 |
+
TactileProcessManager,
|
| 21 |
+
TactileSnapshot,
|
| 22 |
+
TactileState,
|
| 23 |
+
)
|
| 24 |
+
|
| 25 |
+
__all__ = [
|
| 26 |
+
"ConnectionSnapshot",
|
| 27 |
+
"ConnectionState",
|
| 28 |
+
"GripperConnectionController",
|
| 29 |
+
"GripperSerial",
|
| 30 |
+
"SerialCommandCancelled",
|
| 31 |
+
"SerialCommandWorker",
|
| 32 |
+
"TactileDiscovery",
|
| 33 |
+
"TactileProcessBridge",
|
| 34 |
+
"TactileProcessManager",
|
| 35 |
+
"TactileSnapshot",
|
| 36 |
+
"TactileState",
|
| 37 |
+
]
|
collector/core/gripper/devices/gripper_connection.py
ADDED
|
@@ -0,0 +1,717 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""夹爪控制板连接、响应解析和三组定时循环。"""
|
| 2 |
+
|
| 3 |
+
from __future__ import annotations
|
| 4 |
+
|
| 5 |
+
from dataclasses import dataclass
|
| 6 |
+
import threading
|
| 7 |
+
import time
|
| 8 |
+
from types import MappingProxyType
|
| 9 |
+
from typing import Callable, Mapping, Optional, Protocol
|
| 10 |
+
|
| 11 |
+
from .gripper_serial import GripperSerial
|
| 12 |
+
from .serial_command_worker import SerialCommandWorker
|
| 13 |
+
|
| 14 |
+
|
| 15 |
+
UI_LOOP_MS = 33
|
| 16 |
+
SLOW_LOOP_MS = 150
|
| 17 |
+
FAST_GRIP_LOOP_MS = 10
|
| 18 |
+
RESET_EVENT_DEBOUNCE_S = 3.0
|
| 19 |
+
# ESP32 declares the host offline after 1.5 s without a signal (HEARTBEAT
|
| 20 |
+
# in main.cpp). The host-driven "?" poll runs on its own daemon thread so a
|
| 21 |
+
# stalled Tk main thread (e.g. a long synchronous save) cannot starve it and
|
| 22 |
+
# disconnect the physical record button.
|
| 23 |
+
HEARTBEAT_PERIOD_S = 1.0 / 30.0
|
| 24 |
+
|
| 25 |
+
|
| 26 |
+
class SensorLifecycle(Protocol):
|
| 27 |
+
"""Connection controller 唯一允许调用的跨设备生命周期接口。"""
|
| 28 |
+
|
| 29 |
+
def prepare_connect(self) -> object:
|
| 30 |
+
"""只读预检并完成 Open 资源交接;失败应抛异常或返回 ``False``。"""
|
| 31 |
+
|
| 32 |
+
def connected(self) -> object:
|
| 33 |
+
"""串口握手后启动 Fays/SLAM,并重新初始化左右 Sightac。"""
|
| 34 |
+
|
| 35 |
+
def connect_failed(self) -> object:
|
| 36 |
+
"""收回部分 Connect 资源,并按需恢复发起前的 Open 状态。"""
|
| 37 |
+
|
| 38 |
+
def verify_prepared_control_port(self, device_path: str) -> object:
|
| 39 |
+
"""确认交接后串口节点仍指向预检时的同一 ESP32。"""
|
| 40 |
+
|
| 41 |
+
def disconnect(self) -> object:
|
| 42 |
+
"""按依赖逆序停止 SLAM、Sightac 和预览资源。"""
|
| 43 |
+
|
| 44 |
+
|
| 45 |
+
@dataclass(frozen=True)
|
| 46 |
+
class ConnectionSnapshot:
|
| 47 |
+
"""供 UI、录制和诊断读取的不可变连接快照。"""
|
| 48 |
+
|
| 49 |
+
serial_generation: int
|
| 50 |
+
connecting: bool
|
| 51 |
+
connected: bool
|
| 52 |
+
no_esp: bool
|
| 53 |
+
loop_token: int
|
| 54 |
+
connect_time: float
|
| 55 |
+
board_fields: Mapping[str, str]
|
| 56 |
+
actual: str
|
| 57 |
+
as5600: Mapping[str, str]
|
| 58 |
+
raw_state: str
|
| 59 |
+
raw_actual: str
|
| 60 |
+
raw_as5600: str
|
| 61 |
+
updated: str
|
| 62 |
+
error: Optional[str]
|
| 63 |
+
|
| 64 |
+
|
| 65 |
+
class ConnectionState:
|
| 66 |
+
"""连接状态的唯一可变源;只由 GripperConnectionController 写入。"""
|
| 67 |
+
|
| 68 |
+
def __init__(self):
|
| 69 |
+
self.lock = threading.RLock()
|
| 70 |
+
self.serial_generation = 0
|
| 71 |
+
self.connecting = False
|
| 72 |
+
self.connected = False
|
| 73 |
+
self.no_esp = False
|
| 74 |
+
self.loop_token = 0
|
| 75 |
+
self.connect_time = 0.0
|
| 76 |
+
self.attempt_generation = 0
|
| 77 |
+
self.error = None
|
| 78 |
+
self.board_state = {
|
| 79 |
+
"state": {},
|
| 80 |
+
"actual": "--",
|
| 81 |
+
"as5600": {},
|
| 82 |
+
"raw_state": "--",
|
| 83 |
+
"raw_actual": "--",
|
| 84 |
+
"raw_as5600": "--",
|
| 85 |
+
"updated": "--",
|
| 86 |
+
}
|
| 87 |
+
|
| 88 |
+
def snapshot(self) -> ConnectionSnapshot:
|
| 89 |
+
with self.lock:
|
| 90 |
+
fields = MappingProxyType(dict(self.board_state["state"]))
|
| 91 |
+
return ConnectionSnapshot(
|
| 92 |
+
serial_generation=int(self.serial_generation),
|
| 93 |
+
connecting=bool(self.connecting),
|
| 94 |
+
connected=bool(self.connected),
|
| 95 |
+
no_esp=bool(self.no_esp),
|
| 96 |
+
loop_token=int(self.loop_token),
|
| 97 |
+
connect_time=float(self.connect_time),
|
| 98 |
+
board_fields=fields,
|
| 99 |
+
actual=str(self.board_state["actual"]),
|
| 100 |
+
as5600=MappingProxyType(
|
| 101 |
+
dict(self.board_state["as5600"])),
|
| 102 |
+
raw_state=str(self.board_state["raw_state"]),
|
| 103 |
+
raw_actual=str(self.board_state["raw_actual"]),
|
| 104 |
+
raw_as5600=str(self.board_state["raw_as5600"]),
|
| 105 |
+
updated=str(self.board_state["updated"]),
|
| 106 |
+
error=None if self.error is None else str(self.error),
|
| 107 |
+
)
|
| 108 |
+
|
| 109 |
+
|
| 110 |
+
class GripperConnectionController:
|
| 111 |
+
"""协调串口连接,且仅通过注入的 ``lifecycle`` 管理其他设备。
|
| 112 |
+
|
| 113 |
+
本类不持有 App、Tk widget、相机或 SLAM 句柄。UI 调度、定时器和业务事件都
|
| 114 |
+
通过回调注入。
|
| 115 |
+
"""
|
| 116 |
+
|
| 117 |
+
UI_LOOP_MS = UI_LOOP_MS
|
| 118 |
+
SLOW_LOOP_MS = SLOW_LOOP_MS
|
| 119 |
+
FAST_GRIP_LOOP_MS = FAST_GRIP_LOOP_MS
|
| 120 |
+
|
| 121 |
+
def __init__(
|
| 122 |
+
self,
|
| 123 |
+
state: ConnectionState,
|
| 124 |
+
serial_owner: GripperSerial,
|
| 125 |
+
serial_commands: SerialCommandWorker,
|
| 126 |
+
lifecycle: SensorLifecycle,
|
| 127 |
+
*,
|
| 128 |
+
dispatch: Optional[Callable[..., None]] = None,
|
| 129 |
+
schedule: Optional[Callable[[int, Callable[[], None]], object]] = None,
|
| 130 |
+
on_state: Optional[Callable[[ConnectionSnapshot], None]] = None,
|
| 131 |
+
on_board_update: Optional[Callable[[ConnectionSnapshot], None]] = None,
|
| 132 |
+
on_ui_tick: Optional[Callable[[], None]] = None,
|
| 133 |
+
on_grip_check: Optional[Callable[[], None]] = None,
|
| 134 |
+
on_reset_event: Optional[Callable[[], None]] = None,
|
| 135 |
+
on_stop_event: Optional[Callable[[], None]] = None,
|
| 136 |
+
thread_factory: Callable[..., threading.Thread] = threading.Thread,
|
| 137 |
+
clock: Callable[[], float] = time.monotonic,
|
| 138 |
+
wall_clock: Callable[[], float] = time.time,
|
| 139 |
+
logger: Callable[..., None] = print,
|
| 140 |
+
):
|
| 141 |
+
if lifecycle is None:
|
| 142 |
+
raise ValueError("an injected sensor lifecycle is required")
|
| 143 |
+
self.state = state
|
| 144 |
+
self.serial = serial_owner
|
| 145 |
+
self.serial_commands = serial_commands
|
| 146 |
+
self.lifecycle = lifecycle
|
| 147 |
+
self.dispatch = dispatch or (
|
| 148 |
+
lambda callback, *args: callback(*args))
|
| 149 |
+
self.schedule = schedule or (
|
| 150 |
+
lambda _delay_ms, callback: None)
|
| 151 |
+
self.on_state = on_state
|
| 152 |
+
self.on_board_update = on_board_update
|
| 153 |
+
self.on_ui_tick = on_ui_tick
|
| 154 |
+
self.on_grip_check = on_grip_check
|
| 155 |
+
self.on_reset_event = on_reset_event
|
| 156 |
+
self.on_stop_event = on_stop_event
|
| 157 |
+
self._thread_factory = thread_factory
|
| 158 |
+
self._clock = clock
|
| 159 |
+
self._wall_clock = wall_clock
|
| 160 |
+
self._logger = logger
|
| 161 |
+
self._transition_lock = threading.RLock()
|
| 162 |
+
self._connect_thread = None
|
| 163 |
+
self._heartbeat_stop = None
|
| 164 |
+
self._heartbeat_thread = None
|
| 165 |
+
self._grip_stop = None
|
| 166 |
+
self._grip_thread = None
|
| 167 |
+
|
| 168 |
+
def snapshot(self) -> ConnectionSnapshot:
|
| 169 |
+
return self.state.snapshot()
|
| 170 |
+
|
| 171 |
+
def start(self) -> bool:
|
| 172 |
+
return self.serial_commands.start()
|
| 173 |
+
|
| 174 |
+
def clear_serial_queue(self) -> int:
|
| 175 |
+
return self.serial_commands.clear()
|
| 176 |
+
|
| 177 |
+
def stop_serial_worker(self) -> bool:
|
| 178 |
+
return self.serial_commands.stop()
|
| 179 |
+
|
| 180 |
+
def invalidate_loops(self) -> None:
|
| 181 |
+
with self.state.lock:
|
| 182 |
+
self.state.loop_token += 1
|
| 183 |
+
|
| 184 |
+
def refresh_ports(self):
|
| 185 |
+
return self.serial.list_ports()
|
| 186 |
+
|
| 187 |
+
def _publish_state(self) -> None:
|
| 188 |
+
if self.on_state is not None:
|
| 189 |
+
snapshot = self.snapshot()
|
| 190 |
+
try:
|
| 191 |
+
self.dispatch(self.on_state, snapshot)
|
| 192 |
+
except Exception:
|
| 193 |
+
pass
|
| 194 |
+
|
| 195 |
+
def _lifecycle_call(self, method_name: str, **kwargs):
|
| 196 |
+
method = getattr(self.lifecycle, method_name, None)
|
| 197 |
+
if not callable(method):
|
| 198 |
+
raise RuntimeError(
|
| 199 |
+
f"sensor lifecycle has no {method_name}() method")
|
| 200 |
+
result = method(**kwargs)
|
| 201 |
+
if result is False:
|
| 202 |
+
raise RuntimeError(
|
| 203 |
+
f"sensor lifecycle {method_name} rejected transition")
|
| 204 |
+
return result
|
| 205 |
+
|
| 206 |
+
def connect(self, port: str, baud: int = 115200) -> bool:
|
| 207 |
+
"""异步执行预检/交接和串口握手。"""
|
| 208 |
+
if not str(port).strip():
|
| 209 |
+
raise ValueError("serial port is required")
|
| 210 |
+
with self.state.lock:
|
| 211 |
+
if self.state.connecting or self.state.connected:
|
| 212 |
+
return False
|
| 213 |
+
self.state.connecting = True
|
| 214 |
+
self.state.error = None
|
| 215 |
+
self.state.attempt_generation += 1
|
| 216 |
+
attempt = self.state.attempt_generation
|
| 217 |
+
self._publish_state()
|
| 218 |
+
thread = self._thread_factory(
|
| 219 |
+
target=self._connect_worker,
|
| 220 |
+
args=(str(port), int(baud), attempt),
|
| 221 |
+
daemon=True,
|
| 222 |
+
name=f"gripper-connect-{attempt}",
|
| 223 |
+
)
|
| 224 |
+
self._connect_thread = thread
|
| 225 |
+
thread.start()
|
| 226 |
+
return True
|
| 227 |
+
|
| 228 |
+
def connect_sync(self, port: str, baud: int = 115200) -> bool:
|
| 229 |
+
"""测试和无 GUI 调用使用的同步连接入口,顺序与异步入口完全相同。"""
|
| 230 |
+
if not str(port).strip():
|
| 231 |
+
raise ValueError("serial port is required")
|
| 232 |
+
with self.state.lock:
|
| 233 |
+
if self.state.connecting or self.state.connected:
|
| 234 |
+
return False
|
| 235 |
+
self.state.connecting = True
|
| 236 |
+
self.state.error = None
|
| 237 |
+
self.state.attempt_generation += 1
|
| 238 |
+
attempt = self.state.attempt_generation
|
| 239 |
+
self._publish_state()
|
| 240 |
+
ok, error = self._perform_connect(str(port), int(baud), attempt)
|
| 241 |
+
return self._finish_connect(attempt, ok, error)
|
| 242 |
+
|
| 243 |
+
def connect_no_esp(self) -> bool:
|
| 244 |
+
"""Start Fays/SLAM diagnostic mode without opening the ESP serial."""
|
| 245 |
+
with self.state.lock:
|
| 246 |
+
if self.state.connecting or self.state.connected:
|
| 247 |
+
return False
|
| 248 |
+
self.state.connecting = True
|
| 249 |
+
self.state.no_esp = True
|
| 250 |
+
self.state.error = None
|
| 251 |
+
self.state.attempt_generation += 1
|
| 252 |
+
attempt = self.state.attempt_generation
|
| 253 |
+
self._publish_state()
|
| 254 |
+
thread = self._thread_factory(
|
| 255 |
+
target=self._connect_no_esp_worker,
|
| 256 |
+
args=(attempt,),
|
| 257 |
+
daemon=True,
|
| 258 |
+
name=f"fays-no-esp-connect-{attempt}",
|
| 259 |
+
)
|
| 260 |
+
self._connect_thread = thread
|
| 261 |
+
thread.start()
|
| 262 |
+
return True
|
| 263 |
+
|
| 264 |
+
def toggle_no_esp(self) -> bool:
|
| 265 |
+
snapshot = self.snapshot()
|
| 266 |
+
if snapshot.connecting or snapshot.connected:
|
| 267 |
+
return self.disconnect()
|
| 268 |
+
return self.connect_no_esp()
|
| 269 |
+
|
| 270 |
+
def connect_no_esp_sync(self) -> bool:
|
| 271 |
+
"""Headless/test adapter for the no-ESP diagnostic path."""
|
| 272 |
+
with self.state.lock:
|
| 273 |
+
if self.state.connecting or self.state.connected:
|
| 274 |
+
return False
|
| 275 |
+
self.state.connecting = True
|
| 276 |
+
self.state.no_esp = True
|
| 277 |
+
self.state.error = None
|
| 278 |
+
self.state.attempt_generation += 1
|
| 279 |
+
attempt = self.state.attempt_generation
|
| 280 |
+
self._publish_state()
|
| 281 |
+
ok, error = self._perform_connect_no_esp(attempt)
|
| 282 |
+
return self._finish_connect_no_esp(attempt, ok, error)
|
| 283 |
+
|
| 284 |
+
def _connect_worker(self, port: str, baud: int, attempt: int) -> None:
|
| 285 |
+
# ESP handshakes and SDK startup can block for seconds. Keep the full
|
| 286 |
+
# transition off Tk, not just its preflight half.
|
| 287 |
+
with self._transition_lock:
|
| 288 |
+
ok, error = self._perform_connect(port, baud, attempt)
|
| 289 |
+
self._finish_connect(attempt, ok, error)
|
| 290 |
+
|
| 291 |
+
def _connect_no_esp_worker(self, attempt: int) -> None:
|
| 292 |
+
with self._transition_lock:
|
| 293 |
+
ok, error = self._perform_connect_no_esp(attempt)
|
| 294 |
+
self._finish_connect_no_esp(attempt, ok, error)
|
| 295 |
+
|
| 296 |
+
def _perform_connect(self, port: str, baud: int, attempt: int):
|
| 297 |
+
self._logger(f"[Connect] Preparing Fays/Sightac handoff ...")
|
| 298 |
+
try:
|
| 299 |
+
# 必须先完成 Fays 身份预检和 Open 资源释放,失败时不得打开串口。
|
| 300 |
+
self._lifecycle_call("prepare_connect")
|
| 301 |
+
with self.state.lock:
|
| 302 |
+
if attempt != self.state.attempt_generation:
|
| 303 |
+
raise RuntimeError("connection attempt invalidated")
|
| 304 |
+
verify = getattr(
|
| 305 |
+
self.lifecycle, "verify_prepared_control_port", None
|
| 306 |
+
)
|
| 307 |
+
if callable(verify) and verify(port) is False:
|
| 308 |
+
raise RuntimeError(
|
| 309 |
+
"ESP32 control port identity verification failed"
|
| 310 |
+
)
|
| 311 |
+
self._logger(f"[Connect] Trying {port}...")
|
| 312 |
+
ok = self.serial.connect(port, baud)
|
| 313 |
+
self._logger(f"[Connect] Result: {'OK' if ok else 'FAIL'}")
|
| 314 |
+
serial_error = getattr(self.serial, "last_error", None)
|
| 315 |
+
return ok, None if ok else (
|
| 316 |
+
serial_error or "serial handshake timeout"
|
| 317 |
+
)
|
| 318 |
+
except Exception as exc:
|
| 319 |
+
return False, str(exc)
|
| 320 |
+
|
| 321 |
+
def _perform_connect_no_esp(self, attempt: int):
|
| 322 |
+
self._logger("[Connect no_esp] Preparing Fays handoff ...")
|
| 323 |
+
try:
|
| 324 |
+
self._lifecycle_call("prepare_connect")
|
| 325 |
+
with self.state.lock:
|
| 326 |
+
if attempt != self.state.attempt_generation:
|
| 327 |
+
raise RuntimeError("connection attempt invalidated")
|
| 328 |
+
self._lifecycle_call("connected", fake_imu=True)
|
| 329 |
+
return True, None
|
| 330 |
+
except Exception as exc:
|
| 331 |
+
return False, str(exc)
|
| 332 |
+
|
| 333 |
+
def _finish_connect_no_esp(
|
| 334 |
+
self, attempt: int, ok: bool, error: Optional[str],
|
| 335 |
+
) -> bool:
|
| 336 |
+
with self.state.lock:
|
| 337 |
+
current = attempt == self.state.attempt_generation
|
| 338 |
+
if not current:
|
| 339 |
+
return False
|
| 340 |
+
if ok:
|
| 341 |
+
now = self._clock()
|
| 342 |
+
with self.state.lock:
|
| 343 |
+
self.state.connecting = False
|
| 344 |
+
self.state.connected = True
|
| 345 |
+
self.state.no_esp = True
|
| 346 |
+
self.state.loop_token += 1
|
| 347 |
+
self.state.connect_time = now
|
| 348 |
+
self.state.error = None
|
| 349 |
+
token = self.state.loop_token
|
| 350 |
+
self.dispatch(self.update_loops, token)
|
| 351 |
+
self._publish_state()
|
| 352 |
+
self._logger(
|
| 353 |
+
"[Connect no_esp] SLAM started with diagnostic fake IMU")
|
| 354 |
+
return True
|
| 355 |
+
|
| 356 |
+
rollback_error = None
|
| 357 |
+
try:
|
| 358 |
+
self._lifecycle_call("connect_failed")
|
| 359 |
+
except Exception as exc:
|
| 360 |
+
rollback_error = str(exc)
|
| 361 |
+
failure = error or "no-ESP diagnostic connection failed"
|
| 362 |
+
if rollback_error:
|
| 363 |
+
failure = f"{failure}; rollback failed: {rollback_error}"
|
| 364 |
+
with self.state.lock:
|
| 365 |
+
self.state.connecting = False
|
| 366 |
+
self.state.connected = False
|
| 367 |
+
self.state.no_esp = False
|
| 368 |
+
self.state.loop_token += 1
|
| 369 |
+
self.state.error = failure
|
| 370 |
+
self._publish_state()
|
| 371 |
+
return False
|
| 372 |
+
|
| 373 |
+
def _finish_connect(
|
| 374 |
+
self, attempt: int, ok: bool, error: Optional[str],
|
| 375 |
+
) -> bool:
|
| 376 |
+
with self.state.lock:
|
| 377 |
+
current = attempt == self.state.attempt_generation
|
| 378 |
+
if not current:
|
| 379 |
+
if ok:
|
| 380 |
+
self.serial.disconnect()
|
| 381 |
+
return False
|
| 382 |
+
|
| 383 |
+
if ok and self.serial.is_connected and self.serial_commands.is_running:
|
| 384 |
+
try:
|
| 385 |
+
# 新协议握手:先让 ESP32 进入“已连接/等待初始化”,再通知
|
| 386 |
+
# “初始化开始”,随后由 lifecycle 完成 SDK/SLAM/触觉初始化。
|
| 387 |
+
if not self.send_control_signal("C"):
|
| 388 |
+
raise RuntimeError(
|
| 389 |
+
"ESP32 connect signal (C) rejected")
|
| 390 |
+
if not self.send_control_signal("I"):
|
| 391 |
+
raise RuntimeError(
|
| 392 |
+
"ESP32 init-start signal (I) rejected")
|
| 393 |
+
self._lifecycle_call("connected")
|
| 394 |
+
except Exception as exc:
|
| 395 |
+
# 初始化失败时通知 ESP32 进入异常状态(红闪)。
|
| 396 |
+
try:
|
| 397 |
+
self.send_control_signal("E")
|
| 398 |
+
except Exception:
|
| 399 |
+
pass
|
| 400 |
+
error = str(exc)
|
| 401 |
+
ok = False
|
| 402 |
+
|
| 403 |
+
if ok and self.serial.is_connected and self.serial_commands.is_running:
|
| 404 |
+
try:
|
| 405 |
+
# 初始化完成 → ESP32 进入“等待开始录制”(绿常亮)。
|
| 406 |
+
if not self.send_control_signal("F"):
|
| 407 |
+
raise RuntimeError(
|
| 408 |
+
"ESP32 init-complete signal (F) rejected")
|
| 409 |
+
except Exception as exc:
|
| 410 |
+
try:
|
| 411 |
+
self.send_control_signal("E")
|
| 412 |
+
except Exception:
|
| 413 |
+
pass
|
| 414 |
+
error = str(exc)
|
| 415 |
+
ok = False
|
| 416 |
+
|
| 417 |
+
if ok and self.serial.is_connected and self.serial_commands.is_running:
|
| 418 |
+
generation = self.serial_commands.advance_generation()
|
| 419 |
+
now = self._clock()
|
| 420 |
+
with self.state.lock:
|
| 421 |
+
if attempt != self.state.attempt_generation:
|
| 422 |
+
# A concurrent Cancel has invalidated this transition;
|
| 423 |
+
# its disconnect worker owns cleanup under _transition_lock.
|
| 424 |
+
return False
|
| 425 |
+
self.state.connecting = False
|
| 426 |
+
self.state.connected = True
|
| 427 |
+
self.state.no_esp = False
|
| 428 |
+
self.state.serial_generation = generation
|
| 429 |
+
self.state.loop_token += 1
|
| 430 |
+
self.state.connect_time = now
|
| 431 |
+
self.state.error = None
|
| 432 |
+
token = self.state.loop_token
|
| 433 |
+
self._init_fields()
|
| 434 |
+
self.dispatch(self.update_loops, token)
|
| 435 |
+
self._publish_state()
|
| 436 |
+
return True
|
| 437 |
+
|
| 438 |
+
rollback_error = None
|
| 439 |
+
try:
|
| 440 |
+
self._lifecycle_call("connect_failed")
|
| 441 |
+
except Exception as exc:
|
| 442 |
+
rollback_error = str(exc)
|
| 443 |
+
self._logger(
|
| 444 |
+
f"[Connect] Open rollback failed: {rollback_error}")
|
| 445 |
+
self.serial.disconnect()
|
| 446 |
+
failure = error or "connection failed"
|
| 447 |
+
if rollback_error:
|
| 448 |
+
failure = (
|
| 449 |
+
f"{failure}; Open rollback failed: {rollback_error}"
|
| 450 |
+
)
|
| 451 |
+
with self.state.lock:
|
| 452 |
+
self.state.connecting = False
|
| 453 |
+
self.state.connected = False
|
| 454 |
+
self.state.no_esp = False
|
| 455 |
+
self.state.loop_token += 1
|
| 456 |
+
self.state.error = failure
|
| 457 |
+
self._publish_state()
|
| 458 |
+
return False
|
| 459 |
+
|
| 460 |
+
def disconnect(self) -> bool:
|
| 461 |
+
"""停止注入 lifecycle 后断开串口,并使全部旧循环/命令失效。"""
|
| 462 |
+
with self.state.lock:
|
| 463 |
+
was_active = self.state.connecting or self.state.connected
|
| 464 |
+
self.state.attempt_generation += 1
|
| 465 |
+
self.state.loop_token += 1
|
| 466 |
+
self.state.connecting = False
|
| 467 |
+
self.state.connected = False
|
| 468 |
+
self.state.no_esp = False
|
| 469 |
+
self.state.connect_time = 0.0
|
| 470 |
+
token = self.state.loop_token
|
| 471 |
+
generation = self.serial_commands.clear()
|
| 472 |
+
self._stop_heartbeat()
|
| 473 |
+
self._stop_grip_checks()
|
| 474 |
+
with self._transition_lock:
|
| 475 |
+
error = None
|
| 476 |
+
try:
|
| 477 |
+
self._lifecycle_call("disconnect")
|
| 478 |
+
except Exception as exc:
|
| 479 |
+
error = str(exc)
|
| 480 |
+
self.serial.disconnect()
|
| 481 |
+
with self.state.lock:
|
| 482 |
+
self.state.serial_generation = generation
|
| 483 |
+
self.state.loop_token = token
|
| 484 |
+
self.state.error = error
|
| 485 |
+
self._publish_state()
|
| 486 |
+
return was_active and error is None
|
| 487 |
+
|
| 488 |
+
def close(self) -> None:
|
| 489 |
+
"""幂等停止连接、单 worker 和串口 owner。"""
|
| 490 |
+
self.disconnect()
|
| 491 |
+
self.serial_commands.stop()
|
| 492 |
+
self.serial.disconnect()
|
| 493 |
+
|
| 494 |
+
def send_async(
|
| 495 |
+
self,
|
| 496 |
+
command: str,
|
| 497 |
+
callback: Optional[Callable[[str], None]] = None,
|
| 498 |
+
key: Optional[str] = None,
|
| 499 |
+
mode: str = "queue",
|
| 500 |
+
) -> bool:
|
| 501 |
+
return self.serial_commands.submit(
|
| 502 |
+
command, callback=callback, key=key, mode=mode)
|
| 503 |
+
|
| 504 |
+
def send_sync(
|
| 505 |
+
self,
|
| 506 |
+
command: str,
|
| 507 |
+
*,
|
| 508 |
+
timeout: Optional[float] = None,
|
| 509 |
+
key: Optional[str] = None,
|
| 510 |
+
mode: str = "queue",
|
| 511 |
+
) -> str:
|
| 512 |
+
return self.serial_commands.execute(
|
| 513 |
+
command, timeout=timeout, key=key, mode=mode)
|
| 514 |
+
|
| 515 |
+
def send_control_signal(
|
| 516 |
+
self, command: str, *, timeout: float = 1.5,
|
| 517 |
+
) -> bool:
|
| 518 |
+
"""Send one host→ESP32 control signal (C/I/F/E) through the worker.
|
| 519 |
+
|
| 520 |
+
Returns ``False`` when the signal is rejected, times out, or the
|
| 521 |
+
worker is not running. The ESP32 replies ``OK <NAME>`` on success.
|
| 522 |
+
"""
|
| 523 |
+
try:
|
| 524 |
+
response = self.serial_commands.execute(
|
| 525 |
+
command, timeout=timeout, mode="queue")
|
| 526 |
+
except Exception:
|
| 527 |
+
return False
|
| 528 |
+
return bool(response) and not response.startswith("ERR")
|
| 529 |
+
|
| 530 |
+
def request_as5048a(
|
| 531 |
+
self, callback: Optional[Callable[[Mapping[str, str]], None]] = None,
|
| 532 |
+
) -> bool:
|
| 533 |
+
"""异步读取 AS5048A;不绕过唯一串口 worker。"""
|
| 534 |
+
def handled(response: str) -> None:
|
| 535 |
+
values = self.parse_as5048a(response)
|
| 536 |
+
if callback is not None:
|
| 537 |
+
callback(values)
|
| 538 |
+
|
| 539 |
+
return self.send_async(
|
| 540 |
+
"G", handled, key="as5048a", mode="skip")
|
| 541 |
+
|
| 542 |
+
def get_as5048a(self, timeout: Optional[float] = None):
|
| 543 |
+
"""同步读取 AS5048A,同样经过唯一串口 worker。"""
|
| 544 |
+
return self.parse_as5048a(
|
| 545 |
+
self.send_sync("G", timeout=timeout))
|
| 546 |
+
|
| 547 |
+
@staticmethod
|
| 548 |
+
def parse_as5048a(response: str):
|
| 549 |
+
if not response or not response.startswith("AS5048A"):
|
| 550 |
+
return MappingProxyType({})
|
| 551 |
+
return MappingProxyType(dict(
|
| 552 |
+
item.split("=", 1)
|
| 553 |
+
for item in response.split()
|
| 554 |
+
if "=" in item
|
| 555 |
+
))
|
| 556 |
+
|
| 557 |
+
def _init_fields(self) -> None:
|
| 558 |
+
self.send_async("?", self._handle_state_response)
|
| 559 |
+
|
| 560 |
+
@staticmethod
|
| 561 |
+
def parse_pairs(line: str):
|
| 562 |
+
values = {}
|
| 563 |
+
for part in str(line).split():
|
| 564 |
+
if "=" in part:
|
| 565 |
+
key, value = part.split("=", 1)
|
| 566 |
+
values[key] = value
|
| 567 |
+
return values
|
| 568 |
+
|
| 569 |
+
def _handle_state_response(self, response: str) -> None:
|
| 570 |
+
response = response or "--"
|
| 571 |
+
event = ""
|
| 572 |
+
fields = None
|
| 573 |
+
with self.state.lock:
|
| 574 |
+
board = self.state.board_state
|
| 575 |
+
board["raw_state"] = response
|
| 576 |
+
board["updated"] = time.strftime("%H:%M:%S")
|
| 577 |
+
if response.startswith("STATE "):
|
| 578 |
+
fields = self.parse_pairs(response)
|
| 579 |
+
board["state"] = fields
|
| 580 |
+
event = fields.get("EVT", "")
|
| 581 |
+
elif response == "ERR":
|
| 582 |
+
board["state"] = {}
|
| 583 |
+
connect_time = self.state.connect_time
|
| 584 |
+
|
| 585 |
+
if fields is not None:
|
| 586 |
+
if event == "R":
|
| 587 |
+
if self._clock() - connect_time < RESET_EVENT_DEBOUNCE_S:
|
| 588 |
+
self._logger("[Button] EVT=R ignored (debounce)")
|
| 589 |
+
elif self.on_reset_event is not None:
|
| 590 |
+
self._logger(
|
| 591 |
+
"[Button] EVT=R → reset origin + recording")
|
| 592 |
+
self.on_reset_event()
|
| 593 |
+
elif event == "S" and self.on_stop_event is not None:
|
| 594 |
+
self._logger("[Button] EVT=S → stop recording")
|
| 595 |
+
self.on_stop_event()
|
| 596 |
+
|
| 597 |
+
snapshot = self.snapshot()
|
| 598 |
+
if self.on_board_update is not None:
|
| 599 |
+
self.on_board_update(snapshot)
|
| 600 |
+
|
| 601 |
+
def _poll_info(self) -> None:
|
| 602 |
+
self.send_async(
|
| 603 |
+
"?", self._handle_state_response,
|
| 604 |
+
key="poll_info", mode="skip",
|
| 605 |
+
)
|
| 606 |
+
|
| 607 |
+
def _loop_active(self, token: int) -> bool:
|
| 608 |
+
with self.state.lock:
|
| 609 |
+
return (
|
| 610 |
+
token == self.state.loop_token
|
| 611 |
+
and self.state.connected
|
| 612 |
+
and (self.state.no_esp or self.serial.is_connected)
|
| 613 |
+
)
|
| 614 |
+
|
| 615 |
+
def _ui_loop(self, token: int) -> None:
|
| 616 |
+
if not self._loop_active(token):
|
| 617 |
+
return
|
| 618 |
+
if self.on_ui_tick is not None:
|
| 619 |
+
self.on_ui_tick()
|
| 620 |
+
self.schedule(
|
| 621 |
+
self.UI_LOOP_MS, lambda: self._ui_loop(token))
|
| 622 |
+
|
| 623 |
+
def _fast_grip_loop(self, token: int) -> None:
|
| 624 |
+
"""Run force checks off the Tk main thread at the original period."""
|
| 625 |
+
stop_event = self._grip_stop
|
| 626 |
+
thread = self._grip_thread
|
| 627 |
+
if thread is not None and thread.is_alive():
|
| 628 |
+
stop_event.set()
|
| 629 |
+
if thread is not threading.current_thread():
|
| 630 |
+
thread.join(timeout=0.2)
|
| 631 |
+
self._grip_stop = threading.Event()
|
| 632 |
+
worker = self._thread_factory(
|
| 633 |
+
target=self._grip_check_loop,
|
| 634 |
+
args=(token, self._grip_stop),
|
| 635 |
+
name="force-grip-check",
|
| 636 |
+
daemon=True,
|
| 637 |
+
)
|
| 638 |
+
self._grip_thread = worker
|
| 639 |
+
worker.start()
|
| 640 |
+
|
| 641 |
+
def _grip_check_loop(self, token: int, stop_event):
|
| 642 |
+
while not stop_event.wait(self.FAST_GRIP_LOOP_MS / 1000.0):
|
| 643 |
+
if not self._loop_active(token):
|
| 644 |
+
break
|
| 645 |
+
if self.snapshot().no_esp:
|
| 646 |
+
continue
|
| 647 |
+
if self.on_grip_check is not None:
|
| 648 |
+
self.on_grip_check()
|
| 649 |
+
|
| 650 |
+
def _stop_grip_checks(self) -> None:
|
| 651 |
+
stop_event = self._grip_stop
|
| 652 |
+
thread = self._grip_thread
|
| 653 |
+
self._grip_stop = None
|
| 654 |
+
self._grip_thread = None
|
| 655 |
+
if stop_event is not None:
|
| 656 |
+
stop_event.set()
|
| 657 |
+
if (
|
| 658 |
+
thread is not None
|
| 659 |
+
and thread is not threading.current_thread()
|
| 660 |
+
and thread.is_alive()
|
| 661 |
+
):
|
| 662 |
+
thread.join(timeout=0.5)
|
| 663 |
+
|
| 664 |
+
def _start_heartbeat(self, token: int) -> None:
|
| 665 |
+
"""Start the ESP32 keep-alive "?" poll on its own daemon thread.
|
| 666 |
+
|
| 667 |
+
The poll no longer rides ``root.after`` so a stalled Tk main thread
|
| 668 |
+
(e.g. a long synchronous save) cannot let the ESP32 declare the host
|
| 669 |
+
offline and stop accepting the physical record button.
|
| 670 |
+
"""
|
| 671 |
+
stop_event = self._heartbeat_stop
|
| 672 |
+
thread = self._heartbeat_thread
|
| 673 |
+
if thread is not None and thread.is_alive():
|
| 674 |
+
if stop_event is not None:
|
| 675 |
+
stop_event.set()
|
| 676 |
+
if thread is not threading.current_thread():
|
| 677 |
+
thread.join(timeout=0.5)
|
| 678 |
+
self._heartbeat_stop = threading.Event()
|
| 679 |
+
heartbeat = self._thread_factory(
|
| 680 |
+
target=self._heartbeat_loop,
|
| 681 |
+
args=(token, self._heartbeat_stop),
|
| 682 |
+
name="esp-heartbeat",
|
| 683 |
+
daemon=True,
|
| 684 |
+
)
|
| 685 |
+
self._heartbeat_thread = heartbeat
|
| 686 |
+
heartbeat.start()
|
| 687 |
+
|
| 688 |
+
def _heartbeat_loop(
|
| 689 |
+
self, token: int, stop_event: threading.Event,
|
| 690 |
+
) -> None:
|
| 691 |
+
while not stop_event.is_set():
|
| 692 |
+
if self._loop_active(token) and not self.snapshot().no_esp:
|
| 693 |
+
self._poll_info()
|
| 694 |
+
stop_event.wait(HEARTBEAT_PERIOD_S)
|
| 695 |
+
|
| 696 |
+
def _stop_heartbeat(self) -> None:
|
| 697 |
+
stop_event = self._heartbeat_stop
|
| 698 |
+
thread = self._heartbeat_thread
|
| 699 |
+
self._heartbeat_stop = None
|
| 700 |
+
self._heartbeat_thread = None
|
| 701 |
+
if stop_event is not None:
|
| 702 |
+
stop_event.set()
|
| 703 |
+
if (
|
| 704 |
+
thread is not None
|
| 705 |
+
and thread is not threading.current_thread()
|
| 706 |
+
and thread.is_alive()
|
| 707 |
+
):
|
| 708 |
+
thread.join(timeout=0.5)
|
| 709 |
+
|
| 710 |
+
def update_loops(self, token: Optional[int] = None) -> None:
|
| 711 |
+
if token is None:
|
| 712 |
+
with self.state.lock:
|
| 713 |
+
token = self.state.loop_token
|
| 714 |
+
if self._loop_active(token):
|
| 715 |
+
self._start_heartbeat(token)
|
| 716 |
+
self._ui_loop(token)
|
| 717 |
+
self._fast_grip_loop(token)
|
collector/core/gripper/devices/gripper_serial.py
ADDED
|
@@ -0,0 +1,181 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""ESP32S3 夹爪串口的唯一资源所有者。"""
|
| 2 |
+
|
| 3 |
+
from __future__ import annotations
|
| 4 |
+
|
| 5 |
+
import threading
|
| 6 |
+
import time
|
| 7 |
+
from typing import Callable, Optional
|
| 8 |
+
|
| 9 |
+
import serial
|
| 10 |
+
import serial.tools.list_ports
|
| 11 |
+
|
| 12 |
+
|
| 13 |
+
class GripperSerial:
|
| 14 |
+
"""拥有并封装唯一的 ``serial.Serial`` 实例。
|
| 15 |
+
|
| 16 |
+
``connect`` 内的 ``?`` 是打开后的握手,不属于应用命令。握手完成以后,所有
|
| 17 |
+
同步和异步应用命令都应交给 :class:`SerialCommandWorker`,由它串行调用
|
| 18 |
+
:meth:`send`。
|
| 19 |
+
"""
|
| 20 |
+
|
| 21 |
+
CONNECT_BAUD = 115200
|
| 22 |
+
CONNECT_TIMEOUT = 0.5
|
| 23 |
+
CONNECT_WRITE_TIMEOUT = 0.5
|
| 24 |
+
CONNECT_BOOT_DELAY = 2.0
|
| 25 |
+
CONNECT_ATTEMPTS = 8
|
| 26 |
+
CONNECT_RETRY_DELAY = 0.3
|
| 27 |
+
COMMAND_TIMEOUT = 1.0
|
| 28 |
+
|
| 29 |
+
def __init__(
|
| 30 |
+
self,
|
| 31 |
+
serial_factory: Optional[Callable[..., object]] = None,
|
| 32 |
+
*,
|
| 33 |
+
sleep: Callable[[float], None] = time.sleep,
|
| 34 |
+
clock: Callable[[], float] = time.monotonic,
|
| 35 |
+
logger: Callable[..., None] = print,
|
| 36 |
+
):
|
| 37 |
+
self._serial_factory = serial_factory or serial.Serial
|
| 38 |
+
self._sleep = sleep
|
| 39 |
+
self._clock = clock
|
| 40 |
+
self._logger = logger
|
| 41 |
+
self._serial = None
|
| 42 |
+
self._last_error = None
|
| 43 |
+
# connect/disconnect 与 worker send 共用同一把可重入锁;断开不会在一条
|
| 44 |
+
# 命令写入和读取响应之间关闭文件描述符。
|
| 45 |
+
self._io_lock = threading.RLock()
|
| 46 |
+
|
| 47 |
+
@property
|
| 48 |
+
def ser(self):
|
| 49 |
+
"""Return the owner-held handle for read-only connection diagnostics."""
|
| 50 |
+
with self._io_lock:
|
| 51 |
+
return self._serial
|
| 52 |
+
|
| 53 |
+
@property
|
| 54 |
+
def is_connected(self) -> bool:
|
| 55 |
+
with self._io_lock:
|
| 56 |
+
return bool(
|
| 57 |
+
self._serial is not None
|
| 58 |
+
and getattr(self._serial, "is_open", False)
|
| 59 |
+
)
|
| 60 |
+
|
| 61 |
+
@property
|
| 62 |
+
def last_error(self) -> Optional[str]:
|
| 63 |
+
"""返回最近一次连接失败的准确原因,不用握手超时覆盖打开错误。"""
|
| 64 |
+
with self._io_lock:
|
| 65 |
+
return self._last_error
|
| 66 |
+
|
| 67 |
+
def connect(self, port: str, baud: int = CONNECT_BAUD) -> bool:
|
| 68 |
+
"""打开串口并用原有的八次 ``?`` 握手确认主控在线。"""
|
| 69 |
+
with self._io_lock:
|
| 70 |
+
self._disconnect_locked()
|
| 71 |
+
self._last_error = None
|
| 72 |
+
try:
|
| 73 |
+
opened = self._serial_factory(
|
| 74 |
+
port,
|
| 75 |
+
baud,
|
| 76 |
+
timeout=self.CONNECT_TIMEOUT,
|
| 77 |
+
write_timeout=self.CONNECT_WRITE_TIMEOUT,
|
| 78 |
+
)
|
| 79 |
+
self._serial = opened
|
| 80 |
+
self._sleep(self.CONNECT_BOOT_DELAY)
|
| 81 |
+
opened.reset_input_buffer()
|
| 82 |
+
for attempt in range(self.CONNECT_ATTEMPTS):
|
| 83 |
+
opened.write(b"?\r\n")
|
| 84 |
+
line = opened.readline().decode(
|
| 85 |
+
errors="ignore").strip()
|
| 86 |
+
self._logger(
|
| 87 |
+
f"[Serial] connect attempt {attempt + 1}/"
|
| 88 |
+
f"{self.CONNECT_ATTEMPTS}: [{line[:60]}]"
|
| 89 |
+
)
|
| 90 |
+
if line.startswith("STATE"):
|
| 91 |
+
return True
|
| 92 |
+
self._sleep(self.CONNECT_RETRY_DELAY)
|
| 93 |
+
except Exception as exc:
|
| 94 |
+
self._last_error = (
|
| 95 |
+
f"serial open/handshake failed: {exc}"
|
| 96 |
+
)
|
| 97 |
+
self._disconnect_locked()
|
| 98 |
+
return False
|
| 99 |
+
self._last_error = (
|
| 100 |
+
"serial handshake timeout after "
|
| 101 |
+
f"{self.CONNECT_ATTEMPTS} attempts"
|
| 102 |
+
)
|
| 103 |
+
self._disconnect_locked()
|
| 104 |
+
return False
|
| 105 |
+
|
| 106 |
+
def disconnect(self) -> None:
|
| 107 |
+
"""幂等关闭串口;即使底层已关闭也清除 owner 引用。"""
|
| 108 |
+
with self._io_lock:
|
| 109 |
+
self._disconnect_locked()
|
| 110 |
+
|
| 111 |
+
def _disconnect_locked(self) -> None:
|
| 112 |
+
opened, self._serial = self._serial, None
|
| 113 |
+
if opened is None:
|
| 114 |
+
return
|
| 115 |
+
try:
|
| 116 |
+
if getattr(opened, "is_open", False):
|
| 117 |
+
# CDC-ACM 端点停止接收时,close(fd) 也可能等待 tty 输出
|
| 118 |
+
# 队列排空。先中断 pyserial 写等待并丢弃未发送字节,保证
|
| 119 |
+
# Connect 失败和退出路径都有界。
|
| 120 |
+
cancel_write = getattr(opened, "cancel_write", None)
|
| 121 |
+
if callable(cancel_write):
|
| 122 |
+
try:
|
| 123 |
+
cancel_write()
|
| 124 |
+
except Exception:
|
| 125 |
+
pass
|
| 126 |
+
reset_output = getattr(
|
| 127 |
+
opened, "reset_output_buffer", None)
|
| 128 |
+
if callable(reset_output):
|
| 129 |
+
try:
|
| 130 |
+
reset_output()
|
| 131 |
+
except Exception:
|
| 132 |
+
pass
|
| 133 |
+
opened.close()
|
| 134 |
+
except Exception:
|
| 135 |
+
# 关闭失败时也不能把已经失效的句柄重新暴露给调用方。
|
| 136 |
+
pass
|
| 137 |
+
|
| 138 |
+
def send(self, command: str) -> str:
|
| 139 |
+
"""发送一条文本命令并返回匹配的完整响应行。"""
|
| 140 |
+
with self._io_lock:
|
| 141 |
+
opened = self._serial
|
| 142 |
+
if opened is None or not getattr(opened, "is_open", False):
|
| 143 |
+
return "ERR"
|
| 144 |
+
try:
|
| 145 |
+
# 固件在 RECORDING 状态会主动输出 DATA 行。先清旧输入,再只
|
| 146 |
+
# 接受当前命令的响应前缀,保持原程序的防串线行为。
|
| 147 |
+
try:
|
| 148 |
+
opened.reset_input_buffer()
|
| 149 |
+
except Exception:
|
| 150 |
+
pass
|
| 151 |
+
opened.write((str(command) + "\r\n").encode())
|
| 152 |
+
expected = {
|
| 153 |
+
"?": ("STATE",),
|
| 154 |
+
"G": ("AS5048A", "AS5048A ERR"),
|
| 155 |
+
"C": ("OK CONNECT", "ERR"),
|
| 156 |
+
"I": ("OK INIT", "ERR"),
|
| 157 |
+
"F": ("OK READY", "ERR"),
|
| 158 |
+
"E": ("OK ERROR", "ERR"),
|
| 159 |
+
"R": ("OK ASR", "AS5048A ERR"),
|
| 160 |
+
"S": ("OK START", "ERR"),
|
| 161 |
+
}.get(str(command)[:1], ())
|
| 162 |
+
deadline = self._clock() + self.COMMAND_TIMEOUT
|
| 163 |
+
last_line = ""
|
| 164 |
+
while self._clock() < deadline:
|
| 165 |
+
line = opened.readline().decode(
|
| 166 |
+
errors="ignore").strip()
|
| 167 |
+
if not line:
|
| 168 |
+
continue
|
| 169 |
+
last_line = line
|
| 170 |
+
if not expected or line.startswith(expected):
|
| 171 |
+
return line
|
| 172 |
+
return last_line or "ERR"
|
| 173 |
+
except Exception:
|
| 174 |
+
return "ERR"
|
| 175 |
+
|
| 176 |
+
@staticmethod
|
| 177 |
+
def list_ports():
|
| 178 |
+
"""返回系统当前可用串口设备名。"""
|
| 179 |
+
return [
|
| 180 |
+
port.device for port in serial.tools.list_ports.comports()
|
| 181 |
+
]
|
collector/core/gripper/devices/relative_usb_topology.py
ADDED
|
@@ -0,0 +1,455 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""ESP32-anchored discovery for one physical gripper set.
|
| 2 |
+
|
| 3 |
+
The only accepted topology is:
|
| 4 |
+
|
| 5 |
+
ESP32 (VID/PID 303a:1001)
|
| 6 |
+
-> H1: one DECXIN (VID/PID 1bcf:2d4f)
|
| 7 |
+
-> H0 = parent(H1): two Sightac (VID/PID 0c45:636f)
|
| 8 |
+
|
| 9 |
+
Fays is identified independently as one FT602 physical device with
|
| 10 |
+
interface 00 and interface 02. It is assigned to the ESP32 unit only when
|
| 11 |
+
their USB root-port numbers match. No independent camera unit, enumeration-order
|
| 12 |
+
pairing, or fallback topology is accepted.
|
| 13 |
+
"""
|
| 14 |
+
|
| 15 |
+
from __future__ import annotations
|
| 16 |
+
|
| 17 |
+
from dataclasses import dataclass
|
| 18 |
+
import glob
|
| 19 |
+
import os
|
| 20 |
+
import re
|
| 21 |
+
from typing import Callable, Iterable, Optional
|
| 22 |
+
|
| 23 |
+
|
| 24 |
+
FAYS_VENDOR_ID = "0403"
|
| 25 |
+
FAYS_PRODUCT_ID = "602e"
|
| 26 |
+
SIGHTAC_VENDOR_ID = "0c45"
|
| 27 |
+
SIGHTAC_PRODUCT_ID = "636f"
|
| 28 |
+
DECXIN_VENDOR_ID = "1bcf"
|
| 29 |
+
DECXIN_PRODUCT_ID = "2d4f"
|
| 30 |
+
ESP32_VENDOR_ID = "303a"
|
| 31 |
+
ESP32_PRODUCT_ID = "1001"
|
| 32 |
+
DECXIN_NAME = "DECXIN CAMERA: DECXIN CAMERA"
|
| 33 |
+
OUTER_HUB_MIN_SPEED_MBPS = 5000.0
|
| 34 |
+
INNER_HUB_MAX_SPEED_MBPS = 480.0
|
| 35 |
+
|
| 36 |
+
|
| 37 |
+
def _read_text(path: str) -> str:
|
| 38 |
+
with open(path, encoding="utf-8") as stream:
|
| 39 |
+
return stream.read().strip()
|
| 40 |
+
|
| 41 |
+
|
| 42 |
+
def _natural_key(value: str):
|
| 43 |
+
return tuple(
|
| 44 |
+
int(part) if part.isdigit() else part
|
| 45 |
+
for part in re.split(r"([0-9]+)", str(value))
|
| 46 |
+
if part
|
| 47 |
+
)
|
| 48 |
+
|
| 49 |
+
|
| 50 |
+
def _root_port_key(physical_usb_path: str) -> Optional[str]:
|
| 51 |
+
"""Return the bus-independent root-port number from a USB path."""
|
| 52 |
+
match = re.fullmatch(r"\d+-(\d+)(?:\.\d+)*", str(physical_usb_path))
|
| 53 |
+
return match.group(1) if match is not None else None
|
| 54 |
+
|
| 55 |
+
|
| 56 |
+
@dataclass(frozen=True)
|
| 57 |
+
class RelativeDeviceNode:
|
| 58 |
+
role: str
|
| 59 |
+
device_path: str
|
| 60 |
+
physical_usb_path: str
|
| 61 |
+
physical_sysfs_path: str
|
| 62 |
+
parent_hub_sysfs_path: str
|
| 63 |
+
vendor_id: str
|
| 64 |
+
product_id: str
|
| 65 |
+
serial: str
|
| 66 |
+
name: str
|
| 67 |
+
interface_number: str
|
| 68 |
+
stream_index: int
|
| 69 |
+
|
| 70 |
+
@property
|
| 71 |
+
def video_index(self) -> Optional[int]:
|
| 72 |
+
basename = os.path.basename(self.device_path)
|
| 73 |
+
if basename.startswith("video") and basename[5:].isdigit():
|
| 74 |
+
return int(basename[5:])
|
| 75 |
+
return None
|
| 76 |
+
|
| 77 |
+
|
| 78 |
+
@dataclass(frozen=True)
|
| 79 |
+
class RelativeUnitTopology:
|
| 80 |
+
fays_stereo: Optional[RelativeDeviceNode]
|
| 81 |
+
fays_imu: Optional[RelativeDeviceNode]
|
| 82 |
+
sightac_nodes: tuple
|
| 83 |
+
decxin: Optional[RelativeDeviceNode]
|
| 84 |
+
esp32: Optional[RelativeDeviceNode]
|
| 85 |
+
outer_hub_sysfs_path: str
|
| 86 |
+
inner_hub_sysfs_path: str
|
| 87 |
+
fays_physical_sysfs_path: str
|
| 88 |
+
|
| 89 |
+
@property
|
| 90 |
+
def ports(self):
|
| 91 |
+
if self.fays_stereo is None or self.fays_imu is None:
|
| 92 |
+
return {}
|
| 93 |
+
return {
|
| 94 |
+
"stereo_dev_port": self.fays_stereo.device_path,
|
| 95 |
+
"imu_dev_port": self.fays_imu.device_path,
|
| 96 |
+
}
|
| 97 |
+
|
| 98 |
+
@property
|
| 99 |
+
def outer_hub_path(self) -> str:
|
| 100 |
+
"""Diagnostic only; never used as a matching key."""
|
| 101 |
+
return os.path.basename(self.outer_hub_sysfs_path)
|
| 102 |
+
|
| 103 |
+
@property
|
| 104 |
+
def inner_hub_path(self) -> str:
|
| 105 |
+
"""Diagnostic only; never used as a matching key."""
|
| 106 |
+
return os.path.basename(self.inner_hub_sysfs_path)
|
| 107 |
+
|
| 108 |
+
|
| 109 |
+
def _parent_hub(path: str) -> str:
|
| 110 |
+
return os.path.dirname(path)
|
| 111 |
+
|
| 112 |
+
|
| 113 |
+
def _hub_speed_mbps(
|
| 114 |
+
hub_sysfs_path: str,
|
| 115 |
+
*,
|
| 116 |
+
read_text: Callable[[str], str],
|
| 117 |
+
) -> Optional[float]:
|
| 118 |
+
try:
|
| 119 |
+
return float(read_text(os.path.join(hub_sysfs_path, "speed")))
|
| 120 |
+
except (OSError, ValueError):
|
| 121 |
+
return None
|
| 122 |
+
|
| 123 |
+
|
| 124 |
+
def _hub_device_class(
|
| 125 |
+
hub_sysfs_path: str,
|
| 126 |
+
*,
|
| 127 |
+
read_text: Callable[[str], str],
|
| 128 |
+
) -> str:
|
| 129 |
+
try:
|
| 130 |
+
return read_text(os.path.join(hub_sysfs_path, "bDeviceClass")).lower()
|
| 131 |
+
except OSError:
|
| 132 |
+
return ""
|
| 133 |
+
|
| 134 |
+
|
| 135 |
+
def is_hub_path(
|
| 136 |
+
hub_sysfs_path: str,
|
| 137 |
+
*,
|
| 138 |
+
read_text: Callable[[str], str] = _read_text,
|
| 139 |
+
isdir: Callable[[str], bool] = os.path.isdir,
|
| 140 |
+
) -> bool:
|
| 141 |
+
"""Return True when the directory describes a USB hub (class 0x09)."""
|
| 142 |
+
try:
|
| 143 |
+
return (
|
| 144 |
+
isdir(hub_sysfs_path)
|
| 145 |
+
and _hub_device_class(hub_sysfs_path, read_text=read_text) == "09"
|
| 146 |
+
)
|
| 147 |
+
except OSError:
|
| 148 |
+
return False
|
| 149 |
+
|
| 150 |
+
|
| 151 |
+
def _physical_device_from_interface(
|
| 152 |
+
interface_sysfs_path: str,
|
| 153 |
+
*,
|
| 154 |
+
realpath: Callable[[str], str],
|
| 155 |
+
) -> tuple:
|
| 156 |
+
physical_path = _parent_hub(interface_sysfs_path)
|
| 157 |
+
interface_basename = os.path.basename(interface_sysfs_path)
|
| 158 |
+
physical_usb_path = interface_basename.split(":", 1)[0]
|
| 159 |
+
if (
|
| 160 |
+
":" not in interface_basename
|
| 161 |
+
or not re.fullmatch(r"\d+-\d+(?:\.\d+)*", physical_usb_path)
|
| 162 |
+
):
|
| 163 |
+
raise ValueError(f"not a USB interface path: {interface_sysfs_path}")
|
| 164 |
+
return physical_path, physical_usb_path
|
| 165 |
+
|
| 166 |
+
|
| 167 |
+
def inspect_video_node(
|
| 168 |
+
class_path: str,
|
| 169 |
+
*,
|
| 170 |
+
read_text: Callable[[str], str] = _read_text,
|
| 171 |
+
realpath: Callable[[str], str] = os.path.realpath,
|
| 172 |
+
device_exists: Callable[[str], bool] = os.path.exists,
|
| 173 |
+
isdir: Callable[[str], bool] = os.path.isdir,
|
| 174 |
+
):
|
| 175 |
+
"""Classify one video4linux node using only its sysfs identity."""
|
| 176 |
+
del isdir
|
| 177 |
+
basename = os.path.basename(class_path)
|
| 178 |
+
if not re.fullmatch(r"video\d+", basename):
|
| 179 |
+
return None
|
| 180 |
+
try:
|
| 181 |
+
stream_index = int(read_text(os.path.join(class_path, "index")))
|
| 182 |
+
if stream_index != 0:
|
| 183 |
+
return None
|
| 184 |
+
name = read_text(os.path.join(class_path, "name"))
|
| 185 |
+
interface_number = read_text(
|
| 186 |
+
os.path.join(class_path, "device", "bInterfaceNumber")
|
| 187 |
+
).lower()
|
| 188 |
+
modalias = read_text(
|
| 189 |
+
os.path.join(class_path, "device", "modalias")
|
| 190 |
+
).lower()
|
| 191 |
+
interface_sysfs_path = realpath(os.path.join(class_path, "device"))
|
| 192 |
+
physical_sysfs_path, physical_usb_path = _physical_device_from_interface(
|
| 193 |
+
interface_sysfs_path, realpath=realpath
|
| 194 |
+
)
|
| 195 |
+
vendor_id = read_text(
|
| 196 |
+
os.path.join(physical_sysfs_path, "idVendor")
|
| 197 |
+
).lower()
|
| 198 |
+
product_id = read_text(
|
| 199 |
+
os.path.join(physical_sysfs_path, "idProduct")
|
| 200 |
+
).lower()
|
| 201 |
+
try:
|
| 202 |
+
serial = read_text(os.path.join(physical_sysfs_path, "serial"))
|
| 203 |
+
except OSError:
|
| 204 |
+
serial = ""
|
| 205 |
+
parent_hub = _parent_hub(physical_sysfs_path)
|
| 206 |
+
except (OSError, ValueError):
|
| 207 |
+
return None
|
| 208 |
+
|
| 209 |
+
if not device_exists(f"/dev/{basename}"):
|
| 210 |
+
return None
|
| 211 |
+
|
| 212 |
+
lowered_name = name.lower()
|
| 213 |
+
role = None
|
| 214 |
+
if (
|
| 215 |
+
vendor_id == FAYS_VENDOR_ID
|
| 216 |
+
and product_id == FAYS_PRODUCT_ID
|
| 217 |
+
and "ftdi" in lowered_name
|
| 218 |
+
and "superspeed video bridge" in lowered_name
|
| 219 |
+
):
|
| 220 |
+
if interface_number == "00":
|
| 221 |
+
role = "fays_stereo"
|
| 222 |
+
elif interface_number == "02":
|
| 223 |
+
role = "fays_imu"
|
| 224 |
+
elif vendor_id == SIGHTAC_VENDOR_ID and product_id == SIGHTAC_PRODUCT_ID:
|
| 225 |
+
role = "sightac"
|
| 226 |
+
elif (
|
| 227 |
+
vendor_id == DECXIN_VENDOR_ID
|
| 228 |
+
and product_id == DECXIN_PRODUCT_ID
|
| 229 |
+
and name == DECXIN_NAME
|
| 230 |
+
):
|
| 231 |
+
role = "decxin"
|
| 232 |
+
if role is None:
|
| 233 |
+
return None
|
| 234 |
+
return RelativeDeviceNode(
|
| 235 |
+
role=role,
|
| 236 |
+
device_path=f"/dev/{basename}",
|
| 237 |
+
physical_usb_path=physical_usb_path,
|
| 238 |
+
physical_sysfs_path=physical_sysfs_path,
|
| 239 |
+
parent_hub_sysfs_path=parent_hub,
|
| 240 |
+
vendor_id=vendor_id,
|
| 241 |
+
product_id=product_id,
|
| 242 |
+
serial=serial,
|
| 243 |
+
name=name,
|
| 244 |
+
interface_number=interface_number,
|
| 245 |
+
stream_index=stream_index,
|
| 246 |
+
)
|
| 247 |
+
|
| 248 |
+
|
| 249 |
+
def inspect_tty_node(
|
| 250 |
+
class_path: str,
|
| 251 |
+
*,
|
| 252 |
+
read_text: Callable[[str], str] = _read_text,
|
| 253 |
+
realpath: Callable[[str], str] = os.path.realpath,
|
| 254 |
+
device_exists: Callable[[str], bool] = os.path.exists,
|
| 255 |
+
isdir: Callable[[str], bool] = os.path.isdir,
|
| 256 |
+
):
|
| 257 |
+
"""Classify one ttyACM node as an ESP32 using only sysfs identity."""
|
| 258 |
+
del isdir
|
| 259 |
+
basename = os.path.basename(class_path)
|
| 260 |
+
if not re.fullmatch(r"ttyACM\d+", basename):
|
| 261 |
+
return None
|
| 262 |
+
try:
|
| 263 |
+
interface_sysfs_path = realpath(os.path.join(class_path, "device"))
|
| 264 |
+
physical_sysfs_path, physical_usb_path = _physical_device_from_interface(
|
| 265 |
+
interface_sysfs_path, realpath=realpath
|
| 266 |
+
)
|
| 267 |
+
vendor_id = read_text(
|
| 268 |
+
os.path.join(physical_sysfs_path, "idVendor")
|
| 269 |
+
).lower()
|
| 270 |
+
product_id = read_text(
|
| 271 |
+
os.path.join(physical_sysfs_path, "idProduct")
|
| 272 |
+
).lower()
|
| 273 |
+
if vendor_id != ESP32_VENDOR_ID or product_id != ESP32_PRODUCT_ID:
|
| 274 |
+
return None
|
| 275 |
+
serial = read_text(os.path.join(physical_sysfs_path, "serial"))
|
| 276 |
+
parent_hub = _parent_hub(physical_sysfs_path)
|
| 277 |
+
except (OSError, ValueError):
|
| 278 |
+
return None
|
| 279 |
+
|
| 280 |
+
if not device_exists(f"/dev/{basename}"):
|
| 281 |
+
return None
|
| 282 |
+
return RelativeDeviceNode(
|
| 283 |
+
role="esp32",
|
| 284 |
+
device_path=f"/dev/{basename}",
|
| 285 |
+
physical_usb_path=physical_usb_path,
|
| 286 |
+
physical_sysfs_path=physical_sysfs_path,
|
| 287 |
+
parent_hub_sysfs_path=parent_hub,
|
| 288 |
+
vendor_id=vendor_id,
|
| 289 |
+
product_id=product_id,
|
| 290 |
+
serial=serial,
|
| 291 |
+
name=basename,
|
| 292 |
+
interface_number="",
|
| 293 |
+
stream_index=0,
|
| 294 |
+
)
|
| 295 |
+
|
| 296 |
+
|
| 297 |
+
def discover_relative_units(
|
| 298 |
+
*,
|
| 299 |
+
video_sysfs_glob: Callable[[], Iterable[str]] = lambda: glob.glob(
|
| 300 |
+
"/sys/class/video4linux/video*"
|
| 301 |
+
),
|
| 302 |
+
tty_sysfs_glob: Callable[[], Iterable[str]] = lambda: glob.glob(
|
| 303 |
+
"/sys/class/tty/ttyACM*"
|
| 304 |
+
),
|
| 305 |
+
read_text: Callable[[str], str] = _read_text,
|
| 306 |
+
realpath: Callable[[str], str] = os.path.realpath,
|
| 307 |
+
device_exists: Callable[[str], bool] = os.path.exists,
|
| 308 |
+
isdir: Callable[[str], bool] = os.path.isdir,
|
| 309 |
+
):
|
| 310 |
+
"""Discover complete units using only ESP32 -> DECXIN -> H0 -> Sightac."""
|
| 311 |
+
video_nodes = []
|
| 312 |
+
for class_path in sorted(video_sysfs_glob(), key=_natural_key):
|
| 313 |
+
node = inspect_video_node(
|
| 314 |
+
class_path,
|
| 315 |
+
read_text=read_text,
|
| 316 |
+
realpath=realpath,
|
| 317 |
+
device_exists=device_exists,
|
| 318 |
+
isdir=isdir,
|
| 319 |
+
)
|
| 320 |
+
if node is not None:
|
| 321 |
+
video_nodes.append(node)
|
| 322 |
+
|
| 323 |
+
tty_nodes = []
|
| 324 |
+
for class_path in sorted(tty_sysfs_glob(), key=_natural_key):
|
| 325 |
+
node = inspect_tty_node(
|
| 326 |
+
class_path,
|
| 327 |
+
read_text=read_text,
|
| 328 |
+
realpath=realpath,
|
| 329 |
+
device_exists=device_exists,
|
| 330 |
+
isdir=isdir,
|
| 331 |
+
)
|
| 332 |
+
if node is not None:
|
| 333 |
+
tty_nodes.append(node)
|
| 334 |
+
|
| 335 |
+
fays_by_physical = {}
|
| 336 |
+
sightac_by_parent = {}
|
| 337 |
+
decxin_by_parent = {}
|
| 338 |
+
esp32_by_parent = {}
|
| 339 |
+
for node in video_nodes:
|
| 340 |
+
if node.role in {"fays_stereo", "fays_imu"}:
|
| 341 |
+
fays_by_physical.setdefault(
|
| 342 |
+
node.physical_sysfs_path, {}
|
| 343 |
+
)[node.role] = node
|
| 344 |
+
elif node.role == "sightac":
|
| 345 |
+
sightac_by_parent.setdefault(
|
| 346 |
+
node.parent_hub_sysfs_path, []
|
| 347 |
+
).append(node)
|
| 348 |
+
elif node.role == "decxin":
|
| 349 |
+
decxin_by_parent.setdefault(
|
| 350 |
+
node.parent_hub_sysfs_path, []
|
| 351 |
+
).append(node)
|
| 352 |
+
for node in tty_nodes:
|
| 353 |
+
esp32_by_parent.setdefault(node.parent_hub_sysfs_path, []).append(node)
|
| 354 |
+
|
| 355 |
+
fays_groups = []
|
| 356 |
+
for physical_path, interfaces in sorted(
|
| 357 |
+
fays_by_physical.items(), key=lambda item: _natural_key(item[0])
|
| 358 |
+
):
|
| 359 |
+
stereo = interfaces.get("fays_stereo")
|
| 360 |
+
imu = interfaces.get("fays_imu")
|
| 361 |
+
if stereo is None or imu is None:
|
| 362 |
+
continue
|
| 363 |
+
fays_parent = stereo.parent_hub_sysfs_path
|
| 364 |
+
if fays_parent != imu.parent_hub_sysfs_path:
|
| 365 |
+
continue
|
| 366 |
+
if not is_hub_path(fays_parent, read_text=read_text, isdir=isdir):
|
| 367 |
+
continue
|
| 368 |
+
speed = _hub_speed_mbps(fays_parent, read_text=read_text)
|
| 369 |
+
if speed is None or speed < OUTER_HUB_MIN_SPEED_MBPS:
|
| 370 |
+
continue
|
| 371 |
+
fays_groups.append((physical_path, stereo, imu))
|
| 372 |
+
|
| 373 |
+
candidates = []
|
| 374 |
+
for esp_parent, esp32_nodes in sorted(
|
| 375 |
+
esp32_by_parent.items(), key=lambda item: _natural_key(item[0])
|
| 376 |
+
):
|
| 377 |
+
if len(esp32_nodes) != 1:
|
| 378 |
+
continue
|
| 379 |
+
esp32 = esp32_nodes[0]
|
| 380 |
+
if not is_hub_path(esp_parent, read_text=read_text, isdir=isdir):
|
| 381 |
+
continue
|
| 382 |
+
speed = _hub_speed_mbps(esp_parent, read_text=read_text)
|
| 383 |
+
if speed is None or speed > INNER_HUB_MAX_SPEED_MBPS:
|
| 384 |
+
continue
|
| 385 |
+
|
| 386 |
+
decxin_nodes = tuple(decxin_by_parent.get(esp_parent, ()))
|
| 387 |
+
if len(decxin_nodes) != 1:
|
| 388 |
+
continue
|
| 389 |
+
|
| 390 |
+
outer_hub = os.path.dirname(esp_parent)
|
| 391 |
+
if not is_hub_path(outer_hub, read_text=read_text, isdir=isdir):
|
| 392 |
+
continue
|
| 393 |
+
speed = _hub_speed_mbps(outer_hub, read_text=read_text)
|
| 394 |
+
if speed is None or speed > INNER_HUB_MAX_SPEED_MBPS:
|
| 395 |
+
continue
|
| 396 |
+
sightac_nodes = tuple(sorted(
|
| 397 |
+
sightac_by_parent.get(outer_hub, ()),
|
| 398 |
+
key=lambda node: _natural_key(node.physical_usb_path),
|
| 399 |
+
))
|
| 400 |
+
if len(sightac_nodes) != 2:
|
| 401 |
+
continue
|
| 402 |
+
|
| 403 |
+
esp_root_port = _root_port_key(esp32.physical_usb_path)
|
| 404 |
+
if esp_root_port is None:
|
| 405 |
+
continue
|
| 406 |
+
fays_matches = [
|
| 407 |
+
group for group in fays_groups
|
| 408 |
+
if _root_port_key(group[1].physical_usb_path) == esp_root_port
|
| 409 |
+
]
|
| 410 |
+
if len(fays_matches) != 1:
|
| 411 |
+
continue
|
| 412 |
+
candidates.append((
|
| 413 |
+
fays_matches[0], esp32, decxin_nodes[0], sightac_nodes,
|
| 414 |
+
outer_hub, esp_parent,
|
| 415 |
+
))
|
| 416 |
+
|
| 417 |
+
fays_use_count = {}
|
| 418 |
+
for fays_group, *_ in candidates:
|
| 419 |
+
fays_use_count[fays_group[0]] = fays_use_count.get(
|
| 420 |
+
fays_group[0], 0
|
| 421 |
+
) + 1
|
| 422 |
+
|
| 423 |
+
units = []
|
| 424 |
+
for (
|
| 425 |
+
fays_group, esp32, decxin, sightac_nodes, outer_hub, inner_hub
|
| 426 |
+
) in candidates:
|
| 427 |
+
fays_physical_path, stereo, imu = fays_group
|
| 428 |
+
if fays_use_count[fays_physical_path] != 1:
|
| 429 |
+
continue
|
| 430 |
+
units.append(RelativeUnitTopology(
|
| 431 |
+
fays_stereo=stereo,
|
| 432 |
+
fays_imu=imu,
|
| 433 |
+
sightac_nodes=sightac_nodes,
|
| 434 |
+
decxin=decxin,
|
| 435 |
+
esp32=esp32,
|
| 436 |
+
outer_hub_sysfs_path=outer_hub,
|
| 437 |
+
inner_hub_sysfs_path=inner_hub,
|
| 438 |
+
fays_physical_sysfs_path=fays_physical_path,
|
| 439 |
+
))
|
| 440 |
+
|
| 441 |
+
return tuple(units)
|
| 442 |
+
|
| 443 |
+
|
| 444 |
+
def unit_for_esp_serial(
|
| 445 |
+
units: Iterable[RelativeUnitTopology],
|
| 446 |
+
serial: str,
|
| 447 |
+
) -> Optional[RelativeUnitTopology]:
|
| 448 |
+
"""Return the relative unit whose ESP32 matches ``serial`` exactly."""
|
| 449 |
+
wanted = str(serial or "").strip()
|
| 450 |
+
if not wanted:
|
| 451 |
+
return None
|
| 452 |
+
for unit in units:
|
| 453 |
+
if unit.esp32 is not None and unit.esp32.serial == wanted:
|
| 454 |
+
return unit
|
| 455 |
+
return None
|
collector/core/gripper/devices/serial_command_worker.py
ADDED
|
@@ -0,0 +1,294 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""带 generation 的单一串口命令调度线程。"""
|
| 2 |
+
|
| 3 |
+
from __future__ import annotations
|
| 4 |
+
|
| 5 |
+
from dataclasses import dataclass
|
| 6 |
+
import queue
|
| 7 |
+
import threading
|
| 8 |
+
from typing import Callable, Optional
|
| 9 |
+
|
| 10 |
+
|
| 11 |
+
class SerialCommandCancelled(RuntimeError):
|
| 12 |
+
"""命令因断开、清队列、停止或 latest 替换而失效。"""
|
| 13 |
+
|
| 14 |
+
|
| 15 |
+
@dataclass
|
| 16 |
+
class _Completion:
|
| 17 |
+
event: threading.Event
|
| 18 |
+
response: Optional[str] = None
|
| 19 |
+
error: Optional[BaseException] = None
|
| 20 |
+
|
| 21 |
+
def finish(
|
| 22 |
+
self,
|
| 23 |
+
response: Optional[str] = None,
|
| 24 |
+
error: Optional[BaseException] = None,
|
| 25 |
+
) -> None:
|
| 26 |
+
if self.event.is_set():
|
| 27 |
+
return
|
| 28 |
+
self.response = response
|
| 29 |
+
self.error = error
|
| 30 |
+
self.event.set()
|
| 31 |
+
|
| 32 |
+
|
| 33 |
+
@dataclass
|
| 34 |
+
class _Command:
|
| 35 |
+
command: str
|
| 36 |
+
callback: Optional[Callable[[str], None]]
|
| 37 |
+
key: Optional[str]
|
| 38 |
+
mode: str
|
| 39 |
+
generation: int
|
| 40 |
+
completion: Optional[_Completion] = None
|
| 41 |
+
|
| 42 |
+
|
| 43 |
+
class SerialCommandWorker:
|
| 44 |
+
"""顺序执行全部同步/异步应用命令。
|
| 45 |
+
|
| 46 |
+
``queue`` 保留顺序;同 key 的 ``skip`` 忽略重复提交;``latest`` 在当前命令
|
| 47 |
+
尚未结束时只保留最后一次值。``clear`` 和重连会推进 generation,旧命令不能
|
| 48 |
+
再发布回调。
|
| 49 |
+
"""
|
| 50 |
+
|
| 51 |
+
STOP_TIMEOUT = 1.0
|
| 52 |
+
VALID_MODES = frozenset({"queue", "skip", "latest"})
|
| 53 |
+
|
| 54 |
+
def __init__(
|
| 55 |
+
self,
|
| 56 |
+
send_command: Callable[[str], str],
|
| 57 |
+
dispatch_callback: Optional[
|
| 58 |
+
Callable[[Callable[[str], None], str], None]
|
| 59 |
+
] = None,
|
| 60 |
+
*,
|
| 61 |
+
thread_factory: Callable[..., threading.Thread] = threading.Thread,
|
| 62 |
+
):
|
| 63 |
+
self._send_command = send_command
|
| 64 |
+
self._dispatch_callback = (
|
| 65 |
+
dispatch_callback
|
| 66 |
+
if dispatch_callback is not None
|
| 67 |
+
else lambda callback, response: callback(response)
|
| 68 |
+
)
|
| 69 |
+
self._thread_factory = thread_factory
|
| 70 |
+
self._queue = queue.Queue()
|
| 71 |
+
self._state_lock = threading.Lock()
|
| 72 |
+
self._pending_keys = set()
|
| 73 |
+
self._latest_by_key = {}
|
| 74 |
+
self._generation = 0
|
| 75 |
+
self._running = False
|
| 76 |
+
self._thread = None
|
| 77 |
+
self._active = None
|
| 78 |
+
|
| 79 |
+
@property
|
| 80 |
+
def is_running(self) -> bool:
|
| 81 |
+
with self._state_lock:
|
| 82 |
+
return self._running
|
| 83 |
+
|
| 84 |
+
@property
|
| 85 |
+
def generation(self) -> int:
|
| 86 |
+
with self._state_lock:
|
| 87 |
+
return self._generation
|
| 88 |
+
|
| 89 |
+
@property
|
| 90 |
+
def thread(self):
|
| 91 |
+
return self._thread
|
| 92 |
+
|
| 93 |
+
def start(self) -> bool:
|
| 94 |
+
"""启动唯一 worker;重复调用不创建第二条串口线程。"""
|
| 95 |
+
with self._state_lock:
|
| 96 |
+
if self._running:
|
| 97 |
+
return False
|
| 98 |
+
if self._thread is not None and self._thread.is_alive():
|
| 99 |
+
raise RuntimeError("previous serial worker is still exiting")
|
| 100 |
+
self._running = True
|
| 101 |
+
thread = self._thread_factory(
|
| 102 |
+
target=self._run,
|
| 103 |
+
daemon=True,
|
| 104 |
+
name="serial-command-worker",
|
| 105 |
+
)
|
| 106 |
+
self._thread = thread
|
| 107 |
+
thread.start()
|
| 108 |
+
return True
|
| 109 |
+
|
| 110 |
+
def _run(self) -> None:
|
| 111 |
+
while True:
|
| 112 |
+
item = self._queue.get()
|
| 113 |
+
if item is None:
|
| 114 |
+
break
|
| 115 |
+
with self._state_lock:
|
| 116 |
+
self._active = item
|
| 117 |
+
try:
|
| 118 |
+
response = self._send_command(item.command)
|
| 119 |
+
except BaseException as exc: # worker 必须继续服务后续命令
|
| 120 |
+
response = "ERR"
|
| 121 |
+
send_error = exc
|
| 122 |
+
else:
|
| 123 |
+
send_error = None
|
| 124 |
+
|
| 125 |
+
next_item = None
|
| 126 |
+
with self._state_lock:
|
| 127 |
+
current = (
|
| 128 |
+
self._running
|
| 129 |
+
and item.generation == self._generation
|
| 130 |
+
)
|
| 131 |
+
latest = None
|
| 132 |
+
if item.key:
|
| 133 |
+
if item.generation == self._generation and self._running:
|
| 134 |
+
latest = self._latest_by_key.pop(
|
| 135 |
+
item.key, None)
|
| 136 |
+
next_item = latest
|
| 137 |
+
if latest is None:
|
| 138 |
+
self._pending_keys.discard(item.key)
|
| 139 |
+
else:
|
| 140 |
+
self._pending_keys.discard(item.key)
|
| 141 |
+
stale = self._latest_by_key.pop(item.key, None)
|
| 142 |
+
self._cancel_item(
|
| 143 |
+
stale, "serial generation advanced")
|
| 144 |
+
has_newer = (
|
| 145 |
+
item.key is not None
|
| 146 |
+
and item.mode == "latest"
|
| 147 |
+
and latest is not None
|
| 148 |
+
)
|
| 149 |
+
self._active = None
|
| 150 |
+
if next_item is not None:
|
| 151 |
+
self._queue.put(next_item)
|
| 152 |
+
|
| 153 |
+
if item.completion is not None:
|
| 154 |
+
if send_error is not None:
|
| 155 |
+
item.completion.finish(error=send_error)
|
| 156 |
+
elif not current or has_newer:
|
| 157 |
+
item.completion.finish(error=SerialCommandCancelled(
|
| 158 |
+
"serial command became stale"))
|
| 159 |
+
else:
|
| 160 |
+
item.completion.finish(response=response)
|
| 161 |
+
|
| 162 |
+
if current and item.callback and not has_newer:
|
| 163 |
+
try:
|
| 164 |
+
self._dispatch_callback(item.callback, response)
|
| 165 |
+
except Exception:
|
| 166 |
+
# UI dispatcher 的生命周期独立于串口 worker;窗口已销毁时
|
| 167 |
+
# 不能让唯一命令线程随之退出。
|
| 168 |
+
pass
|
| 169 |
+
|
| 170 |
+
with self._state_lock:
|
| 171 |
+
self._active = None
|
| 172 |
+
self._running = False
|
| 173 |
+
|
| 174 |
+
@staticmethod
|
| 175 |
+
def _cancel_item(item, reason: str) -> None:
|
| 176 |
+
if item is not None and item.completion is not None:
|
| 177 |
+
item.completion.finish(
|
| 178 |
+
error=SerialCommandCancelled(reason))
|
| 179 |
+
|
| 180 |
+
def _invalidate_locked(self, reason: str) -> None:
|
| 181 |
+
self._generation += 1
|
| 182 |
+
self._pending_keys.clear()
|
| 183 |
+
latest = tuple(self._latest_by_key.values())
|
| 184 |
+
self._latest_by_key.clear()
|
| 185 |
+
for item in latest:
|
| 186 |
+
self._cancel_item(item, reason)
|
| 187 |
+
|
| 188 |
+
def clear(self) -> int:
|
| 189 |
+
"""推进 generation,并丢弃断开前尚未执行的命令。"""
|
| 190 |
+
with self._state_lock:
|
| 191 |
+
self._invalidate_locked("serial queue cleared")
|
| 192 |
+
generation = self._generation
|
| 193 |
+
while True:
|
| 194 |
+
try:
|
| 195 |
+
item = self._queue.get_nowait()
|
| 196 |
+
except queue.Empty:
|
| 197 |
+
break
|
| 198 |
+
if item is None:
|
| 199 |
+
# 不得吞掉 stop 已发布的唯一唤醒哨兵。
|
| 200 |
+
self._queue.put(None)
|
| 201 |
+
break
|
| 202 |
+
self._cancel_item(item, "serial queue cleared")
|
| 203 |
+
return generation
|
| 204 |
+
|
| 205 |
+
def advance_generation(self) -> int:
|
| 206 |
+
"""成功重连后建立新的回调 generation。"""
|
| 207 |
+
with self._state_lock:
|
| 208 |
+
self._invalidate_locked("serial connection generation advanced")
|
| 209 |
+
return self._generation
|
| 210 |
+
|
| 211 |
+
def submit(
|
| 212 |
+
self,
|
| 213 |
+
command: str,
|
| 214 |
+
callback: Optional[Callable[[str], None]] = None,
|
| 215 |
+
key: Optional[str] = None,
|
| 216 |
+
mode: str = "queue",
|
| 217 |
+
*,
|
| 218 |
+
_completion: Optional[_Completion] = None,
|
| 219 |
+
) -> bool:
|
| 220 |
+
"""提交命令;返回 ``False`` 表示被同 key 的 ``skip`` 合并。"""
|
| 221 |
+
if mode not in self.VALID_MODES:
|
| 222 |
+
raise ValueError(f"unsupported serial command mode: {mode}")
|
| 223 |
+
with self._state_lock:
|
| 224 |
+
if not self._running:
|
| 225 |
+
raise RuntimeError("serial command worker is not running")
|
| 226 |
+
item = _Command(
|
| 227 |
+
command=str(command),
|
| 228 |
+
callback=callback,
|
| 229 |
+
key=key,
|
| 230 |
+
mode=mode,
|
| 231 |
+
generation=self._generation,
|
| 232 |
+
completion=_completion,
|
| 233 |
+
)
|
| 234 |
+
if key:
|
| 235 |
+
if key in self._pending_keys:
|
| 236 |
+
if mode == "latest":
|
| 237 |
+
previous = self._latest_by_key.get(key)
|
| 238 |
+
self._latest_by_key[key] = item
|
| 239 |
+
self._cancel_item(
|
| 240 |
+
previous, "serial latest command superseded")
|
| 241 |
+
return True
|
| 242 |
+
self._cancel_item(
|
| 243 |
+
item, "duplicate serial command skipped")
|
| 244 |
+
return False
|
| 245 |
+
self._pending_keys.add(key)
|
| 246 |
+
self._queue.put(item)
|
| 247 |
+
return True
|
| 248 |
+
|
| 249 |
+
def execute(
|
| 250 |
+
self,
|
| 251 |
+
command: str,
|
| 252 |
+
*,
|
| 253 |
+
timeout: Optional[float] = None,
|
| 254 |
+
key: Optional[str] = None,
|
| 255 |
+
mode: str = "queue",
|
| 256 |
+
) -> str:
|
| 257 |
+
"""通过同一 worker 同步执行命令。"""
|
| 258 |
+
completion = _Completion(threading.Event())
|
| 259 |
+
accepted = self.submit(
|
| 260 |
+
command, key=key, mode=mode, _completion=completion)
|
| 261 |
+
if not accepted:
|
| 262 |
+
raise SerialCommandCancelled(
|
| 263 |
+
"duplicate serial command skipped")
|
| 264 |
+
if not completion.event.wait(timeout=timeout):
|
| 265 |
+
raise TimeoutError(f"serial command timed out: {command}")
|
| 266 |
+
if completion.error is not None:
|
| 267 |
+
raise completion.error
|
| 268 |
+
return completion.response or "ERR"
|
| 269 |
+
|
| 270 |
+
# ``request`` 是语义更明确的同步别名。
|
| 271 |
+
request = execute
|
| 272 |
+
|
| 273 |
+
def stop(self) -> bool:
|
| 274 |
+
"""停止 worker,并保持原实现的 1 秒 join 上限。"""
|
| 275 |
+
with self._state_lock:
|
| 276 |
+
if not self._running and (
|
| 277 |
+
self._thread is None or not self._thread.is_alive()):
|
| 278 |
+
return True
|
| 279 |
+
self._running = False
|
| 280 |
+
self._invalidate_locked("serial worker stopped")
|
| 281 |
+
self._cancel_item(self._active, "serial worker stopped")
|
| 282 |
+
thread = self._thread
|
| 283 |
+
while True:
|
| 284 |
+
try:
|
| 285 |
+
item = self._queue.get_nowait()
|
| 286 |
+
except queue.Empty:
|
| 287 |
+
break
|
| 288 |
+
if item is not None:
|
| 289 |
+
self._cancel_item(item, "serial worker stopped")
|
| 290 |
+
self._queue.put(None)
|
| 291 |
+
if (thread is not None and thread.is_alive()
|
| 292 |
+
and thread is not threading.current_thread()):
|
| 293 |
+
thread.join(timeout=self.STOP_TIMEOUT)
|
| 294 |
+
return not (thread is not None and thread.is_alive())
|
collector/core/gripper/devices/tactile_discovery.py
ADDED
|
@@ -0,0 +1,1707 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Sightac left/right 发现、Flash 缓存和 TouchSensor 构造。"""
|
| 2 |
+
|
| 3 |
+
from __future__ import annotations
|
| 4 |
+
|
| 5 |
+
import ctypes
|
| 6 |
+
import glob
|
| 7 |
+
import importlib
|
| 8 |
+
import json
|
| 9 |
+
import math
|
| 10 |
+
import os
|
| 11 |
+
import re
|
| 12 |
+
import sys
|
| 13 |
+
import threading
|
| 14 |
+
from types import MappingProxyType
|
| 15 |
+
from typing import Callable, Optional
|
| 16 |
+
|
| 17 |
+
import cv2
|
| 18 |
+
|
| 19 |
+
from core.gripper.fays_runtime import GRIPPER_DIR
|
| 20 |
+
|
| 21 |
+
|
| 22 |
+
_ERROR_MARKERS = (
|
| 23 |
+
"error",
|
| 24 |
+
"failed",
|
| 25 |
+
"exception",
|
| 26 |
+
"traceback",
|
| 27 |
+
"cannot",
|
| 28 |
+
"unable",
|
| 29 |
+
"denied",
|
| 30 |
+
"timeout",
|
| 31 |
+
"timed out",
|
| 32 |
+
"device busy",
|
| 33 |
+
"resource busy",
|
| 34 |
+
"错误",
|
| 35 |
+
"失败",
|
| 36 |
+
)
|
| 37 |
+
_SONIX_MODALIAS_PREFIX = "usb:v0c45p636f"
|
| 38 |
+
_SIGHTAC_API_PACKAGES = frozenset({
|
| 39 |
+
"api",
|
| 40 |
+
"api_legacy_2_4_5",
|
| 41 |
+
"api_new",
|
| 42 |
+
"api_v3_2_2_ksq",
|
| 43 |
+
})
|
| 44 |
+
|
| 45 |
+
|
| 46 |
+
class TactileDiscovery:
|
| 47 |
+
"""发现两路 Sonix Sightac,并在开流前缓存完整 Flash 参数。
|
| 48 |
+
|
| 49 |
+
Linux 探测是 fail-closed 的:只有 ``/sys/class/video4linux/videoN`` 的
|
| 50 |
+
``name``、``index`` 和节点自身 ``device/modalias`` 都能只读确认、
|
| 51 |
+
``index == 0`` 且 VID/PID 为 ``0c45:636f`` 时,节点才会交给 Sonix
|
| 52 |
+
SDK。旧模板身份只读解析 video class symlink 对应的 USB interface 及其
|
| 53 |
+
唯一物理父设备,不遍历 USB 总线,不修改 sysfs,也不执行 USB reset。
|
| 54 |
+
"""
|
| 55 |
+
|
| 56 |
+
def __init__(
|
| 57 |
+
self,
|
| 58 |
+
*,
|
| 59 |
+
sysfs_root: str = "/sys/class/video4linux",
|
| 60 |
+
dev_root: str = "/dev",
|
| 61 |
+
platform: Optional[str] = None,
|
| 62 |
+
sightac_root: Optional[str] = None,
|
| 63 |
+
cv2_module=cv2,
|
| 64 |
+
path_exists: Callable[[str], bool] = os.path.exists,
|
| 65 |
+
logger: Callable[..., None] = print,
|
| 66 |
+
full_logs: Optional[bool] = None,
|
| 67 |
+
old_sensor_template_file: Optional[str] = None,
|
| 68 |
+
api_package: Optional[str] = None,
|
| 69 |
+
):
|
| 70 |
+
self.sysfs_root = os.path.abspath(sysfs_root)
|
| 71 |
+
self.dev_root = os.path.abspath(dev_root)
|
| 72 |
+
self.platform = platform or sys.platform
|
| 73 |
+
self.sightac_root = sightac_root
|
| 74 |
+
requested_api = (
|
| 75 |
+
api_package
|
| 76 |
+
or os.environ.get("KSQ_SIGHTAC_API_PACKAGE")
|
| 77 |
+
or "api_new"
|
| 78 |
+
).strip()
|
| 79 |
+
if requested_api not in _SIGHTAC_API_PACKAGES:
|
| 80 |
+
raise ValueError(
|
| 81 |
+
"unsupported Sightac API package: "
|
| 82 |
+
f"{requested_api!r}; expected one of "
|
| 83 |
+
f"{sorted(_SIGHTAC_API_PACKAGES)}"
|
| 84 |
+
)
|
| 85 |
+
self.api_package = requested_api
|
| 86 |
+
self._cv2 = cv2_module
|
| 87 |
+
self._path_exists = path_exists
|
| 88 |
+
self._logger = logger
|
| 89 |
+
if full_logs is None:
|
| 90 |
+
full_logs = os.environ.get(
|
| 91 |
+
"KSQ_FULL_LOG", "").strip().lower() in {
|
| 92 |
+
"1", "true", "yes", "on",
|
| 93 |
+
}
|
| 94 |
+
self.full_logs = bool(full_logs)
|
| 95 |
+
self.flash_param_cache = {}
|
| 96 |
+
self.flash_full_params_cache = {}
|
| 97 |
+
self._camera_names = None
|
| 98 |
+
self._sonix_dshow_map = None
|
| 99 |
+
self._scan_lock = threading.RLock()
|
| 100 |
+
self._reported_rejections = set()
|
| 101 |
+
self._reported_touch_sensor_runtime = False
|
| 102 |
+
self._old_sensor_template_file = (
|
| 103 |
+
None
|
| 104 |
+
if old_sensor_template_file is None
|
| 105 |
+
else os.path.abspath(old_sensor_template_file)
|
| 106 |
+
)
|
| 107 |
+
self._old_sensor_template = self._load_old_sensor_template(
|
| 108 |
+
self._old_sensor_template_file
|
| 109 |
+
)
|
| 110 |
+
|
| 111 |
+
@staticmethod
|
| 112 |
+
def _validated_template_vector(value, name: str, length: int):
|
| 113 |
+
if (
|
| 114 |
+
not isinstance(value, list)
|
| 115 |
+
or len(value) != int(length)
|
| 116 |
+
):
|
| 117 |
+
raise ValueError(
|
| 118 |
+
f"{name} must contain exactly {int(length)} values"
|
| 119 |
+
)
|
| 120 |
+
result = []
|
| 121 |
+
for item in value:
|
| 122 |
+
if isinstance(item, bool):
|
| 123 |
+
raise ValueError(f"{name} must contain finite numbers")
|
| 124 |
+
number = float(item)
|
| 125 |
+
if not math.isfinite(number):
|
| 126 |
+
raise ValueError(f"{name} must contain finite numbers")
|
| 127 |
+
result.append(number)
|
| 128 |
+
return result
|
| 129 |
+
|
| 130 |
+
def _load_old_sensor_template(self, path: Optional[str]):
|
| 131 |
+
"""加载明确禁用力输出的旧传感器临时模板。"""
|
| 132 |
+
if not path:
|
| 133 |
+
return None
|
| 134 |
+
try:
|
| 135 |
+
with open(path, encoding="utf-8") as stream:
|
| 136 |
+
encoded = json.load(stream)
|
| 137 |
+
if not isinstance(encoded, dict):
|
| 138 |
+
raise ValueError("template root must be an object")
|
| 139 |
+
if encoded.get("enabled") is not True:
|
| 140 |
+
return None
|
| 141 |
+
if encoded.get("mode") != "old-sensor-template-no-force":
|
| 142 |
+
raise ValueError("unsupported template mode")
|
| 143 |
+
if encoded.get("force_output_enabled") is not False:
|
| 144 |
+
raise ValueError(
|
| 145 |
+
"old-sensor template must keep force output disabled"
|
| 146 |
+
)
|
| 147 |
+
usb_identities = encoded.get("usb_identities")
|
| 148 |
+
templates = encoded.get("templates")
|
| 149 |
+
if (
|
| 150 |
+
not isinstance(usb_identities, dict)
|
| 151 |
+
or not isinstance(templates, dict)
|
| 152 |
+
):
|
| 153 |
+
raise ValueError(
|
| 154 |
+
"usb_identities and templates must be objects"
|
| 155 |
+
)
|
| 156 |
+
|
| 157 |
+
validated_identities = {}
|
| 158 |
+
validated_templates = {}
|
| 159 |
+
for side in ("left", "right"):
|
| 160 |
+
identity = usb_identities.get(side)
|
| 161 |
+
if not isinstance(identity, dict):
|
| 162 |
+
raise ValueError(
|
| 163 |
+
f"missing USB identity for {side!r}"
|
| 164 |
+
)
|
| 165 |
+
vendor_id = str(
|
| 166 |
+
identity.get("vendor_id", "")).strip().lower()
|
| 167 |
+
product_id = str(
|
| 168 |
+
identity.get("product_id", "")).strip().lower()
|
| 169 |
+
serial = str(identity.get("serial", "")).strip()
|
| 170 |
+
interface_number = str(
|
| 171 |
+
identity.get("interface_number", "")).strip().lower()
|
| 172 |
+
stream_index = identity.get("stream_index")
|
| 173 |
+
if (
|
| 174 |
+
not re.fullmatch(r"[0-9a-f]{4}", vendor_id)
|
| 175 |
+
or not re.fullmatch(r"[0-9a-f]{4}", product_id)
|
| 176 |
+
or not serial
|
| 177 |
+
or not re.fullmatch(r"[A-Za-z0-9._-]+", serial)
|
| 178 |
+
or not re.fullmatch(
|
| 179 |
+
r"[0-9a-f]{2}", interface_number)
|
| 180 |
+
or isinstance(stream_index, bool)
|
| 181 |
+
or not isinstance(stream_index, int)
|
| 182 |
+
or stream_index != 0
|
| 183 |
+
):
|
| 184 |
+
raise ValueError(
|
| 185 |
+
f"invalid USB identity for {side!r}"
|
| 186 |
+
)
|
| 187 |
+
if (
|
| 188 |
+
vendor_id != "0c45"
|
| 189 |
+
or product_id != "636f"
|
| 190 |
+
or interface_number != "00"
|
| 191 |
+
):
|
| 192 |
+
raise ValueError(
|
| 193 |
+
f"non-Sightac USB identity for {side!r}"
|
| 194 |
+
)
|
| 195 |
+
validated_identities[side] = {
|
| 196 |
+
"vendor_id": vendor_id,
|
| 197 |
+
"product_id": product_id,
|
| 198 |
+
"serial": serial,
|
| 199 |
+
"interface_number": interface_number,
|
| 200 |
+
"stream_index": stream_index,
|
| 201 |
+
}
|
| 202 |
+
|
| 203 |
+
source = templates.get(side)
|
| 204 |
+
if not isinstance(source, dict):
|
| 205 |
+
raise ValueError(
|
| 206 |
+
f"missing template for {side!r}"
|
| 207 |
+
)
|
| 208 |
+
device_type = str(
|
| 209 |
+
source.get("device_type", "")).strip().lower()
|
| 210 |
+
if device_type not in {"planar", "curved", "bevel"}:
|
| 211 |
+
raise ValueError(
|
| 212 |
+
f"invalid device_type for {side!r}"
|
| 213 |
+
)
|
| 214 |
+
params = {
|
| 215 |
+
"device_type": device_type,
|
| 216 |
+
"fx_params": self._validated_template_vector(
|
| 217 |
+
source.get("fx_params"),
|
| 218 |
+
f"{side}.fx_params",
|
| 219 |
+
5,
|
| 220 |
+
),
|
| 221 |
+
"fy_params": self._validated_template_vector(
|
| 222 |
+
source.get("fy_params"),
|
| 223 |
+
f"{side}.fy_params",
|
| 224 |
+
5,
|
| 225 |
+
),
|
| 226 |
+
"fz_params": self._validated_template_vector(
|
| 227 |
+
source.get("fz_params"),
|
| 228 |
+
f"{side}.fz_params",
|
| 229 |
+
5,
|
| 230 |
+
),
|
| 231 |
+
"fw_params": self._validated_template_vector(
|
| 232 |
+
source.get("fw_params"),
|
| 233 |
+
f"{side}.fw_params",
|
| 234 |
+
9,
|
| 235 |
+
),
|
| 236 |
+
"flash_state": "old-sensor-template",
|
| 237 |
+
"calibration_source":
|
| 238 |
+
"old-sensor-template-unverified",
|
| 239 |
+
}
|
| 240 |
+
if device_type == "planar":
|
| 241 |
+
fw = params["fw_params"]
|
| 242 |
+
x1, x2, y1, y2 = (
|
| 243 |
+
int(fw[1]),
|
| 244 |
+
int(fw[2]),
|
| 245 |
+
int(fw[3]),
|
| 246 |
+
int(fw[4]),
|
| 247 |
+
)
|
| 248 |
+
if not (
|
| 249 |
+
0 <= x1 < x2 <= 640
|
| 250 |
+
and 0 <= y1 < y2 <= 480
|
| 251 |
+
):
|
| 252 |
+
raise ValueError(
|
| 253 |
+
f"invalid planar ROI for {side!r}"
|
| 254 |
+
)
|
| 255 |
+
validated_templates[side] = params
|
| 256 |
+
|
| 257 |
+
if (
|
| 258 |
+
validated_identities["left"]
|
| 259 |
+
== validated_identities["right"]
|
| 260 |
+
or validated_identities["left"]["serial"]
|
| 261 |
+
== validated_identities["right"]["serial"]
|
| 262 |
+
):
|
| 263 |
+
raise ValueError(
|
| 264 |
+
"left and right cannot use the same USB identity"
|
| 265 |
+
)
|
| 266 |
+
self._logger(
|
| 267 |
+
"[Tactile] old-sensor template armed: "
|
| 268 |
+
f"left={validated_identities['left']['serial']} "
|
| 269 |
+
f"right={validated_identities['right']['serial']} "
|
| 270 |
+
"(force output disabled)"
|
| 271 |
+
)
|
| 272 |
+
return {
|
| 273 |
+
"usb_identities": validated_identities,
|
| 274 |
+
"templates": validated_templates,
|
| 275 |
+
}
|
| 276 |
+
except (
|
| 277 |
+
OSError,
|
| 278 |
+
TypeError,
|
| 279 |
+
ValueError,
|
| 280 |
+
json.JSONDecodeError,
|
| 281 |
+
) as exc:
|
| 282 |
+
self._logger(
|
| 283 |
+
f"[Tactile] old-sensor template disabled: {exc}"
|
| 284 |
+
)
|
| 285 |
+
return None
|
| 286 |
+
|
| 287 |
+
@property
|
| 288 |
+
def old_sensor_template_enabled(self) -> bool:
|
| 289 |
+
return self._old_sensor_template is not None
|
| 290 |
+
|
| 291 |
+
def physical_usb_port(self, video_index: int) -> Optional[str]:
|
| 292 |
+
"""从 video4linux class symlink 提取仅用于诊断的 USB 端口。"""
|
| 293 |
+
if not self.platform.startswith("linux"):
|
| 294 |
+
return None
|
| 295 |
+
device_path = os.path.realpath(os.path.join(
|
| 296 |
+
self._candidate_class_dir(int(video_index)),
|
| 297 |
+
"device",
|
| 298 |
+
))
|
| 299 |
+
interface_name = os.path.basename(device_path)
|
| 300 |
+
port, separator, _interface = interface_name.partition(":")
|
| 301 |
+
if not separator or not port:
|
| 302 |
+
return None
|
| 303 |
+
return port
|
| 304 |
+
|
| 305 |
+
def usb_video_identity(self, video_index: int):
|
| 306 |
+
"""只读返回 Sightac 节点的 VID/PID、serial、interface 和 stream。"""
|
| 307 |
+
try:
|
| 308 |
+
index = int(video_index)
|
| 309 |
+
except (TypeError, ValueError):
|
| 310 |
+
return None
|
| 311 |
+
if not self.platform.startswith("linux"):
|
| 312 |
+
return None
|
| 313 |
+
if self.probe_guard_reason(index) is not None:
|
| 314 |
+
return None
|
| 315 |
+
class_dir = self._candidate_class_dir(index)
|
| 316 |
+
try:
|
| 317 |
+
with open(
|
| 318 |
+
os.path.join(class_dir, "device", "modalias"),
|
| 319 |
+
encoding="utf-8",
|
| 320 |
+
) as stream:
|
| 321 |
+
modalias = stream.read().strip().lower()
|
| 322 |
+
match = re.match(
|
| 323 |
+
r"usb:v([0-9a-f]{4})p([0-9a-f]{4})",
|
| 324 |
+
modalias,
|
| 325 |
+
)
|
| 326 |
+
if match is None:
|
| 327 |
+
return None
|
| 328 |
+
with open(
|
| 329 |
+
os.path.join(
|
| 330 |
+
class_dir, "device", "bInterfaceNumber"),
|
| 331 |
+
encoding="utf-8",
|
| 332 |
+
) as stream:
|
| 333 |
+
interface_number = stream.read().strip().lower()
|
| 334 |
+
with open(
|
| 335 |
+
os.path.join(class_dir, "index"),
|
| 336 |
+
encoding="utf-8",
|
| 337 |
+
) as stream:
|
| 338 |
+
stream_index = int(stream.read().strip())
|
| 339 |
+
|
| 340 |
+
interface_path = os.path.realpath(
|
| 341 |
+
os.path.join(class_dir, "device"))
|
| 342 |
+
physical_path = (
|
| 343 |
+
os.path.dirname(interface_path)
|
| 344 |
+
if ":" in os.path.basename(interface_path)
|
| 345 |
+
else interface_path
|
| 346 |
+
)
|
| 347 |
+
with open(
|
| 348 |
+
os.path.join(physical_path, "serial"),
|
| 349 |
+
encoding="utf-8",
|
| 350 |
+
) as stream:
|
| 351 |
+
serial = stream.read().strip()
|
| 352 |
+
except (OSError, ValueError):
|
| 353 |
+
return None
|
| 354 |
+
if (
|
| 355 |
+
not serial
|
| 356 |
+
or not re.fullmatch(r"[A-Za-z0-9._-]+", serial)
|
| 357 |
+
or not re.fullmatch(
|
| 358 |
+
r"[0-9a-f]{2}", interface_number)
|
| 359 |
+
):
|
| 360 |
+
return None
|
| 361 |
+
return {
|
| 362 |
+
"vendor_id": match.group(1),
|
| 363 |
+
"product_id": match.group(2),
|
| 364 |
+
"serial": serial,
|
| 365 |
+
"interface_number": interface_number,
|
| 366 |
+
"stream_index": stream_index,
|
| 367 |
+
}
|
| 368 |
+
|
| 369 |
+
@staticmethod
|
| 370 |
+
def _usb_identity_matches(actual, expected) -> bool:
|
| 371 |
+
if not isinstance(actual, dict):
|
| 372 |
+
return False
|
| 373 |
+
return all(
|
| 374 |
+
actual.get(key) == expected.get(key)
|
| 375 |
+
for key in (
|
| 376 |
+
"vendor_id",
|
| 377 |
+
"product_id",
|
| 378 |
+
"serial",
|
| 379 |
+
"interface_number",
|
| 380 |
+
"stream_index",
|
| 381 |
+
)
|
| 382 |
+
)
|
| 383 |
+
|
| 384 |
+
@staticmethod
|
| 385 |
+
def _usb_topology_identity_matches(actual, expected) -> bool:
|
| 386 |
+
"""Match a topology-pinned Sightac without trusting duplicate serials."""
|
| 387 |
+
if not isinstance(actual, dict):
|
| 388 |
+
return False
|
| 389 |
+
return all(
|
| 390 |
+
actual.get(key) == expected.get(key)
|
| 391 |
+
for key in (
|
| 392 |
+
"vendor_id",
|
| 393 |
+
"product_id",
|
| 394 |
+
"interface_number",
|
| 395 |
+
"stream_index",
|
| 396 |
+
)
|
| 397 |
+
)
|
| 398 |
+
|
| 399 |
+
def resolve_old_sensor_template_camera(
|
| 400 |
+
self, side: str, excluded=(),
|
| 401 |
+
) -> Optional[int]:
|
| 402 |
+
"""按旧相机严格 USB 身份给无 Flash 相机分配 left/right。"""
|
| 403 |
+
normalized = str(side).strip().lower()
|
| 404 |
+
template = self._old_sensor_template
|
| 405 |
+
if template is None or normalized not in {"left", "right"}:
|
| 406 |
+
return None
|
| 407 |
+
expected = template["usb_identities"][normalized]
|
| 408 |
+
excluded_indices = {
|
| 409 |
+
int(index) for index in excluded if index is not None
|
| 410 |
+
}
|
| 411 |
+
matches = [
|
| 412 |
+
index
|
| 413 |
+
for index in self.safe_sonix_indices(max_devices=64)
|
| 414 |
+
if index not in excluded_indices
|
| 415 |
+
and self._usb_identity_matches(
|
| 416 |
+
self.usb_video_identity(index), expected)
|
| 417 |
+
]
|
| 418 |
+
if len(matches) != 1:
|
| 419 |
+
self._logger(
|
| 420 |
+
"[Tactile] old-sensor template identity unresolved: "
|
| 421 |
+
f"{normalized} expects USB serial "
|
| 422 |
+
f"{expected['serial']}, "
|
| 423 |
+
f"matches={matches}"
|
| 424 |
+
)
|
| 425 |
+
return None
|
| 426 |
+
return int(matches[0])
|
| 427 |
+
|
| 428 |
+
def apply_old_sensor_template(
|
| 429 |
+
self,
|
| 430 |
+
side: str,
|
| 431 |
+
video_index: int,
|
| 432 |
+
*,
|
| 433 |
+
topology_verified: bool = False,
|
| 434 |
+
):
|
| 435 |
+
"""仅在硬件完整 Flash 缺失时注入旧模板;真实 Flash 永远优先。"""
|
| 436 |
+
normalized = str(side).strip().lower()
|
| 437 |
+
index = int(video_index)
|
| 438 |
+
template = self._old_sensor_template
|
| 439 |
+
if template is None or normalized not in {"left", "right"}:
|
| 440 |
+
return None
|
| 441 |
+
with self._scan_lock:
|
| 442 |
+
cached = self.flash_full_params_cache.get(index)
|
| 443 |
+
if cached is not None:
|
| 444 |
+
return cached
|
| 445 |
+
expected = template["usb_identities"][normalized]
|
| 446 |
+
actual = self.usb_video_identity(index)
|
| 447 |
+
exact_identity = self._usb_identity_matches(actual, expected)
|
| 448 |
+
physical_port = self.physical_usb_port(index)
|
| 449 |
+
expected_port_suffix = (
|
| 450 |
+
".2" if normalized == "left" else ".1"
|
| 451 |
+
)
|
| 452 |
+
topology_identity = (
|
| 453 |
+
bool(topology_verified)
|
| 454 |
+
and isinstance(physical_port, str)
|
| 455 |
+
and physical_port.endswith(expected_port_suffix)
|
| 456 |
+
and self._usb_topology_identity_matches(actual, expected)
|
| 457 |
+
)
|
| 458 |
+
if not exact_identity and not topology_identity:
|
| 459 |
+
actual_serial = (
|
| 460 |
+
actual.get("serial")
|
| 461 |
+
if isinstance(actual, dict)
|
| 462 |
+
else None
|
| 463 |
+
)
|
| 464 |
+
self._logger(
|
| 465 |
+
"[Tactile] old-sensor template rejected: "
|
| 466 |
+
f"{normalized}=video{index} has USB serial "
|
| 467 |
+
f"{actual_serial!r}, expected {expected['serial']!r}"
|
| 468 |
+
)
|
| 469 |
+
return None
|
| 470 |
+
params = {
|
| 471 |
+
key: (
|
| 472 |
+
list(value)
|
| 473 |
+
if isinstance(value, list)
|
| 474 |
+
else value
|
| 475 |
+
)
|
| 476 |
+
for key, value in template["templates"][normalized].items()
|
| 477 |
+
}
|
| 478 |
+
if topology_identity and not exact_identity:
|
| 479 |
+
params["flash_state"] = "old-sensor-template-topology"
|
| 480 |
+
params["calibration_source"] = (
|
| 481 |
+
"old-sensor-template-topology-unverified"
|
| 482 |
+
)
|
| 483 |
+
with self._scan_lock:
|
| 484 |
+
existing = self.flash_full_params_cache.get(index)
|
| 485 |
+
if existing is not None:
|
| 486 |
+
return existing
|
| 487 |
+
self.flash_full_params_cache[index] = params
|
| 488 |
+
self.flash_param_cache[normalized] = index
|
| 489 |
+
fw = params["fw_params"]
|
| 490 |
+
actual_serial = (
|
| 491 |
+
actual.get("serial")
|
| 492 |
+
if isinstance(actual, dict)
|
| 493 |
+
else None
|
| 494 |
+
)
|
| 495 |
+
self._logger(
|
| 496 |
+
"[Tactile] old-sensor template applied: "
|
| 497 |
+
f"{normalized}=video{index} serial={actual_serial} "
|
| 498 |
+
f"port={physical_port} "
|
| 499 |
+
f"device={params['device_type']} "
|
| 500 |
+
f"ROI=({int(fw[1])},{int(fw[3])})"
|
| 501 |
+
f"-({int(fw[2])},{int(fw[4])}); "
|
| 502 |
+
"Fx/Fy/Fz publication disabled"
|
| 503 |
+
)
|
| 504 |
+
return params
|
| 505 |
+
|
| 506 |
+
def clear_cache(self) -> None:
|
| 507 |
+
with self._scan_lock:
|
| 508 |
+
self.flash_param_cache.clear()
|
| 509 |
+
self.flash_full_params_cache.clear()
|
| 510 |
+
self._camera_names = None
|
| 511 |
+
self._sonix_dshow_map = None
|
| 512 |
+
self._reported_rejections.clear()
|
| 513 |
+
|
| 514 |
+
def cache_external_flash_params(self, video_index: int, params):
|
| 515 |
+
"""Store one live libusb Flash read for the next sensor instance.
|
| 516 |
+
|
| 517 |
+
This is deliberately an in-memory handoff only. The caller must
|
| 518 |
+
provide parameters read during the current UVC discovery pass; no
|
| 519 |
+
template, persistent cache, or default calibration is accepted here.
|
| 520 |
+
"""
|
| 521 |
+
index = int(video_index)
|
| 522 |
+
if not isinstance(params, dict):
|
| 523 |
+
raise ValueError("external Flash parameters must be an object")
|
| 524 |
+
device_type = str(params.get("device_type") or "").strip().lower()
|
| 525 |
+
if device_type not in {"planar", "curved", "bevel"}:
|
| 526 |
+
raise ValueError(
|
| 527 |
+
f"external Flash device_type is invalid: {device_type!r}")
|
| 528 |
+
expected_lengths = {
|
| 529 |
+
"fx_params": 5,
|
| 530 |
+
"fy_params": 5,
|
| 531 |
+
"fz_params": 21,
|
| 532 |
+
"ft_params": 20,
|
| 533 |
+
"fw_params": 9,
|
| 534 |
+
}
|
| 535 |
+
copied = {"device_type": device_type}
|
| 536 |
+
for key, length in expected_lengths.items():
|
| 537 |
+
values = params.get(key)
|
| 538 |
+
if not isinstance(values, (list, tuple)) or len(values) != length:
|
| 539 |
+
raise ValueError(
|
| 540 |
+
f"external Flash {key} must contain {length} values")
|
| 541 |
+
normalized = []
|
| 542 |
+
for value_index, value in enumerate(values):
|
| 543 |
+
if (
|
| 544 |
+
key == "fw_params"
|
| 545 |
+
and device_type == "bevel"
|
| 546 |
+
and value_index >= 5
|
| 547 |
+
and (
|
| 548 |
+
value is None
|
| 549 |
+
or (
|
| 550 |
+
isinstance(value, float)
|
| 551 |
+
and math.isnan(value)
|
| 552 |
+
)
|
| 553 |
+
)
|
| 554 |
+
):
|
| 555 |
+
# Reserved bevel-table tail; it is not consumed by the
|
| 556 |
+
# current ROI/line algorithm. The libuvc JSON path uses
|
| 557 |
+
# null, while older in-process handoffs may use NaN.
|
| 558 |
+
normalized.append(float("nan"))
|
| 559 |
+
continue
|
| 560 |
+
try:
|
| 561 |
+
number = float(value)
|
| 562 |
+
except (TypeError, ValueError) as exc:
|
| 563 |
+
raise ValueError(
|
| 564 |
+
f"external Flash {key}[{value_index}] is not numeric: "
|
| 565 |
+
f"{value!r}"
|
| 566 |
+
) from exc
|
| 567 |
+
if not math.isfinite(number):
|
| 568 |
+
raise ValueError(
|
| 569 |
+
f"external Flash {key}[{value_index}] contains a "
|
| 570 |
+
f"non-finite value: {value!r}")
|
| 571 |
+
normalized.append(number)
|
| 572 |
+
copied[key] = normalized
|
| 573 |
+
copied["flash_state"] = str(
|
| 574 |
+
params.get("flash_state") or "programmed")
|
| 575 |
+
copied["calibration_source"] = "hardware-live-flash"
|
| 576 |
+
with self._scan_lock:
|
| 577 |
+
self.flash_full_params_cache[index] = copied
|
| 578 |
+
self._logger(
|
| 579 |
+
f"[FlashCache] live libusb read cached for video{index}: "
|
| 580 |
+
f"device={device_type} source=hardware-live-flash")
|
| 581 |
+
return copied
|
| 582 |
+
|
| 583 |
+
def cache_snapshot(self):
|
| 584 |
+
"""返回不会暴露内部列表的完整缓存副本。"""
|
| 585 |
+
with self._scan_lock:
|
| 586 |
+
labels = MappingProxyType(dict(self.flash_param_cache))
|
| 587 |
+
full = {}
|
| 588 |
+
for index, params in self.flash_full_params_cache.items():
|
| 589 |
+
full[int(index)] = MappingProxyType({
|
| 590 |
+
"device_type": str(params["device_type"]),
|
| 591 |
+
"fx_params": tuple(params["fx_params"]),
|
| 592 |
+
"fy_params": tuple(params["fy_params"]),
|
| 593 |
+
"fz_params": tuple(params["fz_params"]),
|
| 594 |
+
"fw_params": tuple(params["fw_params"]),
|
| 595 |
+
"ft_params": tuple(params.get("ft_params", ())),
|
| 596 |
+
"flash_state": str(
|
| 597 |
+
params.get("flash_state", "unknown")),
|
| 598 |
+
"calibration_source": str(
|
| 599 |
+
params.get("calibration_source", "hardware")),
|
| 600 |
+
})
|
| 601 |
+
return labels, MappingProxyType(full)
|
| 602 |
+
|
| 603 |
+
def _candidate_class_dir(self, video_index: int) -> str:
|
| 604 |
+
return os.path.join(self.sysfs_root, f"video{int(video_index)}")
|
| 605 |
+
|
| 606 |
+
def _candidate_device_path(self, video_index: int) -> str:
|
| 607 |
+
return os.path.join(self.dev_root, f"video{int(video_index)}")
|
| 608 |
+
|
| 609 |
+
def probe_guard_reason(self, video_index: int):
|
| 610 |
+
"""返回 ``None`` 表示节点可探测,否则返回 fail-closed 原因。"""
|
| 611 |
+
try:
|
| 612 |
+
index = int(video_index)
|
| 613 |
+
except (TypeError, ValueError):
|
| 614 |
+
return "invalid video index"
|
| 615 |
+
if index < 0:
|
| 616 |
+
return "invalid video index"
|
| 617 |
+
if not self.platform.startswith("linux"):
|
| 618 |
+
return None
|
| 619 |
+
if not self._path_exists(self._candidate_device_path(index)):
|
| 620 |
+
return "video node does not exist"
|
| 621 |
+
class_dir = self._candidate_class_dir(index)
|
| 622 |
+
try:
|
| 623 |
+
with open(
|
| 624 |
+
os.path.join(class_dir, "index"),
|
| 625 |
+
encoding="utf-8",
|
| 626 |
+
) as stream:
|
| 627 |
+
capture_index = int(stream.read().strip())
|
| 628 |
+
with open(
|
| 629 |
+
os.path.join(class_dir, "name"),
|
| 630 |
+
encoding="utf-8",
|
| 631 |
+
) as stream:
|
| 632 |
+
device_name = stream.read().strip().lower()
|
| 633 |
+
with open(
|
| 634 |
+
os.path.join(class_dir, "device", "modalias"),
|
| 635 |
+
encoding="utf-8",
|
| 636 |
+
) as stream:
|
| 637 |
+
modalias = stream.read().strip().lower()
|
| 638 |
+
except (OSError, ValueError):
|
| 639 |
+
return "video identity is unreadable"
|
| 640 |
+
if capture_index != 0:
|
| 641 |
+
return f"non-capture V4L2 index={capture_index}"
|
| 642 |
+
if not device_name:
|
| 643 |
+
return "empty video node name"
|
| 644 |
+
if "ftdi" in device_name or "superspeed video bridge" in device_name:
|
| 645 |
+
return "Fays FTDI bridge"
|
| 646 |
+
if not modalias.startswith(_SONIX_MODALIAS_PREFIX):
|
| 647 |
+
return "non-Sonix VID/PID"
|
| 648 |
+
return None
|
| 649 |
+
|
| 650 |
+
def is_safe_sonix_probe(self, video_index: int) -> bool:
|
| 651 |
+
return self.probe_guard_reason(video_index) is None
|
| 652 |
+
|
| 653 |
+
def _linux_video_indices(self):
|
| 654 |
+
"""按数字顺序返回 sysfs 中实际存在的 ``videoN`` 节点。"""
|
| 655 |
+
indices = set()
|
| 656 |
+
pattern = os.path.join(self.sysfs_root, "video*")
|
| 657 |
+
for class_dir in glob.glob(pattern):
|
| 658 |
+
suffix = os.path.basename(class_dir)[5:]
|
| 659 |
+
if suffix.isdigit():
|
| 660 |
+
indices.add(int(suffix))
|
| 661 |
+
return tuple(sorted(indices))
|
| 662 |
+
|
| 663 |
+
def _linux_sonix_indices(
|
| 664 |
+
self, max_devices: int, *, report_rejections: bool,
|
| 665 |
+
):
|
| 666 |
+
"""发现实际 Sonix 节点;``max_devices`` 限制候选数而非节点号。"""
|
| 667 |
+
limit = max(0, int(max_devices))
|
| 668 |
+
if limit == 0:
|
| 669 |
+
return ()
|
| 670 |
+
candidates = []
|
| 671 |
+
for index in self._linux_video_indices():
|
| 672 |
+
reason = self.probe_guard_reason(index)
|
| 673 |
+
if reason is not None:
|
| 674 |
+
if report_rejections:
|
| 675 |
+
self._log_probe_rejection(index, reason)
|
| 676 |
+
continue
|
| 677 |
+
candidates.append(index)
|
| 678 |
+
if len(candidates) >= limit:
|
| 679 |
+
break
|
| 680 |
+
return tuple(candidates)
|
| 681 |
+
|
| 682 |
+
def safe_sonix_indices(self, max_devices: int = 10):
|
| 683 |
+
"""返回实际存在且 VID/PID 精确为 ``0c45:636f`` 的采集节点。
|
| 684 |
+
|
| 685 |
+
``max_devices`` 是最多返回的安全候选数量,不是 ``/dev/videoN`` 的
|
| 686 |
+
N 上限;因此相机号漂移到 video10 以上仍能被发现。
|
| 687 |
+
"""
|
| 688 |
+
if not self.platform.startswith("linux"):
|
| 689 |
+
return ()
|
| 690 |
+
return self._linux_sonix_indices(
|
| 691 |
+
max_devices, report_rejections=False,
|
| 692 |
+
)
|
| 693 |
+
|
| 694 |
+
def _log_probe_rejection(self, video_index: int, reason: str) -> None:
|
| 695 |
+
key = (int(video_index), str(reason))
|
| 696 |
+
if key in self._reported_rejections:
|
| 697 |
+
return
|
| 698 |
+
self._reported_rejections.add(key)
|
| 699 |
+
self._logger(
|
| 700 |
+
f"[Cameras] Sonix probe skipped: video{video_index} ({reason})")
|
| 701 |
+
|
| 702 |
+
def concise_sdk_print(self, *args, **kwargs) -> None:
|
| 703 |
+
"""第三方 Sightac 日志的唯一父进程过滤器。"""
|
| 704 |
+
if self.full_logs:
|
| 705 |
+
self._logger(*args, **kwargs)
|
| 706 |
+
return
|
| 707 |
+
message = " ".join(str(arg) for arg in args).strip()
|
| 708 |
+
lowered = message.lower()
|
| 709 |
+
if any(marker in lowered for marker in _ERROR_MARKERS):
|
| 710 |
+
self._logger(*args, **kwargs)
|
| 711 |
+
|
| 712 |
+
def _ensure_sightac_import_path(self) -> None:
|
| 713 |
+
candidates = []
|
| 714 |
+
if self.sightac_root:
|
| 715 |
+
candidates.append(self.sightac_root)
|
| 716 |
+
local = os.path.join(GRIPPER_DIR, "sightac_sdk-main")
|
| 717 |
+
candidates.append(str(local))
|
| 718 |
+
explicit = os.environ.get("KSQ_SIGHTAC_ROOT")
|
| 719 |
+
if explicit:
|
| 720 |
+
candidates.append(explicit)
|
| 721 |
+
for candidate in candidates:
|
| 722 |
+
normalized = os.path.abspath(os.path.normpath(candidate))
|
| 723 |
+
if os.path.isdir(normalized):
|
| 724 |
+
if normalized not in sys.path:
|
| 725 |
+
sys.path.insert(0, normalized)
|
| 726 |
+
return
|
| 727 |
+
raise RuntimeError("local Sightac SDK path is unavailable")
|
| 728 |
+
|
| 729 |
+
def install_xu_probe_guard(self, xu_module):
|
| 730 |
+
"""给 SDK init 入口安装本适配器的 fail-closed guard。"""
|
| 731 |
+
xu_module.print = self.concise_sdk_print
|
| 732 |
+
original = getattr(
|
| 733 |
+
xu_module, "_ksq_original_init_sonix_sdk", None)
|
| 734 |
+
if original is None:
|
| 735 |
+
original = xu_module.init_sonix_sdk
|
| 736 |
+
xu_module._ksq_original_init_sonix_sdk = original
|
| 737 |
+
|
| 738 |
+
def guarded_init(*args, **kwargs):
|
| 739 |
+
video_index = (
|
| 740 |
+
args[0] if args
|
| 741 |
+
else kwargs.get("video_index")
|
| 742 |
+
)
|
| 743 |
+
guard = getattr(xu_module, "_ksq_active_probe_guard", None)
|
| 744 |
+
if (video_index is not None and callable(guard)
|
| 745 |
+
and not guard(int(video_index))):
|
| 746 |
+
reporter = getattr(
|
| 747 |
+
xu_module, "_ksq_probe_rejection_reporter", None)
|
| 748 |
+
reason_getter = getattr(
|
| 749 |
+
xu_module, "_ksq_probe_guard_reason", None)
|
| 750 |
+
reason = (
|
| 751 |
+
reason_getter(int(video_index))
|
| 752 |
+
if callable(reason_getter)
|
| 753 |
+
else "rejected by tactile adapter"
|
| 754 |
+
)
|
| 755 |
+
if callable(reporter):
|
| 756 |
+
reporter(int(video_index), reason)
|
| 757 |
+
return False
|
| 758 |
+
return original(*args, **kwargs)
|
| 759 |
+
|
| 760 |
+
xu_module.init_sonix_sdk = guarded_init
|
| 761 |
+
# wrapper 动态读取当前 adapter,避免重复包裹 SDK 全局函数。
|
| 762 |
+
xu_module._ksq_active_probe_guard = self.is_safe_sonix_probe
|
| 763 |
+
xu_module._ksq_probe_guard_reason = self.probe_guard_reason
|
| 764 |
+
xu_module._ksq_probe_rejection_reporter = (
|
| 765 |
+
self._log_probe_rejection)
|
| 766 |
+
return xu_module
|
| 767 |
+
|
| 768 |
+
def _load_xu_module(self):
|
| 769 |
+
self._ensure_sightac_import_path()
|
| 770 |
+
xu_camera = importlib.import_module(
|
| 771 |
+
f"{self.api_package}.xu_camera")
|
| 772 |
+
return self.install_xu_probe_guard(xu_camera)
|
| 773 |
+
|
| 774 |
+
def _load_touch_sensor_class(self):
|
| 775 |
+
self._ensure_sightac_import_path()
|
| 776 |
+
api_module = importlib.import_module(self.api_package)
|
| 777 |
+
TouchSensor = api_module.TouchSensor
|
| 778 |
+
module = sys.modules.get(TouchSensor.__module__)
|
| 779 |
+
with self._scan_lock:
|
| 780 |
+
if not self._reported_touch_sensor_runtime:
|
| 781 |
+
version = str(
|
| 782 |
+
getattr(api_module, "VERSION", "unknown")
|
| 783 |
+
).strip() or "unknown"
|
| 784 |
+
vendor_base = str(
|
| 785 |
+
getattr(api_module, "VENDOR_BASE_VERSION", "unknown")
|
| 786 |
+
).strip() or "unknown"
|
| 787 |
+
compatibility_base = str(
|
| 788 |
+
getattr(
|
| 789 |
+
api_module,
|
| 790 |
+
"COMPATIBILITY_BASE_VERSION",
|
| 791 |
+
"unknown",
|
| 792 |
+
)
|
| 793 |
+
).strip() or "unknown"
|
| 794 |
+
source_file = os.path.abspath(
|
| 795 |
+
str(getattr(module, "__file__", "unknown"))
|
| 796 |
+
) if module is not None else "unknown"
|
| 797 |
+
self._logger(
|
| 798 |
+
"[Tactile] SDK touch_sensor.py "
|
| 799 |
+
f"version={version} package={self.api_package} "
|
| 800 |
+
f"vendor_base={vendor_base} "
|
| 801 |
+
f"compatibility_base={compatibility_base} "
|
| 802 |
+
f"file={source_file}"
|
| 803 |
+
)
|
| 804 |
+
self._reported_touch_sensor_runtime = True
|
| 805 |
+
if module is not None:
|
| 806 |
+
module.print = self.concise_sdk_print
|
| 807 |
+
# TouchSensor 会间接使用同一个 xu_camera;在构造前确保 guard 已安装。
|
| 808 |
+
self._load_xu_module()
|
| 809 |
+
return TouchSensor
|
| 810 |
+
|
| 811 |
+
def get_camera_names(self):
|
| 812 |
+
"""只读枚举名称;Linux 仅访问 video4linux class 目录。"""
|
| 813 |
+
with self._scan_lock:
|
| 814 |
+
if self._camera_names is not None:
|
| 815 |
+
return list(self._camera_names)
|
| 816 |
+
if self.platform.startswith("linux"):
|
| 817 |
+
names = {}
|
| 818 |
+
pattern = os.path.join(self.sysfs_root, "video*")
|
| 819 |
+
for class_dir in sorted(glob.glob(pattern)):
|
| 820 |
+
suffix = os.path.basename(class_dir)[5:]
|
| 821 |
+
if not suffix.isdigit():
|
| 822 |
+
continue
|
| 823 |
+
index = int(suffix)
|
| 824 |
+
try:
|
| 825 |
+
with open(
|
| 826 |
+
os.path.join(class_dir, "name"),
|
| 827 |
+
encoding="utf-8",
|
| 828 |
+
) as stream:
|
| 829 |
+
name = stream.read().strip()
|
| 830 |
+
except OSError:
|
| 831 |
+
# 名称不可读的节点不能作为名称回退候选。
|
| 832 |
+
name = "?"
|
| 833 |
+
names[index] = name
|
| 834 |
+
count = max(names) + 1 if names else 0
|
| 835 |
+
self._camera_names = tuple(
|
| 836 |
+
names.get(index, "?") for index in range(count))
|
| 837 |
+
return list(self._camera_names)
|
| 838 |
+
try:
|
| 839 |
+
from pygrabber.dshow_graph import FilterGraph
|
| 840 |
+
self._camera_names = tuple(
|
| 841 |
+
FilterGraph().get_input_devices())
|
| 842 |
+
except Exception:
|
| 843 |
+
self._camera_names = ()
|
| 844 |
+
return list(self._camera_names)
|
| 845 |
+
|
| 846 |
+
def clear_camera_name_cache(self) -> None:
|
| 847 |
+
with self._scan_lock:
|
| 848 |
+
self._camera_names = None
|
| 849 |
+
|
| 850 |
+
def find_camera_by_name(
|
| 851 |
+
self, keyword: str, occurrence: int = 0,
|
| 852 |
+
) -> int:
|
| 853 |
+
keyword = str(keyword).lower().strip()
|
| 854 |
+
found = 0
|
| 855 |
+
for index, name in enumerate(self.get_camera_names()):
|
| 856 |
+
if keyword in str(name).lower():
|
| 857 |
+
if found == int(occurrence):
|
| 858 |
+
return index
|
| 859 |
+
found += 1
|
| 860 |
+
return -1
|
| 861 |
+
|
| 862 |
+
def get_touch_xu_ids(
|
| 863 |
+
self, max_devices: int = 5, expected_count: Optional[int] = None,
|
| 864 |
+
):
|
| 865 |
+
"""调试枚举,只保留 SDK 明确识别出的 Sonix 触觉候选。"""
|
| 866 |
+
valid = []
|
| 867 |
+
try:
|
| 868 |
+
xu_camera = self._load_xu_module()
|
| 869 |
+
except Exception as exc:
|
| 870 |
+
self._logger(f"[Heatmap] XU enum unavailable: {exc}")
|
| 871 |
+
return valid
|
| 872 |
+
if self.platform.startswith("linux"):
|
| 873 |
+
indices = self._linux_sonix_indices(
|
| 874 |
+
max_devices, report_rejections=True,
|
| 875 |
+
)
|
| 876 |
+
else:
|
| 877 |
+
indices = range(max(0, int(max_devices)))
|
| 878 |
+
for index in indices:
|
| 879 |
+
try:
|
| 880 |
+
camera = xu_camera.XuCamera(index)
|
| 881 |
+
if not camera.is_available():
|
| 882 |
+
continue
|
| 883 |
+
info = camera.get_device_info() or {}
|
| 884 |
+
serial_number = str(
|
| 885 |
+
info.get("serial_number", "")).strip()
|
| 886 |
+
vid_pid = str(info.get("vid_pid", "")).strip().lower()
|
| 887 |
+
self._logger(
|
| 888 |
+
f"[Heatmap] XU candidate {index}: "
|
| 889 |
+
f"SN={serial_number or '?'} VIDPID={vid_pid or '?'}"
|
| 890 |
+
)
|
| 891 |
+
if serial_number.startswith("SN") or vid_pid == "0c45:636f":
|
| 892 |
+
valid.append(index)
|
| 893 |
+
if (
|
| 894 |
+
expected_count is not None
|
| 895 |
+
and len(valid) >= int(expected_count)
|
| 896 |
+
):
|
| 897 |
+
break
|
| 898 |
+
except Exception as exc:
|
| 899 |
+
self._logger(
|
| 900 |
+
f"[Heatmap] XU candidate {index} failed: {exc}")
|
| 901 |
+
self._logger(f"[Heatmap] touch XU ids: {valid}")
|
| 902 |
+
return valid
|
| 903 |
+
|
| 904 |
+
@staticmethod
|
| 905 |
+
def _decode_ascii(raw: bytes) -> str:
|
| 906 |
+
if all(byte == 0xFF for byte in raw):
|
| 907 |
+
return "unknown"
|
| 908 |
+
return raw.split(b"\x00", 1)[0].decode(
|
| 909 |
+
"ascii", errors="replace")
|
| 910 |
+
|
| 911 |
+
def read_flash_side(self, video_index: int) -> Optional[str]:
|
| 912 |
+
"""Read Flash ``device_params[1]`` (``left``/``right``) for one node.
|
| 913 |
+
|
| 914 |
+
The node is opened through the XU control channel only and is
|
| 915 |
+
released before returning; no ``VideoCapture`` or image stream is
|
| 916 |
+
created here.
|
| 917 |
+
"""
|
| 918 |
+
try:
|
| 919 |
+
index = int(video_index)
|
| 920 |
+
except (TypeError, ValueError):
|
| 921 |
+
return None
|
| 922 |
+
reason = self.probe_guard_reason(index)
|
| 923 |
+
if reason is not None:
|
| 924 |
+
self._log_probe_rejection(index, reason)
|
| 925 |
+
return None
|
| 926 |
+
try:
|
| 927 |
+
xc = self._load_xu_module()
|
| 928 |
+
except Exception as exc:
|
| 929 |
+
self._logger(f"[FlashSide] XU unavailable: {exc}")
|
| 930 |
+
return None
|
| 931 |
+
try:
|
| 932 |
+
if getattr(xc, "_sdk_initialized", False):
|
| 933 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 934 |
+
if library is not None:
|
| 935 |
+
try:
|
| 936 |
+
library.SonixCam_UnInit()
|
| 937 |
+
except Exception:
|
| 938 |
+
pass
|
| 939 |
+
xc._sdk_initialized = False
|
| 940 |
+
if not xc.init_sonix_sdk(video_index=index):
|
| 941 |
+
return None
|
| 942 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 943 |
+
if library is None:
|
| 944 |
+
return None
|
| 945 |
+
buf = (ctypes.c_ubyte * 8)()
|
| 946 |
+
if not library.SonixCam_SerialFlashRead(
|
| 947 |
+
xc.DEVICE_BASE_ADDR + 1 * 8, buf, 8
|
| 948 |
+
):
|
| 949 |
+
return None
|
| 950 |
+
value = self._decode_ascii(bytes(buf)).lower().strip()
|
| 951 |
+
return value if value in {"left", "right"} else None
|
| 952 |
+
except Exception as exc:
|
| 953 |
+
self._logger(f"[FlashSide] video{index} read failed: {exc}")
|
| 954 |
+
return None
|
| 955 |
+
finally:
|
| 956 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 957 |
+
if (
|
| 958 |
+
getattr(xc, "_sdk_initialized", False)
|
| 959 |
+
and library is not None
|
| 960 |
+
):
|
| 961 |
+
try:
|
| 962 |
+
library.SonixCam_UnInit()
|
| 963 |
+
except Exception:
|
| 964 |
+
pass
|
| 965 |
+
xc._sdk_initialized = False
|
| 966 |
+
|
| 967 |
+
def read_sonix_product_serial(self, video_index: int) -> Optional[str]:
|
| 968 |
+
"""Read the Sonix XU product serial for one fail-closed node."""
|
| 969 |
+
try:
|
| 970 |
+
index = int(video_index)
|
| 971 |
+
except (TypeError, ValueError):
|
| 972 |
+
return None
|
| 973 |
+
reason = self.probe_guard_reason(index)
|
| 974 |
+
if reason is not None:
|
| 975 |
+
self._log_probe_rejection(index, reason)
|
| 976 |
+
return None
|
| 977 |
+
try:
|
| 978 |
+
xc = self._load_xu_module()
|
| 979 |
+
if getattr(xc, "_sdk_initialized", False):
|
| 980 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 981 |
+
if library is not None:
|
| 982 |
+
try:
|
| 983 |
+
library.SonixCam_UnInit()
|
| 984 |
+
except Exception:
|
| 985 |
+
pass
|
| 986 |
+
xc._sdk_initialized = False
|
| 987 |
+
if not xc.init_sonix_sdk(video_index=index):
|
| 988 |
+
return None
|
| 989 |
+
camera = xc.XuCamera(usb_id=index)
|
| 990 |
+
info = camera.get_device_info() or {}
|
| 991 |
+
serial = str(info.get("serial_number", "")).strip()
|
| 992 |
+
return serial or None
|
| 993 |
+
except Exception as exc:
|
| 994 |
+
self._logger(
|
| 995 |
+
f"[SonixSerial] video{index} read failed: {exc}"
|
| 996 |
+
)
|
| 997 |
+
return None
|
| 998 |
+
finally:
|
| 999 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 1000 |
+
if (
|
| 1001 |
+
getattr(xc, "_sdk_initialized", False)
|
| 1002 |
+
and library is not None
|
| 1003 |
+
):
|
| 1004 |
+
try:
|
| 1005 |
+
library.SonixCam_UnInit()
|
| 1006 |
+
except Exception:
|
| 1007 |
+
pass
|
| 1008 |
+
xc._sdk_initialized = False
|
| 1009 |
+
|
| 1010 |
+
@staticmethod
|
| 1011 |
+
def _decode_double(raw: bytes) -> float:
|
| 1012 |
+
if all(byte == 0xFF for byte in raw):
|
| 1013 |
+
return 0.0
|
| 1014 |
+
value = ctypes.c_double()
|
| 1015 |
+
ctypes.memmove(ctypes.addressof(value), raw, 8)
|
| 1016 |
+
return float(value.value)
|
| 1017 |
+
|
| 1018 |
+
def _read_full_params(self, xc, read_at):
|
| 1019 |
+
reads = {"ok": 0, "all_ff": 0, "payload": 0, "failed": 0}
|
| 1020 |
+
|
| 1021 |
+
def read_raw(address, buf):
|
| 1022 |
+
if not read_at(address, buf):
|
| 1023 |
+
reads["failed"] += 1
|
| 1024 |
+
return False
|
| 1025 |
+
reads["ok"] += 1
|
| 1026 |
+
if all(byte == 0xFF for byte in bytes(buf)):
|
| 1027 |
+
reads["all_ff"] += 1
|
| 1028 |
+
else:
|
| 1029 |
+
reads["payload"] += 1
|
| 1030 |
+
return True
|
| 1031 |
+
|
| 1032 |
+
device_values = []
|
| 1033 |
+
for offset in range(5):
|
| 1034 |
+
buf = (ctypes.c_ubyte * 8)()
|
| 1035 |
+
if read_raw(xc.DEVICE_BASE_ADDR + offset * 8, buf):
|
| 1036 |
+
device_values.append(self._decode_ascii(bytes(buf)))
|
| 1037 |
+
else:
|
| 1038 |
+
device_values.append("unknown")
|
| 1039 |
+
|
| 1040 |
+
def read_doubles(base_address, count):
|
| 1041 |
+
values = []
|
| 1042 |
+
for offset in range(count):
|
| 1043 |
+
buf = (ctypes.c_ubyte * 8)()
|
| 1044 |
+
if read_raw(base_address + offset * 8, buf):
|
| 1045 |
+
values.append(self._decode_double(bytes(buf)))
|
| 1046 |
+
else:
|
| 1047 |
+
values.append(0.0)
|
| 1048 |
+
return values
|
| 1049 |
+
|
| 1050 |
+
result = {
|
| 1051 |
+
"device_type": (
|
| 1052 |
+
device_values[0] if device_values else "unknown"),
|
| 1053 |
+
"fx_params": read_doubles(xc.FX_BASE_ADDR, 5),
|
| 1054 |
+
"fy_params": read_doubles(xc.FY_BASE_ADDR, 5),
|
| 1055 |
+
# 0902 FZ21 包含模型选择及两种布局;FT20 包含四组区域。
|
| 1056 |
+
# 必须完整读取,不可固定把第7/8项当成所有模型的R/B阈值。
|
| 1057 |
+
"fz_params": read_doubles(xc.FZ_BASE_ADDR, 21),
|
| 1058 |
+
"ft_params": read_doubles(0x47000, 20),
|
| 1059 |
+
"fw_params": read_doubles(xc.FW_BASE_ADDR, 9),
|
| 1060 |
+
}
|
| 1061 |
+
if reads["payload"]:
|
| 1062 |
+
result["flash_state"] = (
|
| 1063 |
+
"programmed" if not reads["failed"] else "partial")
|
| 1064 |
+
else:
|
| 1065 |
+
result["flash_state"] = "unreadable"
|
| 1066 |
+
result["calibration_source"] = "hardware"
|
| 1067 |
+
return result
|
| 1068 |
+
|
| 1069 |
+
def read_and_cache_flash_params(self, xc, video_index: int):
|
| 1070 |
+
"""从已绑定节点读取 device/fx/fy/fz/fw 全量参数。"""
|
| 1071 |
+
video_index = int(video_index)
|
| 1072 |
+
with self._scan_lock:
|
| 1073 |
+
cached = self.flash_full_params_cache.get(video_index)
|
| 1074 |
+
if cached is not None:
|
| 1075 |
+
return cached
|
| 1076 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 1077 |
+
if library is None:
|
| 1078 |
+
return None
|
| 1079 |
+
try:
|
| 1080 |
+
cached = None
|
| 1081 |
+
for attempt in range(1, 3):
|
| 1082 |
+
candidate = self._read_full_params(
|
| 1083 |
+
xc,
|
| 1084 |
+
lambda address, buf: library.SonixCam_SerialFlashRead(
|
| 1085 |
+
address, buf, 8),
|
| 1086 |
+
)
|
| 1087 |
+
if (
|
| 1088 |
+
candidate["flash_state"] == "programmed"
|
| 1089 |
+
and candidate["device_type"]
|
| 1090 |
+
in {"planar", "curved", "bevel"}
|
| 1091 |
+
):
|
| 1092 |
+
cached = candidate
|
| 1093 |
+
break
|
| 1094 |
+
if attempt == 1:
|
| 1095 |
+
self._logger(
|
| 1096 |
+
f"[FlashCache] video{video_index}: direct read "
|
| 1097 |
+
f"returned {candidate['flash_state']} "
|
| 1098 |
+
f"(device={candidate['device_type']!r}); retrying"
|
| 1099 |
+
)
|
| 1100 |
+
if cached is None:
|
| 1101 |
+
self._logger(
|
| 1102 |
+
f"[FlashCache] video{video_index} read failed: "
|
| 1103 |
+
"no usable hardware parameters after 2 direct reads; "
|
| 1104 |
+
"not caching or injecting defaults"
|
| 1105 |
+
)
|
| 1106 |
+
return None
|
| 1107 |
+
with self._scan_lock:
|
| 1108 |
+
self.flash_full_params_cache[video_index] = cached
|
| 1109 |
+
self._logger(
|
| 1110 |
+
f"[FlashCache] video{video_index}: "
|
| 1111 |
+
f"device={cached['device_type']}, "
|
| 1112 |
+
f"state={cached['flash_state']}, "
|
| 1113 |
+
f"source={cached['calibration_source']}, "
|
| 1114 |
+
f"fx={cached['fx_params'][0]:.4f}, "
|
| 1115 |
+
f"fy={cached['fy_params'][0]:.4f}, "
|
| 1116 |
+
f"fz={cached['fz_params'][0]:.4f}"
|
| 1117 |
+
)
|
| 1118 |
+
return cached
|
| 1119 |
+
except Exception as exc:
|
| 1120 |
+
self._logger(
|
| 1121 |
+
f"[FlashCache] video{video_index} read failed: {exc}")
|
| 1122 |
+
return None
|
| 1123 |
+
|
| 1124 |
+
_read_and_cache_flash_params = read_and_cache_flash_params
|
| 1125 |
+
|
| 1126 |
+
def _cache_windows_flash_params(
|
| 1127 |
+
self, xc, library, device_pointer, video_index: int,
|
| 1128 |
+
):
|
| 1129 |
+
try:
|
| 1130 |
+
cached = self._read_full_params(
|
| 1131 |
+
xc,
|
| 1132 |
+
lambda address, buf: library.SonixCam_ReadFromSF(
|
| 1133 |
+
device_pointer, address, buf, 8),
|
| 1134 |
+
)
|
| 1135 |
+
self.flash_full_params_cache[int(video_index)] = cached
|
| 1136 |
+
return cached
|
| 1137 |
+
except Exception as exc:
|
| 1138 |
+
self._logger(
|
| 1139 |
+
f"[FlashCache] video{video_index} read failed: {exc}")
|
| 1140 |
+
return None
|
| 1141 |
+
|
| 1142 |
+
def scan_camera_by_flash_param(
|
| 1143 |
+
self,
|
| 1144 |
+
target: str,
|
| 1145 |
+
param_index: int = 1,
|
| 1146 |
+
max_devices: int = 10,
|
| 1147 |
+
*,
|
| 1148 |
+
candidate_indices=None,
|
| 1149 |
+
):
|
| 1150 |
+
"""按 Flash ``device_params[param_index]`` 匹配 left/right。"""
|
| 1151 |
+
normalized = str(target).lower().strip()
|
| 1152 |
+
with self._scan_lock:
|
| 1153 |
+
if normalized in self.flash_param_cache:
|
| 1154 |
+
cached = self.flash_param_cache[normalized]
|
| 1155 |
+
self._logger(
|
| 1156 |
+
f"[Cameras] '{normalized}' → idx={cached} (cached)")
|
| 1157 |
+
return cached
|
| 1158 |
+
|
| 1159 |
+
try:
|
| 1160 |
+
xc = self._load_xu_module()
|
| 1161 |
+
except Exception as exc:
|
| 1162 |
+
self._logger(f"[Cameras] Flash scan unavailable: {exc}")
|
| 1163 |
+
return None
|
| 1164 |
+
|
| 1165 |
+
result = None
|
| 1166 |
+
flash_address = (
|
| 1167 |
+
xc.DEVICE_BASE_ADDR + int(param_index) * 8)
|
| 1168 |
+
if self.platform.startswith("win"):
|
| 1169 |
+
if xc._sonix_lib is None:
|
| 1170 |
+
xc.init_sonix_sdk()
|
| 1171 |
+
library = xc._sonix_lib
|
| 1172 |
+
if library is None:
|
| 1173 |
+
return None
|
| 1174 |
+
devices = (xc.ScDevice * int(max_devices))()
|
| 1175 |
+
count = ctypes.c_uint(0)
|
| 1176 |
+
if not library.SonixCam_EnumCameras(
|
| 1177 |
+
ctypes.byref(count), devices, int(max_devices)
|
| 1178 |
+
):
|
| 1179 |
+
return None
|
| 1180 |
+
for sonix_index in range(
|
| 1181 |
+
min(count.value, int(max_devices))
|
| 1182 |
+
):
|
| 1183 |
+
pointer = ctypes.pointer(devices[sonix_index])
|
| 1184 |
+
buf = (ctypes.c_ubyte * 8)()
|
| 1185 |
+
if not library.SonixCam_ReadFromSF(
|
| 1186 |
+
pointer, flash_address, buf, 8
|
| 1187 |
+
):
|
| 1188 |
+
continue
|
| 1189 |
+
self._build_sonix_dshow_map()
|
| 1190 |
+
video_index = next((
|
| 1191 |
+
dshow_index
|
| 1192 |
+
for dshow_index, mapped in
|
| 1193 |
+
self._sonix_dshow_map.items()
|
| 1194 |
+
if mapped == sonix_index
|
| 1195 |
+
), sonix_index)
|
| 1196 |
+
if video_index not in self.flash_full_params_cache:
|
| 1197 |
+
self._cache_windows_flash_params(
|
| 1198 |
+
xc, library, pointer, video_index)
|
| 1199 |
+
value = self._decode_ascii(bytes(buf)).lower().strip()
|
| 1200 |
+
if value == normalized:
|
| 1201 |
+
result = video_index
|
| 1202 |
+
break
|
| 1203 |
+
else:
|
| 1204 |
+
try:
|
| 1205 |
+
if candidate_indices is None:
|
| 1206 |
+
linux_indices = self._linux_sonix_indices(
|
| 1207 |
+
max_devices, report_rejections=True,
|
| 1208 |
+
)
|
| 1209 |
+
else:
|
| 1210 |
+
linux_indices = []
|
| 1211 |
+
for value in candidate_indices:
|
| 1212 |
+
try:
|
| 1213 |
+
video_index = int(value)
|
| 1214 |
+
except (TypeError, ValueError):
|
| 1215 |
+
continue
|
| 1216 |
+
reason = self.probe_guard_reason(video_index)
|
| 1217 |
+
if reason is not None:
|
| 1218 |
+
self._log_probe_rejection(
|
| 1219 |
+
video_index, reason)
|
| 1220 |
+
continue
|
| 1221 |
+
if video_index not in linux_indices:
|
| 1222 |
+
linux_indices.append(video_index)
|
| 1223 |
+
linux_indices = tuple(
|
| 1224 |
+
linux_indices[:max(0, int(max_devices))]
|
| 1225 |
+
)
|
| 1226 |
+
for video_index in linux_indices:
|
| 1227 |
+
if getattr(xc, "_sdk_initialized", False):
|
| 1228 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 1229 |
+
if library is not None:
|
| 1230 |
+
try:
|
| 1231 |
+
library.SonixCam_UnInit()
|
| 1232 |
+
except Exception:
|
| 1233 |
+
pass
|
| 1234 |
+
xc._sdk_initialized = False
|
| 1235 |
+
if not xc.init_sonix_sdk(
|
| 1236 |
+
video_index=video_index
|
| 1237 |
+
):
|
| 1238 |
+
continue
|
| 1239 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 1240 |
+
if library is None:
|
| 1241 |
+
continue
|
| 1242 |
+
self.read_and_cache_flash_params(
|
| 1243 |
+
xc, video_index)
|
| 1244 |
+
buf = (ctypes.c_ubyte * 8)()
|
| 1245 |
+
if library.SonixCam_SerialFlashRead(
|
| 1246 |
+
flash_address, buf, 8
|
| 1247 |
+
):
|
| 1248 |
+
value = self._decode_ascii(
|
| 1249 |
+
bytes(buf)).lower().strip()
|
| 1250 |
+
if value == normalized:
|
| 1251 |
+
result = video_index
|
| 1252 |
+
break
|
| 1253 |
+
finally:
|
| 1254 |
+
library = getattr(xc, "_sonix_lib", None)
|
| 1255 |
+
if (
|
| 1256 |
+
getattr(xc, "_sdk_initialized", False)
|
| 1257 |
+
and library is not None
|
| 1258 |
+
):
|
| 1259 |
+
try:
|
| 1260 |
+
library.SonixCam_UnInit()
|
| 1261 |
+
except Exception:
|
| 1262 |
+
pass
|
| 1263 |
+
xc._sdk_initialized = False
|
| 1264 |
+
|
| 1265 |
+
if result is not None:
|
| 1266 |
+
self.flash_param_cache[normalized] = int(result)
|
| 1267 |
+
self._logger(
|
| 1268 |
+
f"[Cameras] Flash scan: '{normalized}' → "
|
| 1269 |
+
f"video index {result}")
|
| 1270 |
+
else:
|
| 1271 |
+
self._logger(
|
| 1272 |
+
f"[Cameras] Flash scan: '{normalized}' "
|
| 1273 |
+
"not found in any camera")
|
| 1274 |
+
return result
|
| 1275 |
+
|
| 1276 |
+
_scan_camera_by_flash_param = scan_camera_by_flash_param
|
| 1277 |
+
|
| 1278 |
+
def prepare_topology_camera(
|
| 1279 |
+
self, side: str, video_index: int,
|
| 1280 |
+
):
|
| 1281 |
+
"""Cache one Hub-pinned camera before any member of its set opens."""
|
| 1282 |
+
normalized = str(side).strip().lower()
|
| 1283 |
+
if normalized not in {"left", "right"}:
|
| 1284 |
+
raise ValueError(f"unknown tactile side: {side!r}")
|
| 1285 |
+
index = int(video_index)
|
| 1286 |
+
with self._scan_lock:
|
| 1287 |
+
self.flash_param_cache.pop(normalized, None)
|
| 1288 |
+
self.scan_camera_by_flash_param(
|
| 1289 |
+
normalized,
|
| 1290 |
+
max_devices=1,
|
| 1291 |
+
candidate_indices=(index,),
|
| 1292 |
+
)
|
| 1293 |
+
cached = self.apply_old_sensor_template(
|
| 1294 |
+
normalized,
|
| 1295 |
+
index,
|
| 1296 |
+
topology_verified=True,
|
| 1297 |
+
)
|
| 1298 |
+
if cached is None:
|
| 1299 |
+
with self._scan_lock:
|
| 1300 |
+
cached = self.flash_full_params_cache.get(index)
|
| 1301 |
+
if cached is None:
|
| 1302 |
+
raise RuntimeError(
|
| 1303 |
+
"Sightac calibration unavailable for topology-pinned "
|
| 1304 |
+
f"{normalized}=video{index}"
|
| 1305 |
+
)
|
| 1306 |
+
with self._scan_lock:
|
| 1307 |
+
self.flash_param_cache[normalized] = index
|
| 1308 |
+
return cached
|
| 1309 |
+
|
| 1310 |
+
def resolve_camera_id(self, value):
|
| 1311 |
+
"""解析 left/right、数字、名称和 ``名称#N`` 回退格式。"""
|
| 1312 |
+
text = str(value).strip()
|
| 1313 |
+
if not text:
|
| 1314 |
+
return None
|
| 1315 |
+
lowered = text.lower()
|
| 1316 |
+
if lowered in {"left", "right"}:
|
| 1317 |
+
result = self.scan_camera_by_flash_param(lowered)
|
| 1318 |
+
if result is not None:
|
| 1319 |
+
self._logger(
|
| 1320 |
+
f"[Cameras] '{text}' → USB index {result} "
|
| 1321 |
+
"(matched by Flash param[1])")
|
| 1322 |
+
return result
|
| 1323 |
+
self._logger(
|
| 1324 |
+
f"[Cameras] '{text}' not found via Flash scan")
|
| 1325 |
+
return None
|
| 1326 |
+
if text.isdigit():
|
| 1327 |
+
index = int(text)
|
| 1328 |
+
if (
|
| 1329 |
+
self.platform.startswith("linux")
|
| 1330 |
+
and not self.is_safe_sonix_probe(index)
|
| 1331 |
+
):
|
| 1332 |
+
self._log_probe_rejection(
|
| 1333 |
+
index, self.probe_guard_reason(index))
|
| 1334 |
+
return None
|
| 1335 |
+
return index
|
| 1336 |
+
occurrence = 0
|
| 1337 |
+
keyword = text
|
| 1338 |
+
if "#" in text:
|
| 1339 |
+
keyword, suffix = text.rsplit("#", 1)
|
| 1340 |
+
try:
|
| 1341 |
+
occurrence = int(suffix)
|
| 1342 |
+
except ValueError:
|
| 1343 |
+
occurrence = 0
|
| 1344 |
+
index = self.find_camera_by_name(keyword, occurrence)
|
| 1345 |
+
if index >= 0:
|
| 1346 |
+
if (
|
| 1347 |
+
self.platform.startswith("linux")
|
| 1348 |
+
and not self.is_safe_sonix_probe(index)
|
| 1349 |
+
):
|
| 1350 |
+
self._log_probe_rejection(
|
| 1351 |
+
index, self.probe_guard_reason(index))
|
| 1352 |
+
return None
|
| 1353 |
+
return index
|
| 1354 |
+
try:
|
| 1355 |
+
index = int(text)
|
| 1356 |
+
except ValueError:
|
| 1357 |
+
return None
|
| 1358 |
+
return (
|
| 1359 |
+
index
|
| 1360 |
+
if not self.platform.startswith("linux")
|
| 1361 |
+
or self.is_safe_sonix_probe(index)
|
| 1362 |
+
else None
|
| 1363 |
+
)
|
| 1364 |
+
|
| 1365 |
+
_resolve_camera_id = resolve_camera_id
|
| 1366 |
+
|
| 1367 |
+
def _build_sonix_dshow_map(self) -> None:
|
| 1368 |
+
if self._sonix_dshow_map is not None:
|
| 1369 |
+
return
|
| 1370 |
+
self._sonix_dshow_map = {}
|
| 1371 |
+
if not self.platform.startswith("win"):
|
| 1372 |
+
return
|
| 1373 |
+
sonix_paths = {}
|
| 1374 |
+
try:
|
| 1375 |
+
xc = self._load_xu_module()
|
| 1376 |
+
if xc._sonix_lib is None:
|
| 1377 |
+
xc.init_sonix_sdk()
|
| 1378 |
+
library = xc._sonix_lib
|
| 1379 |
+
if library is None:
|
| 1380 |
+
return
|
| 1381 |
+
devices = (xc.ScDevice * 10)()
|
| 1382 |
+
count = ctypes.c_uint(0)
|
| 1383 |
+
if library.SonixCam_EnumCameras(
|
| 1384 |
+
ctypes.byref(count), devices, 10
|
| 1385 |
+
):
|
| 1386 |
+
for index in range(count.value):
|
| 1387 |
+
sonix_paths[index] = str(devices[index].devPath)
|
| 1388 |
+
except Exception as exc:
|
| 1389 |
+
self._logger(
|
| 1390 |
+
f"[Cameras] Sonix enumeration failed: {exc}")
|
| 1391 |
+
return
|
| 1392 |
+
|
| 1393 |
+
dshow_paths = {}
|
| 1394 |
+
try:
|
| 1395 |
+
from comtypes.client import CreateObject
|
| 1396 |
+
from comtypes.persist import IPropertyBag
|
| 1397 |
+
from pygrabber.dshow_core import ICreateDevEnum
|
| 1398 |
+
from pygrabber.dshow_graph import GUID
|
| 1399 |
+
from pygrabber.dshow_ids import DeviceCategories, clsids
|
| 1400 |
+
|
| 1401 |
+
enumerator = CreateObject(
|
| 1402 |
+
clsids.CLSID_SystemDeviceEnum,
|
| 1403 |
+
interface=ICreateDevEnum,
|
| 1404 |
+
)
|
| 1405 |
+
collection = enumerator.CreateClassEnumerator(
|
| 1406 |
+
GUID(DeviceCategories.VideoInputDevice), dwFlags=0)
|
| 1407 |
+
moniker, count = collection.Next(1)
|
| 1408 |
+
index = 0
|
| 1409 |
+
while count > 0:
|
| 1410 |
+
try:
|
| 1411 |
+
bag = moniker.BindToStorage(
|
| 1412 |
+
0, 0, IPropertyBag._iid_,
|
| 1413 |
+
).QueryInterface(IPropertyBag)
|
| 1414 |
+
dshow_paths[index] = str(
|
| 1415 |
+
bag.Read("DevicePath", pErrorLog=None) or "")
|
| 1416 |
+
except Exception:
|
| 1417 |
+
pass
|
| 1418 |
+
moniker, count = collection.Next(1)
|
| 1419 |
+
index += 1
|
| 1420 |
+
except Exception as exc:
|
| 1421 |
+
self._logger(
|
| 1422 |
+
f"[Cameras] DirectShow enumeration failed: {exc}")
|
| 1423 |
+
return
|
| 1424 |
+
for dshow_index, dshow_path in dshow_paths.items():
|
| 1425 |
+
for sonix_index, sonix_path in sonix_paths.items():
|
| 1426 |
+
if dshow_path == sonix_path:
|
| 1427 |
+
self._sonix_dshow_map[dshow_index] = sonix_index
|
| 1428 |
+
break
|
| 1429 |
+
self._logger(
|
| 1430 |
+
f"[Cameras] DShow->Sonix map: {self._sonix_dshow_map}")
|
| 1431 |
+
|
| 1432 |
+
@staticmethod
|
| 1433 |
+
def _decode_fourcc(value) -> str:
|
| 1434 |
+
try:
|
| 1435 |
+
encoded = int(value)
|
| 1436 |
+
except (TypeError, ValueError, OverflowError):
|
| 1437 |
+
return ""
|
| 1438 |
+
return "".join(
|
| 1439 |
+
chr((encoded >> shift) & 0xFF)
|
| 1440 |
+
for shift in range(0, 32, 8)
|
| 1441 |
+
).rstrip("\x00 ")
|
| 1442 |
+
|
| 1443 |
+
def _require_capture_mjpg(
|
| 1444 |
+
self, cap, video_index: int,
|
| 1445 |
+
) -> str:
|
| 1446 |
+
"""Request and independently verify MJPG before the first read."""
|
| 1447 |
+
try:
|
| 1448 |
+
requested = self._cv2.VideoWriter_fourcc(*"MJPG")
|
| 1449 |
+
cap.set(self._cv2.CAP_PROP_FOURCC, requested)
|
| 1450 |
+
actual_value = cap.get(self._cv2.CAP_PROP_FOURCC)
|
| 1451 |
+
actual_code = self._decode_fourcc(actual_value)
|
| 1452 |
+
except Exception as exc:
|
| 1453 |
+
try:
|
| 1454 |
+
if cap is not None and hasattr(cap, "release"):
|
| 1455 |
+
cap.release()
|
| 1456 |
+
except Exception:
|
| 1457 |
+
pass
|
| 1458 |
+
raise RuntimeError(
|
| 1459 |
+
f"Sightac video{video_index} requires MJPG before "
|
| 1460 |
+
f"the first frame read; FOURCC verification failed: {exc}"
|
| 1461 |
+
) from exc
|
| 1462 |
+
if actual_code != "MJPG":
|
| 1463 |
+
try:
|
| 1464 |
+
if cap is not None and hasattr(cap, "release"):
|
| 1465 |
+
cap.release()
|
| 1466 |
+
except Exception:
|
| 1467 |
+
pass
|
| 1468 |
+
raise RuntimeError(
|
| 1469 |
+
f"Sightac video{video_index} requires MJPG before "
|
| 1470 |
+
"the first frame read; actual FOURCC is "
|
| 1471 |
+
f"{actual_code or '<unknown>'}"
|
| 1472 |
+
)
|
| 1473 |
+
return actual_code
|
| 1474 |
+
|
| 1475 |
+
def _set_sensor_mjpeg(self, sensor, video_index: int) -> str:
|
| 1476 |
+
return self._require_capture_mjpg(
|
| 1477 |
+
sensor.cap, video_index)
|
| 1478 |
+
|
| 1479 |
+
@staticmethod
|
| 1480 |
+
def _release_sensor(sensor) -> None:
|
| 1481 |
+
if sensor is None:
|
| 1482 |
+
return
|
| 1483 |
+
try:
|
| 1484 |
+
cap = getattr(sensor, "cap", None)
|
| 1485 |
+
if cap is not None and hasattr(cap, "release"):
|
| 1486 |
+
cap.release()
|
| 1487 |
+
except Exception:
|
| 1488 |
+
pass
|
| 1489 |
+
|
| 1490 |
+
def init_sensor(self, video_index: int, reserved_camera=None):
|
| 1491 |
+
"""构造 TouchSensor;Linux 命中缓存时完全绕过 Sonix SDK。"""
|
| 1492 |
+
video_index = int(video_index)
|
| 1493 |
+
reserved_is_ipc = (
|
| 1494 |
+
getattr(reserved_camera, "transport", None) == "libuvc-ipc"
|
| 1495 |
+
)
|
| 1496 |
+
if (
|
| 1497 |
+
self.platform.startswith("linux")
|
| 1498 |
+
and not reserved_is_ipc
|
| 1499 |
+
and not self.is_safe_sonix_probe(video_index)
|
| 1500 |
+
):
|
| 1501 |
+
reason = self.probe_guard_reason(video_index)
|
| 1502 |
+
self._log_probe_rejection(video_index, reason)
|
| 1503 |
+
raise RuntimeError(
|
| 1504 |
+
f"unsafe Sightac video node {video_index}: {reason}")
|
| 1505 |
+
|
| 1506 |
+
with self._scan_lock:
|
| 1507 |
+
cached = self.flash_full_params_cache.get(video_index)
|
| 1508 |
+
if reserved_is_ipc and cached is None:
|
| 1509 |
+
external = getattr(reserved_camera, "flash_params", None)
|
| 1510 |
+
if external is not None:
|
| 1511 |
+
cached = self.cache_external_flash_params(
|
| 1512 |
+
video_index, external)
|
| 1513 |
+
if self.platform.startswith("linux") and cached is None:
|
| 1514 |
+
# Linux 上不能在 VideoCapture 已开流后再让 Sonix SDK 绑定同一
|
| 1515 |
+
# 节点。扫描阶段已做两次完整 Flash 读取;仍无有效缓存说明设备
|
| 1516 |
+
# 类型/标定不可用。此时必须在开流前停止,既不能注入伪造系数,
|
| 1517 |
+
# 也不能回退到会造成 select timeout 的 SDK + V4L2 双重打开。
|
| 1518 |
+
raise RuntimeError(
|
| 1519 |
+
"Sightac full Flash calibration is unavailable for "
|
| 1520 |
+
f"video{video_index}; refusing to open an uncalibrated "
|
| 1521 |
+
"or SDK-conflicting stream"
|
| 1522 |
+
)
|
| 1523 |
+
|
| 1524 |
+
BaseTouchSensor = self._load_touch_sensor_class()
|
| 1525 |
+
if reserved_is_ipc:
|
| 1526 |
+
xu_index = video_index
|
| 1527 |
+
else:
|
| 1528 |
+
self._build_sonix_dshow_map()
|
| 1529 |
+
xu_index = self._sonix_dshow_map.get(
|
| 1530 |
+
video_index, video_index)
|
| 1531 |
+
cv = self._cv2
|
| 1532 |
+
platform = self.platform
|
| 1533 |
+
logger = self._logger
|
| 1534 |
+
require_mjpg = self._require_capture_mjpg
|
| 1535 |
+
api_package = self.api_package
|
| 1536 |
+
|
| 1537 |
+
if platform.startswith("linux") and cached is not None:
|
| 1538 |
+
class CachedTouchSensor(BaseTouchSensor):
|
| 1539 |
+
"""从预读缓存恢复算法参数,不再次调用 XU SDK。"""
|
| 1540 |
+
|
| 1541 |
+
def __init__(self, cached_params, camera_index):
|
| 1542 |
+
self._cached_params = cached_params
|
| 1543 |
+
try:
|
| 1544 |
+
super().__init__(
|
| 1545 |
+
usb_id=camera_index,
|
| 1546 |
+
defer_baseline=True,
|
| 1547 |
+
)
|
| 1548 |
+
except Exception:
|
| 1549 |
+
cap = getattr(self, "cap", None)
|
| 1550 |
+
if cap is not None:
|
| 1551 |
+
try:
|
| 1552 |
+
cap.release()
|
| 1553 |
+
except Exception:
|
| 1554 |
+
pass
|
| 1555 |
+
raise
|
| 1556 |
+
|
| 1557 |
+
def camera_init(self, camera_index, fix_config=False):
|
| 1558 |
+
self.platform = "linux"
|
| 1559 |
+
self.xu_camera = None
|
| 1560 |
+
if reserved_camera is None:
|
| 1561 |
+
self.cap = cv.VideoCapture(
|
| 1562 |
+
camera_index, cv.CAP_V4L2)
|
| 1563 |
+
prefetched_frame = None
|
| 1564 |
+
actual_format = None
|
| 1565 |
+
else:
|
| 1566 |
+
(
|
| 1567 |
+
self.cap,
|
| 1568 |
+
prefetched_frame,
|
| 1569 |
+
actual_format,
|
| 1570 |
+
) = reserved_camera.claim()
|
| 1571 |
+
if not self.cap or not self.cap.isOpened():
|
| 1572 |
+
raise RuntimeError(
|
| 1573 |
+
"[CachedTouchSensor] cannot open "
|
| 1574 |
+
f"video_id={camera_index}")
|
| 1575 |
+
if actual_format is None:
|
| 1576 |
+
self._ksq_fourcc_actual = require_mjpg(
|
| 1577 |
+
self.cap, camera_index
|
| 1578 |
+
)
|
| 1579 |
+
else:
|
| 1580 |
+
width, height, fps, actual_fourcc = actual_format
|
| 1581 |
+
if (
|
| 1582 |
+
int(width) != 640
|
| 1583 |
+
or int(height) != 480
|
| 1584 |
+
or abs(float(fps) - 30.0) > 0.5
|
| 1585 |
+
or actual_fourcc != "MJPG"
|
| 1586 |
+
):
|
| 1587 |
+
self.cap.release()
|
| 1588 |
+
raise RuntimeError(
|
| 1589 |
+
"[CachedTouchSensor] reserved format "
|
| 1590 |
+
f"invalid for video{camera_index}: "
|
| 1591 |
+
f"{width}x{height} "
|
| 1592 |
+
f"{actual_fourcc or '<unknown>'}@{fps:g}"
|
| 1593 |
+
)
|
| 1594 |
+
if (
|
| 1595 |
+
prefetched_frame is None
|
| 1596 |
+
or getattr(prefetched_frame, "size", 0) <= 0
|
| 1597 |
+
):
|
| 1598 |
+
self.cap.release()
|
| 1599 |
+
raise RuntimeError(
|
| 1600 |
+
"[CachedTouchSensor] reserved first frame "
|
| 1601 |
+
f"is unavailable for video{camera_index}"
|
| 1602 |
+
)
|
| 1603 |
+
self._ksq_fourcc_actual = actual_fourcc
|
| 1604 |
+
self._ksq_prefetched_frame = prefetched_frame
|
| 1605 |
+
self._apply_cached_params()
|
| 1606 |
+
check_config = getattr(self, "check_config", None)
|
| 1607 |
+
if callable(check_config) and not check_config():
|
| 1608 |
+
logger("WARNING:")
|
| 1609 |
+
logger(
|
| 1610 |
+
f"[CachedTouchSensor] video={camera_index} "
|
| 1611 |
+
"init OK (cached, zero SDK calls)")
|
| 1612 |
+
|
| 1613 |
+
def read_flash(self):
|
| 1614 |
+
return None
|
| 1615 |
+
|
| 1616 |
+
def _apply_cached_params(self):
|
| 1617 |
+
params = self._cached_params
|
| 1618 |
+
self.device_type = params["device_type"]
|
| 1619 |
+
self.apply_flash_params(params)
|
| 1620 |
+
logger(
|
| 1621 |
+
f"[CachedTouchSensor] params injected: device={self.device_type} "
|
| 1622 |
+
f"model={self.bevel_calibrate_model} source={params.get('calibration_source')} "
|
| 1623 |
+
f"ROI=({self.roi_x1},{self.roi_y1})-({self.roi_x2},{self.roi_y2}) "
|
| 1624 |
+
f"RB=({self.mask_r_threshold},{self.mask_b_threshold}) "
|
| 1625 |
+
f"regions={self.rb_filter_regions} hue={self.hue_threshold}"
|
| 1626 |
+
)
|
| 1627 |
+
|
| 1628 |
+
sensor = CachedTouchSensor(cached, video_index)
|
| 1629 |
+
sensor._ksq_api_package = api_package
|
| 1630 |
+
sensor._ksq_deferred_baseline = True
|
| 1631 |
+
sensor._ksq_calibration_source = cached.get(
|
| 1632 |
+
"calibration_source", "hardware")
|
| 1633 |
+
self._logger(
|
| 1634 |
+
f"[Heatmap] CachedTouchSensor video={video_index} "
|
| 1635 |
+
f"FOURCC: {sensor._ksq_fourcc_actual} OK "
|
| 1636 |
+
"(cached, zero SDK calls, source="
|
| 1637 |
+
f"{sensor._ksq_calibration_source})")
|
| 1638 |
+
return sensor
|
| 1639 |
+
|
| 1640 |
+
if xu_index == video_index:
|
| 1641 |
+
sensor = BaseTouchSensor(
|
| 1642 |
+
usb_id=video_index, defer_baseline=True)
|
| 1643 |
+
sensor._ksq_api_package = api_package
|
| 1644 |
+
sensor._ksq_deferred_baseline = True
|
| 1645 |
+
sensor._ksq_fourcc_actual = self._set_sensor_mjpeg(
|
| 1646 |
+
sensor, video_index)
|
| 1647 |
+
self._logger(
|
| 1648 |
+
f"[Heatmap] TouchSensor usb_id={video_index} "
|
| 1649 |
+
f"FOURCC: {sensor._ksq_fourcc_actual} OK")
|
| 1650 |
+
return sensor
|
| 1651 |
+
|
| 1652 |
+
class RoutedTouchSensor(BaseTouchSensor):
|
| 1653 |
+
def __init__(self, routed_xu_index, routed_video_index):
|
| 1654 |
+
self._routed_video_index = routed_video_index
|
| 1655 |
+
super().__init__(
|
| 1656 |
+
usb_id=routed_xu_index, defer_baseline=True)
|
| 1657 |
+
|
| 1658 |
+
def camera_init(self, camera_index, fix_config=False):
|
| 1659 |
+
touch_sensor_module = importlib.import_module(
|
| 1660 |
+
f"{api_package}.touch_sensor")
|
| 1661 |
+
if platform.startswith("win"):
|
| 1662 |
+
self.platform = "win"
|
| 1663 |
+
camera = touch_sensor_module.xu_camera.XuCamera(
|
| 1664 |
+
camera_index)
|
| 1665 |
+
if camera.is_available():
|
| 1666 |
+
self.xu_camera = camera
|
| 1667 |
+
self.cap = cv.VideoCapture(
|
| 1668 |
+
self._routed_video_index, cv.CAP_DSHOW)
|
| 1669 |
+
elif platform.startswith("darwin"):
|
| 1670 |
+
self.platform = "apple"
|
| 1671 |
+
self.cap = cv.VideoCapture(
|
| 1672 |
+
self._routed_video_index,
|
| 1673 |
+
cv.CAP_AVFOUNDATION,
|
| 1674 |
+
)
|
| 1675 |
+
else:
|
| 1676 |
+
self.platform = "unknown"
|
| 1677 |
+
self.cap = cv.VideoCapture(
|
| 1678 |
+
self._routed_video_index)
|
| 1679 |
+
if not self.cap or not self.cap.isOpened():
|
| 1680 |
+
raise RuntimeError(
|
| 1681 |
+
"cannot open video_id="
|
| 1682 |
+
f"{self._routed_video_index}")
|
| 1683 |
+
self._ksq_fourcc_actual = require_mjpg(
|
| 1684 |
+
self.cap, self._routed_video_index
|
| 1685 |
+
)
|
| 1686 |
+
if getattr(self, "xu_camera", None):
|
| 1687 |
+
self.device_type = (
|
| 1688 |
+
self.xu_camera.read_device_params()[0])
|
| 1689 |
+
self.read_flash()
|
| 1690 |
+
check_config = getattr(self, "check_config", None)
|
| 1691 |
+
if callable(check_config) and not check_config():
|
| 1692 |
+
logger("WARNING:")
|
| 1693 |
+
logger(
|
| 1694 |
+
f"[Heatmap] routed: "
|
| 1695 |
+
f"video={self._routed_video_index} "
|
| 1696 |
+
f"xu={camera_index}")
|
| 1697 |
+
|
| 1698 |
+
sensor = RoutedTouchSensor(xu_index, video_index)
|
| 1699 |
+
sensor._ksq_api_package = api_package
|
| 1700 |
+
sensor._ksq_deferred_baseline = True
|
| 1701 |
+
self._logger(
|
| 1702 |
+
f"[Heatmap] TouchSensor video={video_index} "
|
| 1703 |
+
f"xu={xu_index} FOURCC: "
|
| 1704 |
+
f"{sensor._ksq_fourcc_actual} OK")
|
| 1705 |
+
return sensor
|
| 1706 |
+
|
| 1707 |
+
_init_tactile_sensor = init_sensor
|
collector/core/gripper/devices/tactile_process_manager.py
ADDED
|
The diff for this file is too large to render.
See raw diff
|
|
|
collector/core/gripper/devices/usb_camera_ipc.py
ADDED
|
@@ -0,0 +1,255 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Minimal Python consumer for the camera-service Unix-socket prototype.
|
| 2 |
+
|
| 3 |
+
The public shape intentionally follows ``cv2.VideoCapture``:
|
| 4 |
+
|
| 5 |
+
cam = Camera("sightac_left")
|
| 6 |
+
cam.open()
|
| 7 |
+
ok, frame = cam.read()
|
| 8 |
+
|
| 9 |
+
The service sends a fixed-size header followed by one MJPEG payload over a
|
| 10 |
+
Unix byte stream. This module reconstructs one complete frame and does not
|
| 11 |
+
open a V4L2 node.
|
| 12 |
+
"""
|
| 13 |
+
|
| 14 |
+
from __future__ import annotations
|
| 15 |
+
|
| 16 |
+
import socket
|
| 17 |
+
import struct
|
| 18 |
+
import time
|
| 19 |
+
from pathlib import Path
|
| 20 |
+
from typing import Optional
|
| 21 |
+
|
| 22 |
+
import cv2
|
| 23 |
+
import numpy as np
|
| 24 |
+
|
| 25 |
+
|
| 26 |
+
IPC_MAGIC = 0x4B535146 # "KSQF"
|
| 27 |
+
IPC_VERSION = 1
|
| 28 |
+
# The C producer uses the native-aligned struct: uint64_t is aligned to
|
| 29 |
+
# 8 bytes, so there are four padding bytes before timestamp_ns.
|
| 30 |
+
HEADER = struct.Struct("<IHHIIIII4xQ")
|
| 31 |
+
MAX_PAYLOAD = 16 * 1024 * 1024
|
| 32 |
+
MAX_DECODE_ATTEMPTS = 5
|
| 33 |
+
IPC_RECONNECT_ATTEMPTS = 3
|
| 34 |
+
|
| 35 |
+
DEFAULT_SOCKETS = {
|
| 36 |
+
"sightac_left": "/tmp/ksq-camera-service-sightac-left.sock",
|
| 37 |
+
"sightac_right": "/tmp/ksq-camera-service-sightac-right.sock",
|
| 38 |
+
"decxin": "/tmp/ksq-camera-service-decxin.sock",
|
| 39 |
+
"unit_b_sightac_left": "/tmp/ksq-camera-service-unit-b-sightac-left.sock",
|
| 40 |
+
"unit_b_sightac_right": "/tmp/ksq-camera-service-unit-b-sightac-right.sock",
|
| 41 |
+
"unit_b_decxin": "/tmp/ksq-camera-service-unit-b-decxin.sock",
|
| 42 |
+
}
|
| 43 |
+
|
| 44 |
+
|
| 45 |
+
class Camera:
|
| 46 |
+
"""Receive MJPEG frames from one camera-service Unix socket."""
|
| 47 |
+
|
| 48 |
+
def __init__(self, name_or_path: str, timeout: Optional[float] = 2.0):
|
| 49 |
+
self.name = name_or_path
|
| 50 |
+
self.socket_path = DEFAULT_SOCKETS.get(name_or_path, name_or_path)
|
| 51 |
+
self.timeout = timeout
|
| 52 |
+
self._socket: Optional[socket.socket] = None
|
| 53 |
+
self.last_metadata = {}
|
| 54 |
+
self.last_decode_failures = 0
|
| 55 |
+
self.last_decode_diagnostics = ()
|
| 56 |
+
self.reconnect_count = 0
|
| 57 |
+
self.last_transport_error = ""
|
| 58 |
+
self._released = False
|
| 59 |
+
|
| 60 |
+
def open(self) -> bool:
|
| 61 |
+
if self._socket is not None:
|
| 62 |
+
return True
|
| 63 |
+
if self._released:
|
| 64 |
+
return False
|
| 65 |
+
if not Path(self.socket_path).exists():
|
| 66 |
+
raise FileNotFoundError(self.socket_path)
|
| 67 |
+
connection = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
| 68 |
+
if self.timeout is not None:
|
| 69 |
+
connection.settimeout(self.timeout)
|
| 70 |
+
try:
|
| 71 |
+
connection.connect(self.socket_path)
|
| 72 |
+
except BaseException:
|
| 73 |
+
connection.close()
|
| 74 |
+
raise
|
| 75 |
+
if self._released:
|
| 76 |
+
connection.close()
|
| 77 |
+
return False
|
| 78 |
+
self._socket = connection
|
| 79 |
+
return True
|
| 80 |
+
|
| 81 |
+
def isOpened(self) -> bool:
|
| 82 |
+
"""Match the small ``cv2.VideoCapture`` status API used by the app."""
|
| 83 |
+
return self._socket is not None
|
| 84 |
+
|
| 85 |
+
def reconnect(self) -> bool:
|
| 86 |
+
"""Close and recreate one client lease after a transport failure."""
|
| 87 |
+
self.close_socket()
|
| 88 |
+
for _attempt in range(IPC_RECONNECT_ATTEMPTS):
|
| 89 |
+
if self._released:
|
| 90 |
+
return False
|
| 91 |
+
try:
|
| 92 |
+
if not self.open() or self._released:
|
| 93 |
+
self.close_socket()
|
| 94 |
+
return False
|
| 95 |
+
self.reconnect_count += 1
|
| 96 |
+
self.last_transport_error = ""
|
| 97 |
+
return True
|
| 98 |
+
except (OSError, FileNotFoundError) as exc:
|
| 99 |
+
self.last_transport_error = str(exc)
|
| 100 |
+
self.close_socket()
|
| 101 |
+
time.sleep(0.05)
|
| 102 |
+
return False
|
| 103 |
+
|
| 104 |
+
def getBackendName(self) -> str:
|
| 105 |
+
return "libuvc-ipc"
|
| 106 |
+
|
| 107 |
+
def set(self, _property, _value) -> bool:
|
| 108 |
+
"""The UVC mode is negotiated by camera-service, not V4L2."""
|
| 109 |
+
return False
|
| 110 |
+
|
| 111 |
+
def get(self, property_id):
|
| 112 |
+
"""Return negotiated metadata where the IPC protocol can provide it."""
|
| 113 |
+
if property_id == cv2.CAP_PROP_FRAME_WIDTH:
|
| 114 |
+
return float(self.last_metadata.get("width", 0))
|
| 115 |
+
if property_id == cv2.CAP_PROP_FRAME_HEIGHT:
|
| 116 |
+
return float(self.last_metadata.get("height", 0))
|
| 117 |
+
if property_id == cv2.CAP_PROP_FPS:
|
| 118 |
+
return 30.0
|
| 119 |
+
if property_id == cv2.CAP_PROP_FOURCC:
|
| 120 |
+
return float(cv2.VideoWriter_fourcc(*"MJPG"))
|
| 121 |
+
return 0.0
|
| 122 |
+
|
| 123 |
+
def _recv_exact(self, size: int):
|
| 124 |
+
# Capture the socket once: release() may clear the owner's reference
|
| 125 |
+
# while a blocking read is in progress. recv_into avoids accumulating
|
| 126 |
+
# and then copying each individual TCP/Unix stream fragment.
|
| 127 |
+
connection = self._socket
|
| 128 |
+
if connection is None:
|
| 129 |
+
raise ConnectionResetError("camera IPC socket is closed")
|
| 130 |
+
buffer = bytearray(size)
|
| 131 |
+
view = memoryview(buffer)
|
| 132 |
+
offset = 0
|
| 133 |
+
while offset < size:
|
| 134 |
+
count = connection.recv_into(view[offset:])
|
| 135 |
+
if not count:
|
| 136 |
+
raise ConnectionResetError("camera-service closed an incomplete packet")
|
| 137 |
+
offset += count
|
| 138 |
+
return buffer
|
| 139 |
+
|
| 140 |
+
def close_socket(self) -> None:
|
| 141 |
+
socket_obj = self._socket
|
| 142 |
+
self._socket = None
|
| 143 |
+
if socket_obj is not None:
|
| 144 |
+
try:
|
| 145 |
+
socket_obj.shutdown(socket.SHUT_RDWR)
|
| 146 |
+
except OSError:
|
| 147 |
+
pass
|
| 148 |
+
try:
|
| 149 |
+
socket_obj.close()
|
| 150 |
+
except OSError:
|
| 151 |
+
pass
|
| 152 |
+
|
| 153 |
+
def read_packet(self):
|
| 154 |
+
if self._socket is None:
|
| 155 |
+
if self._released or not self.reconnect():
|
| 156 |
+
return False, None
|
| 157 |
+
try:
|
| 158 |
+
header = self._recv_exact(HEADER.size)
|
| 159 |
+
values = HEADER.unpack(header)
|
| 160 |
+
(magic, version, header_bytes, sequence, width, height,
|
| 161 |
+
fourcc, size, timestamp_ns) = values
|
| 162 |
+
if (magic != IPC_MAGIC or version != IPC_VERSION
|
| 163 |
+
or header_bytes != HEADER.size):
|
| 164 |
+
raise ValueError("invalid camera-service IPC header")
|
| 165 |
+
if fourcc != int.from_bytes(b"MJPG", "little"):
|
| 166 |
+
raise ValueError("camera-service did not negotiate MJPG")
|
| 167 |
+
if not (0 < size <= MAX_PAYLOAD and 0 < width <= 8192
|
| 168 |
+
and 0 < height <= 8192 and timestamp_ns > 0):
|
| 169 |
+
raise ValueError("invalid camera-service frame dimensions/size/timestamp")
|
| 170 |
+
payload = self._recv_exact(size)
|
| 171 |
+
except (OSError, ValueError, struct.error) as exc:
|
| 172 |
+
# A timeout can occur in the middle of either header or payload.
|
| 173 |
+
# Continuing on that socket would interpret JPEG bytes as a header.
|
| 174 |
+
# Report this failed read; only a later read may create a new lease.
|
| 175 |
+
self.last_transport_error = str(exc)
|
| 176 |
+
self.close_socket()
|
| 177 |
+
self.last_metadata = {}
|
| 178 |
+
return False, None
|
| 179 |
+
if not payload.startswith(b"\xff\xd8") or payload.rfind(b"\xff\xd9") < 2:
|
| 180 |
+
self.last_transport_error = "incomplete MJPEG payload"
|
| 181 |
+
self.last_metadata = {}
|
| 182 |
+
return False, None
|
| 183 |
+
self.last_transport_error = ""
|
| 184 |
+
self.last_metadata = {
|
| 185 |
+
"sequence": sequence,
|
| 186 |
+
"width": width,
|
| 187 |
+
"height": height,
|
| 188 |
+
"fourcc": "MJPG",
|
| 189 |
+
"timestamp_ns": timestamp_ns,
|
| 190 |
+
"payload_bytes": size,
|
| 191 |
+
}
|
| 192 |
+
return True, payload
|
| 193 |
+
|
| 194 |
+
def read(self):
|
| 195 |
+
"""Return ``(ret, BGR frame)`` like ``cv2.VideoCapture.read()``."""
|
| 196 |
+
# USB startup can deliver an occasional incomplete JPEG before the
|
| 197 |
+
# source settles. Match V4L2 semantics: report read failure instead
|
| 198 |
+
# of killing initialization on the first undecodable packet. A hard
|
| 199 |
+
# error remains if several consecutive payloads cannot be decoded.
|
| 200 |
+
failures = []
|
| 201 |
+
for _attempt in range(MAX_DECODE_ATTEMPTS):
|
| 202 |
+
ok, payload = self.read_packet()
|
| 203 |
+
if not ok:
|
| 204 |
+
return False, None
|
| 205 |
+
encoded = np.frombuffer(payload, dtype=np.uint8)
|
| 206 |
+
try:
|
| 207 |
+
frame = cv2.imdecode(encoded, cv2.IMREAD_COLOR)
|
| 208 |
+
except cv2.error:
|
| 209 |
+
frame = None
|
| 210 |
+
if (frame is not None and frame.size > 0
|
| 211 |
+
and frame.shape[:2] == (self.last_metadata["height"],
|
| 212 |
+
self.last_metadata["width"])):
|
| 213 |
+
self.last_decode_failures = len(failures)
|
| 214 |
+
self.last_decode_diagnostics = tuple(failures)
|
| 215 |
+
return True, frame
|
| 216 |
+
metadata = dict(self.last_metadata)
|
| 217 |
+
failures.append({
|
| 218 |
+
"sequence": metadata.get("sequence"),
|
| 219 |
+
"bytes": len(payload),
|
| 220 |
+
"head": bytes(payload[:4]).hex(),
|
| 221 |
+
"tail": bytes(payload[-4:]).hex(),
|
| 222 |
+
})
|
| 223 |
+
print(
|
| 224 |
+
"[IpcCamera] partial/truncated MJPEG payload "
|
| 225 |
+
f"socket={self.socket_path} attempt={_attempt + 1} "
|
| 226 |
+
f"sequence={metadata.get('sequence')} bytes={len(payload)} "
|
| 227 |
+
f"head={failures[-1]['head']} tail={failures[-1]['tail']} "
|
| 228 |
+
f"transport_error={self.last_transport_error!r}",
|
| 229 |
+
flush=True,
|
| 230 |
+
)
|
| 231 |
+
self.last_decode_failures = len(failures)
|
| 232 |
+
self.last_decode_diagnostics = tuple(failures)
|
| 233 |
+
if failures:
|
| 234 |
+
print(
|
| 235 |
+
"[IpcCamera] MJPEG decode failed "
|
| 236 |
+
f"socket={self.socket_path} "
|
| 237 |
+
f"failures={len(failures)} "
|
| 238 |
+
f"transport_error={self.last_transport_error!r} "
|
| 239 |
+
f"details={failures}",
|
| 240 |
+
flush=True,
|
| 241 |
+
)
|
| 242 |
+
return False, None
|
| 243 |
+
|
| 244 |
+
def close(self) -> None:
|
| 245 |
+
self._released = True
|
| 246 |
+
self.close_socket()
|
| 247 |
+
|
| 248 |
+
release = close
|
| 249 |
+
|
| 250 |
+
def __enter__(self):
|
| 251 |
+
self.open()
|
| 252 |
+
return self
|
| 253 |
+
|
| 254 |
+
def __exit__(self, exc_type, exc_value, traceback):
|
| 255 |
+
self.close()
|
collector/core/gripper/devices/usb_camera_sets.py
ADDED
|
@@ -0,0 +1,81 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Shared camera-node types for the dynamic libusb/libuvc transport.
|
| 2 |
+
|
| 3 |
+
The production selector lives in ``uvc_camera_service``. This module keeps
|
| 4 |
+
only the common node/topology data structures, format constants, and the
|
| 5 |
+
diagnostic ESP32 tty root-path helper. It does not scan or open camera nodes.
|
| 6 |
+
"""
|
| 7 |
+
|
| 8 |
+
from __future__ import annotations
|
| 9 |
+
|
| 10 |
+
from dataclasses import dataclass
|
| 11 |
+
import os
|
| 12 |
+
import re
|
| 13 |
+
from typing import Callable, Optional
|
| 14 |
+
|
| 15 |
+
SIGHTAC_WIDTH = 640
|
| 16 |
+
SIGHTAC_HEIGHT = 480
|
| 17 |
+
SIGHTAC_FPS = 30.0
|
| 18 |
+
SIGHTAC_FOURCC = "MJPG"
|
| 19 |
+
|
| 20 |
+
DECXIN_WIDTH = 1280
|
| 21 |
+
DECXIN_HEIGHT = 960
|
| 22 |
+
DECXIN_FPS = 30.0
|
| 23 |
+
DECXIN_FOURCC = "MJPG"
|
| 24 |
+
|
| 25 |
+
class UsbCameraSetError(RuntimeError):
|
| 26 |
+
"""A required camera stream cannot be reserved."""
|
| 27 |
+
|
| 28 |
+
|
| 29 |
+
@dataclass(frozen=True)
|
| 30 |
+
class UsbCameraNode:
|
| 31 |
+
role: str
|
| 32 |
+
device_path: str
|
| 33 |
+
video_index: int
|
| 34 |
+
physical_usb_path: str
|
| 35 |
+
physical_sysfs_path: str
|
| 36 |
+
direct_parent_hub: str
|
| 37 |
+
root_hub: str
|
| 38 |
+
vendor_id: str
|
| 39 |
+
product_id: str
|
| 40 |
+
serial: str
|
| 41 |
+
|
| 42 |
+
|
| 43 |
+
@dataclass(frozen=True)
|
| 44 |
+
class UsbCameraSetTopology:
|
| 45 |
+
"""One manifest-selected DECXIN plus left/right Sightac pair."""
|
| 46 |
+
|
| 47 |
+
root_hub: str
|
| 48 |
+
tactile_hub: str
|
| 49 |
+
decxin: UsbCameraNode
|
| 50 |
+
left: UsbCameraNode
|
| 51 |
+
right: UsbCameraNode
|
| 52 |
+
|
| 53 |
+
def node(self, role: str) -> UsbCameraNode:
|
| 54 |
+
if role not in {"decxin", "left", "right"}:
|
| 55 |
+
raise KeyError(role)
|
| 56 |
+
return getattr(self, role)
|
| 57 |
+
|
| 58 |
+
|
| 59 |
+
def _root_hub_from_physical(physical_usb_path: str) -> Optional[str]:
|
| 60 |
+
match = re.fullmatch(r"(\d+-\d+)(?:\.\d+)*", physical_usb_path)
|
| 61 |
+
return match.group(1) if match is not None else None
|
| 62 |
+
|
| 63 |
+
|
| 64 |
+
def usb_root_hub_for_tty(
|
| 65 |
+
device_path: str,
|
| 66 |
+
*,
|
| 67 |
+
realpath: Callable[[str], str] = os.path.realpath,
|
| 68 |
+
):
|
| 69 |
+
"""Return the diagnostic root hub for a ttyACM path."""
|
| 70 |
+
basename = os.path.basename(str(device_path).strip())
|
| 71 |
+
if not re.fullmatch(r"ttyACM\d+", basename):
|
| 72 |
+
return None
|
| 73 |
+
interface_path = realpath(
|
| 74 |
+
os.path.join("/sys/class/tty", basename, "device")
|
| 75 |
+
)
|
| 76 |
+
for component in reversed(interface_path.split(os.sep)):
|
| 77 |
+
physical = component.split(":", 1)[0]
|
| 78 |
+
root_hub = _root_hub_from_physical(physical)
|
| 79 |
+
if root_hub is not None:
|
| 80 |
+
return root_hub
|
| 81 |
+
return None
|
collector/core/gripper/devices/uvc_camera_service.py
ADDED
|
@@ -0,0 +1,782 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Runtime libusb/libuvc transport for Sightac and DECXIN.
|
| 2 |
+
|
| 3 |
+
Fays is deliberately absent from this module. The Fays instance manager has
|
| 4 |
+
already identified and leased the S80M through the official SDK; this module
|
| 5 |
+
uses that live assignment only to associate the separate USB2 camera set.
|
| 6 |
+
|
| 7 |
+
The discovery executable is run for every selection. Its Flash calibration
|
| 8 |
+
payload is therefore a per-run read from the camera, not a saved fallback.
|
| 9 |
+
The service then exposes the negotiated MJPEG frames through Unix sockets.
|
| 10 |
+
"""
|
| 11 |
+
|
| 12 |
+
from __future__ import annotations
|
| 13 |
+
|
| 14 |
+
import json
|
| 15 |
+
import math
|
| 16 |
+
import os
|
| 17 |
+
from pathlib import Path
|
| 18 |
+
import shutil
|
| 19 |
+
import subprocess
|
| 20 |
+
import tempfile
|
| 21 |
+
import threading
|
| 22 |
+
import time
|
| 23 |
+
import sys
|
| 24 |
+
|
| 25 |
+
import cv2
|
| 26 |
+
|
| 27 |
+
from core.gripper.fays_runtime import PROJECT_ROOT
|
| 28 |
+
from core.gripper.runtime.device_access import device_access_guard
|
| 29 |
+
|
| 30 |
+
# camera_service/python is a checkout-local transport module, not an ambient
|
| 31 |
+
# dependency. Resolve it before importing the IPC client.
|
| 32 |
+
IPC_PYTHON_ROOT = os.path.join(PROJECT_ROOT, "camera_service", "python")
|
| 33 |
+
if IPC_PYTHON_ROOT not in sys.path:
|
| 34 |
+
sys.path.insert(0, IPC_PYTHON_ROOT)
|
| 35 |
+
from core.gripper.devices.usb_camera_ipc import Camera as IpcCamera
|
| 36 |
+
|
| 37 |
+
from .usb_camera_sets import (
|
| 38 |
+
DECXIN_FOURCC,
|
| 39 |
+
DECXIN_FPS,
|
| 40 |
+
DECXIN_HEIGHT,
|
| 41 |
+
DECXIN_WIDTH,
|
| 42 |
+
SIGHTAC_FOURCC,
|
| 43 |
+
SIGHTAC_FPS,
|
| 44 |
+
SIGHTAC_HEIGHT,
|
| 45 |
+
SIGHTAC_WIDTH,
|
| 46 |
+
UsbCameraSetError,
|
| 47 |
+
UsbCameraSetTopology,
|
| 48 |
+
)
|
| 49 |
+
|
| 50 |
+
|
| 51 |
+
DISCOVERY_BINARY = Path(os.environ.get(
|
| 52 |
+
"KSQ_UVC_DISCOVERY_BINARY",
|
| 53 |
+
os.path.join(PROJECT_ROOT, "camera_service", "build", "discover-uvc-config"),
|
| 54 |
+
)).resolve()
|
| 55 |
+
SERVICE_BINARY = Path(os.environ.get(
|
| 56 |
+
"KSQ_UVC_SERVICE_BINARY",
|
| 57 |
+
os.path.join(PROJECT_ROOT, "camera_service", "build", "ksq-camera-service"),
|
| 58 |
+
)).resolve()
|
| 59 |
+
DISCOVERY_TIMEOUT_S = 45.0
|
| 60 |
+
SERVICE_START_TIMEOUT_S = 10.0
|
| 61 |
+
FIRST_FRAME_ATTEMPTS = 20
|
| 62 |
+
FIRST_FRAME_RETRY_S = 0.05
|
| 63 |
+
|
| 64 |
+
# 原生二进制依赖交付目录内的 libuvc(原上位机由 start_upper_computer.sh
|
| 65 |
+
# 全局注入 LD_LIBRARY_PATH;主程序改为仅对这两个子进程注入,避免污染
|
| 66 |
+
# 主进程及 Fays 桥接的自建库路径)。
|
| 67 |
+
_LIBUVC_DIR = Path(
|
| 68 |
+
PROJECT_ROOT, "camera_service", "build", "third_party", "libuvc")
|
| 69 |
+
|
| 70 |
+
|
| 71 |
+
def _native_env():
|
| 72 |
+
env = dict(os.environ)
|
| 73 |
+
existing = env.get("LD_LIBRARY_PATH", "")
|
| 74 |
+
if _LIBUVC_DIR.is_dir() and str(_LIBUVC_DIR) not in existing:
|
| 75 |
+
env["LD_LIBRARY_PATH"] = (
|
| 76 |
+
str(_LIBUVC_DIR) + (f":{existing}" if existing else ""))
|
| 77 |
+
return env
|
| 78 |
+
|
| 79 |
+
|
| 80 |
+
class UvcCameraServiceError(RuntimeError):
|
| 81 |
+
"""The live libusb/libuvc camera set cannot be used."""
|
| 82 |
+
|
| 83 |
+
|
| 84 |
+
def _root_port(physical_path):
|
| 85 |
+
text = str(physical_path or "").strip()
|
| 86 |
+
if "-" not in text:
|
| 87 |
+
return ""
|
| 88 |
+
return text.split("-", 1)[1].split(".", 1)[0]
|
| 89 |
+
|
| 90 |
+
|
| 91 |
+
def _path_contains(bus, outer_path, child_path):
|
| 92 |
+
outer = str(outer_path or "").strip()
|
| 93 |
+
child = str(child_path or "").strip()
|
| 94 |
+
if not outer or not child:
|
| 95 |
+
return False
|
| 96 |
+
if str(bus) != child.split("-", 1)[0]:
|
| 97 |
+
return False
|
| 98 |
+
return child == f"{bus}-{outer}" or child.startswith(
|
| 99 |
+
f"{bus}-{outer}."
|
| 100 |
+
)
|
| 101 |
+
|
| 102 |
+
|
| 103 |
+
def _require_finite_vector(params, key, length):
|
| 104 |
+
values = params.get(key)
|
| 105 |
+
if not isinstance(values, list) or len(values) != length:
|
| 106 |
+
raise UvcCameraServiceError(
|
| 107 |
+
f"当前 Flash 参数 {key} 长度无效: expected={length}"
|
| 108 |
+
)
|
| 109 |
+
result = []
|
| 110 |
+
for index, value in enumerate(values):
|
| 111 |
+
if key == "fw_params" and params.get("device_type") == "bevel" and index >= 5 and (
|
| 112 |
+
value is None or (
|
| 113 |
+
isinstance(value, float) and math.isnan(value)
|
| 114 |
+
)
|
| 115 |
+
):
|
| 116 |
+
# Reserved bevel-table tail: firmware may store NaN here and
|
| 117 |
+
# the current bevel ROI algorithm consumes only fw[0..4]. Keep
|
| 118 |
+
# either JSON null or a Python NaN sentinel through the external
|
| 119 |
+
# handoff; the tactile cache converts it to its internal NaN
|
| 120 |
+
# representation.
|
| 121 |
+
result.append(None)
|
| 122 |
+
continue
|
| 123 |
+
if isinstance(value, bool):
|
| 124 |
+
raise UvcCameraServiceError(
|
| 125 |
+
f"当前 Flash 参数 {key} 包含非数值"
|
| 126 |
+
)
|
| 127 |
+
try:
|
| 128 |
+
number = float(value)
|
| 129 |
+
except (TypeError, ValueError) as exc:
|
| 130 |
+
raise UvcCameraServiceError(
|
| 131 |
+
f"当前 Flash 参数 {key}[{index}] 包含非数值: {value!r}"
|
| 132 |
+
) from exc
|
| 133 |
+
if not math.isfinite(number):
|
| 134 |
+
raise UvcCameraServiceError(
|
| 135 |
+
f"当前 Flash 参数 {key}[{index}] 包含非有限值: {value!r}"
|
| 136 |
+
)
|
| 137 |
+
result.append(number)
|
| 138 |
+
return result
|
| 139 |
+
|
| 140 |
+
|
| 141 |
+
def _flash_params(candidate):
|
| 142 |
+
if not candidate.get("flash_ok") or not candidate.get(
|
| 143 |
+
"flash_params_ok"
|
| 144 |
+
):
|
| 145 |
+
raise UvcCameraServiceError(
|
| 146 |
+
"Sightac 本次 Flash 身份或完整标定参数读取失败: "
|
| 147 |
+
f"side={candidate.get('side') or '<none>'} "
|
| 148 |
+
f"error={candidate.get('error') or '<unknown>'}"
|
| 149 |
+
)
|
| 150 |
+
device_type = str(
|
| 151 |
+
candidate.get("device_type") or ""
|
| 152 |
+
).strip().lower()
|
| 153 |
+
if device_type not in {"planar", "curved", "bevel"}:
|
| 154 |
+
raise UvcCameraServiceError(
|
| 155 |
+
f"Flash device_type 无效: {device_type!r}"
|
| 156 |
+
)
|
| 157 |
+
encoded = candidate.get("flash_params")
|
| 158 |
+
if not isinstance(encoded, dict):
|
| 159 |
+
raise UvcCameraServiceError(
|
| 160 |
+
"libusb 探测没有返回本次完整 Flash 参数"
|
| 161 |
+
)
|
| 162 |
+
encoded_type = str(encoded.get("device_type") or "").strip().lower()
|
| 163 |
+
if encoded_type != device_type:
|
| 164 |
+
raise UvcCameraServiceError(
|
| 165 |
+
"Flash device_type 身份与参数表不一致"
|
| 166 |
+
)
|
| 167 |
+
return {
|
| 168 |
+
"device_type": device_type,
|
| 169 |
+
"fx_params": _require_finite_vector(encoded, "fx_params", 5),
|
| 170 |
+
"fy_params": _require_finite_vector(encoded, "fy_params", 5),
|
| 171 |
+
"fz_params": _require_finite_vector(encoded, "fz_params", 21),
|
| 172 |
+
"fw_params": _require_finite_vector(encoded, "fw_params", 9),
|
| 173 |
+
"ft_params": _require_finite_vector(encoded, "ft_params", 20),
|
| 174 |
+
"flash_state": str(
|
| 175 |
+
encoded.get("flash_state") or "programmed"
|
| 176 |
+
),
|
| 177 |
+
"calibration_source": "hardware-live-flash",
|
| 178 |
+
}
|
| 179 |
+
|
| 180 |
+
|
| 181 |
+
class IpcCameraNode:
|
| 182 |
+
"""The subset of UsbCameraNode consumed by the existing lifecycle."""
|
| 183 |
+
|
| 184 |
+
def __init__(self, *, role, socket_path, video_index, candidate,
|
| 185 |
+
outer_path, bus, flash_params=None):
|
| 186 |
+
self.role = str(role)
|
| 187 |
+
self.device_path = str(socket_path)
|
| 188 |
+
self.video_index = int(video_index)
|
| 189 |
+
self.physical_usb_path = f"{int(bus)}-{candidate['port']}"
|
| 190 |
+
self.physical_sysfs_path = self.physical_usb_path
|
| 191 |
+
self.direct_parent_hub = f"{int(bus)}-{outer_path}"
|
| 192 |
+
self.root_hub = f"{int(bus)}-{str(outer_path).split('.', 1)[0]}"
|
| 193 |
+
self.vendor_id = f"{int(candidate['vid']):04x}"
|
| 194 |
+
self.product_id = f"{int(candidate['pid']):04x}"
|
| 195 |
+
self.serial = str(
|
| 196 |
+
candidate.get("flash_serial")
|
| 197 |
+
or candidate.get("usb_serial")
|
| 198 |
+
or ""
|
| 199 |
+
)
|
| 200 |
+
self.transport = "libuvc-ipc"
|
| 201 |
+
self.flash_params = flash_params
|
| 202 |
+
self.capture_format = (
|
| 203 |
+
DECXIN_WIDTH,
|
| 204 |
+
DECXIN_HEIGHT,
|
| 205 |
+
DECXIN_FPS,
|
| 206 |
+
DECXIN_FOURCC,
|
| 207 |
+
) if self.role == "decxin" else (
|
| 208 |
+
SIGHTAC_WIDTH,
|
| 209 |
+
SIGHTAC_HEIGHT,
|
| 210 |
+
SIGHTAC_FPS,
|
| 211 |
+
SIGHTAC_FOURCC,
|
| 212 |
+
)
|
| 213 |
+
|
| 214 |
+
|
| 215 |
+
class IpcReservedCamera:
|
| 216 |
+
"""Lazy IPC client; opening it claims exactly one service socket."""
|
| 217 |
+
|
| 218 |
+
def __init__(self, node):
|
| 219 |
+
self.node = node
|
| 220 |
+
self.actual_format = tuple(node.capture_format)
|
| 221 |
+
self.transport = "libuvc-ipc"
|
| 222 |
+
self.flash_params = node.flash_params
|
| 223 |
+
self._camera = None
|
| 224 |
+
self._first_frame = None
|
| 225 |
+
self._claimed = False
|
| 226 |
+
self._lock = threading.Lock()
|
| 227 |
+
|
| 228 |
+
@property
|
| 229 |
+
def claimed(self):
|
| 230 |
+
with self._lock:
|
| 231 |
+
return self._claimed
|
| 232 |
+
|
| 233 |
+
def claim(self):
|
| 234 |
+
with self._lock:
|
| 235 |
+
if self._claimed:
|
| 236 |
+
raise UsbCameraSetError(
|
| 237 |
+
f"{self.node.role} IPC reservation was already claimed"
|
| 238 |
+
)
|
| 239 |
+
camera = IpcCamera(self.node.device_path, timeout=0.5)
|
| 240 |
+
try:
|
| 241 |
+
camera.open()
|
| 242 |
+
first_frame = None
|
| 243 |
+
failed_reads = 0
|
| 244 |
+
last_decode_packets = ()
|
| 245 |
+
for attempt in range(FIRST_FRAME_ATTEMPTS):
|
| 246 |
+
ok, frame = camera.read()
|
| 247 |
+
if ok and frame is not None and getattr(
|
| 248 |
+
frame, "size", 0
|
| 249 |
+
) > 0:
|
| 250 |
+
first_frame = frame
|
| 251 |
+
break
|
| 252 |
+
failed_reads += 1
|
| 253 |
+
last_decode_packets = (
|
| 254 |
+
camera.last_decode_diagnostics or ()
|
| 255 |
+
)
|
| 256 |
+
if attempt + 1 < FIRST_FRAME_ATTEMPTS:
|
| 257 |
+
time.sleep(FIRST_FRAME_RETRY_S)
|
| 258 |
+
if first_frame is None:
|
| 259 |
+
details = "".join(
|
| 260 |
+
f" seq={item.get('sequence')} bytes={item.get('bytes')}"
|
| 261 |
+
f" head={item.get('head')} tail={item.get('tail')}"
|
| 262 |
+
for item in last_decode_packets[-3:]
|
| 263 |
+
)
|
| 264 |
+
raise UsbCameraSetError(
|
| 265 |
+
f"{self.node.role} libuvc IPC has no first frame: "
|
| 266 |
+
f"{self.node.device_path} "
|
| 267 |
+
f"failed_reads={failed_reads}{details}"
|
| 268 |
+
)
|
| 269 |
+
self._claimed = True
|
| 270 |
+
self._camera = camera
|
| 271 |
+
self._first_frame = first_frame
|
| 272 |
+
return camera, first_frame, self.actual_format
|
| 273 |
+
except Exception:
|
| 274 |
+
camera.release()
|
| 275 |
+
raise
|
| 276 |
+
|
| 277 |
+
def release(self):
|
| 278 |
+
with self._lock:
|
| 279 |
+
camera = self._camera
|
| 280 |
+
self._camera = None
|
| 281 |
+
self._first_frame = None
|
| 282 |
+
if camera is not None:
|
| 283 |
+
camera.release()
|
| 284 |
+
|
| 285 |
+
|
| 286 |
+
class IpcCameraSetReservation:
|
| 287 |
+
def __init__(self, topology, cameras, on_release=None):
|
| 288 |
+
self.topology = topology
|
| 289 |
+
self._cameras = dict(cameras)
|
| 290 |
+
self._on_release = on_release
|
| 291 |
+
self._released = False
|
| 292 |
+
|
| 293 |
+
def camera(self, role):
|
| 294 |
+
return self._cameras[role]
|
| 295 |
+
|
| 296 |
+
def release_unclaimed(self):
|
| 297 |
+
for camera in self._cameras.values():
|
| 298 |
+
camera.release()
|
| 299 |
+
if not self._released:
|
| 300 |
+
self._released = True
|
| 301 |
+
if self._on_release is not None:
|
| 302 |
+
self._on_release()
|
| 303 |
+
|
| 304 |
+
|
| 305 |
+
class UvcCameraServiceManager:
|
| 306 |
+
"""Own dynamic camera-service processes for the active UI slots.
|
| 307 |
+
|
| 308 |
+
One service process is used per selected physical camera set. This is
|
| 309 |
+
intentional: the current socket protocol accepts one client per output,
|
| 310 |
+
and two UI slots must not force a running first set to be reconfigured
|
| 311 |
+
underneath its clients.
|
| 312 |
+
"""
|
| 313 |
+
|
| 314 |
+
def __init__(self, *, logger=print, discovery_binary=DISCOVERY_BINARY,
|
| 315 |
+
service_binary=SERVICE_BINARY,
|
| 316 |
+
service_cpus: tuple[int, ...] | None = None):
|
| 317 |
+
self._logger = logger
|
| 318 |
+
self._discovery_binary = Path(discovery_binary).resolve()
|
| 319 |
+
self._service_binary = Path(service_binary).resolve()
|
| 320 |
+
self._service_cpus = tuple(
|
| 321 |
+
int(cpu) for cpu in (service_cpus or ())
|
| 322 |
+
)
|
| 323 |
+
self._lock = threading.RLock()
|
| 324 |
+
self._services = {}
|
| 325 |
+
|
| 326 |
+
def _run_discovery(self, *, usb_bus=None, port_prefix=None):
|
| 327 |
+
if not self._discovery_binary.is_file():
|
| 328 |
+
raise UvcCameraServiceError(
|
| 329 |
+
f"libusb/libuvc 发现程序不存在: {self._discovery_binary}"
|
| 330 |
+
)
|
| 331 |
+
if not os.access(self._discovery_binary, os.X_OK):
|
| 332 |
+
raise UvcCameraServiceError(
|
| 333 |
+
f"libusb/libuvc 发现程序不可执行: {self._discovery_binary}"
|
| 334 |
+
)
|
| 335 |
+
command = [str(self._discovery_binary), "--json"]
|
| 336 |
+
if usb_bus is not None and port_prefix is not None:
|
| 337 |
+
command += ["--bus", str(usb_bus), "--port-prefix", str(port_prefix)]
|
| 338 |
+
completed = subprocess.run(
|
| 339 |
+
command,
|
| 340 |
+
capture_output=True,
|
| 341 |
+
text=True,
|
| 342 |
+
timeout=DISCOVERY_TIMEOUT_S,
|
| 343 |
+
check=False,
|
| 344 |
+
env=_native_env(),
|
| 345 |
+
)
|
| 346 |
+
if completed.returncode != 0:
|
| 347 |
+
details = "\n".join(
|
| 348 |
+
(completed.stderr or "").splitlines()[-20:]
|
| 349 |
+
)
|
| 350 |
+
raise UvcCameraServiceError(
|
| 351 |
+
"libusb/libuvc 动态发现失败 "
|
| 352 |
+
f"rc={completed.returncode}: {details or '<no detail>'}"
|
| 353 |
+
)
|
| 354 |
+
try:
|
| 355 |
+
report = json.loads(completed.stdout)
|
| 356 |
+
except json.JSONDecodeError as exc:
|
| 357 |
+
raise UvcCameraServiceError(
|
| 358 |
+
f"libusb/libuvc 发现 JSON 无效: {exc}"
|
| 359 |
+
) from exc
|
| 360 |
+
groups = report.get("groups") if isinstance(report, dict) else None
|
| 361 |
+
if not isinstance(groups, list) or not groups:
|
| 362 |
+
raise UvcCameraServiceError(
|
| 363 |
+
"libusb/libuvc 没有发现完整 Sightac + DECXIN 组"
|
| 364 |
+
)
|
| 365 |
+
return groups
|
| 366 |
+
|
| 367 |
+
@staticmethod
|
| 368 |
+
def _assignment_identity(assignment):
|
| 369 |
+
if not isinstance(assignment, dict):
|
| 370 |
+
raise UvcCameraServiceError(
|
| 371 |
+
"Fays 尚未完成 device_setup 身份分配"
|
| 372 |
+
)
|
| 373 |
+
esp = assignment.get("esp32") or {}
|
| 374 |
+
fays = assignment.get("fays") or {}
|
| 375 |
+
esp_serial = str(esp.get("serial") or "").strip()
|
| 376 |
+
fays_serial = str(fays.get("product_serial") or "").strip()
|
| 377 |
+
fays_path = str(
|
| 378 |
+
fays.get("physical_usb_path") or ""
|
| 379 |
+
).strip()
|
| 380 |
+
if not esp_serial or not fays_serial or not fays_path:
|
| 381 |
+
raise UvcCameraServiceError(
|
| 382 |
+
"Fays 当前分配缺少 ESP32 serial、Fays serial 或物理路径"
|
| 383 |
+
)
|
| 384 |
+
return esp_serial, fays_serial, fays_path
|
| 385 |
+
|
| 386 |
+
def _associate_group(self, group, assignment, live_esp):
|
| 387 |
+
esp_serial, fays_serial, fays_path = self._assignment_identity(
|
| 388 |
+
assignment
|
| 389 |
+
)
|
| 390 |
+
bus = int(group.get("bus", -1))
|
| 391 |
+
outer_path = str(group.get("outer_path") or "").strip()
|
| 392 |
+
matching_esp = [
|
| 393 |
+
item for item in live_esp
|
| 394 |
+
if str(item.get("serial") or "").strip() == esp_serial
|
| 395 |
+
and _path_contains(bus, outer_path,
|
| 396 |
+
item.get("physical_usb_path"))
|
| 397 |
+
]
|
| 398 |
+
if len(matching_esp) != 1:
|
| 399 |
+
raise UvcCameraServiceError(
|
| 400 |
+
"当前 UVC 组无法按 ESP32 serial 唯一关联: "
|
| 401 |
+
f"serial={esp_serial} matches={len(matching_esp)} "
|
| 402 |
+
f"group={bus}-{outer_path}"
|
| 403 |
+
)
|
| 404 |
+
# Fays is USB3/FT602 and may be reported on a different libusb bus;
|
| 405 |
+
# the existing topology rule intentionally correlates its root port.
|
| 406 |
+
if _root_port(fays_path) != str(outer_path).split(".", 1)[0].split(
|
| 407 |
+
"-", 1
|
| 408 |
+
)[-1]:
|
| 409 |
+
raise UvcCameraServiceError(
|
| 410 |
+
"当前 UVC 组与已分配 Fays 不在同一根端口: "
|
| 411 |
+
f"fays={fays_path} group={bus}-{outer_path}"
|
| 412 |
+
)
|
| 413 |
+
normalized = {
|
| 414 |
+
"bus": bus,
|
| 415 |
+
"outer_path": outer_path,
|
| 416 |
+
"esp32_serial": esp_serial,
|
| 417 |
+
"fays_serial": fays_serial,
|
| 418 |
+
"sightac": {},
|
| 419 |
+
"decxin": group.get("decxin"),
|
| 420 |
+
}
|
| 421 |
+
sightac = group.get("sightac")
|
| 422 |
+
if not isinstance(sightac, list) or len(sightac) != 2:
|
| 423 |
+
raise UvcCameraServiceError(
|
| 424 |
+
"UVC 组 Sightac 数量不是 2"
|
| 425 |
+
)
|
| 426 |
+
for candidate in sightac:
|
| 427 |
+
side = str(candidate.get("side") or "").strip().lower()
|
| 428 |
+
if side not in {"left", "right"} or side in normalized[
|
| 429 |
+
"sightac"
|
| 430 |
+
]:
|
| 431 |
+
raise UvcCameraServiceError(
|
| 432 |
+
f"UVC 组 left/right Flash 身份不唯一: side={side!r}"
|
| 433 |
+
)
|
| 434 |
+
normalized["sightac"][side] = candidate
|
| 435 |
+
if set(normalized["sightac"]) != {"left", "right"}:
|
| 436 |
+
raise UvcCameraServiceError(
|
| 437 |
+
"UVC 组缺少动态 Flash left/right 身份"
|
| 438 |
+
)
|
| 439 |
+
if not group.get("valid"):
|
| 440 |
+
raise UvcCameraServiceError(
|
| 441 |
+
"UVC 组未通过动态模式或 Flash 完整参数校验"
|
| 442 |
+
)
|
| 443 |
+
for side in ("left", "right"):
|
| 444 |
+
normalized["sightac"][side]["flash_params_local"] = (
|
| 445 |
+
_flash_params(normalized["sightac"][side])
|
| 446 |
+
)
|
| 447 |
+
decxin = normalized["decxin"]
|
| 448 |
+
if not isinstance(decxin, dict) or not decxin.get("mode_ok"):
|
| 449 |
+
raise UvcCameraServiceError(
|
| 450 |
+
"UVC 组 DECXIN 未通过动态模式校验"
|
| 451 |
+
)
|
| 452 |
+
return normalized
|
| 453 |
+
|
| 454 |
+
def _discovery_scope(self, assignment, live_esp=None):
|
| 455 |
+
from core.gripper.fays_runtime import discover_esp32_devices
|
| 456 |
+
serial, _, _ = self._assignment_identity(assignment)
|
| 457 |
+
devices = tuple(discover_esp32_devices()) if live_esp is None else live_esp
|
| 458 |
+
matches = [d for d in devices if str(d.get("serial", "")).strip() == serial]
|
| 459 |
+
if len(matches) != 1:
|
| 460 |
+
raise UvcCameraServiceError("ESP32 身份无法唯一定位,拒绝启动全设备扫描")
|
| 461 |
+
physical = str(matches[0].get("physical_usb_path", ""))
|
| 462 |
+
bus, separator, ports = physical.partition("-")
|
| 463 |
+
root = ports.split(".")[0]
|
| 464 |
+
if not separator or not bus.isdigit() or not root.isdigit():
|
| 465 |
+
raise UvcCameraServiceError("ESP32 USB 路径无效,请重建设备清单")
|
| 466 |
+
return int(bus), root
|
| 467 |
+
|
| 468 |
+
def _resolve_current_group(self, assignment):
|
| 469 |
+
from core.gripper.fays_runtime import discover_esp32_devices
|
| 470 |
+
|
| 471 |
+
live_esp = tuple(discover_esp32_devices())
|
| 472 |
+
bus, port = self._discovery_scope(assignment, live_esp)
|
| 473 |
+
groups = self._run_discovery(usb_bus=bus, port_prefix=port)
|
| 474 |
+
matches = []
|
| 475 |
+
for group in groups:
|
| 476 |
+
try:
|
| 477 |
+
matches.append(self._associate_group(
|
| 478 |
+
group, assignment, live_esp))
|
| 479 |
+
except UvcCameraServiceError:
|
| 480 |
+
continue
|
| 481 |
+
if len(matches) != 1:
|
| 482 |
+
esp_serial, fays_serial, _ = self._assignment_identity(assignment)
|
| 483 |
+
raise UvcCameraServiceError(
|
| 484 |
+
"当前设备清单身份与动态 UVC 拓扑无法唯一配对: "
|
| 485 |
+
f"esp32={esp_serial} fays={fays_serial} "
|
| 486 |
+
f"matches={len(matches)} groups={len(groups)}"
|
| 487 |
+
)
|
| 488 |
+
return matches[0]
|
| 489 |
+
|
| 490 |
+
@staticmethod
|
| 491 |
+
def _signature_for(records):
|
| 492 |
+
return tuple(sorted(
|
| 493 |
+
(
|
| 494 |
+
int(record["bus"]),
|
| 495 |
+
record["outer_path"],
|
| 496 |
+
tuple(sorted(
|
| 497 |
+
(side, str(candidate["port"]))
|
| 498 |
+
for side, candidate in record["sightac"].items()
|
| 499 |
+
)),
|
| 500 |
+
str(record["decxin"]["port"]),
|
| 501 |
+
)
|
| 502 |
+
for record in records
|
| 503 |
+
))
|
| 504 |
+
|
| 505 |
+
def _write_config(self, records):
|
| 506 |
+
runtime_dir = Path(tempfile.mkdtemp(prefix="ksq-camera-service-"))
|
| 507 |
+
lines = [
|
| 508 |
+
"# Generated at runtime from the current libusb/libuvc scan.",
|
| 509 |
+
"# Fays is not handled by this service.",
|
| 510 |
+
"",
|
| 511 |
+
]
|
| 512 |
+
sockets = {}
|
| 513 |
+
for index, record in enumerate(records, 1):
|
| 514 |
+
for role in ("left", "right", "decxin"):
|
| 515 |
+
candidate = (
|
| 516 |
+
record["sightac"][role]
|
| 517 |
+
if role in {"left", "right"}
|
| 518 |
+
else record["decxin"]
|
| 519 |
+
)
|
| 520 |
+
name = f"unit_{index}_{role}"
|
| 521 |
+
socket_path = runtime_dir / f"{name}.sock"
|
| 522 |
+
sockets[(index, role)] = str(socket_path)
|
| 523 |
+
lines.extend([
|
| 524 |
+
f"[{name}]",
|
| 525 |
+
f"vid=0x{int(candidate['vid']):04x}",
|
| 526 |
+
f"pid=0x{int(candidate['pid']):04x}",
|
| 527 |
+
f"usb_bus={int(record['bus'])}",
|
| 528 |
+
f"usb_port_path={candidate['port']}",
|
| 529 |
+
f"streaming_interface={int(candidate['streaming_interface'])}",
|
| 530 |
+
f"forced_altsetting={int(candidate['altsetting'])}",
|
| 531 |
+
"forced_payload=800" if role in {"left", "right"}
|
| 532 |
+
else "forced_payload=1280",
|
| 533 |
+
"width=640" if role in {"left", "right"}
|
| 534 |
+
else "width=1280",
|
| 535 |
+
"height=480" if role in {"left", "right"}
|
| 536 |
+
else "height=960",
|
| 537 |
+
"fps=30",
|
| 538 |
+
"format=MJPEG",
|
| 539 |
+
f"socket_path={socket_path}",
|
| 540 |
+
"",
|
| 541 |
+
])
|
| 542 |
+
config_path = runtime_dir / "current.ini"
|
| 543 |
+
config_path.write_text("\n".join(lines), encoding="utf-8")
|
| 544 |
+
return runtime_dir, config_path, sockets
|
| 545 |
+
|
| 546 |
+
def _ensure_service(self, records):
|
| 547 |
+
signature = self._signature_for(records)
|
| 548 |
+
with self._lock:
|
| 549 |
+
entry = self._services.get(signature)
|
| 550 |
+
if entry is not None and entry["process"].poll() is None:
|
| 551 |
+
if entry["leases"]:
|
| 552 |
+
raise UvcCameraServiceError(
|
| 553 |
+
"当前物理相机组已有活动 libuvc 租约;"
|
| 554 |
+
"不能让两个界面共用同一组相机"
|
| 555 |
+
)
|
| 556 |
+
return signature, entry["runtime_dir"], entry["sockets"]
|
| 557 |
+
if entry is not None:
|
| 558 |
+
self._stop_entry_locked(entry)
|
| 559 |
+
self._services.pop(signature, None)
|
| 560 |
+
if not self._service_binary.is_file():
|
| 561 |
+
raise UvcCameraServiceError(
|
| 562 |
+
f"libusb/libuvc 服务程序不存在: {self._service_binary}"
|
| 563 |
+
)
|
| 564 |
+
runtime_dir, config_path, sockets = self._write_config(records)
|
| 565 |
+
service_log_path = runtime_dir / "camera-service.log"
|
| 566 |
+
service_log = service_log_path.open("a", encoding="utf-8")
|
| 567 |
+
try:
|
| 568 |
+
process = subprocess.Popen(
|
| 569 |
+
[
|
| 570 |
+
str(self._service_binary),
|
| 571 |
+
"--config", str(config_path),
|
| 572 |
+
"--parent-pid", str(os.getpid()),
|
| 573 |
+
],
|
| 574 |
+
stdin=subprocess.DEVNULL,
|
| 575 |
+
stdout=subprocess.DEVNULL,
|
| 576 |
+
stderr=service_log,
|
| 577 |
+
start_new_session=True,
|
| 578 |
+
env=_native_env(),
|
| 579 |
+
)
|
| 580 |
+
finally:
|
| 581 |
+
service_log.close()
|
| 582 |
+
def startup_details():
|
| 583 |
+
with service_log_path.open("rb") as stream:
|
| 584 |
+
stream.seek(max(0, service_log_path.stat().st_size - 8192))
|
| 585 |
+
tail = stream.read().decode("utf-8", errors="replace")
|
| 586 |
+
return "\n".join(tail.splitlines()[-12:])
|
| 587 |
+
if self._service_cpus:
|
| 588 |
+
try:
|
| 589 |
+
os.sched_setaffinity(
|
| 590 |
+
process.pid, set(self._service_cpus))
|
| 591 |
+
actual_cpus = tuple(sorted(
|
| 592 |
+
os.sched_getaffinity(process.pid)))
|
| 593 |
+
if actual_cpus != self._service_cpus:
|
| 594 |
+
self._logger(
|
| 595 |
+
"[UVC] camera-service affinity mismatch: "
|
| 596 |
+
f"pid={process.pid} "
|
| 597 |
+
f"expected={self._service_cpus} "
|
| 598 |
+
f"actual={actual_cpus}"
|
| 599 |
+
)
|
| 600 |
+
except (OSError, PermissionError) as exc:
|
| 601 |
+
self._logger(
|
| 602 |
+
"[UVC] camera-service affinity bind FAILED: "
|
| 603 |
+
f"pid={process.pid} "
|
| 604 |
+
f"cpus={self._service_cpus}: {exc}"
|
| 605 |
+
)
|
| 606 |
+
deadline = time.monotonic() + SERVICE_START_TIMEOUT_S
|
| 607 |
+
required = tuple(sockets.values())
|
| 608 |
+
while time.monotonic() < deadline:
|
| 609 |
+
if process.poll() is not None:
|
| 610 |
+
raise UvcCameraServiceError(
|
| 611 |
+
f"camera-service 提前退出 rc={process.returncode}; "
|
| 612 |
+
f"日志={service_log_path}\n{startup_details()}"
|
| 613 |
+
)
|
| 614 |
+
if all(os.path.exists(path) for path in required):
|
| 615 |
+
self._services[signature] = {
|
| 616 |
+
"process": process,
|
| 617 |
+
"runtime_dir": runtime_dir,
|
| 618 |
+
"sockets": sockets,
|
| 619 |
+
"leases": 0,
|
| 620 |
+
}
|
| 621 |
+
self._logger(
|
| 622 |
+
"[UVC] libusb/libuvc camera-service started: "
|
| 623 |
+
f"group={signature[0][1]} pid={process.pid} "
|
| 624 |
+
f"cpus={self._service_cpus or '<inherit>'} "
|
| 625 |
+
f"log={service_log_path}"
|
| 626 |
+
)
|
| 627 |
+
return signature, runtime_dir, sockets
|
| 628 |
+
time.sleep(0.05)
|
| 629 |
+
process.terminate()
|
| 630 |
+
try:
|
| 631 |
+
process.wait(timeout=2.0)
|
| 632 |
+
except subprocess.TimeoutExpired:
|
| 633 |
+
process.kill()
|
| 634 |
+
process.wait(timeout=2.0)
|
| 635 |
+
raise UvcCameraServiceError(
|
| 636 |
+
"camera-service 未在限定时间取得全部相机的有效 MJPG 帧;"
|
| 637 |
+
f"日志={service_log_path}\n{startup_details()}"
|
| 638 |
+
)
|
| 639 |
+
|
| 640 |
+
def _release_lease(self, signature):
|
| 641 |
+
with self._lock:
|
| 642 |
+
entry = self._services.get(signature)
|
| 643 |
+
if entry is not None:
|
| 644 |
+
entry["leases"] = max(0, entry["leases"] - 1)
|
| 645 |
+
if (
|
| 646 |
+
entry is not None
|
| 647 |
+
and int(entry["leases"]) <= 0
|
| 648 |
+
):
|
| 649 |
+
# 租约归零说明该物理相机组已无人使用。必须立刻停掉并释放
|
| 650 |
+
# libusb,否则重连时 discover 二进制无法读取已被占用的
|
| 651 |
+
# Flash 身份,_resolve_current_group 会得到 matches=0
|
| 652 |
+
# (“设备清单身份与动态 UVC 拓扑无法唯一配对”)。
|
| 653 |
+
self._stop_entry_locked(entry)
|
| 654 |
+
self._services.pop(signature, None)
|
| 655 |
+
|
| 656 |
+
def select(self, manifest_provider):
|
| 657 |
+
"""Claim the physical group before any discovery opens its cameras."""
|
| 658 |
+
assignment = manifest_provider()
|
| 659 |
+
bus, port = self._discovery_scope(assignment)
|
| 660 |
+
guard = device_access_guard(f"/tmp/ksq-uvc-group-{bus}-{port}.lock", timeout=0)
|
| 661 |
+
guard.__enter__()
|
| 662 |
+
try:
|
| 663 |
+
with self._lock:
|
| 664 |
+
reservation = self._select_assignment(assignment)
|
| 665 |
+
release = reservation._on_release
|
| 666 |
+
def release_guarded():
|
| 667 |
+
try:
|
| 668 |
+
release()
|
| 669 |
+
finally:
|
| 670 |
+
guard.__exit__(None, None, None)
|
| 671 |
+
reservation._on_release = release_guarded
|
| 672 |
+
return reservation
|
| 673 |
+
except BaseException:
|
| 674 |
+
try:
|
| 675 |
+
with self._lock:
|
| 676 |
+
for signature, entry in tuple(self._services.items()):
|
| 677 |
+
if (not entry["leases"] and any(
|
| 678 |
+
item[0] == bus and item[1].split(".")[0] == port
|
| 679 |
+
for item in signature)):
|
| 680 |
+
self._stop_entry_locked(entry)
|
| 681 |
+
self._services.pop(signature, None)
|
| 682 |
+
finally:
|
| 683 |
+
guard.__exit__(None, None, None)
|
| 684 |
+
raise
|
| 685 |
+
|
| 686 |
+
def _select_assignment(self, assignment):
|
| 687 |
+
selected = self._resolve_current_group(assignment)
|
| 688 |
+
# Use one service per physical set so selecting a second device does
|
| 689 |
+
# not restart or invalidate the first set's Unix-socket clients.
|
| 690 |
+
signature, _, sockets = self._ensure_service((selected,))
|
| 691 |
+
index = 1
|
| 692 |
+
left_candidate = selected["sightac"]["left"]
|
| 693 |
+
right_candidate = selected["sightac"]["right"]
|
| 694 |
+
decxin_candidate = selected["decxin"]
|
| 695 |
+
left_params = left_candidate["flash_params_local"]
|
| 696 |
+
right_params = right_candidate["flash_params_local"]
|
| 697 |
+
left_node = IpcCameraNode(
|
| 698 |
+
role="left", socket_path=sockets[(index, "left")],
|
| 699 |
+
video_index=1000000 + index * 10 + 1,
|
| 700 |
+
candidate=left_candidate, outer_path=selected["outer_path"],
|
| 701 |
+
bus=selected["bus"], flash_params=left_params,
|
| 702 |
+
)
|
| 703 |
+
right_node = IpcCameraNode(
|
| 704 |
+
role="right", socket_path=sockets[(index, "right")],
|
| 705 |
+
video_index=1000000 + index * 10 + 2,
|
| 706 |
+
candidate=right_candidate, outer_path=selected["outer_path"],
|
| 707 |
+
bus=selected["bus"], flash_params=right_params,
|
| 708 |
+
)
|
| 709 |
+
decxin_node = IpcCameraNode(
|
| 710 |
+
role="decxin", socket_path=sockets[(index, "decxin")],
|
| 711 |
+
video_index=1000000 + index * 10 + 3,
|
| 712 |
+
candidate=decxin_candidate, outer_path=selected["outer_path"],
|
| 713 |
+
bus=selected["bus"],
|
| 714 |
+
)
|
| 715 |
+
topology = UsbCameraSetTopology(
|
| 716 |
+
root_hub=f"{selected['bus']}-{selected['outer_path'].split('.', 1)[0]}",
|
| 717 |
+
tactile_hub=f"{selected['bus']}-{selected['outer_path']}",
|
| 718 |
+
decxin=decxin_node,
|
| 719 |
+
left=left_node,
|
| 720 |
+
right=right_node,
|
| 721 |
+
)
|
| 722 |
+
cameras = {
|
| 723 |
+
"left": IpcReservedCamera(left_node),
|
| 724 |
+
"right": IpcReservedCamera(right_node),
|
| 725 |
+
"decxin": IpcReservedCamera(decxin_node),
|
| 726 |
+
}
|
| 727 |
+
with self._lock:
|
| 728 |
+
self._services[signature]["leases"] += 1
|
| 729 |
+
return IpcCameraSetReservation(
|
| 730 |
+
topology,
|
| 731 |
+
cameras,
|
| 732 |
+
on_release=lambda sig=signature: self._release_lease(sig),
|
| 733 |
+
)
|
| 734 |
+
|
| 735 |
+
@staticmethod
|
| 736 |
+
def _stop_entry_locked(entry):
|
| 737 |
+
process = entry["process"]
|
| 738 |
+
runtime_dir = entry["runtime_dir"]
|
| 739 |
+
if process is not None and process.poll() is None:
|
| 740 |
+
process.terminate()
|
| 741 |
+
try:
|
| 742 |
+
process.wait(timeout=3.0)
|
| 743 |
+
except subprocess.TimeoutExpired:
|
| 744 |
+
process.kill()
|
| 745 |
+
process.wait(timeout=2.0)
|
| 746 |
+
shutil.rmtree(runtime_dir, ignore_errors=True)
|
| 747 |
+
|
| 748 |
+
def _stop_locked(self):
|
| 749 |
+
for entry in tuple(self._services.values()):
|
| 750 |
+
self._stop_entry_locked(entry)
|
| 751 |
+
self._services.clear()
|
| 752 |
+
|
| 753 |
+
def stop(self):
|
| 754 |
+
with self._lock:
|
| 755 |
+
self._stop_locked()
|
| 756 |
+
|
| 757 |
+
|
| 758 |
+
def build_uvc_camera_set_selector(discovery, manifest_provider,
|
| 759 |
+
service_manager, *, logger=print):
|
| 760 |
+
"""Build the production selector backed by dynamic libusb/libuvc."""
|
| 761 |
+
|
| 762 |
+
class Selector:
|
| 763 |
+
def select(self):
|
| 764 |
+
reservation = service_manager.select(manifest_provider)
|
| 765 |
+
try:
|
| 766 |
+
for side in ("left", "right"):
|
| 767 |
+
camera = reservation.camera(side)
|
| 768 |
+
cache = getattr(
|
| 769 |
+
discovery, "cache_external_flash_params")
|
| 770 |
+
cache(camera.node.video_index, camera.flash_params)
|
| 771 |
+
except Exception:
|
| 772 |
+
reservation.release_unclaimed()
|
| 773 |
+
raise
|
| 774 |
+
logger(
|
| 775 |
+
"[CameraSet] libusb/libuvc selected dynamic group: "
|
| 776 |
+
f"left={reservation.topology.left.device_path} "
|
| 777 |
+
f"right={reservation.topology.right.device_path} "
|
| 778 |
+
f"decxin={reservation.topology.decxin.device_path}"
|
| 779 |
+
)
|
| 780 |
+
return reservation
|
| 781 |
+
|
| 782 |
+
return Selector()
|
collector/core/gripper/fays_runtime.py
ADDED
|
@@ -0,0 +1,1638 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
#!/usr/bin/env python3
|
| 2 |
+
"""FaysSense S80M 的单一运行时配置来源。
|
| 3 |
+
|
| 4 |
+
这个模块只使用 Python 标准库,因此上位机、启动器和检查脚本都可以安全导入。
|
| 5 |
+
相机设备节点来自 Fays SDK 配置,禁止在业务代码中另写一套默认值。
|
| 6 |
+
"""
|
| 7 |
+
|
| 8 |
+
import fcntl
|
| 9 |
+
import glob
|
| 10 |
+
import hashlib
|
| 11 |
+
import json
|
| 12 |
+
import os
|
| 13 |
+
import re
|
| 14 |
+
import threading
|
| 15 |
+
import time
|
| 16 |
+
import sys
|
| 17 |
+
|
| 18 |
+
if "__compiled__" in globals():
|
| 19 |
+
GRIPPER_DIR = os.path.dirname(os.path.abspath(sys.argv[0])) # 二进制所在目录
|
| 20 |
+
else:
|
| 21 |
+
GRIPPER_DIR = os.path.dirname(os.path.abspath(__file__))
|
| 22 |
+
PROJECT_ROOT = os.path.dirname(GRIPPER_DIR)
|
| 23 |
+
SUPPORTED_CAMERA = "FaysSense S80M stereo fisheye + IMU"
|
| 24 |
+
FAYS_MIN_USB_SPEED_MBPS = 5000.0
|
| 25 |
+
FAYS_STEREO_FPS = 30.0
|
| 26 |
+
|
| 27 |
+
FAYS_SDK_ROOT = os.path.abspath(os.environ.get(
|
| 28 |
+
"FAYS_SDK_ROOT",
|
| 29 |
+
os.path.join(PROJECT_ROOT, "FaysSense_VI_Kit_Release"),
|
| 30 |
+
))
|
| 31 |
+
ORB_ROOT = os.path.abspath(os.environ.get(
|
| 32 |
+
"KSQ_ORB_ROOT",
|
| 33 |
+
os.path.join(PROJECT_ROOT, "ORB-SLAM"),
|
| 34 |
+
))
|
| 35 |
+
|
| 36 |
+
# There is one production ORB bridge. ``device_setup`` records this executable
|
| 37 |
+
# together with the per-device SDK/ORB YAML paths in the canonical manifest.
|
| 38 |
+
FAYS_MARK_ONLY_BINARY = os.path.join(
|
| 39 |
+
PROJECT_ROOT,
|
| 40 |
+
"dist",
|
| 41 |
+
"fays_opencv48",
|
| 42 |
+
"bin",
|
| 43 |
+
"fayssense_orb_slam_sn219_opencv48_mark_only",
|
| 44 |
+
)
|
| 45 |
+
# Do not expose an executable override: the production bridge is fixed.
|
| 46 |
+
FAYS_ORB_BINARY = FAYS_MARK_ONLY_BINARY
|
| 47 |
+
FAYS_DEVICE_CONFIG = os.path.join(
|
| 48 |
+
FAYS_SDK_ROOT,
|
| 49 |
+
"config",
|
| 50 |
+
"fays_vikit_s80m.yaml",
|
| 51 |
+
)
|
| 52 |
+
DEVICE_MANIFEST = os.path.join(
|
| 53 |
+
GRIPPER_DIR,
|
| 54 |
+
"device_manifest.json",
|
| 55 |
+
)
|
| 56 |
+
_ORB_VOCABULARY_CONFIG = os.environ.get("KSQ_ORB_VOCABULARY", "ORBvoc.txt")
|
| 57 |
+
ORB_VOCABULARY = os.path.abspath(
|
| 58 |
+
_ORB_VOCABULARY_CONFIG
|
| 59 |
+
if os.path.isabs(_ORB_VOCABULARY_CONFIG)
|
| 60 |
+
else os.path.join(PROJECT_ROOT, _ORB_VOCABULARY_CONFIG)
|
| 61 |
+
)
|
| 62 |
+
ORB_LIBRARY = os.path.abspath(os.environ.get(
|
| 63 |
+
"KSQ_ORB_LIBRARY",
|
| 64 |
+
os.path.join(
|
| 65 |
+
PROJECT_ROOT,
|
| 66 |
+
"dist",
|
| 67 |
+
"orb_mark_only",
|
| 68 |
+
"lib",
|
| 69 |
+
"libORB_SLAM3.so",
|
| 70 |
+
),
|
| 71 |
+
))
|
| 72 |
+
ORB_WORK_DIR = os.path.join(PROJECT_ROOT, "ORB-SLAM", "Examples", "fays")
|
| 73 |
+
ORB_TRAJECTORY = os.path.join(PROJECT_ROOT, "ORB-SLAM", "traj.txt")
|
| 74 |
+
|
| 75 |
+
# AIKit is a peer localization backend, not an ORB-SLAM calibration mode. The
|
| 76 |
+
# official service keeps its runtime tree in a bind-mounted host directory;
|
| 77 |
+
# before each launch we replace only the current device's factory calibration
|
| 78 |
+
# and ephemeral video nodes in that tree.
|
| 79 |
+
AIKIT_BRIDGE_BINARY = os.path.abspath(os.environ.get(
|
| 80 |
+
"KSQ_FAYS_AIKIT_BRIDGE",
|
| 81 |
+
os.path.join(
|
| 82 |
+
PROJECT_ROOT, "dist", "fays_aikit", "bin",
|
| 83 |
+
"fays_aikit_slam_bridge",
|
| 84 |
+
),
|
| 85 |
+
))
|
| 86 |
+
AIKIT_CLIENT_LIBRARY_DIR = os.path.abspath(os.environ.get(
|
| 87 |
+
"KSQ_FAYS_AIKIT_LIBRARY_DIR",
|
| 88 |
+
os.path.join(PROJECT_ROOT, "dist", "fays_aikit", "lib"),
|
| 89 |
+
))
|
| 90 |
+
AIKIT_CLIENT_CONFIG = os.path.abspath(os.environ.get(
|
| 91 |
+
"KSQ_FAYS_AIKIT_CLIENT_CONFIG",
|
| 92 |
+
os.path.join(
|
| 93 |
+
PROJECT_ROOT, "dist", "fays_aikit", "config",
|
| 94 |
+
"active_tracker_client.yaml",
|
| 95 |
+
),
|
| 96 |
+
))
|
| 97 |
+
AIKIT_SERVICE_CONTAINER = os.environ.get(
|
| 98 |
+
"KSQ_FAYS_AIKIT_CONTAINER", "ksq-fays-aikit-sn198"
|
| 99 |
+
).strip()
|
| 100 |
+
AIKIT_SERVICE_HOST = os.environ.get(
|
| 101 |
+
"KSQ_FAYS_AIKIT_HOST", "127.0.0.1"
|
| 102 |
+
).strip()
|
| 103 |
+
AIKIT_SERVICE_PORT = int(os.environ.get("KSQ_FAYS_AIKIT_PORT", "5413"))
|
| 104 |
+
AIKIT_SERVICE_RUNTIME_ROOT = os.path.abspath(os.environ.get(
|
| 105 |
+
"KSQ_FAYS_AIKIT_RUNTIME_ROOT",
|
| 106 |
+
os.path.expanduser("~/Downloads/FaysSense_AIKit/runtime_sn198"),
|
| 107 |
+
))
|
| 108 |
+
AIKIT_SERVICE_DEVICE_CONFIG = os.path.join(
|
| 109 |
+
AIKIT_SERVICE_RUNTIME_ROOT, "driver", "fays_vikit.yaml"
|
| 110 |
+
)
|
| 111 |
+
AIKIT_SERVICE_CALIBRATION = os.path.join(
|
| 112 |
+
AIKIT_SERVICE_RUNTIME_ROOT, "calib", "calib.yaml"
|
| 113 |
+
)
|
| 114 |
+
AIKIT_FACTORY_CALIBRATION_DIR = os.path.join(
|
| 115 |
+
PROJECT_ROOT, "config", "calibration"
|
| 116 |
+
)
|
| 117 |
+
|
| 118 |
+
FAYS_VENDOR_ID = "0403"
|
| 119 |
+
FAYS_PRODUCT_ID = "602e"
|
| 120 |
+
ESP32_VENDOR_ID = "303a"
|
| 121 |
+
ESP32_PRODUCT_ID = "1001"
|
| 122 |
+
_USB_PHYSICAL_PATTERN = re.compile(r"\d+-\d+(?:\.\d+)*")
|
| 123 |
+
_INSTANCE_IPC_FILES = (
|
| 124 |
+
"orb_current_frame_tmp.jpg",
|
| 125 |
+
"orb_current_frame.jpg",
|
| 126 |
+
"orb_meta_tmp.json",
|
| 127 |
+
"orb_meta.json",
|
| 128 |
+
"orb_pose.json.tmp",
|
| 129 |
+
"orb_pose.json",
|
| 130 |
+
"orb_control.json.tmp",
|
| 131 |
+
"orb_control.json",
|
| 132 |
+
"orb_raw_stream.sock",
|
| 133 |
+
"camera_calibration.yaml",
|
| 134 |
+
"imu.yaml",
|
| 135 |
+
)
|
| 136 |
+
|
| 137 |
+
|
| 138 |
+
class FaysRuntimeError(RuntimeError):
|
| 139 |
+
"""Fays 运行时文件或配置不完整。"""
|
| 140 |
+
|
| 141 |
+
|
| 142 |
+
def fays_slam_profile(calibration_serial):
|
| 143 |
+
"""Return the setup-time artifact paths for one discovered Fays serial.
|
| 144 |
+
|
| 145 |
+
This helper is used only while ``device_setup`` creates the manifest. It
|
| 146 |
+
does not select among historical devices or provide a runtime fallback.
|
| 147 |
+
"""
|
| 148 |
+
|
| 149 |
+
serial = str(calibration_serial or "").strip()
|
| 150 |
+
if not serial:
|
| 151 |
+
raise FaysRuntimeError(
|
| 152 |
+
"device_setup 无法为缺少 product serial 的 Fays 生成运行资源"
|
| 153 |
+
)
|
| 154 |
+
settings = os.path.join(
|
| 155 |
+
PROJECT_ROOT,
|
| 156 |
+
"dist",
|
| 157 |
+
"fays_opencv48",
|
| 158 |
+
f"s80m_{serial}_stereo_inertial.yaml",
|
| 159 |
+
)
|
| 160 |
+
if not os.path.isfile(FAYS_MARK_ONLY_BINARY):
|
| 161 |
+
raise FaysRuntimeError(
|
| 162 |
+
f"稳定版 ORB-SLAM 程序不存在: {FAYS_MARK_ONLY_BINARY}"
|
| 163 |
+
)
|
| 164 |
+
if not os.path.isfile(settings):
|
| 165 |
+
raise FaysRuntimeError(
|
| 166 |
+
"device_setup 尚未生成当前 Fays 的 ORB YAML: "
|
| 167 |
+
f"serial={serial} path={settings}"
|
| 168 |
+
)
|
| 169 |
+
return {
|
| 170 |
+
"backend": "orb",
|
| 171 |
+
"calibration_serial": serial,
|
| 172 |
+
"executable": os.path.abspath(FAYS_MARK_ONLY_BINARY),
|
| 173 |
+
"settings": os.path.abspath(settings),
|
| 174 |
+
}
|
| 175 |
+
|
| 176 |
+
|
| 177 |
+
def _atomic_write(path, payload, *, binary=False):
|
| 178 |
+
destination = os.path.abspath(path)
|
| 179 |
+
temporary = (
|
| 180 |
+
f"{destination}.tmp.{os.getpid()}.{threading.get_ident()}"
|
| 181 |
+
)
|
| 182 |
+
mode = "wb" if binary else "w"
|
| 183 |
+
kwargs = {} if binary else {"encoding": "utf-8"}
|
| 184 |
+
try:
|
| 185 |
+
with open(temporary, mode, **kwargs) as stream:
|
| 186 |
+
stream.write(payload)
|
| 187 |
+
stream.flush()
|
| 188 |
+
os.fsync(stream.fileno())
|
| 189 |
+
os.replace(temporary, destination)
|
| 190 |
+
except OSError as exc:
|
| 191 |
+
try:
|
| 192 |
+
os.unlink(temporary)
|
| 193 |
+
except OSError:
|
| 194 |
+
pass
|
| 195 |
+
raise FaysRuntimeError(
|
| 196 |
+
f"更新 AIKit 运行配置失败: {destination}: {exc}"
|
| 197 |
+
) from exc
|
| 198 |
+
|
| 199 |
+
|
| 200 |
+
def _factory_calibration_for_serial(calibration_serial):
|
| 201 |
+
serial = str(calibration_serial or "").strip()
|
| 202 |
+
candidates = sorted(glob.glob(os.path.join(
|
| 203 |
+
AIKIT_FACTORY_CALIBRATION_DIR,
|
| 204 |
+
f"FS-VI80-*_{serial}_dump_calib.yaml",
|
| 205 |
+
)))
|
| 206 |
+
if len(candidates) != 1:
|
| 207 |
+
raise FaysRuntimeError(
|
| 208 |
+
"device_setup 未生成当前 Fays 的唯一工厂标定 YAML: "
|
| 209 |
+
f"serial={serial or '<missing>'} files="
|
| 210 |
+
+ (", ".join(candidates) if candidates else "<none>")
|
| 211 |
+
)
|
| 212 |
+
return os.path.abspath(candidates[0])
|
| 213 |
+
|
| 214 |
+
|
| 215 |
+
def prepare_aikit_service_runtime(calibration_serial, selected_ports):
|
| 216 |
+
"""Atomically select the current device for the mounted AIKit service."""
|
| 217 |
+
|
| 218 |
+
serial = str(calibration_serial or "").strip()
|
| 219 |
+
if not serial:
|
| 220 |
+
raise FaysRuntimeError("当前 Fays 缺少 product serial,不能启动 AIKit")
|
| 221 |
+
missing_ports = [
|
| 222 |
+
name for name in ("stereo_dev_port", "imu_dev_port")
|
| 223 |
+
if not str(selected_ports.get(name, "")).strip()
|
| 224 |
+
]
|
| 225 |
+
if missing_ports:
|
| 226 |
+
raise FaysRuntimeError(
|
| 227 |
+
"device_setup 设备清单缺少 AIKit 所需节点: "
|
| 228 |
+
+ ", ".join(missing_ports)
|
| 229 |
+
)
|
| 230 |
+
ports = {
|
| 231 |
+
name: f"/dev/video{video_index_from_device(selected_ports[name])}"
|
| 232 |
+
for name in ("stereo_dev_port", "imu_dev_port")
|
| 233 |
+
}
|
| 234 |
+
calibration_source = _factory_calibration_for_serial(serial)
|
| 235 |
+
try:
|
| 236 |
+
with open(AIKIT_SERVICE_DEVICE_CONFIG, encoding="utf-8") as stream:
|
| 237 |
+
driver_config = stream.read()
|
| 238 |
+
with open(calibration_source, "rb") as stream:
|
| 239 |
+
calibration_payload = stream.read()
|
| 240 |
+
except OSError as exc:
|
| 241 |
+
raise FaysRuntimeError(f"无法读取 AIKit 运行配置: {exc}") from exc
|
| 242 |
+
|
| 243 |
+
for key, value in ports.items():
|
| 244 |
+
driver_config, count = re.subn(
|
| 245 |
+
rf"^(\s*{re.escape(key)}\s*:)\s*[^\s#]+",
|
| 246 |
+
rf"\1 {value}",
|
| 247 |
+
driver_config,
|
| 248 |
+
count=1,
|
| 249 |
+
flags=re.MULTILINE,
|
| 250 |
+
)
|
| 251 |
+
if count != 1:
|
| 252 |
+
raise FaysRuntimeError(
|
| 253 |
+
f"AIKit driver 配置无法定位字段: {key}"
|
| 254 |
+
)
|
| 255 |
+
|
| 256 |
+
lock_path = os.path.join(
|
| 257 |
+
AIKIT_SERVICE_RUNTIME_ROOT, ".ksq_runtime_config.lock"
|
| 258 |
+
)
|
| 259 |
+
try:
|
| 260 |
+
with open(lock_path, "a+", encoding="utf-8") as lock_stream:
|
| 261 |
+
fcntl.flock(lock_stream.fileno(), fcntl.LOCK_EX)
|
| 262 |
+
_atomic_write(
|
| 263 |
+
AIKIT_SERVICE_CALIBRATION, calibration_payload, binary=True
|
| 264 |
+
)
|
| 265 |
+
_atomic_write(AIKIT_SERVICE_DEVICE_CONFIG, driver_config)
|
| 266 |
+
except OSError as exc:
|
| 267 |
+
raise FaysRuntimeError(f"无法锁定 AIKit 运行配置: {exc}") from exc
|
| 268 |
+
return {
|
| 269 |
+
"calibration_serial": serial,
|
| 270 |
+
"calibration_source": calibration_source,
|
| 271 |
+
"calibration_target": AIKIT_SERVICE_CALIBRATION,
|
| 272 |
+
"device_config": AIKIT_SERVICE_DEVICE_CONFIG,
|
| 273 |
+
"ports": ports,
|
| 274 |
+
}
|
| 275 |
+
|
| 276 |
+
|
| 277 |
+
def fays_aikit_profile(calibration_serial, selected_ports):
|
| 278 |
+
"""Return an AIKit profile for any device configured by device_setup."""
|
| 279 |
+
|
| 280 |
+
required = (
|
| 281 |
+
AIKIT_BRIDGE_BINARY,
|
| 282 |
+
AIKIT_CLIENT_CONFIG,
|
| 283 |
+
os.path.join(
|
| 284 |
+
AIKIT_CLIENT_LIBRARY_DIR, "libfayssense_aikit_client.so"
|
| 285 |
+
),
|
| 286 |
+
AIKIT_SERVICE_DEVICE_CONFIG,
|
| 287 |
+
)
|
| 288 |
+
missing = [path for path in required if not os.path.isfile(path)]
|
| 289 |
+
if missing:
|
| 290 |
+
raise FaysRuntimeError(
|
| 291 |
+
"AIKit 运行文件不存在: " + ", ".join(missing)
|
| 292 |
+
)
|
| 293 |
+
if not os.access(AIKIT_BRIDGE_BINARY, os.X_OK):
|
| 294 |
+
raise FaysRuntimeError(
|
| 295 |
+
f"AIKit 文件不可执行: {AIKIT_BRIDGE_BINARY}"
|
| 296 |
+
)
|
| 297 |
+
if not AIKIT_SERVICE_CONTAINER:
|
| 298 |
+
raise FaysRuntimeError("KSQ_FAYS_AIKIT_CONTAINER 不能为空")
|
| 299 |
+
|
| 300 |
+
prepare_aikit_service_runtime(calibration_serial, selected_ports)
|
| 301 |
+
return {
|
| 302 |
+
"backend": "aikit",
|
| 303 |
+
"calibration_serial": str(calibration_serial).strip(),
|
| 304 |
+
"executable": AIKIT_BRIDGE_BINARY,
|
| 305 |
+
"settings": AIKIT_CLIENT_CONFIG,
|
| 306 |
+
"arguments": (AIKIT_CLIENT_CONFIG,),
|
| 307 |
+
"service_container": AIKIT_SERVICE_CONTAINER,
|
| 308 |
+
"service_host": AIKIT_SERVICE_HOST,
|
| 309 |
+
"service_port": AIKIT_SERVICE_PORT,
|
| 310 |
+
}
|
| 311 |
+
|
| 312 |
+
|
| 313 |
+
def video_index_from_device(device_path):
|
| 314 |
+
"""把严格的 ``/dev/videoN`` 路径转换为 OpenCV 索引 N。"""
|
| 315 |
+
match = re.fullmatch(r"/dev/video(\d+)", str(device_path).strip())
|
| 316 |
+
if not match:
|
| 317 |
+
raise FaysRuntimeError(f"不是有效的 V4L2 视频节点: {device_path!r}")
|
| 318 |
+
return int(match.group(1))
|
| 319 |
+
|
| 320 |
+
|
| 321 |
+
def inspect_video_node(device_path):
|
| 322 |
+
"""只读检查 V4L2 节点、USB interface 编号和 FT602 身份。"""
|
| 323 |
+
index = video_index_from_device(device_path)
|
| 324 |
+
class_dir = f"/sys/class/video4linux/video{index}"
|
| 325 |
+
info = {
|
| 326 |
+
"device": device_path,
|
| 327 |
+
"exists": os.path.exists(device_path),
|
| 328 |
+
"name": "?",
|
| 329 |
+
"stream_index": None,
|
| 330 |
+
"interface_number": None,
|
| 331 |
+
"modalias": "",
|
| 332 |
+
"vendor_id": "",
|
| 333 |
+
"product_id": "",
|
| 334 |
+
"is_fays_ft602": False,
|
| 335 |
+
"physical_usb_path": None,
|
| 336 |
+
"usb_device_sysfs_path": None,
|
| 337 |
+
}
|
| 338 |
+
|
| 339 |
+
fields = {
|
| 340 |
+
"name": os.path.join(class_dir, "name"),
|
| 341 |
+
"stream_index": os.path.join(class_dir, "index"),
|
| 342 |
+
"interface_number": os.path.join(class_dir, "device", "bInterfaceNumber"),
|
| 343 |
+
"modalias": os.path.join(class_dir, "device", "modalias"),
|
| 344 |
+
}
|
| 345 |
+
for key, path in fields.items():
|
| 346 |
+
try:
|
| 347 |
+
with open(path, encoding="utf-8") as stream:
|
| 348 |
+
value = stream.read().strip()
|
| 349 |
+
info[key] = int(value) if key == "stream_index" else value
|
| 350 |
+
except (OSError, ValueError):
|
| 351 |
+
pass
|
| 352 |
+
|
| 353 |
+
name = str(info["name"]).lower()
|
| 354 |
+
modalias = str(info["modalias"]).lower()
|
| 355 |
+
info["is_fays_ft602"] = (
|
| 356 |
+
"ftdi" in name
|
| 357 |
+
and "superspeed video bridge" in name
|
| 358 |
+
and "v0403p602e" in modalias
|
| 359 |
+
)
|
| 360 |
+
try:
|
| 361 |
+
interface_path = os.path.realpath(os.path.join(class_dir, "device"))
|
| 362 |
+
physical_path = os.path.dirname(interface_path)
|
| 363 |
+
with open(
|
| 364 |
+
os.path.join(physical_path, "idVendor"), encoding="utf-8"
|
| 365 |
+
) as stream:
|
| 366 |
+
info["vendor_id"] = stream.read().strip().lower()
|
| 367 |
+
with open(
|
| 368 |
+
os.path.join(physical_path, "idProduct"), encoding="utf-8"
|
| 369 |
+
) as stream:
|
| 370 |
+
info["product_id"] = stream.read().strip().lower()
|
| 371 |
+
info["physical_usb_path"] = os.path.basename(
|
| 372 |
+
interface_path
|
| 373 |
+
).split(":", 1)[0]
|
| 374 |
+
info["usb_device_sysfs_path"] = physical_path
|
| 375 |
+
except OSError:
|
| 376 |
+
pass
|
| 377 |
+
info["is_fays_ft602"] = (
|
| 378 |
+
info["vendor_id"] == FAYS_VENDOR_ID
|
| 379 |
+
and info["product_id"] == FAYS_PRODUCT_ID
|
| 380 |
+
and str(info["interface_number"]) in {"00", "02"}
|
| 381 |
+
)
|
| 382 |
+
return info
|
| 383 |
+
|
| 384 |
+
|
| 385 |
+
def inspect_tty_device(device_path):
|
| 386 |
+
"""Resolve one selected ESP32 tty to stable read-only USB identity."""
|
| 387 |
+
basename = os.path.basename(str(device_path).strip())
|
| 388 |
+
if not re.fullmatch(r"ttyACM\d+", basename):
|
| 389 |
+
raise FaysRuntimeError(f"不是有效的 ESP32 串口节点: {device_path!r}")
|
| 390 |
+
interface_path = os.path.realpath(
|
| 391 |
+
os.path.join("/sys/class/tty", basename, "device")
|
| 392 |
+
)
|
| 393 |
+
interface_name = os.path.basename(interface_path)
|
| 394 |
+
if (
|
| 395 |
+
not interface_path.startswith("/sys/devices/")
|
| 396 |
+
or ":" not in interface_name
|
| 397 |
+
):
|
| 398 |
+
raise FaysRuntimeError(
|
| 399 |
+
f"无法解析 ESP32 串口 USB 身份: {device_path}"
|
| 400 |
+
)
|
| 401 |
+
physical_usb_path = interface_name.split(":", 1)[0]
|
| 402 |
+
if not _USB_PHYSICAL_PATTERN.fullmatch(physical_usb_path):
|
| 403 |
+
raise FaysRuntimeError(
|
| 404 |
+
f"ESP32 串口物理路径无效: {physical_usb_path!r}"
|
| 405 |
+
)
|
| 406 |
+
physical_path = os.path.dirname(interface_path)
|
| 407 |
+
|
| 408 |
+
def read_attribute(name):
|
| 409 |
+
try:
|
| 410 |
+
with open(
|
| 411 |
+
os.path.join(physical_path, name), encoding="utf-8"
|
| 412 |
+
) as stream:
|
| 413 |
+
return stream.read().strip()
|
| 414 |
+
except OSError as exc:
|
| 415 |
+
raise FaysRuntimeError(
|
| 416 |
+
f"无法读取 ESP32 {name}: {device_path}: {exc}"
|
| 417 |
+
) from exc
|
| 418 |
+
|
| 419 |
+
root_match = re.match(r"(\d+-\d+)", physical_usb_path)
|
| 420 |
+
return {
|
| 421 |
+
"device": f"/dev/{basename}",
|
| 422 |
+
"physical_usb_path": physical_usb_path,
|
| 423 |
+
"root_hub": root_match.group(1),
|
| 424 |
+
"vendor_id": read_attribute("idVendor").lower(),
|
| 425 |
+
"product_id": read_attribute("idProduct").lower(),
|
| 426 |
+
"serial": read_attribute("serial"),
|
| 427 |
+
}
|
| 428 |
+
|
| 429 |
+
|
| 430 |
+
def discover_esp32_devices():
|
| 431 |
+
"""只读枚举当前在线的 ESP32 控制板节点。
|
| 432 |
+
|
| 433 |
+
``/dev/ttyACM*`` 是本次枚举得到的路径,VID/PID 和 USB serial 才是
|
| 434 |
+
设备身份。这里沿用 ``inspect_tty_device`` 的 sysfs 读取路径,不打开串口,
|
| 435 |
+
不访问 USB interface 目录,也不修改硬件状态。
|
| 436 |
+
"""
|
| 437 |
+
devices = []
|
| 438 |
+
for class_path in sorted(
|
| 439 |
+
glob.glob("/sys/class/tty/ttyACM*"),
|
| 440 |
+
key=lambda path: tuple(
|
| 441 |
+
int(part) if part.isdigit() else part
|
| 442 |
+
for part in re.split(r"([0-9]+)", str(path))
|
| 443 |
+
if part
|
| 444 |
+
),
|
| 445 |
+
):
|
| 446 |
+
basename = os.path.basename(class_path)
|
| 447 |
+
device_path = f"/dev/{basename}"
|
| 448 |
+
try:
|
| 449 |
+
info = inspect_tty_device(device_path)
|
| 450 |
+
except FaysRuntimeError:
|
| 451 |
+
continue
|
| 452 |
+
if (
|
| 453 |
+
info["vendor_id"] == ESP32_VENDOR_ID
|
| 454 |
+
and info["product_id"] == ESP32_PRODUCT_ID
|
| 455 |
+
and info.get("serial")
|
| 456 |
+
):
|
| 457 |
+
devices.append(info)
|
| 458 |
+
return tuple(devices)
|
| 459 |
+
|
| 460 |
+
|
| 461 |
+
def read_fays_usb_speed_mbps(
|
| 462 |
+
usb_device_sysfs_path,
|
| 463 |
+
physical_usb_path,
|
| 464 |
+
):
|
| 465 |
+
"""从 USB 设备目录只读获取 Fays 当前协商链路速度。"""
|
| 466 |
+
physical = str(physical_usb_path or "").strip()
|
| 467 |
+
if not re.fullmatch(r"\d+-\d+(?:\.\d+)*", physical):
|
| 468 |
+
raise FaysRuntimeError(
|
| 469 |
+
f"Fays 物理 USB 路径无效: {physical_usb_path!r}"
|
| 470 |
+
)
|
| 471 |
+
device_path = os.path.realpath(
|
| 472 |
+
str(usb_device_sysfs_path or "").strip()
|
| 473 |
+
)
|
| 474 |
+
if (
|
| 475 |
+
not device_path.startswith("/sys/devices/")
|
| 476 |
+
or os.path.basename(device_path) != physical
|
| 477 |
+
or ":" in os.path.basename(device_path)
|
| 478 |
+
):
|
| 479 |
+
raise FaysRuntimeError(
|
| 480 |
+
"Fays USB 设备 sysfs 路径无效: "
|
| 481 |
+
f"{usb_device_sysfs_path!r}"
|
| 482 |
+
)
|
| 483 |
+
# ``inspect_video_node`` 已将 video class symlink 解析到无冒号的
|
| 484 |
+
# 物理 device 父目录。这里只读该目录的 speed,不访问 ``*:*``
|
| 485 |
+
# interface 属性,也不写 sysfs、不复位或重绑驱动。
|
| 486 |
+
speed_path = os.path.join(device_path, "speed")
|
| 487 |
+
try:
|
| 488 |
+
with open(speed_path, encoding="utf-8") as stream:
|
| 489 |
+
speed = float(stream.read().strip())
|
| 490 |
+
except (OSError, ValueError) as exc:
|
| 491 |
+
raise FaysRuntimeError(
|
| 492 |
+
f"无法读取 Fays USB 链路速度: {speed_path}: {exc}"
|
| 493 |
+
) from exc
|
| 494 |
+
if speed <= 0:
|
| 495 |
+
raise FaysRuntimeError(
|
| 496 |
+
f"Fays USB 链路速度无效: {speed:g}M ({physical})"
|
| 497 |
+
)
|
| 498 |
+
return speed
|
| 499 |
+
|
| 500 |
+
|
| 501 |
+
def validate_fays_superspeed(devices):
|
| 502 |
+
"""拒绝 FT602 降级到 USB 2.0,返回当前链路速度(Mb/s)。"""
|
| 503 |
+
physical_paths = [
|
| 504 |
+
devices.get(role, {}).get("physical_usb_path")
|
| 505 |
+
for role in ("stereo", "imu")
|
| 506 |
+
]
|
| 507 |
+
device_paths = [
|
| 508 |
+
devices.get(role, {}).get("usb_device_sysfs_path")
|
| 509 |
+
for role in ("stereo", "imu")
|
| 510 |
+
]
|
| 511 |
+
if (
|
| 512 |
+
any(not path for path in physical_paths)
|
| 513 |
+
or len(set(physical_paths)) != 1
|
| 514 |
+
or any(not path for path in device_paths)
|
| 515 |
+
or len(set(map(os.path.realpath, device_paths))) != 1
|
| 516 |
+
):
|
| 517 |
+
raise FaysRuntimeError(
|
| 518 |
+
"无法确认 Fays stereo/IMU 的唯一物理 USB 设备"
|
| 519 |
+
)
|
| 520 |
+
physical = physical_paths[0]
|
| 521 |
+
speed = read_fays_usb_speed_mbps(device_paths[0], physical)
|
| 522 |
+
if speed < FAYS_MIN_USB_SPEED_MBPS:
|
| 523 |
+
raise FaysRuntimeError(
|
| 524 |
+
"Fays USB 链路不是 SuperSpeed: "
|
| 525 |
+
f"physical={physical} speed={speed:g}M,"
|
| 526 |
+
f"要求至少 {FAYS_MIN_USB_SPEED_MBPS:g}M;"
|
| 527 |
+
"请将 Fays 直连 USB 3.x 端口"
|
| 528 |
+
)
|
| 529 |
+
return speed
|
| 530 |
+
|
| 531 |
+
|
| 532 |
+
def fays_usb_control_node(devices):
|
| 533 |
+
"""Resolve the FT602 USB device node used by the D3XX control path."""
|
| 534 |
+
physical_paths = [
|
| 535 |
+
devices.get(role, {}).get("physical_usb_path")
|
| 536 |
+
for role in ("stereo", "imu")
|
| 537 |
+
]
|
| 538 |
+
device_paths = [
|
| 539 |
+
devices.get(role, {}).get("usb_device_sysfs_path")
|
| 540 |
+
for role in ("stereo", "imu")
|
| 541 |
+
]
|
| 542 |
+
if (
|
| 543 |
+
any(not path for path in physical_paths)
|
| 544 |
+
or len(set(physical_paths)) != 1
|
| 545 |
+
or any(not path for path in device_paths)
|
| 546 |
+
or len(set(map(os.path.realpath, device_paths))) != 1
|
| 547 |
+
):
|
| 548 |
+
raise FaysRuntimeError(
|
| 549 |
+
"无法确认 Fays stereo/IMU 的唯一物理 USB 设备"
|
| 550 |
+
)
|
| 551 |
+
|
| 552 |
+
physical = str(physical_paths[0])
|
| 553 |
+
device_path = os.path.realpath(str(device_paths[0]))
|
| 554 |
+
if (
|
| 555 |
+
not _USB_PHYSICAL_PATTERN.fullmatch(physical)
|
| 556 |
+
or not device_path.startswith("/sys/devices/")
|
| 557 |
+
or os.path.basename(device_path) != physical
|
| 558 |
+
or ":" in os.path.basename(device_path)
|
| 559 |
+
):
|
| 560 |
+
raise FaysRuntimeError(
|
| 561 |
+
f"Fays USB 设备 sysfs 路径无效: {device_paths[0]!r}"
|
| 562 |
+
)
|
| 563 |
+
|
| 564 |
+
values = {}
|
| 565 |
+
for attribute in ("busnum", "devnum"):
|
| 566 |
+
path = os.path.join(device_path, attribute)
|
| 567 |
+
try:
|
| 568 |
+
with open(path, encoding="utf-8") as stream:
|
| 569 |
+
values[attribute] = int(stream.read().strip())
|
| 570 |
+
except (OSError, ValueError) as exc:
|
| 571 |
+
raise FaysRuntimeError(
|
| 572 |
+
f"无法读取 Fays USB {attribute}: {path}: {exc}"
|
| 573 |
+
) from exc
|
| 574 |
+
if not 0 < values[attribute] <= 999:
|
| 575 |
+
raise FaysRuntimeError(
|
| 576 |
+
f"Fays USB {attribute} 无效: {values[attribute]!r}"
|
| 577 |
+
)
|
| 578 |
+
return (
|
| 579 |
+
f"/dev/bus/usb/{values['busnum']:03d}/"
|
| 580 |
+
f"{values['devnum']:03d}"
|
| 581 |
+
)
|
| 582 |
+
|
| 583 |
+
|
| 584 |
+
def validate_fays_control_access(devices):
|
| 585 |
+
"""Require read/write access to the FT602 D3XX register-control node."""
|
| 586 |
+
node = fays_usb_control_node(devices)
|
| 587 |
+
if os.path.exists(node) and os.access(node, os.R_OK | os.W_OK):
|
| 588 |
+
return node
|
| 589 |
+
|
| 590 |
+
details = "missing"
|
| 591 |
+
try:
|
| 592 |
+
metadata = os.stat(node)
|
| 593 |
+
details = (
|
| 594 |
+
f"mode={metadata.st_mode & 0o777:04o} "
|
| 595 |
+
f"uid={metadata.st_uid} gid={metadata.st_gid}"
|
| 596 |
+
)
|
| 597 |
+
except OSError:
|
| 598 |
+
pass
|
| 599 |
+
installer = os.path.join(
|
| 600 |
+
PROJECT_ROOT, "env_init", "install_fays_permissions.sh"
|
| 601 |
+
)
|
| 602 |
+
raise FaysRuntimeError(
|
| 603 |
+
"Fays FT602 D3XX 控制节点不可读写: "
|
| 604 |
+
f"{node} ({details});双目/IMU 可能仍能取流,但曝光、增益和旋转"
|
| 605 |
+
"写入会报 FT_Create failed。请先执行 "
|
| 606 |
+
f"sudo {installer},然后重新 Connect"
|
| 607 |
+
)
|
| 608 |
+
|
| 609 |
+
|
| 610 |
+
def validate_fays_sdk_access(devices):
|
| 611 |
+
"""Validate both the SuperSpeed stream and writable D3XX control path."""
|
| 612 |
+
speed = validate_fays_superspeed(devices)
|
| 613 |
+
validate_fays_control_access(devices)
|
| 614 |
+
return speed
|
| 615 |
+
|
| 616 |
+
|
| 617 |
+
def discover_fays_device_groups():
|
| 618 |
+
"""只读发现全部完整 S80M,并按物理 USB 路径分组。"""
|
| 619 |
+
groups = {}
|
| 620 |
+
for class_path in sorted(glob.glob("/sys/class/video4linux/video*")):
|
| 621 |
+
basename = os.path.basename(class_path)
|
| 622 |
+
if not re.fullmatch(r"video\d+", basename):
|
| 623 |
+
continue
|
| 624 |
+
info = inspect_video_node(f"/dev/{basename}")
|
| 625 |
+
if not info["is_fays_ft602"] or info["stream_index"] != 0:
|
| 626 |
+
continue
|
| 627 |
+
physical = info.get("physical_usb_path")
|
| 628 |
+
interface = info.get("interface_number")
|
| 629 |
+
if physical and interface in {"00", "02"}:
|
| 630 |
+
groups.setdefault(physical, {}).setdefault(interface, []).append(info)
|
| 631 |
+
|
| 632 |
+
complete = []
|
| 633 |
+
for physical, interfaces in groups.items():
|
| 634 |
+
if len(interfaces.get("00", [])) == 1 and len(interfaces.get("02", [])) == 1:
|
| 635 |
+
stereo, imu = interfaces["00"][0], interfaces["02"][0]
|
| 636 |
+
complete.append({
|
| 637 |
+
"physical_usb_path": physical,
|
| 638 |
+
"ports": {
|
| 639 |
+
"stereo_dev_port": stereo["device"],
|
| 640 |
+
"imu_dev_port": imu["device"],
|
| 641 |
+
},
|
| 642 |
+
"stereo": stereo,
|
| 643 |
+
"imu": imu,
|
| 644 |
+
"config_updated": False,
|
| 645 |
+
})
|
| 646 |
+
return tuple(sorted(
|
| 647 |
+
complete,
|
| 648 |
+
key=lambda item: tuple(
|
| 649 |
+
int(part) if part.isdigit() else part
|
| 650 |
+
for part in re.split(
|
| 651 |
+
r"([0-9]+)", item["physical_usb_path"]
|
| 652 |
+
)
|
| 653 |
+
if part
|
| 654 |
+
),
|
| 655 |
+
))
|
| 656 |
+
|
| 657 |
+
|
| 658 |
+
def read_fays_config_ports(config_path):
|
| 659 |
+
"""Read configured access nodes; never infer a product identity from them."""
|
| 660 |
+
with open(config_path, encoding="utf-8") as stream:
|
| 661 |
+
text = stream.read()
|
| 662 |
+
ports = {}
|
| 663 |
+
for key in ("stereo_dev_port", "imu_dev_port"):
|
| 664 |
+
match = re.search(r"^\s*" + key + r"\s*:\s*['\"]?(/dev/video[0-9]+)(?=['\"\s#]|$)",
|
| 665 |
+
text, re.MULTILINE)
|
| 666 |
+
if match is None:
|
| 667 |
+
raise FaysRuntimeError(f"Fays SDK 配置缺少有效 {key}: {config_path}")
|
| 668 |
+
ports[key] = match.group(1)
|
| 669 |
+
return ports
|
| 670 |
+
|
| 671 |
+
|
| 672 |
+
def materialize_fays_device_config(
|
| 673 |
+
ports,
|
| 674 |
+
destination,
|
| 675 |
+
template_path,
|
| 676 |
+
):
|
| 677 |
+
"""从只读模板原子生成一个实例专用 SDK 配置。"""
|
| 678 |
+
try:
|
| 679 |
+
with open(template_path, encoding="utf-8") as stream:
|
| 680 |
+
content = stream.read()
|
| 681 |
+
except OSError as exc:
|
| 682 |
+
raise FaysRuntimeError(
|
| 683 |
+
f"无法读取 Fays 配置模板: {template_path}: {exc}"
|
| 684 |
+
) from exc
|
| 685 |
+
replacements = {
|
| 686 |
+
"stereo_dev_port": (
|
| 687 |
+
f"/dev/video{video_index_from_device(ports['stereo_dev_port'])}"
|
| 688 |
+
),
|
| 689 |
+
"imu_dev_port": (
|
| 690 |
+
f"/dev/video{video_index_from_device(ports['imu_dev_port'])}"
|
| 691 |
+
),
|
| 692 |
+
}
|
| 693 |
+
for key, value in replacements.items():
|
| 694 |
+
content, count = re.subn(
|
| 695 |
+
rf"^(\s*{re.escape(key)}\s*:)\s*[^\s#]+",
|
| 696 |
+
rf"\1 {value}",
|
| 697 |
+
content,
|
| 698 |
+
count=1,
|
| 699 |
+
flags=re.MULTILINE,
|
| 700 |
+
)
|
| 701 |
+
if count != 1:
|
| 702 |
+
raise FaysRuntimeError(f"Fays 配置模板无法定位字段: {key}")
|
| 703 |
+
destination = os.path.abspath(destination)
|
| 704 |
+
os.makedirs(os.path.dirname(destination), exist_ok=True)
|
| 705 |
+
temporary = f"{destination}.tmp.{os.getpid()}"
|
| 706 |
+
try:
|
| 707 |
+
with open(temporary, "w", encoding="utf-8") as stream:
|
| 708 |
+
stream.write(content)
|
| 709 |
+
stream.flush()
|
| 710 |
+
os.fsync(stream.fileno())
|
| 711 |
+
os.chmod(temporary, 0o600)
|
| 712 |
+
os.replace(temporary, destination)
|
| 713 |
+
except OSError as exc:
|
| 714 |
+
try:
|
| 715 |
+
os.unlink(temporary)
|
| 716 |
+
except OSError:
|
| 717 |
+
pass
|
| 718 |
+
raise FaysRuntimeError(
|
| 719 |
+
f"生成 Fays 实例配置失败: {destination}: {exc}"
|
| 720 |
+
) from exc
|
| 721 |
+
return destination
|
| 722 |
+
|
| 723 |
+
|
| 724 |
+
def discover_fays_manifest_device_groups():
|
| 725 |
+
"""Discover Fays groups only after device_setup manifest resolution."""
|
| 726 |
+
from fays_serial_probe import discover_fays_manifest_groups
|
| 727 |
+
|
| 728 |
+
return discover_fays_manifest_groups()
|
| 729 |
+
|
| 730 |
+
|
| 731 |
+
class FaysInstanceManager:
|
| 732 |
+
"""Own one manifest-selected Fays lease and all per-GUI runtime paths."""
|
| 733 |
+
|
| 734 |
+
def __init__(
|
| 735 |
+
self,
|
| 736 |
+
runtime_dir,
|
| 737 |
+
*,
|
| 738 |
+
run_id=None,
|
| 739 |
+
lock_dir="/tmp/ksq-gripper-fays-locks",
|
| 740 |
+
ipc_root="/dev/shm",
|
| 741 |
+
discover_devices=None,
|
| 742 |
+
discover_selected_devices=None,
|
| 743 |
+
inspect_tty=inspect_tty_device,
|
| 744 |
+
validate_link=validate_fays_sdk_access,
|
| 745 |
+
logger=print,
|
| 746 |
+
pid=None,
|
| 747 |
+
):
|
| 748 |
+
self.runtime_dir = os.path.abspath(runtime_dir)
|
| 749 |
+
self.run_id = str(
|
| 750 |
+
run_id or os.path.basename(self.runtime_dir)
|
| 751 |
+
)
|
| 752 |
+
self.pid = int(os.getpid() if pid is None else pid)
|
| 753 |
+
rendered = re.sub(
|
| 754 |
+
r"[^A-Za-z0-9_.-]+", "-", self.run_id
|
| 755 |
+
).strip("-") or "run"
|
| 756 |
+
self.instance_id = f"{rendered}-{self.pid}"
|
| 757 |
+
self.manifest_path = os.path.abspath(DEVICE_MANIFEST)
|
| 758 |
+
self.lock_dir = os.path.abspath(lock_dir)
|
| 759 |
+
self.ipc_dir = os.path.join(
|
| 760 |
+
os.path.abspath(ipc_root),
|
| 761 |
+
f"ksq-gripper-{self.instance_id}",
|
| 762 |
+
)
|
| 763 |
+
self.device_config_path = os.path.join(
|
| 764 |
+
self.runtime_dir, "fays_vikit_runtime.yaml"
|
| 765 |
+
)
|
| 766 |
+
self.trajectory_path = os.path.join(
|
| 767 |
+
self.runtime_dir, "orb_trajectory.txt"
|
| 768 |
+
)
|
| 769 |
+
self.assignment_path = os.path.join(
|
| 770 |
+
self.runtime_dir, "hardware_assignment.json"
|
| 771 |
+
)
|
| 772 |
+
self.event_log_path = os.path.join(
|
| 773 |
+
self.runtime_dir, "fays_instance.log"
|
| 774 |
+
)
|
| 775 |
+
self.native_log_path = os.path.join(
|
| 776 |
+
self.runtime_dir, "fays_native.log"
|
| 777 |
+
)
|
| 778 |
+
self._discover_selected_devices = discover_selected_devices
|
| 779 |
+
self._discover_devices = (
|
| 780 |
+
discover_devices or discover_fays_manifest_device_groups
|
| 781 |
+
)
|
| 782 |
+
self._inspect_tty = inspect_tty
|
| 783 |
+
self._validate_link = validate_link
|
| 784 |
+
self._logger = logger
|
| 785 |
+
self._lock = threading.RLock()
|
| 786 |
+
self._lock_stream = None
|
| 787 |
+
self._selected = None
|
| 788 |
+
self._assignment = None
|
| 789 |
+
self._tty = None
|
| 790 |
+
os.makedirs(self.runtime_dir, exist_ok=True)
|
| 791 |
+
os.makedirs(self.lock_dir, mode=0o700, exist_ok=True)
|
| 792 |
+
os.makedirs(self.ipc_dir, mode=0o700, exist_ok=True)
|
| 793 |
+
self._clear_ipc_files()
|
| 794 |
+
self._event("instance_created")
|
| 795 |
+
|
| 796 |
+
def ipc_path(self, leaf):
|
| 797 |
+
if str(leaf) not in _INSTANCE_IPC_FILES:
|
| 798 |
+
raise ValueError(f"unknown Fays IPC leaf: {leaf!r}")
|
| 799 |
+
return os.path.join(self.ipc_dir, str(leaf))
|
| 800 |
+
|
| 801 |
+
@property
|
| 802 |
+
def pose_paths(self):
|
| 803 |
+
return (
|
| 804 |
+
self.ipc_path("orb_pose.json.tmp"),
|
| 805 |
+
self.ipc_path("orb_pose.json"),
|
| 806 |
+
)
|
| 807 |
+
|
| 808 |
+
@property
|
| 809 |
+
def frame_reader_paths(self):
|
| 810 |
+
return {
|
| 811 |
+
"meta_path": self.ipc_path("orb_meta.json"),
|
| 812 |
+
"current_frame_path": self.ipc_path(
|
| 813 |
+
"orb_current_frame.jpg"
|
| 814 |
+
),
|
| 815 |
+
}
|
| 816 |
+
|
| 817 |
+
@property
|
| 818 |
+
def control_paths(self):
|
| 819 |
+
return (
|
| 820 |
+
self.ipc_path("orb_control.json.tmp"),
|
| 821 |
+
self.ipc_path("orb_control.json"),
|
| 822 |
+
)
|
| 823 |
+
|
| 824 |
+
@property
|
| 825 |
+
def raw_stream_socket_path(self):
|
| 826 |
+
return self.ipc_path("orb_raw_stream.sock")
|
| 827 |
+
|
| 828 |
+
@property
|
| 829 |
+
def calibration_yaml_paths(self):
|
| 830 |
+
return (
|
| 831 |
+
self.ipc_path("camera_calibration.yaml"),
|
| 832 |
+
self.ipc_path("imu.yaml"),
|
| 833 |
+
)
|
| 834 |
+
|
| 835 |
+
def runtime_environment(self, base=None):
|
| 836 |
+
environment = dict(base or {})
|
| 837 |
+
environment.update({
|
| 838 |
+
"KSQ_FAYS_IPC_DIR": self.ipc_dir,
|
| 839 |
+
"KSQ_FAYS_INSTANCE_ID": self.instance_id,
|
| 840 |
+
})
|
| 841 |
+
assignment = self.snapshot()
|
| 842 |
+
if assignment:
|
| 843 |
+
calibration_serial = assignment.get("fays", {}).get(
|
| 844 |
+
"calibration_serial"
|
| 845 |
+
)
|
| 846 |
+
if calibration_serial:
|
| 847 |
+
environment["KSQ_FAYS_CALIBRATION_SERIAL"] = str(
|
| 848 |
+
calibration_serial
|
| 849 |
+
)
|
| 850 |
+
return environment
|
| 851 |
+
|
| 852 |
+
def record_camera_set(self, topology):
|
| 853 |
+
"""Append the selected USB2 camera topology to this instance log."""
|
| 854 |
+
if topology is None:
|
| 855 |
+
return False
|
| 856 |
+
roles = {}
|
| 857 |
+
try:
|
| 858 |
+
for role in ("decxin", "left", "right"):
|
| 859 |
+
node = topology.node(role)
|
| 860 |
+
roles[role] = {
|
| 861 |
+
"device_path": node.device_path,
|
| 862 |
+
"physical_usb_path": node.physical_usb_path,
|
| 863 |
+
"vendor_id": node.vendor_id,
|
| 864 |
+
"product_id": node.product_id,
|
| 865 |
+
"serial": node.serial,
|
| 866 |
+
}
|
| 867 |
+
camera_set = {
|
| 868 |
+
"root_hub": topology.root_hub,
|
| 869 |
+
"tactile_hub": topology.tactile_hub,
|
| 870 |
+
"roles": roles,
|
| 871 |
+
}
|
| 872 |
+
except (AttributeError, KeyError) as exc:
|
| 873 |
+
raise FaysRuntimeError(
|
| 874 |
+
f"无法记录 USB2 相机套件拓扑: {exc}"
|
| 875 |
+
) from exc
|
| 876 |
+
with self._lock:
|
| 877 |
+
if (
|
| 878 |
+
self._assignment is None
|
| 879 |
+
or self._assignment.get("status") != "acquired"
|
| 880 |
+
or self._selected is None
|
| 881 |
+
or self._lock_stream is None
|
| 882 |
+
or self._lock_stream.closed
|
| 883 |
+
):
|
| 884 |
+
return False
|
| 885 |
+
expected_root = (
|
| 886 |
+
self._tty.get("root_hub") if self._tty else None
|
| 887 |
+
)
|
| 888 |
+
# The schema-v2 selector already resolved the camera set from the
|
| 889 |
+
# ESP32 serial and the relative H0/H1 topology. ``root_hub`` is
|
| 890 |
+
# diagnostic only for this path, because a nested USB3 hub can
|
| 891 |
+
# legitimately put the cameras one level below the ESP root hub
|
| 892 |
+
# (e.g. ESP root 1-5 vs camera H0 1-5.2).
|
| 893 |
+
actual_root = str(camera_set["root_hub"] or "")
|
| 894 |
+
same_usb_branch = bool(
|
| 895 |
+
actual_root
|
| 896 |
+
and expected_root
|
| 897 |
+
and (
|
| 898 |
+
actual_root == expected_root
|
| 899 |
+
or actual_root.startswith(f"{expected_root}.")
|
| 900 |
+
)
|
| 901 |
+
)
|
| 902 |
+
if expected_root and not same_usb_branch:
|
| 903 |
+
raise FaysRuntimeError(
|
| 904 |
+
"USB2 相机套件不属于当前 ESP32: "
|
| 905 |
+
f"expected={expected_root} "
|
| 906 |
+
f"actual={actual_root}"
|
| 907 |
+
)
|
| 908 |
+
self._assignment["usb2_camera_set"] = camera_set
|
| 909 |
+
self._write_assignment()
|
| 910 |
+
self._event("camera_set_selected", camera_set=camera_set)
|
| 911 |
+
return True
|
| 912 |
+
|
| 913 |
+
def set_preferred_tty(self, device_path):
|
| 914 |
+
with self._lock:
|
| 915 |
+
rendered = str(device_path or "").strip()
|
| 916 |
+
candidate = self._inspect_tty(rendered) if rendered else None
|
| 917 |
+
if self.is_acquired():
|
| 918 |
+
previous = self._tty
|
| 919 |
+
self._tty = candidate
|
| 920 |
+
try:
|
| 921 |
+
group, _unit, _manifest = self._resolve_assignment()
|
| 922 |
+
except Exception:
|
| 923 |
+
self._tty = previous
|
| 924 |
+
raise
|
| 925 |
+
if (
|
| 926 |
+
group["physical_usb_path"]
|
| 927 |
+
!= self._selected["physical_usb_path"]
|
| 928 |
+
):
|
| 929 |
+
self._tty = previous
|
| 930 |
+
raise FaysRuntimeError(
|
| 931 |
+
"当前 Fays 正在使用,不能切换到另一套 ESP32;"
|
| 932 |
+
"请先 Disconnect"
|
| 933 |
+
)
|
| 934 |
+
else:
|
| 935 |
+
self._tty = candidate
|
| 936 |
+
self._event(
|
| 937 |
+
"control_port_selected",
|
| 938 |
+
tty=self._tty,
|
| 939 |
+
)
|
| 940 |
+
return dict(self._tty) if self._tty else None
|
| 941 |
+
|
| 942 |
+
def verify_preferred_tty(self, device_path):
|
| 943 |
+
"""Recheck that a tty node still names the preflight ESP32."""
|
| 944 |
+
with self._lock:
|
| 945 |
+
if self._tty is None:
|
| 946 |
+
raise FaysRuntimeError("尚未选择 ESP32 控制串口")
|
| 947 |
+
current = self._inspect_tty(device_path)
|
| 948 |
+
fields = (
|
| 949 |
+
"vendor_id",
|
| 950 |
+
"product_id",
|
| 951 |
+
"serial",
|
| 952 |
+
"physical_usb_path",
|
| 953 |
+
"root_hub",
|
| 954 |
+
)
|
| 955 |
+
differences = [
|
| 956 |
+
name for name in fields
|
| 957 |
+
if current.get(name) != self._tty.get(name)
|
| 958 |
+
]
|
| 959 |
+
if differences:
|
| 960 |
+
self._event(
|
| 961 |
+
"control_port_identity_changed",
|
| 962 |
+
expected=self._tty,
|
| 963 |
+
actual=current,
|
| 964 |
+
differences=differences,
|
| 965 |
+
)
|
| 966 |
+
raise FaysRuntimeError(
|
| 967 |
+
"串口在 Connect 交接期间指向了另一设备: "
|
| 968 |
+
+ ", ".join(differences)
|
| 969 |
+
)
|
| 970 |
+
return dict(current)
|
| 971 |
+
|
| 972 |
+
def is_acquired(self):
|
| 973 |
+
with self._lock:
|
| 974 |
+
return bool(
|
| 975 |
+
self._selected is not None
|
| 976 |
+
and self._lock_stream is not None
|
| 977 |
+
and not self._lock_stream.closed
|
| 978 |
+
and self._assignment is not None
|
| 979 |
+
and self._assignment.get("status") == "acquired"
|
| 980 |
+
)
|
| 981 |
+
|
| 982 |
+
def selected_ports(self):
|
| 983 |
+
with self._lock:
|
| 984 |
+
if self._selected is None:
|
| 985 |
+
raise FaysRuntimeError("当前界面尚未分配 Fays 设备")
|
| 986 |
+
return dict(self._selected["ports"])
|
| 987 |
+
|
| 988 |
+
def slam_profile(self, backend="orb"):
|
| 989 |
+
"""Return the selected localization backend's runtime profile."""
|
| 990 |
+
|
| 991 |
+
with self._lock:
|
| 992 |
+
if self._selected is None:
|
| 993 |
+
raise FaysRuntimeError("当前界面尚未分配 Fays 设备")
|
| 994 |
+
selected = dict(self._selected)
|
| 995 |
+
calibration_serial = selected.get("calibration_serial")
|
| 996 |
+
normalized_backend = str(backend or "orb").strip().lower()
|
| 997 |
+
if normalized_backend == "orb":
|
| 998 |
+
serial = str(calibration_serial or "").strip()
|
| 999 |
+
if not serial:
|
| 1000 |
+
raise FaysRuntimeError(
|
| 1001 |
+
"设备清单缺少当前 Fays 标定序列号,拒绝启动 ORB-SLAM;"
|
| 1002 |
+
"请先运行 device_setup"
|
| 1003 |
+
)
|
| 1004 |
+
sdk_yaml = os.path.abspath(
|
| 1005 |
+
os.fspath(selected.get("sdk_yaml") or "")
|
| 1006 |
+
) if selected.get("sdk_yaml") else ""
|
| 1007 |
+
manifest_orb_yaml = os.path.abspath(
|
| 1008 |
+
os.fspath(selected.get("orb_yaml") or "")
|
| 1009 |
+
) if selected.get("orb_yaml") else ""
|
| 1010 |
+
missing_manifest = [
|
| 1011 |
+
label for label, path in (
|
| 1012 |
+
("SDK YAML", sdk_yaml),
|
| 1013 |
+
("ORB YAML", manifest_orb_yaml),
|
| 1014 |
+
)
|
| 1015 |
+
if not path or not os.path.isfile(path)
|
| 1016 |
+
]
|
| 1017 |
+
if missing_manifest:
|
| 1018 |
+
raise FaysRuntimeError(
|
| 1019 |
+
"设备清单缺少当前 Fays 的运行文件,拒绝启动 ORB-SLAM: "
|
| 1020 |
+
f"serial={serial} 缺少="
|
| 1021 |
+
+ ", ".join(missing_manifest)
|
| 1022 |
+
+ ";请先运行 device_setup"
|
| 1023 |
+
)
|
| 1024 |
+
for label, path in (
|
| 1025 |
+
("SDK YAML", sdk_yaml),
|
| 1026 |
+
("ORB YAML", manifest_orb_yaml),
|
| 1027 |
+
):
|
| 1028 |
+
if serial not in os.path.basename(path):
|
| 1029 |
+
raise FaysRuntimeError(
|
| 1030 |
+
"设备清单运行文件与当前 Fays 序列号不匹配,"
|
| 1031 |
+
"拒绝启动 ORB-SLAM: "
|
| 1032 |
+
f"serial={serial} {label}={path}"
|
| 1033 |
+
)
|
| 1034 |
+
executable = os.path.abspath(
|
| 1035 |
+
os.fspath(selected.get("orb_binary") or "")
|
| 1036 |
+
) if selected.get("orb_binary") else ""
|
| 1037 |
+
if not executable:
|
| 1038 |
+
raise FaysRuntimeError(
|
| 1039 |
+
"device_setup 设备清单缺少当前 Fays 的 ORB 程序,"
|
| 1040 |
+
"请先运行 device_setup"
|
| 1041 |
+
)
|
| 1042 |
+
profile = {
|
| 1043 |
+
"backend": "orb",
|
| 1044 |
+
"calibration_serial": serial,
|
| 1045 |
+
"executable": executable,
|
| 1046 |
+
"settings": manifest_orb_yaml,
|
| 1047 |
+
}
|
| 1048 |
+
elif normalized_backend == "aikit":
|
| 1049 |
+
profile = fays_aikit_profile(
|
| 1050 |
+
calibration_serial,
|
| 1051 |
+
selected.get("ports") or {},
|
| 1052 |
+
)
|
| 1053 |
+
else:
|
| 1054 |
+
raise FaysRuntimeError(
|
| 1055 |
+
f"未知定位后端: {backend!r}"
|
| 1056 |
+
)
|
| 1057 |
+
missing = [
|
| 1058 |
+
path for path in (
|
| 1059 |
+
profile["executable"], profile["settings"]
|
| 1060 |
+
)
|
| 1061 |
+
if not os.path.isfile(path)
|
| 1062 |
+
]
|
| 1063 |
+
if missing:
|
| 1064 |
+
raise FaysRuntimeError(
|
| 1065 |
+
"Fays 标定运行时文件不存在: " + ", ".join(missing)
|
| 1066 |
+
)
|
| 1067 |
+
if not os.access(profile["executable"], os.X_OK):
|
| 1068 |
+
raise FaysRuntimeError(
|
| 1069 |
+
f"Fays 定位文件不可执行: {profile['executable']}"
|
| 1070 |
+
)
|
| 1071 |
+
return profile
|
| 1072 |
+
|
| 1073 |
+
def snapshot(self):
|
| 1074 |
+
with self._lock:
|
| 1075 |
+
return (
|
| 1076 |
+
json.loads(json.dumps(self._assignment))
|
| 1077 |
+
if self._assignment is not None else None
|
| 1078 |
+
)
|
| 1079 |
+
|
| 1080 |
+
def verify_active_link(self):
|
| 1081 |
+
"""Revalidate the selected Fays immediately before SDK startup."""
|
| 1082 |
+
with self._lock:
|
| 1083 |
+
if not self.is_acquired():
|
| 1084 |
+
raise FaysRuntimeError("当前实例尚未持有 Fays 租约")
|
| 1085 |
+
try:
|
| 1086 |
+
group, _unit, _manifest = self._resolve_assignment()
|
| 1087 |
+
speed = self._validate_link(group)
|
| 1088 |
+
selected = self._selected
|
| 1089 |
+
if selected["ports"] != group["ports"]:
|
| 1090 |
+
selected = self.acquire()
|
| 1091 |
+
else:
|
| 1092 |
+
selected = dict(selected)
|
| 1093 |
+
selected["usb_speed_mbps"] = speed
|
| 1094 |
+
self._event(
|
| 1095 |
+
"runtime_link_verified",
|
| 1096 |
+
physical_usb_path=group["physical_usb_path"],
|
| 1097 |
+
ports=group["ports"],
|
| 1098 |
+
usb_speed_mbps=speed,
|
| 1099 |
+
)
|
| 1100 |
+
return selected
|
| 1101 |
+
except Exception as exc:
|
| 1102 |
+
try:
|
| 1103 |
+
self._event(
|
| 1104 |
+
"runtime_link_failed",
|
| 1105 |
+
error=str(exc),
|
| 1106 |
+
selected=self._selected,
|
| 1107 |
+
)
|
| 1108 |
+
except Exception:
|
| 1109 |
+
pass
|
| 1110 |
+
if isinstance(exc, FaysRuntimeError):
|
| 1111 |
+
raise
|
| 1112 |
+
raise FaysRuntimeError(
|
| 1113 |
+
f"Fays 运行时链路校验失败: {exc}"
|
| 1114 |
+
) from exc
|
| 1115 |
+
|
| 1116 |
+
def inspect(self):
|
| 1117 |
+
"""Resolve the manifest assignment without locking or opening Fays."""
|
| 1118 |
+
with self._lock:
|
| 1119 |
+
group, unit, manifest = self._resolve_assignment()
|
| 1120 |
+
result = dict(group)
|
| 1121 |
+
result.update({
|
| 1122 |
+
"unit_id": unit["unit_id"],
|
| 1123 |
+
"calibration_serial": unit.get("fays", {}).get(
|
| 1124 |
+
"calibration_serial"
|
| 1125 |
+
) or result.get("product_serial"),
|
| 1126 |
+
"mapping_source": result.get(
|
| 1127 |
+
"mapping_source",
|
| 1128 |
+
"device_setup_manifest_sdk_serial",
|
| 1129 |
+
),
|
| 1130 |
+
"manifest_sha256": manifest["sha256"],
|
| 1131 |
+
})
|
| 1132 |
+
return result
|
| 1133 |
+
|
| 1134 |
+
def acquire(self):
|
| 1135 |
+
with self._lock:
|
| 1136 |
+
group, unit, manifest = self._resolve_assignment()
|
| 1137 |
+
physical = group["physical_usb_path"]
|
| 1138 |
+
lease_key = str(
|
| 1139 |
+
group.get("product_serial")
|
| 1140 |
+
or group.get("calibration_serial")
|
| 1141 |
+
or physical
|
| 1142 |
+
)
|
| 1143 |
+
sdk_template = str(group.get("sdk_yaml") or "").strip()
|
| 1144 |
+
if not sdk_template:
|
| 1145 |
+
raise FaysRuntimeError(
|
| 1146 |
+
"device_setup 设备清单缺少当前 Fays 的 SDK YAML,"
|
| 1147 |
+
"请先运行 device_setup"
|
| 1148 |
+
)
|
| 1149 |
+
if self.is_acquired():
|
| 1150 |
+
if self._selected["physical_usb_path"] != physical:
|
| 1151 |
+
raise FaysRuntimeError(
|
| 1152 |
+
"当前实例仍持有另一台 Fays;请先停止资源并释放租约"
|
| 1153 |
+
)
|
| 1154 |
+
if self._selected["ports"] != group["ports"]:
|
| 1155 |
+
materialize_fays_device_config(
|
| 1156 |
+
group["ports"],
|
| 1157 |
+
self.device_config_path,
|
| 1158 |
+
sdk_template,
|
| 1159 |
+
)
|
| 1160 |
+
refreshed = dict(group)
|
| 1161 |
+
refreshed.update({
|
| 1162 |
+
key: self._selected[key]
|
| 1163 |
+
for key in (
|
| 1164 |
+
"unit_id",
|
| 1165 |
+
"calibration_serial",
|
| 1166 |
+
"mapping_source",
|
| 1167 |
+
"runtime_config",
|
| 1168 |
+
"ipc_dir",
|
| 1169 |
+
"trajectory",
|
| 1170 |
+
"lock_path",
|
| 1171 |
+
"usb_speed_mbps",
|
| 1172 |
+
)
|
| 1173 |
+
})
|
| 1174 |
+
assignment = self._build_assignment(
|
| 1175 |
+
refreshed, unit, manifest, "acquired"
|
| 1176 |
+
)
|
| 1177 |
+
assignment["created_at"] = self._assignment[
|
| 1178 |
+
"created_at"
|
| 1179 |
+
]
|
| 1180 |
+
if "usb2_camera_set" in self._assignment:
|
| 1181 |
+
assignment["usb2_camera_set"] = self._assignment[
|
| 1182 |
+
"usb2_camera_set"
|
| 1183 |
+
]
|
| 1184 |
+
self._write_assignment(assignment)
|
| 1185 |
+
self._selected = refreshed
|
| 1186 |
+
self._assignment = assignment
|
| 1187 |
+
self._event(
|
| 1188 |
+
"video_nodes_refreshed",
|
| 1189 |
+
ports=group["ports"],
|
| 1190 |
+
physical_usb_path=physical,
|
| 1191 |
+
)
|
| 1192 |
+
return dict(self._selected)
|
| 1193 |
+
self.release()
|
| 1194 |
+
speed = self._validate_link(group)
|
| 1195 |
+
lock_path = os.path.join(
|
| 1196 |
+
self.lock_dir,
|
| 1197 |
+
f"{re.sub(r'[^A-Za-z0-9_.-]+', '-', lease_key)}.lock",
|
| 1198 |
+
)
|
| 1199 |
+
stream = open(lock_path, "a+", encoding="utf-8")
|
| 1200 |
+
try:
|
| 1201 |
+
fcntl.flock(stream.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB)
|
| 1202 |
+
except BlockingIOError as exc:
|
| 1203 |
+
stream.seek(0)
|
| 1204 |
+
owner = stream.read().strip() or "unknown owner"
|
| 1205 |
+
stream.close()
|
| 1206 |
+
self._event(
|
| 1207 |
+
"lease_busy",
|
| 1208 |
+
unit_id=unit["unit_id"],
|
| 1209 |
+
physical_usb_path=physical,
|
| 1210 |
+
owner=owner,
|
| 1211 |
+
)
|
| 1212 |
+
raise FaysRuntimeError(
|
| 1213 |
+
f"Fays {physical} 已被另一界面占用: {owner}"
|
| 1214 |
+
) from exc
|
| 1215 |
+
assignment = None
|
| 1216 |
+
try:
|
| 1217 |
+
owner = {
|
| 1218 |
+
"pid": self.pid,
|
| 1219 |
+
"run_id": self.run_id,
|
| 1220 |
+
"instance_id": self.instance_id,
|
| 1221 |
+
"unit_id": unit["unit_id"],
|
| 1222 |
+
"physical_usb_path": physical,
|
| 1223 |
+
}
|
| 1224 |
+
stream.seek(0)
|
| 1225 |
+
stream.truncate()
|
| 1226 |
+
json.dump(owner, stream, ensure_ascii=False, sort_keys=True)
|
| 1227 |
+
stream.flush()
|
| 1228 |
+
os.fsync(stream.fileno())
|
| 1229 |
+
materialize_fays_device_config(
|
| 1230 |
+
group["ports"],
|
| 1231 |
+
self.device_config_path,
|
| 1232 |
+
sdk_template,
|
| 1233 |
+
)
|
| 1234 |
+
self._clear_ipc_files()
|
| 1235 |
+
selected = dict(group)
|
| 1236 |
+
selected.update({
|
| 1237 |
+
"unit_id": unit["unit_id"],
|
| 1238 |
+
"calibration_serial": unit.get("fays", {}).get(
|
| 1239 |
+
"calibration_serial"
|
| 1240 |
+
) or selected.get("product_serial"),
|
| 1241 |
+
"product_serial": selected.get("product_serial"),
|
| 1242 |
+
"sdk_yaml": selected.get("sdk_yaml"),
|
| 1243 |
+
"orb_yaml": selected.get("orb_yaml"),
|
| 1244 |
+
"orb_binary": selected.get("orb_binary"),
|
| 1245 |
+
"decxin": unit.get("decxin"),
|
| 1246 |
+
"sightac_left": unit.get("sightac_left"),
|
| 1247 |
+
"sightac_right": unit.get("sightac_right"),
|
| 1248 |
+
"mapping_source": selected.get(
|
| 1249 |
+
"mapping_source",
|
| 1250 |
+
"device_setup_manifest_sdk_serial",
|
| 1251 |
+
),
|
| 1252 |
+
"runtime_config": self.device_config_path,
|
| 1253 |
+
"ipc_dir": self.ipc_dir,
|
| 1254 |
+
"trajectory": self.trajectory_path,
|
| 1255 |
+
"lock_path": lock_path,
|
| 1256 |
+
"usb_speed_mbps": speed,
|
| 1257 |
+
})
|
| 1258 |
+
assignment = self._build_assignment(
|
| 1259 |
+
selected, unit, manifest, "acquired"
|
| 1260 |
+
)
|
| 1261 |
+
self._write_assignment(assignment)
|
| 1262 |
+
self._event("lease_acquired", assignment=assignment)
|
| 1263 |
+
try:
|
| 1264 |
+
self._logger(
|
| 1265 |
+
"[Fays-Instance] selected: "
|
| 1266 |
+
f"unit={unit['unit_id']} "
|
| 1267 |
+
f"esp={(self._tty or unit.get('esp32', {})).get('serial', '-')} "
|
| 1268 |
+
f"fays={physical} "
|
| 1269 |
+
f"stereo={group['ports']['stereo_dev_port']} "
|
| 1270 |
+
f"imu={group['ports']['imu_dev_port']} "
|
| 1271 |
+
f"ipc={self.ipc_dir}"
|
| 1272 |
+
)
|
| 1273 |
+
except Exception:
|
| 1274 |
+
pass
|
| 1275 |
+
self._lock_stream = stream
|
| 1276 |
+
self._selected = selected
|
| 1277 |
+
self._assignment = assignment
|
| 1278 |
+
return dict(selected)
|
| 1279 |
+
except Exception as exc:
|
| 1280 |
+
self._lock_stream = None
|
| 1281 |
+
self._selected = None
|
| 1282 |
+
self._assignment = None
|
| 1283 |
+
try:
|
| 1284 |
+
if assignment is not None:
|
| 1285 |
+
assignment["status"] = "acquire_failed"
|
| 1286 |
+
assignment["error"] = str(exc)
|
| 1287 |
+
self._write_assignment(assignment)
|
| 1288 |
+
self._event(
|
| 1289 |
+
"lease_acquire_failed",
|
| 1290 |
+
unit_id=unit["unit_id"],
|
| 1291 |
+
physical_usb_path=physical,
|
| 1292 |
+
error=str(exc),
|
| 1293 |
+
)
|
| 1294 |
+
except Exception:
|
| 1295 |
+
pass
|
| 1296 |
+
try:
|
| 1297 |
+
fcntl.flock(stream.fileno(), fcntl.LOCK_UN)
|
| 1298 |
+
finally:
|
| 1299 |
+
stream.close()
|
| 1300 |
+
raise
|
| 1301 |
+
|
| 1302 |
+
def release(self):
|
| 1303 |
+
with self._lock:
|
| 1304 |
+
stream = self._lock_stream
|
| 1305 |
+
selected = self._selected
|
| 1306 |
+
self._lock_stream = None
|
| 1307 |
+
self._selected = None
|
| 1308 |
+
if stream is not None:
|
| 1309 |
+
try:
|
| 1310 |
+
fcntl.flock(stream.fileno(), fcntl.LOCK_UN)
|
| 1311 |
+
finally:
|
| 1312 |
+
stream.close()
|
| 1313 |
+
self._clear_ipc_files()
|
| 1314 |
+
if self._assignment is not None:
|
| 1315 |
+
self._assignment["status"] = "released"
|
| 1316 |
+
self._assignment["released_at"] = time.time()
|
| 1317 |
+
self._write_assignment()
|
| 1318 |
+
if selected is not None:
|
| 1319 |
+
self._event(
|
| 1320 |
+
"lease_released",
|
| 1321 |
+
unit_id=selected.get("unit_id"),
|
| 1322 |
+
physical_usb_path=selected.get(
|
| 1323 |
+
"physical_usb_path"
|
| 1324 |
+
),
|
| 1325 |
+
)
|
| 1326 |
+
return True
|
| 1327 |
+
|
| 1328 |
+
def _resolve_assignment(self):
|
| 1329 |
+
"""Resolve one selected ESP32 strictly through device_setup."""
|
| 1330 |
+
if self._tty is None:
|
| 1331 |
+
raise FaysRuntimeError(
|
| 1332 |
+
"尚未选择 ESP32 控制串口;设备身份只能由 device_setup "
|
| 1333 |
+
"设备清单中的 ESP32 serial 确定"
|
| 1334 |
+
)
|
| 1335 |
+
manifest = self._load_manifest()
|
| 1336 |
+
matches = [
|
| 1337 |
+
unit for unit in manifest["units"]
|
| 1338 |
+
if unit["esp32"]["vendor_id"] == self._tty["vendor_id"]
|
| 1339 |
+
and unit["esp32"]["product_id"] == self._tty["product_id"]
|
| 1340 |
+
and unit["esp32"]["serial"] == self._tty["serial"]
|
| 1341 |
+
]
|
| 1342 |
+
if len(matches) != 1:
|
| 1343 |
+
raise FaysRuntimeError(
|
| 1344 |
+
"当前 ESP32 不在 device_setup 设备清单中,拒绝猜测 Fays 归属: "
|
| 1345 |
+
f"serial={self._tty.get('serial', '<missing>')} "
|
| 1346 |
+
f"matches={len(matches)} manifest={self.manifest_path}"
|
| 1347 |
+
)
|
| 1348 |
+
unit = matches[0]
|
| 1349 |
+
groups = tuple(self._discover_selected_devices(unit)
|
| 1350 |
+
if self._discover_selected_devices is not None
|
| 1351 |
+
else self._discover_devices())
|
| 1352 |
+
if not groups:
|
| 1353 |
+
raise FaysRuntimeError("未发现所选 FaysSense S80M FT602")
|
| 1354 |
+
product_serial = unit["fays"]["product_serial"]
|
| 1355 |
+
group_matches = [
|
| 1356 |
+
group for group in groups
|
| 1357 |
+
if str(group.get("manifest_unit") or "").strip()
|
| 1358 |
+
== unit["unit_id"]
|
| 1359 |
+
and str(group.get("product_serial") or "").strip()
|
| 1360 |
+
== product_serial
|
| 1361 |
+
]
|
| 1362 |
+
if len(group_matches) != 1:
|
| 1363 |
+
available = ", ".join(
|
| 1364 |
+
str(group.get("product_serial") or "<missing>")
|
| 1365 |
+
for group in groups
|
| 1366 |
+
)
|
| 1367 |
+
raise FaysRuntimeError(
|
| 1368 |
+
"device_setup 设备清单与当前 Fays SDK 序列号无法唯一匹配: "
|
| 1369 |
+
f"unit={unit['unit_id']} expected={product_serial} "
|
| 1370 |
+
f"available={available}"
|
| 1371 |
+
)
|
| 1372 |
+
selected = dict(group_matches[0])
|
| 1373 |
+
selected.update({
|
| 1374 |
+
"unit_id": unit["unit_id"],
|
| 1375 |
+
"product_serial": product_serial,
|
| 1376 |
+
"calibration_serial": product_serial,
|
| 1377 |
+
"sdk_yaml": unit["fays"]["sdk_yaml"],
|
| 1378 |
+
"orb_yaml": unit["fays"]["orb_yaml"],
|
| 1379 |
+
"orb_binary": unit["fays"]["orb_binary"],
|
| 1380 |
+
"mapping_source": "device_setup_manifest_sdk_serial",
|
| 1381 |
+
})
|
| 1382 |
+
return selected, unit, manifest
|
| 1383 |
+
|
| 1384 |
+
def _load_manifest(self):
|
| 1385 |
+
from device_manifest_writer import load_device_manifest
|
| 1386 |
+
|
| 1387 |
+
try:
|
| 1388 |
+
manifest = load_device_manifest()
|
| 1389 |
+
with open(self.manifest_path, "rb") as stream:
|
| 1390 |
+
raw = stream.read()
|
| 1391 |
+
except (OSError, RuntimeError) as exc:
|
| 1392 |
+
raise FaysRuntimeError(
|
| 1393 |
+
f"无法读取 device_setup 设备清单: {self.manifest_path}: {exc}"
|
| 1394 |
+
) from exc
|
| 1395 |
+
manifest["sha256"] = hashlib.sha256(raw).hexdigest()
|
| 1396 |
+
return manifest
|
| 1397 |
+
|
| 1398 |
+
def _build_assignment(self, selected, unit, manifest, status):
|
| 1399 |
+
return {
|
| 1400 |
+
"schema_version": 1,
|
| 1401 |
+
"status": status,
|
| 1402 |
+
"created_at": time.time(),
|
| 1403 |
+
"pid": self.pid,
|
| 1404 |
+
"run_id": self.run_id,
|
| 1405 |
+
"instance_id": self.instance_id,
|
| 1406 |
+
"unit_id": unit["unit_id"],
|
| 1407 |
+
"pairing_status": unit.get(
|
| 1408 |
+
"pairing_status", "unspecified"
|
| 1409 |
+
),
|
| 1410 |
+
"mapping_source": selected["mapping_source"],
|
| 1411 |
+
"manifest": {
|
| 1412 |
+
"path": self.manifest_path,
|
| 1413 |
+
"sha256": manifest["sha256"],
|
| 1414 |
+
},
|
| 1415 |
+
"esp32": dict(self._tty or unit.get("esp32", {})),
|
| 1416 |
+
"decxin": selected.get("decxin"),
|
| 1417 |
+
"sightac_left": selected.get("sightac_left"),
|
| 1418 |
+
"sightac_right": selected.get("sightac_right"),
|
| 1419 |
+
"fays": {
|
| 1420 |
+
"calibration_serial": selected.get(
|
| 1421 |
+
"calibration_serial"
|
| 1422 |
+
),
|
| 1423 |
+
"product_serial": selected.get("product_serial"),
|
| 1424 |
+
"sdk_yaml": selected.get("sdk_yaml"),
|
| 1425 |
+
"orb_yaml": selected.get("orb_yaml"),
|
| 1426 |
+
"orb_binary": selected.get("orb_binary"),
|
| 1427 |
+
"physical_usb_path": selected["physical_usb_path"],
|
| 1428 |
+
"stereo_dev_port": selected["ports"][
|
| 1429 |
+
"stereo_dev_port"
|
| 1430 |
+
],
|
| 1431 |
+
"imu_dev_port": selected["ports"]["imu_dev_port"],
|
| 1432 |
+
"stereo_interface": selected["stereo"].get(
|
| 1433 |
+
"interface_number"
|
| 1434 |
+
),
|
| 1435 |
+
"imu_interface": selected["imu"].get("interface_number"),
|
| 1436 |
+
"usb_speed_mbps": selected["usb_speed_mbps"],
|
| 1437 |
+
},
|
| 1438 |
+
"runtime": {
|
| 1439 |
+
"device_config": self.device_config_path,
|
| 1440 |
+
"ipc_dir": self.ipc_dir,
|
| 1441 |
+
"trajectory": self.trajectory_path,
|
| 1442 |
+
"work_dir": self.runtime_dir,
|
| 1443 |
+
"lock_path": selected["lock_path"],
|
| 1444 |
+
},
|
| 1445 |
+
}
|
| 1446 |
+
|
| 1447 |
+
def _write_assignment(self, assignment=None):
|
| 1448 |
+
assignment = self._assignment if assignment is None else assignment
|
| 1449 |
+
if assignment is None:
|
| 1450 |
+
return
|
| 1451 |
+
temporary = f"{self.assignment_path}.tmp.{self.pid}"
|
| 1452 |
+
with open(temporary, "w", encoding="utf-8") as stream:
|
| 1453 |
+
json.dump(
|
| 1454 |
+
assignment,
|
| 1455 |
+
stream,
|
| 1456 |
+
ensure_ascii=False,
|
| 1457 |
+
indent=2,
|
| 1458 |
+
sort_keys=True,
|
| 1459 |
+
)
|
| 1460 |
+
stream.write("\n")
|
| 1461 |
+
stream.flush()
|
| 1462 |
+
os.fsync(stream.fileno())
|
| 1463 |
+
os.replace(temporary, self.assignment_path)
|
| 1464 |
+
|
| 1465 |
+
def _event(self, event, **payload):
|
| 1466 |
+
record = {
|
| 1467 |
+
"time": time.time(),
|
| 1468 |
+
"event": str(event),
|
| 1469 |
+
"pid": self.pid,
|
| 1470 |
+
"run_id": self.run_id,
|
| 1471 |
+
"instance_id": self.instance_id,
|
| 1472 |
+
**payload,
|
| 1473 |
+
}
|
| 1474 |
+
with open(self.event_log_path, "a", encoding="utf-8") as stream:
|
| 1475 |
+
stream.write(json.dumps(
|
| 1476 |
+
record, ensure_ascii=False, sort_keys=True
|
| 1477 |
+
))
|
| 1478 |
+
stream.write("\n")
|
| 1479 |
+
|
| 1480 |
+
def _clear_ipc_files(self):
|
| 1481 |
+
for leaf in _INSTANCE_IPC_FILES:
|
| 1482 |
+
try:
|
| 1483 |
+
os.unlink(os.path.join(self.ipc_dir, leaf))
|
| 1484 |
+
except FileNotFoundError:
|
| 1485 |
+
pass
|
| 1486 |
+
|
| 1487 |
+
|
| 1488 |
+
def build_fays_runtime_env():
|
| 1489 |
+
"""构造生产 Fays/OpenCV 4.8/ORB-SLAM3 子进程环境。"""
|
| 1490 |
+
env = os.environ.copy()
|
| 1491 |
+
arch = os.uname().machine if hasattr(os, "uname") else "x86_64"
|
| 1492 |
+
priority_dirs = [
|
| 1493 |
+
"/usr/local/lib",
|
| 1494 |
+
os.path.join(FAYS_SDK_ROOT, f"lib/fays_atrak/{arch}/Release"),
|
| 1495 |
+
os.path.join(FAYS_SDK_ROOT, f"thirdparty/ft602-linux-{arch}"),
|
| 1496 |
+
os.path.dirname(ORB_LIBRARY),
|
| 1497 |
+
os.path.join(ORB_ROOT, "lib"),
|
| 1498 |
+
os.path.join(ORB_ROOT, "Thirdparty", "DBoW2", "lib"),
|
| 1499 |
+
os.path.join(ORB_ROOT, "Thirdparty", "g2o", "lib"),
|
| 1500 |
+
]
|
| 1501 |
+
|
| 1502 |
+
# 生产桥接程序只允许 OpenCV 4.8。继承环境中的 SDK 4.2 和旧 dist/lib
|
| 1503 |
+
# 会让同一进程混装两个 OpenCV ABI,因此必须过滤。
|
| 1504 |
+
legacy_dist = os.path.abspath(os.path.join(PROJECT_ROOT, "dist", "lib"))
|
| 1505 |
+
inherited_dirs = []
|
| 1506 |
+
for path in env.get("LD_LIBRARY_PATH", "").split(os.pathsep):
|
| 1507 |
+
if not path:
|
| 1508 |
+
continue
|
| 1509 |
+
absolute = os.path.abspath(path)
|
| 1510 |
+
if "opencv-4.2.0" in absolute or absolute == legacy_dist:
|
| 1511 |
+
continue
|
| 1512 |
+
inherited_dirs.append(absolute)
|
| 1513 |
+
|
| 1514 |
+
ordered = []
|
| 1515 |
+
for path in priority_dirs + inherited_dirs:
|
| 1516 |
+
if path not in ordered:
|
| 1517 |
+
ordered.append(path)
|
| 1518 |
+
env["LD_LIBRARY_PATH"] = os.pathsep.join(ordered)
|
| 1519 |
+
|
| 1520 |
+
# 当前 ORB 核心直接链接系统 Boost 1.74。清除继承环境中人为预加载的
|
| 1521 |
+
# Boost serialization,避免另一 ABI 在系统 1.74 之前发生符号抢占;其余
|
| 1522 |
+
# 与 ORB 无关的预加载项保持不变。
|
| 1523 |
+
inherited_preloads = [
|
| 1524 |
+
path for path in re.split(
|
| 1525 |
+
r"[\s:]+", env.get("LD_PRELOAD", "").strip()
|
| 1526 |
+
)
|
| 1527 |
+
if path and "libboost_serialization.so" not in os.path.basename(path)
|
| 1528 |
+
]
|
| 1529 |
+
if inherited_preloads:
|
| 1530 |
+
env["LD_PRELOAD"] = " ".join(inherited_preloads)
|
| 1531 |
+
else:
|
| 1532 |
+
env.pop("LD_PRELOAD", None)
|
| 1533 |
+
env["FAYS_SDK_ROOT"] = FAYS_SDK_ROOT
|
| 1534 |
+
env["KSQ_ORB_ROOT"] = ORB_ROOT
|
| 1535 |
+
return env
|
| 1536 |
+
|
| 1537 |
+
|
| 1538 |
+
def build_fays_probe_env():
|
| 1539 |
+
"""构造官方标定探针子进程环境(OpenCV 4.2 + fays_vikit + ft602)。
|
| 1540 |
+
|
| 1541 |
+
探针二进制依赖 opencv-4.2(libopencv_imgproc/core.so.4.2),与桥接
|
| 1542 |
+
进程的 OpenCV 4.8 环境必须隔离;探针环境不过滤 opencv-4.2,也不含
|
| 1543 |
+
orb48_env/ORB-SLAM 目录。
|
| 1544 |
+
"""
|
| 1545 |
+
env = os.environ.copy()
|
| 1546 |
+
arch = os.uname().machine if hasattr(os, "uname") else "x86_64"
|
| 1547 |
+
priority_dirs = [
|
| 1548 |
+
"/usr/local/lib",
|
| 1549 |
+
os.path.join(FAYS_SDK_ROOT, f"lib/fays_atrak/{arch}/Release"),
|
| 1550 |
+
os.path.join(FAYS_SDK_ROOT, f"thirdparty/ft602-linux-{arch}"),
|
| 1551 |
+
os.path.join(FAYS_SDK_ROOT,
|
| 1552 |
+
f"thirdparty/opencv-4.2.0-linux-{arch}/lib"),
|
| 1553 |
+
# opencv-4.2 的传递依赖(libtbb.so.2 等)只存在于 orb48_env;
|
| 1554 |
+
# 排在 opencv-4.2 之后,4.2 同名 soname 优先命中
|
| 1555 |
+
os.path.join(FAYS_SDK_ROOT, "thirdparty", "orb48_env", "lib"),
|
| 1556 |
+
]
|
| 1557 |
+
inherited_dirs = [
|
| 1558 |
+
os.path.abspath(path)
|
| 1559 |
+
for path in env.get("LD_LIBRARY_PATH", "").split(os.pathsep)
|
| 1560 |
+
if path
|
| 1561 |
+
]
|
| 1562 |
+
ordered = []
|
| 1563 |
+
for path in priority_dirs + inherited_dirs:
|
| 1564 |
+
if path not in ordered:
|
| 1565 |
+
ordered.append(path)
|
| 1566 |
+
env["LD_LIBRARY_PATH"] = os.pathsep.join(ordered)
|
| 1567 |
+
env["FAYS_SDK_ROOT"] = FAYS_SDK_ROOT
|
| 1568 |
+
return env
|
| 1569 |
+
|
| 1570 |
+
|
| 1571 |
+
def validate_fays_runtime():
|
| 1572 |
+
"""校验启动所需静态文件;不要求相机当前已插入。"""
|
| 1573 |
+
from device_manifest_writer import load_device_manifest
|
| 1574 |
+
|
| 1575 |
+
required_files = {
|
| 1576 |
+
"Fays ORB 可执行文件": FAYS_ORB_BINARY,
|
| 1577 |
+
"设备清单": DEVICE_MANIFEST,
|
| 1578 |
+
"ORB 词典": ORB_VOCABULARY,
|
| 1579 |
+
"ORB-SLAM3 核心库": ORB_LIBRARY,
|
| 1580 |
+
}
|
| 1581 |
+
try:
|
| 1582 |
+
manifest = load_device_manifest()
|
| 1583 |
+
except RuntimeError as exc:
|
| 1584 |
+
raise FaysRuntimeError(str(exc)) from exc
|
| 1585 |
+
for unit in manifest["units"]:
|
| 1586 |
+
fays = unit["fays"]
|
| 1587 |
+
prefix = f"{unit['unit_id']} ({fays['product_serial']})"
|
| 1588 |
+
required_files[f"{prefix} SDK YAML"] = fays["sdk_yaml"]
|
| 1589 |
+
required_files[f"{prefix} ORB YAML"] = fays["orb_yaml"]
|
| 1590 |
+
required_files[f"{prefix} ORB 程序"] = fays["orb_binary"]
|
| 1591 |
+
errors = [
|
| 1592 |
+
f"{label}不存在: {path}"
|
| 1593 |
+
for label, path in required_files.items()
|
| 1594 |
+
if not os.path.isfile(path)
|
| 1595 |
+
]
|
| 1596 |
+
if os.path.isfile(FAYS_ORB_BINARY) and not os.access(FAYS_ORB_BINARY, os.X_OK):
|
| 1597 |
+
errors.append(f"Fays ORB 文件不可执行: {FAYS_ORB_BINARY}")
|
| 1598 |
+
if errors:
|
| 1599 |
+
raise FaysRuntimeError("Fays 运行时检查失败:\n- " + "\n- ".join(errors))
|
| 1600 |
+
return required_files
|
| 1601 |
+
|
| 1602 |
+
|
| 1603 |
+
def main():
|
| 1604 |
+
"""执行不打开相机、不复位 USB 的 Fays 运行时与设备预检。"""
|
| 1605 |
+
try:
|
| 1606 |
+
required = validate_fays_runtime()
|
| 1607 |
+
groups = discover_fays_manifest_device_groups()
|
| 1608 |
+
verified = []
|
| 1609 |
+
for group in groups:
|
| 1610 |
+
speed = validate_fays_sdk_access(group)
|
| 1611 |
+
verified.append((group, speed, fays_usb_control_node(group)))
|
| 1612 |
+
except FaysRuntimeError as exc:
|
| 1613 |
+
print(f"[FAYS-PREFLIGHT] FAILED\n{exc}")
|
| 1614 |
+
return 1
|
| 1615 |
+
|
| 1616 |
+
print(f"[FAYS-PREFLIGHT] OK: {SUPPORTED_CAMERA}")
|
| 1617 |
+
for label, path in required.items():
|
| 1618 |
+
print(f" {label}: {path}")
|
| 1619 |
+
for group, usb_speed, control_node in verified:
|
| 1620 |
+
print(
|
| 1621 |
+
f" unit={group['unit_id']} serial={group['product_serial']} "
|
| 1622 |
+
f"physical={group['physical_usb_path']}"
|
| 1623 |
+
)
|
| 1624 |
+
for role in ("stereo", "imu"):
|
| 1625 |
+
info = group[role]
|
| 1626 |
+
print(
|
| 1627 |
+
f" {role}: {info['device']} "
|
| 1628 |
+
f"interface={info['interface_number']} "
|
| 1629 |
+
f"index={info['stream_index']}"
|
| 1630 |
+
)
|
| 1631 |
+
print(f" Fays USB link: {usb_speed:g}M SuperSpeed")
|
| 1632 |
+
print(f" Fays D3XX control: {control_node} read/write OK")
|
| 1633 |
+
print(" Hardware access: sysfs read-only; no VideoCapture; no USB reset")
|
| 1634 |
+
return 0
|
| 1635 |
+
|
| 1636 |
+
|
| 1637 |
+
if __name__ == "__main__":
|
| 1638 |
+
raise SystemExit(main())
|
collector/core/gripper/fays_serial_probe.py
ADDED
|
@@ -0,0 +1,246 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Read a Fays S80M product serial through the official VI Kit SDK.
|
| 2 |
+
|
| 3 |
+
The FT602 USB bridge has duplicate USB descriptor data, so the only reliable
|
| 4 |
+
product identifier is ``ViKitDeviceInfo.serial_number``. This module wraps
|
| 5 |
+
the existing calibration-probe executable and returns that serial without
|
| 6 |
+
opening a video stream or touching USB state.
|
| 7 |
+
"""
|
| 8 |
+
|
| 9 |
+
from __future__ import annotations
|
| 10 |
+
|
| 11 |
+
import os
|
| 12 |
+
import re
|
| 13 |
+
import subprocess
|
| 14 |
+
import tempfile
|
| 15 |
+
from core.gripper.runtime.device_access import device_access_guard, FAYS_SDK_INITIALIZATION_LOCK, fays_device_guard
|
| 16 |
+
from typing import Mapping, Optional
|
| 17 |
+
|
| 18 |
+
from core.gripper.fays_runtime import (
|
| 19 |
+
FAYS_DEVICE_CONFIG,
|
| 20 |
+
GRIPPER_DIR,
|
| 21 |
+
PROJECT_ROOT,
|
| 22 |
+
discover_fays_device_groups,
|
| 23 |
+
read_fays_config_ports,
|
| 24 |
+
)
|
| 25 |
+
|
| 26 |
+
|
| 27 |
+
DEFAULT_PROBE_BINARY = os.path.abspath(os.environ.get(
|
| 28 |
+
"KSQ_FAYS_CALIBRATION_PROBE",
|
| 29 |
+
os.path.join(
|
| 30 |
+
PROJECT_ROOT, "dist", "fays_aikit", "bin",
|
| 31 |
+
"fays_vikit_calibration_probe",
|
| 32 |
+
),
|
| 33 |
+
))
|
| 34 |
+
DEFAULT_TEMPLATE = FAYS_DEVICE_CONFIG
|
| 35 |
+
_SERIAL_LINE = re.compile(r"^device\.serial=(.+)$", re.MULTILINE)
|
| 36 |
+
|
| 37 |
+
# Temporary probes share the SDK initialization guard with the native launcher.
|
| 38 |
+
# A separate per-device lease prevents probes of an already streaming device.
|
| 39 |
+
|
| 40 |
+
|
| 41 |
+
def render_probe_config(
|
| 42 |
+
ports: Mapping[str, str],
|
| 43 |
+
destination: str,
|
| 44 |
+
*,
|
| 45 |
+
template_path: Optional[str] = None,
|
| 46 |
+
):
|
| 47 |
+
"""Render one candidate SDK YAML from the canonical template."""
|
| 48 |
+
template = os.path.abspath(template_path or DEFAULT_TEMPLATE)
|
| 49 |
+
try:
|
| 50 |
+
with open(template, encoding="utf-8") as stream:
|
| 51 |
+
content = stream.read()
|
| 52 |
+
except OSError as exc:
|
| 53 |
+
raise RuntimeError(f"无法读取 Fays 配置模板: {template}: {exc}") from exc
|
| 54 |
+
for key in ("stereo_dev_port", "imu_dev_port"):
|
| 55 |
+
value = str(ports.get(key, "")).strip()
|
| 56 |
+
if not value:
|
| 57 |
+
raise RuntimeError(f"缺少 Fays 端口字段: {key}")
|
| 58 |
+
content, count = re.subn(
|
| 59 |
+
rf"^(\s*{re.escape(key)}\s*:)\s*[^\s#]+",
|
| 60 |
+
rf"\1 {value}",
|
| 61 |
+
content,
|
| 62 |
+
count=1,
|
| 63 |
+
flags=re.MULTILINE,
|
| 64 |
+
)
|
| 65 |
+
if count != 1:
|
| 66 |
+
raise RuntimeError(f"Fays 配置模板无法定位字段: {key}")
|
| 67 |
+
temporary = f"{destination}.tmp.{os.getpid()}"
|
| 68 |
+
try:
|
| 69 |
+
with open(temporary, "w", encoding="utf-8") as stream:
|
| 70 |
+
stream.write(content)
|
| 71 |
+
stream.flush()
|
| 72 |
+
os.fsync(stream.fileno())
|
| 73 |
+
os.replace(temporary, destination)
|
| 74 |
+
except OSError as exc:
|
| 75 |
+
try:
|
| 76 |
+
os.unlink(temporary)
|
| 77 |
+
except OSError:
|
| 78 |
+
pass
|
| 79 |
+
raise RuntimeError(f"写入探测配置失败: {destination}: {exc}") from exc
|
| 80 |
+
return destination
|
| 81 |
+
|
| 82 |
+
|
| 83 |
+
def probe_product_serial(
|
| 84 |
+
ports: Mapping[str, str],
|
| 85 |
+
*,
|
| 86 |
+
template_path: Optional[str] = None,
|
| 87 |
+
probe_binary: Optional[str] = None,
|
| 88 |
+
timeout: float = 60.0,
|
| 89 |
+
environment: Optional[Mapping[str, str]] = None,
|
| 90 |
+
) -> str:
|
| 91 |
+
"""Run the official calibration probe and return ``device.serial``.
|
| 92 |
+
|
| 93 |
+
timeout 同时是 SDK 全局初始化锁的等待上限与子进程运行上限:双夹爪
|
| 94 |
+
并发打开时,第二台的探针要等第一台 SLAM 初始化(最长 45s)释放锁,
|
| 95 |
+
20s 会在等待期间误报超时,故放宽到 60s。
|
| 96 |
+
"""
|
| 97 |
+
executable = os.path.abspath(probe_binary or DEFAULT_PROBE_BINARY)
|
| 98 |
+
if not os.path.isfile(executable):
|
| 99 |
+
raise RuntimeError(f"Fays 标定探测程序不存在: {executable}")
|
| 100 |
+
if not os.access(executable, os.X_OK):
|
| 101 |
+
raise RuntimeError(f"Fays 标定探测程序不可执行: {executable}")
|
| 102 |
+
with tempfile.TemporaryDirectory(prefix="ksq-fays-probe-") as temp_dir:
|
| 103 |
+
config_path = render_probe_config(
|
| 104 |
+
ports,
|
| 105 |
+
os.path.join(temp_dir, "fays_vikit_probe.yaml"),
|
| 106 |
+
template_path=template_path,
|
| 107 |
+
)
|
| 108 |
+
try:
|
| 109 |
+
# The thread lock covers two slots in this process; flock also
|
| 110 |
+
# protects against a second application instance probing through
|
| 111 |
+
# the same vendor SDK at the same time. This is synchronization
|
| 112 |
+
# only: identity still comes exclusively from the live SDK result
|
| 113 |
+
# and the device_setup manifest below.
|
| 114 |
+
with device_access_guard(FAYS_SDK_INITIALIZATION_LOCK, timeout=timeout), \
|
| 115 |
+
fays_device_guard(ports["stereo_dev_port"]):
|
| 116 |
+
completed = subprocess.run(
|
| 117 |
+
[executable, config_path], capture_output=True, text=True,
|
| 118 |
+
timeout=timeout,
|
| 119 |
+
env=dict(os.environ if environment is None else environment),
|
| 120 |
+
check=False,
|
| 121 |
+
)
|
| 122 |
+
except subprocess.TimeoutExpired as exc:
|
| 123 |
+
raise RuntimeError(
|
| 124 |
+
f"Fays 标定探测超时: {ports} ({exc.timeout}s)"
|
| 125 |
+
) from exc
|
| 126 |
+
output = (
|
| 127 |
+
(completed.stdout or "") + "\n" + (completed.stderr or "")
|
| 128 |
+
)
|
| 129 |
+
if completed.returncode != 0:
|
| 130 |
+
tail = "\n".join(
|
| 131 |
+
line for line in output.splitlines()[-20:]
|
| 132 |
+
)
|
| 133 |
+
raise RuntimeError(
|
| 134 |
+
"Fays 标定探测失败: "
|
| 135 |
+
f"returncode={completed.returncode} ports={ports}\n{tail}"
|
| 136 |
+
)
|
| 137 |
+
match = _SERIAL_LINE.search(output)
|
| 138 |
+
if match is None:
|
| 139 |
+
raise RuntimeError(
|
| 140 |
+
f"Fays 标定探测未返回 device.serial: ports={ports}"
|
| 141 |
+
)
|
| 142 |
+
serial = match.group(1).strip()
|
| 143 |
+
if not serial:
|
| 144 |
+
raise RuntimeError(
|
| 145 |
+
f"Fays 标定探测返回空序列号: ports={ports}"
|
| 146 |
+
)
|
| 147 |
+
return serial
|
| 148 |
+
|
| 149 |
+
|
| 150 |
+
def discover_fays_manifest_groups(
|
| 151 |
+
*,
|
| 152 |
+
target_unit=None,
|
| 153 |
+
probe_binary: Optional[str] = None,
|
| 154 |
+
timeout: float = 60.0,
|
| 155 |
+
environment: Optional[Mapping[str, str]] = None,
|
| 156 |
+
logger=None,
|
| 157 |
+
):
|
| 158 |
+
"""Resolve native Fays groups strictly through the device_setup manifest.
|
| 159 |
+
|
| 160 |
+
Setup may probe every visible group. Connect supplies target_unit, limits
|
| 161 |
+
access to its configured nodes, then verifies the live SDK product serial.
|
| 162 |
+
Other rigs are never probed as a fallback for a missing selected device.
|
| 163 |
+
Probe failure, an unknown serial, or missing manifest runtime artifacts is
|
| 164 |
+
fatal. YAML ports, old pair files, cached assignments and unique-group
|
| 165 |
+
fallbacks are deliberately not accepted as identity sources.
|
| 166 |
+
"""
|
| 167 |
+
from device_manifest_writer import load_device_manifest
|
| 168 |
+
|
| 169 |
+
groups = tuple(discover_fays_device_groups())
|
| 170 |
+
manifest = load_device_manifest()
|
| 171 |
+
units = tuple(manifest["units"])
|
| 172 |
+
units_by_serial = {
|
| 173 |
+
str(unit["fays"]["product_serial"]).strip(): unit
|
| 174 |
+
for unit in units
|
| 175 |
+
}
|
| 176 |
+
if target_unit is not None:
|
| 177 |
+
# Physical position only limits access; identity still MUST come from
|
| 178 |
+
# this live SDK probe and match the manifest serial below.
|
| 179 |
+
expected = str(target_unit["fays"]["product_serial"])
|
| 180 |
+
ports = read_fays_config_ports(target_unit["fays"]["sdk_yaml"])
|
| 181 |
+
groups = tuple(g for g in groups if g["ports"] == ports)
|
| 182 |
+
if len(groups) != 1:
|
| 183 |
+
raise RuntimeError("所选 Fays 节点已变化,请重建设备清单;禁止探测其他夹爪")
|
| 184 |
+
if expected not in units_by_serial:
|
| 185 |
+
raise RuntimeError("所选 Fays 不在当前设备清单中")
|
| 186 |
+
resolved = []
|
| 187 |
+
probed_serials = set()
|
| 188 |
+
for group in groups:
|
| 189 |
+
try:
|
| 190 |
+
serial = probe_product_serial(
|
| 191 |
+
group["ports"],
|
| 192 |
+
template_path=None,
|
| 193 |
+
probe_binary=probe_binary,
|
| 194 |
+
timeout=timeout,
|
| 195 |
+
environment=environment,
|
| 196 |
+
)
|
| 197 |
+
except Exception as exc:
|
| 198 |
+
raise RuntimeError(
|
| 199 |
+
"Fays SDK 产品序列号探测失败,拒绝使用设备清单外的回退: "
|
| 200 |
+
f"physical={group['physical_usb_path']} error={exc}"
|
| 201 |
+
) from exc
|
| 202 |
+
if target_unit is not None and serial != expected:
|
| 203 |
+
raise RuntimeError(f"所选 Fays SDK 序列号不匹配: expected={expected} actual={serial}")
|
| 204 |
+
if serial in probed_serials:
|
| 205 |
+
raise RuntimeError(
|
| 206 |
+
"Fays SDK 返回重复产品序列号,拒绝猜测归属: "
|
| 207 |
+
f"serial={serial}"
|
| 208 |
+
)
|
| 209 |
+
probed_serials.add(serial)
|
| 210 |
+
unit = units_by_serial.get(serial)
|
| 211 |
+
if unit is None:
|
| 212 |
+
raise RuntimeError(
|
| 213 |
+
"当前 Fays 不在 device_setup 设备清单中,拒绝启动: "
|
| 214 |
+
f"serial={serial} physical={group['physical_usb_path']}"
|
| 215 |
+
)
|
| 216 |
+
fays = unit["fays"]
|
| 217 |
+
required = ("sdk_yaml", "orb_yaml", "orb_binary")
|
| 218 |
+
missing = [
|
| 219 |
+
key for key in required
|
| 220 |
+
if not fays.get(key) or not os.path.isfile(fays[key])
|
| 221 |
+
]
|
| 222 |
+
if missing:
|
| 223 |
+
raise RuntimeError(
|
| 224 |
+
"device_setup 设备清单缺少 Fays 运行资源: "
|
| 225 |
+
f"unit={unit['unit_id']} missing={','.join(missing)}"
|
| 226 |
+
)
|
| 227 |
+
annotated = dict(group)
|
| 228 |
+
annotated.update({
|
| 229 |
+
"unit_id": unit["unit_id"],
|
| 230 |
+
"product_serial": serial,
|
| 231 |
+
"calibration_serial": serial,
|
| 232 |
+
"sdk_yaml": fays["sdk_yaml"],
|
| 233 |
+
"orb_yaml": fays["orb_yaml"],
|
| 234 |
+
"orb_binary": fays["orb_binary"],
|
| 235 |
+
"mapping_source": "device_setup_manifest_sdk_serial",
|
| 236 |
+
"manifest_esp32": dict(unit["esp32"]),
|
| 237 |
+
"manifest_unit": unit["unit_id"],
|
| 238 |
+
})
|
| 239 |
+
resolved.append(annotated)
|
| 240 |
+
|
| 241 |
+
if len(resolved) != len(groups):
|
| 242 |
+
raise RuntimeError(
|
| 243 |
+
"Fays 设备清单与当前硬件数量不一致,拒绝继续: "
|
| 244 |
+
f"manifest={len(units)} visible={len(groups)} matched={len(resolved)}"
|
| 245 |
+
)
|
| 246 |
+
return tuple(resolved)
|
collector/core/gripper/fays_single.py
ADDED
|
@@ -0,0 +1,334 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""单夹爪场景的 Fays S80M 租约(无 device_setup 清单版本)。
|
| 2 |
+
|
| 3 |
+
主程序一次只服务一套夹爪 rig,设备身份全部来自运行时探测:
|
| 4 |
+
1. ESP32 串口(VID/PID + USB serial)唯一匹配;
|
| 5 |
+
2. discover_fays_device_groups() 发现且仅发现一台完整 S80M;
|
| 6 |
+
3. USB 链路速度 >= FAYS_MIN_USB_SPEED_MBPS,否则拒绝(提示换 USB3 口);
|
| 7 |
+
4. probe_product_serial() 通过官方 SDK 探针读取真实产品序列号;
|
| 8 |
+
5. 按序列号定位 fays_config/fays_vikit_{serial}.yaml 与
|
| 9 |
+
dist/fays_opencv48/s80m_{serial}_stereo_inertial.yaml;
|
| 10 |
+
6. flock 租约 /tmp/ksq-gripper-fays-locks/{serial}.lock 防多开;
|
| 11 |
+
7. materialize 当前端口的运行时 SDK yaml,并在 /dev/shm 建 IPC 目录。
|
| 12 |
+
|
| 13 |
+
返回的 selected 字典携带 SlamProcessController 与相机服务组合所需字段
|
| 14 |
+
(ports/runtime_config/ipc_dir/trajectory/settings/executable/...)。
|
| 15 |
+
"""
|
| 16 |
+
|
| 17 |
+
from __future__ import annotations
|
| 18 |
+
|
| 19 |
+
import fcntl
|
| 20 |
+
import json
|
| 21 |
+
import os
|
| 22 |
+
import re
|
| 23 |
+
import shutil
|
| 24 |
+
import tempfile
|
| 25 |
+
import threading
|
| 26 |
+
import time
|
| 27 |
+
|
| 28 |
+
from core.gripper import paths
|
| 29 |
+
from core.gripper.fays_runtime import (
|
| 30 |
+
FAYS_MIN_USB_SPEED_MBPS,
|
| 31 |
+
build_fays_probe_env,
|
| 32 |
+
build_fays_runtime_env,
|
| 33 |
+
discover_esp32_devices,
|
| 34 |
+
discover_fays_device_groups,
|
| 35 |
+
materialize_fays_device_config,
|
| 36 |
+
validate_fays_sdk_access,
|
| 37 |
+
)
|
| 38 |
+
from core.gripper.fays_serial_probe import probe_product_serial
|
| 39 |
+
|
| 40 |
+
_INSTANCE_IPC_FILES = (
|
| 41 |
+
"orb_pose.json.tmp", "orb_pose.json", "orb_meta.json",
|
| 42 |
+
"orb_current_frame.jpg", "orb_control.json.tmp", "orb_control.json",
|
| 43 |
+
"orb_raw_stream.sock", "camera_calibration.yaml", "imu.yaml",
|
| 44 |
+
)
|
| 45 |
+
|
| 46 |
+
|
| 47 |
+
class GripperFaysError(RuntimeError):
|
| 48 |
+
"""夹爪 Fays 租约失败(设备缺失、速度不足、标定缺失、被占用)。"""
|
| 49 |
+
|
| 50 |
+
|
| 51 |
+
class SingleFaysLease:
|
| 52 |
+
"""一次打开夹爪的 Fays 租约;release() 幂等。"""
|
| 53 |
+
|
| 54 |
+
def __init__(self, *, logger=print):
|
| 55 |
+
self._logger = logger
|
| 56 |
+
self._lock = threading.RLock()
|
| 57 |
+
self._stream = None
|
| 58 |
+
self._selected = None
|
| 59 |
+
self._runtime_dir = None
|
| 60 |
+
self._ipc_dir = None
|
| 61 |
+
self._lock_path = None
|
| 62 |
+
|
| 63 |
+
@property
|
| 64 |
+
def ipc_dir(self):
|
| 65 |
+
return self._ipc_dir
|
| 66 |
+
|
| 67 |
+
@property
|
| 68 |
+
def runtime_config_path(self):
|
| 69 |
+
if self._selected is None:
|
| 70 |
+
raise GripperFaysError("Fays 租约尚未持有")
|
| 71 |
+
return self._selected["runtime_config"]
|
| 72 |
+
|
| 73 |
+
def is_acquired(self):
|
| 74 |
+
with self._lock:
|
| 75 |
+
return bool(
|
| 76 |
+
self._selected is not None
|
| 77 |
+
and self._stream is not None
|
| 78 |
+
and not self._stream.closed
|
| 79 |
+
)
|
| 80 |
+
|
| 81 |
+
def _match_esp(self, esp_serial):
|
| 82 |
+
"""按 USB serial 唯一匹配 ESP32 控制板,返回其 tty 设备路径。"""
|
| 83 |
+
devices = discover_esp32_devices()
|
| 84 |
+
matches = [
|
| 85 |
+
info for info in devices
|
| 86 |
+
if str(info.get("serial") or "").strip() == str(esp_serial).strip()
|
| 87 |
+
]
|
| 88 |
+
if len(matches) != 1:
|
| 89 |
+
available = ", ".join(
|
| 90 |
+
str(info.get("serial") or "<missing>") for info in devices
|
| 91 |
+
)
|
| 92 |
+
raise GripperFaysError(
|
| 93 |
+
"ESP32 控制板未唯一匹配: 期望 serial={} 当前=[{}]".format(
|
| 94 |
+
esp_serial, available)
|
| 95 |
+
)
|
| 96 |
+
return matches[0]
|
| 97 |
+
|
| 98 |
+
def _discover_unique_fays_group(self, esp):
|
| 99 |
+
"""按 ESP 的根端口关联 rig 内的 S80M,要求唯一命中。
|
| 100 |
+
|
| 101 |
+
Fays 是 USB3/FT602:插在 USB3 口时走 5000M 伴生总线,与 ESP/
|
| 102 |
+
触觉相机的 480M 总线 bus 号不同(如 ESP=7-2、Fays=8-2 是同一
|
| 103 |
+
物理口的双总线伴生),因此原程序的拓扑规则只按根端口关联
|
| 104 |
+
(见 uvc_camera_service._associate_group 的同名注释)。
|
| 105 |
+
同一台机器可能还插着主程序自己的独立 S80M(不同根端口),
|
| 106 |
+
必须按夹爪 rig 的拓扑范围区分,不能全机枚举;多台命中即报错
|
| 107 |
+
不猜归属。rig 内仍只允许 1 台完整 S80M。
|
| 108 |
+
多控制器机器(双夹爪)还必须带控制器身份比较:两套 rig 可能
|
| 109 |
+
都插在各自控制器的同号根端口上(实测都是根端口 2,控制器
|
| 110 |
+
0000:0a:00.0 与 0000:74:00.4),只比端口号会互相误配。
|
| 111 |
+
"""
|
| 112 |
+
esp_controller, esp_root = self._bus_root(
|
| 113 |
+
str(esp.get("physical_usb_path") or ""))
|
| 114 |
+
if not esp_controller or not esp_root:
|
| 115 |
+
raise GripperFaysError(
|
| 116 |
+
"ESP32 物理 USB 路径无效: {!r}".format(
|
| 117 |
+
esp.get("physical_usb_path")))
|
| 118 |
+
groups = tuple(discover_fays_device_groups())
|
| 119 |
+
same_rig = [
|
| 120 |
+
group for group in groups
|
| 121 |
+
if self._bus_root(group["physical_usb_path"])
|
| 122 |
+
== (esp_controller, esp_root)
|
| 123 |
+
]
|
| 124 |
+
if len(same_rig) != 1:
|
| 125 |
+
found = ", ".join(
|
| 126 |
+
str(g["physical_usb_path"]) for g in same_rig)
|
| 127 |
+
raise GripperFaysError(
|
| 128 |
+
"夹爪 rig(控制器 {} 根端口 {})内发现 {} 台 Fays S80M,"
|
| 129 |
+
"只允许 1 台: [{}]".format(
|
| 130 |
+
esp_controller, esp_root, len(same_rig), found))
|
| 131 |
+
return same_rig[0]
|
| 132 |
+
|
| 133 |
+
@staticmethod
|
| 134 |
+
def _bus_root(physical):
|
| 135 |
+
""""1-3.4.1" → (controller, "3")。
|
| 136 |
+
|
| 137 |
+
controller 取 /sys/bus/usb/devices/usb{bus} 的 realpath 父路径
|
| 138 |
+
(usbN 节点之上的 PCI devpath,如 .../0000:0a:00.0)——多控制器
|
| 139 |
+
机器上不同控制器的同号根端口必须区分;USB2/USB3 伴生双总线
|
| 140 |
+
(同一物理口 ESP=7-2、Fays=8-2)仍只按控制器+根端口关联,
|
| 141 |
+
不比较总线号,伴生容忍不变。
|
| 142 |
+
"""
|
| 143 |
+
bus, separator, ports = str(physical or "").partition("-")
|
| 144 |
+
root = ports.split(".")[0] if separator else ""
|
| 145 |
+
if not separator:
|
| 146 |
+
return "", ""
|
| 147 |
+
try:
|
| 148 |
+
real = os.path.realpath(
|
| 149 |
+
os.path.join("/sys", "bus", "usb", "devices", f"usb{bus}"))
|
| 150 |
+
except OSError:
|
| 151 |
+
return f"bus-{bus}", root # sysfs 不可读时退回总线区分
|
| 152 |
+
return os.path.dirname(real), root
|
| 153 |
+
|
| 154 |
+
def _probe_serial(self, group):
|
| 155 |
+
"""官方 SDK 探针读取产品序列号;USB 速度不足时给出换口提示。"""
|
| 156 |
+
try:
|
| 157 |
+
speed = validate_fays_sdk_access(group)
|
| 158 |
+
except Exception as exc:
|
| 159 |
+
raise GripperFaysError(
|
| 160 |
+
f"Fays USB 链路校验失败(疑似插在 USB2 口,"
|
| 161 |
+
f"SuperSpeed 需 >= {FAYS_MIN_USB_SPEED_MBPS:g}M): {exc}"
|
| 162 |
+
) from exc
|
| 163 |
+
if speed < FAYS_MIN_USB_SPEED_MBPS:
|
| 164 |
+
raise GripperFaysError(
|
| 165 |
+
"Fays USB 链路速度 {:.0f}M,低于 {:.0f}M SuperSpeed 要求;"
|
| 166 |
+
"请把夹爪 USB3 接头插到电脑的 USB3 口后重试".format(
|
| 167 |
+
speed, FAYS_MIN_USB_SPEED_MBPS)
|
| 168 |
+
)
|
| 169 |
+
try:
|
| 170 |
+
serial = probe_product_serial(
|
| 171 |
+
group["ports"],
|
| 172 |
+
environment=build_fays_probe_env(),
|
| 173 |
+
)
|
| 174 |
+
except Exception as exc:
|
| 175 |
+
raise GripperFaysError(
|
| 176 |
+
f"Fays SDK 产品序列号探测失败: {exc}"
|
| 177 |
+
) from exc
|
| 178 |
+
return serial, speed
|
| 179 |
+
|
| 180 |
+
def acquire(self, esp_serial):
|
| 181 |
+
"""锁定并返回 selected 字典;重复调用返回当前租约快照。"""
|
| 182 |
+
with self._lock:
|
| 183 |
+
if self.is_acquired():
|
| 184 |
+
return dict(self._selected)
|
| 185 |
+
self.release()
|
| 186 |
+
esp = self._match_esp(esp_serial)
|
| 187 |
+
group = self._discover_unique_fays_group(esp)
|
| 188 |
+
serial, speed = self._probe_serial(group)
|
| 189 |
+
sdk_yaml, orb_yaml = paths.per_device_fays_yamls(serial)
|
| 190 |
+
|
| 191 |
+
os.makedirs(paths.FAYS_LOCK_DIR, mode=0o700, exist_ok=True)
|
| 192 |
+
lease_key = re.sub(r"[^A-Za-z0-9_.-]+", "-", serial)
|
| 193 |
+
lock_path = os.path.join(
|
| 194 |
+
paths.FAYS_LOCK_DIR, f"{lease_key}.lock",
|
| 195 |
+
)
|
| 196 |
+
stream = open(lock_path, "a+", encoding="utf-8")
|
| 197 |
+
try:
|
| 198 |
+
fcntl.flock(stream.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB)
|
| 199 |
+
except BlockingIOError as exc:
|
| 200 |
+
stream.seek(0)
|
| 201 |
+
owner = stream.read().strip() or "unknown owner"
|
| 202 |
+
stream.close()
|
| 203 |
+
raise GripperFaysError(
|
| 204 |
+
f"Fays {serial} 已被另一进程占用: {owner}"
|
| 205 |
+
) from exc
|
| 206 |
+
|
| 207 |
+
runtime_dir = tempfile.mkdtemp(prefix="ksq-gripper-fays-")
|
| 208 |
+
# 双夹爪同进程各持一份租约:IPC 目录必须含租约身份(序列号),
|
| 209 |
+
# 否则第二台撞 orb_raw_stream.sock/orb_pose.json 导致 SLAM
|
| 210 |
+
# 初始化失败,且一方 release() 会连带删掉另一方的 IPC 文件。
|
| 211 |
+
instance_id = f"{lease_key}-{os.getpid()}"
|
| 212 |
+
ipc_dir = os.path.join("/dev/shm", f"ksq-gripper-{instance_id}")
|
| 213 |
+
os.makedirs(ipc_dir, mode=0o700, exist_ok=True)
|
| 214 |
+
try:
|
| 215 |
+
owner = {
|
| 216 |
+
"pid": os.getpid(),
|
| 217 |
+
"run_id": "collector-main",
|
| 218 |
+
"instance_id": instance_id,
|
| 219 |
+
"esp_serial": str(esp["serial"]),
|
| 220 |
+
"fays_serial": serial,
|
| 221 |
+
"physical_usb_path": group["physical_usb_path"],
|
| 222 |
+
}
|
| 223 |
+
stream.seek(0)
|
| 224 |
+
stream.truncate()
|
| 225 |
+
json.dump(owner, stream, ensure_ascii=False, sort_keys=True)
|
| 226 |
+
stream.flush()
|
| 227 |
+
os.fsync(stream.fileno())
|
| 228 |
+
runtime_config = materialize_fays_device_config(
|
| 229 |
+
group["ports"],
|
| 230 |
+
os.path.join(runtime_dir, "fays_vikit_runtime.yaml"),
|
| 231 |
+
sdk_yaml,
|
| 232 |
+
)
|
| 233 |
+
selected = dict(group)
|
| 234 |
+
selected.update({
|
| 235 |
+
"unit_id": f"gripper_{serial[-4:]}",
|
| 236 |
+
"esp_serial": str(esp["serial"]),
|
| 237 |
+
"product_serial": serial,
|
| 238 |
+
"calibration_serial": serial,
|
| 239 |
+
"mapping_source": "runtime_probe_sdk_serial",
|
| 240 |
+
"sdk_yaml": sdk_yaml,
|
| 241 |
+
"orb_yaml": orb_yaml,
|
| 242 |
+
"orb_binary": paths.FAYS_MARK_ONLY_BINARY,
|
| 243 |
+
"runtime_config": runtime_config,
|
| 244 |
+
"ipc_dir": ipc_dir,
|
| 245 |
+
"trajectory": os.path.join(
|
| 246 |
+
runtime_dir, "orb_trajectory.txt",
|
| 247 |
+
),
|
| 248 |
+
"work_dir": runtime_dir,
|
| 249 |
+
"lock_path": lock_path,
|
| 250 |
+
"usb_speed_mbps": speed,
|
| 251 |
+
"esp_tty": str(esp["device"]),
|
| 252 |
+
})
|
| 253 |
+
self._stream = stream
|
| 254 |
+
self._selected = selected
|
| 255 |
+
self._runtime_dir = runtime_dir
|
| 256 |
+
self._ipc_dir = ipc_dir
|
| 257 |
+
self._lock_path = lock_path
|
| 258 |
+
self._logger(
|
| 259 |
+
"[Gripper-Fays] 租约已锁定: serial={} "
|
| 260 |
+
"stereo={} imu={} speed={:g}M ipc={}".format(
|
| 261 |
+
serial,
|
| 262 |
+
group["ports"]["stereo_dev_port"],
|
| 263 |
+
group["ports"]["imu_dev_port"],
|
| 264 |
+
speed, ipc_dir,
|
| 265 |
+
)
|
| 266 |
+
)
|
| 267 |
+
return dict(selected)
|
| 268 |
+
except Exception:
|
| 269 |
+
try:
|
| 270 |
+
shutil.rmtree(runtime_dir, ignore_errors=True)
|
| 271 |
+
except Exception:
|
| 272 |
+
pass
|
| 273 |
+
try:
|
| 274 |
+
fcntl.flock(stream.fileno(), fcntl.LOCK_UN)
|
| 275 |
+
finally:
|
| 276 |
+
stream.close()
|
| 277 |
+
raise
|
| 278 |
+
|
| 279 |
+
def resolve_assignment(self, esp_serial):
|
| 280 |
+
"""相机服务所需的身份 assignment(不探测、不锁租约、不校验速度)。
|
| 281 |
+
|
| 282 |
+
UvcCameraServiceManager 只把 fays identity 用作同根端口关联与
|
| 283 |
+
错误文案,从不打开 Fays 相机;此处用实时拓扑推导的物理路径即可,
|
| 284 |
+
S80M 的官方 SDK 串行探测留给 acquire()(P3 全链,含 USB3 校验)。
|
| 285 |
+
"""
|
| 286 |
+
esp = self._match_esp(esp_serial)
|
| 287 |
+
group = self._discover_unique_fays_group(esp)
|
| 288 |
+
return {
|
| 289 |
+
"esp32": {"serial": str(esp["serial"])},
|
| 290 |
+
"fays": {
|
| 291 |
+
"product_serial": "runtime-unprobed",
|
| 292 |
+
"physical_usb_path": group["physical_usb_path"],
|
| 293 |
+
},
|
| 294 |
+
}
|
| 295 |
+
|
| 296 |
+
def snapshot(self):
|
| 297 |
+
with self._lock:
|
| 298 |
+
return (
|
| 299 |
+
json.loads(json.dumps(self._selected))
|
| 300 |
+
if self._selected is not None else None
|
| 301 |
+
)
|
| 302 |
+
|
| 303 |
+
def release(self):
|
| 304 |
+
with self._lock:
|
| 305 |
+
stream, selected = self._stream, self._selected
|
| 306 |
+
self._stream = None
|
| 307 |
+
self._selected = None
|
| 308 |
+
if stream is not None:
|
| 309 |
+
try:
|
| 310 |
+
fcntl.flock(stream.fileno(), fcntl.LOCK_UN)
|
| 311 |
+
finally:
|
| 312 |
+
stream.close()
|
| 313 |
+
if self._ipc_dir:
|
| 314 |
+
for leaf in _INSTANCE_IPC_FILES:
|
| 315 |
+
try:
|
| 316 |
+
os.unlink(os.path.join(self._ipc_dir, leaf))
|
| 317 |
+
except FileNotFoundError:
|
| 318 |
+
pass
|
| 319 |
+
try:
|
| 320 |
+
os.rmdir(self._ipc_dir)
|
| 321 |
+
except OSError:
|
| 322 |
+
pass
|
| 323 |
+
self._ipc_dir = None
|
| 324 |
+
if self._runtime_dir:
|
| 325 |
+
shutil.rmtree(self._runtime_dir, ignore_errors=True)
|
| 326 |
+
self._runtime_dir = None
|
| 327 |
+
if selected is not None:
|
| 328 |
+
self._logger(
|
| 329 |
+
"[Gripper-Fays] 租约已释放: serial={}".format(
|
| 330 |
+
selected.get("product_serial")
|
| 331 |
+
)
|
| 332 |
+
)
|
| 333 |
+
self._lock_path = None
|
| 334 |
+
return True
|
collector/core/gripper/paths.py
ADDED
|
@@ -0,0 +1,164 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""core/gripper 包的原生资源路径与运行时常量补丁。
|
| 2 |
+
|
| 3 |
+
夹爪 Python 代码与原生资源全部随本包交付(有意从 online/gripper_version1
|
| 4 |
+
分叉):原生二进制、官方 SDK、标定文件与 orb48_env 都镜像在
|
| 5 |
+
core/gripper/native/ 下(布局与 online/ 一致,保持 $ORIGIN 相对 RUNPATH
|
| 6 |
+
可解析);触觉 Sightac SDK 单独放在 core/gripper/sightac_sdk/(pyarmor
|
| 7 |
+
加密后随包入库)。主程序不再依赖 online/ 树。本模块是唯一路径解析点。
|
| 8 |
+
|
| 9 |
+
主程序导入 core.gripper 时由 __init__ 执行 patch_fays_runtime_constants(),
|
| 10 |
+
把 fays_runtime 的 PROJECT_ROOT/GRIPPER_DIR 等常量改指到 native/ 下,并向
|
| 11 |
+
build_fays_runtime_env 注入 orb48_env(与 1234 启动脚本的顺序一致)。
|
| 12 |
+
"""
|
| 13 |
+
|
| 14 |
+
from __future__ import annotations
|
| 15 |
+
|
| 16 |
+
import os
|
| 17 |
+
|
| 18 |
+
# 采集器仓库根(core/gripper/paths.py 的上三级)。
|
| 19 |
+
COLLECTOR_ROOT = os.path.dirname(os.path.dirname(os.path.dirname(
|
| 20 |
+
os.path.abspath(__file__))))
|
| 21 |
+
PACKAGE_ROOT = os.path.dirname(os.path.abspath(__file__))
|
| 22 |
+
|
| 23 |
+
NATIVE_ROOT = os.path.abspath(os.environ.get(
|
| 24 |
+
"KSQ_GRIPPER_NATIVE_ROOT",
|
| 25 |
+
os.path.join(PACKAGE_ROOT, "native"),
|
| 26 |
+
))
|
| 27 |
+
GRIPPER_DIR = os.path.join(NATIVE_ROOT, "gripper_version1")
|
| 28 |
+
FAYS_SDK_ROOT = os.path.join(NATIVE_ROOT, "FaysSense_VI_Kit_Release")
|
| 29 |
+
ORB_ROOT = os.path.join(NATIVE_ROOT, "ORB-SLAM")
|
| 30 |
+
DIST_ROOT = os.path.join(NATIVE_ROOT, "dist")
|
| 31 |
+
ORB48_ENV_DIR = os.path.join(FAYS_SDK_ROOT, "thirdparty", "orb48_env")
|
| 32 |
+
|
| 33 |
+
FAYS_MARK_ONLY_BINARY = os.path.join(
|
| 34 |
+
DIST_ROOT, "fays_opencv48", "bin",
|
| 35 |
+
"fayssense_orb_slam_sn219_opencv48_mark_only",
|
| 36 |
+
)
|
| 37 |
+
FAYS_CALIBRATION_PROBE = os.path.join(
|
| 38 |
+
DIST_ROOT, "fays_aikit", "bin", "fays_vikit_calibration_probe",
|
| 39 |
+
)
|
| 40 |
+
ORB_LIBRARY = os.path.join(
|
| 41 |
+
DIST_ROOT, "orb_mark_only", "lib", "libORB_SLAM3.so",
|
| 42 |
+
)
|
| 43 |
+
ORB_VOCABULARY = os.path.join(NATIVE_ROOT, "ORBvoc.txt")
|
| 44 |
+
CAMERA_SERVICE_DIR = os.path.join(NATIVE_ROOT, "camera_service")
|
| 45 |
+
CAMERA_SERVICE_BINARY = os.path.join(
|
| 46 |
+
CAMERA_SERVICE_DIR, "build", "ksq-camera-service",
|
| 47 |
+
)
|
| 48 |
+
DISCOVER_UVC_CONFIG_BINARY = os.path.join(
|
| 49 |
+
CAMERA_SERVICE_DIR, "build", "discover-uvc-config",
|
| 50 |
+
)
|
| 51 |
+
SIGHTAC_SDK_ROOT = os.path.abspath(os.environ.get(
|
| 52 |
+
"KSQ_SIGHTAC_ROOT",
|
| 53 |
+
os.path.join(PACKAGE_ROOT, "sightac_sdk"),
|
| 54 |
+
))
|
| 55 |
+
FAYS_CONFIG_DIR = os.path.join(GRIPPER_DIR, "fays_config")
|
| 56 |
+
ORB_DEVICE_CONFIG_DIR = os.path.join(DIST_ROOT, "fays_opencv48")
|
| 57 |
+
FAYS_LOCK_DIR = "/tmp/ksq-gripper-fays-locks"
|
| 58 |
+
|
| 59 |
+
|
| 60 |
+
def required_resources():
|
| 61 |
+
"""资源校验表:路径 -> 是否需要可执行位。全部就绪夹爪才对外可见。"""
|
| 62 |
+
return {
|
| 63 |
+
"Fays ORB 桥接": (FAYS_MARK_ONLY_BINARY, True),
|
| 64 |
+
"Fays 标定探测": (FAYS_CALIBRATION_PROBE, True),
|
| 65 |
+
"ORB-SLAM3 核心库": (ORB_LIBRARY, False),
|
| 66 |
+
"ORB 词典": (ORB_VOCABULARY, False),
|
| 67 |
+
"libuvc 相机服务": (CAMERA_SERVICE_BINARY, True),
|
| 68 |
+
"Sightac SDK": (os.path.join(SIGHTAC_SDK_ROOT, "api_new"), False),
|
| 69 |
+
"Fays SDK lib": (os.path.join(FAYS_SDK_ROOT, "lib"), False),
|
| 70 |
+
}
|
| 71 |
+
|
| 72 |
+
|
| 73 |
+
def gripper_resources_available():
|
| 74 |
+
"""全部原生资源就绪才允许夹爪设备出现在主程序设备列表中。"""
|
| 75 |
+
for path, executable in required_resources().values():
|
| 76 |
+
if not os.path.exists(path):
|
| 77 |
+
return False
|
| 78 |
+
if executable and not os.access(path, os.X_OK):
|
| 79 |
+
return False
|
| 80 |
+
return True
|
| 81 |
+
|
| 82 |
+
|
| 83 |
+
def per_device_fays_yamls(serial):
|
| 84 |
+
"""按 SDK 探测序列号定位这台 Fays 的 SDK 模板与 ORB 标定 yaml。"""
|
| 85 |
+
serial = str(serial or "").strip()
|
| 86 |
+
if not serial:
|
| 87 |
+
raise RuntimeError("Fays 序列号为空,无法定位设备标定")
|
| 88 |
+
sdk_yaml = os.path.join(FAYS_CONFIG_DIR, f"fays_vikit_{serial}.yaml")
|
| 89 |
+
orb_yaml = os.path.join(
|
| 90 |
+
ORB_DEVICE_CONFIG_DIR, f"s80m_{serial}_stereo_inertial.yaml",
|
| 91 |
+
)
|
| 92 |
+
missing = [
|
| 93 |
+
label for label, path in (
|
| 94 |
+
("SDK YAML", sdk_yaml), ("ORB YAML", orb_yaml),
|
| 95 |
+
)
|
| 96 |
+
if not os.path.isfile(path)
|
| 97 |
+
]
|
| 98 |
+
if missing:
|
| 99 |
+
raise RuntimeError(
|
| 100 |
+
"当前 Fays 缺少运行标定文件: serial={} 缺少={};"
|
| 101 |
+
"请先在夹爪上位机中运行 device_setup".format(
|
| 102 |
+
serial, ",".join(missing))
|
| 103 |
+
)
|
| 104 |
+
return sdk_yaml, orb_yaml
|
| 105 |
+
|
| 106 |
+
|
| 107 |
+
def patch_fays_runtime_constants():
|
| 108 |
+
"""把 fays_runtime 模块常量改指到 native/ 树,并注入 orb48_env。
|
| 109 |
+
|
| 110 |
+
必须在任何 `from core.gripper.fays_runtime import <常量>` 之前调用——
|
| 111 |
+
core/gripper/__init__.py 在包导入时最先执行,顺序天然保证。
|
| 112 |
+
"""
|
| 113 |
+
from core.gripper import fays_runtime as runtime
|
| 114 |
+
|
| 115 |
+
runtime.GRIPPER_DIR = GRIPPER_DIR
|
| 116 |
+
runtime.PROJECT_ROOT = NATIVE_ROOT
|
| 117 |
+
runtime.FAYS_SDK_ROOT = FAYS_SDK_ROOT
|
| 118 |
+
runtime.ORB_ROOT = ORB_ROOT
|
| 119 |
+
runtime.FAYS_MARK_ONLY_BINARY = FAYS_MARK_ONLY_BINARY
|
| 120 |
+
runtime.FAYS_ORB_BINARY = FAYS_MARK_ONLY_BINARY
|
| 121 |
+
runtime.ORB_VOCABULARY = ORB_VOCABULARY
|
| 122 |
+
runtime.ORB_LIBRARY = ORB_LIBRARY
|
| 123 |
+
# 派生常量在 fays_runtime 导入时已按旧根计算,必须逐一改写。
|
| 124 |
+
runtime.FAYS_DEVICE_CONFIG = os.path.join(
|
| 125 |
+
FAYS_SDK_ROOT, "config", "fays_vikit_s80m.yaml")
|
| 126 |
+
runtime.DEVICE_MANIFEST = os.path.join(
|
| 127 |
+
GRIPPER_DIR, "device_manifest.json")
|
| 128 |
+
runtime.ORB_WORK_DIR = os.path.join(
|
| 129 |
+
NATIVE_ROOT, "ORB-SLAM", "Examples", "fays")
|
| 130 |
+
runtime.ORB_TRAJECTORY = os.path.join(
|
| 131 |
+
NATIVE_ROOT, "ORB-SLAM", "traj.txt")
|
| 132 |
+
runtime.AIKIT_BRIDGE_BINARY = os.path.join(
|
| 133 |
+
DIST_ROOT, "fays_aikit", "bin", "fays_aikit_slam_bridge")
|
| 134 |
+
runtime.AIKIT_CLIENT_LIBRARY_DIR = os.path.join(
|
| 135 |
+
DIST_ROOT, "fays_aikit", "lib")
|
| 136 |
+
runtime.AIKIT_CLIENT_CONFIG = os.path.join(
|
| 137 |
+
DIST_ROOT, "fays_aikit", "config", "active_tracker_client.yaml")
|
| 138 |
+
runtime.AIKIT_FACTORY_CALIBRATION_DIR = os.path.join(
|
| 139 |
+
NATIVE_ROOT, "config", "calibration")
|
| 140 |
+
|
| 141 |
+
_original_build_env = runtime.build_fays_runtime_env
|
| 142 |
+
|
| 143 |
+
def build_fays_runtime_env():
|
| 144 |
+
environment = _original_build_env()
|
| 145 |
+
orb48_lib = os.path.join(ORB48_ENV_DIR, "lib")
|
| 146 |
+
if not os.path.isdir(orb48_lib):
|
| 147 |
+
return environment
|
| 148 |
+
# 原版优先目录缺少 orb48_env(opencv4.8/pangolin 等桥接依赖)。
|
| 149 |
+
# 按 1234 启动脚本的固定顺序插在 ft602 之后、ORB 库之前。
|
| 150 |
+
parts = [
|
| 151 |
+
part for part in
|
| 152 |
+
environment.get("LD_LIBRARY_PATH", "").split(os.pathsep)
|
| 153 |
+
if part
|
| 154 |
+
]
|
| 155 |
+
insert_at = len(parts)
|
| 156 |
+
for index, part in enumerate(parts):
|
| 157 |
+
if os.path.basename(part).startswith("ft602-linux"):
|
| 158 |
+
insert_at = index + 1
|
| 159 |
+
break
|
| 160 |
+
parts.insert(insert_at, orb48_lib)
|
| 161 |
+
environment["LD_LIBRARY_PATH"] = os.pathsep.join(parts)
|
| 162 |
+
return environment
|
| 163 |
+
|
| 164 |
+
runtime.build_fays_runtime_env = build_fays_runtime_env
|
collector/core/gripper/recording/__init__.py
ADDED
|
@@ -0,0 +1,7 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""core.gripper 录制子集:只保留 raw 双目/IMU 流客户端。
|
| 2 |
+
|
| 3 |
+
原版 barrel 引用的 controller/dataset_index/episode_writer/lerobot_v3/
|
| 4 |
+
quality/session 未复制;主程序录制走 core/egodata_writer.py。
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from __future__ import annotations
|
collector/core/gripper/recording/fays_raw_client.py
ADDED
|
@@ -0,0 +1,314 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Client for the Fays ORB bridge's raw stereo/IMU recording socket.
|
| 2 |
+
|
| 3 |
+
主程序适配版:原始上位机把包直接交给 LerobotV3 writer;这里改为
|
| 4 |
+
``on_stereo`` 回调(bridge 拆左右目、只取左目转 BGR 投递 UI)。
|
| 5 |
+
IMU 原始数据不累积、不外送:SLAM 解算在桥接进程内部完成,
|
| 6 |
+
主程序只落盘 SLAM 位姿/轨迹(imu_packets 仅作包级诊断计数)。
|
| 7 |
+
"""
|
| 8 |
+
|
| 9 |
+
from __future__ import annotations
|
| 10 |
+
|
| 11 |
+
import socket
|
| 12 |
+
import struct
|
| 13 |
+
import threading
|
| 14 |
+
import time
|
| 15 |
+
from typing import Callable, Optional
|
| 16 |
+
|
| 17 |
+
|
| 18 |
+
RAW_HEADER = struct.Struct("<6I3Q4i2hi2I")
|
| 19 |
+
RAW_STREAM_MAGIC = 0x53544F55
|
| 20 |
+
RAW_STREAM_VERSION = 1
|
| 21 |
+
RAW_PACKET_STEREO = 1
|
| 22 |
+
RAW_PACKET_IMU = 2
|
| 23 |
+
HEADER_SIZE = 80
|
| 24 |
+
IMU_PAYLOAD_SIZE = 48
|
| 25 |
+
# 断线重连节奏:1s 一次尝试,累计 60s 仍连不上才算致命错误
|
| 26 |
+
RECONNECT_ATTEMPT_INTERVAL_S = 1.0
|
| 27 |
+
RECONNECT_TOTAL_TIMEOUT_S = 60.0
|
| 28 |
+
|
| 29 |
+
|
| 30 |
+
class FaysRawStreamError(RuntimeError):
|
| 31 |
+
"""The native raw stream is incomplete or malformed."""
|
| 32 |
+
|
| 33 |
+
|
| 34 |
+
class FaysRawStreamClient:
|
| 35 |
+
"""Convert one native raw stream into bridge stereo/IMU callbacks."""
|
| 36 |
+
|
| 37 |
+
def __init__(
|
| 38 |
+
self,
|
| 39 |
+
socket_path: str,
|
| 40 |
+
on_stereo: Callable[..., None],
|
| 41 |
+
*,
|
| 42 |
+
logger: Callable[[str], object] = print,
|
| 43 |
+
on_error: Optional[Callable[[str], None]] = None,
|
| 44 |
+
on_reconnected: Optional[Callable[[], None]] = None,
|
| 45 |
+
raw_cpu: Optional[int] = None,
|
| 46 |
+
) -> None:
|
| 47 |
+
self.socket_path = str(socket_path)
|
| 48 |
+
self._on_stereo = on_stereo
|
| 49 |
+
self._logger = logger
|
| 50 |
+
self._on_error = on_error
|
| 51 |
+
# 断线重连成功回调(bridge 用来清 paired 待配对帧 + 重置
|
| 52 |
+
# 空桶看门狗:断连窗口的空桶是显示侧问题,不计入取帧质量)
|
| 53 |
+
self._on_reconnected = on_reconnected
|
| 54 |
+
# 接收线程专用核(P5 补丁):None = 不绑(继承掩码即可)
|
| 55 |
+
self._raw_cpu = raw_cpu
|
| 56 |
+
self._stop = threading.Event()
|
| 57 |
+
self._thread: Optional[threading.Thread] = None
|
| 58 |
+
self._sock: Optional[socket.socket] = None
|
| 59 |
+
self._lock = threading.Lock()
|
| 60 |
+
self._last_stereo_sequence: Optional[int] = None
|
| 61 |
+
self._last_stereo_timestamp_ns = 0
|
| 62 |
+
self._reconnect_count = 0
|
| 63 |
+
self.imu_packets = 0
|
| 64 |
+
self.stereo_packets = 0
|
| 65 |
+
self.connected = threading.Event()
|
| 66 |
+
self.first_packet = threading.Event()
|
| 67 |
+
self.error: Optional[str] = None
|
| 68 |
+
|
| 69 |
+
@property
|
| 70 |
+
def alive(self) -> bool:
|
| 71 |
+
thread = self._thread
|
| 72 |
+
return bool(thread and thread.is_alive())
|
| 73 |
+
|
| 74 |
+
def start(self) -> None:
|
| 75 |
+
with self._lock:
|
| 76 |
+
if self._thread is not None:
|
| 77 |
+
raise RuntimeError("Fays raw stream is already running")
|
| 78 |
+
self._stop.clear()
|
| 79 |
+
self.error = None
|
| 80 |
+
thread = threading.Thread(
|
| 81 |
+
target=self._run,
|
| 82 |
+
name="fays-raw-stream",
|
| 83 |
+
daemon=False,
|
| 84 |
+
)
|
| 85 |
+
self._thread = thread
|
| 86 |
+
thread.start()
|
| 87 |
+
|
| 88 |
+
def stop(self, timeout: float = 3.0) -> None:
|
| 89 |
+
self._stop.set()
|
| 90 |
+
with self._lock:
|
| 91 |
+
sock = self._sock
|
| 92 |
+
if sock is not None:
|
| 93 |
+
try:
|
| 94 |
+
sock.shutdown(socket.SHUT_RDWR)
|
| 95 |
+
except OSError:
|
| 96 |
+
pass
|
| 97 |
+
thread = self._thread
|
| 98 |
+
if thread is not None and thread.is_alive():
|
| 99 |
+
thread.join(timeout=max(0.1, float(timeout)))
|
| 100 |
+
with self._lock:
|
| 101 |
+
self._thread = None
|
| 102 |
+
if self._sock is not None:
|
| 103 |
+
try:
|
| 104 |
+
self._sock.close()
|
| 105 |
+
except OSError:
|
| 106 |
+
pass
|
| 107 |
+
self._sock = None
|
| 108 |
+
|
| 109 |
+
def _fail(self, message: str) -> None:
|
| 110 |
+
self.error = message
|
| 111 |
+
self._logger(f"[Gripper-Raw] Fays raw stream failed: {message}")
|
| 112 |
+
if self._on_error is not None:
|
| 113 |
+
self._on_error(message)
|
| 114 |
+
self._stop.set()
|
| 115 |
+
|
| 116 |
+
def _connect(self, deadline_s: float = 30.0) -> socket.socket:
|
| 117 |
+
# 原生桥接在 ORB 系统装载完成后才创建 socket(通常在
|
| 118 |
+
# [FAYS-CALIB] 输出之后数秒),比本客户端的启动时刻晚;
|
| 119 |
+
# 带重试等待其出现(bind/listen 竞争也一并覆盖)。
|
| 120 |
+
# deadline_s:首次连接 30s(SLAM 进程还在装载),断线重连
|
| 121 |
+
# 收短到 5s(服务端 accept 循环常驻,正常秒级即可连上)
|
| 122 |
+
deadline = time.monotonic() + float(deadline_s)
|
| 123 |
+
last_error: Optional[OSError] = None
|
| 124 |
+
while not self._stop.is_set():
|
| 125 |
+
try:
|
| 126 |
+
sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
| 127 |
+
sock.settimeout(5.0)
|
| 128 |
+
# 加大接收缓冲,吸收 native 抓帧线程被瞬时抢占时的突发:
|
| 129 |
+
# 每包 ~1.4MB(1280x480x2+IMU),2MB 申请值内核翻倍后
|
| 130 |
+
# 可容纳 ≥2 包突发,避免窗口抖动导致丢包降帧
|
| 131 |
+
try:
|
| 132 |
+
sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF,
|
| 133 |
+
2 * 1024 * 1024)
|
| 134 |
+
except OSError:
|
| 135 |
+
pass
|
| 136 |
+
sock.connect(self.socket_path)
|
| 137 |
+
sock.settimeout(0.2)
|
| 138 |
+
return sock
|
| 139 |
+
except OSError as exc:
|
| 140 |
+
last_error = exc
|
| 141 |
+
if time.monotonic() >= deadline:
|
| 142 |
+
raise FaysRawStreamError(
|
| 143 |
+
f"raw socket {self.socket_path} not ready "
|
| 144 |
+
f"after 30s: {exc}"
|
| 145 |
+
) from exc
|
| 146 |
+
time.sleep(0.5)
|
| 147 |
+
raise FaysRawStreamError(
|
| 148 |
+
"Fays raw stream client stopped before connect")
|
| 149 |
+
|
| 150 |
+
def _recv_exact(self, sock: socket.socket, size: int) -> bytes:
|
| 151 |
+
chunks = bytearray()
|
| 152 |
+
while len(chunks) < size and not self._stop.is_set():
|
| 153 |
+
try:
|
| 154 |
+
chunk = sock.recv(size - len(chunks))
|
| 155 |
+
except socket.timeout:
|
| 156 |
+
continue
|
| 157 |
+
except OSError as exc:
|
| 158 |
+
if self._stop.is_set():
|
| 159 |
+
return bytes(chunks)
|
| 160 |
+
raise FaysRawStreamError(f"socket read failed: {exc}") from exc
|
| 161 |
+
if not chunk:
|
| 162 |
+
if self._stop.is_set():
|
| 163 |
+
return bytes(chunks)
|
| 164 |
+
raise FaysRawStreamError("Fays raw stream closed unexpectedly")
|
| 165 |
+
chunks.extend(chunk)
|
| 166 |
+
return bytes(chunks)
|
| 167 |
+
|
| 168 |
+
def _close_sock(self, sock) -> None:
|
| 169 |
+
with self._lock:
|
| 170 |
+
if self._sock is sock:
|
| 171 |
+
self._sock = None
|
| 172 |
+
try:
|
| 173 |
+
sock.close()
|
| 174 |
+
except OSError:
|
| 175 |
+
pass
|
| 176 |
+
|
| 177 |
+
def _stream_loop(self, sock: socket.socket) -> None:
|
| 178 |
+
while not self._stop.is_set():
|
| 179 |
+
header_bytes = self._recv_exact(sock, HEADER_SIZE)
|
| 180 |
+
if not header_bytes:
|
| 181 |
+
break
|
| 182 |
+
header = RAW_HEADER.unpack(header_bytes)
|
| 183 |
+
(
|
| 184 |
+
magic, version, kind, header_size, payload_size,
|
| 185 |
+
_reserved0, sensor_timestamp_ns, host_monotonic_ns,
|
| 186 |
+
host_realtime_ns, sequence, width, height, channels,
|
| 187 |
+
encoding, _reserved1, step, _reserved2, _reserved3,
|
| 188 |
+
) = header
|
| 189 |
+
if magic != RAW_STREAM_MAGIC or version != RAW_STREAM_VERSION:
|
| 190 |
+
raise FaysRawStreamError("unknown raw stream protocol")
|
| 191 |
+
if header_size != HEADER_SIZE:
|
| 192 |
+
raise FaysRawStreamError(
|
| 193 |
+
f"unexpected raw header size {header_size}")
|
| 194 |
+
payload = self._recv_exact(sock, payload_size)
|
| 195 |
+
if len(payload) != payload_size:
|
| 196 |
+
break
|
| 197 |
+
if kind == RAW_PACKET_IMU:
|
| 198 |
+
if payload_size != IMU_PAYLOAD_SIZE:
|
| 199 |
+
raise FaysRawStreamError(
|
| 200 |
+
f"invalid IMU payload size {payload_size}")
|
| 201 |
+
# 只做包级诊断计数:IMU 原始数据不外送(SLAM 解算
|
| 202 |
+
# 在桥接进程内部完成,主程序只落盘 SLAM 位姿/轨迹)
|
| 203 |
+
self.imu_packets += 1
|
| 204 |
+
self.first_packet.set()
|
| 205 |
+
continue
|
| 206 |
+
if kind != RAW_PACKET_STEREO:
|
| 207 |
+
raise FaysRawStreamError(f"unknown raw packet kind {kind}")
|
| 208 |
+
if channels not in (1, 3) or width <= 0 or height <= 0:
|
| 209 |
+
raise FaysRawStreamError(
|
| 210 |
+
f"invalid stereo frame {width}x{height}x{channels}")
|
| 211 |
+
normalized_step = int(step) if int(step) > 0 else (
|
| 212 |
+
int(width) * int(channels))
|
| 213 |
+
if payload_size < normalized_step * height:
|
| 214 |
+
raise FaysRawStreamError(
|
| 215 |
+
f"short stereo payload: {payload_size} bytes")
|
| 216 |
+
# 时间戳单调校验仅对 side_by_side(单包含双目)成立:
|
| 217 |
+
# paired_packets 模式左右目交替到达,两目传感器时钟有
|
| 218 |
+
# ~20ms 固定偏移,全局比较会把正常交错误判为回退
|
| 219 |
+
side_by_side = int(width) >= int(height) * 3
|
| 220 |
+
if (side_by_side and self._last_stereo_timestamp_ns and (
|
| 221 |
+
sensor_timestamp_ns <= self._last_stereo_timestamp_ns)):
|
| 222 |
+
raise FaysRawStreamError(
|
| 223 |
+
"Fays stereo timestamp regression: "
|
| 224 |
+
f"previous={self._last_stereo_timestamp_ns} "
|
| 225 |
+
f"current={sensor_timestamp_ns}")
|
| 226 |
+
self._last_stereo_sequence = int(sequence)
|
| 227 |
+
self._last_stereo_timestamp_ns = int(sensor_timestamp_ns)
|
| 228 |
+
self.stereo_packets += 1
|
| 229 |
+
self.first_packet.set()
|
| 230 |
+
self._on_stereo(
|
| 231 |
+
payload,
|
| 232 |
+
int(sensor_timestamp_ns),
|
| 233 |
+
int(host_monotonic_ns),
|
| 234 |
+
int(sequence),
|
| 235 |
+
int(width),
|
| 236 |
+
int(height),
|
| 237 |
+
int(channels),
|
| 238 |
+
normalized_step,
|
| 239 |
+
int(encoding),
|
| 240 |
+
)
|
| 241 |
+
if not self._stop.is_set():
|
| 242 |
+
raise FaysRawStreamError("Fays raw stream closed by bridge")
|
| 243 |
+
|
| 244 |
+
def _run(self) -> None:
|
| 245 |
+
# 线程级自绑专用核(P5 补丁):接收线程负载轻但节律敏感,
|
| 246 |
+
# 独占一个 SMT 核可保证 native 30fps 取帧不被主进程重线程
|
| 247 |
+
# 抢占;失败不致命(继承掩码已避开 SLAM 分区)
|
| 248 |
+
if self._raw_cpu is not None:
|
| 249 |
+
from core.gripper.affinity import pin_raw_thread
|
| 250 |
+
if pin_raw_thread(self._raw_cpu):
|
| 251 |
+
self._logger(f"[Gripper-Raw] 接收线程已绑定专用核 CPU{self._raw_cpu}")
|
| 252 |
+
else:
|
| 253 |
+
self._logger("[Gripper-Raw] 接收线程绑核失败(沿用继承掩码)")
|
| 254 |
+
# 断线自动重连:服务端(SLAM 进程)在客户端短暂卡顿导致其
|
| 255 |
+
# 原始队列溢出时会主动断开连接(stereo loss detected →
|
| 256 |
+
# close);显示链路重连即可恢复,不视为致命。只有首连失败
|
| 257 |
+
# 或重连持续失败超时才算错误。
|
| 258 |
+
first_connection = True
|
| 259 |
+
reconnect_deadline = 0.0
|
| 260 |
+
while not self._stop.is_set():
|
| 261 |
+
try:
|
| 262 |
+
sock = self._connect(
|
| 263 |
+
deadline_s=30.0 if first_connection else 5.0)
|
| 264 |
+
except FaysRawStreamError as exc:
|
| 265 |
+
if self._stop.is_set():
|
| 266 |
+
return
|
| 267 |
+
if first_connection:
|
| 268 |
+
# 首连失败=致命(SLAM 进程未起/ipc 目录缺失)
|
| 269 |
+
self._fail(str(exc))
|
| 270 |
+
return
|
| 271 |
+
if time.monotonic() >= reconnect_deadline:
|
| 272 |
+
self._fail(
|
| 273 |
+
"Fays raw stream reconnect failed for "
|
| 274 |
+
f"{RECONNECT_TOTAL_TIMEOUT_S:.0f}s: {exc}")
|
| 275 |
+
return
|
| 276 |
+
time.sleep(RECONNECT_ATTEMPT_INTERVAL_S)
|
| 277 |
+
continue
|
| 278 |
+
first_connection = False
|
| 279 |
+
reconnect_deadline = 0.0
|
| 280 |
+
with self._lock:
|
| 281 |
+
self._sock = sock
|
| 282 |
+
self.connected.set()
|
| 283 |
+
if self._reconnect_count:
|
| 284 |
+
self._logger(
|
| 285 |
+
"[Gripper-Raw] 原始流已重连(第 {} 次),显示链路恢复"
|
| 286 |
+
.format(self._reconnect_count))
|
| 287 |
+
if self._on_reconnected is not None:
|
| 288 |
+
try:
|
| 289 |
+
self._on_reconnected()
|
| 290 |
+
except Exception:
|
| 291 |
+
pass
|
| 292 |
+
else:
|
| 293 |
+
self._logger(
|
| 294 |
+
"[Gripper-Raw] Fays raw stereo/IMU stream connected")
|
| 295 |
+
try:
|
| 296 |
+
self._stream_loop(sock)
|
| 297 |
+
except FaysRawStreamError as exc:
|
| 298 |
+
self._close_sock(sock)
|
| 299 |
+
if self._stop.is_set():
|
| 300 |
+
return
|
| 301 |
+
self._reconnect_count += 1
|
| 302 |
+
self._logger(
|
| 303 |
+
"[Gripper-Raw] 原始流连接中断:{};{}s 后自动重连"
|
| 304 |
+
.format(exc, RECONNECT_ATTEMPT_INTERVAL_S))
|
| 305 |
+
reconnect_deadline = (
|
| 306 |
+
time.monotonic() + RECONNECT_TOTAL_TIMEOUT_S)
|
| 307 |
+
time.sleep(RECONNECT_ATTEMPT_INTERVAL_S)
|
| 308 |
+
continue
|
| 309 |
+
except Exception as exc:
|
| 310 |
+
self._close_sock(sock)
|
| 311 |
+
self._fail(str(exc))
|
| 312 |
+
return
|
| 313 |
+
self._close_sock(sock)
|
| 314 |
+
return
|
collector/core/gripper/runtime/__init__.py
ADDED
|
@@ -0,0 +1,7 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""core.gripper 运行时的最小子集:CPU 绑核、线程亲和与设备访问互斥。
|
| 2 |
+
|
| 3 |
+
主程序只依赖 cpu_policy/thread_affinity/device_access/interruptible_queue;
|
| 4 |
+
原版 barrel 引用的 app_config/log_policy/performance_reporter/shutdown 未复制。
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from __future__ import annotations
|
collector/core/gripper/runtime/cpu_policy.py
ADDED
|
@@ -0,0 +1,777 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""KSQ Gripper 的固定 CPU 角色和 Linux 混合核拓扑识别。
|
| 2 |
+
|
| 3 |
+
本模块只读取 affinity 与 sysfs。它不创建线程、不修改调度策略,也不接触
|
| 4 |
+
任何设备。实际 affinity 写入由 :mod:`runtime.thread_affinity` 独占。
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from __future__ import annotations
|
| 8 |
+
|
| 9 |
+
from dataclasses import dataclass
|
| 10 |
+
import os
|
| 11 |
+
from types import MappingProxyType
|
| 12 |
+
from typing import Callable, Iterable, Mapping, Optional, Sequence, Tuple
|
| 13 |
+
|
| 14 |
+
|
| 15 |
+
CPU_ROLE_ORDER = (
|
| 16 |
+
"input",
|
| 17 |
+
"fays_input",
|
| 18 |
+
"left_process",
|
| 19 |
+
"right_process",
|
| 20 |
+
"idle_smt",
|
| 21 |
+
"fays_prepare",
|
| 22 |
+
"fays_left_orb",
|
| 23 |
+
"fays_right_orb",
|
| 24 |
+
"fays_track",
|
| 25 |
+
"fays_background",
|
| 26 |
+
"general",
|
| 27 |
+
)
|
| 28 |
+
|
| 29 |
+
DEDICATED_THREAD_ROLES = frozenset({
|
| 30 |
+
"input",
|
| 31 |
+
"fays_input",
|
| 32 |
+
"left_process",
|
| 33 |
+
"right_process",
|
| 34 |
+
"fays_prepare",
|
| 35 |
+
"fays_left_orb",
|
| 36 |
+
"fays_right_orb",
|
| 37 |
+
"fays_track",
|
| 38 |
+
})
|
| 39 |
+
|
| 40 |
+
RESERVED_ROLES = DEDICATED_THREAD_ROLES | frozenset({
|
| 41 |
+
"idle_smt",
|
| 42 |
+
"fays_background",
|
| 43 |
+
})
|
| 44 |
+
|
| 45 |
+
FAYS_PROCESS_ROLES = (
|
| 46 |
+
"fays_input",
|
| 47 |
+
"fays_prepare",
|
| 48 |
+
"fays_left_orb",
|
| 49 |
+
"fays_right_orb",
|
| 50 |
+
"fays_track",
|
| 51 |
+
"fays_background",
|
| 52 |
+
)
|
| 53 |
+
|
| 54 |
+
EXPECTED_ROLE_CPUS = MappingProxyType({
|
| 55 |
+
"input": (4,),
|
| 56 |
+
"fays_input": (4,),
|
| 57 |
+
"left_process": (0,),
|
| 58 |
+
"right_process": (2,),
|
| 59 |
+
"idle_smt": (1, 3),
|
| 60 |
+
"fays_prepare": (5,),
|
| 61 |
+
"fays_left_orb": (6,),
|
| 62 |
+
"fays_right_orb": (7,),
|
| 63 |
+
"fays_track": (8,),
|
| 64 |
+
"fays_background": (9,),
|
| 65 |
+
"general": (10, 11),
|
| 66 |
+
})
|
| 67 |
+
|
| 68 |
+
|
| 69 |
+
def parse_cpu_list(value: object) -> frozenset[int]:
|
| 70 |
+
"""解析 Linux sysfs 的 ``0-3,8`` CPU 列表格式。
|
| 71 |
+
|
| 72 |
+
与 sysfs 格式一致,空字段会被忽略;负数、倒序范围和非数字字段会抛出
|
| 73 |
+
:class:`ValueError`,防止错误拓扑被静默接受。
|
| 74 |
+
"""
|
| 75 |
+
|
| 76 |
+
cpus: set[int] = set()
|
| 77 |
+
for encoded_part in str(value).strip().split(","):
|
| 78 |
+
part = encoded_part.strip()
|
| 79 |
+
if not part:
|
| 80 |
+
continue
|
| 81 |
+
if "-" in part:
|
| 82 |
+
start_text, separator, end_text = part.partition("-")
|
| 83 |
+
if not separator or not start_text or not end_text:
|
| 84 |
+
raise ValueError(f"invalid CPU range: {part!r}")
|
| 85 |
+
start = int(start_text, 10)
|
| 86 |
+
end = int(end_text, 10)
|
| 87 |
+
if start < 0 or end < start:
|
| 88 |
+
raise ValueError(f"invalid CPU range: {part!r}")
|
| 89 |
+
cpus.update(range(start, end + 1))
|
| 90 |
+
else:
|
| 91 |
+
cpu = int(part, 10)
|
| 92 |
+
if cpu < 0:
|
| 93 |
+
raise ValueError(f"invalid CPU id: {part!r}")
|
| 94 |
+
cpus.add(cpu)
|
| 95 |
+
return frozenset(cpus)
|
| 96 |
+
|
| 97 |
+
|
| 98 |
+
@dataclass(frozen=True)
|
| 99 |
+
class CoreTopology:
|
| 100 |
+
"""一个物理核在当前进程 affinity 范围内可见的拓扑。"""
|
| 101 |
+
|
| 102 |
+
cpus: Tuple[int, ...]
|
| 103 |
+
sibling_count: int = 1
|
| 104 |
+
max_freq: int = 0
|
| 105 |
+
core_type: Optional[int] = None
|
| 106 |
+
package_id: int = 0
|
| 107 |
+
core_id: int = 0
|
| 108 |
+
|
| 109 |
+
def __post_init__(self) -> None:
|
| 110 |
+
normalized = tuple(sorted({int(cpu) for cpu in self.cpus}))
|
| 111 |
+
if not normalized or normalized[0] < 0:
|
| 112 |
+
raise ValueError("a physical core must contain non-negative CPUs")
|
| 113 |
+
object.__setattr__(self, "cpus", normalized)
|
| 114 |
+
object.__setattr__(
|
| 115 |
+
self, "sibling_count", max(1, int(self.sibling_count)))
|
| 116 |
+
object.__setattr__(self, "max_freq", max(0, int(self.max_freq)))
|
| 117 |
+
|
| 118 |
+
|
| 119 |
+
@dataclass(frozen=True)
|
| 120 |
+
class CpuPolicySnapshot:
|
| 121 |
+
"""不可变的 CPU0-11 角色分区。"""
|
| 122 |
+
|
| 123 |
+
source: str
|
| 124 |
+
input: Tuple[int, ...] = (4,)
|
| 125 |
+
fays_input: Tuple[int, ...] = (4,)
|
| 126 |
+
left_process: Tuple[int, ...] = (0,)
|
| 127 |
+
right_process: Tuple[int, ...] = (2,)
|
| 128 |
+
idle_smt: Tuple[int, ...] = (1, 3)
|
| 129 |
+
fays_prepare: Tuple[int, ...] = (5,)
|
| 130 |
+
fays_left_orb: Tuple[int, ...] = (6,)
|
| 131 |
+
fays_right_orb: Tuple[int, ...] = (7,)
|
| 132 |
+
fays_track: Tuple[int, ...] = (8,)
|
| 133 |
+
fays_background: Tuple[int, ...] = (9,)
|
| 134 |
+
general: Tuple[int, ...] = (10, 11)
|
| 135 |
+
_dual_slam: bool = False
|
| 136 |
+
_balanced_dual_slam: bool = False
|
| 137 |
+
_relaxed: bool = False
|
| 138 |
+
|
| 139 |
+
def __post_init__(self) -> None:
|
| 140 |
+
for role in CPU_ROLE_ORDER:
|
| 141 |
+
cpus = tuple(int(cpu) for cpu in getattr(self, role))
|
| 142 |
+
if not cpus or len(cpus) != len(set(cpus)) or min(cpus) < 0:
|
| 143 |
+
raise ValueError(f"invalid CPU role {role!r}: {cpus!r}")
|
| 144 |
+
object.__setattr__(self, role, cpus)
|
| 145 |
+
if self._relaxed:
|
| 146 |
+
self.validate_relaxed()
|
| 147 |
+
elif self._balanced_dual_slam:
|
| 148 |
+
self.validate_balanced_dual_slam()
|
| 149 |
+
elif self._dual_slam:
|
| 150 |
+
self.validate_dual_slam()
|
| 151 |
+
else:
|
| 152 |
+
self.validate_strict()
|
| 153 |
+
|
| 154 |
+
@classmethod
|
| 155 |
+
def dual_slam(
|
| 156 |
+
cls,
|
| 157 |
+
track_cpu: int = 6,
|
| 158 |
+
input_cpu: int = 5,
|
| 159 |
+
) -> "CpuPolicySnapshot":
|
| 160 |
+
"""双设备实例:第一套 input=4/track=8,第二套 input/fays_input=5、
|
| 161 |
+
track/output=6;其余角色与单设备严格分区一致(general=10/11)。"""
|
| 162 |
+
return cls(
|
| 163 |
+
source="dual_slam",
|
| 164 |
+
input=(int(input_cpu),),
|
| 165 |
+
fays_input=(int(input_cpu),),
|
| 166 |
+
fays_track=(int(track_cpu),),
|
| 167 |
+
general=(10, 11),
|
| 168 |
+
_dual_slam=True,
|
| 169 |
+
)
|
| 170 |
+
|
| 171 |
+
@classmethod
|
| 172 |
+
def exclusive_dual_slam(
|
| 173 |
+
cls,
|
| 174 |
+
*,
|
| 175 |
+
secondary: bool,
|
| 176 |
+
) -> "CpuPolicySnapshot":
|
| 177 |
+
"""把两套 Fays/ORB 子进程放进互不重叠的物理核域。
|
| 178 |
+
|
| 179 |
+
当前主机 SMT 关系为 ``(0,6) (1,7) (2,8) (3,9) (4,10)
|
| 180 |
+
(5,11)``。第一套 Fays/ORB 使用物理核 0/1,第二套使用物理核
|
| 181 |
+
2/3;两套 ORB 的重负载阶段不再落到对方的 SMT 兄弟上。
|
| 182 |
+
|
| 183 |
+
2026-08-27 双设备共用 GUI 修正:GUI 和 camera-service 固定保留在
|
| 184 |
+
CPU10/11,不再跟随最后连接设备切换。四路 Sightac 计算分散到
|
| 185 |
+
CPU4/5/1/3。CPU0/2 是两套 Fays Track/Output 主线程,不能与 Sightac
|
| 186 |
+
重计算同核;CPU1/3 只承担较短的 Fays prepare/background 突发,
|
| 187 |
+
竞争代价远小于直接阻塞 Track。Sightac 输入线程主要阻塞在 UVC
|
| 188 |
+
读取,允许与对应计算子进程共用同一逻辑核。
|
| 189 |
+
"""
|
| 190 |
+
if secondary:
|
| 191 |
+
return cls(
|
| 192 |
+
source="exclusive_dual_slam_secondary",
|
| 193 |
+
input=(1, 3),
|
| 194 |
+
fays_input=(2,),
|
| 195 |
+
left_process=(1,),
|
| 196 |
+
right_process=(3,),
|
| 197 |
+
idle_smt=(4, 5),
|
| 198 |
+
fays_prepare=(3,),
|
| 199 |
+
fays_left_orb=(7,),
|
| 200 |
+
fays_right_orb=(7,),
|
| 201 |
+
fays_track=(2,),
|
| 202 |
+
fays_background=(3,),
|
| 203 |
+
general=(10, 11),
|
| 204 |
+
_relaxed=True,
|
| 205 |
+
)
|
| 206 |
+
return cls(
|
| 207 |
+
source="exclusive_dual_slam_primary",
|
| 208 |
+
input=(4, 5),
|
| 209 |
+
fays_input=(0,),
|
| 210 |
+
left_process=(4,),
|
| 211 |
+
right_process=(5,),
|
| 212 |
+
idle_smt=(2, 3, 8, 9),
|
| 213 |
+
fays_prepare=(1,),
|
| 214 |
+
fays_left_orb=(6,),
|
| 215 |
+
fays_right_orb=(6,),
|
| 216 |
+
fays_track=(0,),
|
| 217 |
+
fays_background=(1,),
|
| 218 |
+
general=(10, 11),
|
| 219 |
+
_relaxed=True,
|
| 220 |
+
)
|
| 221 |
+
|
| 222 |
+
@classmethod
|
| 223 |
+
def balanced_dual_slam(
|
| 224 |
+
cls,
|
| 225 |
+
*,
|
| 226 |
+
secondary: bool,
|
| 227 |
+
) -> "CpuPolicySnapshot":
|
| 228 |
+
"""双设备正式性能方案。
|
| 229 |
+
|
| 230 |
+
当前主机的 SMT 关系为 (0,6)、(1,7)、(2,8)、(3,9)、
|
| 231 |
+
(4,10)、(5,11)。清单顺序中的主设备使用
|
| 232 |
+
Sightac input=4/5、Fays callback=4、prepare=5、ORB=6/7、
|
| 233 |
+
track=8;次设备把对应阶段放到 Sightac input=10/11、Fays
|
| 234 |
+
callback=10、prepare=11、ORB=0/1、track=2。
|
| 235 |
+
四个 Sightac 计算进程分散到三个物理核的四个逻辑 CPU:主设备
|
| 236 |
+
left/right=1/5,次设备 left/right=9/11。设备1 Left 不再与
|
| 237 |
+
UI/general 线程共用 CPU3;CPU1 是设备2右目 ORB CPU7 的 SMT 兄弟,
|
| 238 |
+
这是当前双设备满载下避免 CPU3 抢占的明确取舍。CPU5/11 与负载
|
| 239 |
+
相对较低的 Fays 预处理阶段共享,避免占用 Track 固定逻辑 CPU。
|
| 240 |
+
普通/UI线程继续放在 CPU3。
|
| 241 |
+
|
| 242 |
+
C++ mark_only 二进制的显式 MP cleanup 初始使用 background=CPU9。
|
| 243 |
+
LocalMapping 和 LoopClosing 保留各自 ORB 子进程的完整 CPU 域,不再
|
| 244 |
+
被外部审计器持续压到单个 CPU9。
|
| 245 |
+
"""
|
| 246 |
+
if secondary:
|
| 247 |
+
return cls(
|
| 248 |
+
source="balanced_dual_slam_secondary",
|
| 249 |
+
# Sightac 的两个读取线程允许在 10/11 之间调度;Fays
|
| 250 |
+
# SDK 回调仍固定在 10,避免所有输入工作都锁在一个逻辑核。
|
| 251 |
+
input=(10, 11),
|
| 252 |
+
fays_input=(10,),
|
| 253 |
+
idle_smt=(6, 7, 8),
|
| 254 |
+
fays_prepare=(11,),
|
| 255 |
+
fays_left_orb=(0,),
|
| 256 |
+
fays_right_orb=(1,),
|
| 257 |
+
fays_track=(2,),
|
| 258 |
+
fays_background=(9,),
|
| 259 |
+
left_process=(9,),
|
| 260 |
+
right_process=(11,),
|
| 261 |
+
general=(3,),
|
| 262 |
+
_balanced_dual_slam=True,
|
| 263 |
+
)
|
| 264 |
+
return cls(
|
| 265 |
+
source="balanced_dual_slam_primary",
|
| 266 |
+
# Sightac 的两个读取线程允许在 4/5 之间调度;Fays SDK
|
| 267 |
+
# 回调仍固定在 4。两者因此使用不同的角色和 affinity 域。
|
| 268 |
+
input=(4, 5),
|
| 269 |
+
fays_input=(4,),
|
| 270 |
+
idle_smt=(1,),
|
| 271 |
+
left_process=(1,),
|
| 272 |
+
right_process=(5,),
|
| 273 |
+
general=(3,),
|
| 274 |
+
_balanced_dual_slam=True,
|
| 275 |
+
)
|
| 276 |
+
|
| 277 |
+
def validate_balanced_dual_slam(self) -> None:
|
| 278 |
+
secondary = self.source == "balanced_dual_slam_secondary"
|
| 279 |
+
expected = dict(EXPECTED_ROLE_CPUS)
|
| 280 |
+
expected.update({
|
| 281 |
+
"input": (10, 11) if secondary else (4, 5),
|
| 282 |
+
"fays_input": (10,) if secondary else (4,),
|
| 283 |
+
"idle_smt": (1,) if not secondary else (6, 7, 8),
|
| 284 |
+
"left_process": (1,) if not secondary else (9,),
|
| 285 |
+
"right_process": (5,) if not secondary else (11,),
|
| 286 |
+
"general": (3,),
|
| 287 |
+
})
|
| 288 |
+
if secondary:
|
| 289 |
+
expected.update({
|
| 290 |
+
"fays_prepare": (11,),
|
| 291 |
+
"fays_left_orb": (0,),
|
| 292 |
+
"fays_right_orb": (1,),
|
| 293 |
+
"fays_track": (2,),
|
| 294 |
+
"fays_background": (9,),
|
| 295 |
+
"left_process": (9,),
|
| 296 |
+
"right_process": (11,),
|
| 297 |
+
})
|
| 298 |
+
actual = {
|
| 299 |
+
role: getattr(self, role) for role in CPU_ROLE_ORDER
|
| 300 |
+
}
|
| 301 |
+
if actual != expected:
|
| 302 |
+
raise ValueError(
|
| 303 |
+
"invalid balanced dual-SLAM CPU partition: "
|
| 304 |
+
f"expected={expected!r} actual={actual!r}"
|
| 305 |
+
)
|
| 306 |
+
# left/right_process are child-process domains, not parent Python
|
| 307 |
+
# threads. The primary left worker is kept off the UI/general CPU3.
|
| 308 |
+
# CPU1 is the SMT sibling of the secondary right ORB CPU7; this is the
|
| 309 |
+
# least-invasive available slot in the fully occupied dual-device
|
| 310 |
+
# layout. CPU5/11 carry the comparatively light Fays preprocess stages
|
| 311 |
+
# as well as one tactile worker each; no two tactile workers are pinned
|
| 312 |
+
# to the same logical CPU.
|
| 313 |
+
overlap = (
|
| 314 |
+
set(self.reserved)
|
| 315 |
+
- set(self.left_process)
|
| 316 |
+
- set(self.right_process)
|
| 317 |
+
) & set(self.general)
|
| 318 |
+
if overlap:
|
| 319 |
+
raise ValueError(
|
| 320 |
+
"invalid balanced dual-SLAM reserved/general overlap: "
|
| 321 |
+
f"expected=[] actual={sorted(overlap)}"
|
| 322 |
+
)
|
| 323 |
+
|
| 324 |
+
def validate_dual_slam(self) -> None:
|
| 325 |
+
expected = dict(EXPECTED_ROLE_CPUS)
|
| 326 |
+
expected.update({
|
| 327 |
+
"input": self.input,
|
| 328 |
+
"fays_input": self.fays_input,
|
| 329 |
+
"fays_track": self.fays_track,
|
| 330 |
+
})
|
| 331 |
+
actual = {
|
| 332 |
+
role: getattr(self, role) for role in CPU_ROLE_ORDER
|
| 333 |
+
}
|
| 334 |
+
if actual != expected:
|
| 335 |
+
raise ValueError(
|
| 336 |
+
"invalid dual-SLAM CPU partition: "
|
| 337 |
+
f"expected={expected!r} actual={actual!r}"
|
| 338 |
+
)
|
| 339 |
+
if self.fays_track not in {(6,), (8,), (10,)}:
|
| 340 |
+
raise ValueError(
|
| 341 |
+
"dual-SLAM track must be CPU6, CPU8 or CPU10: "
|
| 342 |
+
f"{self.fays_track!r}"
|
| 343 |
+
)
|
| 344 |
+
if self.input not in {(4,), (5,), (6,)}:
|
| 345 |
+
raise ValueError(
|
| 346 |
+
"dual-SLAM input must be CPU4, CPU5 or CPU6: "
|
| 347 |
+
f"{self.input!r}"
|
| 348 |
+
)
|
| 349 |
+
if self.input != self.fays_input:
|
| 350 |
+
raise ValueError(
|
| 351 |
+
"dual-SLAM input/fays_input must share the same CPU: "
|
| 352 |
+
f"input={self.input!r} fays_input={self.fays_input!r}"
|
| 353 |
+
)
|
| 354 |
+
reserved = set(self.reserved)
|
| 355 |
+
general = set(self.general)
|
| 356 |
+
if general != {10, 11}:
|
| 357 |
+
raise ValueError(
|
| 358 |
+
f"invalid dual-SLAM boundary: "
|
| 359 |
+
f"reserved={sorted(reserved)} general={sorted(general)}"
|
| 360 |
+
)
|
| 361 |
+
if reserved & general:
|
| 362 |
+
raise ValueError("dual-SLAM reserved/general overlap")
|
| 363 |
+
if any(cpu < 0 or cpu > 10 for cpu in reserved):
|
| 364 |
+
raise ValueError(
|
| 365 |
+
"dual-SLAM reserved CPU outside 0-10: "
|
| 366 |
+
f"{sorted(reserved)}"
|
| 367 |
+
)
|
| 368 |
+
|
| 369 |
+
@property
|
| 370 |
+
def roles(self) -> Mapping[str, Tuple[int, ...]]:
|
| 371 |
+
"""返回不可变 role → CPU 元组视图。"""
|
| 372 |
+
|
| 373 |
+
return MappingProxyType({
|
| 374 |
+
role: getattr(self, role) for role in CPU_ROLE_ORDER
|
| 375 |
+
})
|
| 376 |
+
|
| 377 |
+
@property
|
| 378 |
+
def fays_process(self) -> Tuple[int, ...]:
|
| 379 |
+
"""ORB 子进程需要作为 cpuset 超集继承的全部 Fays 阶段 CPU。
|
| 380 |
+
|
| 381 |
+
进程启动时 taskset 必须包含所有内部阶段 CPU;否则 TrackStereo
|
| 382 |
+
里的 ORB worker 无法用 sched_setaffinity 绑定到自己的角色核。
|
| 383 |
+
"""
|
| 384 |
+
return tuple(sorted({
|
| 385 |
+
cpu
|
| 386 |
+
for role in FAYS_PROCESS_ROLES
|
| 387 |
+
for cpu in getattr(self, role)
|
| 388 |
+
}))
|
| 389 |
+
|
| 390 |
+
@property
|
| 391 |
+
def reserved(self) -> Tuple[int, ...]:
|
| 392 |
+
return tuple(sorted({
|
| 393 |
+
cpu
|
| 394 |
+
for role in RESERVED_ROLES
|
| 395 |
+
for cpu in getattr(self, role)
|
| 396 |
+
}))
|
| 397 |
+
|
| 398 |
+
def cpus_for(self, role: str) -> Tuple[int, ...]:
|
| 399 |
+
try:
|
| 400 |
+
return self.roles[str(role)]
|
| 401 |
+
except KeyError as exc:
|
| 402 |
+
raise KeyError(f"unknown CPU role: {role!r}") from exc
|
| 403 |
+
|
| 404 |
+
def validate_relaxed(self) -> None:
|
| 405 |
+
"""放松模式:只检查预留域和普通域不重叠。"""
|
| 406 |
+
reserved = set(self.reserved)
|
| 407 |
+
general = set(self.general)
|
| 408 |
+
if reserved & general:
|
| 409 |
+
raise ValueError(
|
| 410 |
+
"relaxed CPU policy: reserved/general overlap: "
|
| 411 |
+
f"reserved={sorted(reserved)} general={sorted(general)}"
|
| 412 |
+
)
|
| 413 |
+
all_cpus = reserved | general
|
| 414 |
+
if len(all_cpus) < 4:
|
| 415 |
+
raise ValueError(
|
| 416 |
+
"relaxed CPU policy requires at least 4 logical CPUs: "
|
| 417 |
+
f"found={sorted(all_cpus)}"
|
| 418 |
+
)
|
| 419 |
+
|
| 420 |
+
def validate_strict(self) -> None:
|
| 421 |
+
actual = {role: getattr(self, role) for role in CPU_ROLE_ORDER}
|
| 422 |
+
if actual != dict(EXPECTED_ROLE_CPUS):
|
| 423 |
+
raise ValueError(
|
| 424 |
+
"invalid fixed CPU partition: "
|
| 425 |
+
f"expected={dict(EXPECTED_ROLE_CPUS)!r} actual={actual!r}"
|
| 426 |
+
)
|
| 427 |
+
reserved = set(self.reserved)
|
| 428 |
+
general = set(self.general)
|
| 429 |
+
if reserved != set(range(10)) or general != {10, 11}:
|
| 430 |
+
raise ValueError(
|
| 431 |
+
f"invalid strict boundary: reserved={sorted(reserved)} "
|
| 432 |
+
f"general={sorted(general)}"
|
| 433 |
+
)
|
| 434 |
+
if reserved & general:
|
| 435 |
+
raise ValueError("reserved and general CPU domains overlap")
|
| 436 |
+
|
| 437 |
+
def describe(self) -> str:
|
| 438 |
+
return ", ".join(
|
| 439 |
+
f"{role}={getattr(self, role)}" for role in CPU_ROLE_ORDER)
|
| 440 |
+
|
| 441 |
+
|
| 442 |
+
def _read_int_file(path: str, default: Optional[int] = None) -> Optional[int]:
|
| 443 |
+
try:
|
| 444 |
+
with open(path, encoding="utf-8") as stream:
|
| 445 |
+
return int(stream.read().strip())
|
| 446 |
+
except (OSError, ValueError):
|
| 447 |
+
return default
|
| 448 |
+
|
| 449 |
+
|
| 450 |
+
def discover_cpu_cores(
|
| 451 |
+
sysfs_cpu_root: os.PathLike[str] | str = "/sys/devices/system/cpu",
|
| 452 |
+
affinity_getter: Optional[Callable[[int], Iterable[int]]] = None,
|
| 453 |
+
) -> Tuple[CoreTopology, ...]:
|
| 454 |
+
"""返回当前进程 affinity 允许范围内的物理核拓扑。
|
| 455 |
+
|
| 456 |
+
``sysfs_cpu_root`` 和 ``affinity_getter`` 可注入,便于无硬件测试;默认
|
| 457 |
+
路径只执行只读访问。
|
| 458 |
+
"""
|
| 459 |
+
|
| 460 |
+
getter = affinity_getter
|
| 461 |
+
if getter is None:
|
| 462 |
+
getter = getattr(os, "sched_getaffinity", None)
|
| 463 |
+
if getter is None:
|
| 464 |
+
return ()
|
| 465 |
+
try:
|
| 466 |
+
allowed = {int(cpu) for cpu in getter(0)}
|
| 467 |
+
except (OSError, TypeError, ValueError):
|
| 468 |
+
return ()
|
| 469 |
+
|
| 470 |
+
root = os.fspath(sysfs_cpu_root)
|
| 471 |
+
records: dict[tuple[int, int], dict[str, object]] = {}
|
| 472 |
+
for cpu in sorted(allowed):
|
| 473 |
+
cpu_root = os.path.join(root, f"cpu{cpu}")
|
| 474 |
+
topology = os.path.join(cpu_root, "topology")
|
| 475 |
+
package_id = _read_int_file(
|
| 476 |
+
os.path.join(topology, "physical_package_id"), 0)
|
| 477 |
+
core_id = _read_int_file(
|
| 478 |
+
os.path.join(topology, "core_id"), cpu)
|
| 479 |
+
key = (int(package_id or 0), int(core_id if core_id is not None else cpu))
|
| 480 |
+
record = records.setdefault(key, {
|
| 481 |
+
"cpus": set(),
|
| 482 |
+
"sibling_count": 1,
|
| 483 |
+
"max_freq": 0,
|
| 484 |
+
"core_type": None,
|
| 485 |
+
})
|
| 486 |
+
cast_cpus = record["cpus"]
|
| 487 |
+
assert isinstance(cast_cpus, set)
|
| 488 |
+
cast_cpus.add(cpu)
|
| 489 |
+
|
| 490 |
+
try:
|
| 491 |
+
with open(
|
| 492 |
+
os.path.join(topology, "thread_siblings_list"),
|
| 493 |
+
encoding="utf-8",
|
| 494 |
+
) as stream:
|
| 495 |
+
sibling_count = len(parse_cpu_list(stream.read()))
|
| 496 |
+
record["sibling_count"] = max(
|
| 497 |
+
int(record["sibling_count"]), sibling_count)
|
| 498 |
+
except (OSError, ValueError):
|
| 499 |
+
pass
|
| 500 |
+
|
| 501 |
+
max_freq = _read_int_file(
|
| 502 |
+
os.path.join(cpu_root, "cpufreq", "cpuinfo_max_freq"), 0)
|
| 503 |
+
record["max_freq"] = max(
|
| 504 |
+
int(record["max_freq"]), int(max_freq or 0))
|
| 505 |
+
core_type = _read_int_file(
|
| 506 |
+
os.path.join(topology, "core_type"))
|
| 507 |
+
if core_type is not None:
|
| 508 |
+
record["core_type"] = core_type
|
| 509 |
+
|
| 510 |
+
cores = (
|
| 511 |
+
CoreTopology(
|
| 512 |
+
cpus=tuple(record["cpus"]),
|
| 513 |
+
sibling_count=int(record["sibling_count"]),
|
| 514 |
+
max_freq=int(record["max_freq"]),
|
| 515 |
+
core_type=(
|
| 516 |
+
None if record["core_type"] is None
|
| 517 |
+
else int(record["core_type"])
|
| 518 |
+
),
|
| 519 |
+
package_id=key[0],
|
| 520 |
+
core_id=key[1],
|
| 521 |
+
)
|
| 522 |
+
for key, record in records.items()
|
| 523 |
+
)
|
| 524 |
+
return tuple(sorted(cores, key=lambda item: min(item.cpus)))
|
| 525 |
+
|
| 526 |
+
|
| 527 |
+
def select_cpu_policy(
|
| 528 |
+
cores: Sequence[CoreTopology | Mapping[str, object]],
|
| 529 |
+
) -> Optional[CpuPolicySnapshot]:
|
| 530 |
+
"""在已验证的混合核拓扑上选择项目唯一的固定 CPU 方案。
|
| 531 |
+
|
| 532 |
+
核类型识别顺序和重构前实现一致:``core_type``、SMT 拓扑、最高频率。
|
| 533 |
+
编号或物理拓扑不匹配时返回 ``None``,绝不把固定方案套到未知机器。
|
| 534 |
+
"""
|
| 535 |
+
|
| 536 |
+
normalized: list[CoreTopology] = []
|
| 537 |
+
for core in cores:
|
| 538 |
+
if isinstance(core, CoreTopology):
|
| 539 |
+
normalized.append(core)
|
| 540 |
+
continue
|
| 541 |
+
cpus_value = core.get("cpus", ())
|
| 542 |
+
cpus = tuple(int(cpu) for cpu in cpus_value) # type: ignore[arg-type]
|
| 543 |
+
if not cpus:
|
| 544 |
+
continue
|
| 545 |
+
normalized.append(CoreTopology(
|
| 546 |
+
cpus=cpus,
|
| 547 |
+
sibling_count=int(core.get("sibling_count", 1)),
|
| 548 |
+
max_freq=int(core.get("max_freq", 0)),
|
| 549 |
+
core_type=(
|
| 550 |
+
None if core.get("core_type") is None
|
| 551 |
+
else int(core["core_type"])
|
| 552 |
+
),
|
| 553 |
+
))
|
| 554 |
+
|
| 555 |
+
if len(normalized) < 3:
|
| 556 |
+
return None
|
| 557 |
+
|
| 558 |
+
p_cores: list[CoreTopology] = []
|
| 559 |
+
e_cores: list[CoreTopology] = []
|
| 560 |
+
source = ""
|
| 561 |
+
|
| 562 |
+
typed = {
|
| 563 |
+
core.core_type for core in normalized if core.core_type is not None
|
| 564 |
+
}
|
| 565 |
+
if len(typed) >= 2:
|
| 566 |
+
by_type: dict[int, list[CoreTopology]] = {}
|
| 567 |
+
for core in normalized:
|
| 568 |
+
if core.core_type is not None:
|
| 569 |
+
by_type.setdefault(core.core_type, []).append(core)
|
| 570 |
+
|
| 571 |
+
def type_rank(
|
| 572 |
+
item: tuple[int, list[CoreTopology]],
|
| 573 |
+
) -> tuple[int, int, int]:
|
| 574 |
+
core_type, members = item
|
| 575 |
+
return (
|
| 576 |
+
max(member.sibling_count for member in members),
|
| 577 |
+
max(member.max_freq for member in members),
|
| 578 |
+
core_type,
|
| 579 |
+
)
|
| 580 |
+
|
| 581 |
+
ordered_types = sorted(by_type.items(), key=type_rank)
|
| 582 |
+
e_cores = ordered_types[0][1]
|
| 583 |
+
p_cores = ordered_types[-1][1]
|
| 584 |
+
source = "sysfs core_type"
|
| 585 |
+
|
| 586 |
+
if len(p_cores) < 2 or not e_cores:
|
| 587 |
+
sibling_counts = sorted({
|
| 588 |
+
core.sibling_count for core in normalized
|
| 589 |
+
})
|
| 590 |
+
if len(sibling_counts) >= 2:
|
| 591 |
+
e_count = sibling_counts[0]
|
| 592 |
+
p_count = sibling_counts[-1]
|
| 593 |
+
e_cores = [
|
| 594 |
+
core for core in normalized
|
| 595 |
+
if core.sibling_count == e_count
|
| 596 |
+
]
|
| 597 |
+
p_cores = [
|
| 598 |
+
core for core in normalized
|
| 599 |
+
if core.sibling_count == p_count
|
| 600 |
+
]
|
| 601 |
+
source = "SMT topology"
|
| 602 |
+
|
| 603 |
+
if len(p_cores) < 2 or not e_cores:
|
| 604 |
+
frequencies = sorted({
|
| 605 |
+
core.max_freq for core in normalized if core.max_freq > 0
|
| 606 |
+
})
|
| 607 |
+
if (
|
| 608 |
+
len(frequencies) >= 2
|
| 609 |
+
and frequencies[-1] >= frequencies[0] * 1.05
|
| 610 |
+
):
|
| 611 |
+
e_freq = frequencies[0]
|
| 612 |
+
p_freq = frequencies[-1]
|
| 613 |
+
e_cores = [
|
| 614 |
+
core for core in normalized if core.max_freq == e_freq
|
| 615 |
+
]
|
| 616 |
+
p_cores = [
|
| 617 |
+
core for core in normalized if core.max_freq == p_freq
|
| 618 |
+
]
|
| 619 |
+
source = "maximum frequency"
|
| 620 |
+
|
| 621 |
+
if len(p_cores) < 2 or not e_cores:
|
| 622 |
+
all_cpus = {
|
| 623 |
+
cpu for core in normalized for cpu in core.cpus
|
| 624 |
+
}
|
| 625 |
+
if _can_use_homogeneous_fallback(normalized, all_cpus):
|
| 626 |
+
return _build_homogeneous_policy(normalized)
|
| 627 |
+
return None
|
| 628 |
+
|
| 629 |
+
all_cpus = {
|
| 630 |
+
cpu for core in normalized for cpu in core.cpus
|
| 631 |
+
}
|
| 632 |
+
if not set(range(12)).issubset(all_cpus):
|
| 633 |
+
return None
|
| 634 |
+
|
| 635 |
+
p_cpu_groups = [set(core.cpus) for core in p_cores]
|
| 636 |
+
e_cpu_set = {cpu for core in e_cores for cpu in core.cpus}
|
| 637 |
+
if (
|
| 638 |
+
not any({0, 1}.issubset(group) for group in p_cpu_groups)
|
| 639 |
+
or not any({2, 3}.issubset(group) for group in p_cpu_groups)
|
| 640 |
+
or not set(range(4, 12)).issubset(e_cpu_set)
|
| 641 |
+
):
|
| 642 |
+
return None
|
| 643 |
+
|
| 644 |
+
return CpuPolicySnapshot(source=source)
|
| 645 |
+
|
| 646 |
+
|
| 647 |
+
def _can_use_homogeneous_fallback(
|
| 648 |
+
cores: list[CoreTopology],
|
| 649 |
+
all_cpus: set[int],
|
| 650 |
+
) -> bool:
|
| 651 |
+
"""检查是否为同构 12 逻辑 CPU 系统(如 AMD Ryzen 无大小核)。"""
|
| 652 |
+
return (
|
| 653 |
+
len(cores) >= 3
|
| 654 |
+
and all(core.sibling_count == 2 for core in cores)
|
| 655 |
+
and set(range(12)).issubset(all_cpus)
|
| 656 |
+
)
|
| 657 |
+
|
| 658 |
+
|
| 659 |
+
def _build_homogeneous_policy(
|
| 660 |
+
cores: list[CoreTopology],
|
| 661 |
+
) -> CpuPolicySnapshot:
|
| 662 |
+
"""在同构 CPU 上构建放松策略。
|
| 663 |
+
|
| 664 |
+
以 AMD SMT 编号方案 (0,6), (1,7), (2,8), (3,9), (4,10), (5,11)
|
| 665 |
+
为基准排列角色,保证每个重角色独占一个物理核。
|
| 666 |
+
"""
|
| 667 |
+
|
| 668 |
+
sibling_map: dict[int, int] = {}
|
| 669 |
+
for core in cores:
|
| 670 |
+
cpus = sorted(core.cpus)
|
| 671 |
+
if len(cpus) == 2:
|
| 672 |
+
sibling_map[cpus[0]] = cpus[1]
|
| 673 |
+
sibling_map[cpus[1]] = cpus[0]
|
| 674 |
+
|
| 675 |
+
def _sibling(cpu: int) -> int:
|
| 676 |
+
return sibling_map.get(cpu, cpu)
|
| 677 |
+
|
| 678 |
+
matched = True
|
| 679 |
+
try:
|
| 680 |
+
if sibling_map.get(0) == 6 and sibling_map.get(2) == 8:
|
| 681 |
+
# Standard AMD SMT (0,6) (1,7) (2,8) (3,9) (4,10) (5,11)
|
| 682 |
+
return CpuPolicySnapshot(
|
| 683 |
+
source="homogeneous_amd",
|
| 684 |
+
input=(4,),
|
| 685 |
+
fays_input=(4,),
|
| 686 |
+
left_process=(0,),
|
| 687 |
+
right_process=(2,),
|
| 688 |
+
idle_smt=(6, 8),
|
| 689 |
+
fays_prepare=(5,),
|
| 690 |
+
fays_left_orb=(1,),
|
| 691 |
+
fays_right_orb=(3,),
|
| 692 |
+
fays_track=(7,),
|
| 693 |
+
fays_background=(9,),
|
| 694 |
+
general=(10, 11),
|
| 695 |
+
_relaxed=True,
|
| 696 |
+
)
|
| 697 |
+
except Exception:
|
| 698 |
+
matched = False
|
| 699 |
+
|
| 700 |
+
# Generic fallback: assign roles to distinct physical cores
|
| 701 |
+
used_physical: set[int] = set()
|
| 702 |
+
role_cpus: dict[str, tuple[int, ...]] = {}
|
| 703 |
+
role_order = [
|
| 704 |
+
("left_process", 1),
|
| 705 |
+
("right_process", 1),
|
| 706 |
+
("fays_left_orb", 1),
|
| 707 |
+
("fays_right_orb", 1),
|
| 708 |
+
("fays_track", 1),
|
| 709 |
+
("fays_background", 1),
|
| 710 |
+
("fays_prepare", 1),
|
| 711 |
+
("input", 1),
|
| 712 |
+
("fays_input", 1),
|
| 713 |
+
("general", 2),
|
| 714 |
+
]
|
| 715 |
+
cpu_pool: list[int] = []
|
| 716 |
+
for core in sorted(cores, key=lambda c: min(c.cpus)):
|
| 717 |
+
for cpu in sorted(core.cpus):
|
| 718 |
+
cpu_pool.append(cpu)
|
| 719 |
+
idx = 0
|
| 720 |
+
idle_cpus: list[int] = []
|
| 721 |
+
for role_name, count in role_order:
|
| 722 |
+
assigned: list[int] = []
|
| 723 |
+
while len(assigned) < count and idx < len(cpu_pool):
|
| 724 |
+
cpu = cpu_pool[idx]
|
| 725 |
+
idx += 1
|
| 726 |
+
physical = sibling_map.get(cpu, cpu)
|
| 727 |
+
if physical in used_physical:
|
| 728 |
+
idle_cpus.append(cpu)
|
| 729 |
+
continue
|
| 730 |
+
used_physical.add(physical)
|
| 731 |
+
assigned.append(cpu)
|
| 732 |
+
# mark sibling as idle
|
| 733 |
+
sibling = sibling_map.get(cpu)
|
| 734 |
+
if sibling is not None and sibling not in assigned:
|
| 735 |
+
idle_cpus.append(sibling)
|
| 736 |
+
used_physical.add(physical)
|
| 737 |
+
if not assigned:
|
| 738 |
+
assigned = [cpu_pool[idx % len(cpu_pool)]]
|
| 739 |
+
idx += 1
|
| 740 |
+
role_cpus[role_name] = tuple(assigned)
|
| 741 |
+
idle_smt = tuple(sorted(set(idle_cpus))[:4])
|
| 742 |
+
if not idle_smt:
|
| 743 |
+
idle_smt = (cpu_pool[-1],) if cpu_pool else (0,)
|
| 744 |
+
|
| 745 |
+
return CpuPolicySnapshot(
|
| 746 |
+
source="homogeneous_generic",
|
| 747 |
+
input=role_cpus.get("input", (0,)),
|
| 748 |
+
fays_input=role_cpus.get("fays_input", (0,)),
|
| 749 |
+
left_process=role_cpus.get("left_process", (0,)),
|
| 750 |
+
right_process=role_cpus.get("right_process", (2,)),
|
| 751 |
+
idle_smt=idle_smt,
|
| 752 |
+
fays_prepare=role_cpus.get("fays_prepare", (5,)),
|
| 753 |
+
fays_left_orb=role_cpus.get("fays_left_orb", (1,)),
|
| 754 |
+
fays_right_orb=role_cpus.get("fays_right_orb", (3,)),
|
| 755 |
+
fays_track=role_cpus.get("fays_track", (7,)),
|
| 756 |
+
fays_background=role_cpus.get("fays_background", (9,)),
|
| 757 |
+
general=role_cpus.get("general", (10, 11)),
|
| 758 |
+
_relaxed=True,
|
| 759 |
+
)
|
| 760 |
+
|
| 761 |
+
|
| 762 |
+
def detect_cpu_policy(
|
| 763 |
+
sysfs_cpu_root: os.PathLike[str] | str = "/sys/devices/system/cpu",
|
| 764 |
+
affinity_getter: Optional[Callable[[int], Iterable[int]]] = None,
|
| 765 |
+
) -> CpuPolicySnapshot:
|
| 766 |
+
"""发现并严格验证项目所需的 CPU0-11 分区。"""
|
| 767 |
+
|
| 768 |
+
policy = select_cpu_policy(discover_cpu_cores(
|
| 769 |
+
sysfs_cpu_root=sysfs_cpu_root,
|
| 770 |
+
affinity_getter=affinity_getter,
|
| 771 |
+
))
|
| 772 |
+
if policy is None:
|
| 773 |
+
raise RuntimeError(
|
| 774 |
+
"strict CPU isolation unavailable: expected CPU0-9 reserved "
|
| 775 |
+
"by role and CPU10-11 available for general work"
|
| 776 |
+
)
|
| 777 |
+
return policy
|
collector/core/gripper/runtime/device_access.py
ADDED
|
@@ -0,0 +1,66 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Cross-process device-operation guards; no SDK calls or affinity changes."""
|
| 2 |
+
from contextlib import contextmanager
|
| 3 |
+
import fcntl
|
| 4 |
+
import os
|
| 5 |
+
import stat
|
| 6 |
+
import time
|
| 7 |
+
|
| 8 |
+
|
| 9 |
+
@contextmanager
|
| 10 |
+
def device_access_guard(path, *, timeout=30.0, cancelled=None):
|
| 11 |
+
# Read-only flock also works for a lock created by a sudo-launched process.
|
| 12 |
+
# Do not truncate, unlink or follow symlinks: every caller must lock the
|
| 13 |
+
# same inode for the whole operation/lease lifetime.
|
| 14 |
+
flags = os.O_RDONLY | os.O_CLOEXEC | os.O_NOFOLLOW
|
| 15 |
+
try:
|
| 16 |
+
fd = os.open(path, flags)
|
| 17 |
+
except FileNotFoundError:
|
| 18 |
+
try:
|
| 19 |
+
fd = os.open(path, flags | os.O_CREAT | os.O_EXCL, 0o644)
|
| 20 |
+
except FileExistsError:
|
| 21 |
+
fd = os.open(path, flags)
|
| 22 |
+
acquired = False
|
| 23 |
+
owner_pid = os.getpid()
|
| 24 |
+
try:
|
| 25 |
+
if not stat.S_ISREG(os.fstat(fd).st_mode):
|
| 26 |
+
raise RuntimeError(f"设备锁不是普通文件: {path}")
|
| 27 |
+
deadline = time.monotonic() + timeout
|
| 28 |
+
while True:
|
| 29 |
+
if cancelled is not None and cancelled():
|
| 30 |
+
raise RuntimeError("设备操作已取消")
|
| 31 |
+
try:
|
| 32 |
+
fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
|
| 33 |
+
acquired = True
|
| 34 |
+
break
|
| 35 |
+
except BlockingIOError:
|
| 36 |
+
if time.monotonic() >= deadline:
|
| 37 |
+
raise RuntimeError(f"设备正在使用或初始化,请稍后重试: {path}")
|
| 38 |
+
time.sleep(0.05)
|
| 39 |
+
yield
|
| 40 |
+
finally:
|
| 41 |
+
try:
|
| 42 |
+
# O_CLOEXEC does not prevent inheritance by fork-only workers.
|
| 43 |
+
# close(fd) alone leaves flock held by their duplicate descriptors.
|
| 44 |
+
# Only the acquiring process may release the shared lock explicitly.
|
| 45 |
+
if acquired and os.getpid() == owner_pid:
|
| 46 |
+
fcntl.flock(fd, fcntl.LOCK_UN)
|
| 47 |
+
finally:
|
| 48 |
+
os.close(fd)
|
| 49 |
+
|
| 50 |
+
|
| 51 |
+
FAYS_SDK_INITIALIZATION_LOCK = "/tmp/ksq-fays-sdk-serial-probe.lock"
|
| 52 |
+
|
| 53 |
+
|
| 54 |
+
def fays_device_guard(stereo_port, *, timeout=0.0):
|
| 55 |
+
import re
|
| 56 |
+
if not re.fullmatch(r"/dev/video[0-9]+", str(stereo_port)):
|
| 57 |
+
raise RuntimeError("Fays stereo 节点无效,拒绝访问 SDK")
|
| 58 |
+
return device_access_guard(
|
| 59 |
+
f"/tmp/ksq-fays-sdk-device-{os.path.basename(stereo_port)}.lock",
|
| 60 |
+
timeout=timeout,
|
| 61 |
+
)
|
| 62 |
+
|
| 63 |
+
|
| 64 |
+
def fays_config_device_guard(config_path):
|
| 65 |
+
from core.gripper.fays_runtime import read_fays_config_ports
|
| 66 |
+
return fays_device_guard(read_fays_config_ports(config_path)["stereo_dev_port"])
|
collector/core/gripper/runtime/interruptible_queue.py
ADDED
|
@@ -0,0 +1,75 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Bounded pipe-message reads for a single parent consumer on Linux/CPython.
|
| 2 |
+
|
| 3 |
+
multiprocessing.Queue.get(timeout) times out before recv_bytes, not during it.
|
| 4 |
+
An exiting producer can leave a partial frame. This adapter preserves partial
|
| 5 |
+
frames across timeouts and never enters Connection's blocking recv_bytes.
|
| 6 |
+
|
| 7 |
+
Own this queue's read end exclusively: do not mix with Queue.get or another
|
| 8 |
+
reader. Queue capacity is returned exactly once after receiving a whole frame.
|
| 9 |
+
The CPython-private queue access is deliberately isolated in this module.
|
| 10 |
+
"""
|
| 11 |
+
import os
|
| 12 |
+
import queue
|
| 13 |
+
import select
|
| 14 |
+
import struct
|
| 15 |
+
import time
|
| 16 |
+
from multiprocessing.reduction import ForkingPickler
|
| 17 |
+
|
| 18 |
+
|
| 19 |
+
class InterruptibleQueueReader:
|
| 20 |
+
MAX_MESSAGE_BYTES = 16 * 1024 * 1024
|
| 21 |
+
|
| 22 |
+
def __init__(self, channel):
|
| 23 |
+
self.channel = channel
|
| 24 |
+
self.fd = channel._reader.fileno()
|
| 25 |
+
os.set_blocking(self.fd, False)
|
| 26 |
+
self.buffer = bytearray()
|
| 27 |
+
self.header_size = 4
|
| 28 |
+
self.payload_size = None
|
| 29 |
+
|
| 30 |
+
def get(self, timeout=0.05, *, cancelled=lambda: False):
|
| 31 |
+
deadline = time.monotonic() + max(0.0, timeout)
|
| 32 |
+
if not self.channel._rlock.acquire(True, max(0.0, timeout)):
|
| 33 |
+
raise queue.Empty
|
| 34 |
+
try:
|
| 35 |
+
while True:
|
| 36 |
+
if cancelled():
|
| 37 |
+
raise queue.Empty
|
| 38 |
+
if self.payload_size is None and len(self.buffer) >= self.header_size:
|
| 39 |
+
if self.header_size == 4:
|
| 40 |
+
size, = struct.unpack('!i', self.buffer[:4])
|
| 41 |
+
if size == -1:
|
| 42 |
+
self.header_size = 12
|
| 43 |
+
continue
|
| 44 |
+
else:
|
| 45 |
+
size, = struct.unpack('!Q', self.buffer[4:12])
|
| 46 |
+
if size < 0 or size > self.MAX_MESSAGE_BYTES:
|
| 47 |
+
raise ValueError(f'Invalid IPC message length: {size}')
|
| 48 |
+
self.payload_size = size
|
| 49 |
+
target = self.header_size + (self.payload_size or 0)
|
| 50 |
+
if self.payload_size is not None and len(self.buffer) == target:
|
| 51 |
+
payload = self.buffer[self.header_size:]
|
| 52 |
+
self.buffer = bytearray()
|
| 53 |
+
self.header_size = 4
|
| 54 |
+
self.payload_size = None
|
| 55 |
+
self.channel._sem.release()
|
| 56 |
+
return ForkingPickler.loads(payload)
|
| 57 |
+
remaining = max(0.0, deadline - time.monotonic())
|
| 58 |
+
ready, _, _ = select.select([self.fd], [], [], min(.02, remaining))
|
| 59 |
+
if ready:
|
| 60 |
+
try:
|
| 61 |
+
chunk = os.read(self.fd, min(65536, target - len(self.buffer)))
|
| 62 |
+
except BlockingIOError:
|
| 63 |
+
chunk = None
|
| 64 |
+
if chunk == b'':
|
| 65 |
+
raise EOFError('IPC producer closed during message read')
|
| 66 |
+
if chunk:
|
| 67 |
+
self.buffer.extend(chunk)
|
| 68 |
+
# Decode complete messages even if this read used the
|
| 69 |
+
# final instant of the deadline; otherwise retain bytes.
|
| 70 |
+
if len(self.buffer) == target:
|
| 71 |
+
continue
|
| 72 |
+
if time.monotonic() >= deadline:
|
| 73 |
+
raise queue.Empty
|
| 74 |
+
finally:
|
| 75 |
+
self.channel._rlock.release()
|
collector/core/gripper/runtime/thread_affinity.py
ADDED
|
@@ -0,0 +1,677 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Linux 原生线程 affinity 的唯一写入服务。
|
| 2 |
+
|
| 3 |
+
锁顺序固定为 ``_state_lock`` → 无其他内部锁。外部回调(日志与 fatal
|
| 4 |
+
dispatcher)始终在释放锁后调用,避免关机路径与审计线程交叉死锁。
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from __future__ import annotations
|
| 8 |
+
|
| 9 |
+
from dataclasses import dataclass
|
| 10 |
+
import os
|
| 11 |
+
from pathlib import Path
|
| 12 |
+
import threading
|
| 13 |
+
from typing import Callable, Iterable, Optional, Tuple
|
| 14 |
+
|
| 15 |
+
from .cpu_policy import (
|
| 16 |
+
DEDICATED_THREAD_ROLES,
|
| 17 |
+
CpuPolicySnapshot,
|
| 18 |
+
)
|
| 19 |
+
|
| 20 |
+
|
| 21 |
+
AUDIT_INTERVAL_SECONDS = 1.0
|
| 22 |
+
AUDITOR_JOIN_TIMEOUT_SECONDS = 1.5
|
| 23 |
+
|
| 24 |
+
_FAYS_STAGE_ROLES = {
|
| 25 |
+
"fays-input": "fays_input",
|
| 26 |
+
"fays-prep": "fays_prepare",
|
| 27 |
+
"fays-orb-left": "fays_left_orb",
|
| 28 |
+
"fays-orb-right": "fays_right_orb",
|
| 29 |
+
"fays-track": "fays_track",
|
| 30 |
+
"fays-output": "fays_track",
|
| 31 |
+
}
|
| 32 |
+
|
| 33 |
+
# These long-lived ORB workers must be able to use the complete ORB child
|
| 34 |
+
# domain. Treating them as a fixed background stage pins both to CPU9 and
|
| 35 |
+
# recreates the LocalMapping/TrackStereo contention seen in dual-device runs.
|
| 36 |
+
_FAYS_CHILD_DOMAIN_THREADS = {
|
| 37 |
+
"fays-local-map",
|
| 38 |
+
"fays-loop-close",
|
| 39 |
+
}
|
| 40 |
+
|
| 41 |
+
|
| 42 |
+
@dataclass(frozen=True)
|
| 43 |
+
class ThreadBindingSnapshot:
|
| 44 |
+
tid: int
|
| 45 |
+
role: str
|
| 46 |
+
label: str
|
| 47 |
+
cpus: Tuple[int, ...]
|
| 48 |
+
|
| 49 |
+
|
| 50 |
+
@dataclass(frozen=True)
|
| 51 |
+
class ChildDomainSnapshot:
|
| 52 |
+
name: str
|
| 53 |
+
pid: int
|
| 54 |
+
cpus: Tuple[int, ...]
|
| 55 |
+
allow_fays_stage_threads: bool
|
| 56 |
+
stage_cpus: Tuple[Tuple[str, Tuple[int, ...]], ...] = ()
|
| 57 |
+
|
| 58 |
+
|
| 59 |
+
@dataclass(frozen=True)
|
| 60 |
+
class ThreadAffinitySnapshot:
|
| 61 |
+
installed: bool
|
| 62 |
+
auditor_running: bool
|
| 63 |
+
faulted: bool
|
| 64 |
+
bindings: Tuple[ThreadBindingSnapshot, ...]
|
| 65 |
+
children: Tuple[ChildDomainSnapshot, ...]
|
| 66 |
+
|
| 67 |
+
|
| 68 |
+
class ThreadAffinityService:
|
| 69 |
+
"""应用固定 CPU 角色、登记专用线程并持续审计边界。"""
|
| 70 |
+
|
| 71 |
+
def __init__(
|
| 72 |
+
self,
|
| 73 |
+
policy: CpuPolicySnapshot,
|
| 74 |
+
*,
|
| 75 |
+
set_affinity: Optional[Callable[[int, Iterable[int]], None]] = None,
|
| 76 |
+
get_affinity: Optional[Callable[[int], Iterable[int]]] = None,
|
| 77 |
+
list_task_ids: Optional[Callable[[int], Iterable[int]]] = None,
|
| 78 |
+
read_task_name: Optional[Callable[[int, int], str]] = None,
|
| 79 |
+
native_id: Callable[[], int] = threading.get_native_id,
|
| 80 |
+
process_id: Callable[[], int] = os.getpid,
|
| 81 |
+
logger: Callable[[str], None] = print,
|
| 82 |
+
fatal_callback: Optional[Callable[[str], None]] = None,
|
| 83 |
+
) -> None:
|
| 84 |
+
if policy._relaxed:
|
| 85 |
+
policy.validate_relaxed()
|
| 86 |
+
else:
|
| 87 |
+
policy.validate_strict()
|
| 88 |
+
self._policy = policy
|
| 89 |
+
self._set_affinity = (
|
| 90 |
+
set_affinity
|
| 91 |
+
if set_affinity is not None
|
| 92 |
+
else getattr(os, "sched_setaffinity", None)
|
| 93 |
+
)
|
| 94 |
+
self._get_affinity = (
|
| 95 |
+
get_affinity
|
| 96 |
+
if get_affinity is not None
|
| 97 |
+
else getattr(os, "sched_getaffinity", None)
|
| 98 |
+
)
|
| 99 |
+
self._list_task_ids = list_task_ids or self._default_list_task_ids
|
| 100 |
+
self._read_task_name = (
|
| 101 |
+
read_task_name or self._default_read_task_name)
|
| 102 |
+
self._native_id = native_id
|
| 103 |
+
self._process_id = process_id
|
| 104 |
+
self._logger = logger
|
| 105 |
+
self._fatal_callback = fatal_callback
|
| 106 |
+
|
| 107 |
+
self._state_lock = threading.RLock()
|
| 108 |
+
self._bindings: dict[int, ThreadBindingSnapshot] = {}
|
| 109 |
+
self._children: dict[str, ChildDomainSnapshot] = {}
|
| 110 |
+
self._reported: set[tuple[int, str]] = set()
|
| 111 |
+
self._installed = False
|
| 112 |
+
self._faulted = False
|
| 113 |
+
self._stop = threading.Event()
|
| 114 |
+
self._auditor_thread: Optional[threading.Thread] = None
|
| 115 |
+
|
| 116 |
+
@property
|
| 117 |
+
def policy(self) -> CpuPolicySnapshot:
|
| 118 |
+
return self._policy
|
| 119 |
+
|
| 120 |
+
@staticmethod
|
| 121 |
+
def _default_list_task_ids(pid: int) -> Tuple[int, ...]:
|
| 122 |
+
try:
|
| 123 |
+
names = os.listdir(f"/proc/{int(pid)}/task")
|
| 124 |
+
except FileNotFoundError:
|
| 125 |
+
return ()
|
| 126 |
+
return tuple(sorted(
|
| 127 |
+
int(name) for name in names if str(name).isdigit()))
|
| 128 |
+
|
| 129 |
+
@staticmethod
|
| 130 |
+
def _default_read_task_name(pid: int, tid: int) -> str:
|
| 131 |
+
try:
|
| 132 |
+
with open(
|
| 133 |
+
f"/proc/{int(pid)}/task/{int(tid)}/comm",
|
| 134 |
+
encoding="utf-8",
|
| 135 |
+
) as stream:
|
| 136 |
+
return stream.read().strip()
|
| 137 |
+
except (FileNotFoundError, ProcessLookupError):
|
| 138 |
+
return ""
|
| 139 |
+
|
| 140 |
+
def _require_api(self) -> None:
|
| 141 |
+
if self._set_affinity is None or self._get_affinity is None:
|
| 142 |
+
raise RuntimeError("Linux thread affinity API is unavailable")
|
| 143 |
+
|
| 144 |
+
def snapshot(self) -> ThreadAffinitySnapshot:
|
| 145 |
+
with self._state_lock:
|
| 146 |
+
thread = self._auditor_thread
|
| 147 |
+
return ThreadAffinitySnapshot(
|
| 148 |
+
installed=self._installed,
|
| 149 |
+
auditor_running=bool(
|
| 150 |
+
thread is not None and thread.is_alive()),
|
| 151 |
+
faulted=self._faulted,
|
| 152 |
+
bindings=tuple(sorted(
|
| 153 |
+
self._bindings.values(), key=lambda item: item.tid)),
|
| 154 |
+
children=tuple(sorted(
|
| 155 |
+
self._children.values(), key=lambda item: item.name)),
|
| 156 |
+
)
|
| 157 |
+
|
| 158 |
+
def _apply_exact(self, tid: int, cpus: Iterable[int]) -> Tuple[int, ...]:
|
| 159 |
+
self._require_api()
|
| 160 |
+
target = {int(cpu) for cpu in cpus}
|
| 161 |
+
if not target:
|
| 162 |
+
raise RuntimeError(f"empty CPU affinity for tid={int(tid)}")
|
| 163 |
+
assert self._set_affinity is not None
|
| 164 |
+
assert self._get_affinity is not None
|
| 165 |
+
self._set_affinity(int(tid), target)
|
| 166 |
+
actual = tuple(sorted(int(cpu) for cpu in self._get_affinity(int(tid))))
|
| 167 |
+
expected = tuple(sorted(target))
|
| 168 |
+
if actual != expected:
|
| 169 |
+
raise RuntimeError(
|
| 170 |
+
f"tid={int(tid)} affinity verification failed: "
|
| 171 |
+
f"expected={list(expected)} actual={list(actual)}"
|
| 172 |
+
)
|
| 173 |
+
return actual
|
| 174 |
+
|
| 175 |
+
def install_strict_boundary(self) -> None:
|
| 176 |
+
"""在任何应用 worker 创建前把已有原生线程收进普通线程域。"""
|
| 177 |
+
|
| 178 |
+
self._require_api()
|
| 179 |
+
general = self._policy.general
|
| 180 |
+
pid = self._process_id()
|
| 181 |
+
try:
|
| 182 |
+
tids = tuple(int(tid) for tid in self._list_task_ids(pid))
|
| 183 |
+
except (OSError, TypeError, ValueError) as exc:
|
| 184 |
+
raise RuntimeError(
|
| 185 |
+
f"cannot enumerate startup threads: {exc}") from exc
|
| 186 |
+
if not tids:
|
| 187 |
+
raise RuntimeError("cannot enumerate startup threads: empty task set")
|
| 188 |
+
|
| 189 |
+
for tid in tids:
|
| 190 |
+
try:
|
| 191 |
+
self._apply_exact(tid, general)
|
| 192 |
+
except ProcessLookupError:
|
| 193 |
+
continue
|
| 194 |
+
except (OSError, ValueError) as exc:
|
| 195 |
+
raise RuntimeError(
|
| 196 |
+
f"cannot isolate startup thread tid={tid}: {exc}"
|
| 197 |
+
) from exc
|
| 198 |
+
|
| 199 |
+
with self._state_lock:
|
| 200 |
+
self._installed = True
|
| 201 |
+
self._logger(
|
| 202 |
+
"[CPU-ISOLATION] default boundary verified: "
|
| 203 |
+
f"reserved-only={self._policy.reserved} "
|
| 204 |
+
f"general-only={self._policy.general}"
|
| 205 |
+
)
|
| 206 |
+
|
| 207 |
+
def set_xhci_irq_affinity_to_general(self) -> bool:
|
| 208 |
+
"""Pin xHCI interrupt handling to the shared general CPU domain."""
|
| 209 |
+
general_text = ",".join(str(cpu) for cpu in self._policy.general)
|
| 210 |
+
changed = 0
|
| 211 |
+
try:
|
| 212 |
+
interrupt_lines = Path("/proc/interrupts").read_text(
|
| 213 |
+
encoding="utf-8").splitlines()
|
| 214 |
+
for line in interrupt_lines:
|
| 215 |
+
irq_text, separator, _description = line.partition(":")
|
| 216 |
+
if not separator or "xhci_hcd" not in line:
|
| 217 |
+
continue
|
| 218 |
+
try:
|
| 219 |
+
irq = int(irq_text.strip())
|
| 220 |
+
except ValueError:
|
| 221 |
+
continue
|
| 222 |
+
try:
|
| 223 |
+
Path(f"/proc/irq/{irq}/smp_affinity_list").write_text(
|
| 224 |
+
f"{general_text}\n", encoding="ascii")
|
| 225 |
+
changed += 1
|
| 226 |
+
except (OSError, PermissionError) as exc:
|
| 227 |
+
self._logger(
|
| 228 |
+
"[CPU-ISOLATION] xHCI IRQ affinity FAILED: "
|
| 229 |
+
f"irq={irq} cpus={general_text}: {exc}"
|
| 230 |
+
)
|
| 231 |
+
except OSError as exc:
|
| 232 |
+
self._logger(
|
| 233 |
+
"[CPU-ISOLATION] xHCI IRQ scan failed: "
|
| 234 |
+
f"{exc}"
|
| 235 |
+
)
|
| 236 |
+
return False
|
| 237 |
+
self._logger(
|
| 238 |
+
"[CPU-ISOLATION] xHCI IRQ affinity set: "
|
| 239 |
+
f"irqs={changed} cpus={general_text}"
|
| 240 |
+
)
|
| 241 |
+
return changed > 0
|
| 242 |
+
|
| 243 |
+
def bind_general_thread(self, label: str) -> bool:
|
| 244 |
+
"""把当前普通线程严格绑定到当前策略的普通线程域。"""
|
| 245 |
+
|
| 246 |
+
tid = int(self._native_id())
|
| 247 |
+
try:
|
| 248 |
+
self._apply_exact(tid, self._policy.general)
|
| 249 |
+
return True
|
| 250 |
+
except (OSError, RuntimeError, ValueError) as exc:
|
| 251 |
+
self._logger(
|
| 252 |
+
"[CPU-ISOLATION] general bind FAILED: "
|
| 253 |
+
f"thread={label} cpus={self._policy.general}: {exc}"
|
| 254 |
+
)
|
| 255 |
+
return False
|
| 256 |
+
|
| 257 |
+
def bind_general_thread_for(
|
| 258 |
+
self, policy: CpuPolicySnapshot, label: str,
|
| 259 |
+
) -> bool:
|
| 260 |
+
"""Bind a thread using a device slot's immutable policy snapshot."""
|
| 261 |
+
tid = int(self._native_id())
|
| 262 |
+
try:
|
| 263 |
+
self._apply_exact(tid, policy.general)
|
| 264 |
+
return True
|
| 265 |
+
except (OSError, RuntimeError, ValueError) as exc:
|
| 266 |
+
self._logger(
|
| 267 |
+
"[CPU-ISOLATION] slot general bind FAILED: "
|
| 268 |
+
f"thread={label} cpus={policy.general}: {exc}"
|
| 269 |
+
)
|
| 270 |
+
return False
|
| 271 |
+
|
| 272 |
+
def bind_gui_main_thread(self, cpu: int, label: str = "gui-main") -> bool:
|
| 273 |
+
"""绑定并登记 Tk 主线程到指定核;auditor 不再纠回 general。
|
| 274 |
+
|
| 275 |
+
后续未登记的工作线程若继承该核,会被 auditor 按 general 域纠回,
|
| 276 |
+
因此 GUI 主线程可以独占该核而不被轻量线程挤占。
|
| 277 |
+
"""
|
| 278 |
+
self._require_api()
|
| 279 |
+
tid = int(self._native_id())
|
| 280 |
+
cpus = (int(cpu),)
|
| 281 |
+
binding = ThreadBindingSnapshot(
|
| 282 |
+
tid=tid, role="gui_main", label=label, cpus=cpus)
|
| 283 |
+
with self._state_lock:
|
| 284 |
+
self._bindings[tid] = binding
|
| 285 |
+
try:
|
| 286 |
+
actual = self._apply_exact(tid, cpus)
|
| 287 |
+
except (OSError, RuntimeError, ValueError) as exc:
|
| 288 |
+
with self._state_lock:
|
| 289 |
+
self._bindings.pop(tid, None)
|
| 290 |
+
self._logger(
|
| 291 |
+
"[CPU-ISOLATION] GUI main bind FAILED: "
|
| 292 |
+
f"cpu={cpu}: {exc}"
|
| 293 |
+
)
|
| 294 |
+
return False
|
| 295 |
+
self._logger(
|
| 296 |
+
f"[CPU-ISOLATION] GUI main thread cpu={cpu} verified "
|
| 297 |
+
f"actual={actual}")
|
| 298 |
+
return True
|
| 299 |
+
|
| 300 |
+
def bind_dedicated_thread(self, role: str, label: str = "") -> bool:
|
| 301 |
+
"""绑定并登记当前专用线程;失败时撤销登记。"""
|
| 302 |
+
|
| 303 |
+
role = str(role)
|
| 304 |
+
if role not in DEDICATED_THREAD_ROLES:
|
| 305 |
+
return False
|
| 306 |
+
cpus = self._policy.cpus_for(role)
|
| 307 |
+
tid = int(self._native_id())
|
| 308 |
+
binding = ThreadBindingSnapshot(
|
| 309 |
+
tid=tid,
|
| 310 |
+
role=role,
|
| 311 |
+
label=str(label or threading.current_thread().name),
|
| 312 |
+
cpus=cpus,
|
| 313 |
+
)
|
| 314 |
+
with self._state_lock:
|
| 315 |
+
self._bindings[tid] = binding
|
| 316 |
+
try:
|
| 317 |
+
actual = self._apply_exact(tid, cpus)
|
| 318 |
+
except (OSError, RuntimeError, ValueError) as exc:
|
| 319 |
+
with self._state_lock:
|
| 320 |
+
self._bindings.pop(tid, None)
|
| 321 |
+
self._logger(
|
| 322 |
+
"[CPU-ISOLATION] dedicated bind FAILED: "
|
| 323 |
+
f"role={role} cpus={cpus}: {exc}"
|
| 324 |
+
)
|
| 325 |
+
return False
|
| 326 |
+
|
| 327 |
+
report_key = (tid, role)
|
| 328 |
+
with self._state_lock:
|
| 329 |
+
should_report = report_key not in self._reported
|
| 330 |
+
self._reported.add(report_key)
|
| 331 |
+
if should_report:
|
| 332 |
+
scope = "Fays" if role == "fays_input" else "Tactile"
|
| 333 |
+
self._logger(
|
| 334 |
+
f"[{scope}] CPU bound: thread={binding.label} "
|
| 335 |
+
f"role={role} cpus={actual}"
|
| 336 |
+
)
|
| 337 |
+
return True
|
| 338 |
+
|
| 339 |
+
def bind_dedicated_thread_for(
|
| 340 |
+
self,
|
| 341 |
+
policy: CpuPolicySnapshot,
|
| 342 |
+
role: str,
|
| 343 |
+
label: str = "",
|
| 344 |
+
) -> bool:
|
| 345 |
+
"""Bind/register a dedicated thread without changing global policy."""
|
| 346 |
+
role = str(role)
|
| 347 |
+
if role not in DEDICATED_THREAD_ROLES:
|
| 348 |
+
return False
|
| 349 |
+
cpus = policy.cpus_for(role)
|
| 350 |
+
tid = int(self._native_id())
|
| 351 |
+
binding = ThreadBindingSnapshot(
|
| 352 |
+
tid=tid,
|
| 353 |
+
role=role,
|
| 354 |
+
label=str(label or threading.current_thread().name),
|
| 355 |
+
cpus=cpus,
|
| 356 |
+
)
|
| 357 |
+
with self._state_lock:
|
| 358 |
+
self._bindings[tid] = binding
|
| 359 |
+
try:
|
| 360 |
+
actual = self._apply_exact(tid, cpus)
|
| 361 |
+
except (OSError, RuntimeError, ValueError) as exc:
|
| 362 |
+
with self._state_lock:
|
| 363 |
+
self._bindings.pop(tid, None)
|
| 364 |
+
self._logger(
|
| 365 |
+
"[CPU-ISOLATION] slot dedicated bind FAILED: "
|
| 366 |
+
f"role={role} cpus={cpus}: {exc}"
|
| 367 |
+
)
|
| 368 |
+
return False
|
| 369 |
+
report_key = (tid, role)
|
| 370 |
+
with self._state_lock:
|
| 371 |
+
should_report = report_key not in self._reported
|
| 372 |
+
self._reported.add(report_key)
|
| 373 |
+
if should_report:
|
| 374 |
+
self._logger(
|
| 375 |
+
f"[CPU-ISOLATION] slot thread bound: "
|
| 376 |
+
f"thread={binding.label} role={role} cpus={actual}"
|
| 377 |
+
)
|
| 378 |
+
return True
|
| 379 |
+
|
| 380 |
+
def unbind_current_thread(self) -> None:
|
| 381 |
+
tid = int(self._native_id())
|
| 382 |
+
with self._state_lock:
|
| 383 |
+
self._bindings.pop(tid, None)
|
| 384 |
+
|
| 385 |
+
def register_child(
|
| 386 |
+
self,
|
| 387 |
+
name: str,
|
| 388 |
+
pid: int,
|
| 389 |
+
role: str,
|
| 390 |
+
*,
|
| 391 |
+
allow_fays_stage_threads: bool = False,
|
| 392 |
+
cpus: Optional[Iterable[int]] = None,
|
| 393 |
+
stage_policy: Optional[CpuPolicySnapshot] = None,
|
| 394 |
+
) -> None:
|
| 395 |
+
"""登记一个子进程的 CPU 域供审计器维护。
|
| 396 |
+
|
| 397 |
+
Fays 子进程的 ``role`` 仍然记录为 ``fays_background``,但其实际
|
| 398 |
+
启动域是全部 Fays stage CPU 的并集。已命名的 native
|
| 399 |
+
输入、预处理、特征提取和跟踪线程由 ``_FAYS_STAGE_ROLES`` 收窄到
|
| 400 |
+
对应 CPU。LocalMapping/LoopClosing 及其他未识别线程保留在子进程
|
| 401 |
+
启动域,避免长期后台计算被压到单个逻辑 CPU。
|
| 402 |
+
"""
|
| 403 |
+
|
| 404 |
+
child_pid = int(pid)
|
| 405 |
+
if child_pid <= 0:
|
| 406 |
+
raise ValueError(f"invalid child pid: {pid!r}")
|
| 407 |
+
child_cpus = tuple(sorted({
|
| 408 |
+
int(cpu) for cpu in (
|
| 409 |
+
self._policy.cpus_for(role) if cpus is None else cpus
|
| 410 |
+
)
|
| 411 |
+
}))
|
| 412 |
+
if not child_cpus:
|
| 413 |
+
raise ValueError(f"empty child CPU domain for {name!r}")
|
| 414 |
+
if role not in {
|
| 415 |
+
"left_process", "right_process", "fays_background"
|
| 416 |
+
}:
|
| 417 |
+
raise ValueError(f"unsupported child CPU role: {role!r}")
|
| 418 |
+
if allow_fays_stage_threads and role != "fays_background":
|
| 419 |
+
raise ValueError(
|
| 420 |
+
"Fays stage thread routing requires fays_background role")
|
| 421 |
+
registration = ChildDomainSnapshot(
|
| 422 |
+
name=str(name),
|
| 423 |
+
pid=child_pid,
|
| 424 |
+
cpus=child_cpus,
|
| 425 |
+
allow_fays_stage_threads=bool(allow_fays_stage_threads),
|
| 426 |
+
stage_cpus=(
|
| 427 |
+
tuple(
|
| 428 |
+
(role, tuple(stage_policy.cpus_for(role)))
|
| 429 |
+
for role in sorted(set(_FAYS_STAGE_ROLES.values()))
|
| 430 |
+
)
|
| 431 |
+
if stage_policy is not None else ()
|
| 432 |
+
),
|
| 433 |
+
)
|
| 434 |
+
with self._state_lock:
|
| 435 |
+
self._children[registration.name] = registration
|
| 436 |
+
|
| 437 |
+
def bind_child_process(
|
| 438 |
+
self,
|
| 439 |
+
name: str,
|
| 440 |
+
pid: int,
|
| 441 |
+
role: str,
|
| 442 |
+
*,
|
| 443 |
+
allow_fays_stage_threads: bool = False,
|
| 444 |
+
) -> Tuple[int, ...]:
|
| 445 |
+
"""Apply and register one child CPU domain through the sole owner."""
|
| 446 |
+
|
| 447 |
+
child_pid = int(pid)
|
| 448 |
+
cpus = self._policy.cpus_for(role)
|
| 449 |
+
actual = self._apply_exact(child_pid, cpus)
|
| 450 |
+
self.register_child(
|
| 451 |
+
name,
|
| 452 |
+
child_pid,
|
| 453 |
+
role,
|
| 454 |
+
allow_fays_stage_threads=allow_fays_stage_threads,
|
| 455 |
+
)
|
| 456 |
+
return actual
|
| 457 |
+
|
| 458 |
+
def bind_fays_orb_process(
|
| 459 |
+
self, name: str, pid: int,
|
| 460 |
+
policy: Optional[CpuPolicySnapshot] = None,
|
| 461 |
+
) -> Tuple[int, ...]:
|
| 462 |
+
"""Apply the full Fays-stage cpuset to the ORB process.
|
| 463 |
+
|
| 464 |
+
The child is registered with the ``fays_background`` role for lifecycle
|
| 465 |
+
bookkeeping, while its audited domain is the union of every Fays
|
| 466 |
+
stage role. Otherwise a worker created by TrackStereo cannot bind to
|
| 467 |
+
its ORB CPU. Native LocalMapping/LoopClosing threads are restored to
|
| 468 |
+
this complete child domain by the affinity auditor.
|
| 469 |
+
"""
|
| 470 |
+
child_pid = int(pid)
|
| 471 |
+
selected_policy = policy or self._policy
|
| 472 |
+
cpus = selected_policy.fays_process
|
| 473 |
+
actual = self._apply_exact(child_pid, cpus)
|
| 474 |
+
self.register_child(
|
| 475 |
+
str(name),
|
| 476 |
+
child_pid,
|
| 477 |
+
"fays_background",
|
| 478 |
+
allow_fays_stage_threads=True,
|
| 479 |
+
cpus=cpus,
|
| 480 |
+
stage_policy=selected_policy,
|
| 481 |
+
)
|
| 482 |
+
return actual
|
| 483 |
+
|
| 484 |
+
def bind_child_process_for(
|
| 485 |
+
self,
|
| 486 |
+
policy: CpuPolicySnapshot,
|
| 487 |
+
name: str,
|
| 488 |
+
pid: int,
|
| 489 |
+
role: str,
|
| 490 |
+
) -> Tuple[int, ...]:
|
| 491 |
+
"""Apply/register a non-Fays child with one slot's CPU policy."""
|
| 492 |
+
cpus = policy.cpus_for(role)
|
| 493 |
+
actual = self._apply_exact(int(pid), cpus)
|
| 494 |
+
self.register_child(name, int(pid), role, cpus=cpus)
|
| 495 |
+
return actual
|
| 496 |
+
|
| 497 |
+
def enable_dual_slam(self, track_cpu: int = 6, input_cpu: int = 5) -> None:
|
| 498 |
+
"""双设备模式:第二套 input/fays_input=5、track/output=6;
|
| 499 |
+
其余角色与单设备严格分区一致(general=10/11)。"""
|
| 500 |
+
with self._state_lock:
|
| 501 |
+
self._policy = CpuPolicySnapshot.dual_slam(
|
| 502 |
+
track_cpu=track_cpu,
|
| 503 |
+
input_cpu=input_cpu,
|
| 504 |
+
)
|
| 505 |
+
|
| 506 |
+
def enable_balanced_dual_slam(self, *, secondary: bool) -> None:
|
| 507 |
+
"""切换到现场验证后的双设备正式性能分区。"""
|
| 508 |
+
with self._state_lock:
|
| 509 |
+
self._policy = CpuPolicySnapshot.balanced_dual_slam(
|
| 510 |
+
secondary=secondary,
|
| 511 |
+
)
|
| 512 |
+
|
| 513 |
+
def enable_exclusive_dual_slam(self, *, secondary: bool) -> None:
|
| 514 |
+
"""切换到两套 ORB 子进程互不共享物理核的实验分区。"""
|
| 515 |
+
with self._state_lock:
|
| 516 |
+
self._policy = CpuPolicySnapshot.exclusive_dual_slam(
|
| 517 |
+
secondary=secondary,
|
| 518 |
+
)
|
| 519 |
+
|
| 520 |
+
def child_affinity(self, pid: int) -> Tuple[int, ...]:
|
| 521 |
+
"""Return a child affinity for controller-side post-bind checks."""
|
| 522 |
+
|
| 523 |
+
self._require_api()
|
| 524 |
+
assert self._get_affinity is not None
|
| 525 |
+
return tuple(sorted(
|
| 526 |
+
int(cpu) for cpu in self._get_affinity(int(pid))
|
| 527 |
+
))
|
| 528 |
+
|
| 529 |
+
def unregister_child(self, name: str) -> None:
|
| 530 |
+
with self._state_lock:
|
| 531 |
+
self._children.pop(str(name), None)
|
| 532 |
+
|
| 533 |
+
def audit_task_group(
|
| 534 |
+
self,
|
| 535 |
+
pid: int,
|
| 536 |
+
*,
|
| 537 |
+
allowed_cpus: Optional[Iterable[int]] = None,
|
| 538 |
+
allow_fays_stage_threads: bool = False,
|
| 539 |
+
stage_cpus: Optional[
|
| 540 |
+
Iterable[Tuple[str, Iterable[int]]]
|
| 541 |
+
] = None,
|
| 542 |
+
) -> int:
|
| 543 |
+
"""纠正父进程或指定子进程中越过职责边界的线程。"""
|
| 544 |
+
|
| 545 |
+
self._require_api()
|
| 546 |
+
process_pid = int(pid)
|
| 547 |
+
own_process = process_pid == int(self._process_id())
|
| 548 |
+
with self._state_lock:
|
| 549 |
+
bindings = dict(self._bindings) if own_process else {}
|
| 550 |
+
|
| 551 |
+
general = set(self._policy.general)
|
| 552 |
+
child_domain = {int(cpu) for cpu in (allowed_cpus or ())}
|
| 553 |
+
child_stage_cpus = {
|
| 554 |
+
str(role): {int(cpu) for cpu in cpus}
|
| 555 |
+
for role, cpus in (stage_cpus or ())
|
| 556 |
+
}
|
| 557 |
+
if not own_process and not child_domain:
|
| 558 |
+
raise RuntimeError(
|
| 559 |
+
f"missing CPU domain for child process pid={process_pid}")
|
| 560 |
+
|
| 561 |
+
tids = {
|
| 562 |
+
int(tid) for tid in self._list_task_ids(process_pid)
|
| 563 |
+
}
|
| 564 |
+
corrected = 0
|
| 565 |
+
assert self._get_affinity is not None
|
| 566 |
+
for tid in tids:
|
| 567 |
+
expected_binding = bindings.get(tid)
|
| 568 |
+
try:
|
| 569 |
+
actual = {
|
| 570 |
+
int(cpu) for cpu in self._get_affinity(tid)
|
| 571 |
+
}
|
| 572 |
+
except ProcessLookupError:
|
| 573 |
+
continue
|
| 574 |
+
|
| 575 |
+
if expected_binding is not None:
|
| 576 |
+
expected = set(expected_binding.cpus)
|
| 577 |
+
target = expected if actual != expected else None
|
| 578 |
+
elif own_process:
|
| 579 |
+
target = None if actual and actual <= general else general
|
| 580 |
+
elif allow_fays_stage_threads:
|
| 581 |
+
task_name = self._read_task_name(process_pid, tid)
|
| 582 |
+
stage_role = _FAYS_STAGE_ROLES.get(task_name)
|
| 583 |
+
if task_name in _FAYS_CHILD_DOMAIN_THREADS:
|
| 584 |
+
expected = child_domain
|
| 585 |
+
elif stage_role is not None:
|
| 586 |
+
expected = child_stage_cpus.get(
|
| 587 |
+
stage_role,
|
| 588 |
+
set(self._policy.cpus_for(stage_role)),
|
| 589 |
+
)
|
| 590 |
+
else:
|
| 591 |
+
expected = child_domain
|
| 592 |
+
target = None if actual == expected else expected
|
| 593 |
+
else:
|
| 594 |
+
target = None if actual == child_domain else child_domain
|
| 595 |
+
|
| 596 |
+
if target is None:
|
| 597 |
+
continue
|
| 598 |
+
try:
|
| 599 |
+
self._apply_exact(tid, target)
|
| 600 |
+
except ProcessLookupError:
|
| 601 |
+
continue
|
| 602 |
+
corrected += 1
|
| 603 |
+
|
| 604 |
+
if own_process:
|
| 605 |
+
with self._state_lock:
|
| 606 |
+
for stale_tid in set(self._bindings) - tids:
|
| 607 |
+
self._bindings.pop(stale_tid, None)
|
| 608 |
+
return corrected
|
| 609 |
+
|
| 610 |
+
def audit_once(self) -> int:
|
| 611 |
+
corrected = self.audit_task_group(self._process_id())
|
| 612 |
+
with self._state_lock:
|
| 613 |
+
children = tuple(self._children.values())
|
| 614 |
+
for child in children:
|
| 615 |
+
corrected += self.audit_task_group(
|
| 616 |
+
child.pid,
|
| 617 |
+
allowed_cpus=child.cpus,
|
| 618 |
+
allow_fays_stage_threads=child.allow_fays_stage_threads,
|
| 619 |
+
stage_cpus=child.stage_cpus,
|
| 620 |
+
)
|
| 621 |
+
return corrected
|
| 622 |
+
|
| 623 |
+
def _fail(self, reason: str) -> None:
|
| 624 |
+
with self._state_lock:
|
| 625 |
+
if self._faulted:
|
| 626 |
+
return
|
| 627 |
+
self._faulted = True
|
| 628 |
+
message = str(reason)
|
| 629 |
+
self._logger(f"[CPU-ISOLATION] FATAL: {message}")
|
| 630 |
+
if self._fatal_callback is not None:
|
| 631 |
+
self._fatal_callback(message)
|
| 632 |
+
|
| 633 |
+
def _audit_worker(self) -> None:
|
| 634 |
+
if not self.bind_general_thread("cpu-boundary-audit"):
|
| 635 |
+
self._fail("cannot bind boundary auditor to general CPUs")
|
| 636 |
+
return
|
| 637 |
+
while not self._stop.is_set():
|
| 638 |
+
try:
|
| 639 |
+
corrected = self.audit_once()
|
| 640 |
+
if corrected:
|
| 641 |
+
self._logger(
|
| 642 |
+
"[CPU-ISOLATION] corrected "
|
| 643 |
+
f"{corrected} thread affinity violation(s)"
|
| 644 |
+
)
|
| 645 |
+
except (OSError, RuntimeError, ValueError) as exc:
|
| 646 |
+
self._fail(str(exc))
|
| 647 |
+
return
|
| 648 |
+
self._stop.wait(AUDIT_INTERVAL_SECONDS)
|
| 649 |
+
|
| 650 |
+
def start_auditor(self) -> None:
|
| 651 |
+
with self._state_lock:
|
| 652 |
+
thread = self._auditor_thread
|
| 653 |
+
if thread is not None and thread.is_alive():
|
| 654 |
+
return
|
| 655 |
+
self._stop.clear()
|
| 656 |
+
thread = threading.Thread(
|
| 657 |
+
target=self._audit_worker,
|
| 658 |
+
daemon=True,
|
| 659 |
+
name="cpu-boundary-audit",
|
| 660 |
+
)
|
| 661 |
+
self._auditor_thread = thread
|
| 662 |
+
thread.start()
|
| 663 |
+
|
| 664 |
+
def stop_auditor(
|
| 665 |
+
self,
|
| 666 |
+
timeout: float = AUDITOR_JOIN_TIMEOUT_SECONDS,
|
| 667 |
+
) -> None:
|
| 668 |
+
self._stop.set()
|
| 669 |
+
with self._state_lock:
|
| 670 |
+
thread = self._auditor_thread
|
| 671 |
+
self._auditor_thread = None
|
| 672 |
+
if (
|
| 673 |
+
thread is not None
|
| 674 |
+
and thread.is_alive()
|
| 675 |
+
and thread is not threading.current_thread()
|
| 676 |
+
):
|
| 677 |
+
thread.join(timeout=float(timeout))
|
collector/core/gripper/sightac_sdk/API_VERSIONS.md
ADDED
|
@@ -0,0 +1,27 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Sightac API 版本
|
| 2 |
+
|
| 3 |
+
生产默认继续使用经过现场验证的 `api`(2.4.5)。版本选择必须显式,不能通过
|
| 4 |
+
替换目录或修改 `sys.path` 顺序暗中切换。
|
| 5 |
+
|
| 6 |
+
| 包目录 | 用途 | 默认加载 |
|
| 7 |
+
| --- | --- | --- |
|
| 8 |
+
| `api/` | 当前生产稳定版 2.4.5 | 是 |
|
| 9 |
+
| `api_legacy_2_4_5/` | 2.4.5 完整恢复快照 | 否 |
|
| 10 |
+
| `api_v3_2_2_ksq/` | 新兼容版 3.2.3-ksq.1 | 否 |
|
| 11 |
+
| `api_new/` | 厂商 3.2.2 原始参考包;目录结构不完整 | 否 |
|
| 12 |
+
|
| 13 |
+
新兼容版以稳定 2.4.5 上位机契约为基础,保留 `defer_baseline`、
|
| 14 |
+
`read_raw_frame`、`preprocess_raw_frame`、`process_raw_frame`、
|
| 15 |
+
`update_frame` 和 `update_calculation`,并合入斜面款负 Hue 不参与 Fz
|
| 16 |
+
标定的修复。新旧目录携带的 `libSonixCamera.so` 内容相同。
|
| 17 |
+
|
| 18 |
+
仅在完成 `FAYS_ONLY.md` 的现场顺序验收时,临时选择新版本:
|
| 19 |
+
|
| 20 |
+
```bash
|
| 21 |
+
KSQ_SIGHTAC_API_PACKAGE=api_v3_2_2_ksq \
|
| 22 |
+
/home/admin1/gripper_env/bin/python \
|
| 23 |
+
/home/admin1/KSQ_Gripper/gripper_version1/upper_computer_fays_opencv48.py
|
| 24 |
+
```
|
| 25 |
+
|
| 26 |
+
不设置 `KSQ_SIGHTAC_API_PACKAGE` 时始终回到生产稳定版。紧急恢复快照可显式
|
| 27 |
+
设置为 `api_legacy_2_4_5`;不得删除或覆盖恢复快照。
|
collector/core/gripper/sightac_sdk/api_new/__init__.py
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Pyarmor 8.5.12 (trial), 000000, non-profits, 2026-09-09T18:33:47.704240
|
| 2 |
+
from pyarmor_runtime_000000 import __pyarmor__
|
| 3 |
+
__pyarmor__(__name__, __file__, b'PY000000\x00\x03\x0c\x00\xcb\r\r\n\x80\x00\x01\x00\x08\x00\x00\x00\x04\x00\x00\x00@\x00\x00\x00\xb0\x02\x00\x00\x12\t\x04\x00\x06\xf9!\x9cF\xf9{jW\x03\x87\x8f7\x81\xa0s\x00\x00\x00\x00\x00\x00\x00\x00\x86Y\xa2\xf1\x1f$\xca)\xa0f/\xd0\xcf\xb3\xc9\xe7@4\xec\x89\xc139\xf6cK9\xd3BG\xe3\x80\x8f\x15\x15\xd8\xb2\x9d\xf0?!8T\xc8\xaf\xa8\x91,dV<\xd1\xda\xb3\xbf5\xb8\xd2\x84\xf5\xe5i\xba\n\xbf\x1d\xaf?.Y\xe9\xff\xbcy\xd5\x04)!\xff\x14,\x02\xa8\xe9\xac\xbe\xc9\x03\xad\xdcv\x93\xf0\xa6k\xf8bOw\xc5\xaa\xcf\xeb\xb0\xafO\xec\x94\x9c\xc0\xa1\x11&E\x9a6\xef\xdd\xfag\x84a\xfej\xe8-\xf9qy\xc3G8\xee(\xf9\xf8\xb5o\xdf&\x04\x9dp\x84Ox}V\x84\xfc\xfd\xffZ$\x1f\xd0}\xb2\\Q\x90\xedY\xdd\xb2\xd9\x1c`d\xb1\xa9\x80\xe3\xfa\x81\x987sx\xa8X\xf2\x14o\xc9\xd4~\xe2\xda\xf9\x06[\xe36\xd7\x96\xd4\xfbM\xcbv\x16\x9a\xc7\xb5q\xc7\xf3\xd4\x9cyy(\xe9m\x03.\x94Y(|/\xab\xed+\xa6h\x01Q>\\\xef\x0b\x92\x034l5Cuz\x1c\xa4N>~\xf8&\x19\x05\xa1W\xa0\xa0\xf5\xe2F<\x82+\xef<\x84\x9f\x8c\xd1Z\x935D\x89\x8bH\t\xd4\x99R\x85\xa1\xc8!\x13=4f\xdf\xbb6\x12\xab\xb3\x14nZ\x1aF\x8al\xbc\xab\'7\xa8\xae\x85q\x07\xee&\xc8\xda-\xe1\xe5+\xa6\xaa~jN\x13\xc1\x88\xed\xff\'Y\x0e\x05\xf9l7jb#\xe3\xc9\r\\\xb9)\x9c\xcb|\xb6\xc2/\xe3{\xc2u\x05\xb6/4KY\xfe\xe9Yt\xdd\xd4\xeeNV\xfe%\x0c\xfdJj:\xa2\x84\xb3\xae\xfbu\xc9\x05g\xdc-\x92%\xbf.\x80\x1c+\xe9\xb3t33F\xba\xee\xe8\xe0\x19\xd1y\x8b\x13\x8f\xf6\xaa\xa6\xf8\r\x06HO\xc0\x11d\x07\x14\x9f\x1e&\xe7x\x07\xb4\xbe\xed\x9bC\x12x\x88\xd5\x0f\x85\xff\x85\x9e\xfc\xae;\xc2\x80\xd5\xf6\t\xc9\x15F\xee\xba\xf3\xc4\xf8\x9c>\x87\x08\x08>\xecF\x06\xbf\x99\x9f\xc3?8\xc9f/D\xf5\x05\'\xbe\xc8\xb2%e\xd2!X\x87\xe7\t\x0e\xd6x\xc4\xcdH\xac\xec\xdd1\xab\xda`}S\x90\xa64\xb1\x8fN3\xf2\xb8\x8a\xd7\x92\x15^%!Z\xc54\xac\xf8\x1b\x9d-\xec\xda\xd7<\xfd\xa2\xe8\x17?9\xe5\x1e\xdb\x1a;\xa7U#\'\xe6\x0f-\xc8\x95\x96\x00\x92\xaaIR\x1b\x80\xd8t"c\x99\xb7?\xe4\t\x17*\xe1\xf1m;z6rA\x00~r\x9d\x9d\xa7H\x06u"e\xbf(9@\x86C\xf0\xdc\xc0\x9c\xaczD,P\x8e\xe6\xf9p\x00\xd6IgOW\xde\x82\x9d\xb6R8\xfe\x7f4\xd0\xc6\xb5\xe5\xce\x97\xaa!*r\x8c\xaf>^\xeceW\xd5\xef\x07\xe0\xcd\xbe\xbe\xe1\xa2\x99De\xb6Lhj\x86\x8fW\x8d\xff&\xf0\x10\xa5,\xac\xccQ\xdc\x99\x12\xc3\xa8\x8f\x1c\x80}\xd0\x99\xbb\xdf\xb8SREL')
|
collector/core/gripper/sightac_sdk/api_new/libSonixCamera.so
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
version https://git-lfs.github.com/spec/v1
|
| 2 |
+
oid sha256:f016ba98f81a7acdb1c33dcbfa5de9b756bee671df8c3d6c9a7506170d95063c
|
| 3 |
+
size 167528
|
collector/core/gripper/sightac_sdk/api_new/parameters.yaml
ADDED
|
@@ -0,0 +1,80 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Farneback光流参数
|
| 2 |
+
params:
|
| 3 |
+
pyr_scale: 0.5 # 图像金字塔缩放比例
|
| 4 |
+
levels: 4 # 金字塔级别数
|
| 5 |
+
winsize: 61 # 搜索窗口大小
|
| 6 |
+
iterations: 7 # 迭代次数
|
| 7 |
+
poly_n: 7 # 多项式邻域大小
|
| 8 |
+
poly_sigma: 1.1 # 多项式标准差
|
| 9 |
+
flags: 4 # cv2.OPTFLOW_USE_INITIAL_FLOW # 标志
|
| 10 |
+
|
| 11 |
+
# 单元格大小
|
| 12 |
+
cell_size: 1
|
| 13 |
+
|
| 14 |
+
# 步长
|
| 15 |
+
optical_flow_step: 35
|
| 16 |
+
|
| 17 |
+
# 压力缩放系数
|
| 18 |
+
pressure_scale: 4
|
| 19 |
+
|
| 20 |
+
# 压力图显示模式(0=灰度,1=彩色,2=3D指尖视图,3=3D点云视图)
|
| 21 |
+
pressure_form_switch: 3.0
|
| 22 |
+
|
| 23 |
+
|
| 24 |
+
scale_factor: 0.5
|
| 25 |
+
|
| 26 |
+
# 默认端口号
|
| 27 |
+
usb_id: 2
|
| 28 |
+
|
| 29 |
+
# ROI区域 (640x480分辨率)
|
| 30 |
+
roi_x1: 0
|
| 31 |
+
roi_x2: 640
|
| 32 |
+
roi_y1: 0
|
| 33 |
+
roi_y2: 480
|
| 34 |
+
display_angle: -1
|
| 35 |
+
|
| 36 |
+
# 触摸状态阈值
|
| 37 |
+
contact_threshold: 10.0
|
| 38 |
+
release_threshold: 8.0
|
| 39 |
+
slide_threshold: 1.0
|
| 40 |
+
slide_force_std_threshold: 0.2
|
| 41 |
+
|
| 42 |
+
# 摄像头参数 windows系统平面款器件
|
| 43 |
+
#camera_params:
|
| 44 |
+
# hue: 0.0 # 色调
|
| 45 |
+
# saturation: 50 # 饱和度
|
| 46 |
+
# gamma: 251 # 伽玛值:
|
| 47 |
+
# auto_exposure: 0.0 # 自动曝光
|
| 48 |
+
# exposure: -5.0 # 曝光值
|
| 49 |
+
# contrast: 50 # 对比度
|
| 50 |
+
# brightness: 0 # 亮度
|
| 51 |
+
# sharpness: 100 # 锐度
|
| 52 |
+
# auto_wb: 0.0 # 自动白平衡
|
| 53 |
+
# white_balance_blue: 4600 # 白平衡色温
|
| 54 |
+
|
| 55 |
+
# 摄像头参数 windows系统fpc平面款器件
|
| 56 |
+
camera_params:
|
| 57 |
+
auto_exposure: 0.0
|
| 58 |
+
auto_wb: 0.0
|
| 59 |
+
brightness: 10
|
| 60 |
+
contrast: 10
|
| 61 |
+
exposure: -5
|
| 62 |
+
gamma: 100
|
| 63 |
+
hue: 0.0
|
| 64 |
+
saturation: 64
|
| 65 |
+
sharpness: 0
|
| 66 |
+
white_balance_blue: 4600
|
| 67 |
+
gain: 18
|
| 68 |
+
|
| 69 |
+
## 默认摄像头参数 (根据 v4l2-ctl -d /dev/video2 -l 输出配置)
|
| 70 |
+
#camera_params:
|
| 71 |
+
# hue: 0.0 # 色调: min=-180 max=180 default=0
|
| 72 |
+
# saturation: 64 # 饱和度: min=0 max=100 default=64
|
| 73 |
+
# gamma: 300 # 伽玛值: min=100 max=500 default=300
|
| 74 |
+
# auto_exposure: 0.0 # 自动曝光: min=0 max=3 default=3 (Aperture Priority Mode)
|
| 75 |
+
# exposure: 0.0 # 曝光值: min=50 max=10000 default=166
|
| 76 |
+
# contrast: 50 # 对比度: min=0 max=100 default=50
|
| 77 |
+
# brightness: 5 # 亮度: min=-64 max=64 default=5
|
| 78 |
+
# sharpness: 50 # 锐度: min=0 max=100 default=50
|
| 79 |
+
# auto_wb: 1.0 # 自动白平衡: default=1
|
| 80 |
+
# white_balance_blue: 4600 # 白平衡色温: min=2800 max=6500 default=4600
|
collector/core/gripper/sightac_sdk/api_new/platform_utils.py
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Pyarmor 8.5.12 (trial), 000000, non-profits, 2026-09-09T18:33:47.771312
|
| 2 |
+
from pyarmor_runtime_000000 import __pyarmor__
|
| 3 |
+
__pyarmor__(__name__, __file__, b'PY000000\x00\x03\x0c\x00\xcb\r\r\n\x80\x00\x01\x00\x08\x00\x00\x00\x04\x00\x00\x00@\x00\x00\x00\x1a\r\x00\x00\x12\t\x04\x00\xe7\xcf\xa2\x98Nr\x8f\x86H\x03IC\x8c\xdc\x0b\xd2\x00\x00\x00\x00\x00\x00\x00\x00h\xc9\x9af\x88\xa8\x12*S\xcf7O\xc6\x14\xaf\xf238D44K\x93s=T\xbdp\xdb\xeb)K\xa6^\x02\xc4\xe5\xeb\xbc\xe9?u.\x1d\xe5\xdf\xa5\xe2\xbc@\xa5\x8c\xf9j1u0K\x9a\xdd\x9c0\xe7\xf7l&\xbe\xd0+6\x1a\xe1\x05\x99\x98\x8aT:\xdd\xae>R\xcf3D\xdf\x8d\xc0\xcc\x95\x8at\xce\xea\xf5\xc0\xfa\xdbh\xc82\x9d\x0f\x95\xdbh\x02@c\xbc\x07\xb3\xf3\xf6\xf9q\x97K\x1f\xfb\x10\x9d\x92\xef%|Y\xaa;3\xba:\x94\x90}>M\xaaeH\xc0OG\x8c}\xd8\xc1\xac\x17\x8eA\x08\t\xf8\xf5\\: R>G\xc4\xb9\xd4q\x86\x0e\xdf\x8aU\xb4\xdc\xb7c \xe6\xbcd0\xe3w\xfd)\xd4\xbf\x89j\x11z\xd5\xa3\xc4F?S\x90\x04cM1\\q\xb9\x96>@\xf4u\x11\x15\x16\x98\x03\xfcZ/_f0&S\x1c;\x15\xfb\xb40<\xfa\xc9l\xce\xbb\xaa\xb0C\xe9\x86\xae\xa9\xb1\xd6\xfdc<8\xf0}\xba\x06\xfb\x0f\xe5(\x11\xdd\xfa\xa3\xed\xae\\\xd4\xc2\x1e\xa3\xe1\xcf\xf0\xc8\x10\x9a\x88\x99\x93\x115<se\xd3\x1c\x84\xde\xf1\x1b">T\xaf\x9f\xda\xdb\xa6\xff5\xddma\x04\x13\r\xaaB\x19\x88\x0f\x7f)\xfb\x1e\x0c\x8b7j\xea\x9a\xa4H\xbb>\xcd\xef\xabK\xef? ^o2KW\xce\xae;\xec8\xb5}\x86\xd4?\x85\xb1\x83Z\xf8\xa7\x04\x81\x9f\xe6\xb8"@>.\xc6\xe0\t\x1eHL?\x07\xfb\xb0\xb52k\x87X\xd1\xc9\xc6\xbc\xef\xcbT\x1aU\xa0\xabqW\x12\x88\xab\xff\x1e\xd8x\xa0\x9bZ\x84.!g#\xc46\xa6\xc6U\xee\xeb#\xe6\x9fz\x86w\x90\x96K\xf7\xdel\xc6\xb4_R\xe3\t\x9f\xfa\xd1lx\x12t&\x11\x11\xe8\xbdK\xa5o\xf2\xedN\x95\t\x8a\x8aN\xc0\xe2\xcd\xdd\xfdx\x02\x0f\x17\xe4\xf4\xdf"8\xa2Dqf\xfb.\xc1\x0f\xe2\xfc\x82\x92\x9b\xd3\x8e1YA\xf2\xb9\x94*\xaaR4h\x1c\xd2\x02\x19\x99\xe2;\xf2o\xb4\x0b\x81\xca;\xc5\xbb\xce\x9c\x18\x85\xa7*G\xba\x85RQ\xde\x16\x06qq\x1c\x85\x80\xa0\x02&fn\x82\xa0T4y\xfb\xfa\x01\xcfy|y\x88\x81\xfc\x14K\x12w\xc0\x86\x82vX_\x8bR\xc2\xe9^T8\xaf\xe9\x84\xaf\xd1^\xbe\x80;o\x04,O\x92c\x9b8=\x0e\xea\xde\x12\x8f8\x0bC\x03\x1dk\x84\xd8/X\xf8}9\xe3\xcb\xab<\x99\xaf\x99#\xfd\xc5\x83"]cd\x01"\xdf\xd0&\x06\xf7\xaf\xb8\x84\xca!\x8aw,\xecs\xbaF\xb6F|\xa5\xfdW\x88\x10\xc6\xea\xa0\xfa\xcdv\xc1\x97n\xda8\xf9O+5\xa0\xf5\xaa!\xbdp\x19\x8c\x9b_\xf7\x18\xc9\xe5\x0f\x8du\xc32?\xd0F\x86\xd7\xc0\x9e+/\xc29\xf8PN\xcfw\xcbE\x7f7\xf3\x89*\xf2\x12n\xce\xf7T\xd7\xbaV\xcfG\x8b\x08\xf3>!\xe2\x03\xa0\xad\x9b\x93;\xe6[{\xe3\x13X\x11[\xf7\xa5\x89\xe3q%\xc7\xd1\x12\xbaEmy\x9b\xb4\x9d\t\x7f\xce)v\x99\xcb)\xa0\x82\x8b/\x8d\xa1\xfam\x1a?\xc7<\xcez\xbe\x05Yr\xe9\xae(\x0f\xe5\'\x85\xcf\re\x80;\x1f\xb1\x18\x17\x18\x969\xe7\xee\x84AL\xbb>\x06\xf5\xcf\x99\xa1t\xde\x08\x93\x18;\xbfs\x1b\xc0\x81{V\xf5N\xf8\xdfZ\xd4Zr\x90\x941[O\x15!\x88\x06\x90&@\xd6\xeb\x86!\xfc\x80\x05A\xc3S\xff\xe8\x9b\x9e\x13TX\xf9\xf5\xe5\xfd\xfc\xc9\xfe\x00$\x8fao\x9cd`\xf9j\x95T*\x8e\x9d\xb8DR`=XL}\x89cwn\xe6\xcb&\x10\xf7<\xf3\x97\xcf\x86\xbd\xc6\xb9<XL\x91\x8c:\xfe\xe6?X\xa7\xbf\x00\xd84\xa7\x16\x152\xd8\xb9\xd4\x03\x16\x10\x9f\x91\xb3 \xf2\xb0\x1e\xc1`\x04G\x8f`\x92\xe3,\xb2\x8a\xad\x81_\x9f\xb1R\xaf\xb7\x94\xa8\x90\x02\xd5l\xe1\xd60\xb9v\x97\x8e&3\x1ey\x15S\x04\xdb~\xf9E\xc3\xd3\x17\x93\xac\xe9s\xdc\x9b*\x90 `[\xc1\x0b\x13P#\xb8}\xc5\x04\x1c*\xb5\x12\xff(\x04\xc1\xb3\xfaa#\x08p\xff\xd5N\xb0\x01]\xd57\xc8u\x02\xb4\xacjL\xaaL\xd2N\xd2\x1a]\x1c\xd1\x1bwp\xf1l\xdda\xf2\xde\x81\x0f\x06R\x1f\x924\xc3\ra\xfd\xe3\xaa\xf3\x13\x18\xe2V\xb4\x08`\xec\x84\x0cz\x02\xec\xb2\x97j\xa0\x1a\xc3\x92H\xb3\xf3\xe9\x83\xe2.\xe9E\xf2\xb5\xfcc\xc9tPq\xfc\xf6\xfc\xe7Nj\xbd\xd6\x98[ya\xb0\x15\x11\xd5\xb3\x0ef\xec\xb7\xbe\xe2\xc2u\xfa\xd62\x86\x8a!\xab\xdb\x12\xb2\xb8\xf6\xb9\x1fK\x8eNK\xdc\x0f#_GU\xd1\xb6\xc8\xdd\xf3N\x9c\xac^\xf3\xae(\x82\x02\xaa\x95\x1d\x08m+-\xac\xa3e\x9a}5\x1a\xaeq8eEL"{\x03\x87ip\xeb\x90\xa1`h\xf5\xce\xea\xec\xc7?\xber\x97\xf8\xec#\xa5\xbf\x8fY\x11\xec\x8a\xd5\x05\x1e*z\x9a\xe3K`6+\xa2\xb6\xbb/E\xe9G\xbc?\xd9|\xb4\xf2\x8f+?\x98v\x00xJ\xd2\xa9{\xf6\x97!\x1c\xc3\xaa\x08\xea\xed\xd8]]\x8d\xe8\xdd\x96\x9f\x84>?\x06,\x14\x90\x8cx\xc0k\xc3\xa80\xa0\xfc\nRp\x1f\x8c<\x8a\x83\x0f/\xd5\x89b#/u\xcc):\x11\x81m\xf0L\x1f\xe8\x1c\x9b9\xf5\xf4\xc4n\x1e\xb8\x83k\xbf\x1a\xc3\x16$\xc7b\\\x9cQzY0\xee\xb0k\xbf\x86"\x1f\x02G\xf5\xcb\x8b@\xc8::\xc3\xff\x1eS\x19=\x12\xdc\xf9\xc54\xe9KK\x94\xdc\xbb\x92\x1c\x0f\x03WC\x14\xa0\x86\xfd\x0e\x9fk\x1b\xba&\x13\x13\xb5F\xce\xf0\x1fQk1\x86.\x9e\x0e_\xef\xce\xa2\xcf\x82\x08\x12\xfa\xd5>\xb1\x8ee\x17\xb0\xd1\x8fV\xca\xdf\x82\x05oG\x9dvT\x15\x8a\xa2\xed\xc1/Y\xcb\xe8\xee\xa2\x1f\x103\xc7\x05\xc3d\x92\x8do\x00\x94\x99\x16\xea\xb2]\x9bQ\x8e\xa2\xcd\xfd\x8b-\xa5{\xbc\x7f\tZa|\x15\x07Q\x18\xc3\xeb*\x15mN\x15!)\x07\x01\x99L\xda\xd8\xfcn\xb9+*)\x88\x1f\xd6A\x19\x9ba\xfd/\xaa\x9fc\xb3\xf8\xc3\xb6\x96\x87\x15,\xa7\x89\xfe%y\xce%\xdd\xa7\xd3\x12\xd3\x1c\xa7r\x06\x95\x8c\x91\xcc\xdb2b#RA\x9d\xe0&\xd2\x14\x91R\xd2+\xcf\x1bS\xb1\x80)0\xc0\xb3L\x9ag-\xa1@\xb5\xd9:\x99\xd5:.m\x05n\xb3[%\xf6\x0f_;\x95\x9fL\xb8z\xd8\xcf\xd6\xc2\x8f\xfb\x02\xe6\x9f\xbdD\xb3!\x87\x8c\xa4\xe0\xc8d\x0cF\x94\xec\xa4]\xbaL\xea\x94\xba`\x9e*\x08\x93G\x80\xb3L8\xf1V\xfc\x0bN\xda@\xd3q\x17\xb1\xd0jm/\xdd\xda\xc6\x88R\x03{\xca\x11XS\xad\x19g\x96\x1dK\xde\xb8B\xbe\xce\xbb\x0f\x9b\x80\x9f\xf4\x04+\x19:\\\x05\x99\xf3\xee\xa6\xf4\xc6\x1b#\xed!\x02\xd3j\x8dN\r\xc6\x00n\xd3\xaf\xfb\xe0\xbc\x02g\xac\x80\x08L\xc9vB\xe5\x89\xb7Y^0j\xf5qL\xb7\xe6\xde\xf7\x8e\xcdG\xd84Gr\x88\xe4c\x93\x14N\xe2\xf7^\xd4;=!\x96\xc6\xb5\xba\x06\x96\xf9\x88\xdb\xab\x18\x0f\x19u\xa8\xb3\xad\xf46*\xf5\x99\x19\x9e\x86;\x91\xe8\xc3\xbb|p\xc7L\xfd\xf1\xbe\xf28\xbb\xe3#\x02F\x0ej!\x01\xef\x11\xe9\x87$\x91\xd0W\x18C,4\x03\x11@\x0f\x14\xe2r\xcd\xca\xd7\xf4\xc5\x9c\x94\xfe\x06\\f\x86\xbfo\x8b\x80\x8c~E\xf6\x1e\xbd\xf5\xa7\xb4?.\'hfL\x86\x81\xc9\x19C|\x80\x0eL\xa7Vim\xc4l\xaf\xe37(\xb4p\x9e\x0e\x1a2&1|\xe8\xc3\xbc\xc6\xe0\x07v\xd3j\xbftM\xd9\xf4\x91\x7f\xe6\xe1plr\xccUMj\xb7\xe2\x17\x06p\x18\xd1$6!\x03\x80\x9dc\xec>\xc2\xc6\xe0\x8a\xd2Ds\x19O96\xa2uV\x1fq\x9f\x91\xe4P\x85o6D\x07\x86\xd3w\x00a\xc8\xd5\xd2\x06\x90U\x9e\xa8\xa0\x8a\x8bq\xeb3/\xb7\xe8f\xf4\xf3\xde\x17\xae\xd1[h\x15\x06G\x97\x17~\xe9\x85\x90h\xdc0&\xb9\x11,\xda\xa8\xb3~.\x19\xb1\x0c\x16\xd2\x83\xff\x85\xdf[y>\xc9\xef\x07\xd8\x89\x9at\x17\x1b\xd0o\xb5I\xedJ\x9a>\xe4\xebY\xb99\xb6\x16\x11H\xf2W\xe8\x1b\x8c\x86\xdf\xe7\rT\x17\xb3\xa1\x9bMg\x98\xb1M\x8f,\n\xef8\xc5\xb5>!t^J\x1dw\xe4o\xe1)\xd8\xe4u\xe3X\xe6\xabi\x9e\xb9#\x0f\xb3,J\xb1\xba\x9dfm\xcb\xd5\x9e]\xc4\xbdV6\x89\xd2\x8b\n\x99,\x01Nl\xd3\xe3qB\x8d`{\x0e\x9d\x12\xc1\x18\xc6+\xf5\xf1\x13\x92\x19\xec\x14m\xa9$G[\xb9?\x89\xac\xf8\xfcssT\x0b_\xaa\xae\xbeS9\x05\xfe\xbf\xb1\x87\x05X\xca@\n\x04\'}\xe7 \x87\x9b"\xc1\xcc\xcb,\x8f\xa0=v\x08\xda\xfdX4\xd2.\xe2Z_z\xf0\xfd\xbf\xd5m\xb6\n0L\xe6q\xa4\xf3\x17PvF\x04|\x03\x8a\xb2i\x05Bxb\x8b\xa9\xb0\xc5]\xe6\x1bn\xd2H\xb5\xdd!E#\xa8J\xc8C\xf1\xea&T+r\xac\xf7\xc1;\xd8\x04u5O\xcf.\xb4\xf7\x8eq\x13\x9b\xc9F\x91/\xf9\xe9\xd7\xa9\xbds\xc5$h\xdd\xa4\xcc\xd1\x9a\x0fg\xfdR sK\x9bB\xec\xa3$[\xed\x9b:\xe3\xfa\x87\x8ec\x04,\xcfm9y\x02\xf0]tTMA\xa7\x03n5L\x16X\x86D\x95\x9e\x86\x83x;\xc5J\xd1\xafO\x98\xfa \x8f\x8f\xfc\x13\x93_\x1e\xa4\xe2\x1e\x03R\xc3i\xa1\xb4\xaf\x9a\xd8\x91\xe2\xaam\x0f\xa3\xb6\x0f\x85\xe1\x7f\xca\xa2\xa2\xa0\xf9\xb1\xb4\xea\x1aJ\xdf\xa6\x1eu\x04\xd5\xef\n\xa8\xaa\xec\xb5\x91\x140X\xc9\x8b\xbe\x9fa\r\x1b\xb1\xfc\x07A\x980\x07\xdc/\xca\tE{\xcf\xef\xb7{\x90\x9e\xb7\x97\x8f\xfb8\x065K\t\xe0V\x15\x16\xf2\xa8Z\x19\xd3\xc6:\xa6\x8a\x87\xc7*\xfe+5M\xe5y)\xd9\xf8\xe3\xcb\xe3\xeay\xe9V\x84\xa0\xe4?A\xf7\xf4/\xe8\xa5\x95z\x05\'\x16\x057\xaa=\xe1\x1f{$\xec3g\x10D\xfa\xe5\xe6\x05\xca\x99\xb0\\\x9ac\xbew\xb5+GD\x18s\\Q\xban\xc7\xf8?\x8c\xf9|\xb2\xbc\x85\xe9\x96\xf3\x86\xe7K\x0b\xfeO~b\xa4\xf1\xf7\\b\x02\x0f{\xd2\x89\xd5\xc4\n\xec\x87ZNr!\x95v\xda\x16\xdcI\xca(k\x7fH\xe9\x89\x9f8\x16\x9f\xbb\xe8`\xb2\x1e\xe9\x1cMA#\x8b\x1c\xf9\x16\xe7kook]v\x95\xd8\x83\xc6k\ngB2\xca=\xa7\x9e@\x168\x86\xf9T\x08\xe2p \x92\x10"&L\xe8{:\xb9v\xc3On\x84\x19\xe6\xf3\xa5\xaa\xae\xdfK\x9d\x91\x0c=\x11B-V\x9e\xbb\xb1\xb3x9\xe1 \xf8&\xfc1\xc5\xf7\x08%\xbe\xe4P\xbc\xeb\x9f\xd8\x07\x7f\xb6\xe6\xdb\x85\x8c\x82\x96\xea\x0f\x06\xf1U\xb6\x9a,\x95+L\x07\x14mJZ)r\x81M\xceD,S\xb8bb\xf56\xbe\t\x96\x96g\xf1\x8d\xde\x11:u\xe7\xea\x9c\x06\x00LeG\xf4\xe8-rn\xb7\xb1\xc9\xb4.\\\xa3\xce\xef\xf4{\x0f%s\x03\xbdv\xd5{)\xa0\xa25\xb2\x82\xfcO\xf48\x89\xb5"][$P\x8a\xc6D\x8e\xdf\xd4(K\xf13\x00\xbf\xe4\xf5\xb56\xf3\xa0\xa4\x8f5\x08\x9b\xba\xc3\xf0\xfb\xd9z\xf6\x02 \xca\xa3\r6\xd3xslU/z\xc7Y\xc8\x1cQ\xe3\x0e\x93B\xd4u-J\x96P\xcf\xda\xeb\x9d\xc2\x1a7\x8cY<!\x0b\xf10\x0b\xd8"Nm\x82B\x85\xa08\x95,\xc5\x83-\xa1\xa1\x86\x95\xecU\x12\xeap\x81Xkj\x94{\xfe\x98[\xa5\x84\x17Y\xdfX\x95\x89\xa6b^\xe5cG\xe0\n\x8eb\x9d\xdceD\x03\x06\xf6\xa6k\xdb\x18F\\\xa7\xdb\xe3\xc2\xc37\xedFN\xed\xbb\xa1\x84Ua\x18xY\x95\xf7XYH\x9a\xd6\n\x00&\xf4M\x16\xbf\x13!\x93\xb7a_\xc5\xc5\xa7\x12\xe8\xdc\xb4[\x7f[?\xd9!\xe6\x0e\xb2\x8e5\x84\x99\xec\x11\x1d\xafr\x84\xe2\xabb$\xbd?\xe9\x0e\x07\xf7\xd9\x97\xda\xba@S\xac\x7f6\x02\x99\xd14p|\xc9\xb4%\xf3!\xefD\xe0_\xb5\xbf\xd5\x8b#\x17\x95\x95\x0bO\xdd5%mk\x80\x04\x7f8\x1cNu!\xfe`\x1d\x7f\x12!\xe2)\xdfl\x97\x83Y\x80GW/\x1bW\xe4\xfb)\x0f\x15\x0c\xf0\x1e\xe2\x98\x0e\x83\x0f}D\x91\x18\x9eC\xef\xd4\xac\xcby<\x92\xd5<\xfb\xaf\xbdXmu\x85\xeb\\\x05\xc29\x97o\xf99\xff\xef\x02.\xf4g\xf8.+&+\xc2\xa5o\x01\x88e\xba\x92\x86\xcdb]\xa6/\xb3\t\xbd\xfc\xc9\x1ec\xae\xa4}\xde\x18R\x96\xb0\xecMR\xfc\xea\x12y\x9a\xba\x0cS\xb5\xd4\x92\x06\x11\x96\xd0pg\x80\xcc\xe7\xeb\xbd\xea7\xc3\xe4\xc1(x\xeb\x0b\xe2\xbc\xae\x1a c9\xd2b\x8c-\x16\xb6BC1\xfa\x06\xec\xdd\xdem\x0b\xb1F\x13O\xde\xb2\xee\xbd\xeezFmIiV,\\\xd9\xba\xbcN\xe2_\xfcD\xd4\xa6.\xa6H\xc7AC\xdd,\x98\xf4L\xce\xfb[\x07\xfb\xe2\x03\x9d|\xce5\x14x`g\xef\x8d\xf57\xb8\xaby\x1f\xe0\xfc7z!\x8fM\x85\xa82y\x0c[#\x1bW\xdc\t\xf9a\x1dQ\xa5U:\xab\xf6]\x8e\x83|\x1d\xf8\x1c\xad\x92bt\xfd\xd2\x90\x8f\xa4|\xac\xc3\xcbJ\x8d\x90\x12\x8bt\xb0\x904\xfd\xc8Q%Yv\xa1\xb6<\x0cf\xb2\xca\xa0\xa1&\xa1AvEZ\xf4U\x96~\xe6\xbd\xde\xbe\xd6\x97~%m\xa4\xed\xda\x15FW\x0b^\x9a\xa99\xf6IH\xc7\xcc\xd0! w\xd3U\x84i\xf2\x0f\xd84!\xb4C\xceuP\xee\xc7cr0\xf3\xec\xd9\x008\xaf\xde\xf8\xb7\xabp\xc2\xef')
|
collector/core/gripper/sightac_sdk/api_new/sensor_function.py
ADDED
|
The diff for this file is too large to render.
See raw diff
|
|
|
collector/core/gripper/sightac_sdk/api_new/sensor_gripper.py
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Pyarmor 8.5.12 (trial), 000000, non-profits, 2026-09-09T18:33:48.062364
|
| 2 |
+
from pyarmor_runtime_000000 import __pyarmor__
|
| 3 |
+
__pyarmor__(__name__, __file__, b'PY000000\x00\x03\x0c\x00\xcb\r\r\n\x80\x00\x01\x00\x08\x00\x00\x00\x04\x00\x00\x00@\x00\x00\x00\xf5s\x00\x00\x12\t\x04\x00\xc9D!\x85O\xc7\x1a\xe56\xbb\x80&\xd42}Q\x00\x00\x00\x00\x00\x00\x00\x00\x8ccVH\x02{r\xff9\xa2N\x8e\x02\xaf\x83v\x8a\xc0\x8b\x7f5\x12\xff\x06\x93A\xfd\xbd\x86\xe4\xbd\xd3y\xf2\xc2x\xdc\x99\xcf\xe4\x9aU\x8d\xd3\xf7\xbb\xea\xe7}\xa0\x99\x1a\xacE\x17\xa2q\x06\x8c\xa8\x9a\xcd\xd6r\xce\x82\x13\xff\xda\xbf\xdd\xb0\x90f\xbf/`31\x8f\x0e\xce]\xa3\x0e\xc4\x99\xa8\x9d\xe5\xbeq\xb8k\xac#\xa7\xcc\x7f\xda\x9aMM\x86&\x0b\xa7q[i\x15b\x1a\xdc+\x0e\xba[\xfe\x1dM\x17\xc3]\xfe\xf0%75\xf0b]\x91\xc29\xc90\x9b\xb4\x9d8\x81\xbb\x94\x15\xc1\xce\xe5\x808j\x91zs\xb6Xj\x84\xcfa\x07\xc2e\x85\x91z\xe8\'\x1a\xae\xab\xcbw$=\xf1y\xef\xd2\xa79\xea4Z"x!\xd4k\xf1\xd8-\xb2&s\x8b\xeau\x84\xbb\x03p!N\x13\xb4C}_\xf4p\x8b\xaa\xe6\xb5^\xaf.4\\\xebW\x93q\xcd\xf8\x1f\xff\xfc?]\xb2\xa8\xa8\xbb\xdd\x0f\xe5A\x16B.@f\x0c\xcb[w)\x9c\xcc\xa5*\\\x00_*\\\xf0\x92\xcd\xe2\xcb\xc4\xf9\x99\x14\xb1\xb7\xf4\xb14Q\x96\xe3*\xf3F\x1e\x0bITB\xf4\x8c\x80#\xfa\xc8\x83\x80\xe3Q\x11\xe2\xcb\xa6\xe5p \x03SK\xde8\x1a\x8eb\x1b\x8aQTm\xdaD\x90L\xdf\x07zq\x1e\xcf\'nV\xe3j\xc8\x15\xa1\xa2\x16Z \x1a\x8b\x95<1\xd1\x15\xbc\xb3u\xda/i\x8b\xc5y\xae\x9dL\xc4"\xbe\xd4U\x85\x9e\x93\x84\xa6v-X\xf9\x82\x82\xd8\x01qD{$\xef\xba\x857Hn\xb5\x9e\xd3\xeb\xa3M&_l\xc9O\xde\x01:\x90K\x1e\x8c\xaf\xa8\x04\x01\x8ek\x84\x1e5\xd7B\x0e\'\x92\x86\xe8\x14^\x1ee\xf2\x16h\xc3\xdc\xf2X\x9d\xc4\x17S\xbcyat\xd7(\x83\xa4$\xc8\x08\xa5\xbb\xe3\x92\xd6\xc7\xae\xf9 \xbd\x93\xbf\x0cv\xfcN\xd2\xa2F"\x12\x044K\xce:*\x13F\x7fR\xd1\xef\x87&\x04\xc2\xdd\x0cC\xc7-@\xda\x87\x87\x11p\xe8\xb4L\xbdfc\xc4\xe3\t\xdf\x85\x99\x8b\x1a\xddE+\x83\xd9\xcf%\xbe\x81\xe1\xdb)\xa8\x0f\x01\x1bk\xf1\xfbDH\x9d\xd4\xb7\x95.\xe2\xd1\\\xeeo\xbc\x82a\x14\xae\xa1\x8cd\x8d\xad[W\x9ft\x16\xc3U[\xfb\x1cv~!\xbd\x1a\xc5\x97\x83f\xc9U\xf4\xfe\x7f\xc1 \x7f\xdb\xca\xbd\x03\xda\x83\x00\x8b=\xd4Ng\x076\x0c\x15E\x92\xa0Gi>\xecbL\x10\xfe\x8b}\xa0\xab\x0b\x95\xabK\x88\x851\x05\xbd\xe2\x14\xe4\xfe\xd13WI\xb5\x8d\x84\xe2\xbe\xa6\x8bG\xf8G2\xc1)D\xd6\x86\xa6\xf8\xc3K8B\xaar\xf6o\xdd\xb30\xafNAP\xc9\xe3h\xff\xb2\xba]\x82\x06-\x0bB\xcf\xc1\x16\x00\xd0\xb2\x14\xe5\xfb\xfc\'\xb2\xa0\xd0\xbdFT\xc5[\xe10\x92\x00\xab?,l\x9b\x8f\xd8\xc9N]\xfc\xb5+:k\xe2\x06\x14\x0b\xee[\xeawe\tB\x86\xdc\xf1$\x8f\x08\x8e\x95\xd8\x90\xb7\x94\xb8\x9f\xc9\x92\xff\xaa\xae\x17\xc7&\xce]\x9d\xef\x94T\xc5\xf80\x89\xffG\x81\xe5`I\x96Oh\xed\x15\xcd\xce<\x80t\xe1\xbd\x98l\xf9\xe5@\xa0\x9d\x1c\xe72\xde\x9a\xbf\xf6\x07\xaa\t\xa1s\xc8\xf2<\x05Y\x89\xbf\xb7\xddf\x8d^\xca\t\xd4rM\xe0\x02r\x9cm}\xc1\x92\xceiRO\x97\xe0\x1b\x08@\xee\xf8\x7fi\xa7\xf1\xb0\x01m\xa8-\x87\n\xeb\xc9\xfd]}\xe1\xde\'\x0cC\xdcC\xd4<7J\x06\'!I\xc9\xde\xb9\xb6\xa7<{\xa9\xe2\x86\x03*\xa1\x97\xfcp\x83\x18\x03\x7f[V=\xf9\xb0\x8d\xe7\xf8\x0c`O\xe3\xe8\xc0\xf3\x8fX\xb0C\x17G\x9f\xd4\xfb`6\xb7\xf0\xa4\xfa\x04Y4\x1aa|U-`/\x18\x89\x8fA\xbb\xe5#\xc4tvIr|\x06+{\xc5\xb4\xd6N|q"\x9d\xad\xdc\x1f\xbc\xf8\xcd\x0c/s\xa9F*\xb2\xa3\x06\xa7D\xf0\xd3\x1b->\xf0\x1d^\x89\xc4\xa3\xf2V!\x11{\xceF\xa3J\xec`)\x80\x01X\xa7\xc9\xac_\xc7t_\x1f\xb1\x0b\x9c\xe3\xf3\xe3\xbc\x89\xcd\xdc\x06\x8c\xc1\x9e\x9b\xae\xd8Y]\xa8\xc4A\xf2\xeb\xc3n\x85K\x8a\xd7|\x81\xaf*\x0b\r\x89U\xa7\x1eE\n\x1d\xd4`\xd0r\xc1\xab\x01q\xb2\xb1*X\xb3\xc7\xd3[\xcbc-y\xc4D$#\x90\xa4H\xb5\x84\x82;-b\x1e\n\xaa<\tZ\xc9R!\xcf\xf3\x11\xaf>\xf0\xfb\x08\xfb`_L4mlm\x8f\xec\xb1\xa2R1{Q\xf5\xce)\x91\xbf/|V\x0b\xcf$\xf3V\xcf\xa7Z\x9c\x90\x06\xbb\xc9p\xf6\x8d\x1ey\x85\xdf\xf9\x96\r\xc81G\n\x8b\x05,\xf4\xbb\x90\xf0y\xd4\nb\x01L\xea\x1d \x83\x95\x9f\x81$\x98\xed\x94x\xe9d\xdb\x10@\xe3\xdd\x87\xeb\x83E\xa1A\xe4MV\xe5@Do\xc3Y\xc7\x06m\x15@\xccd\xfd*\x87-\x83r\xd3\xaa\xf1\x1e\xf8\xa9\xd0\x98\xd18\x90\te\x02\xab\xfa\xa3\xf7q/8\xf8\xdb\x91\x8d-S\x009\xd9\xd8\x07\x15\x11\x9b\xe0\x85\xa3nR\x8a\xad\xa43\xb9@\xb20\xa41\xde\xe2\xdax\t\x94\x92\x13"O\x93\xdb\x19\xb40`w\xa2\xe0\xf0\r\x14\xd6\x87\'U\xe1\xf5in\xc0vX#*&\x1c\xf6\xd0\xc2\xde\xc1vd\x80\x9c^=P\xf0\xd6(\x1fp\xb8\x8cp\x8e\xaf\xd3\xd3e\x82\xe3\xdb\xb9\xf1u_\x91\xfd\xccu\x17"\xdb\xe1G^\xaa\x96\xaf\xb7\xe8\xa60P\xc8\xc8Z@0\xa3\xe6\xbb!p\x93\xece\xb7\xe2\x17uw\xd2\x7fi\x1dP\x05h\xaeQ\xa1\xc3\x04\xbb\x01\xa2<Z\xbc[\xbb|\x9a\xd5\xa7\xf5\xa28l\x85\xa8\x88JKT\xe1\xc5\xf0y\x19\x1d\x10pi\xf1\xf1U\xb3c \xfc\x15\xf7\xe5\xf7\xdf,\xbdVmA\xd5\x92\x9c\x00O\xbc\xa3~\xb5pyX\xb8`U]:6\x19\xf8jeB{\xef\xff6\xf0&2`\\f\x80\xb8\xb6b\xb9\xaa\xa6\x92R\'s\nP\xe9(\xd2\x04_\xc0@S\r\x02T9L\xafgk\xd1\x18\xcdM\xbfv\x97 X\xd2\xd1\xa4\xa8\x05\xd3\x80\xc45\x08(\x93\xb5wN\x80^\xfe\xb9%\xbezV\xfft$ C\xba\xe7H\x11lV\xe7\xf1\xfb\xa3j\x1d\xffoI\xa8[\xdfa\n8$/\xc6\xea\xa8||\xdaj]\xe9\x86~\xb5\xe7^)B#`\xde9\xdd\xc1\xd9\x9b\xd4\x0b\xb1*\xabWO%\xd8_Y\x9c\x95\xd7Z\x08\xce\xe3\x8e\xb1\xbd\x19\xe8X\x16@r;\xf3\xcfa(U\x81\x00\xae\xaa_\x8e\xfe\xf8\xa1\xa9\x1c\xdc\x10\x03\xebBp\xf0\x9b\xc3\xc9kM\x1c\x9e\xb5a\xfba\x12\xd3Q\xdc:2u\x83\xad\xedA\xd7EAND\xc0\x97\xa0\xd5jQ\xfc\xa4.\x13\xa8\xfa\xa2\xed\xae\xfe\xceH\x01\xdd28\x900Q\x82%\x00M\x9fV3\x99\xdb\x95n-O\x07XX\xe9\x11\xda\xa4\xfeS\xb5\xd4\xfc\x96\xb4\xb8\x87\x8dY\x80\x8a$\x93\xc8e\x1a\xf8\x87XJ\x15\x85`\x13\xdd\xad\xf9\x81\r\xbf\x81&\xce\x00\xb1\x1a{y\x9f\x02@\xabmA\xf9%U\x97\x1e\xda\xda*\x1eV\xe6\xa6\xaf\xa4\xbc\xdd\xbeiD\xf2}j\xb1\xe5\xccp\xe7\xa9hd\xc7\xa6\x17`\x84(CCm7\xc6\xe9V\x16V\xab\xe5C\x12T\x83\'\x10\n#\xd0\xa6\xe6\\\xbdy\xed\x12\x96\x03 \xb3~\xa3\xb9E\xfaT\x97\x89\x90\xe2Vg\xad\x1d\xca\xa6k\xe3R\x94\xd8(y\t\xa2V\xadT\xc5!9\x10\xc0\xae+y.Cd\xe2\xe5h$\xc3\xca\xda\x18\x87K\xbb.@\x02N(\xf3P\xba\x1e\xc8\x91;=s<\xae\xcb!\xce\x8b\x82\x03\x92\xd1\x87sP\x89~\'m:1\xd9\xaa\xfc\x0b\xf26\x14}\x9e%\xa21X\x8e\xe4/\x8d\xe2\x0e\xf4t\xd1\xff\xa7\xa2\x1b\x96\xad\x1c\xcdH\x98\xcd]\x85\xda\xb9\x90\xfd\x05\\t\\\xdf\xc4\xbc{\xbdS\x9ev`\x13U;\xd7\r\x9c?S\xce\xaf\xac&\xb7\x90\xb7\\\xe5\xda\x98p:b\x98\x97v\xb3W\xbf\xb4o\x83\xfc\xa2_\x9f\xec{\x03\x0f\xc3\x9f8\xcf\x87\xf4\xc0+\x01\xc6T\x9d\xd3}\x9c];\xb4J\xedT\x84\x17\xb4:\xc2t\xef~\x8b\xebXA\x04\xda\xe8^\x88Q\xd3\xe3\x94\x05ao\x0e\xfeH5\xbf\x87\xc7\x10\x85\xf2\x1e1\xf7i\xa2\xa3/\xc3\xa2\t\xcd+)3\xb9\xc9p1\xb4&\x90lO\xb7\xf9\xf9o\x1dY\x11\xd1\x1d4\xf7\xee>~!d\xc1\x82s\xf5\xeb\tw$_\xd1\xa0M8\xf7\x02\x8d;>\xc3\\5\xf2\xee\xb8\xfdN\xa3\r\xc5\xa7\xa4\xc3\x0e\xf6\n\x84\xdf\x94\xe0\x03\t\xbf\x82g\xbeI\x84NR\xbb\x97\xb8\xc7\xc5\xfev\x824F\xed\xb4\x9eD#@vm\xde\xb0%\xb5\xf3\'\x12\xdb?&\x18\xeb\xb5\x1e\xd8\x17m\x82U1d8\xab\xbe|\xde\xda\xa8n\x88\x10\xfa+\x89\xb1\x92\xc6\xbbr\xbee\xa4\xd2\x89_\x91\xda\x8ai\xf0$EO).\x08\xa1M\x91p\n\xc9\x19K\x88\x10\xce`\xc8\xe6\xf7h\x07\x94[v\xcf\xe5\xfd\x86\xc2|\xcc\x1d\x88\xcb`\xab\xe5\xc2\x18\xf6\xbd\xfdZ\x98\xf9\x0b\x0f\xc2\xaf\xb8t\x15\xa2\xa0\x985\x84\xd3%\x15jd\x9fM\xaa\xc7\xd7[\xaa\xb1\x1f\x87\xf3R\xdf\xf9v\xd9\x95\x01\xf1\xa1\r\xe4\xec\xb6+\xfb\xcf=Xf\xa78D\xa7\xc8\x93\xa6\xd4\xab\x8dt\x90\xf7r\x14\xba\xb6\x804\x08p\x05\xfcE\x8d\x06\x02\x16\x14h91x,\x06gKz\t\xa5\x04u\xbf\x8d\xbd/\xc9\xb6rt\xdd\xcfEc!3A,\xd1\x0b\x9d{\xc0\x05@\xb3k\x90j\x9e\xdf\x19\x07OyA\x8e(Q\xb7\xf2\x1b\xc1\xaa\\l\xdc\x17\xbc\xcc\xd1\xc77\x91\x129^\xa26\xc5\xf4k}\x8cJ\xf6<\x0c\xdc~h\xbf\xe2\xbc\x01Ui\xf7\xd7\xd6\xb0B9\x8dA\xdd\x1c\x9f\x04p\xfa\\2:>pD\x99#J\xcfV\xd6\xdd\xb4f\xa0Oe\x8dxR\xfc\x05S\nw6B\xa97k\x8f\x8c\x1e\xb8L<\xf2\xaf\xdf\xd7\r\x0c]0\x1e\n\xa3j^Ku\x86DP+\x16\xd2\xb73\xd3\xa9\xc6BlZ\\\xa2\x8cd\x83\xf8\x9c1\xc0\xbbn8v5&\xae\xba\xfb\xf6\n\xf6\xbf\xbd\xbe"\xff|\xa8\x1d\xbc\x8b\xc5U\x05\x9c\xb7\xdf4\x07\x05\x13.\xe6=V\xd6b\xf9#\t\xeaN3R\x81\xdf\xab\xa9c\x9b\xc0\\\xf50\xd7\xfd\x02\xfc\x0b\xb8\xefR\x88\x1eY\xd7_\x8dGs\xc6QE\xafX\xf8\xed\xc3\xbaN\xd4\xe6\xf7\x0b\x11\x12\x96\xb18,\xf4\xd3\xf1\xd8v\xe3\x04+\xae\x1a\xef\xab\xedU"l7\xc50S\x83\x8a\xf6a|\xd2.\x7fx\n-\x1d:\xb0Z\xd7{\x0c\xb5\x9fk\xd7\xdc\xf3$\xd3^7\xca\xc4}\x0e\xd9F\x07c\xdd\xb6G\xe9HG\xdb\x05\x97#3qy\x9a\x99L<\xf6\x89\xe1"\xcc\xd0k\xcf\x87Hog\x1e@\xf3\xe1=\xaa`M\x99o\'\xb3$\xba\x86[&>\xe22\xbc\xe1\x17\xd2.\x86\x03t\xef\x86\xcf\xd0\x1eA\tC\x9b\xdd{\xe4\xf2\x17\xff\xfebc}\xcbq\xa1\xe8\x1bG*\xfb\xec\x9c\x1c\x89|\xe4H\xca\x89\xe2\x99\xed\xe4\xff\x07\xec\x9a%\xad\xdaL\xb6\x81=rT\ny\x89\xed!Cs\xde\xe6\x90\xa205\xf6\x87\xf7f\xd0ch\x93\xc36t\xa1.\r\xd2\xf6\xa8~\xacW\x82\xf0\xe2\xfa\xdcuLh\x83\xbc\xa0{\xf9lM&gVh\xed\\\xd98\xafU\xcf\x86\xa21\xd7\x93\x8e\xa0J\x91\xff\x0c\xccJ\xe3f\xb8FI\xf6\x1e\xfc\xbe\xb5_\xd3\xce5\xd8f>xY\xa6\xdd\xb9"\x7f\xb3\x9e\x88.4\x00\n\x8e\xf6\xe2vz\xffC\x04\xd33\xd83\xc3\x8d9?\xabb\xb2\x9e\xad\xccrFB\xfe^\x9bh\x95\xa7N\x89\xb9\x92$\xb1AuLfJS\xa8\x04\x81\x98\xda\x02$\'\x99\xc6\xfc\x14\xdfL\xebNOR\x03#\xa7[\xff\xe2s\xb8G\xf1\xecV\xd0\x91xJ\x95\r\xa6\x15A\'\x1eym\xa0\xa1\x83\xea/\x80yuP\xa2\x07\xe0\x12\x05\x07lX\x01\xa2h\x9a\x83\xa1N\xd0a\x03 k\'j\xceMs\xe7\x01\xa6\x08\x7f\r\x99\xda-\xe9F\x9e\x18\xbc\x05QJ\xfd\x9e?r\x16\xdeya\xec\xfe\xa8-_y\xb4\xfd\x85\xc5B\xecUUI\x89\x03\xaf\x8b\x12\x9a\xb4|\x14\xcc\xed\xb5\xe3\x0b\x1f\x8f3\xfd\xcc\xeb\x98\xad\xa2\xf0\xd7\x15?\x8c\xe4\xdc}\xeb\xe9\xe9a\xfd\x05S\t\xad-q\x04\xe0\xd2*\xd9\xf6\xfeZ\xc8)r\xf3\xa2T\x07\x91UT\x16\xaa\xe2\xac\xf7!\x9cE\xf1\xd5M\xc3\x03J\xc3\xe4\xe4I\xe4nL\x87\xc3}\x10\x06\xf1\xear\xb5D\x81I\x9b|\xc1\xb3\xd7\xd2FB\x8d\xc5\xefZ\x10\x956%\xfb?\xac8#\x85s\xab\x15q\xb9\xfc\xf6\xa4\xce\x05\xd1\x1a\x10j\x92\xd8A0\x94\xd4\xfe$\xde\xac\xb8\x06\x10l\xc673\xfeh\x90\x19J\xdb"\xd8tf\x0e\xc2Q\xd2\x107U\xbe\x18\xd2\x03\x87\xaf\x1bU9\xde\x18\xbe\xd3\x05\x97\x94\x01\xe5\xe0K5\xa1\x00\x15WR8\xa3\xfe\x07\xecB{\xba\xa5eR\xed\x04\xf7\x8a\x16\xc0>\xfb"\xb0.P-\x80\'9\xbb&\xa5\xdb\x9b\xcb\x94"\xad\n\xd0\x94x\xa6\x84\xe5f\xa2\'\x14\x80\xbcS_&i\xa5\x84\xd1\x0eB\xe2\x0eth\x8d\xe0\x12\xc6\x0cO\x97\x93!\xc9\xb5X\xdbcP\x06\xedM\xc4\xb8\xc5\xa6\xe6=\x00\xd1\xa8\x1dJ\xfb\x01\xf4\x96hvan\x16\x1f\x9b\x9c[\x85.\xd7\xfc\xee>~u\x18\xed\xf6@\xad\xa7\x1c\xfe\xee\xdf\xb6c\x91\xf9\x9e\x18\x9b<`\xd4\xa0\xf4D\x01\xa4\x8c\xaf$\xb5\xefO\x80\xc5^lG\xf2\x16\x15\xfc\xf6\xfd\xe3U=<\x8d\xde\xbb\x8c*\x85\xdb\xc2v\xbd\xfb\xc6(\x0b\x99&^7)P\x9c\xee\'\xbf\xb2x\xa7qX\xc9\x05r\xd3\xf1\x1a\xe1\x8c\xfe\xa6\x96\x02\x1b\xedV\x97\x97\x8c\xabifFZ\xad\x85VU\x05\xe8\xaaN\xc4\xf1\xfc\x98\xff8:\xbc3uv\xfbeF\x0c\xc0+Z\xc9UU[\xfa|5\xfe\xcf\xc1\x13f\xe2\x8f\xf9:/\x90p\xda\xd8\xd7\xc9\x86J\xb3\xcba\x17zB/\xda\x16s\xc7\xe5\xc1\x08"\x03;\x01.\x8c\n\xc5aL\xb4\xc1>V\xc9\xf3t\x1c\x91_\x16\xe0\xa7\x1a\x0e \xc8\x04D]B\x96\xd00\xa5\x85\xa0\xba\xa3l\x9f\xfco]J\xf1\xf8\x1bs\x8a2U\xea\x881\xa3uG\x94\xf4=\x87v\xe3e\x17\x0b\xa4T\x1b\xc4\xa4\x10>\x96\'\xebN\xa0\x11\xec\x84\x98W\xb2\xd7\x16\'\x1f\\s\x07?Kc\xde\xd9\xa9\x81\xac~\x0e\xba\xdc\xd6\xfc\xdf\x9c\xbc\x00\xbdC\x11\x1b2\x8aI\xbf\xc6\x8d\x15(\x0fw\xb6y5>y\xda\xee\xc2\xb3l\xe0Au,\xf3\xcf*\xfc\xa7\xff\nN\xf4\xed\xd0\xd1\xe66\n\xfb\xac\xc2\x82\xa5\xb8,<h\x8c\\\t\x14\xa4\xa2V\r\xff\xe1\xeb@7#\xa8z\xcdE\xd5\xfay\xef\x16\x80\x97\xce\xce\x11\x9c$\x91\x01\xda\xdf\xec\x11\xd1\xe8\xa8\xe0k_\x18*\x15\xa2H\xaf\xb0^\xfc\xf1\xd05\xa0\xb6;\x0c\xd4\xd1\xd4\x89\x88\x05\x1d\xf7\xf2\x8ba&\x13^driuF\xfd\xae\xa4?\xd0S1e\x91\x03\xf8@\xa4\xfaC8\xeb\x8b\xb6\x99\xa5\xbd\x10e\xc2O\xd0\xd0\x13!\x1f5\x93\'\xf2\x1e\x811\x94\xaeB0\n<\xb99\xfe\xe3\xa2\xc35-p\xf3\xb95`fD\x1f!\x9f\xd28&\x91\xd3\x147\xe7F\xabL\x11\xf3\x1cn\x98\x15p8\xa6\xbc\x17\x0f\xbc\x18\x97\xb1Uu\xa5Vy\xbe\x99a\xe7\xf3aQ"JQ#\x94{\xabK\x18\x17\xb2T\x04:\xab\xd2\x9fv\xbcg\x87\x18\xd7*%\xd2\r\xfa\xfa\x99FG\x0b\x01K\xb5s\x15\x89S\xa5\tt\x08\x9bK4\xc8]6\'\x03[->\x04\xa4\xd5\n\x92\xe6\xf8\x1e\x98n\xe7Qw\xe0\x91\x12\xde3\xc2\xdc\x03|\xa6\x0e\xc7\xa6\xa2\xf2{/\x0f\x8b%q#V,6\x1b\xe5 ?\xb2\x95\x02Z&\xc16U~\xc7Jn-\x1c\x8e\xd0\xb1U\x85\xfa\xc0\x95\xe8|\x9ac\xb7?P\xdf\xdeR\x17E\x9amL:\xab3\xf0\xda\xb7\x81\xe4\x02MA\xbc\xb0\x9a\xdb\x89\xc3\x1d\x0b6x\xc4S+\x9e\xa6\x8d\x88\r\x0b4\x01\xca$\x1c\xa6\xef\xddw\xbf\xcfB\xb0\xb9\xb3a7M\xc7\xa2P?7\xcd4\x19\',\xf1\xf3\x8b\x10\xbb\x14L\xf7\xc5\xf3\xf48.#\xd1\xda\xd0\xf2\xc9\x18m\xc4z\xe5G\xb0\x9e\x80e&C\xaa\xee\x1b\xbc\x15\xa5n|~\xe1\x1d\x08k|\xe1\x97d\xff\xf1MvPW:\xe4\xcbf[\xb6\xfa\x87\x8f\x13\xf1}r|\xaa\xc7\xcc9\x1bvx\xdb\xfdo\x8eD\xb0\xd9\xd2\x82o\xa1\x15\x81|\xff\xe3\x07\xe2\x88\x05\xb86cA\xb86\x90\x1bq\xfb\xe6<\\\x0b\xbc\x8a\xb8\x15\xce\x9bEc\x96\x83S\xe8\x8a)\xd4(\xd1\xca\x9a1\xa6%y\xd0\x1f\xb7*-G\xf5\xb4\xe1P\x91Akr\x89\x1fK\xeaS\x86(\xccp}p,\xdcW\x08R\x156-\xd7\r\xc1\xf1@\xb0\xf1k\x14\x1e\xe7\xc7[\x82\x1c\xb2l\xffI\xe2\xcb\xd5Lo\x89\x1b\xf9Z\xa2\x1d\x99\x02\x14\xc3\x91.\x1c\xb9\x83M\xb8\xd3\xc6r6,\x95q\xde\xf5\xe2:@\xe2ZKU\x90\x85HY\x12\x0f\xf8\xa4\xf0\xe1"\x84\xb2\x97\xe1\xea\xa8;;\xf2\x18.\xa7^\xb9\xd4-k[}\xfe\xa1\xef\xaf\\0\xc5X\x96\x81\x15\x97,\xcau\xc9\x02x\xc4\x019\xa0\x91\xa0\x7f\xb0uFyPAI\xbf\x7f\xd8\\e\xbb\xd8TR\xe0\x9a\xadD\x0e\x12p\x9e2\xbf\xcc\xa5L\xfdH\r0\x8d\xb7\xdcf\xd9I\xa4\xb2\x9c\xb1\xfe\x91\xfb\x15\xfb*\xec\xc9T\xc7i\xb8\xdb\xa9.\xd4\x0b8 o\xb1\xe8{\xf1\xf1\xbd\xb2{\x8c,\x88e\xf7g\xf9Gz\x8d\x06B\xa4\xed\xb1\xfe\xd8O.r_\xba\xb6^b\x9a5\xf3u\xedp\x057\xc9J\'Z$a|\xd9&\xb6\xb5\xb6z\xb7\xec_.\x8f7@st\xb9\xc2N\xfd\x89\x07\x92U]i\x91\xbb\xb5W\x94\x7f\x1a\x19\xd3\xfd\xcf1\xd2\xb6\xfbs<\xb6\xcc\xa3\x0b\x92\xc1Z8\xf7Ze\x13\xe0=\x7fV\xad\x03P\xe6\xc0\x89\xcd\x12\x8d\xe3\t\x15\xbd?\x11oR\xb1Pj"l\xf3\x10\x1e\xfd3\x95\xf5\xe4\x85z\xd7\xf0~\xae\xbd\x80\xf1\xaa\xf4u\xe6\xc3\xa02\xfcY0\xf3H\xfd\x15\xf0\x87;\x05uH5\x9ee\xff\xe3t?k\x7f\x0c\xcc\xda\x9f\x8e}\xb6\x82P\x05\xc5U\x10!]Y:\xb0\x8eY\xd3\xf5C\xd0\\\x06E\x90\xdf\xa1\x9a1\xc0,\xec\xe4\xc6\x83\xb2\xd3T\xd8J\xf7\x86\xa5\xbe\xff<;\xe1\x92\xf8\x167\x0b\x10\xb2\xe1\xe9\xc6W3\x81/\xc8_\xb3\x997\xf0\xd6\xfe2\xd0AW\xc7xn\x8dtb\xb0X\xa6\x101\xd8\x1c\xc1g\x17\x81\x97\xa9\xeb2\xa4\xb6\xe4\\$\'\'\xe6\x1d\xf6G\x02\x032\xb6\xd9\xe4\xc6%\xdf\x12B\xf1\x04@s\xab7\xd7\xadJ\x14\xed\xfdN\xca\x7f\xb1$\x07m\xd9Y\xc7\x8e\xe8}*%4\x0f\xb6\xdb\xc0\x1e\x9by\xb0\x83\xec\x9e&\x86;\x84!\x9f"\xeb\xa6\xcc\xb5\xa8\xfd\xef\xe8\xa2\x06\x91\xca\n\x0fy\xa3\xf6D>x\x14\xd2t\xb1aG\xf2t\x1c\xda\x93\xb7\x02\xa7qs\x8b\x87\xcb\xdf[\xf4\x0f\xf9\xc3\x18\xd1v\x80\x92\xd2\xc8\x13m\xe76\x92r\x06,\t\xe91\x84S\xca\x95p]I\xae\xb0\xe8k\x8bXu\xfd\x80\xb4\xe7\x17\xdb\xc64[\xd1rg\xe0\x9a\xb8\xa1rn\xc1\xb7H\x11\x19~\xbc\xdb j\x90J\xa7e{\xfd\xbc\xfbrJ\x1c\xd0w\\\x84\x94\x95\x03\xf77\xdfj\xdc\xee\x0bF+$\xf2\xc4%\x1d\xd4\xd5\xe3P!\xd8~\xfaD\x86r\xd5\x15h"\xe1@L`\x9a\xfch\x8b*\x9aQ\x08y3\xa8\xea\xd3\x93\xe1D\x9a\x1f\xba\x9b\x03\x98\xde3s|\x80\x0e\xdfog\xf0\x899qQO\xfd\x1fs}\x92W\xe7^/D\xa7\x9b@\x8c"\xae{\xe0]\x06\x0b\x9cA\xa4!\xd3U\x86\xcbo1~\xd1e\xd6\x1b";\xfe\xdd\x93\xfdd\xf0\x11\x8b\xd5(\x07\xea\xa1\xe3\xd3^L|s\xa7\x1c\xbbW&\xf7;\x15\x96\xf6_4"\xb9ul1ko\x1b\x81\xa3\x94\x12\xfdc\xc3\x8d\xb9\x97\xc3>o\\_\x02?\x1cL^\xe4\x99\xaf\x1d\x118\x9aME\xe4h\x15\xa4\xa1\xbbq\xb9\xfbpm\x98^H\x8f\xb5\x9b[\x85\xb0a\x8c\x95\x03Xm\xe2\xe06\xadzq\xf9\xda\xe9\xb5WP\xef\x01\'\xb1\x9aRF%\xb9\xf9\x8c\xb1#\xa3V\xd5\x8f\x16\xae\x9a\x9c\xf9\xa3(&\xa7\x06 3\xa5\x876j4\xfd\x08\xe6\xfa\xc5\x9am\x05\xa05r_C\xcb\xf0U6\xdc\xa99]\xc2\xe6g~U\x8bA\xe2\xa4\x8eS\x99\xaa\x8ck\xaf\x95\xadwS\x8f\xabd\xb1\xdb\x852\x0fDO\xed\xbc\xb4\xd3&\xb2_?K\x17\x00\xf6\xc7\xb9^\xb9\xb5\x1e\xa0{\t\x01\xf38\x92\x08\xa5\xa6\xa1\xf5\x12\xe3k\x85?g\x91g"\x07\xd8\x86`\x8e\xec\xa3\xc5\xfb\xe4\x9a\xdf?\xb1\xcf\xea\t\xd8\xec\xd9\x8c0\x1f9[\xd3\x89ta\x90cJ\xd5\x80\xbd<?\x87)\x8c/\x905\xcc\xb0s(R\x0f\xf1E\x19:\x90\xd4\xf3Q\xe2\\\xb7\xea\xf0E\x8e\xcd\xa1I\x14\xf9X\xda\xae\xbb\xb94\x12\xea\x93\x88\xde\xe7x{6\x81P\xd0\xd4.\x9c\xde\xc9\xf5\xdd\xf0:\x05\xdfc*L8\x07\x00\xb0\xf5u\x0e\x0b\xc2\xd3\xc6?\xbe\xb8\x9b\xf8ThMC\x1e\x81\x14T\xa3U\xae^F\x0c\xd4.\x0b\x98\x92\xd2\x81\xe1\t#l\xe9\x97\x91\x8e\xf79\xe6:9\xbek\xd0G\x8f\r\x0f/\xdaS\xfa(\x9bQ\xfa-\xc6\x0fr\xe7\xf2\x83+\x07N\xb6\xb9\xd4\r\x00\x88\x84dI\x97\x96\x1c\xd5W\x8a\x93\x9e=\xf8|3-\x10\xa2\xdd+\xc2\xf1\x7f~\xc0n\xbf\x99\xc9L\xc41\xe2\x06\xadU\x90S\xa0)\xdc\xac\xaa\xdf\xcc\xf2 :}iu\xffB\xe57\xf9\x8d6c\x03\x95\xb2S)\xe07\x9c\x81U"\xf5?\xf86\x8f\xfc\xc8-\xe2\x1c%u\x93\xd7\x9byh\xb6\xca?T\x91\xe6\xac\xa3B\xf6\xe3\xa3\xf8\xc7\x93\xb2\x057\xc1\xb2\xd2?\xe7n\xf7\xbf\x18\xce[\xecv\x053\xd1\xec\xf8?\xc8\xf7\x87\xc8c,\xe7\xbd\xdcQ`\x8aT\xb5\xd8$s\xaa\xe9\xa9\xd5N\x0c.F\xc7\xac\x1b\xe0\xe7fz\x80\xcef\xd5\xb0\x0f\xb2H\xf6\xebH1`\xc8S\x8a\xcb\x8b\x00\xc7\xb4\xee\xe0J\xc7\xe0\xa6R\x8c\xcf\n\x89v\xa2`\x18\xcf\xcd!\xa3\xabGC\x17-\xb7\xc4\x91\xd29\xf1T\xefZ\xff\x81\x028*h\xe9\x13*\x013\xb4\xfe"\x06 c\xce\xe0\x07\xa8\xe3\xe2d\xcb"\xef?&\x9a\x08\xa5\x0f\x05y(j8\x90\xa2_\x1cW,K\x92\xad;J`c\xfag\xc1KY\xb7\xee\x04\x7f\xe4\x93\x86\xd8\x05\xe4\xcc\xbdrd\xf9~:\x89\xa9\x1a\xc6I\xca\xbf[Z\xf9\x81G\xe893\xa44\xe27<\x87\xbe\x89ReY\x12\xdd?\xcb\xb4\xfb@\x8c\xe3AI9\x1f&d\xec<%n\x93A\xe3tA\xc2t\xb4\xbcN\xa3\xa7\x99\xfa\xe9`\x18\xc1\x87\x8bTE\xb0<i\xe7\x05\x81Q \xa6\xccC>\xcf\x1e\xcf\xc8\xa9Yvw 3\xbe\xcec+\x10%\x02\xd2\xc5\xcc\xce\xf44jZ`\xf4\x15\x03\xc7\xfb\xf0D\xdf\xac\x9a(\xf7!]#\x1b\xac\x11\xa6\x12oGu`\x82\xed\xeb\xa8|\xab\xa8/%k\xf7\xc2\xbc\xb3/\t\xe2\xd0\x81]J\xd1\x94\xf8)\xc7\x7fR\x16*\xac\xc9\xcc\x87##\xb1\xa0\xe2\xd8\x82w\x97\xde\xff\x17W$\x9f\xc6Y\xac\x01\xaa\x1c\xeeX\xea\xad\xc3\n\xa4\xd6\x9c\x85\xb1&\n\xecf7\xad\xc2|\xd6\x1f\xf0\xfd_\xe1$V\xc5`\x98\x90\xc3\xff\xae\xbd\xc8\x1c+\x99\xf6\xe2^\xbc\x92=\xa3\t\x8bx<2\x868\xc5Y\x97\xff\x8e\n\x0bU\xd8\x86\xfeT\x8e!L\x0c\xf3e\'\x8a\xd1W\xaf\xe8O\x9e\xf1_\xadN\x13\xb5\xa2\xfb\xb3\x98\xd9vo\xe6\x7f\x87\x9b_\x95\xd2\t\xe3g`>\x1c\xf7\x01\x8f\xcd\x98_\x9d\xcf\x0f\xaa\x0f\x08\x00\xf4\x8e\'\xbd\x87\xea\x0c\xc7\xec\x96\xe4>\xc1`\xb0u%\x82\xe6I\xa5\xaa\xfc:L\tpK\xc8\x01\x83\xbeY:\xdc\x93x\x84\xd3\x7f\xe9~\xa9No\xc5\xdd\xb4\x91W\x8dx\xbf\xb3+\x1f;\xaf\xfd\xd9\xc3\xe8\xc58\xba-\xb4\xed\xe09\xa5\xd0\x1c\xea1-\xa0\x97+\xdel\xf9H\x8c\xe8x\x91770\x0e\xdc\x1c7\xe8\x06\xbf\x0c.w\x15\xd6+\xb5\x9dR\xa3-\x07\xffQ\xf9\xa2\xfal\x8b\x98o\x86\xeaD\xbf\xc9\xb7\xd3\xcelA"\x88A\xd6\x92\x97<\xeb\xf0\x98\x03\x95>\xb0\xd6\x7fA\xf3\xeaV\x95L\x16y<\x84\xa7\r\x158\xcec\xfb\xea?\xf1\xac\xf2@=\xf9A\xcc\xaa\x16\x92Y\x0c\x88p\x81\x1d\xf3O=\xa1q\x8fRg\xd9\x97g\xd0\xe3\x93\x9d\xbc\xe1\x16\xa7\x7fz\x19<J\x9e\x0e"\t~\x82\xc7\x1fr^\xea@\r{\xa4\x15\xcb\xc5\xb3\x17\xbbD4\xccv\x9c\xd8P\x9b\xeb\x9d\xb7\x88\xca!t@\xcaW\'\xd9\xc8jw.\x87\x91\xe8\xc0\xe2jxl\xf92\xe8\x05o\xbc\xac\x89\xe9\\\x95\x87\x80\xb7\xd0o@\x95\x00\x8a!\xcc\x113\n\xf4\xd3\xe1\xe9\x85\x8d+Z\xb7\xbd9\x9d\xc3\x82\x88\xf8\xa6A\xee\x9e)vbX\x86\xc2[D\x15(=!K\x9f]\xbd\x17\xd7M5x\xbc\x80\x86\x86\x00\x9fC\x01\x8fn\xf4E\x12fn\xbe\xb4\xff\xa5r.\xaa\xcd\x96\xcb\x12\x1e\xaa$S\xbbH\xe4\xc2n\x0co\xc8\xa5\xbb\xb4;\xb8\x06-A8\x1b\xf4\xf3\x89\x98\x9c\xd2}1\xae\xb3S\x1c\xde\x14\x8b\xa3rl\xfc\x81\x96\xc8B[T\x12\x0e\x7f\x04l\xe2\xda\xe6\xdc\x0c@\x96\x1b\xf8\xf6\x08v\xa3O\xf1\xa3@\xb5\xa5>L\x07\xed\xea\xfbys(\x1c\x9a|)\xbdX\xbc\x98\x95;\x91jT\x12i[j\xb7\xf1Yo\x1f\x10\xa0y\xa7x\xd5\xe9\x91^\x88m@\x07fx\xa0\xd4\x91[\xd7\x7f\xae\xf5\xe3\x85\xeb\xca\xcbi7U>9R\xa6\xb0/\x05\xd4|a\x119qn\xe1.\xfd \xc1\x00\x97\xe1\xfc\xb9\x15\x13\x87S\xfe\x8b\x80r\x0b]\xa5\x87\xbe^D\xa89\xc7b\x98\xe8\xf7\xfb\xa8\xc4\xf9\xb2\xdc\xd1U\x808\xd1\xa3+\xa0\x1f\x97U\x9b\x89\xd0\x12\xe3\n6q>%C\xe2\xe7\xdeI\xd9\x8b\xfa\x97\xa6\x97\x13\t\xe9\x869\x1a\x07\xea\x8dY\xf0\xc5{\xef}+\xdaRl\x80\x1f\xea"#\xbe@\xc8\xa9\xe8A\xaaz\xbc\xf3\xe8\x1e\xb0\x02[\xaf\x18\x8b\x14iN\xf27\x01\xa4`\x87\x98G\x0f\x906\xb2F.9J\xd9\xad\xa1\xca\rs\xca\xc6j\xabWz\xaaHN?\x7f\x97\xec+l\xa4WI\xa8\x94\x15G\x9f\xca\xdc\x1d\xcc\x89\x81\xaf{E\x9e\xe6\x1c+\x04\xaaP\xdfpe*\x81\x97\x04\xb9Z\xe4\xb5\xe2#\x8f\xd2\xd27\x1b\x07\xeb\xae\xc4\xc3\x0cB\xb1o7\x7f\x03\xa02!Y\xc9\xb8j\\Z"h\xa5\'\x1f\x00\xd5q\xceD\xf5\x89\xc76\t\x96\xa4\x1e\xc9)\xcb\x8c\xd9\xbb\x96C\xcb\xd9\xf1\x92\xd7\'Z\xa2\x9e\xd2\xa4\x03\xf4\x0f\xcfy\x9f\xf7\x00\xb2\xcd\xb6k\xe5\x9b\x05\xde]X\x8c\x94\xb9Ogc\xc4\xec\xf7\xd0\xfe\x1e\xa2\xa0\xc1\xc5\xae7\x00.u?\xc1>\xf3\xac0M\xe5\xae\xfe\xfdp"%\'\xba\x04J\xef\xe0\x17I\xb5\xfaM\x86\xbb=\xed0p\xab\x97w\x1e\xf2\xb5\xed\x1a\x13lY\xfc\xec\xc8\x06\x023\x90f|\xac-\xeb\x87\xbdk\xf4\xc2]\xd4b\xe6\x9a\x11h7\xd3\xbc\x81C\xe6\t\x00\xd7\xb2\x80\x8d\xba\xd1\xaa\xafh\r\x8e\xce)F\xc7!#&\t\x96\xa2}A?\xf7b\xd3u\xad\xe4\x17b6Z\x1d\xfe2\xa7L\xfa\x83\x1c\'\x06b\x10l\x1d\x03\xe0\xe1\x92\x04\xb4\xfb\xce\x01\x95\xba\xffZ3D\x8c\x1a\xc0\xff\xd9o\x17e1\x1a_\xe8~x\x06\x84RN\xa2l\r\xb8\xbc\x98_\xc7\x868\xcb\xf6\x80\xe9\xa0h\xb7WA\xa1#\xe2\xac-8p\xc8\xb8`\xc8p\xec_p4M\x1c\x00\xa3\xd59\xe4n\xdcT\x98P\xaf\xa9\xe04y\xe5\xaf@\x9b\xae\xa5\xb7\x85\x8c|0\x93\xfd\x85\xff0\xd0\xaf\x08\xe1\xa0\x94\xf8U\xa9u,\x0b\xea\xb4x_@p\x1fUn%\xe7u\xe0\xa0\xfd\xd4h92\x07\x1f\x81\xdb\x022\xc0\xb0\xb4H\x8a_\x8fo\xf2\xf1Y\xa78}\xba\x8eK+\xd5\x8c\xba>%\xa4\x04Gb\x87\xa4y\xf7<\x08$\xaa\x15\xa3T\xdfkG\xba\x86\x9a=\x08\x97[mk{\xd5b\x05\xd7(t7\xde\x07\x83\xbe\xc9\xc3\xd9yp\xd8\xa1\xeeUU\x1f\xf2\x03\xa4k\x9a\x89\xe5\xc2\x84\xbaK\t\xc4F\\F\xb4%\xdd\x0b\x8f\xb8\xc3r=\xb4\x17\x9c\x87\xa8\xe38\x1ad\xc9i\xbc\xe5,\x07\xfdq\x85:\xa1\x84H\xecu\xb7\x84\xbb\x02\x99\xf5\x16\xb0\x87f\x8a\x9d\x1e\xfa\xafM\x9cTa\xaf\xfb\xc3\x80\x95\x92\x05\xcd\xc8\xb0\x93\x93>Txn\x009z2\x9a\xc3O\x7f\xf1\xd9 /\x06\xb1\xe8\x86\xcaC\xb4\x14J\xa69\xbecXeR!\x9d\xcf\x93Zh4\xc7\xc0stG\x90\xa8#\xf4\x1d34i\x08T\'r\xfau\x18\xe4\x082\x85\x10[1\xce\xbbK\xea\x8f\x05H\x87\xb1\xe9\xa6g\xce\x14\xaeT\xd9^\xbe!k\x88\xf1d(Y\xbc0\x89*\x93\x05\x1c8\xde\x1e\xf6;u\xc4Yg\xbd\x8d\x1c\x06\xe6\xd3\x85\t\x1dL\x1c&?\xb0j\xbczL\xf9\xed\xdbNI\xb5ANtU\x9b\xc3T\x80\xe2\xfc(\x93\x89\xdd\x01\xa9\xfc(=\x81\x0eL\xf1\xa2\xe3\x0cb\xd3\xdb\xd3\xb4\xe3\\\x94\xab\xe23\xf0g\xc0IX\x80\ns\x1e\xec\xda\x99\xcf\xcebb\xb5\xcch\x8b\x94\xd9.\xf6\xd1u\x06R)8?o\x90*L>\xe2\xe14\x8c\xd26}t1\xe7\xac\xc1:Sl\x00\x1f\xebiz\xc6\x98Z\xc1z0\xa4\x07p\xca\xde\x01\xf4\xbf\x14\xa6\x85i\xb3\xdc\x0co\t\xe6|\x8fb\xf3s`\x15\xe6\x82\x7f\xaa\x962\x8ct\xc5\r\xdd\x85\xebJ\xaa|\xefb\xdb\xf7\x1c\x8fUE\x86\x89n\x13\xc8\xc8Bw\xe2\xa8{\xf4\x11\xb79O\xe4\xb1\xd7u\xd8\xe4\xd7\xe7\xe2\xc8\xc7}\x1e\xbd\xbb\xab<\x05\xbes\xbep1\xacN\x17\xf1#\xa6L\xfch\x91\xa0s\x98\xf6\xaf/\xb1\x17k8\xa7\xdb\xe2\x92\xa1\x86\x9e\x0e\xdd\x81\'|\xb4\x03\xc8.\x81\xd5\xaf\x1c\x14y\xd6}\xad\xe9\xb5\x0f3IzY<]D\xd6)\xc3\xbc\xef(\xbd\x89\xdb\xee\xa9\x1c\xb3\xfa\xfd\x19M\xe0\xac\xe8zV\xc5\x08B}Ko\x859v\x15\x840\xdf\xd10\xca\xc4\xfe\xca\xa0\x15\xd6\x17]\x8d\xa5\x96&\xba08\x95\xb2`\x19\x81\xbd\x1emk\x03\xcfi\xe8\xd9\x83E\xb7\xdf\xba\x98\xea>`\x98@n\xa7k\t;\x8d\xf7\x1a\x9c\xbd\xb6/t`I\x9es^x\x0c\xcf\nV\xd3\x82\x86\\\x89\x01\xce\xa9/\nZ\xa5\x92m\xcf\x8e\x11\x05\xfa\x93\xe1\xb7\xf1)\x8b\xd7]\xab4\xb9,D\xf0H*\xabH8\x0cG\xc8\x14\xcd\x11\xc6IS\x93N\xa0\x06\xe4\x17\xf8\x92\x04f\xa8\xb6\x86\xd8\x83\xc7x\x89R\xf0|\x9c\x0c\t@\xe9\x80\\?\xe90\x05\xdc\x12r\xd4`\x93t~\xf6|e-\x8c\x9eo\xa3\tL\xe6\x95g1\x98\xda\xfd\xdc\x89B\xa2\xaf\xdeM\xa5\x9al\x17\xaa\x95\x12\xb2\x99\x04\xbbQ\xc2"\xc7\x97\x87B\xf5\x1c\xf1\x9f\xb5\xb5\xa7\xcf\x9d\x0f?\x83\x92\xdc\x8d\x1a_\xa1\x1c$\xb4\xb9\xd4\xc5\xcd\x15\xbc\x8b^o\xd98d\x16\t/-g\x15\x00\xcf;\xc7\xa7\x08\xe9\xf8\xa4\x1dW\xbe\xb6\x87@\xba\xc1m\'\xf8\xaf\xd2\xdb\x8am\xf8M\xdb\xa7l\x9f\xa2\xb7\xa3\xe3\xe5\xf2?\x1d~\x9d\x94\xa4\xee\xbe7^\x9a\x03\x81\xe7\x91v\x0b\x05`?\x12\xe3\x02\xeb\xb1q\'\xe6L\xa0\x83J\xc9\x1ax\xf0[\x05GfMO\x91\xcb\xa0\xbf\xb6\x9b\xae\xdem\xcc+\xfd\xba\xaf\xb4\xf0(\x1a\xeb|\x02H.\x87\xaa_7\xbah\x1cn%GM\xe5\xef39-\xf7W\x00\xacG(\xf1\xe7\xa5wa\x19\xfd\x1c\xe5\xc1\xde\xb8\xdae\x0c\x80\xee`\xed\xbdi\x05\x8ff\x92\x97W\x86&qK\xde\xc9\xaf\xaa\xc3\x06\x1d\x92w*\xb7\xee&kO\xdf\x15mzL\x88#8>\xb3!\xf7\x95\xf3z\xa3P\x7f\xfb\x9bz\xe9h4\xf4\x89d$Q\xa4)\x98\x99Y<\x91\xd6_\x95\x0ez\xe30\x9dD\x03\x88\x9b\xfdExQy\xee\xe4\xd2\xa5\xe1(\xab\xfe\xeeM\xb7\xe0\xc0\xf1\xdb2\'\x95H\x83\x86\xb3\x1c\xedmI\xd8"\xbf2\x84HN\xf1l2\t\x9a?\xefc\xf0(\xc9\n\x82\xe2\xdf\x9a\xf8\x8b\xd9\x8b}5\x1aJ->\x8cn\x89j\xc8\x07N\x95\xb6\x8d\x0b\xf2\xd4\xff8\x11\xe7c\xb1\x01#?\x9c\xf7Z\x0bx\xad\xf7\x03\xe1J\xab$=\xf2\x11]\x7f#\x947#[\xc9\xeb\x12\x0c\x94b\xeaH\xc0\x0c\x84\xac\xfc(\xfe\x02\x97\n(\x00u\xa0\xf3\xad\xb6Z\xa2\xb5\xb5d?\x934[S\xa4\'\x974\xd9\xba\x03U\x15-\xd05\xf86\xa2\xcf\x11]\xc7\x13wi\xe4\xb0=m\xael\x02\x05\x9aZ\xd1\x86\xfb\xf5^bb\xc1\xb8=]\xd1P\x13h\x92d\xa2%\x8f\xf2\x86\xa8\xe5\x87\x1f\x14"\x92~\xb8[QAX\x88\xf9I\xef\xe2\xdd$[\xd3\xef]a\xdc3\xe8\x99i\xed\xdb\xa3.\tU\x08\x19\xe1\x073[\xbePJT\xfa\xf9\x94\xf3\x8f\x16A\x8142\xe4\x89\xbc\x05\x0e\xecE\x17\x08\x8a\xbb*\x11\xc1\xe2\nd^m/*\x02\x95\xae\x08\x88:\x12%$bH\xf5TU\xe5\xe6\x1a6\\&6(\xf3\xaa\x1f\x13\x84\x9d7$\xa0\xc7NK\xf2>+\xd2\x93\xaf\n\x0b\xe6\xa1\x8c\x93\xd65\xfc \xc2S\xf7L\x1ev\xb9\xde\xdb\x02\x01\xcf\x87\x93\xf9\xe7\x12n\xe18q\x14\x06\xeeP\xd3\x9b\xd0\xc9\r\xca#:\xdeg\xaeFQ\xa4\xa4\x1e\xa5\x08\x8a\xa3\x8b\x99[\xcc\xbd\xce T3}x\xbf\x11/\xd5p\xf2t7\xefD\x17\xea66\xfd(\xb4\xb7\x0e<\xb7\xc9\xd5\xbe\x053\r\xd1\xbf\xbfl\xe3\xc2\x9e]\xbf\xec\t\xb8N\x83J\xff\xf0\xf6\x8dWs\x1c\x15\xa0\xd8!I\x9c<#\xf5\xc0\x85g2\xf6\xde\x06\xb5\xb2\xe4\xae\xc91\n\xad\xb3V\x9bR\x83Q5f\xad\x88)\xbcw\xe2\n\xa5n\xe3\xdatM\xb4\xaaOI\xb7\xc8\x03G\x9d\x96\x9b\x9a\xf1\xac\xb7\x96|\x98\xb9/m.`2EZ\x8f\xbd\xa1|!\x9b\xb2\xc6RLH\x1ca\x07\x1a\xe6\xfb\xaf\xae\x97\xdf\xc0RF\xaff\xe6\xeat\xf5)\xd7\xdc\xbfD\xe5\xee^\x00\xc8\xeb"\xc4*+\x84\xa1}\xfb\xb6i\x92\xfb\x9fkM*{\xc9\x80\xe5\xde\x8c\x91\xb0,-y\x14mi\xccp\xe9\xf7+\xd42`\xc4Q6Qjo:!\x005QH\n\xcd\x8d{\nk\xd1\xe2\x99Q\x120\x89\xb3\x0eL]\xd4\x82\x16O!\xaf~\xec :\x10\x9a\xc0\xf0\xa9\x92\xbf&+\xeb"\r\xd4\xa4\xa7\xdd(\xcd\xe2H0y\x06\xec\x9a\x14\xb3\x94\xd0\xc9\x08\r\xf6J\xd2^\xc9N[\x04+\x9e\x9c\xfb\xb3p\x15p\xc9\x18\xa7r,\xaef\x8b\xf1\x86\xfe\x1cn\x8c\xa18bp\n0\xc4m\x9d\x95\x8d\xcb\x8c\x81_\x0b\x0b,\tB\t\x10\x05\tr\xa6@\xbc\xc1\\\xbc\xedI<[\x89z9\xf0||U\x1e\xf9h\xfe\xc7\x8e\xf7\xfe\xb8+\xe9^y\x99IE~\x87\xc2\xda>K^\xa8P\xafo\xf2\x01\xa3\xb2Q\x15\xeb\xb6e\xf6PX:\xae\xaavL\xea\x97nKF\xd9,\x8d\xff\xb6\x91\xee\xd7\xd5\xe6!\xf3\xc2\xdd\x10)\x05\xdc\xac\xd2\x8637\xd8\xf9\xba|\xccy\x0bU\x06\xa9\x9f\xd0`\xf3\t\xc2[\x92]\x97\xbf\x98\xc0\xeb\xc3(\xef\x10\xd0\xad|\xa7\xf1p^8\xcc\x87\xd7@9\xe4\xae\x02<\xe54\xbd\xc9`m8\x17\xf8{#f\xc6\x9d=\xc5\x80\x0e.\x18\xb1\xdc\xea\xf7N\xaf\xb5\xba\xbb\xaa\xd78\xea\x84[\x7f\x9a\xa9\xb7\x0c\xee\x80\x8e\xfaq\x13Ux\xa6%\x88\x1f\xe3*\x9c\xb3\t\xa3\x11\x9f\x10\xa9\xb87\xa5\xe8\xa1\xf6\xc2\xa2+\x8e2A\x83\xcb\xbd\x9d*y\xddb^n\x19\x19\xf5w\x8a\xac\xc9\xe8m\x90s@%\xae\n\xc5W\xc9\xfaxFw\xad\xcf\x8d\xde\x96\xd1d\xd8\xed\x07\xfb\x00m\x01\xa5\x0e9\'+\x81\xa2i\x96G*\xac\x9c\xda\x91\xc9\xd3HA\xa0E\n\x03B\x16rh\x1f\x98\xe6\x14x(\x96^\xa0T\xbf`,Zl\x8a\xf7#\xf1\x9e\xfdq\xdd3\x0c\xd7zQ\xdcJ(\xa9-\x81\x8b\xfd\x8dC\xfa\xd0\xc3yO\x9a]\xaf\xa3\xaa\x8egiBvd\t\x9a\xcfx\xd5\xc1\xb8\x83Y\x98n\xe5\xbc\xc5\'m*\xa8\x00Y\x0f\xd7Iu\x15-\x81\x80\xc0Q\xa1\xe0\x0c\xb3\t\xfa\xfc\xad\xf6;\xcd4\x9f\x9d\xe1\x0b\xba\x9a\xec\xe1,\xd3<\xe3\xc4\xfb9\xcd^\x12\xb0O\x94\xb01|\xe9<m\x06\xc3H\x9b\xca\x13\x12\x89\xac\x1b\xafb\x87=\xf3\xe7\x07\xff\xcfX*\xec\xefOj\x9d\xe7\x80\xdb\xb2\xfe\xc2y\x02\xe3\x8e\x1f\x1d\xf6\xe6\xa0\xdd\xc1;\x0c\x9f<%2U\xfa\xd0\xb8~\x95#\xd3O}J\xe4>5\xe7\xec\x9f\xd2\x9a\xbf+\r\\\x8e8.\xe5BP%\xbe\x1b\x7f\xc6\xad53E\xb1\x14\xbe\xb9\x0b\xd0\x9c\x18\x12\xbef\xc4m\x96%*aF}y\xa6\xa6\xea\xd4\xa5V"{.\x10\x12\x8c\x81\x00\x9bx|\xee\xd5\x9e[?\x1aa\x81\xa1\xa3%\xd2\x88\xa4x\x98\x02(\x94|\xa4\x83.\'\xe5\xaa\x14\x15\xe3Q{r\x07\xfd\x82Ce\x9bkM>\xed\x940a\'Nk\xbc\xd6\x99\xa2e\x1f\x9ez\xe1\xdc|s1\xd2Y2\x99\xe3)Lq4\x90\xadR\xde*/?\xf7\xa4Y\xe4\x00S\xabr\x0bThM5\xde\xca\x17\xe8G1\xd0\x07\x11\xa4y\xb1;yFH+{\xffK\xf5zY\x05\t`eL\xe9.\x85\xdd\xda\xa46\xacZ\xde!v\xd3\xef\x8eu\xbd\xf6X\x1d\xfd\xa4Y\x01J\x92\xcb\xee\x15Lc+A+_\xe5\x99L\x9eM\t\xaa>\x0b\xf9\xfb\xa1C&\xf1\x05\x0e\xa0\x1b\xb6\xfc>\xf0\xce\x1e\xcea[\xe4X\x92\xb9\x8eT\x7f\xdd\xb9\xd32\xb7\xc3\xfb\xdd\x16\x8b\x19\xde-O\x84\x17\xbf\xc8\\\x8a\xc3\xfaLSW\xf6jx+\xad#tc\xffZ\xb5\x86\xfbcq\x89\xa9y/^\xf5{@L\xe1m>f\x10\xa7\xb9u\x8c\x9f2d\x19\xbd\x12%\xaat1\x16\xcf\xb0\\\xe6]<<]\xa2\xcf\xcd\xd1\x8cl\x07\r\x06\x08\xaf\xd9\x96\xc1\x85\xc9h5\xb5\xfd\xbc3"\x9a\x05\xf1\x10\x80\xfa#\x1e\\\xbcC\xd7\x9d\xba]\x00r\xa9\xfb8\x88\xa4Ljc9x\xfd\xd8jM\xda\'\xc4\xe3\xdbZ\xe9\xa8\xea\x91WF\xb4\xcf\xa09\x84\\\x919E\xfa\xd4x\xfb]\xc6\x7fF\xa5\xa5O\'\xd7\x10\xab 4[\x08\nZ\xcc\xbe:&3\\\xb4NCqZ\xf3\x9d9\xd7\xbaY5\x1eaT0\xd4q\xe9\x1b$9\x98\xd2\x04o\xabW\x17+\xcf\t\x91\xde\xfc\x1d\xa2nB\xed7\xbc\xc1\x06\'\xc9%~9o(\xbe\x05\xc1+\xe7qA0\x1c\xea\x8f\x90\x0f\xa1R\xf7\x92\xbd\xb0\x18E\xd398G\xc2\xbf\x04\xd3f\x9b\xed\xf2\xbb\x1f+\xe9\xca\x03L\xf9Ij\x8a\xb9\x8c\xc9\xa8\x08B[j\xc6\xff\x0b\xf0Y\x86\x12\xfa[U;D\xb6]\x11n\xd3\xf7\xa58\xaf\xcd\x98\x9fO\xe0z\xa4/\xd2\xe7\x85\xd7\x1b\xe0\xe2\xca\xc2#\x8c.h\x9b\xbd`\xfa\x85\xc8\xfdd\xa6MC\xab\xb9\xa5\xad\x8c\xa8U\xd4\'C/\xf0\x9f\xb1{^V\xcd9gc\xd9;\xdd\x8a\\\xc0\xbf_M\xa5\x15S\x9c\x8b\x87\'\x85=U\x01\x9c\x15OA\xff\xe9V\xbe\':\x96\x91)&\x90/\xdet\xcc\xdau]IyK=D\x91\x86A\x85\xca\x91\x96\x94+\x19z\xdd\xc9\xbe$\x94\x10\xadd\xce\xd9\x98k%\x01+\xc9\x00b\xf7\xc0\x95\xb3\x86\x8a\x04g\xfa~\x7f\x17\xbery\x8f\xf3\xeb\xe6\x0cb\xa7q&\xdf\xc3@\x8c81\x8fV\x15E/\x11@\x1c\x04\xfb\xb8\x83@\xdf\x02\xbb_\x98\xd8\x81\x9e\xe2\xa8\xff;^yg\xb0]\t\x12\xdc\x16\xd2\x19\xa6]P\x199\xe8=NTo\xeep\x08J\x08\x92\x8d\xa9\xed\xec\xd8*\xc2\x85\xde\xdbo\x8a\xc0G\xc3\xd65\x91\xed\xf1M\xcf]\xc5\x04\xfb\xe6@\xdb+\xb9\xa3i\xf0\x9f4\xda\x08\xcd\x19\xe6\xd4\xd6I\\7?\x82$R\x14\x81\xd6\xbf^\x0cdV\x18\x87\x1d\xf8\xe0\xbf\xd8}\x12\xbd\xe1\xf3@\xb1^\xa9\n\xd2hs\xf2\xb5q\xba\xce\xc0\x07\xa2P8\xd1\x8a\xfb\x86\x01g\x03p\xcf\xacQ\xd6\xe1\xb9\x8cf\xd3\x9c\xcf\x00\xf5\xdc4Hn\xeb\xef\xa4\xe4n\xd1\'\xaf\x08\xc2\xadl\x99\xd9\x0eH\x96\x9b\tM\xd7mL\xd3\x99\x02&8\xd7|\xca0\xa1a_+\x8f)\x80p\xea \xd5L\xcc\xbdz\xc8\xe1\xe3~\xa7l\x85\xd0\xef\xca\x9e\xf4q\xe3\xe4\x87\x99\'\xabf\x1e\xfe\x91P\xa6\xe9}\xda/\x94\xfd\x9d"\xe3\xd1\xed\xb8\xa5P\xb3`\xe8\xf1\xb5Y\xc2\xe0\xec\x1d\x1c\xf5(\x82 \xca\xce)[\x7f\t\xff\x10\x8a\xa8q{\xca\xb9\x1esA\xfdr\xc2N\x00\xda1+\r#\xad0hV\x0b\xc0\xc4\x9f\xff\xf6Pw\xe2\xe4\x95f1\xaa\xd1\xa9"\xd6\x05\xd2\xc7\x8a\x18cy\xd0\xcb\xdd\xfa\xe7\xfa7g \x93\x99\xe8Q\xf3\xb9\x81\x06\xac6\x00e\xf2\xd0\xc7\xbb\x1a\'L\xe6\x9c\xb2\xf2\xa9r\xf1~e\xab\xa2 \xe8\x9b\xed8%\xc8\\?v\x0e\xb1x(\xd9J2\x1ar\xdd\xd9\xdc=z\xc4\x82\xfe\xd7\xa6\x8b\xb7+\x92B\xc1\\7Mc\xeek\xde\x9dnv,\x88\xc5\xf3a\xd4j\x9f\xc04pB\xd8R\xec\xe5{\tI\xfe\x11\xde\xbf`X\xaf\x92\xaf\xaa\x0f\x11BpI?\xcf\xdf\xdd\xcc\x1f\x8e\x07\xa2\x8f\xcel\'\xcb\xe9\xdc\xb2\xe0\xbe\xbf\xc4\xa7~\xf8Z\\\x9c\xa9\x04Y\xcb\r\xe5\x8e\xfb\xd8\x15\xfb\x80\x8bz\x9cj\x8d\xad :\xa1\x83\xee\x8bA\xcc\xb7\xc4\xc7\xc1>F!\x8f38\xde\xb3\x85i\x88\xa9D\xc1\t}\xe37\xdb\xa4Y\x86\xb6\x86\xd8,(I\x97\n\x03U\x06\x17x\xe5\x8dS\x9b\xa9\xef\xa7\xe2\xd58\x8d\xa8\xf9\xc7f\xa4\'\xc2\x9b\x0f\x06\x11\x17\x0f\xdd\xee\x80$J~\x93\x00\xb0\xdd\xc4\xb8\x85X\xef\xd8\x9aZ/\xdd\x1c\xe2\x903\xeb*\xb7K\x06\x17\xe71\xaeH\xa5\x82\xd2D\xf2)\xcf\xd5\x19\x00\xef\xa6\xef\xe16;\xbd\x88\x1c\xf9\x19=\xa8GW\xe6\xf3q\x12\xb5\tW>7\xfa\x80\x12\xf0hI@@m\x84\x93\x81Y\x8dE\xc4\xaa\xa0|>4\xe2>SY\xbca\x81\xb4|\x87\xc4 \x82\xa2\xeb\x1e\xc8\xee\xa3@RS\xc3B\xd4W\x17o{\x9f\xce\xc0-\xd0Pfm\x8d 1\xa1\x08\xdc\xa9(F\x9a\xab\x95s\x96A\x8e\xe12\xf6=A\x96\xd5\xac\xb3\xe5\x18\x0f\xb8Ej\xd1\xb3\x19\xff=\x86N\xce\x9e\x95?\xc2\xbc\x0c\xc1u,\xd8!n\n|\xf1\xaa\x95\xec\x01\xd2\x11\xf3\xc7+\xb6\x1e\x0c7bc\x1de\xe5\x8c4\xff\x96P\xd0\xfd\xe1\x9aZt\xb46L\xab\x86\x94z\x85\xaa\xcaa\x87A\'\xa9BRp\xa2bf\xd2\xa6\xb4\x8dGB\xad\x04\x9a.X8\xb3\xe2\x96\xc3\xab\xb9\x0eS\t[W 0\x8e\xbd\xfa\xac\x8b2#\xdfM\xd4\x03#:\xd18\xd0\xc4\xfa\x99B\x88\x84Y\x1b\xe4\xc0\x0c\x8bND\xe5\xbd\xdax\x17b\xd2\xdfr"_\xac\xce]\x96zK\xe4\xbc;\x18\xda\'\x85\xf9~\xda\x9b\x8a\x8f\x82#\x8ec\xe4\xe2\xd8\x9c\xafJ\xf9t5\xf7\xed\x8c+\xa4\x1a!i\x0c\xb1p\x1bcD\xe7\xee\xf5\xdey?\x8c\xd4\xc0\xb0\xed[\xc56ih\xe3i]Z\x97f\x14/>\x1c\xb3\x85\xc6@nI\x0e\xb8zy\x0e\x84M$\xeb\xce-s"f\x07\xa2\xa6}\xb9\x9a\x1bbV8\x0c\x8f\xa9\xac\xb3\x11o\xee\r\xf1\x97w+\x97\x97\xc15\x88\xf7_\x10d\xd4\xe9y)Q\xd5:\xe9\x18\x9bu\x86\xb0\x04\x8a)\x8e\xa2B\x0c\x10\x9b\xe5\xe2\xbd-sKQDf@\x92J\xf3\xb95s\xab\xd3\r$\x98j\xf4@\xe2\x8a\xa7{2*V&Q\x8b}U\xa9n\xf6C\xc4\x88\x9be\xa0n\x07V\xd5a\x13\x8aj\xeb:s\xabK\xb2\'\x0f4\xac/\xd2\xc2 \x01`\xf8\rHw\xaf5\x80\x15qs\xf0rBkQ\x96\xec\xf4P\x84e\x07\xf9\xae\xd1T\x7f\xdf\xeb2O\x7f\xc2L\x9f\xda\x9f\x9e\x82\x123K\x8d\x7fA#\xeeq\xf8\xd9w\xb1\x1c\xa5\x8d\xcb\xb3<\xba\x8d\xd6\x02\x84\x90{\xcd>N\x06]\xb7\x11\x83f\t\xda\xfa/\x0f\xbfux5^\xcam]>\x01W\xd3\xe7\x83I\xbf\xf0xoV\x1f\x92\x18\xca\xd4\xd2-\xf3CJ\xb4\xf9X\x03\xc2\xf9\x86\x00\xe0\xba\xa1c\xe8\xc3\xfa\xabz\xc4h\x18\xfc{vH/\xdb$@\x10\xb0\xa5\xf7f>Pa"\xebB^E\xe0\xfb\xb5\xc5\x18\x16\x0f\xd2\x19.\x07\xb2=\xee\x8b.v\x8a\xf7X2\xecy\x8d\xb2\xd7\xcd\xd7\xec\xbeJ\xb4\x05H\xa1\x9eV_\x1f,\xd7\x07\xaa\xb8\xaf\x18\x06\t\xd5\xdc\x1e\xc0\\C\xd6?\xd4\x0e\xa1<\xf1\xef\xf6\xe8\xf0\x0c\x8a<y\xca\xb5\xcd\xfd\x08J\x05\xd5\xef\xd5sh\xef\x86\xf4\x1fS\x06F(\xd2$\xe5\t\x14n\xa8|\x86\xc4\xdd\xc6\xf3Q\x06f`\x9a\xad\x80NxVY)\xf3\x86\xd4\xfcNy\xc78\x0e\xde\xa3\xc5@\xa7~\xa5\x93\xb9\x07K5\x9b\xb28\xdf\x1b^\x7f\xdd;\x1eN\xfa\xdc\xf6\xd3\x1dH\x9b\xab\xbb5\x07\x84\x8fS\xb9\xa9\xb9\x96\xb3\xe9\xf8\xc2\xa1\x08\xa4\xd4x\xd8\xed9\x87\xa9\x86R\xb2I\x0f`\xa3\xbdD\xff\x9b0\x9c\xf5\x89\x1a0\xb5Pu\xd7\x13\xe01Mo.\xc7VsC]Y\x98p\x82I+m%\xbe8>4\xb7F\x00@\xcf\xae\xa0\x9b\xe2.w\xe9\x9b~\xe8\x8aT!"pi\xbdr\x9d\xc3<\xabam\x17F\xfd\x8aCc\xcc\xf3\xa0.\xa8\x8e;\xda\xd2]X\x8bx/@\x1f#\xd4\xa2\x86:(\xf1B\xda\x86\x1a\xaea\xae\xc7\xecE\xeas\xe7\xf55"\x1f\xf5\xcb\xae\xb9>\x9b\x94#9\xbe>\x94\xd2\x8a\x8a\x0c\xfdz\xe1b\xa8\xfd\x1b\x91*G{4z\x1b\x9a\x1b\xb9E\xc5\x8dS\x82[\xd9\xa4\x87\xb1mG\xdf#\xa6\xf5\xefX[<\xa0\xcb%\xe2\x1f\xb4\xf1\xc5;\x9d\x15\xea\xb3\x8b\xc5\xbc\x82V\x17\xe9b\x1f\x85\x86\'\x1b+\x83\x83\xa6\x84I=\x89\x19\xa9]\xf2\xb2\r$\x14+H\xd5\x98\x8an\xe2\x00\xce\xf8\xab\xc2|\xcc\xf1\x88\x9d5\xc4\x92(\xbdW\xf0O4\x0f\xce\xf6;\x8c\xcbsU\x13o8\x9a\xf3:\x12\xe3?\xb1r\x0e\x85\x7f\x02y!\xd0\x08\xbee\xe6Xa\x8e\xb2W\xd7\x0f\x13\x1cA\x90\xa7=k\xb8\xae\xad\x13\xd7\xe60a\xe5]4\xc5uMf\xe2\x16/\xbe\xb1\x0f\rr^.^\xfd2O8\xf0\x19\xb1#/\xc8\x13l$A5D\xf9\x9aQgUS8\xe3}\xdc\xfb\xb3c\xc7\xb1\xc3\x8dA1.;\xa9b\xfd&\xe9\xd4\xad\xd4R\xa2\x80\xafi\xed\xe7(\x15\xfa\x1a\xba@\xb8\xf9\xeao\xfb\xf9\xbd=\xd8.\xcf\x89\x88\x87*\xd6\x1dfz]\x08V\x07\x95vok\x15\xea\xbd\xa5\xed\x8b%\xbb\xfb\xb9\x1f\x08\x8b\x0f\xf6M_\x07V\xc4\x9d\xcdt\xbf\xab\xac\xfd\x10\x91WP\x8d\x15@\xa0\xf8\xc0\xa0}\xbe7\xabg\x95\x1586i$\xee\xcc\x9cu\xf5\xaeo\xf4\xcf\x83UZS\x8cq\xb2\x19\xcf\xc8S\xaf\xb6\xa7xb\x02\xbe"\xac\xf8\xdc\'\x99e\x90\xe6A<Ws|SI\xa9\xf8\x92\xe6\xe85\x9f \xae\t\xac\xf8\xeeS\xbb\x168<\rKyf\x93\xe2\xbb\xd5\xa6\xbbO\xd8l\x06I/kJ\xf3)\x99i\xa3a\xe3\xd3\xc7\xf9\x8b\xd1^\x14\xda,\xe24\xc5\xbf?\xb1\x0e\x0c\xf9\x056\x86[\r5\xe4\x99-S\x8a>*\x1em\xdd\xc0w\xd3w\x0b\x84_\t&^\xf3^/w6\xf0\xc17!3\xaf\xba\x0c\x0b?F{\xfd\xdf\xdf)\xff\xa8\xe4P\xe6g\x7f\xcfl0\xe0!\xbc1L*F\xe9,\xdd\x9e\xf3W\xfb^h\xcd\n.\x14u\xc9\xcb\xd9\x10\xf2\xc4y\xb1\xea\x9e\xd6\x9a\x03\x89\x1d\xa0{\x1a~42\xd0y\x0bQT\xa9\xe7\xe4w9\x9d\xb3\xa9\xbd\xdb#3h\xaf\xef\xdd\xa1\xc8\x87k\xb2\xe0v\xb3\x07\xec\xb3\xe4\xa4\x83\x90\xf9z\xcf\x03\xe1\x06\xd60\x94\x86\xa8\xfaH\xcc1YT\x9a\xa8\x02\x8a\x03h&\xff\'\xc3D\xb8j\xd9\x81\n\'\x90\xe6%n\xa9\x0e\xc0\xd9\x93C\xd9\r_T\xfa\xf4\xc9_\xa2/\xce\x99\xad>\x9d\x0cb\xb85.\xf6\xf7Y\xa9\x01\xb5\xefU\x80\xda\xb2\x1d\x07\x01\xe6v\xc66\x0fH\x0c\xf7\xf7\x97\xb0x\x0e\xd1w\xd5\xd2\xe9\x8e\x95Q\x19z\xe0C\x7f\xa9\x98\xcf\xeb\x1a\x91\x94\x87h\xd2\x08g3\xb6t\xf9vj\xa6`\x8f\xbb\x03\xe7\x16\x87,T+\xae\xa0\x199jy\x06F~\x80\x05OK\xc6\xce\x1e*\xeb\x94%n$[\x8a,\xcb\x07\xe7\xbd\xa8\xf2\x80\x92\x812\xc7A\xdbf\xd4Dm\xe4Y\xe9"\x12\xc1\x95M\xd6\xabo\xc0\xdd\xa2l\x91\xa1\xb7;\xc3w\x95l\xae\'>\xb3\xd50\xd5\xe3\xb0~\x1a\xe2[\x01\xfb\x9c\x90\xca\x07\xa4\xa1p\xbc_q\xb2\x7f\xabyxm\x8f\xa1\x15~g\xa6\xc5\xb7\xaa\xe6V\xb1n\x81s\xdd\xfaU\xfbr_t9\xe8a\xe4\xd9\x10;\xc2Gj\xcc\xcd\x0b\xe3\xbe\x0e\xd2\xdbX\xeb-,v\x05wal\xc6lI\x1a\x1fP\'X\xac\xfe\x9a\xed\x9ah\xa1\x00D\r\x80/\xa2\x9e\xb4\x1c=["`\x9c\x81B\x8bN\x0b\x157\xe6\x9f5\x9aJ\x0c\xa9\xbd\x9a$\xf7\x1d\xb4x\xbb)\x8f9r\x01\xe45a\x0b\x01\x90\xdcI\x02n\x82\xe3-\xe7\xb7\x97Hh\x8a(\xa1\x92\x11\xf8d\x9f\x80\x96\x1e\x0c\xa5gRKG\xeeEk?\x11\xc4\xd6HG\xa2\xba\xf3\xa0\xbdA\xd2\x18cZ=\xcd\x01\xa3\n\x12\xc6\'"\xbb\xb6+\xb8\x9d\xaf>\x8d\xe8)\xcf2zM]\xc1\xd6\xee=\ti\x84+*D:\x0e\x1d\xc5Fg\xd3^0\xbf\x1b+N\xe9\x97d\x99\x0f\xad\x0e\xab\x0b\xfc\x1e\xb5\xb0\xea\x93\x04\\\xf4\xc6\x15\xfd\x16\x87\xfd\xe3\x05H\xb6%\x9b"\x05\xcbK\xb7Q\xff\xad\xb0\x11\x07;\xd4\xa3?NZ\xa7\xc7[\x9a\x93a\x1bd\xb2Z\xd4=\xce\xd1\x8e\xe9\x82+_\x06\xb8:\xe1:\xc3\xf5\xe7dmj(-\xe3\xf6\x15=)\xd9\xe4\xfd?\xf3\x0e&\xb8\xbe\xb6$\n]\x1f\xdf\x83\x96\xbf\xe4\xdf\x8e$d6\xd3R\xcbJ\x98\x85/\xbdX\xe1\x85&n6_\n\xc8\x00\xca|70\xd1\x94\x02XK\x89\xebg=QS\xde\xef\x8e4)\x04\x0c$PM\xe1l(\\I\x84\x91\xe7K\xb8\xca\xe5\xa4\xcb\xd6\x07\x87w\xd8m!\x9cG\x94ym\xed\xb8i%\xeecD(1\x95\xe6B\xa2K\xa8Tv\xa9?os\x03\xe5_\x92r\x82\xc4\x1f,\x19\xd1\xbd\x9eVI\xa2\xa9\xf8\x1f\xfb\xcd\xa4\x89%\xa7\xe9\xe8\x95\x8f\xd3g\xebCD\xe6;\x0f\x9a\xdd\x03\xb8jd#\xbcwKm\xae\xbf\xbb3h\xa9\xcfw=cYa\xde\xc8EN\xe3\xb9 W0\ny\xf6\xf4}\xc0,\xfeq\xc9\x9f\xb9e\xaa\x01\x87=*\x9bu\x03\xa1\x8e \xdc\xa8p{Y-,N\'\x1d\xd8\x11\xe9W\xfa\xc4\xcd\xe9UD\xda\xb8\x00\xf1t\xbc\x9bOm\xf7\xce\x1a\xbc \xc9\xd7\x81H\x8aE\xaa\x972\xcd:\x19\xb8G\xb7\x10am(\xfd=\xdf?uHN&k\xa3\x81\xed\x15\x16>\xbb\x88>\x89\x9dQ\x1a\xa5i\x19\xe4\x05\xfd\x074=\xccd\x0b\xf2_\xf7[\xd1\x9a\xd7\x16\xeb\x1c6:\xc1Gfc\xd7\xe8\x82r\x9b?2\xb8VU>k\xc7\x04\xa5d\xad[\x95\x1a\xeb\x00m\xbf\x15=@\x85D\x8b\xe0\xb7\xe8\xc8 \x0f\x0c\xb1\xed\xd7\xf4o\x14\x8b\x8a\x8fl\xc8e\x82\xe4\xa0p\xee\x07O \xf9Sd#\x03Vu\x0b\x8a+\xda\x95.\x1f\\\x97\x90\xc5J\x97\xe7\xea\xe6,T>_-4\xce\xf3 z\xd71\x1e#\xebsx~\xf4{v\xe5:\x8bo\xcd\xa8\x02\xd9\x1c\x94\x01\x9fo8\xa4\x03bMbx)\xbeB?\xb6\xf3U\x13\x9c\x8f\xdf\xf8\xd7AX69\xc7-wB\x88\xe6\xe58v\xd2M\xcc\x96dC\xa5\xdd>\xfdd\xaa#\xc1\xe7L\xb6\xe2,\xe2q\xeb\xb3\xc8\x96nF\xc8\xa8\xc5\x06\xb2\x1d\xb9\xf5y\x1f\xe2Y\x1b\x02\xa3n\x9e<\x91\x957\x17\x9e\x0b\x10\x1cOG&\xcbRa\xc7\xe8\xd6\xa1G\xcc\x10\xa2\xd6\xa5\xd1\xb5\xe6\xfa\x14\x05\x17\xe3\x19\xd0\xb9\xcag6\xb4\x19\n\x82 \xeaqE\xe1j\x822\xfb\x1f\xa7}[[d\xf7.\xc7\x1d#\tY\xb7\x13\x08\xf41\xa9f\x05\xa5\xcaB\x03\xb7wLha\x1c\x98\x87\xdc0F\xb3v\xe5C`\xe8\xb8\x08\xea\x83\x85_\xe6\x97\xc8b,\xb4c\xcd\xe9W\xd24iZ\xe5\xfdh\xe3w5Le\xd9\xb6f")8\x08\xba\x91\x96\x14\xc7\xb5\x08\x1f[D/ \xd7\xf0En\xcc\xc0m\x91y0"\x1f+\xcc%\xddZ}\xdeA\x97\xd3^&\x97u|T\xa7R\x8f\xacn\xac\x93\xab\xd8\xaco\x1e\xe9|\x93P\xd0\xc6\xdd:\x96\x8d(\x0b$\xa6\x8a\x92\x9cd-;\x0fd~\x12\x86\xe3\xf0\x17\xb7\x87\xa9\xaa)\xf3\xaf\x1a\xbf\xbb\x16\x15\xceCt\x04>c\xb9\xa7k$BL\x122\xe2I\x82@\xf9\xeb\x11l&\xdd\x04\xc3?\x8eP\xfc\x0f5\xfd\x8e)\xd1D\xef\r?K\xa5\xf3\x00.{f\x00\xb3\x15{\x87\xa1\xbb\xbc\xfb\x13\xc1,{\xb7\x0b\xf8\x88\xfe\xe8\xff\x1cg\x96VUD\x07\xe1"p\x9bi12\xb95\xac\xdd\xb7\xf6\x11\xc2\xda?n\xf9\xf42P\x0ew\xa5\xb0g\xab\xe8\x08\x01\xbe\xe6a6\xb9\xe2=\xd9\xf9\xc8\x9e\x0f\x0f\xb6B\xfe\xbetFS\xcc\r\xc9Ut\xf54\xc3,\xa4YT\xba\xb8o\\7\xe9\xee\xb9\xab=}@\x95\xc6Q\xf1\xc2\xb8\x96\x17\xba\xd6\xbb\xf8\xc0\xcbb\xec\x082\x1b\x07\xde \xcb\xb9\xc0\xa8\x1b\xe5*n\xa3y0\x890q\xa8\xd1\x9b\x15\xa4\x07\xfd\xdc\x9ein\xa6\t\xab\x9d\xa2f\xf0Q\xa1\xce\xa7\xebk\xf2\xc2\n\xd4\xe0\x9b7\x93\x94j\xf7\xe2\xf2\xf34,\xb20\x86{\x8d\xbb%\x99\x1f\x85Q\xd2]\xfb\x97|F~\xde\xab\xff\xc2x\xe0T\x9f\x8c\xd8J!B8\x8f\xaf\xee$\xb9\x08\xdeop\x1aC\x8eLg\x9e\xfco\xd0Ey\xf3\xcd\x19Q$\xa2\x82\xf0\xe2\xfa\xd9x\x7fX\xeb\x1ac\xaf:\xe8\xcd\x9eA\\\xc0\x97r`\x1b9M\x9d\x83\xe2]\x00YM\xd0\x10\xa3B(u\xe8[bW\xa7%\x08\xa5\x88\xb3\xa5Y\x9f}\x19\xdd\x94X\xce0\xd4\xaea\x0c\'\xdf\\\x05\xb6$T\x93\xf4\xc0\'\x03R\x99\x0b\x83\xe5\xb5\xe8\xcd\x1a\x07S1S \x0f\xcaI\xd1\xac\xa3@\xf6H\xd0\xcd\xf0\xd3#_avh\xc5\x13\xe0;\xb0\xf5\x08\x8b\x87\xf73\xdcY\t\xca\x1f\x02\xf9k\x166x\xffB\xd2\x8c\xdf\x9a\xc2I\x92\xf4c/cBA\xa4`\xbfG\xaa\x94i\nK\xbcA\xc8\x9c5[\xa5\xebF\xb9t\x8a\xb3\xe87\xd8\xc4\x81&\xdb\xd0T\xe8\xe5\x86\x15g\xcfN#\x9b\x9f\x1f\x92\x12\x84\x89\xd4\xf1[Y\xce\xf5\xae\xed\x00\xed\xd0\x0e\xdb\xa57\xaf\xb7\xa1\xefs4\xdc\x08\xe8\x84~a\xd5$\xb4\xd5\x8bM;\xc7r*\xd7\x8ck\xf5ZL\r-\xc8)\x12T\xa5\xf8E\x17\xfb\xd3a\xe2/\x17\xe1E\x8d\x0b;<\xa7\x0b\xc3u\xf6\xcb\xfd\xc9|\xe4\xe5\xb0\x13zH\x83\x04\xdb5\x1a"]\xfc>\x1cB7\xbb\xe0\xf0\x84\x13\xe9\xf1\xda\x8d\\\xd4\n\xd2B7\x15.\x17\xb0\xe61\xa19\xb4\xbf\x8c\x86v\xcc\x84\xe8\xee\xaf9\xbc\xc7 8\xba\x81\xab\x7f\x87\xef\x01y3\xb2K\x97\x95\t\n\xcd\x8efa\xdfTt\xc0G\xbd)\xe3\xe9}\xec\xbf]\xb6LD\xb2\x19\xb2\xeeJ\xfc\xa8[\x04\xe66\x08\'\xfdH\xf0A[\x13\x92\xad\xb2\x91\xe0\xfb\xdb\xef\xdf\xa6<S\xe9M\x87\xea\xa5:\x8ej!K#\xe9\xebA\x02W\x89}+\xcbN\xffQdJ\x0b{s& H"\xc7\xac\x87Ug\xf6\xfbO\xb4\xe46\x14Q\xbd\xb3\xbe,!\xecf\x95\x98B/\x0c!~\xcb}Q\xbc\xc7\xba`@\x93\xb6-&3IX=\x9eR\x93\x12\xabrg\xaaU\ra\xcf\xf0\xc8\xfa\xc0tih\xa2\xcd-\x9d\x96\x9ct\xcc\x84\xbcBN\xe0Z\x9abejHCi\xddv\x9d\x8dz\xbb\xf0\x14hPH\n\xd7\xa8\xb2\xbe\x146a\xfbl\xaa\x96\x06Eg\x82\x1f\xce\xbcl\xe2+]a~\x9b?rB.\x08\xd1D\xb6\xfb\xcf,:\xfai\xb5B\x9c\xf9\x86\xb4\xa9x\x92o\x97\xd8\xde#\xbe\xde\x07\xca\xc0\xd3\x0b\xfdl\x9aa\xe9\x1a\xe9e\x1c\xafn\xe0\xe1\x1a\xff\xce\x8cY\xf9*\x08\xb4\xb4\x97\xd6!W\x92\xfc\xd9K\xf3-%o\rP\x8a\xec#\xdf\x9d\xb3\xd6\xf9\x9f~\x8f\x16\t\xf5\x96 i\xb9\x92g\xd0\xa6\xe2e\xc0\xba\xeb\x10\xf4\t;\xbe\xe5H\xdf\x02\xd5T\xfdf\xe7b\xa0\xd65V*\xd9\xb7\x13\x1cQ\x9e\x9fh\xb9\x93\xd5\x7f\x98GC\x9c\x13?\xf6\x07k\x07\x9d\xfa<\xb3h\x91\x81b\x90]\xcf-\xdaR6\x91\xde)}g\x16\xcaT\x06\xb2\xed\xdb\x93\x92\x0e|\xb7\x9c\xf5\xab\x15\x19s\x9f\xdc\xd7L=\xe1\xa2\xb9\xb7)9\xb4\xe7\xe2MRwb\xfeo]6\r\xae\x11>\x19\xfc\x12\xc6K\x07\xd7\xc8\xb1\xd0\xb6fzYx\'y\xbc\xed1\xd2\xd5m\x00\xf3?6[1j)(\x8c\xa8;\xe4\xaf\x01\xe7\x91\xd4g\x9c\x06\x81\x1e4\xb4\xc1\x87(C\xb4\xb1\xf3\xb3K#Y\xb3\x06Q\x1b2\xd2\x9f4z\xb7\x81\xc2\xa7\x93\xd9P\xcc*I\x89P\x1f\x95\xc7\x0fJb\xcf\xdaK\x1f.\x9cs\xd9\xc6\x0f\xeeH\xa4\x81?\x14\xab\xaa\x0e5\xc9\xa4jd\x06\x02mUE$O|\x17\xfeT\x8f\x0f\x99\xca\x1f\x85\x9b\xc9\x00\\\xd8\xbb\xf0\xbbm&\xce\xf9Oa}R\x1a\xf1\xc3\x9b\xc8\x1e\xea\xd3\xab\xb6O\xb3\x10\xabb\xbe&:\xf7\x81QwF\xf8\xd9\x165\xd3\xbc\x99\xe2hy7\xfb\x9b#\x18\x92\xd5\xa1\xb0Y\x13j\xe4*\xec\xd5\x11\x9dD\x97\xea\xd6h\x91\x1e\xdb|K\xce\xb3\xc0\xb3\xfek[\x9bs\x11\xb5\x0b\xccT\x11\xb4\xeb\x93\xa8Ju\xc0$\x0e\xd7[\x9b\xc0\x98x\xd0\x9a\xe5\xb2\x9c\x86k\x0f\xe4\xad\x98\xe3\x98\x9fm\xfd=@\x15P4}\x1e\x19Q\xe9\xe0\xc7\xaf\xdb\xdc\xac\x95pCa\xb5\xcd\n_\xcat\x1dQ p\xdc\xf8<\xfb\xbc\x97z+f\x80\x80a;\xae\xbb\xdf2\x8fa\x1b\xbaZ\x1e\xc5]\xcf\xd2\t\xe6L\xace\x08b\x8f\xc5wjA\xd5\x93\xecj\xb9\x19F;\x03+4\x8b\xca\xa8\xf7d\xf3\x8fX\x88XTaIW\xb9\xee\xf8\xdd\xc7\x0f\xf5\xbc\xec\x7f\xa8\xc7:>\xfa\x87\xeb\x029\x1f8\xc1\x02\xc1<\xd8r-f\xb3E\xb9\xbfIK-\xc2\xca\xc6\xb2*\xa4\x9e\x92a\xd1\xbb,\x12\xf7\x89q25s\xef\x06!\x96\x1e\x7f\\*\xee\x9a\xe4\xe1:\x11bK\xa6\xdd\xb8-\x12\xf4bK\xa7\x95S\x0e\xf1\xbfU\x92\x15|\x81ID\xf3\xb2\xd7d\xd4\xfa\xbb\xbe\xcd\x06#|\x863w,\xae5\x10\x0e\x8b\xa9i\x0fe\xcb.\x9c\xc6\xb4\xad\xa6\xb7\xbd\xcb\xc2\x02\xb1\x0f\'\x9d\xf7\xc6\x02\xd1\xa8E.j\x18\x81\xe0\xed2\xf4\x1fm6\xae\xc0\xff\x00\xad\xfa\xdf\xb3\x82\xa0\xd0>\x8f\xb1\x8a\xd4\x90\x14\x0c\xb7\xf7\xbe\t\xb8\xc2L\x99\x9d\x1c\x9fw\x05\xd4)\xbbV\xd6\x16\x07\x0c\xcc\xc2\xab9`\x8c\x9b(\x19\xe0\x9f\x18\x17\xce\xfe\xce\xdc\xe5\x9bpr\xaa\xba\xa5w8]4\xcc\xa6_\xee X\x84\x98q\xec\x1bN\x92\xecgX\xef\x98w\x97\xb1?\xb4\xe4\xaaN\x18kZ\xcf\x9b\x83)\xddc \xdb\xc2@2\x0b\x1b\xf2\xcdV\x06\r\x95e\xa9\xecu\x0c\'\xe46\x99\xf6\xe6m\xeb\xb1\xd2\xc9\xe6\xbam\\\xd5\t\xcf\x12\xa2\xd1\x9bI*\x95\xb6\xfe\xde\xf1\x83\x96\xa5D\xb1\xd0\x9fEv\xbd\xc2&\x14<\xc7L+\x8fUY\x80\xe3\x17M\xa7CN\xcfH\x11\xd0\x07x9d\x92B\x8b:\xea\x0f\xec^\xce\xb8{\xfaK\xa2W\xb6}\xa6[\xb9h4\x9d\x92\x90\xf8v8YxY\xcf\xa6\xf4\xdb\x06\r\x83i?y,\x0b\x96\x82$sn[<\x02e.\x1c\n\xb0\xc2\xaa\x17<b\xf9\x1d\xda\xa67\xf8\x07\xab\x18Pe\xd8\xe0\x06T\xf0:p\xdf\xf3\xae"\xfc\xbb\x0b|:2f\x86K\x81\xd3\xd3\x02wF\xc1\x8e\xd8h\xac\xb5\x9e>\x9bk\xca\xe6\x1b\xf2\xf5C7\x85J\xe9\xc2]m9A\xa5%\xfe\xe7\xd7E\xe1\xf5\xee\x82>fn\x8a$\x94Si+\x9b\x90k\x06E\xb0Tj\xce\xe3\x05\xc9\x06\x8d\xa9\xe4\x05\x87\xd28]\x1a\xa3\xc9H\xe6\x1di]Bz\xa2\xe4%x\x95E\x8c\x8fT\xe8\x02\x08\xaa\xca\x9e\x973\xe9\xf8\xa8Jc\'\xdcg\x86\x1d\xb7\xefB\xd6\x88L\x86\xf1\xdfT\x05j\\\x90A\x07u\x9bx\x13\xdc;\xd8\x08\x87+\xba\n\x98\xb5n?n\x00\xe7\xcb\xc7>D\r\xc9\xc8$\x9a\xa0\xdd\x90\x99\xda.%`Y\x00\x9b\xceF\xfa\xdb\x08\xa7\x87r# y\xa1\x12x\xfe\xee\x10\'6\xcd\xec\x9bu\xddw\xfa\x91\xfe[\x96\xde\x95\xfb+|Q\x1d\x9ffzU\xfak\x0c\x9d\x87\x9a>\xcc\xf4\xbfA\xb4pb\x0eK\xb7\xeb\xdc\'\x97q\x13\x83\xcb\xe7\x17Y\x00\xd4\xa4AAp\x86\x9d\x901v\xb0\xd7}\xc2\x1d\x18\xb9\x8e\xc6u\xb6\xb5\'\xdar\xf4\x8d\tz)\xb7n\xa6\x9a3\x10\xdd\x89s\xb6\\E\xa4<\xed\xc5(6\xdc\x9a\xdd\x80V\xdb\xe0\xb9\xb1(\x87;d\xc4\xbd\xa7@,\n\xb5\xe4+\x0f\xc2\xc2\xeb\xd5\xe9\xc5\xce\xbc\xe2U\x0c:J\xb61sT?\xfc\x13\xc1g\xa7H;\xaa\xd8\xd2Yp\x93\xe7j<U\xad\xdb\xf9\xfe\xc3\xd3\x88\x04\x08:i\x93\xfc\xdfm:\x16\xb0\xeex\xa7X\xfcP\xb0\xad\xb8\x02\xbb\xe8\xf9n8\x18\xb4\xe6~\xc6\x98\xd0A\x15s\x9c\xff\xc4\x82\xb8]K~\xcdh/Z\r5v\xd2\x91@p\x15\x97\x01\xcf\xe1`\xd9\xb5\x88aq\x84\xa2\xcc3\x8a\x98\xdb\x1a\r~]%c;"\x0c\xb6R?Y\xc1\x1a\x87)\xe3\x83\xcd\xc3\xb3.\x14\x93\x97=\'!\xae7cg4\x9b\x89[\xc4\xd4\x8c`K/J\xfe\x87\xd1\xd5[!Mj\xe6\x98\xe0~\xe8\xb9AH\xeb\x08\xe5\x13\'O\xd9Bt\xabs\xb6(\xe0\x0bx\x8b7\xc7\xb4Y:W\xa4\x06B{\xd3r\xbb\xaeh\x80#\x07\xc2\xc2\x13w\xba\xca\x97J\xf4$\x9e\xdfc\x89\x1f\x02\x83\xdf\x7f`\xd3\x94i\x00\xb4z\xd9\xfe\x9f\xc7\xbf\xb6\x9e(\x1a\xcc\x9e\x0c\xe6\xdf\xe5Q\xbe\xa6\xd1o\xd7\xc5L\xe7v+\xd5\xab\x84\xe3\xf3G\xdf\xae\xc4\xae\xc5>\xaa\xb7Ma\xc0\x8eHC\x88\x99%\xcd\xf0\xc8\xfa2\x02\xad\xdcx\xc84\xd5\xee4\xfc\x82\ty\xa1\xd6\x16\xfe\xe5[\x1e\x8c"\xc7\x08\xdf\x18\xdf\xd4@\xba\xf9^v\x17\x14\x8f\xc0\xb5\xde1\xae\xb2i3\xb9\x9ck\x08\xc1\xc9\xae\xa9\xb7\x06\xb0\xd0\xfc\t\x8f\xec\xae\xc0!,$\x1e\x05\xc1CE$\xbf9\xab\x10H\xa5\xc3#j\x86&\x9euf\\\\\xd3\xc2\x90T\xc4\xc9\xb9 V\x03T\x876%\xfdT>\x9cqy\xa5\x8b}\x0e=\x83\x1cY)*\xe7!Fv\x87\xa8\xc7,\xe5\x93\xc8:\x96\xef\xd5\xe6\xe2\xf8\x0c\\rh\xcf\x9c\'\xb6\x810\xa7\xd0u\x0b\x0c\xb2j2\x1a\x16\x1e\x0b!\xfa\xa9\t\x0eeh\x88\x97\xdd\xc8\xcb\x19\xb9\x89\xc5\xd7_\x94\xad\x8f\x9e\xe9f\x03\xc74\x87J4\xac\x94\x1d\x9cr\xf7\x1a\x9c\xa3V0\x93_\xffR\\\x18c\xac\xb9 \xecB\xfa!27h\x0f\x9f\xc2G5v\xd9\xca\xa2\xaf\xc6R\xc5\xc1\xf5\x85\xf88\x9cD;"\x0c\xe1\x15*\x8fH\xa7\xb0\x9e<\x9e\xda\xf5V\x80\xd7U\x04\r)\xcf\xb2\x84E?"\xab]\x06>o\x87\x15\xa0D\xae\x1d\xe8\xa0\x15\xf5\xcc\xd3\x8b\x93\xcb,W\x83^c\xa3\x82q6\x8d,\xb6K\x1d\x92\x18]ng\xa4\\\x14y\xdb\xb8\xd3\x858\xc4\xc0\xf4\r\xbb\xb5*\x1c\x18p\xd6\x15\x86\r!\xf7\xb3\x107\x90]\xc9\xd1;[_\xd7\x81\xe55\x802\x8856}\x04\xaf\x86]\xed\x88\xe6\xfck\x8eS\x91H\x91\x10C\xddp\x84\x11F\xe0A.\xc0L\xae6\x93\xe1|fM_\xe4\x97\x00\xb2\x92\x7fNv\x87\xc5\xb8\xe3\xfcs&2Lecp\xcd_yW\x8ddE\xd9\x0f\xfb\xdb=\xab\xaa\t\x8d\xdd\x10R\x9b\xfe\x1b\x89\x88Yx\xe2z8\x9f\xd5X\x06/\xa6\x95qU\x8b\x9eC\xb0\x81\xf4\xe8/\xd7\x02\x9d\x89=\x9d.\xbc\x8b\xaa\xc40A\xdd\xe0Je\xce\xf4\x9b|\tp\xf6h\xa8\xe3\x9e\x08O^\x1d\xbb\x8c\xb40O\x1e\xab\xa0&\x84?\xb6^oz\x838\x87\x816\xbb\xf9{\x1e\xae\x13\xbc\x86[D]\xa1\xbe2\xe3\x19\xeflkn\x8c&q\xe5\x95\xa7\xf8\x82\xfb\x91\xc9k\x86%\x8a8V\x03\x99\x0e\x96\xd9Ki\x1f\x92?\x9d1\xc5\x1e\x1e\xf9\x9bA\x94\x86\r\xa1\xa1v\xc7\x12\x16\x84\x13.\x98\xc5\xa0\n\x9f\xedA\xc1\n\xbdls\xf9\xb7\xa1\x07\xda*u\xc5Hj\xa5\x8a_\x17TV7>q\xbe\xce\xc1:\\\xf9\xa5O\xd6\xec\x13m\x10l\xad\xcfA|;\x95\nI\x97J!\x19\xb7\x93&][\xb2dQi-}\xacL\x9a\xca\xe9\xde%\xc5m\x8eZ\xff\xe2j\xe8\x83n\n\xa6^\xec\xb6\xa7\xb8\x9f\x80\xaa\x89\xa9~\xb3\xd2~\x93QH8\xcb^\x0c\xd5\xfaj\xb2\xdeby\x80\xfb;\x19#t\x0c\xb4\xb5\x8b\xff\xdb\xbdL6?\xb0\xf4\xb5&\x1ac\x95\xfc+\x94I\xd2\xde\xa9\x84\x9b\xd3\n]\x8c\xc8\xac\xc4\n\x8d\xcd[\x02\xbc`\x17|\xab\xf5\xa29\n\xd9\x99\x19\x93\xe2l\x83\xa7\xfb\xa4\xa0\x03\x16\x0b&V\x7f\xb9\xb9\xf8w\xe3\xd2 "z?\xd4\x0f|7\xddb\xa6\x03-\x96\xc3\x19].\x9fm\x9f\xa7M\xbc\xfbuo\x8b\x86\x1e\x93\xd84\x93\x15\x05u\xaa\xd1Q\xb8\xfa\x9d\xfa\xa2\x86\xcc\x88\x04N\xb8a\x1f\xd1\x88E\xd7\xb1\x00\xc0\x99\xa9\xd0\x07\xddaY@:\xd84\\\xb7\xa8\xbb\x0c\xe9r\xf1HRY-]\x1c\x9f\xd0\xdf\xa2\x81\x80\xa8\x92\x83\x08\xb2\xea\xf8rT\x05\xbe\xf6\x99\xa1\xfd(\xa6\xb9\xc9{\x82\x80|o\x8f\x05aU\xc9/^\xa0\x9dmU\xae\xe8g`\x1e\xcd\x0b\xd1E\x91)\xc7\xf9\x0b2gvD\x9d\xb1\x13\xf5m\x0e}o,<\xf3z\x8d\xb4\x8c^u\x1cs\xb6-*\xc7\x80H\x17\xa9d\xbe>\x13Z\t\x11\xb2c\x93\xe8\x1bk\x838iE\x90\xefdN>\xfd\xdd\x94\xdb\x10\xbc\xe3\x93.j\xbb\xc11\r\x06b\x9a\xb8\xb8f\xc4\xce\xdd\xfd\xf2\x8fko \xe0\xa62\x16\x92[\xf1\xc7\x18\x1d\xb8\x11\xf2;\x17X\xf0\xd2\x8d\xd4m\x93c\x10a\x0cK\xe4\xaa\xa3n\xbd\xc5\xab\xb0\xcc\x9f\xcf.s\x8e\x9a\xce\x15\xed\x9f\xceSa\x92`<\xda\xce%6x\xb7\xf5\x02\xc2I\xbbNo\xe1\xb2\x94\x1c\xd7b\r\x8a{\x9b\xef\x1a\x9a\x81*\x91\x06u2\xf1\x16\xf3L\x9a\xecV\xa0\xdd\x9a_\xe8:\xa7\xf0@\r\xe8\x91\xe933\x08\xc58F/\x04O\xe5\xddl\x18\xf8\xbd\x7f\xb2\x08_\x0e\x94\xfci!A7Y\xcb9\x8f\x06\xb5\xbd\xa2\\\xa1\xbc\xbb\xd9\xea\x15\x988\xcb\xfa0L\xf3+|\xb3d\xd6\xa2\xb5\x91\xbd\xda\x1cHX\xf3\xce\xb4\xba/\xa0\xa2|C<\x10\xa8\x93\xbb\x17\xa5\xd2\x02\xeb%\x1b\xd4\\\xba\x91\nfa\x13)3\xa5I`F\x99\xf7\x19)\xdf\x1c\x81\x89m\xabF\x05\xe7\xaa\xf9\x12W\xbclb\x0c\xac\xab\x11\xfe\xa1\\\x06\t\xbf\x81\xa0\x19\x8d\x0bl0\xf2o\xdb=\xd0\ra\x16w\x8dV.\x1dPt8\xcf\xd5&\x1d\x9f\x04\xc4W\x01g\x0c\x13h?T\x8b\xb8v\xd7\xd6\xb73\xb6\x05\xec\x83sK\xe2\x05r\x85(\xd3v,\xd3\xc5\xd67m\xccD\xcc\xbc\xad\x96*>\xc7\xd5\xc7/M\xae\xaf\xa4\xf4b\xaa\xf5]U\xcb(\xea\x9c\xa1\x0b\xc1\x82m0\xedG\xeaH\xb1\x1f}3\xcb\xa7\xbd\xc6g-\x03gH>8\x90\xdb\x8e\xa3|\x7f\n\xf4\xc5\xb92\xb9\x12/\x19\x03\x01\xc9#\x96\xd2o#\x8a/\x94\xbcw\xae\x0c\xddKD\x87\xd8\xa6H\xb9YDi\x06\xdc\xbe\xfe\x03\xfb\xc8\xfabO\x9f\x869\x10!\xe1\x9cV\x99\t\x1fp\xa3\x1b\xa7N\xe6\x07r\x15\xc9\x94.\xbf\x88 l3\xf9\xe1\xa9"ThF4Q\x115\x19\x0cP\x82jmag\xe6\x10mZ\xcf=\xa5&\x9a\xb7h\xad\x7fN\x17[@\nL\xaf\x83*[&\xd67\xb2\x7f\x14sw\x05\xf6\xf4\x8f\xec6\xa7WW\x07\x85\xe1,\xd1\xf9\xb9\x80\xede\xf9\xef`\x9f\xe2\xa9\x1a\xd2\x90\x05s\xd3\xc2u)\xc2\xd1e\xef+L\r_\x95\xf3>l\xbfX"U\xf2\x8e\x91\xe4i\xc9 C\x8e\x83X/\xca\xc0\xd6C\xec\xf2`\xd8y\xd6\x93/t|\x8fd\'\xfb\xcdi\xa6\xe9\x88\xbe\xce\xd9{\x05\x06\x92\xe6?al\xcf$SV_\x881\xdbo\x8f\x97\x17\x02\xa8\xe7\xc2\xbf\xb9\x83e\n\xea\xb5\xa0\xbfg-5]\xd2{\xa9\xd7r\xe6P\xe7\xe42\x86\x13\xbe\xac\xcc#\xbd\xc3\x92x+\xe2\xf8\xb2\xdf\xb5YCl\xd0b\xea\xab\xdfwY\xa8\xd2\xad\x91\xb1;\xe5\x15\t\xc5\x04\x91N\x8a\n\xe9h\xa7GFw\x91\xbci\xba`Z\x88E\x91\x83\xe0Z\xd5\x8f\x0e\xc6-\x81\xd6\x82eq@\xa9]\xa0\xbcy\x87\x89\x07\xc7\x92\x81;\x998\x82g%\x07\xc4\xa7\xc61\x82\x8bLgd\x0b28vf\xee\xb0r\xce\x1d\xc3\xa9QBt\x17i+\xf6r\xe0xB\xa7\xd1\x19\x07\xaa\x00\x87\xd8\xd3D\xe2\xcfv\xc1\xed\x07`\xf2\xf1\xdb\xbd\t\xa3\x9c\xf8N\x10}\x96\xcc\xc2\xcd\x1a\xc1\xfak\xd6\x17\x1c\xb0\xa2\x96\x08#\xb8\xaf\x1c\x17\xd2\xba\x10?\xf2\xaa\xe6&\x1cf:\x14k\xecf\xf0\x94\xf6\x93\x02\xdd\xda,c\x9f4\xce\xef\xfe4\xbb*\xce\x0b\x1b\x95\xef\x05\x15\xab\xb0\x9d\x8bKJz\x074\xd0I\xc6\xfbKE]\xcdB\x85\x96\xba\xc4\x08\x1bf\xace\xe6ot\x19\n\xc5\x1e\xce-\x06\xfa\xe7\xbd\x9b\xaa}\x8c\x1a4J1Od\x8a\xae\xeb\xe15\x97LhRC\xea\x97\xef\xc3\xc0O\x9eRs\x90nG \x18\xb6w\x1b"\xd5\xf8\x84\x12b\x94\x89\xacMNs\x89\xa7\xae+\x88\xb3Z#\xaa\xf7\xdd\xcbo8\xe4d\xdd\xae\xbf\xb6\xfdOf\x1c\x06\x9d]\xc1{?\xbd\xec\xba9\xdb\x19\xf1\x07_y\x85\xfd\x82\x1d\xe0y\xeaW\xebt\xad\x9c\xa8\xa5.\xc8\x81\xc2j\xc0\x8a\xf4\xff\x87\xc7T\xf5/\xf7<z\xcc6$\xcb\xd2\x17\r\xeb\xcd\xc4\x88M\xec4U.\x96X\xaf\xbdh\xd5\rd1\x9e\x18\x89\xe6\x94\xb0SC\xbd\xfd\xa6[p\xc0\x18\xcb\x14P|\xdc\xb8\xc7\xb8XT\x90\x0c\xca\xd8\xc8z\xef\xc9\xdceI\xddo\xa0\x0f\xa3\xc2\xb8\x99!\r\xc8\xec\x14\xda\x8d\xd2\t\xd6\x8a\xa5\xeaC\xdbNc\xa0\x92<\x8a\x98\r\x8eQpa\xcc\xc2{q|V\x14\x03\x9f\xa1\x9e\x0b\xd4\xdacJ\xb4\x07EM<\x8a\x1eS\xe9\xa0k\xf7\xad\x84\xc7\xe3Q\x8b\xda!\xf3\x94\xc5:\xcf\xb0\xa6w\x7f\xf4\xa2(\x9a\xad\x8c\xb5\x8f\xe1\x05u\xd8\xe4e\xa8[\xd2\xbb\xba\xb0\x81\xbd=\xf1\xb9\xd1\x95\xe6_\xbbp\xc6F\x93\xa6,\xa0\xfb\xde\xda~9\xac;!\xaf`\x8cI\xc5\x1bH\xc1\x1a\x82\xaa\xdaS\xd41n\xc4\xae9c\x8b\xc2\xf4\xb2\x8f\x85o\xb1\xf4\xa3\x9a\xbd*\r\xedH5mQ\xa8_C\x19\xbd\xebv\x82]H\xc3\xe2\x8e\xbb\xdf\x97\x82\x8a\x04I\xf1\xe9\xba)\x9fds_\xba\x8a\xbf\xe2n\x8f\x11\xe2=K\x0f\'\xfe\xdf\xcc}\xef{\xff\x85#\xfc+H\xed\x9b\x9cOTC\x00v\xa9H\xf9\x046\x08"\xb6 \'\xecC.\xde)\xa3\xcb\xe3\xe3\x08S\xe3\xeb\xc8\x0f\xb6?\x8du\xf0d\xa6\xce\x8f\xae\xea\x8e\x8dr\xfd\xa2L\xb2\xd0\x03\xd9\x19\x90\xed\xe6\x84\x180\xcf\xea\x84:n\x7f\x89T\xbc\xda(\x05\xe0\xc8\xc0\xb3\x9dr\xb7c\x12\x13\xeb\x80.\x9bg\x00X\x01\x1e\xd0y/\x92t\xcd\xaf\x8aR\xa9\x1e`\x9bRB\xdb~">pH\x90<\x1dqZ\xcc\x8aUA\x1cf<k\x15\x1d#\xb7E\\\n\x14\xcan\x15J\x0f\x13~\x9a\x10<-\xba\xea\xc0v(>\xd1\x851L\xf1C\xc3\xbb\xeb\x03\xd8z\xf0\xf4l\xc8\x9d\x96w\xc9\x8d\xb6\xdb\x91\xf7\xf0\xa7\xa877\xfd4\xffi\x14a\xf0\xb2\xb4)0\xce\xab\x1f4\x9bj\x94G\x8b\x02s\xd3\x1a\x8d\xeb\xf6\xab\xdcQ\x9a\xd3uS\xd7s/\xe4n\x1d\xe6\x14\xc7\x13C\x1c+ \x99_\xebI\xda-l\x12f\xea\xd3y\x04;\xd0R\x03\x9f\xce\x16 \xd7%f\xd9\xb9\xb3\x83!\x19\xca30\x9e|w|c\x9a\xc6\x0e\xd1\xce\x99\x84S\xcd=\xbc/\x08"\xaf\x934\xdeg\xffg\x01\xc0@Z]\x1cW\x8d\xa6\xdfv\x02S\x82|/\x1f\x07\xb6\xe3\x1f\xaa\xd5\xdd\xcf\xec\'\xb6k\xf7f\xa2\xa0\xbc\x11]\x01\xcbJW\xcb\x0f\x8a\xad\n\x953*\xd8m\x10\xf1\xe9\xd3?\xa6\xe3\x8bg\x88\xe4\xdd\xf7\x18\x0b\xfaa\xa7E5\\\x9fZ\xc4\x81#\xa6\x881_4\xf6\xf2\x85\xe5\x10\xa3[}\x99s\x01\xdc\x13\x8b\x067\x9d\xd5\xe4\xf94\xf2N5M\xd2=\xa8\x9ch\xdc\xfa\x0e\xcf\x84\xaau\x9b2\xd0\xbf\xe5|\x94\xa4\x82CB\xa9z\xc7\xc0HW\xd3\th:\xdb\x94\xbc3\xc8,\xb53\xbf\xf7s\x8b\xf2\x98?j"\'\xe9H\xd5\x0eTb\x82\x11\xcd\x8dc^\x98t\x0c\xdd\xa6\x00\x88/C\xcd\xc7)\xdc\xcb\xe1\x97\x8c5c\x8f\xddT&/\\\x120\xde@\xadk\xc4\x9b\x94B\x10\xa7\t\xfb*Wz\x86 \x91\x1f\xadm\x01D\xd2\xaf\x0cr4\xda\xe2\x7fn\xd2\xbf<\xc0\xb1 >\xb0E\xe0"\xa3&+7\x9fE34\xd1$\xa2\xbfM\xadI\xd6\x88\xc8\xca\xad\x87r%e\xc6\xaf\x14\xe2.\x1a\xe6\x02A\x85\xbd\xec\xfbn\t\x01\x9eQx\'\x8e\xf2\x07\xdfk\\\x81\xa5\xfd\x80\x05e/Y=.yF\xfc~\x00\x9f\xde\xb3\xe7\xcd\x94\xe2/\xfc\xa8\xc69;i\xd0S\x85<\xfd\xa0\xf2\xf8\x8a\x0c\x9f\xcc\xa4\x8f\xd3+\xff\xa8\x1d\x9f\xf8\x03\xca\xc4\xc0\xe6\xd0\t\xc1\xca\xbdA\xb41Gx\xc4\xbeX\x9c\xc2i^4$\xa7O\xee/\xa8~f\x88t\x08\x04\xe0%\x9a4o/>\x82\xb2\x12\xe5\xde\x0e-O\xac\xbf\xb1\xf76&\xfas\xd1\x95`\xa5:o\xf45\x84U\xe13\x03p\xc1@Y\xee\xf7^\xe0\x1e\xc7&\xbd\x01\x8c\xfd\x85\x86\xe1sbF#\xad\xf7\xc8\xffF\x9dX#\xb4\xa3\x80\xf6\xc8)*\x7f\xc2\x16B0o\x1d\x17\x0c\x91L\x0b\xb5\xd0\xb9\xd9\xee\xd50\x14\xa4\xbd\xc8M8A+\xcdKcc\xb0\x1d\xce\xc9\xa0\x83p^ d\x954\xaf-\x90\xeb\xa0W@\x17!\xf3\xfc\xfc\xd8i\xb4\xdc9\xe0\xcb<\xa2\xf2\x9b\xad\x8e\xfa\xf7\x15\xd0\xb1R_\xe6\x1e\x96-\x86\xd9\xe9\x15\xc2\xe3t\x15\x85q\x19Y|\xad\xbf\xf3\xff\x0c\xb8Z\xef\x10\xab\x92*\x95?\x9dQ\xa5\xe4X\xddk\x08\x90\x9d\xa8Y\xb0q\xc3\x99Q\xaf\x1cJ\xc1\xe8\x92\x98\rB~P\x98,\xcc\xce|\xc3bk\xb2\xd3\x7fh\xa6\xa8\xbfn\x1eo\xb8\xe0\xbb\x0e\xff#^V\x8c\xdb\xc8Dh\xfb\xfc.\xcb\xad\x81\xfaMn\x8d\xf9T\x92\xe7\x9e\x12\xf6mmi\xa0\xddG\xdb\xa5\x8e\xf3\x88\x8dT\x1e\xeb\xa1\xc5\xc1{_[A\xbbpX\xb8\x84\xdeB\xf8L\xee\xe8@\xc5|9\xcf\xbf%Tc\x7fr\xdc\x1cO\xc9\xc5\x12\xc9\x90\x02G\xa5\xf3\xab\x9e\'\xce\x9e\xab\'\x9eiuS\xf3\xed3\xb9\xa2\x94\xfb\x0c\xd7\xbbs\xec=\xfb\xbf\xd8B\x8d\xed~!1\x9aA\xe6\xac\xe2\x1b\xf1\xcb1\xef\xd2\t2\xad\x05,l5\xd7\xf1\xf5.\x9b\xcd\xf7\x10\xad\xe4\x1bXP\xa8,6\xe8"c-\xd6yj\xaa\xbd\xae\xab1\xc6\xad\x02\xcc\xfe\xea\xbb|\x03\xea\xc1]\x83\x04&\x93rr\xca\xe7\xc3\x11w&M&\xb0Q\xdf\x1a\xb5L\x05\xc2\x05D\x9dg\xd4Kh\x03\xc2wF\xf7\x80\x00\xa9\x89V\x0c\xd8s\x8b\xc0\xf9y\xf3\x98\xd8\x15\xday`\xa0X\xd8%\x19\xfa"r\x85i\\\xe6\xec\x0f\xc5\x90\xff\xd0\xac`#\xe9\x0c&\x85`M\x98Z\x92\xf0\x88\xaf \xfd\x013\xca\xe5\xd5\xfdC\xc4 \xf1EU\x9ac\xee\xc1\xcf\xef9\xbac\xe5\xb6^\xae\xb1E\xe3\x9en\xe22\x07\xcd\x10\x83\x0b\xb1\xd3\xf2\xcd\x92;\xe5\xbe\xd1\x19\xc1\xff\x84N3\xdf\xba2\xe2\xfb@\x04\xfbM\x1a\x06\xcc\xff=h\xafBw^>m\xe9I2\xa0+eNg\xc0\xc8u\x05\xc3\xfe\xb1\x90n\x1d\x03\xdarWP\xd3\xc7J9\xc5G\xe7]\xdf\x99&\xa12\xd7>O\xc8\x9f\xff\x93;q\x91\xf0\x1da\xaf\x16\xc35\x84\xaa\xefIcF\x97\xd1u[\xa7v\xcf\xe0\x0f%\x88\x1c\xed#\x84\xe2 V\xcbbm\xff\xd6:/\x95\xdd\xdc\xe5&$e\xcb|y\xb1\xcc\xa04\x1d*\xfcT\xfe\x1e\xa4P\xaa\xad\xa01e\xef\xef5\xd3\xd3\xa6\xd7\xb4R/\x80\xea,\xe5\x7f\x19%Z\xc63\x889\xf4\xa2=\xd5\x11\xees\x19^\xe3\xcc\xc5\x18g\xde\xf5\xe2lp\xb5\x83\x08,\xfe\t1\x05\xe3\xb2Q%=\xc0\xb0F\xf7\xf1\x1d\xc3\xb9%D%\x06\xf0\xb7\xc9\x1c(u\x18\x05.!\xdb\xcaj^*[\xa4s.#o]\x82&^\xe7\r6\xcb<\xdd\x1cFN!n\x92k.t^@]\xe7"\xd3\xfc\xa7/\x80x~Q\x16h\x80\xa2\x8b3\xde\xfc\x88\xfa\xca\xbe>\xf3\xdf3\xd6\xa9a\x8a\xcd\'h\xaf.\xd7&R\x94\xf4u"8\x85\xb7b~l\x9e5\x1eHe\x9d\x7fp?*^R1\x08-\xca\x84k\xad\x84\xa4\x92@~\x9dZ\xf6\x10\xaa\xb3\x8f\x80\xb6\xae\xf4\xefv\x17\xfe^\t\x81zIqd\x80\r\xa4\xb5\xebX\x7f\x99\xff\xe7\x16\xd6\x97%8 \xb3\x94k{v\x92\xe1\xd3\xf8\xc9\x8d\xddSfz\xcew\xd2#\xa7\xd8\x17)"\xd0@?nB\xb5\xb6\x03\xba\x80\xf1\xf5c\xb2\x96\xf9\x84\xad[\x91f\x9fO\xa1g\x86\xfdp!\xf8\xe3\x0f\xdebZR-\xc1\x82\xd8\x0b\xc4b\x1c\xcf\xf8r\x1a\xe9\xe8l\\\x10\x89]\xf9\\ibw\xe8\xa8\xbb\xcc}\xa3\xb8\x88\'%\xa7\x8eWP\xde\xe5}\xc4>E\xc7\xc6\xd2=\x08\xb6TQ\xe9N\xbe=\xc7\xa4j\x1a\xdd\xbc\x92Yw\x92\x85\xf9\x17\xa3\xa5\xf9\x14\xcd_\xb8\xf2\xfd{F=\x8a\x07\xe6\x11\xd7\xf8YS*\x8f\x82\x99M\xb5\x1fU\xa2}\xea\xbb\xee{#\xf9\xa6\xad\x9e\x10_Q;.\x9f=\x17<8\xebl\x10\x8f\x1d\x10J;s~\x89\xc4;\xa1Q\xb7\xeeV\x14\x8b\xb4\xa9\xd5\x9eP\xff\x9c\xe3\xa2[:\\\xb5\xa5QdN\x08b+5\x99 \xa6#\xe4e\x07\x85\xa6Y\xed\x03\x97BO2\xfd\x92\xbe\xa9\xd0[\xc5\xee\xa1\x99l\x1d\xdd\x0f\xdf\x18\x818Z\xbeN\x86\xd9\xe0\x03\x1bI\x9e%}\x15w<\xfa\x8b\x06\xd6\xeaU\xb1\xef\xd61.\xcf-\x8e]T\x01\xa9\xf8\x8dP\xaf\x17\xc7)\xc8\xe8\x93\xe1\xc9.:\x05AU%\x9aYj\xa1O\xd4\x8c\xb1\xe7q\xf723\x07%\x1b\x9e:,XG\x10q\xa3\x84\xe0\x8d2xp$w\xb0\x9b`\xc4c\xc3\t\xd9\x81[H\xbc\xb6\xbe\xaf\xba\x1f\xf4\x99\x9d\x86\xb7\x02\xb8\xf5\xf2\x0ewo\x03\x8d&\xec\xd4\xab\xc0iK\xcf\xe9/\xe5z]\xd4\xfe\xf3\x0eY|\xe8P\xdc\xc8\x90\xdf\x1fP\xd0\\\xe5\x1e\xd8B \x89\xa0W\xba9\xec\x17\xfa~\xd9\x18\xb9\x02\xb0\x84V\xd1j\xaa\xec\xfc\x02\xf8O\xf3\rW\xef/\xe9\xc9a\xbf#\xbf\xcd\xaes\xa0\xe2\x0e7\xe5\x08\rC\xb9t\xf6\xa8\xcaT\xa7\xb1\xa5q\x02\xf4\x91\xdc\xee\xb58\xbf\xb4\xac\x96:\x9e\x98d\x98\xe6c?\x08\xebS2z\xc0\xb3Cr}Q\xda*aJ\xdd\x8e\x14\x8e`\xc6\xe9\xd0[\x8e\xdd\xa9\xd2\x92;\xac\xe9\xc7\xb0\xab\x1al\xf6\xa2\xda\xc4.\x1fd\xc1O\x94\x822h\xa0\x8d{\r`[\xec\xff+\xd8Y\xf4H\x01\xbe\xd8J\xd7\xe8\xf3\xe5\xb5xL\xf7\x1c[\x18a\xf3w\xf6\xf9\xa8W\xebBf\xe0\xe6\x95\xd0\xe3\xea\x8ffH\x16\xaax\x9d^\n\xdf\xe9\x05\x822\xfby\xb2\x1b\xfd\xee9\xa8\x17\xb8(\xa5\x05,\x93\x0f\xf1\x0f\xde\xbb\x08\xf0l\xe5R|\x02\xf0kh!X\x1f\x18\xd65\xadl\x06x\xd7\xdbJ\x04]\xa3\x8d>\x87q=\xea\xe8:D\xe1\x0c\xc5\xc4C\xdf\x19\xe9\xf0g\x98f\xb07\x02\xf9\xf5Q7M\x84f\x1e\xc6\xdc\xd4\xc0%g\x1a\xf6\x06\x8b\xbd86\x8d\xe7\xd2\x12I\xfcU\'\x17\xb6\xc37R{\xef\xf6HB\xa7\xdf\xc0=\x17\xf0\xbd\x12\xfb\x1e2\x11.%+Q5f\x94\xb5;\xe6\x94\xaaV>S#\x0fq\x15\xa9X\xe9\xe7\xe1\xe4\xa7\x94\xed\x19\x9bSOs?\xa9\xa5\xaf-JyW_\xb7`-\xc3O\x99\xd9\xfdxm\x1f\x95\xe8 \x92`\xf8\x8es\xe9\xd2\xea\xa1\x91\xe2\xd0!\xa2\x9a\x84\xf0]>\xb6\x81F\xf5\xe3-(\x18\xd2\xc4t\xf8\x16\xae\x99\xc8\x8d\xd0sz\xeb\'\x9d\xef\xd1\xb3*W\xa7iI\xce\\\x058\xaf\x80S\xa9\x93t\x9b\xc5\xe8\xe7t\x85\x0c?\xe1d\xff\xd0(\xab\xf2\xb9\xff%I\x9c\xb5\x93K\xbaI\xb4\xb7*\xf6\xd9\xe8\r.\xa2biK\x8c\xe9\x98\xd3Y\xce\xc2\xad\xb1"l\xcfS\x94\x10\xf6z1\xfacO\x9eM\xc5\x91S\xd9\xd1ZZ\x03V\x17\x99\x8c r3\xberc2y\x9c\xee\x00R\xe8L\x82,4Y\xa8\x9e\x18\x17\xdf\x15\xfe\xf2\xdd[\xfd\x0ff\x82-\x94\x8bI\x8a\xa6\t+\x8f\x16\x9d\xac\xb4\x87\xc0I\xa7\xf7kxz\x08\xfb\xbes\x9e\xf0\x8dA\x08\x1c\xc7\x8fro4!\x0f_\x15J&\x0f\x0f\'x\xa4\x1c\t\x80\xc0\xed\x0f/g\xeb\x0e\x1ba\xa2\xff\xdb\x96\xdb\x8d\x14K6F*#\x07\x9dB9agR\x11s}\xbe\xfc\xa5^8P\xc84\xac.f\xfc\xd2\nV\xf5A\x8a\xdd\xca\x04\xaaG}5\x81\x00\xa5od[h9=\x10\xe1,\xe22\x9d\x0cW\xe3\x84\xa6\xf1\x13&\x99\x1aD^\x8c"\xd0\xd1\x1e\xae\x91\xfdp\xc5\x0eM\xe1\x16\xf8\xect#\x06\xbcdGH\xc4*\x1cJ[8\xc1\xd1,"\xd3w\x82\x82\xcc\xab\xb9\x1d\xa6\x86\xb49\x19r\x94c\x8eU{U9\\\xaf\x92\xdf\x86\x8f\xf7\x95\xf8\x1d\x1cRv\x05\xf3\xc5%H\xfd\xd9J0AM\xb1\xb6\xe6\x1e\x87jG\x05\n\x82\x10T\x12p]B\xa5\x1f\xd5\xfd\x06\xa1\x82\xa8k\xbbr\xba\xfb\x93\'R\x078\xb5;\xcdm\xfa\xf0\x1f/\x1e\xbe\\\x9aM{\t\x85k\\\xac\x07\x13\x1d?\xb2\xb7\x9b+V\xeb\xe8\xfc\xa9\n|"}\x0c\x18Vd3b\x7f>\xd6\xa4\x90\xa6+\xc3E3"r\xeb.^\x88\xec\x11\xd1f\xb1\x15\xab\x87\x8dv^\x91\x1a\x86\x9c\x97\x87\x1b"\xaaV\x0b\xaf\xc9!|?oa\x8c\x87\x02}R\xac\xccD\xb1\x10t\x8cT\x1b\xb2\x17V\xf2\xeb\x8bn\xc4U\xc1\xd7\x05=\n\xb6\xfa\x1d\xf8C\xc8\x1eg\xa9\xd1+\x9ed\xde\x9d\xc3\x9c\x11A\x93\xd3\xe3*\xec\xb6\xce\xa2\x90|\'\x842\x05\xc3\x05\xd0De\x92\x85\'\xddo\xd7d{\x91\x9ee\x0c\xc5\x08\xfc\x9d\x05\xab><\x1bw\x948\xbc\xea\xacb\xd6\x86\xefm\x84\xca\x88D\xdc\xae\x0cRZn\xe7\xcd\xd5\x10\xf5wL\x18\xf5\x8ac\x82t\xe3\xe7C\xad.\x95R+\x89\x9a\xcd\xba\x1f\xfa\x83\x11\xd9\x01\xca5t\x9b\xc5\x17\xde\\\x8e0\xe2\xacb\':\x1f)\x85\xfd\x01t\xae\x04%\xb4\xf2\xc6\xa1u\xe1\x93\xca\xb3\x15\xce\xfc\x02\xbc\xfaF\x94[\x9b\xcf\xb6\'\xc3n\'%\xfa8\x1d7\xe3\x0cN\x83&\x8c\xb5;\xfe\xf7\x93\xae\t;F4s\xd4Cc4<\xa58#7(\xe7\xc1w\x89\xc4(\x12\xb0\x98\x89\xbe\xdcL)=\xc9\xd0%8\xab\xb8\x1d\xccN\x1e\x8d\x94\xe7{B&\n\xb4\xb30\xeb7/\x11\nKZ!\xee\xbc\x0b\x93\xa5\n\x81\xe6\xeb\xa4ey\x98t\xaah\xfd\x9e\x15@\xf7\xcf\xd4h\xd8=\xbb\xb7\x15\x82\xae\xb0L\x07\x1b\x05\x00\x82.\x03\x19\xa1\xf9v\xf0lu\xc6\x9f\x15\x03u\xa0n\xc5\x9dw\xd2*\x07O<5~\x10\x97\xf5\xe1\xea7\x82\xfbz\x9dm\xc4\xeaf`9\xce\xf5\xe7\x8b\x8f/\x11V\x187\xad\x02\xc5\x02\xe1\xb5i\xbcU2o3\xfdrk\xac\xeee\xa4!\\\xb1K\xed\xaf\x95*\xcc\xf4\xb9)`\xc7\xadE\xa5\x18F\xc0\xdf\xb1\xb71\xce\xb6nb7\xd5\xe4\x8e7eAfCh\xc6\x8cR\x00\'\xd3RB*xq\x98_\r\xe4mc\xba\xb0Zvf\x1c\x14l\x0f\t\xef\x8a\x98\x92s\'\x97\xc8`\xff\xe8\x85\xafw}~\x05\x12\xc0\xde\x8b\xf5d\x1a.\x17\x1b^om\xd7ya\x02\xc0<\xa6\xc3,[4\x0f\x80D+\xbd\x1e\xbcIb\xb5\x81\xc4\xf2\xcc\xbd\xba\xf1\x1f\xeb\x86r\xe6\xd6\xce\x1e\x1e1\x937~gP\xc5I\xff\xd2%vc\x06S7\xc0~\xcd\r\xdc\xd4<\x02\xab\xf7N\x0c-\x88\xd8\x8bY\n\x94\xe0\xac8c\xabgH\xcd\x9f\xe5\x05\x8d*4\xe6E\x90OS=\xc2\x00\xe7\x0c\xe6\xa8\xa3\xe0\xf3\xf6/H\\0\xb7\xac\xa6\xfa\x16[m-\x12\x1fP\xae\xa8Ax\x97\x82\x90\xe7\xf1\x19Q\xae\xe6^\x02\x9a H\x8f\xf4\xe8\xec?y_\xbcZ\xff\xdcZ\xe51\xb6\xe9\x17\xb9\xc8Y\x11\xd0&\x8d\xbaC\x1d\xe3\xbc!\xbd\xd7\x91\xbb\xc0\x81\x8c\x0c\x15bKn\x95\xef2\xc6\x15z\x8fY1\xac]\xfb\xa2\xb9e#\x08\xb0V@\xf7\x93\x1c=\xe1\xecUe\x1b8\x85x&\xb3\xbfy \x96\xb2\x0fX\x99{\xb9\x86\xf4\x16\x01\xafh+\xca\xd2X\xa4\x19\x87\xc7\x15\xb9\x1b\x0f\x12\xb3lX\xa3\x9f>F\n\xfc-\x1d\xfb\t\xf2Xk\x8e)\xc6\xb63\xdc\xe3L8\x89:*\x8a\xda;X\r@|A1S\xa2\x83\x04\xb5Z\xb4\xa4\xd8\x19-\xef\xf9p\x1a+C\x7f\xd6\x7f\x83\x87\x91\xba\xc7K\xf4!\x1az*L\xc2\xce\xfaV\xb4\x03K\x92\t\xb2KjH\xf7S\xf9< \xe8!\xe6\x8cX\x88\xdd\xca\x1d\xe9\x93c\xccX\x7fP\x98\xb1\xa0C\x15\x00\x8a\x950+\xecQ\xde\x12~c\xb64\xaf\xd7\x8b\xf7,6\x1f\x11mT\xfah\xe3*s\xf8\xafL\x03\xed\xda\x88\x8f\xac\n\n\x82\xb68\xb5c0$\x02\x05G\xc1\xc7t?\xb0\xbb\xf3\xf6\x05F4\x1fO%\xb8\xb0\xd3\x16\x03\x14\x9c{b\x1e\xaa\'DS\x05\xeb\xb1\x8f\x1ep\xb1\x18\x1e\xae\xf2\xc8\x00\x8e=\xab\xd7}\x93\xfa:\xdf\x7f(\xaf\xd8\xaey\xf6\xc0\x8a\xe0@b+!\xb0\xc0\x17\xec\x04N\xcb\x8c\x7f\xd6\xd7\xc2E\x95,\xed\xb7\x99\x83\x12\x9alP\xd4B\xf4\x07\xbc\xe6\'!E\xda6\xe7\x93\xe5\x1c\x00_\xd9uS\xc2\xf2r\xa1hW\xa3\xa1\x94\x9d\xf61\xc7\xdf\xb0\x82#\x1e\x15B]4\xc0\xb9C\xab\x1a\xadi\xf33\xbcR0\xec\xa9&\xccf\x939+y\xbf\xb0R\xcd\x16\xacD\xb8h\xa4\xf1 p\xf6\x08\xf07\x15\xf2\xe9\xca\xb8=\x1c\'\xef\x12\x8d\xf0\xb0\xa0\xa0\xaa\x0e\xec\xd2Uc\xc5$Z\xa1/\xf3.B_\x8c\xbf\xe7\xf1\x93\xc2Yi\xde\x9e\xf3\xaea\x95\xa6\x8a\xab!\xf1\x18\xda\xe7\x81\x92\xf04\xbf~\xf26\x88\x9fBZ(n\x18\xc6\xeaIu\x12\xc6/\xea2y\n\xdd\xaf\xec$\xce\x84\x91mp\xed\x1fwk\xd9`\tQ\xc6\xb3\xdc\xb2]\x0e\xd0x\x06\xba\xed\x11\xec L\xd7\x9b\xb6\xe5\xdf\xd3\xa3\xc7\xd0\xe8\x80\x06)\x97U\xf0\xd1H\x82\xaa\xc1:P\x85x\xce\xf1\xa4\xdb\xa3V\x9b\x08\x08W(\xfa\x1b\xca\xdf\x82\xa4\xae6q\x89\xc07\x0c\x05(.\xa9\xf3\xfe\x8e\x8e84\x1a\x10\xa3\x9b\x05U\xdb\xf70\xe7V}3\x9b\xa1GNc\xcb\xe8D\x92,xOJ\x19\x9a\x1fdF\xe8\xed]\xc3\xf8\xb1/w\xf7FX\x8e\x1c].\xfc\x9d\xef\xb03\xcb\xe6yNl\x1e\x01\xe8\x89Z\xe2\xb1\xbd\x9f\xf9;\xbc\xce@Y\xfa\xa9\xda\xdc;\x0e\xfa\x1a\xb9\x1f\x04\x97ky\xce\xcd\x96\x9a\xc7\xd1j\x86\xe5\xc6"\x80\xa5Q\x94\x82\x9c\x0c\x03$QP4\x01\xafEx\xf9\x00\xf5\xbd\xec\xf9\xf1b%\xbd\xc0<\xf9\'\x8a\xdbi\xf2\x81\xa5\x0f\xd0g\x85\\\x94\xc9\x03A\xc1\x12\xe6\x9bp\x1bV\xb1u"\xa6km?\x15\x98\xe6\x89\xb7\x04~9\xcf\x11\xb0\x833>\x88\xb0\xe9\x1bl\x8f\xd4\xb9\xe7c\xda\x93\xa0\x83\xe2\xd0\x10\r\x8cm\xc4\x7fk\x12u0#\xac\x88h\x0c\xb8N\x8a\x19\x07\xbd\xd6t\x88\xf2W\t\xa1\x02\xf4\xe3 \xfc\xe3\x00@\x95p\x9b\x92=b\xd2C\xcb\xd2#~\xdbd\n[\xea\x87q\x8cq\xc9\xc9\xf6B`\xa3\x91\xf4\xf6\x93\x0f\xbc\xe3\x16\x01K\x19C\xc2\xaa:\x84cd\x99\xe0\x1a\xd3\xe9N\n\xac\x00I\xbc1\xf8\x86G\xa4D\xfa\xf9n/\xf3\x0e\x00\x00\xdc\x1e\xa1\x1a\x9b%+\xc5\xe4\xc1\xaa\xca\x01\xec\xce\xde\x19\xe8\xf0\x15\x00\xd7;\xbc\xbd%\x9b\xe8\x84\x9f\xf7\xc3\x05\x94>B\xfb\xf6\xe8\x80\xb7h\x02q\xee\xfe\xb6 m\xfd\x8eP\x03g!\xee\xc3\x82/\x19\x9aF(A\xe1\xaf\xddC\x92\xb4\x9cg\xa17H\x0c~\xed\x97t,\x11\x95W?^\xf6p\xa0w\xdd<\x12\xcbx\x07\x1dClC\x04\xa9t\xadL\x03}\xfc\x0fZ \xde\xfc\x05\xee/\xe2(V`K"y\xd8r\x9d\xe6\x89*\xc7\x11\xb1\xd4\xa5\xcb\xfa\xcc\xc5(\xbfxn\xa9\xd9\xca\xed\xb7}\x993\x86\x9f\xd6\x00\xf70\xb3\x7fr)\xd3\x95\xf1\xe8}d\xd3\xcf\xa0\x1b;\\\xd27\xe8\xee\xf56or\x10\xcd?ZD\xf8A\xc8\xda\x13\xd0\')\x92T\xab\xf5\xe1\x15\x13\xdf\xa2,\x07\xb8\xe1\x1f?\xc0\xe7n\xb1Z\xf4E}ST\xcb\xe5\x85\x8a\x10\xce/\xbc\x95F\x80\x13K\xb5.l\xf4N\xb8\xc5_\xcf\xf69R\x8dG{V\x13O\x82\xa8\x0f$\xf7\xdf5\x11\xa7\xf26\\DBV\'\xea\x05\xcb\x01\xdf\xeb\x08Tr\xee\xd02\xadBIk~[\xd1@\x08\xcc\x9a\x8f=\n3\xa5\xcb\xa9Q\x10\xb7x?\x0e\xb6\xfd\xa6\xaa^\xe5_(@E\x173\xe3>\xbc\xaeO\xbd\xcby,\x86\xbdg\xd3\xed\x12p\xf1\x9e\x02\x99\xd3\xd9\xcb\x1e\x8b\xc8f\xbf\x92\xf7\x82\x84\xa2\x89)X\x10\xf80\xd2\x02\xae_\xf3\xffB\xb6\x82\x81\xfb\xf0\x94\x14\xbf\x1c\x0b@\xe6\x0f\x96\xe0\xc2\xa6]\\F\xbc\xa7\x9cQ"\xf0\x16\xb7\xc4\x97O\x91o\x11\xea\xa6\x90\x9e\xe5x\xa9y\xb5\x17\xa1b\x98\x03\t\x10f\xe1\xb8\xeb?v\xcc\x91\x920\xd5//i\xea R\x0b\xd9\x06\xbf\xea\xb5\x14V\xdbXc\xa5Ace\xa67N&\xb2\xab5#\xa23\x92\xd4\xb0\x12\xb6\xb5)\xe0\x90\xbb\x9f\xbf4)\xae\xa7\xe2j\x97\x0bm\x9azc\x8cq\xa1\xdc\xed\x86\xe5\xa9~\x8f[=\x8e\t\no\x1b\x1e\xb2yZ\x85\xff)z\xb9\x02\xb5\x8f\x93\x82\x95\xf6\xb4\x92L\xe7\x17L\x9e^\x83\xc2dT\xa1\x90A\x0f] \x8a\xaf=\x1c\xb74\xf6\n\xea\xe8\xb5ocm\r\x01\xb69T\x94\x80D\xfa\xd5C`\x8c\x8e\xecD<\xcdk\x06\xf6{\xb2\xd6$}}\x16\x17\x8aB\x97{c52\x81\x94f\x10sa\xe7\xdb\x1b2\x03\x8f\x8f\x11T\x01\xe51`\x0e\xbb\x19=\xc4b@Pn\xa5\x1d\x11\x88j\x9f\xcb\x1d\x16\xe1\xd5\x1c/0\x18\x82G\xeav\x8d9!\xa4f\x15\x187\x81\x8c\x98_\xcd\xa7\xe9\xf2+\xfdm}\x18\tO\xfc0gn\x02n\xca\x92\x00\xe6`\x03\x14eeq-6\xaa2\x15=z\xf5RC(\xa4\xcc\xb3\xb7\x0cS\xcc\xc0\xc5\xda\x8f\r\xb29\xf8Iz\x16\x82:\x9bU\x9e\x01\xed\xc7\xfe\x8e\xb8\xb9\x84/\x91\x91\x06\xa7G?\xe7~l\xc0V\x9c}\xf4\xc2x;\xc0d\xdcf%\xde\r\x1eB3]\x7fl\x9d]gU\xe3\xf78\xa6sK`u\x889\x87b\xaa"\x04f\x0b\xa0\xc4.\x02>\xc8,KA\x1a\x1c\xf6\x0c\x8b\x81%m\xfaq\x0b2\xd1\xa0\xcf{\x914\xed\x8d\x8e\xe8\xa63\x9b\x9at\\\x84u\xd5\x0eO\xbeX\xdc\x90\xc4\xc21\x16\xe2V\xd0\x0c\x91\x8f(\xf4\xad\xa5\x9c|"\x9da\x91bd\x19\x13\xadt\x915T\x97z\xedE\xdc#p\xb7\xec\xf6m`[\xc3\xcf\x7f\xae]8\xf5\xf7\xe4\x8a\x8f\x19C~\xdd\xafI\xeb\x8c#\x85\x93\xb2\x9a.@\xb2FFa\x83\xcds\x1f\x97Ds~\xe5\xbdz\xd8\xde\x04-\xca\x0c\x7f\xcd\xe9\xe7\xbd\xc8\x987\xcf\xf5<*\xe4,dK d\x0c|\x8de\x12\xe4T\x1f\xef\x1c\xf5\xdeO\x96\xa1\x86\x98\xf9\xdb\xed\xbe}<\xaa\xd0\x0c\xba\xc3V\xb9\x89u(\xd1\xe1\x0fC\x12\x8f\xd6\x80K\xc8\xbb&\x86\xff?\xb4\xac\xd0Q\x0bO$qn\xd9\xfa5k5;\x8a\x92\xff0\xc1\xf8\xeeP\xdcUc\x1b\xab^\xe7\x02\xbd)\x93]\x19O\x02`\xdfQH\x86\x07\x878\xe78\x1c:\xb1D\xd6\xd9\x81-\x82\x1e.E\x83\xea\xe6\x92]A(QM\xda\x9c\t\x16VgK{\xf7\xd8\x87f\x02\x91\xbe\xf5\xc68z\x98SJ\x14\x95\x1a\xaf\xbbi\xdb\x85%\xc1\xb4\xb4\x89\xe8\xe4\xa9\xf7\xb3:w\xc0\xc5\xe5\x95um\x8d\x9b\x07^\xb5\x9e~w\x95N\x90g\xfe\xc4\xa6\x82\xd3\x11\xbb\rz\' @YW\xfa\x83\x1eS\x19\xb4\xe2I\xf56\xa8 ^\x8e\x18?\x08z\x19n%p\xc4\xc2\x04\xf1Z\xc33DP\xae\x83\xacy^K*\xa4C\x9a\x80\xce\xce+<\xb7\xeb\x13FM\x07\xb3\xe1\x1e\xac>\x17\xf4\x11\x89\t|GT=\xc5\xea\\\x1f>lFV}\xdb\x93Ag"0\\\xc2;\x84\xb9L3\xcc\x9e\xf2i~\xb6\xd8fV$":S\xb9)\xaaP\xdb\xf1\x98\xb3\x0b\xe8^[\x7fJ\xbf.\xd3LK\xffx\x1a\x9aSt\xb9,\xc4\xdc\x98\x8f\x84\xfaE\x18FUmc4~\xe0G\x1f\xad\x03\x80#a-\xef\x95\xcb?\xa9\x18d\xae\xbakB\xef\xbe\xc5+\x9a\x920\xb3\x173\x01\x1a\x9bw\xf4n*\xb6\xf05\x1a(+\xb0:tL\nG\x95\x88\xf8\xfcT\x82\x14b\xc7\xd8\xb9\xa6\xda.\xbcFe4^q\x9d\x08\xb1@m\xc8I\xa4\xbd\x8dE\xa4Q\x1al5l\x11\xff\x1e\xb7\x04`\x0e\xb6\xcbe`\xfd\xcef5\x8e\x1a#\xdd\x14\xd2\xd0\x90\xbe\x93K\xb5\x8c4iQ$$4\xd1\xe2\x10](\xd9\x18\x17#\x1cR\xea\rz\\\x9a\xbd\xc3I%\x1aJ%$\x0f\xef9\xfc\xd7\x95;\xef7\x017\x92\xa3\xe1yD\xf2!\xd2})hH\xf9\'\xbf\xa5\x91a7\x19\x8b\xadT{\x00/\x155+\xb7\x00\xe1\xa5Z\xe4:/s\x13\x14\n[X\xb8\xa0\xc5]\xc5@\x8a\xb9\xaf\xf3\x82?\x9a4\xeb\x1d\x8f\xfe\xf1\xd7\x14\x93\x90\xa9\xb7\xf8\x80\xcd\xb4\x16\xd0\xa2\xec\xe5"\xe4\xb2\x0c\xfd\x0e\x0e\xb4\xdf\x8c\x84)\xc3N\x99\xa3\x97\xf0\xe6\xc76mLN\x85NM<\x1e]l\xd0`\xa1D\xdd\xb6\xcev\xb5\xce\x87\xa60\xe4\xa2nXG\xd2\xe7\x83\xfe\xb7]nC\xbb\xce\xed/z\x8d|\xa0\x99\xb0\xa4\x8b\x9b\xd4\xb3\xbf(\xd7\x960\x03\xc6\x8b\xa0\xe0\x1c\xfe\x9d\xf0\xc5\xaa\xb0\x04\x8fm\xd9s\xf3g\xc7\xfa\x0f\xaf*\x80\xef\xd3d#\r\x8f\x13\x85@\xe0\xc9\xb7\x0e\x1c\x94\xe0#\xca\x89Q\xb6\na\xb6X\x9bRc\xfeI0\x86u\x9do\x8ao?\xd0\x19\xc1\xcdm\x11\xef:\xb0\xe4\xd7\x84:\xd2H]W\x95\xdb\n8\x9d\xdd\x04I\xed\xbe\xfcX\xb8\x07\xfdx\xefP\xd3\xf6=\x18\xf8\xae\'\xcdn\xe3\xf60\xde\xae\x1a\xc4\xac\xb0>]f\xfd\xe8z\x9a\xe3T\xef\x9e\xac\xb01\x93]fb \x9c\xb5\xd3\x0fY\xf0\xa8\xa8|e\xb8\xe3\x01bu\x0b\xeb\x99\xbak\xe03\xa2c\x19\xbd\xa9.\x13ME\x1a\xa6\x05\xfa\x8f\xe2\xc4W\xca\x95"%\xe8\xe0X\xac\x1e\xb4\xe2\x08\xe0\xe6c.\xb41\xaa\x12\x01\xaa\xb6\xc5\xd7>\xcb\xbd\x9e\xe8\xf4\xb2\xbc8m7P\x0c\xa8\xd3k\xcfl\xcfG\xf44\xdf!\x8a\xa9\x1f\xe8\xe8\xa9\xb9\xa3\xc1D!\xfc\x18\xa8\x962\x9f2\xcb\x96\xe82C\xa5\xdf"+\x0bT\xa1\xe2 r\x97\x80\xa0\xe4\x84\xed\x1c\x8d\xcfI\xe8\xe6q\xb6\xe1\xad\x1f\xda\x13H\xa7\x95\xb5H\x98 Ze\xf2\xbe\xc8(i\x19`(\x83\x13\xcb\x19\xf2\xde\x88#\x0f\x8bu}\xdcS\x0e\x12mN\xdd\xf8\x13M\xe4\x8e\x01\x9d4h\xb5\xbd\xa3\x02\xc4GhN{\xcc\x13\xbd\xdaW\x84\xba\x0fI\xf1Z\x96v\xda\xb9\x89q\xa3\x1e\xf4b-:\xb7\xe3\xa9#\rp\xe1`\xc1b9\xdbj\x80\xc6\t\xe9jg\x82\x05:\xb1\x96%\xb2\xbf\xce\xe9\xf8\x8a\xf1\xfd\x8f\xd2\xa79\xfd\x98G\xdd\xb0\xec?2!\x0b\x8b\x90\x92\xe9:C\xb0\xbe\xaa\xe6\x8f\xe9]b\x86:\x87)\xb5S\x98Z\x92x\xe8%\x05G\xde\xb6&\xc4\x87\xc0F\xfe\x83?\xc4\xc3\xc9\xba\x90y\x90bQz\rvB\xa9\xc4\x02\xdb\x88q\xe7I\x100X\xe9 \x88\xa21\xe5\xd8\xee^Yw\x8c\x98\xac\x00c\xe3\x973,\xbb\x08Y(\xc2\xab6G\xabh\x1b-\xe9\x155\xbb\xeb\x96\xd0Zq\x82+\xd1\xa2\xe7$\xe1MQ\xac\xc3\xa8\x1d\x0c\x82Lw\xd2Y:\xda~\x92\xb4\x99\xbaw8S^<\r\xd2=\xe6Ko>\x05p\x9b-\x08\xa6\xe6\xee\xcbdz\xc6L\xfc\xec"\xf8[\xe4OV\xd6\xe9\x8b\x98b\xaa\x81\xf84\x94\xebm\x8bt\xbf\x0e\xc2\xf0&g\x1b|\x89\xbbv&\x11\xc2\x85\x17\xfcV\x16\xc8"\x81\xedl\xf8\xdf<m)\x94\x00z\xbbn\xe1\x04-\xddR_\x9c|\xa4u\xa6\xad\xaf\xdb\xfd~\xeb\xf6\xde\xca\xd5\xdf#N^X\xad\xeb\xddp\xecr\xd3\xed\x9eJ\x0f\x91V-0\xd5\x9e\x90?\x7fF\xebl\x1b\x7f\xcbl\x9d\xa9&\x1a\xc8\xa7\x97s\x1eo\xfb.\x0b\xf4=\xdb%-%\x1a\xc7`g\n16\x05\xbf\xce8b\xe4\xf0\xa7\xb0\x1d\xa9\x15-D^\xb2\xde\xcc\x10\r\x06I\xb5\x9e\xa5\x93\xfd}\xe8\x97\x04N\xf5>0\xc5\x03\xd8\xd05\xdf\x94_\xb2\x80\xd6}\xddn\xb9\x16\xbf\xf3j\xd5\xee\x93\xa3\x16\xb6\xff\xc0\xeb]\x08[g\xb8N\x15\x8b\x85\x01\xdc<q\xd2\x9e\x82\xfb\xa9k\x1cX\xe5\x05\x87\xc8\xfdG\x12\xb2iN!\xe5\x0bw\xd0\x8a\xae\xaea\xa1\xc0\xcf.\xbdz]\x84i\x9d\xe2\xf8\xa6h\x8f\r\x9c\xd2\xac\xba\xf5U\x90\xdb\xbd\xec\x9b\x9c\x95UjZe\x1e\x0f\xc5\xc8\xee\xb9O\xbe\xb9\xa1\xa6\xb8\xacD\xc8\xff\x16E5\x11\xa6\t:\x8bW[l\xd3\nyS\x08\xdd8\x19\x04G\xa1*\xeb\xb0\xf1\xd0s\x91|\x0bO\n\xc24A\x81\xa9c\xf8\xa9\xce\x12\xd1\xb9\xbdH\x86\xf6\x90\xcbV\xcdQ5\x07,\xceX,Y/\x8a\xca\xe8\xf2\xc6\x15\xb9\xed\xee2G2\xbc\xbbXHaM\x17\xee\xfd\xb0\xaa\x8b\xe2\x01^\x10\x92W\xa2A\x98\x83\xde\xed0|\xee\xef\xfb\xfe\x9c\xa0\x8c\xc6d\xb252\xd6\xe2\x82\x14+\x0cR%\xa9D\x0c5\x80\xcd\x99o\xaf\xa0~\xaf\xc3W\x85\xd0~\x1c\xf2\x15\xc3\xb1\x14_+\xe7\xe2S7\xaf\x05x\xf9\xb6\x1ePML\xcb\'v\xb4\x89\x10\x08h-\x05\xdc\n\x8a\x83\xaeW.\x13\xaa\x88\x84?\xd4\x10\x15\x92\xeb\xfc\x17\xd7\x81\xea\xca\xe1B8\x00\xb5L+\xd2-\x7f\tD\xe8\xbf\xfdA:v\x8a\x03\x19\x7f\xbc\x98t\xfacL\x1a\x83\xc6\xea\x19\xb1\r\x87\xb8yk\x88\xb4\xb8\xdaw*}C\x9a\xbe\x1d\xec\xbb$\xc3Y(#\xe1\xa1* \x99\xf4\x1c\x0c6\xdd=Kn*\x80\xe3\xd9\xca\xc8W.\x8c\x7fC\x86\x0b\x8e\x0b\xc2%7V\x01.W\x83\xa6\xd53X\xcf\xba\xd8 \xe6K\x84\xfe6\x1f\xfb/<\x9fM\xdd.\x90\xe8\xa5\x88\xd44\x87I\xc2Y\x0c\xdd\x16kRm\xf6-\xcbA\xe6\x11\x12*\xa0\x81,(\xac#\xd0r\xd2\xff\xbd\xd00n\x1c\x10\xbf,,\x8c6k\xb3"\xb1Zv\x18zn/\xd1\xbf(\xd6~\\\xb0%(\xce\x91\xcf^|\x0b\xa7tIR\x97\x8b\x0f\x8b\x8e\x87U2r\x01\xea<\xea\\\x15O\xbf\x8f7\xb1f\xb0\xd8\xa2\xe0\x8a@\xb9a\x8dC^K\x1dB\xb1\xa2+"\x80>\x97\x11*.\xfd\x80\xaf\x86\xb8\xcc\x97\xfc\xe4\x93iqb\xae\x9aRA\xc5v\xcb\xe1}x\xf4\xbd\xe3xJU\xf6\xb8\xb8m\xb6\xafT\x9e\x10-\x83\x83\x17\xa2[Q\xdf\x07\x8e\x9e\xb2\x17\xda\xa4\x89\x0f\x97\xa7\x80\xfe\x1cS 6\x01%\xbb\x8e\xca \x82F\xf5 \xbd\xabG\x85w\x7f\xa6\x9e\xab(\xb72\xd0\x80\xcev\xba\x96d\x08\x99|lU\x08;PL\x18ec\xcb\x81\xcb\xa8\xe6\xda\x88Or[Ms\xed\xcb\x8d\x99\xbaSpq\xdau\x86\x16\x7f\xf9\x80\x18+g\x98\xa6l\xe3^D>tw\x80\xf6\x0c#\xfe\xd24\x93\xaet\xc6\xd6C,<"\xdc\x97W{\xdf\xab\x11\x95e\x11N\xeb\xf8\xb4.\xf5\x97\x02?\x8d\xde\x0bv\xady\xadDg\xf7\xf9\x00\xe0\x14\x9b*\xab\xaaj{\x83\x03\x0f\x84~\xb9Q\xc3\x9c`\xf4@C\xdd\xc9\xfb\xd5~$\x0f\x7f\xcbR\xb5\xc1\x1c\xc8o\x929\x82y\\\xc9O9\xf9\xc4\xa2E\x0f\x05\xc7\x14\\\x80\x1c1\xa4\x1dI|]^Rl\x01\x0c\xa0\x84\xa5b\xe6m\xb8o"\xdfM\xf0w\x80K\xc2{b\xb1\xc0\xb2.\x9e\xd9\xd4\xf6\'\x16!`\xc2\xb3\x97\x96#\xeb\xbe\xf5.\x04H\xd6j+p\xf1\x01\xe8\xc4\x00\xed\x18\x96\xbc\x88\x9c\xb8-\xc7&\xd5\xfc\xbds\x8d\xcd5\xcf\xfdt\x88\x9e\xc3\x1e\x9c\x16\xc3\xe0\x0cBLJ\xc2\x8fi%\x05^&\xe8fR\xd2\x1c\x90\xb4\x916\t\x07\x8cA\x15|\x88$#\x18\x88\xe7\x98\xba\xf7\xa8\xc6\xd0\xab\xa9\xae"\x98m\xc1v\x87BG\xf9\xbb\\\xa0\x83\xa8S\x81K\xd8\x01\x9c\t)\xc8u.\xa7\x85w\xbc\x1aJ\xda\xea\xd9kD\x01\x93l\x0cS\xae\xd91c\xc567\xd7sc\xfe9\xb9\x81\xc6!\xc4\xfeR\x06\x9c\xaa\xa4\xf8>\xcf\xa0{\x04\xfc\xbc8G\xb0$s\xbb\xc4\xdc\x80\xb6\xa2\x95\xa3c\x10@\x1c\xdc\x1fD\xe2\x1d\xcf\xd8\x85\x81\x10G\x85,\xbb\x16Q\x0b|\xf6p\xfb\xe4\xe8\x1b\xc9^5rhB\xdc\x11\xd1Q\xf4\xd0\xd9\x0f\x90\xe3\x9b\x9b\x1f\x00k\xa9h\xefV\xf2\xf4\x98\xdf\x83o$\xa4\xc2\xb0\xc2\xe6(\xbe/2b-\xca\x87\xdf\xb0\x92\x04\xca\t\x82\xe2+o\x92^#\x96\xf3\xaf\xbaW:\x1fF\x1bn\xfdQ\xe0q`x`\xe45\x06\x14x\xcdD\x96(\x82\xd3R\xdb\xa4\xc1\xad\x15\x16\x03\xbe\xfa\xfc+\xb3\x10x$\xf1\x18\xec\xcf\xde\xd2\xda.iD\xfa8O\x9c\xf5m\xca\xce\x08\xabw\xa7\xf6\x91\x1f\xb0h\xdf%%2\xba[\x91\xb0#\x1f\xb1n\x1e\xa6\xd8\xb0\x85\xef\xd3\xf2h\xb1\xe0\xa1\x947\xde\xe6\xf4\xb2\xf5\x11\xfde>\xcc\xe0\x9f\xc6y\xae\xc4\xab\xab\xe1\xf7C\xecE\x17\xa1\xb0]\x18\xa1\x1d(m\x19\x83\xe0\xde\xf8\xc6Idk\x9801\xb6*\xd5\xfd\x94\x90\xca\xb0\x184\xbdK\xf0\xf4\xf00\'\xae\x7f%\xe0\xc6\x91\xe4<\x88\x1a-\xd0\x0c\\\x07\x82\xba\xedp\xce\x85B=\xf0\x1fz\x18\xf3R\xc5\x13\xdb8\xbd\x806\x88\xab\xee>\xe4\xbb\x95\xf4\xe4,^\xb3\x19\xdf\xb2E\xf8G\x90\x97\xaaO\xa0n\xde\xaa-Y\x86B\xba\x0et\x1f\xbf6B\x85\xa0\xee\x93\x03$:rF\x96\x1a>\xb79\xc0-z\xf6\x18\xb7 \x90\x98I\xe7k\x11Pw\xa2\x045\xde@\x9e\xfe\xac\x12MKf\xefp\xc5\x17\x19k\xa1\xf9\x04\xb6?\x8e\xef5\xdf\x02\x91\xe5\xa9@\x80\xd1\xe7\xb8\xb2)\xabG\xb5\x8f3\xfb\'s\xcbQq\xc8\x86\xaa\xfc\x19g8Xf\x14\xf8D\xd4\xcc\xdcc\xa3A\x08\xe1|W\xe82\x9d<\xb4\x8f7\x87\xed\xf9&K\x05\xb7\x8a\xdf\xc8\xc9j\x11=\x1e\x7f\xcf\t\x18\xc3\xae\x87\xa7E-@\x11\x0b\xdd\r\xb2L\xba\x18\x80\xd2\\j\x1e\xeb\xbd\x055\x05\xb6\xb8\xcap\xdd\xc9:\xbe\xb7\xd9\xd81\xb6r\x8e\xa3&\xab\xcdl\xeb\xefe\x15.]\x92\x1f&\xca\x15\x01\x98\xd6\x9d\xf0h\x06\xa4\x94,\xfd\xbc\xb2\x7f\xf5/H\x8aV\xdfb\xda\x0e\x88bx\xad\x9d\x00T\x0c\xee\xf2\x1b\xd3\x0e1\xf9h\xc8\x97\xe1\xe7\x82\xc4\xbc\n:4\x88\x050\xb7\xfb\xd5I\xcb\xdf\x8d(\xd8\xc6\xce\xb1u\xb3Eb\xe0+\x91\xaci}\x05\xe2\x92\xbd\x88\x9c\x03\xa5#T\xd5\xfd\x1f\xe6\x98\xf1$l\xb1\r\xe9y\x8a\x0e9\xe8\xe6g\t\xc3\xb6|\xbf\x8d\xac\x0c\xc3\xd7H\xd7Ft\xd8\xfd\x06\x15\x80l\xe1{\xb2Q\xa8\xd5\x02\x03\xa4R2z\x07\xec\x9f\xc9D\x13\x84\x14B\x01\x0e\xe2R_+(8$C\x0cd\xc8r\xce\xaa\xd8.\xce\x0bu<5\xd0"\xa3\xa8V\t\xb0\x85R\xbak\xd9[\xb3\xaff\x0c\xb3\x91o\x7f\xf9\xc6\x1e\x0e\xda\xe2?w\x88\xc7\xe9p\xef\x9c\xd2\x99B\xae\xa1\xe1\xd0\xf0nI>\x02\xe1\xa58$C\\l\xae\xf5w\x9d\xbc\xcd\xb1\xbd\xb3cM\x82\x17\xd8\xf8{VC\x07\x14\xfa\xf8k\xe5^\x05\xad\xac\xde\xb87\x84\'8\x837\xdf#\x9f\xed\xf0\xc2\x02WUsN\x1eE}l\xcb\xa2LE\xd0r\xa6\x9d\x90\xbd\x9d\x06\x10x\x8d\xb75S^\xa2\xa6>\xc0\xfc\xe0\x8d\xabS.\'QZ\xd3\xbc-\xc9\x981\x98h\xef\x9f\xbc\x89p\xd5\xc0\xe3\x15\xa8"\'\xd7i_\x95\x91\x8c\xb3\x98\xfc\xfb\xa68\xdb\xc7}<\xd3x\x8b\xa4z2\xdfa\x12d+\x80i\xcd\\\xd0C\xf7i\x80Gse\xc7\xce\xa3\x10"~o\xb3\xe6L/3E\xdc\x11\xff\x8b\xe8\x02\x92\x8d\x00~W$\xc2\xe3\xbal\x1b\x0c\xa7\xc9\xa7\xd32tj\x08\x9dX\xb0k\x9aP9\xde\x077\xa5\xa3\x0b\xb6\xd6\xf9\xc3\xf1\x1f\xd3\xfa\x8b\xc1\xe9fYf=\x0b\xcct\xf6\xa47\xaa\t\xf3\x97\xe9!\xc9\x05$\x1e\x96\'b[\xfd\xcdwAee\xeeq\x9d_\x10\xf3\xef\x89E\xac\xb4\n\xf6\xc7\x9b\xf6w\xbd\x9d;\xc9\x0b\x8b\xb0\x9d\x86\xe6j\x85\x0b\xe02R\xcc\x94(QNpO\xdd;$\xaex\xd3z\x8f\x9c\x1f\xc3\xd2\x8d\x02\r\xda\xb1Ei\x1d \x17\x97t\xb2\xf6t\x10\xbf&\x8e\xba\x94\x19\xd4\x84s\xe3\xcfA\x91\x95Y\x7f\xf3\x98\xf8\x93Y\xd0\xa1F\x9d@g\xeb\x02\xb6*a\xbe-;\x8c\x16)1\xa5\xd1Q!\xb6\t\x89q\xae\x0f\x7f\xe3(F\xd8\xf1\xd4\xa1\x03[\xdbu\xd7\xc8\xbb\xd7\x08}\\\xd7\x7fW\xb4\xf9\x18aNg\xbe\\\xa4\x1b]B$m\x02X=\xc1\xe1>~u}\xea\xd5\x07\xa8r\x0b,&\xe8H\xb9z\x7f\t\x9da\xffX\xab\r\xd58\xa2#B\xb6#O*%\x8bG\xd2#}z\xca\x03C\x19\xce\x07C\x11r\xe9u\xde"W\xdf\xe8\xe1\xb7~\xde\xf4\xf5l\xc3\xe09\xbd}\xa1\xae\xef\xde\xb6\xa2\xc7\xc9\x151\xea\x17.\x99\x03\xf2-\xb6\x82\x04+2\xc3BL\x18\xda)\n\xe9\x94\x03+n\xd4\xff\x1ahO5\x88\x89\xec,<\x04\xc5\x1a\xa0\xadI\x91\xe5\xcc\xb9\x02\x929\xa0\xc0a\xf0_\xa1\xfen;\xf5\xd8\x8e\x9c1\x1eE\xa7\xd6\xb5\xd5S\xa2\x0f4\xf8\x03\xe3fkP\x02_\xb6\x98?\x98\xf4%\xd7\xab\x04\xda\x82\x8c\xc9R\x1b\x15\xc9m\x02{A\xec7G\xcc"8Ki\xa9\xbbnV\xb9\xa9i\x0e\x7f\xcf\xc1J32\x1d\xa1\xa3^\x07\xddkX\x8aM\xad/\xc8?8\xbe\xfd|\x13\x1e\xb72!\xd9\x0eB\x1c46\xff\x14\xeb\x8f\x10\xb1\xe1!\x0f\xfa\xb9\xc5V\xf2\'\r\xf7^\xd1\xc3\n\xf5\xee\xef\xa2\xd2z\xafw\xe7\xa6\xdb\xb9\xb2s\x14\xa4\x83q\xce\x13\xdd\x83\xdf\xfa\xc6yk\xb9\x95\x08$]\x14\xcfmUQ\xdb\xcd\xec\x86"T\x11\xa7\xfb\xc6\xf2H\xf5n\x84\x182L\xf4K\xf0\xed\x8cuB27*\xd6xk\x16\xfe\xc8\x86\x88#\xf9\xd8\x88rI\xd3\xca\xe6b-C\xe0E\xae\xc2\x8b0\xd8k\xe0\x98\xc7\xb3\x02%\xb5"\xef\x83\xedh*\xfdH\x00{jo\x0b\xb2L\xdd\xe9FS\x1d\x99\xd2}\x08\xae\xb7\xaf\xbb&#\x8a.\xb3\x84\xf1Y\xa2m\xed\xa5P+\xb7\xc7\xc5#\xb9\x1f p\xd2a\xed+y\xe3\x16\xff\xa0\x1e\xb8\xf2yroU\x10\x11lK\xea\x18f\x07\xce\xdb\xba#\x00C\xcb\xc2\x81>\xd1\xd4\xd8\xfd7&\x10\xf7\x7f+zH*^\xfd\x08m\xae\n\xb7\xb2\xd7\x11\x92\xfb\xce\x0b\xdb\xb8\xd0\xf4\xb3B\x1a\xd5\xd65#!Ow\xfd\x90\x0b-s\xda)\x95Qd\x9f\x80\xea\xbbf\x03\x9a\x84\x00Q\t\xb2\xe7\xb4\xcc\x9c\x89\xbey#\xc3\x8f\x16\x13}HL^.%\x80p\xb1ZQ|\xfaE!\x8d`\xa7\xe5\x8ey\xc1\xdfl\xb2f"P\xfc\x9f\xc036\xdc\xac\x05\xdealU\x90\xeb\xbbO\xbaI^\xb87R-\xc9\xee\t7\xf8\x16\x95J\xa9\xce<\x16\xd9\xfb-K\x11\xddY>\x0e}\x18?\xfd\xd5\x84\xa7\'\xf5=\xa7\xe8\xd5A:\xcbp\xec\xebR\x94\x8b\xe9\xc6\x17\xbaL\x9f5\x04\xef\xcdkbEzQW\xbd\x9d\xae\x15n\xbe=U\x92\x99\x8bkq\xcf{+\xde\x1aVz\xf0\x00;C\x8d\xbd\n7\xe2\\%J\xd8\xedXRV\xc91tHU\xbe\xbfV\xf9\xc6\x90D\xda\xadG\xdc\x13\xb0\xd6\xc7\x18\xd4\xeb\x12~\xe1 p\x8d\x17\xce\xc6\x02\xd7v\xcb\xe1\x07\xf7D\xfcb\xaf\x10\xb7t\xd9\x87\xac\xf8\x0b6\x86\xb9\x9ct\x9f\x1e\x12\xc42\x1a\xf9\xc9\x85\x89@6\x94\xce\xd5\xa2\x86c\xb6\xa5\x96\xae`!\xc8\x9e\x05\x87E\x0f\t\\*\xdd\xfdR\x0c\xa3\xff\xae\x9b\xa1y\xa7\x04}\xd2K\xbc\x9fiw\xe1\xff\x92xyc\xa8\xfc\xb8\x96\xf8Q~\xc0\xc8\x8d\x01\xbe\x12k\xaa\x1a\x89\xf4\x15I\x10\xffR\xbb\xb3/\xc9c\xe5/Q\x8aT\x8a\xc1!\xfd\x8a\x97\xd6\xec\x81@\xec+C\x8a\xc4\xbe}Y\xb5\x9d\xfb\x82n\x99\x9c!\xca\xe4\xa0&\x88>J\x872YBu\xce\x1a\x8a&\x01\xeb\xe9\xdf\x1c\xe5\xc9\x17\x1e\x86\xa3\x7f\x9c\x81gf\x85\x8c!\x9e{$\x15\xeer\xe0\x9d }\xe0D\x9f\xf4H>C\xfb%\x96\x87F\x00\xfa\xc6XqL(\x85\xbb\xc1{\x90\xe7!\xa4\xe6C\xbc\xe7\x9fz~\xd0\xff\x0f\xa2\x9e`\xc9\x0e\x93_\xc4A\x8d\n\x02\xe8)\xe1\xa0\x95\xcb\xa3\xdaI\xca\xfd\xf2\xf3\xbc\xf3\x04\xd6\xa0\x0c\x9c\xaf\xe9\xf4?\xbb\xa8\x9b0\x10\x81\x00\x08\xd41\xc3\x94\x06\xae\x89\xeb\xeb\xb7\xd6\x8e\xddKri\xe0bXiRs\x84DJ\x1d\xe9\x7f\xbd\xea\xa6\x9d|\xec#+\x0e\xf91c\xcf\x1bl\x90\xdb}\xfd\xb4z]\xf1\xcd\x89\xb6\xd9\xa9\x87f\xce\xac0\xe2\x9a\xd8\x1b\x97\xb1\xdfX\x9f\xf1\x9a\xafn\x0f\x1d6Y\xbcT\xbc\xd9\xda\xc6\x80IH\xf8\xf0P\xc83\xccm\xebU\x91\x1e\xb6\xdc~C\xf6\xfd\x8e\xcd4?Dk\xc5\xcdc\x80\xfd\xfb;\xf4\x0f\xd24\xdf\x91\xcc%\t\x94\xb3\x0f\xc8\x95\xf2Uj\x82\x079\xc2\x9au\x81"\x8c/\x03\xad\xcf\xe6\x9b\xbeu#K\xc7#\x92u\xa5\x13o\xb9\x0bh\xd6\x81b\x9f\x94\x921?\xfc\xf4z\x95\x00<>\x13={\xb1\xa8|L\xbe\xcav\x17w(D\xb4=J\x16\x9c\x97\x9fWD\xf2(c\xab\xa3\x10\x06\x9c%\xa5\xc6\xf1L\xef\xf2\x93\xb6>\x93\x1f\x0f\xde\xf0$;\x08C%\t\xecJ\xfe\xd7\xb1`\xc2\xf9\x8c;E\xfdQ\x98Q\xb26\xf3~\xf7E9Gr\x92\x11[\x1bE\xb9g\xc8Z\xff\'g\x8c\xbe]_\x9e\xf7q#\x92b\x91S\x13\xd2\xfb\x8e\x19\xb10K@\x1a\x126\x8c\xe4X\xa6\x0c\xaf\x90\x04L\xc3H\x0eV\xa3}\\\xd9\xa1\x99{FE>\x896\x88\x1ca\xf5\x16A+f\x06\xbc\xed\x8b\x87\x0c\xa9\x8eV>*\xfeEa\x1b\xd2\x1b\xf6\xea\x0c\x8b\xfd\xf6*\xf0\xc7\x15y\x9b\xb4\xb6\xc0rL}\x89\xa5\xe0WW\x12\x80r.y8\x86\xddi\xdf%>*,\x88\xe1\x13\x1f_s\x9bRR#a\x9fB&\x16 \xd1\xb1\xc0D\xdc\x7f\x8a\x8a\xf5L\ti\x91\xa8\xc9.\x991\x07I\xdb\xb5|\xa0\xc0\xa0\\\x12\xd9\xed\xb9\xa31P\x1d\x84\x840H\xd6\xfd0S\x8f\xe2\xcd\x05\xb2\xaa\x12Jl(\x0b\xcd6\xc331\xc9%\x0c\xa8\x06\xb5\x93!#|\x13+\xfe\x9b\x15\x17\xb6%d\x84\x87\x11\xd0\xb8!\xae\xab\x1a\xe2\xe6\xf6T\xbe\x18\xe1\\e\xffF\xf0\xf7\xea?\xac,B\xa4\x10o\xdf\xa1*\xb6\'\x12>\xeaF\xf9=\xde\t\xf3\xc1%\x1b\xc75\x82|\xa26\x9d\xc3\xc87\xaa\x95\xc7s\xd4.\x0f\x0fU\t?\x89X\x91\x90\xb4uX\xb9\xa9\xaf\x12\x8e\xd7\x86\xbd:\x08\xff\xb2q\x82\xfe\xe94f~\xb1-\x87 \xce2\xbe\xe2\x91\x13\xbd\xbb\x02\xc9\xde\x13\xeft\x99\xfc \xb7#\x1a\xdbSX\xea\xc4\x8c\xaf\x06i P\xf4\xfcw\x11\x80\x07\xc3\x8f\x06\xe64=J\xa0q\x02\x8f\xedo%\x98O%h\x1cj\xfah\xd4\xdaBc\n(\x1e\x85\xca\xeb\xfc;\xd1NI\x19\xfeS\xe1\xd5\xc4RM\x9fl\r\x18\xba\xffy"\xf0\x85\xa4U\xb8\x1a6\xed\xcf\xe8\xa9kR\x99a\xd7\x82\x7f\x0c\xa1\xd29hL\xa3\x96\xf7P\x8a\xb2%9M\x13\xdb\x86\xb3\xd0x\xe0\x93c\xa9\xe8t\xec\x83\xe0\xe7\x7f\xf0{\xccli\xdb\x19\xaa\x7fM\xe5\xf1\x1e1\xb4\xab\x9c\xfbR\xe3\xb4\x17v\xde\x97\x13\xef\x98\x90\xe5N\x8e\x0cgWu\xf6\x17TZYU\x87\x12\xc5\x061\xa7\xc0\x07\xd7\xd1N\x8c\x07\xfa8d\x93\x002\x9a\x8d/\x04\xfa\xed\xeb\x17\xef\x10\xe9e\xb7\xc5\xc1\xafNX\xec\xd2\xcd\x07e\xd9\xeb\xf6.\xb2\xeb!$\x175\xd0\xf5\xa9&z\xa6\xb0\xec-?\xf0c*%y\xa9\xb2O\x92\x88R\xfa*\x89*\xf8tT\x95kb\xaf\xc8\xc4S\x11\x81\xb9\x968\x9a\xce\xcf\xa2\xaa\xf9\x1c\x01\x05\xf6\xef\x165\x15"\x90J*\x95\x1aK\x12\xa9\xb6\x1f\x83\xba\xb5\xc5}\xda,\xa4\xa6\xb7\xdb\x10\'\x10\x81V\x9d1\xe6\x00>\x17\xdeU\x05\xad\x93\x1d\x90\xe5\x1e~\xfa;\xd2\xe4\x06B\x85\x04\x96\xfd\x0et1\xf9\x8asbk\xb9\xbfy\x17\x1a\xd0I7\x7f\x1fK\x84\xff\xed\xeb\xfd%9RG/\xab@/\x8a\xa0\x87]T\xaet}&C|\xa1t\xd8\xfbK\\*Ws\xde\xdag\xb5j\x9a\xcc0.O\xd29p\x1d.\xea\\\x97p\xb5\xe3^\x05\x08RnYb-\x9b\xd71\xd9\xfb\xe5\xe7\xbe\x02/\x90?|\x95\xf9@\xc5\x12\x9epfK\x1e\x9c\x0f\xce\xc0_\xd8j.No\x98b<\xaeE$A\xa9\xac\xc2\xee\xad0Nl\x84\x0e\xf7T\x8e\x10\xa1R\xb9\x89\x8e4r\xa5[\xbar\x92\xf8p[63\xbd\x8b\x08\x0eb\x97lY\x89\xfcYJ\xc4\xa9(C\xde\xa9\x909@\xd5\x03\x1c\xb3\xb9\xd4d~\xd8\x7fX\xc9`\xa0\xfbVq\xa0\x14\xffLB[\xcc0\xd4,\xc9\\\x87T\xcc9\xdc\x05\n\xc8kA\xad\x05\xb3\x8dA\x0b&W3\x82\x10 q\xdc\xaeI\x91\x01\x1c5P\x118J\x8b\x95K\xd0\xff\x89\xf4\xe9v/\x13\xf4t~~\xbe\\\xff\xb8\xf8d\xb8\xc7\xdf\xad\x8dc5u\xd3\xd2\xcf6\xd0.\xd6by\xcb\x85\xc4\x1a>\xc6\xa7\x98\xc1\x15\xb6\xecH[\x01\xda\x0c\x9a\xb1\x17u\x899V\xba\x1f\xc6\x03\xaczo\x18f\x9b\x8b%\xc6\x14\xed\x1a\xf4\x0f\\k\xa3\xe8W|.!,\xf9\xe5Y\xf32\x0b\x9ayq\xe9\xff\x19?-p\x8c\xe09,$\x8a\xc9\xdd\x8a\x12\x1dH\xe9N\xa5\x06Cl\xa0j,\x81\x89\xb8\x9eFYi\xf9\x03\xcd\xe3\x8b\x89N\x16E\xf1\x84\xd25`\xd5\xbf\xad\xf4\xee\xfb\x9bu\xdcu\xf8\x9cL\x85,J\xa0\x90#\n#\x97\x90h\xea_r*\xc4L$x!4\x87f\x14\x81\xf7b\r\x81\x969\xf9n#\rO\x16\xbf}\r\xab\xc5\xbd\xdd\xd4\xa7\x1c\xf4\x8a[\xb2\xcf\xa1r\x1eR\xe2\xa5\x0e\x9b9\xd3l"\x8a\xf1\xf2\xca\xfb\xd7\x81\xdd\xa0\x9b\r\xd6f\xee\xb01d\x07W\xadxn\x9fBL\x06\xf6gK\x7f7\xd9\x9c\xb5aWp{Z\x8d\xec\xc9mv\xa8\xc8\xc4\xeb\xc2\xd7SE\xfd\x19\xbd\x19T\x8eJ\xc8\xf9*W7-\xb6\x8f\xd4\xc2#\xaav\\)\x80\xa7\x95~lP\x98\x98.\xdd\xd0-K\xc6I\xba;\x89L\xcd;\x92\xeb&\x061\xa0\xfb\xae\xf6\x00\xc8\x05\xa8\x1a\xe6G\xe5\x07\xf1\xb2\x0e\xd5\x8f\x9e\xbc\x85\x9d\x8cH\xe3\x90\x9ck\xee\x96\xd2\xae\xce4\x89\xc2\x0e\xff\xf0[\x15\xba| \xca\xf9\xa6}4[rd\xfd\xe2z\x9e\x8c\x88Jk\xf4\x08\xfe\x83\xf8\x937\xbf\xdcXH\x98>\xcb\x019~@^\x06\xae\xe4\xe3c\xc14\xe0\xea\x06Ko\xed\x8b\xbc\xf5\xefE\x85\x14\x07\xae\x8ey\xba \x88\xb4\x9c\x1feE\xaa\x7f\xd0\x02\xc0\xee\x13f\xdd}\xceJ\xf8\x89\x18\xda\x901sc\xb8\x0b\x84\xee\x15Z|\x8bg\\\xe8\xd1`\xd0k\x89\x1e\x075\x16\x87\x11\x02,\xca,\xccA\xd9\xfb~\xea4\xdd\xad\xc7\x94_\xe4N\xd4\x1b)\xb9\xe0|\xfd\xdf\xd5\x9c*q\xd1\x99\x1b\x01\x13\xf7\xe3\x88\xb0\x89\xb6%i\x8a8Q\x97FV\xa1\xa0UI\xe0\xbcM\xddii\xa8FTm\x88\xb1\x13=\xad\x80y\xb2\xf2\xf68jSzq[)\x0c\xbb\xfa\xdeO\xcd\xde[\x02\xa3\x0e\xcbb\x1f\xf9*rh\x9d\x1c(\xc6\x04\xf7\x94\x19\x08Rt\x08\xee\xe4E\xdb\xbc\xd5\xbe\x7f\xa7\x95o\x94\xe3\x08\x0f;\x10\x15\xe1"M\xde\xbe:<\xe2u%]\x18[v\x1a\xb7#\xfdwv/\xbd\x02"\xfdjO\x92\xcc\x84]\xff\x88\x08\xf9\x95\xde\x95\xb7\x13\x99\xbb\xfd\xcf\xb8\xbd)\xa6\xff\xda\xf4\xd6\xc6\\\xc3X\xe5\x93iI,$eu\x13\xb2E\x1bs\xa5\xc0Q\xc2\x96Z\xb3\xdcZi50\x80\xed!\x1aO\x8f\xb0\xec\x037V|F\x1e\xfb\xc0Ig\x929\x1dC\xed\x94U\x96gr\x13\x82\xdf\xf2\x1a\xa2\xeb\x02\x98e\xaaC\x87\xfbl:B\x02\xc2\xe2\xd3w\x9d\xb3\x9b\xcfD\xb5gaS\xbf\xce\xf0_\x0b\xc2\xfbY\xff\xaf\xc9\x8aJ\x1c\xc8 \x98K3\x18\xb3\x94\xd4j%\x99\xcb\xf5\xb9\xae\x03\xdf\x9a{\xfe\xc7e\xe3\x0bq~\xee\\BrK7\xd0]\xae\xec\x12\xca\xfcylR\xd7\x0b\x0c\n\x07\t\xf2\x0b\xe5;+0QG\xf6\x13\xb9$w\xf4N\xbd\x01\x8ehM\xdfJc\xbd\xe6 *\xa7\x9eB\xecK\x18\xa2\x9eY\xe9 \xc5\xdd^\xf9O\xf6\xf16k\xae\xfa4\xb9\xd4k\x10-\x14\xc0,\x19 \xf0\x0ed\x0evr\xd4X\xcf\xba\xb0\x1b|$c|V3\xefg/\x18Z\x10)\x01\x8e+\x063\x05\xb0\xfb\xb6\xf1VvP\xb7]\xdf\x8f\xdf\xc7\x04\xe4f\x93\x06\x13\xb2M\xc1\xd0?YQ!Zp\xf9\xf3D\xb1\xe9\xc4\xfah.l&[1\x9b\xc9/\xf4\x9e|\xe3\x06\xfa\xf8h\x15\xea<\xf5\xc6\xdef\x912(\xe2\x9e9\xdb" \xaf$\xdf\xccP\x17%0\x99\x83\xd70f>\x9f\n(,\x00\x99w\xee\xbd\x9aN\xe3\xff\xd75\xd98\xfb\xc6G\xa9$%\x98EV<\xbc}\xab\xb9\r\x81p\x1c\x0f!\x8c1\xbc\xc1$~\xc8ll\x89\xbf\xa4\xa0\x03\x82\xff\x1f\x91k:\x80K?\x120\x03\xfd\x8d\x1d\xfb%-\x86M~\x1fzJ\xdc#G\xbbU\x10\x84\x834o\xdd\xe5jt\x11\xd0\xf6\t\x12\xef\x8e\xc9\xa6\x8b\x95\x14z\xbcv\xe0\xff\xe8^\xea\x0b\x8d\xc5\x9c\x9e\x0bR\xb9\x8b\x0b\xfc`\xfb\x943^x^o\xd6\xe3\x1f\x7f')
|
collector/core/gripper/sightac_sdk/api_new/touch_sensor.py
ADDED
|
The diff for this file is too large to render.
See raw diff
|
|
|
collector/core/gripper/sightac_sdk/api_new/touch_sensor_base.py
ADDED
|
The diff for this file is too large to render.
See raw diff
|
|
|
collector/core/gripper/sightac_sdk/api_new/xu_camera.py
ADDED
|
The diff for this file is too large to render.
See raw diff
|
|
|
collector/core/gripper/sightac_sdk/pyarmor_runtime_000000/__init__.py
ADDED
|
@@ -0,0 +1,2 @@
|
|
|
|
|
|
|
|
|
|
| 1 |
+
# Pyarmor 8.5.12 (trial), 000000, 2026-09-09T18:33:48.325537
|
| 2 |
+
from .pyarmor_runtime import __pyarmor__
|
collector/core/gripper/sightac_sdk/pyarmor_runtime_000000/pyarmor_runtime.so
ADDED
|
@@ -0,0 +1,3 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
version https://git-lfs.github.com/spec/v1
|
| 2 |
+
oid sha256:6f8655b0125d80acc0d8bf62bc2bf97beae5f88d23f0b0b8f60c8a5473cd68ca
|
| 3 |
+
size 796488
|
collector/core/gripper/slam/__init__.py
ADDED
|
@@ -0,0 +1,12 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""core.gripper SLAM 子集:桥接进程控制器与 stdout 协议。
|
| 2 |
+
|
| 3 |
+
原版 barrel 引用的 frame_reader(/dev/shm 预览 jpg 读取)未复制;主程序
|
| 4 |
+
双目显示走 orb_raw_stream 原始流(recording.fays_raw_client)。
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from __future__ import annotations
|
| 8 |
+
|
| 9 |
+
from .process_controller import SlamProcessController
|
| 10 |
+
from .protocol import PoseSample
|
| 11 |
+
|
| 12 |
+
__all__ = ["PoseSample", "SlamProcessController"]
|
collector/core/gripper/slam/process_controller.py
ADDED
|
@@ -0,0 +1,1157 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Lifecycle owner for the FaysSense ORB-SLAM3 bridge process.
|
| 2 |
+
|
| 3 |
+
Lock order is ``_lifecycle_lock`` then ``SlamState._lock``. Stdout callbacks
|
| 4 |
+
are invoked without either lock. All terminate/kill operations are centralized
|
| 5 |
+
in this controller; callers only use ``start()``, ``send_origin()`` and
|
| 6 |
+
``stop()``.
|
| 7 |
+
"""
|
| 8 |
+
|
| 9 |
+
from dataclasses import dataclass, replace
|
| 10 |
+
import json
|
| 11 |
+
import os
|
| 12 |
+
import queue
|
| 13 |
+
import shutil
|
| 14 |
+
import socket
|
| 15 |
+
import subprocess
|
| 16 |
+
import threading
|
| 17 |
+
import time
|
| 18 |
+
import traceback
|
| 19 |
+
|
| 20 |
+
from core.gripper.runtime.device_access import device_access_guard, FAYS_SDK_INITIALIZATION_LOCK, fays_config_device_guard
|
| 21 |
+
|
| 22 |
+
from .protocol import (
|
| 23 |
+
PoseSample, parse_slam_line, pose_rejection_reason, quat_conjugate,
|
| 24 |
+
quat_product, rotate_pose_z90,
|
| 25 |
+
)
|
| 26 |
+
|
| 27 |
+
|
| 28 |
+
STDOUT_QUEUE_CAPACITY = 2000
|
| 29 |
+
NATIVE_LOG_FLUSH_INTERVAL_S = 0.1
|
| 30 |
+
NATIVE_LOG_FLUSH_LINES = 50
|
| 31 |
+
PROCESS_WAIT_TIMEOUT_S = 5.0
|
| 32 |
+
WORKER_JOIN_TIMEOUT_S = 2.0
|
| 33 |
+
STDOUT_JOIN_TIMEOUT_S = 1.0
|
| 34 |
+
FINAL_WAIT_TIMEOUT_S = 3.0
|
| 35 |
+
STALL_TIMEOUT_S = 5.0
|
| 36 |
+
MONITOR_INTERVAL_S = 1.0
|
| 37 |
+
RUNNING_REPORT_INTERVAL_S = 60.0 # 心跳行稀疏化,避免刷屏;卡死检测仍 5s
|
| 38 |
+
ALLOWED_NICE_ADJUSTMENTS = frozenset({0, 5})
|
| 39 |
+
|
| 40 |
+
|
| 41 |
+
@dataclass(frozen=True)
|
| 42 |
+
class SlamSnapshot:
|
| 43 |
+
running: bool
|
| 44 |
+
pid: object
|
| 45 |
+
status: str
|
| 46 |
+
raw_pose_fps: float
|
| 47 |
+
valid_pose_fps: float
|
| 48 |
+
last_frame_id: int
|
| 49 |
+
stalled: bool
|
| 50 |
+
error: object
|
| 51 |
+
stdout_backpressure_count: int
|
| 52 |
+
pose_lost_count: int
|
| 53 |
+
|
| 54 |
+
|
| 55 |
+
class SlamState:
|
| 56 |
+
"""Single mutable state source written only by ``SlamProcessController``."""
|
| 57 |
+
|
| 58 |
+
def __init__(self):
|
| 59 |
+
self._lock = threading.RLock()
|
| 60 |
+
self.process = None
|
| 61 |
+
self.running = False
|
| 62 |
+
self.status = "Disconnected"
|
| 63 |
+
self.raw_pose_fps = 0.0
|
| 64 |
+
self.valid_pose_fps = 0.0
|
| 65 |
+
self.last_frame_id = -1
|
| 66 |
+
self.stalled = False
|
| 67 |
+
self.error = None
|
| 68 |
+
self.stdout_backpressure_count = 0
|
| 69 |
+
self.pose_lost_count = 0
|
| 70 |
+
|
| 71 |
+
def snapshot(self):
|
| 72 |
+
with self._lock:
|
| 73 |
+
process = self.process
|
| 74 |
+
return SlamSnapshot(
|
| 75 |
+
running=self.running,
|
| 76 |
+
pid=getattr(process, "pid", None),
|
| 77 |
+
status=self.status,
|
| 78 |
+
raw_pose_fps=self.raw_pose_fps,
|
| 79 |
+
valid_pose_fps=self.valid_pose_fps,
|
| 80 |
+
last_frame_id=self.last_frame_id,
|
| 81 |
+
stalled=self.stalled,
|
| 82 |
+
error=self.error,
|
| 83 |
+
stdout_backpressure_count=self.stdout_backpressure_count,
|
| 84 |
+
pose_lost_count=self.pose_lost_count,
|
| 85 |
+
)
|
| 86 |
+
|
| 87 |
+
|
| 88 |
+
class SlamProcessController:
|
| 89 |
+
"""Start, parse, monitor and stop the single Fays bridge process."""
|
| 90 |
+
|
| 91 |
+
def __init__(
|
| 92 |
+
self,
|
| 93 |
+
state,
|
| 94 |
+
*,
|
| 95 |
+
executable,
|
| 96 |
+
vocabulary,
|
| 97 |
+
settings,
|
| 98 |
+
device_config,
|
| 99 |
+
trajectory,
|
| 100 |
+
work_dir,
|
| 101 |
+
runtime_env_factory,
|
| 102 |
+
cpu_roles,
|
| 103 |
+
on_pose=None,
|
| 104 |
+
on_fays_rates=None,
|
| 105 |
+
on_orb_diagnostic=None,
|
| 106 |
+
on_mp_cleanup=None,
|
| 107 |
+
on_status=None,
|
| 108 |
+
frame_snapshot=None,
|
| 109 |
+
log=None,
|
| 110 |
+
process_factory=subprocess.Popen,
|
| 111 |
+
sdk_device_guard_factory=fays_config_device_guard,
|
| 112 |
+
thread_factory=threading.Thread,
|
| 113 |
+
clock=time.monotonic,
|
| 114 |
+
wall_clock=time.time,
|
| 115 |
+
sleep=time.sleep,
|
| 116 |
+
taskset_finder=lambda: shutil.which("taskset"),
|
| 117 |
+
nice_finder=lambda: shutil.which("nice"),
|
| 118 |
+
stdbuf_finder=lambda: shutil.which("stdbuf"),
|
| 119 |
+
nice_adjustment=0,
|
| 120 |
+
set_affinity=os.sched_setaffinity,
|
| 121 |
+
get_affinity=os.sched_getaffinity,
|
| 122 |
+
on_process_stop=None,
|
| 123 |
+
auto_origin=True,
|
| 124 |
+
native_log_path=None,
|
| 125 |
+
pose_paths=(
|
| 126 |
+
"/dev/shm/orb_pose.json.tmp",
|
| 127 |
+
"/dev/shm/orb_pose.json",
|
| 128 |
+
),
|
| 129 |
+
):
|
| 130 |
+
self.state = state
|
| 131 |
+
self.executable = executable
|
| 132 |
+
self.vocabulary = vocabulary
|
| 133 |
+
self.settings = settings
|
| 134 |
+
self.device_config = device_config
|
| 135 |
+
self.trajectory = trajectory
|
| 136 |
+
self.work_dir = work_dir
|
| 137 |
+
self.runtime_env_factory = runtime_env_factory
|
| 138 |
+
self.cpu_roles = cpu_roles
|
| 139 |
+
self._on_pose = on_pose or (lambda _pose: None)
|
| 140 |
+
self._on_fays_rates = on_fays_rates or (lambda _rates: None)
|
| 141 |
+
self._on_orb_diagnostic = (
|
| 142 |
+
on_orb_diagnostic or (lambda _diagnostic: None)
|
| 143 |
+
)
|
| 144 |
+
self._on_mp_cleanup = on_mp_cleanup or (lambda _diagnostic: None)
|
| 145 |
+
self._on_status = on_status or (lambda _status: None)
|
| 146 |
+
self._frame_snapshot = frame_snapshot or (lambda: None)
|
| 147 |
+
self._log = log or print
|
| 148 |
+
self._process_factory = process_factory
|
| 149 |
+
self._sdk_device_guard_factory = sdk_device_guard_factory
|
| 150 |
+
self._thread_factory = thread_factory
|
| 151 |
+
self._clock = clock
|
| 152 |
+
self._wall_clock = wall_clock
|
| 153 |
+
self._sleep = sleep
|
| 154 |
+
self._taskset_finder = taskset_finder
|
| 155 |
+
self._nice_finder = nice_finder
|
| 156 |
+
self._stdbuf_finder = stdbuf_finder
|
| 157 |
+
self._nice_adjustment = int(nice_adjustment)
|
| 158 |
+
if self._nice_adjustment not in ALLOWED_NICE_ADJUSTMENTS:
|
| 159 |
+
raise ValueError(
|
| 160 |
+
"Fays nice adjustment must be 0 or 5")
|
| 161 |
+
self._set_affinity = set_affinity
|
| 162 |
+
self._get_affinity = get_affinity
|
| 163 |
+
self._on_process_stop = on_process_stop
|
| 164 |
+
self._auto_origin = bool(auto_origin)
|
| 165 |
+
self._native_log_path = native_log_path
|
| 166 |
+
self._pose_tmp_path, self._pose_path = pose_paths
|
| 167 |
+
self._backend = "orb"
|
| 168 |
+
self._runtime_arguments = None
|
| 169 |
+
self._service_container = None
|
| 170 |
+
self._service_host = None
|
| 171 |
+
self._service_port = None
|
| 172 |
+
|
| 173 |
+
self._lifecycle_lock = threading.RLock()
|
| 174 |
+
self._process_stop_lock = threading.Lock()
|
| 175 |
+
self._service_lock = threading.Lock()
|
| 176 |
+
self._service_active = False
|
| 177 |
+
self._stop_event = threading.Event()
|
| 178 |
+
self._worker_thread = None
|
| 179 |
+
self._monitor_thread = None
|
| 180 |
+
self._generation = 0
|
| 181 |
+
self._fake_imu = False
|
| 182 |
+
self._origin_sent_pids = set()
|
| 183 |
+
self._initial_origin_callback = None
|
| 184 |
+
self._initial_origin_generation = None
|
| 185 |
+
self._pose_buffer = {}
|
| 186 |
+
self._pose_buffer_lock = threading.Lock()
|
| 187 |
+
self._last_accepted_pose = None
|
| 188 |
+
self._pose_rejecting = False
|
| 189 |
+
# 原点后姿态相对化:设原点事件后捕获首帧显示姿态作为基准,
|
| 190 |
+
# 此后所有姿态左乘基准的逆 → 第一帧姿态恒为单位矩阵,
|
| 191 |
+
# 帧轴与轨迹同基准(相对原点),见 _publish_pose。
|
| 192 |
+
self._pose_rotation_zero = None
|
| 193 |
+
self._pose_rotation_zero_pending = True
|
| 194 |
+
self._last_pose_reject_log = float("-inf")
|
| 195 |
+
self._unmatched_lines = 0
|
| 196 |
+
self._started_event = threading.Event()
|
| 197 |
+
self._sdk_ready_event = threading.Event()
|
| 198 |
+
self._sdk_initialization_done = threading.Event()
|
| 199 |
+
self._pose_raw_seq = 0
|
| 200 |
+
self._pose_lost_count = 0
|
| 201 |
+
self._last_accepted_seq = 0
|
| 202 |
+
self._latest_pose = None
|
| 203 |
+
self._latest_pose_lock = threading.Lock()
|
| 204 |
+
|
| 205 |
+
def wait_sdk_ready(self, timeout=45.0):
|
| 206 |
+
"""Wait for SDK + factory calibration, not SLAM tracking/origin."""
|
| 207 |
+
if self._backend != "orb":
|
| 208 |
+
return self.wait_started(timeout)
|
| 209 |
+
if not self._sdk_initialization_done.wait(timeout):
|
| 210 |
+
return False
|
| 211 |
+
snapshot = self.state.snapshot()
|
| 212 |
+
return self._sdk_ready_event.is_set() and snapshot.running and not snapshot.error
|
| 213 |
+
|
| 214 |
+
@property
|
| 215 |
+
def running(self):
|
| 216 |
+
return self.state.snapshot().running
|
| 217 |
+
|
| 218 |
+
@property
|
| 219 |
+
def backend(self):
|
| 220 |
+
return self._backend
|
| 221 |
+
|
| 222 |
+
def set_backend(self, backend):
|
| 223 |
+
normalized = str(backend or "").strip().lower()
|
| 224 |
+
if normalized not in {"orb", "aikit"}:
|
| 225 |
+
raise ValueError(f"unsupported localization backend: {backend!r}")
|
| 226 |
+
with self._lifecycle_lock:
|
| 227 |
+
if self.running:
|
| 228 |
+
raise RuntimeError(
|
| 229 |
+
"cannot change localization backend while SLAM is running"
|
| 230 |
+
)
|
| 231 |
+
self._backend = normalized
|
| 232 |
+
self._runtime_arguments = None
|
| 233 |
+
self._service_container = None
|
| 234 |
+
self._service_host = None
|
| 235 |
+
self._service_port = None
|
| 236 |
+
self._log(f"[SLAM] Localization backend selected: {normalized}")
|
| 237 |
+
|
| 238 |
+
def snapshot(self):
|
| 239 |
+
return self.state.snapshot()
|
| 240 |
+
|
| 241 |
+
def pose_buffer_snapshot(self):
|
| 242 |
+
with self._pose_buffer_lock:
|
| 243 |
+
return dict(self._pose_buffer)
|
| 244 |
+
|
| 245 |
+
def clear_pose_buffer(self):
|
| 246 |
+
with self._pose_buffer_lock:
|
| 247 |
+
self._pose_buffer.clear()
|
| 248 |
+
|
| 249 |
+
def read_latest_pose(self):
|
| 250 |
+
"""Single-slot non-blocking read of the most recent accepted pose.
|
| 251 |
+
|
| 252 |
+
Returns ``(pose, raw_seq, lost_count)`` or ``(None, 0, 0)`` when
|
| 253 |
+
no pose has been accepted yet. This is the dedicated control/GUI
|
| 254 |
+
channel — it always returns the freshest sample without queue
|
| 255 |
+
contention.
|
| 256 |
+
"""
|
| 257 |
+
with self._latest_pose_lock:
|
| 258 |
+
return (
|
| 259 |
+
self._latest_pose,
|
| 260 |
+
self._pose_raw_seq,
|
| 261 |
+
self._pose_lost_count,
|
| 262 |
+
)
|
| 263 |
+
|
| 264 |
+
def pose_lost_count(self):
|
| 265 |
+
with self._latest_pose_lock:
|
| 266 |
+
return self._pose_lost_count
|
| 267 |
+
|
| 268 |
+
def _set_state(self, **changes):
|
| 269 |
+
with self.state._lock:
|
| 270 |
+
for name, value in changes.items():
|
| 271 |
+
setattr(self.state, name, value)
|
| 272 |
+
status = self.state.status
|
| 273 |
+
if "status" in changes:
|
| 274 |
+
self._on_status(status)
|
| 275 |
+
|
| 276 |
+
def start(self, generation=0, fake_imu=False):
|
| 277 |
+
"""Start one worker. Existing process ownership is released first."""
|
| 278 |
+
self.stop()
|
| 279 |
+
with self._lifecycle_lock:
|
| 280 |
+
self._generation = generation
|
| 281 |
+
self._fake_imu = bool(fake_imu)
|
| 282 |
+
self._initial_origin_generation = None
|
| 283 |
+
self._last_accepted_pose = None
|
| 284 |
+
self._pose_rejecting = False
|
| 285 |
+
self._last_pose_reject_log = float("-inf")
|
| 286 |
+
self._pose_raw_seq = 0
|
| 287 |
+
self._pose_lost_count = 0
|
| 288 |
+
self._last_accepted_seq = 0
|
| 289 |
+
with self._latest_pose_lock:
|
| 290 |
+
self._latest_pose = None
|
| 291 |
+
self._stop_event.clear()
|
| 292 |
+
self._started_event.clear()
|
| 293 |
+
self._sdk_ready_event.clear()
|
| 294 |
+
self._sdk_initialization_done.clear()
|
| 295 |
+
self._set_state(
|
| 296 |
+
running=True,
|
| 297 |
+
process=None,
|
| 298 |
+
status="Starting",
|
| 299 |
+
raw_pose_fps=0.0,
|
| 300 |
+
valid_pose_fps=0.0,
|
| 301 |
+
last_frame_id=-1,
|
| 302 |
+
stalled=False,
|
| 303 |
+
error=None,
|
| 304 |
+
stdout_backpressure_count=0,
|
| 305 |
+
pose_lost_count=0,
|
| 306 |
+
)
|
| 307 |
+
worker = self._thread_factory(
|
| 308 |
+
target=self._run,
|
| 309 |
+
args=(generation,),
|
| 310 |
+
name="slam-supervisor",
|
| 311 |
+
daemon=True,
|
| 312 |
+
)
|
| 313 |
+
monitor = self._thread_factory(
|
| 314 |
+
target=self._monitor_frames,
|
| 315 |
+
args=(generation,),
|
| 316 |
+
name="slam-monitor",
|
| 317 |
+
daemon=True,
|
| 318 |
+
)
|
| 319 |
+
self._worker_thread = worker
|
| 320 |
+
self._monitor_thread = monitor
|
| 321 |
+
worker.start()
|
| 322 |
+
monitor.start()
|
| 323 |
+
return True
|
| 324 |
+
|
| 325 |
+
def set_initial_origin_callback(self, callback):
|
| 326 |
+
"""Register the lifecycle continuation invoked after READY newline."""
|
| 327 |
+
self._initial_origin_callback = callback
|
| 328 |
+
|
| 329 |
+
def initial_origin_sent(self):
|
| 330 |
+
return self._initial_origin_generation == self._generation
|
| 331 |
+
|
| 332 |
+
def wait_started(self, timeout=10.0):
|
| 333 |
+
return self._started_event.wait(timeout)
|
| 334 |
+
|
| 335 |
+
@property
|
| 336 |
+
def nice_adjustment(self):
|
| 337 |
+
return self._nice_adjustment
|
| 338 |
+
|
| 339 |
+
def set_nice_adjustment(self, adjustment):
|
| 340 |
+
target = int(adjustment)
|
| 341 |
+
if target not in ALLOWED_NICE_ADJUSTMENTS:
|
| 342 |
+
raise ValueError("Fays nice adjustment must be 0 or 5")
|
| 343 |
+
if self.running:
|
| 344 |
+
raise RuntimeError(
|
| 345 |
+
"cannot change Fays nice adjustment while SLAM is running")
|
| 346 |
+
self._nice_adjustment = target
|
| 347 |
+
|
| 348 |
+
def configure_runtime(
|
| 349 |
+
self, *, executable, settings, calibration_serial=None,
|
| 350 |
+
backend="orb", arguments=None, service_container=None,
|
| 351 |
+
service_host=None, service_port=None,
|
| 352 |
+
):
|
| 353 |
+
"""Select one localization bridge/config pair before process start."""
|
| 354 |
+
|
| 355 |
+
with self._lifecycle_lock:
|
| 356 |
+
with self.state._lock:
|
| 357 |
+
process = self.state.process
|
| 358 |
+
active = bool(
|
| 359 |
+
self.state.running
|
| 360 |
+
or (
|
| 361 |
+
process is not None
|
| 362 |
+
and process.poll() is None
|
| 363 |
+
)
|
| 364 |
+
)
|
| 365 |
+
if active:
|
| 366 |
+
raise RuntimeError(
|
| 367 |
+
"cannot change Fays calibration while SLAM is running"
|
| 368 |
+
)
|
| 369 |
+
executable_path = os.path.abspath(os.fspath(executable))
|
| 370 |
+
settings_path = os.path.abspath(os.fspath(settings))
|
| 371 |
+
if not os.path.isfile(settings_path):
|
| 372 |
+
raise RuntimeError(
|
| 373 |
+
"Fays 定位配置不存在,拒绝启动: "
|
| 374 |
+
f"{settings_path}"
|
| 375 |
+
)
|
| 376 |
+
if not os.path.isfile(executable_path):
|
| 377 |
+
raise RuntimeError(
|
| 378 |
+
"Fays 定位二进制不存在,拒绝启动: "
|
| 379 |
+
f"{executable_path}"
|
| 380 |
+
)
|
| 381 |
+
if not os.access(executable_path, os.X_OK):
|
| 382 |
+
raise RuntimeError(
|
| 383 |
+
"Fays 定位二进制不可执行,拒绝启动: "
|
| 384 |
+
f"{executable_path}"
|
| 385 |
+
)
|
| 386 |
+
self.executable = executable_path
|
| 387 |
+
self.settings = settings_path
|
| 388 |
+
normalized_backend = str(backend or "orb").strip().lower()
|
| 389 |
+
if normalized_backend not in {"orb", "aikit"}:
|
| 390 |
+
raise ValueError(
|
| 391 |
+
f"unsupported localization backend: {backend!r}"
|
| 392 |
+
)
|
| 393 |
+
self._backend = normalized_backend
|
| 394 |
+
self._runtime_arguments = (
|
| 395 |
+
None if arguments is None
|
| 396 |
+
else tuple(os.fspath(value) for value in arguments)
|
| 397 |
+
)
|
| 398 |
+
self._service_container = (
|
| 399 |
+
str(service_container).strip()
|
| 400 |
+
if service_container else None
|
| 401 |
+
)
|
| 402 |
+
self._service_host = (
|
| 403 |
+
str(service_host).strip() if service_host else None
|
| 404 |
+
)
|
| 405 |
+
self._service_port = (
|
| 406 |
+
int(service_port) if service_port is not None else None
|
| 407 |
+
)
|
| 408 |
+
serial = str(calibration_serial or "").strip()
|
| 409 |
+
if serial:
|
| 410 |
+
self._log(
|
| 411 |
+
"[SLAM] Runtime profile selected: "
|
| 412 |
+
f"backend={self._backend} serial={serial} "
|
| 413 |
+
f"settings={self.settings}"
|
| 414 |
+
)
|
| 415 |
+
|
| 416 |
+
def _build_launch(self):
|
| 417 |
+
taskset = self._taskset_finder()
|
| 418 |
+
if not taskset:
|
| 419 |
+
raise RuntimeError(
|
| 420 |
+
"taskset is required for Fays CPU9 background isolation"
|
| 421 |
+
)
|
| 422 |
+
background = tuple(self.cpu_roles["fays_background"])
|
| 423 |
+
if not background:
|
| 424 |
+
raise RuntimeError("Fays background CPU role is empty")
|
| 425 |
+
launch_cpus = tuple(sorted({
|
| 426 |
+
cpu
|
| 427 |
+
for role in (
|
| 428 |
+
"fays_input",
|
| 429 |
+
"fays_prepare",
|
| 430 |
+
"fays_left_orb",
|
| 431 |
+
"fays_right_orb",
|
| 432 |
+
"fays_track",
|
| 433 |
+
"fays_background",
|
| 434 |
+
)
|
| 435 |
+
for cpu in self.cpu_roles.get(role, ())
|
| 436 |
+
}))
|
| 437 |
+
if not launch_cpus:
|
| 438 |
+
raise RuntimeError("Fays ORB process CPU domain is empty")
|
| 439 |
+
if self._runtime_arguments is None:
|
| 440 |
+
if self._backend != "orb":
|
| 441 |
+
raise RuntimeError(
|
| 442 |
+
"AIKit runtime arguments were not configured"
|
| 443 |
+
)
|
| 444 |
+
arguments = (
|
| 445 |
+
self.vocabulary,
|
| 446 |
+
self.settings,
|
| 447 |
+
self.device_config,
|
| 448 |
+
self.trajectory,
|
| 449 |
+
)
|
| 450 |
+
else:
|
| 451 |
+
arguments = self._runtime_arguments
|
| 452 |
+
command = [self.executable, *arguments]
|
| 453 |
+
stdbuf = self._stdbuf_finder()
|
| 454 |
+
if not stdbuf:
|
| 455 |
+
raise RuntimeError(
|
| 456 |
+
"stdbuf is required for line-buffered Fays stdout"
|
| 457 |
+
)
|
| 458 |
+
command = [stdbuf, "-oL", "-eL", *command]
|
| 459 |
+
cpu_spec = ",".join(str(cpu) for cpu in launch_cpus)
|
| 460 |
+
launch_command = [taskset, "--cpu-list", cpu_spec, *command]
|
| 461 |
+
if self._nice_adjustment:
|
| 462 |
+
nice = self._nice_finder()
|
| 463 |
+
if not nice:
|
| 464 |
+
raise RuntimeError(
|
| 465 |
+
"nice is required for the dual-device Fays profile")
|
| 466 |
+
launch_command = [
|
| 467 |
+
nice,
|
| 468 |
+
"-n",
|
| 469 |
+
str(self._nice_adjustment),
|
| 470 |
+
*launch_command,
|
| 471 |
+
]
|
| 472 |
+
environment = self.runtime_env_factory()
|
| 473 |
+
for name in (
|
| 474 |
+
"GOMP_CPU_AFFINITY",
|
| 475 |
+
"OMP_PLACES",
|
| 476 |
+
"KMP_AFFINITY",
|
| 477 |
+
"KMP_HW_SUBSET",
|
| 478 |
+
"KMP_PLACE_THREADS",
|
| 479 |
+
):
|
| 480 |
+
environment.pop(name, None)
|
| 481 |
+
environment.update({
|
| 482 |
+
"OMP_PROC_BIND": "FALSE",
|
| 483 |
+
"OMP_DYNAMIC": "FALSE",
|
| 484 |
+
"OMP_NUM_THREADS": str(len(launch_cpus)),
|
| 485 |
+
"KSQ_FAYS_INPUT_CPU": str(
|
| 486 |
+
(self.cpu_roles.get("fays_input") or (4,))[0]
|
| 487 |
+
),
|
| 488 |
+
"KSQ_FAYS_PREPARE_CPU": str(
|
| 489 |
+
(self.cpu_roles.get("fays_prepare") or (5,))[0]
|
| 490 |
+
),
|
| 491 |
+
"KSQ_FAYS_TRACK_CPU": str(
|
| 492 |
+
(self.cpu_roles.get("fays_track") or (8,))[0]
|
| 493 |
+
),
|
| 494 |
+
"KSQ_FAYS_BACKGROUND_CPU": str(
|
| 495 |
+
(self.cpu_roles.get("fays_background") or (9,))[0]
|
| 496 |
+
),
|
| 497 |
+
"KSQ_FAYS_ORB_LEFT_CPU": str(
|
| 498 |
+
(self.cpu_roles.get("fays_left_orb") or (6,))[0]
|
| 499 |
+
),
|
| 500 |
+
"KSQ_FAYS_ORB_RIGHT_CPU": str(
|
| 501 |
+
(self.cpu_roles.get("fays_right_orb") or (7,))[0]
|
| 502 |
+
),
|
| 503 |
+
})
|
| 504 |
+
# Connect-no-ESP requests fake IMU only from the ORB bridge. AIKit is
|
| 505 |
+
# still allowed in that UI mode and continues to consume the real S80M
|
| 506 |
+
# IMU inside its service container.
|
| 507 |
+
if self._fake_imu and self._backend == "orb":
|
| 508 |
+
environment["KSQ_FAYS_FAKE_IMU"] = "1"
|
| 509 |
+
else:
|
| 510 |
+
environment.pop("KSQ_FAYS_FAKE_IMU", None)
|
| 511 |
+
# ASan diagnostic builds must load the sanitizer runtime before any
|
| 512 |
+
# instrumented library; the linker cannot guarantee DT_NEEDED order
|
| 513 |
+
# across the vendor libraries, so preload it explicitly. Inactive for
|
| 514 |
+
# every normal binary name.
|
| 515 |
+
if "_asan" in self.executable:
|
| 516 |
+
libasan = "/lib/x86_64-linux-gnu/libasan.so.5"
|
| 517 |
+
if os.path.exists(libasan):
|
| 518 |
+
environment["LD_PRELOAD"] = libasan + (
|
| 519 |
+
os.pathsep + environment["LD_PRELOAD"]
|
| 520 |
+
if environment.get("LD_PRELOAD") else "")
|
| 521 |
+
return launch_command, environment, launch_cpus
|
| 522 |
+
|
| 523 |
+
def _docker_command(self, *arguments, check=True):
|
| 524 |
+
docker = shutil.which("docker")
|
| 525 |
+
if not docker:
|
| 526 |
+
raise RuntimeError("docker is required for AIKit mode")
|
| 527 |
+
result = subprocess.run(
|
| 528 |
+
[docker, *arguments],
|
| 529 |
+
stdin=subprocess.DEVNULL,
|
| 530 |
+
stdout=subprocess.PIPE,
|
| 531 |
+
stderr=subprocess.STDOUT,
|
| 532 |
+
text=True,
|
| 533 |
+
check=False,
|
| 534 |
+
)
|
| 535 |
+
if check and result.returncode != 0:
|
| 536 |
+
detail = result.stdout.strip() or f"exit={result.returncode}"
|
| 537 |
+
raise RuntimeError(
|
| 538 |
+
f"docker {' '.join(arguments)} failed: {detail}"
|
| 539 |
+
)
|
| 540 |
+
return result
|
| 541 |
+
|
| 542 |
+
def _start_aikit_service(self):
|
| 543 |
+
if self._backend != "aikit":
|
| 544 |
+
return False
|
| 545 |
+
if not (
|
| 546 |
+
self._service_container
|
| 547 |
+
and self._service_host
|
| 548 |
+
and self._service_port
|
| 549 |
+
):
|
| 550 |
+
raise RuntimeError("AIKit service profile is incomplete")
|
| 551 |
+
with self._service_lock:
|
| 552 |
+
self._docker_command("inspect", self._service_container)
|
| 553 |
+
running = self._docker_command(
|
| 554 |
+
"inspect", "--format", "{{.State.Running}}",
|
| 555 |
+
self._service_container,
|
| 556 |
+
).stdout.strip().lower() == "true"
|
| 557 |
+
if running:
|
| 558 |
+
# The device calibration and volatile /dev/video nodes are
|
| 559 |
+
# selected immediately before this call. Restart a lingering
|
| 560 |
+
# service so it cannot keep the previous device configuration.
|
| 561 |
+
self._log(
|
| 562 |
+
"[AIKIT_SERVICE] Reloading current device configuration"
|
| 563 |
+
)
|
| 564 |
+
stopped = self._docker_command(
|
| 565 |
+
"stop", "--time", "5", self._service_container,
|
| 566 |
+
check=False,
|
| 567 |
+
)
|
| 568 |
+
if stopped.returncode != 0:
|
| 569 |
+
raise RuntimeError(
|
| 570 |
+
"AIKit service could not reload configuration: "
|
| 571 |
+
+ stopped.stdout.strip()
|
| 572 |
+
)
|
| 573 |
+
self._log(
|
| 574 |
+
"[AIKIT_SERVICE] Starting container="
|
| 575 |
+
+ self._service_container
|
| 576 |
+
)
|
| 577 |
+
self._docker_command("start", self._service_container)
|
| 578 |
+
self._service_active = True
|
| 579 |
+
deadline = self._clock() + 20.0
|
| 580 |
+
while not self._stop_event.is_set() and self._clock() < deadline:
|
| 581 |
+
try:
|
| 582 |
+
with socket.create_connection(
|
| 583 |
+
(self._service_host, self._service_port), timeout=0.5
|
| 584 |
+
):
|
| 585 |
+
self._log(
|
| 586 |
+
"[AIKIT_SERVICE] Ready address="
|
| 587 |
+
f"{self._service_host}:{self._service_port}"
|
| 588 |
+
)
|
| 589 |
+
return True
|
| 590 |
+
except OSError:
|
| 591 |
+
self._sleep(0.2)
|
| 592 |
+
logs = self._docker_command(
|
| 593 |
+
"logs", "--tail", "40", self._service_container,
|
| 594 |
+
check=False,
|
| 595 |
+
).stdout.strip()
|
| 596 |
+
raise RuntimeError(
|
| 597 |
+
"AIKit service did not become ready: " + logs[-2000:]
|
| 598 |
+
)
|
| 599 |
+
|
| 600 |
+
def _stop_aikit_service(self):
|
| 601 |
+
with self._service_lock:
|
| 602 |
+
if not self._service_active or not self._service_container:
|
| 603 |
+
return
|
| 604 |
+
self._service_active = False
|
| 605 |
+
result = self._docker_command(
|
| 606 |
+
"stop", "--time", "5", self._service_container,
|
| 607 |
+
check=False,
|
| 608 |
+
)
|
| 609 |
+
if result.returncode != 0:
|
| 610 |
+
self._log(
|
| 611 |
+
"[AIKIT_SERVICE] Stop failed: " + result.stdout.strip()
|
| 612 |
+
)
|
| 613 |
+
else:
|
| 614 |
+
self._log("[AIKIT_SERVICE] Stopped")
|
| 615 |
+
|
| 616 |
+
def _drain_stdout(
|
| 617 |
+
self, process, line_queue, drain_stop, native_log=None,
|
| 618 |
+
):
|
| 619 |
+
log_buffer = []
|
| 620 |
+
last_flush = time.monotonic()
|
| 621 |
+
try:
|
| 622 |
+
for line in process.stdout:
|
| 623 |
+
if drain_stop.is_set():
|
| 624 |
+
break
|
| 625 |
+
arrival_monotonic = self._clock()
|
| 626 |
+
arrival_wall_time = self._wall_clock()
|
| 627 |
+
if native_log is not None:
|
| 628 |
+
log_buffer.append(json.dumps({
|
| 629 |
+
"time": arrival_wall_time,
|
| 630 |
+
"arrival_monotonic": arrival_monotonic,
|
| 631 |
+
"line": line.rstrip("\n"),
|
| 632 |
+
}, ensure_ascii=False) + "\n")
|
| 633 |
+
now = time.monotonic()
|
| 634 |
+
if (
|
| 635 |
+
len(log_buffer) >= NATIVE_LOG_FLUSH_LINES
|
| 636 |
+
or now - last_flush >= NATIVE_LOG_FLUSH_INTERVAL_S
|
| 637 |
+
):
|
| 638 |
+
try:
|
| 639 |
+
native_log.writelines(log_buffer)
|
| 640 |
+
native_log.flush()
|
| 641 |
+
except OSError:
|
| 642 |
+
pass
|
| 643 |
+
log_buffer.clear()
|
| 644 |
+
last_flush = now
|
| 645 |
+
item = (line, arrival_monotonic, arrival_wall_time)
|
| 646 |
+
while not drain_stop.is_set():
|
| 647 |
+
try:
|
| 648 |
+
line_queue.put(item, timeout=0.1)
|
| 649 |
+
break
|
| 650 |
+
except queue.Full:
|
| 651 |
+
with self.state._lock:
|
| 652 |
+
self.state.stdout_backpressure_count += 1
|
| 653 |
+
except Exception:
|
| 654 |
+
pass
|
| 655 |
+
finally:
|
| 656 |
+
if native_log is not None and log_buffer:
|
| 657 |
+
try:
|
| 658 |
+
native_log.writelines(log_buffer)
|
| 659 |
+
native_log.flush()
|
| 660 |
+
except OSError:
|
| 661 |
+
pass
|
| 662 |
+
|
| 663 |
+
def _log_native_tail(self):
|
| 664 |
+
"""进程意外退出时把原生 stdout 日志最后几行打进应用日志。
|
| 665 |
+
|
| 666 |
+
SIGSEGV 类崩溃无回溯可打,最后的输出行是唯一现场;drain 线程
|
| 667 |
+
可能尚未完成最终 flush,稍等再读。
|
| 668 |
+
"""
|
| 669 |
+
path = self._native_log_path
|
| 670 |
+
if not path:
|
| 671 |
+
return
|
| 672 |
+
try:
|
| 673 |
+
time.sleep(0.3)
|
| 674 |
+
with open(path, encoding="utf-8", errors="replace") as stream:
|
| 675 |
+
lines = stream.readlines()
|
| 676 |
+
if lines:
|
| 677 |
+
self._log("[SLAM] 原生进程最后输出 ({} 行):".format(
|
| 678 |
+
len(lines)))
|
| 679 |
+
for line in lines[-15:]:
|
| 680 |
+
self._log("[SLAM-NATIVE] " + line.rstrip()[:240])
|
| 681 |
+
except OSError as exc:
|
| 682 |
+
self._log("[SLAM] 读取原生日志失败: {}".format(exc))
|
| 683 |
+
|
| 684 |
+
def _publish_pose(self, pose):
|
| 685 |
+
# S80M 坐标约定(正对=+Y、Z=上、X=右):见
|
| 686 |
+
# protocol.rotate_pose_z90(真机实测定案,位置与姿态统一
|
| 687 |
+
# 纯左乘 R_z(+90°),新 ORB 核心后 native=X=正对 Y=左 Z=上)。
|
| 688 |
+
# 此处是位姿唯一发布入口,UI/落盘/pose.json/回填缓冲全部统一旋转。
|
| 689 |
+
pose = rotate_pose_z90(pose)
|
| 690 |
+
# 原点后姿态相对化:C++ 设原点只把位置归零,静止首帧的姿态
|
| 691 |
+
# 仍带部署校正/启动朝向的常数旋转(旋转跳变的根源,两轮
|
| 692 |
+
# 变换形式修正都没能抵消它)。改为捕获首帧显示姿态为基准,
|
| 693 |
+
# 全部姿态左乘基准的逆 → 首帧姿态恒为单位矩阵(帧轴与世界
|
| 694 |
+
# 轴对齐,不跳变),此后姿态 = 相对原点的真实转动,与轨迹
|
| 695 |
+
# (位置相对原点)同基准。位置不动,保持已验证的轨迹映射。
|
| 696 |
+
if self._pose_rotation_zero_pending:
|
| 697 |
+
self._pose_rotation_zero = pose.rotation
|
| 698 |
+
self._pose_rotation_zero_pending = False
|
| 699 |
+
if self._pose_rotation_zero is not None:
|
| 700 |
+
pose = replace(
|
| 701 |
+
pose,
|
| 702 |
+
rotation=quat_product(
|
| 703 |
+
quat_conjugate(self._pose_rotation_zero),
|
| 704 |
+
pose.rotation,
|
| 705 |
+
),
|
| 706 |
+
)
|
| 707 |
+
self._on_pose(pose)
|
| 708 |
+
timestamp = pose.timestamp
|
| 709 |
+
x, y, z = pose.position
|
| 710 |
+
qx, qy, qz, qw = pose.rotation
|
| 711 |
+
with self._pose_buffer_lock:
|
| 712 |
+
self._pose_buffer[timestamp] = (
|
| 713 |
+
x, y, z, qx, qy, qz, qw
|
| 714 |
+
)
|
| 715 |
+
cutoff = timestamp - 2.0
|
| 716 |
+
for key in [
|
| 717 |
+
key for key in self._pose_buffer if key < cutoff
|
| 718 |
+
]:
|
| 719 |
+
del self._pose_buffer[key]
|
| 720 |
+
if not self._pose_tmp_path or not self._pose_path:
|
| 721 |
+
return
|
| 722 |
+
try:
|
| 723 |
+
encoded = json.dumps({
|
| 724 |
+
"ts": timestamp,
|
| 725 |
+
"x": x,
|
| 726 |
+
"y": y,
|
| 727 |
+
"z": z,
|
| 728 |
+
"qx": qx,
|
| 729 |
+
"qy": qy,
|
| 730 |
+
"qz": qz,
|
| 731 |
+
"qw": qw,
|
| 732 |
+
})
|
| 733 |
+
with open(self._pose_tmp_path, "w", encoding="utf-8") as stream:
|
| 734 |
+
stream.write(encoded)
|
| 735 |
+
os.replace(self._pose_tmp_path, self._pose_path)
|
| 736 |
+
except OSError:
|
| 737 |
+
pass
|
| 738 |
+
|
| 739 |
+
def send_origin(self, reason="origin reset"):
|
| 740 |
+
"""Write the bridge's newline protocol; never use SIGUSR1."""
|
| 741 |
+
with self.state._lock:
|
| 742 |
+
process = self.state.process
|
| 743 |
+
if (
|
| 744 |
+
process is None
|
| 745 |
+
or process.poll() is not None
|
| 746 |
+
or process.stdin is None
|
| 747 |
+
):
|
| 748 |
+
return False
|
| 749 |
+
pid = process.pid
|
| 750 |
+
if reason == "initial origin" and pid in self._origin_sent_pids:
|
| 751 |
+
return True
|
| 752 |
+
try:
|
| 753 |
+
process.stdin.write("\n")
|
| 754 |
+
process.stdin.flush()
|
| 755 |
+
except (BrokenPipeError, OSError, ValueError) as error:
|
| 756 |
+
self._log(f"[SLAM] Failed to send {reason}: {error}")
|
| 757 |
+
return False
|
| 758 |
+
if reason == "initial origin":
|
| 759 |
+
self._origin_sent_pids.add(pid)
|
| 760 |
+
self._initial_origin_generation = self._generation
|
| 761 |
+
self._log("[SLAM] READY; automatic origin command sent")
|
| 762 |
+
callback = self._initial_origin_callback
|
| 763 |
+
if callback is not None:
|
| 764 |
+
callback(self._generation)
|
| 765 |
+
else:
|
| 766 |
+
self._log("[SLAM] Origin reset command sent")
|
| 767 |
+
return True
|
| 768 |
+
|
| 769 |
+
def _handle_event(
|
| 770 |
+
self, event, counters, arrival_monotonic=None,
|
| 771 |
+
arrival_wall_time=None,
|
| 772 |
+
):
|
| 773 |
+
if event.kind == "fays_rates":
|
| 774 |
+
self._on_fays_rates(event.payload)
|
| 775 |
+
return
|
| 776 |
+
if event.kind == "orb_diagnostic":
|
| 777 |
+
self._on_orb_diagnostic(event.payload)
|
| 778 |
+
return
|
| 779 |
+
if event.kind == "mp_cleanup":
|
| 780 |
+
self._on_mp_cleanup(event.payload)
|
| 781 |
+
return
|
| 782 |
+
# 周期诊断行(ORB_STAGE/FAYS-AFFINITY 每秒各一条)只在
|
| 783 |
+
# slam_stdout.log 全量留档,不打 GUI 日志避免刷屏;
|
| 784 |
+
# 崩溃取证由 _log_native_tail 读回。
|
| 785 |
+
if event.kind in ("orb_stage", "affinity"):
|
| 786 |
+
return
|
| 787 |
+
if event.kind == "ready":
|
| 788 |
+
self._set_state(status="READY - Press ENTER")
|
| 789 |
+
if self._auto_origin:
|
| 790 |
+
self.send_origin("initial origin")
|
| 791 |
+
else:
|
| 792 |
+
self._log(f"[SLAM] {event.raw}")
|
| 793 |
+
return
|
| 794 |
+
if event.kind == "origin":
|
| 795 |
+
self._last_accepted_pose = None
|
| 796 |
+
self._pose_rejecting = False
|
| 797 |
+
# 位置在原点帧被 C++ 归零,姿态却带着部署校正/启动朝向的
|
| 798 |
+
# 常数残留(上次跳变的根源);每个 origin 事件(含 NEW
|
| 799 |
+
# ORIGIN)都重新捕获首帧姿态基准,与位置重归零保持一致。
|
| 800 |
+
self._pose_rotation_zero = None
|
| 801 |
+
self._pose_rotation_zero_pending = True
|
| 802 |
+
self._set_state(status="Tracking")
|
| 803 |
+
self._log(f"[SLAM] {event.raw}")
|
| 804 |
+
return
|
| 805 |
+
if event.kind == "status":
|
| 806 |
+
self._log(f"[SLAM] {event.raw}")
|
| 807 |
+
return
|
| 808 |
+
if event.kind == "pose":
|
| 809 |
+
pose = event.payload
|
| 810 |
+
counters["raw"] += 1
|
| 811 |
+
self._pose_raw_seq += 1
|
| 812 |
+
pose = replace(
|
| 813 |
+
pose,
|
| 814 |
+
sequence=self._pose_raw_seq,
|
| 815 |
+
arrival_monotonic=arrival_monotonic,
|
| 816 |
+
arrival_wall_time=arrival_wall_time,
|
| 817 |
+
)
|
| 818 |
+
rejection = pose_rejection_reason(
|
| 819 |
+
pose, self._last_accepted_pose,
|
| 820 |
+
)
|
| 821 |
+
if rejection is not None:
|
| 822 |
+
now = self._wall_clock()
|
| 823 |
+
if (
|
| 824 |
+
not self._pose_rejecting
|
| 825 |
+
or now - self._last_pose_reject_log >= 1.0
|
| 826 |
+
):
|
| 827 |
+
self._log(f"[SLAM] Pose held: {rejection}")
|
| 828 |
+
self._last_pose_reject_log = now
|
| 829 |
+
if not self._pose_rejecting:
|
| 830 |
+
self._set_state(status="Tracking (pose held)")
|
| 831 |
+
self._pose_rejecting = True
|
| 832 |
+
return
|
| 833 |
+
if self._pose_rejecting:
|
| 834 |
+
self._log("[SLAM] Pose continuity recovered")
|
| 835 |
+
self._pose_rejecting = False
|
| 836 |
+
self._last_accepted_pose = pose
|
| 837 |
+
counters["valid"] += 1
|
| 838 |
+
self._publish_pose(pose)
|
| 839 |
+
with self._latest_pose_lock:
|
| 840 |
+
if self._last_accepted_seq:
|
| 841 |
+
gap = self._pose_raw_seq - self._last_accepted_seq - 1
|
| 842 |
+
if gap > 0:
|
| 843 |
+
self._pose_lost_count += gap
|
| 844 |
+
self._last_accepted_seq = self._pose_raw_seq
|
| 845 |
+
self._latest_pose = pose
|
| 846 |
+
lost_count = self._pose_lost_count
|
| 847 |
+
self._set_state(pose_lost_count=lost_count)
|
| 848 |
+
self._set_state(status="Tracking")
|
| 849 |
+
return
|
| 850 |
+
if event.kind == "error":
|
| 851 |
+
self._log(f"[SLAM] ⚠ {event.raw}")
|
| 852 |
+
self._set_state(
|
| 853 |
+
status=f"Error: {event.raw[:60]}",
|
| 854 |
+
error=event.raw,
|
| 855 |
+
)
|
| 856 |
+
return
|
| 857 |
+
self._unmatched_lines += 1
|
| 858 |
+
if self._unmatched_lines <= 50:
|
| 859 |
+
self._log(f"[SLAM] {event.raw}")
|
| 860 |
+
elif self._unmatched_lines == 51:
|
| 861 |
+
self._log("[SLAM] (suppressing further unmatched lines)")
|
| 862 |
+
|
| 863 |
+
def _run(self, generation):
|
| 864 |
+
sdk_guard = None
|
| 865 |
+
sdk_device_guard = None
|
| 866 |
+
sdk_deadline = None
|
| 867 |
+
process = None
|
| 868 |
+
drain_thread = None
|
| 869 |
+
drain_stop = threading.Event()
|
| 870 |
+
line_queue = queue.Queue(maxsize=STDOUT_QUEUE_CAPACITY)
|
| 871 |
+
native_log = None
|
| 872 |
+
counters = {"raw": 0, "valid": 0}
|
| 873 |
+
fps_started = self._wall_clock()
|
| 874 |
+
service_started = self._backend == "aikit"
|
| 875 |
+
try:
|
| 876 |
+
if service_started:
|
| 877 |
+
self._start_aikit_service()
|
| 878 |
+
launch_command, environment, launch_cpus = self._build_launch()
|
| 879 |
+
role_text = (
|
| 880 |
+
f"input={(self.cpu_roles.get('fays_input') or (4,))[0]}, "
|
| 881 |
+
f"prep={(self.cpu_roles.get('fays_prepare') or (5,))[0]}, "
|
| 882 |
+
f"ORB={self.cpu_roles['fays_left_orb'][0]}/"
|
| 883 |
+
f"{self.cpu_roles['fays_right_orb'][0]}, "
|
| 884 |
+
f"track/output="
|
| 885 |
+
f"{(self.cpu_roles.get('fays_track') or (8,))[0]}, "
|
| 886 |
+
f"background={self.cpu_roles['fays_background'][0]}, "
|
| 887 |
+
f"general={','.join(str(cpu) for cpu in (self.cpu_roles.get('general') or (10, 11)))}, "
|
| 888 |
+
f"nice=+{self._nice_adjustment}"
|
| 889 |
+
)
|
| 890 |
+
self._log(
|
| 891 |
+
f"[SLAM] Starting (Fays {role_text}): "
|
| 892 |
+
+ " ".join(launch_command)
|
| 893 |
+
)
|
| 894 |
+
if self._backend == "orb":
|
| 895 |
+
sdk_guard = device_access_guard(
|
| 896 |
+
FAYS_SDK_INITIALIZATION_LOCK, timeout=45,
|
| 897 |
+
cancelled=self._stop_event.is_set)
|
| 898 |
+
sdk_guard.__enter__()
|
| 899 |
+
sdk_device_guard = self._sdk_device_guard_factory(self.device_config)
|
| 900 |
+
sdk_device_guard.__enter__()
|
| 901 |
+
sdk_deadline = time.monotonic() + 30.0
|
| 902 |
+
process = self._process_factory(
|
| 903 |
+
launch_command,
|
| 904 |
+
stdout=subprocess.PIPE,
|
| 905 |
+
stderr=subprocess.STDOUT,
|
| 906 |
+
stdin=subprocess.PIPE,
|
| 907 |
+
text=True,
|
| 908 |
+
cwd=self.work_dir,
|
| 909 |
+
env=environment,
|
| 910 |
+
)
|
| 911 |
+
if self._native_log_path:
|
| 912 |
+
try:
|
| 913 |
+
os.makedirs(
|
| 914 |
+
os.path.dirname(
|
| 915 |
+
os.path.abspath(self._native_log_path)
|
| 916 |
+
),
|
| 917 |
+
exist_ok=True,
|
| 918 |
+
)
|
| 919 |
+
native_log = open(
|
| 920 |
+
self._native_log_path,
|
| 921 |
+
"a",
|
| 922 |
+
encoding="utf-8",
|
| 923 |
+
)
|
| 924 |
+
native_log.write(json.dumps({
|
| 925 |
+
"time": time.time(),
|
| 926 |
+
"event": "native_process_started",
|
| 927 |
+
"pid": int(process.pid),
|
| 928 |
+
"device_config": self.device_config,
|
| 929 |
+
}, ensure_ascii=False) + "\n")
|
| 930 |
+
native_log.flush()
|
| 931 |
+
except OSError:
|
| 932 |
+
native_log = None
|
| 933 |
+
self._set_affinity(process.pid, set(launch_cpus))
|
| 934 |
+
if set(self._get_affinity(process.pid)) != set(launch_cpus):
|
| 935 |
+
raise RuntimeError(
|
| 936 |
+
"Fays child did not accept the full Fays CPU boundary: "
|
| 937 |
+
f"{','.join(str(cpu) for cpu in launch_cpus)}"
|
| 938 |
+
)
|
| 939 |
+
with self.state._lock:
|
| 940 |
+
if (
|
| 941 |
+
self._stop_event.is_set()
|
| 942 |
+
or generation != self._generation
|
| 943 |
+
):
|
| 944 |
+
raise RuntimeError("stale SLAM generation")
|
| 945 |
+
self.state.process = process
|
| 946 |
+
self.state.status = "Waiting IMU init..."
|
| 947 |
+
self._on_status("Waiting IMU init...")
|
| 948 |
+
self._started_event.set()
|
| 949 |
+
drain_thread = self._thread_factory(
|
| 950 |
+
target=self._drain_stdout,
|
| 951 |
+
args=(process, line_queue, drain_stop, native_log),
|
| 952 |
+
name="slam-stdout-drain",
|
| 953 |
+
daemon=True,
|
| 954 |
+
)
|
| 955 |
+
drain_thread.start()
|
| 956 |
+
|
| 957 |
+
while (
|
| 958 |
+
not self._stop_event.is_set()
|
| 959 |
+
and generation == self._generation
|
| 960 |
+
):
|
| 961 |
+
if sdk_guard is not None and time.monotonic() > sdk_deadline:
|
| 962 |
+
raise RuntimeError("Fays SDK 初始化超时,停止本夹爪连接")
|
| 963 |
+
try:
|
| 964 |
+
item = line_queue.get(timeout=0.5)
|
| 965 |
+
except queue.Empty:
|
| 966 |
+
if process.poll() is not None:
|
| 967 |
+
if (not self._stop_event.is_set()
|
| 968 |
+
and generation == self._generation):
|
| 969 |
+
error = f"SLAM subprocess exited unexpectedly (code={process.returncode})"
|
| 970 |
+
self._log(f"[SLAM] {error}")
|
| 971 |
+
self._log_native_tail()
|
| 972 |
+
self._set_state(status=f"Error: {error}", error=error,
|
| 973 |
+
running=False)
|
| 974 |
+
break
|
| 975 |
+
continue
|
| 976 |
+
if isinstance(item, tuple):
|
| 977 |
+
line, arrival_monotonic, arrival_wall_time = item
|
| 978 |
+
else:
|
| 979 |
+
line = item
|
| 980 |
+
arrival_monotonic = None
|
| 981 |
+
arrival_wall_time = None
|
| 982 |
+
if any(marker in line for marker in (
|
| 983 |
+
"SetupImu(): VIDIOC_S_FMT failed", "ImuInit(): setup imu failed")):
|
| 984 |
+
raise RuntimeError("Fays IMU 初始化失败,停止本夹爪连接: " + line.strip())
|
| 985 |
+
if sdk_guard is not None and "[FAYS-CALIB] orb_runtime_yaml=" in line:
|
| 986 |
+
# SDK construction AND calibration/YAML export completed.
|
| 987 |
+
sdk_guard.__exit__(None, None, None)
|
| 988 |
+
sdk_guard = None
|
| 989 |
+
self._sdk_ready_event.set()
|
| 990 |
+
self._sdk_initialization_done.set()
|
| 991 |
+
event = parse_slam_line(line)
|
| 992 |
+
if event is not None:
|
| 993 |
+
self._handle_event(
|
| 994 |
+
event,
|
| 995 |
+
counters,
|
| 996 |
+
arrival_monotonic,
|
| 997 |
+
arrival_wall_time,
|
| 998 |
+
)
|
| 999 |
+
now = self._wall_clock()
|
| 1000 |
+
elapsed = now - fps_started
|
| 1001 |
+
if elapsed >= 1.0:
|
| 1002 |
+
self._set_state(
|
| 1003 |
+
raw_pose_fps=counters["raw"] / elapsed,
|
| 1004 |
+
valid_pose_fps=counters["valid"] / elapsed,
|
| 1005 |
+
)
|
| 1006 |
+
counters = {"raw": 0, "valid": 0}
|
| 1007 |
+
fps_started = now
|
| 1008 |
+
except Exception as error:
|
| 1009 |
+
self._log(f"[SLAM] Failed to start/run: {error}")
|
| 1010 |
+
self._set_state(status=f"Error: {error}", error=str(error))
|
| 1011 |
+
traceback.print_exc()
|
| 1012 |
+
finally:
|
| 1013 |
+
self._started_event.set()
|
| 1014 |
+
drain_stop.set()
|
| 1015 |
+
if (
|
| 1016 |
+
drain_thread is not None
|
| 1017 |
+
and drain_thread.is_alive()
|
| 1018 |
+
and drain_thread is not threading.current_thread()
|
| 1019 |
+
):
|
| 1020 |
+
drain_thread.join(timeout=STDOUT_JOIN_TIMEOUT_S)
|
| 1021 |
+
self._terminate_process(process)
|
| 1022 |
+
if sdk_device_guard is not None:
|
| 1023 |
+
sdk_device_guard.__exit__(None, None, None)
|
| 1024 |
+
sdk_device_guard = None
|
| 1025 |
+
if sdk_guard is not None:
|
| 1026 |
+
sdk_guard.__exit__(None, None, None)
|
| 1027 |
+
sdk_guard = None
|
| 1028 |
+
if service_started:
|
| 1029 |
+
try:
|
| 1030 |
+
self._stop_aikit_service()
|
| 1031 |
+
except Exception as error:
|
| 1032 |
+
self._log(f"[AIKIT_SERVICE] Stop failed: {error}")
|
| 1033 |
+
if native_log is not None:
|
| 1034 |
+
try:
|
| 1035 |
+
native_log.write(json.dumps({
|
| 1036 |
+
"time": time.time(),
|
| 1037 |
+
"event": "native_process_stopped",
|
| 1038 |
+
"returncode": (
|
| 1039 |
+
None if process is None else process.returncode
|
| 1040 |
+
),
|
| 1041 |
+
}, ensure_ascii=False) + "\n")
|
| 1042 |
+
native_log.close()
|
| 1043 |
+
except OSError:
|
| 1044 |
+
pass
|
| 1045 |
+
if process is not None and self._on_process_stop is not None:
|
| 1046 |
+
try:
|
| 1047 |
+
self._on_process_stop(int(process.pid))
|
| 1048 |
+
except Exception:
|
| 1049 |
+
pass
|
| 1050 |
+
with self.state._lock:
|
| 1051 |
+
if self.state.process is process:
|
| 1052 |
+
self.state.process = None
|
| 1053 |
+
self.state.running = False
|
| 1054 |
+
if not str(self.state.status).startswith("Error:"):
|
| 1055 |
+
self.state.status = "Stopped"
|
| 1056 |
+
self._on_pose(None)
|
| 1057 |
+
self._on_status(self.state.snapshot().status)
|
| 1058 |
+
self._sdk_initialization_done.set()
|
| 1059 |
+
self._log("[SLAM] Worker stopped.")
|
| 1060 |
+
|
| 1061 |
+
def _monitor_frames(self, generation):
|
| 1062 |
+
last_frame = -1
|
| 1063 |
+
last_change = self._clock()
|
| 1064 |
+
last_report = 0.0
|
| 1065 |
+
stalled = False
|
| 1066 |
+
while (
|
| 1067 |
+
not self._stop_event.wait(MONITOR_INTERVAL_S)
|
| 1068 |
+
and generation == self._generation
|
| 1069 |
+
):
|
| 1070 |
+
snapshot = self._frame_snapshot()
|
| 1071 |
+
frame_id = getattr(snapshot, "frame_id", -1)
|
| 1072 |
+
now = self._clock()
|
| 1073 |
+
if frame_id >= 0 and frame_id != last_frame:
|
| 1074 |
+
last_frame = frame_id
|
| 1075 |
+
last_change = now
|
| 1076 |
+
self._set_state(last_frame_id=frame_id, stalled=False)
|
| 1077 |
+
if stalled:
|
| 1078 |
+
self._log("[SLAM] Fays video stream recovered")
|
| 1079 |
+
stalled = False
|
| 1080 |
+
status = self.state.snapshot().status
|
| 1081 |
+
if frame_id >= 0 and now - last_report >= RUNNING_REPORT_INTERVAL_S:
|
| 1082 |
+
self._log(
|
| 1083 |
+
f"[SLAM] Running: frame={frame_id}, state={status}"
|
| 1084 |
+
)
|
| 1085 |
+
last_report = now
|
| 1086 |
+
if (
|
| 1087 |
+
frame_id >= 0
|
| 1088 |
+
and now - last_change >= STALL_TIMEOUT_S
|
| 1089 |
+
and not stalled
|
| 1090 |
+
):
|
| 1091 |
+
stalled = True
|
| 1092 |
+
self._set_state(stalled=True)
|
| 1093 |
+
self._log(
|
| 1094 |
+
"[SLAM] ERROR: Fays video stream has stalled for 5 seconds"
|
| 1095 |
+
)
|
| 1096 |
+
|
| 1097 |
+
def _terminate_process(self, process):
|
| 1098 |
+
if process is None or process.poll() is not None:
|
| 1099 |
+
return
|
| 1100 |
+
with self._process_stop_lock:
|
| 1101 |
+
if process.poll() is not None:
|
| 1102 |
+
return
|
| 1103 |
+
try:
|
| 1104 |
+
process.terminate()
|
| 1105 |
+
process.wait(timeout=PROCESS_WAIT_TIMEOUT_S)
|
| 1106 |
+
return
|
| 1107 |
+
except Exception:
|
| 1108 |
+
pass
|
| 1109 |
+
try:
|
| 1110 |
+
process.kill()
|
| 1111 |
+
process.wait(timeout=FINAL_WAIT_TIMEOUT_S)
|
| 1112 |
+
except Exception:
|
| 1113 |
+
pass
|
| 1114 |
+
|
| 1115 |
+
def stop(self, join=True):
|
| 1116 |
+
"""Stop monitor/stdout/process; safe before or after partial start."""
|
| 1117 |
+
with self._lifecycle_lock:
|
| 1118 |
+
self._generation += 1
|
| 1119 |
+
self._stop_event.set()
|
| 1120 |
+
with self.state._lock:
|
| 1121 |
+
self.state.running = False
|
| 1122 |
+
process = self.state.process
|
| 1123 |
+
self.state.process = None
|
| 1124 |
+
worker = self._worker_thread
|
| 1125 |
+
monitor = self._monitor_thread
|
| 1126 |
+
self._worker_thread = None
|
| 1127 |
+
self._monitor_thread = None
|
| 1128 |
+
self._terminate_process(process)
|
| 1129 |
+
if self._backend == "aikit":
|
| 1130 |
+
try:
|
| 1131 |
+
self._stop_aikit_service()
|
| 1132 |
+
except Exception as error:
|
| 1133 |
+
self._log(f"[AIKIT_SERVICE] Stop failed: {error}")
|
| 1134 |
+
if join:
|
| 1135 |
+
for thread in (worker, monitor):
|
| 1136 |
+
if (
|
| 1137 |
+
thread is not None
|
| 1138 |
+
and thread.is_alive()
|
| 1139 |
+
and thread is not threading.current_thread()
|
| 1140 |
+
):
|
| 1141 |
+
thread.join(timeout=WORKER_JOIN_TIMEOUT_S)
|
| 1142 |
+
self.clear_pose_buffer()
|
| 1143 |
+
self._on_pose(None)
|
| 1144 |
+
self._set_state(
|
| 1145 |
+
status="Disconnected",
|
| 1146 |
+
raw_pose_fps=0.0,
|
| 1147 |
+
valid_pose_fps=0.0,
|
| 1148 |
+
last_frame_id=-1,
|
| 1149 |
+
stalled=False,
|
| 1150 |
+
stdout_backpressure_count=0,
|
| 1151 |
+
pose_lost_count=0,
|
| 1152 |
+
)
|
| 1153 |
+
process_stopped = process is None or process.poll() is not None
|
| 1154 |
+
return process_stopped and all(
|
| 1155 |
+
thread is None or not thread.is_alive()
|
| 1156 |
+
for thread in (worker, monitor)
|
| 1157 |
+
)
|
collector/core/gripper/slam/protocol.py
ADDED
|
@@ -0,0 +1,440 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Strict parsing for the Fays ORB bridge stdout protocol.
|
| 2 |
+
|
| 3 |
+
This module is deliberately side-effect free. It does not know about Tk,
|
| 4 |
+
process handles, files, or the application composition root.
|
| 5 |
+
"""
|
| 6 |
+
|
| 7 |
+
from dataclasses import dataclass
|
| 8 |
+
import math
|
| 9 |
+
import re
|
| 10 |
+
from types import MappingProxyType
|
| 11 |
+
|
| 12 |
+
# 桥接 stdout 诊断协议的三张字段表。原版挂在 fps_history.FpsHistory 上,
|
| 13 |
+
# 本包不复制 fps_history 全模块,只内嵌这三张被协议解析依赖的常量表。
|
| 14 |
+
ORB_DIAGNOSTIC_SPECS = (
|
| 15 |
+
("state", "orb_tracking_state", "int"),
|
| 16 |
+
("inliers", "orb_inliers", "int"),
|
| 17 |
+
("maps", "orb_maps", "int"),
|
| 18 |
+
("active_kf", "orb_active_kf", "int"),
|
| 19 |
+
("active_mp", "orb_active_mp", "int"),
|
| 20 |
+
("local_kf", "orb_local_kf", "int"),
|
| 21 |
+
("local_mp", "orb_local_mp", "int"),
|
| 22 |
+
("lm_queue", "orb_lm_queue", "int"),
|
| 23 |
+
("imu_batch_avg", "orb_imu_batch_avg", "float"),
|
| 24 |
+
("imu_integrated", "orb_imu_integrated", "int"),
|
| 25 |
+
("imu_queue", "orb_imu_queue", "int"),
|
| 26 |
+
("kf_created", "orb_kf_created", "int"),
|
| 27 |
+
("kf_destroyed", "orb_kf_destroyed", "int"),
|
| 28 |
+
("kf_heap_live", "orb_kf_heap_live", "int"),
|
| 29 |
+
("kf_nonactive_live", "orb_kf_nonactive_live", "int"),
|
| 30 |
+
("mp_created", "orb_mp_created", "int"),
|
| 31 |
+
("mp_destroyed", "orb_mp_destroyed", "int"),
|
| 32 |
+
("mp_heap_live", "orb_mp_heap_live", "int"),
|
| 33 |
+
("mp_nonactive_live", "orb_mp_nonactive_live", "int"),
|
| 34 |
+
("mp_cleanup_passes", "orb_mp_cleanup_passes", "int"),
|
| 35 |
+
("mp_cleanup_candidates", "orb_mp_cleanup_candidates", "int"),
|
| 36 |
+
("mp_cleanup_retired", "orb_mp_cleanup_retired", "int"),
|
| 37 |
+
("rss_kb", "orb_rss_kb", "int"),
|
| 38 |
+
("swap_kb", "orb_swap_kb", "int"),
|
| 39 |
+
("minor_faults", "orb_minor_faults", "int"),
|
| 40 |
+
("major_faults", "orb_major_faults", "int"),
|
| 41 |
+
)
|
| 42 |
+
ORB_MAP_DIAGNOSTIC_SPECS = (
|
| 43 |
+
("state", "orb_tracking_state", "int"),
|
| 44 |
+
("inliers", "orb_inliers", "int"),
|
| 45 |
+
("maps", "orb_maps", "int"),
|
| 46 |
+
("active_kf", "orb_active_kf", "int"),
|
| 47 |
+
("active_mp", "orb_active_mp", "int"),
|
| 48 |
+
("local_kf", "orb_local_kf", "int"),
|
| 49 |
+
("local_mp", "orb_local_mp", "int"),
|
| 50 |
+
("lm_queue", "orb_lm_queue", "int"),
|
| 51 |
+
("kf_created", "orb_kf_created", "int"),
|
| 52 |
+
("mp_created", "orb_mp_created", "int"),
|
| 53 |
+
)
|
| 54 |
+
MP_CLEANUP_SPECS = (
|
| 55 |
+
("passes", "orb_mp_payload_cleanup_passes", "int"),
|
| 56 |
+
("candidates", "orb_mp_payload_cleanup_candidates", "int"),
|
| 57 |
+
("compacted", "orb_mp_payload_cleanup_compacted", "int"),
|
| 58 |
+
("queued", "orb_mp_payload_cleanup_queued", "int"),
|
| 59 |
+
("duration_ms", "orb_mp_payload_cleanup_duration_ms", "float"),
|
| 60 |
+
("rss_before_kb", "orb_mp_payload_cleanup_rss_before_kb", "int"),
|
| 61 |
+
("rss_after_kb", "orb_mp_payload_cleanup_rss_after_kb", "int"),
|
| 62 |
+
("trigger_elapsed_s", "orb_mp_payload_cleanup_trigger_s", "float"),
|
| 63 |
+
)
|
| 64 |
+
|
| 65 |
+
|
| 66 |
+
FAYS_STAGE_KEYS = ("wait_ms", "preprocess_ms", "middle_ms", "post_ms")
|
| 67 |
+
|
| 68 |
+
# The gripper cannot physically cover these distances between consecutive
|
| 69 |
+
# 25 Hz SLAM samples. The small fixed allowance absorbs normal estimator
|
| 70 |
+
# corrections; the speed term keeps the gate independent of the exact FPS.
|
| 71 |
+
POSE_MAX_ABS_POSITION_M = 10.0
|
| 72 |
+
POSE_STEP_ALLOWANCE_M = 0.05
|
| 73 |
+
POSE_MAX_LINEAR_SPEED_MPS = 3.0
|
| 74 |
+
POSE_MAX_GUARD_INTERVAL_S = 0.1
|
| 75 |
+
POSE_ANGLE_ALLOWANCE_RAD = math.radians(20.0)
|
| 76 |
+
POSE_MAX_ANGULAR_SPEED_RAD_S = math.radians(1080.0)
|
| 77 |
+
|
| 78 |
+
_POSE_RE = re.compile(
|
| 79 |
+
r"\[([\d.]+)\]\s*XYZ:\(([-\d.e+]+),([-\d.e+]+),([-\d.e+]+)\)\s*"
|
| 80 |
+
r"Quat:\(w=([-\d.e+]+),x=([-\d.e+]+),y=([-\d.e+]+),z=([-\d.e+]+)\)"
|
| 81 |
+
)
|
| 82 |
+
_FPS_RE = re.compile(
|
| 83 |
+
r"^\[FPS_DATA\]\s+image=([\d.]+)\s+imu=([\d.]+)\s+"
|
| 84 |
+
r"process=([\d.]+)"
|
| 85 |
+
r"(?:\s+wait_ms=([\d.]+)\s+preprocess_ms=([\d.]+)"
|
| 86 |
+
r"\s+middle_ms=([\d.]+)\s+post_ms=([\d.]+))?$"
|
| 87 |
+
)
|
| 88 |
+
_ERROR_RE = re.compile(
|
| 89 |
+
r"(error|failed|exception|cannot|unable|denied|timeout|timed out|"
|
| 90 |
+
r"no device|not found|no such|device busy|resource busy)",
|
| 91 |
+
re.IGNORECASE,
|
| 92 |
+
)
|
| 93 |
+
_IGNORED_NON_FATAL_RE = re.compile(
|
| 94 |
+
r"^\[FAYS-CALIB\]\s+WARN\s+SetStereoFPS\(\d+\)\s+failed$"
|
| 95 |
+
)
|
| 96 |
+
|
| 97 |
+
|
| 98 |
+
@dataclass(frozen=True)
|
| 99 |
+
class PoseSample:
|
| 100 |
+
"""One pose emitted by the Fays ORB bridge."""
|
| 101 |
+
|
| 102 |
+
position: tuple
|
| 103 |
+
rotation: tuple
|
| 104 |
+
timestamp: float
|
| 105 |
+
device_name: str = "SLAM"
|
| 106 |
+
sequence: int = 0
|
| 107 |
+
arrival_monotonic: object = None
|
| 108 |
+
arrival_wall_time: object = None
|
| 109 |
+
|
| 110 |
+
@property
|
| 111 |
+
def valid(self):
|
| 112 |
+
values = (*self.position, *self.rotation)
|
| 113 |
+
quaternion_norm = math.sqrt(sum(
|
| 114 |
+
value * value for value in self.rotation
|
| 115 |
+
))
|
| 116 |
+
arrival_values = (
|
| 117 |
+
self.arrival_monotonic,
|
| 118 |
+
self.arrival_wall_time,
|
| 119 |
+
)
|
| 120 |
+
return (
|
| 121 |
+
math.isfinite(self.timestamp)
|
| 122 |
+
and all(math.isfinite(value) for value in values)
|
| 123 |
+
and int(self.sequence) >= 0
|
| 124 |
+
and all(
|
| 125 |
+
value is None or math.isfinite(float(value))
|
| 126 |
+
for value in arrival_values
|
| 127 |
+
)
|
| 128 |
+
and all(
|
| 129 |
+
abs(value) < POSE_MAX_ABS_POSITION_M
|
| 130 |
+
for value in self.position
|
| 131 |
+
)
|
| 132 |
+
and 0.5 < quaternion_norm < 1.5
|
| 133 |
+
)
|
| 134 |
+
|
| 135 |
+
|
| 136 |
+
def pose_rejection_reason(current, previous=None):
|
| 137 |
+
"""Return why a live pose must be held, or ``None`` when it is safe.
|
| 138 |
+
|
| 139 |
+
The native bridge is the primary continuity owner. This side-effect-free
|
| 140 |
+
check is the final boundary before a pose can reach UI/recording state.
|
| 141 |
+
"""
|
| 142 |
+
|
| 143 |
+
if not isinstance(current, PoseSample) or not current.valid:
|
| 144 |
+
return "invalid pose components"
|
| 145 |
+
if previous is None:
|
| 146 |
+
return None
|
| 147 |
+
if not isinstance(previous, PoseSample) or not previous.valid:
|
| 148 |
+
return "invalid previous pose"
|
| 149 |
+
|
| 150 |
+
interval = current.timestamp - previous.timestamp
|
| 151 |
+
if not math.isfinite(interval) or interval <= 0.0:
|
| 152 |
+
return "non-monotonic pose timestamp"
|
| 153 |
+
guarded_interval = min(interval, POSE_MAX_GUARD_INTERVAL_S)
|
| 154 |
+
|
| 155 |
+
distance = math.sqrt(sum(
|
| 156 |
+
(current.position[index] - previous.position[index]) ** 2
|
| 157 |
+
for index in range(3)
|
| 158 |
+
))
|
| 159 |
+
distance_limit = (
|
| 160 |
+
POSE_STEP_ALLOWANCE_M
|
| 161 |
+
+ POSE_MAX_LINEAR_SPEED_MPS * guarded_interval
|
| 162 |
+
)
|
| 163 |
+
if distance > distance_limit:
|
| 164 |
+
return (
|
| 165 |
+
f"translation step {distance:.3f}m exceeds "
|
| 166 |
+
f"{distance_limit:.3f}m"
|
| 167 |
+
)
|
| 168 |
+
|
| 169 |
+
previous_norm = math.sqrt(sum(
|
| 170 |
+
value * value for value in previous.rotation
|
| 171 |
+
))
|
| 172 |
+
current_norm = math.sqrt(sum(
|
| 173 |
+
value * value for value in current.rotation
|
| 174 |
+
))
|
| 175 |
+
dot = abs(sum(
|
| 176 |
+
previous.rotation[index] * current.rotation[index]
|
| 177 |
+
for index in range(4)
|
| 178 |
+
) / (previous_norm * current_norm))
|
| 179 |
+
angle = 2.0 * math.acos(max(-1.0, min(1.0, dot)))
|
| 180 |
+
angle_limit = (
|
| 181 |
+
POSE_ANGLE_ALLOWANCE_RAD
|
| 182 |
+
+ POSE_MAX_ANGULAR_SPEED_RAD_S * guarded_interval
|
| 183 |
+
)
|
| 184 |
+
if angle > angle_limit:
|
| 185 |
+
return (
|
| 186 |
+
f"rotation step {math.degrees(angle):.1f}deg exceeds "
|
| 187 |
+
f"{math.degrees(angle_limit):.1f}deg"
|
| 188 |
+
)
|
| 189 |
+
return None
|
| 190 |
+
|
| 191 |
+
|
| 192 |
+
@dataclass(frozen=True)
|
| 193 |
+
class FaysRateSample:
|
| 194 |
+
image_input: float
|
| 195 |
+
imu_input: float
|
| 196 |
+
process: float
|
| 197 |
+
stage_times: object = None
|
| 198 |
+
|
| 199 |
+
|
| 200 |
+
@dataclass(frozen=True)
|
| 201 |
+
class ProtocolEvent:
|
| 202 |
+
"""A classified stdout line.
|
| 203 |
+
|
| 204 |
+
``kind`` is one of ``pose``, ``fays_rates``, ``orb_diagnostic``,
|
| 205 |
+
``orb_stage``, ``affinity``, ``ready``, ``origin``, ``status``, ``error``
|
| 206 |
+
or ``log``.
|
| 207 |
+
"""
|
| 208 |
+
|
| 209 |
+
kind: str
|
| 210 |
+
payload: object
|
| 211 |
+
raw: str
|
| 212 |
+
|
| 213 |
+
|
| 214 |
+
def parse_orb_diagnostic_line(line):
|
| 215 |
+
"""Parse one complete ``[ORB_DIAG]`` record or return ``None``."""
|
| 216 |
+
prefix = "[ORB_DIAG] "
|
| 217 |
+
if not isinstance(line, str) or not line.startswith(prefix):
|
| 218 |
+
return None
|
| 219 |
+
|
| 220 |
+
encoded = {}
|
| 221 |
+
for token in line[len(prefix):].split():
|
| 222 |
+
name, separator, value = token.partition("=")
|
| 223 |
+
if not separator or not name or not value or name in encoded:
|
| 224 |
+
return None
|
| 225 |
+
encoded[name] = value
|
| 226 |
+
|
| 227 |
+
specs = ORB_DIAGNOSTIC_SPECS
|
| 228 |
+
expected = {protocol_name for protocol_name, _csv_name, _kind in specs}
|
| 229 |
+
if not expected.issubset(encoded):
|
| 230 |
+
return None
|
| 231 |
+
|
| 232 |
+
parsed = {}
|
| 233 |
+
try:
|
| 234 |
+
for protocol_name, _csv_name, kind in specs:
|
| 235 |
+
if kind == "float":
|
| 236 |
+
value = float(encoded[protocol_name])
|
| 237 |
+
if not math.isfinite(value):
|
| 238 |
+
return None
|
| 239 |
+
else:
|
| 240 |
+
value = int(encoded[protocol_name], 10)
|
| 241 |
+
if protocol_name != "state" and value < 0:
|
| 242 |
+
return None
|
| 243 |
+
parsed[protocol_name] = value
|
| 244 |
+
except (TypeError, ValueError, OverflowError):
|
| 245 |
+
return None
|
| 246 |
+
return MappingProxyType(parsed)
|
| 247 |
+
|
| 248 |
+
|
| 249 |
+
def parse_orb_map_diagnostic_line(line):
|
| 250 |
+
"""Parse one complete read-only ``[ORB_MAP_DIAG]`` record."""
|
| 251 |
+
prefix = "[ORB_MAP_DIAG] "
|
| 252 |
+
if not isinstance(line, str) or not line.startswith(prefix):
|
| 253 |
+
return None
|
| 254 |
+
|
| 255 |
+
encoded = {}
|
| 256 |
+
for token in line[len(prefix):].split():
|
| 257 |
+
name, separator, value = token.partition("=")
|
| 258 |
+
if not separator or not name or not value or name in encoded:
|
| 259 |
+
return None
|
| 260 |
+
encoded[name] = value
|
| 261 |
+
|
| 262 |
+
specs = ORB_MAP_DIAGNOSTIC_SPECS
|
| 263 |
+
expected = {protocol_name for protocol_name, _csv_name, _kind in specs}
|
| 264 |
+
if set(encoded) != expected:
|
| 265 |
+
return None
|
| 266 |
+
|
| 267 |
+
parsed = {}
|
| 268 |
+
try:
|
| 269 |
+
for protocol_name, _csv_name, kind in specs:
|
| 270 |
+
value = (
|
| 271 |
+
float(encoded[protocol_name])
|
| 272 |
+
if kind == "float"
|
| 273 |
+
else int(encoded[protocol_name], 10)
|
| 274 |
+
)
|
| 275 |
+
if kind == "float" and not math.isfinite(value):
|
| 276 |
+
return None
|
| 277 |
+
if protocol_name != "state" and value < 0:
|
| 278 |
+
return None
|
| 279 |
+
parsed[protocol_name] = value
|
| 280 |
+
except (TypeError, ValueError, OverflowError):
|
| 281 |
+
return None
|
| 282 |
+
return MappingProxyType(parsed)
|
| 283 |
+
|
| 284 |
+
|
| 285 |
+
def parse_mp_cleanup_line(line):
|
| 286 |
+
"""Parse one complete ``[MP_CLEANUP]`` result or return ``None``."""
|
| 287 |
+
prefix = "[MP_CLEANUP] "
|
| 288 |
+
if not isinstance(line, str) or not line.startswith(prefix):
|
| 289 |
+
return None
|
| 290 |
+
encoded = {}
|
| 291 |
+
for token in line[len(prefix):].split():
|
| 292 |
+
name, separator, value = token.partition("=")
|
| 293 |
+
if not separator or not name or not value or name in encoded:
|
| 294 |
+
return None
|
| 295 |
+
encoded[name] = value
|
| 296 |
+
parsed = {}
|
| 297 |
+
try:
|
| 298 |
+
for name, _csv_name, kind in MP_CLEANUP_SPECS:
|
| 299 |
+
value = float(encoded[name]) if kind == "float" else int(encoded[name])
|
| 300 |
+
if not math.isfinite(value) or value < 0:
|
| 301 |
+
return None
|
| 302 |
+
parsed[name] = value
|
| 303 |
+
except (KeyError, TypeError, ValueError, OverflowError):
|
| 304 |
+
return None
|
| 305 |
+
return MappingProxyType(parsed)
|
| 306 |
+
|
| 307 |
+
|
| 308 |
+
def _parse_pose(line):
|
| 309 |
+
match = _POSE_RE.search(line)
|
| 310 |
+
if match is None:
|
| 311 |
+
return None
|
| 312 |
+
try:
|
| 313 |
+
timestamp = float(match.group(1))
|
| 314 |
+
x, y, z = (float(match.group(index)) for index in range(2, 5))
|
| 315 |
+
qw, qx, qy, qz = (
|
| 316 |
+
float(match.group(index)) for index in range(5, 9)
|
| 317 |
+
)
|
| 318 |
+
except (TypeError, ValueError, OverflowError):
|
| 319 |
+
return None
|
| 320 |
+
return PoseSample(
|
| 321 |
+
position=(x, y, z),
|
| 322 |
+
rotation=(qx, qy, qz, qw),
|
| 323 |
+
timestamp=timestamp,
|
| 324 |
+
)
|
| 325 |
+
|
| 326 |
+
|
| 327 |
+
def rotate_pose_z90(pose):
|
| 328 |
+
"""S80M 轨迹坐标约定:绕 Z 轴旋转 +90°(X=右 Y=正对 Z=上)。
|
| 329 |
+
|
| 330 |
+
真机实测定案(2026-09-09,新 ORB 核心上线后):native 输出
|
| 331 |
+
相对物理系为 R_z(−90°)——native X=正对、Y=左、Z=上(实测:
|
| 332 |
+
物理 +Y(正对) 位移 → native +X;物理 +X(右) → native −Y),
|
| 333 |
+
与桥接 .cc 注释约定一致。注意:旧核心时代的约定是 R_x(+90°)
|
| 334 |
+
(X=右 Y=下 Z=正对),换核后世界系约定已变,此前基于旧核的
|
| 335 |
+
rotate_pose_xm90(绕 X −90°)已随旧核失效。修正 = 位置与姿态
|
| 336 |
+
统一纯左乘 R_z(+90°)(= native 约定之逆,纯世界重标,无体轴
|
| 337 |
+
共轭):
|
| 338 |
+
位置 (x,y,z)→(−y,x,z);四元数 q' = q_z(+90°)⊗q 化简为
|
| 339 |
+
c·(x−y, y+x, z+w, w−z),c=√2/2。
|
| 340 |
+
结果:X=右、Y=夹爪正对、Z=上(右手系,等价 ENU)。
|
| 341 |
+
静止首帧姿态 = R_z(+90°)·correction(部署校正的常数残留,
|
| 342 |
+
约 R_x(−90°),非单位矩阵)——由 process_controller 的原点后
|
| 343 |
+
姿态相对化(捕获首帧显示姿态为基准)吸收,首帧显示恒为
|
| 344 |
+
单位矩阵,帧轴与世界轴对齐、无起始跳变。
|
| 345 |
+
"""
|
| 346 |
+
x, y, z = pose.position
|
| 347 |
+
qx, qy, qz, qw = pose.rotation
|
| 348 |
+
c = math.sqrt(0.5)
|
| 349 |
+
return PoseSample(
|
| 350 |
+
position=(-y, x, z),
|
| 351 |
+
rotation=(c * (qx - qy), c * (qy + qx), c * (qz + qw),
|
| 352 |
+
c * (qw - qz)),
|
| 353 |
+
timestamp=pose.timestamp,
|
| 354 |
+
)
|
| 355 |
+
|
| 356 |
+
|
| 357 |
+
def quat_conjugate(q):
|
| 358 |
+
"""四元数共轭(单位四元数时即逆)。"""
|
| 359 |
+
x, y, z, w = q
|
| 360 |
+
return (-x, -y, -z, w)
|
| 361 |
+
|
| 362 |
+
|
| 363 |
+
def quat_product(a, b):
|
| 364 |
+
"""Hamilton 积 a⊗b(等价矩阵左乘 a:先施加 b 再施加 a)。"""
|
| 365 |
+
ax, ay, az, aw = a
|
| 366 |
+
bx, by, bz, bw = b
|
| 367 |
+
return (
|
| 368 |
+
aw * bx + ax * bw + ay * bz - az * by,
|
| 369 |
+
aw * by + ay * bw + az * bx - ax * bz,
|
| 370 |
+
aw * bz + az * bw + ax * by - ay * bx,
|
| 371 |
+
aw * bw - ax * bx - ay * by - az * bz,
|
| 372 |
+
)
|
| 373 |
+
|
| 374 |
+
|
| 375 |
+
def _parse_fays_rates(line):
|
| 376 |
+
match = _FPS_RE.fullmatch(line)
|
| 377 |
+
if match is None:
|
| 378 |
+
return None
|
| 379 |
+
values = tuple(float(match.group(index)) for index in range(1, 4))
|
| 380 |
+
stages = None
|
| 381 |
+
if match.group(4) is not None:
|
| 382 |
+
stages = MappingProxyType({
|
| 383 |
+
key: float(match.group(index))
|
| 384 |
+
for key, index in zip(FAYS_STAGE_KEYS, range(4, 8))
|
| 385 |
+
})
|
| 386 |
+
return FaysRateSample(*values, stage_times=stages)
|
| 387 |
+
|
| 388 |
+
|
| 389 |
+
def parse_slam_line(line):
|
| 390 |
+
"""Classify one stripped Fays bridge stdout line."""
|
| 391 |
+
if not isinstance(line, str):
|
| 392 |
+
return None
|
| 393 |
+
raw = line.strip()
|
| 394 |
+
if not raw:
|
| 395 |
+
return None
|
| 396 |
+
# This SDK build exposes SetStereoFPS as an unavailable/stubbed entry.
|
| 397 |
+
# The serial-matched YAML already selects the requested 25 FPS mode, so
|
| 398 |
+
# this non-fatal diagnostic must not put the GUI into the Error state.
|
| 399 |
+
if _IGNORED_NON_FATAL_RE.fullmatch(raw):
|
| 400 |
+
return None
|
| 401 |
+
|
| 402 |
+
rates = _parse_fays_rates(raw)
|
| 403 |
+
if rates is not None:
|
| 404 |
+
return ProtocolEvent("fays_rates", rates, raw)
|
| 405 |
+
|
| 406 |
+
map_diagnostic = parse_orb_map_diagnostic_line(raw)
|
| 407 |
+
if map_diagnostic is not None:
|
| 408 |
+
return ProtocolEvent("orb_diagnostic", map_diagnostic, raw)
|
| 409 |
+
|
| 410 |
+
diagnostic = parse_orb_diagnostic_line(raw)
|
| 411 |
+
if diagnostic is not None:
|
| 412 |
+
return ProtocolEvent("orb_diagnostic", diagnostic, raw)
|
| 413 |
+
cleanup = parse_mp_cleanup_line(raw)
|
| 414 |
+
if cleanup is not None:
|
| 415 |
+
return ProtocolEvent("mp_cleanup", cleanup, raw)
|
| 416 |
+
|
| 417 |
+
if raw.startswith("[ORB_STAGE]"):
|
| 418 |
+
return ProtocolEvent("orb_stage", raw, raw)
|
| 419 |
+
if raw.startswith("[FAYS-AFFINITY]"):
|
| 420 |
+
return ProtocolEvent("affinity", raw, raw)
|
| 421 |
+
if "READY" in raw and "Press ENTER" in raw:
|
| 422 |
+
return ProtocolEvent("ready", raw, raw)
|
| 423 |
+
if "NEW ORIGIN" in raw or "ORIGIN SET" in raw:
|
| 424 |
+
return ProtocolEvent("origin", raw, raw)
|
| 425 |
+
|
| 426 |
+
pose = _parse_pose(raw)
|
| 427 |
+
if pose is not None:
|
| 428 |
+
return ProtocolEvent("pose", pose, raw)
|
| 429 |
+
|
| 430 |
+
if (
|
| 431 |
+
"IMU init" in raw
|
| 432 |
+
or "Recording" in raw
|
| 433 |
+
or "Segment" in raw
|
| 434 |
+
or raw.startswith("[POSE_HOLD]")
|
| 435 |
+
or raw.startswith("[POSE_REBASE]")
|
| 436 |
+
):
|
| 437 |
+
return ProtocolEvent("status", raw, raw)
|
| 438 |
+
if _ERROR_RE.search(raw):
|
| 439 |
+
return ProtocolEvent("error", raw, raw)
|
| 440 |
+
return ProtocolEvent("log", raw, raw)
|