feat: add head control and joint data decoder

.gitignore

@@ -14,6 +14,12 @@ python/build/
 *.egg-info/
 *.so
 
+# Final datasets stay local and must never be committed.  Keep only the two
+# tracked instructions/ignore files inside the destination directory.
+Data_Get/*
+!Data_Get/.gitignore
+!Data_Get/README.md
+
 # Local deployment and editor leftovers.
 .deploy-*/
 .DS_Store

Data_Get/.gitignore

@@ -0,0 +1,3 @@
+*
+!.gitignore
+!README.md

Data_Get/README.md

@@ -0,0 +1,9 @@
+# Data_Get
+
+完成的数采 episode 自动保存到本目录，每个子目录包含 MCAP、`metadata.yaml`、录制日志和
+`manifest.json`、`READY`。数据来自机器人 Nvidia 上独立的项目录制服务，不影响厂家
+`record_bag_node`。运行数据、临时同步目录及 `sync_status.json` 均不会提交到 Git。
+
+只有通过大小和 SHA-256 校验、fsync 和原子发布的目录才会从 `.incoming` 进入这里，
+并写入持久化 `VERIFIED` 凭据。之后才按 episode ID 与 manifest SHA 精确删除机器人端
+对应 `ready` 副本。传输或删除确认中断会自动重试，不要把 `.incoming` 当作完整数据集。

README.md

@@ -10,8 +10,13 @@
   读取数据，仅在左 Z + 右 C 连续 3 秒开启后注册并发送，STOP 后立即断开；还包含
   不走公网时在 EAI 运行 `kcpserver` 的本地 Hub 用户服务模板。
 - `tg3_local_teleop/`：部署到机器人 Nvidia；直接接收 OmniSocket 数据，发布双臂、
-  双手及 `/hric/robot/cmd_vel`，并提供限速回 Home。
-- `docs/`：可提交的跨机器人迁移步骤。含实测帧和现场拓扑的汇报/证据文档只保留在
+  双手及 `/hric/robot/cmd_vel`，提供限速回 Home，并独立录制控制数据 MCAP。
+- `tg3_data_collection/`：部署在本机；通过本地 SSH/rsync 拉取已完整收尾并校验的
+  episode，原子保存到 `Data_Get/`；持久化验真凭据后自动回收机器人上的对应副本。
+- `Data_Get/`：最终数采目录。实际 MCAP、清单和同步状态默认不提交 Git。
+- `docs/`：跨设备迁移先看
+  [`天工3.0同构臂遥操与数采跨设备迁移手册.md`](docs/天工3.0同构臂遥操与数采跨设备迁移手册.md)，
+  更底层的公网/本地 Hub 与 ZMQ 细节见原迁移部署指南。含实测帧和现场拓扑的汇报/证据文档只保留在
   当前本地工作副本，不同步到匿名可读的远端仓库。
 
 ## 外部依赖（不随仓库提交）
@@ -40,10 +45,38 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务
 - 左 Z + 右 C 连续 3 秒：开始遥操；再次连续 3 秒：结束并限速回 Home。
   若 Hub 尚未运行，本次长按只检测一次并立即结束，不发送控制数据；服务器启动后须先
   稳定松开，再重新长按 3 秒。pending 和活动会话都不会自动重试或自动恢复。
-- 右 C + 左摇杆上下：HBWALK 前进/后退。
-- 左 Z + 右摇杆左右：HBWALK 原地转向。
-- 右 B 连续 1 秒：右手进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续
+- 遥操开启且双摇杆先回中过一次后，左摇杆上下直接控制 HBWALK 前进/后退。
+- 遥操开启且双摇杆先回中过一次后，右摇杆左右直接控制 HBWALK 原地转向。
+- 遥操开启且右摇杆纵轴先回中过一次后，按住左 Z + 右摇杆上下调整
+  头部相机视角：上推抬头、下推低头；无需 R3。按住 Z 时会屏蔽右摇杆水平
+  转向，避免调整视角时误转。软件工作区为 `-10°～+50°`，最大 `0.2 rad/s`。
+- 右 B 连续 1 秒：左右手同时进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续
   1 秒退出。手指动作继续受 `400 units/s` 限速。
+- 按住右 A：右手限速摆到目标位置 `[428,735,500,77,77,72]`；松开 A 后恢复
+  原有右手输入。A 在每个新遥操会话中必须先稳定松开 0.5 秒，防止跨会话误触。
+- 左摇杆按下（L3）连续 1 秒：开始数采；松开至少 0.5 秒后再次连续 1 秒：
+  结束并保存。只有遥操已开启时才接受；Z+C 结束遥操或安全解除会自动结束数采。
+
+## 数采保存
+
+Nvidia 独立服务录制明确白名单内的机器人实测状态、双臂/双手命令与反馈、行走、
+IMU、电源状态、xTELE 应用帧以及头部 Orbbec 的 RGB-D。RGB 使用标准 JPEG 压缩流，
+16 位深度使用无损 compressedDepth PNG，并同时保存相机内参与静态 TF。头部相机
+是可选流：服务运行就自动采集，未运行不阻塞核心 episode；manifest 会标记
+`absent/healthy/partial/low_rate`。它不接管或停止厂家
+`/record_bag_node`，也不录点云。正常结束后先校验 MCAP、生成 SHA-256
+`manifest.json` 和 `READY`，再由
+本机服务拉取到：
+
+```text
+/home/ps/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get/<episode_id>/
+```
+
+传输中断只会留在 `Data_Get/.incoming/`，不会显示为完成 episode。机器人在录制期间
+必须临时写 `active/ready`；只有本机完成全文件 SHA-256、fsync、原子发布并写入
+`VERIFIED` 后，才会自动删除机器人上同 ID、同 manifest 哈希的 `ready` 目录。EAI
+从不保存 MCAP 或图像 payload。部署、状态检查和恢复步骤见
+`tg3_data_collection/README.md`。
 
 ## 迁移前必须修改
 

docs/天工3.0同构臂遥操与数采跨设备迁移手册.md

