TG3/tg3_local_teleop/README.md

# 天工 3.0 本地同构臂双臂、BrainCo 灵巧手与 HBWALK 行走控制

这套桥接不修改工控机 xTELE、`robot_tele_server` 或机器人控制源码。数据链路为：

```text
TS1P 同构臂
  -> 工控机 xTELE tcp://127.0.0.1:5003
  -> OmniSocket KCP Hub 175.178.116.187:14049
  -> tg3_local_teleop（直接 OmniSocket Session；HBWALK / 错误 / 限位门控）
  -> /encoder_identical_joint（14 维 JointState）
  -> 厂家 freq_change_tg3_node（同步、滤波、200 Hz）
  -> /arm/cmd
  -> 天工 3.0 双臂
  -> /left_hand/set_motor_multi + /right_hand/set_motor_multi
  -> 天工 3.0 BrainCo Revo2 双灵巧手
  -> /hric/robot/cmd_vel（左摇杆前后 / 右摇杆转向；50 Hz TwistStamped）
  -> 天工 3.0 HBWALK 行走
  -> /head/cmd（左 Z + 右摇杆上下；Head Pitch 位置模式）
  -> 天工 3.0 头部相机上下视角
```

## 自动运行与操作

- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
- 用户服务启动前会用短生命周期 ROS 探针等待 `/hric/robot/rl_state` 可发现，再创建
  长生命周期桥接节点，避免开机早期网络接口尚未就绪时 Fast DDS 固化为空接口。
- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
  本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
  机器人立即停控并以 `0.25 rad/s` 自动回 Home。
- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
  前后/转向；不控制腰和头。
- 行走：遥操已启动且双摇杆先回中过一次后，左摇杆上下直接控制前后，右摇杆左右
  直接控制原地转向；无需组合键，在下一个 `50 Hz` 周期立即响应。摇杆回中立即把
  对应轴清零，再推动也立即恢复。
  前后仍使用二次细控曲线；转向在死区后使用线性曲线，使右摇杆中段有足够角速度，
  但最大值仍受官方 `0.8 rad/s` 上限约束。
  按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
  `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
- xTELE 0.1.2 的摇杆数组顺序是“纵轴、横轴”；左纵轴映射到 `linear.x`，右横轴
  映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。
- 头部视角：遥操已启动且右摇杆纵轴先回中过一次后，按住左 Z，右摇杆上推使
  Head Pitch 向负值移动（抬头），下推使其向正值移动（低头）；不需要 R3。按 Z
  期间右摇杆水平转向会被屏蔽，避免调整视角时误转。控制只使用 `mode=0` 绝对
  位置、`cur=1.0 A`，从 `/robot_state` 的真实 `name=2` 反馈起步，限制在 `-10°～+50°`、
  最大 `0.2 rad/s`、加速度 `0.5 rad/s²`。摇杆回中、输入/反馈失联或头电机报错时停止
  更新目标。检测到 `control_forward`、`patrol_agent`、`joystick_bridge` 等外部
  `/head/cmd` 实际消息后，本项目暂停头部输出，静默 1 秒并再次观察摇杆回中后才恢复。
- 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
  在不清楚厂商语义的情况下把它擅自绑定为状态切换。
- 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
  BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
  若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
- 双手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开
  至少 `0.5 s` 后，再连续长按右 `B` 1 秒关闭。首次启动和每个新遥操会话都必须先
  稳定松开 B，保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈
  中断均不触发；计时期间冻结双手各自最后一条命令，双臂仍照常跟随。
- 指向同时覆盖左右手。目标采用工控机 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` 限速器。

## 安全门控

