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 files
This view is limited to 50 files because it contains too many changes.   See raw diff
Files changed (50) hide show
  1. collector/.gitignore +7 -0
  2. collector/README.md +36 -1
  3. collector/config/__init__.py +1 -1
  4. collector/config/i18n.py +27 -0
  5. collector/config/settings.py +19 -0
  6. collector/core/api_client.py +64 -5
  7. collector/core/device_detector.py +176 -29
  8. collector/core/device_manager.py +16 -3
  9. collector/core/egodata_writer.py +71 -1
  10. collector/core/gripper/__init__.py +15 -0
  11. collector/core/gripper/affinity.py +118 -0
  12. collector/core/gripper/bridge.py +811 -0
  13. collector/core/gripper/control/__init__.py +19 -0
  14. collector/core/gripper/control/grip_state.py +233 -0
  15. collector/core/gripper/devices/__init__.py +37 -0
  16. collector/core/gripper/devices/gripper_connection.py +717 -0
  17. collector/core/gripper/devices/gripper_serial.py +181 -0
  18. collector/core/gripper/devices/relative_usb_topology.py +455 -0
  19. collector/core/gripper/devices/serial_command_worker.py +294 -0
  20. collector/core/gripper/devices/tactile_discovery.py +1707 -0
  21. collector/core/gripper/devices/tactile_process_manager.py +0 -0
  22. collector/core/gripper/devices/usb_camera_ipc.py +255 -0
  23. collector/core/gripper/devices/usb_camera_sets.py +81 -0
  24. collector/core/gripper/devices/uvc_camera_service.py +782 -0
  25. collector/core/gripper/fays_runtime.py +1638 -0
  26. collector/core/gripper/fays_serial_probe.py +246 -0
  27. collector/core/gripper/fays_single.py +334 -0
  28. collector/core/gripper/paths.py +164 -0
  29. collector/core/gripper/recording/__init__.py +7 -0
  30. collector/core/gripper/recording/fays_raw_client.py +314 -0
  31. collector/core/gripper/runtime/__init__.py +7 -0
  32. collector/core/gripper/runtime/cpu_policy.py +777 -0
  33. collector/core/gripper/runtime/device_access.py +66 -0
  34. collector/core/gripper/runtime/interruptible_queue.py +75 -0
  35. collector/core/gripper/runtime/thread_affinity.py +677 -0
  36. collector/core/gripper/sightac_sdk/API_VERSIONS.md +27 -0
  37. collector/core/gripper/sightac_sdk/api_new/__init__.py +3 -0
  38. collector/core/gripper/sightac_sdk/api_new/libSonixCamera.so +3 -0
  39. collector/core/gripper/sightac_sdk/api_new/parameters.yaml +80 -0
  40. collector/core/gripper/sightac_sdk/api_new/platform_utils.py +3 -0
  41. collector/core/gripper/sightac_sdk/api_new/sensor_function.py +0 -0
  42. collector/core/gripper/sightac_sdk/api_new/sensor_gripper.py +3 -0
  43. collector/core/gripper/sightac_sdk/api_new/touch_sensor.py +0 -0
  44. collector/core/gripper/sightac_sdk/api_new/touch_sensor_base.py +0 -0
  45. collector/core/gripper/sightac_sdk/api_new/xu_camera.py +0 -0
  46. collector/core/gripper/sightac_sdk/pyarmor_runtime_000000/__init__.py +2 -0
  47. collector/core/gripper/sightac_sdk/pyarmor_runtime_000000/pyarmor_runtime.so +3 -0
  48. collector/core/gripper/slam/__init__.py +12 -0
  49. collector/core/gripper/slam/process_controller.py +1157 -0
  50. 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
