meiqi/TG3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: meiqi:9d25bc9bff127b06097e9d6ce2d2eb34fcc61f62
Branches Tags
meiqi:main
...
compare: meiqi:4cceebfa5fba1f38be725204488c9f40fa170773
Branches Tags
meiqi:main

7 Commits
9d25bc9bff ... 4cceebfa5f

Author SHA1 Message Date
meiqi
4cceebfa5f feat: update hand gestures and direct locomotion 2026-08-11 14:35:51 +08:00
meiqi
89a8c3418f docs: add cross-device migration runbook 2026-08-10 17:29:01 +08:00
meiqi
67c9b65768 feat: keep RGB-D captures only on workstation 2026-08-10 17:14:44 +08:00
meiqi
d02b99d2aa feat: capture head and waist RGB-D data 2026-08-10 16:40:00 +08:00
LengedZhao
4ba0f63758 fix: use complete vendor message overlay for rosbag 2026-08-10 16:01:28 +08:00
LengedZhao
5851c26d43 fix: treat missing recorder ready directory as empty 2026-08-10 15:56:36 +08:00
LengedZhao
6caad268b8 feat: add session-gated TG3 data collection 2026-08-10 15:54:15 +08:00
29 changed files with 8691 additions and 78 deletions
Expand all files Collapse all files

6
.gitignore vendored
View File

@@ -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

3
Data_Get/.gitignore vendored Normal file
View File

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

9
Data_Get/README.md Normal file
View File

@@ -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` 当作完整数据集。

40
README.md
View File

@@ -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,35 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务
- 左 Z + 右 C 连续 3 秒：开始遥操；再次连续 3 秒：结束并限速回 Home。
若 Hub 尚未运行，本次长按只检测一次并立即结束，不发送控制数据；服务器启动后须先
稳定松开，再重新长按 3 秒。pending 和活动会话都不会自动重试或自动恢复。
- 右 C + 左摇杆上下：HBWALK 前进/后退。
- 左 Z + 右摇杆左右：HBWALK 原地转向。
- 右 B 连续 1 秒：右手进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续
- 遥操开启且双摇杆先回中过一次后，左摇杆上下直接控制 HBWALK 前进/后退。
- 遥操开启且双摇杆先回中过一次后，右摇杆左右直接控制 HBWALK 原地转向。
- 右 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`。
## 迁移前必须修改

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

@@ -0,0 +1,743 @@
# 天工 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、方向和厂家速度上限；
- `base_directory` 是否仍是 `/home/nvidia/tg3_data_collection`；
- 相机 Topic 是否仍为 `/ob_camera_head/...` 和 `/ob_camera_waist/...`。
### 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，并记录质量状态 |
## 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. L3 长按 1 秒开始短数采，再次长按 1 秒结束；
9. 长按 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：
机器人临时副本回收：
```

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

@@ -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,11 +923,9 @@ 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，把右摇杆小幅横推，确认机器人原地转向；松开任一输入
应立即清零角速度；
11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：先确认双摇杆回中，
再把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；回中后确认立即零速停止；
12. 在同一行走前提下，直接把右摇杆小幅横推，确认机器人原地转向；回中应立即清零角速度；
13. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home；
14. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP
反馈超时约为 500 ms，触发后会作废 session；机器人若持续约 2 秒收不到活动帧，会另行
@@ -1012,3 +1010,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
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` 绑定，会遮蔽厂家完整 `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_waist.service orbbec_head.service
sudo systemctl enable --now orbbec_waist.service
# 同时需要头部 RGB-D 时：
sudo systemctl enable --now orbbec_head.service
ros2 topic info /ob_camera_waist/color/image_raw/compressed
ros2 topic info /ob_camera_waist/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。

122
tg3_data_collection/README.md Normal file
View File

@@ -0,0 +1,122 @@
# 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` 以保留故障
诊断，必须另建独立验真与回收协议，不能整目录清空。
迁移后需要同步修改 service 中的本机项目路径和机器人 SSH 地址。如果 Nvidia 的
`192.168.41.2` 改变，只改 unit 的 `--remote` 并重新部署 helper；它不在机器人
`config.toml` 或 OmniSocket Peer 设置中。启用自动删除时远端路径必须保持固定的
`/home/nvidia/tg3_data_collection/ready`，同步器会拒绝对其他 root 启用删除。

1070
tg3_data_collection/data_get_sync.py Executable file
View File

File diff suppressed because it is too large Load Diff

599
tg3_data_collection/test_data_get_sync.py Executable file
View File

@@ -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()

14
tg3_data_collection/tg3-data-get-sync.service Normal file
View File

@@ -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

113
tg3_local_teleop/README.md
View File

@@ -13,7 +13,7 @@ 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 行走
```
@@ -29,9 +29,9 @@ 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`、转向
@@ -43,15 +43,19 @@ TS1P 同构臂
- 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 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,8 +69,8 @@ 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 也走同一停止逻辑。
灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
@@ -74,7 +78,7 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态，但不会在
匹配的 STOP、安全解除武装或服务退出都会清除右 B 与右 A 手势的逻辑状态，但不会在
STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。
## 限速双臂回 Home
@@ -145,13 +149,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）。现场单台腰部相机实测合计约 `22–25 MB/s`（约 `1.3–1.5 GB/min`，随
场景变化）。头、腰两组相机都不是 required topics：服务在运行就由 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 分钟，
加入双 RGB-D 后启动/运行最低保留 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` 绑定使用。两者混在同一个
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'
```

138
tg3_local_teleop/config.toml
View File