只有以下条件全部满足才允许开始：机器人 `current_state` 和 `child_state` 均为
`HBWALK`、状态为 `running`；同构臂数据新鲜；左右 CAN/伺服无错误；采样频率正常；
启动瞬间的 14 个目标均在天工 3.0 文档限位内；没有检测到云端第二路同构臂命令。
自定义关节目标限位仅对每个新 `session_id` 的 START 检查一次，采集/跟随过程中不再检查；电机错误、
HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 原有的姿态同步、
关节限位和碰撞保护保持不变。为启用二次开放文档规定的 HBWALK topic 行走，本机
`xmigcs/config/dex_config.yaml` 的 HBWALK 速度许可已由全零改为当前 NAVIGATE 的上限；
实际发出的速度由本桥按官方 Topic 范围限制为 `linear.x=[-0.8, 1.0] m/s`、
`angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。

行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且左摇杆前后
或右摇杆横向越过死区时发布。摇杆回中后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
回 Home 或机器人退出 HBWALK 也走同一停止逻辑。

头部控制只在同一次遥操武装期间启用，不改变 Head Yaw，也不改变厂家头部机械限位、
电流、高温或碰撞保护。服务重启或新会话不会把软件累计角度当真实位置，必须重新读取
`/robot_state` 的 Head Pitch 反馈。

灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
匹配的 STOP、安全解除武装或服务退出都会清除右 B 与右 A 手势的逻辑状态，但不会在
STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。

## 限速双臂回 Home

回 Home 命令读取机器人真实 `/freq_change/arm_status` 作为轨迹起点，以 `0.25 rad/s`
把关节 11..17、21..27 平滑移动到现场保存的双臂 Home 姿态；Home 不是电机全零位。
只有未武装、HBWALK、机器人双臂反馈新鲜且无错误时才接受。Home 轨迹不依赖已经主动
停止的 EAI 数据，只依赖机器人反馈和机器人侧安全门；执行取消命令会立即停止发布。
若机器人实际位置落后指令超过 `0.08 rad`，也会自动停止。

```bash
./home.sh start
./home.sh status
./home.sh cancel
```

`home.sh start` 会引起机器人双臂运动，执行前必须确认防摔、净空和急停。当前 Home
角度保存在 `config.toml` 的 `home.joint_goal_rad` 中。

## 部署与检查

程序目录放在机器人算力主机 `/home/nvidia/tg3_local_teleop`。只读监测测试：

```bash
./run.sh --duration 10
./status.sh
```

安装并启用用户服务自动启动：

```bash
mkdir -p ~/.config/systemd/user
cp tg3-local-teleop.service ~/.config/systemd/user/
systemctl --user daemon-reload
systemctl --user enable tg3-local-teleop.service
```

现场确认防摔、双臂活动范围无人、急停可用后，才可启动：

```bash
systemctl --user start tg3-local-teleop.service
systemctl --user status tg3-local-teleop.service
```

停止与查看状态：

```bash
systemctl --user stop tg3-local-teleop.service
./status.sh
```

当前 `config.toml` 由桥接程序直接建立 OmniSocket Session，不再经过机器人本机 ZMQ
接收代理。不走公网时推荐在 EAI 运行本地 `kcpserver` Hub：EAI sender 指向
`127.0.0.1:14049`，机器人保持 `network.transport="omnisocket"` 并把
`omnisocket_server` 改为 EAI 局域网地址。这样保留 5003+5001 合并、session 和 Peer
校验，完整步骤见迁移指南第 6 节。

若一定要完全无 Hub，可把 `network.transport` 改成
`zmq`；保留的 `iarm_endpoint` 为 `tcp://192.168.5.14:5003`。纯 ZMQ 模式只读取
5003 原始帧，不合并 5001 的其他六维手势；此前删除的 0.25 秒输入陈旧门控和下面的
2 秒 OmniSocket 重建看门狗都不作用于 ZMQ，因此断流会保留最后输入。当前切换到纯直连
时仅允许 monitor-only 链路验证：只停用行走仍不能防止陈旧 Z+C 在断网后计时或双臂/手
继续使用旧帧。完成 ZMQ 专用断流门控前不得启用运动；本地 OmniSocket Hub 推荐方案见
`docs/天工3.0本地同构臂遥操迁移部署指南.md` 第 6 节，纯 ZMQ 备选见第 7 节。

机器人侧不再以 `0.25 s` 公网输入间隔解除武装。活动会话超过 `2 s` 仍无帧时重建机器人
OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或收到 STOP 后没有业务帧是
正常待机，不触发反复重启。只有匹配的操作员 STOP 自动回 Home；意外断网不自动产生
回位运动。

