feat: package TG3 TS1P OmniSocket teleoperation
This commit is contained in:
21
.gitignore
vendored
Normal file
21
.gitignore
vendored
Normal file
@@ -0,0 +1,21 @@
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.pytest_cache/
|
||||
|
||||
# Runtime state must never be committed. It can contain live session and robot data.
|
||||
status.json
|
||||
status.json.tmp
|
||||
*.log
|
||||
*.jsonl
|
||||
|
||||
# ROS and Python build products are architecture-specific.
|
||||
ros2_py/build/
|
||||
ros2_py/install/
|
||||
ros2_py/log/
|
||||
python/build/
|
||||
*.egg-info/
|
||||
*.so
|
||||
|
||||
.DS_Store
|
||||
*~
|
||||
*.bak
|
||||
4
.gitmodules
vendored
Normal file
4
.gitmodules
vendored
Normal file
@@ -0,0 +1,4 @@
|
||||
[submodule "OmniSocketGo"]
|
||||
path = OmniSocketGo
|
||||
url = https://gitea.public.snrc.site/limingjie/OmniSocketGo.git
|
||||
branch = c
|
||||
1
OmniSocketGo
Submodule
1
OmniSocketGo
Submodule
Submodule OmniSocketGo added at de3f5c9677
79
README.md
Normal file
79
README.md
Normal file
@@ -0,0 +1,79 @@
|
||||
# 天工 3.0 TS1P 同构臂本地遥操
|
||||
|
||||
本仓库汇总当前已经部署并验证的三部分：
|
||||
|
||||
```text
|
||||
TS1P / xTELE
|
||||
-> tg3_omnisocket_transport（EAI 会话门控发送端）
|
||||
-> OmniSocketGo / KCP Hub
|
||||
-> tg3_local_teleop（天工 3.0 机器人接收、双臂、双手和行走桥）
|
||||
```
|
||||
|
||||
启动和结束遥操都使用左 `Z` + 右 `C` 连续 3 秒。待机时 EAI 不连接 Hub、
|
||||
不发送业务数据；启动后约以 xTELE 原始频率发送；结束时发送匹配会话的最后一个
|
||||
`STOP`，随后断开。遥操已启动时，右 `C` + 左摇杆在下一个 50 Hz 周期立即控制
|
||||
HBWALK 行走，不再有第二个 3 秒等待。
|
||||
|
||||
## 目录
|
||||
|
||||
```text
|
||||
tg3_omnisocket_transport/ EAI 发送端、用户服务和离线测试
|
||||
tg3_local_teleop/ 机器人桥、配置、用户服务、ROS 消息和离线测试
|
||||
OmniSocketGo/ 固定版本的传输依赖（Git 子模块）
|
||||
docs/ 部署汇报、迁移指南和 Topic/SBUS 证据记录
|
||||
verify.sh 不连接机器人、不发布 ROS 的离线检查
|
||||
```
|
||||
|
||||
OmniSocketGo 固定在提交 `de3f5c96779dbe1571c10feb22fc7f2331b6b222`。克隆时必须拉取
|
||||
子模块：
|
||||
|
||||
```bash
|
||||
git clone --recurse-submodules <本仓库地址>
|
||||
cd TG3_TS1P_OmniSocket_Teleop
|
||||
./verify.sh
|
||||
```
|
||||
|
||||
如果已经普通克隆：
|
||||
|
||||
```bash
|
||||
git submodule update --init --recursive
|
||||
```
|
||||
|
||||
## 迁移时必须修改
|
||||
|
||||
当前提交保留了现场已经运行的配置，便于恢复当前机器人。迁移到其他机器人时，不可
|
||||
原样启动，至少要检查：
|
||||
|
||||
- `tg3_omnisocket_transport/tg3-omnisocket-sender.service` 中的 Hub、发送 Peer ID、
|
||||
目标 Peer ID 和 EAI 用户路径；
|
||||
- `tg3_local_teleop/config.toml` 中的 Hub、机器人 Peer ID、预期发送 Peer ID、
|
||||
同构臂 ID、灵巧手类型和直接局域网回退地址；
|
||||
- 新机器人的 14 个真实 Home 关节值。现有 `home.joint_goal_rad` 只能用于当前机器人，
|
||||
绝不能作为另一台机器人的 Home；
|
||||
- ROS 安装路径、消息类型、HBWALK Topic 和机器人侧厂家速度许可。
|
||||
|
||||
完整流程见
|
||||
[`docs/天工3.0本地同构臂遥操迁移部署指南.md`](docs/天工3.0本地同构臂遥操迁移部署指南.md)。
|
||||
|
||||
## 构建传输依赖
|
||||
|
||||
OmniSocket Python 扩展包含本机架构代码，必须分别在 EAI 和机器人上原生编译，不能
|
||||
复制另一种 CPU/Python 版本生成的 `.so`：
|
||||
|
||||
```bash
|
||||
cd OmniSocketGo
|
||||
make python-ext
|
||||
```
|
||||
|
||||
EAI 和机器人部署命令、systemd 用户服务安装及完整重启顺序分别记录在两个项目的
|
||||
README 和 `docs/` 中。
|
||||
|
||||
## 安全与仓库可见性
|
||||
|
||||
这是物理机器人控制项目。执行 `--allow-publish`、启动用户服务或回 Home 前，必须确认
|
||||
机器人处于 HBWALK、防护和急停有效、双臂及行走区域净空。
|
||||
|
||||
仓库包含现场 Hub 地址、设备/Peer ID、内网地址、实机 Home 姿态以及标记为
|
||||
`Proprietary` 的 ROS 消息定义，建议 Gitea 仓库保持私有。运行产生的 `status.json`、
|
||||
日志、缓存和本机编译文件已由 `.gitignore` 排除。原厂 SDK 文档和 PDF 未收入仓库，
|
||||
避免上传其中的下载授权码、Wi-Fi 信息及版权资料。
|
||||
168
docs/天工3.0_joystick_bridge与SBUS证据记录.md
Normal file
168
docs/天工3.0_joystick_bridge与SBUS证据记录.md
Normal file
@@ -0,0 +1,168 @@
|
||||
# 天工 3.0 `joystick_bridge` 与 `/sbus_data` 证据记录
|
||||
|
||||
记录时间：2026-08-07
|
||||
|
||||
本文只区分三类结论：SDK 文档明确说明、机器人运行时直接观测、尚未证明。不能用 ROS
|
||||
节点存在某个订阅或发布端，代替对其内部算法的证明。
|
||||
|
||||
## 1. 对此前说法的证据审计
|
||||
|
||||
|此前说法|目前证据|结论|
|
||||
|---|---|---|
|
||||
|`joystick_bridge` 处理按键|它订阅 `/sbus_data`；SDK 定义 A～H 语义；实机 `bridge_config.yaml` 给出 A～H 的规则|已证明|
|
||||
|它处理“持续按住”条件|实机 YAML 配置 `a/b/c` 长按阈值分别为 `1.0/1.0/0.5 s`，并给出 `press_state` 规则|已证明厂商遥控器长按机制；与本地 xTELE 的 3 秒保护不是同一机制|
|
||||
|它进行 `/robot_state` 状态检查|运行中的节点确实订阅 `/robot_state`|只证明订阅；如何检查、检查哪些字段尚未证明|
|
||||
|它进行速度缩放和方向映射|已只读取得 `/home/ubuntu/data/param/bridge_config.yaml`，其中包含状态速度上限、偏置和速度处理流水线参数|已证明配置能力；核心算法在编译包中|
|
||||
|它发布 `/fsm_state_cmd`、`/fsm_resume_cmd`、`/stand_cmd`|运行中的 `ros2 node info /joystick_bridge` 直接列出这三个发布端|已证明发布能力；触发条件和消息内容尚未证明|
|
||||
|它有特定行走启停顺序|YAML 的 `fsm_command_bindings` 明确列出状态规则和持续速度动作|已证明配置规则；未做带运动的完整时序采样|
|
||||
|
||||
因此可以准确地说：`joystick_bridge` 是厂商 SBUS 遥控器适配器，包含按键规则、状态命令
|
||||
和速度计算配置。但二次开放文档同时提供独立的 `/hric/robot/cmd_vel` topic 接口，本地
|
||||
xTELE 桥不必伪装成 SBUS 遥控器。
|
||||
|
||||
## 2. SDK 文档证据
|
||||
|
||||
来源：`具身天工3.0+SDK+文档（26.04.x）+(1).md` 第 299～319 行。
|
||||
|
||||
文档明确规定：
|
||||
|
||||
- `/sbus_data` 类型为 `sensor_msgs/msg/Joy`；
|
||||
- `axes` 包含 12 个 `float32`，取值范围 `[-1.0, 1.0]`；
|
||||
- `buttons` 为空，不使用；
|
||||
- `/sbus_data/event` 类型为 `bodyctrl_msgs/msg/SbusData`；
|
||||
- 事件消息包含 A～H 的状态，以及 `x1/y1/x2/y2` 四个摇杆轴；
|
||||
- 四个摇杆轴范围为 `[-1.0, 1.0]`，直接透传、不滤波。
|
||||
|
||||
文档没有给出 `/sbus_data.axes[0..11]` 与 `x1/y1/x2/y2/A..H` 的逐项下标映射，
|
||||
因此不能只根据字段数量猜测 12 个下标。
|
||||
|
||||
## 3. 机器人运行时证据
|
||||
|
||||
在 Nvidia 侧加载机器人 ROS 环境后执行：
|
||||
|
||||
```bash
|
||||
ros2 node info /joystick_bridge
|
||||
ros2 param get /joystick_bridge config_file
|
||||
ros2 param get /joystick_bridge joy_topic
|
||||
ros2 topic info -v /sbus_data
|
||||
```
|
||||
|
||||
观测结果：
|
||||
|
||||
```text
|
||||
/joystick_bridge subscribers:
|
||||
/robot_state: ros2_bridge_msgs/msg/RobotState
|
||||
/sbus_data: sensor_msgs/msg/Joy
|
||||
|
||||
/joystick_bridge publishers:
|
||||
/hric/robot/cmd_vel: geometry_msgs/msg/TwistStamped
|
||||
/hric/robot/fsm_resume_cmd: std_msgs/msg/String
|
||||
/hric/robot/fsm_state_cmd: std_msgs/msg/String
|
||||
/hric/robot/stand_cmd: geometry_msgs/msg/TwistStamped
|
||||
（另有双臂、头、腰和双手控制话题）
|
||||
|
||||
config_file = /home/ubuntu/data/param/bridge_config.yaml
|
||||
joy_topic = /sbus_data
|
||||
```
|
||||
|
||||
`/sbus_data` 当前只有一个发布者 `/joystick`，QoS 为 `RELIABLE/VOLATILE`。空闲帧实测为：
|
||||
|
||||
```yaml
|
||||
axes: [-0.0, -0.0, -0.0, -0.0, -1.0, -0.0, -1.0, -0.0,
|
||||
-1.0, -1.0, -1.0, -1.0]
|
||||
buttons: []
|
||||
```
|
||||
|
||||
对应时刻 `/sbus_data/event` 的语义状态为：
|
||||
|
||||
```yaml
|
||||
button_a: -1
|
||||
button_b: -1
|
||||
button_c: -1
|
||||
button_d: -1
|
||||
button_e: -1
|
||||
button_f: 0
|
||||
button_g: 0
|
||||
button_h: -1
|
||||
x1: -0.0
|
||||
y1: -0.0
|
||||
x2: -0.0
|
||||
y2: -0.0
|
||||
```
|
||||
|
||||
这组同时采样可作为空闲状态基准，但不足以单独反推出所有按键下标和状态编码；需要读取
|
||||
`bridge_config.yaml`，或对原遥控器逐键采样。
|
||||
|
||||
## 4. 同构臂侧是否具备生成 SBUS 输入的数据
|
||||
|
||||
EAI 上 xTELE 原始流 `tcp://127.0.0.1:5003` 的一帧实测包含：
|
||||
|
||||
```text
|
||||
joystick = {'left': [-0.009, 0.012], 'right': [-0.059, 0.011]}
|
||||
button = {'left': [0, 0, 0, 0, 0], 'right': [0, 0, 0, 0, 0]}
|
||||
```
|
||||
|
||||
同一帧还包含 `button_joystick`、`button_wheel`、`scroll`、`trigger` 等输入。因此：
|
||||
|
||||
- 如果“灵巧手”指同构臂手柄：可以取得左右摇杆和实体按键原始数据，数据源足够构造
|
||||
`sensor_msgs/msg/Joy`；
|
||||
- 如果“灵巧手”指机器人 BrainCo 灵巧手：不可以。灵巧手只提供电机位置/状态和触觉等，
|
||||
不会产生遥控器摇杆与 A～H 开关数据；
|
||||
- xTELE 按键是布尔输入，而 SBUS 中含二档、三档开关语义。需要通过明确映射或本地状态机
|
||||
转换，不能把布尔数组直接当成 12 个 SBUS 轴发布。
|
||||
|
||||
## 5. 本地桥不再发布 FSM 命令
|
||||
|
||||
已从 `tg3_local_teleop.py` 删除：
|
||||
|
||||
- `std_msgs/msg/String` FSM 发布器；
|
||||
- `gotoHBWALK` 消息构造与重复发布；
|
||||
- FSM burst、间隔和等待参数。
|
||||
|
||||
当前桥只在机器人已经报告 `HBWALK/running` 且安全门通过时发布
|
||||
`/hric/robot/cmd_vel`，释放操作后仍发送限量零速度帧。
|
||||
|
||||
本地公网链路的唯一 3 秒门控现位于 EAI：左 Z + 右 C 产生 START/STOP 会话，待机不发送
|
||||
xTELE 业务帧。会话启动后，右 C + 左摇杆在机器人端即时映射到 `/hric/robot/cmd_vel`；
|
||||
松开立即零速，再次操作不重新计时。
|
||||
|
||||
部署后运行时检查 `/hric/robot/fsm_state_cmd` 只有厂商节点
|
||||
`control_forward` 与 `joystick_bridge` 两个发布者，不再包含 `tg3_local_teleop`；状态文件同时显示：
|
||||
|
||||
```text
|
||||
locomotion_fsm_publish_enabled = false
|
||||
```
|
||||
|
||||
## 6. 最终采用 direct topic，而不是模拟 `/sbus_data`
|
||||
|
||||
`twice dev.pdf` 明确规定 `/hric/robot/cmd_vel` 为
|
||||
`geometry_msgs/msg/TwistStamped`，其中 `linear.x/linear.y/angular.z` 分别控制前后、侧移和
|
||||
转向。运行中的 `/hric_topic_bridge` 也直接订阅该话题。因此本项目继续使用 direct topic，
|
||||
不增加第二个 `/sbus_data` 发布者，避免与原 `/joystick` 的约 43 Hz 数据交错。
|
||||
|
||||
实机 xmigcs `dex_config.yaml` 原先把 HBWALK 的四个速度上限全部设为零，这才是 HBWALK
|
||||
topic 无法行走的关键配置。2026-08-07 已按文档把 HBWALK 上限改为当前 NAVIGATE 同值，
|
||||
并保留原配置备份。`twice dev.pdf` 第 1 页规定半身行走 Topic 的 `linear.x` 范围为
|
||||
`[-0.8, 1.0] m/s`、`angular.z` 范围为 `[-0.8, 0.8] rad/s`；本地桥现按这组官方 Topic
|
||||
范围做前进/后退非对称限幅，不发布 `linear.y`。
|
||||
|
||||
## 7. xmigcs 配置的加载与重启边界
|
||||
|
||||
`dex_config.yaml` 只在 xmigcs 初始化时由 `robot_interface.init()` 读取，当前节点没有动态
|
||||
reload 参数、service 或 action。现场固件的 `/proc_manager/config/notify` 也不是进程
|
||||
启停接口：其回调只记录 String 内容并重新读取 proc_manager 自身的固定
|
||||
`proc_manager.json`。曾尝试向该话题发送 `CMD_STOP_PROC/CMD_START_PROC` JSON，实测
|
||||
xmigcs PID 始终为 `3269`，日志也没有任何启停记录；该用法已确认无效并从部署说明删除。
|
||||
|
||||
当前固件没有启用内部遗留 `OnRequest` 的 fd 通道，也没有单进程 ROS service、action、
|
||||
监听端口或 CLI。因此加载新 YAML 应使用厂商真实生命周期：先退出遥操，按原流程切到
|
||||
`48V Off`，由 proc_manager 停止 `rl` 与 `robot_control`，再恢复 48V 并完成原有物理启动
|
||||
动作。只有 xmigcs 出现新的 PID/启动时间，才表示新配置已经进入运行态。不要直接 kill
|
||||
xmigcs，也不要伪造 `/robotcontrol_state`。
|
||||
|
||||
2026-08-07 14:32 的实际重启结果为 PID `3125`。新日志明确打印 HBWALK 已加载
|
||||
`max_x_plus=1.2`、`max_x_minus=0.6`、`max_y=0.6`、`max_yaw=1.0`，随后完成
|
||||
Robot interface、SHM RPC 与 100 Hz 控制线程初始化，并进入 `HBWALK`。该进程会把
|
||||
Linux `comm` 名称设为 `xmigcs`，因此 `ps -C python3` 可能无输出；应使用
|
||||
`ps -C xmigcs -o pid,lstart,cmd` 或上述精确 `pgrep`。本机切断动力电源时 Ubuntu/网络也
|
||||
可能同时离线，断电期间 SSH 失败属于正常现象，应在重新上电并完成启动后检查。
|
||||
451
docs/天工3.0本地同构臂遥操迁移部署指南.md
Normal file
451
docs/天工3.0本地同构臂遥操迁移部署指南.md
Normal file
@@ -0,0 +1,451 @@
|
||||
# 天工 3.0 本地同构臂遥操迁移部署指南
|
||||
|
||||
适用范围：天工 3.0（HBWALK）+ TS1P 同构臂。当前灵巧手实现适用于机器人实际配置为
|
||||
BrainCo Revo2 的情况。
|
||||
|
||||
## 1. 迁移前先区分四种地址
|
||||
|
||||
| 地址/标识 | 示例 | IP 变化时是否修改运行配置 |
|
||||
|---|---|---|
|
||||
| 新机器人 SSH 地址 | `nvidia@192.168.41.2` | 只影响安装、维护命令；公网 OmniSocket 运行时不使用这个地址 |
|
||||
| EAI SSH 地址/别名 | `eai` | 只影响安装、维护命令；xTELE 使用本机回环地址，不依赖 EAI 局域网 IP |
|
||||
| OmniSocket Hub 地址 | `175.178.116.187:14049` | 必须同时修改 EAI 发送服务和机器人 `config.toml` |
|
||||
| OmniSocket Peer ID | `tg3-...-iarm/robot` | 每套链路必须成对匹配；换机器人时建议使用新的机器人唯一 ID |
|
||||
|
||||
当前公网模式下，机器人从 Wi-Fi 换到有线、DHCP 地址变化或 EAI 局域网地址变化，通常都
|
||||
不需要修改运行配置，只需保证两端能主动访问 Hub 的 UDP 端口。新的 SSH 地址需要更新到
|
||||
运维命令或设备清单中。
|
||||
|
||||
`config.toml` 中保留的 `iarm_endpoint` 只在 `transport="zmq"` 时使用；当前
|
||||
`transport="omnisocket"` 时它被忽略。不要因为机器人或 EAI IP 变化而修改这个字段。
|
||||
|
||||
## 2. 每次迁移必须确认或重做的项目
|
||||
|
||||
### 2.1 必须使用唯一、成对匹配的 Peer ID
|
||||
|
||||
推荐命名：
|
||||
|
||||
```text
|
||||
EAI 发送端：tg3-<机器人编号>-iarm
|
||||
机器人接收端：tg3-<机器人编号>-robot
|
||||
```
|
||||
|
||||
三处必须满足：
|
||||
|
||||
```text
|
||||
EAI --peer-id == 机器人 omnisocket_expected_sender
|
||||
EAI --target-peer == 机器人 omnisocket_peer_id
|
||||
EAI --server == 机器人 omnisocket_server
|
||||
```
|
||||
|
||||
同一台 EAI 同一时刻只应控制一台机器人。现有发送服务只有一个 `--target-peer`，迁移到
|
||||
另一台机器人后要修改目标 Peer 并重启发送服务，不要用一套 TS1P 同时向多台机器人发指令。
|
||||
|
||||
### 2.2 新机器人必须重新保存 Home
|
||||
|
||||
不要把当前机器人的 `home.joint_goal_rad` 直接用于另一台机器人。即使型号相同，机械零位、
|
||||
装配偏差和期望停放姿态也可能不同。
|
||||
|
||||
在新机器人上：
|
||||
|
||||
1. 使用厂家认可的方法把双臂移动到希望保存的 Home；不要强行扳动上电电机；
|
||||
2. 确认本地遥操为 `armed=false`；
|
||||
3. 读取实测位置：
|
||||
|
||||
```bash
|
||||
python3 -c 'import json; p="/home/nvidia/tg3_local_teleop/status.json"; print(json.load(open(p))["robot_arm_position_rad"])'
|
||||
```
|
||||
|
||||
4. 将输出的 14 个弧度值原样写入新机器人
|
||||
`/home/nvidia/tg3_local_teleop/config.toml` 的 `[home].joint_goal_rad`；
|
||||
5. 重启服务后，先检查状态，再在净空和急停可用的条件下测试限速 Home。
|
||||
|
||||
### 2.3 必须确认灵巧手型号
|
||||
|
||||
```bash
|
||||
grep -E 'left_hand_type|right_hand_type' /home/nvidia/data/param/hand_driver.yaml
|
||||
```
|
||||
|
||||
只有左右都显示 `brainco` 时，才能直接使用当前 `[hands]` 配置。如果是 `inspire` 或其他
|
||||
型号，不要启动灵巧手控制；消息类型、Topic 和关节映射不同，需要单独适配。
|
||||
|
||||
BrainCo 新手初次部署时应在默认打开状态记录：
|
||||
|
||||
```bash
|
||||
python3 -c 'import json; p="/home/nvidia/tg3_local_teleop/status.json"; d=json.load(open(p)); print(d["robot_hand_positions"])'
|
||||
```
|
||||
|
||||
当前打开端点约为 `[400,400,50,50,50,50]`。若新机器明显不同，需要按
|
||||
`normalized=(position-1)/999` 重新计算 `[hands].open_normalized`。闭合端点也应从小幅、
|
||||
低速测试开始确认，不能直接假设所有手的机械校准完全相同。
|
||||
|
||||
### 2.4 确认机器人软件接口
|
||||
|
||||
至少确认以下 Topic/消息仍存在：
|
||||
|
||||
```text
|
||||
/hric/robot/rl_state
|
||||
/freq_change/arm_status
|
||||
/encoder_identical_joint
|
||||
/left_hand/set_motor_multi
|
||||
/right_hand/set_motor_multi
|
||||
/left_hand/motor_status
|
||||
/right_hand/motor_status
|
||||
```
|
||||
|
||||
若新机器人不是天工 3.0、SDK 版本接口有变化、不是 14 维双臂或不是 BrainCo Revo2，先停止
|
||||
部署并适配，不要仅靠修改 IP 强行运行。
|
||||
|
||||
## 3. 场景 A：保留当前 EAI，只换机器人
|
||||
|
||||
这是推荐迁移方式。EAI 上的 xTELE、OmniSocketGo 和发送脚本不需要重新安装。
|
||||
|
||||
### 3.1 准备变量
|
||||
|
||||
以下变量只用于本次终端命令，不写入服务：
|
||||
|
||||
```bash
|
||||
export TG3_NEW_ROBOT_IP=192.168.41.XX
|
||||
export TG3_NEW_ROBOT_PEER=tg3-<新机器人编号>-robot
|
||||
export TG3_IARM_PEER=tg3-<新机器人编号>-iarm
|
||||
export TG3_HUB_ADDR=175.178.116.187:14049
|
||||
```
|
||||
|
||||
不要使用旧机器人和新机器人相同的 Robot Peer ID。如果 EAI Peer ID 也随机器人编号更换，
|
||||
机器人 `omnisocket_expected_sender` 必须同步更新。
|
||||
|
||||
### 3.2 在新机器人原生编译 OmniSocket Python 扩展
|
||||
|
||||
开发机上的 OmniSocket 扩展是 x86_64，而天工 3.0 机器人是 aarch64，不能复制已编译的
|
||||
`.so`。传输源码时排除已有二进制，在机器人上重新编译：
|
||||
|
||||
```bash
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" 'mkdir -p /home/nvidia/OmniSocketGo'
|
||||
|
||||
rsync -av \
|
||||
--exclude=.git \
|
||||
--exclude=bin \
|
||||
--exclude=python/build \
|
||||
--exclude='python/omnisocket/_omnisocket*.so' \
|
||||
/home/ps/Desktop/OmniSocketGo/ \
|
||||
nvidia@"${TG3_NEW_ROBOT_IP}":/home/nvidia/OmniSocketGo/
|
||||
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||
'cd /home/nvidia/OmniSocketGo && make python-ext'
|
||||
```
|
||||
|
||||
验证扩展架构和导入：
|
||||
|
||||
```bash
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||
'uname -m; find /home/nvidia/OmniSocketGo/python -name "_omnisocket*.so" -print'
|
||||
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||
'PYTHONPATH=/home/nvidia/OmniSocketGo/python python3 -c "from omnisocket import Session; print(Session)"'
|
||||
```
|
||||
|
||||
预期机器人架构为 `aarch64`，扩展文件名包含 `aarch64` 和机器人实际 Python 版本。
|
||||
|
||||
### 3.3 部署机器人桥，但暂不启动
|
||||
|
||||
```bash
|
||||
rsync -av \
|
||||
--exclude=__pycache__ \
|
||||
--exclude=status.json \
|
||||
/home/ps/Downloads/tg3_local_teleop/ \
|
||||
nvidia@"${TG3_NEW_ROBOT_IP}":/home/nvidia/tg3_local_teleop/
|
||||
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||
'mkdir -p /home/nvidia/.config/systemd/user && cp /home/nvidia/tg3_local_teleop/tg3-local-teleop.service /home/nvidia/.config/systemd/user/'
|
||||
```
|
||||
|
||||
此时不要立即长按启动。先编辑新机器人：
|
||||
|
||||
```bash
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}"
|
||||
nano /home/nvidia/tg3_local_teleop/config.toml
|
||||
```
|
||||
|
||||
必须检查的字段：
|
||||
|
||||
```toml
|
||||
[network]
|
||||
transport = "omnisocket"
|
||||
omnisocket_server = "175.178.116.187:14049" # TG3_HUB_ADDR
|
||||
omnisocket_peer_id = "tg3-<新机器人编号>-robot" # TG3_NEW_ROBOT_PEER
|
||||
omnisocket_expected_sender = "tg3-<新机器人编号>-iarm" # TG3_IARM_PEER
|
||||
expected_iarm_id = "IArm009027FA8190" # 若仍用当前 TS1P，可保持
|
||||
expected_iarm_type = "TS1P"
|
||||
|
||||
[home]
|
||||
joint_goal_rad = [ ...新机器人实测的 14 个 Home 值... ]
|
||||
```
|
||||
|
||||
还要检查：
|
||||
|
||||
- `[hands]` 是否与新机器人的真实手型相符；
|
||||
- `[control].joint_lower_rad/joint_upper_rad` 是否仍适用于同型号和当前 SDK；
|
||||
- `run.sh`、service 内的用户名和路径是否仍为 `/home/nvidia`；
|
||||
- ROS 安装路径是否仍有 `/opt/ros/jazzy`、`/home/nvidia/xos` 或
|
||||
`/opt/robot_tele_server/install`。
|
||||
|
||||
### 3.4 修改 EAI 的目标 Peer
|
||||
|
||||
编辑：
|
||||
|
||||
```bash
|
||||
ssh eai
|
||||
nano /home/eai/.config/systemd/user/tg3-omnisocket-sender.service
|
||||
```
|
||||
|
||||
修改 `ExecStart` 中：
|
||||
|
||||
```text
|
||||
--server <TG3_HUB_ADDR>
|
||||
--peer-id <TG3_IARM_PEER>
|
||||
--target-peer <TG3_NEW_ROBOT_PEER>
|
||||
```
|
||||
|
||||
不要修改：
|
||||
|
||||
```text
|
||||
--zmq-endpoint tcp://127.0.0.1:5003
|
||||
--cmd-zmq-endpoint tcp://127.0.0.1:5001
|
||||
--cmd-max-age-s 0.25
|
||||
```
|
||||
|
||||
应用 EAI 配置：
|
||||
|
||||
```bash
|
||||
systemctl --user daemon-reload
|
||||
systemctl --user restart tg3-omnisocket-sender.service
|
||||
systemctl --user status tg3-omnisocket-sender.service --no-pager
|
||||
cat /home/eai/tg3_omnisocket_transport/status.json
|
||||
```
|
||||
|
||||
### 3.5 启动新机器人监测服务
|
||||
|
||||
```bash
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||
'systemctl --user daemon-reload && systemctl --user enable --now tg3-local-teleop.service'
|
||||
```
|
||||
|
||||
先观察，不要长按武装：
|
||||
|
||||
```bash
|
||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||
'sleep 5; cat /home/nvidia/tg3_local_teleop/status.json'
|
||||
```
|
||||
|
||||
必须看到：
|
||||
|
||||
```text
|
||||
mode = active-capable
|
||||
armed = false
|
||||
returning_home = false
|
||||
operator_session_state = inactive（尚未有首帧时也可为空闲）
|
||||
iarm_transport_status.connected = true
|
||||
foreign_source_seen = false
|
||||
foreign_hand_source_seen = false
|
||||
```
|
||||
|
||||
待机时 EAI 的 `frames_received` 应持续增长，但 `frames_sent/bytes_sent` 不增长；机器人
|
||||
`iarm_age_s` 为空或逐渐变旧属于预期。身份、频率和扳机原始值先在 EAI 本机检查，完整
|
||||
启动门控在 START 首帧到达机器人后执行。
|
||||
|
||||
然后再保存新 Home、核对手部打开位，并按第 6 节进行现场验收。
|
||||
|
||||
## 4. 场景 B：EAI 工控机也更换
|
||||
|
||||
新 EAI 除场景 A 的机器人步骤外，还要完成以下内容。
|
||||
|
||||
### 4.1 原厂 xTELE 必须先独立正常
|
||||
|
||||
先按 TS1P 厂家方法完成串口、CAN、关节方向、偏置和 Home 标定，确认 xTELE 能在 EAI
|
||||
本机持续发布：
|
||||
|
||||
```text
|
||||
tcp://127.0.0.1:5003 原始关节、按键和诊断帧
|
||||
tcp://127.0.0.1:5001 xTELE 处理后的控制帧（组合手势需要）
|
||||
```
|
||||
|
||||
若 5003 正常而 5001 不存在，双臂、Z+C 和摇杆仍能收到原始数据，但飞书指南中的手势组合键
|
||||
不会生成 BrainCo 六维目标。迁移时必须同时检查两个端口。
|
||||
|
||||
不要把旧 EAI 的串口设备路径和关节偏置盲目复制到不同硬件。只有同一套 TS1P 搬到新
|
||||
工控机且串口设备一致时，才可参考旧配置。
|
||||
|
||||
### 4.2 在新 EAI 原生编译 OmniSocket 扩展
|
||||
|
||||
新 EAI 当前通常是 `x86_64`，但仍应在目标机本地构建，以匹配其 Python 版本：
|
||||
|
||||
```bash
|
||||
rsync -av \
|
||||
--exclude=.git \
|
||||
--exclude=bin \
|
||||
--exclude=python/build \
|
||||
--exclude='python/omnisocket/_omnisocket*.so' \
|
||||
/home/ps/Desktop/OmniSocketGo/ \
|
||||
<新EAI用户>@<新EAI地址>:/home/<新EAI用户>/OmniSocketGo/
|
||||
|
||||
ssh <新EAI用户>@<新EAI地址> \
|
||||
'cd /home/<新EAI用户>/OmniSocketGo && make python-ext'
|
||||
```
|
||||
|
||||
将当前 EAI 的自有发送目录复制到新 EAI，并保持原厂目录不变：
|
||||
|
||||
```text
|
||||
/home/eai/tg3_omnisocket_transport/
|
||||
omnisocket_xtele_sender.py
|
||||
README.md
|
||||
```
|
||||
|
||||
在新 EAI 创建对应用户服务，修改所有 `/home/eai` 为新用户真实目录，并设置：
|
||||
|
||||
```text
|
||||
Environment=PYTHONPATH=/home/<新EAI用户>/OmniSocketGo/python
|
||||
--server <Hub IP:Port>
|
||||
--peer-id <EAI Peer ID>
|
||||
--target-peer <机器人 Peer ID>
|
||||
--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
|
||||
--max-feedback-age-ms 500
|
||||
--max-pending-frames 100
|
||||
Restart=always
|
||||
```
|
||||
|
||||
如果希望用户级服务在没有图形登录时也随开机运行，需要由管理员为实际账号开启 linger：
|
||||
|
||||
```bash
|
||||
sudo loginctl enable-linger <新EAI用户>
|
||||
```
|
||||
|
||||
机器人用户服务同理，是否需要 linger 取决于机器人系统是否会自动创建 `nvidia` 用户会话。
|
||||
|
||||
## 5. Hub IP 或端口变化时修改哪里
|
||||
|
||||
假设新 Hub 为 `203.0.113.10:15000`：
|
||||
|
||||
### EAI
|
||||
|
||||
修改：
|
||||
|
||||
```text
|
||||
/home/eai/.config/systemd/user/tg3-omnisocket-sender.service
|
||||
```
|
||||
|
||||
把：
|
||||
|
||||
```text
|
||||
--server 175.178.116.187:14049
|
||||
```
|
||||
|
||||
改为：
|
||||
|
||||
```text
|
||||
--server 203.0.113.10:15000
|
||||
```
|
||||
|
||||
### 机器人
|
||||
|
||||
修改：
|
||||
|
||||
```text
|
||||
/home/nvidia/tg3_local_teleop/config.toml
|
||||
```
|
||||
|
||||
把：
|
||||
|
||||
```toml
|
||||
omnisocket_server = "175.178.116.187:14049"
|
||||
```
|
||||
|
||||
改为：
|
||||
|
||||
```toml
|
||||
omnisocket_server = "203.0.113.10:15000"
|
||||
```
|
||||
|
||||
### Hub/防火墙
|
||||
|
||||
- KCP Hub 应监听 `0.0.0.0:15000`；
|
||||
- 云安全组和主机防火墙应允许对应 UDP 端口；
|
||||
- 不要只开放同端口的 TCP；当前跨公网控制使用 UDP/KCP。
|
||||
|
||||
修改后按顺序重启：
|
||||
|
||||
1. Hub；
|
||||
2. EAI `tg3-omnisocket-sender.service`；
|
||||
3. 机器人接收端。2 秒业务帧看门狗只在活动会话内生效，待机不会反复重启。
|
||||
|
||||
## 6. 新机器人首次现场验收顺序
|
||||
|
||||
全程确认防护、活动空间、急停和 HBWALK 状态。
|
||||
|
||||
1. 服务启动后保持未武装；在 EAI 状态中确认本地接收计数增长而公网发送计数不增长；
|
||||
2. 不武装时分别扣左右扳机，在 EAI 本地数据中确认左右值独立从 0 到 1；
|
||||
3. 确认左右手物理默认打开位和 `robot_hand_states` 正常；
|
||||
4. 把 TS1P 双臂摆到与机器人当前位置尽量接近的安全姿态；
|
||||
5. 松开两个扳机，长按左 Z + 右 C 3 秒启动；
|
||||
6. 先做小幅单关节跟随，再逐渐扩大动作；
|
||||
7. 分别小幅扣左右扳机，验证双手方向、范围和限速；
|
||||
8. 先停止并确认 `armed=false`，按飞书指南逐个选择手势组合键；检查 EAI `status.json`
|
||||
的 `command_hand_merges` 增长，并确认机器人 `iarm_hand_position` 变为左右各 6 维数组；
|
||||
9. 再次武装后只做小幅扳机动作，逐个验证组合手势方向和限速；
|
||||
10. 保持右 C，把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；松 C，确认
|
||||
立即零速停止。再次按 C/推杆也应立即响应，不再等待 3 秒；
|
||||
11. 再次长按停止，观察双臂限速回到新机器人保存的 Home；
|
||||
12. 测试 Hub 短暂断线：应在 0.25 秒后停止发布，恢复后先确认未武装，再重新长按启动；
|
||||
13. 记录最终 Peer ID、Hub、SSH 地址、Home、行走限速和手部端点到该机器人的设备档案。
|
||||
|
||||
迁移时确认目标机提供 `/hric/robot/cmd_vel`（`geometry_msgs/msg/TwistStamped`），并保持
|
||||
`locomotion.command_topic` 与目标固件一致。行走限速、死区、曲线和方向符号均在机器人
|
||||
`config.toml` 的 `[locomotion]` 中调整。当前天工 3.0 按二次开放文档的半身行走 Topic
|
||||
范围配置：`max_forward_m_s=1.0`、`max_reverse_m_s=0.8`、
|
||||
`max_angular_rad_s=0.8`；迁移到不同型号或固件时应重新核对其官方 Topic 范围。
|
||||
|
||||
## 7. 常见问题
|
||||
|
||||
### 机器人 SSH IP 变了，需要改 `config.toml` 吗？
|
||||
|
||||
公网 OmniSocket 模式不需要。只修改 SSH 命令中的地址。运行时机器人主动连接 Hub。
|
||||
|
||||
### EAI IP 变了，需要改 `tcp://127.0.0.1:5003/5001` 吗？
|
||||
|
||||
不需要。`127.0.0.1` 永远表示 EAI 本机，和网卡地址无关。
|
||||
|
||||
### 只改机器人 Peer，不改 EAI 可以吗？
|
||||
|
||||
不可以。EAI 的 `--target-peer` 必须等于机器人的 `omnisocket_peer_id`，机器人
|
||||
`omnisocket_expected_sender` 必须等于 EAI 的 `--peer-id`。
|
||||
|
||||
### 可以直接复制当前机器人的整个 OmniSocketGo 目录吗？
|
||||
|
||||
只有目标机器架构和 Python ABI 完全一致时才可能复用。更稳妥的方式是复制源码并在目标
|
||||
机器执行 `make python-ext`。EAI 的 x86_64 `.so` 绝对不能用于 aarch64 机器人。
|
||||
|
||||
### 可以复用当前 Home 吗？
|
||||
|
||||
不建议。Home 是现场机器人真实反馈值，不是型号固定常数。每台机器人都应重新保存。
|
||||
|
||||
### 新机器人是因时手，可以直接改 Topic 吗？
|
||||
|
||||
不可以。当前桥导入并发布 BrainCo 消息，因时手是不同消息类型和 13 自由度映射，需要
|
||||
单独实现适配后再部署。
|
||||
|
||||
## 8. 迁移时真正需要修改的最小清单
|
||||
|
||||
同一 EAI、同一 TS1P、同一 Hub、换一台同配置 BrainCo 天工 3.0 时，最少修改：
|
||||
|
||||
1. EAI service 的 `--target-peer`；
|
||||
2. 若更换 EAI Peer ID，同时修改 EAI `--peer-id` 和机器人
|
||||
`omnisocket_expected_sender`；
|
||||
3. 机器人 `omnisocket_peer_id`；
|
||||
4. 新机器人 `[home].joint_goal_rad`；
|
||||
5. 核对并按实测修正 `[hands].open_normalized`；
|
||||
6. SSH 命令中的新机器人 IP。
|
||||
|
||||
Hub IP/端口变化时，额外同时修改 EAI `--server` 和机器人 `omnisocket_server`。
|
||||
354
docs/天工3.0本地同构臂遥操部署汇报.md
Normal file
354
docs/天工3.0本地同构臂遥操部署汇报.md
Normal file
@@ -0,0 +1,354 @@
|
||||
# 天工 3.0 本地同构臂遥操部署汇报
|
||||
|
||||
日期：2026-08-06
|
||||
设备：天工 3.0（HBWALK）+ TS1P 同构臂 + BrainCo Revo2 双灵巧手
|
||||
|
||||
## 1. 最终结果
|
||||
|
||||
已实现不依赖厂商“智能驾驶舱平台”配对的本地双臂、双手遥操。运行时链路为：
|
||||
|
||||
```text
|
||||
TS1P 同构臂
|
||||
-> EAI 原厂 xTELE
|
||||
-> tcp://127.0.0.1:5003（原始关节、按键和诊断，仅 EAI 机内）
|
||||
-> tcp://127.0.0.1:5001（xTELE 处理后的双手目标，仅 EAI 机内）
|
||||
-> 发送器仅把 5001 的 hand.position 合并到 5003 原始帧
|
||||
-> tg3-omnisocket-sender
|
||||
-> OmniSocket / KCP（UDP）
|
||||
-> 175.178.116.187:14049 公网 Hub
|
||||
-> 机器人内 tg3_local_teleop 直接接收 OmniSocket
|
||||
-> /encoder_identical_joint（双臂 14 维 JointState）
|
||||
-> 原厂 freq_change_tg3_node
|
||||
-> /arm/cmd
|
||||
-> 天工 3.0 双臂
|
||||
|
||||
同一个 tg3_local_teleop
|
||||
-> /left_hand/set_motor_multi
|
||||
-> /right_hand/set_motor_multi
|
||||
-> 原厂 BrainCo Revo2 双手驱动
|
||||
```
|
||||
|
||||
机器人端不再设置单独的 `tg3-omnisocket-receiver.service`，OmniSocket 接收、校验和
|
||||
ROS 发布都在 `tg3_local_teleop` 内完成。开发电脑不参与运行链路，只保存源码和本报告。
|
||||
|
||||
## 2. 为什么采用这条链路
|
||||
|
||||
### 2.1 保留原厂 xTELE 与机器人控制栈
|
||||
|
||||
TS1P 的串口/CAN、关节标定、按键、扳机和错误状态已经由原厂 xTELE 正确处理。xTELE
|
||||
当前安装版本的核心模块是编译后的扩展，直接修改或替换会增加标定、按键兼容和设备安全
|
||||
风险，因此保留原程序及配置，不修改其源码。
|
||||
|
||||
机器人端也继续使用原厂 `freq_change_tg3_node`、`/arm/cmd`、BrainCo 驱动及其默认
|
||||
限位、电流、堵转和碰撞保护。本项目只在原厂已有输入接口之前增加网络桥和安全门控。
|
||||
|
||||
### 2.2 为什么链路中仍能看到 TCP
|
||||
|
||||
`tcp://127.0.0.1:5003` 和 `tcp://127.0.0.1:5001` 都是 xTELE 已有的本机 ZMQ
|
||||
发布接口，地址为回环地址：
|
||||
|
||||
- 数据不会离开 EAI 工控机；
|
||||
- 它不是 EAI 到机器人之间的网络传输；
|
||||
- 保留它可以避免修改编译后的 xTELE；
|
||||
- 真正跨机器、跨公网的部分使用 OmniSocket KCP/UDP。
|
||||
|
||||
如果连这段本机 TCP 也删除，就必须修改或替换原厂 xTELE，违背“不改原有代码”的约束。
|
||||
|
||||
### 2.3 为什么不直接使用原无线 UDP
|
||||
|
||||
普通点对点 UDP 适合同一局域网，但公网环境会遇到 NAT、地址变化、丢包、乱序和重传问题。
|
||||
OmniSocket 使用固定 Peer ID 经过 KCP Hub 转发，提供有序传输、丢包重传、会话注册和统计，
|
||||
因此同一套代码可通过 Wi-Fi、有线互联网运行，不要求机器人与 EAI 位于同一网段。
|
||||
|
||||
当前 Peer 配置：
|
||||
|
||||
| 项目 | 值 |
|
||||
|---|---|
|
||||
| Hub | `175.178.116.187:14049` |
|
||||
| EAI Peer ID | `tg3-009027fa8190-iarm` |
|
||||
| 机器人 Peer ID | `tg3-009027fa8190-robot` |
|
||||
| EAI 本机 xTELE 原始数据 | `tcp://127.0.0.1:5003` |
|
||||
| EAI 本机 xTELE 处理后命令 | `tcp://127.0.0.1:5001`（只合并 `hand.position`） |
|
||||
|
||||
## 3. 端到端频率
|
||||
|
||||
不同位置的“频率”含义不同，不能只用一个数表示整条链路。
|
||||
|
||||
| 环节 | 频率/策略 | 说明 |
|
||||
|---|---:|---|
|
||||
| TS1P 关节采样 `freq` | 现场约 `80~92 Hz` | xTELE JSON 自带的左右臂/CAN采样频率；本次抓帧为 `91.405 Hz`，启动最低门槛为 `30 Hz` |
|
||||
| xTELE ZMQ 发布 | 实测 `98.02 Hz` | 连续接收 500 帧耗时 5.101 秒 |
|
||||
| xTELE 处理后命令 `5001` | 现场约 `50 Hz` | 只取最新有效双手目标，最大允许年龄 `0.25 s` |
|
||||
| EAI OmniSocket 发送 | 待机 `0 Hz`，遥操约 `98~100 Hz` | Z+C 3 秒后才连接并逐帧发送；STOP 刷新后关闭 Session，待机无注册/心跳 |
|
||||
| 机器人 OmniSocket 接收 | 待机无业务帧，遥操约 `100 Hz` | 网络状况变化时会波动；待机无帧不再被判为故障 |
|
||||
| 本地桥双臂 ROS 发布 | `50 Hz` | 仅合法新会话通过启动门控后向 `/encoder_identical_joint` 发布 |
|
||||
| 本地桥双手 ROS 发布 | `50 Hz` | 与双臂共用同一次武装；BrainCo 指令同频发布 |
|
||||
| 原厂双臂频率转换 | `200 Hz` | 由原厂 `freq_change_tg3_node` 完成同步、滤波并输出 `/arm/cmd` |
|
||||
| BrainCo 状态反馈 | `30 Hz` | 原厂 `brainco_hand.yaml` 的 `publish_hz` |
|
||||
|
||||
这里的 `98~100 Hz` 是现场测量值而非硬实时保证；公网延迟、Hub 负载和系统调度都会造成
|
||||
小幅波动。控制桥以“最新帧”为准，不排队回放旧动作。
|
||||
|
||||
## 4. 数据格式
|
||||
|
||||
### 4.1 xTELE 原始 JSON
|
||||
|
||||
xTELE 每帧发布 UTF-8 JSON。现场帧的结构如下（数值仅作格式示例）：
|
||||
|
||||
```json
|
||||
{
|
||||
"timestamp": 1786018731538,
|
||||
"isomorphic_arm_id": "IArm009027FA8190",
|
||||
"isomorphic_arm_type": "TS1P",
|
||||
"arm": {
|
||||
"position": {
|
||||
"left": [0.021, 0.021, 0.499, -0.153, -0.442, 0.183, 0.060],
|
||||
"right": [-0.021, -0.003, -0.683, -0.206, 0.874, 0.069, -0.160]
|
||||
}
|
||||
},
|
||||
"hand": {"position": {"left": 0.0, "right": 0.0}},
|
||||
"waist": {"position": 0.236},
|
||||
"servo_error": {
|
||||
"left": [0, 0, 0, 0, 0, 0, 0],
|
||||
"right": [0, 0, 0, 0, 0, 0, 0],
|
||||
"waist": [0]
|
||||
},
|
||||
"freq": {"left": 91.405, "right": 91.405, "waist": 91.405},
|
||||
"button": {
|
||||
"left": [0, 0, 0, 0, 0],
|
||||
"right": [0, 0, 0, 0, 0]
|
||||
},
|
||||
"trigger": {"left": 0.0, "right": 0.0},
|
||||
"joycan_error": [0, 0]
|
||||
}
|
||||
```
|
||||
|
||||
原帧还包含 `scroll`、`joystick`、`button_joystick`、`button_wheel`、`vibration_mode`、
|
||||
`total_current` 和 `total_power`。发送端保留 5003 的双臂、按键、诊断等字段，只用 5001
|
||||
同一时刻的处理结果替换 `hand.position`，并增加 `tg3_transport` 来源标记。机器人桥仅消费
|
||||
本项目所需字段：
|
||||
|
||||
- `arm.position.left/right`：左右各 7 个关节，单位为弧度；
|
||||
- `hand.position.left/right`：未选择组合手势时为 `0~1` 扳机开合量；选择手势后可为厂家
|
||||
xTELE 生成的 6 维 BrainCoRevo2 归一化目标；
|
||||
- `button.left/right`：左侧顺序以 X/Y/Z 开头，右侧以 A/B/C 开头；索引 2 即 Z/C；
|
||||
- `servo_error`、`joycan_error` 和 `freq`：启动与运行安全门控；
|
||||
- `isomorphic_arm_id/type`：设备身份校验。
|
||||
|
||||
腰、头、腿和行走数据不由本桥控制。
|
||||
|
||||
### 4.2 OmniSocket 二进制封包
|
||||
|
||||
EAI 在原 JSON 前增加 24 字节网络序头部，格式为 `!4sQQI`：
|
||||
|
||||
| 偏移 | 长度 | 类型 | 含义 |
|
||||
|---:|---:|---|---|
|
||||
| 0 | 4 | bytes | 固定魔数 `TG3A` |
|
||||
| 4 | 8 | uint64 | 单调递增序列号 |
|
||||
| 12 | 8 | uint64 | EAI 发送时间 `time.time_ns()` |
|
||||
| 20 | 4 | uint32 | 后续 JSON 字节长度 |
|
||||
| 24 | 可变 | bytes | 原始 UTF-8 JSON |
|
||||
|
||||
JSON 内的 `tg3_transport` 由 EAI sender 强制覆盖，包含协议版本 `2`、128-bit 随机
|
||||
`session_id`、会话内递增 `session_seq` 和 `session_state=start|active|stop`。START
|
||||
连续发送 50 个源帧，机器人对每个 ID 只执行一次启动安全检查；STOP 是最后一个业务帧。
|
||||
旧 ID、缺少 START 的 ACTIVE、错 ID 的 STOP 或伪造的源 metadata 均不能武装机器人。
|
||||
|
||||
单帧 JSON 上限为 1 MiB。机器人端依次检查发送 Peer、消息类型、魔数、长度、JSON
|
||||
结构、设备 ID、序列号和包龄。由于 EAI 与机器人系统时钟可能存在固定偏差，包龄使用
|
||||
“当前包原始年龄减去本会话最小年龄”的相对值；额外排队超过 `300 ms` 的包会被丢弃。
|
||||
|
||||
### 4.3 ROS 双臂格式
|
||||
|
||||
机器人桥发布 `sensor_msgs/msg/JointState`：
|
||||
|
||||
- Topic：`/encoder_identical_joint`；
|
||||
- `header.frame_id`：`tg3_local_teleop`；
|
||||
- `name`：`left_joints_0..6`、`right_joints_0..6`；
|
||||
- `position`：14 个弧度值，先左臂后右臂；
|
||||
- `velocity`：14 个零值，保持原厂位置控制路径。
|
||||
|
||||
原厂节点继续负责转换为电机 11..17、21..27 的 `/arm/cmd`。
|
||||
|
||||
### 4.4 BrainCo 双手格式
|
||||
|
||||
双手分别发布：
|
||||
|
||||
- `/left_hand/set_motor_multi`；
|
||||
- `/right_hand/set_motor_multi`；
|
||||
- 类型：`brainco_hand_msgs/msg/SetMotorMulti`；
|
||||
- `mode=5`（位置 + 期望时间）；
|
||||
- `positions`：6 个 `1~1000` 位置值；
|
||||
- `durations`：6 个 `800 ms`；
|
||||
- `speeds/currents/pwms`：均为 0，由原厂位置+时间模式处理。
|
||||
|
||||
一维扳机值映射如下：
|
||||
|
||||
| 扳机 | 含义 | 六电机目标 |
|
||||
|---:|---|---|
|
||||
| `0` | 现场实测默认打开位 | `[401, 401, 51, 51, 51, 51]` |
|
||||
| `1` | 保守常规闭合抓握 | `[900, 500, 451, 520, 520, 451]` |
|
||||
|
||||
中间值线性插值。手部从机器人实测位置开始，每秒最多变化 400 个位置单位。BrainCo
|
||||
状态 `0=空闲`、`1=运动`、`2=接触/堵转或到限位`、`3=持续力` 均为原厂正常状态；
|
||||
仅状态失联或未知/非法值会触发本地停止，原厂电流、堵转和限位保护保持不变。
|
||||
|
||||
xTELE 已注册的手势组合键由工控机原程序负责短按/长按、互斥和手势编号判定。选中手势后，
|
||||
5001 的 `hand.position.left/right` 会变为 6 维数组，机器人桥直接换算为 BrainCo 的
|
||||
`1~1000` 位置值；本项目不再在机器人端复刻组合键状态机。5002 的日志、帮助、RGB/D、
|
||||
关节浮层等 `webui` 事件不发送到机器人；腰和头不接入本地桥。HBWALK 行走使用 5003
|
||||
传输的原始右 C 与左摇杆；遥操开启后按键和摇杆在下一个 50 Hz 周期即时解析并发布
|
||||
`geometry_msgs/msg/TwistStamped` 到 `/hric/robot/cmd_vel`，不再叠加第二个 3 秒计时。
|
||||
|
||||
## 5. 启停、回 Home 与安全逻辑
|
||||
|
||||
### 5.1 操作方式
|
||||
|
||||
1. 机器人进入 `HBWALK`，状态为 `running`；
|
||||
2. 两个扳机先松开；
|
||||
3. 同时长按左 Z + 右 C 3 秒，EAI 生成 START 会话并开始发送，机器人通过启动门控后
|
||||
启动双臂和双手跟随；
|
||||
4. 左扳机控制左手，右扳机控制右手，松开为张开、扣下为闭合；
|
||||
5. 行走时按住右 C 并推出左摇杆即刻响应；上下为前后、左右为转向；
|
||||
6. 松开 C 或摇杆回中立即停走，再次操作无需重新等待；
|
||||
7. 再次长按左 Z + 右 C 3 秒发送 STOP；STOP 后 EAI 不再发送 xTELE 业务数据；
|
||||
8. 停止后双臂以 `0.25 rad/s` 限速返回现场保存的 Home 姿态，手保持最后目标。
|
||||
|
||||
行走以 50 Hz 发布，死区为 0.2，二次曲线，并按官方半身行走 Topic 范围限制为前进
|
||||
`1.0 m/s`、后退 `0.8 m/s`、转向 `0.8 rad/s`；
|
||||
停止时立即发零速并补发 10 帧。已安装的 xTELE 0.1.2 没有注册右侧同手 `C+A`，所以
|
||||
本地桥也不猜测其含义、不对它执行状态切换。
|
||||
|
||||
服务随系统自动启动只代表“等待数据并监测”，不会自动武装，也不会绕过物理长按。
|
||||
|
||||
### 5.2 本地门控
|
||||
|
||||
启动前要求：
|
||||
|
||||
- 当前状态和子状态均为 `HBWALK`，运行状态为 `running`；
|
||||
- xTELE 数据新鲜，设备 ID/type 正确，采样频率不低于 30 Hz；
|
||||
- 同构臂 CAN、双臂电机反馈无错误；
|
||||
- BrainCo 双手状态反馈新鲜且完整；
|
||||
- 启动瞬间 14 个同构臂目标位于天工 3.0 文档关节范围内；
|
||||
- 没有检测到第二路云端/外部手臂或灵巧手命令源。
|
||||
|
||||
自定义关节目标限位只在启动时检查；采集/跟随期间不重复增加该限位。运行时仍持续检查
|
||||
HBWALK、数据失联、CAN/电机错误和命令源冲突，原厂默认保护没有修改。
|
||||
|
||||
首次启动时，双臂限速器从机器人实时关节位置开始，双手限速器从实时手指位置开始，
|
||||
避免第一帧直接跳到同构臂目标。
|
||||
|
||||
### 5.3 断线处理
|
||||
|
||||
- 活动会话的同构臂输入超过 `0.25 s` 未更新：立即解除武装并停止发布；
|
||||
- 活动会话超过 `2 s` 仍没有新帧才重建机器人接收进程；待机或 STOP 后无业务帧是正常状态；
|
||||
- 只有匹配的操作员 STOP 触发自动 Home，意外断网只停控；Home 继续依赖机器人反馈，
|
||||
不再依赖已经停止的 EAI 数据；
|
||||
- 公网 Hub 重启后，建议重启 EAI 发送服务以清空 KCP 旧队列：
|
||||
|
||||
```bash
|
||||
systemctl --user restart tg3-omnisocket-sender.service
|
||||
```
|
||||
|
||||
本次服务器恢复后重启发送服务，KCP 状态恢复为反馈约 6 ms、发送队列 0；机器人随后恢复
|
||||
约 100 Hz 收包并通过安全门控。
|
||||
|
||||
## 6. EAI 工控机上完成的操作
|
||||
|
||||
运行账号：`eai`
|
||||
|
||||
1. 保留原厂目录 `/home/eai/dev/sysEAI`、`xtele_main.py`、TS1P 标定和
|
||||
`/home/eai/.config/xhumanoid/xtele/default.toml`，未修改原厂控制源码；
|
||||
2. 确认系统级 `xtele-monitor.service` 已启用并运行，它调用原有
|
||||
`/home/eai/dev/sysEAI/start_xtele.sh`；
|
||||
3. 部署 OmniSocketGo Python 运行库到 `/home/eai/OmniSocketGo`；
|
||||
4. 新增 `/home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py`：
|
||||
- 订阅 `tcp://127.0.0.1:5003` 原始帧；
|
||||
- 订阅 `tcp://127.0.0.1:5001` 处理后命令，只提取双侧 `hand.position`；
|
||||
- 在 EAI 本机用 Z+C 连续 3 秒生成唯一 START/STOP 会话；待机仍读本地数据但不上公网；
|
||||
- 保留 5003 的双臂、按键、摇杆和 CAN/舵机诊断，用 5001 的厂家手势目标覆盖双手字段；
|
||||
- 只保留最新帧；
|
||||
- 添加 `TG3A` 头部；
|
||||
- 仅在 START 到 STOP 之间发送到机器人 Peer，并写入不可伪造的 protocol/session 元数据；
|
||||
- 输出 `status.json` 统计；
|
||||
5. 新增并启用用户服务
|
||||
`/home/eai/.config/systemd/user/tg3-omnisocket-sender.service`，设置
|
||||
`Restart=always`；
|
||||
6. 服务器恢复后重启上述发送服务，清除了断线期间 KCP 积压；
|
||||
7. 删除的仅是此前由本项目写入、现已废弃的接收代理/旧服务；未删除任何原厂代码。
|
||||
|
||||
常用检查命令：
|
||||
|
||||
```bash
|
||||
systemctl status xtele-monitor.service
|
||||
systemctl --user status tg3-omnisocket-sender.service
|
||||
cat /home/eai/tg3_omnisocket_transport/status.json
|
||||
```
|
||||
|
||||
## 7. 机器人上完成的操作
|
||||
|
||||
运行账号：`nvidia`，地址：`192.168.41.2`
|
||||
|
||||
1. 部署 OmniSocketGo Python 运行库到 `/home/nvidia/OmniSocketGo`；
|
||||
2. 新增 `/home/nvidia/tg3_local_teleop`，主要文件为：
|
||||
- `tg3_local_teleop.py`：OmniSocket 接收、校验、ROS 双臂/双手发布、安全门控、
|
||||
断线看门狗和回 Home；
|
||||
- `config.toml`：Peer、频率、话题、关节范围、手部映射和 Home 姿态；
|
||||
- `run.sh`、`status.sh`、`home.sh`：运行、状态和回 Home 命令；
|
||||
- `status.json`：实时状态；
|
||||
3. 新增并启用用户服务
|
||||
`/home/nvidia/.config/systemd/user/tg3-local-teleop.service`；
|
||||
4. 将 OmniSocket 接收直接集成进 `tg3_local_teleop`，不再经过机器人本机 ZMQ/TCP
|
||||
接收代理；
|
||||
5. 删除本项目此前创建的旧 `tg3-omnisocket-receiver.service` 和无用代理代码；
|
||||
6. 接入原厂 `/encoder_identical_joint` 和 `freq_change_tg3_node` 完成双臂控制；
|
||||
7. 根据机器人实际配置 `left_hand_type/right_hand_type=brainco`，接入 BrainCo
|
||||
`/set_motor_multi` 与 `/motor_status` 完成双手控制；未修改原厂手型配置；
|
||||
8. 保存当前真实双臂姿态为 Home，而不是把 14 个关节归到电机零位；
|
||||
9. 加入服务器断线后的 2 秒进程级自动重连看门狗；
|
||||
10. 历次部署前均在未武装状态检查，旧版本备份只包含本项目文件；未删除或覆盖原厂程序。
|
||||
|
||||
常用检查命令：
|
||||
|
||||
```bash
|
||||
systemctl --user status tg3-local-teleop.service
|
||||
cat /home/nvidia/tg3_local_teleop/status.json
|
||||
|
||||
cd /home/nvidia/tg3_local_teleop
|
||||
./home.sh status
|
||||
./home.sh start
|
||||
./home.sh cancel
|
||||
```
|
||||
|
||||
`home.sh start` 会驱动机器人双臂，执行前必须确认防护、活动空间和急停。
|
||||
|
||||
## 8. 当前保留与未修改内容
|
||||
|
||||
- 原厂 xTELE、TS1P 串口/CAN、按键和标定逻辑未修改；
|
||||
- 原厂 `freq_change_tg3_node`、`/arm/cmd` 及其默认限位未修改；
|
||||
- 原厂 BrainCo 驱动、手型配置、电流/堵转/限位保护未修改；
|
||||
- 原有代码没有删除；清理范围仅限本项目早期写入的旧代理和旧服务；
|
||||
- 本地开发电脑没有部署运行服务，不是遥操链路中的单点依赖。
|
||||
|
||||
## 9. 验收结论与参考资料
|
||||
|
||||
现场已完成以下验证：
|
||||
|
||||
- 待机时 EAI 本地 `frames_received` 增长而 `frames_sent` 保持不变；START 后机器人稳定
|
||||
收到约 100 Hz 数据，STOP 后再次归零；
|
||||
- 左右 Z/C 长按启停正常，服务不会自行武装；
|
||||
- 双臂在 HBWALK 下正常跟随，停止后按限速逻辑返回保存的 Home；
|
||||
- 左右扳机分别输出 `0~1`，双灵巧手开合、限速和 BrainCo 运动状态处理正常；
|
||||
- 工控机 5001 双手语义帧已通过 OmniSocket 合并发送，实测无畸形帧、无字段丢失；
|
||||
- 服务器断开时机器人停止发布，服务器恢复并重建会话后可继续工作。
|
||||
|
||||
参考资料：
|
||||
|
||||
- 用户提供的《具身天工 3.0 + SDK + 文档（26.04.x）》；
|
||||
- 机器人已安装的 `brainco_hand_msgs`、`brainco_hand.yaml` 和 BrainCo 示例；
|
||||
- [BrainCo Hand 官方 SDK 文档](https://www.brainco-hz.com/docs/revolimb-hand/revo2/get_sdk.html)；
|
||||
- `/home/eai/OmniSocketGo/README.md` 及本项目实际运行统计；
|
||||
- 本项目 `omnisocket_xtele_sender.py`、`tg3_local_teleop.py` 和 `config.toml`。
|
||||
|
||||
后续迁移步骤、IP/Peer 修改表和新机器人验收流程见
|
||||
《天工3.0本地同构臂遥操迁移部署指南.md》。
|
||||
185
tg3_local_teleop/README.md
Normal file
185
tg3_local_teleop/README.md
Normal file
@@ -0,0 +1,185 @@
|
||||
# 天工 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（右 C + 左摇杆；50 Hz TwistStamped）
|
||||
-> 天工 3.0 HBWALK 行走
|
||||
```
|
||||
|
||||
## 自动运行与操作
|
||||
|
||||
- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
|
||||
- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
|
||||
本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
|
||||
- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
|
||||
机器人立即停控并以 `0.25 rad/s` 自动回 Home。
|
||||
- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
|
||||
- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
|
||||
前后/转向；不控制腰和头。
|
||||
- 行走：遥操已启动后，右 `C` 是即时 deadman；按住 C 并把左摇杆推出死区后，在下一个
|
||||
`50 Hz` 周期立即响应，不再等待 3 秒。松开 C 或摇杆回中立即发零速，再按也立即恢复。
|
||||
左摇杆上下控制前后，左右控制原地转向。
|
||||
按官方半身行走 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`。
|
||||
- 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
|
||||
在不清楚厂商语义的情况下把它擅自绑定为状态切换。
|
||||
- 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
|
||||
BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
|
||||
若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
|
||||
|
||||
## 安全门控
|
||||
|
||||
只有以下条件全部满足才允许开始：机器人 `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`、网络数据新鲜且右 C + 左摇杆
|
||||
即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
|
||||
回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
|
||||
|
||||
灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
|
||||
位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
|
||||
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态、同构臂失联
|
||||
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
|
||||
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
|
||||
|
||||
## 限速双臂回 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
|
||||
接收代理。若需回退到机器人与工控机的局域网直连，可把 `network.transport` 改成
|
||||
`zmq`；保留的 `iarm_endpoint` 为 `tcp://192.168.5.14:5003`。
|
||||
|
||||
遥操活动期间公网输入超过 `0.25 s` 未更新会立即解除武装并停止发布，同一会话不能自动
|
||||
重新武装；活动会话超过 `2 s` 仍无帧时才重建机器人 OmniSocket 进程。未 START 或收到
|
||||
STOP 后没有业务帧是正常待机，不触发反复重启。只有匹配的操作员 STOP 自动回 Home；
|
||||
意外断网只停控，不自动产生回位运动。
|
||||
|
||||
## 完整重启顺序
|
||||
|
||||
只重启本项目的网络与机器人桥时，先启动 Nvidia 接收端，再启动 EAI 本地门控服务；
|
||||
EAI 在收到物理 START 前不会建立发送 Session：
|
||||
|
||||
```bash
|
||||
ssh nvidia@192.168.41.2 'systemctl --user restart 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
|
||||
```
|
||||
119
tg3_local_teleop/config.toml
Normal file
119
tg3_local_teleop/config.toml
Normal file
@@ -0,0 +1,119 @@
|
||||
[network]
|
||||
# Direct OmniSocket KCP input. No robot-side ZMQ receiving proxy is used.
|
||||
transport = "omnisocket"
|
||||
omnisocket_server = "175.178.116.187:14049"
|
||||
omnisocket_peer_id = "tg3-009027fa8190-robot"
|
||||
omnisocket_expected_sender = "tg3-009027fa8190-iarm"
|
||||
omnisocket_max_packet_age_ms = 300.0
|
||||
# The native session can remain blocked on an old connection after the public
|
||||
# Hub disappears. Publication stops at source_timeout_s; after this longer
|
||||
# interval the process exits and systemd creates a completely fresh session.
|
||||
omnisocket_restart_after_stale_s = 2.0
|
||||
# Retained only as a direct-LAN fallback when transport is changed to "zmq".
|
||||
iarm_endpoint = "tcp://192.168.5.14:5003"
|
||||
expected_iarm_id = "IArm009027FA8190"
|
||||
expected_iarm_type = "TS1P"
|
||||
source_timeout_s = 0.25
|
||||
minimum_arm_frequency_hz = 30.0
|
||||
|
||||
[ros]
|
||||
command_topic = "/encoder_identical_joint"
|
||||
rl_state_topic = "/hric/robot/rl_state"
|
||||
arm_state_topic = "/freq_change/arm_status"
|
||||
home_service = "/tg3_local_teleop/return_home"
|
||||
cancel_home_service = "/tg3_local_teleop/cancel_home"
|
||||
publish_rate_hz = 50.0
|
||||
|
||||
[robot]
|
||||
required_state = "HBWALK"
|
||||
required_status = "running"
|
||||
state_timeout_s = 1.0
|
||||
arm_state_timeout_s = 0.25
|
||||
|
||||
[control]
|
||||
# OmniSocket start/stop is timed on EAI before any business frame is sent.
|
||||
# This value remains for the direct-ZMQ fallback latch.
|
||||
auto_home_on_stop = true
|
||||
start_stop_hold_seconds = 3.0
|
||||
max_slew_rad_s = 1.0
|
||||
# This custom margin is checked only when teleoperation starts. It is not
|
||||
# checked while collecting/following. Vendor limit protection is unchanged.
|
||||
joint_limit_margin_rad = 0.03
|
||||
|
||||
# Joint order: left 11..17, then right 21..27. Values are the TG3.0 limits
|
||||
# from the supplied 26.04.x SDK document, expressed in radians.
|
||||
joint_lower_rad = [
|
||||
-2.8449, -0.1920, -2.8449, -2.5482, -2.8449, -1.3265, -1.3265,
|
||||
-2.8449, -3.3336, -2.8449, -2.5482, -2.8449, -1.3265, -1.3265,
|
||||
]
|
||||
joint_upper_rad = [
|
||||
2.8449, 3.3336, 2.8449, 0.1920, 2.8449, 1.3265, 1.3265,
|
||||
2.8449, 0.1920, 2.8449, 0.1920, 2.8449, 1.3265, 1.3265,
|
||||
]
|
||||
|
||||
[locomotion]
|
||||
# Right C remains an immediate deadman: while teleoperation is active, C plus
|
||||
# a left-stick command acts on the next 50 Hz tick. There is no second hold.
|
||||
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
|
||||
# TianGong secondary-development /cmd_vel limits for full/half-body walking:
|
||||
# linear.x [-0.8, +1.0] m/s and angular.z [-0.8, +0.8] rad/s.
|
||||
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
|
||||
|
||||
[hands]
|
||||
# TianGong 3.0 on this robot uses BrainCo Revo2 hands. Commands are published
|
||||
# only while the same physical 3-second teleoperation latch is armed.
|
||||
enabled = true
|
||||
left_command_topic = "/left_hand/set_motor_multi"
|
||||
right_command_topic = "/right_hand/set_motor_multi"
|
||||
left_status_topic = "/left_hand/motor_status"
|
||||
right_status_topic = "/right_hand/motor_status"
|
||||
status_timeout_s = 0.25
|
||||
|
||||
# Vendor robot_tele_server uses mode 5 and an 800 ms expected duration.
|
||||
mode = 5
|
||||
duration_ms = 800
|
||||
position_min = 1
|
||||
position_max = 1000
|
||||
# Commissioning limit: no commanded motor may change faster than 400/1000 of
|
||||
# its full stroke per second. The driver keeps its own current/stall protection.
|
||||
slew_units_per_s = 400.0
|
||||
invert_scalar = false
|
||||
|
||||
# Scalar hand.position=0 is the robot's measured default-open pose. The closed
|
||||
# endpoint is the conservative BrainCoRevo2 "normal" grasp from the installed
|
||||
# xTELE GestureController.
|
||||
# Order: thumb bend, thumb rotation, index, middle, ring, little.
|
||||
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]
|
||||
|
||||
[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,
|
||||
# followed by 21..27; values are radians. Home is not the motor-zero pose.
|
||||
slew_rad_s = 0.25
|
||||
joint_goal_rad = [
|
||||
0.1842578799, 0.1234146282, 0.0396494754, -0.4828015864,
|
||||
-0.0024294013, 0.0065013529, -0.0040423824,
|
||||
0.1721130311, -0.1383204311, -0.0142812012, -0.3408825397,
|
||||
0.0026911981, 0.0039934102, -0.0030838961,
|
||||
]
|
||||
# Abort if the commanded trajectory gets more than ~4.6 degrees ahead of
|
||||
# measured robot motion.
|
||||
max_tracking_error_rad = 0.08
|
||||
command_tolerance_rad = 0.005
|
||||
actual_tolerance_rad = 0.03
|
||||
settle_s = 1.0
|
||||
timeout_s = 30.0
|
||||
31
tg3_local_teleop/home.sh
Executable file
31
tg3_local_teleop/home.sh
Executable file
@@ -0,0 +1,31 @@
|
||||
#!/usr/bin/env bash
|
||||
set -eo pipefail
|
||||
|
||||
APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
if [[ -f /home/nvidia/xos/setup.bash ]]; then
|
||||
source /home/nvidia/xos/setup.bash
|
||||
fi
|
||||
if [[ -f /opt/robot_tele_server/install/setup.bash ]]; then
|
||||
source /opt/robot_tele_server/install/setup.bash
|
||||
fi
|
||||
if [[ -f "$APP_DIR/ros2_py/install/setup.bash" ]]; then
|
||||
source "$APP_DIR/ros2_py/install/setup.bash"
|
||||
fi
|
||||
set -u
|
||||
|
||||
case "${1:-status}" in
|
||||
start)
|
||||
ros2 service call /tg3_local_teleop/return_home std_srvs/srv/Trigger "{}"
|
||||
;;
|
||||
cancel|stop)
|
||||
ros2 service call /tg3_local_teleop/cancel_home std_srvs/srv/Trigger "{}"
|
||||
;;
|
||||
status)
|
||||
"$APP_DIR/status.sh"
|
||||
;;
|
||||
*)
|
||||
echo "usage: $0 {start|cancel|status}" >&2
|
||||
exit 2
|
||||
;;
|
||||
esac
|
||||
15
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt
Normal file
15
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt
Normal file
@@ -0,0 +1,15 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(ros2_bridge_msgs)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/MotorStatus.msg"
|
||||
"msg/ArmStatus.msg"
|
||||
DEPENDENCIES std_msgs
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@@ -0,0 +1,4 @@
|
||||
std_msgs/Header header
|
||||
uint8 label
|
||||
uint8 reserved
|
||||
ros2_bridge_msgs/MotorStatus[] status
|
||||
@@ -0,0 +1,7 @@
|
||||
uint16 name
|
||||
float64 pos
|
||||
float64 speed
|
||||
float64 current
|
||||
float64 temperature
|
||||
float64 mos_temperature
|
||||
uint32 error
|
||||
13
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml
Normal file
13
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml
Normal file
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>ros2_bridge_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Python bindings for the installed TG3 ArmStatus wire type.</description>
|
||||
<maintainer email="nvidia@example.com">nvidia</maintainer>
|
||||
<license>Proprietary</license>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<depend>std_msgs</depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
</package>
|
||||
26
tg3_local_teleop/run.sh
Executable file
26
tg3_local_teleop/run.sh
Executable file
@@ -0,0 +1,26 @@
|
||||
#!/usr/bin/env bash
|
||||
# ROS setup scripts probe optional variables, so enable nounset only after
|
||||
# sourcing the overlays.
|
||||
set -eo pipefail
|
||||
|
||||
APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
|
||||
if [[ -f /home/nvidia/xos/setup.bash ]]; then
|
||||
source /home/nvidia/xos/setup.bash
|
||||
fi
|
||||
if [[ -f /opt/robot_tele_server/install/setup.bash ]]; then
|
||||
source /opt/robot_tele_server/install/setup.bash
|
||||
fi
|
||||
if [[ -f "$APP_DIR/ros2_py/install/setup.bash" ]]; then
|
||||
source "$APP_DIR/ros2_py/install/setup.bash"
|
||||
fi
|
||||
if [[ -d /home/nvidia/OmniSocketGo/python ]]; then
|
||||
export PYTHONPATH="/home/nvidia/OmniSocketGo/python${PYTHONPATH:+:$PYTHONPATH}"
|
||||
fi
|
||||
set -u
|
||||
|
||||
exec python3 "$APP_DIR/tg3_local_teleop.py" \
|
||||
--config "$APP_DIR/config.toml" \
|
||||
--status-file "$APP_DIR/status.json" \
|
||||
"$@"
|
||||
9
tg3_local_teleop/status.sh
Executable file
9
tg3_local_teleop/status.sh
Executable file
@@ -0,0 +1,9 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
if [[ ! -f "$APP_DIR/status.json" ]]; then
|
||||
echo "status.json does not exist; the bridge has not produced status yet"
|
||||
exit 1
|
||||
fi
|
||||
python3 -m json.tool "$APP_DIR/status.json"
|
||||
217
tg3_local_teleop/test_session_gate.py
Normal file
217
tg3_local_teleop/test_session_gate.py
Normal file
@@ -0,0 +1,217 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Offline protocol tests for the robot-side session gate."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
from pathlib import Path
|
||||
from types import MethodType, ModuleType
|
||||
import sys
|
||||
import unittest
|
||||
|
||||
|
||||
class Dummy:
|
||||
pass
|
||||
|
||||
|
||||
def install_module(name: str, **attributes: object) -> ModuleType:
|
||||
module = ModuleType(name)
|
||||
for key, value in attributes.items():
|
||||
setattr(module, key, value)
|
||||
sys.modules[name] = module
|
||||
return module
|
||||
|
||||
|
||||
install_module("rclpy")
|
||||
install_module("rclpy.node", Node=Dummy)
|
||||
for package, names in {
|
||||
"brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"),
|
||||
"diagnostic_msgs.msg": ("DiagnosticStatus",),
|
||||
"geometry_msgs.msg": ("TwistStamped",),
|
||||
"ros2_bridge_msgs.msg": ("ArmStatus",),
|
||||
"sensor_msgs.msg": ("JointState",),
|
||||
"std_srvs.srv": ("Trigger",),
|
||||
}.items():
|
||||
install_module(package, **{name: Dummy for name in names})
|
||||
|
||||
module_path = Path(__file__).with_name("tg3_local_teleop.py")
|
||||
spec = importlib.util.spec_from_file_location("tg3_local_teleop_module", module_path)
|
||||
assert spec is not None and spec.loader is not None
|
||||
bridge_module = importlib.util.module_from_spec(spec)
|
||||
sys.modules[spec.name] = bridge_module
|
||||
spec.loader.exec_module(bridge_module)
|
||||
ArmSnapshot = bridge_module.ArmSnapshot
|
||||
LocalTeleopBridge = bridge_module.LocalTeleopBridge
|
||||
|
||||
|
||||
class NullLogger:
|
||||
def info(self, _message: str) -> None:
|
||||
pass
|
||||
|
||||
def warning(self, _message: str) -> None:
|
||||
pass
|
||||
|
||||
def error(self, _message: str) -> None:
|
||||
pass
|
||||
|
||||
|
||||
def sample(session_id: str, seq: int, state: str, reason: str = "") -> object:
|
||||
metadata: dict[str, object] = {
|
||||
"protocol_version": 2,
|
||||
"session_id": session_id,
|
||||
"session_seq": seq,
|
||||
"session_state": state,
|
||||
}
|
||||
if reason:
|
||||
metadata["stop_reason"] = reason
|
||||
return ArmSnapshot({"tg3_transport": metadata}, received_at=10.0)
|
||||
|
||||
|
||||
class RobotSessionGateTest(unittest.TestCase):
|
||||
def make_bridge(self) -> object:
|
||||
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
||||
bridge.last_session_state = "inactive"
|
||||
bridge.last_session_stop_reason = ""
|
||||
bridge.active_session_id = None
|
||||
bridge.session_start_attempted_id = None
|
||||
bridge.armed = False
|
||||
bridge.returning_home = False
|
||||
bridge.allow_publish = True
|
||||
bridge.cfg = {"control": {"auto_home_on_stop": True}}
|
||||
bridge.hands_enabled = False
|
||||
bridge.robot_arm_positions = [0.0] * 14
|
||||
bridge.last_command = None
|
||||
bridge.last_publish_at = 0.0
|
||||
bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
|
||||
bridge._safety_reasons = MethodType(
|
||||
lambda _self, _now, _sample, _error: [], bridge
|
||||
)
|
||||
|
||||
def disarm(self: object, _reason: str) -> None:
|
||||
self.armed = False
|
||||
|
||||
bridge._disarm = MethodType(disarm, bridge)
|
||||
bridge.home_started = False
|
||||
|
||||
def start_home(
|
||||
self: object,
|
||||
_now: float,
|
||||
_sample: object,
|
||||
_error: str,
|
||||
_reason: str,
|
||||
) -> bool:
|
||||
self.home_started = True
|
||||
return True
|
||||
|
||||
bridge._start_home_if_safe = MethodType(start_home, bridge)
|
||||
return bridge
|
||||
|
||||
def test_start_active_stop_is_session_scoped(self) -> None:
|
||||
bridge = self.make_bridge()
|
||||
session_id = "a" * 32
|
||||
bridge._update_session_gate(10.0, sample(session_id, 1, "start"), "")
|
||||
self.assertTrue(bridge.armed)
|
||||
self.assertEqual(bridge.active_session_id, session_id)
|
||||
|
||||
bridge._update_session_gate(10.1, sample(session_id, 2, "active"), "")
|
||||
self.assertTrue(bridge.armed)
|
||||
bridge._update_session_gate(
|
||||
10.2, sample("b" * 32, 3, "stop", "operator"), ""
|
||||
)
|
||||
self.assertTrue(bridge.armed)
|
||||
|
||||
bridge._update_session_gate(
|
||||
10.3, sample(session_id, 4, "stop", "operator"), ""
|
||||
)
|
||||
self.assertFalse(bridge.armed)
|
||||
self.assertIsNone(bridge.active_session_id)
|
||||
self.assertTrue(bridge.home_started)
|
||||
|
||||
def test_active_without_start_never_arms(self) -> None:
|
||||
bridge = self.make_bridge()
|
||||
bridge._update_session_gate(10.0, sample("c" * 32, 1, "active"), "")
|
||||
self.assertFalse(bridge.armed)
|
||||
self.assertIsNone(bridge.active_session_id)
|
||||
|
||||
def test_rejected_start_is_not_retried_in_same_session(self) -> None:
|
||||
bridge = self.make_bridge()
|
||||
calls = 0
|
||||
|
||||
def reject(_self: object, _now: float, _sample: object, _error: str) -> list[str]:
|
||||
nonlocal calls
|
||||
calls += 1
|
||||
return ["blocked"]
|
||||
|
||||
bridge._safety_reasons = MethodType(reject, bridge)
|
||||
session_id = "d" * 32
|
||||
bridge._update_session_gate(10.0, sample(session_id, 1, "start"), "")
|
||||
bridge._update_session_gate(10.1, sample(session_id, 2, "start"), "")
|
||||
bridge._update_session_gate(10.2, sample(session_id, 3, "active"), "")
|
||||
self.assertEqual(calls, 1)
|
||||
self.assertFalse(bridge.armed)
|
||||
|
||||
def test_metadata_validation(self) -> None:
|
||||
valid = {
|
||||
"tg3_transport": {
|
||||
"protocol_version": 2,
|
||||
"session_id": "e" * 32,
|
||||
"session_seq": 1,
|
||||
"session_state": "start",
|
||||
}
|
||||
}
|
||||
self.assertIsNotNone(LocalTeleopBridge._teleop_session_info(valid))
|
||||
valid["tg3_transport"]["protocol_version"] = 1
|
||||
self.assertIsNone(LocalTeleopBridge._teleop_session_info(valid))
|
||||
|
||||
def test_locomotion_reacts_immediately_after_repress(self) -> None:
|
||||
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
||||
bridge.locomotion_cfg = {
|
||||
"hold_seconds": 0.0,
|
||||
"joystick_deadzone": 0.2,
|
||||
"joystick_expo": 2.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_combo_started_at = None
|
||||
bridge.walk_active = False
|
||||
bridge.walk_command = [0.0, 0.0]
|
||||
bridge.walk_zero_frames_remaining = 0
|
||||
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)
|
||||
|
||||
moving = ArmSnapshot(
|
||||
{
|
||||
"button": {"right": [False, False, True]},
|
||||
"joystick": {"left": [1.0, 0.0]},
|
||||
},
|
||||
received_at=1.0,
|
||||
)
|
||||
released = ArmSnapshot(
|
||||
{
|
||||
"button": {"right": [False, False, False]},
|
||||
"joystick": {"left": [1.0, 0.0]},
|
||||
},
|
||||
received_at=1.1,
|
||||
)
|
||||
bridge._tick_locomotion(1.0, moving)
|
||||
self.assertEqual(published[-1], (1.0, -0.0))
|
||||
bridge._tick_locomotion(1.1, released)
|
||||
self.assertEqual(published[-1], (0.0, 0.0))
|
||||
bridge._tick_locomotion(1.2, moving)
|
||||
self.assertEqual(published[-1], (1.0, -0.0))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
15
tg3_local_teleop/tg3-local-teleop.service
Normal file
15
tg3_local_teleop/tg3-local-teleop.service
Normal file
@@ -0,0 +1,15 @@
|
||||
[Unit]
|
||||
Description=TG3 session-gated local TS1P teleoperation bridge
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
ExecStart=/home/nvidia/tg3_local_teleop/run.sh --allow-publish
|
||||
Restart=on-failure
|
||||
RestartSec=2
|
||||
KillSignal=SIGINT
|
||||
TimeoutStopSec=5
|
||||
|
||||
[Install]
|
||||
WantedBy=default.target
|
||||
1523
tg3_local_teleop/tg3_local_teleop.py
Executable file
1523
tg3_local_teleop/tg3_local_teleop.py
Executable file
File diff suppressed because it is too large
Load Diff
53
tg3_omnisocket_transport/README.md
Normal file
53
tg3_omnisocket_transport/README.md
Normal file
@@ -0,0 +1,53 @@
|
||||
# TG3 OmniSocket 同构臂传输代理
|
||||
|
||||
链路：
|
||||
|
||||
```text
|
||||
EAI xTELE tcp://127.0.0.1:5003（原始帧）
|
||||
+ tcp://127.0.0.1:5001（处理后的双手目标）
|
||||
-> 仅将 5001 hand.position 合并到 5003
|
||||
-> tg3-omnisocket-sender
|
||||
-> KCP Hub 175.178.116.187:14049
|
||||
-> tg3_local_teleop（直接 OmniSocket Session）
|
||||
```
|
||||
|
||||
Peer ID：
|
||||
|
||||
- 工控机：`tg3-009027fa8190-iarm`
|
||||
- 机器人：`tg3-009027fa8190-robot`
|
||||
|
||||
EAI 上仅保留 `omnisocket_xtele_sender.py`。它以 5003 最新帧为基础，保留双臂、
|
||||
Z+C 按键及诊断字段；若 5001 的双侧 `hand.position` 在 250 ms 内有效，则合并厂家
|
||||
xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和腰、头、腿、行走、
|
||||
底盘命令不会进入机器人双臂桥。
|
||||
|
||||
公网业务数据由 EAI 本地会话门控：服务启动后仍持续读取本机 5003/5001，但不发送
|
||||
xTELE 帧；连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_id` 并开始发送。
|
||||
再次长按 3 秒时发送最后一帧 `stop`，等待有界 KCP 刷新后关闭 OmniSocket Session；
|
||||
之后 `frames_sent/bytes_sent` 不再增长，待机既没有 xTELE 业务帧，也没有该 sender 的
|
||||
底层注册/心跳。即使 Hub 离线，sender 仍持续读取本机 xTELE 并保持会话关闭；启动连接
|
||||
失败会使本次会话失效，必须松开组合键后重新长按，不会在 Hub 恢复时续发旧动作。
|
||||
活动期间若 KCP 反馈超过 `500 ms` 未更新，或 `snd_queue+snd_buffer` 超过 100 帧，sender
|
||||
立即作废会话并关闭 Session，从源头丢弃待发队列，防止网络恢复后回放旧动作。
|
||||
|
||||
每个业务 JSON 的 `tg3_transport` 由 sender 强制覆盖，不能由 5003 输入伪造：
|
||||
|
||||
```json
|
||||
{
|
||||
"protocol_version": 2,
|
||||
"session_id": "32位十六进制随机值",
|
||||
"session_seq": 1,
|
||||
"session_state": "start | active | stop",
|
||||
"stop_reason": "operator"
|
||||
}
|
||||
```
|
||||
|
||||
`start` 连续发送 50 个源帧，避免接收端只取最新帧时错过唯一启动事件；机器人对同一
|
||||
`session_id` 只做一次启动安全检查。`stop` 是该会话最后一个业务帧。服务重启、源数据
|
||||
失联或网络错误都会使会话失效，恢复后必须先松开组合键，再重新长按 3 秒。
|
||||
`tg3_local_teleop` 内部直接拒绝非预期发送端、
|
||||
乱序、格式错误以及相对本次连接最低包龄额外排队超过 300 ms 的数据；相对包龄会消除
|
||||
两端系统时钟的固定偏差。遥操桥仍有 250 ms 输入失联保护。
|
||||
|
||||
机器人直接在 `tg3_local_teleop` 中接收 OmniSocket 数据，不再安装或运行单独的
|
||||
`tg3-omnisocket-receiver.service`，也不再经过机器人本机 ZMQ 接收代理。
|
||||
595
tg3_omnisocket_transport/omnisocket_xtele_sender.py
Normal file
595
tg3_omnisocket_transport/omnisocket_xtele_sender.py
Normal file
@@ -0,0 +1,595 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Session-gate local xTELE data and send it to the robot through OmniSocket.
|
||||
|
||||
The raw/local-data stream remains authoritative for arm positions, physical
|
||||
buttons and hardware diagnostics. When configured, only the processed hand
|
||||
target from xTELE's command stream is merged into that raw frame. This keeps
|
||||
xTELE's built-in combo/gesture state machine and BrainCoRevo2 poses without
|
||||
letting unrelated UI, walking or base commands reach the dual-arm bridge.
|
||||
The physical left-Z + right-C hold is evaluated locally: idle frames never
|
||||
leave EAI, while each active interval carries a unique START/ACTIVE/STOP ID.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import struct
|
||||
import time
|
||||
from typing import Any
|
||||
import uuid
|
||||
|
||||
import zmq
|
||||
from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_ERROR, Session
|
||||
|
||||
|
||||
MAGIC = b"TG3A"
|
||||
HEADER = struct.Struct("!4sQQI")
|
||||
MAX_PAYLOAD_BYTES = 1_048_576
|
||||
TELEOP_PROTOCOL_VERSION = 2
|
||||
|
||||
|
||||
class XteleSender:
|
||||
def __init__(self, args: argparse.Namespace) -> None:
|
||||
if args.start_stop_hold_s <= 0.0:
|
||||
raise ValueError("start/stop hold time must be positive")
|
||||
if args.start_marker_frames <= 0:
|
||||
raise ValueError("start marker frame count must be positive")
|
||||
if args.source_timeout_s <= 0.0:
|
||||
raise ValueError("source timeout must be positive")
|
||||
if args.max_feedback_age_ms <= 0.0:
|
||||
raise ValueError("maximum KCP feedback age must be positive")
|
||||
if args.max_pending_frames <= 0:
|
||||
raise ValueError("maximum pending frame count must be positive")
|
||||
self.args = args
|
||||
self.stop = False
|
||||
self.session: Session | None = None
|
||||
self.session_connected_at = 0.0
|
||||
self.started_at = time.time()
|
||||
self.last_status_at = 0.0
|
||||
self.last_frame_at = 0.0
|
||||
self.last_source_frame_at = 0.0
|
||||
self.last_command_at = 0.0
|
||||
self.last_error = ""
|
||||
self.teleop_active = False
|
||||
self.teleop_session_id: str | None = None
|
||||
self.teleop_session_seq = 0
|
||||
self.packet_sequence = time.time_ns()
|
||||
self.start_markers_remaining = 0
|
||||
self.combo_started_at: float | None = None
|
||||
# A service restart must never turn an already-held combo into a start.
|
||||
self.require_combo_release = True
|
||||
self.counters = {
|
||||
"connected": 0,
|
||||
"reconnects": 0,
|
||||
"frames_received": 0,
|
||||
"frames_sent": 0,
|
||||
"bytes_received": 0,
|
||||
"bytes_sent": 0,
|
||||
"dropped_malformed": 0,
|
||||
"command_frames_received": 0,
|
||||
"command_frames_accepted": 0,
|
||||
"command_frames_malformed": 0,
|
||||
"command_hand_merges": 0,
|
||||
"remote_errors": 0,
|
||||
"frames_suppressed_inactive": 0,
|
||||
"teleop_starts": 0,
|
||||
"teleop_stops": 0,
|
||||
"teleop_aborts": 0,
|
||||
}
|
||||
signal.signal(signal.SIGINT, self._request_stop)
|
||||
signal.signal(signal.SIGTERM, self._request_stop)
|
||||
|
||||
def _request_stop(self, _signum: int, _frame: Any) -> None:
|
||||
self.stop = True
|
||||
|
||||
def connect(self) -> bool:
|
||||
self.close_session()
|
||||
session = Session()
|
||||
try:
|
||||
session.connect(
|
||||
server_addr=self.args.server,
|
||||
peer_id=self.args.peer_id,
|
||||
**CONTROL_DEFAULTS,
|
||||
)
|
||||
except OSError as exc:
|
||||
self.last_error = f"OmniSocket connect failed: {exc}"
|
||||
session.close()
|
||||
self.write_status(force=True)
|
||||
return False
|
||||
self.session = session
|
||||
self.session_connected_at = time.monotonic()
|
||||
self.counters["connected"] = 1
|
||||
self.counters["reconnects"] += 1
|
||||
self.last_error = ""
|
||||
self.write_status(force=True)
|
||||
return True
|
||||
|
||||
def close_session(self) -> None:
|
||||
if self.session is not None:
|
||||
try:
|
||||
self.session.close()
|
||||
except OSError:
|
||||
pass
|
||||
self.session = None
|
||||
self.session_connected_at = 0.0
|
||||
self.counters["connected"] = 0
|
||||
|
||||
def write_status(self, force: bool = False) -> None:
|
||||
now = time.monotonic()
|
||||
if not force and now - self.last_status_at < 0.5:
|
||||
return
|
||||
session_stats: dict[str, object] = {}
|
||||
kcp_stats: dict[str, object] = {}
|
||||
if self.session is not None:
|
||||
try:
|
||||
session_stats = self.session.stats()
|
||||
kcp_stats = self.session.kcp_stats()
|
||||
except OSError as exc:
|
||||
self.last_error = f"OmniSocket stats failed: {exc}"
|
||||
status = {
|
||||
"role": "xtele_sender",
|
||||
"server": self.args.server,
|
||||
"peer_id": self.args.peer_id,
|
||||
"target_peer": self.args.target_peer,
|
||||
"zmq_endpoint": self.args.zmq_endpoint,
|
||||
"cmd_zmq_endpoint": self.args.cmd_zmq_endpoint or None,
|
||||
"uptime_s": round(time.time() - self.started_at, 1),
|
||||
"last_frame_age_s": None
|
||||
if self.last_frame_at == 0.0
|
||||
else round(time.monotonic() - self.last_frame_at, 4),
|
||||
"last_command_age_s": None
|
||||
if self.last_command_at == 0.0
|
||||
else round(time.monotonic() - self.last_command_at, 4),
|
||||
"last_source_frame_age_s": None
|
||||
if self.last_source_frame_at == 0.0
|
||||
else round(time.monotonic() - self.last_source_frame_at, 4),
|
||||
"teleop_active": self.teleop_active,
|
||||
"teleop_session_id": self.teleop_session_id,
|
||||
"teleop_session_seq": self.teleop_session_seq,
|
||||
"teleop_combo_hold_s": 0.0
|
||||
if self.combo_started_at is None
|
||||
else round(time.monotonic() - self.combo_started_at, 2),
|
||||
"teleop_require_combo_release": self.require_combo_release,
|
||||
"application_data_sending": (
|
||||
self.teleop_active and self.session is not None
|
||||
),
|
||||
"last_error": self.last_error,
|
||||
"session_stats": session_stats,
|
||||
"kcp_stats": kcp_stats,
|
||||
**self.counters,
|
||||
"updated_unix_s": time.time(),
|
||||
}
|
||||
path = Path(self.args.status_file)
|
||||
try:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = path.with_suffix(path.suffix + ".tmp")
|
||||
temporary.write_text(json.dumps(status, indent=2) + "\n")
|
||||
os.replace(temporary, path)
|
||||
except OSError:
|
||||
pass
|
||||
self.last_status_at = now
|
||||
|
||||
def _drain_responses(self) -> None:
|
||||
if self.session is None:
|
||||
return
|
||||
try:
|
||||
while True:
|
||||
response = self.session.recv(timeout_ms=0)
|
||||
if response is None:
|
||||
break
|
||||
_from_peer, msg_type, payload = response
|
||||
if msg_type == MSG_TYPE_ERROR:
|
||||
self.counters["remote_errors"] += 1
|
||||
detail = payload.decode("utf-8", errors="replace")
|
||||
self._abort_teleop(f"OmniSocket remote error: {detail}")
|
||||
self.close_session()
|
||||
return
|
||||
except OSError as exc:
|
||||
self._abort_teleop(f"OmniSocket receive failed: {exc}")
|
||||
self.close_session()
|
||||
|
||||
@staticmethod
|
||||
def _valid_hand_side(value: object) -> bool:
|
||||
if isinstance(value, bool):
|
||||
return False
|
||||
if isinstance(value, (int, float)):
|
||||
numeric = float(value)
|
||||
return math.isfinite(numeric) and -0.05 <= numeric <= 1.05
|
||||
if not isinstance(value, list) or len(value) != 6:
|
||||
return False
|
||||
try:
|
||||
numeric_values = [float(item) for item in value]
|
||||
except (TypeError, ValueError):
|
||||
return False
|
||||
return all(
|
||||
math.isfinite(item) and -0.05 <= item <= 1.05
|
||||
for item in numeric_values
|
||||
)
|
||||
|
||||
@classmethod
|
||||
def _processed_hand_position(
|
||||
cls, command: dict[str, object]
|
||||
) -> dict[str, object] | None:
|
||||
try:
|
||||
position = command["hand"]["position"] # type: ignore[index]
|
||||
left = position["left"] # type: ignore[index]
|
||||
right = position["right"] # type: ignore[index]
|
||||
except (KeyError, TypeError):
|
||||
return None
|
||||
if not cls._valid_hand_side(left) or not cls._valid_hand_side(right):
|
||||
return None
|
||||
return {"left": copy.deepcopy(left), "right": copy.deepcopy(right)}
|
||||
|
||||
@classmethod
|
||||
def _build_payload(
|
||||
cls,
|
||||
data: dict[str, object],
|
||||
command: dict[str, object] | None,
|
||||
session_id: str,
|
||||
session_seq: int,
|
||||
session_state: str,
|
||||
stop_reason: str = "",
|
||||
) -> tuple[bytes, bool]:
|
||||
merged = False
|
||||
try:
|
||||
position = cls._processed_hand_position(command) if command else None
|
||||
hand = data.get("hand")
|
||||
if position is not None and isinstance(hand, dict):
|
||||
hand["position"] = position
|
||||
merged = True
|
||||
|
||||
metadata = data.get("tg3_transport")
|
||||
if not isinstance(metadata, dict):
|
||||
metadata = {}
|
||||
data["tg3_transport"] = metadata
|
||||
if merged:
|
||||
metadata["processed_hand_from_xtele_cmd"] = True
|
||||
assert command is not None
|
||||
metadata["xtele_cmd_timestamp"] = command.get("timestamp")
|
||||
else:
|
||||
metadata.pop("processed_hand_from_xtele_cmd", None)
|
||||
metadata.pop("xtele_cmd_timestamp", None)
|
||||
# Always overwrite untrusted source metadata. The robot accepts
|
||||
# start/active/stop only from this sender and expected Omni peer.
|
||||
metadata["protocol_version"] = TELEOP_PROTOCOL_VERSION
|
||||
metadata["session_id"] = session_id
|
||||
metadata["session_seq"] = session_seq
|
||||
metadata["session_state"] = session_state
|
||||
if stop_reason:
|
||||
metadata["stop_reason"] = stop_reason
|
||||
else:
|
||||
metadata.pop("stop_reason", None)
|
||||
encoded = json.dumps(
|
||||
data, ensure_ascii=False, separators=(",", ":")
|
||||
).encode("utf-8")
|
||||
except (TypeError, ValueError):
|
||||
raise ValueError("cannot encode xTELE session payload")
|
||||
return encoded, merged
|
||||
|
||||
@staticmethod
|
||||
def _start_stop_pressed(data: dict[str, object]) -> bool:
|
||||
try:
|
||||
buttons = data["button"]
|
||||
left = buttons["left"] # type: ignore[index]
|
||||
right = buttons["right"] # type: ignore[index]
|
||||
return (
|
||||
len(left) >= 3 # type: ignore[arg-type]
|
||||
and len(right) >= 3 # type: ignore[arg-type]
|
||||
and bool(left[2]) # type: ignore[index]
|
||||
and bool(right[2]) # type: ignore[index]
|
||||
)
|
||||
except (KeyError, TypeError):
|
||||
return False
|
||||
|
||||
def _update_teleop_gate(
|
||||
self, now: float, data: dict[str, object]
|
||||
) -> str | None:
|
||||
pressed = self._start_stop_pressed(data)
|
||||
if self.require_combo_release:
|
||||
self.combo_started_at = None
|
||||
if not pressed:
|
||||
self.require_combo_release = False
|
||||
return None
|
||||
if not pressed:
|
||||
self.combo_started_at = None
|
||||
return None
|
||||
if self.combo_started_at is None:
|
||||
self.combo_started_at = now
|
||||
if now - self.combo_started_at < self.args.start_stop_hold_s:
|
||||
return None
|
||||
|
||||
self.combo_started_at = None
|
||||
self.require_combo_release = True
|
||||
if self.teleop_active:
|
||||
self.teleop_active = False
|
||||
self.counters["teleop_stops"] += 1
|
||||
return "stop"
|
||||
|
||||
self.teleop_active = True
|
||||
self.teleop_session_id = uuid.uuid4().hex
|
||||
self.teleop_session_seq = 0
|
||||
self.start_markers_remaining = self.args.start_marker_frames
|
||||
self.counters["teleop_starts"] += 1
|
||||
return "start"
|
||||
|
||||
def _abort_teleop(self, reason: str) -> None:
|
||||
if self.teleop_active:
|
||||
self.counters["teleop_aborts"] += 1
|
||||
self.teleop_active = False
|
||||
self.teleop_session_id = None
|
||||
self.teleop_session_seq = 0
|
||||
self.start_markers_remaining = 0
|
||||
self.combo_started_at = None
|
||||
self.require_combo_release = True
|
||||
self.last_error = reason
|
||||
|
||||
def _send_payload(self, payload: bytes) -> bool:
|
||||
if self.session is None:
|
||||
self._abort_teleop("OmniSocket session is unavailable")
|
||||
return False
|
||||
unhealthy = self._session_unhealthy_reason()
|
||||
if unhealthy:
|
||||
self._abort_teleop(unhealthy)
|
||||
self.close_session()
|
||||
return False
|
||||
self.packet_sequence += 1
|
||||
packet = HEADER.pack(
|
||||
MAGIC, self.packet_sequence, time.time_ns(), len(payload)
|
||||
) + payload
|
||||
try:
|
||||
self.session.send(to=self.args.target_peer, data=packet)
|
||||
except OSError as exc:
|
||||
self._abort_teleop(f"OmniSocket send failed: {exc}")
|
||||
self.close_session()
|
||||
return False
|
||||
self.counters["frames_sent"] += 1
|
||||
self.counters["bytes_sent"] += len(payload)
|
||||
self.last_frame_at = time.monotonic()
|
||||
self.last_error = ""
|
||||
return True
|
||||
|
||||
def _session_unhealthy_reason(self) -> str | None:
|
||||
if self.session is None:
|
||||
return "OmniSocket session is unavailable"
|
||||
try:
|
||||
session_stats = self.session.stats()
|
||||
kcp_stats = self.session.kcp_stats()
|
||||
if int(session_stats.get("connected", 0)) != 1 or int(
|
||||
session_stats.get("registered", 0)
|
||||
) != 1:
|
||||
return "OmniSocket session lost registration"
|
||||
pending = int(kcp_stats.get("snd_queue", 0)) + int(
|
||||
kcp_stats.get("snd_buffer", 0)
|
||||
)
|
||||
if pending > self.args.max_pending_frames:
|
||||
return (
|
||||
f"OmniSocket pending queue reached {pending} frames; "
|
||||
"session invalidated to prevent stale replay"
|
||||
)
|
||||
feedback_age_ms = float(kcp_stats.get("last_feedback_age_ms", 0.0))
|
||||
connection_age_s = time.monotonic() - self.session_connected_at
|
||||
if (
|
||||
connection_age_s >= 0.5
|
||||
and feedback_age_ms > self.args.max_feedback_age_ms
|
||||
):
|
||||
return (
|
||||
f"OmniSocket feedback stale for {feedback_age_ms:.0f} ms; "
|
||||
"session invalidated to prevent stale replay"
|
||||
)
|
||||
except (OSError, TypeError, ValueError) as exc:
|
||||
return f"cannot verify OmniSocket session health: {exc}"
|
||||
return None
|
||||
|
||||
def _flush_session(self, timeout_s: float = 0.75) -> None:
|
||||
"""Bound the final STOP flush, without sending another business frame."""
|
||||
|
||||
deadline = time.monotonic() + timeout_s
|
||||
while self.session is not None and time.monotonic() < deadline:
|
||||
try:
|
||||
stats = self.session.kcp_stats()
|
||||
pending = int(stats.get("snd_queue", 0)) + int(
|
||||
stats.get("snd_buffer", 0)
|
||||
)
|
||||
except (OSError, TypeError, ValueError):
|
||||
return
|
||||
if pending == 0:
|
||||
return
|
||||
self._drain_responses()
|
||||
time.sleep(0.01)
|
||||
|
||||
def run(self) -> int:
|
||||
context = zmq.Context()
|
||||
source = context.socket(zmq.SUB)
|
||||
source.setsockopt(zmq.SUBSCRIBE, b"")
|
||||
source.setsockopt(zmq.CONFLATE, 1)
|
||||
source.setsockopt(zmq.RCVHWM, 1)
|
||||
source.setsockopt(zmq.LINGER, 0)
|
||||
source.connect(self.args.zmq_endpoint)
|
||||
poller = zmq.Poller()
|
||||
poller.register(source, zmq.POLLIN)
|
||||
command_source = None
|
||||
if self.args.cmd_zmq_endpoint:
|
||||
command_source = context.socket(zmq.SUB)
|
||||
command_source.setsockopt(zmq.SUBSCRIBE, b"")
|
||||
command_source.setsockopt(zmq.CONFLATE, 1)
|
||||
command_source.setsockopt(zmq.RCVHWM, 1)
|
||||
command_source.setsockopt(zmq.LINGER, 0)
|
||||
command_source.connect(self.args.cmd_zmq_endpoint)
|
||||
poller.register(command_source, zmq.POLLIN)
|
||||
latest_command: dict[str, object] | None = None
|
||||
try:
|
||||
while not self.stop:
|
||||
events = dict(poller.poll(100))
|
||||
if command_source is not None and command_source in events:
|
||||
command_raw = command_source.recv()
|
||||
self.counters["command_frames_received"] += 1
|
||||
try:
|
||||
parsed_command = json.loads(command_raw)
|
||||
if not isinstance(parsed_command, dict):
|
||||
raise ValueError("xTELE command must be a JSON object")
|
||||
if self._processed_hand_position(parsed_command) is None:
|
||||
raise ValueError(
|
||||
"xTELE command has no valid bilateral hand target"
|
||||
)
|
||||
latest_command = parsed_command
|
||||
self.last_command_at = time.monotonic()
|
||||
self.counters["command_frames_accepted"] += 1
|
||||
except (TypeError, ValueError, json.JSONDecodeError):
|
||||
self.counters["command_frames_malformed"] += 1
|
||||
|
||||
if source not in events:
|
||||
now = time.monotonic()
|
||||
if (
|
||||
self.last_source_frame_at != 0.0
|
||||
and now - self.last_source_frame_at
|
||||
> self.args.source_timeout_s
|
||||
):
|
||||
self.combo_started_at = None
|
||||
if self.teleop_active:
|
||||
self._abort_teleop(
|
||||
"local xTELE source became stale; a new Z+C hold "
|
||||
"is required"
|
||||
)
|
||||
self.close_session()
|
||||
self._drain_responses()
|
||||
self.write_status()
|
||||
continue
|
||||
|
||||
raw = source.recv()
|
||||
self.counters["frames_received"] += 1
|
||||
self.counters["bytes_received"] += len(raw)
|
||||
if not raw or len(raw) > MAX_PAYLOAD_BYTES:
|
||||
self.counters["dropped_malformed"] += 1
|
||||
continue
|
||||
|
||||
try:
|
||||
data = json.loads(raw)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("xTELE raw frame must be a JSON object")
|
||||
except (TypeError, ValueError, json.JSONDecodeError):
|
||||
# A malformed/frozen frame may never contribute time to a
|
||||
# physical three-second start/stop hold.
|
||||
self.combo_started_at = None
|
||||
self.counters["dropped_malformed"] += 1
|
||||
continue
|
||||
|
||||
now = time.monotonic()
|
||||
self.last_source_frame_at = now
|
||||
transition = self._update_teleop_gate(now, data)
|
||||
|
||||
command = None
|
||||
if (
|
||||
latest_command is not None
|
||||
and now - self.last_command_at <= self.args.cmd_max_age_s
|
||||
):
|
||||
command = latest_command
|
||||
|
||||
if not self.teleop_active and transition != "stop":
|
||||
self.counters["frames_suppressed_inactive"] += 1
|
||||
self._drain_responses()
|
||||
self.write_status()
|
||||
continue
|
||||
|
||||
session_id = self.teleop_session_id
|
||||
if not session_id:
|
||||
self._abort_teleop("teleoperation session ID is unavailable")
|
||||
self.counters["frames_suppressed_inactive"] += 1
|
||||
continue
|
||||
|
||||
if self.session is None and not self.connect():
|
||||
self._abort_teleop(
|
||||
"cannot start teleoperation because OmniSocket Hub is "
|
||||
"unavailable; release Z+C before retrying"
|
||||
)
|
||||
self.counters["frames_suppressed_inactive"] += 1
|
||||
continue
|
||||
|
||||
self.teleop_session_seq += 1
|
||||
if transition == "stop":
|
||||
session_state = "stop"
|
||||
stop_reason = "operator"
|
||||
elif self.start_markers_remaining > 0:
|
||||
session_state = "start"
|
||||
stop_reason = ""
|
||||
else:
|
||||
session_state = "active"
|
||||
stop_reason = ""
|
||||
try:
|
||||
payload, merged = self._build_payload(
|
||||
data,
|
||||
command,
|
||||
session_id,
|
||||
self.teleop_session_seq,
|
||||
session_state,
|
||||
stop_reason,
|
||||
)
|
||||
except ValueError:
|
||||
self.counters["dropped_malformed"] += 1
|
||||
continue
|
||||
if merged:
|
||||
self.counters["command_hand_merges"] += 1
|
||||
if len(payload) > MAX_PAYLOAD_BYTES:
|
||||
self.counters["dropped_malformed"] += 1
|
||||
continue
|
||||
|
||||
sent = self._send_payload(payload)
|
||||
if sent and session_state == "start":
|
||||
self.start_markers_remaining -= 1
|
||||
if transition == "stop":
|
||||
# STOP is the final xTELE business frame. The underlying
|
||||
# registered OmniSocket session remains warm for low-latency
|
||||
# next start and KCP delivery, but no arm data follows.
|
||||
self.teleop_session_id = None
|
||||
self.teleop_session_seq = 0
|
||||
self.start_markers_remaining = 0
|
||||
self._flush_session()
|
||||
self.close_session()
|
||||
self._drain_responses()
|
||||
self.write_status()
|
||||
finally:
|
||||
source.close()
|
||||
if command_source is not None:
|
||||
command_source.close()
|
||||
context.term()
|
||||
self.close_session()
|
||||
self.write_status(force=True)
|
||||
return 0
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--server", required=True)
|
||||
parser.add_argument("--peer-id", required=True)
|
||||
parser.add_argument("--target-peer", required=True)
|
||||
parser.add_argument("--zmq-endpoint", required=True)
|
||||
parser.add_argument(
|
||||
"--cmd-zmq-endpoint",
|
||||
default="",
|
||||
help=(
|
||||
"optional xTELE processed-command PUB endpoint; only its bilateral "
|
||||
"hand.position target is merged into the raw frame"
|
||||
),
|
||||
)
|
||||
parser.add_argument("--cmd-max-age-s", type=float, default=0.25)
|
||||
parser.add_argument("--source-timeout-s", type=float, default=0.25)
|
||||
parser.add_argument("--max-feedback-age-ms", type=float, default=500.0)
|
||||
parser.add_argument("--max-pending-frames", type=int, default=100)
|
||||
parser.add_argument("--start-stop-hold-s", type=float, default=3.0)
|
||||
parser.add_argument(
|
||||
"--start-marker-frames",
|
||||
type=int,
|
||||
default=50,
|
||||
help="repeat START for this many source frames before ACTIVE",
|
||||
)
|
||||
parser.add_argument("--status-file", required=True)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(XteleSender(parse_args()).run())
|
||||
132
tg3_omnisocket_transport/test_session_gate.py
Normal file
132
tg3_omnisocket_transport/test_session_gate.py
Normal file
@@ -0,0 +1,132 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Offline tests for the EAI teleoperation session gate."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import ModuleType, SimpleNamespace
|
||||
import sys
|
||||
import unittest
|
||||
|
||||
|
||||
fake_omnisocket = ModuleType("omnisocket")
|
||||
fake_omnisocket.CONTROL_DEFAULTS = {}
|
||||
fake_omnisocket.MSG_TYPE_ERROR = 5
|
||||
fake_omnisocket.Session = object
|
||||
sys.modules.setdefault("omnisocket", fake_omnisocket)
|
||||
|
||||
module_path = Path(__file__).with_name("omnisocket_xtele_sender.py")
|
||||
spec = importlib.util.spec_from_file_location("omnisocket_xtele_sender", module_path)
|
||||
assert spec is not None and spec.loader is not None
|
||||
sender_module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(sender_module)
|
||||
XteleSender = sender_module.XteleSender
|
||||
|
||||
|
||||
def buttons(pressed: bool) -> dict[str, object]:
|
||||
return {
|
||||
"button": {
|
||||
"left": [False, False, pressed],
|
||||
"right": [False, False, pressed],
|
||||
},
|
||||
"hand": {"position": {"left": 0.0, "right": 0.0}},
|
||||
}
|
||||
|
||||
|
||||
class SessionGateTest(unittest.TestCase):
|
||||
def setUp(self) -> None:
|
||||
args = SimpleNamespace(
|
||||
start_stop_hold_s=3.0,
|
||||
start_marker_frames=50,
|
||||
source_timeout_s=0.25,
|
||||
max_feedback_age_ms=500.0,
|
||||
max_pending_frames=100,
|
||||
)
|
||||
self.sender = XteleSender(args)
|
||||
|
||||
def test_start_and_stop_each_require_a_new_continuous_hold(self) -> None:
|
||||
# Boot requires a release, so a button held across service restart
|
||||
# cannot start a session.
|
||||
self.assertIsNone(self.sender._update_teleop_gate(0.0, buttons(True)))
|
||||
self.assertIsNone(self.sender._update_teleop_gate(0.1, buttons(False)))
|
||||
|
||||
self.assertIsNone(self.sender._update_teleop_gate(1.0, buttons(True)))
|
||||
self.assertIsNone(self.sender._update_teleop_gate(3.99, buttons(True)))
|
||||
self.assertEqual(
|
||||
self.sender._update_teleop_gate(4.01, buttons(True)), "start"
|
||||
)
|
||||
first_id = self.sender.teleop_session_id
|
||||
self.assertTrue(self.sender.teleop_active)
|
||||
self.assertIsNotNone(first_id)
|
||||
|
||||
# Keeping the same hold cannot immediately toggle the new session off.
|
||||
self.assertIsNone(self.sender._update_teleop_gate(8.0, buttons(True)))
|
||||
self.assertTrue(self.sender.teleop_active)
|
||||
self.assertIsNone(self.sender._update_teleop_gate(8.1, buttons(False)))
|
||||
|
||||
self.assertIsNone(self.sender._update_teleop_gate(9.0, buttons(True)))
|
||||
self.assertIsNone(self.sender._update_teleop_gate(11.99, buttons(True)))
|
||||
self.assertEqual(
|
||||
self.sender._update_teleop_gate(12.01, buttons(True)), "stop"
|
||||
)
|
||||
self.assertFalse(self.sender.teleop_active)
|
||||
self.assertEqual(self.sender.teleop_session_id, first_id)
|
||||
|
||||
def test_releasing_during_hold_resets_the_timer(self) -> None:
|
||||
self.sender._update_teleop_gate(0.0, buttons(False))
|
||||
self.sender._update_teleop_gate(1.0, buttons(True))
|
||||
self.sender._update_teleop_gate(2.0, buttons(False))
|
||||
self.sender._update_teleop_gate(3.0, buttons(True))
|
||||
self.assertIsNone(self.sender._update_teleop_gate(5.9, buttons(True)))
|
||||
self.assertEqual(
|
||||
self.sender._update_teleop_gate(6.01, buttons(True)), "start"
|
||||
)
|
||||
|
||||
def test_transport_metadata_is_overwritten(self) -> None:
|
||||
data = buttons(False)
|
||||
data["tg3_transport"] = {
|
||||
"protocol_version": 999,
|
||||
"session_id": "forged",
|
||||
"session_state": "stop",
|
||||
}
|
||||
payload, merged = self.sender._build_payload(
|
||||
data,
|
||||
None,
|
||||
"a" * 32,
|
||||
7,
|
||||
"start",
|
||||
)
|
||||
self.assertFalse(merged)
|
||||
metadata = json.loads(payload)["tg3_transport"]
|
||||
self.assertEqual(metadata["protocol_version"], 2)
|
||||
self.assertEqual(metadata["session_id"], "a" * 32)
|
||||
self.assertEqual(metadata["session_seq"], 7)
|
||||
self.assertEqual(metadata["session_state"], "start")
|
||||
|
||||
def test_stale_or_backlogged_transport_is_rejected(self) -> None:
|
||||
class FakeSession:
|
||||
def __init__(self, feedback_age: int, pending: int) -> None:
|
||||
self.feedback_age = feedback_age
|
||||
self.pending = pending
|
||||
|
||||
def stats(self) -> dict[str, int]:
|
||||
return {"connected": 1, "registered": 1}
|
||||
|
||||
def kcp_stats(self) -> dict[str, int]:
|
||||
return {
|
||||
"snd_queue": self.pending,
|
||||
"snd_buffer": 0,
|
||||
"last_feedback_age_ms": self.feedback_age,
|
||||
}
|
||||
|
||||
self.sender.session_connected_at = 0.0
|
||||
self.sender.session = FakeSession(600, 0)
|
||||
self.assertIn("feedback stale", self.sender._session_unhealthy_reason())
|
||||
self.sender.session = FakeSession(1, 101)
|
||||
self.assertIn("pending queue", self.sender._session_unhealthy_reason())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
17
tg3_omnisocket_transport/tg3-omnisocket-sender.service
Normal file
17
tg3_omnisocket_transport/tg3-omnisocket-sender.service
Normal file
@@ -0,0 +1,17 @@
|
||||
[Unit]
|
||||
Description=TG3 session-gated TS1P OmniSocket KCP sender
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
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 --start-marker-frames 50 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json
|
||||
Restart=always
|
||||
RestartSec=1
|
||||
KillSignal=SIGINT
|
||||
TimeoutStopSec=5
|
||||
|
||||
[Install]
|
||||
WantedBy=default.target
|
||||
16
verify.sh
Executable file
16
verify.sh
Executable file
@@ -0,0 +1,16 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
expected_omnisocket_commit="de3f5c96779dbe1571c10feb22fc7f2331b6b222"
|
||||
|
||||
python3 "$repo_dir/tg3_omnisocket_transport/test_session_gate.py"
|
||||
python3 "$repo_dir/tg3_local_teleop/test_session_gate.py"
|
||||
|
||||
actual_omnisocket_commit="$(git -C "$repo_dir/OmniSocketGo" rev-parse HEAD)"
|
||||
if [[ "$actual_omnisocket_commit" != "$expected_omnisocket_commit" ]]; then
|
||||
echo "OmniSocketGo commit mismatch: $actual_omnisocket_commit" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "Offline checks passed; OmniSocketGo is pinned to $actual_omnisocket_commit"
|
||||
Reference in New Issue
Block a user