- ![Version](https://img.shields.io/badge/version-1.2.0-blue)
4
  ![Python](https://img.shields.io/badge/python-3.12-blue)
5
  ![License](https://img.shields.io/badge/license-TBD-lightgrey)
6
 
@@ -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
+ ![Version](https://img.shields.io/badge/version-1.3.0-blue)
4
  ![Python](https://img.shields.io/badge/python-3.12-blue)
5
  ![License](https://img.shields.io/badge/license-TBD-lightgrey)
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.2.1" # 修复大文件上传被 10 秒误杀(urllib3 发送请求体阶段 socket 超时错用连接超时,服务器慢读停顿超 10s 即 Connection aborted,现已改用读超时窗口);上传对话框关闭后任务在后台继续完成(手动上传并入主窗口共享上传队列,重复提交自动跳过)。
 
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
- self._session.mount("http://", _PatientSendAdapter())
92
- self._session.mount("https://", _PatientSendAdapter())
 
93
 
94
  def close(self):
95
  if self._own_session:
@@ -212,7 +260,14 @@ class APIClient:
212
 
213
  # ── 查询 ──────────────────────────────────────────
214
 
215
- def get_sessions(self, limit: int = 50) -> list[dict]:
 
 
 
 
 
 
 
216
  try:
217
  r = self._session.get(
218
  f"{self.base_url}/api/v1/sessions",
@@ -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:ftdi" | "ble:{MAC}" | "usbglove:{serial}"
39
- kind: str # "uvc" | "d435" | "s80m" | "data_ble" | "ble" | "usb_glove"
40
  display_name: str # 设备内部命名(by-id 解码 / rs 权威名 / 蓝牙广播名 / USB 手套)
41
  serial: str = "" # 设备序号(无则为空串)
42
  video_index: int = -1 # UVC 设备的 /dev/videoN 索引(其它为 -1)
43
  by_id_path: Optional[str] = None # /dev/v4l/by-id 永久路径(有则填)
44
  backend: str = "" # 打开后端(开关打开后由 CameraWorker 决定,枚举时为空)
45
  address: str = "" # BLE MAC 地址 / USB 手套串口路径(非 BLE 为空串)
 
46
  rssi: int = 0 # BLE 信号强度(排序用)
47
  user_name: str = "" # 用户命名(枚举后由 MainWindow 从 device_names.json 填充)
48
 
@@ -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 节点与 FTDI SDK 设备)。
149
 
150
- Linux: sysfs 只读枚举(/dev/v4l/by-id 分组,轮询安全)。
151
- Windows: pygrabber DirectShow 枚举(见 _list_uvc_devices_windows)。
 
152
  """
153
  if os.name == "nt":
154
  return _list_uvc_devices_windows(max_index)
@@ -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 命中一次即返回单条 S80M 条目(SDK 配置写死 video0/video2)。
203
 
204
- 序号尝试从 by-id 条目解析(FT602 通常无序号 → 空串)。
 
 
 
205
  """
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
206
  for i in range(max_index):
207
  if not _is_sdk_device(i):
208
  continue
209
- serial, by_id_path = "", None
210
- by_id_dir = "/dev/v4l/by-id"
211
- if os.path.isdir(by_id_dir):
212
- for entry in sorted(os.listdir(by_id_dir)):
213
- p = os.path.join(by_id_dir, entry)
214
- try:
215
- if os.readlink(p) == f"../../video{i}":
216
- parsed = _parse_by_id_entry(entry)
217
- if parsed:
218
- serial = parsed.get("serial", "")
219
- by_id_path = p
220
- break
221
- except OSError:
222
- continue
223
  return [DeviceInfo(
224
  key="s80m:ftdi",
225
  kind="s80m",
226
  display_name="FaysSense S80M",
227
- serial=serial,
228
  video_index=i,
229
- by_id_path=by_id_path,
230
  )]
231
  return []
232
 
@@ -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
- """五段枚举(UVC + D435 + S80M + BLE + USB 手套),各自容错,整体不崩。"""
351
  devices: List[DeviceInfo] = []
 
352
  try:
353
- devices += _list_uvc_devices(max_index)
354
  except Exception:
355
- pass
356
  try:
357
- devices += _list_d435_devices()
358
  except Exception:
359
  pass
360
  try:
361
- devices += _list_s80m_devices(max_index)
362
  except Exception:
363
  pass
 
 
 
 
 
 
 
364
  try:
365
  devices += _list_ble_devices()
366
  except Exception:
@@ -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)