## 独立数采

数采使用左摇杆按下键（L3），不是左侧 X/Y/Z 面键：

- 遥操已成功开启后，先保持 L3 松开至少 `0.5 s`；
- 连续按住 L3 `1 s` 开始一条 episode；
- 松开至少 `0.5 s`，再次连续按住 `1 s` 正常结束；
- 长按 Z+C 结束遥操、安全解除或桥退出时，也会异步请求结束当前 episode；
- 数采失败只记录错误，不解除遥操、不阻塞 50 Hz 控制，也不延迟 STOP/Home。

`button_joystick.left` 是 xTELE 5003 中独立的摇杆按压字段。已安装的 xTELE 0.1.2
没有给它注册处理函数；X/Y/Z/A/B/C 均已有厂家功能或本项目绑定，因此不复用面键。
新会话和服务重启后都先锁定为“必须松开”，畸形或陈旧按键帧不能被当作有效松开。

项目自有 `tg3-data-recorder.service` 在 Nvidia 上运行独立 `ros2 bag record`，不停止、
重配或接管 Ubuntu 厂家 `/record_bag_node`。仅录 `config.toml` 中的明确白名单，包含
`/robot_state`、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态、完整 xTELE
应用帧，以及头部 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。

为避免 1280x720 原始 RGB+深度每台相机约 `8.3 GB/min` 的持续写盘负载，默认录制
`color/image_raw/compressed`（JPEG）和 `depth/image_raw/compressedDepth`（16UC1
无损 PNG）。头部相机不是 required topic：服务在运行就由 rosbag 自动发现并
采集，未运行不阻塞 L3 或核心 READY。manifest 记录 `absent`、`healthy`、
`partial` 或 `low_rate`，并保存各话题消息数和图像平均频率；相机异常会产生明确质量
警告，但不会丢弃其余机器人控制数据。静态 TF 也尽力采集，不作为启动门槛。

机器人暂存目录：

```text
/home/nvidia/tg3_data_collection/
  active/    # 录制期间的临时数据，不能取走
  ready/     # 等待 PS 本机完成持久化验真；随后自动回收
  failed/    # 当前策略不保留失败 payload
  deleting/  # 固定根删除助手的短暂原子隔离目录
```

MCAP 使用 `zstd_fast`、64 MiB cache、每 300 秒分片；每条 episode 最长 30 分钟，
启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完
`metadata.yaml`，随后执行
`ros2 bag info`，并确认所有 required topics 都有非零消息数；然后计算 SHA-256、写
`manifest.json` 和 `READY`，最后才原子进入 `ready/`。Nvidia 需已安装 `python3-yaml`
（现场已验证 PyYAML 6.0.1）。

当前 `retain_failed_episodes=false`：失败原因仍进入 recorder 状态和 journal，但失败
MCAP 不留在机器人磁盘。成功 episode 必须先在 PS 本机完成逐文件 SHA-256、fsync、
原子发布并生成 `VERIFIED`，然后由固定根目录删除助手按 episode ID + manifest SHA
精确回收；它不能访问 `active/`、`failed/` 或数据根目录。PS 断线时 `ready/` 会临时保留，
避免尚未落到本机的数据丢失，恢复后自动续传和回收。

相机由厂家系统服务管理，本项目只订阅、不自动启停。录制前检查或按需启动：

```bash
systemctl is-active orbbec_waist.service orbbec_head.service
sudo systemctl enable --now orbbec_waist.service
# 需要头部图像时再执行；启动后下一条 episode 会自动包含头部 RGB-D：
sudo systemctl enable --now orbbec_head.service
```

录制服务只加载厂家完整 `/opt/robot_tele_server/install` 消息 overlay，不加载本项目
`ros2_py/install`；项目 overlay 仅供遥操桥的 Python 消息绑定使用
（包括 `ArmStatus`/`RobotState`/`HeadCtrl`，但不是厂家完整包）。两者混在同一个
rosbag 环境会让部分 `ros2_bridge_msgs` C++ 类型支持出现动态库符号冲突。