@@ -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,132 @@ 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. Both RGB-D cameras are optional:
# rosbag discovers and records either one 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",
"/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",
"/ob_camera_waist/color/image_raw/compressed",
"/ob_camera_waist/color/camera_info",
"/ob_camera_waist/color/metadata",
"/ob_camera_waist/depth/image_raw/compressedDepth",
"/ob_camera_waist/depth/camera_info",
"/ob_camera_waist/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
[data_collection.optional_topic_groups.waist_rgbd]
topics = [
"/ob_camera_waist/color/image_raw/compressed",
"/ob_camera_waist/color/camera_info",
"/ob_camera_waist/color/metadata",
"/ob_camera_waist/depth/image_raw/compressedDepth",
"/ob_camera_waist/depth/camera_info",
"/ob_camera_waist/depth/metadata",
]
[data_collection.optional_topic_groups.waist_rgbd.minimum_topic_rates_hz]
"/ob_camera_waist/color/image_raw/compressed" = 20.0
"/ob_camera_waist/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
@@ -108,8 +223,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 +234,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,

1474
tg3_local_teleop/data_collection.py Executable file
View File

File diff suppressed because it is too large Load Diff

440
tg3_local_teleop/data_recorder_node.py Executable file
View File

@@ -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())

546
tg3_local_teleop/data_recorder_protocol.py Executable file
View File

@@ -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",
]

537
tg3_local_teleop/delete_ready_episode.py Executable file
View File

@@ -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())

164
tg3_local_teleop/gesture_toggle.py
View File

@@ -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,
)

31
tg3_local_teleop/run_data_recorder.sh Executable file
View File

@@ -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" \
"$@"

1054
tg3_local_teleop/test_data_collection.py Executable file
View File

File diff suppressed because it is too large Load Diff

274
tg3_local_teleop/test_data_recorder_protocol.py Executable file
View File

@@ -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()

376
tg3_local_teleop/test_delete_ready_episode.py Executable file
View File

@@ -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()

85
tg3_local_teleop/test_gesture_toggle.py
View File

@@ -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()

209
tg3_local_teleop/test_session_gate.py
View File

@@ -30,6 +30,7 @@ for package, names in {
"geometry_msgs.msg": ("TwistStamped",),
"ros2_bridge_msgs.msg": ("ArmStatus",),
"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,7 @@ 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.robot_arm_positions = [0.0] * 14
bridge.last_command = None
bridge.last_publish_at = 0.0
@@ -227,10 +230,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 +243,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 +262,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 +278,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 +294,7 @@ class RobotSessionGateTest(unittest.TestCase):
turning = ArmSnapshot(
{
"button": {
"left": [False, False, True],
"left": [False, False, False],
"right": [False, False, False],
},
"joystick": {
@@ -321,6 +323,201 @@ 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(
{"joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}},
received_at=2.0,
)
neutral = ArmSnapshot(
{"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()

23
tg3_local_teleop/tg3-data-recorder.service Normal file
View File

@@ -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

4
tg3_local_teleop/tg3-local-teleop.service
View File

@@ -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

498
tg3_local_teleop/tg3_local_teleop.py
View File

@@ -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
@@ -28,13 +29,17 @@ from geometry_msgs.msg import TwistStamped
from rclpy.node import Node
from ros2_bridge_msgs.msg import ArmStatus
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,
)
@@ -422,10 +427,58 @@ 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.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,11 +498,11 @@ 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.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10)
@@ -492,6 +545,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 +596,8 @@ 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"data_collection={self.data_collection_enabled}"
)
def close(self) -> None:
@@ -558,6 +629,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 (
@@ -690,6 +997,11 @@ 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.right_point_gesture_enabled:
gesture_toggled = self.right_point_gesture.update(
now,
@@ -706,9 +1018,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 +1070,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 +1286,14 @@ 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 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 +1358,14 @@ 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 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 +1383,21 @@ 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)
self.armed = False
self.hand_output_ready = False
self.runtime_hand_output_reasons = []
@@ -1056,12 +1428,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].
@@ -1094,19 +1462,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 +1482,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 +1501,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)
@@ -1550,6 +1917,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 +2006,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 +2054,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],

2
tg3_omnisocket_transport/README.md
View File

@@ -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 原始开合量，才恢复双侧合并；无法确认时持续使用原始
右手值，防止释放沿后的锁存/延迟目标漏入。

2
tg3_omnisocket_transport/tg3-omnisocket-sender.service
View File

@@ -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

58
verify.sh
View File

@@ -7,10 +7,14 @@ 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"; do
if [[ ! -x "$executable" ]]; then
echo "Required executable bit is missing: $executable" >&2
exit 1
@@ -20,17 +24,65 @@ 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/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"
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_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",
}
waist_rgbd = {
"/ob_camera_waist/color/image_raw/compressed",
"/ob_camera_waist/color/camera_info",
"/ob_camera_waist/color/metadata",
"/ob_camera_waist/depth/image_raw/compressedDepth",
"/ob_camera_waist/depth/camera_info",
"/ob_camera_waist/depth/metadata",
}
assert head_rgbd <= topics, "head RGB-D topics must stay in the recording whitelist"
assert waist_rgbd <= topics, "waist RGB-D topics must stay in the recording whitelist"
assert head_rgbd.isdisjoint(required), "head RGB-D must not block recording"
assert waist_rgbd.isdisjoint(required), "waist RGB-D must not block recording"
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", "waist_rgbd"}
assert set(groups["head_rgbd"]["topics"]) == head_rgbd
assert set(groups["waist_rgbd"]["topics"]) == waist_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 groups["waist_rgbd"]["minimum_topic_rates_hz"] == {
"/ob_camera_waist/color/image_raw/compressed": 20.0,
"/ob_camera_waist/depth/image_raw/compressedDepth": 20.0,
}
assert section["minimum_free_gib"] >= 100.0
assert section["retain_failed_episodes"] is False
print("config.toml parse passed")
PY