From 57e582e1d72f59c5d63416d7b45e324812660820 Mon Sep 17 00:00:00 2001 From: meiqi <2510105031@mails.szu.edu.cn> Date: Fri, 7 Aug 2026 16:13:41 +0800 Subject: [PATCH] feat: package TG3 TS1P OmniSocket teleoperation --- .gitignore | 21 + .gitmodules | 4 + OmniSocketGo | 1 + README.md | 79 + docs/天工3.0_joystick_bridge与SBUS证据记录.md | 168 ++ docs/天工3.0本地同构臂遥操迁移部署指南.md | 451 +++++ docs/天工3.0本地同构臂遥操部署汇报.md | 354 ++++ tg3_local_teleop/README.md | 185 ++ tg3_local_teleop/config.toml | 119 ++ tg3_local_teleop/home.sh | 31 + .../src/ros2_bridge_msgs/CMakeLists.txt | 15 + .../src/ros2_bridge_msgs/msg/ArmStatus.msg | 4 + .../src/ros2_bridge_msgs/msg/MotorStatus.msg | 7 + .../ros2_py/src/ros2_bridge_msgs/package.xml | 13 + tg3_local_teleop/run.sh | 26 + tg3_local_teleop/status.sh | 9 + tg3_local_teleop/test_session_gate.py | 217 +++ tg3_local_teleop/tg3-local-teleop.service | 15 + tg3_local_teleop/tg3_local_teleop.py | 1523 +++++++++++++++++ tg3_omnisocket_transport/README.md | 53 + .../omnisocket_xtele_sender.py | 595 +++++++ tg3_omnisocket_transport/test_session_gate.py | 132 ++ .../tg3-omnisocket-sender.service | 17 + verify.sh | 16 + 24 files changed, 4055 insertions(+) create mode 100644 .gitignore create mode 100644 .gitmodules create mode 160000 OmniSocketGo create mode 100644 README.md create mode 100644 docs/天工3.0_joystick_bridge与SBUS证据记录.md create mode 100644 docs/天工3.0本地同构臂遥操迁移部署指南.md create mode 100644 docs/天工3.0本地同构臂遥操部署汇报.md create mode 100644 tg3_local_teleop/README.md create mode 100644 tg3_local_teleop/config.toml create mode 100755 tg3_local_teleop/home.sh create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ArmStatus.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorStatus.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml create mode 100755 tg3_local_teleop/run.sh create mode 100755 tg3_local_teleop/status.sh create mode 100644 tg3_local_teleop/test_session_gate.py create mode 100644 tg3_local_teleop/tg3-local-teleop.service create mode 100755 tg3_local_teleop/tg3_local_teleop.py create mode 100644 tg3_omnisocket_transport/README.md create mode 100644 tg3_omnisocket_transport/omnisocket_xtele_sender.py create mode 100644 tg3_omnisocket_transport/test_session_gate.py create mode 100644 tg3_omnisocket_transport/tg3-omnisocket-sender.service create mode 100755 verify.sh diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..83097a9 --- /dev/null +++ b/.gitignore @@ -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 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..5ba1d57 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "OmniSocketGo"] + path = OmniSocketGo + url = https://gitea.public.snrc.site/limingjie/OmniSocketGo.git + branch = c diff --git a/OmniSocketGo b/OmniSocketGo new file mode 160000 index 0000000..de3f5c9 --- /dev/null +++ b/OmniSocketGo @@ -0,0 +1 @@ +Subproject commit de3f5c96779dbe1571c10feb22fc7f2331b6b222 diff --git a/README.md b/README.md new file mode 100644 index 0000000..72f2741 --- /dev/null +++ b/README.md @@ -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 信息及版权资料。 diff --git a/docs/天工3.0_joystick_bridge与SBUS证据记录.md b/docs/天工3.0_joystick_bridge与SBUS证据记录.md new file mode 100644 index 0000000..0e68ba5 --- /dev/null +++ b/docs/天工3.0_joystick_bridge与SBUS证据记录.md @@ -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 失败属于正常现象，应在重新上电并完成启动后检查。 diff --git a/docs/天工3.0本地同构臂遥操迁移部署指南.md b/docs/天工3.0本地同构臂遥操迁移部署指南.md new file mode 100644 index 0000000..3ae9cde --- /dev/null +++ b/docs/天工3.0本地同构臂遥操迁移部署指南.md @@ -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 +--peer-id +--target-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 +--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`。 diff --git a/docs/天工3.0本地同构臂遥操部署汇报.md b/docs/天工3.0本地同构臂遥操部署汇报.md new file mode 100644 index 0000000..1bb419f --- /dev/null +++ b/docs/天工3.0本地同构臂遥操部署汇报.md @@ -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》。 diff --git a/tg3_local_teleop/README.md b/tg3_local_teleop/README.md new file mode 100644 index 0000000..0261da0 --- /dev/null +++ b/tg3_local_teleop/README.md @@ -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 +``` diff --git a/tg3_local_teleop/config.toml b/tg3_local_teleop/config.toml new file mode 100644 index 0000000..8577fd6 --- /dev/null +++ b/tg3_local_teleop/config.toml @@ -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 diff --git a/tg3_local_teleop/home.sh b/tg3_local_teleop/home.sh new file mode 100755 index 0000000..b764412 --- /dev/null +++ b/tg3_local_teleop/home.sh @@ -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 diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt new file mode 100644 index 0000000..d388987 --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt @@ -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() diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ArmStatus.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ArmStatus.msg new file mode 100644 index 0000000..6e6e77e --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ArmStatus.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 label +uint8 reserved +ros2_bridge_msgs/MotorStatus[] status diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorStatus.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorStatus.msg new file mode 100644 index 0000000..9248bc9 --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorStatus.msg @@ -0,0 +1,7 @@ +uint16 name +float64 pos +float64 speed +float64 current +float64 temperature +float64 mos_temperature +uint32 error diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml new file mode 100644 index 0000000..da2a442 --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml @@ -0,0 +1,13 @@ + + + ros2_bridge_msgs + 0.0.0 + Python bindings for the installed TG3 ArmStatus wire type. + nvidia + Proprietary + ament_cmake + rosidl_default_generators + std_msgs + rosidl_default_runtime + rosidl_interface_packages + diff --git a/tg3_local_teleop/run.sh b/tg3_local_teleop/run.sh new file mode 100755 index 0000000..b4bc4c2 --- /dev/null +++ b/tg3_local_teleop/run.sh @@ -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" \ + "$@" diff --git a/tg3_local_teleop/status.sh b/tg3_local_teleop/status.sh new file mode 100755 index 0000000..160d1f4 --- /dev/null +++ b/tg3_local_teleop/status.sh @@ -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" diff --git a/tg3_local_teleop/test_session_gate.py b/tg3_local_teleop/test_session_gate.py new file mode 100644 index 0000000..050b745 --- /dev/null +++ b/tg3_local_teleop/test_session_gate.py @@ -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() diff --git a/tg3_local_teleop/tg3-local-teleop.service b/tg3_local_teleop/tg3-local-teleop.service new file mode 100644 index 0000000..8455175 --- /dev/null +++ b/tg3_local_teleop/tg3-local-teleop.service @@ -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 diff --git a/tg3_local_teleop/tg3_local_teleop.py b/tg3_local_teleop/tg3_local_teleop.py new file mode 100755 index 0000000..bfcb4e9 --- /dev/null +++ b/tg3_local_teleop/tg3_local_teleop.py @@ -0,0 +1,1523 @@ +#!/usr/bin/env python3 +"""Local TS1P isomorphic-arm bridge for TianGong 3.0. + +Arm targets use the vendor ``/encoder_identical_joint`` input, while guarded +HBWALK velocity commands use the vendor ``/hric/robot/cmd_vel`` input. This +bridge never changes the robot FSM; HBWALK must already be active and running. +""" + +from __future__ import annotations + +import argparse +import json +import math +import os +import struct +import threading +import time +import tomllib +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import rclpy +import zmq +from brainco_hand_msgs.msg import MotorStatus, SetMotorMulti +from diagnostic_msgs.msg import DiagnosticStatus +from geometry_msgs.msg import TwistStamped +from rclpy.node import Node +from ros2_bridge_msgs.msg import ArmStatus +from sensor_msgs.msg import JointState +from std_srvs.srv import Trigger + + +JOINT_NAMES = [ + *(f"left_joints_{i}" for i in range(7)), + *(f"right_joints_{i}" for i in range(7)), +] +MOTOR_IDS = [*range(11, 18), *range(21, 28)] +HAND_SIDES = ("left", "right") +OMNI_MAGIC = b"TG3A" +OMNI_HEADER = struct.Struct("!4sQQI") +TELEOP_PROTOCOL_VERSION = 2 + + +@dataclass(frozen=True) +class ArmSnapshot: + data: dict[str, Any] + received_at: float + + +class LatestArmData: + """Receive the latest industrial-PC sample without buffering old motion.""" + + def __init__(self, config: dict[str, Any]) -> None: + self.cfg = config + self.transport = str(config.get("transport", "zmq")).lower() + if self.transport not in ("zmq", "omnisocket"): + raise ValueError(f"unsupported iarm transport: {self.transport}") + self.endpoint = str(config.get("iarm_endpoint", "")) + if self.transport == "omnisocket": + self.description = ( + f"omnisocket://{config['omnisocket_server']}/" + f"{config['omnisocket_peer_id']}" + ) + else: + self.description = self.endpoint + self._lock = threading.Lock() + self._latest: ArmSnapshot | None = None + self._last_error = f"waiting for the first {self.transport} sample" + self._metrics: dict[str, Any] = { + "transport": self.transport, + "connected": False, + "frames_received": 0, + "frames_accepted": 0, + "dropped_sender": 0, + "dropped_malformed": 0, + "dropped_stale": 0, + "dropped_sequence": 0, + "raw_packet_age_ms": None, + "packet_age_baseline_ms": None, + "effective_packet_age_ms": None, + "max_effective_packet_age_ms": None, + } + self._stop = threading.Event() + self._thread = threading.Thread( + target=self._run, name=f"iarm-{self.transport}", daemon=True + ) + + def start(self) -> None: + self._thread.start() + + def close(self) -> None: + self._stop.set() + self._thread.join(timeout=2.0) + + def get(self) -> tuple[ArmSnapshot | None, str]: + with self._lock: + return self._latest, self._last_error + + def metrics(self) -> dict[str, Any]: + with self._lock: + return dict(self._metrics) + + def _run(self) -> None: + if self.transport == "omnisocket": + self._run_omnisocket() + else: + self._run_zmq() + + def _run_zmq(self) -> None: + context = zmq.Context.instance() + sock = context.socket(zmq.SUB) + sock.setsockopt(zmq.SUBSCRIBE, b"") + sock.setsockopt(zmq.CONFLATE, 1) + sock.setsockopt(zmq.RCVHWM, 1) + sock.setsockopt(zmq.LINGER, 0) + sock.connect(self.endpoint) + poller = zmq.Poller() + poller.register(sock, zmq.POLLIN) + try: + while not self._stop.is_set(): + if sock not in dict(poller.poll(100)): + continue + try: + raw = sock.recv(zmq.NOBLOCK) + data = json.loads(raw) + self._validate_shape(data) + with self._lock: + self._latest = ArmSnapshot( + data=data, received_at=time.monotonic() + ) + self._last_error = "" + self._metrics["connected"] = True + self._metrics["frames_received"] += 1 + self._metrics["frames_accepted"] += 1 + except Exception as exc: # Keep receiving after one malformed packet. + with self._lock: + self._last_error = f"invalid ZMQ sample: {exc}" + self._metrics["dropped_malformed"] += 1 + finally: + sock.close() + + def _run_omnisocket(self) -> None: + try: + from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, Session + except ImportError as exc: + with self._lock: + self._last_error = f"cannot import OmniSocket extension: {exc}" + return + + expected_sender = str(self.cfg["omnisocket_expected_sender"]) + max_age_ms = float(self.cfg["omnisocket_max_packet_age_ms"]) + last_sequence = 0 + while not self._stop.is_set(): + session = Session() + baseline_ms: float | None = None + try: + session.connect( + server_addr=str(self.cfg["omnisocket_server"]), + peer_id=str(self.cfg["omnisocket_peer_id"]), + **CONTROL_DEFAULTS, + ) + with self._lock: + self._metrics["connected"] = True + self._last_error = "" + + while not self._stop.is_set(): + message = session.recv(timeout_ms=100) + if message is None: + continue + messages = [message] + while True: + pending = session.recv(timeout_ms=0) + if pending is None: + break + messages.append(pending) + + newest: tuple[int, bytes] | None = None + now_ns = time.time_ns() + for from_peer, msg_type, packet in messages: + with self._lock: + self._metrics["frames_received"] += 1 + if from_peer != expected_sender: + with self._lock: + self._metrics["dropped_sender"] += 1 + continue + decoded = self._decode_omni_packet( + msg_type, + MSG_TYPE_BINARY, + packet, + now_ns, + baseline_ms, + ) + if decoded is None: + continue + sequence, payload, raw_age_ms, effective_age_ms = decoded + if baseline_ms is None or raw_age_ms < baseline_ms: + baseline_ms = raw_age_ms + effective_age_ms = 0.0 + self._record_packet_age( + raw_age_ms, baseline_ms, effective_age_ms + ) + if effective_age_ms > max_age_ms: + with self._lock: + self._metrics["dropped_stale"] += 1 + continue + if sequence <= last_sequence: + with self._lock: + self._metrics["dropped_sequence"] += 1 + continue + if newest is None or sequence > newest[0]: + newest = (sequence, payload) + + if newest is None: + continue + sequence, payload = newest + try: + data = json.loads(payload) + self._validate_shape(data) + except Exception as exc: + with self._lock: + self._last_error = f"invalid OmniSocket sample: {exc}" + self._metrics["dropped_malformed"] += 1 + continue + last_sequence = sequence + with self._lock: + self._latest = ArmSnapshot( + data=data, received_at=time.monotonic() + ) + self._last_error = "" + self._metrics["frames_accepted"] += 1 + except Exception as exc: + with self._lock: + self._metrics["connected"] = False + self._last_error = f"OmniSocket connection failed: {exc}" + finally: + try: + session.close() + except OSError: + pass + with self._lock: + self._metrics["connected"] = False + self._stop.wait(1.0) + + def _decode_omni_packet( + self, + msg_type: int, + binary_type: int, + packet: bytes, + now_ns: int, + baseline_ms: float | None, + ) -> tuple[int, bytes, float, float] | None: + if msg_type != binary_type or len(packet) < OMNI_HEADER.size: + with self._lock: + self._metrics["dropped_malformed"] += 1 + return None + magic, sequence, sent_ns, payload_len = OMNI_HEADER.unpack_from(packet) + payload = packet[OMNI_HEADER.size :] + if magic != OMNI_MAGIC or payload_len != len(payload) or not payload: + with self._lock: + self._metrics["dropped_malformed"] += 1 + return None + raw_age_ms = (now_ns - sent_ns) / 1_000_000.0 + effective_age_ms = ( + 0.0 if baseline_ms is None else max(0.0, raw_age_ms - baseline_ms) + ) + return sequence, payload, raw_age_ms, effective_age_ms + + def _record_packet_age( + self, raw_age_ms: float, baseline_ms: float, effective_age_ms: float + ) -> None: + with self._lock: + self._metrics["raw_packet_age_ms"] = round(raw_age_ms, 3) + self._metrics["packet_age_baseline_ms"] = round(baseline_ms, 3) + self._metrics["effective_packet_age_ms"] = round(effective_age_ms, 3) + previous_max = self._metrics["max_effective_packet_age_ms"] + if previous_max is None or effective_age_ms > previous_max: + self._metrics["max_effective_packet_age_ms"] = round( + effective_age_ms, 3 + ) + + @staticmethod + def _validate_shape(data: dict[str, Any]) -> None: + position = data["arm"]["position"] + left = position["left"] + right = position["right"] + if len(left) != 7 or len(right) != 7: + raise ValueError("arm.position must contain left[7] and right[7]") + if not all(math.isfinite(float(v)) for v in [*left, *right]): + raise ValueError("arm.position contains a non-finite value") + + +class LocalTeleopBridge(Node): + FRAME_ID = "tg3_local_teleop" + + def __init__(self, config: dict[str, Any], allow_publish: bool, status_file: Path) -> None: + super().__init__("tg3_local_teleop") + self.cfg = config + self.allow_publish = allow_publish + self.status_file = status_file + + ros_cfg = config["ros"] + net_cfg = config["network"] + self.hands_cfg = config.get("hands", {}) + self.hands_enabled = bool(self.hands_cfg.get("enabled", False)) + self.locomotion_cfg = config.get("locomotion", {}) + self.locomotion_enabled = bool( + self.locomotion_cfg.get("enabled", False) + ) + self.hand_publishers: dict[str, Any] = {} + self.robot_hand_positions: dict[str, list[int] | None] = { + side: None for side in HAND_SIDES + } + self.robot_hand_states: dict[str, list[int]] = { + side: [] for side in HAND_SIDES + } + self.robot_hand_at: dict[str, float] = {side: 0.0 for side in HAND_SIDES} + self.last_hand_commands: dict[str, list[int] | None] = { + side: None for side in HAND_SIDES + } + self.expected_hand_messages: dict[ + str, tuple[tuple[Any, ...], float] | None + ] = {side: None for side in HAND_SIDES} + self.last_hand_publish_at = 0.0 + self.hand_publish_count = 0 + self.hand_output_ready = False + self.runtime_hand_output_reasons: list[str] = [] + self.foreign_hand_source_seen = False + self.walk_combo_started_at: float | None = None + self.walk_active = False + self.walk_command = [0.0, 0.0] + self.walk_publish_count = 0 + self.walk_zero_frames_remaining = 0 + self.last_walk_publish_at = 0.0 + + self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10) + self.walk_publisher = None + if self.locomotion_enabled: + self.walk_publisher = self.create_publisher( + TwistStamped, self.locomotion_cfg["command_topic"], 10 + ) + self.create_subscription( + DiagnosticStatus, ros_cfg["rl_state_topic"], self._on_rl_state, 10 + ) + self.create_subscription( + ArmStatus, ros_cfg["arm_state_topic"], self._on_arm_state, 10 + ) + # A foreign sample means cloud teleoperation is active. Never mix two + # command sources on the vendor frequency-conversion input. + self.create_subscription( + JointState, ros_cfg["command_topic"], self._on_command_topic, 10 + ) + if self.hands_enabled: + for side in HAND_SIDES: + command_topic = str(self.hands_cfg[f"{side}_command_topic"]) + status_topic = str(self.hands_cfg[f"{side}_status_topic"]) + self.hand_publishers[side] = self.create_publisher( + SetMotorMulti, command_topic, 10 + ) + self.create_subscription( + MotorStatus, + status_topic, + lambda msg, hand_side=side: self._on_hand_status(hand_side, msg), + 10, + ) + self.create_subscription( + SetMotorMulti, + command_topic, + lambda msg, hand_side=side: self._on_hand_command(hand_side, msg), + 10, + ) + self.create_service(Trigger, ros_cfg["home_service"], self._on_home_request) + self.create_service( + Trigger, ros_cfg["cancel_home_service"], self._on_cancel_home_request + ) + + self.source = LatestArmData(net_cfg) + self.source.start() + + self.rl_state: dict[str, str] = {} + self.rl_state_at = 0.0 + self.robot_arm_positions: list[float] | None = None + self.robot_arm_errors: list[int] = [] + self.robot_arm_at = 0.0 + self.foreign_source_seen = False + self.armed = False + self.returning_home = False + self.home_command: list[float] | None = None + self.home_started_at = 0.0 + self.home_settle_started_at: float | None = None + self.home_status = "idle" + self.combo_started_at: float | None = None + self.combo_latched = False + self.active_session_id: str | None = None + self.session_start_attempted_id: str | None = None + self.last_session_state = "inactive" + self.last_session_stop_reason = "" + self.last_command: list[float] | None = None + self.last_publish_at = 0.0 + self.last_status_at = 0.0 + self.last_log_signature: tuple[Any, ...] | None = None + self.started_at = time.monotonic() + + rate = float(ros_cfg["publish_rate_hz"]) + self.timer = self.create_timer(1.0 / rate, self._tick) + mode = "ACTIVE-CAPABLE" if allow_publish else "MONITOR-ONLY" + self.get_logger().info( + f"local bridge started in {mode}; source={self.source.description}; " + f"target={ros_cfg['command_topic']}; brainco_hands={self.hands_enabled}; " + f"locomotion={self.locomotion_enabled}" + ) + + def close(self) -> None: + self._disarm("bridge is shutting down") + self._finish_home("bridge is shutting down", success=False) + self.source.close() + + def _on_rl_state(self, msg: DiagnosticStatus) -> None: + self.rl_state = {item.key: item.value for item in msg.values} + self.rl_state_at = time.monotonic() + + def _on_arm_state(self, msg: ArmStatus) -> None: + by_id = {int(motor.name): motor for motor in msg.status} + if not all(motor_id in by_id for motor_id in MOTOR_IDS): + return + positions = [float(by_id[motor_id].pos) for motor_id in MOTOR_IDS] + if not all(math.isfinite(value) for value in positions): + return + self.robot_arm_positions = positions + self.robot_arm_errors = [int(by_id[motor_id].error) for motor_id in MOTOR_IDS] + self.robot_arm_at = time.monotonic() + + def _on_hand_status(self, side: str, msg: MotorStatus) -> None: + positions = [int(value) for value in msg.positions] + states = [int(value) for value in msg.states] + if len(positions) != 6 or len(states) != 6: + return + self.robot_hand_positions[side] = positions + self.robot_hand_states[side] = states + self.robot_hand_at[side] = time.monotonic() + + @staticmethod + def _hand_message_signature(msg: SetMotorMulti) -> tuple[Any, ...]: + return ( + int(msg.mode), + tuple(int(value) for value in msg.positions), + tuple(int(value) for value in msg.speeds), + tuple(int(value) for value in msg.currents), + tuple(int(value) for value in msg.pwms), + tuple(int(value) for value in msg.durations), + ) + + def _on_hand_command(self, side: str, msg: SetMotorMulti) -> None: + now = time.monotonic() + signature = self._hand_message_signature(msg) + expected = self.expected_hand_messages[side] + if ( + expected is not None + and now - expected[1] <= 0.25 + and signature == expected[0] + ): + return + if not self.foreign_hand_source_seen: + self.get_logger().error( + f"foreign {side} BrainCo hand command detected; local control is locked " + "until this bridge is restarted" + ) + self.foreign_hand_source_seen = True + if self.armed: + self._disarm("foreign BrainCo hand command source detected") + if self.returning_home: + self._finish_home( + "foreign BrainCo hand command source detected", success=False + ) + + def _on_command_topic(self, msg: JointState) -> None: + if msg.header.frame_id != self.FRAME_ID and len(msg.position) in (14, 16): + if not self.foreign_source_seen: + self.get_logger().error( + "foreign /encoder_identical_joint data detected; local control is locked " + "until this bridge is restarted" + ) + self.foreign_source_seen = True + if self.armed: + self._disarm("foreign/cloud arm command source detected") + if self.returning_home: + self._finish_home("foreign/cloud arm command source detected", success=False) + + def _on_home_request( + self, _request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + now = time.monotonic() + sample, source_error = self.source.get() + reasons = self._home_start_reasons(now, sample, source_error) + if self.armed: + reasons.append("manual isomorphic-arm control is armed") + if reasons: + response.success = False + response.message = "return-home rejected: " + "; ".join(reasons) + return response + + self._start_home(now, "operator return-home service") + response.success = True + response.message = ( + "limited-speed return-home accepted; use cancel service or hold left Z + " + "right C for 3s to stop" + ) + return response + + def _on_cancel_home_request( + self, _request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + if not self.returning_home: + response.success = False + response.message = "return-home is not running" + return response + self._finish_home("operator cancel service", success=False, cancelled=True) + response.success = True + response.message = "return-home cancelled; arm command publication stopped" + return response + + def _tick(self) -> None: + now = time.monotonic() + sample, source_error = self.source.get() + if self.source.transport == "omnisocket": + self._update_session_gate(now, sample, source_error) + else: + # Direct-LAN fallback retains the original local raw-button latch. + self._update_combo(now, sample) + # Custom joint-target limits are checked only when arming below. Once + # capture/following starts, the vendor node keeps its own unchanged + # limit protection while this bridge continues the non-limit gates. + startup_reasons = self._safety_reasons(now, sample, source_error) + armed_runtime_reasons = self._safety_reasons( + now, + sample, + source_error, + check_iarm_target=False, + check_hands=self.armed, + # Hand feedback is not an all-teleop teardown gate. A separate + # runtime gate below pauses only hand publication during a status + # gap. Foreign publishers and command-shape validation remain + # active through check_hands. + check_hand_feedback=False, + ) + home_runtime_reasons = self._safety_reasons( + now, + sample, + source_error, + check_iarm=False, + check_iarm_target=False, + check_hands=False, + check_hand_feedback=False, + ) + runtime_hand_reasons = ( + self._hand_feedback_reasons(now) + if self.hands_enabled and self.armed + else [] + ) + self.runtime_hand_output_reasons = runtime_hand_reasons + + if self.armed and armed_runtime_reasons: + self._disarm( + "runtime safety gate failed: " + + "; ".join(armed_runtime_reasons) + ) + if self.returning_home and home_runtime_reasons: + self._finish_home( + "runtime safety gate failed: " + + "; ".join(home_runtime_reasons), + success=False, + ) + + if self.returning_home: + self._tick_home(now) + elif self.armed and sample is not None: + desired = self._positions(sample.data) + command = self._slew_limit(desired, now) + self._publish_target(command, now) + self.last_command = command + if self.hands_enabled: + if runtime_hand_reasons: + if self.hand_output_ready: + self.get_logger().warning( + "BRAINCO HAND OUTPUT PAUSED: " + + "; ".join(runtime_hand_reasons) + ) + self.hand_output_ready = False + else: + if not self.hand_output_ready: + # Resume the hand slew limiter at measured feedback, + # never at the last target from before the status gap. + self.last_hand_commands = { + side: list(self.robot_hand_positions[side] or []) + for side in HAND_SIDES + } + self.last_hand_publish_at = now + self.get_logger().info( + "BRAINCO HAND OUTPUT READY: feedback healthy" + ) + self.hand_output_ready = True + desired_hands = self._hand_targets(sample.data) + commands = { + side: self._slew_hand(side, desired_hands[side], now) + for side in HAND_SIDES + } + self._publish_hands(commands, now) + self.last_hand_commands = commands + if self.locomotion_enabled: + self._tick_locomotion(now, sample) + elif self.locomotion_enabled: + self._tick_walk_zero_burst(now) + + if self.returning_home: + reasons = home_runtime_reasons + elif self.armed: + reasons = armed_runtime_reasons + else: + reasons = startup_reasons + + if now - self.last_status_at >= 0.2: + self._write_status(now, sample, reasons) + self.last_status_at = now + + signature = ( + self.armed, + self.returning_home, + bool(sample), + tuple(reasons), + self.rl_state.get("current_state"), + self.rl_state.get("status"), + ) + if signature != self.last_log_signature: + if self.returning_home: + state = "RETURNING_HOME" + else: + state = "ARMED" if self.armed else "DISARMED" + detail = "all safety gates ready" if not reasons else "; ".join(reasons) + # rclpy associates severity with the Python call site, therefore + # INFO and WARN need separate call sites instead of a bound method. + if reasons: + self.get_logger().warning(f"{state}: {detail}") + else: + self.get_logger().info(f"{state}: {detail}") + self.last_log_signature = signature + + restart_reason = self._source_restart_reason(now, sample) + if restart_reason is not None: + # A server outage can leave the native OmniSocket recv call stuck in + # an apparently connected session. Raising out of the ROS loop lets + # systemd replace the whole process (and native session) cleanly. + # The normal runtime gate above has already stopped publication. + self.get_logger().error(restart_reason) + raise RuntimeError(restart_reason) + + def _source_restart_reason( + self, now: float, sample: ArmSnapshot | None + ) -> str | None: + net_cfg = self.cfg["network"] + if str(net_cfg.get("transport", "zmq")).lower() != "omnisocket": + return None + restart_after = float( + net_cfg.get("omnisocket_restart_after_stale_s", 0.0) + ) + if restart_after <= 0.0: + return None + # No xTELE business frame before START, and no frame after a matching + # STOP, are both normal idle states. Only a session that was last seen + # as START/ACTIVE may use business-frame staleness to rebuild a stuck + # native OmniSocket receiver. + if sample is None: + return None + info = self._teleop_session_info(sample.data) + if info is not None and info[2] == "stop": + return None + stale_for = now - sample.received_at + if stale_for < restart_after: + return None + return ( + f"OmniSocket input stale for {stale_for:.3f}s; exiting so systemd " + "can establish a fresh session" + ) + + @staticmethod + def _teleop_session_info( + data: dict[str, Any] + ) -> tuple[str, int, str, str] | None: + metadata = data.get("tg3_transport") + if not isinstance(metadata, dict): + return None + version = metadata.get("protocol_version") + session_id = metadata.get("session_id") + session_seq = metadata.get("session_seq") + session_state = metadata.get("session_state") + stop_reason = metadata.get("stop_reason", "") + if version != TELEOP_PROTOCOL_VERSION: + return None + if not ( + isinstance(session_id, str) + and len(session_id) == 32 + and all(character in "0123456789abcdef" for character in session_id) + ): + return None + if ( + isinstance(session_seq, bool) + or not isinstance(session_seq, int) + or session_seq <= 0 + ): + return None + if session_state not in ("start", "active", "stop"): + return None + if not isinstance(stop_reason, str): + return None + return session_id, session_seq, session_state, stop_reason + + def _update_session_gate( + self, + now: float, + sample: ArmSnapshot | None, + source_error: str, + ) -> None: + if sample is None: + return + info = self._teleop_session_info(sample.data) + if info is None: + self.last_session_state = "invalid" + if self.armed: + self._disarm("missing or invalid teleoperation session metadata") + self.active_session_id = None + return + + session_id, _session_seq, session_state, stop_reason = info + self.last_session_state = session_state + self.last_session_stop_reason = stop_reason + + if session_state == "active": + if self.armed and session_id != self.active_session_id: + self._disarm("teleoperation session ID changed without START") + self.active_session_id = None + return + + if session_state == "stop": + if session_id != self.active_session_id: + return + was_armed = self.armed + self._disarm("matching teleoperation STOP received") + self.active_session_id = None + if ( + was_armed + and stop_reason == "operator" + and bool(self.cfg["control"].get("auto_home_on_stop", True)) + ): + self._start_home_if_safe( + now, + sample, + source_error, + "operator teleoperation STOP", + ) + return + + # START is repeated for a short bounded window so the receiver's + # latest-frame conflation cannot hide the only arming event. A given + # session is attempted exactly once; a rejected or interrupted session + # cannot arm later without a new physical Z+C cycle and new ID. + if session_id == self.active_session_id and self.armed: + return + if session_id == self.session_start_attempted_id: + return + self.session_start_attempted_id = session_id + if self.armed: + self._disarm("new START arrived while another session was armed") + self.active_session_id = None + return + if not self.allow_publish: + self.get_logger().warning( + "teleoperation START received, but this process is MONITOR-ONLY" + ) + return + + reasons = self._safety_reasons(now, sample, source_error) + if reasons: + self.get_logger().error("cannot arm session: " + "; ".join(reasons)) + return + if self.returning_home: + self._finish_home( + "new teleoperation START accepted", success=False, cancelled=True + ) + self.active_session_id = session_id + self.armed = True + # Start both slew limiters at measured robot feedback, never at a + # potentially distant first network target. + self.last_command = list(self.robot_arm_positions or []) + self.last_publish_at = now + if self.hands_enabled: + self.last_hand_commands = { + side: list(self.robot_hand_positions[side] or []) + for side in HAND_SIDES + } + self.last_hand_publish_at = now + self.get_logger().warning( + "LOCAL DUAL-ARM + BRAINCO HAND CONTROL ARMED by validated EAI " + "teleoperation session; vendor drivers remain active" + ) + + def _update_combo(self, now: float, sample: ArmSnapshot | None) -> None: + pressed = False + if sample is not None: + buttons = sample.data.get("button", {}) + left = buttons.get("left", []) + right = buttons.get("right", []) + # TS1P raw order is left X/Y/Z/... and right A/B/C/.... + pressed = len(left) >= 3 and len(right) >= 3 and bool(left[2]) and bool(right[2]) + + if not pressed: + self.combo_started_at = None + self.combo_latched = False + return + + if self.combo_started_at is None: + self.combo_started_at = now + hold_seconds = float(self.cfg["control"]["start_stop_hold_seconds"]) + if self.combo_latched or now - self.combo_started_at < hold_seconds: + return + + self.combo_latched = True + if self.returning_home: + self._finish_home( + "left Z + right C held: operator cancel", success=False, cancelled=True + ) + return + if self.armed: + self._disarm("left Z + right C held: teleoperation ended") + if bool(self.cfg["control"].get("auto_home_on_stop", True)): + snapshot, source_error = self.source.get() + self._start_home_if_safe( + now, + snapshot, + source_error, + "teleoperation ended by left Z + right C", + ) + return + if not self.allow_publish: + self.get_logger().warning( + "left Z + right C held, but this process is MONITOR-ONLY" + ) + return + snapshot, source_error = self.source.get() + reasons = self._safety_reasons(now, snapshot, source_error) + if reasons: + self.get_logger().error("cannot arm: " + "; ".join(reasons)) + return + self.armed = True + # Start the slew limiter at measured robot feedback. Using None here + # would make the first armed frame jump directly to the TS1P target. + self.last_command = list(self.robot_arm_positions or []) + self.last_publish_at = now + if self.hands_enabled: + self.last_hand_commands = { + side: list(self.robot_hand_positions[side] or []) + for side in HAND_SIDES + } + self.last_hand_publish_at = now + self.get_logger().warning( + "LOCAL DUAL-ARM + BRAINCO HAND CONTROL ARMED; vendor arm and hand " + "drivers remain active" + ) + + def _disarm(self, reason: str) -> None: + was_armed = self.armed + self._stop_locomotion(reason) + self.armed = False + self.hand_output_ready = False + self.runtime_hand_output_reasons = [] + self.last_command = None + self.last_hand_commands = {side: None for side in HAND_SIDES} + if was_armed: + self.get_logger().warning( + f"LOCAL DUAL-ARM + BRAINCO HAND CONTROL DISARMED: {reason}" + ) + + @staticmethod + def _shape_joystick_axis(value: float, deadzone: float, expo: float) -> float: + """Apply the xTELE deadzone and a normalized exponential response.""" + + if not math.isfinite(value): + raise ValueError("joystick axis is non-finite") + if not 0.0 <= deadzone < 1.0: + raise ValueError("joystick deadzone must be in [0, 1)") + if not math.isfinite(expo) or expo <= 0.0: + raise ValueError("joystick exponent must be positive") + value = max(-1.0, min(1.0, value)) + magnitude = abs(value) + if magnitude <= deadzone: + return 0.0 + normalized = (magnitude - deadzone) / (1.0 - deadzone) + return math.copysign(normalized**expo, value) + + def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None: + cfg = self.locomotion_cfg + try: + buttons = sample.data["button"]["right"] + joystick = sample.data["joystick"]["left"] + right_c = len(buttons) >= 3 and bool(buttons[2]) + if len(joystick) != 2: + raise ValueError("left joystick must contain x/y") + # TS1P reports the physical forward/back axis first and the + # left/right axis second. This was verified on the installed + # xTELE 0.1.2 stream; treating the pair as Cartesian x/y made a + # forward stick command become pure yaw. + raw_forward, raw_yaw = (float(joystick[0]), float(joystick[1])) + deadzone = float(cfg["joystick_deadzone"]) + expo = float(cfg["joystick_expo"]) + shaped_forward = self._shape_joystick_axis(raw_forward, deadzone, expo) + shaped_yaw = self._shape_joystick_axis(raw_yaw, deadzone, expo) + signed_forward = shaped_forward * float( + cfg.get("forward_axis_sign", 1.0) + ) + max_forward = float(cfg["max_forward_m_s"]) + max_reverse = float(cfg["max_reverse_m_s"]) + max_angular = float(cfg["max_angular_rad_s"]) + if not all( + math.isfinite(value) and value >= 0.0 + for value in (max_forward, max_reverse, max_angular) + ): + raise ValueError("locomotion limits must be finite and non-negative") + except (KeyError, TypeError, ValueError) as exc: + self._stop_locomotion(f"invalid locomotion input: {exc}") + return + + joystick_active = shaped_forward != 0.0 or shaped_yaw != 0.0 + if not right_c or not joystick_active: + self._stop_locomotion("right C released or left joystick returned to center") + self._tick_walk_zero_burst(now) + return + + if self.walk_combo_started_at is None: + self.walk_combo_started_at = now + hold_seconds = float(cfg["hold_seconds"]) + if now - self.walk_combo_started_at < hold_seconds: + self._tick_walk_zero_burst(now) + return + + if not self.walk_active: + self.walk_active = True + self.walk_zero_frames_remaining = 0 + self.get_logger().warning( + "LOCAL HBWALK VELOCITY STARTED: immediate right C + left " + "joystick input" + ) + + linear_limit = max_forward if signed_forward >= 0.0 else max_reverse + linear_x = signed_forward * linear_limit + angular_z = ( + shaped_yaw + * float(cfg.get("yaw_axis_sign", -1.0)) + * max_angular + ) + self._publish_walk(linear_x, angular_z, now) + + def _stop_locomotion(self, reason: str) -> None: + was_active = self.walk_active + had_nonzero_command = any(abs(value) > 1e-9 for value in self.walk_command) + self.walk_active = False + self.walk_combo_started_at = None + if was_active or had_nonzero_command: + now = time.monotonic() + self._publish_walk(0.0, 0.0, now) + self.walk_zero_frames_remaining = max( + 0, int(self.locomotion_cfg.get("zero_burst_frames", 1)) - 1 + ) + self.get_logger().warning(f"LOCAL HBWALK LOCOMOTION STOPPED: {reason}") + + def _tick_walk_zero_burst(self, now: float) -> None: + if self.walk_zero_frames_remaining <= 0: + return + self._publish_walk(0.0, 0.0, now) + self.walk_zero_frames_remaining -= 1 + + def _publish_walk(self, linear_x: float, angular_z: float, now: float) -> None: + if not self.allow_publish or self.walk_publisher is None: + return + msg = TwistStamped() + msg.header.stamp = self.get_clock().now().to_msg() + # The TG3 secondary-development topic specification uses "pelvis". + msg.header.frame_id = str(self.locomotion_cfg.get("frame_id", "")) + msg.twist.linear.x = float(linear_x) + msg.twist.linear.y = 0.0 + msg.twist.linear.z = 0.0 + msg.twist.angular.x = 0.0 + msg.twist.angular.y = 0.0 + msg.twist.angular.z = float(angular_z) + self.walk_publisher.publish(msg) + self.walk_command = [float(linear_x), float(angular_z)] + self.last_walk_publish_at = now + self.walk_publish_count += 1 + + def _home_start_reasons( + self, + now: float, + sample: ArmSnapshot | None, + source_error: str, + ) -> list[str]: + reasons = self._safety_reasons( + now, + sample, + source_error, + check_iarm=False, + check_iarm_target=False, + check_hands=False, + ) + if not self.allow_publish: + reasons.append("bridge is monitor-only") + if self.returning_home: + reasons.append("return-home is already running") + if self.robot_arm_positions is None: + reasons.append("robot arm position is unavailable") + reasons.extend(self._home_config_reasons()) + return reasons + + def _start_home(self, now: float, reason: str) -> None: + assert self.robot_arm_positions is not None + self.returning_home = True + self.home_command = list(self.robot_arm_positions) + self.home_started_at = now + self.home_settle_started_at = None + self.home_status = "running" + self.last_publish_at = now + self.get_logger().warning( + f"LIMITED-SPEED RETURN-HOME STARTED from measured robot arm state ({reason})" + ) + + def _start_home_if_safe( + self, + now: float, + sample: ArmSnapshot | None, + source_error: str, + reason: str, + ) -> bool: + reasons = self._home_start_reasons(now, sample, source_error) + if reasons: + detail = "; ".join(reasons) + self.home_status = "failed: automatic return-home rejected: " + detail + self.get_logger().error( + f"AUTOMATIC RETURN-HOME REJECTED after {reason}: {detail}" + ) + return False + self._start_home(now, reason) + return True + + def _tick_home(self, now: float) -> None: + if self.home_command is None or self.robot_arm_positions is None: + self._finish_home("home trajectory state is unavailable", success=False) + return + home_cfg = self.cfg["home"] + if now - self.home_started_at > float(home_cfg["timeout_s"]): + self._finish_home("return-home timeout", success=False) + return + + tracking_error = max( + abs(commanded - measured) + for commanded, measured in zip( + self.home_command, self.robot_arm_positions + ) + ) + if tracking_error > float(home_cfg["max_tracking_error_rad"]): + self._finish_home( + f"robot is not following the home trajectory " + f"(max error {tracking_error:.3f} rad)", + success=False, + ) + return + + dt = max(0.001, now - self.last_publish_at) + max_step = float(home_cfg["slew_rad_s"]) * dt + goal = [float(v) for v in home_cfg["joint_goal_rad"]] + if len(goal) != 14: + self._finish_home("configuration must define a 14-joint home goal", success=False) + return + command = [ + current + max(-max_step, min(max_step, target - current)) + for current, target in zip(self.home_command, goal) + ] + self._publish_target(command, now) + self.home_command = command + + command_tolerance = float(home_cfg["command_tolerance_rad"]) + actual_tolerance = float(home_cfg["actual_tolerance_rad"]) + command_at_goal = all( + abs(current - target) <= command_tolerance + for current, target in zip(command, goal) + ) + robot_at_goal = all( + abs(current - target) <= actual_tolerance + for current, target in zip(self.robot_arm_positions, goal) + ) + if command_at_goal and robot_at_goal: + if self.home_settle_started_at is None: + self.home_settle_started_at = now + elif now - self.home_settle_started_at >= float(home_cfg["settle_s"]): + self._finish_home("both arms reached the configured home pose", success=True) + else: + self.home_settle_started_at = None + + def _finish_home( + self, reason: str, success: bool, cancelled: bool = False + ) -> None: + was_running = self.returning_home + self.returning_home = False + self.home_command = None + self.home_settle_started_at = None + if not was_running: + return + if success: + self.home_status = "complete" + self.get_logger().warning(f"LIMITED-SPEED RETURN-HOME COMPLETE: {reason}") + elif cancelled: + self.home_status = "cancelled: " + reason + self.get_logger().warning(f"LIMITED-SPEED RETURN-HOME CANCELLED: {reason}") + else: + self.home_status = "failed: " + reason + self.get_logger().error(f"LIMITED-SPEED RETURN-HOME FAILED: {reason}") + + def _publish_target(self, command: list[float], now: float) -> None: + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.FRAME_ID + msg.name = JOINT_NAMES + msg.position = command + # The vendor node checks both arrays for 14/16 DOF. Zero velocity + # preserves its position-control path and avoids malformed-DOF warnings. + msg.velocity = [0.0] * 14 + self.publisher.publish(msg) + self.last_publish_at = now + + def _publish_hands(self, commands: dict[str, list[int]], now: float) -> None: + mode = int(self.hands_cfg["mode"]) + duration = int(self.hands_cfg["duration_ms"]) + for side in HAND_SIDES: + msg = SetMotorMulti() + msg.mode = mode + msg.positions = commands[side] + msg.speeds = [0] * 6 + msg.currents = [0] * 6 + msg.pwms = [0] * 6 + msg.durations = [duration] * 6 + signature = self._hand_message_signature(msg) + self.expected_hand_messages[side] = (signature, now) + self.hand_publishers[side].publish(msg) + self.last_hand_publish_at = now + self.hand_publish_count += 1 + + def _safety_reasons( + self, + now: float, + sample: ArmSnapshot | None, + source_error: str, + check_iarm: bool = True, + check_iarm_target: bool = True, + check_hands: bool = True, + check_hand_feedback: bool | None = None, + ) -> list[str]: + reasons: list[str] = [] + net_cfg = self.cfg["network"] + robot_cfg = self.cfg["robot"] + control_cfg = self.cfg["control"] + if check_hand_feedback is None: + check_hand_feedback = check_hands + + if check_iarm and sample is None: + reasons.append(source_error or "no isomorphic-arm data") + elif check_iarm: + age = now - sample.received_at + if age > float(net_cfg["source_timeout_s"]): + reasons.append(f"isomorphic-arm data stale ({age:.3f}s)") + data = sample.data + expected_id = str(net_cfg.get("expected_iarm_id", "")) + expected_type = str(net_cfg.get("expected_iarm_type", "")) + if expected_id and data.get("isomorphic_arm_id") != expected_id: + reasons.append("unexpected isomorphic-arm ID") + if expected_type and data.get("isomorphic_arm_type") != expected_type: + reasons.append("unexpected isomorphic-arm type") + + errors = data.get("servo_error", {}) + arm_errors = [*errors.get("left", []), *errors.get("right", [])] + if len(arm_errors) != 14 or any(int(v) != 0 for v in arm_errors): + reasons.append("arm servo error is non-zero or incomplete") + joy_errors = data.get("joycan_error", []) + if len(joy_errors) < 2 or any(int(v) != 0 for v in joy_errors[:2]): + reasons.append("TS1P controller/CAN error") + minimum_hz = float(net_cfg["minimum_arm_frequency_hz"]) + freq = data.get("freq", {}) + if float(freq.get("left", 0.0)) < minimum_hz or float( + freq.get("right", 0.0) + ) < minimum_hz: + reasons.append("TS1P arm sampling frequency too low") + if check_iarm_target: + reasons.extend(self._limit_reasons(self._positions(data), control_cfg)) + if self.hands_enabled and check_hands: + try: + self._hand_targets(data) + except (KeyError, TypeError, ValueError) as exc: + reasons.append(f"invalid isomorphic-hand target: {exc}") + + arm_state_age = now - self.robot_arm_at + if self.robot_arm_at == 0.0 or arm_state_age > float( + robot_cfg["arm_state_timeout_s"] + ): + reasons.append("robot arm state unavailable/stale") + elif len(self.robot_arm_errors) != 14 or any(self.robot_arm_errors): + reasons.append("robot arm motor error is non-zero or incomplete") + + if self.hands_enabled and check_hand_feedback: + reasons.extend(self._hand_feedback_reasons(now)) + + rl_age = now - self.rl_state_at + if self.rl_state_at == 0.0 or rl_age > float(robot_cfg["state_timeout_s"]): + reasons.append("robot RL state unavailable/stale") + else: + required = str(robot_cfg["required_state"]) + if self.rl_state.get("current_state") != required: + reasons.append(f"robot current_state is not {required}") + if self.rl_state.get("child_state") != required: + reasons.append(f"robot child_state is not {required}") + if self.rl_state.get("status") != str(robot_cfg["required_status"]): + reasons.append("robot RL status is not running") + if self.foreign_source_seen: + reasons.append("foreign/cloud arm command source was detected") + if self.hands_enabled and check_hands and self.foreign_hand_source_seen: + reasons.append("foreign BrainCo hand command source was detected") + return reasons + + def _hand_feedback_reasons(self, now: float) -> list[str]: + """Return reasons that should pause hand output, without stopping arms.""" + + reasons: list[str] = [] + hand_timeout = float(self.hands_cfg["status_timeout_s"]) + for side in HAND_SIDES: + age = now - self.robot_hand_at[side] + positions = self.robot_hand_positions[side] + states = self.robot_hand_states[side] + if self.robot_hand_at[side] == 0.0 or age > hand_timeout: + reasons.append(f"robot {side} hand state unavailable/stale") + elif positions is None or len(positions) != 6: + reasons.append(f"robot {side} hand position is incomplete") + elif len(states) != 6 or any( + state not in (0, 1, 2, 3) for state in states + ): + # BrainCo MotorState: 0=idle, 1=running, 2=stall/contact or + # limit, 3=turbo/continuous force, 255=unknown. Running and + # contact are normal operating states; the vendor driver + # retains its own current, stall and limit protection. + reasons.append( + f"robot {side} hand state is unknown/invalid or incomplete" + ) + return reasons + + @staticmethod + def _positions(data: dict[str, Any]) -> list[float]: + position = data["arm"]["position"] + return [float(v) for v in [*position["left"], *position["right"]]] + + def _hand_targets(self, data: dict[str, Any]) -> dict[str, list[int]]: + raw_positions = data["hand"]["position"] + open_pose = [float(value) for value in self.hands_cfg["open_normalized"]] + closed_pose = [float(value) for value in self.hands_cfg["closed_normalized"]] + if len(open_pose) != 6 or len(closed_pose) != 6: + raise ValueError("hand open/closed poses must each contain 6 values") + if not all( + math.isfinite(value) and 0.0 <= value <= 1.0 + for value in [*open_pose, *closed_pose] + ): + raise ValueError("hand open/closed poses must be finite values in [0, 1]") + + minimum = int(self.hands_cfg["position_min"]) + maximum = int(self.hands_cfg["position_max"]) + if minimum < 0 or maximum <= minimum: + raise ValueError("invalid BrainCo position range") + targets: dict[str, list[int]] = {} + for side in HAND_SIDES: + raw = raw_positions[side] + if isinstance(raw, (int, float)) and not isinstance(raw, bool): + scalar = float(raw) + if not math.isfinite(scalar) or scalar < -0.05 or scalar > 1.05: + raise ValueError(f"{side} scalar must be in [0, 1]") + scalar = max(0.0, min(1.0, scalar)) + if bool(self.hands_cfg.get("invert_scalar", False)): + scalar = 1.0 - scalar + normalized = [ + opened + scalar * (closed - opened) + for opened, closed in zip(open_pose, closed_pose) + ] + elif isinstance(raw, list) and len(raw) == 6: + normalized = [float(value) for value in raw] + if not all( + math.isfinite(value) and -0.05 <= value <= 1.05 + for value in normalized + ): + raise ValueError(f"{side} 6-D target must be in [0, 1]") + normalized = [max(0.0, min(1.0, value)) for value in normalized] + else: + raise ValueError(f"{side} target must be a scalar or 6-D list") + targets[side] = [ + int(round(minimum + value * (maximum - minimum))) + for value in normalized + ] + return targets + + @staticmethod + def _limit_reasons(positions: list[float], cfg: dict[str, Any]) -> list[str]: + lower = [float(v) for v in cfg["joint_lower_rad"]] + upper = [float(v) for v in cfg["joint_upper_rad"]] + margin = float(cfg["joint_limit_margin_rad"]) + if len(lower) != 14 or len(upper) != 14: + return ["configuration must define 14 joint limits"] + bad = [ + JOINT_NAMES[i] + for i, value in enumerate(positions) + if value < lower[i] + margin or value > upper[i] - margin + ] + return ["joint target outside safe limit: " + ",".join(bad)] if bad else [] + + def _home_config_reasons(self) -> list[str]: + home_cfg = self.cfg["home"] + try: + goal = [float(v) for v in home_cfg["joint_goal_rad"]] + values = [ + float(home_cfg["slew_rad_s"]), + float(home_cfg["max_tracking_error_rad"]), + float(home_cfg["command_tolerance_rad"]), + float(home_cfg["actual_tolerance_rad"]), + float(home_cfg["settle_s"]), + float(home_cfg["timeout_s"]), + ] + except (KeyError, TypeError, ValueError): + return ["invalid return-home configuration"] + if len(goal) != 14 or not all(math.isfinite(v) for v in [*goal, *values]): + return ["return-home configuration must contain finite values for 14 joints"] + if any(v <= 0.0 for v in values): + return ["return-home speed, tolerances and timeouts must be positive"] + return self._limit_reasons(goal, self.cfg["control"]) + + def _slew_limit(self, desired: list[float], now: float) -> list[float]: + if self.last_command is None: + return list(desired) + dt = max(0.001, now - self.last_publish_at) + max_step = float(self.cfg["control"]["max_slew_rad_s"]) * dt + return [ + previous + max(-max_step, min(max_step, target - previous)) + for previous, target in zip(self.last_command, desired) + ] + + def _slew_hand(self, side: str, desired: list[int], now: float) -> list[int]: + previous = self.last_hand_commands[side] + if previous is None or len(previous) != 6: + return list(desired) + dt = max(0.001, now - self.last_hand_publish_at) + max_step = float(self.hands_cfg["slew_units_per_s"]) * dt + return [ + int(round(current + max(-max_step, min(max_step, target - current)))) + for current, target in zip(previous, desired) + ] + + def _write_status( + self, now: float, sample: ArmSnapshot | None, reasons: list[str] + ) -> None: + combo_elapsed = 0.0 + if self.combo_started_at is not None: + combo_elapsed = now - self.combo_started_at + source_metrics = self.source.metrics() + status = { + "mode": "active-capable" if self.allow_publish else "monitor-only", + "armed": self.armed, + "returning_home": self.returning_home, + "home_status": self.home_status, + "home_speed_rad_s": self.cfg["home"]["slew_rad_s"], + "home_goal_rad": self.cfg["home"]["joint_goal_rad"], + "auto_home_on_stop": bool( + self.cfg["control"].get("auto_home_on_stop", True) + ), + "custom_joint_limit_policy": "startup_only", + "custom_hand_feedback_policy": ( + "startup_qualifies_teleop; runtime_gap_pauses_hands_only" + ), + "runtime_hand_output_ready": self.hand_output_ready, + "runtime_hand_output_reasons": self.runtime_hand_output_reasons, + "safety_ready": not reasons, + "safety_reasons": reasons, + "iarm_transport": self.source.transport, + "iarm_endpoint": self.source.description, + "iarm_transport_status": source_metrics, + "iarm_watchdog_restart_after_s": self.cfg["network"].get( + "omnisocket_restart_after_stale_s" + ), + "iarm_connected": sample is not None, + "iarm_stream_fresh": ( + sample is not None + and now - sample.received_at + <= float(self.cfg["network"]["source_timeout_s"]) + ), + "iarm_age_s": None if sample is None else round(now - sample.received_at, 4), + "iarm_id": None if sample is None else sample.data.get("isomorphic_arm_id"), + "iarm_frequency_hz": None if sample is None else sample.data.get("freq"), + "rl_state": self.rl_state, + "rl_state_age_s": None + if self.rl_state_at == 0.0 + else round(now - self.rl_state_at, 4), + "robot_arm_state_age_s": None + if self.robot_arm_at == 0.0 + else round(now - self.robot_arm_at, 4), + "robot_arm_position_rad": self.robot_arm_positions, + "robot_arm_errors": self.robot_arm_errors, + "foreign_source_seen": self.foreign_source_seen, + "operator_session_state": self.last_session_state, + "operator_session_id": self.active_session_id, + "operator_session_start_attempted_id": self.session_start_attempted_id, + "operator_session_stop_reason": self.last_session_stop_reason, + "brainco_hands_enabled": self.hands_enabled, + "iarm_hand_position": None + if sample is None + else sample.data.get("hand", {}).get("position"), + "robot_hand_positions": self.robot_hand_positions, + "robot_hand_states": self.robot_hand_states, + "robot_hand_state_age_s": { + side: None + if self.robot_hand_at[side] == 0.0 + else round(now - self.robot_hand_at[side], 4) + for side in HAND_SIDES + }, + "last_hand_commands": self.last_hand_commands, + "last_hand_publish_age_s": None + if self.last_hand_publish_at == 0.0 + else round(now - self.last_hand_publish_at, 4), + "hand_publish_count": self.hand_publish_count, + "foreign_hand_source_seen": self.foreign_hand_source_seen, + "locomotion_enabled": self.locomotion_enabled, + "locomotion_binding": "right_C + left_joystick, immediate", + "locomotion_active": self.walk_active, + "locomotion_hold_s": 0.0, + "locomotion_command": { + "linear_x_m_s": self.walk_command[0], + "angular_z_rad_s": self.walk_command[1], + }, + "locomotion_limits": { + "forward_m_s": self.locomotion_cfg.get("max_forward_m_s"), + "reverse_m_s": self.locomotion_cfg.get("max_reverse_m_s"), + "angular_rad_s": self.locomotion_cfg.get("max_angular_rad_s"), + }, + "locomotion_publish_count": self.walk_publish_count, + "locomotion_fsm_publish_enabled": False, + "last_locomotion_publish_age_s": None + if self.last_walk_publish_at == 0.0 + else round(now - self.last_walk_publish_at, 4), + "unsupported_binding": ( + "right_C + right_A (C+A) is not registered by xTELE 0.1.2" + ), + "start_stop_combo": ( + "EAI-gated left_Z + right_C, hold 3s; matching STOP triggers Home" + if self.source.transport == "omnisocket" + else "local left_Z + right_C, hold 3s; stop triggers Home" + ), + "combo_hold_s": round(combo_elapsed, 2), + "last_publish_age_s": None + if self.last_publish_at == 0.0 + else round(now - self.last_publish_at, 4), + "updated_unix_s": time.time(), + } + try: + self.status_file.parent.mkdir(parents=True, exist_ok=True) + temporary = self.status_file.with_suffix(self.status_file.suffix + ".tmp") + temporary.write_text(json.dumps(status, ensure_ascii=False, indent=2) + "\n") + os.replace(temporary, self.status_file) + except OSError as exc: + self.get_logger().error(f"cannot write status file: {exc}") + + +def load_config(path: Path) -> dict[str, Any]: + with path.open("rb") as handle: + return tomllib.load(handle) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--config", type=Path, required=True) + parser.add_argument( + "--allow-publish", + action="store_true", + help=( + "allow a validated EAI START session (or direct-ZMQ fallback " + "combo) to arm ROS publication" + ), + ) + parser.add_argument( + "--status-file", type=Path, default=Path("/tmp/tg3_local_teleop_status.json") + ) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="exit after this many seconds (used for monitor-only validation)", + ) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + config = load_config(args.config) + rclpy.init() + node = LocalTeleopBridge(config, args.allow_publish, args.status_file) + deadline = time.monotonic() + args.duration if args.duration > 0 else None + try: + while rclpy.ok() and (deadline is None or time.monotonic() < deadline): + rclpy.spin_once(node, timeout_sec=0.1) + except KeyboardInterrupt: + pass + finally: + node.close() + node.destroy_node() + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tg3_omnisocket_transport/README.md b/tg3_omnisocket_transport/README.md new file mode 100644 index 0000000..b182ad9 --- /dev/null +++ b/tg3_omnisocket_transport/README.md @@ -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 接收代理。 diff --git a/tg3_omnisocket_transport/omnisocket_xtele_sender.py b/tg3_omnisocket_transport/omnisocket_xtele_sender.py new file mode 100644 index 0000000..c80b081 --- /dev/null +++ b/tg3_omnisocket_transport/omnisocket_xtele_sender.py @@ -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()) diff --git a/tg3_omnisocket_transport/test_session_gate.py b/tg3_omnisocket_transport/test_session_gate.py new file mode 100644 index 0000000..c72b2f8 --- /dev/null +++ b/tg3_omnisocket_transport/test_session_gate.py @@ -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() diff --git a/tg3_omnisocket_transport/tg3-omnisocket-sender.service b/tg3_omnisocket_transport/tg3-omnisocket-sender.service new file mode 100644 index 0000000..724ced3 --- /dev/null +++ b/tg3_omnisocket_transport/tg3-omnisocket-sender.service @@ -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 diff --git a/verify.sh b/verify.sh new file mode 100755 index 0000000..d7361fa --- /dev/null +++ b/verify.sh @@ -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"