diff --git a/.gitignore b/.gitignore index 83097a9..a2d5cae 100644 --- a/.gitignore +++ b/.gitignore @@ -1,21 +1,22 @@ __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 +status.json +status.json.tmp +.pytest_cache/ -# ROS and Python build products are architecture-specific. -ros2_py/build/ -ros2_py/install/ -ros2_py/log/ +# ROS/colcon generated output is rebuilt on the target robot. +tg3_local_teleop/ros2_py/build/ +tg3_local_teleop/ros2_py/install/ +tg3_local_teleop/ros2_py/log/ python/build/ *.egg-info/ *.so +# Local deployment and editor leftovers. +.deploy-*/ .DS_Store -*~ *.bak +*.before-* +*~ diff --git a/README.md b/README.md index 72f2741..1325297 100644 --- a/README.md +++ b/README.md @@ -1,79 +1,61 @@ -# 天工 3.0 TS1P 同构臂本地遥操 +# TG3 天工 3.0 本地同构臂遥操 -本仓库汇总当前已经部署并验证的三部分: - -```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 秒等待。 +本仓库保存 TS1P 同构臂经 OmniSocket 控制天工 3.0 双臂、BrainCo Revo2 +双手和 HBWALK 前后/转向所需的自写代码。它不包含、不修改 xTELE、机器人厂家 +控制源码或 OmniSocketGo 本体。 ## 目录 +- `tg3_omnisocket_transport/`:部署到 EAI 工控机;从 xTELE 的本机 5003/5001 + 读取数据,仅在左 Z + 右 C 连续 3 秒开启后注册并发送,STOP 后立即断开。 +- `tg3_local_teleop/`:部署到机器人 Nvidia;直接接收 OmniSocket 数据,发布双臂、 + 双手及 `/hric/robot/cmd_vel`,并提供限速回 Home。 +- `docs/`:可提交的跨机器人迁移步骤。含实测帧和现场拓扑的汇报/证据文档只保留在 + 当前本地工作副本,不同步到匿名可读的远端仓库。 + +## 外部依赖(不随仓库提交) + +两端都需要单独安装 OmniSocketGo 的 Python 扩展。当前验证版本为: + ```text -tg3_omnisocket_transport/ EAI 发送端、用户服务和离线测试 -tg3_local_teleop/ 机器人桥、配置、用户服务、ROS 消息和离线测试 -OmniSocketGo/ 固定版本的传输依赖(Git 子模块) -docs/ 部署汇报、迁移指南和 Topic/SBUS 证据记录 -verify.sh 不连接机器人、不发布 ROS 的离线检查 +https://gitea.public.snrc.site/limingjie/OmniSocketGo.git +commit de3f5c96779dbe1571c10feb22fc7f2331b6b222 ``` -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 和机器人侧厂家速度许可。 - -完整流程见 +EAI 还需已安装并运行 xTELE;机器人端需已有 ROS 2 Jazzy、厂家消息包与驱动。 +详见 [`docs/天工3.0本地同构臂遥操迁移部署指南.md`](docs/天工3.0本地同构臂遥操迁移部署指南.md)。 -## 构建传输依赖 +OmniSocket Python 扩展包含本机架构代码,必须分别在 EAI 和机器人上从上述固定提交 +原生执行 `make python-ext`,不能复制另一种 CPU 或 Python 版本生成的 `.so`。 -OmniSocket Python 扩展包含本机架构代码,必须分别在 EAI 和机器人上原生编译,不能 -复制另一种 CPU/Python 版本生成的 `.so`: +提交或部署前可运行 `./verify.sh` 完成两端协议、手势、接收端刷新、Python 编译和 +TOML 解析检查。若还要核对外置 OmniSocketGo 版本,可设置任务专用环境变量 +`OMNISOCKETGO_DIR=/path/to/OmniSocketGo` 后再运行。 -```bash -cd OmniSocketGo -make python-ext -``` +## 当前按键 -EAI 和机器人部署命令、systemd 用户服务安装及完整重启顺序分别记录在两个项目的 -README 和 `docs/` 中。 +- 左 Z + 右 C 连续 3 秒:开始遥操;再次连续 3 秒:结束并限速回 Home。 +- 右 C + 左摇杆上下:HBWALK 前进/后退。 +- 左 Z + 右摇杆左右:HBWALK 原地转向。 +- 右 B 连续 3 秒:右手进入厂商“单食指”姿态;松开至少 0.5 秒后再次连续 + 3 秒退出。手指动作继续受 `400 units/s` 限速。 + +## 迁移前必须修改 + +至少核对并修改服务器地址、两端 Peer ID、同构臂 ID、直连回退 IP、安装用户名和 +路径。`config.toml` 中的 14 关节 Home 是当前实机采集值,不能直接用于另一台机器人; +必须在新机器人上重新采集并完成低速验证。所有厂家默认限位、碰撞、电流和急停保护 +保持不变。 + +运行产生的 `status.json`、日志、Python 缓存及 ROS 构建目录已由 `.gitignore` +排除。 ## 安全与仓库可见性 -这是物理机器人控制项目。执行 `--allow-publish`、启动用户服务或回 Home 前,必须确认 -机器人处于 HBWALK、防护和急停有效、双臂及行走区域净空。 +这是物理机器人控制项目。启动带 `--allow-publish` 的服务、测试手势、行走或回 Home +前,必须确认机器人处于 HBWALK,防护和急停有效,双臂、灵巧手与行走区域净空。 -仓库包含现场 Hub 地址、设备/Peer ID、内网地址、实机 Home 姿态以及标记为 -`Proprietary` 的 ROS 消息定义,建议 Gitea 仓库保持私有。运行产生的 `status.json`、 -日志、缓存和本机编译文件已由 `.gitignore` 排除。原厂 SDK 文档和 PDF 未收入仓库, -避免上传其中的下载授权码、Wi-Fi 信息及版权资料。 +仓库保留了现场 Hub 地址、设备/Peer ID、内网地址、当前实机 Home 姿态,以及标记为 +`Proprietary` 的 ROS 消息定义,建议 Gitea 仓库保持私有。原厂 SDK 文档和 PDF 未收入 +仓库,避免上传其中的下载授权码、Wi-Fi 信息及版权资料。 diff --git a/docs/天工3.0本地同构臂遥操迁移部署指南.md b/docs/天工3.0本地同构臂遥操迁移部署指南.md index 3ae9cde..cd53ce1 100644 --- a/docs/天工3.0本地同构臂遥操迁移部署指南.md +++ b/docs/天工3.0本地同构臂遥操迁移部署指南.md @@ -91,6 +91,7 @@ python3 -c 'import json; p="/home/nvidia/tg3_local_teleop/status.json"; d=json.l /right_hand/set_motor_multi /left_hand/motor_status /right_hand/motor_status +/hric/robot/cmd_vel ``` 若新机器人不是天工 3.0、SDK 版本接口有变化、不是 14 维双臂或不是 BrainCo Revo2,先停止 @@ -152,7 +153,7 @@ ssh nvidia@"${TG3_NEW_ROBOT_IP}" \ rsync -av \ --exclude=__pycache__ \ --exclude=status.json \ - /home/ps/Downloads/tg3_local_teleop/ \ + /home/ps/Downloads/TG3/tg3_local_teleop/ \ nvidia@"${TG3_NEW_ROBOT_IP}":/home/nvidia/tg3_local_teleop/ ssh nvidia@"${TG3_NEW_ROBOT_IP}" \ @@ -184,6 +185,8 @@ joint_goal_rad = [ ...新机器人实测的 14 个 Home 值... ] 还要检查: - `[hands]` 是否与新机器人的真实手型相符; +- `[hands].right_b_point_pose_normalized` 是否适用于新 BrainCo 手的校准;首次只在净空、 + 急停可用且低速限制生效时测试; - `[control].joint_lower_rad/joint_upper_rad` 是否仍适用于同型号和当前 SDK; - `run.sh`、service 内的用户名和路径是否仍为 `/home/nvidia`; - ROS 安装路径是否仍有 `/opt/ros/jazzy`、`/home/nvidia/xos` 或 @@ -312,6 +315,8 @@ Environment=PYTHONPATH=/home/<新EAI用户>/OmniSocketGo/python --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 +--combo-release-s 0.5 +--start-marker-frames 500 --max-feedback-age-ms 500 --max-pending-frames 100 Restart=always @@ -392,14 +397,19 @@ omnisocket_server = "203.0.113.10:15000" 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,确认 +8. 保持右摇杆回中,连续长按右 B 3 秒,确认右手以 `400 units/s` 限速进入厂商 + “单食指”姿态;松开至少 0.5 秒后再次长按 3 秒,确认退出并恢复正常右手跟随; +9. 先停止并确认 `armed=false`,按飞书指南逐个选择其他手势组合键;检查 EAI + `status.json` 的 `command_frames_accepted` 增长,并在 EAI 本机 5001 抓帧确认处理后的 + `hand.position` 为六维数组;待机不会构包,`command_hand_merges` 此时不应增长; +10. 再次武装后检查 `command_hand_merges` 增长和机器人 `iarm_hand_position` 为六维, + 再只做小幅动作,逐个验证所需的其他组合手势方向和限速; +11. 保持右 C,把左摇杆小幅向前推出死区,确认下一个 50 Hz 周期立即响应;松 C,确认 立即零速停止。再次按 C/推杆也应立即响应,不再等待 3 秒; -11. 再次长按停止,观察双臂限速回到新机器人保存的 Home; -12. 测试 Hub 短暂断线:应在 0.25 秒后停止发布,恢复后先确认未武装,再重新长按启动; -13. 记录最终 Peer ID、Hub、SSH 地址、Home、行走限速和手部端点到该机器人的设备档案。 +12. 保持左 Z,把右摇杆小幅横推,确认机器人原地转向;松开任一输入应立即清零角速度; +13. 再次长按 Z+C 停止,观察双臂限速回到新机器人保存的 Home; +14. 测试 Hub 短暂断线:应在 0.25 秒后停止发布,恢复后先确认未武装,再重新长按启动; +15. 记录最终 Peer ID、Hub、SSH 地址、Home、行走限速、手部端点和单食指姿态到该机器人的设备档案。 迁移时确认目标机提供 `/hric/robot/cmd_vel`(`geometry_msgs/msg/TwistStamped`),并保持 `locomotion.command_topic` 与目标固件一致。行走限速、死区、曲线和方向符号均在机器人 diff --git a/tg3_local_teleop/README.md b/tg3_local_teleop/README.md index 0261da0..f4acecf 100644 --- a/tg3_local_teleop/README.md +++ b/tg3_local_teleop/README.md @@ -13,13 +13,15 @@ TS1P 同构臂 -> 天工 3.0 双臂 -> /left_hand/set_motor_multi + /right_hand/set_motor_multi -> 天工 3.0 BrainCo Revo2 双灵巧手 - -> /hric/robot/cmd_vel(右 C + 左摇杆;50 Hz TwistStamped) + -> /hric/robot/cmd_vel(右 C + 左摇杆前后 / 左 Z + 右摇杆转向;50 Hz TwistStamped) -> 天工 3.0 HBWALK 行走 ``` ## 自动运行与操作 - 桥接服务随机器人算力主机的用户服务自动启动;未进入 `HBWALK` 时只监测、不发布。 +- 用户服务启动前会用短生命周期 ROS 探针等待 `/hric/robot/rl_state` 可发现,再创建 + 长生命周期桥接节点,避免开机早期网络接口尚未就绪时 Fast DDS 固化为空接口。 - 开始遥操:进入 `HBWALK` 后,同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI 本机完成;计时未通过前不向 Hub 发送 xTELE 业务帧。 - 结束遥操:再次同时长按 3 秒;EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据, @@ -27,18 +29,29 @@ TS1P 同构臂 - 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。 - 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK 前后/转向;不控制腰和头。 -- 行走:遥操已启动后,右 `C` 是即时 deadman;按住 C 并把左摇杆推出死区后,在下一个 - `50 Hz` 周期立即响应,不再等待 3 秒。松开 C 或摇杆回中立即发零速,再按也立即恢复。 - 左摇杆上下控制前后,左右控制原地转向。 +- 行走:遥操已启动后,右 `C` + 左摇杆上下控制前后,左 `Z` + 右摇杆左右控制原地 + 转向;两个组合都在下一个 `50 Hz` 周期立即响应,不再等待 3 秒。松开对应按键或 + 摇杆回中立即把该轴清零,再按也立即恢复。 + 前后仍使用二次细控曲线;转向在死区后使用线性曲线,使右摇杆中段有足够角速度, + 但最大值仍受官方 `0.8 rad/s` 上限约束。 按官方半身行走 Topic 范围限幅:前进 `1.0 m/s`、后退 `0.8 m/s`、转向 `0.8 rad/s`;死区 `0.2`,输出频率 `50 Hz`。 -- xTELE 0.1.2 的左摇杆原始数组顺序是“前后、左右”;前后映射到 `linear.x`,左右 +- 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 后续直接输出六维归一化位置,会自动使用六维位置。 +- 右手指向手势:遥操已启动且灵巧手反馈健康时,连续长按右 `B` 3 秒开启;松开 + 至少 `0.5 s` 后,再连续长按右 `B` 3 秒关闭。首次启动和每个新遥操会话都必须先 + 稳定松开 B,保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈 + 中断均不触发;计时期间冻结右手最后一条命令,左手和双臂仍照常跟随。 +- 指向只覆盖右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号 + “单食指”手势 `state 0`:归一化目标 + `[0.2, 0.688, 0.0, 0.98, 0.98, 0.98]`,对应当前 `1~1000` 位置范围约为 + `[201, 688, 1, 980, 980, 980]`;顺序为大拇指弯曲、大拇指旋转、食指、中指、 + 无名指、小拇指。进入和退出手势都继续使用每秒最多 400 个位置单位的现有限速。 ## 安全门控 @@ -53,7 +66,7 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` `angular.z=[-0.8, 0.8] rad/s`;本桥不发布侧移速度。 行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆 -即时成立时发布。解除组合后立即发布零速,并补发 10 帧零速。公网断线、退出遥操、 +前后或左 Z + 右摇杆横向即时成立时发布。解除组合后立即发布零速,并补发 10 帧零速。公网断线、退出遥操、 回 Home 或机器人退出 HBWALK 也走同一停止逻辑。 灵巧手仅在同一次长按武装后发布;以机器人真实手指位置为起点,并按每秒最多 400 个 @@ -61,6 +74,8 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 和 `3`(持续力)均为厂家定义的正常运行状态;仅状态失联、未知/非法状态、同构臂失联 或检测到另一命令源时,双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持 最后目标,双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。 +匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态,但不会在 +STOP 后额外发送张手/恢复命令;下一次会话从机器人实测手指位置重新限速跟随。 ## 限速双臂回 Home diff --git a/tg3_local_teleop/config.toml b/tg3_local_teleop/config.toml index 8577fd6..f3e76c2 100644 --- a/tg3_local_teleop/config.toml +++ b/tg3_local_teleop/config.toml @@ -5,6 +5,11 @@ 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 deployed OmniSocket receiver does not exchange a Hub heartbeat while idle. +# Re-register periodically with make-before-break so a Hub restart cannot leave +# a false connected state and healthy refreshes never create a routing gap. +# Active traffic resets this timer and is never interrupted by this refresh. +omnisocket_idle_session_refresh_s = 30.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. @@ -52,8 +57,9 @@ joint_upper_rad = [ ] [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. +# The two original immediate bindings are independent: right C + left-stick +# vertical controls translation; left Z + right-stick horizontal controls +# in-place yaw. Both act on the next 50 Hz tick, with no second hold. enabled = true command_topic = "/hric/robot/cmd_vel" # This bridge never publishes FSM commands. The robot must already report @@ -63,6 +69,10 @@ frame_id = "pelvis" hold_seconds = 0.0 joystick_deadzone = 0.2 joystick_expo = 2.0 +# Turning is linear after the deadzone so medium right-stick travel is not +# suppressed by the quadratic forward-motion curve. The official yaw cap is +# unchanged. +yaw_joystick_expo = 1.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 @@ -99,6 +109,17 @@ invert_scalar = false open_normalized = [0.4, 0.4, 0.05, 0.05, 0.05, 0.05] closed_normalized = [0.9, 0.5, 0.45, 0.52, 0.52, 0.45] +# Right B owns a robot-side, release-guarded pointing gesture while a teleop +# session is armed. Both activation and deactivation require one continuous +# 3-second hold, separated by at least 0.5 seconds of stable release. A new +# session is always release-locked. The pose is xTELE GestureController's +# BrainCoRevo2 gesture 2 ("single index finger"), state 0, in motor order: +# thumb bend, thumb rotation, index, middle, ring, little. +right_b_point_gesture_enabled = true +right_b_point_gesture_hold_seconds = 3.0 +right_b_point_gesture_release_seconds = 0.5 +right_b_point_pose_normalized = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98] + [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, diff --git a/tg3_local_teleop/gesture_toggle.py b/tg3_local_teleop/gesture_toggle.py new file mode 100644 index 0000000..bc7f94f --- /dev/null +++ b/tg3_local_teleop/gesture_toggle.py @@ -0,0 +1,243 @@ +#!/usr/bin/env python3 +"""Pure state machine for the right-B pointing-hand gesture. + +This module deliberately has no ROS dependencies so the hold/release safety +rules can be exercised offline before the bridge is deployed on a robot. +""" + +from __future__ import annotations + +import math +from typing import Any, Mapping, Sequence + + +class GestureToggle: + """Toggle one persistent gesture with a guarded long press. + + A new session always starts release-locked. A continuous release clears + that lock, then a continuous press toggles the gesture once. The same + physical press cannot toggle twice; another stable release is required. + """ + + def __init__(self, hold_seconds: float, release_seconds: float) -> None: + if not math.isfinite(hold_seconds) or hold_seconds <= 0.0: + raise ValueError("gesture hold time must be positive and finite") + if not math.isfinite(release_seconds) or release_seconds <= 0.0: + raise ValueError("gesture release time must be positive and finite") + self.hold_seconds = float(hold_seconds) + self.release_seconds = float(release_seconds) + self.active = False + self.hold_started_at: float | None = None + self.release_started_at: float | None = None + self.require_release = True + self.armed = False + self.input_healthy = False + self.button_pressed: bool | None = None + self._freeze_right_hand = False + self.toggle_count = 0 + self.last_transition = "initialized" + + def new_session(self) -> None: + """Start release-locked and never carry a gesture across sessions.""" + + self.active = False + self.hold_started_at = None + self.release_started_at = None + self.require_release = True + self.armed = True + self.input_healthy = False + self.button_pressed = None + self._freeze_right_hand = True + self.last_transition = "new_session" + + def disarm(self, reason: str = "disarmed") -> None: + """Clear the logical gesture without commanding any hand movement.""" + + self.active = False + self.hold_started_at = None + self.release_started_at = None + self.require_release = True + self.armed = False + self.input_healthy = False + self.button_pressed = None + self._freeze_right_hand = False + self.last_transition = reason + + def update( + self, + now: float, + *, + armed: bool, + input_healthy: bool, + pressed: bool | None, + ) -> bool: + """Advance the state and return ``True`` only when a toggle occurs. + + ``pressed=None`` represents a missing or malformed B-button sample. It + cancels a pending hold and requires a fresh stable release. A hand + feedback gap behaves the same way, but an already active gesture is + retained so it can resume through the bridge's measured-feedback slew. + """ + + if not math.isfinite(now): + raise ValueError("gesture clock must be finite") + if not armed: + if self.armed or self.active: + self.disarm() + return False + if not self.armed: + self.new_session() + + self.input_healthy = bool(input_healthy) + self.button_pressed = pressed + if not input_healthy or pressed is None: + self.hold_started_at = None + self.release_started_at = None + self.require_release = True + if not self.active: + self._freeze_right_hand = True + return False + + if self.require_release: + self.hold_started_at = None + if pressed: + self.release_started_at = None + return False + if self.release_started_at is None: + self.release_started_at = now + return False + if now - self.release_started_at >= self.release_seconds: + self.require_release = False + self.release_started_at = None + if not self.active: + self._freeze_right_hand = False + return False + + self.release_started_at = None + if not pressed: + if self.hold_started_at is not None: + # A short/interrupted press is not a toggle. Debounce the + # release before another hold can start. + self.hold_started_at = None + self.release_started_at = now + self.require_release = True + if not self.active: + self._freeze_right_hand = True + return False + + if self.hold_started_at is None: + self.hold_started_at = now + if not self.active: + self._freeze_right_hand = True + return False + if now - self.hold_started_at < self.hold_seconds: + return False + + self.active = not self.active + self.toggle_count += 1 + self.last_transition = "activated" if self.active else "deactivated" + # Activation uses the point target. Deactivation starts slewing back + # to live hand input immediately; the release lock only prevents a + # second toggle from the same physical press. + self._freeze_right_hand = False + self.hold_started_at = None + self.release_started_at = None + self.require_release = True + return True + + @property + def freeze_right_hand(self) -> bool: + """Whether the bridge must hold its last right-hand command. + + While inactive, freeze through an incomplete B hold and its required + release. A completed deactivation starts returning to live input at + once; its release lock prevents only another toggle. This keeps a + short B press from leaking through xTELE's processed command stream. + """ + + return self.armed and not self.active and self._freeze_right_hand + + def hold_elapsed(self, now: float) -> float: + if self.hold_started_at is None: + return 0.0 + return max(0.0, now - self.hold_started_at) + + def release_elapsed(self, now: float) -> float: + if self.release_started_at is None: + return 0.0 + return max(0.0, now - self.release_started_at) + + @property + def state(self) -> str: + if not self.armed: + return "disarmed" + if not self.input_healthy: + return "input_unhealthy" + if self.button_pressed is None: + return "invalid_button" + if self.require_release: + return "awaiting_release" + if self.hold_started_at is not None: + return "holding_stop" if self.active else "holding_start" + return "active" if self.active else "idle" + + +def right_b_pressed(data: Mapping[str, Any]) -> bool | None: + """Return TS1P right-B state, or ``None`` for a malformed sample.""" + + try: + buttons = data["button"] + if not isinstance(buttons, Mapping): + return None + right = buttons["right"] + if not isinstance(right, (list, tuple)) or len(right) < 2: + return None + value = right[1] + if isinstance(value, bool): + return value + if isinstance(value, int) and value in (0, 1): + return bool(value) + return None + except (KeyError, TypeError): + return None + + +def normalized_pose_to_positions( + pose: Sequence[float], minimum: int, maximum: int +) -> list[int]: + """Convert a six-motor normalized BrainCo pose to driver positions.""" + + if len(pose) != 6: + raise ValueError("gesture pose must contain 6 normalized values") + values = [float(value) for value in pose] + if not all(math.isfinite(value) and 0.0 <= value <= 1.0 for value in values): + raise ValueError("gesture pose must contain finite values in [0, 1]") + if minimum < 0 or maximum <= minimum: + raise ValueError("invalid BrainCo position range") + return [ + int(round(minimum + value * (maximum - minimum))) for value in values + ] + + +def select_right_hand_target( + source_target: Sequence[int], + previous_command: Sequence[int] | None, + gesture_target: Sequence[int], + *, + active: bool, + freeze: bool, +) -> list[int]: + """Apply the right-only gesture override or fail-safe input freeze.""" + + source = list(source_target) + gesture = list(gesture_target) + if len(source) != 6 or len(gesture) != 6: + raise ValueError("right-hand targets must each contain 6 positions") + if active: + return gesture + if freeze: + previous = [] if previous_command is None else list(previous_command) + if len(previous) != 6: + raise ValueError("cannot freeze without a complete previous command") + return previous + return source diff --git a/tg3_local_teleop/test_gesture_toggle.py b/tg3_local_teleop/test_gesture_toggle.py new file mode 100644 index 0000000..a33c231 --- /dev/null +++ b/tg3_local_teleop/test_gesture_toggle.py @@ -0,0 +1,268 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import unittest + +from gesture_toggle import ( + GestureToggle, + normalized_pose_to_positions, + right_b_pressed, + select_right_hand_target, +) + + +POINT_POSE = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98] + + +class GestureToggleTest(unittest.TestCase): + def setUp(self) -> None: + self.gesture = GestureToggle(hold_seconds=3.0, release_seconds=0.5) + self.gesture.new_session() + + def stable_release(self, started_at: float) -> float: + self.assertFalse( + self.gesture.update( + started_at, armed=True, input_healthy=True, pressed=False + ) + ) + finished_at = started_at + 0.5 + self.assertFalse( + self.gesture.update( + finished_at, armed=True, input_healthy=True, pressed=False + ) + ) + self.assertFalse(self.gesture.require_release) + return finished_at + + def test_new_session_held_button_cannot_activate(self) -> None: + self.assertFalse( + self.gesture.update(0.0, armed=True, input_healthy=True, pressed=True) + ) + self.assertFalse( + self.gesture.update(10.0, armed=True, input_healthy=True, pressed=True) + ) + self.assertFalse(self.gesture.active) + self.assertTrue(self.gesture.freeze_right_hand) + + def test_continuous_three_second_holds_toggle_once_each(self) -> None: + released_at = self.stable_release(0.0) + self.assertFalse( + self.gesture.update( + released_at + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + ) + self.assertFalse( + self.gesture.update( + released_at + 3.0, + armed=True, + input_healthy=True, + pressed=True, + ) + ) + self.assertTrue( + self.gesture.update( + released_at + 3.01, + armed=True, + input_healthy=True, + pressed=True, + ) + ) + self.assertTrue(self.gesture.active) + self.assertEqual(self.gesture.toggle_count, 1) + + # Keeping the same physical press held cannot turn the gesture off. + self.assertFalse( + self.gesture.update( + released_at + 10.0, + armed=True, + input_healthy=True, + pressed=True, + ) + ) + self.assertTrue(self.gesture.active) + + second_release = self.stable_release(released_at + 10.01) + self.gesture.update( + second_release + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + self.assertTrue( + self.gesture.update( + second_release + 3.02, + armed=True, + input_healthy=True, + pressed=True, + ) + ) + self.assertFalse(self.gesture.active) + self.assertEqual(self.gesture.toggle_count, 2) + self.assertFalse(self.gesture.freeze_right_hand) + + def test_short_press_freezes_and_requires_stable_release(self) -> None: + released_at = self.stable_release(0.0) + self.gesture.update( + released_at + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + self.assertTrue(self.gesture.freeze_right_hand) + self.assertEqual(self.gesture.state, "holding_start") + + self.gesture.update( + released_at + 2.0, + armed=True, + input_healthy=True, + pressed=False, + ) + self.assertFalse(self.gesture.active) + self.assertTrue(self.gesture.require_release) + self.assertTrue(self.gesture.freeze_right_hand) + self.gesture.update( + released_at + 2.49, + armed=True, + input_healthy=True, + pressed=False, + ) + self.assertTrue(self.gesture.require_release) + self.gesture.update( + released_at + 2.5, + armed=True, + input_healthy=True, + pressed=False, + ) + self.assertFalse(self.gesture.require_release) + self.assertFalse(self.gesture.freeze_right_hand) + + def test_invalid_button_or_feedback_gap_cancels_pending_hold(self) -> None: + released_at = self.stable_release(0.0) + self.gesture.update( + released_at + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + self.gesture.update( + released_at + 2.99, + armed=True, + input_healthy=True, + pressed=None, + ) + self.assertTrue(self.gesture.require_release) + self.assertIsNone(self.gesture.hold_started_at) + + released_at = self.stable_release(released_at + 3.0) + self.gesture.update( + released_at + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + self.gesture.update( + released_at + 2.99, + armed=True, + input_healthy=False, + pressed=True, + ) + self.assertFalse(self.gesture.active) + self.assertTrue(self.gesture.require_release) + self.assertIsNone(self.gesture.hold_started_at) + + def test_feedback_gap_preserves_already_active_gesture(self) -> None: + released_at = self.stable_release(0.0) + self.gesture.update( + released_at + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + self.assertTrue( + self.gesture.update( + released_at + 3.02, + armed=True, + input_healthy=True, + pressed=True, + ) + ) + self.gesture.update( + released_at + 3.03, + armed=True, + input_healthy=False, + pressed=True, + ) + self.assertTrue(self.gesture.active) + self.assertTrue(self.gesture.require_release) + + def test_disarm_clears_active_without_an_output_action(self) -> None: + released_at = self.stable_release(0.0) + self.gesture.update( + released_at + 0.01, + armed=True, + input_healthy=True, + pressed=True, + ) + self.gesture.update( + released_at + 3.02, + armed=True, + input_healthy=True, + pressed=True, + ) + self.assertTrue(self.gesture.active) + self.assertFalse( + self.gesture.update( + released_at + 3.03, + armed=False, + input_healthy=True, + pressed=True, + ) + ) + self.assertFalse(self.gesture.active) + self.assertEqual(self.gesture.state, "disarmed") + self.assertTrue(self.gesture.require_release) + + +class GestureHelpersTest(unittest.TestCase): + def test_right_b_is_index_one_and_malformed_values_fail_closed(self) -> None: + self.assertTrue(right_b_pressed({"button": {"right": [0, 1, 0]}})) + self.assertFalse(right_b_pressed({"button": {"right": [1, 0, 1]}})) + self.assertIsNone(right_b_pressed({"button": {"right": [0]}})) + self.assertIsNone(right_b_pressed({"button": {"right": [0, -1]}})) + self.assertIsNone(right_b_pressed({"button": {"right": [0, 0.5]}})) + + def test_point_pose_converts_to_expected_brainco_positions(self) -> None: + self.assertEqual( + normalized_pose_to_positions(POINT_POSE, 1, 1000), + [201, 688, 1, 980, 980, 980], + ) + + def test_right_only_override_and_short_press_freeze(self) -> None: + source = [401, 401, 51, 51, 51, 51] + previous = [450, 430, 80, 90, 100, 110] + point = [201, 688, 1, 980, 980, 980] + self.assertEqual( + select_right_hand_target( + source, previous, point, active=True, freeze=False + ), + point, + ) + self.assertEqual( + select_right_hand_target( + source, previous, point, active=False, freeze=True + ), + previous, + ) + self.assertEqual( + select_right_hand_target( + source, previous, point, active=False, freeze=False + ), + source, + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/tg3_local_teleop/test_idle_session_refresh.py b/tg3_local_teleop/test_idle_session_refresh.py new file mode 100644 index 0000000..e2eff11 --- /dev/null +++ b/tg3_local_teleop/test_idle_session_refresh.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import importlib.util +import json +from pathlib import Path +import struct +import sys +import time +from types import ModuleType +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=object) +for package, message_name in ( + ("brainco_hand_msgs.msg", "MotorStatus"), + ("brainco_hand_msgs.msg", "SetMotorMulti"), + ("diagnostic_msgs.msg", "DiagnosticStatus"), + ("geometry_msgs.msg", "TwistStamped"), + ("ros2_bridge_msgs.msg", "ArmStatus"), + ("sensor_msgs.msg", "JointState"), + ("std_srvs.srv", "Trigger"), +): + module = sys.modules.get(package) or install_module(package) + setattr(module, message_name, Dummy) + + +fake_omnisocket = install_module( + "omnisocket", CONTROL_DEFAULTS={}, MSG_TYPE_BINARY=2 +) + +module_path = Path(__file__).with_name("tg3_local_teleop.py") +spec = importlib.util.spec_from_file_location("tg3_local_teleop", module_path) +assert spec is not None and spec.loader is not None +teleop = importlib.util.module_from_spec(spec) +sys.modules[spec.name] = teleop +spec.loader.exec_module(teleop) + + +class FakeSession: + mode = "idle" + connect_count = 0 + sequence = 0 + sent_one = False + next_instance_id = 0 + events: list[str] = [] + + def __init__(self) -> None: + type(self).next_instance_id += 1 + self.instance_id = type(self).next_instance_id + + def connect(self, **_kwargs: object) -> None: + type(self).connect_count += 1 + type(self).events.append(f"connect:{self.instance_id}") + + def stats(self) -> dict[str, int]: + return {"connected": 1, "registered": 1} + + def recv(self, timeout_ms: int) -> tuple[str, int, bytes] | None: + if timeout_ms == 0: + return None + time.sleep(0.003) + if self.mode == "idle": + return None + if self.mode == "one_then_idle" and type(self).sent_one: + return None + type(self).sent_one = True + type(self).sequence += 1 + data = { + "arm": { + "position": {"left": [0.0] * 7, "right": [0.0] * 7} + } + } + payload = json.dumps(data).encode() + packet = struct.pack( + "!4sQQI", + b"TG3A", + type(self).sequence, + time.time_ns(), + len(payload), + ) + payload + return "expected-sender", 2, packet + + def close(self) -> None: + type(self).events.append(f"close:{self.instance_id}") + return None + + +def config(refresh_s: float) -> dict[str, object]: + return { + "transport": "omnisocket", + "omnisocket_server": "127.0.0.1:14049", + "omnisocket_peer_id": "robot", + "omnisocket_expected_sender": "expected-sender", + "omnisocket_max_packet_age_ms": 300.0, + "omnisocket_idle_session_refresh_s": refresh_s, + } + + +class IdleSessionRefreshTest(unittest.TestCase): + def setUp(self) -> None: + FakeSession.connect_count = 0 + FakeSession.sequence = 0 + FakeSession.sent_one = False + FakeSession.next_instance_id = 0 + FakeSession.events = [] + fake_omnisocket.Session = FakeSession + + def run_source(self, mode: str, run_s: float = 0.13) -> dict[str, object]: + FakeSession.mode = mode + source = teleop.LatestArmData(config(0.02)) + source.start() + time.sleep(run_s) + source.close() + return source.metrics() + + def test_idle_session_is_periodically_reconnected(self) -> None: + metrics = self.run_source("idle") + self.assertGreaterEqual(FakeSession.connect_count, 2) + self.assertGreaterEqual(metrics["idle_session_refreshes"], 1) + self.assertEqual( + FakeSession.events[:3], + ["connect:1", "connect:2", "close:1"], + ) + + def test_continuous_valid_business_frames_prevent_refresh(self) -> None: + metrics = self.run_source("active", 0.08) + self.assertEqual(FakeSession.connect_count, 1) + self.assertEqual(metrics["idle_session_refreshes"], 0) + self.assertGreater(metrics["frames_accepted"], 1) + + def test_session_refreshes_after_last_valid_frame(self) -> None: + metrics = self.run_source("one_then_idle") + self.assertGreaterEqual(FakeSession.connect_count, 2) + self.assertGreaterEqual(metrics["idle_session_refreshes"], 1) + self.assertEqual(metrics["frames_accepted"], 1) + + def test_refresh_boundary_and_disable_switch(self) -> None: + due = teleop.LatestArmData._idle_session_refresh_due + self.assertFalse(due(10.0, 0.0, 0.0)) + self.assertFalse(due(1.999, 0.0, 2.0)) + self.assertTrue(due(2.0, 0.0, 2.0)) + + +if __name__ == "__main__": + unittest.main() diff --git a/tg3_local_teleop/test_session_gate.py b/tg3_local_teleop/test_session_gate.py index 050b745..dcae4c0 100644 --- a/tg3_local_teleop/test_session_gate.py +++ b/tg3_local_teleop/test_session_gate.py @@ -79,6 +79,7 @@ class RobotSessionGateTest(unittest.TestCase): bridge.allow_publish = True bridge.cfg = {"control": {"auto_home_on_stop": True}} bridge.hands_enabled = False + bridge.right_point_gesture_enabled = False bridge.robot_arm_positions = [0.0] * 14 bridge.last_command = None bridge.last_publish_at = 0.0 @@ -169,6 +170,7 @@ class RobotSessionGateTest(unittest.TestCase): "hold_seconds": 0.0, "joystick_deadzone": 0.2, "joystick_expo": 2.0, + "yaw_joystick_expo": 1.0, "max_forward_m_s": 1.0, "max_reverse_m_s": 0.8, "max_angular_rad_s": 0.8, @@ -193,15 +195,27 @@ class RobotSessionGateTest(unittest.TestCase): moving = ArmSnapshot( { - "button": {"right": [False, False, True]}, - "joystick": {"left": [1.0, 0.0]}, + "button": { + "left": [False, False, False], + "right": [False, False, True], + }, + "joystick": { + "left": [1.0, 0.0], + "right": [0.0, 0.0], + }, }, received_at=1.0, ) released = ArmSnapshot( { - "button": {"right": [False, False, False]}, - "joystick": {"left": [1.0, 0.0]}, + "button": { + "left": [False, False, False], + "right": [False, False, False], + }, + "joystick": { + "left": [1.0, 0.0], + "right": [0.0, 0.0], + }, }, received_at=1.1, ) @@ -212,6 +226,38 @@ class RobotSessionGateTest(unittest.TestCase): bridge._tick_locomotion(1.2, moving) self.assertEqual(published[-1], (1.0, -0.0)) + turning = ArmSnapshot( + { + "button": { + "left": [False, False, True], + "right": [False, False, False], + }, + "joystick": { + "left": [0.0, 0.0], + "right": [0.0, 1.0], + }, + }, + received_at=1.3, + ) + bridge._tick_locomotion(1.3, turning) + self.assertEqual(published[-1], (0.0, -0.8)) + + old_wrong_binding = ArmSnapshot( + { + "button": { + "left": [False, False, False], + "right": [False, False, True], + }, + "joystick": { + "left": [0.0, 1.0], + "right": [0.0, 0.0], + }, + }, + received_at=1.4, + ) + bridge._tick_locomotion(1.4, old_wrong_binding) + self.assertEqual(published[-1], (0.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 index 8455175..70281eb 100644 --- a/tg3_local_teleop/tg3-local-teleop.service +++ b/tg3_local_teleop/tg3-local-teleop.service @@ -1,14 +1,16 @@ [Unit] -Description=TG3 session-gated local TS1P teleoperation bridge +Description=TG3 local TS1P dual-arm bridge (no cloud pairing) After=network-online.target Wants=network-online.target [Service] Type=simple +ExecStartPre=/home/nvidia/tg3_local_teleop/wait_ros_ready.sh ExecStart=/home/nvidia/tg3_local_teleop/run.sh --allow-publish Restart=on-failure RestartSec=2 KillSignal=SIGINT +TimeoutStartSec=150 TimeoutStopSec=5 [Install] diff --git a/tg3_local_teleop/tg3_local_teleop.py b/tg3_local_teleop/tg3_local_teleop.py index bfcb4e9..eef19c0 100755 --- a/tg3_local_teleop/tg3_local_teleop.py +++ b/tg3_local_teleop/tg3_local_teleop.py @@ -30,6 +30,13 @@ from ros2_bridge_msgs.msg import ArmStatus from sensor_msgs.msg import JointState from std_srvs.srv import Trigger +from gesture_toggle import ( + GestureToggle, + normalized_pose_to_positions, + right_b_pressed, + select_right_hand_target, +) + JOINT_NAMES = [ *(f"left_joints_{i}" for i in range(7)), @@ -70,6 +77,10 @@ class LatestArmData: self._metrics: dict[str, Any] = { "transport": self.transport, "connected": False, + "registered": False, + "session_connects": 0, + "idle_session_refreshes": 0, + "idle_session_refresh_failures": 0, "frames_received": 0, "frames_accepted": 0, "dropped_sender": 0, @@ -150,6 +161,11 @@ class LatestArmData: expected_sender = str(self.cfg["omnisocket_expected_sender"]) max_age_ms = float(self.cfg["omnisocket_max_packet_age_ms"]) + idle_refresh_s = float( + self.cfg.get("omnisocket_idle_session_refresh_s", 2.0) + ) + if idle_refresh_s < 0.0: + raise ValueError("OmniSocket idle session refresh must be non-negative") last_sequence = 0 while not self._stop.is_set(): session = Session() @@ -160,13 +176,83 @@ class LatestArmData: peer_id=str(self.cfg["omnisocket_peer_id"]), **CONTROL_DEFAULTS, ) + last_accepted_at = time.monotonic() + session_stats = session.stats() with self._lock: self._metrics["connected"] = True + self._metrics["registered"] = bool( + int(session_stats.get("registered", 0)) == 1 + ) + self._metrics["session_connects"] += 1 self._last_error = "" while not self._stop.is_set(): message = session.recv(timeout_ms=100) if message is None: + now = time.monotonic() + if self._idle_session_refresh_due( + now, last_accepted_at, idle_refresh_s + ): + # The deployed OmniSocket receiver has no idle Hub + # heartbeat. After a Hub restart it may therefore + # keep reporting connected although its server-side + # registration is gone. Refresh with make-before- + # break: register a replacement with the same peer + # ID first, then close the old instance. The Hub + # tracks registration instances, so closing the old + # session does not unregister the replacement and + # there is no healthy-idle routing gap. + replacement = Session() + try: + replacement.connect( + server_addr=str( + self.cfg["omnisocket_server"] + ), + peer_id=str( + self.cfg["omnisocket_peer_id"] + ), + **CONTROL_DEFAULTS, + ) + replacement_stats = replacement.stats() + except Exception as exc: + try: + replacement.close() + except OSError: + pass + last_accepted_at = now + with self._lock: + self._metrics[ + "idle_session_refresh_failures" + ] += 1 + self._last_error = ( + "OmniSocket idle registration refresh " + f"failed: {exc}" + ) + continue + + old_session = session + session = replacement + baseline_ms = None + last_accepted_at = time.monotonic() + with self._lock: + self._metrics["connected"] = True + self._metrics["registered"] = bool( + int( + replacement_stats.get( + "registered", 0 + ) + ) + == 1 + ) + self._metrics["session_connects"] += 1 + self._metrics[ + "idle_session_refreshes" + ] += 1 + self._last_error = "" + try: + old_session.close() + except OSError: + pass continue messages = [message] while True: @@ -229,9 +315,11 @@ class LatestArmData: ) self._last_error = "" self._metrics["frames_accepted"] += 1 + last_accepted_at = time.monotonic() except Exception as exc: with self._lock: self._metrics["connected"] = False + self._metrics["registered"] = False self._last_error = f"OmniSocket connection failed: {exc}" finally: try: @@ -240,8 +328,18 @@ class LatestArmData: pass with self._lock: self._metrics["connected"] = False + self._metrics["registered"] = False self._stop.wait(1.0) + @staticmethod + def _idle_session_refresh_due( + now: float, last_accepted_at: float, refresh_after_s: float + ) -> bool: + return ( + refresh_after_s > 0.0 + and now - last_accepted_at >= refresh_after_s + ) + def _decode_omni_packet( self, msg_type: int, @@ -303,6 +401,27 @@ class LocalTeleopBridge(Node): net_cfg = config["network"] self.hands_cfg = config.get("hands", {}) self.hands_enabled = bool(self.hands_cfg.get("enabled", False)) + self.right_point_gesture_enabled = self.hands_enabled and bool( + self.hands_cfg.get("right_b_point_gesture_enabled", False) + ) + self.right_point_gesture = GestureToggle( + hold_seconds=float( + self.hands_cfg.get("right_b_point_gesture_hold_seconds", 3.0) + ), + release_seconds=float( + self.hands_cfg.get("right_b_point_gesture_release_seconds", 0.5) + ), + ) + point_pose = self.hands_cfg.get( + "right_b_point_pose_normalized", + [0.2, 0.688, 0.0, 0.98, 0.98, 0.98], + ) + self.right_point_gesture_pose = [float(value) for value in point_pose] + self.right_point_gesture_target = normalized_pose_to_positions( + self.right_point_gesture_pose, + int(self.hands_cfg.get("position_min", 1)), + int(self.hands_cfg.get("position_max", 1000)), + ) self.locomotion_cfg = config.get("locomotion", {}) self.locomotion_enabled = bool( self.locomotion_cfg.get("enabled", False) @@ -571,6 +690,26 @@ class LocalTeleopBridge(Node): success=False, ) + if self.right_point_gesture_enabled: + gesture_toggled = self.right_point_gesture.update( + now, + armed=self.armed, + input_healthy=( + self.armed + and sample is not None + and not runtime_hand_reasons + ), + pressed=( + None if sample is None else right_b_pressed(sample.data) + ), + ) + if gesture_toggled: + state = "ACTIVE" if self.right_point_gesture.active else "INACTIVE" + self.get_logger().warning( + f"RIGHT-HAND POINT GESTURE {state}: right B held for " + f"{self.right_point_gesture.hold_seconds:.1f}s" + ) + if self.returning_home: self._tick_home(now) elif self.armed and sample is not None: @@ -600,6 +739,17 @@ class LocalTeleopBridge(Node): ) self.hand_output_ready = True desired_hands = self._hand_targets(sample.data) + if self.right_point_gesture_enabled: + freeze_reference = self.last_hand_commands["right"] + if freeze_reference is None or len(freeze_reference) != 6: + freeze_reference = self.robot_hand_positions["right"] + desired_hands["right"] = select_right_hand_target( + desired_hands["right"], + freeze_reference, + self.right_point_gesture_target, + active=self.right_point_gesture.active, + freeze=self.right_point_gesture.freeze_right_hand, + ) commands = { side: self._slew_hand(side, desired_hands[side], now) for side in HAND_SIDES @@ -786,6 +936,8 @@ class LocalTeleopBridge(Node): "new teleoperation START accepted", success=False, cancelled=True ) self.active_session_id = session_id + if self.right_point_gesture_enabled: + self.right_point_gesture.new_session() self.armed = True # Start both slew limiters at measured robot feedback, never at a # potentially distant first network target. @@ -849,6 +1001,8 @@ class LocalTeleopBridge(Node): if reasons: self.get_logger().error("cannot arm: " + "; ".join(reasons)) return + if self.right_point_gesture_enabled: + self.right_point_gesture.new_session() 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. @@ -868,6 +1022,11 @@ class LocalTeleopBridge(Node): def _disarm(self, reason: str) -> None: was_armed = self.armed self._stop_locomotion(reason) + if self.right_point_gesture_enabled: + # Clearing the logical override must not publish a hand target. + # Existing STOP behavior leaves the physical hand at its last + # limited command until a later, newly armed session. + self.right_point_gesture.disarm(reason) self.armed = False self.hand_output_ready = False self.runtime_hand_output_reasons = [] @@ -898,20 +1057,29 @@ class LocalTeleopBridge(Node): 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])) + left_buttons = sample.data["button"]["left"] + right_buttons = sample.data["button"]["right"] + left_joystick = sample.data["joystick"]["left"] + right_joystick = sample.data["joystick"]["right"] + left_z = len(left_buttons) >= 3 and bool(left_buttons[2]) + right_c = len(right_buttons) >= 3 and bool(right_buttons[2]) + if len(left_joystick) != 2 or len(right_joystick) != 2: + raise ValueError("left and right joysticks must each contain two axes") + # xTELE 0.1.2 stores each TS1P stick as [vertical, horizontal]. + # Use the left vertical axis for translation and the right + # horizontal axis for in-place yaw, matching the physical control + # convention requested for this installation. + raw_forward = float(left_joystick[0]) + raw_yaw = float(right_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) + forward_expo = float(cfg["joystick_expo"]) + yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo)) + shaped_forward = self._shape_joystick_axis( + raw_forward, deadzone, forward_expo + ) + shaped_yaw = self._shape_joystick_axis( + raw_yaw, deadzone, yaw_expo + ) signed_forward = shaped_forward * float( cfg.get("forward_axis_sign", 1.0) ) @@ -927,9 +1095,12 @@ class LocalTeleopBridge(Node): 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") + forward_active = right_c and shaped_forward != 0.0 + yaw_active = left_z and shaped_yaw != 0.0 + if not forward_active and not yaw_active: + self._stop_locomotion( + "right C + left forward and left Z + right yaw are both inactive" + ) self._tick_walk_zero_burst(now) return @@ -944,17 +1115,19 @@ class LocalTeleopBridge(Node): self.walk_active = True self.walk_zero_frames_remaining = 0 self.get_logger().warning( - "LOCAL HBWALK VELOCITY STARTED: immediate right C + left " - "joystick input" + "LOCAL HBWALK VELOCITY STARTED: right C + left-stick forward " + "or left Z + right-stick yaw" ) 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 - ) + linear_x = signed_forward * linear_limit if forward_active else 0.0 + angular_z = 0.0 + if yaw_active: + 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: @@ -1432,8 +1605,62 @@ class LocalTeleopBridge(Node): 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, + "right_point_gesture_enabled": self.right_point_gesture_enabled, + "right_point_gesture_binding": ( + "right_B hold " + f"{self.right_point_gesture.hold_seconds:.1f}s toggle" + ), + "right_point_gesture_active": ( + self.right_point_gesture.active + if self.right_point_gesture_enabled + else False + ), + "right_point_gesture_state": ( + self.right_point_gesture.state + if self.right_point_gesture_enabled + else "disabled" + ), + "right_point_gesture_hold_s": round( + self.right_point_gesture.hold_elapsed(now), 2 + ), + "right_point_gesture_release_s": round( + self.right_point_gesture.release_elapsed(now), 2 + ), + "right_point_gesture_requires_release": ( + self.right_point_gesture.require_release + if self.right_point_gesture_enabled + else False + ), + "right_point_gesture_freezing_input": ( + self.right_point_gesture.freeze_right_hand + if self.right_point_gesture_enabled + else False + ), + "right_point_gesture_toggle_count": ( + self.right_point_gesture.toggle_count + if self.right_point_gesture_enabled + else 0 + ), + "right_point_gesture_last_transition": ( + self.right_point_gesture.last_transition + if self.right_point_gesture_enabled + else "disabled" + ), + "right_point_gesture_target_normalized": ( + self.right_point_gesture_pose + if self.right_point_gesture_enabled + else None + ), + "right_point_gesture_target_positions": ( + self.right_point_gesture_target + if self.right_point_gesture_enabled + else None + ), "locomotion_enabled": self.locomotion_enabled, - "locomotion_binding": "right_C + left_joystick, immediate", + "locomotion_binding": ( + "right_C + left_stick_vertical; " + "left_Z + right_stick_horizontal, immediate" + ), "locomotion_active": self.walk_active, "locomotion_hold_s": 0.0, "locomotion_command": { @@ -1444,6 +1671,11 @@ class LocalTeleopBridge(Node): "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"), + "forward_expo": self.locomotion_cfg.get("joystick_expo"), + "yaw_expo": self.locomotion_cfg.get( + "yaw_joystick_expo", + self.locomotion_cfg.get("joystick_expo"), + ), }, "locomotion_publish_count": self.walk_publish_count, "locomotion_fsm_publish_enabled": False, diff --git a/tg3_local_teleop/wait_ros_ready.sh b/tg3_local_teleop/wait_ros_ready.sh new file mode 100755 index 0000000..90dbdcf --- /dev/null +++ b/tg3_local_teleop/wait_ros_ready.sh @@ -0,0 +1,34 @@ +#!/usr/bin/env bash +# Avoid creating the long-lived Fast DDS participant before the robot network +# interfaces and the vendor ROS graph are available. A participant created +# while the configured interface whitelist is empty does not recover later. +set -eo pipefail + +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 + +required_topic="/hric/robot/rl_state" +wait_seconds="${TG3_ROS_READY_WAIT_SECONDS:-120}" +retry_seconds="${TG3_ROS_READY_RETRY_SECONDS:-2}" +deadline=$((SECONDS + wait_seconds)) + +while ((SECONDS < deadline)); do + # Capture first: grep -q in a pipe can close stdout early and make ros2 + # report SIGPIPE under pipefail even though the topic was found. + topic_list="$( + timeout 8 ros2 topic list --no-daemon --spin-time 3 2>/dev/null || true + )" + if grep -Fxq "$required_topic" <<<"$topic_list"; then + echo "ROS discovery ready: $required_topic" + exit 0 + fi + sleep "$retry_seconds" +done + +echo "ROS discovery did not expose $required_topic within ${wait_seconds}s" >&2 +exit 1 diff --git a/tg3_omnisocket_transport/README.md b/tg3_omnisocket_transport/README.md index b182ad9..0bf8095 100644 --- a/tg3_omnisocket_transport/README.md +++ b/tg3_omnisocket_transport/README.md @@ -21,6 +21,17 @@ Z+C 按键及诊断字段;若 5001 的双侧 `hand.position` 在 250 ms 内有 xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和腰、头、腿、行走、 底盘命令不会进入机器人双臂桥。 +右侧原始按键顺序是 `A/B/C`。按住右 `B` 时,sender 仍合并 5001 的左手 +目标,但保留 5003 的原始右手开合量,不用 xTELE 处理后的右手目标覆盖。 +这样右 B 的自定义长按手势还未满 3 秒时,原厂 B 处理不会让右手提前动作。 +松开 B 并连续稳定释放 `0.5 s` 后,还要等新鲜 5001 右手目标连续至少 `0.25 s` +恢复到按键前基线或当前 5003 原始开合量,才恢复双侧合并;无法确认时持续使用原始 +右手值,防止释放沿后的锁存/延迟目标漏入。 +5001 目标的时间戳也必须不晚于当前 5003 原始帧;若两个本机 ZMQ socket 的轮询顺序 +暂时颠倒,该 processed 目标会等到对应或更新的原始帧到达后才可合并。 +`tg3_transport.processed_hand_sides` 记录本帧实际合并 +的侧;右 B 抑制生效时还会设置 `processed_right_hand_suppressed_by_b=true`。 + 公网业务数据由 EAI 本地会话门控:服务启动后仍持续读取本机 5003/5001,但不发送 xTELE 帧;连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_id` 并开始发送。 再次长按 3 秒时发送最后一帧 `stop`,等待有界 KCP 刷新后关闭 OmniSocket Session; @@ -42,7 +53,7 @@ xTELE 帧;连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_i } ``` -`start` 连续发送 50 个源帧,避免接收端只取最新帧时错过唯一启动事件;机器人对同一 +当前用户服务将 `start` 连续发送 500 个源帧,避免接收端只取最新帧时错过唯一启动事件;机器人对同一 `session_id` 只做一次启动安全检查。`stop` 是该会话最后一个业务帧。服务重启、源数据 失联或网络错误都会使会话失效,恢复后必须先松开组合键,再重新长按 3 秒。 `tg3_local_teleop` 内部直接拒绝非预期发送端、 diff --git a/tg3_omnisocket_transport/omnisocket_xtele_sender.py b/tg3_omnisocket_transport/omnisocket_xtele_sender.py old mode 100644 new mode 100755 index c80b081..b81ab53 --- a/tg3_omnisocket_transport/omnisocket_xtele_sender.py +++ b/tg3_omnisocket_transport/omnisocket_xtele_sender.py @@ -38,6 +38,8 @@ 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.combo_release_s <= 0.0: + raise ValueError("combo release confirmation 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: @@ -62,8 +64,18 @@ class XteleSender: self.packet_sequence = time.time_ns() self.start_markers_remaining = 0 self.combo_started_at: float | None = None + self.combo_release_started_at: float | None = None # A service restart must never turn an already-held combo into a start. self.require_combo_release = True + # Keep xTELE's processed right-hand stream isolated for the complete + # right-B press and stable-release transaction. The robot runs the + # authoritative three-second gesture toggle from the raw B state. + self.right_b_merge_suppressed = False + self.right_b_release_started_at: float | None = None + self.right_b_recovery_started_at: float | None = None + self.right_b_processed_baseline: object | None = None + self.right_b_baseline_candidate: object | None = None + self.right_b_baseline_candidate_started_at: float | None = None self.counters = { "connected": 0, "reconnects": 0, @@ -156,6 +168,16 @@ class XteleSender: if self.combo_started_at is None else round(time.monotonic() - self.combo_started_at, 2), "teleop_require_combo_release": self.require_combo_release, + "teleop_combo_release_hold_s": 0.0 + if self.combo_release_started_at is None + else round(time.monotonic() - self.combo_release_started_at, 2), + "right_b_processed_merge_suppressed": self.right_b_merge_suppressed, + "right_b_merge_release_hold_s": 0.0 + if self.right_b_release_started_at is None + else round(time.monotonic() - self.right_b_release_started_at, 2), + "right_b_processed_recovery_hold_s": 0.0 + if self.right_b_recovery_started_at is None + else round(time.monotonic() - self.right_b_recovery_started_at, 2), "application_data_sending": ( self.teleop_active and self.session is not None ), @@ -226,6 +248,171 @@ class XteleSender: return None return {"left": copy.deepcopy(left), "right": copy.deepcopy(right)} + @staticmethod + def _command_aligned_with_raw( + command: dict[str, object], data: dict[str, object] + ) -> bool: + """Reject a processed 5001 target newer than the current raw frame.""" + + command_timestamp = command.get("timestamp") + raw_timestamp = data.get("timestamp") + if ( + isinstance(command_timestamp, bool) + or isinstance(raw_timestamp, bool) + or not isinstance(command_timestamp, (int, float)) + or not isinstance(raw_timestamp, (int, float)) + ): + return False + command_value = float(command_timestamp) + raw_value = float(raw_timestamp) + return ( + math.isfinite(command_value) + and math.isfinite(raw_value) + and command_value <= raw_value + ) + + @staticmethod + def _right_b_pressed(data: dict[str, object]) -> bool | None: + """Return raw TS1P right-B, or ``None`` for malformed input.""" + try: + buttons = data["button"] + right = buttons["right"] # type: ignore[index] + value = right[1] # type: ignore[index] + except (IndexError, KeyError, TypeError): + return None + if isinstance(value, bool): + return value + if isinstance(value, int) and value in (0, 1): + return bool(value) + return None + + @classmethod + def _hand_targets_equivalent(cls, first: object, second: object) -> bool: + """Compare normalized processed/raw hand targets with small jitter.""" + + if not cls._valid_hand_side(first) or not cls._valid_hand_side(second): + return False + if isinstance(first, (int, float)) and not isinstance(first, bool): + if not isinstance(second, (int, float)) or isinstance(second, bool): + return False + return abs(float(first) - float(second)) <= 0.02 + if not isinstance(first, list) or not isinstance(second, list): + return False + return all( + abs(float(left) - float(right)) <= 0.02 + for left, right in zip(first, second) + ) + + @staticmethod + def _raw_right_hand_target(data: dict[str, object]) -> object | None: + try: + hand = data["hand"] + position = hand["position"] # type: ignore[index] + return position["right"] # type: ignore[index] + except (KeyError, TypeError): + return None + + def _update_right_b_merge_gate( + self, + now: float, + data: dict[str, object], + processed_right: object | None, + ) -> bool: + """Suppress processed right hand until B release and target recovery.""" + + pressed = self._right_b_pressed(data) + if pressed is True: + self.right_b_merge_suppressed = True + self.right_b_release_started_at = None + self.right_b_recovery_started_at = None + self.right_b_baseline_candidate = None + self.right_b_baseline_candidate_started_at = None + return True + if pressed is None: + # A malformed button sample may never clear an in-progress gate. + self.right_b_merge_suppressed = True + self.right_b_release_started_at = None + self.right_b_recovery_started_at = None + self.right_b_baseline_candidate = None + self.right_b_baseline_candidate_started_at = None + return True + if not self.right_b_merge_suppressed: + self.right_b_release_started_at = None + self.right_b_recovery_started_at = None + if not self._valid_hand_side(processed_right): + self.right_b_baseline_candidate = None + self.right_b_baseline_candidate_started_at = None + return False + if not self._hand_targets_equivalent( + processed_right, self.right_b_baseline_candidate + ): + self.right_b_baseline_candidate = copy.deepcopy(processed_right) + self.right_b_baseline_candidate_started_at = now + return False + if self.right_b_baseline_candidate_started_at is None: + self.right_b_baseline_candidate_started_at = now + return False + baseline_seconds = max(0.1, float(self.args.cmd_max_age_s)) + if now - self.right_b_baseline_candidate_started_at >= baseline_seconds: + self.right_b_processed_baseline = copy.deepcopy(processed_right) + return False + if self.right_b_release_started_at is None: + self.right_b_release_started_at = now + self.right_b_recovery_started_at = None + return True + if now - self.right_b_release_started_at < self.args.combo_release_s: + self.right_b_recovery_started_at = None + return True + + # A stable raw release alone is insufficient: xTELE may retain a + # processed B gesture after the release edge. Re-enable the processed + # side only after fresh 5001 data continuously matches either its + # pre-B baseline or the current raw scalar target. + raw_right = self._raw_right_hand_target(data) + recovered = self._hand_targets_equivalent( + processed_right, self.right_b_processed_baseline + ) or self._hand_targets_equivalent(processed_right, raw_right) + if not recovered: + self.right_b_recovery_started_at = None + return True + if self.right_b_recovery_started_at is None: + self.right_b_recovery_started_at = now + return True + recovery_seconds = max(0.1, float(self.args.cmd_max_age_s)) + if now - self.right_b_recovery_started_at < recovery_seconds: + return True + + self.right_b_merge_suppressed = False + self.right_b_release_started_at = None + self.right_b_recovery_started_at = None + self.right_b_processed_baseline = copy.deepcopy(processed_right) + self.right_b_baseline_candidate = copy.deepcopy(processed_right) + self.right_b_baseline_candidate_started_at = now + return False + + @staticmethod + def _select_processed_hand_position( + raw_position: object, + processed_position: dict[str, object] | None, + *, + suppress_right: bool, + ) -> tuple[dict[str, object] | None, tuple[str, ...]]: + """Select processed sides without mutating either source structure.""" + if processed_position is None: + return None, () + if not suppress_right: + return copy.deepcopy(processed_position), ("left", "right") + + # While right B is held, xTELE may emit its own right-hand gesture + # before our separate three-second B latch fires on the robot. Keep + # the authoritative raw 5003 right-hand value, but allow an unrelated + # processed left-hand target to pass through. + if not isinstance(raw_position, dict) or "right" not in raw_position: + return None, () + selected = copy.deepcopy(raw_position) + selected["left"] = copy.deepcopy(processed_position["left"]) + return selected, ("left",) + @classmethod def _build_payload( cls, @@ -235,26 +422,44 @@ class XteleSender: session_seq: int, session_state: str, stop_reason: str = "", + suppress_processed_right: bool | None = None, ) -> tuple[bytes, bool]: - merged = False + merged_sides: tuple[str, ...] = () try: + # Build from a snapshot so callers retain the unmodified raw 5003 + # frame even when a processed 5001 hand target is selected. + payload_data = copy.deepcopy(data) 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 + hand = payload_data.get("hand") + right_b_pressed = cls._right_b_pressed(payload_data) + if suppress_processed_right is None: + suppress_processed_right = right_b_pressed is not False + if isinstance(hand, dict): + selected, merged_sides = cls._select_processed_hand_position( + hand.get("position"), + position, + suppress_right=suppress_processed_right, + ) + if selected is not None: + hand["position"] = selected - metadata = data.get("tg3_transport") + metadata = payload_data.get("tg3_transport") if not isinstance(metadata, dict): metadata = {} - data["tg3_transport"] = metadata - if merged: + payload_data["tg3_transport"] = metadata + if merged_sides: metadata["processed_hand_from_xtele_cmd"] = True + metadata["processed_hand_sides"] = list(merged_sides) assert command is not None metadata["xtele_cmd_timestamp"] = command.get("timestamp") else: metadata.pop("processed_hand_from_xtele_cmd", None) + metadata.pop("processed_hand_sides", None) metadata.pop("xtele_cmd_timestamp", None) + if suppress_processed_right and position is not None: + metadata["processed_right_hand_suppressed_by_b"] = True + else: + metadata.pop("processed_right_hand_suppressed_by_b", 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 @@ -266,11 +471,11 @@ class XteleSender: else: metadata.pop("stop_reason", None) encoded = json.dumps( - data, ensure_ascii=False, separators=(",", ":") + payload_data, ensure_ascii=False, separators=(",", ":") ).encode("utf-8") except (TypeError, ValueError): raise ValueError("cannot encode xTELE session payload") - return encoded, merged + return encoded, bool(merged_sides) @staticmethod def _start_stop_pressed(data: dict[str, object]) -> bool: @@ -293,9 +498,20 @@ class XteleSender: pressed = self._start_stop_pressed(data) if self.require_combo_release: self.combo_started_at = None - if not pressed: + if pressed: + self.combo_release_started_at = None + return None + if self.combo_release_started_at is None: + self.combo_release_started_at = now + return None + if ( + now - self.combo_release_started_at + >= self.args.combo_release_s + ): self.require_combo_release = False + self.combo_release_started_at = None return None + self.combo_release_started_at = None if not pressed: self.combo_started_at = None return None @@ -305,6 +521,7 @@ class XteleSender: return None self.combo_started_at = None + self.combo_release_started_at = None self.require_combo_release = True if self.teleop_active: self.teleop_active = False @@ -451,6 +668,7 @@ class XteleSender: > self.args.source_timeout_s ): self.combo_started_at = None + self.combo_release_started_at = None if self.teleop_active: self._abort_teleop( "local xTELE source became stale; a new Z+C hold " @@ -476,6 +694,7 @@ class XteleSender: # A malformed/frozen frame may never contribute time to a # physical three-second start/stop hold. self.combo_started_at = None + self.combo_release_started_at = None self.counters["dropped_malformed"] += 1 continue @@ -487,8 +706,22 @@ class XteleSender: if ( latest_command is not None and now - self.last_command_at <= self.args.cmd_max_age_s + and self._command_aligned_with_raw(latest_command, data) ): command = latest_command + processed_position = ( + self._processed_hand_position(command) + if command is not None + else None + ) + processed_right = ( + None + if processed_position is None + else processed_position["right"] + ) + suppress_processed_right = self._update_right_b_merge_gate( + now, data, processed_right + ) if not self.teleop_active and transition != "stop": self.counters["frames_suppressed_inactive"] += 1 @@ -528,6 +761,7 @@ class XteleSender: self.teleop_session_seq, session_state, stop_reason, + suppress_processed_right, ) except ValueError: self.counters["dropped_malformed"] += 1 @@ -543,8 +777,8 @@ class XteleSender: 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. + # Session is flushed for bounded delivery and then closed; + # the next physical START creates a fresh registration. self.teleop_session_id = None self.teleop_session_seq = 0 self.start_markers_remaining = 0 @@ -581,6 +815,15 @@ def parse_args() -> argparse.Namespace: 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( + "--combo-release-s", + type=float, + default=0.5, + help=( + "require both combo buttons to remain released for this long " + "before another start/stop hold can begin" + ), + ) parser.add_argument( "--start-marker-frames", type=int, diff --git a/tg3_omnisocket_transport/test_session_gate.py b/tg3_omnisocket_transport/test_session_gate.py index c72b2f8..6a2ba2c 100644 --- a/tg3_omnisocket_transport/test_session_gate.py +++ b/tg3_omnisocket_transport/test_session_gate.py @@ -1,13 +1,13 @@ #!/usr/bin/env python3 -"""Offline tests for the EAI teleoperation session gate.""" - from __future__ import annotations +import argparse +import copy import importlib.util import json from pathlib import Path -from types import ModuleType, SimpleNamespace import sys +from types import ModuleType import unittest @@ -22,75 +22,109 @@ spec = importlib.util.spec_from_file_location("omnisocket_xtele_sender", module_ 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]: +def args() -> argparse.Namespace: + return argparse.Namespace( + server="127.0.0.1:14049", + peer_id="sender", + target_peer="robot", + zmq_endpoint="tcp://127.0.0.1:5003", + cmd_zmq_endpoint="", + cmd_max_age_s=0.25, + source_timeout_s=0.25, + max_feedback_age_ms=500.0, + max_pending_frames=100, + start_stop_hold_s=3.0, + combo_release_s=0.5, + start_marker_frames=50, + status_file="/tmp/tg3_sender_test_status.json", + ) + + +def frame(pressed: bool) -> dict[str, object]: + value = 1 if pressed else 0 return { "button": { - "left": [False, False, pressed], - "right": [False, False, pressed], - }, - "hand": {"position": {"left": 0.0, "right": 0.0}}, + "left": [0, 0, value], + "right": [0, 0, value], + } } 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 = sender_module.XteleSender(args()) + + def stable_release(self, started_at: float) -> float: + self.assertIsNone( + self.sender._update_teleop_gate(started_at, frame(False)) ) - 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" + finished_at = started_at + 0.51 + self.assertIsNone( + self.sender._update_teleop_gate(finished_at, frame(False)) ) - first_id = self.sender.teleop_session_id - self.assertTrue(self.sender.teleop_active) - self.assertIsNotNone(first_id) + self.assertFalse(self.sender.require_combo_release) + return finished_at - # 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))) + def test_boot_requires_stable_release_before_start(self) -> None: + self.assertIsNone(self.sender._update_teleop_gate(0.0, frame(True))) + self.assertIsNone(self.sender._update_teleop_gate(4.0, frame(True))) + released_at = self.stable_release(5.0) + self.assertIsNone( + self.sender._update_teleop_gate(released_at + 0.01, frame(True)) + ) self.assertEqual( - self.sender._update_teleop_gate(12.01, buttons(True)), "stop" + self.sender._update_teleop_gate(released_at + 3.02, frame(True)), + "start", + ) + self.assertTrue(self.sender.teleop_active) + + def test_single_false_frame_cannot_rearm_stop(self) -> None: + released_at = self.stable_release(0.0) + self.sender._update_teleop_gate(released_at + 0.01, frame(True)) + self.assertEqual( + self.sender._update_teleop_gate(released_at + 3.02, frame(True)), + "start", + ) + + self.assertIsNone( + self.sender._update_teleop_gate(released_at + 3.03, frame(False)) + ) + self.assertIsNone( + self.sender._update_teleop_gate(released_at + 3.04, frame(True)) + ) + self.assertIsNone( + self.sender._update_teleop_gate(released_at + 7.00, frame(True)) + ) + self.assertTrue(self.sender.teleop_active) + self.assertTrue(self.sender.require_combo_release) + + def test_stable_release_allows_separate_stop_hold(self) -> None: + released_at = self.stable_release(0.0) + self.sender._update_teleop_gate(released_at + 0.01, frame(True)) + self.assertEqual( + self.sender._update_teleop_gate(released_at + 3.02, frame(True)), + "start", + ) + second_release = self.stable_release(released_at + 3.03) + self.sender._update_teleop_gate(second_release + 0.01, frame(True)) + self.assertEqual( + self.sender._update_teleop_gate(second_release + 3.02, frame(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) + def test_untrusted_transport_metadata_is_overwritten(self) -> None: + data = frame(False) data["tg3_transport"] = { "protocol_version": 999, "session_id": "forged", + "session_seq": 999, "session_state": "stop", + "stop_reason": "forged", } + payload, merged = self.sender._build_payload( data, None, @@ -98,34 +132,230 @@ class SessionGateTest(unittest.TestCase): 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") + self.assertNotIn("stop_reason", metadata) 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 + def __init__(self, feedback_age_ms: int, pending: int) -> None: + self.feedback_age_ms = feedback_age_ms self.pending = pending - def stats(self) -> dict[str, int]: + @staticmethod + def stats() -> 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, + "last_feedback_age_ms": self.feedback_age_ms, } self.sender.session_connected_at = 0.0 self.sender.session = FakeSession(600, 0) - self.assertIn("feedback stale", self.sender._session_unhealthy_reason()) + self.assertIn( + "feedback stale", self.sender._session_unhealthy_reason() or "" + ) self.sender.session = FakeSession(1, 101) - self.assertIn("pending queue", self.sender._session_unhealthy_reason()) + self.assertIn( + "pending queue", self.sender._session_unhealthy_reason() or "" + ) + + +class ProcessedHandMergeTest(unittest.TestCase): + @staticmethod + def raw_frame(right_b: int) -> dict[str, object]: + return { + "button": { + "left": [0, 0, 0], + "right": [0, right_b, 0], + }, + "trigger": {"left": 0.12, "right": 0.73}, + "hand": {"position": {"left": 0.12, "right": 0.73}}, + } + + @staticmethod + def processed_command() -> dict[str, object]: + return { + "timestamp": 123.5, + "hand": { + "position": { + "left": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6], + "right": [0.6, 0.5, 0.4, 0.3, 0.2, 0.1], + } + }, + } + + def test_processed_command_cannot_run_ahead_of_raw_timestamp(self) -> None: + command = self.processed_command() + self.assertFalse( + sender_module.XteleSender._command_aligned_with_raw( + command, {"timestamp": 123.4} + ) + ) + self.assertTrue( + sender_module.XteleSender._command_aligned_with_raw( + command, {"timestamp": 123.5} + ) + ) + self.assertTrue( + sender_module.XteleSender._command_aligned_with_raw( + command, {"timestamp": 124.0} + ) + ) + self.assertFalse( + sender_module.XteleSender._command_aligned_with_raw( + command, {"timestamp": "124.0"} + ) + ) + + def build( + self, + data: dict[str, object], + suppress_processed_right: bool | None = None, + ) -> tuple[dict[str, object], bool]: + encoded, merged = sender_module.XteleSender._build_payload( + data, + self.processed_command(), + "session-id", + 7, + "active", + suppress_processed_right=suppress_processed_right, + ) + return json.loads(encoded), merged + + def test_right_b_preserves_raw_right_hand_and_trigger(self) -> None: + raw = self.raw_frame(right_b=1) + original = copy.deepcopy(raw) + + payload, merged = self.build(raw) + + self.assertTrue(merged) + self.assertEqual( + payload["hand"]["position"]["left"], + self.processed_command()["hand"]["position"]["left"], + ) + self.assertEqual(payload["hand"]["position"]["right"], 0.73) + self.assertEqual(payload["trigger"], original["trigger"]) + self.assertEqual( + payload["tg3_transport"]["processed_hand_sides"], ["left"] + ) + self.assertTrue( + payload["tg3_transport"][ + "processed_right_hand_suppressed_by_b" + ] + ) + self.assertEqual(raw, original, "payload building must not mutate raw 5003") + + def test_right_b_release_restores_bilateral_processed_merge(self) -> None: + raw = self.raw_frame(right_b=0) + + payload, merged = self.build(raw) + + self.assertTrue(merged) + self.assertEqual( + payload["hand"]["position"], + self.processed_command()["hand"]["position"], + ) + self.assertEqual(payload["trigger"]["right"], 0.73) + self.assertEqual( + payload["tg3_transport"]["processed_hand_sides"], + ["left", "right"], + ) + self.assertNotIn( + "processed_right_hand_suppressed_by_b", + payload["tg3_transport"], + ) + + def test_runtime_gate_suppresses_through_stable_b_release(self) -> None: + sender = sender_module.XteleSender(args()) + pressed = self.raw_frame(right_b=1) + released = self.raw_frame(right_b=0) + baseline = self.processed_command()["hand"]["position"]["right"] + b_gesture = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98] + + self.assertFalse(sender._update_right_b_merge_gate(0.0, released, baseline)) + self.assertFalse( + sender._update_right_b_merge_gate(0.26, released, baseline) + ) + self.assertTrue( + sender._update_right_b_merge_gate(1.0, pressed, b_gesture) + ) + self.assertTrue( + sender._update_right_b_merge_gate(1.1, released, b_gesture) + ) + self.assertTrue( + sender._update_right_b_merge_gate(1.61, released, b_gesture) + ) + held_payload, _ = self.build( + released, suppress_processed_right=True + ) + self.assertEqual(held_payload["hand"]["position"]["right"], 0.73) + + # Stable B release is not enough: processed 5001 must also remain at + # a safe baseline for at least cmd_max_age_s before it is trusted. + self.assertTrue( + sender._update_right_b_merge_gate(1.62, released, baseline) + ) + self.assertTrue( + sender._update_right_b_merge_gate(1.86, released, baseline) + ) + self.assertFalse( + sender._update_right_b_merge_gate(1.88, released, baseline) + ) + restored_payload, _ = self.build( + released, suppress_processed_right=False + ) + self.assertEqual( + restored_payload["hand"]["position"]["right"], + self.processed_command()["hand"]["position"]["right"], + ) + + def test_malformed_b_cannot_clear_runtime_suppression(self) -> None: + sender = sender_module.XteleSender(args()) + self.assertTrue( + sender._update_right_b_merge_gate( + 1.0, self.raw_frame(right_b=1), None + ) + ) + malformed = self.raw_frame(right_b=0) + malformed["button"] = {"right": [0]} + self.assertTrue( + sender._update_right_b_merge_gate(2.0, malformed, 0.73) + ) + self.assertIsNone(sender.right_b_release_started_at) + + def test_one_early_processed_change_cannot_replace_pre_b_baseline(self) -> None: + sender = sender_module.XteleSender(args()) + released = self.raw_frame(right_b=0) + pressed = self.raw_frame(right_b=1) + baseline = self.processed_command()["hand"]["position"]["right"] + b_gesture = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98] + + sender._update_right_b_merge_gate(0.0, released, baseline) + sender._update_right_b_merge_gate(0.26, released, baseline) + self.assertEqual(sender.right_b_processed_baseline, baseline) + + # Model 5001 being polled just before the corresponding raw B frame. + # It may become a candidate, but cannot immediately replace the last + # stable, B-false baseline. + sender._update_right_b_merge_gate(1.0, released, b_gesture) + self.assertEqual(sender.right_b_processed_baseline, baseline) + sender._update_right_b_merge_gate(1.01, pressed, b_gesture) + sender._update_right_b_merge_gate(1.02, released, b_gesture) + self.assertTrue( + sender._update_right_b_merge_gate(1.53, released, b_gesture) + ) + self.assertTrue( + sender._update_right_b_merge_gate(2.0, released, b_gesture) + ) if __name__ == "__main__": diff --git a/tg3_omnisocket_transport/tg3-omnisocket-sender.service b/tg3_omnisocket_transport/tg3-omnisocket-sender.service index 724ced3..62320a9 100644 --- a/tg3_omnisocket_transport/tg3-omnisocket-sender.service +++ b/tg3_omnisocket_transport/tg3-omnisocket-sender.service @@ -7,7 +7,7 @@ Wants=network-online.target Type=simple WorkingDirectory=/home/eai/tg3_omnisocket_transport Environment=PYTHONPATH=/home/eai/OmniSocketGo/python -ExecStart=/usr/bin/python3 /home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py --server 175.178.116.187:14049 --peer-id tg3-009027fa8190-iarm --target-peer tg3-009027fa8190-robot --zmq-endpoint tcp://127.0.0.1:5003 --cmd-zmq-endpoint tcp://127.0.0.1:5001 --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 --start-marker-frames 50 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json +ExecStart=/usr/bin/python3 /home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py --server 175.178.116.187:14049 --peer-id tg3-009027fa8190-iarm --target-peer tg3-009027fa8190-robot --zmq-endpoint tcp://127.0.0.1:5003 --cmd-zmq-endpoint tcp://127.0.0.1:5001 --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 --combo-release-s 0.5 --start-marker-frames 500 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json Restart=always RestartSec=1 KillSignal=SIGINT diff --git a/verify.sh b/verify.sh index d7361fa..5c2b01f 100755 --- a/verify.sh +++ b/verify.sh @@ -4,13 +4,45 @@ set -euo pipefail repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" expected_omnisocket_commit="de3f5c96779dbe1571c10feb22fc7f2331b6b222" +for executable in \ + "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \ + "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \ + "$repo_dir/tg3_local_teleop/run.sh" \ + "$repo_dir/tg3_local_teleop/wait_ros_ready.sh" \ + "$repo_dir/tg3_local_teleop/home.sh" \ + "$repo_dir/tg3_local_teleop/status.sh"; do + if [[ ! -x "$executable" ]]; then + echo "Required executable bit is missing: $executable" >&2 + exit 1 + fi +done + +python3 -m py_compile \ + "$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \ + "$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \ + "$repo_dir/tg3_local_teleop/gesture_toggle.py" python3 "$repo_dir/tg3_omnisocket_transport/test_session_gate.py" python3 "$repo_dir/tg3_local_teleop/test_session_gate.py" +python3 "$repo_dir/tg3_local_teleop/test_gesture_toggle.py" +python3 "$repo_dir/tg3_local_teleop/test_idle_session_refresh.py" +python3 - "$repo_dir/tg3_local_teleop/config.toml" <<'PY' +import sys +import tomllib -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 +with open(sys.argv[1], "rb") as stream: + tomllib.load(stream) +print("config.toml parse passed") +PY + +if [[ -n "${OMNISOCKETGO_DIR:-}" ]]; then + actual_omnisocket_commit="$(git -C "$OMNISOCKETGO_DIR" rev-parse HEAD)" + if [[ "$actual_omnisocket_commit" != "$expected_omnisocket_commit" ]]; then + echo "OmniSocketGo commit mismatch: $actual_omnisocket_commit" >&2 + exit 1 + fi + echo "OmniSocketGo commit verified: $actual_omnisocket_commit" +else + echo "OmniSocketGo is external and was not checked; expected commit: $expected_omnisocket_commit" fi -echo "Offline checks passed; OmniSocketGo is pinned to $actual_omnisocket_commit" +echo "All offline TG3 checks passed"