@@ -0,0 +1,747 @@
+# 天工 3.0 同构臂遥操与数采跨设备迁移手册
+
+适用范围：天工 3.0、HBWALK、TS1P 同构臂、BrainCo Revo2 双手、EAI 本地
+OmniSocket Hub，以及 PS 主机保存数采数据的当前方案。
+
+本文按“换机器人、换 EAI 工控机、换 PS 主机、全部更换”分别说明。更底层的公网 Hub、
+纯 ZMQ 备选和协议证据见
+[`天工3.0本地同构臂遥操迁移部署指南.md`](天工3.0本地同构臂遥操迁移部署指南.md)。
+
+## 1. 当前系统基线
+
+迁移前先记录实际提交，不要只记录文件夹名称：
+
+```bash
+cd ~/Desktop/TG3_TS1P_OmniSocket_Teleop
+git rev-parse HEAD
+git status --short
+```
+
+本文编写时的功能基线为：
+
+```text
+TG3 项目：67c9b657688680c686f55b3f868bbc310edbfd41
+OmniSocketGo：de3f5c96779dbe1571c10feb22fc7f2331b6b222
+TG3 仓库：https://gitea.public.snrc.site/meiqi/TG3.git
+OmniSocketGo：https://gitea.public.snrc.site/limingjie/OmniSocketGo.git
+```
+
+迁移时应优先使用 `main` 上更新且已经过现场验收的提交，并把最终提交号写入设备档案。
+
+### 1.1 当前链路
+
+```text
+EAI 工控机
+  xTELE tcp://127.0.0.1:5003（原始双臂、按钮、摇杆、扳机）
+       + tcp://127.0.0.1:5001（处理后的手势目标）
+  -> tg3-omnisocket-sender.service
+  -> 本机 tg3-omnisocket-hub.service，UDP 0.0.0.0:14049
+  -> 局域网 192.168.5.14:14049
+
+机器人 Nvidia
+  tg3-local-teleop.service
+  -> 双臂、BrainCo 双手、/hric/robot/cmd_vel
+  -> tg3-data-recorder.service 临时录制 MCAP
+
+PS 主机
+  tg3-data-get-sync.service
+  -> SSH/rsync 从 Nvidia 拉取 READY episode
+  -> SHA-256 + fsync + VERIFIED
+  -> Data_Get/<episode>/
+  -> 验真后精确删除 Nvidia 对应临时副本
+```
+
+EAI 不保存数采数据。Nvidia 在录制和等待 PS 同步期间必须临时落盘；最终持久副本只保留在
+PS 主机。PS 离线时，机器人会保留尚未验真的 `ready/`，恢复连接后再同步和回收。
+
+### 1.2 当前地址与标识
+
+| 用途 | 当前值 | 变化时修改位置 |
+|---|---|---|
+| EAI 运维 SSH | SSH 别名 `eai` | PS 的 SSH 配置和运维命令 |
+| EAI 控制局域网 IP | `192.168.5.14` | 机器人 `config.toml` 的 `omnisocket_server`；ZMQ 回退时还改 `iarm_endpoint` |
+| 机器人控制局域网 IP | `192.168.5.15` | 通常只影响 EAI 防火墙规则 |
+| 机器人 Nvidia SSH | `nvidia@192.168.41.2` | PS 同步 unit 的 `--remote` 和运维命令 |
+| 机器人 Ubuntu SSH | `ubuntu@192.168.41.1` | 仅厂家运维；本项目不部署到 Ubuntu |
+| 本地 Hub | EAI UDP `14049` | EAI Hub、EAI sender、机器人 `omnisocket_server` 和防火墙同时修改 |
+| EAI Peer | `tg3-009027fa8190-iarm` | EAI `--peer-id` 与机器人 `omnisocket_expected_sender` 同时修改 |
+| 机器人 Peer | `tg3-009027fa8190-robot` | EAI `--target-peer` 与机器人 `omnisocket_peer_id` 同时修改 |
+| TS1P ID | `IArm009027FA8190` | 更换同构臂时修改机器人 `expected_iarm_id` |
+| 本机数据目录 | `~/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get` | PS 同步 unit 的 `WorkingDirectory`、脚本和 `--destination` |
+
+不要把机器人内部厂家链路 `192.168.41.1/192.168.41.2` 与 EAI—机器人控制局域网
+`192.168.5.0/24` 混为一谈，也不要把 EAI 本机的 `127.0.0.1:5001/5003` 改成网卡地址。
+
+## 2. 三台设备分别需要哪些文件
+
+### 2.1 PS 主机
+
+必须保留：
+
+```text
+TG3_TS1P_OmniSocket_Teleop/
+  tg3_omnisocket_transport/   # 用于部署 EAI
+  tg3_local_teleop/           # 用于部署机器人
+  tg3_data_collection/        # PS 同步程序
+  Data_Get/                   # 唯一持久数采目录，不进入 Git
+  docs/
+  verify.sh
+
+~/.config/systemd/user/tg3-data-get-sync.service
+~/.ssh/known_hosts
+~/.ssh/config                  # 如果使用 eai 等别名
+```
+
+`Data_Get` 必须单独迁移，Git clone 不会带回 MCAP。SSH 私钥也不应提交到仓库，应在新主机
+重新生成并使用 `ssh-copy-id` 登记。
+
+### 2.2 EAI 工控机
+
+必须具备：
+
+```text
+原厂 xTELE 与 TS1P 标定配置             # 不在本仓库
+/home/eai/OmniSocketGo/                 # 固定提交，目标机原生编译
+/home/eai/tg3_omnisocket_transport/
+~/.config/systemd/user/tg3-omnisocket-hub.service
+~/.config/systemd/user/tg3-omnisocket-sender.service
+```
+
+不要从旧 EAI 复制 Python 扩展 `.so` 到不同架构或不同 Python ABI 的新机器。不要把旧串口
+路径、CAN 配置和关节偏置盲目用于另一套 TS1P。
+
+### 2.3 机器人 Nvidia
+
+必须具备：
+
+```text
+/home/nvidia/OmniSocketGo/              # 固定提交，aarch64 原生编译
+/home/nvidia/tg3_local_teleop/          # 整个目录
+~/.config/systemd/user/tg3-local-teleop.service
+~/.config/systemd/user/tg3-data-recorder.service
+/home/nvidia/tg3_data_collection/       # 运行时临时目录，由程序创建
+```
+
+不要把旧机器人的 `status.json`、`__pycache__`、ROS `build/install/log` 或编译好的
+OmniSocket `.so` 当成部署文件。`ros2_py` 必须在新机器人重新构建。
+
+### 2.4 机器人 Ubuntu
+
+本项目不向 Ubuntu 安装文件，不修改厂家 `record_bag_node`、`proc_manager`、FSM 或
+HBWALK 服务。迁移后仍由操作者按厂家流程切换到 HBWALK；本项目不发布 `gotoHBWALK`。
+
+## 3. 迁移前的统一停机与备份
+
+1. 松开所有按键和摇杆，长按 Z+C 3 秒退出遥操；
+2. 等待 Home 流程结束，确认机器人 `armed=false`；
+3. 若正在数采，等待本机出现完整 episode；
+4. 确认同步状态为 `idle` 且 `pending_remote_cleanup=0`；
+5. 停止旧 PS 的同步服务，防止两台 PS 同时拉取和回收同一个 episode；
+6. 记录当前配置、提交号、设备 ID 和服务文件。
+
+检查命令：
+
+```bash
+ssh nvidia@192.168.41.2 \
+  'python3 -m json.tool /home/nvidia/tg3_local_teleop/status.json | head -80'
+
+python3 -m json.tool \
+  ~/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get/sync_status.json
+
+systemctl --user stop tg3-data-get-sync.service
+```
+
+至少备份以下内容：
+
+```bash
+mkdir -p ~/tg3_migration_backup
+
+scp nvidia@192.168.41.2:/home/nvidia/tg3_local_teleop/config.toml \
+  ~/tg3_migration_backup/robot-config.toml
+
+scp eai:/home/eai/.config/systemd/user/tg3-omnisocket-sender.service \
+  ~/tg3_migration_backup/eai-sender.service
+
+scp eai:/home/eai/.config/systemd/user/tg3-omnisocket-hub.service \
+  ~/tg3_migration_backup/eai-hub.service
+
+cp ~/.config/systemd/user/tg3-data-get-sync.service \
+  ~/tg3_migration_backup/ps-sync.service
+```
+
+把 `Data_Get` 复制到新磁盘或新主机时，先停同步服务，再执行：
+
+```bash
+rsync -aH --info=progress2 \
+  ~/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get/ \
+  <新主机>:/home/<新用户>/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get/
+```
+
+在新主机验证前保留旧主机副本。若状态仍为 `delete_pending`，先在旧主机完成同步回收，不要
+带着未决收据更换机器人地址。
+
+## 4. 场景一：只更换 PS 主机
+
+PS 不在实时控制链路中，因此更换 PS IP 不需要修改 EAI sender、Hub Peer 或机器人
+`config.toml`。需要迁移的是代码、`Data_Get`、SSH 信任和同步服务。
+
+### 4.1 安装代码与依赖
+
+```bash
+sudo apt update
+sudo apt install -y git rsync python3
+
+git clone https://gitea.public.snrc.site/meiqi/TG3.git \
+  ~/Desktop/TG3_TS1P_OmniSocket_Teleop
+
+cd ~/Desktop/TG3_TS1P_OmniSocket_Teleop
+git checkout main
+./verify.sh
+```
+
+如果迁移时使用的现场提交尚未推到 Gitea，应从旧 PS 用 `rsync` 复制完整仓库；不要只 clone
+一个落后的远端版本。
+
+### 4.2 建立到新机器人的 SSH 信任
+
+```bash
+ssh-keygen -t ed25519
+ssh-copy-id nvidia@<新机器人SSH地址>
+ssh nvidia@<新机器人SSH地址> true
+```
+
+首次 SSH 时应人工核对主机指纹。同步器使用 `StrictHostKeyChecking=yes`，不要为了省事关闭
+主机密钥检查。机器人更换后应删除旧地址的错误 host key，再登记新机器人的真实指纹。
+
+### 4.3 安装本机同步服务
+
+先确认机器人已经部署：
+
+```text
+/home/nvidia/tg3_local_teleop/delete_ready_episode.py
+```
+
+然后修改仓库中的 `tg3_data_collection/tg3-data-get-sync.service`：
+
+- `WorkingDirectory` 改成新 PS 的真实项目目录；
+- `ExecStart` 中脚本路径和 `--destination` 改成新目录；
+- `--remote` 改成新机器人 Nvidia SSH 地址；
+- 保留 `--delete-remote-after-sync`；
+- 保留远端固定根 `/home/nvidia/tg3_data_collection/ready`。
+
+安装并**显式 restart**，仅执行 `enable --now` 不会替换已经运行的旧进程：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp tg3_data_collection/tg3-data-get-sync.service \
+  ~/.config/systemd/user/tg3-data-get-sync.service
+systemctl --user daemon-reload
+systemctl --user enable tg3-data-get-sync.service
+systemctl --user restart tg3-data-get-sync.service
+systemctl --user show tg3-data-get-sync.service -p ExecStart -p ActiveState
+```
+
+验收：
+
+```bash
+python3 -m json.tool Data_Get/sync_status.json
+```
+
+必须看到 `state=idle`、`last_error=""`、`pending_remote_cleanup=0`。确认新服务正常后，旧 PS
+同步服务保持停用；不能让两台主机同时运行自动删除模式。
+
+## 5. 场景二：只更换 EAI 工控机
+
+机器人、PS 和 Home 均可保留，但必须先把原厂 xTELE 与 TS1P 在新 EAI 上独立调通。
+
+### 5.1 先验收原厂 xTELE
+
+```bash
+ss -lntp | grep -E ':(5001|5003)[[:space:]]'
+```
+
+必须同时看到本机 TCP 5003 和 5001。可用下面的只读命令读取一个 5003 帧并记录同构臂
+身份：
+
+```bash
+python3 - <<'PY'
+import json, zmq
+ctx = zmq.Context.instance()
+s = ctx.socket(zmq.SUB)
+s.setsockopt(zmq.SUBSCRIBE, b"")
+s.setsockopt(zmq.CONFLATE, 1)
+s.connect("tcp://127.0.0.1:5003")
+d = json.loads(s.recv().decode())
+print(d.get("isomorphic_arm_id"), d.get("isomorphic_arm_type"))
+PY
+```
+
+输出的 ID 必须写入机器人 `config.toml` 的 `expected_iarm_id`。类型应为 `TS1P`。如果串口、
+CAN、方向、偏置或频率不正常，不要启动本项目 sender。
+
+### 5.2 在新 EAI 原生构建 OmniSocketGo
+
+```bash
+sudo apt update
+sudo apt install -y git build-essential python3-dev python3-pip
+python3 -m pip install --user pyzmq
+
+git clone https://gitea.public.snrc.site/limingjie/OmniSocketGo.git \
+  /home/eai/OmniSocketGo
+git -C /home/eai/OmniSocketGo checkout \
+  de3f5c96779dbe1571c10feb22fc7f2331b6b222
+make -C /home/eai/OmniSocketGo python-ext bin/kcpserver
+
+PYTHONPATH=/home/eai/OmniSocketGo/python python3 -c \
+  'from omnisocket import Session; print(Session)'
+```
+
+若新用户名不是 `eai`，把所有 `/home/eai` 改成实际绝对路径。服务模板和 Python 扩展必须
+使用同一个用户环境。
+
+### 5.3 部署 sender 和本地 Hub
+
+在 PS 执行：
+
+```bash
+rsync -av --exclude=__pycache__ \
+  ~/Desktop/TG3_TS1P_OmniSocket_Teleop/tg3_omnisocket_transport/ \
+  eai@<新EAI地址>:/home/eai/tg3_omnisocket_transport/
+```
+
+新 EAI 上修改两个 unit：
+
+```text
+tg3-omnisocket-hub.service
+  ExecStart=.../kcpserver -mode hub -listen 0.0.0.0:14049
+
+tg3-omnisocket-sender.service
+  --server 127.0.0.1:14049
+  --peer-id tg3-<机器人编号>-iarm
+  --target-peer tg3-<机器人编号>-robot
+  --zmq-endpoint tcp://127.0.0.1:5003
+  --cmd-zmq-endpoint tcp://127.0.0.1:5001
+```
+
+Peer 等式必须成立：
+
+```text
+EAI --peer-id       == 机器人 omnisocket_expected_sender
+EAI --target-peer   == 机器人 omnisocket_peer_id
+```
+
+安装服务：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp /home/eai/tg3_omnisocket_transport/tg3-omnisocket-hub.service \
+  ~/.config/systemd/user/
+cp /home/eai/tg3_omnisocket_transport/tg3-omnisocket-sender.service \
+  ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable --now tg3-omnisocket-hub.service
+systemctl --user enable --now tg3-omnisocket-sender.service
+sudo loginctl enable-linger eai
+```
+
+EAI 控制局域网 IP 变化后，在机器人修改：
+
+```toml
+[network]
+omnisocket_server = "<新EAI局域网IP>:14049"
+iarm_endpoint = "tcp://<新EAI局域网IP>:5003" # 仅保留作 ZMQ 回退
+```
+
+KCP 使用 UDP 14049，不是 TCP。防火墙只允许机器人控制局域网 IP 访问该 UDP 端口。修改后
+重启机器人 `tg3-local-teleop.service`。
+
+### 5.4 EAI 验收
+
+```bash
+systemctl --user is-active \
+  tg3-omnisocket-hub.service tg3-omnisocket-sender.service
+ss -lunp | grep ':14049'
+python3 -m json.tool /home/eai/tg3_omnisocket_transport/status.json
+```
+
+待机时应看到：
+
+- `frames_received` 持续增长；
+- `teleop_active=false`；
+- `connected=0`；
+- `frames_sent` 不增长；
+- `last_error` 为空。
+
+Hub 未启动时长按 Z+C 只消费本次请求，不会在 Hub 恢复后自行启动。必须稳定松开至少
+0.5 秒，再重新长按 3 秒。
+
+## 6. 场景三：只更换机器人
+
+这是风险最高的单项迁移。旧机器人的 Home、手部校准和编译产物不能直接当成新机器人的
+实测配置。
+
+### 6.1 新机器人前置条件
+
+确认：
+
+```bash
+uname -m
+test -f /opt/ros/jazzy/setup.bash
+test -f /home/nvidia/xos/setup.bash
+test -f /opt/robot_tele_server/install/setup.bash
+ros2 topic type /hric/robot/rl_state
+ros2 topic type /freq_change/arm_status
+ros2 topic type /encoder_identical_joint
+ros2 topic type /hric/robot/cmd_vel
+```
+
+目标应仍为天工 3.0、14 维双臂，并提供当前项目使用的 Topic。若型号、SDK 或消息类型不同，
+先适配代码，不能只改 IP。
+
+确认灵巧手：
+
+```bash
+grep -E 'left_hand_type|right_hand_type' \
+  /home/nvidia/data/param/hand_driver.yaml
+```
+
+只有左右均为 BrainCo 时才能直接启用现有 `[hands]`。因时手或其他型号必须单独适配消息和
+映射。
+
+### 6.2 在新机器人原生构建 OmniSocketGo
+
+在 PS 执行，复制源码但排除其他架构的二进制：
+
+```bash
+ssh nvidia@<新机器人SSH地址> 'mkdir -p /home/nvidia/OmniSocketGo'
+
+rsync -av \
+  --exclude=.git \
+  --exclude=bin \
+  --exclude=python/build \
+  --exclude='python/omnisocket/_omnisocket*.so' \
+  ~/Desktop/OmniSocketGo/ \
+  nvidia@<新机器人SSH地址>:/home/nvidia/OmniSocketGo/
+
+ssh nvidia@<新机器人SSH地址> \
+  'cd /home/nvidia/OmniSocketGo && make python-ext'
+
+ssh nvidia@<新机器人SSH地址> \
+  'PYTHONPATH=/home/nvidia/OmniSocketGo/python python3 -c \
+  "from omnisocket import Session; print(Session)"'
+```
+
+EAI 的 x86_64 `.so` 不能复制给 aarch64 机器人。
+
+### 6.3 部署机器人项目并构建消息 overlay
+
+```bash
+rsync -av \
+  --exclude=__pycache__ \
+  --exclude=status.json \
+  --exclude=ros2_py/build \
+  --exclude=ros2_py/install \
+  --exclude=ros2_py/log \
+  ~/Desktop/TG3_TS1P_OmniSocket_Teleop/tg3_local_teleop/ \
+  nvidia@<新机器人SSH地址>:/home/nvidia/tg3_local_teleop/
+
+ssh nvidia@<新机器人SSH地址> 'bash -lc '\''
+  source /opt/ros/jazzy/setup.bash
+  cd /home/nvidia/tg3_local_teleop/ros2_py
+  colcon build --symlink-install --packages-select ros2_bridge_msgs
+'\''
+```
+
+录制器还需要 `python3-yaml` 和 `rsync`：
+
+```bash
+ssh nvidia@<新机器人SSH地址> \
+  'sudo apt install -y python3-yaml rsync'
+```
+
+### 6.4 修改新机器人专属配置
+
+先不要安装或启动 publish-capable service。编辑：
+
+```text
+/home/nvidia/tg3_local_teleop/config.toml
+```
+
+至少修改或重新确认：
+
+```toml
+[network]
+transport = "omnisocket"
+omnisocket_server = "<EAI局域网IP>:14049"
+omnisocket_peer_id = "tg3-<新机器人编号>-robot"
+omnisocket_expected_sender = "tg3-<新机器人编号>-iarm"
+expected_iarm_id = "<实际TS1P ID>"
+expected_iarm_type = "TS1P"
+
+[home]
+joint_goal_rad = [<新机器人实测的14个弧度值>]
+```
+
+还要核对：
+
+- `[hands].open_normalized` 与新 BrainCo 手的实测打开位；
+- `[hands].closed_normalized` 和单食指姿态；
+- `[control]` 关节限位是否仍对应当前型号和 SDK；
+- `[locomotion]` 的 Topic、方向和厂家速度上限；
+- `[head]` 的 `/head/cmd`、`/robot_state`、俯仰方向、工作区、速度/加速度和外部命令租约；
+- `base_directory` 是否仍是 `/home/nvidia/tg3_data_collection`；
+- 头部相机 Topic 是否仍为 `/ob_camera_head/...`。
+
+### 6.5 新机器人必须重新采集 Home
+
+当前仓库的 Home 是旧机器人实测姿态，不是“型号统一零位”。先用厂家认可方法把新机器人
+双臂移动到希望保存的安全 Home，然后以 monitor-only 方式运行桥，不带
+`--allow-publish`：
+
+```bash
+ssh nvidia@<新机器人SSH地址>
+cd /home/nvidia/tg3_local_teleop
+timeout 15 ./run.sh || true
+
+python3 -c 'import json; p="/home/nvidia/tg3_local_teleop/status.json"; \
+print(json.load(open(p))["robot_arm_position_rad"])'
+```
+
+把输出的 14 个弧度值写入 `[home].joint_goal_rad`。在防护、净空和急停有效的前提下，首次
+只做低速 Home 验证。Home 未确认前不要启用正式遥操服务。
+
+### 6.6 配对 Peer 并安装服务
+
+在 EAI sender unit 中把 `--target-peer` 改成新机器人 Peer；如果 EAI Peer 也改名，机器人
+`omnisocket_expected_sender` 必须同步修改。
+
+在机器人安装：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp /home/nvidia/tg3_local_teleop/tg3-local-teleop.service \
+  ~/.config/systemd/user/
+cp /home/nvidia/tg3_local_teleop/tg3-data-recorder.service \
+  ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable tg3-data-recorder.service tg3-local-teleop.service
+systemctl --user restart tg3-data-recorder.service
+systemctl --user restart tg3-local-teleop.service
+```
+
+在新机器人启用自动回收所需 helper 已包含在 `tg3_local_teleop/` 中：
+
+```text
+/home/nvidia/tg3_local_teleop/delete_ready_episode.py
+```
+
+最后在 PS 修改 `tg3-data-get-sync.service` 的 `--remote`，重新登记 SSH host key，并显式
+restart 本机同步服务。
+
+## 7. 场景四：机器人、EAI 和 PS 全部更换
+
+按以下顺序，不要同时边改边做运动测试：
+
+1. 在旧系统退出遥操和数采，等待同步 `idle/pending=0`；
+2. 备份 Git 工作树、Data_Get、机器人 config 和三个用户服务；
+3. 部署新 PS，但暂不启动同步服务；
+4. 在新 EAI 完成原厂 xTELE/TS1P 标定，确认 5003/5001；
+5. 在新 EAI 原生编译 OmniSocket，启动本地 Hub；
+6. 在新机器人原生编译 OmniSocket 和 ROS 消息 overlay；
+7. 生成全新 Peer 对，设置新 TS1P ID；
+8. 以 monitor-only 模式确认机器人 ROS 状态和新 Home；
+9. 启动机器人 recorder、机器人 bridge，再启动 EAI sender；
+10. 在新 PS 登记机器人 SSH key，启动同步服务；
+11. 先做无运动链路验收，再按第 9 节逐级做实机验收；
+12. 验收全部通过后再退役旧设备，旧 Data_Get 至少保留一份只读备份。
+
+## 8. 修改项速查表
+
+| 发生变化 | 必须修改 |
+|---|---|
+| 只换 PS IP | 实时控制无需修改；检查 PS 到 Nvidia SSH 可达 |
+| PS 用户名或项目路径变化 | PS 同步 unit 的 `WorkingDirectory`、脚本路径、`--destination` |
+| Nvidia SSH IP 变化 | PS 同步 unit 的 `--remote`、SSH host key 和运维命令 |
+| EAI SSH IP 变化 | 仅 PS SSH 配置/运维命令；控制配置取决于 EAI 局域网 IP 是否变化 |
+| EAI 控制局域网 IP 变化 | 机器人 `omnisocket_server`；防火墙；保留的 `iarm_endpoint` |
+| Hub UDP 端口变化 | EAI Hub listen、EAI sender `--server`、机器人 `omnisocket_server`、防火墙 |
+| 换机器人 | Robot Peer、EAI target Peer、新 Home、手型/端点、PS `--remote` |
+| 换 TS1P | EAI 原厂标定、EAI Peer、机器人 `expected_iarm_id`，必要时方向和按键复核 |
+| 换灵巧手型号 | 必须改代码和消息映射；不能只换 Topic 字符串 |
+| 相机未安装或未启动 | 不阻塞数采；manifest 标记 `absent` |
+| 头部相机启动 | 下一条数采自动包含头部压缩 RGB-D，并记录质量状态 |
+| 腰部相机启动 | 不录制；当前白名单不含 `/ob_camera_waist/...` |
+
+## 9. 统一启动顺序与现场验收
+
+### 9.1 启动顺序
+
+确认机器人未武装后：
+
+```bash
+# 1. EAI 本地 Hub
+ssh eai 'systemctl --user restart tg3-omnisocket-hub.service'
+
+# 2. 机器人 recorder 和 bridge
+ssh nvidia@<机器人SSH地址> \
+  'systemctl --user restart tg3-data-recorder.service tg3-local-teleop.service'
+
+# 3. EAI sender
+ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
+
+# 4. PS 数据同步
+systemctl --user restart tg3-data-get-sync.service
+```
+
+### 9.2 无运动验收
+
+```bash
+cd ~/Desktop/TG3_TS1P_OmniSocket_Teleop
+./verify.sh
+
+ssh eai 'systemctl --user is-active \
+  tg3-omnisocket-hub.service tg3-omnisocket-sender.service'
+
+ssh nvidia@<机器人SSH地址> \
+  'systemctl --user is-active tg3-data-recorder.service tg3-local-teleop.service'
+
+ssh nvidia@<机器人SSH地址> \
+  'python3 -m json.tool /home/nvidia/tg3_local_teleop/status.json | head -120'
+
+python3 -m json.tool Data_Get/sync_status.json
+```
+
+必须确认：
+
+- 机器人处于 `HBWALK/HBWALK/running`，但 `armed=false`；
+- 14 个手臂反馈错误码为 0；
+- 左右手反馈存在且型号正确；
+- OmniSocket receiver 已连接并注册；
+- EAI 本地 5003/5001 数据持续更新；
+- 待机时 EAI 不发送业务帧；
+- PS 同步无错误且 pending 为 0；
+- 头部相机未启动不影响 recorder 服务 active；腰部相机不在数采白名单。
+
+### 9.3 逐级运动验收
+
+全程确保防护、净空、急停有效，操作者先按厂家流程进入 HBWALK：
+
+1. 松开扳机和摇杆，TS1P 与机器人姿态尽量接近；
+2. 长按左 Z + 右 C 3 秒启动；
+3. 先验证双臂小幅单关节跟随；
+4. 小幅验证左右扳机对应的双手；
+5. 长按右 B 1 秒验证左右手同时单食指，再松开 0.5 秒、长按 1 秒退出；
+6. 确认双摇杆回中后，直接小幅推动左摇杆验证前后，回中应立即零速；
+7. 直接小幅横推右摇杆验证原地转向，回中应立即零速；
+8. 右摇杆纵轴回中后，按住左 Z 再小幅上推/下推，确认头部相机视角
+   抬高/降低；同时小幅横向偏移不应让机器人转向，松开 Z 或回中后停止更新；
+9. L3 长按 1 秒开始短数采，再次长按 1 秒结束；
+10. 长按 Z+C 3 秒结束遥操，确认限速回新机器人的 Home。
+
+不要用本项目发布 `gotoHBWALK`，不要向厂家 `/proc_manager/config/notify` 发送伪进程启停
+JSON，也不要为了迁移删除厂家限位、电流、碰撞或急停保护。
+
+### 9.4 数采验收
+
+结束短数采后，在 PS 检查：
+
+```bash
+python3 -m json.tool Data_Get/sync_status.json
+find Data_Get -mindepth 1 -maxdepth 1 -type d -printf '%T@ %f\n' | sort -nr | head
+```
+
+最新 episode 必须包含：
+
+```text
+READY
+VERIFIED
+manifest.json
+bag/metadata.yaml
+bag/*.mcap
+```
+
+`manifest.json` 中应满足：
+
+- `state=complete`、`status=ready`；
+- `bag_info_validation.passed=true`；
+- 所有 `required_topic_message_counts` 大于 0；
+- 已启动的相机为 `healthy`，未启动为 `absent`；
+- `data_quality_warnings` 为空，或已明确接受其中的相机降级说明。
+
+同步状态应为 `idle`、`pending_remote_cleanup=0`。Nvidia 的 `active/ready/failed` 最终应为空；
+本机完整验真前不得人工删除机器人 `ready/`。
+
+## 10. 回滚与故障边界
+
+### 10.1 新机器人 Home 未确认
+
+保持 `tg3-local-teleop.service` 停止，只运行 monitor-only。不要把旧 Home 当作新机器人的
+零位，也不要测试自动回 Home。
+
+### 10.2 EAI 5003 或 5001 不正常
+
+停止 sender，先修复原厂 xTELE。5003 缺失时双臂和按键无数据；5001 缺失时厂家处理后的
+组合手势不会进入机器人。
+
+### 10.3 本地 Hub 不可用
+
+先恢复 EAI `tg3-omnisocket-hub.service` 和 UDP 14049，再重启机器人桥。已经失败的 Z+C
+请求不会自动恢复，必须松开 0.5 秒后重新长按。
+
+### 10.4 同步显示 delete_pending
+
+本机数据已经发布，但机器人回收尚未确认。不要删除本机 episode。检查 SSH、host key、
+`delete_ready_episode.py` 和机器人磁盘；服务会按 30 秒至 15 分钟退避自动重试。
+
+若 pending 收据属于旧机器人 IP，系统会保守拒绝把它用于新机器人删除，但不会阻塞其他新
+episode。迁移前应在旧地址完成 pending，或人工核对本机与旧机器人 manifest 后再处理。
+
+### 10.5 回到旧设备
+
+1. 退出遥操并停止新系统的 EAI sender、机器人 bridge 和 PS sync；
+2. 恢复备份的 unit 与机器人专属 config；
+3. 按 Hub → recorder/bridge → sender → sync 顺序启动；
+4. 仍从无运动验收开始，不能直接恢复大幅跟随或行走。
+
+## 11. 每套设备的迁移记录模板
+
+```text
+迁移日期：
+项目 Git commit：
+OmniSocketGo commit：de3f5c96779dbe1571c10feb22fc7f2331b6b222
+
+PS 主机：
+  用户/项目路径：
+  Data_Get 路径：
+  Nvidia SSH 地址：
+
+EAI：
+  SSH 地址：
+  控制局域网 IP：
+  xTELE 版本：
+  TS1P ID/类型：
+  EAI Peer：
+  Hub UDP 地址：
+
+机器人：
+  机器人编号：
+  Nvidia SSH 地址：
+  控制局域网 IP：
+  Robot Peer：
+  SDK/固件版本：
+  左右手型号：
+  Home 14 维值：
+  头相机服务/Topic：
+  腰相机服务/Topic：
+
+验收：
+  离线测试：
+  双臂：
+  双手：
+  单食指：
+  前后行走：
+  原地转向：
+  自动 Home：
+  数采/VERIFIED：
+  机器人临时副本回收：
+```

docs/天工3.0本地同构臂遥操迁移部署指南.md

@@ -839,7 +839,7 @@ mode = monitor-only
 | 左右扳机 BrainCo 手控 | 5003 原始 `hand.position` 存在；当前禁止武装 |
 | 右 B 长按 1 秒单食指 | 5003 原始 B 键存在；当前禁止武装 |
 | Z+C 长按 3 秒启动/停止、STOP 后限速 Home | 改由机器人本地计时；断流可锁存按键，当前禁止武装 |
-| C + 左摇杆行走、Z + 右摇杆转向 | 代码路径存在；当前必须 `enabled=false` |
+| 左摇杆直接行走、右摇杆直接转向 | 代码路径存在；当前必须 `enabled=false` |
 | xTELE 5001 处理后的其他六维组合手势 | 不保留；机器人只订阅 5003 |
 | OmniSocket session ID、Peer 校验、300 ms 晚包过滤 | 不使用 |
 | 公网 Hub、EAI OmniSocket sender | 不需要 |
@@ -855,7 +855,7 @@ Hub 模式，不是本节的“完全无 Hub”直连。
 因此纯 ZMQ 模式断网后：
 
 - 最后一帧双臂和手目标仍会以桥的周期重复发布；
-- 如果行走仍启用且最后一帧是非零速度组合，非零 `/hric/robot/cmd_vel` 也会持续发布；
+- 如果行走仍启用且最后一帧摇杆速度非零，非零 `/hric/robot/cmd_vel` 也会持续发布；
 - 如果最后一帧仍按住 Z+C，本地 3 秒计时会在没有新数据时继续，可能在断网后武装或停止；
 - 网络恢复后会直接继续当前 armed 会话，不要求重新 Z+C；
 - `connected=true` 不能证明数据仍然新鲜。
@@ -914,8 +914,8 @@ systemctl --user enable --now tg3-omnisocket-sender.service
 5. 松开两个扳机，长按左 Z + 右 C 3 秒启动；
 6. 先做小幅单关节跟随，再逐渐扩大动作；
 7. 分别小幅扣左右扳机，验证双手方向、范围和限速；
-8. 保持右摇杆回中，连续长按右 B 1 秒，确认右手以 `400 units/s` 限速进入厂商
-   “单食指”姿态；松开至少 0.5 秒后再次长按 1 秒，确认退出并恢复正常右手跟随；
+8. 保持右摇杆回中，连续长按右 B 1 秒，确认左右手都以 `400 units/s` 限速进入厂家
+   “单食指”姿态；松开至少 0.5 秒后再次长按 1 秒，确认退出并恢复双手正常跟随；
 9. 仅在 OmniSocket/5001 合并模式下：先停止并确认 `armed=false`，按飞书指南逐个选择
    其他手势组合键；检查 EAI
    `status.json` 的 `command_frames_accepted` 增长，并在 EAI 本机 5001 抓帧确认处理后的
@@ -923,17 +923,18 @@ systemctl --user enable --now tg3-omnisocket-sender.service
 10. 仅在 OmniSocket/5001 合并模式下：再次武装后检查 `command_hand_merges` 增长和机器人
     `iarm_hand_position` 为六维，
     再只做小幅动作，逐个验证所需的其他组合手势方向和限速；
-11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：保持右 C，把左摇杆
-    小幅向前推出死区，确认下一个 50 Hz 周期立即响应；松 C，确认
-    立即零速停止。再次按 C/推杆也应立即响应，不再等待 3 秒；
-12. 在同一行走前提下，保持左 Z，把右摇杆小幅横推，确认机器人原地转向；松开任一输入
-    应立即清零角速度；
-13. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home；
-14. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP
+11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：先确认双摇杆回中，
+    再把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；回中后确认立即零速停止；
+12. 在同一行走前提下，直接把右摇杆小幅横推，确认机器人原地转向；回中应立即清零角速度；
+13. 确认右摇杆纵轴回中后，按住左 Z 再小幅上推/下推，验证头部相机抬头/低头；
+    按 Z 期间右摇杆水平轴不应让机器人转向。检查角度始终位于 `-10°～+50°`，且
+    外部 `/head/cmd` 活跃时本项目会暂停头部输出；
+14. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home；
+15. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP
     反馈超时约为 500 ms，触发后会作废 session；机器人若持续约 2 秒收不到活动帧，会另行
     重建我们的接收桥。不应期待断线后透明续控；恢复后先确认未武装，稳定松开 Z+C 后再重新长按
     启动。纯 ZMQ 模式在完成本地断流门控前禁止做运动中的断链测试；
-15. 记录 SSH 地址、Home、行走限速、手部端点和单食指姿态到设备档案；OmniSocket 模式
+16. 记录 SSH 地址、Home、行走限速、手部端点和单食指姿态到设备档案；OmniSocket 模式
     另记 Peer ID/Hub，纯 ZMQ 模式改记 EAI 固定局域网 IP、TCP 5003 防火墙范围和断流门控参数。
 
 迁移时确认目标机提供 `/hric/robot/cmd_vel`（`geometry_msgs/msg/TwistStamped`），并保持
@@ -1012,3 +1013,122 @@ Hub IP/端口变化时，额外同时修改 EAI `--server` 和机器人 `omnisoc
 当前没有直连断流门控的版本中设置 `locomotion.enabled=false`，同时停用 EAI 的
 `tg3-omnisocket-sender.service`，并仅以机器人 monitor-only 模式验证；完成第 7.5 节的
 ZMQ 专用断流门控前不得启用运动发布。
+
+## 11. 数采部署与迁移
+
+数采是本项目新增功能，不调用 `/bag_record/control/notify`，也不停止或修改 Ubuntu 厂家
+`record_bag_node`。Nvidia 独立录制，PS 本机负责把完成数据同步到：
+
+```text
+/home/ps/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get
+```
+
+### 11.1 机器人 Nvidia
+
+部署 `tg3_local_teleop/` 全目录，至少必须包含：
+
+```text
+tg3_local_teleop.py
+head_control.py
+data_collection.py
+data_recorder_protocol.py
+data_recorder_node.py
+delete_ready_episode.py
+config.toml
+run.sh
+run_data_recorder.sh
+wait_ros_ready.sh
+tg3-local-teleop.service
+tg3-data-recorder.service
+ros2_py/
+```
+
+目标路径固定为 `/home/nvidia/tg3_local_teleop`。先按原项目步骤构建 `ros2_py`，再安装：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp /home/nvidia/tg3_local_teleop/tg3-data-recorder.service \
+   /home/nvidia/tg3_local_teleop/tg3-local-teleop.service \
+   ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable --now tg3-data-recorder.service
+systemctl --user restart tg3-local-teleop.service
+```
+
+录制脚本必须依次加载 `/opt/ros/jazzy`、`/home/nvidia/xos` 和
+`/opt/robot_tele_server/install`，但**不能**再加载本项目 `ros2_py/install`。后者只是遥操桥
+Python 进程专用的消息绑定（含 `ArmStatus`/`RobotState`/`HeadCtrl`），并不是
+厂家完整 `ros2_bridge_msgs` 包；它会遮蔽厂家 C++ 库，导致
+rosbag 加载 `RobotState/ArmCtrl` 时出现 undefined symbol。`run.sh` 仍需项目 overlay；
+`run_data_recorder.sh` 只用厂家完整消息 overlay。
+还需 Python 3 的 PyYAML（Ubuntu 包 `python3-yaml`）解析 rosbag metadata。
+迁移时在 `config.toml [data_collection]` 核对 `base_directory`、100 GiB 余量、30 分钟
+上限、明确 topic 白名单和 required topics。不要改成 `ros2 bag record -a`，点云需另行
+估算带宽和磁盘后再加入。当前白名单只包含头部 Orbbec 的 JPEG RGB、无损 PNG 深度、
+CameraInfo、Metadata 和 `/tf_static`。头部相机是 optional：服务已启动时 rosbag 自动
+发现并采集；未启动时不影响核心 episode 开始或进入 READY。收尾后 manifest 会分别把
+头部相机标成 `absent`、`healthy`、`partial` 或 `low_rate`，后两种附带质量警告但不阻塞
+核心数据保存。
+
+需要采集头部相机的完整 episode 时，先确认相机服务和发布者；不需要相机时可保持服务
+停止，L3 数采仍可正常启动：
+
+```bash
+systemctl is-active orbbec_head.service
+sudo systemctl enable --now orbbec_head.service
+
+ros2 topic info /ob_camera_head/color/image_raw/compressed
+ros2 topic info /ob_camera_head/depth/image_raw/compressedDepth
+```
+
+希望釆头部图像时，两条话题都应显示 `Publisher count: 1`。原始
+1280x720 RGB+深度约
+`8.3 GB/min/相机`，所以默认不重复录 raw，而是录标准 image_transport 压缩流；现场
+单相机约 `1.3–1.5 GB/min`，实际随画面变化。`minimum_free_gib=100` 是停止下限，
+不是整条 episode 的容量预算；长录制前至少预留“100 GiB + 预计数据量”，并在短录包中
+先确认实际带宽。
+
+### 11.2 PS 本机
+
+项目需包含 `tg3_data_collection/` 和 `Data_Get/`。确认到新 Nvidia 的免密 SSH 后安装：
+
+```bash
+ssh -o BatchMode=yes nvidia@192.168.41.2 true
+mkdir -p ~/.config/systemd/user
+cp tg3_data_collection/tg3-data-get-sync.service ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable --now tg3-data-get-sync.service
+```
+
+机器人 SSH IP 改变时，只改同步 unit；最简命令（替换两个地址）是：
+
+```bash
+sed -i 's/nvidia@旧IP/nvidia@新IP/g' \
+  ~/.config/systemd/user/tg3-data-get-sync.service
+systemctl --user daemon-reload
+systemctl --user restart tg3-data-get-sync.service
+```
+
+这不修改 OmniSocket Hub、Peer ID 或机器人 `config.toml`。若 PS 用户名/项目路径改变，
+同时修改 unit 的 `WorkingDirectory`、脚本路径和 `--destination`。
+
+### 11.3 操作与验收
+
+1. 保持 L3 松开，长按 Z+C 3 秒正常开启遥操；
+2. 左摇杆按下（L3）连续 1 秒开始数采；该键是 `button_joystick.left`，不是 X/Y/Z；
+3. 松开 L3 至少 0.5 秒，再长按 1 秒结束；或正常 Z+C 结束遥操自动收尾；
+4. 查看 Nvidia `/tg3/data_collection/status`，必须先到 `ready`，不能从 `active/` 取数据；
+   recorder 会要求核心 required topics 有实时发布者且最终消息计数大于零；相机质量看
+   manifest 的 `optional_topic_groups`，相机未启动显示 `absent`，不会阻塞；
+5. 查看本机 `Data_Get/sync_status.json`，最终目录必须有 `READY`、`manifest.json`、
+   `VERIFIED`、`bag/metadata.yaml` 和非空 `*.mcap`；
+6. 执行 `ros2 bag info <episode>/bag` 并确认所需 topic 有消息；
+7. 断开 PS 网络再采一条，确认 Nvidia 暂存 `ready/`；恢复网络后应续传，完成 SHA-256、
+   fsync 和 `VERIFIED` 后，只删除机器人上 manifest 完全匹配的该 episode。
+
+每个新会话先要求 L3 稳定松开 0.5 秒，同一次持续按压只切换一次。数采故障不得解除
+遥操或延迟 STOP/Home；桥心跳中断超过 3 秒时 recorder 会自行 SIGINT 收尾。当前架构在
+录制期间必须先写 Nvidia 的项目专用 `active/`，PS 离线时也会暂存在 `ready/`；它不是
+长期副本。本机完成全文件验真并持久化 `VERIFIED` 后，固定根目录删除助手才会原子移走并
+删除该 exact episode，不能触碰 `active/`、`failed/` 或根目录。`retain_failed_episodes=false`
+时失败录包也不在机器人保留。EAI 只转发控制数据，不保存 MCAP 或相机 payload。

tg3_data_collection/README.md

@@ -0,0 +1,148 @@
+# TG3 data episode sync
+
+机器人只在 `/home/nvidia/tg3_data_collection/ready` 暴露已经收到 SIGINT、写完
+`metadata.yaml`、通过 `ros2 bag info` 并生成 SHA-256 清单的 episode。本机服务用免密 SSH/rsync 复制到
+`Data_Get/.incoming`，逐文件校验并 fsync 后再原子改名为 `Data_Get/<episode_id>`。本机
+`VERIFIED` 收据持久化成功后，正式服务会再次完整哈希本机 payload，再让 Nvidia 侧固定根目录
+helper 比对原始 manifest SHA-256，原子移入 `deleting/` 后清理。因此完整数据最终只保留在 PS 本机，Nvidia 只是临时
+staging；中断、哈希错误或本机落盘失败都不会触发远端删除。
+
+数据只走本机到 Nvidia 的 SSH 链路，与公网或 EAI 本地 OmniSocket Hub 无关。当前默认：
+
+```text
+nvidia@192.168.41.2:/home/nvidia/tg3_data_collection/ready/
+  -> /home/ps/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get/
+```
+
+EAI 不运行 recorder/sync，也不保存 MCAP 或图像数据。
+
+先确认免密与依赖：
+
+```bash
+ssh -o BatchMode=yes nvidia@192.168.41.2 true
+command -v rsync
+ssh nvidia@192.168.41.2 command -v rsync
+```
+
+先把固定根目录删除 helper 部署到 Nvidia。它的 CLI 只接受安全 episode ID 和 64 位
+manifest SHA-256，不接受 root 参数，也不会枚举或触碰 `active/`、`failed/`。helper 在
+`deleting/` 树之外的 `delete_ledger/` 先持久化独立删除授权；即使递归删除中断且 tombstone
+里的 READY/manifest 已消失，仍能凭该授权安全续删，最后才删除授权文件：
+
+```bash
+scp tg3_local_teleop/delete_ready_episode.py \
+  nvidia@192.168.41.2:/home/nvidia/tg3_local_teleop/
+ssh nvidia@192.168.41.2 \
+  chmod 755 /home/nvidia/tg3_local_teleop/delete_ready_episode.py
+```
+
+安装本机用户服务：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp tg3_data_collection/tg3-data-get-sync.service ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable --now tg3-data-get-sync.service
+```
+
+状态：
+
+```bash
+systemctl --user --no-pager status tg3-data-get-sync.service
+python3 -m json.tool Data_Get/sync_status.json
+```
+
+也可以只执行一次，便于首次部署验收：
+
+```bash
+systemctl --user stop tg3-data-get-sync.service
+python3 tg3_data_collection/data_get_sync.py --once --delete-remote-after-sync
+systemctl --user start tg3-data-get-sync.service
+```
+
+同步器使用 `Data_Get/.data_get_sync.lock` 做 flock；正式服务运行时，另一个 `--once` 会明确
+报“已有同步进程”，不会与服务同时复制或删除。不带 `--delete-remote-after-sync` 的手动命令
+只复制、验真并保留远端，正式 unit 已显式启用自动回收。
+
+日常轮询不会每 2 秒重新读取并哈希全部历史 MCAP；首次原子发布时已经完成深度校验。
+如果进程在远端删除 ACK 返回前退出，`VERIFIED` 会保持 `pending`。每次真正调用 helper
+之前都会重新完整哈希本机 payload；失败后从 30 秒开始指数退避，最大 15 分钟，进程重启也
+遵守持久化的 `next_retry_unix_s`，避免每 2 秒重读大型 MCAP。helper 使用独立 600 秒超时，
+能继续清理同摘要 tombstone/ledger，也会把已经不存在的同一目标视为成功。
+需要定期审计已有本地数据时单独执行（可能耗时较长）：
+
+```bash
+python3 tg3_data_collection/data_get_sync.py --once --verify-existing
+```
+
+某个旧 episode 损坏会写入 `sync_status.json`，但不会阻止同一轮继续拉取其他新 episode。
+
+一个完成目录至少包含：
+
+```text
+<episode_id>/
+  READY
+  VERIFIED
+  manifest.json
+  bag/metadata.yaml
+  bag/*.mcap
+  ros2_bag.stdout.log
+  ros2_bag.stderr.log
+```
+
+同步端会再次核对 `READY`、episode ID、每个 MCAP/metadata 的长度和 SHA-256；远端
+manifest 在传输中发生变化也会拒绝发布最终目录。随后 fsync 所有复制文件和目录、以
+NOREPLACE 语义原子发布、fsync 父目录，最后写入并 fsync `VERIFIED`。收据绑定 episode、
+远端身份、原始 manifest SHA-256、已校验文件数和字节数，并记录 `pending/deleted`。
+`.incoming`、`sync_status.json` 和全部 episode 已由 `Data_Get/.gitignore` 排除。
+
+临时目录按 `<episode>.<manifest_sha256>.partial` 隔离，rsync 同时使用删除同步；发布前还会
+拒绝 manifest 未列出的任何 `*.mcap` 或 `metadata.yaml`，避免旧中断文件混入新 episode。
+
+`sync_status.json` 的 `state=delete_pending` 表示本机数据已完整发布，但远端回收尚未收到
+成功 ACK；`pending_remote_cleanup`、`last_delete_error`、`delete_count` 和
+`last_deleted_episode` 可用于排查。此状态下不要手工删除本机数据，服务会在下一轮重试。
+
+已有本机同名目录不会直接授权删除：没有可信 `VERIFIED` 时必须重新做完整哈希，并要求
+本机与当前远端 manifest 原始摘要一致，之后才补收据并回收远端。同名碰撞、损坏、符号
+链接、错误 root 或摘要不符都保留远端并报错。
+
+历史 `retained/deleted` 收据可以来自旧机器人 IP，不会阻塞新地址的同步；仍为 `pending` 的
+跨 IP 收据不会静默授权新机器人删除，只会逐项报错且不影响其他新 episode 继续同步。
+
+自动回收 helper 严格只覆盖 `ready/`，录制中的 `active/` 永远不能由 PS 同步器处理，
+也不能对 `failed/` 使用 READY 凭据。当前正式配置为
+`retain_failed_episodes=false`：录制器只把失败原因写入状态和 journal，随后精确删除自己
+刚创建的失败 payload，因此机器人不会长期保留失败 MCAP。若以后改回 `true` 以保留故障
+诊断，必须另建独立验真与回收协议，不能整目录清空。
+
+## 解码双臂关节数据
+
+MCAP 内的自定义 TG3 消息使用 CDR 编码。解码器读取项目保存的消息定义，并把实际反馈、
+遥操目标、厂家桥后命令和 xTELE 原始目标分别导出为长表 CSV。首次使用在独立虚拟环境
+安装依赖：
+
+```bash
+python3 -m venv /tmp/tg3-mcap-venv
+/tmp/tg3-mcap-venv/bin/pip install mcap==1.4.0 mcap-ros2-support==0.5.7
+
+/tmp/tg3-mcap-venv/bin/python tg3_data_collection/decode_joint_data.py \
+  Data_Get/<episode_id>
+```
+
+输出位于 `Data_Get/<episode_id>/decoded_joints/`：
+
+- `robot_arm_feedback.csv`：`/robot_state`、`/freq_change/arm_status` 和
+  `/data_logger/arm_status` 的实测位置、速度、电流、温度及错误码；
+- `teleop_joint_target.csv`：`/encoder_identical_joint` 的 14 轴遥操目标；
+- `vendor_arm_command.csv`：`/arm/cmd` 的厂家桥后控制参数；
+- `iarm_source_joint.csv`：`/tg3/data_collection/iarm_frame` 中的 TS1P 原始目标；
+- `summary.json`：关节顺序、Motor ID、单位和行数。
+
+关节顺序为左臂 7 轴后右臂 7 轴，Motor ID 分别为 `11..17` 和 `21..27`；位置单位
+为 rad，速度单位为 rad/s。输出目录必须不存在，避免误覆盖已经分析或标注过的数据。
+
+迁移后需要同步修改 service 中的本机项目路径和机器人 SSH 地址。如果 Nvidia 的
+`192.168.41.2` 改变，只改 unit 的 `--remote` 并重新部署 helper；它不在机器人
+`config.toml` 或 OmniSocket Peer 设置中。启用自动删除时远端路径必须保持固定的
+`/home/nvidia/tg3_data_collection/ready`，同步器会拒绝对其他 root 启用删除。

tg3_data_collection/decode_joint_data.py

@@ -0,0 +1,377 @@
+#!/usr/bin/env python3
+"""Decode TG3 joint streams from one locally stored MCAP episode into CSV."""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+from pathlib import Path
+from typing import Any, Callable
+
+
+ARM_MOTOR_IDS = [*range(11, 18), *range(21, 28)]
+JOINT_NAMES = [
+    *(f"left_joints_{index}" for index in range(7)),
+    *(f"right_joints_{index}" for index in range(7)),
+]
+CUSTOM_TOPICS = {
+    "/robot_state": "ros2_bridge_msgs/msg/RobotState",
+    "/freq_change/arm_status": "ros2_bridge_msgs/msg/ArmStatus",
+    "/data_logger/arm_status": "ros2_bridge_msgs/msg/ArmStatus",
+    "/arm/cmd": "ros2_bridge_msgs/msg/ArmCtrl",
+}
+TOPICS = [
+    "/robot_state",
+    "/encoder_identical_joint",
+    "/freq_change/arm_status",
+    "/data_logger/arm_status",
+    "/arm/cmd",
+    "/tg3/data_collection/iarm_frame",
+]
+
+
+def _message_directory() -> Path:
+    return (
+        Path(__file__).resolve().parents[1]
+        / "tg3_local_teleop"
+        / "ros2_py"
+        / "src"
+        / "ros2_bridge_msgs"
+        / "msg"
+    )
+
+
+def _read_definition(name: str) -> str:
+    path = _message_directory() / f"{name}.msg"
+    if not path.is_file():
+        raise RuntimeError(f"required message definition is missing: {path}")
+    return path.read_text(encoding="utf-8")
+
+
+def _definition_bundle(root_name: str, dependencies: list[tuple[str, str]]) -> str:
+    if root_name == "ArmCtrl":
+        text = (
+            "std_msgs/Header header\n"
+            "uint8 mode\n"
+            "uint8 label\n"
+            "uint8 reserved\n"
+            "ros2_bridge_msgs/MotorCtrl[] ctrl\n"
+        )
+    else:
+        text = _read_definition(root_name)
+    for full_name, definition in dependencies:
+        text += f"\n===\nMSG: {full_name}\n{definition}"
+    return text
+
+
+def _custom_decoders() -> dict[str, Callable[[bytes], Any]]:
+    try:
+        from mcap_ros2._dynamic import generate_dynamic
+    except ImportError as exc:
+        raise RuntimeError(
+            "missing MCAP decoder; install with: pip install mcap mcap-ros2-support"
+        ) from exc
+
+    header = "builtin_interfaces/Time stamp\nstring frame_id\n"
+    motor_status = _read_definition("MotorStatus")
+    common = [
+        ("std_msgs/msg/Header", header),
+        ("ros2_bridge_msgs/msg/MotorStatus", motor_status),
+    ]
+    robot_dependencies = common + [
+        (f"ros2_bridge_msgs/msg/{name}", _read_definition(name))
+        for name in (
+            "HeadStatus",
+            "WaistStatus",
+            "LegStatus",
+            "ArmStatus",
+            "ImuStatus",
+        )
+    ]
+    schemas = {
+        "ros2_bridge_msgs/msg/RobotState": _definition_bundle(
+            "RobotState", robot_dependencies
+        ),
+        "ros2_bridge_msgs/msg/ArmStatus": _definition_bundle(
+            "ArmStatus", common
+        ),
+        "ros2_bridge_msgs/msg/ArmCtrl": _definition_bundle(
+            "ArmCtrl",
+            [
+                ("std_msgs/msg/Header", header),
+                ("ros2_bridge_msgs/msg/MotorCtrl", _read_definition("MotorCtrl")),
+            ],
+        ),
+    }
+    return {
+        type_name: generate_dynamic(type_name, text)[type_name]
+        for type_name, text in schemas.items()
+    }
+
+
+def _stamp_ns(header: Any) -> int:
+    return int(header.stamp.sec) * 1_000_000_000 + int(header.stamp.nanosec)
+
+
+def _side_and_index(motor_id: int) -> tuple[str, int]:
+    if 11 <= motor_id <= 17:
+        return "left", motor_id - 11
+    if 21 <= motor_id <= 27:
+        return "right", motor_id - 21
+    return "unknown", -1
+
+
+def _open_csv(path: Path, columns: list[str]) -> tuple[Any, csv.DictWriter]:
+    stream = path.open("x", encoding="utf-8", newline="")
+    writer = csv.DictWriter(stream, fieldnames=columns)
+    writer.writeheader()
+    return stream, writer
+
+
+def decode_episode(episode: Path, output: Path) -> dict[str, int]:
+    try:
+        from mcap.reader import make_reader
+        from mcap_ros2.decoder import DecoderFactory
+    except ImportError as exc:
+        raise RuntimeError(
+            "missing MCAP decoder; install with: pip install mcap mcap-ros2-support"
+        ) from exc
+
+    bag_files = sorted((episode / "bag").glob("*.mcap"))
+    if not bag_files:
+        raise RuntimeError(f"no MCAP files found under {episode / 'bag'}")
+    output.mkdir(parents=True, exist_ok=False)
+
+    feedback_columns = [
+        "bag_time_ns",
+        "ros_time_ns",
+        "topic",
+        "side",
+        "joint_index",
+        "motor_id",
+        "position_rad",
+        "speed_rad_s",
+        "current_a",
+        "temperature_c",
+        "mos_temperature_c",
+        "error",
+    ]
+    target_columns = [
+        "bag_time_ns",
+        "ros_time_ns",
+        "joint_index",
+        "joint_name",
+        "motor_id",
+        "side",
+        "position_rad",
+        "velocity_rad_s",
+        "effort",
+    ]
+    command_columns = [
+        "bag_time_ns",
+        "ros_time_ns",
+        "mode",
+        "label",
+        "motor_id",
+        "side",
+        "joint_index",
+        "kp",
+        "kd",
+        "position_rad",
+        "speed_rad_s",
+        "torque_feedforward",
+        "current_limit_a",
+    ]
+    source_columns = [
+        "bag_time_ns",
+        "source_time_ms",
+        "joint_index",
+        "joint_name",
+        "motor_id",
+        "side",
+        "position_rad",
+    ]
+    streams: list[Any] = []
+    feedback_stream, feedback_writer = _open_csv(
+        output / "robot_arm_feedback.csv", feedback_columns
+    )
+    target_stream, target_writer = _open_csv(
+        output / "teleop_joint_target.csv", target_columns
+    )
+    command_stream, command_writer = _open_csv(
+        output / "vendor_arm_command.csv", command_columns
+    )
+    source_stream, source_writer = _open_csv(
+        output / "iarm_source_joint.csv", source_columns
+    )
+    streams.extend((feedback_stream, target_stream, command_stream, source_stream))
+
+    counts = {
+        "robot_arm_feedback_rows": 0,
+        "teleop_joint_target_rows": 0,
+        "vendor_arm_command_rows": 0,
+        "iarm_source_joint_rows": 0,
+    }
+    custom_decoders = _custom_decoders()
+    standard_factory = DecoderFactory()
+    try:
+        for bag_file in bag_files:
+            with bag_file.open("rb") as bag_stream:
+                reader = make_reader(bag_stream)
+                for schema, channel, message in reader.iter_messages(topics=TOPICS):
+                    topic = channel.topic
+                    if topic in CUSTOM_TOPICS:
+                        decoded = custom_decoders[CUSTOM_TOPICS[topic]](message.data)
+                    else:
+                        decoder = standard_factory.decoder_for(
+                            channel.message_encoding, schema
+                        )
+                        if decoder is None:
+                            raise RuntimeError(
+                                f"no decoder for {topic} ({schema.name if schema else 'no schema'})"
+                            )
+                        decoded = decoder(message.data)
+
+                    if topic in (
+                        "/robot_state",
+                        "/freq_change/arm_status",
+                        "/data_logger/arm_status",
+                    ):
+                        status_message = decoded.arm if topic == "/robot_state" else decoded
+                        for motor in status_message.status:
+                            motor_id = int(motor.name)
+                            side, joint_index = _side_and_index(motor_id)
+                            feedback_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "ros_time_ns": _stamp_ns(decoded.header),
+                                    "topic": topic,
+                                    "side": side,
+                                    "joint_index": joint_index,
+                                    "motor_id": motor_id,
+                                    "position_rad": motor.pos,
+                                    "speed_rad_s": motor.speed,
+                                    "current_a": motor.current,
+                                    "temperature_c": motor.temperature,
+                                    "mos_temperature_c": motor.mos_temperature,
+                                    "error": motor.error,
+                                }
+                            )
+                            counts["robot_arm_feedback_rows"] += 1
+                    elif topic == "/encoder_identical_joint":
+                        for index, position in enumerate(decoded.position):
+                            motor_id = ARM_MOTOR_IDS[index] if index < 14 else -1
+                            side, _ = _side_and_index(motor_id)
+                            target_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "ros_time_ns": _stamp_ns(decoded.header),
+                                    "joint_index": index,
+                                    "joint_name": decoded.name[index]
+                                    if index < len(decoded.name)
+                                    else "",
+                                    "motor_id": motor_id,
+                                    "side": side,
+                                    "position_rad": position,
+                                    "velocity_rad_s": decoded.velocity[index]
+                                    if index < len(decoded.velocity)
+                                    else "",
+                                    "effort": decoded.effort[index]
+                                    if index < len(decoded.effort)
+                                    else "",
+                                }
+                            )
+                            counts["teleop_joint_target_rows"] += 1
+                    elif topic == "/arm/cmd":
+                        for motor in decoded.ctrl:
+                            motor_id = int(motor.name)
+                            side, joint_index = _side_and_index(motor_id)
+                            command_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "ros_time_ns": _stamp_ns(decoded.header),
+                                    "mode": decoded.mode,
+                                    "label": decoded.label,
+                                    "motor_id": motor_id,
+                                    "side": side,
+                                    "joint_index": joint_index,
+                                    "kp": motor.kp,
+                                    "kd": motor.kd,
+                                    "position_rad": motor.pos,
+                                    "speed_rad_s": motor.spd,
+                                    "torque_feedforward": motor.tor,
+                                    "current_limit_a": motor.cur,
+                                }
+                            )
+                            counts["vendor_arm_command_rows"] += 1
+                    elif topic == "/tg3/data_collection/iarm_frame":
+                        payload = json.loads(decoded.data)
+                        positions = payload.get("arm", {}).get("position", {})
+                        values = [
+                            *positions.get("left", []),
+                            *positions.get("right", []),
+                        ]
+                        if len(values) != 14:
+                            continue
+                        source_time = payload.get("timestamp", "")
+                        for index, position in enumerate(values):
+                            motor_id = ARM_MOTOR_IDS[index]
+                            side, _ = _side_and_index(motor_id)
+                            source_writer.writerow(
+                                {
+                                    "bag_time_ns": message.log_time,
+                                    "source_time_ms": source_time,
+                                    "joint_index": index,
+                                    "joint_name": JOINT_NAMES[index],
+                                    "motor_id": motor_id,
+                                    "side": side,
+                                    "position_rad": position,
+                                }
+                            )
+                            counts["iarm_source_joint_rows"] += 1
+    finally:
+        for stream in streams:
+            stream.close()
+
+    (output / "summary.json").write_text(
+        json.dumps(
+            {
+                "episode": episode.name,
+                "joint_order": JOINT_NAMES,
+                "motor_ids": ARM_MOTOR_IDS,
+                "units": {
+                    "position": "rad",
+                    "speed": "rad/s",
+                    "current": "A",
+                    "temperature": "degC",
+                },
+                "rows": counts,
+            },
+            ensure_ascii=False,
+            indent=2,
+        )
+        + "\n",
+        encoding="utf-8",
+    )
+    return counts
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("episode", type=Path, help="Data_Get episode directory")
+    parser.add_argument(
+        "--output",
+        type=Path,
+        help="new output directory (default: <episode>/decoded_joints)",
+    )
+    args = parser.parse_args()
+    episode = args.episode.resolve()
+    output = (args.output or episode / "decoded_joints").resolve()
+    counts = decode_episode(episode, output)
+    print(json.dumps({"output": str(output), "rows": counts}, indent=2))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tg3_data_collection/test_data_get_sync.py

@@ -0,0 +1,599 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import hashlib
+import json
+import os
+import shutil
+import subprocess
+import tempfile
+import unittest
+from pathlib import Path
+from unittest import mock
+
+from data_get_sync import (
+    DataGetSync,
+    FIXED_REMOTE_READY,
+    ManifestDocument,
+    VERIFIED_RECEIPT_NAME,
+    fsync_episode_tree,
+    parse_manifest,
+    safe_episode_name,
+    safe_relative_path,
+    validate_episode_dir,
+)
+
+
+class LocalRsyncRunner:
+    def __init__(self, remote_root: Path) -> None:
+        self.remote_root = remote_root
+        self.mode = "normal"
+        self.rsync_calls = 0
+
+    def run(
+        self, command: list[str], *, timeout: float
+    ) -> subprocess.CompletedProcess[str]:
+        del timeout
+        if not command or command[0] != "rsync":
+            return subprocess.CompletedProcess(command, 1, "", "unexpected command")
+        self.rsync_calls += 1
+        remote_source = command[-2].rstrip("/")
+        episode = remote_source.rsplit("/", 1)[-1]
+        destination = Path(command[-1])
+        shutil.copytree(
+            self.remote_root / episode,
+            destination,
+            dirs_exist_ok=True,
+            symlinks=True,
+        )
+        if self.mode == "truncate":
+            (destination / "bag" / "bag_0.mcap").write_bytes(b"truncated")
+        elif self.mode == "manifest_changed":
+            manifest = destination / "manifest.json"
+            manifest.write_bytes(manifest.read_bytes() + b"\n")
+        elif self.mode == "manifest_symlink":
+            manifest = destination / "manifest.json"
+            manifest.unlink()
+            manifest.symlink_to("bag/metadata.yaml")
+        return subprocess.CompletedProcess(command, 0, "", "")
+
+
+class LocalSync(DataGetSync):
+    def __init__(
+        self,
+        *,
+        remote_root: Path,
+        destination: Path,
+        runner: LocalRsyncRunner,
+        delete: bool = True,
+        remote: str = "nvidia@test",
+    ) -> None:
+        super().__init__(
+            remote=remote,
+            remote_ready=FIXED_REMOTE_READY,
+            destination=destination,
+            status_file=destination / "sync_status.json",
+            runner=runner,
+            reserve_bytes=0,
+            delete_remote_after_sync=delete,
+            remote_delete_helper="/fixed/delete_ready_episode.py",
+        )
+        self.remote_root = remote_root
+        self.delete_calls: list[tuple[str, str]] = []
+        self.delete_outcomes: list[str] = []
+
+    def list_remote_episodes(self) -> list[str]:
+        return sorted(
+            path.name
+            for path in self.remote_root.iterdir()
+            if path.is_dir() and not path.is_symlink()
+        )
+
+    def get_remote_manifest(self, episode: str) -> ManifestDocument:
+        raw = (self.remote_root / episode / "manifest.json").read_bytes()
+        return ManifestDocument.from_bytes(raw, episode)
+
+    def _invoke_delete_helper(
+        self, episode: str, manifest_sha256: str
+    ) -> dict[str, object]:
+        self.delete_calls.append((episode, manifest_sha256))
+        outcome = self.delete_outcomes.pop(0) if self.delete_outcomes else "success"
+        target = self.remote_root / episode
+        if outcome == "failure":
+            raise RuntimeError("injected delete failure")
+        if outcome == "ack_lost_after_delete":
+            if target.exists():
+                shutil.rmtree(target)
+            raise RuntimeError("injected lost acknowledgement")
+        if target.exists():
+            shutil.rmtree(target)
+            state = "deleted"
+        else:
+            state = "already_absent"
+        return {
+            "state": state,
+            "episode_id": episode,
+            "manifest_sha256": manifest_sha256,
+        }
+
+
+class DataGetSyncTests(unittest.TestCase):
+    def test_safe_episode_name(self) -> None:
+        self.assertTrue(safe_episode_name("episode_20260810T120000000_deadbeef"))
+        self.assertFalse(safe_episode_name("../escape"))
+        self.assertFalse(safe_episode_name("bad/name"))
+        self.assertFalse(safe_episode_name(""))
+
+    def test_safe_relative_path(self) -> None:
+        self.assertEqual(safe_relative_path("bag/metadata.yaml"), Path("bag/metadata.yaml"))
+        for invalid in ("", "/etc/passwd", "../escape", "bag/../../escape", None):
+            with self.subTest(invalid=invalid):
+                with self.assertRaises(ValueError):
+                    safe_relative_path(invalid)
+
+    def _episode(self, root: Path, name: str) -> tuple[Path, dict]:
+        episode = root / name
+        (episode / "bag").mkdir(parents=True)
+        mcap = episode / "bag" / "bag_0.mcap"
+        metadata = episode / "bag" / "metadata.yaml"
+        mcap.write_bytes(b"mcap-data")
+        metadata.write_text("rosbag2_bagfile_information: {}\n", encoding="utf-8")
+        files = []
+        for path in (mcap, metadata):
+            raw = path.read_bytes()
+            files.append(
+                {
+                    "path": str(path.relative_to(episode)),
+                    "size": len(raw),
+                    "sha256": hashlib.sha256(raw).hexdigest(),
+                }
+            )
+        manifest = {"state": "complete", "episode_id": name, "files": files}
+        (episode / "manifest.json").write_text(json.dumps(manifest), encoding="utf-8")
+        (episode / "READY").write_text("ready\n", encoding="ascii")
+        return episode, manifest
+
+    def test_parse_and_validate_episode(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            episode, manifest = self._episode(Path(temporary), name)
+            parsed = parse_manifest(json.dumps(manifest), name)
+            validate_episode_dir(episode, parsed)
+
+    def test_manifest_requires_complete_matching_episode_and_mcap(self) -> None:
+        digest = "0" * 64
+        base = {
+            "state": "complete",
+            "episode_id": "episode_ok",
+            "files": [{"path": "bag/bag_0.mcap", "size": 1, "sha256": digest}],
+        }
+        with self.assertRaises(ValueError):
+            parse_manifest(json.dumps({**base, "state": "active"}), "episode_ok")
+        with self.assertRaises(ValueError):
+            parse_manifest(json.dumps(base), "episode_other")
+        no_mcap = {
+            **base,
+            "files": [{"path": "bag/metadata.yaml", "size": 1, "sha256": digest}],
+        }
+        with self.assertRaises(ValueError):
+            parse_manifest(json.dumps(no_mcap), "episode_ok")
+
+    def test_validate_detects_tampering(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            episode, manifest = self._episode(Path(temporary), name)
+            (episode / "bag" / "bag_0.mcap").write_bytes(b"changed")
+            with self.assertRaises(ValueError):
+                validate_episode_dir(episode, manifest)
+
+    def test_validate_requires_regular_ready_marker(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            episode, manifest = self._episode(Path(temporary), name)
+            (episode / "READY").unlink()
+            with self.assertRaises(ValueError):
+                validate_episode_dir(episode, manifest)
+
+    def test_fsync_episode_tree_rejects_intermediate_symlink(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            episode = root / "episode"
+            episode.mkdir()
+            outside = root / "outside"
+            outside.mkdir()
+            (outside / "file").write_bytes(b"sentinel")
+            (episode / "linked").symlink_to(outside, target_is_directory=True)
+            with self.assertRaises(ValueError):
+                fsync_episode_tree(episode)
+
+    def _local_sync(
+        self, root: Path, name: str = "episode_20260810T120000000_deadbeef"
+    ) -> tuple[LocalSync, LocalRsyncRunner, Path, Path]:
+        remote = root / "remote_ready"
+        destination = root / "Data_Get"
+        remote.mkdir()
+        destination.mkdir()
+        self._episode(remote, name)
+        runner = LocalRsyncRunner(remote)
+        syncer = LocalSync(
+            remote_root=remote,
+            destination=destination,
+            runner=runner,
+        )
+        return syncer, runner, remote, destination
+
+    def test_copy_is_verified_receipted_then_remote_is_deleted(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            syncer, _runner, remote, destination = self._local_sync(
+                Path(temporary), name
+            )
+            self.assertEqual(syncer.run_once(), 1)
+            final = destination / name
+            receipt = json.loads((final / VERIFIED_RECEIPT_NAME).read_text())
+            self.assertEqual(receipt["state"], "VERIFIED")
+            self.assertEqual(receipt["remote_identity"], syncer.remote_identity)
+            self.assertEqual(receipt["verified_files"], 2)
+            self.assertGreater(receipt["verified_bytes"], 0)
+            self.assertEqual(receipt["remote_delete"]["state"], "deleted")
+            self.assertFalse((remote / name).exists())
+            self.assertEqual(syncer.delete_count, 1)
+            status = json.loads((destination / "sync_status.json").read_text())
+            self.assertEqual(status["state"], "idle")
+            self.assertEqual(status["pending_remote_cleanup"], 0)
+
+    def test_truncated_copy_never_calls_delete(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            syncer, runner, remote, _destination = self._local_sync(Path(temporary))
+            runner.mode = "truncate"
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertEqual(len(list(remote.iterdir())), 1)
+
+    def test_manifest_change_during_copy_never_calls_delete(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            syncer, runner, remote, _destination = self._local_sync(Path(temporary))
+            runner.mode = "manifest_changed"
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertEqual(len(list(remote.iterdir())), 1)
+
+    def test_copied_manifest_symlink_never_calls_delete(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            syncer, runner, remote, _destination = self._local_sync(Path(temporary))
+            runner.mode = "manifest_symlink"
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertEqual(len(list(remote.iterdir())), 1)
+
+    def test_delete_failure_is_pending_and_retried(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            syncer, _runner, remote, destination = self._local_sync(
+                Path(temporary), name
+            )
+            syncer.delete_outcomes = ["failure", "success"]
+            self.assertEqual(syncer.run_once(), 1)
+            receipt_path = destination / name / VERIFIED_RECEIPT_NAME
+            receipt = json.loads(receipt_path.read_text())
+            self.assertEqual(receipt["remote_delete"]["state"], "pending")
+            self.assertTrue((remote / name).exists())
+            status = json.loads((destination / "sync_status.json").read_text())
+            self.assertEqual(status["state"], "delete_pending")
+            self.assertEqual(status["pending_remote_cleanup"], 1)
+
+            # Persistent backoff prevents a 2-second poll loop from repeatedly
+            # hashing a large MCAP after a network/helper failure.
+            self.assertEqual(syncer.run_once(), 0)
+            self.assertEqual(len(syncer.delete_calls), 1)
+            receipt = json.loads(receipt_path.read_text())
+            receipt["remote_delete"]["next_retry_unix_s"] = 0.0
+            receipt_path.write_text(json.dumps(receipt), encoding="utf-8")
+
+            self.assertEqual(syncer.run_once(), 0)
+            receipt = json.loads(receipt_path.read_text())
+            self.assertEqual(receipt["remote_delete"]["state"], "deleted")
+            self.assertFalse((remote / name).exists())
+            self.assertEqual(len(syncer.delete_calls), 2)
+
+    def test_lost_delete_ack_is_idempotent_after_process_restart(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            root = Path(temporary)
+            first, runner, remote, destination = self._local_sync(root, name)
+            first.delete_outcomes = ["ack_lost_after_delete"]
+            self.assertEqual(first.run_once(), 1)
+            self.assertFalse((remote / name).exists())
+            receipt_path = destination / name / VERIFIED_RECEIPT_NAME
+            self.assertEqual(
+                json.loads(receipt_path.read_text())["remote_delete"]["state"],
+                "pending",
+            )
+
+            restarted = LocalSync(
+                remote_root=remote,
+                destination=destination,
+                runner=runner,
+            )
+            with mock.patch(
+                "data_get_sync.validate_episode_dir",
+                wraps=validate_episode_dir,
+            ) as deep_validator:
+                self.assertEqual(restarted.run_once(), 0)
+                self.assertEqual(deep_validator.call_count, 0)
+                self.assertEqual(restarted.delete_calls, [])
+
+                receipt = json.loads(receipt_path.read_text())
+                receipt["remote_delete"]["next_retry_unix_s"] = 0.0
+                receipt_path.write_text(json.dumps(receipt), encoding="utf-8")
+                self.assertEqual(restarted.run_once(), 0)
+                self.assertGreaterEqual(deep_validator.call_count, 1)
+            self.assertEqual(
+                json.loads(receipt_path.read_text())["remote_delete"]["state"],
+                "deleted",
+            )
+            self.assertEqual(restarted.delete_calls[0][0], name)
+
+    def test_retry_rehashes_and_refuses_delete_after_local_tamper(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            syncer, _runner, remote, destination = self._local_sync(
+                Path(temporary), name
+            )
+            syncer.delete_outcomes = ["failure", "success"]
+            self.assertEqual(syncer.run_once(), 1)
+            (destination / name / "bag" / "bag_0.mcap").write_bytes(b"tampered")
+            receipt_path = destination / name / VERIFIED_RECEIPT_NAME
+            receipt = json.loads(receipt_path.read_text())
+            receipt["remote_delete"]["next_retry_unix_s"] = 0.0
+            receipt_path.write_text(json.dumps(receipt), encoding="utf-8")
+
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(len(syncer.delete_calls), 1)
+            self.assertTrue((remote / name).exists())
+
+    def test_unlisted_stale_mcap_is_never_published_or_deleted(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            syncer, _runner, remote, destination = self._local_sync(
+                Path(temporary), name
+            )
+            (remote / name / "bag" / "stale_old_payload.mcap").write_bytes(
+                b"stale"
+            )
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertFalse((destination / name).exists())
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertTrue((remote / name).exists())
+
+    def test_reused_hash_staging_cannot_smuggle_old_mcap(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            syncer, _runner, remote, destination = self._local_sync(
+                Path(temporary), name
+            )
+            document = syncer.get_remote_manifest(name)
+            staging = (
+                destination
+                / ".incoming"
+                / f"{name}.{document.sha256}.partial"
+                / "bag"
+            )
+            staging.mkdir(parents=True)
+            (staging / "stale_old_payload.mcap").write_bytes(b"old interrupted bag")
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertFalse((destination / name).exists())
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertTrue((remote / name).exists())
+
+    def test_deleted_receipt_from_old_ip_does_not_block_new_ip(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            root = Path(temporary)
+            first, runner, remote, destination = self._local_sync(root, name)
+            self.assertEqual(first.run_once(), 1)
+            receipt_path = destination / name / VERIFIED_RECEIPT_NAME
+            receipt = json.loads(receipt_path.read_text())
+            receipt["remote"] = "nvidia@old-ip"
+            receipt["remote_identity"] = (
+                f"nvidia@old-ip:{FIXED_REMOTE_READY}"
+            )
+            receipt_path.write_text(json.dumps(receipt), encoding="utf-8")
+
+            migrated = LocalSync(
+                remote_root=remote,
+                destination=destination,
+                runner=runner,
+                remote="nvidia@new-ip",
+            )
+            self.assertEqual(migrated.run_once(), 0)
+            status = json.loads((destination / "sync_status.json").read_text())
+            self.assertEqual(status["state"], "idle")
+            self.assertEqual(status["pending_remote_cleanup"], 0)
+
+    def test_bad_pending_old_ip_does_not_starve_new_episode(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            old_name = "episode_old"
+            first, _runner, _old_remote, destination = self._local_sync(
+                root, old_name
+            )
+            first.delete_outcomes = ["failure"]
+            self.assertEqual(first.run_once(), 1)
+
+            new_remote = root / "new_remote_ready"
+            new_remote.mkdir()
+            new_name = "episode_new"
+            self._episode(new_remote, new_name)
+            new_runner = LocalRsyncRunner(new_remote)
+            migrated = LocalSync(
+                remote_root=new_remote,
+                destination=destination,
+                runner=new_runner,
+                remote="nvidia@new-ip",
+            )
+            self.assertEqual(migrated.run_once(), 1)
+            self.assertFalse((new_remote / new_name).exists())
+            self.assertTrue((destination / new_name).is_dir())
+            status = json.loads((destination / "sync_status.json").read_text())
+            self.assertEqual(status["state"], "delete_pending")
+            self.assertEqual(status["pending_remote_cleanup"], 1)
+            self.assertIn("remote_identity", status["last_delete_error"])
+
+    def test_delete_helper_uses_independent_long_timeout(self) -> None:
+        class AckRunner:
+            def __init__(self) -> None:
+                self.timeouts: list[float] = []
+
+            def run(
+                self, command: list[str], *, timeout: float
+            ) -> subprocess.CompletedProcess[str]:
+                self.timeouts.append(timeout)
+                payload = {
+                    "state": "already_absent",
+                    "episode_id": "episode_timeout",
+                    "manifest_sha256": "0" * 64,
+                }
+                return subprocess.CompletedProcess(
+                    command, 0, json.dumps(payload) + "\n", ""
+                )
+
+        with tempfile.TemporaryDirectory() as temporary:
+            runner = AckRunner()
+            syncer = DataGetSync(
+                remote="nvidia@test",
+                remote_ready=FIXED_REMOTE_READY,
+                destination=Path(temporary),
+                status_file=Path(temporary) / "status.json",
+                runner=runner,
+                delete_remote_after_sync=True,
+                remote_delete_timeout_s=600.0,
+            )
+            syncer._invoke_delete_helper("episode_timeout", "0" * 64)
+            self.assertEqual(runner.timeouts, [600.0])
+
+    def test_existing_final_without_receipt_is_deep_verified_before_delete(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            root = Path(temporary)
+            syncer, _runner, remote, destination = self._local_sync(root, name)
+            shutil.copytree(remote / name, destination / name)
+            with mock.patch(
+                "data_get_sync.validate_episode_dir",
+                wraps=validate_episode_dir,
+            ) as deep_validator:
+                self.assertEqual(syncer.run_once(), 0)
+                self.assertGreaterEqual(deep_validator.call_count, 1)
+            self.assertTrue((destination / name / VERIFIED_RECEIPT_NAME).is_file())
+            self.assertFalse((remote / name).exists())
+
+    def test_existing_corrupt_final_without_receipt_never_deletes_remote(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            root = Path(temporary)
+            syncer, _runner, remote, destination = self._local_sync(root, name)
+            shutil.copytree(remote / name, destination / name)
+            (destination / name / "bag" / "bag_0.mcap").write_bytes(b"corrupt")
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertTrue((remote / name).exists())
+
+    def test_existing_manifest_collision_never_deletes_remote(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            name = "episode_20260810T120000000_deadbeef"
+            root = Path(temporary)
+            syncer, _runner, remote, destination = self._local_sync(root, name)
+            self._episode(destination, name)
+            local_manifest = destination / name / "manifest.json"
+            local_manifest.write_bytes(local_manifest.read_bytes() + b"\n")
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertTrue((remote / name).exists())
+
+    def test_receipt_write_failure_never_calls_delete(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            syncer, _runner, remote, _destination = self._local_sync(Path(temporary))
+            with mock.patch(
+                "data_get_sync.atomic_write_json",
+                side_effect=RuntimeError("injected receipt fsync failure"),
+            ):
+                with self.assertRaises(RuntimeError):
+                    syncer.sync_episode(next(remote.iterdir()).name)
+            self.assertEqual(syncer.delete_calls, [])
+            self.assertEqual(len(list(remote.iterdir())), 1)
+
+    def test_sync_flock_rejects_concurrent_once(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            first, runner, remote, destination = self._local_sync(root)
+            second = LocalSync(
+                remote_root=remote,
+                destination=destination,
+                runner=runner,
+            )
+            with first.process_lock():
+                with self.assertRaisesRegex(RuntimeError, "already holds"):
+                    second.run_once()
+
+    def test_symlink_lock_is_refused(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            syncer, _runner, _remote, destination = self._local_sync(root)
+            sentinel = root / "sentinel"
+            sentinel.write_text("do not touch", encoding="utf-8")
+            (destination / ".data_get_sync.lock").symlink_to(sentinel)
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(sentinel.read_text(encoding="utf-8"), "do not touch")
+
+    def test_delete_mode_rejects_nonfixed_remote_ready(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            with self.assertRaises(ValueError):
+                DataGetSync(
+                    remote="nvidia@test",
+                    remote_ready="/tmp/not-ready",
+                    destination=root,
+                    status_file=root / "status.json",
+                    delete_remote_after_sync=True,
+                )
+
+    def test_bad_old_episode_does_not_starve_newer_episode(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+
+            class ProbeSync(DataGetSync):
+                def __init__(self) -> None:
+                    super().__init__(
+                        remote="unused",
+                        remote_ready="/unused",
+                        destination=root,
+                        status_file=root / "status.json",
+                    )
+                    self.seen: list[str] = []
+
+                def list_remote_episodes(self) -> list[str]:
+                    return ["episode_bad", "episode_new"]
+
+                def sync_episode(self, episode: str) -> bool:
+                    self.seen.append(episode)
+                    if episode == "episode_bad":
+                        raise RuntimeError("damaged")
+                    return True
+
+            syncer = ProbeSync()
+            with self.assertRaises(RuntimeError):
+                syncer.run_once()
+            self.assertEqual(syncer.seen, ["episode_bad", "episode_new"])
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_data_collection/tg3-data-get-sync.service

@@ -0,0 +1,14 @@
+[Unit]
+Description=TG3 completed data episode sync to Data_Get
+
+[Service]
+Type=simple
+WorkingDirectory=/home/ps/Desktop/TG3_TS1P_OmniSocket_Teleop
+ExecStart=/usr/bin/python3 /home/ps/Desktop/TG3_TS1P_OmniSocket_Teleop/tg3_data_collection/data_get_sync.py --remote nvidia@192.168.41.2 --remote-ready /home/nvidia/tg3_data_collection/ready --destination /home/ps/Desktop/TG3_TS1P_OmniSocket_Teleop/Data_Get --delete-remote-after-sync --poll-seconds 2
+Restart=always
+RestartSec=2
+KillSignal=SIGINT
+TimeoutStopSec=10
+
+[Install]
+WantedBy=default.target

tg3_local_teleop/README.md

@@ -13,8 +13,10 @@ TS1P 同构臂
   -> 天工 3.0 双臂
   -> /left_hand/set_motor_multi + /right_hand/set_motor_multi
   -> 天工 3.0 BrainCo Revo2 双灵巧手
-  -> /hric/robot/cmd_vel（右 C + 左摇杆前后 / 左 Z + 右摇杆转向；50 Hz TwistStamped）
+  -> /hric/robot/cmd_vel（左摇杆前后 / 右摇杆转向；50 Hz TwistStamped）
   -> 天工 3.0 HBWALK 行走
+  -> /head/cmd（左 Z + 右摇杆上下；Head Pitch 位置模式）
+  -> 天工 3.0 头部相机上下视角
 ```
 
 ## 自动运行与操作
@@ -29,29 +31,40 @@ TS1P 同构臂
 - 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
 - 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
   前后/转向；不控制腰和头。
-- 行走：遥操已启动后，右 `C` + 左摇杆上下控制前后，左 `Z` + 右摇杆左右控制原地
-  转向；两个组合都在下一个 `50 Hz` 周期立即响应，不再等待 3 秒。松开对应按键或
-  摇杆回中立即把该轴清零，再按也立即恢复。
+- 行走：遥操已启动且双摇杆先回中过一次后，左摇杆上下直接控制前后，右摇杆左右
+  直接控制原地转向；无需组合键，在下一个 `50 Hz` 周期立即响应。摇杆回中立即把
+  对应轴清零，再推动也立即恢复。
   前后仍使用二次细控曲线；转向在死区后使用线性曲线，使右摇杆中段有足够角速度，
   但最大值仍受官方 `0.8 rad/s` 上限约束。
   按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
   `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
 - xTELE 0.1.2 的摇杆数组顺序是“纵轴、横轴”；左纵轴映射到 `linear.x`，右横轴
   映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。
+- 头部视角：遥操已启动且右摇杆纵轴先回中过一次后，按住左 Z，右摇杆上推使
+  Head Pitch 向负值移动（抬头），下推使其向正值移动（低头）；不需要 R3。按 Z
+  期间右摇杆水平转向会被屏蔽，避免调整视角时误转。控制只使用 `mode=0` 绝对
+  位置、`cur=1.0 A`，从 `/robot_state` 的真实 `name=2` 反馈起步，限制在 `-10°～+50°`、
+  最大 `0.2 rad/s`、加速度 `0.5 rad/s²`。摇杆回中、输入/反馈失联或头电机报错时停止
+  更新目标。检测到 `control_forward`、`patrol_agent`、`joystick_bridge` 等外部
+  `/head/cmd` 实际消息后，本项目暂停头部输出，静默 1 秒并再次观察摇杆回中后才恢复。
 - 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
   在不清楚厂商语义的情况下把它擅自绑定为状态切换。
 - 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
   BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
   若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
-- 右手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开
+- 双手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开
   至少 `0.5 s` 后，再连续长按右 `B` 1 秒关闭。首次启动和每个新遥操会话都必须先
   稳定松开 B，保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈
-  中断均不触发；计时期间冻结右手最后一条命令，左手和双臂仍照常跟随。
-- 指向只覆盖右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号
+  中断均不触发；计时期间冻结双手各自最后一条命令，双臂仍照常跟随。
+- 指向同时覆盖左右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号
   “单食指”手势 `state 0`：归一化目标
   `[0.2, 0.688, 0.0, 0.98, 0.98, 0.98]`，对应当前 `1~1000` 位置范围约为
   `[201, 688, 1, 980, 980, 980]`；顺序为大拇指弯曲、大拇指旋转、食指、中指、
   无名指、小拇指。进入和退出手势都继续使用每秒最多 400 个位置单位的现有限速。
+- 右 `A` 是右手瞬时姿态键：按住时采用六电机目标位置
+  `[428, 735, 500, 77, 77, 72]`，松开后恢复已有右手输入；若右 B 双手指向手势已开启，
+  A 按住期间只在右手优先，左手维持单食指；松开后右手恢复单食指。A 也必须在新会话中先稳定松开 `0.5 s`，
+  全程继续使用同一个 `400 units/s` 限速器。
 
 ## 安全门控
 
@@ -65,16 +78,20 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
 实际发出的速度由本桥按官方 Topic 范围限制为 `linear.x=[-0.8, 1.0] m/s`、
 `angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。
 
-行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆
-前后或左 Z + 右摇杆横向即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
+行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且左摇杆前后
+或右摇杆横向越过死区时发布。摇杆回中后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
 回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
 
+头部控制只在同一次遥操武装期间启用，不改变 Head Yaw，也不改变厂家头部机械限位、
+电流、高温或碰撞保护。服务重启或新会话不会把软件累计角度当真实位置，必须重新读取
+`/robot_state` 的 Head Pitch 反馈。
+
 灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
 位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
 和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态
 或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
 最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
-匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态，但不会在
+匹配的 STOP、安全解除武装或服务退出都会清除右 B 与右 A 手势的逻辑状态，但不会在
 STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。
 
 ## 限速双臂回 Home
@@ -145,13 +162,100 @@ OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或
 正常待机，不触发反复重启。只有匹配的操作员 STOP 自动回 Home；意外断网不自动产生
 回位运动。
 
+## 独立数采
+
+数采使用左摇杆按下键（L3），不是左侧 X/Y/Z 面键：
+
+- 遥操已成功开启后，先保持 L3 松开至少 `0.5 s`；
+- 连续按住 L3 `1 s` 开始一条 episode；
+- 松开至少 `0.5 s`，再次连续按住 `1 s` 正常结束；
+- 长按 Z+C 结束遥操、安全解除或桥退出时，也会异步请求结束当前 episode；
+- 数采失败只记录错误，不解除遥操、不阻塞 50 Hz 控制，也不延迟 STOP/Home。
+
+`button_joystick.left` 是 xTELE 5003 中独立的摇杆按压字段。已安装的 xTELE 0.1.2
+没有给它注册处理函数；X/Y/Z/A/B/C 均已有厂家功能或本项目绑定，因此不复用面键。
+新会话和服务重启后都先锁定为“必须松开”，畸形或陈旧按键帧不能被当作有效松开。
+
+项目自有 `tg3-data-recorder.service` 在 Nvidia 上运行独立 `ros2 bag record`，不停止、
+重配或接管 Ubuntu 厂家 `/record_bag_node`。仅录 `config.toml` 中的明确白名单，包含
+`/robot_state`、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态、完整 xTELE
+应用帧，以及头部 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。
+
+为避免 1280x720 原始 RGB+深度每台相机约 `8.3 GB/min` 的持续写盘负载，默认录制
+`color/image_raw/compressed`（JPEG）和 `depth/image_raw/compressedDepth`（16UC1
+无损 PNG）。头部相机不是 required topic：服务在运行就由 rosbag 自动发现并
+采集，未运行不阻塞 L3 或核心 READY。manifest 记录 `absent`、`healthy`、
+`partial` 或 `low_rate`，并保存各话题消息数和图像平均频率；相机异常会产生明确质量
+警告，但不会丢弃其余机器人控制数据。静态 TF 也尽力采集，不作为启动门槛。
+
+机器人暂存目录：
+
+```text
+/home/nvidia/tg3_data_collection/
+  active/    # 录制期间的临时数据，不能取走
+  ready/     # 等待 PS 本机完成持久化验真；随后自动回收
+  failed/    # 当前策略不保留失败 payload
+  deleting/  # 固定根删除助手的短暂原子隔离目录
+```
+
+MCAP 使用 `zstd_fast`、64 MiB cache、每 300 秒分片；每条 episode 最长 30 分钟，
+启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完
+`metadata.yaml`，随后执行
+`ros2 bag info`，并确认所有 required topics 都有非零消息数；然后计算 SHA-256、写
+`manifest.json` 和 `READY`，最后才原子进入 `ready/`。Nvidia 需已安装 `python3-yaml`
+（现场已验证 PyYAML 6.0.1）。
+
+当前 `retain_failed_episodes=false`：失败原因仍进入 recorder 状态和 journal，但失败
+MCAP 不留在机器人磁盘。成功 episode 必须先在 PS 本机完成逐文件 SHA-256、fsync、
+原子发布并生成 `VERIFIED`，然后由固定根目录删除助手按 episode ID + manifest SHA
+精确回收；它不能访问 `active/`、`failed/` 或数据根目录。PS 断线时 `ready/` 会临时保留，
+避免尚未落到本机的数据丢失，恢复后自动续传和回收。
+
+相机由厂家系统服务管理，本项目只订阅、不自动启停。录制前检查或按需启动：
+
+```bash
+systemctl is-active orbbec_waist.service orbbec_head.service
+sudo systemctl enable --now orbbec_waist.service
+# 需要头部图像时再执行；启动后下一条 episode 会自动包含头部 RGB-D：
+sudo systemctl enable --now orbbec_head.service
+```
+
+录制服务只加载厂家完整 `/opt/robot_tele_server/install` 消息 overlay，不加载本项目
+`ros2_py/install`；项目 overlay 仅供遥操桥的 Python 消息绑定使用
+（包括 `ArmStatus`/`RobotState`/`HeadCtrl`，但不是厂家完整包）。两者混在同一个
+rosbag 环境会让部分 `ros2_bridge_msgs` C++ 类型支持出现动态库符号冲突。
+
+部署两个机器人侧服务：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp tg3-data-recorder.service tg3-local-teleop.service ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable --now tg3-data-recorder.service
+systemctl --user restart tg3-local-teleop.service
+```
+
+只读检查：
+
+```bash
+systemctl --user --no-pager status tg3-data-recorder.service
+ros2 topic echo --once /tg3/data_collection/status
+python3 -m json.tool /home/nvidia/tg3_local_teleop/status.json
+find /home/nvidia/tg3_data_collection/ready -mindepth 1 -maxdepth 1 -type d
+```
+
+完成 episode 由 PS 本机服务校验后保存到项目 `Data_Get`，详见
+`../tg3_data_collection/README.md`。本机 `VERIFIED` 持久化后，机器人对应 `ready/`
+副本会自动删除；EAI 不保存数采 payload。
+
 ## 完整重启顺序
 
 只重启本项目的网络与机器人桥时，先启动 Nvidia 接收端，再启动 EAI 本地门控服务；
 EAI 在收到物理 START 前不会建立发送 Session：
 
 ```bash
-ssh nvidia@192.168.41.2 'systemctl --user restart tg3-local-teleop.service'
+ssh nvidia@192.168.41.2 \
+  'systemctl --user restart tg3-data-recorder.service tg3-local-teleop.service'
 ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
 ```
 

tg3_local_teleop/config.toml

@@ -1,7 +1,7 @@
 [network]
 # Direct OmniSocket KCP input. No robot-side ZMQ receiving proxy is used.
 transport = "omnisocket"
-omnisocket_server = "175.178.116.187:14049"
+omnisocket_server = "192.168.5.14:14049"
 omnisocket_peer_id = "tg3-009027fa8190-robot"
 omnisocket_expected_sender = "tg3-009027fa8190-iarm"
 omnisocket_max_packet_age_ms = 300.0
@@ -55,17 +55,113 @@ joint_upper_rad = [
    2.8449,  0.1920,  2.8449,  0.1920,  2.8449,  1.3265,  1.3265,
 ]
 
+[data_collection]
+# Left joystick press (L3) is a dedicated raw xTELE field and is not registered
+# by the installed xTELE 0.1.2 button handlers.  It only toggles recording while
+# a validated teleoperation session is armed.  Z+C STOP/safety disarm always
+# ends an active recording without delaying robot control or Home.
+enabled = true
+button_hold_seconds = 1.0
+button_release_seconds = 0.5
+# This threshold qualifies only the L3 button edge; it is not a robot-motion
+# watchdog and does not restore the removed 0.25 s network disarm gate.
+button_input_timeout_s = 0.25
+control_retry_seconds = 0.5
+ack_timeout_seconds = 5.0
+heartbeat_interval_seconds = 0.5
+heartbeat_timeout_seconds = 3.0
+status_stale_seconds = 4.0
+control_topic = "/tg3/data_collection/control"
+status_topic = "/tg3/data_collection/status"
+iarm_frame_topic = "/tg3/data_collection/iarm_frame"
+base_directory = "/home/nvidia/tg3_data_collection"
+# Failed/incomplete payloads are discarded after the error is reported through
+# recorder status and journal. Completed READY episodes remain only until the
+# PS sync has durably verified its local copy and requests exact-episode removal.
+retain_failed_episodes = false
+minimum_free_gib = 100.0
+max_duration_seconds = 1800.0
+
+# Explicit data whitelist; never replace this with `-a` on the live robot.
+# RGB is recorded through image_transport JPEG and 16UC1 depth through its
+# lossless compressedDepth PNG transport. Only the head RGB-D camera is in the
+# whitelist. It is optional and is discovered whenever its service is running.
+# /robot_state is the authoritative measured robot state.
+topics = [
+  "/robot_state",
+  "/encoder_identical_joint",
+  "/arm/cmd",
+  "/freq_change/arm_status",
+  "/data_logger/arm_status",
+  "/left_hand/set_motor_multi",
+  "/right_hand/set_motor_multi",
+  "/left_hand/motor_status",
+  "/right_hand/motor_status",
+  "/left_hand/touch_status",
+  "/right_hand/touch_status",
+  "/hric/robot/cmd_vel",
+  "/hric/robot/cmd_vel_status",
+  "/hric/robot/rl_state",
+  "/head/cmd",
+  "/imu_data",
+  "/power/board/key_status",
+  "/power/board/status",
+  "/power/battery/status",
+  "/ob_camera_head/color/image_raw/compressed",
+  "/ob_camera_head/color/camera_info",
+  "/ob_camera_head/color/metadata",
+  "/ob_camera_head/depth/image_raw/compressedDepth",
+  "/ob_camera_head/depth/camera_info",
+  "/ob_camera_head/depth/metadata",
+  "/tf_static",
+  "/tg3/data_collection/control",
+  "/tg3/data_collection/status",
+  "/tg3/data_collection/iarm_frame",
+]
+
+# Refuse a new episode if these core streams are absent from the live ROS
+# graph. Other whitelisted streams may appear later and rosbag discovery will
+# subscribe to them without restarting the recorder.
+required_topics = [
+  "/robot_state",
+  "/encoder_identical_joint",
+  "/freq_change/arm_status",
+  "/left_hand/motor_status",
+  "/right_hand/motor_status",
+  "/hric/robot/rl_state",
+  "/tg3/data_collection/control",
+  "/tg3/data_collection/status",
+  "/tg3/data_collection/iarm_frame",
+]
+
+# Optional sensor groups never block capture or core READY. Their message
+# counts and whole-episode average rates are recorded in manifest.json as
+# absent/healthy/partial/low_rate quality observations. The Orbbec driver
+# targets 30 Hz; 20 Hz leaves room for discovery and compression jitter.
+[data_collection.optional_topic_groups.head_rgbd]
+topics = [
+  "/ob_camera_head/color/image_raw/compressed",
+  "/ob_camera_head/color/camera_info",
+  "/ob_camera_head/color/metadata",
+  "/ob_camera_head/depth/image_raw/compressedDepth",
+  "/ob_camera_head/depth/camera_info",
+  "/ob_camera_head/depth/metadata",
+]
+
+[data_collection.optional_topic_groups.head_rgbd.minimum_topic_rates_hz]
+"/ob_camera_head/color/image_raw/compressed" = 20.0
+"/ob_camera_head/depth/image_raw/compressedDepth" = 20.0
+
 [locomotion]
-# The two original immediate bindings are independent: right C + left-stick
-# vertical controls translation; left Z + right-stick horizontal controls
-# in-place yaw. Both act on the next 50 Hz tick, with no second hold.
+# Once Z+C has armed teleoperation and both sticks have been observed neutral,
+# left-stick vertical directly controls translation and right-stick horizontal
+# directly controls in-place yaw. No locomotion button or second hold is used.
 enabled = true
 command_topic = "/hric/robot/cmd_vel"
 # This bridge never publishes FSM commands. The robot must already report
 # HBWALK/running through the existing safety gate before velocity is allowed.
 # Match the TG3 secondary-development TwistStamped example.
 frame_id = "pelvis"
-hold_seconds = 0.0
 joystick_deadzone = 0.2
 joystick_expo = 2.0
 # Turning is linear after the deadzone so medium right-stick travel is not
@@ -81,6 +177,39 @@ forward_axis_sign = 1.0
 yaw_axis_sign = -1.0
 zero_burst_frames = 10
 
+[head]
+# Camera-pitch mode: after Z+C arms teleoperation and the right-stick vertical
+# axis has been observed neutral once, hold left Z while pushing the right stick
+# up/down. While Z is held, right-stick yaw is suppressed so camera adjustment
+# cannot accidentally turn the robot. No R3 is used.
+enabled = true
+command_topic = "/head/cmd"
+state_topic = "/robot_state"
+pitch_motor_id = 2
+frame_id = "head"
+mode = 0
+label = 151
+max_current_a = 1.0
+command_speed_rad_s = 0.2
+
+# Documented mechanical range is -15..+58 deg. Keep the commissioned visual
+# working range at -10..+50 deg and always start from fresh measured feedback.
+min_pitch_rad = -0.1745329252
+max_pitch_rad = 0.872664626
+max_pitch_speed_rad_s = 0.2
+max_pitch_accel_rad_s2 = 0.5
+pitch_axis_sign = -1.0
+joystick_deadzone = 0.1
+max_integration_dt_s = 0.1
+input_timeout_s = 0.3
+feedback_timeout_s = 0.25
+
+# Existing vendor nodes advertise /head/cmd publishers. They are allowed when
+# idle, but any actual foreign message leases head control for this interval;
+# our bridge resumes only after quiet plus another neutral-stick observation.
+foreign_command_quiet_s = 1.0
+self_command_window_s = 0.25
+
 [hands]
 # TianGong 3.0 on this robot uses BrainCo Revo2 hands. Commands are published
 # only while the same physical 3-second teleoperation latch is armed.
@@ -108,8 +237,8 @@ invert_scalar = false
 open_normalized = [0.4, 0.4, 0.05, 0.05, 0.05, 0.05]
 closed_normalized = [0.9, 0.5, 0.45, 0.52, 0.52, 0.45]
 
-# Right B owns a robot-side, release-guarded pointing gesture while a teleop
-# session is armed. Both activation and deactivation require one continuous
+# Right B owns a robot-side, release-guarded bilateral pointing gesture while
+# a teleop session is armed. Both activation and deactivation require one continuous
 # 1-second hold, separated by at least 0.5 seconds of stable release. A new
 # session is always release-locked. The pose is xTELE GestureController's
 # BrainCoRevo2 gesture 2 ("single index finger"), state 0, in motor order:
@@ -119,6 +248,15 @@ right_b_point_gesture_hold_seconds = 1.0
 right_b_point_gesture_release_seconds = 0.5
 right_b_point_pose_normalized = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
 
+# Right A is a momentary, robot-side pose: hold A to move the right hand to
+# this measured BrainCo MotorStatus position and release A to resume the
+# existing right-hand input (or an active right-B gesture). A new teleop
+# session must first observe A released for 0.5 seconds. The normal hand slew
+# limit remains in force; these are raw driver positions, not normalized data.
+right_a_pose_enabled = true
+right_a_pose_release_seconds = 0.5
+right_a_pose_positions = [428, 735, 500, 77, 77, 72]
+
 [home]
 # Deliberately slower than manual teleoperation. This Home pose was captured
 # from the robot's real arm feedback on 2026-08-06. Joint order is 11..17,

tg3_local_teleop/data_recorder_node.py

@@ -0,0 +1,440 @@
+#!/usr/bin/env python3
+"""Independent ROS 2 supervisor for TG3 teleoperation data collection.
+
+This node only controls the project-owned ``ros2 bag record`` process through
+``DataRecorderManager``.  It never calls, stops, or reconfigures TianGong's
+factory ``record_bag_node``.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import sys
+import threading
+import tomllib
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    DurabilityPolicy,
+    HistoryPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+)
+from rclpy.utilities import remove_ros_args
+from std_msgs.msg import String
+
+from data_collection import (
+    DataRecorderManager,
+    OptionalTopicGroupConfig,
+    RecorderConfig,
+)
+from data_recorder_protocol import (
+    RecorderControlProtocol,
+    topics_without_publishers,
+)
+
+
+DEFAULT_CONFIG = Path(__file__).with_name("config.toml")
+
+
+def _boolean(value: Any, name: str) -> bool:
+    if type(value) is not bool:
+        raise ValueError(f"data_collection.{name} must be true or false")
+    return value
+
+
+def _positive_number(value: Any, name: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise ValueError(f"data_collection.{name} must be a number")
+    result = float(value)
+    if not math.isfinite(result) or result <= 0.0:
+        raise ValueError(f"data_collection.{name} must be positive and finite")
+    return result
+
+
+def _nonnegative_number(value: Any, name: str) -> float:
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise ValueError(f"data_collection.{name} must be a number")
+    result = float(value)
+    if not math.isfinite(result) or result < 0.0:
+        raise ValueError(
+            f"data_collection.{name} must be non-negative and finite"
+        )
+    return result
+
+
+def _topic(value: Any, name: str) -> str:
+    if (
+        not isinstance(value, str)
+        or not value.startswith("/")
+        or value.strip() != value
+        or any(character.isspace() for character in value)
+    ):
+        raise ValueError(
+            f"data_collection.{name} must be an absolute ROS topic name"
+        )
+    return value
+
+
+def _topic_list(value: Any, name: str, *, allow_empty: bool) -> tuple[str, ...]:
+    if not isinstance(value, list):
+        raise ValueError(f"data_collection.{name} must be a TOML array")
+    result = tuple(_topic(item, name) for item in value)
+    if not allow_empty and not result:
+        raise ValueError(f"data_collection.{name} must not be empty")
+    if len(set(result)) != len(result):
+        raise ValueError(f"data_collection.{name} contains duplicate topics")
+    return result
+
+
+def _minimum_topic_rates(value: Any) -> dict[str, float]:
+    name = "minimum_topic_rates_hz"
+    if not isinstance(value, dict):
+        raise ValueError(f"data_collection.{name} must be a TOML table")
+    result: dict[str, float] = {}
+    for topic, rate in value.items():
+        normalized_topic = _topic(topic, f"{name} key")
+        result[normalized_topic] = _positive_number(
+            rate, f"{name}.{normalized_topic}"
+        )
+    return result
+
+
+def _optional_topic_groups(
+    value: Any,
+) -> dict[str, OptionalTopicGroupConfig]:
+    name = "optional_topic_groups"
+    if not isinstance(value, dict):
+        raise ValueError(f"data_collection.{name} must be a TOML table")
+    result: dict[str, OptionalTopicGroupConfig] = {}
+    for group_name, raw_group in value.items():
+        if not isinstance(raw_group, dict):
+            raise ValueError(
+                f"data_collection.{name}.{group_name} must be a TOML table"
+            )
+        unknown = sorted(
+            set(raw_group) - {"topics", "minimum_topic_rates_hz"}
+        )
+        if unknown:
+            raise ValueError(
+                f"data_collection.{name}.{group_name} has unknown keys: "
+                + ", ".join(unknown)
+            )
+        topics = _topic_list(
+            raw_group.get("topics", []),
+            f"{name}.{group_name}.topics",
+            allow_empty=False,
+        )
+        rates = _minimum_topic_rates(
+            raw_group.get("minimum_topic_rates_hz", {})
+        )
+        result[group_name] = OptionalTopicGroupConfig(
+            topics=topics,
+            minimum_topic_rates_hz=rates,
+        )
+    return result
+
+
+@dataclass(frozen=True)
+class SupervisorSettings:
+    enabled: bool
+    control_topic: str
+    status_topic: str
+    heartbeat_timeout_seconds: float
+    status_publish_period_seconds: float
+    required_topics: tuple[str, ...]
+    shutdown_timeout_seconds: float
+    recorder: RecorderConfig
+
+
+def load_settings(path: Path | str) -> SupervisorSettings:
+    """Load only ``[data_collection]`` and tolerate bridge-only extra keys."""
+
+    config_path = Path(path).expanduser()
+    with config_path.open("rb") as stream:
+        document = tomllib.load(stream)
+    section = document.get("data_collection")
+    if not isinstance(section, dict):
+        raise ValueError("config.toml is missing [data_collection]")
+
+    enabled = _boolean(section.get("enabled", True), "enabled")
+    control_topic = _topic(
+        section.get("control_topic", "/tg3/data_collection/control"),
+        "control_topic",
+    )
+    status_topic = _topic(
+        section.get("status_topic", "/tg3/data_collection/status"),
+        "status_topic",
+    )
+    if control_topic == status_topic:
+        raise ValueError("data collection control and status topics must differ")
+    topics = _topic_list(section.get("topics", []), "topics", allow_empty=False)
+    required_topics = _topic_list(
+        section.get("required_topics", []),
+        "required_topics",
+        allow_empty=True,
+    )
+    unknown_required = sorted(set(required_topics) - set(topics))
+    if unknown_required:
+        raise ValueError(
+            "data_collection.required_topics must also appear in topics: "
+            + ", ".join(unknown_required)
+        )
+    minimum_topic_rates = _minimum_topic_rates(
+        section.get("minimum_topic_rates_hz", {})
+    )
+    optional_topic_groups = _optional_topic_groups(
+        section.get("optional_topic_groups", {})
+    )
+    unknown_rate_topics = sorted(
+        set(minimum_topic_rates) - set(required_topics)
+    )
+    if unknown_rate_topics:
+        raise ValueError(
+            "data_collection.minimum_topic_rates_hz keys must also appear in "
+            "required_topics: " + ", ".join(unknown_rate_topics)
+        )
+
+    heartbeat_timeout = _positive_number(
+        section.get("heartbeat_timeout_seconds", 3.0),
+        "heartbeat_timeout_seconds",
+    )
+    status_rate = _positive_number(
+        section.get("status_publish_rate_hz", 5.0),
+        "status_publish_rate_hz",
+    )
+    minimum_free_gib = _nonnegative_number(
+        section.get("minimum_free_gib", 10.0), "minimum_free_gib"
+    )
+    max_duration = _positive_number(
+        section.get("max_duration_seconds", 1800.0),
+        "max_duration_seconds",
+    )
+    base_directory = section.get(
+        "base_directory", "/home/nvidia/tg3_data_collection"
+    )
+    if not isinstance(base_directory, str) or not base_directory.strip():
+        raise ValueError(
+            "data_collection.base_directory must be a non-empty path"
+        )
+
+    recorder = RecorderConfig(
+        base_directory=Path(base_directory).expanduser(),
+        topics=topics,
+        required_topics=required_topics,
+        minimum_topic_rates_hz=minimum_topic_rates,
+        optional_topic_groups=optional_topic_groups,
+        retain_failed_episodes=_boolean(
+            section.get("retain_failed_episodes", True),
+            "retain_failed_episodes",
+        ),
+        minimum_free_bytes=int(minimum_free_gib * 1024**3),
+        max_duration_seconds=max_duration,
+        poll_interval_seconds=0.1,
+        sigint_timeout_seconds=15.0,
+        kill_timeout_seconds=3.0,
+        ros2_executable="ros2",
+        validate_bag_info=True,
+        bag_info_timeout_seconds=15.0,
+    )
+    return SupervisorSettings(
+        enabled=enabled,
+        control_topic=control_topic,
+        status_topic=status_topic,
+        heartbeat_timeout_seconds=heartbeat_timeout,
+        status_publish_period_seconds=1.0 / status_rate,
+        required_topics=required_topics,
+        # Finalization includes rosbag2 SIGINT, bag-info validation and SHA256
+        # generation.  Leave enough time for a large, fully valid episode.
+        shutdown_timeout_seconds=_positive_number(
+            section.get("shutdown_timeout_seconds", 120.0),
+            "shutdown_timeout_seconds",
+        ),
+        recorder=recorder,
+    )
+
+
+class DataRecorderNode(Node):
+    """Reliable String-topic adapter around ``RecorderControlProtocol``."""
+
+    def __init__(self, settings: SupervisorSettings) -> None:
+        super().__init__("tg3_data_recorder_supervisor")
+        self.settings = settings
+        self._closed = False
+        self._close_lock = threading.Lock()
+        self._last_logged_error = ""
+
+        recorder = DataRecorderManager(settings.recorder)
+        self.protocol = RecorderControlProtocol(
+            recorder,
+            heartbeat_timeout_seconds=settings.heartbeat_timeout_seconds,
+            start_preflight=self._start_preflight,
+        )
+
+        control_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=20,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        status_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self._status_publisher = self.create_publisher(
+            String, settings.status_topic, status_qos
+        )
+        self._control_subscription = self.create_subscription(
+            String,
+            settings.control_topic,
+            self._on_control,
+            control_qos,
+        )
+        self._timer = self.create_timer(
+            settings.status_publish_period_seconds, self._on_timer
+        )
+        self._publish_status(self.protocol.status())
+        self.get_logger().info(
+            "TG3 data recorder ready: control=%s status=%s base=%s topics=%d "
+            "heartbeat_timeout=%.3fs"
+            % (
+                settings.control_topic,
+                settings.status_topic,
+                settings.recorder.base_directory,
+                len(settings.recorder.topics),
+                settings.heartbeat_timeout_seconds,
+            )
+        )
+
+    def _start_preflight(self) -> tuple[bool, str]:
+        if not self.settings.required_topics:
+            return True, ""
+        try:
+            missing = topics_without_publishers(
+                self.settings.required_topics,
+                self.get_publishers_info_by_topic,
+            )
+        except Exception as error:
+            return False, f"cannot inspect ROS graph: {type(error).__name__}: {error}"
+        if missing:
+            return (
+                False,
+                "required ROS topics have no live publisher: "
+                + ", ".join(missing),
+            )
+        return True, ""
+
+    def _on_control(self, message: String) -> None:
+        try:
+            status = self.protocol.handle_json(message.data)
+            self._publish_status(status)
+        except Exception as error:
+            # A malformed or otherwise bad data request must never kill the
+            # node or have any effect on the independent teleoperation bridge.
+            self.get_logger().error(
+                f"data recorder control callback failed: {type(error).__name__}: {error}"
+            )
+
+    def _on_timer(self) -> None:
+        try:
+            self._publish_status(self.protocol.poll())
+        except Exception as error:
+            self.get_logger().error(
+                f"data recorder status timer failed: {type(error).__name__}: {error}"
+            )
+
+    def _publish_status(self, status: dict[str, Any]) -> None:
+        message = String()
+        message.data = json.dumps(
+            status,
+            ensure_ascii=False,
+            sort_keys=True,
+            separators=(",", ":"),
+            allow_nan=False,
+        )
+        self._status_publisher.publish(message)
+        error = status.get("last_error")
+        error_text = error if isinstance(error, str) else ""
+        if error_text and error_text != self._last_logged_error:
+            self.get_logger().warning(f"data recorder status error: {error_text}")
+        self._last_logged_error = error_text
+
+    def close(self) -> bool:
+        with self._close_lock:
+            if self._closed:
+                return True
+            self._closed = True
+        self.get_logger().info("stopping data recorder supervisor")
+        try:
+            return self.protocol.shutdown(
+                timeout=self.settings.shutdown_timeout_seconds
+            )
+        except Exception as error:
+            self.get_logger().error(
+                f"data recorder shutdown failed: {type(error).__name__}: {error}"
+            )
+            return False
+
+
+def _arguments(argv: list[str]) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="TG3 independent ROS 2 MCAP recorder supervisor"
+    )
+    parser.add_argument(
+        "--config",
+        type=Path,
+        default=DEFAULT_CONFIG,
+        help="TG3 bridge config.toml containing [data_collection]",
+    )
+    return parser.parse_args(remove_ros_args(argv)[1:])
+
+
+def main(argv: list[str] | None = None) -> int:
+    process_argv = sys.argv if argv is None else argv
+    arguments = _arguments(process_argv)
+    try:
+        settings = load_settings(arguments.config)
+    except Exception as error:
+        print(
+            f"data recorder configuration error: {type(error).__name__}: {error}",
+            file=sys.stderr,
+        )
+        return 2
+    if not settings.enabled:
+        print("TG3 data collection is disabled in config.toml")
+        return 0
+
+    rclpy.init(args=process_argv)
+    node: DataRecorderNode | None = None
+    exit_code = 0
+    try:
+        node = DataRecorderNode(settings)
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    except Exception as error:
+        print(
+            f"data recorder supervisor failed: {type(error).__name__}: {error}",
+            file=sys.stderr,
+        )
+        exit_code = 1
+    finally:
+        if node is not None:
+            if not node.close():
+                exit_code = 1
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    return exit_code
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tg3_local_teleop/data_recorder_protocol.py

@@ -0,0 +1,546 @@
+#!/usr/bin/env python3
+"""Pure control protocol for the independent TG3 data recorder.
+
+The ROS node is deliberately kept as a thin transport adapter.  This module
+owns JSON validation, idempotence, session/capture matching, acknowledgement
+state, and the bridge-heartbeat watchdog, and can therefore be tested without
+ROS installed.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import time
+from collections import OrderedDict
+from collections.abc import Callable, Mapping, Sequence
+from pathlib import Path
+from typing import Any, Protocol
+
+
+PROTOCOL_VERSION = 1
+ACTIVE_MANAGER_STATES = {
+    "start_pending",
+    "starting",
+    "recording",
+    "stop_pending",
+    "finalizing",
+}
+
+
+class RecorderBackend(Protocol):
+    """The non-blocking subset exposed by ``DataRecorderManager``."""
+
+    def request_start(
+        self,
+        session_id: str,
+        reason: str = "button",
+        *,
+        episode_id: str | None = None,
+    ) -> str | None: ...
+
+    def request_stop(self, reason: str = "operator_button") -> bool: ...
+
+    def status(self) -> dict[str, Any]: ...
+
+    def shutdown(
+        self, *, wait: bool = True, timeout: float | None = None
+    ) -> bool: ...
+
+
+class ProtocolError(ValueError):
+    """A control message is malformed or invalid for the current session."""
+
+    def __init__(self, code: str, message: str) -> None:
+        super().__init__(message)
+        self.code = code
+
+
+def _nonempty_text(value: Any, name: str, *, maximum: int = 128) -> str:
+    if (
+        not isinstance(value, str)
+        or not value
+        or value.strip() != value
+        or len(value) > maximum
+        or any(ord(character) < 0x20 for character in value)
+    ):
+        raise ProtocolError(
+            f"invalid_{name}",
+            f"{name} must be non-empty text up to {maximum} characters",
+        )
+    return value
+
+
+def _capture_id(value: Any) -> str:
+    capture_id = _nonempty_text(value, "capture_id")
+    if (
+        not capture_id[0].isalnum()
+        or Path(capture_id).name != capture_id
+        or capture_id in (".", "..")
+        or any(
+            character
+            not in "-_.0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz"
+            for character in capture_id
+        )
+    ):
+        raise ProtocolError(
+            "invalid_capture_id",
+            "capture_id must be one safe path component beginning with a letter or digit",
+        )
+    return capture_id
+
+
+def _event_sequence(value: Any) -> int:
+    if type(value) is not int or value < 0 or value > (2**63 - 1):
+        raise ProtocolError(
+            "invalid_event_seq",
+            "event_seq must be an integer from 0 through 2^63-1",
+        )
+    return value
+
+
+def _optional_unix_time(value: Any) -> float | None:
+    if value is None:
+        return None
+    if isinstance(value, bool) or not isinstance(value, (int, float)):
+        raise ProtocolError(
+            "invalid_sent_unix_s", "sent_unix_s must be a finite number"
+        )
+    result = float(value)
+    if not math.isfinite(result) or result < 0.0:
+        raise ProtocolError(
+            "invalid_sent_unix_s", "sent_unix_s must be a finite number"
+        )
+    return result
+
+
+def parse_control_message(text: str) -> dict[str, Any]:
+    """Parse one strict but forward-compatible recorder control object."""
+
+    if not isinstance(text, str) or not text or len(text.encode("utf-8")) > 16384:
+        raise ProtocolError(
+            "invalid_json", "control payload must be 1 through 16384 UTF-8 bytes"
+        )
+    try:
+        raw = json.loads(text)
+    except (json.JSONDecodeError, UnicodeError) as error:
+        raise ProtocolError("invalid_json", f"invalid JSON: {error}") from error
+    if not isinstance(raw, Mapping):
+        raise ProtocolError("invalid_json_object", "control JSON must be an object")
+    if raw.get("version") != PROTOCOL_VERSION:
+        raise ProtocolError(
+            "unsupported_version", f"version must equal {PROTOCOL_VERSION}"
+        )
+
+    command = raw.get("command")
+    if command not in ("start", "stop", "heartbeat"):
+        raise ProtocolError(
+            "invalid_command", "command must be start, stop, or heartbeat"
+        )
+    event_seq = _event_sequence(raw.get("event_seq"))
+    request_id = _nonempty_text(raw.get("request_id"), "request_id")
+    session_id = _nonempty_text(
+        raw.get("teleop_session_id"), "teleop_session_id"
+    )
+    capture_id = _capture_id(raw.get("capture_id"))
+    sent_unix_s = _optional_unix_time(raw.get("sent_unix_s"))
+
+    reason_value = raw.get("reason", "bridge_heartbeat" if command == "heartbeat" else "button")
+    reason = _nonempty_text(reason_value, "reason", maximum=512)
+    return {
+        "version": PROTOCOL_VERSION,
+        "command": command,
+        "event_seq": event_seq,
+        "request_id": request_id,
+        "teleop_session_id": session_id,
+        "capture_id": capture_id,
+        "reason": reason,
+        "sent_unix_s": sent_unix_s,
+    }
+
+
+def topics_without_publishers(
+    required_topics: Sequence[str],
+    publisher_lookup: Callable[[str], Sequence[Any]],
+) -> tuple[str, ...]:
+    """Return required topics that currently have no live publisher endpoint.
+
+    ROS graph discovery can retain a topic name solely because this process is
+    subscribed to it.  Looking up publisher endpoints is therefore the
+    meaningful start preflight; topic-name presence alone is insufficient.
+    Exceptions deliberately propagate so the caller can report graph-query
+    failure separately from an ordinary missing publisher.
+    """
+
+    missing: list[str] = []
+    for topic in required_topics:
+        if not publisher_lookup(topic):
+            missing.append(topic)
+    return tuple(missing)
+
+
+class RecorderControlProtocol:
+    """Idempotent bridge-to-recorder protocol and heartbeat watchdog."""
+
+    def __init__(
+        self,
+        recorder: RecorderBackend,
+        *,
+        heartbeat_timeout_seconds: float = 3.0,
+        monotonic: Callable[[], float] = time.monotonic,
+        unix_time: Callable[[], float] = time.time,
+        start_preflight: Callable[[], tuple[bool, str]] | None = None,
+        request_cache_size: int = 512,
+        session_cache_size: int = 64,
+    ) -> None:
+        if (
+            not math.isfinite(heartbeat_timeout_seconds)
+            or heartbeat_timeout_seconds <= 0.0
+        ):
+            raise ValueError("heartbeat timeout must be positive and finite")
+        if request_cache_size <= 0 or session_cache_size <= 0:
+            raise ValueError("protocol cache sizes must be positive")
+        self.recorder = recorder
+        self.heartbeat_timeout_seconds = float(heartbeat_timeout_seconds)
+        self._monotonic = monotonic
+        self._unix_time = unix_time
+        self._start_preflight = start_preflight
+        self._request_cache_size = request_cache_size
+        self._session_cache_size = session_cache_size
+        self._request_cache: OrderedDict[str, tuple[str, bool, str, int]] = (
+            OrderedDict()
+        )
+        self._last_sequence_by_session: OrderedDict[str, int] = OrderedDict()
+        self._capture_id: str | None = None
+        self._session_id: str | None = None
+        self._last_heartbeat_monotonic: float | None = None
+        self._watchdog_stop_requested = False
+        self._last_protocol_error = ""
+        self._ack_event_seq: int | None = None
+        self._ack_request_id: str | None = None
+        self._ack_accepted: bool | None = None
+        self._ack_code = "initialized"
+        self._accepted_count = 0
+        self._rejected_count = 0
+        self._duplicate_count = 0
+        self._watchdog_stop_count = 0
+
+    def handle_json(self, text: str, *, now: float | None = None) -> dict[str, Any]:
+        """Handle one payload without blocking on recorder I/O."""
+
+        timestamp = self._monotonic() if now is None else float(now)
+        try:
+            event = parse_control_message(text)
+        except ProtocolError as error:
+            self._ack_event_seq = None
+            self._ack_request_id = None
+            self._ack_accepted = False
+            self._ack_code = error.code
+            self._last_protocol_error = str(error)
+            self._rejected_count += 1
+            return self.status(now=timestamp)
+
+        fingerprint = json.dumps(event, sort_keys=True, separators=(",", ":"))
+        request_id = event["request_id"]
+        cached = self._request_cache.get(request_id)
+        if cached is not None:
+            cached_fingerprint, accepted, code, event_seq = cached
+            if cached_fingerprint != fingerprint:
+                return self._reject(
+                    event,
+                    "request_id_reused",
+                    "request_id was already used for a different payload",
+                    fingerprint=fingerprint,
+                    cache=False,
+                    now=timestamp,
+                )
+            self._request_cache.move_to_end(request_id)
+            self._duplicate_count += 1
+            self._set_ack(event_seq, request_id, accepted, code)
+            return self.status(now=timestamp)
+
+        session_id = event["teleop_session_id"]
+        last_sequence = self._last_sequence_by_session.get(session_id)
+        if last_sequence is not None and event["event_seq"] <= last_sequence:
+            return self._reject(
+                event,
+                "stale_event_seq",
+                f"event_seq must be greater than the previous value {last_sequence}",
+                fingerprint=fingerprint,
+                now=timestamp,
+            )
+
+        self._remember_sequence(session_id, event["event_seq"])
+        self._last_protocol_error = ""
+        try:
+            accepted, code = self._execute(event, timestamp)
+            message = (
+                ""
+                if accepted
+                else (self._last_protocol_error or code)
+            )
+        except Exception as error:
+            accepted = False
+            code = "recorder_exception"
+            message = f"{type(error).__name__}: {error}"
+
+        if accepted:
+            self._accepted_count += 1
+            self._last_protocol_error = ""
+        else:
+            self._rejected_count += 1
+            self._last_protocol_error = message
+        self._remember_request(
+            request_id,
+            fingerprint,
+            accepted,
+            code,
+            event["event_seq"],
+        )
+        self._set_ack(
+            event["event_seq"],
+            request_id,
+            accepted,
+            code,
+        )
+        return self.status(now=timestamp)
+
+    def poll(self, *, now: float | None = None) -> dict[str, Any]:
+        """Advance the watchdog and return the current status snapshot."""
+
+        timestamp = self._monotonic() if now is None else float(now)
+        recorder_status = self._safe_recorder_status()
+        manager_state = str(recorder_status.get("state", "unknown"))
+        if (
+            manager_state in ("start_pending", "starting", "recording")
+            and self._last_heartbeat_monotonic is not None
+            and not self._watchdog_stop_requested
+            and timestamp - self._last_heartbeat_monotonic
+            > self.heartbeat_timeout_seconds
+        ):
+            try:
+                accepted = self.recorder.request_stop("bridge_heartbeat_timeout")
+            except Exception as error:
+                self._last_protocol_error = (
+                    f"watchdog stop failed: {type(error).__name__}: {error}"
+                )
+            else:
+                if accepted:
+                    self._watchdog_stop_requested = True
+                    self._watchdog_stop_count += 1
+        return self._status_from_snapshot(recorder_status, timestamp)
+
+    def status(self, *, now: float | None = None) -> dict[str, Any]:
+        timestamp = self._monotonic() if now is None else float(now)
+        return self._status_from_snapshot(self._safe_recorder_status(), timestamp)
+
+    def shutdown(self, *, timeout: float | None = None) -> bool:
+        """Request clean recorder finalization when the supervisor exits."""
+
+        return self.recorder.shutdown(wait=True, timeout=timeout)
+
+    def _execute(self, event: Mapping[str, Any], now: float) -> tuple[bool, str]:
+        command = event["command"]
+        if command == "start":
+            return self._start(event, now)
+        if command == "stop":
+            return self._stop(event)
+        return self._heartbeat(event, now)
+
+    def _start(self, event: Mapping[str, Any], now: float) -> tuple[bool, str]:
+        recorder_status = self._safe_recorder_status()
+        manager_state = str(recorder_status.get("state", "unknown"))
+        if manager_state in ACTIVE_MANAGER_STATES:
+            if self._matches_context(event):
+                return True, "already_active"
+            return False, "recorder_busy"
+        if manager_state not in ("idle",):
+            return False, "recorder_unavailable"
+        if self._start_preflight is not None:
+            allowed, detail = self._start_preflight()
+            if not allowed:
+                self._last_protocol_error = detail
+                return False, "preflight_failed"
+
+        actual_capture_id = self.recorder.request_start(
+            event["teleop_session_id"],
+            event["reason"],
+            episode_id=event["capture_id"],
+        )
+        if actual_capture_id is None:
+            return False, "recorder_busy"
+        if actual_capture_id != event["capture_id"]:
+            # A mismatched directory would make bridge samples and manifests
+            # impossible to correlate, so immediately fail safe by stopping.
+            self.recorder.request_stop("capture_id_mismatch")
+            return False, "capture_id_mismatch"
+        self._capture_id = actual_capture_id
+        self._session_id = event["teleop_session_id"]
+        self._last_heartbeat_monotonic = now
+        self._watchdog_stop_requested = False
+        return True, "start_accepted"
+
+    def _stop(self, event: Mapping[str, Any]) -> tuple[bool, str]:
+        recorder_status = self._safe_recorder_status()
+        manager_state = str(recorder_status.get("state", "unknown"))
+        if manager_state not in ACTIVE_MANAGER_STATES:
+            if self._matches_context(event) or self._capture_id is None:
+                return True, "already_stopped"
+            return False, "capture_not_active"
+        if not self._matches_context(event):
+            return False, "capture_mismatch"
+        accepted = self.recorder.request_stop(event["reason"])
+        if not accepted:
+            return False, "stop_rejected"
+        self._watchdog_stop_requested = True
+        return True, "stop_accepted"
+
+    def _heartbeat(
+        self, event: Mapping[str, Any], now: float
+    ) -> tuple[bool, str]:
+        if not self._matches_context(event):
+            return False, "capture_mismatch"
+        recorder_status = self._safe_recorder_status()
+        manager_state = str(recorder_status.get("state", "unknown"))
+        if manager_state not in ACTIVE_MANAGER_STATES:
+            return True, "already_stopped"
+        self._last_heartbeat_monotonic = now
+        return True, "heartbeat_accepted"
+
+    def _matches_context(self, event: Mapping[str, Any]) -> bool:
+        return (
+            self._capture_id == event["capture_id"]
+            and self._session_id == event["teleop_session_id"]
+        )
+
+    def _reject(
+        self,
+        event: Mapping[str, Any],
+        code: str,
+        message: str,
+        *,
+        fingerprint: str,
+        cache: bool = True,
+        now: float,
+    ) -> dict[str, Any]:
+        self._rejected_count += 1
+        self._last_protocol_error = message
+        self._set_ack(
+            event["event_seq"], event["request_id"], False, code
+        )
+        if cache:
+            self._remember_request(
+                event["request_id"],
+                fingerprint,
+                False,
+                code,
+                event["event_seq"],
+            )
+        return self.status(now=now)
+
+    def _remember_sequence(self, session_id: str, sequence: int) -> None:
+        self._last_sequence_by_session[session_id] = sequence
+        self._last_sequence_by_session.move_to_end(session_id)
+        while len(self._last_sequence_by_session) > self._session_cache_size:
+            self._last_sequence_by_session.popitem(last=False)
+
+    def _remember_request(
+        self,
+        request_id: str,
+        fingerprint: str,
+        accepted: bool,
+        code: str,
+        event_seq: int,
+    ) -> None:
+        self._request_cache[request_id] = (
+            fingerprint,
+            accepted,
+            code,
+            event_seq,
+        )
+        self._request_cache.move_to_end(request_id)
+        while len(self._request_cache) > self._request_cache_size:
+            self._request_cache.popitem(last=False)
+
+    def _set_ack(
+        self,
+        event_seq: int,
+        request_id: str,
+        accepted: bool,
+        code: str,
+    ) -> None:
+        self._ack_event_seq = event_seq
+        self._ack_request_id = request_id
+        self._ack_accepted = accepted
+        self._ack_code = code
+
+    def _safe_recorder_status(self) -> dict[str, Any]:
+        try:
+            status = self.recorder.status()
+        except Exception as error:
+            self._last_protocol_error = (
+                f"recorder status failed: {type(error).__name__}: {error}"
+            )
+            return {
+                "state": "unknown",
+                "recording": False,
+                "last_error": self._last_protocol_error,
+            }
+        return dict(status)
+
+    def _status_from_snapshot(
+        self, recorder_status: Mapping[str, Any], now: float
+    ) -> dict[str, Any]:
+        manager_state = str(recorder_status.get("state", "unknown"))
+        if manager_state in ("start_pending", "starting"):
+            state = "starting"
+        elif manager_state == "recording":
+            state = "recording"
+        elif manager_state in ("stop_pending", "finalizing"):
+            state = "stopping"
+        elif manager_state == "idle":
+            last_result = recorder_status.get("last_result")
+            state = last_result if last_result in ("ready", "failed") else "idle"
+        else:
+            state = "failed"
+
+        heartbeat_age: float | None = None
+        if self._last_heartbeat_monotonic is not None:
+            heartbeat_age = max(0.0, now - self._last_heartbeat_monotonic)
+        manager_error = recorder_status.get("last_error")
+        last_error = (
+            str(manager_error)
+            if isinstance(manager_error, str) and manager_error
+            else self._last_protocol_error
+        )
+        return {
+            "version": PROTOCOL_VERSION,
+            "state": state,
+            "capture_id": self._capture_id,
+            "teleop_session_id": self._session_id,
+            "ack_event_seq": self._ack_event_seq,
+            "ack_request_id": self._ack_request_id,
+            "ack_accepted": self._ack_accepted,
+            "ack_code": self._ack_code,
+            "last_error": last_error,
+            "stop_reason": recorder_status.get("stop_reason"),
+            "heartbeat_age_s": heartbeat_age,
+            "heartbeat_timeout_s": self.heartbeat_timeout_seconds,
+            "updated_unix_s": float(self._unix_time()),
+            "statistics": {
+                "accepted": self._accepted_count,
+                "rejected": self._rejected_count,
+                "duplicates": self._duplicate_count,
+                "watchdog_stops": self._watchdog_stop_count,
+            },
+            "recorder": dict(recorder_status),
+        }
+
+
+__all__ = [
+    "PROTOCOL_VERSION",
+    "ProtocolError",
+    "RecorderControlProtocol",
+    "parse_control_message",
+    "topics_without_publishers",
+]

tg3_local_teleop/delete_ready_episode.py

@@ -0,0 +1,537 @@
+#!/usr/bin/env python3
+"""Safely and idempotently remove one verified recorder ``ready`` episode.
+
+This helper intentionally has no command-line option for either root directory.
+It can only move a direct child of the fixed recorder ``ready`` directory into
+the fixed ``deleting`` staging directory and remove that tombstone.  The caller
+must provide both the episode id and the SHA-256 of the exact manifest that was
+verified and durably published on the PS workstation.
+
+Before recursive removal, an authorization ledger is fsynced in the fixed
+``delete_ledger`` sibling directory. It survives partial tombstone deletion and
+is removed only after the tree cleanup itself is durable.
+"""
+
+from __future__ import annotations
+
+import argparse
+import fcntl
+import hashlib
+import json
+import os
+import re
+import stat
+import sys
+import uuid
+from pathlib import Path
+from typing import Any
+
+
+BASE_ROOT = Path("/home/nvidia/tg3_data_collection")
+READY_ROOT = BASE_ROOT / "ready"
+DELETING_ROOT = BASE_ROOT / "deleting"
+LEDGER_ROOT = BASE_ROOT / "delete_ledger"
+SAFE_EPISODE_NAME = re.compile(r"^[A-Za-z0-9][A-Za-z0-9_.-]{0,127}$")
+SAFE_SHA256 = re.compile(r"^[0-9a-f]{64}$")
+MAX_MANIFEST_BYTES = 16 * 1024 * 1024
+MAX_LEDGER_BYTES = 64 * 1024
+LEDGER_SCHEMA_VERSION = 1
+
+
+class DeleteSafetyError(RuntimeError):
+    """The requested deletion did not satisfy the fixed safety contract."""
+
+
+class DeleteCollisionError(DeleteSafetyError):
+    """A conflicting ready/deleting episode exists and was left untouched."""
+
+
+def safe_episode_name(value: str) -> bool:
+    return bool(SAFE_EPISODE_NAME.fullmatch(value)) and value not in (".", "..")
+
+
+def _open_directory(path: Path) -> int:
+    flags = os.O_RDONLY | getattr(os, "O_DIRECTORY", 0) | getattr(os, "O_NOFOLLOW", 0)
+    try:
+        descriptor = os.open(path, flags)
+    except OSError as exc:
+        raise DeleteSafetyError(f"unsafe or missing directory: {path}: {exc}") from exc
+    try:
+        metadata = os.fstat(descriptor)
+        if not stat.S_ISDIR(metadata.st_mode):
+            raise DeleteSafetyError(f"not a directory: {path}")
+        return descriptor
+    except Exception:
+        os.close(descriptor)
+        raise
+
+
+def _open_child_directory(parent_fd: int, name: str) -> int:
+    flags = os.O_RDONLY | getattr(os, "O_DIRECTORY", 0) | getattr(os, "O_NOFOLLOW", 0)
+    try:
+        descriptor = os.open(name, flags, dir_fd=parent_fd)
+    except OSError as exc:
+        raise DeleteSafetyError(f"unsafe or missing episode directory: {name}: {exc}") from exc
+    try:
+        if not stat.S_ISDIR(os.fstat(descriptor).st_mode):
+            raise DeleteSafetyError(f"episode is not a directory: {name}")
+        return descriptor
+    except Exception:
+        os.close(descriptor)
+        raise
+
+
+def _read_regular_file(parent_fd: int, name: str, *, max_bytes: int) -> bytes:
+    flags = os.O_RDONLY | getattr(os, "O_NOFOLLOW", 0)
+    try:
+        descriptor = os.open(name, flags, dir_fd=parent_fd)
+    except OSError as exc:
+        raise DeleteSafetyError(f"missing or unsafe regular file {name!r}: {exc}") from exc
+    try:
+        metadata = os.fstat(descriptor)
+        if not stat.S_ISREG(metadata.st_mode):
+            raise DeleteSafetyError(f"not a regular file: {name}")
+        if metadata.st_size > max_bytes:
+            raise DeleteSafetyError(f"file is too large: {name}")
+        chunks: list[bytes] = []
+        remaining = max_bytes + 1
+        while remaining:
+            block = os.read(descriptor, min(1024 * 1024, remaining))
+            if not block:
+                break
+            chunks.append(block)
+            remaining -= len(block)
+        payload = b"".join(chunks)
+        if len(payload) > max_bytes:
+            raise DeleteSafetyError(f"file is too large: {name}")
+        return payload
+    finally:
+        os.close(descriptor)
+
+
+def _validate_tree_entries(directory_fd: int) -> None:
+    """Require the quarantined recorder tree to contain only dirs/files."""
+
+    for child in os.listdir(directory_fd):
+        if child in (".", "..") or "/" in child or "\x00" in child:
+            raise DeleteSafetyError(f"unsafe directory entry: {child!r}")
+        metadata = os.stat(child, dir_fd=directory_fd, follow_symlinks=False)
+        if stat.S_ISDIR(metadata.st_mode):
+            child_fd = _open_child_directory(directory_fd, child)
+            try:
+                _validate_tree_entries(child_fd)
+            finally:
+                os.close(child_fd)
+        elif not stat.S_ISREG(metadata.st_mode):
+            raise DeleteSafetyError(f"episode contains a symlink/special entry: {child}")
+
+
+def _ledger_name(episode: str, manifest_sha256: str) -> str:
+    return f"{episode}.{manifest_sha256}.authorized.json"
+
+
+def _ledger_payload(
+    episode: str, manifest_sha256: str, tombstone: str
+) -> dict[str, Any]:
+    return {
+        "schema_version": LEDGER_SCHEMA_VERSION,
+        "state": "AUTHORIZED_DELETE",
+        "episode_id": episode,
+        "manifest_sha256": manifest_sha256,
+        "tombstone": tombstone,
+    }
+
+
+def _validate_ledger(
+    ledger_fd: int,
+    ledger_name: str,
+    *,
+    episode: str,
+    manifest_sha256: str,
+    tombstone: str,
+) -> None:
+    raw = _read_regular_file(ledger_fd, ledger_name, max_bytes=MAX_LEDGER_BYTES)
+    try:
+        payload: Any = json.loads(raw.decode("utf-8"))
+    except (UnicodeDecodeError, json.JSONDecodeError) as exc:
+        raise DeleteSafetyError(f"invalid deletion authorization ledger: {exc}") from exc
+    if payload != _ledger_payload(episode, manifest_sha256, tombstone):
+        raise DeleteSafetyError("deletion authorization ledger identity mismatch")
+
+
+def _write_ledger(
+    ledger_fd: int,
+    ledger_name: str,
+    *,
+    episode: str,
+    manifest_sha256: str,
+    tombstone: str,
+) -> None:
+    payload = (
+        json.dumps(
+            _ledger_payload(episode, manifest_sha256, tombstone),
+            sort_keys=True,
+            separators=(",", ":"),
+        )
+        + "\n"
+    ).encode("utf-8")
+    temporary = f".{ledger_name}.{uuid.uuid4().hex}.tmp"
+    flags = os.O_WRONLY | os.O_CREAT | os.O_EXCL | getattr(os, "O_NOFOLLOW", 0)
+    descriptor = os.open(temporary, flags, 0o600, dir_fd=ledger_fd)
+    try:
+        view = memoryview(payload)
+        while view:
+            written = os.write(descriptor, view)
+            if written <= 0:
+                raise OSError("short write while creating deletion ledger")
+            view = view[written:]
+        os.fsync(descriptor)
+    finally:
+        os.close(descriptor)
+    try:
+        # A hard-link publish is atomic and refuses to replace any existing
+        # sidecar. The helper flock serializes legitimate writers as well.
+        os.link(
+            temporary,
+            ledger_name,
+            src_dir_fd=ledger_fd,
+            dst_dir_fd=ledger_fd,
+            follow_symlinks=False,
+        )
+        os.fsync(ledger_fd)
+    finally:
+        try:
+            os.unlink(temporary, dir_fd=ledger_fd)
+            os.fsync(ledger_fd)
+        except FileNotFoundError:
+            pass
+
+
+def _validate_episode(
+    parent_fd: int,
+    directory_name: str,
+    *,
+    episode: str,
+    expected_manifest_sha256: str,
+) -> None:
+    episode_fd = _open_child_directory(parent_fd, directory_name)
+    try:
+        # READY must be a real regular file.  Its contents are deliberately not
+        # trusted as a deletion credential; the exact manifest digest is.
+        _read_regular_file(episode_fd, "READY", max_bytes=4096)
+        raw_manifest = _read_regular_file(
+            episode_fd, "manifest.json", max_bytes=MAX_MANIFEST_BYTES
+        )
+        actual_digest = hashlib.sha256(raw_manifest).hexdigest()
+        if actual_digest != expected_manifest_sha256:
+            raise DeleteSafetyError(
+                "manifest SHA-256 mismatch: "
+                f"expected {expected_manifest_sha256}, got {actual_digest}"
+            )
+        try:
+            manifest: Any = json.loads(raw_manifest.decode("utf-8"))
+        except (UnicodeDecodeError, json.JSONDecodeError) as exc:
+            raise DeleteSafetyError(f"invalid manifest JSON: {exc}") from exc
+        if not isinstance(manifest, dict):
+            raise DeleteSafetyError("manifest root is not an object")
+        if manifest.get("state") != "complete":
+            raise DeleteSafetyError("manifest state is not complete")
+        if manifest.get("episode_id") != episode:
+            raise DeleteSafetyError("manifest episode_id does not match target")
+        _validate_tree_entries(episode_fd)
+    finally:
+        os.close(episode_fd)
+
+
+def _remove_tree_at(parent_fd: int, name: str) -> None:
+    """Remove one child tree without following any symbolic link."""
+
+    directory_fd = _open_child_directory(parent_fd, name)
+    try:
+        for child in os.listdir(directory_fd):
+            if child in (".", "..") or "/" in child or "\x00" in child:
+                raise DeleteSafetyError(f"unsafe directory entry: {child!r}")
+            metadata = os.stat(child, dir_fd=directory_fd, follow_symlinks=False)
+            if stat.S_ISDIR(metadata.st_mode):
+                _remove_tree_at(directory_fd, child)
+            else:
+                # Symlinks and all non-directory entries are unlinked, never
+                # followed. Recorder output is expected to contain only files.
+                os.unlink(child, dir_fd=directory_fd)
+        os.fsync(directory_fd)
+    finally:
+        os.close(directory_fd)
+    os.rmdir(name, dir_fd=parent_fd)
+
+
+def _child_kind(parent_fd: int, name: str) -> str:
+    try:
+        metadata = os.stat(name, dir_fd=parent_fd, follow_symlinks=False)
+    except FileNotFoundError:
+        return "absent"
+    if stat.S_ISDIR(metadata.st_mode):
+        return "directory"
+    if stat.S_ISLNK(metadata.st_mode):
+        return "symlink"
+    if stat.S_ISREG(metadata.st_mode):
+        return "regular"
+    return "other"
+
+
+def delete_ready_episode(
+    episode: str,
+    manifest_sha256: str,
+    *,
+    ready_root: Path = READY_ROOT,
+    deleting_root: Path = DELETING_ROOT,
+    ledger_root: Path | None = None,
+) -> dict[str, str]:
+    """Compare, atomically quarantine, and remove exactly one ready episode.
+
+    Root arguments are injectable only for unit tests. The CLI below always
+    uses the immutable production constants, including a ledger directory that
+    is outside the recursively removed ``deleting`` tree.
+    """
+
+    if not safe_episode_name(episode):
+        raise DeleteSafetyError(f"unsafe episode id: {episode!r}")
+    if not SAFE_SHA256.fullmatch(manifest_sha256):
+        raise DeleteSafetyError("manifest SHA-256 must be 64 lowercase hex characters")
+
+    ready_root = Path(ready_root)
+    deleting_root = Path(deleting_root)
+    ledger_root = Path(ledger_root or (deleting_root.parent / LEDGER_ROOT.name))
+    if not (
+        ready_root.parent == deleting_root.parent == ledger_root.parent
+    ):
+        raise DeleteSafetyError("ready, deleting, and ledger roots must be siblings")
+    base_root = ready_root.parent
+
+    # Refuse a symlinked/wrong parent even in tests. resolve(strict=True) also
+    # proves that all existing parent components resolve to this exact path.
+    try:
+        if base_root.resolve(strict=True) != base_root.absolute():
+            raise DeleteSafetyError("base root must not contain symlinks")
+        if ready_root.resolve(strict=True) != ready_root.absolute():
+            raise DeleteSafetyError("ready root must not contain symlinks")
+    except FileNotFoundError as exc:
+        raise DeleteSafetyError(f"fixed recorder root is missing: {exc}") from exc
+
+    base_fd = _open_directory(base_root)
+    ready_fd = _open_directory(ready_root)
+    deleting_fd: int | None = None
+    ledger_fd: int | None = None
+    lock_fd: int | None = None
+    try:
+        base_stat = os.fstat(base_fd)
+        ready_stat = os.fstat(ready_fd)
+        if base_stat.st_dev != ready_stat.st_dev:
+            raise DeleteSafetyError("ready root is not on the recorder filesystem")
+
+        deleting_kind = _child_kind(base_fd, deleting_root.name)
+        if deleting_kind == "absent":
+            os.mkdir(deleting_root.name, mode=0o750, dir_fd=base_fd)
+            os.fsync(base_fd)
+        elif deleting_kind != "directory":
+            raise DeleteSafetyError("deleting root is not a real directory")
+        deleting_fd = _open_directory(deleting_root)
+        if os.fstat(deleting_fd).st_dev != ready_stat.st_dev:
+            raise DeleteSafetyError("deleting root is not on the ready filesystem")
+
+        ledger_kind = _child_kind(base_fd, ledger_root.name)
+        if ledger_kind == "absent":
+            os.mkdir(ledger_root.name, mode=0o750, dir_fd=base_fd)
+            os.fsync(base_fd)
+        elif ledger_kind != "directory":
+            raise DeleteSafetyError("delete ledger root is not a real directory")
+        ledger_fd = _open_directory(ledger_root)
+        if os.fstat(ledger_fd).st_dev != ready_stat.st_dev:
+            raise DeleteSafetyError("delete ledger is not on the ready filesystem")
+
+        lock_flags = os.O_RDWR | os.O_CREAT | getattr(os, "O_NOFOLLOW", 0)
+        lock_fd = os.open(".delete_ready.lock", lock_flags, 0o600, dir_fd=deleting_fd)
+        if not stat.S_ISREG(os.fstat(lock_fd).st_mode):
+            raise DeleteSafetyError("deletion lock is not a regular file")
+        fcntl.flock(lock_fd, fcntl.LOCK_EX)
+
+        tombstone = f"{episode}.{manifest_sha256}.deleting"
+        authorization = _ledger_name(episode, manifest_sha256)
+        ready_kind = _child_kind(ready_fd, episode)
+        tombstone_kind = _child_kind(deleting_fd, tombstone)
+        authorization_kind = _child_kind(ledger_fd, authorization)
+
+        # Any other tombstone for this episode is a manifest-identity collision.
+        prefix = f"{episode}."
+        collisions = [
+            name
+            for name in os.listdir(deleting_fd)
+            if name.startswith(prefix)
+            and name.endswith(".deleting")
+            and name != tombstone
+        ]
+        if collisions:
+            raise DeleteCollisionError(
+                f"conflicting deleting tombstone(s) exist: {sorted(collisions)!r}"
+            )
+        ledger_collisions = [
+            name
+            for name in os.listdir(ledger_fd)
+            if name.startswith(prefix)
+            and name.endswith(".authorized.json")
+            and name != authorization
+        ]
+        if ledger_collisions:
+            raise DeleteCollisionError(
+                "conflicting deletion authorization(s) exist: "
+                f"{sorted(ledger_collisions)!r}"
+            )
+        if ready_kind not in ("absent", "directory"):
+            raise DeleteSafetyError(f"ready target is unsafe: {ready_kind}")
+        if tombstone_kind not in ("absent", "directory"):
+            raise DeleteSafetyError(f"deleting target is unsafe: {tombstone_kind}")
+        if authorization_kind not in ("absent", "regular"):
+            raise DeleteSafetyError(
+                f"deletion authorization is unsafe: {authorization_kind}"
+            )
+        if ready_kind == "directory" and tombstone_kind == "directory":
+            raise DeleteCollisionError("ready target and deleting tombstone both exist")
+        if ready_kind == "directory" and authorization_kind == "regular":
+            raise DeleteCollisionError(
+                "ready target exists alongside a stale deletion authorization"
+            )
+
+        if ready_kind == "directory":
+            _validate_episode(
+                ready_fd,
+                episode,
+                episode=episode,
+                expected_manifest_sha256=manifest_sha256,
+            )
+            os.rename(
+                episode,
+                tombstone,
+                src_dir_fd=ready_fd,
+                dst_dir_fd=deleting_fd,
+            )
+            os.fsync(ready_fd)
+            os.fsync(deleting_fd)
+            # Re-validate after the rename.  If the source name was exchanged
+            # between the first comparison and rename, the unexpected tree is
+            # quarantined but is never unlinked.
+            _validate_episode(
+                deleting_fd,
+                tombstone,
+                episode=episode,
+                expected_manifest_sha256=manifest_sha256,
+            )
+            _write_ledger(
+                ledger_fd,
+                authorization,
+                episode=episode,
+                manifest_sha256=manifest_sha256,
+                tombstone=tombstone,
+            )
+            authorization_kind = "regular"
+            result_state = "deleted"
+        elif tombstone_kind == "directory":
+            if authorization_kind == "regular":
+                # The durable out-of-tree ledger remains usable even if an
+                # interrupted recursive removal already removed READY and the
+                # manifest from inside the tombstone.
+                _validate_ledger(
+                    ledger_fd,
+                    authorization,
+                    episode=episode,
+                    manifest_sha256=manifest_sha256,
+                    tombstone=tombstone,
+                )
+            else:
+                # A crash between rename and ledger publication is recoverable
+                # only while the tombstone still proves its original identity.
+                _validate_episode(
+                    deleting_fd,
+                    tombstone,
+                    episode=episode,
+                    expected_manifest_sha256=manifest_sha256,
+                )
+                _write_ledger(
+                    ledger_fd,
+                    authorization,
+                    episode=episode,
+                    manifest_sha256=manifest_sha256,
+                    tombstone=tombstone,
+                )
+                authorization_kind = "regular"
+            result_state = "resumed_delete"
+        else:
+            if authorization_kind == "regular":
+                # The tree was fully removed and only the final ledger cleanup
+                # (or its ACK) was interrupted.
+                _validate_ledger(
+                    ledger_fd,
+                    authorization,
+                    episode=episode,
+                    manifest_sha256=manifest_sha256,
+                    tombstone=tombstone,
+                )
+                os.unlink(authorization, dir_fd=ledger_fd)
+                os.fsync(ledger_fd)
+            return {
+                "state": "already_absent",
+                "episode_id": episode,
+                "manifest_sha256": manifest_sha256,
+            }
+
+        _remove_tree_at(deleting_fd, tombstone)
+        os.fsync(deleting_fd)
+        # Delete authorization is deliberately the final artifact removed.
+        # If this unlink/ACK is lost, the next invocation validates it and
+        # safely returns already_absent.
+        _validate_ledger(
+            ledger_fd,
+            authorization,
+            episode=episode,
+            manifest_sha256=manifest_sha256,
+            tombstone=tombstone,
+        )
+        os.unlink(authorization, dir_fd=ledger_fd)
+        os.fsync(ledger_fd)
+        return {
+            "state": result_state,
+            "episode_id": episode,
+            "manifest_sha256": manifest_sha256,
+        }
+    finally:
+        if lock_fd is not None:
+            os.close(lock_fd)
+        if deleting_fd is not None:
+            os.close(deleting_fd)
+        if ledger_fd is not None:
+            os.close(ledger_fd)
+        os.close(ready_fd)
+        os.close(base_fd)
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("episode_id")
+    parser.add_argument("manifest_sha256")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    try:
+        result = delete_ready_episode(args.episode_id, args.manifest_sha256)
+    except DeleteCollisionError as exc:
+        print(json.dumps({"state": "collision", "error": str(exc)}))
+        return 3
+    except (DeleteSafetyError, OSError) as exc:
+        print(json.dumps({"state": "refused", "error": str(exc)}))
+        return 2
+    print(json.dumps(result, sort_keys=True))
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

tg3_local_teleop/gesture_toggle.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-"""Pure state machine for the right-B pointing-hand gesture.
+"""Pure state machines for robot-side right-hand button gestures.
 
 This module deliberately has no ROS dependencies so the hold/release safety
 rules can be exercised offline before the bridge is deployed on a robot.
@@ -182,6 +182,145 @@ class GestureToggle:
         return "active" if self.active else "idle"
 
 
+class GuardedMomentaryGesture:
+    """Hold one gesture only while its button is pressed.
+
+    A newly armed teleoperation session must first observe a stable release,
+    preventing a button held before START from moving the hand.  Missing or
+    malformed input fails closed and requires another stable release.
+    """
+
+    def __init__(self, release_seconds: float) -> None:
+        if not math.isfinite(release_seconds) or release_seconds <= 0.0:
+            raise ValueError("gesture release time must be positive and finite")
+        self.release_seconds = float(release_seconds)
+        self.active = False
+        self.release_started_at: float | None = None
+        self.require_release = True
+        self.armed = False
+        self.input_healthy = False
+        self.button_pressed: bool | None = None
+        self._freeze_right_hand = False
+        self.activation_count = 0
+        self.last_transition = "initialized"
+
+    def new_session(self) -> None:
+        self.active = False
+        self.release_started_at = None
+        self.require_release = True
+        self.armed = True
+        self.input_healthy = False
+        self.button_pressed = None
+        self._freeze_right_hand = True
+        self.last_transition = "new_session"
+
+    def disarm(self, reason: str = "disarmed") -> None:
+        self.active = False
+        self.release_started_at = None
+        self.require_release = True
+        self.armed = False
+        self.input_healthy = False
+        self.button_pressed = None
+        self._freeze_right_hand = False
+        self.last_transition = reason
+
+    def update(
+        self,
+        now: float,
+        *,
+        armed: bool,
+        input_healthy: bool,
+        pressed: bool | None,
+    ) -> bool:
+        """Advance the gate and report only active/inactive edge changes."""
+
+        if not math.isfinite(now):
+            raise ValueError("gesture clock must be finite")
+        was_active = self.active
+        if not armed:
+            if self.armed or self.active:
+                self.disarm()
+            return was_active != self.active
+        if not self.armed:
+            self.new_session()
+
+        self.input_healthy = bool(input_healthy)
+        self.button_pressed = pressed
+        if not input_healthy or pressed is None:
+            self.active = False
+            self.release_started_at = None
+            self.require_release = True
+            self._freeze_right_hand = True
+            self.last_transition = "input_unhealthy"
+            return was_active != self.active
+
+        if self.require_release:
+            self.active = False
+            self._freeze_right_hand = True
+            if pressed:
+                self.release_started_at = None
+            elif self.release_started_at is None:
+                self.release_started_at = now
+            elif now - self.release_started_at >= self.release_seconds:
+                self.require_release = False
+                self.release_started_at = None
+                self._freeze_right_hand = False
+                self.last_transition = "release_ready"
+            return was_active != self.active
+
+        self.release_started_at = None
+        self._freeze_right_hand = False
+        self.active = bool(pressed)
+        if self.active != was_active:
+            if self.active:
+                self.activation_count += 1
+                self.last_transition = "activated"
+            else:
+                self.last_transition = "deactivated"
+        return was_active != self.active
+
+    @property
+    def freeze_right_hand(self) -> bool:
+        return self.armed and not self.active and self._freeze_right_hand
+
+    def release_elapsed(self, now: float) -> float:
+        if self.release_started_at is None:
+            return 0.0
+        return max(0.0, now - self.release_started_at)
+
+    @property
+    def state(self) -> str:
+        if not self.armed:
+            return "disarmed"
+        if not self.input_healthy:
+            return "input_unhealthy"
+        if self.button_pressed is None:
+            return "invalid_button"
+        if self.require_release:
+            return "awaiting_release"
+        return "active" if self.active else "idle"
+
+
+def right_a_pressed(data: Mapping[str, Any]) -> bool | None:
+    """Return TS1P right-A state, or ``None`` for a malformed sample."""
+
+    try:
+        buttons = data["button"]
+        if not isinstance(buttons, Mapping):
+            return None
+        right = buttons["right"]
+        if not isinstance(right, (list, tuple)) or len(right) < 1:
+            return None
+        value = right[0]
+        if isinstance(value, bool):
+            return value
+        if isinstance(value, int) and value in (0, 1):
+            return bool(value)
+        return None
+    except (KeyError, TypeError):
+        return None
+
+
 def right_b_pressed(data: Mapping[str, Any]) -> bool | None:
     """Return TS1P right-B state, or ``None`` for a malformed sample."""
 
@@ -219,7 +358,7 @@ def normalized_pose_to_positions(
     ]
 
 
-def select_right_hand_target(
+def select_hand_target(
     source_target: Sequence[int],
     previous_command: Sequence[int] | None,
     gesture_target: Sequence[int],
@@ -227,7 +366,7 @@ def select_right_hand_target(
     active: bool,
     freeze: bool,
 ) -> list[int]:
-    """Apply the right-only gesture override or fail-safe input freeze."""
+    """Apply one hand's gesture override or fail-safe input freeze."""
 
     source = list(source_target)
     gesture = list(gesture_target)
@@ -241,3 +380,22 @@ def select_right_hand_target(
             raise ValueError("cannot freeze without a complete previous command")
         return previous
     return source
+
+
+def select_right_hand_target(
+    source_target: Sequence[int],
+    previous_command: Sequence[int] | None,
+    gesture_target: Sequence[int],
+    *,
+    active: bool,
+    freeze: bool,
+) -> list[int]:
+    """Backward-compatible name for the original right-only implementation."""
+
+    return select_hand_target(
+        source_target,
+        previous_command,
+        gesture_target,
+        active=active,
+        freeze=freeze,
+    )

tg3_local_teleop/head_control.py

@@ -0,0 +1,226 @@
+#!/usr/bin/env python3
+"""Pure safety state for direct TS1P right-stick head-pitch control."""
+
+from __future__ import annotations
+
+import math
+from typing import Any, Mapping
+
+
+def right_stick_vertical(data: Mapping[str, Any]) -> float | None:
+    """Return the TS1P right-stick vertical axis or fail closed."""
+
+    try:
+        joysticks = data["joystick"]
+        if not isinstance(joysticks, Mapping):
+            return None
+        right = joysticks["right"]
+        if not isinstance(right, (list, tuple)) or len(right) != 2:
+            return None
+        value = float(right[0])
+        if not math.isfinite(value) or abs(value) > 1.2:
+            return None
+        return max(-1.0, min(1.0, value))
+    except (KeyError, TypeError, ValueError):
+        return None
+
+
+def left_z_pressed(data: Mapping[str, Any]) -> bool | None:
+    """Return the physical left Z state, accepting only explicit binary data."""
+
+    try:
+        buttons = data["button"]
+        if not isinstance(buttons, Mapping):
+            return None
+        left = buttons["left"]
+        if not isinstance(left, (list, tuple)) or len(left) < 3:
+            return None
+        raw = left[2]
+        if isinstance(raw, bool):
+            return raw
+        if isinstance(raw, int) and not isinstance(raw, bool) and raw in (0, 1):
+            return bool(raw)
+        return None
+    except (KeyError, TypeError):
+        return None
+
+
+def head_pitch_command_axis(data: Mapping[str, Any]) -> float | None:
+    """Gate right-stick pitch behind left Z; malformed input fails closed."""
+
+    pressed = left_z_pressed(data)
+    axis = right_stick_vertical(data)
+    if pressed is None or axis is None:
+        return None
+    return axis if pressed else 0.0
+
+
+class HeadPitchController:
+    """Integrate a joystick velocity request into a bounded pitch target."""
+
+    def __init__(
+        self,
+        *,
+        deadzone: float,
+        max_speed_rad_s: float,
+        max_accel_rad_s2: float,
+        min_pitch_rad: float,
+        max_pitch_rad: float,
+        axis_sign: float,
+        max_dt_s: float = 0.1,
+    ) -> None:
+        values = (
+            deadzone,
+            max_speed_rad_s,
+            max_accel_rad_s2,
+            min_pitch_rad,
+            max_pitch_rad,
+            axis_sign,
+            max_dt_s,
+        )
+        if not all(math.isfinite(value) for value in values):
+            raise ValueError("head-pitch settings must be finite")
+        if not 0.0 <= deadzone < 1.0:
+            raise ValueError("head-pitch deadzone must be in [0, 1)")
+        if max_speed_rad_s <= 0.0 or max_accel_rad_s2 <= 0.0:
+            raise ValueError("head-pitch speed and acceleration must be positive")
+        if min_pitch_rad >= max_pitch_rad:
+            raise ValueError("head-pitch range is invalid")
+        if axis_sign == 0.0 or max_dt_s <= 0.0:
+            raise ValueError("head-pitch sign and maximum dt must be non-zero")
+        self.deadzone = float(deadzone)
+        self.max_speed_rad_s = float(max_speed_rad_s)
+        self.max_accel_rad_s2 = float(max_accel_rad_s2)
+        self.min_pitch_rad = float(min_pitch_rad)
+        self.max_pitch_rad = float(max_pitch_rad)
+        self.axis_sign = math.copysign(1.0, axis_sign)
+        self.max_dt_s = float(max_dt_s)
+        self.armed = False
+        self.require_neutral = True
+        self.active = False
+        self.target_rad: float | None = None
+        self.velocity_rad_s = 0.0
+        self.last_update_at: float | None = None
+        self.last_transition = "initialized"
+
+    def new_session(self) -> None:
+        self.armed = True
+        self.require_neutral = True
+        self.active = False
+        self.target_rad = None
+        self.velocity_rad_s = 0.0
+        self.last_update_at = None
+        self.last_transition = "new_session"
+
+    def disarm(self, reason: str = "disarmed") -> None:
+        self.armed = False
+        self.require_neutral = True
+        self.active = False
+        self.target_rad = None
+        self.velocity_rad_s = 0.0
+        self.last_update_at = None
+        self.last_transition = reason
+
+    def _shaped_axis(self, axis: float) -> float:
+        magnitude = abs(axis)
+        if magnitude <= self.deadzone:
+            return 0.0
+        normalized = (magnitude - self.deadzone) / (1.0 - self.deadzone)
+        return math.copysign(normalized, axis)
+
+    def update(
+        self,
+        now: float,
+        *,
+        armed: bool,
+        input_healthy: bool,
+        feedback_healthy: bool,
+        external_busy: bool,
+        axis: float | None,
+        feedback_position_rad: float | None,
+    ) -> float | None:
+        """Return one absolute pitch target, or ``None`` when output is gated."""
+
+        if not math.isfinite(now):
+            raise ValueError("head-pitch clock must be finite")
+        if not armed:
+            if self.armed:
+                self.disarm()
+            return None
+        if not self.armed:
+            self.new_session()
+
+        feedback_valid = (
+            feedback_position_rad is not None
+            and math.isfinite(float(feedback_position_rad))
+        )
+        if (
+            not input_healthy
+            or not feedback_healthy
+            or external_busy
+            or axis is None
+            or not feedback_valid
+        ):
+            self.require_neutral = True
+            self.active = False
+            self.target_rad = None
+            self.velocity_rad_s = 0.0
+            self.last_update_at = now
+            if external_busy:
+                self.last_transition = "external_command_busy"
+            elif not feedback_healthy or not feedback_valid:
+                self.last_transition = "feedback_unhealthy"
+            else:
+                self.last_transition = "input_unhealthy"
+            return None
+
+        feedback = float(feedback_position_rad)
+        shaped = self._shaped_axis(float(axis))
+        if self.require_neutral:
+            self.active = False
+            self.target_rad = feedback
+            self.velocity_rad_s = 0.0
+            self.last_update_at = now
+            if shaped == 0.0:
+                self.require_neutral = False
+                self.last_transition = "neutral_ready"
+            return None
+
+        if shaped == 0.0:
+            self.active = False
+            self.target_rad = feedback
+            self.velocity_rad_s = 0.0
+            self.last_update_at = now
+            self.last_transition = "neutral"
+            return None
+
+        if self.target_rad is None:
+            self.target_rad = feedback
+        dt = 0.0
+        if self.last_update_at is not None:
+            dt = min(self.max_dt_s, max(0.0, now - self.last_update_at))
+        self.last_update_at = now
+        desired_velocity = shaped * self.axis_sign * self.max_speed_rad_s
+        max_velocity_step = self.max_accel_rad_s2 * dt
+        self.velocity_rad_s += max(
+            -max_velocity_step,
+            min(max_velocity_step, desired_velocity - self.velocity_rad_s),
+        )
+        self.target_rad = max(
+            self.min_pitch_rad,
+            min(
+                self.max_pitch_rad,
+                self.target_rad + self.velocity_rad_s * dt,
+            ),
+        )
+        self.active = True
+        self.last_transition = "active"
+        return self.target_rad
+
+
+__all__ = [
+    "HeadPitchController",
+    "head_pitch_command_axis",
+    "left_z_pressed",
+    "right_stick_vertical",
+]

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt

@@ -7,7 +7,14 @@ find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/MotorStatus.msg"
+  "msg/MotorCtrl.msg"
   "msg/ArmStatus.msg"
+  "msg/HeadCtrl.msg"
+  "msg/HeadStatus.msg"
+  "msg/WaistStatus.msg"
+  "msg/LegStatus.msg"
+  "msg/ImuStatus.msg"
+  "msg/RobotState.msg"
   DEPENDENCIES std_msgs
 )
 

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+uint8 mode
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorCtrl[] ctrl

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorStatus[] status

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg

@@ -0,0 +1,20 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+float64 qx
+float64 qy
+float64 qz
+float64 qw
+float64 roll
+float64 pitch
+float64 yaw
+float64 wx
+float64 wy
+float64 wz
+float64 ax
+float64 ay
+float64 az
+float64[9] orientation_covariance
+float64[9] angular_velocity_covariance
+float64[9] linear_acceleration_covariance
+uint32 error

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorStatus[] status

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg

@@ -0,0 +1,8 @@
+uint16 name
+float64 kp
+float64 kd
+float64 pos
+float64 spd
+float64 tor
+float64 cur
+string joint_ids

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+ros2_bridge_msgs/HeadStatus head
+ros2_bridge_msgs/WaistStatus waist
+ros2_bridge_msgs/LegStatus leg
+ros2_bridge_msgs/ArmStatus arm
+ros2_bridge_msgs/ImuStatus imu

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+uint8 label
+uint8 reserved
+ros2_bridge_msgs/MotorStatus[] status

tg3_local_teleop/run_data_recorder.sh

@@ -0,0 +1,31 @@
+#!/usr/bin/env bash
+# Load every overlay needed to deserialize the TianGong custom message types
+# before rosbag2 discovers the configured topics.
+set -eo pipefail
+
+APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+required_setups=(
+  /opt/ros/jazzy/setup.bash
+  /home/nvidia/xos/setup.bash
+  /opt/robot_tele_server/install/setup.bash
+)
+for setup_file in "${required_setups[@]}"; do
+  if [[ ! -f "$setup_file" ]]; then
+    echo "Required ROS setup is missing: $setup_file" >&2
+    exit 1
+  fi
+  # shellcheck disable=SC1090
+  source "$setup_file"
+done
+
+# Do not source $APP_DIR/ros2_py here.  That deliberately minimal package only
+# supplies the two Python wire types used by tg3_local_teleop.py; placing its
+# partial ros2_bridge_msgs libraries ahead of the vendor's complete C++ package
+# makes rosbag2 fail to load RobotState/ArmCtrl type support.  The recorder must
+# use /opt/robot_tele_server/install for the complete vendor message package.
+set -u
+
+exec python3 "$APP_DIR/data_recorder_node.py" \
+  --config "$APP_DIR/config.toml" \
+  "$@"

tg3_local_teleop/test_data_recorder_protocol.py

@@ -0,0 +1,274 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import json
+import unittest
+from typing import Any
+
+from data_recorder_protocol import (
+    ProtocolError,
+    RecorderControlProtocol,
+    parse_control_message,
+    topics_without_publishers,
+)
+
+
+class FakeRecorder:
+    def __init__(self) -> None:
+        self.snapshot: dict[str, Any] = {
+            "state": "idle",
+            "recording": False,
+            "episode_id": None,
+            "session_id": None,
+            "stop_reason": None,
+            "last_result": None,
+            "last_error": "",
+            "topics": ["/robot_state"],
+        }
+        self.starts: list[tuple[str, str, str | None]] = []
+        self.stops: list[str] = []
+        self.shutdowns = 0
+        self.accept_start = True
+
+    def request_start(
+        self,
+        session_id: str,
+        reason: str = "button",
+        *,
+        episode_id: str | None = None,
+    ) -> str | None:
+        self.starts.append((session_id, reason, episode_id))
+        if not self.accept_start or self.snapshot["state"] != "idle":
+            return None
+        self.snapshot.update(
+            {
+                "state": "start_pending",
+                "episode_id": episode_id,
+                "session_id": session_id,
+                "last_result": None,
+            }
+        )
+        return episode_id
+
+    def request_stop(self, reason: str = "operator_button") -> bool:
+        self.stops.append(reason)
+        if self.snapshot["state"] not in (
+            "start_pending",
+            "starting",
+            "recording",
+            "stop_pending",
+        ):
+            return False
+        self.snapshot["state"] = "stop_pending"
+        self.snapshot["stop_reason"] = reason
+        return True
+
+    def status(self) -> dict[str, Any]:
+        return dict(self.snapshot)
+
+    def shutdown(
+        self, *, wait: bool = True, timeout: float | None = None
+    ) -> bool:
+        self.shutdowns += 1
+        return True
+
+
+def control(
+    command: str,
+    sequence: int,
+    request_id: str,
+    *,
+    capture_id: str = "capture-001",
+    session_id: str = "teleop-session-001",
+    reason: str = "test",
+) -> str:
+    return json.dumps(
+        {
+            "version": 1,
+            "command": command,
+            "event_seq": sequence,
+            "request_id": request_id,
+            "capture_id": capture_id,
+            "teleop_session_id": session_id,
+            "reason": reason,
+            "sent_unix_s": 1000.0 + sequence,
+        }
+    )
+
+
+class ParseControlMessageTest(unittest.TestCase):
+    def test_accepts_forward_compatible_object(self) -> None:
+        raw = json.loads(control("start", 0, "request-0"))
+        raw["future_field"] = {"ignored": True}
+        parsed = parse_control_message(json.dumps(raw))
+        self.assertEqual(parsed["command"], "start")
+        self.assertEqual(parsed["capture_id"], "capture-001")
+
+    def test_rejects_unsafe_or_ambiguous_values(self) -> None:
+        bad_objects = [
+            [],
+            {"version": 2},
+            {
+                "version": 1,
+                "command": "erase",
+                "event_seq": 1,
+                "request_id": "r",
+                "capture_id": "c",
+                "teleop_session_id": "s",
+            },
+            {
+                "version": 1,
+                "command": "start",
+                "event_seq": True,
+                "request_id": "r",
+                "capture_id": "../escape",
+                "teleop_session_id": "s",
+            },
+        ]
+        for value in bad_objects:
+            with self.subTest(value=value):
+                with self.assertRaises(ProtocolError):
+                    parse_control_message(json.dumps(value))
+
+    def test_required_topic_preflight_demands_a_publisher_endpoint(self) -> None:
+        endpoints = {
+            "/robot_state": [object()],
+            # A discovered topic with no publisher is intentionally missing.
+            "/encoder_identical_joint": [],
+            "/hric/robot/rl_state": [object(), object()],
+        }
+        calls: list[str] = []
+
+        def lookup(topic: str) -> list[object]:
+            calls.append(topic)
+            return endpoints[topic]
+
+        required = tuple(endpoints)
+        self.assertEqual(
+            topics_without_publishers(required, lookup),
+            ("/encoder_identical_joint",),
+        )
+        self.assertEqual(calls, list(required))
+
+
+class RecorderControlProtocolTest(unittest.TestCase):
+    def setUp(self) -> None:
+        self.recorder = FakeRecorder()
+        self.protocol = RecorderControlProtocol(
+            self.recorder,
+            heartbeat_timeout_seconds=3.0,
+            monotonic=lambda: 0.0,
+            unix_time=lambda: 1234.5,
+        )
+
+    def test_start_is_idempotent_and_capture_id_is_preserved(self) -> None:
+        message = control("start", 1, "start-request")
+        first = self.protocol.handle_json(message, now=10.0)
+        second = self.protocol.handle_json(message, now=10.2)
+        self.assertEqual(len(self.recorder.starts), 1)
+        self.assertTrue(first["ack_accepted"])
+        self.assertEqual(first["ack_code"], "start_accepted")
+        self.assertEqual(second["ack_code"], "start_accepted")
+        self.assertEqual(second["statistics"]["duplicates"], 1)
+        self.assertEqual(second["capture_id"], "capture-001")
+
+    def test_request_id_reuse_and_stale_sequences_are_rejected(self) -> None:
+        self.protocol.handle_json(control("start", 5, "same"), now=1.0)
+        reused = self.protocol.handle_json(
+            control("heartbeat", 6, "same"), now=1.1
+        )
+        self.assertFalse(reused["ack_accepted"])
+        self.assertEqual(reused["ack_code"], "request_id_reused")
+
+        stale = self.protocol.handle_json(
+            control("heartbeat", 4, "different"), now=1.2
+        )
+        self.assertFalse(stale["ack_accepted"])
+        self.assertEqual(stale["ack_code"], "stale_event_seq")
+
+    def test_matching_heartbeat_and_stop_are_immediate(self) -> None:
+        self.protocol.handle_json(control("start", 1, "start"), now=5.0)
+        self.recorder.snapshot["state"] = "recording"
+        heartbeat = self.protocol.handle_json(
+            control("heartbeat", 2, "heartbeat"), now=6.0
+        )
+        self.assertTrue(heartbeat["ack_accepted"])
+        self.assertEqual(heartbeat["ack_code"], "heartbeat_accepted")
+        stopped = self.protocol.handle_json(
+            control("stop", 3, "stop", reason="teleop_disarmed"), now=6.1
+        )
+        self.assertTrue(stopped["ack_accepted"])
+        self.assertEqual(self.recorder.stops, ["teleop_disarmed"])
+
+    def test_wrong_capture_cannot_heartbeat_or_stop_active_episode(self) -> None:
+        self.protocol.handle_json(control("start", 1, "start"), now=5.0)
+        self.recorder.snapshot["state"] = "recording"
+        bad_heartbeat = self.protocol.handle_json(
+            control(
+                "heartbeat",
+                2,
+                "bad-heartbeat",
+                capture_id="another-capture",
+            ),
+            now=5.5,
+        )
+        bad_stop = self.protocol.handle_json(
+            control("stop", 3, "bad-stop", capture_id="another-capture"),
+            now=5.6,
+        )
+        self.assertFalse(bad_heartbeat["ack_accepted"])
+        self.assertFalse(bad_stop["ack_accepted"])
+        self.assertEqual(self.recorder.stops, [])
+
+    def test_missing_bridge_heartbeat_stops_recording_once(self) -> None:
+        self.protocol.handle_json(control("start", 1, "start"), now=10.0)
+        self.recorder.snapshot["state"] = "recording"
+        before = self.protocol.poll(now=13.0)
+        self.assertEqual(before["statistics"]["watchdog_stops"], 0)
+        after = self.protocol.poll(now=13.001)
+        again = self.protocol.poll(now=30.0)
+        self.assertEqual(self.recorder.stops, ["bridge_heartbeat_timeout"])
+        self.assertEqual(after["statistics"]["watchdog_stops"], 1)
+        self.assertEqual(again["statistics"]["watchdog_stops"], 1)
+
+    def test_manager_completion_maps_to_ready_or_failed_status(self) -> None:
+        self.protocol.handle_json(control("start", 1, "start"), now=1.0)
+        self.recorder.snapshot.update(
+            {
+                "state": "idle",
+                "episode_id": None,
+                "session_id": None,
+                "last_result": "ready",
+            }
+        )
+        ready = self.protocol.poll(now=2.0)
+        self.assertEqual(ready["state"], "ready")
+        self.assertEqual(ready["capture_id"], "capture-001")
+        self.recorder.snapshot.update(
+            {"last_result": "failed", "last_error": "bag invalid"}
+        )
+        failed = self.protocol.poll(now=2.1)
+        self.assertEqual(failed["state"], "failed")
+        self.assertEqual(failed["last_error"], "bag invalid")
+
+    def test_preflight_failure_is_status_only(self) -> None:
+        protocol = RecorderControlProtocol(
+            self.recorder,
+            start_preflight=lambda: (False, "missing /robot_state"),
+        )
+        status = protocol.handle_json(control("start", 1, "start"), now=1.0)
+        self.assertFalse(status["ack_accepted"])
+        self.assertEqual(status["ack_code"], "preflight_failed")
+        self.assertEqual(status["last_error"], "missing /robot_state")
+        self.assertEqual(self.recorder.starts, [])
+
+    def test_invalid_json_never_reaches_recorder(self) -> None:
+        status = self.protocol.handle_json("not-json", now=1.0)
+        self.assertFalse(status["ack_accepted"])
+        self.assertEqual(status["ack_code"], "invalid_json")
+        self.assertEqual(self.recorder.starts, [])
+        self.assertEqual(self.recorder.stops, [])
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/test_delete_ready_episode.py

@@ -0,0 +1,376 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import hashlib
+import json
+import tempfile
+import unittest
+from pathlib import Path
+from unittest import mock
+
+from delete_ready_episode import (
+    DeleteCollisionError,
+    DeleteSafetyError,
+    delete_ready_episode,
+)
+
+
+class DeleteReadyEpisodeTests(unittest.TestCase):
+    def _roots(self, root: Path) -> tuple[Path, Path, Path]:
+        base = root / "tg3_data_collection"
+        ready = base / "ready"
+        deleting = base / "deleting"
+        active = base / "active"
+        failed = base / "failed"
+        for path in (ready, active, failed):
+            path.mkdir(parents=True, exist_ok=True)
+        (active / "ACTIVE_SENTINEL").write_text("active", encoding="utf-8")
+        (failed / "FAILED_SENTINEL").write_text("failed", encoding="utf-8")
+        return ready, deleting, base
+
+    def _episode(
+        self,
+        ready: Path,
+        name: str = "episode_20260810T120000000_deadbeef",
+        *,
+        state: str = "complete",
+    ) -> tuple[Path, str]:
+        episode = ready / name
+        (episode / "bag").mkdir(parents=True)
+        (episode / "bag" / "bag_0.mcap").write_bytes(b"mcap")
+        manifest = {"state": state, "episode_id": name, "files": []}
+        raw = (json.dumps(manifest, sort_keys=True) + "\n").encode("utf-8")
+        (episode / "manifest.json").write_bytes(raw)
+        (episode / "READY").write_text("ready\n", encoding="ascii")
+        return episode, hashlib.sha256(raw).hexdigest()
+
+    def _authorization(
+        self,
+        base: Path,
+        episode: str,
+        digest: str,
+        *,
+        payload_digest: str | None = None,
+    ) -> Path:
+        ledger = base / "delete_ledger"
+        ledger.mkdir(exist_ok=True)
+        tombstone = f"{episode}.{digest}.deleting"
+        path = ledger / f"{episode}.{digest}.authorized.json"
+        path.write_text(
+            json.dumps(
+                {
+                    "schema_version": 1,
+                    "state": "AUTHORIZED_DELETE",
+                    "episode_id": episode,
+                    "manifest_sha256": payload_digest or digest,
+                    "tombstone": tombstone,
+                }
+            ),
+            encoding="utf-8",
+        )
+        return path
+
+    def test_compare_rename_delete_and_absent_retry_are_idempotent(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            name = episode.name
+            result = delete_ready_episode(
+                name,
+                digest,
+                ready_root=ready,
+                deleting_root=deleting,
+            )
+            self.assertEqual(result["state"], "deleted")
+            self.assertFalse(episode.exists())
+            self.assertEqual(
+                delete_ready_episode(
+                    name,
+                    digest,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )["state"],
+                "already_absent",
+            )
+            self.assertEqual(
+                (base / "active" / "ACTIVE_SENTINEL").read_text(), "active"
+            )
+            self.assertEqual(
+                (base / "failed" / "FAILED_SENTINEL").read_text(), "failed"
+            )
+
+    def test_resume_matching_tombstone_after_lost_ack(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            deleting.mkdir()
+            tombstone = deleting / f"{episode.name}.{digest}.deleting"
+            episode.rename(tombstone)
+            result = delete_ready_episode(
+                "episode_20260810T120000000_deadbeef",
+                digest,
+                ready_root=ready,
+                deleting_root=deleting,
+            )
+            self.assertEqual(result["state"], "resumed_delete")
+            self.assertFalse(tombstone.exists())
+
+    def test_partial_tree_without_manifest_resumes_from_durable_ledger(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            tombstone = deleting / f"{episode.name}.{digest}.deleting"
+            ledger = (
+                base
+                / "delete_ledger"
+                / f"{episode.name}.{digest}.authorized.json"
+            )
+
+            def interrupt_after_control_files_removed(
+                _parent_fd: int, _name: str
+            ) -> None:
+                (tombstone / "READY").unlink()
+                (tombstone / "manifest.json").unlink()
+                raise RuntimeError("injected power loss during recursive delete")
+
+            with mock.patch(
+                "delete_ready_episode._remove_tree_at",
+                side_effect=interrupt_after_control_files_removed,
+            ):
+                with self.assertRaises(RuntimeError):
+                    delete_ready_episode(
+                        episode.name,
+                        digest,
+                        ready_root=ready,
+                        deleting_root=deleting,
+                    )
+            self.assertTrue(tombstone.is_dir())
+            self.assertFalse((tombstone / "manifest.json").exists())
+            self.assertTrue(ledger.is_file())
+
+            result = delete_ready_episode(
+                episode.name,
+                digest,
+                ready_root=ready,
+                deleting_root=deleting,
+            )
+            self.assertEqual(result["state"], "resumed_delete")
+            self.assertFalse(tombstone.exists())
+            self.assertFalse(ledger.exists())
+
+    def test_partial_tombstone_without_ledger_is_refused(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            deleting.mkdir()
+            tombstone = deleting / f"{episode.name}.{digest}.deleting"
+            episode.rename(tombstone)
+            (tombstone / "manifest.json").unlink()
+            with self.assertRaises(DeleteSafetyError):
+                delete_ready_episode(
+                    episode.name,
+                    digest,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertTrue(tombstone.exists())
+
+    def test_wrong_digest_or_symlink_ledger_is_refused(self) -> None:
+        for mode in ("wrong_digest", "symlink"):
+            with self.subTest(mode=mode), tempfile.TemporaryDirectory() as temporary:
+                root = Path(temporary)
+                ready, deleting, base = self._roots(root)
+                episode, digest = self._episode(ready)
+                deleting.mkdir()
+                tombstone = deleting / f"{episode.name}.{digest}.deleting"
+                episode.rename(tombstone)
+                (tombstone / "manifest.json").unlink()
+                if mode == "wrong_digest":
+                    ledger = self._authorization(
+                        base, episode.name, digest, payload_digest="f" * 64
+                    )
+                else:
+                    ledger_root = base / "delete_ledger"
+                    ledger_root.mkdir()
+                    ledger = (
+                        ledger_root
+                        / f"{episode.name}.{digest}.authorized.json"
+                    )
+                    outside = root / "outside"
+                    outside.write_text("keep", encoding="utf-8")
+                    ledger.symlink_to(outside)
+                with self.assertRaises(DeleteSafetyError):
+                    delete_ready_episode(
+                        episode.name,
+                        digest,
+                        ready_root=ready,
+                        deleting_root=deleting,
+                    )
+                self.assertTrue(tombstone.exists())
+                self.assertTrue(ledger.exists())
+
+    def test_ledger_only_after_tree_removal_is_finalized_idempotently(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, base = self._roots(Path(temporary))
+            deleting.mkdir()
+            episode = "episode_20260810T120000000_deadbeef"
+            digest = "a" * 64
+            ledger = self._authorization(base, episode, digest)
+            result = delete_ready_episode(
+                episode,
+                digest,
+                ready_root=ready,
+                deleting_root=deleting,
+            )
+            self.assertEqual(result["state"], "already_absent")
+            self.assertFalse(ledger.exists())
+
+    def test_conflicting_ledger_digest_refuses_ready_target(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            collision = self._authorization(base, episode.name, "b" * 64)
+            with self.assertRaises(DeleteCollisionError):
+                delete_ready_episode(
+                    episode.name,
+                    digest,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertTrue(episode.exists())
+            self.assertTrue(collision.exists())
+
+    def test_wrong_manifest_digest_refuses_and_preserves_target(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            episode, _digest = self._episode(ready)
+            with self.assertRaises(DeleteSafetyError):
+                delete_ready_episode(
+                    episode.name,
+                    "0" * 64,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertTrue(episode.exists())
+
+    def test_noncomplete_manifest_refuses_and_preserves_target(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready, state="failed")
+            with self.assertRaises(DeleteSafetyError):
+                delete_ready_episode(
+                    episode.name,
+                    digest,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertTrue(episode.exists())
+
+    def test_unsafe_episode_and_digest_are_rejected(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            for episode, digest in (("../active", "0" * 64), ("safe", "BAD")):
+                with self.subTest(episode=episode, digest=digest):
+                    with self.assertRaises(DeleteSafetyError):
+                        delete_ready_episode(
+                            episode,
+                            digest,
+                            ready_root=ready,
+                            deleting_root=deleting,
+                        )
+
+    def test_symlink_episode_is_refused_without_touching_outside(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            ready, deleting, _base = self._roots(root)
+            outside = root / "outside"
+            outside.mkdir()
+            sentinel = outside / "sentinel"
+            sentinel.write_text("keep", encoding="utf-8")
+            name = "episode_20260810T120000000_deadbeef"
+            (ready / name).symlink_to(outside, target_is_directory=True)
+            with self.assertRaises(DeleteSafetyError):
+                delete_ready_episode(
+                    name,
+                    "0" * 64,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertEqual(sentinel.read_text(), "keep")
+
+    def test_symlink_manifest_ready_or_nested_entry_is_refused(self) -> None:
+        for target in ("manifest", "ready", "nested"):
+            with self.subTest(target=target), tempfile.TemporaryDirectory() as temporary:
+                root = Path(temporary)
+                ready, deleting, _base = self._roots(root)
+                episode, digest = self._episode(ready)
+                outside = root / "outside"
+                outside.write_text("keep", encoding="utf-8")
+                if target == "manifest":
+                    (episode / "manifest.json").unlink()
+                    (episode / "manifest.json").symlink_to(outside)
+                elif target == "ready":
+                    (episode / "READY").unlink()
+                    (episode / "READY").symlink_to(outside)
+                else:
+                    (episode / "nested_link").symlink_to(outside)
+                with self.assertRaises(DeleteSafetyError):
+                    delete_ready_episode(
+                        episode.name,
+                        digest,
+                        ready_root=ready,
+                        deleting_root=deleting,
+                    )
+                self.assertEqual(outside.read_text(), "keep")
+
+    def test_conflicting_tombstone_refuses_without_moving_ready(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            deleting.mkdir()
+            collision = deleting / f"{episode.name}.{'1' * 64}.deleting"
+            collision.mkdir()
+            with self.assertRaises(DeleteCollisionError):
+                delete_ready_episode(
+                    episode.name,
+                    digest,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertTrue(episode.exists())
+            self.assertTrue(collision.exists())
+
+    def test_ready_and_same_tombstone_collision_refuses_both(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            ready, deleting, _base = self._roots(Path(temporary))
+            episode, digest = self._episode(ready)
+            deleting.mkdir()
+            tombstone = deleting / f"{episode.name}.{digest}.deleting"
+            tombstone.mkdir()
+            with self.assertRaises(DeleteCollisionError):
+                delete_ready_episode(
+                    episode.name,
+                    digest,
+                    ready_root=ready,
+                    deleting_root=deleting,
+                )
+            self.assertTrue(episode.exists())
+            self.assertTrue(tombstone.exists())
+
+    def test_wrong_parent_relationship_is_refused(self) -> None:
+        with tempfile.TemporaryDirectory() as temporary:
+            root = Path(temporary)
+            ready, _deleting, _base = self._roots(root)
+            elsewhere = root / "elsewhere" / "deleting"
+            elsewhere.mkdir(parents=True)
+            with self.assertRaises(DeleteSafetyError):
+                delete_ready_episode(
+                    "episode_safe",
+                    "0" * 64,
+                    ready_root=ready,
+                    deleting_root=elsewhere,
+                )
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/test_gesture_toggle.py

@@ -4,9 +4,12 @@ from __future__ import annotations
 import unittest
 
 from gesture_toggle import (
+    GuardedMomentaryGesture,
     GestureToggle,
     normalized_pose_to_positions,
+    right_a_pressed,
     right_b_pressed,
+    select_hand_target,
     select_right_hand_target,
 )
 
@@ -227,6 +230,13 @@ class GestureToggleTest(unittest.TestCase):
 
 
 class GestureHelpersTest(unittest.TestCase):
+    def test_right_a_is_index_zero_and_malformed_values_fail_closed(self) -> None:
+        self.assertTrue(right_a_pressed({"button": {"right": [1, 0, 0]}}))
+        self.assertFalse(right_a_pressed({"button": {"right": [0, 1, 1]}}))
+        self.assertIsNone(right_a_pressed({"button": {"right": []}}))
+        self.assertIsNone(right_a_pressed({"button": {"right": [-1]}}))
+        self.assertIsNone(right_a_pressed({"button": {"right": [0.5]}}))
+
     def test_right_b_is_index_one_and_malformed_values_fail_closed(self) -> None:
         self.assertTrue(right_b_pressed({"button": {"right": [0, 1, 0]}}))
         self.assertFalse(right_b_pressed({"button": {"right": [1, 0, 1]}}))
@@ -263,6 +273,81 @@ class GestureHelpersTest(unittest.TestCase):
             source,
         )
 
+    def test_same_point_target_can_be_applied_to_both_hands(self) -> None:
+        point = [201, 688, 1, 980, 980, 980]
+        sources = {
+            "left": [401, 401, 51, 51, 51, 51],
+            "right": [402, 402, 52, 52, 52, 52],
+        }
+        previous = {
+            "left": [410, 420, 60, 70, 80, 90],
+            "right": [430, 440, 100, 110, 120, 130],
+        }
+        selected = {
+            side: select_hand_target(
+                sources[side],
+                previous[side],
+                point,
+                active=True,
+                freeze=False,
+            )
+            for side in ("left", "right")
+        }
+        self.assertEqual(selected, {"left": point, "right": point})
+
+
+class GuardedMomentaryGestureTest(unittest.TestCase):
+    def setUp(self) -> None:
+        self.gesture = GuardedMomentaryGesture(release_seconds=0.5)
+        self.gesture.new_session()
+
+    def test_held_at_session_start_cannot_activate(self) -> None:
+        self.assertFalse(
+            self.gesture.update(
+                0.0, armed=True, input_healthy=True, pressed=True
+            )
+        )
+        self.assertFalse(
+            self.gesture.update(
+                10.0, armed=True, input_healthy=True, pressed=True
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertTrue(self.gesture.freeze_right_hand)
+
+    def test_press_is_immediate_after_stable_release_and_release_restores(self) -> None:
+        self.gesture.update(0.0, armed=True, input_healthy=True, pressed=False)
+        self.gesture.update(0.5, armed=True, input_healthy=True, pressed=False)
+        self.assertFalse(self.gesture.require_release)
+        self.assertTrue(
+            self.gesture.update(
+                0.51, armed=True, input_healthy=True, pressed=True
+            )
+        )
+        self.assertTrue(self.gesture.active)
+        self.assertEqual(self.gesture.activation_count, 1)
+        self.assertTrue(
+            self.gesture.update(
+                0.52, armed=True, input_healthy=True, pressed=False
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertFalse(self.gesture.freeze_right_hand)
+
+    def test_malformed_input_deactivates_and_requires_release(self) -> None:
+        self.gesture.update(0.0, armed=True, input_healthy=True, pressed=False)
+        self.gesture.update(0.5, armed=True, input_healthy=True, pressed=False)
+        self.gesture.update(0.6, armed=True, input_healthy=True, pressed=True)
+        self.assertTrue(self.gesture.active)
+        self.assertTrue(
+            self.gesture.update(
+                0.7, armed=True, input_healthy=True, pressed=None
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertTrue(self.gesture.require_release)
+        self.assertTrue(self.gesture.freeze_right_hand)
+
 
 if __name__ == "__main__":
     unittest.main()

tg3_local_teleop/test_head_control.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import unittest
+
+from head_control import (
+    HeadPitchController,
+    head_pitch_command_axis,
+    left_z_pressed,
+    right_stick_vertical,
+)
+
+
+def controller() -> HeadPitchController:
+    return HeadPitchController(
+        deadzone=0.1,
+        max_speed_rad_s=0.2,
+        max_accel_rad_s2=0.5,
+        min_pitch_rad=-0.1745329252,
+        max_pitch_rad=0.872664626,
+        axis_sign=-1.0,
+        max_dt_s=0.1,
+    )
+
+
+class RightStickParserTest(unittest.TestCase):
+    def test_accepts_only_finite_two_axis_right_stick(self) -> None:
+        self.assertEqual(
+            right_stick_vertical({"joystick": {"right": [0.5, -0.2]}}),
+            0.5,
+        )
+        self.assertIsNone(right_stick_vertical({"joystick": {"right": [0.5]}}))
+        self.assertIsNone(
+            right_stick_vertical({"joystick": {"right": [float("nan"), 0.0]}})
+        )
+        self.assertIsNone(right_stick_vertical({"joystick": {"right": [2.0, 0.0]}}))
+
+    def test_left_z_strictly_gates_head_pitch_axis(self) -> None:
+        released = {
+            "button": {"left": [0, 0, 0]},
+            "joystick": {"right": [0.8, 0.1]},
+        }
+        pressed = {
+            "button": {"left": [0, 0, 1]},
+            "joystick": {"right": [0.8, 0.1]},
+        }
+        self.assertFalse(left_z_pressed(released))
+        self.assertEqual(head_pitch_command_axis(released), 0.0)
+        self.assertTrue(left_z_pressed(pressed))
+        self.assertEqual(head_pitch_command_axis(pressed), 0.8)
+        self.assertIsNone(
+            head_pitch_command_axis(
+                {
+                    "button": {"left": [0, 0, "pressed"]},
+                    "joystick": {"right": [0.8, 0.1]},
+                }
+            )
+        )
+
+
+class HeadPitchControllerTest(unittest.TestCase):
+    def test_new_session_requires_neutral_before_direct_motion(self) -> None:
+        head = controller()
+        head.new_session()
+        self.assertIsNone(
+            head.update(
+                0.0,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=1.0,
+                feedback_position_rad=0.1,
+            )
+        )
+        self.assertTrue(head.require_neutral)
+        head.update(
+            0.1,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=0.1,
+        )
+        self.assertFalse(head.require_neutral)
+        target = head.update(
+            0.2,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=1.0,
+            feedback_position_rad=0.1,
+        )
+        self.assertIsNotNone(target)
+        self.assertLess(target, 0.1)  # stick up -> negative pitch -> view up
+
+    def test_center_stops_and_resynchronizes_to_real_feedback(self) -> None:
+        head = controller()
+        head.new_session()
+        head.update(
+            0.0,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=0.2,
+        )
+        head.update(
+            0.1,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=-1.0,
+            feedback_position_rad=0.2,
+        )
+        self.assertTrue(head.active)
+        self.assertIsNone(
+            head.update(
+                0.2,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=0.0,
+                feedback_position_rad=0.25,
+            )
+        )
+        self.assertFalse(head.active)
+        self.assertEqual(head.target_rad, 0.25)
+        self.assertEqual(head.velocity_rad_s, 0.0)
+
+    def test_feedback_gap_or_external_command_fails_closed(self) -> None:
+        for feedback_healthy, external_busy in ((False, False), (True, True)):
+            head = controller()
+            head.new_session()
+            head.update(
+                0.0,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=0.0,
+                feedback_position_rad=0.0,
+            )
+            self.assertIsNone(
+                head.update(
+                    0.1,
+                    armed=True,
+                    input_healthy=True,
+                    feedback_healthy=feedback_healthy,
+                    external_busy=external_busy,
+                    axis=1.0,
+                    feedback_position_rad=0.0,
+                )
+            )
+            self.assertTrue(head.require_neutral)
+            self.assertFalse(head.active)
+
+    def test_target_is_clamped_to_software_working_range(self) -> None:
+        head = controller()
+        head.new_session()
+        head.update(
+            0.0,
+            armed=True,
+            input_healthy=True,
+            feedback_healthy=True,
+            external_busy=False,
+            axis=0.0,
+            feedback_position_rad=-0.17,
+        )
+        target = None
+        for index in range(1, 200):
+            target = head.update(
+                index * 0.1,
+                armed=True,
+                input_healthy=True,
+                feedback_healthy=True,
+                external_busy=False,
+                axis=1.0,
+                feedback_position_rad=-0.17,
+            )
+        self.assertEqual(target, head.min_pitch_rad)
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/test_idle_session_refresh.py

@@ -31,6 +31,9 @@ for package, message_name in (
     ("diagnostic_msgs.msg", "DiagnosticStatus"),
     ("geometry_msgs.msg", "TwistStamped"),
     ("ros2_bridge_msgs.msg", "ArmStatus"),
+    ("ros2_bridge_msgs.msg", "HeadCtrl"),
+    ("ros2_bridge_msgs.msg", "MotorCtrl"),
+    ("ros2_bridge_msgs.msg", "RobotState"),
     ("sensor_msgs.msg", "JointState"),
     ("std_srvs.srv", "Trigger"),
 ):

tg3_local_teleop/test_session_gate.py

@@ -28,8 +28,9 @@ for package, names in {
     "brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"),
     "diagnostic_msgs.msg": ("DiagnosticStatus",),
     "geometry_msgs.msg": ("TwistStamped",),
-    "ros2_bridge_msgs.msg": ("ArmStatus",),
+    "ros2_bridge_msgs.msg": ("ArmStatus", "HeadCtrl", "MotorCtrl", "RobotState"),
     "sensor_msgs.msg": ("JointState",),
+    "std_msgs.msg": ("String",),
     "std_srvs.srv": ("Trigger",),
 }.items():
     install_module(package, **{name: Dummy for name in names})
@@ -42,6 +43,7 @@ sys.modules[spec.name] = bridge_module
 spec.loader.exec_module(bridge_module)
 ArmSnapshot = bridge_module.ArmSnapshot
 LocalTeleopBridge = bridge_module.LocalTeleopBridge
+RecordingToggleGate = bridge_module.RecordingToggleGate
 
 
 class NullLogger:
@@ -80,6 +82,8 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge.cfg = {"control": {"auto_home_on_stop": True}}
         bridge.hands_enabled = False
         bridge.right_point_gesture_enabled = False
+        bridge.right_a_pose_enabled = False
+        bridge.head_enabled = False
         bridge.robot_arm_positions = [0.0] * 14
         bridge.last_command = None
         bridge.last_publish_at = 0.0
@@ -227,10 +231,9 @@ class RobotSessionGateTest(unittest.TestCase):
             bridge._source_restart_reason(12.0, active),
         )
 
-    def test_locomotion_reacts_immediately_after_repress(self) -> None:
+    def test_locomotion_reacts_directly_to_sticks_without_buttons(self) -> None:
         bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
         bridge.locomotion_cfg = {
-            "hold_seconds": 0.0,
             "joystick_deadzone": 0.2,
             "joystick_expo": 2.0,
             "yaw_joystick_expo": 1.0,
@@ -241,10 +244,10 @@ class RobotSessionGateTest(unittest.TestCase):
             "yaw_axis_sign": -1.0,
             "zero_burst_frames": 10,
         }
-        bridge.walk_combo_started_at = None
         bridge.walk_active = False
         bridge.walk_command = [0.0, 0.0]
         bridge.walk_zero_frames_remaining = 0
+        bridge.walk_require_neutral = False
         bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
         published: list[tuple[float, float]] = []
 
@@ -260,7 +263,7 @@ class RobotSessionGateTest(unittest.TestCase):
             {
                 "button": {
                     "left": [False, False, False],
-                    "right": [False, False, True],
+                    "right": [False, False, False],
                 },
                 "joystick": {
                     "left": [1.0, 0.0],
@@ -276,7 +279,7 @@ class RobotSessionGateTest(unittest.TestCase):
                     "right": [False, False, False],
                 },
                 "joystick": {
-                    "left": [1.0, 0.0],
+                    "left": [0.0, 0.0],
                     "right": [0.0, 0.0],
                 },
             },
@@ -292,7 +295,7 @@ class RobotSessionGateTest(unittest.TestCase):
         turning = ArmSnapshot(
             {
                 "button": {
-                    "left": [False, False, True],
+                    "left": [False, False, False],
                     "right": [False, False, False],
                 },
                 "joystick": {
@@ -305,6 +308,22 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge._tick_locomotion(1.3, turning)
         self.assertEqual(published[-1], (0.0, -0.8))
 
+        head_mode_with_sideways_error = ArmSnapshot(
+            {
+                "button": {
+                    "left": [False, False, True],
+                    "right": [False, False, False],
+                },
+                "joystick": {
+                    "left": [0.0, 0.0],
+                    "right": [0.8, 1.0],
+                },
+            },
+            received_at=1.35,
+        )
+        bridge._tick_locomotion(1.35, head_mode_with_sideways_error)
+        self.assertEqual(published[-1], (0.0, 0.0))
+
         old_wrong_binding = ArmSnapshot(
             {
                 "button": {
@@ -321,6 +340,207 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge._tick_locomotion(1.4, old_wrong_binding)
         self.assertEqual(published[-1], (0.0, 0.0))
 
+    def test_locomotion_requires_one_neutral_sample_after_arming(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.locomotion_cfg = {
+            "joystick_deadzone": 0.2,
+            "joystick_expo": 2.0,
+            "yaw_joystick_expo": 1.0,
+            "max_forward_m_s": 1.0,
+            "max_reverse_m_s": 0.8,
+            "max_angular_rad_s": 0.8,
+            "forward_axis_sign": 1.0,
+            "yaw_axis_sign": -1.0,
+            "zero_burst_frames": 10,
+        }
+        bridge.walk_active = False
+        bridge.walk_command = [0.0, 0.0]
+        bridge.walk_zero_frames_remaining = 0
+        bridge.walk_require_neutral = True
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+        published: list[tuple[float, float]] = []
+
+        def publish(
+            self: object, linear_x: float, angular_z: float, _now: float
+        ) -> None:
+            published.append((linear_x, angular_z))
+            self.walk_command = [linear_x, angular_z]
+
+        bridge._publish_walk = MethodType(publish, bridge)
+        deflected = ArmSnapshot(
+            {
+                "button": {"left": [0, 0, 0]},
+                "joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]},
+            },
+            received_at=2.0,
+        )
+        neutral = ArmSnapshot(
+            {
+                "button": {"left": [0, 0, 0]},
+                "joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]},
+            },
+            received_at=2.1,
+        )
+        bridge._tick_locomotion(2.0, deflected)
+        self.assertTrue(bridge.walk_require_neutral)
+        self.assertEqual(bridge.walk_command, [0.0, 0.0])
+        bridge._tick_locomotion(2.1, neutral)
+        self.assertFalse(bridge.walk_require_neutral)
+        bridge._tick_locomotion(2.2, deflected)
+        self.assertEqual(published[-1], (1.0, -0.0))
+
+    def test_l3_hold_reaches_nonblocking_capture_request(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.data_collection_enabled = True
+        bridge.data_collection_cfg = {
+            "button_input_timeout_s": 0.25,
+            "control_retry_seconds": 0.5,
+            "heartbeat_interval_seconds": 0.5,
+        }
+        bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
+        bridge.data_collection_gate.new_session()
+        bridge.armed = True
+        bridge.data_pending_control = None
+        bridge.data_last_control_publish_at = 0.0
+        bridge.data_recorder_status = {}
+        bridge.data_recorder_status_at = 0.0
+        bridge.data_last_heartbeat_at = 0.0
+        bridge.data_last_iarm_received_at = 0.0
+        bridge.data_iarm_publisher = None
+        bridge.data_toggle_count = 0
+        requested: list[str] = []
+        bridge._request_data_capture = MethodType(
+            lambda _self, command, _reason: requested.append(command), bridge
+        )
+
+        def frame(now: float, pressed: int) -> object:
+            return ArmSnapshot(
+                {"button_joystick": {"left": pressed}}, received_at=now
+            )
+
+        bridge._tick_data_collection(10.0, frame(10.0, 0))
+        bridge._tick_data_collection(10.5, frame(10.5, 0))
+        bridge._tick_data_collection(10.6, frame(10.6, 1))
+        bridge._tick_data_collection(11.61, frame(11.61, 1))
+        self.assertEqual(requested, ["start"])
+        self.assertEqual(bridge.data_toggle_count, 1)
+
+    def test_rejected_recorder_start_clears_requested_active_state(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
+        bridge.data_collection_gate.new_session()
+        bridge.data_collection_gate.active = True
+        bridge.data_pending_control = {
+            "command": "start",
+            "request_id": "request-1",
+            "event_seq": 4,
+        }
+        bridge.data_recorder_status = {}
+        bridge.data_recorder_status_at = 0.0
+        bridge.data_capture_id = "capture-1"
+        bridge.data_last_transition = "start_requested"
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+        message = Dummy()
+        message.data = (
+            '{"version":1,"state":"failed","capture_id":"capture-1",'
+            '"ack_request_id":"request-1","ack_event_seq":4,'
+            '"ack_accepted":false,"ack_code":"preflight_failed",'
+            '"last_error":"required topic missing"}'
+        )
+
+        bridge._on_data_recorder_status(message)
+        self.assertIsNone(bridge.data_pending_control)
+        self.assertFalse(bridge.data_collection_gate.active)
+        self.assertIn("start_rejected", bridge.data_last_transition)
+
+    def test_supervisor_context_loss_clears_false_recording_state(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
+        bridge.data_collection_gate.new_session()
+        bridge.data_collection_gate.active = True
+        bridge.data_pending_control = None
+        bridge.data_recorder_status = {}
+        bridge.data_recorder_status_at = 0.0
+        bridge.data_capture_id = "capture-old"
+        bridge.data_last_transition = "recording"
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+        message = Dummy()
+        message.data = (
+            '{"version":1,"state":"idle","capture_id":null,'
+            '"ack_request_id":null,"ack_event_seq":null,'
+            '"ack_accepted":null,"last_error":""}'
+        )
+
+        bridge._on_data_recorder_status(message)
+        self.assertFalse(bridge.data_collection_gate.active)
+        self.assertEqual(
+            bridge.data_last_transition,
+            "recorder context was lost or replaced",
+        )
+
+    def test_missing_supervisor_ack_cancels_false_start(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.data_collection_enabled = True
+        bridge.data_collection_cfg = {
+            "control_retry_seconds": 0.5,
+            "ack_timeout_seconds": 5.0,
+            "heartbeat_interval_seconds": 0.5,
+            "status_stale_seconds": 4.0,
+        }
+        bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
+        bridge.data_collection_gate.new_session()
+        bridge.data_collection_gate.active = True
+        bridge.armed = False
+        bridge.data_pending_control = {"command": "start"}
+        bridge.data_pending_control_since = 10.0
+        bridge.data_last_control_publish_at = 10.0
+        bridge.data_control_publisher = None
+        bridge.data_recorder_status = {}
+        bridge.data_recorder_status_at = 0.0
+        bridge.data_last_heartbeat_at = 0.0
+        bridge.data_last_iarm_received_at = 0.0
+        bridge.data_iarm_publisher = None
+        bridge.data_last_transition = "start_requested"
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+
+        bridge._tick_data_collection(15.0, None)
+        self.assertIsNone(bridge.data_pending_control)
+        self.assertFalse(bridge.data_collection_gate.active)
+        self.assertEqual(bridge.data_last_transition, "start_ack_timeout")
+
+    def test_stale_recorder_status_requests_capture_stop(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.data_collection_enabled = True
+        bridge.data_collection_cfg = {
+            "control_retry_seconds": 0.5,
+            "ack_timeout_seconds": 5.0,
+            "heartbeat_interval_seconds": 0.5,
+            "status_stale_seconds": 4.0,
+        }
+        bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
+        bridge.data_collection_gate.new_session()
+        bridge.data_collection_gate.active = True
+        bridge.armed = False
+        bridge.data_pending_control = None
+        bridge.data_pending_control_since = 0.0
+        bridge.data_last_control_publish_at = 0.0
+        bridge.data_recorder_status = {"state": "recording"}
+        bridge.data_recorder_status_at = 10.0
+        bridge.data_last_heartbeat_at = 10.0
+        bridge.data_last_iarm_received_at = 0.0
+        bridge.data_iarm_publisher = None
+        bridge.data_last_transition = "recording"
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+        requested: list[str] = []
+        bridge._request_data_capture = MethodType(
+            lambda _self, command, _reason: requested.append(command), bridge
+        )
+
+        bridge._tick_data_collection(14.0, None)
+        self.assertFalse(bridge.data_collection_gate.active)
+        self.assertEqual(requested, ["stop"])
+        self.assertEqual(bridge.data_last_transition, "recorder_status_stale")
+
 
 if __name__ == "__main__":
     unittest.main()

tg3_local_teleop/tg3-data-recorder.service

@@ -0,0 +1,23 @@
+[Unit]
+Description=TG3 independent teleoperation MCAP recorder supervisor
+After=network-online.target
+Wants=network-online.target
+Before=tg3-local-teleop.service
+
+[Service]
+Type=simple
+WorkingDirectory=/home/nvidia/tg3_local_teleop
+UMask=0027
+ExecStartPre=/home/nvidia/tg3_local_teleop/wait_ros_ready.sh
+ExecStart=/home/nvidia/tg3_local_teleop/run_data_recorder.sh
+Restart=on-failure
+RestartSec=2
+KillSignal=SIGINT
+# Signal only the Python supervisor first; it owns clean SIGINT/finalization
+# of rosbag2.  The whole cgroup is still killed if the generous deadline ends.
+KillMode=mixed
+TimeoutStartSec=150
+TimeoutStopSec=150
+
+[Install]
+WantedBy=default.target

tg3_local_teleop/tg3-local-teleop.service

@@ -1,7 +1,7 @@
 [Unit]
 Description=TG3 local TS1P dual-arm bridge (no cloud pairing)
-After=network-online.target
-Wants=network-online.target
+After=network-online.target tg3-data-recorder.service
+Wants=network-online.target tg3-data-recorder.service
 
 [Service]
 Type=simple

tg3_local_teleop/tg3_local_teleop.py

@@ -16,6 +16,7 @@ import struct
 import threading
 import time
 import tomllib
+import uuid
 from dataclasses import dataclass
 from pathlib import Path
 from typing import Any
@@ -26,16 +27,21 @@ from brainco_hand_msgs.msg import MotorStatus, SetMotorMulti
 from diagnostic_msgs.msg import DiagnosticStatus
 from geometry_msgs.msg import TwistStamped
 from rclpy.node import Node
-from ros2_bridge_msgs.msg import ArmStatus
+from ros2_bridge_msgs.msg import ArmStatus, HeadCtrl, MotorCtrl, RobotState
 from sensor_msgs.msg import JointState
+from std_msgs.msg import String
 from std_srvs.srv import Trigger
 
+from data_collection import RecordingToggleGate, left_joystick_pressed
 from gesture_toggle import (
+    GuardedMomentaryGesture,
     GestureToggle,
     normalized_pose_to_positions,
+    right_a_pressed,
     right_b_pressed,
-    select_right_hand_target,
+    select_hand_target,
 )
+from head_control import HeadPitchController, head_pitch_command_axis, left_z_pressed
 
 
 JOINT_NAMES = [
@@ -422,10 +428,77 @@ class LocalTeleopBridge(Node):
             int(self.hands_cfg.get("position_min", 1)),
             int(self.hands_cfg.get("position_max", 1000)),
         )
+        self.right_a_pose_enabled = self.hands_enabled and bool(
+            self.hands_cfg.get("right_a_pose_enabled", False)
+        )
+        self.right_a_pose = GuardedMomentaryGesture(
+            release_seconds=float(
+                self.hands_cfg.get("right_a_pose_release_seconds", 0.5)
+            )
+        )
+        right_a_target = self.hands_cfg.get(
+            "right_a_pose_positions", [428, 735, 500, 77, 77, 72]
+        )
+        self.right_a_pose_target = [int(value) for value in right_a_target]
+        hand_position_min = int(self.hands_cfg.get("position_min", 1))
+        hand_position_max = int(self.hands_cfg.get("position_max", 1000))
+        if len(self.right_a_pose_target) != 6 or not all(
+            hand_position_min <= value <= hand_position_max
+            for value in self.right_a_pose_target
+        ):
+            raise ValueError(
+                "hands.right_a_pose_positions must contain 6 values within "
+                "the configured BrainCo position range"
+            )
         self.locomotion_cfg = config.get("locomotion", {})
         self.locomotion_enabled = bool(
             self.locomotion_cfg.get("enabled", False)
         )
+        self.head_cfg = config.get("head", {})
+        self.head_enabled = bool(self.head_cfg.get("enabled", False))
+        self.head_pitch_controller = HeadPitchController(
+            deadzone=float(self.head_cfg.get("joystick_deadzone", 0.1)),
+            max_speed_rad_s=float(
+                self.head_cfg.get("max_pitch_speed_rad_s", 0.2)
+            ),
+            max_accel_rad_s2=float(
+                self.head_cfg.get("max_pitch_accel_rad_s2", 0.5)
+            ),
+            min_pitch_rad=float(
+                self.head_cfg.get("min_pitch_rad", -0.1745329252)
+            ),
+            max_pitch_rad=float(
+                self.head_cfg.get("max_pitch_rad", 0.872664626)
+            ),
+            axis_sign=float(self.head_cfg.get("pitch_axis_sign", -1.0)),
+            max_dt_s=float(self.head_cfg.get("max_integration_dt_s", 0.1)),
+        )
+        self.data_collection_cfg = config.get("data_collection", {})
+        self.data_collection_enabled = bool(
+            self.data_collection_cfg.get("enabled", False)
+        )
+        self.data_collection_gate = RecordingToggleGate(
+            hold_seconds=float(
+                self.data_collection_cfg.get("button_hold_seconds", 1.0)
+            ),
+            release_seconds=float(
+                self.data_collection_cfg.get("button_release_seconds", 0.5)
+            ),
+        )
+        self.data_control_publisher = None
+        self.data_iarm_publisher = None
+        self.data_recorder_status: dict[str, Any] = {}
+        self.data_recorder_status_at = 0.0
+        self.data_pending_control: dict[str, Any] | None = None
+        self.data_pending_control_since = 0.0
+        self.data_last_control_publish_at = 0.0
+        self.data_last_heartbeat_at = 0.0
+        self.data_last_iarm_received_at = 0.0
+        self.data_capture_id: str | None = None
+        self.data_capture_session_id: str | None = None
+        self.data_event_seq = 0
+        self.data_toggle_count = 0
+        self.data_last_transition = "initialized; waiting for an armed session"
         self.hand_publishers: dict[str, Any] = {}
         self.robot_hand_positions: dict[str, list[int] | None] = {
             side: None for side in HAND_SIDES
@@ -445,12 +518,23 @@ class LocalTeleopBridge(Node):
         self.hand_output_ready = False
         self.runtime_hand_output_reasons: list[str] = []
         self.foreign_hand_source_seen = False
-        self.walk_combo_started_at: float | None = None
         self.walk_active = False
         self.walk_command = [0.0, 0.0]
         self.walk_publish_count = 0
         self.walk_zero_frames_remaining = 0
+        self.walk_require_neutral = True
         self.last_walk_publish_at = 0.0
+        self.head_publisher = None
+        self.robot_head_positions: dict[int, float] = {}
+        self.robot_head_errors: dict[int, int] = {}
+        self.robot_head_at = 0.0
+        self.expected_head_messages: list[tuple[tuple[Any, ...], float]] = []
+        self.foreign_head_command_at = 0.0
+        self.foreign_head_command_count = 0
+        self.head_publish_count = 0
+        self.last_head_publish_at = 0.0
+        self.head_output_reasons: list[str] = []
+        self.head_z_pressed: bool | None = None
 
         self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10)
         self.walk_publisher = None
@@ -458,6 +542,22 @@ class LocalTeleopBridge(Node):
             self.walk_publisher = self.create_publisher(
                 TwistStamped, self.locomotion_cfg["command_topic"], 10
             )
+        if self.head_enabled:
+            self.head_publisher = self.create_publisher(
+                HeadCtrl, self.head_cfg["command_topic"], 10
+            )
+            self.create_subscription(
+                RobotState,
+                self.head_cfg["state_topic"],
+                self._on_robot_state,
+                10,
+            )
+            self.create_subscription(
+                HeadCtrl,
+                self.head_cfg["command_topic"],
+                self._on_head_command,
+                10,
+            )
         self.create_subscription(
             DiagnosticStatus, ros_cfg["rl_state_topic"], self._on_rl_state, 10
         )
@@ -492,6 +592,23 @@ class LocalTeleopBridge(Node):
         self.create_service(
             Trigger, ros_cfg["cancel_home_service"], self._on_cancel_home_request
         )
+        if getattr(self, "data_collection_enabled", False):
+            self.data_control_publisher = self.create_publisher(
+                String,
+                str(self.data_collection_cfg["control_topic"]),
+                10,
+            )
+            self.data_iarm_publisher = self.create_publisher(
+                String,
+                str(self.data_collection_cfg["iarm_frame_topic"]),
+                10,
+            )
+            self.create_subscription(
+                String,
+                str(self.data_collection_cfg["status_topic"]),
+                self._on_data_recorder_status,
+                10,
+            )
 
         self.source = LatestArmData(net_cfg)
         self.source.start()
@@ -526,7 +643,9 @@ class LocalTeleopBridge(Node):
         self.get_logger().info(
             f"local bridge started in {mode}; source={self.source.description}; "
             f"target={ros_cfg['command_topic']}; brainco_hands={self.hands_enabled}; "
-            f"locomotion={self.locomotion_enabled}"
+            f"locomotion={self.locomotion_enabled}; "
+            f"head_pitch={self.head_enabled}; "
+            f"data_collection={self.data_collection_enabled}"
         )
 
     def close(self) -> None:
@@ -549,6 +668,20 @@ class LocalTeleopBridge(Node):
         self.robot_arm_errors = [int(by_id[motor_id].error) for motor_id in MOTOR_IDS]
         self.robot_arm_at = time.monotonic()
 
+    def _on_robot_state(self, msg: RobotState) -> None:
+        by_id = {int(motor.name): motor for motor in msg.head.status}
+        pitch_id = int(self.head_cfg.get("pitch_motor_id", 2))
+        if pitch_id not in by_id:
+            return
+        positions = {motor_id: float(motor.pos) for motor_id, motor in by_id.items()}
+        if not all(math.isfinite(value) for value in positions.values()):
+            return
+        self.robot_head_positions = positions
+        self.robot_head_errors = {
+            motor_id: int(motor.error) for motor_id, motor in by_id.items()
+        }
+        self.robot_head_at = time.monotonic()
+
     def _on_hand_status(self, side: str, msg: MotorStatus) -> None:
         positions = [int(value) for value in msg.positions]
         states = [int(value) for value in msg.states]
@@ -558,6 +691,242 @@ class LocalTeleopBridge(Node):
         self.robot_hand_states[side] = states
         self.robot_hand_at[side] = time.monotonic()
 
+    def _on_data_recorder_status(self, msg: String) -> None:
+        """Consume recorder acknowledgements without affecting robot control."""
+
+        try:
+            status = json.loads(msg.data)
+            if not isinstance(status, dict) or status.get("version") != 1:
+                raise ValueError("unsupported recorder status")
+        except (TypeError, ValueError, json.JSONDecodeError) as exc:
+            self.get_logger().warning(f"ignored invalid data-recorder status: {exc}")
+            return
+
+        self.data_recorder_status = status
+        self.data_recorder_status_at = time.monotonic()
+        pending = self.data_pending_control
+        if (
+            pending is not None
+            and status.get("ack_request_id") == pending.get("request_id")
+            and status.get("ack_event_seq") == pending.get("event_seq")
+        ):
+            accepted = status.get("ack_accepted")
+            if type(accepted) is bool:
+                self.data_pending_control = None
+                self.data_pending_control_since = 0.0
+                if not accepted:
+                    detail = str(
+                        status.get("last_error")
+                        or status.get("ack_code")
+                        or "recorder rejected request"
+                    )
+                    if pending.get("command") == "start":
+                        self.data_collection_gate.force_inactive(
+                            f"recorder_rejected: {detail}"
+                        )
+                    self.data_last_transition = (
+                        f"{pending.get('command')}_rejected: {detail}"
+                    )
+                    self.get_logger().error(
+                        "data recorder rejected %s: %s"
+                        % (pending.get("command"), detail)
+                    )
+
+        state = status.get("state")
+        if (
+            state in ("ready", "failed")
+            and status.get("capture_id") == self.data_capture_id
+            and self.data_collection_gate.active
+        ):
+            self.data_collection_gate.force_inactive(
+                f"recorder_{state}: {status.get('last_error', '')}".rstrip()
+            )
+            self.data_last_transition = self.data_collection_gate.last_transition
+        elif (
+            self.data_pending_control is None
+            and self.data_collection_gate.active
+            and state in ("idle", "ready", "failed", "recording", "stopping")
+            and status.get("capture_id") != self.data_capture_id
+        ):
+            # A restarted supervisor has no in-memory context for the old bag.
+            # Reconcile the UI gate instead of displaying a false recording
+            # state forever; the supervisor owns cleanup of its old cgroup and
+            # active/failed directory.
+            self.data_collection_gate.force_inactive(
+                "recorder context was lost or replaced"
+            )
+            self.data_last_transition = self.data_collection_gate.last_transition
+            self.get_logger().error(
+                "data recorder no longer owns the requested capture; "
+                "release L3 before starting a new episode"
+            )
+
+    def _publish_data_control(self, payload: dict[str, Any]) -> None:
+        if self.data_control_publisher is None:
+            return
+        try:
+            message = String()
+            message.data = json.dumps(
+                payload, ensure_ascii=False, separators=(",", ":")
+            )
+            self.data_control_publisher.publish(message)
+            self.data_last_control_publish_at = time.monotonic()
+        except Exception as exc:  # Data collection must never stop robot control.
+            self.get_logger().error(f"cannot publish data-recorder control: {exc}")
+
+    def _request_data_capture(self, command: str, reason: str) -> None:
+        if not self.data_collection_enabled:
+            return
+        if command == "start":
+            self.data_capture_id = (
+                time.strftime("%Y%m%dT%H%M%SZ", time.gmtime())
+                + "_TG3_"
+                + uuid.uuid4().hex[:8]
+            )
+        if self.data_capture_id is None:
+            return
+        if self.data_capture_session_id is None:
+            self.data_capture_session_id = self.active_session_id or (
+                "direct_" + uuid.uuid4().hex
+            )
+
+        self.data_event_seq += 1
+        payload: dict[str, Any] = {
+            "version": 1,
+            "command": command,
+            "event_seq": self.data_event_seq,
+            "request_id": uuid.uuid4().hex,
+            "capture_id": self.data_capture_id,
+            "teleop_session_id": self.data_capture_session_id,
+            "reason": reason,
+            "sent_unix_s": time.time(),
+        }
+        self.data_pending_control = payload
+        self.data_pending_control_since = time.monotonic()
+        self._publish_data_control(payload)
+        self.data_last_transition = f"{command}_requested: {reason}"
+
+    def _publish_data_heartbeat(self) -> None:
+        if self.data_capture_id is None or self.data_capture_session_id is None:
+            return
+        self.data_event_seq += 1
+        payload = {
+            "version": 1,
+            "command": "heartbeat",
+            "event_seq": self.data_event_seq,
+            "request_id": uuid.uuid4().hex,
+            "capture_id": self.data_capture_id,
+            "teleop_session_id": self.data_capture_session_id,
+            "sent_unix_s": time.time(),
+        }
+        self._publish_data_control(payload)
+        self.data_last_heartbeat_at = time.monotonic()
+
+    def _tick_data_collection(
+        self, now: float, sample: ArmSnapshot | None
+    ) -> None:
+        if not self.data_collection_enabled:
+            return
+
+        if self.armed:
+            input_timeout = float(
+                self.data_collection_cfg.get("button_input_timeout_s", 0.25)
+            )
+            input_healthy = (
+                sample is not None and now - sample.received_at <= input_timeout
+            )
+            pressed = (
+                None if sample is None else left_joystick_pressed(sample.data)
+            )
+            action = self.data_collection_gate.update(
+                now,
+                input_healthy=input_healthy,
+                pressed=pressed,
+            )
+            if action is not None:
+                self.data_toggle_count += 1
+                self._request_data_capture(
+                    action,
+                    "left joystick press held for "
+                    f"{self.data_collection_gate.hold_seconds:.1f}s",
+                )
+
+        retry_s = float(self.data_collection_cfg.get("control_retry_seconds", 0.5))
+        if (
+            self.data_pending_control is not None
+            and now - self.data_last_control_publish_at >= retry_s
+        ):
+            self._publish_data_control(self.data_pending_control)
+
+        ack_timeout_s = float(
+            self.data_collection_cfg.get("ack_timeout_seconds", 5.0)
+        )
+        if (
+            self.data_pending_control is not None
+            and self.data_pending_control_since > 0.0
+            and now - self.data_pending_control_since >= ack_timeout_s
+        ):
+            expired_command = str(self.data_pending_control.get("command"))
+            self.data_pending_control = None
+            self.data_pending_control_since = 0.0
+            if expired_command == "start":
+                self.data_collection_gate.force_inactive(
+                    "recorder START acknowledgement timed out"
+                )
+            self.data_last_transition = f"{expired_command}_ack_timeout"
+            self.get_logger().error(
+                f"data recorder {expired_command} acknowledgement timed out"
+            )
+
+        status_stale_s = float(
+            self.data_collection_cfg.get("status_stale_seconds", 4.0)
+        )
+        if (
+            self.data_collection_gate.active
+            and self.data_pending_control is None
+            and self.data_recorder_status_at > 0.0
+            and now - self.data_recorder_status_at >= status_stale_s
+        ):
+            self.data_collection_gate.force_inactive(
+                "recorder status heartbeat became stale"
+            )
+            self.data_last_transition = "recorder_status_stale"
+            self._request_data_capture(
+                "stop", "recorder status heartbeat became stale"
+            )
+            self.get_logger().error(
+                "data recorder status became stale; capture stop requested"
+            )
+
+        recorder_state = self.data_recorder_status.get("state")
+        heartbeat_s = float(
+            self.data_collection_cfg.get("heartbeat_interval_seconds", 0.5)
+        )
+        if (
+            self.data_collection_gate.active
+            and self.data_pending_control is None
+            and recorder_state in ("starting", "recording")
+            and now - self.data_last_heartbeat_at >= heartbeat_s
+        ):
+            self._publish_data_heartbeat()
+
+        if (
+            self.data_collection_gate.active
+            and sample is not None
+            and sample.received_at != self.data_last_iarm_received_at
+            and self.data_iarm_publisher is not None
+        ):
+            try:
+                message = String()
+                # Preserve the complete xTELE/OmniSocket application frame.
+                message.data = json.dumps(
+                    sample.data, ensure_ascii=False, separators=(",", ":")
+                )
+                self.data_iarm_publisher.publish(message)
+                self.data_last_iarm_received_at = sample.received_at
+            except Exception as exc:
+                self.get_logger().error(f"cannot publish xTELE capture frame: {exc}")
+
     @staticmethod
     def _hand_message_signature(msg: SetMotorMulti) -> tuple[Any, ...]:
         return (
@@ -592,6 +961,45 @@ class LocalTeleopBridge(Node):
                 "foreign BrainCo hand command source detected", success=False
             )
 
+    @staticmethod
+    def _head_message_signature(msg: HeadCtrl) -> tuple[Any, ...]:
+        return (
+            int(msg.mode),
+            int(msg.label),
+            int(msg.reserved),
+            tuple(
+                (
+                    int(motor.name),
+                    float(motor.kp),
+                    float(motor.kd),
+                    float(motor.pos),
+                    float(motor.spd),
+                    float(motor.tor),
+                    float(motor.cur),
+                    str(motor.joint_ids),
+                )
+                for motor in msg.ctrl
+            ),
+        )
+
+    def _on_head_command(self, msg: HeadCtrl) -> None:
+        now = time.monotonic()
+        window = float(self.head_cfg.get("self_command_window_s", 0.25))
+        self.expected_head_messages = [
+            expected
+            for expected in self.expected_head_messages
+            if now - expected[1] <= window
+        ]
+        signature = self._head_message_signature(msg)
+        if any(signature == expected[0] for expected in self.expected_head_messages):
+            return
+        self.foreign_head_command_at = now
+        self.foreign_head_command_count += 1
+        if self.head_pitch_controller.active:
+            self.get_logger().warning(
+                "external /head/cmd detected; local head-pitch output paused"
+            )
+
     def _on_command_topic(self, msg: JointState) -> None:
         if msg.header.frame_id != self.FRAME_ID and len(msg.position) in (14, 16):
             if not self.foreign_source_seen:
@@ -690,6 +1098,13 @@ class LocalTeleopBridge(Node):
                 success=False,
             )
 
+        # The recorder toggle is deliberately downstream of every disarm
+        # decision, so a matching STOP or safety teardown always wins over an
+        # L3 press observed in the same control tick.
+        self._tick_data_collection(now, sample)
+        if self.head_enabled:
+            self._tick_head(now, sample)
+
         if self.right_point_gesture_enabled:
             gesture_toggled = self.right_point_gesture.update(
                 now,
@@ -706,9 +1121,27 @@ class LocalTeleopBridge(Node):
             if gesture_toggled:
                 state = "ACTIVE" if self.right_point_gesture.active else "INACTIVE"
                 self.get_logger().warning(
-                    f"RIGHT-HAND POINT GESTURE {state}: right B held for "
+                    f"BILATERAL POINT GESTURE {state}: right B held for "
                     f"{self.right_point_gesture.hold_seconds:.1f}s"
                 )
+        if self.right_a_pose_enabled:
+            pose_changed = self.right_a_pose.update(
+                now,
+                armed=self.armed,
+                input_healthy=(
+                    self.armed
+                    and sample is not None
+                    and not runtime_hand_reasons
+                ),
+                pressed=(
+                    None if sample is None else right_a_pressed(sample.data)
+                ),
+            )
+            if pose_changed:
+                state = "ACTIVE" if self.right_a_pose.active else "INACTIVE"
+                self.get_logger().warning(
+                    f"RIGHT-HAND A POSE {state}: momentary right A"
+                )
 
         if self.returning_home:
             self._tick_home(now)
@@ -740,15 +1173,34 @@ class LocalTeleopBridge(Node):
                     self.hand_output_ready = True
                     desired_hands = self._hand_targets(sample.data)
                     if self.right_point_gesture_enabled:
+                        for side in HAND_SIDES:
+                            freeze_reference = self.last_hand_commands[side]
+                            if (
+                                freeze_reference is None
+                                or len(freeze_reference) != 6
+                            ):
+                                freeze_reference = self.robot_hand_positions[side]
+                            desired_hands[side] = select_hand_target(
+                                desired_hands[side],
+                                freeze_reference,
+                                self.right_point_gesture_target,
+                                active=self.right_point_gesture.active,
+                                freeze=self.right_point_gesture.freeze_right_hand,
+                            )
+                    if self.right_a_pose_enabled:
                         freeze_reference = self.last_hand_commands["right"]
                         if freeze_reference is None or len(freeze_reference) != 6:
                             freeze_reference = self.robot_hand_positions["right"]
-                        desired_hands["right"] = select_right_hand_target(
+                        # Right A has momentary priority over the persistent B
+                        # bilateral gesture on the right hand; releasing A
+                        # returns through the same slew limiter to B or live
+                        # xTELE input. The left B target remains unchanged.
+                        desired_hands["right"] = select_hand_target(
                             desired_hands["right"],
                             freeze_reference,
-                            self.right_point_gesture_target,
-                            active=self.right_point_gesture.active,
-                            freeze=self.right_point_gesture.freeze_right_hand,
+                            self.right_a_pose_target,
+                            active=self.right_a_pose.active,
+                            freeze=self.right_a_pose.freeze_right_hand,
                         )
                     commands = {
                         side: self._slew_hand(side, desired_hands[side], now)
@@ -937,7 +1389,16 @@ class LocalTeleopBridge(Node):
         self.active_session_id = session_id
         if self.right_point_gesture_enabled:
             self.right_point_gesture.new_session()
+        if self.right_a_pose_enabled:
+            self.right_a_pose.new_session()
+        if self.head_enabled:
+            self.head_pitch_controller.new_session()
+        if getattr(self, "data_collection_enabled", False):
+            self.data_capture_session_id = session_id
+            self.data_collection_gate.new_session()
+            self.data_last_transition = "new teleoperation session"
         self.armed = True
+        self.walk_require_neutral = True
         # Start both slew limiters at measured robot feedback, never at a
         # potentially distant first network target.
         self.last_command = list(self.robot_arm_positions or [])
@@ -1002,7 +1463,16 @@ class LocalTeleopBridge(Node):
             return
         if self.right_point_gesture_enabled:
             self.right_point_gesture.new_session()
+        if self.right_a_pose_enabled:
+            self.right_a_pose.new_session()
+        if self.head_enabled:
+            self.head_pitch_controller.new_session()
+        if getattr(self, "data_collection_enabled", False):
+            self.data_capture_session_id = "direct_" + uuid.uuid4().hex
+            self.data_collection_gate.new_session()
+            self.data_last_transition = "new direct-LAN teleoperation session"
         self.armed = True
+        self.walk_require_neutral = True
         # Start the slew limiter at measured robot feedback.  Using None here
         # would make the first armed frame jump directly to the TS1P target.
         self.last_command = list(self.robot_arm_positions or [])
@@ -1020,12 +1490,23 @@ class LocalTeleopBridge(Node):
 
     def _disarm(self, reason: str) -> None:
         was_armed = self.armed
+        if getattr(self, "data_collection_enabled", False):
+            action = self.data_collection_gate.end_session(reason)
+            self.data_last_transition = self.data_collection_gate.last_transition
+            if action == "stop":
+                # This is a non-blocking ROS request.  It never delays robot
+                # disarm, locomotion zeroing, or the existing Home sequence.
+                self._request_data_capture("stop", reason)
         self._stop_locomotion(reason)
         if self.right_point_gesture_enabled:
             # Clearing the logical override must not publish a hand target.
             # Existing STOP behavior leaves the physical hand at its last
             # limited command until a later, newly armed session.
             self.right_point_gesture.disarm(reason)
+        if self.right_a_pose_enabled:
+            self.right_a_pose.disarm(reason)
+        if self.head_enabled:
+            self.head_pitch_controller.disarm(reason)
         self.armed = False
         self.hand_output_ready = False
         self.runtime_hand_output_reasons = []
@@ -1056,12 +1537,8 @@ class LocalTeleopBridge(Node):
     def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None:
         cfg = self.locomotion_cfg
         try:
-            left_buttons = sample.data["button"]["left"]
-            right_buttons = sample.data["button"]["right"]
             left_joystick = sample.data["joystick"]["left"]
             right_joystick = sample.data["joystick"]["right"]
-            left_z = len(left_buttons) >= 3 and bool(left_buttons[2])
-            right_c = len(right_buttons) >= 3 and bool(right_buttons[2])
             if len(left_joystick) != 2 or len(right_joystick) != 2:
                 raise ValueError("left and right joysticks must each contain two axes")
             # xTELE 0.1.2 stores each TS1P stick as [vertical, horizontal].
@@ -1070,6 +1547,9 @@ class LocalTeleopBridge(Node):
             # convention requested for this installation.
             raw_forward = float(left_joystick[0])
             raw_yaw = float(right_joystick[1])
+            head_mode = left_z_pressed(sample.data)
+            if head_mode is None:
+                raise ValueError("left Z state is malformed")
             deadzone = float(cfg["joystick_deadzone"])
             forward_expo = float(cfg["joystick_expo"])
             yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo))
@@ -1079,6 +1559,11 @@ class LocalTeleopBridge(Node):
             shaped_yaw = self._shape_joystick_axis(
                 raw_yaw, deadzone, yaw_expo
             )
+            # Left Z reserves the entire right stick for head-pitch mode.
+            # Suppress yaw so a small sideways component while looking up or
+            # down cannot turn the robot.
+            if head_mode:
+                shaped_yaw = 0.0
             signed_forward = shaped_forward * float(
                 cfg.get("forward_axis_sign", 1.0)
             )
@@ -1094,19 +1579,19 @@ class LocalTeleopBridge(Node):
             self._stop_locomotion(f"invalid locomotion input: {exc}")
             return
 
-        forward_active = right_c and shaped_forward != 0.0
-        yaw_active = left_z and shaped_yaw != 0.0
-        if not forward_active and not yaw_active:
-            self._stop_locomotion(
-                "right C + left forward and left Z + right yaw are both inactive"
-            )
-            self._tick_walk_zero_burst(now)
-            return
+        forward_active = shaped_forward != 0.0
+        yaw_active = shaped_yaw != 0.0
+        if self.walk_require_neutral:
+            if forward_active or yaw_active:
+                self._stop_locomotion(
+                    "waiting for both locomotion sticks to return to neutral"
+                )
+                self._tick_walk_zero_burst(now)
+                return
+            self.walk_require_neutral = False
 
-        if self.walk_combo_started_at is None:
-            self.walk_combo_started_at = now
-        hold_seconds = float(cfg["hold_seconds"])
-        if now - self.walk_combo_started_at < hold_seconds:
+        if not forward_active and not yaw_active:
+            self._stop_locomotion("both locomotion stick axes are neutral")
             self._tick_walk_zero_burst(now)
             return
 
@@ -1114,8 +1599,8 @@ class LocalTeleopBridge(Node):
             self.walk_active = True
             self.walk_zero_frames_remaining = 0
             self.get_logger().warning(
-                "LOCAL HBWALK VELOCITY STARTED: right C + left-stick forward "
-                "or left Z + right-stick yaw"
+                "LOCAL HBWALK VELOCITY STARTED: direct left-stick forward "
+                "or right-stick yaw"
             )
 
         linear_limit = max_forward if signed_forward >= 0.0 else max_reverse
@@ -1133,7 +1618,6 @@ class LocalTeleopBridge(Node):
         was_active = self.walk_active
         had_nonzero_command = any(abs(value) > 1e-9 for value in self.walk_command)
         self.walk_active = False
-        self.walk_combo_started_at = None
         if was_active or had_nonzero_command:
             now = time.monotonic()
             self._publish_walk(0.0, 0.0, now)
@@ -1166,6 +1650,96 @@ class LocalTeleopBridge(Node):
         self.last_walk_publish_at = now
         self.walk_publish_count += 1
 
+    def _tick_head(self, now: float, sample: ArmSnapshot | None) -> None:
+        pitch_id = int(self.head_cfg.get("pitch_motor_id", 2))
+        pitch_position = self.robot_head_positions.get(pitch_id)
+        feedback_timeout = float(self.head_cfg.get("feedback_timeout_s", 0.25))
+        feedback_age = (
+            math.inf if self.robot_head_at == 0.0 else now - self.robot_head_at
+        )
+        feedback_healthy = (
+            pitch_position is not None
+            and math.isfinite(float(pitch_position))
+            and feedback_age <= feedback_timeout
+            and int(self.robot_head_errors.get(pitch_id, -1)) == 0
+        )
+        input_timeout = float(self.head_cfg.get("input_timeout_s", 0.3))
+        input_healthy = (
+            self.armed
+            and sample is not None
+            and now - sample.received_at <= input_timeout
+        )
+        self.head_z_pressed = (
+            None if sample is None else left_z_pressed(sample.data)
+        )
+        axis = None if sample is None else head_pitch_command_axis(sample.data)
+        quiet_s = float(self.head_cfg.get("foreign_command_quiet_s", 1.0))
+        foreign_age = (
+            math.inf
+            if self.foreign_head_command_at == 0.0
+            else now - self.foreign_head_command_at
+        )
+        external_busy = foreign_age <= quiet_s
+
+        reasons: list[str] = []
+        if not self.armed:
+            reasons.append("teleoperation is not armed")
+        if not input_healthy:
+            reasons.append("right-stick input is unavailable/stale")
+        if axis is None:
+            reasons.append("left Z or right-stick vertical input is malformed")
+        elif not self.head_z_pressed:
+            reasons.append("left Z is not held; head-pitch control is idle")
+        if not feedback_healthy:
+            reasons.append("head pitch feedback is unavailable/stale or in error")
+        if external_busy:
+            reasons.append("external /head/cmd publisher has an active lease")
+        self.head_output_reasons = reasons
+
+        target = self.head_pitch_controller.update(
+            now,
+            armed=self.armed,
+            input_healthy=input_healthy,
+            feedback_healthy=feedback_healthy,
+            external_busy=external_busy,
+            axis=axis,
+            feedback_position_rad=pitch_position,
+        )
+        if target is not None:
+            self._publish_head_pitch(target, now)
+
+    def _publish_head_pitch(self, target_rad: float, now: float) -> None:
+        if not self.allow_publish or self.head_publisher is None:
+            return
+        motor = MotorCtrl()
+        motor.name = int(self.head_cfg.get("pitch_motor_id", 2))
+        motor.kp = 0.0
+        motor.kd = 0.0
+        motor.pos = float(target_rad)
+        motor.spd = float(self.head_cfg.get("command_speed_rad_s", 0.2))
+        motor.tor = 0.0
+        motor.cur = float(self.head_cfg.get("max_current_a", 1.0))
+        motor.joint_ids = ""
+
+        msg = HeadCtrl()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = str(self.head_cfg.get("frame_id", "head"))
+        msg.mode = int(self.head_cfg.get("mode", 0))
+        msg.label = int(self.head_cfg.get("label", 151))
+        msg.reserved = 0
+        msg.ctrl = [motor]
+        signature = self._head_message_signature(msg)
+        window = float(self.head_cfg.get("self_command_window_s", 0.25))
+        self.expected_head_messages = [
+            expected
+            for expected in self.expected_head_messages
+            if now - expected[1] <= window
+        ][-64:]
+        self.expected_head_messages.append((signature, now))
+        self.head_publisher.publish(msg)
+        self.head_publish_count += 1
+        self.last_head_publish_at = now
+
     def _home_start_reasons(
         self,
         now: float,
@@ -1550,6 +2124,46 @@ class LocalTeleopBridge(Node):
             "custom_hand_feedback_policy": (
                 "startup_qualifies_teleop; runtime_gap_pauses_hands_only"
             ),
+            "data_collection_enabled": self.data_collection_enabled,
+            "data_collection_binding": (
+                "left joystick press (L3), hold "
+                f"{self.data_collection_gate.hold_seconds:.1f}s toggle; "
+                f"release {self.data_collection_gate.release_seconds:.1f}s"
+            ),
+            "data_collection_gate_state": self.data_collection_gate.state,
+            "data_collection_requested_active": self.data_collection_gate.active,
+            "data_collection_requires_release": (
+                self.data_collection_gate.require_release
+            ),
+            "data_collection_button_pressed": (
+                self.data_collection_gate.button_pressed
+            ),
+            "data_collection_hold_s": (
+                0.0
+                if self.data_collection_gate.hold_started_at is None
+                else round(now - self.data_collection_gate.hold_started_at, 2)
+            ),
+            "data_collection_capture_id": self.data_capture_id,
+            "data_collection_session_id": self.data_capture_session_id,
+            "data_collection_toggle_count": self.data_toggle_count,
+            "data_collection_last_transition": self.data_last_transition,
+            "data_collection_pending_command": (
+                None
+                if self.data_pending_control is None
+                else self.data_pending_control.get("command")
+            ),
+            "data_collection_pending_age_s": (
+                None
+                if self.data_pending_control is None
+                or self.data_pending_control_since == 0.0
+                else round(now - self.data_pending_control_since, 3)
+            ),
+            "data_recorder_status": self.data_recorder_status,
+            "data_recorder_status_age_s": (
+                None
+                if self.data_recorder_status_at == 0.0
+                else round(now - self.data_recorder_status_at, 3)
+            ),
             "runtime_hand_output_ready": self.hand_output_ready,
             "runtime_hand_output_reasons": self.runtime_hand_output_reasons,
             "safety_ready": not reasons,
@@ -1599,7 +2213,7 @@ class LocalTeleopBridge(Node):
             "right_point_gesture_enabled": self.right_point_gesture_enabled,
             "right_point_gesture_binding": (
                 "right_B hold "
-                f"{self.right_point_gesture.hold_seconds:.1f}s toggle"
+                f"{self.right_point_gesture.hold_seconds:.1f}s bilateral toggle"
             ),
             "right_point_gesture_active": (
                 self.right_point_gesture.active
@@ -1647,12 +2261,43 @@ class LocalTeleopBridge(Node):
                 if self.right_point_gesture_enabled
                 else None
             ),
+            "right_a_pose_enabled": self.right_a_pose_enabled,
+            "right_a_pose_binding": "right_A hold; release restores live input",
+            "right_a_pose_active": (
+                self.right_a_pose.active if self.right_a_pose_enabled else False
+            ),
+            "right_a_pose_state": (
+                self.right_a_pose.state if self.right_a_pose_enabled else "disabled"
+            ),
+            "right_a_pose_requires_release": (
+                self.right_a_pose.require_release
+                if self.right_a_pose_enabled
+                else False
+            ),
+            "right_a_pose_release_s": (
+                round(self.right_a_pose.release_elapsed(now), 2)
+                if self.right_a_pose_enabled
+                else 0.0
+            ),
+            "right_a_pose_activation_count": (
+                self.right_a_pose.activation_count
+                if self.right_a_pose_enabled
+                else 0
+            ),
+            "right_a_pose_last_transition": (
+                self.right_a_pose.last_transition
+                if self.right_a_pose_enabled
+                else "disabled"
+            ),
+            "right_a_pose_target_positions": (
+                self.right_a_pose_target if self.right_a_pose_enabled else None
+            ),
             "locomotion_enabled": self.locomotion_enabled,
             "locomotion_binding": (
-                "right_C + left_stick_vertical; "
-                "left_Z + right_stick_horizontal, immediate"
+                "left_stick_vertical; right_stick_horizontal, direct"
             ),
             "locomotion_active": self.walk_active,
+            "locomotion_requires_neutral": self.walk_require_neutral,
             "locomotion_hold_s": 0.0,
             "locomotion_command": {
                 "linear_x_m_s": self.walk_command[0],
@@ -1673,6 +2318,42 @@ class LocalTeleopBridge(Node):
             "last_locomotion_publish_age_s": None
             if self.last_walk_publish_at == 0.0
             else round(now - self.last_walk_publish_at, 4),
+            "head_pitch_enabled": self.head_enabled,
+            "head_pitch_binding": "left_Z + right_stick_vertical after neutral",
+            "head_pitch_left_z_pressed": self.head_z_pressed,
+            "head_pitch_active": self.head_pitch_controller.active,
+            "head_pitch_state": self.head_pitch_controller.last_transition,
+            "head_pitch_requires_neutral": (
+                self.head_pitch_controller.require_neutral
+            ),
+            "head_pitch_target_rad": self.head_pitch_controller.target_rad,
+            "head_pitch_velocity_rad_s": (
+                self.head_pitch_controller.velocity_rad_s
+            ),
+            "head_pitch_feedback_rad": self.robot_head_positions.get(
+                int(self.head_cfg.get("pitch_motor_id", 2))
+            ),
+            "head_pitch_feedback_error": self.robot_head_errors.get(
+                int(self.head_cfg.get("pitch_motor_id", 2))
+            ),
+            "head_pitch_feedback_age_s": None
+            if self.robot_head_at == 0.0
+            else round(now - self.robot_head_at, 4),
+            "head_pitch_output_reasons": self.head_output_reasons,
+            "head_pitch_limits_rad": {
+                "up": self.head_pitch_controller.min_pitch_rad,
+                "down": self.head_pitch_controller.max_pitch_rad,
+                "max_speed": self.head_pitch_controller.max_speed_rad_s,
+                "max_accel": self.head_pitch_controller.max_accel_rad_s2,
+            },
+            "head_pitch_foreign_command_age_s": None
+            if self.foreign_head_command_at == 0.0
+            else round(now - self.foreign_head_command_at, 4),
+            "head_pitch_foreign_command_count": self.foreign_head_command_count,
+            "head_pitch_publish_count": self.head_publish_count,
+            "last_head_pitch_publish_age_s": None
+            if self.last_head_publish_at == 0.0
+            else round(now - self.last_head_publish_at, 4),
             "unsupported_binding": (
                 "right_C + right_A (C+A) is not registered by xTELE 0.1.2"
             ),

tg3_omnisocket_transport/README.md

@@ -29,7 +29,7 @@ xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和
 
 右侧原始按键顺序是 `A/B/C`。按住右 `B` 时，sender 仍合并 5001 的左手
 目标，但保留 5003 的原始右手开合量，不用 xTELE 处理后的右手目标覆盖。
-这样右 B 的自定义长按手势还未满 1 秒时，原厂 B 处理不会让右手提前动作。
+这样右 B 的自定义双手长按手势还未满 1 秒时，原厂 B 处理不会让右手提前动作。
 松开 B 并连续稳定释放 `0.5 s` 后，还要等新鲜 5001 右手目标连续至少 `0.25 s`
 恢复到按键前基线或当前 5003 原始开合量，才恢复双侧合并；无法确认时持续使用原始
 右手值，防止释放沿后的锁存/延迟目标漏入。

tg3_omnisocket_transport/tg3-omnisocket-sender.service

@@ -7,7 +7,7 @@ Wants=network-online.target
 Type=simple
 WorkingDirectory=/home/eai/tg3_omnisocket_transport
 Environment=PYTHONPATH=/home/eai/OmniSocketGo/python
-ExecStart=/usr/bin/python3 /home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py --server 175.178.116.187:14049 --peer-id tg3-009027fa8190-iarm --target-peer tg3-009027fa8190-robot --zmq-endpoint tcp://127.0.0.1:5003 --cmd-zmq-endpoint tcp://127.0.0.1:5001 --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 --combo-release-s 0.5 --start-marker-frames 500 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json
+ExecStart=/usr/bin/python3 /home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py --server 127.0.0.1:14049 --peer-id tg3-009027fa8190-iarm --target-peer tg3-009027fa8190-robot --zmq-endpoint tcp://127.0.0.1:5003 --cmd-zmq-endpoint tcp://127.0.0.1:5001 --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 --combo-release-s 0.5 --start-marker-frames 500 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json
 Restart=always
 RestartSec=1
 KillSignal=SIGINT

verify.sh

@@ -7,10 +7,15 @@ expected_omnisocket_commit="de3f5c96779dbe1571c10feb22fc7f2331b6b222"
 for executable in \
   "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \
   "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \
+  "$repo_dir/tg3_local_teleop/data_recorder_node.py" \
+  "$repo_dir/tg3_local_teleop/delete_ready_episode.py" \
   "$repo_dir/tg3_local_teleop/run.sh" \
+  "$repo_dir/tg3_local_teleop/run_data_recorder.sh" \
   "$repo_dir/tg3_local_teleop/wait_ros_ready.sh" \
   "$repo_dir/tg3_local_teleop/home.sh" \
-  "$repo_dir/tg3_local_teleop/status.sh"; do
+  "$repo_dir/tg3_local_teleop/status.sh" \
+  "$repo_dir/tg3_data_collection/data_get_sync.py" \
+  "$repo_dir/tg3_data_collection/decode_joint_data.py"; do
   if [[ ! -x "$executable" ]]; then
     echo "Required executable bit is missing: $executable" >&2
     exit 1
@@ -20,17 +25,68 @@ done
 python3 -m py_compile \
   "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \
   "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \
-  "$repo_dir/tg3_local_teleop/gesture_toggle.py"
+  "$repo_dir/tg3_local_teleop/gesture_toggle.py" \
+  "$repo_dir/tg3_local_teleop/head_control.py" \
+  "$repo_dir/tg3_local_teleop/data_collection.py" \
+  "$repo_dir/tg3_local_teleop/data_recorder_protocol.py" \
+  "$repo_dir/tg3_local_teleop/data_recorder_node.py" \
+  "$repo_dir/tg3_local_teleop/delete_ready_episode.py" \
+  "$repo_dir/tg3_data_collection/data_get_sync.py" \
+  "$repo_dir/tg3_data_collection/decode_joint_data.py"
 python3 "$repo_dir/tg3_omnisocket_transport/test_session_gate.py"
 python3 "$repo_dir/tg3_local_teleop/test_session_gate.py"
 python3 "$repo_dir/tg3_local_teleop/test_gesture_toggle.py"
+python3 "$repo_dir/tg3_local_teleop/test_head_control.py"
 python3 "$repo_dir/tg3_local_teleop/test_idle_session_refresh.py"
+python3 "$repo_dir/tg3_local_teleop/test_data_collection.py"
+python3 "$repo_dir/tg3_local_teleop/test_data_recorder_protocol.py"
+python3 "$repo_dir/tg3_local_teleop/test_delete_ready_episode.py"
+python3 "$repo_dir/tg3_data_collection/test_data_get_sync.py"
 python3 - "$repo_dir/tg3_local_teleop/config.toml" <<'PY'
 import sys
 import tomllib
 
 with open(sys.argv[1], "rb") as stream:
-    tomllib.load(stream)
+    document = tomllib.load(stream)
+section = document["data_collection"]
+topics = set(section["topics"])
+required = set(section["required_topics"])
+head_rgbd = {
+    "/ob_camera_head/color/image_raw/compressed",
+    "/ob_camera_head/color/camera_info",
+    "/ob_camera_head/color/metadata",
+    "/ob_camera_head/depth/image_raw/compressedDepth",
+    "/ob_camera_head/depth/camera_info",
+    "/ob_camera_head/depth/metadata",
+}
+assert head_rgbd <= topics, "head RGB-D topics must stay in the recording whitelist"
+assert head_rgbd.isdisjoint(required), "head RGB-D must not block recording"
+assert not any(topic.startswith("/ob_camera_waist/") for topic in topics), (
+    "waist camera topics must stay out of the recording whitelist"
+)
+assert "/tf_static" in topics and "/tf_static" not in required
+assert section.get("minimum_topic_rates_hz", {}) == {}
+groups = section["optional_topic_groups"]
+assert set(groups) == {"head_rgbd"}
+assert set(groups["head_rgbd"]["topics"]) == head_rgbd
+assert groups["head_rgbd"]["minimum_topic_rates_hz"] == {
+    "/ob_camera_head/color/image_raw/compressed": 20.0,
+    "/ob_camera_head/depth/image_raw/compressedDepth": 20.0,
+}
+assert section["minimum_free_gib"] >= 100.0
+assert section["retain_failed_episodes"] is False
+head = document["head"]
+assert head["enabled"] is True
+assert head["command_topic"] == "/head/cmd"
+assert head["state_topic"] == "/robot_state"
+assert head["pitch_motor_id"] == 2
+assert head["mode"] == 0
+assert 0.0 < head["max_current_a"] <= 1.0
+assert -0.2617994 <= head["min_pitch_rad"] < head["max_pitch_rad"] <= 1.012291
+assert 0.0 < head["max_pitch_speed_rad_s"] <= 0.2
+assert 0.0 < head["max_pitch_accel_rad_s2"] <= 0.5
+assert 0.0 <= head["joystick_deadzone"] < 1.0
+assert "/head/cmd" in topics and "/head/cmd" not in required
 print("config.toml parse passed")
 PY