部署两个机器人侧服务：

```bash
mkdir -p ~/.config/systemd/user
cp tg3-data-recorder.service tg3-local-teleop.service ~/.config/systemd/user/
systemctl --user daemon-reload
systemctl --user enable --now tg3-data-recorder.service
systemctl --user restart tg3-local-teleop.service
```

只读检查：

```bash
systemctl --user --no-pager status tg3-data-recorder.service
ros2 topic echo --once /tg3/data_collection/status
python3 -m json.tool /home/nvidia/tg3_local_teleop/status.json
find /home/nvidia/tg3_data_collection/ready -mindepth 1 -maxdepth 1 -type d
```

完成 episode 由 PS 本机服务校验后保存到项目 `Data_Get`，详见
`../tg3_data_collection/README.md`。本机 `VERIFIED` 持久化后，机器人对应 `ready/`
副本会自动删除；EAI 不保存数采 payload。

## 完整重启顺序

只重启本项目的网络与机器人桥时，先启动 Nvidia 接收端，再启动 EAI 本地门控服务；
EAI 在收到物理 START 前不会建立发送 Session：

```bash
ssh nvidia@192.168.41.2 \
  'systemctl --user restart tg3-data-recorder.service tg3-local-teleop.service'
ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
```

检查：

```bash
ssh eai 'systemctl --user --no-pager status tg3-omnisocket-sender.service'
ssh nvidia@192.168.41.2 'systemctl --user --no-pager status tg3-local-teleop.service'
```

修改 Ubuntu 的 `xmigcs/config/dex_config.yaml` 后，必须重启 xmigcs 才会加载新值。现场
固件没有公开的单独 `rl/xmigcs` 启停 service、action 或 CLI；
`/proc_manager/config/notify` 只通知进程管理器重新读取它自己的固定
`proc_manager.json`，消息内容只写入日志，不能执行 `CMD_STOP_PROC` 或
`CMD_START_PROC`。不要向该话题发送进程启停 JSON。

首选做法是先退出遥操并停止本项目两个服务，再按厂商原有物理流程切到 `48V Off`。
现场 `proc_manager` 会在真实的 `48vOff` 状态下停止 `rl` 和 `robot_control`；随后按原流程
恢复 48V 并启动机器人，`robotcontrol_state` 恢复后会重新创建 xmigcs，新进程才会读取
修改后的配置：

```bash
ssh eai 'systemctl --user stop tg3-omnisocket-sender.service'
ssh nvidia@192.168.41.2 'systemctl --user stop tg3-local-teleop.service'

# 此处按厂商原有物理流程切到 48V Off。
# 本机断电后 Ubuntu/网络可能同时离线，SSH 失败是正常现象，不在断电期间执行检查。

# 按厂商原有流程恢复 48V 并启动机器人，等待状态初始化完成；下列命令应显示新 PID。
ssh ubuntu@192.168.41.1 \
  "pgrep -af '^/usr/bin/python3 /home/ubuntu/.local/bin/xmigcs '"
ssh ubuntu@192.168.41.1 \
  "ps -eo pid,lstart,comm,args | grep '[x]migcs'"

ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
ssh nvidia@192.168.41.2 'systemctl --user restart tg3-local-teleop.service'
```

不要直接 `kill` xmigcs，也不要仅为加载此配置执行
`sudo systemctl restart proc_manager.service`。后者会同时停止 joystick、robot_control、
xmigcs 等全部子进程；而 `robot_control` 和 `rl` 均为 `boot_start=false`，可能不能自动
恢复，不能把它当作 xmigcs 单进程重启命令。

EAI 当前用户服务虽已启用，但该用户的 systemd linger 为关闭状态；重启命令可通过 SSH
正常执行，整机冷启动后则需先登录 EAI。若需要无人登录也随系统启动，只执行一次：

```bash
sudo loginctl enable-linger eai
```

当前厂商配置备份位于：

```text
/home/ubuntu/.local/lib/python3.12/site-packages/xmigcs/config/
  dex_config.yaml.before-local-teleop-20260807
```