Compare commits
3 Commits
eab60273bc
...
6bf610295b
| Author | SHA1 | Date | |
|---|---|---|---|
|
6bf610295b | ||
|
|
29030c0b25 | ||
|
|
1a79fca36f |
21
.gitignore
vendored
21
.gitignore
vendored
@@ -1,21 +1,22 @@
|
|||||||
__pycache__/
|
__pycache__/
|
||||||
*.py[cod]
|
*.py[cod]
|
||||||
.pytest_cache/
|
|
||||||
|
|
||||||
# Runtime state must never be committed. It can contain live session and robot data.
|
|
||||||
status.json
|
|
||||||
status.json.tmp
|
|
||||||
*.log
|
*.log
|
||||||
*.jsonl
|
*.jsonl
|
||||||
|
status.json
|
||||||
|
status.json.tmp
|
||||||
|
.pytest_cache/
|
||||||
|
|
||||||
# ROS and Python build products are architecture-specific.
|
# ROS/colcon generated output is rebuilt on the target robot.
|
||||||
ros2_py/build/
|
tg3_local_teleop/ros2_py/build/
|
||||||
ros2_py/install/
|
tg3_local_teleop/ros2_py/install/
|
||||||
ros2_py/log/
|
tg3_local_teleop/ros2_py/log/
|
||||||
python/build/
|
python/build/
|
||||||
*.egg-info/
|
*.egg-info/
|
||||||
*.so
|
*.so
|
||||||
|
|
||||||
|
# Local deployment and editor leftovers.
|
||||||
|
.deploy-*/
|
||||||
.DS_Store
|
.DS_Store
|
||||||
*~
|
|
||||||
*.bak
|
*.bak
|
||||||
|
*.before-*
|
||||||
|
*~
|
||||||
|
|||||||
110
README.md
110
README.md
@@ -1,79 +1,63 @@
|
|||||||
# 天工 3.0 TS1P 同构臂本地遥操
|
# TG3 天工 3.0 本地同构臂遥操
|
||||||
|
|
||||||
本仓库汇总当前已经部署并验证的三部分：
|
本仓库保存 TS1P 同构臂经 OmniSocket 控制天工 3.0 双臂、BrainCo Revo2
|
||||||
|
双手和 HBWALK 前后/转向所需的自写代码。它不包含、不修改 xTELE、机器人厂家
|
||||||
```text
|
控制源码或 OmniSocketGo 本体。
|
||||||
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 秒等待。
|
|
||||||
|
|
||||||
## 目录
|
## 目录
|
||||||
|
|
||||||
|
- `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
|
```text
|
||||||
tg3_omnisocket_transport/ EAI 发送端、用户服务和离线测试
|
https://gitea.public.snrc.site/limingjie/OmniSocketGo.git
|
||||||
tg3_local_teleop/ 机器人桥、配置、用户服务、ROS 消息和离线测试
|
commit de3f5c96779dbe1571c10feb22fc7f2331b6b222
|
||||||
OmniSocketGo/ 固定版本的传输依赖（Git 子模块）
|
|
||||||
docs/ 部署汇报、迁移指南和 Topic/SBUS 证据记录
|
|
||||||
verify.sh 不连接机器人、不发布 ROS 的离线检查
|
|
||||||
```
|
```
|
||||||
|
|
||||||
OmniSocketGo 固定在提交 `de3f5c96779dbe1571c10feb22fc7f2331b6b222`。克隆时必须拉取
|
EAI 还需已安装并运行 xTELE；机器人端需已有 ROS 2 Jazzy、厂家消息包与驱动。
|
||||||
子模块：
|
详见
|
||||||
|
|
||||||
```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)。
|
[`docs/天工3.0本地同构臂遥操迁移部署指南.md`](docs/天工3.0本地同构臂遥操迁移部署指南.md)。
|
||||||
|
|
||||||
## 构建传输依赖
|
OmniSocket Python 扩展包含本机架构代码，必须分别在 EAI 和机器人上从上述固定提交
|
||||||
|
原生执行 `make python-ext`，不能复制另一种 CPU 或 Python 版本生成的 `.so`。
|
||||||
|
|
||||||
OmniSocket Python 扩展包含本机架构代码，必须分别在 EAI 和机器人上原生编译，不能
|
提交或部署前可运行 `./verify.sh` 完成两端协议、手势、接收端刷新、Python 编译和
|
||||||
复制另一种 CPU/Python 版本生成的 `.so`：
|
TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务专用环境变量
|
||||||
|
`OMNISOCKETGO_DIR=/path/to/OmniSocketGo` 后再运行。
|
||||||
|
|
||||||
```bash
|
## 当前按键
|
||||||
cd OmniSocketGo
|
|
||||||
make python-ext
|
|
||||||
```
|
|
||||||
|
|
||||||
EAI 和机器人部署命令、systemd 用户服务安装及完整重启顺序分别记录在两个项目的
|
- 左 Z + 右 C 连续 3 秒：开始遥操；再次连续 3 秒：结束并限速回 Home。
|
||||||
README 和 `docs/` 中。
|
若 Hub 尚未运行，本次长按只检测一次并立即结束，不发送控制数据；服务器启动后须先
|
||||||
|
稳定松开，再重新长按 3 秒。pending 和活动会话都不会自动重试或自动恢复。
|
||||||
|
- 右 C + 左摇杆上下：HBWALK 前进/后退。
|
||||||
|
- 左 Z + 右摇杆左右：HBWALK 原地转向。
|
||||||
|
- 右 B 连续 1 秒：右手进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续
|
||||||
|
1 秒退出。手指动作继续受 `400 units/s` 限速。
|
||||||
|
|
||||||
|
## 迁移前必须修改
|
||||||
|
|
||||||
|
至少核对并修改服务器地址、两端 Peer ID、同构臂 ID、直连回退 IP、安装用户名和
|
||||||
|
路径。`config.toml` 中的 14 关节 Home 是当前实机采集值，不能直接用于另一台机器人；
|
||||||
|
必须在新机器人上重新采集并完成低速验证。所有厂家默认限位、碰撞、电流和急停保护
|
||||||
|
保持不变。
|
||||||
|
|
||||||
|
运行产生的 `status.json`、日志、Python 缓存及 ROS 构建目录已由 `.gitignore`
|
||||||
|
排除。
|
||||||
|
|
||||||
## 安全与仓库可见性
|
## 安全与仓库可见性
|
||||||
|
|
||||||
这是物理机器人控制项目。执行 `--allow-publish`、启动用户服务或回 Home 前，必须确认
|
这是物理机器人控制项目。启动带 `--allow-publish` 的服务、测试手势、行走或回 Home
|
||||||
机器人处于 HBWALK、防护和急停有效、双臂及行走区域净空。
|
前，必须确认机器人处于 HBWALK，防护和急停有效，双臂、灵巧手与行走区域净空。
|
||||||
|
|
||||||
仓库包含现场 Hub 地址、设备/Peer ID、内网地址、实机 Home 姿态以及标记为
|
仓库保留了现场 Hub 地址、设备/Peer ID、内网地址、当前实机 Home 姿态，以及标记为
|
||||||
`Proprietary` 的 ROS 消息定义，建议 Gitea 仓库保持私有。运行产生的 `status.json`、
|
`Proprietary` 的 ROS 消息定义，建议 Gitea 仓库保持私有。原厂 SDK 文档和 PDF 未收入
|
||||||
日志、缓存和本机编译文件已由 `.gitignore` 排除。原厂 SDK 文档和 PDF 未收入仓库，
|
仓库，避免上传其中的下载授权码、Wi-Fi 信息及版权资料。
|
||||||
避免上传其中的下载授权码、Wi-Fi 信息及版权资料。
|
|
||||||
|
|||||||
@@ -91,6 +91,7 @@ python3 -c 'import json; p="/home/nvidia/tg3_local_teleop/status.json"; d=json.l
|
|||||||
/right_hand/set_motor_multi
|
/right_hand/set_motor_multi
|
||||||
/left_hand/motor_status
|
/left_hand/motor_status
|
||||||
/right_hand/motor_status
|
/right_hand/motor_status
|
||||||
|
/hric/robot/cmd_vel
|
||||||
```
|
```
|
||||||
|
|
||||||
若新机器人不是天工 3.0、SDK 版本接口有变化、不是 14 维双臂或不是 BrainCo Revo2，先停止
|
若新机器人不是天工 3.0、SDK 版本接口有变化、不是 14 维双臂或不是 BrainCo Revo2，先停止
|
||||||
@@ -152,7 +153,7 @@ ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
|||||||
rsync -av \
|
rsync -av \
|
||||||
--exclude=__pycache__ \
|
--exclude=__pycache__ \
|
||||||
--exclude=status.json \
|
--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/
|
nvidia@"${TG3_NEW_ROBOT_IP}":/home/nvidia/tg3_local_teleop/
|
||||||
|
|
||||||
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
ssh nvidia@"${TG3_NEW_ROBOT_IP}" \
|
||||||
@@ -184,6 +185,8 @@ joint_goal_rad = [ ...新机器人实测的 14 个 Home 值... ]
|
|||||||
还要检查：
|
还要检查：
|
||||||
|
|
||||||
- `[hands]` 是否与新机器人的真实手型相符；
|
- `[hands]` 是否与新机器人的真实手型相符；
|
||||||
|
- `[hands].right_b_point_pose_normalized` 是否适用于新 BrainCo 手的校准；首次只在净空、
|
||||||
|
急停可用且低速限制生效时测试；
|
||||||
- `[control].joint_lower_rad/joint_upper_rad` 是否仍适用于同型号和当前 SDK；
|
- `[control].joint_lower_rad/joint_upper_rad` 是否仍适用于同型号和当前 SDK；
|
||||||
- `run.sh`、service 内的用户名和路径是否仍为 `/home/nvidia`；
|
- `run.sh`、service 内的用户名和路径是否仍为 `/home/nvidia`；
|
||||||
- ROS 安装路径是否仍有 `/opt/ros/jazzy`、`/home/nvidia/xos` 或
|
- ROS 安装路径是否仍有 `/opt/ros/jazzy`、`/home/nvidia/xos` 或
|
||||||
@@ -312,6 +315,8 @@ Environment=PYTHONPATH=/home/<新EAI用户>/OmniSocketGo/python
|
|||||||
--cmd-max-age-s 0.25
|
--cmd-max-age-s 0.25
|
||||||
--source-timeout-s 0.25
|
--source-timeout-s 0.25
|
||||||
--start-stop-hold-s 3.0
|
--start-stop-hold-s 3.0
|
||||||
|
--combo-release-s 0.5
|
||||||
|
--start-marker-frames 500
|
||||||
--max-feedback-age-ms 500
|
--max-feedback-age-ms 500
|
||||||
--max-pending-frames 100
|
--max-pending-frames 100
|
||||||
Restart=always
|
Restart=always
|
||||||
@@ -392,14 +397,19 @@ omnisocket_server = "203.0.113.10:15000"
|
|||||||
5. 松开两个扳机，长按左 Z + 右 C 3 秒启动；
|
5. 松开两个扳机，长按左 Z + 右 C 3 秒启动；
|
||||||
6. 先做小幅单关节跟随，再逐渐扩大动作；
|
6. 先做小幅单关节跟随，再逐渐扩大动作；
|
||||||
7. 分别小幅扣左右扳机，验证双手方向、范围和限速；
|
7. 分别小幅扣左右扳机，验证双手方向、范围和限速；
|
||||||
8. 先停止并确认 `armed=false`，按飞书指南逐个选择手势组合键；检查 EAI `status.json`
|
8. 保持右摇杆回中，连续长按右 B 1 秒，确认右手以 `400 units/s` 限速进入厂商
|
||||||
的 `command_hand_merges` 增长，并确认机器人 `iarm_hand_position` 变为左右各 6 维数组；
|
“单食指”姿态；松开至少 0.5 秒后再次长按 1 秒，确认退出并恢复正常右手跟随；
|
||||||
9. 再次武装后只做小幅扳机动作，逐个验证组合手势方向和限速；
|
9. 先停止并确认 `armed=false`，按飞书指南逐个选择其他手势组合键；检查 EAI
|
||||||
10. 保持右 C，把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；松 C，确认
|
`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 秒；
|
立即零速停止。再次按 C/推杆也应立即响应，不再等待 3 秒；
|
||||||
11. 再次长按停止，观察双臂限速回到新机器人保存的 Home；
|
12. 保持左 Z，把右摇杆小幅横推，确认机器人原地转向；松开任一输入应立即清零角速度；
|
||||||
12. 测试 Hub 短暂断线：应在 0.25 秒后停止发布，恢复后先确认未武装，再重新长按启动；
|
13. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home；
|
||||||
13. 记录最终 Peer ID、Hub、SSH 地址、Home、行走限速和手部端点到该机器人的设备档案。
|
14. 测试 Hub 短暂断线：应在 0.25 秒后停止发布，恢复后先确认未武装，再重新长按启动；
|
||||||
|
15. 记录最终 Peer ID、Hub、SSH 地址、Home、行走限速、手部端点和单食指姿态到该机器人的设备档案。
|
||||||
|
|
||||||
迁移时确认目标机提供 `/hric/robot/cmd_vel`（`geometry_msgs/msg/TwistStamped`），并保持
|
迁移时确认目标机提供 `/hric/robot/cmd_vel`（`geometry_msgs/msg/TwistStamped`），并保持
|
||||||
`locomotion.command_topic` 与目标固件一致。行走限速、死区、曲线和方向符号均在机器人
|
`locomotion.command_topic` 与目标固件一致。行走限速、死区、曲线和方向符号均在机器人
|
||||||
@@ -417,6 +427,15 @@ omnisocket_server = "203.0.113.10:15000"
|
|||||||
|
|
||||||
不需要。`127.0.0.1` 永远表示 EAI 本机，和网卡地址无关。
|
不需要。`127.0.0.1` 永远表示 EAI 本机，和网卡地址无关。
|
||||||
|
|
||||||
|
### Hub 没启动时已经长按了 Z+C，服务器恢复后怎么办？
|
||||||
|
|
||||||
|
本次长按只做一次后台连接检测。Hub 未启动、机器人 Peer 不存在或首帧发送失败时，
|
||||||
|
`teleop_start_pending` 会回到 `false`；Hub 离线时没有业务帧发出，Peer/发送错误时机器人
|
||||||
|
不会收到控制数据。旧请求不会在服务器稍后恢复
|
||||||
|
时自动重试。稳定松开 Z+C 至少 0.5 秒，待 Hub 与机器人接收端就绪后重新长按 3 秒即可。
|
||||||
|
后台检测期间 sender 仍持续读取 5003，因此松键不会因连接等待而漏掉。活动会话断网同样
|
||||||
|
按故障关闭处理，必须稳定松开后重新长按。
|
||||||
|
|
||||||
### 只改机器人 Peer，不改 EAI 可以吗？
|
### 只改机器人 Peer，不改 EAI 可以吗？
|
||||||
|
|
||||||
不可以。EAI 的 `--target-peer` 必须等于机器人的 `omnisocket_peer_id`，机器人
|
不可以。EAI 的 `--target-peer` 必须等于机器人的 `omnisocket_peer_id`，机器人
|
||||||
|
|||||||
@@ -13,13 +13,15 @@ TS1P 同构臂
|
|||||||
-> 天工 3.0 双臂
|
-> 天工 3.0 双臂
|
||||||
-> /left_hand/set_motor_multi + /right_hand/set_motor_multi
|
-> /left_hand/set_motor_multi + /right_hand/set_motor_multi
|
||||||
-> 天工 3.0 BrainCo Revo2 双灵巧手
|
-> 天工 3.0 BrainCo Revo2 双灵巧手
|
||||||
-> /hric/robot/cmd_vel（右 C + 左摇杆；50 Hz TwistStamped）
|
-> /hric/robot/cmd_vel（右 C + 左摇杆前后 / 左 Z + 右摇杆转向；50 Hz TwistStamped）
|
||||||
-> 天工 3.0 HBWALK 行走
|
-> 天工 3.0 HBWALK 行走
|
||||||
```
|
```
|
||||||
|
|
||||||
## 自动运行与操作
|
## 自动运行与操作
|
||||||
|
|
||||||
- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
|
- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
|
||||||
|
- 用户服务启动前会用短生命周期 ROS 探针等待 `/hric/robot/rl_state` 可发现，再创建
|
||||||
|
长生命周期桥接节点，避免开机早期网络接口尚未就绪时 Fast DDS 固化为空接口。
|
||||||
- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
|
- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
|
||||||
本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
|
本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
|
||||||
- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
|
- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
|
||||||
@@ -27,18 +29,29 @@ TS1P 同构臂
|
|||||||
- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
|
- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
|
||||||
- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
|
- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
|
||||||
前后/转向；不控制腰和头。
|
前后/转向；不控制腰和头。
|
||||||
- 行走：遥操已启动后，右 `C` 是即时 deadman；按住 C 并把左摇杆推出死区后，在下一个
|
- 行走：遥操已启动后，右 `C` + 左摇杆上下控制前后，左 `Z` + 右摇杆左右控制原地
|
||||||
`50 Hz` 周期立即响应，不再等待 3 秒。松开 C 或摇杆回中立即发零速，再按也立即恢复。
|
转向；两个组合都在下一个 `50 Hz` 周期立即响应，不再等待 3 秒。松开对应按键或
|
||||||
左摇杆上下控制前后，左右控制原地转向。
|
摇杆回中立即把该轴清零，再按也立即恢复。
|
||||||
|
前后仍使用二次细控曲线；转向在死区后使用线性曲线，使右摇杆中段有足够角速度，
|
||||||
|
但最大值仍受官方 `0.8 rad/s` 上限约束。
|
||||||
按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
|
按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
|
||||||
`0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
|
`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`。
|
映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。
|
||||||
- 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
|
- 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
|
||||||
在不清楚厂商语义的情况下把它擅自绑定为状态切换。
|
在不清楚厂商语义的情况下把它擅自绑定为状态切换。
|
||||||
- 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
|
- 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
|
||||||
BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
|
BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
|
||||||
若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
|
若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
|
||||||
|
- 右手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开
|
||||||
|
至少 `0.5 s` 后，再连续长按右 `B` 1 秒关闭。首次启动和每个新遥操会话都必须先
|
||||||
|
稳定松开 B，保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈
|
||||||
|
中断均不触发；计时期间冻结右手最后一条命令，左手和双臂仍照常跟随。
|
||||||
|
- 指向只覆盖右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号
|
||||||
|
“单食指”手势 `state 0`：归一化目标
|
||||||
|
`[0.2, 0.688, 0.0, 0.98, 0.98, 0.98]`，对应当前 `1~1000` 位置范围约为
|
||||||
|
`[201, 688, 1, 980, 980, 980]`；顺序为大拇指弯曲、大拇指旋转、食指、中指、
|
||||||
|
无名指、小拇指。进入和退出手势都继续使用每秒最多 400 个位置单位的现有限速。
|
||||||
|
|
||||||
## 安全门控
|
## 安全门控
|
||||||
|
|
||||||
@@ -53,7 +66,7 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
|
|||||||
`angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。
|
`angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。
|
||||||
|
|
||||||
行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆
|
行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆
|
||||||
即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
|
前后或左 Z + 右摇杆横向即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
|
||||||
回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
|
回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
|
||||||
|
|
||||||
灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
|
灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
|
||||||
@@ -61,6 +74,8 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
|
|||||||
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态、同构臂失联
|
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态、同构臂失联
|
||||||
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
|
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
|
||||||
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
|
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
|
||||||
|
匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态，但不会在
|
||||||
|
STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。
|
||||||
|
|
||||||
## 限速双臂回 Home
|
## 限速双臂回 Home
|
||||||
|
|
||||||
|
|||||||
@@ -5,6 +5,11 @@ omnisocket_server = "175.178.116.187:14049"
|
|||||||
omnisocket_peer_id = "tg3-009027fa8190-robot"
|
omnisocket_peer_id = "tg3-009027fa8190-robot"
|
||||||
omnisocket_expected_sender = "tg3-009027fa8190-iarm"
|
omnisocket_expected_sender = "tg3-009027fa8190-iarm"
|
||||||
omnisocket_max_packet_age_ms = 300.0
|
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
|
# The native session can remain blocked on an old connection after the public
|
||||||
# Hub disappears. Publication stops at source_timeout_s; after this longer
|
# Hub disappears. Publication stops at source_timeout_s; after this longer
|
||||||
# interval the process exits and systemd creates a completely fresh session.
|
# interval the process exits and systemd creates a completely fresh session.
|
||||||
@@ -52,8 +57,9 @@ joint_upper_rad = [
|
|||||||
]
|
]
|
||||||
|
|
||||||
[locomotion]
|
[locomotion]
|
||||||
# Right C remains an immediate deadman: while teleoperation is active, C plus
|
# The two original immediate bindings are independent: right C + left-stick
|
||||||
# a left-stick command acts on the next 50 Hz tick. There is no second hold.
|
# 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
|
enabled = true
|
||||||
command_topic = "/hric/robot/cmd_vel"
|
command_topic = "/hric/robot/cmd_vel"
|
||||||
# This bridge never publishes FSM commands. The robot must already report
|
# This bridge never publishes FSM commands. The robot must already report
|
||||||
@@ -63,6 +69,10 @@ frame_id = "pelvis"
|
|||||||
hold_seconds = 0.0
|
hold_seconds = 0.0
|
||||||
joystick_deadzone = 0.2
|
joystick_deadzone = 0.2
|
||||||
joystick_expo = 2.0
|
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:
|
# 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.
|
# linear.x [-0.8, +1.0] m/s and angular.z [-0.8, +0.8] rad/s.
|
||||||
max_forward_m_s = 1.0
|
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]
|
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]
|
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
|
||||||
|
# 1-second hold, separated by at least 0.5 seconds of stable release. A new
|
||||||
|
# session is always release-locked. The pose is xTELE GestureController's
|
||||||
|
# BrainCoRevo2 gesture 2 ("single index finger"), state 0, in motor order:
|
||||||
|
# thumb bend, thumb rotation, index, middle, ring, little.
|
||||||
|
right_b_point_gesture_enabled = true
|
||||||
|
right_b_point_gesture_hold_seconds = 1.0
|
||||||
|
right_b_point_gesture_release_seconds = 0.5
|
||||||
|
right_b_point_pose_normalized = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
|
||||||
|
|
||||||
[home]
|
[home]
|
||||||
# Deliberately slower than manual teleoperation. This Home pose was captured
|
# 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,
|
# from the robot's real arm feedback on 2026-08-06. Joint order is 11..17,
|
||||||
|
|||||||
243
tg3_local_teleop/gesture_toggle.py
Normal file
243
tg3_local_teleop/gesture_toggle.py
Normal file
@@ -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
|
||||||
268
tg3_local_teleop/test_gesture_toggle.py
Normal file
268
tg3_local_teleop/test_gesture_toggle.py
Normal file
@@ -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=1.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_one_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 + 1.0,
|
||||||
|
armed=True,
|
||||||
|
input_healthy=True,
|
||||||
|
pressed=True,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
self.assertTrue(
|
||||||
|
self.gesture.update(
|
||||||
|
released_at + 1.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 + 1.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 + 0.8,
|
||||||
|
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 + 1.29,
|
||||||
|
armed=True,
|
||||||
|
input_healthy=True,
|
||||||
|
pressed=False,
|
||||||
|
)
|
||||||
|
self.assertTrue(self.gesture.require_release)
|
||||||
|
self.gesture.update(
|
||||||
|
released_at + 1.3,
|
||||||
|
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 + 0.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 + 1.0)
|
||||||
|
self.gesture.update(
|
||||||
|
released_at + 0.01,
|
||||||
|
armed=True,
|
||||||
|
input_healthy=True,
|
||||||
|
pressed=True,
|
||||||
|
)
|
||||||
|
self.gesture.update(
|
||||||
|
released_at + 0.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 + 1.02,
|
||||||
|
armed=True,
|
||||||
|
input_healthy=True,
|
||||||
|
pressed=True,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
self.gesture.update(
|
||||||
|
released_at + 1.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 + 1.02,
|
||||||
|
armed=True,
|
||||||
|
input_healthy=True,
|
||||||
|
pressed=True,
|
||||||
|
)
|
||||||
|
self.assertTrue(self.gesture.active)
|
||||||
|
self.assertFalse(
|
||||||
|
self.gesture.update(
|
||||||
|
released_at + 1.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()
|
||||||
159
tg3_local_teleop/test_idle_session_refresh.py
Normal file
159
tg3_local_teleop/test_idle_session_refresh.py
Normal file
@@ -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()
|
||||||
@@ -79,6 +79,7 @@ class RobotSessionGateTest(unittest.TestCase):
|
|||||||
bridge.allow_publish = True
|
bridge.allow_publish = True
|
||||||
bridge.cfg = {"control": {"auto_home_on_stop": True}}
|
bridge.cfg = {"control": {"auto_home_on_stop": True}}
|
||||||
bridge.hands_enabled = False
|
bridge.hands_enabled = False
|
||||||
|
bridge.right_point_gesture_enabled = False
|
||||||
bridge.robot_arm_positions = [0.0] * 14
|
bridge.robot_arm_positions = [0.0] * 14
|
||||||
bridge.last_command = None
|
bridge.last_command = None
|
||||||
bridge.last_publish_at = 0.0
|
bridge.last_publish_at = 0.0
|
||||||
@@ -169,6 +170,7 @@ class RobotSessionGateTest(unittest.TestCase):
|
|||||||
"hold_seconds": 0.0,
|
"hold_seconds": 0.0,
|
||||||
"joystick_deadzone": 0.2,
|
"joystick_deadzone": 0.2,
|
||||||
"joystick_expo": 2.0,
|
"joystick_expo": 2.0,
|
||||||
|
"yaw_joystick_expo": 1.0,
|
||||||
"max_forward_m_s": 1.0,
|
"max_forward_m_s": 1.0,
|
||||||
"max_reverse_m_s": 0.8,
|
"max_reverse_m_s": 0.8,
|
||||||
"max_angular_rad_s": 0.8,
|
"max_angular_rad_s": 0.8,
|
||||||
@@ -193,15 +195,27 @@ class RobotSessionGateTest(unittest.TestCase):
|
|||||||
|
|
||||||
moving = ArmSnapshot(
|
moving = ArmSnapshot(
|
||||||
{
|
{
|
||||||
"button": {"right": [False, False, True]},
|
"button": {
|
||||||
"joystick": {"left": [1.0, 0.0]},
|
"left": [False, False, False],
|
||||||
|
"right": [False, False, True],
|
||||||
|
},
|
||||||
|
"joystick": {
|
||||||
|
"left": [1.0, 0.0],
|
||||||
|
"right": [0.0, 0.0],
|
||||||
|
},
|
||||||
},
|
},
|
||||||
received_at=1.0,
|
received_at=1.0,
|
||||||
)
|
)
|
||||||
released = ArmSnapshot(
|
released = ArmSnapshot(
|
||||||
{
|
{
|
||||||
"button": {"right": [False, False, False]},
|
"button": {
|
||||||
"joystick": {"left": [1.0, 0.0]},
|
"left": [False, False, False],
|
||||||
|
"right": [False, False, False],
|
||||||
|
},
|
||||||
|
"joystick": {
|
||||||
|
"left": [1.0, 0.0],
|
||||||
|
"right": [0.0, 0.0],
|
||||||
|
},
|
||||||
},
|
},
|
||||||
received_at=1.1,
|
received_at=1.1,
|
||||||
)
|
)
|
||||||
@@ -212,6 +226,38 @@ class RobotSessionGateTest(unittest.TestCase):
|
|||||||
bridge._tick_locomotion(1.2, moving)
|
bridge._tick_locomotion(1.2, moving)
|
||||||
self.assertEqual(published[-1], (1.0, -0.0))
|
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__":
|
if __name__ == "__main__":
|
||||||
unittest.main()
|
unittest.main()
|
||||||
|
|||||||
@@ -1,14 +1,16 @@
|
|||||||
[Unit]
|
[Unit]
|
||||||
Description=TG3 session-gated local TS1P teleoperation bridge
|
Description=TG3 local TS1P dual-arm bridge (no cloud pairing)
|
||||||
After=network-online.target
|
After=network-online.target
|
||||||
Wants=network-online.target
|
Wants=network-online.target
|
||||||
|
|
||||||
[Service]
|
[Service]
|
||||||
Type=simple
|
Type=simple
|
||||||
|
ExecStartPre=/home/nvidia/tg3_local_teleop/wait_ros_ready.sh
|
||||||
ExecStart=/home/nvidia/tg3_local_teleop/run.sh --allow-publish
|
ExecStart=/home/nvidia/tg3_local_teleop/run.sh --allow-publish
|
||||||
Restart=on-failure
|
Restart=on-failure
|
||||||
RestartSec=2
|
RestartSec=2
|
||||||
KillSignal=SIGINT
|
KillSignal=SIGINT
|
||||||
|
TimeoutStartSec=150
|
||||||
TimeoutStopSec=5
|
TimeoutStopSec=5
|
||||||
|
|
||||||
[Install]
|
[Install]
|
||||||
|
|||||||
@@ -30,6 +30,13 @@ from ros2_bridge_msgs.msg import ArmStatus
|
|||||||
from sensor_msgs.msg import JointState
|
from sensor_msgs.msg import JointState
|
||||||
from std_srvs.srv import Trigger
|
from std_srvs.srv import Trigger
|
||||||
|
|
||||||
|
from gesture_toggle import (
|
||||||
|
GestureToggle,
|
||||||
|
normalized_pose_to_positions,
|
||||||
|
right_b_pressed,
|
||||||
|
select_right_hand_target,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
JOINT_NAMES = [
|
JOINT_NAMES = [
|
||||||
*(f"left_joints_{i}" for i in range(7)),
|
*(f"left_joints_{i}" for i in range(7)),
|
||||||
@@ -70,6 +77,10 @@ class LatestArmData:
|
|||||||
self._metrics: dict[str, Any] = {
|
self._metrics: dict[str, Any] = {
|
||||||
"transport": self.transport,
|
"transport": self.transport,
|
||||||
"connected": False,
|
"connected": False,
|
||||||
|
"registered": False,
|
||||||
|
"session_connects": 0,
|
||||||
|
"idle_session_refreshes": 0,
|
||||||
|
"idle_session_refresh_failures": 0,
|
||||||
"frames_received": 0,
|
"frames_received": 0,
|
||||||
"frames_accepted": 0,
|
"frames_accepted": 0,
|
||||||
"dropped_sender": 0,
|
"dropped_sender": 0,
|
||||||
@@ -150,6 +161,11 @@ class LatestArmData:
|
|||||||
|
|
||||||
expected_sender = str(self.cfg["omnisocket_expected_sender"])
|
expected_sender = str(self.cfg["omnisocket_expected_sender"])
|
||||||
max_age_ms = float(self.cfg["omnisocket_max_packet_age_ms"])
|
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
|
last_sequence = 0
|
||||||
while not self._stop.is_set():
|
while not self._stop.is_set():
|
||||||
session = Session()
|
session = Session()
|
||||||
@@ -160,13 +176,83 @@ class LatestArmData:
|
|||||||
peer_id=str(self.cfg["omnisocket_peer_id"]),
|
peer_id=str(self.cfg["omnisocket_peer_id"]),
|
||||||
**CONTROL_DEFAULTS,
|
**CONTROL_DEFAULTS,
|
||||||
)
|
)
|
||||||
|
last_accepted_at = time.monotonic()
|
||||||
|
session_stats = session.stats()
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._metrics["connected"] = True
|
self._metrics["connected"] = True
|
||||||
|
self._metrics["registered"] = bool(
|
||||||
|
int(session_stats.get("registered", 0)) == 1
|
||||||
|
)
|
||||||
|
self._metrics["session_connects"] += 1
|
||||||
self._last_error = ""
|
self._last_error = ""
|
||||||
|
|
||||||
while not self._stop.is_set():
|
while not self._stop.is_set():
|
||||||
message = session.recv(timeout_ms=100)
|
message = session.recv(timeout_ms=100)
|
||||||
if message is None:
|
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
|
continue
|
||||||
messages = [message]
|
messages = [message]
|
||||||
while True:
|
while True:
|
||||||
@@ -229,9 +315,11 @@ class LatestArmData:
|
|||||||
)
|
)
|
||||||
self._last_error = ""
|
self._last_error = ""
|
||||||
self._metrics["frames_accepted"] += 1
|
self._metrics["frames_accepted"] += 1
|
||||||
|
last_accepted_at = time.monotonic()
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._metrics["connected"] = False
|
self._metrics["connected"] = False
|
||||||
|
self._metrics["registered"] = False
|
||||||
self._last_error = f"OmniSocket connection failed: {exc}"
|
self._last_error = f"OmniSocket connection failed: {exc}"
|
||||||
finally:
|
finally:
|
||||||
try:
|
try:
|
||||||
@@ -240,8 +328,18 @@ class LatestArmData:
|
|||||||
pass
|
pass
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._metrics["connected"] = False
|
self._metrics["connected"] = False
|
||||||
|
self._metrics["registered"] = False
|
||||||
self._stop.wait(1.0)
|
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(
|
def _decode_omni_packet(
|
||||||
self,
|
self,
|
||||||
msg_type: int,
|
msg_type: int,
|
||||||
@@ -303,6 +401,27 @@ class LocalTeleopBridge(Node):
|
|||||||
net_cfg = config["network"]
|
net_cfg = config["network"]
|
||||||
self.hands_cfg = config.get("hands", {})
|
self.hands_cfg = config.get("hands", {})
|
||||||
self.hands_enabled = bool(self.hands_cfg.get("enabled", False))
|
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", 1.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_cfg = config.get("locomotion", {})
|
||||||
self.locomotion_enabled = bool(
|
self.locomotion_enabled = bool(
|
||||||
self.locomotion_cfg.get("enabled", False)
|
self.locomotion_cfg.get("enabled", False)
|
||||||
@@ -571,6 +690,26 @@ class LocalTeleopBridge(Node):
|
|||||||
success=False,
|
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:
|
if self.returning_home:
|
||||||
self._tick_home(now)
|
self._tick_home(now)
|
||||||
elif self.armed and sample is not None:
|
elif self.armed and sample is not None:
|
||||||
@@ -600,6 +739,17 @@ class LocalTeleopBridge(Node):
|
|||||||
)
|
)
|
||||||
self.hand_output_ready = True
|
self.hand_output_ready = True
|
||||||
desired_hands = self._hand_targets(sample.data)
|
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 = {
|
commands = {
|
||||||
side: self._slew_hand(side, desired_hands[side], now)
|
side: self._slew_hand(side, desired_hands[side], now)
|
||||||
for side in HAND_SIDES
|
for side in HAND_SIDES
|
||||||
@@ -786,6 +936,8 @@ class LocalTeleopBridge(Node):
|
|||||||
"new teleoperation START accepted", success=False, cancelled=True
|
"new teleoperation START accepted", success=False, cancelled=True
|
||||||
)
|
)
|
||||||
self.active_session_id = session_id
|
self.active_session_id = session_id
|
||||||
|
if self.right_point_gesture_enabled:
|
||||||
|
self.right_point_gesture.new_session()
|
||||||
self.armed = True
|
self.armed = True
|
||||||
# Start both slew limiters at measured robot feedback, never at a
|
# Start both slew limiters at measured robot feedback, never at a
|
||||||
# potentially distant first network target.
|
# potentially distant first network target.
|
||||||
@@ -849,6 +1001,8 @@ class LocalTeleopBridge(Node):
|
|||||||
if reasons:
|
if reasons:
|
||||||
self.get_logger().error("cannot arm: " + "; ".join(reasons))
|
self.get_logger().error("cannot arm: " + "; ".join(reasons))
|
||||||
return
|
return
|
||||||
|
if self.right_point_gesture_enabled:
|
||||||
|
self.right_point_gesture.new_session()
|
||||||
self.armed = True
|
self.armed = True
|
||||||
# Start the slew limiter at measured robot feedback. Using None here
|
# Start the slew limiter at measured robot feedback. Using None here
|
||||||
# would make the first armed frame jump directly to the TS1P target.
|
# 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:
|
def _disarm(self, reason: str) -> None:
|
||||||
was_armed = self.armed
|
was_armed = self.armed
|
||||||
self._stop_locomotion(reason)
|
self._stop_locomotion(reason)
|
||||||
|
if self.right_point_gesture_enabled:
|
||||||
|
# Clearing the logical override must not publish a hand target.
|
||||||
|
# Existing STOP behavior leaves the physical hand at its last
|
||||||
|
# limited command until a later, newly armed session.
|
||||||
|
self.right_point_gesture.disarm(reason)
|
||||||
self.armed = False
|
self.armed = False
|
||||||
self.hand_output_ready = False
|
self.hand_output_ready = False
|
||||||
self.runtime_hand_output_reasons = []
|
self.runtime_hand_output_reasons = []
|
||||||
@@ -898,20 +1057,29 @@ class LocalTeleopBridge(Node):
|
|||||||
def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None:
|
def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None:
|
||||||
cfg = self.locomotion_cfg
|
cfg = self.locomotion_cfg
|
||||||
try:
|
try:
|
||||||
buttons = sample.data["button"]["right"]
|
left_buttons = sample.data["button"]["left"]
|
||||||
joystick = sample.data["joystick"]["left"]
|
right_buttons = sample.data["button"]["right"]
|
||||||
right_c = len(buttons) >= 3 and bool(buttons[2])
|
left_joystick = sample.data["joystick"]["left"]
|
||||||
if len(joystick) != 2:
|
right_joystick = sample.data["joystick"]["right"]
|
||||||
raise ValueError("left joystick must contain x/y")
|
left_z = len(left_buttons) >= 3 and bool(left_buttons[2])
|
||||||
# TS1P reports the physical forward/back axis first and the
|
right_c = len(right_buttons) >= 3 and bool(right_buttons[2])
|
||||||
# left/right axis second. This was verified on the installed
|
if len(left_joystick) != 2 or len(right_joystick) != 2:
|
||||||
# xTELE 0.1.2 stream; treating the pair as Cartesian x/y made a
|
raise ValueError("left and right joysticks must each contain two axes")
|
||||||
# forward stick command become pure yaw.
|
# xTELE 0.1.2 stores each TS1P stick as [vertical, horizontal].
|
||||||
raw_forward, raw_yaw = (float(joystick[0]), float(joystick[1]))
|
# 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"])
|
deadzone = float(cfg["joystick_deadzone"])
|
||||||
expo = float(cfg["joystick_expo"])
|
forward_expo = float(cfg["joystick_expo"])
|
||||||
shaped_forward = self._shape_joystick_axis(raw_forward, deadzone, expo)
|
yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo))
|
||||||
shaped_yaw = self._shape_joystick_axis(raw_yaw, deadzone, 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(
|
signed_forward = shaped_forward * float(
|
||||||
cfg.get("forward_axis_sign", 1.0)
|
cfg.get("forward_axis_sign", 1.0)
|
||||||
)
|
)
|
||||||
@@ -927,9 +1095,12 @@ class LocalTeleopBridge(Node):
|
|||||||
self._stop_locomotion(f"invalid locomotion input: {exc}")
|
self._stop_locomotion(f"invalid locomotion input: {exc}")
|
||||||
return
|
return
|
||||||
|
|
||||||
joystick_active = shaped_forward != 0.0 or shaped_yaw != 0.0
|
forward_active = right_c and shaped_forward != 0.0
|
||||||
if not right_c or not joystick_active:
|
yaw_active = left_z and shaped_yaw != 0.0
|
||||||
self._stop_locomotion("right C released or left joystick returned to center")
|
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)
|
self._tick_walk_zero_burst(now)
|
||||||
return
|
return
|
||||||
|
|
||||||
@@ -944,17 +1115,19 @@ class LocalTeleopBridge(Node):
|
|||||||
self.walk_active = True
|
self.walk_active = True
|
||||||
self.walk_zero_frames_remaining = 0
|
self.walk_zero_frames_remaining = 0
|
||||||
self.get_logger().warning(
|
self.get_logger().warning(
|
||||||
"LOCAL HBWALK VELOCITY STARTED: immediate right C + left "
|
"LOCAL HBWALK VELOCITY STARTED: right C + left-stick forward "
|
||||||
"joystick input"
|
"or left Z + right-stick yaw"
|
||||||
)
|
)
|
||||||
|
|
||||||
linear_limit = max_forward if signed_forward >= 0.0 else max_reverse
|
linear_limit = max_forward if signed_forward >= 0.0 else max_reverse
|
||||||
linear_x = signed_forward * linear_limit
|
linear_x = signed_forward * linear_limit if forward_active else 0.0
|
||||||
angular_z = (
|
angular_z = 0.0
|
||||||
shaped_yaw
|
if yaw_active:
|
||||||
* float(cfg.get("yaw_axis_sign", -1.0))
|
angular_z = (
|
||||||
* max_angular
|
shaped_yaw
|
||||||
)
|
* float(cfg.get("yaw_axis_sign", -1.0))
|
||||||
|
* max_angular
|
||||||
|
)
|
||||||
self._publish_walk(linear_x, angular_z, now)
|
self._publish_walk(linear_x, angular_z, now)
|
||||||
|
|
||||||
def _stop_locomotion(self, reason: str) -> None:
|
def _stop_locomotion(self, reason: str) -> None:
|
||||||
@@ -1432,8 +1605,62 @@ class LocalTeleopBridge(Node):
|
|||||||
else round(now - self.last_hand_publish_at, 4),
|
else round(now - self.last_hand_publish_at, 4),
|
||||||
"hand_publish_count": self.hand_publish_count,
|
"hand_publish_count": self.hand_publish_count,
|
||||||
"foreign_hand_source_seen": self.foreign_hand_source_seen,
|
"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_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_active": self.walk_active,
|
||||||
"locomotion_hold_s": 0.0,
|
"locomotion_hold_s": 0.0,
|
||||||
"locomotion_command": {
|
"locomotion_command": {
|
||||||
@@ -1444,6 +1671,11 @@ class LocalTeleopBridge(Node):
|
|||||||
"forward_m_s": self.locomotion_cfg.get("max_forward_m_s"),
|
"forward_m_s": self.locomotion_cfg.get("max_forward_m_s"),
|
||||||
"reverse_m_s": self.locomotion_cfg.get("max_reverse_m_s"),
|
"reverse_m_s": self.locomotion_cfg.get("max_reverse_m_s"),
|
||||||
"angular_rad_s": self.locomotion_cfg.get("max_angular_rad_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_publish_count": self.walk_publish_count,
|
||||||
"locomotion_fsm_publish_enabled": False,
|
"locomotion_fsm_publish_enabled": False,
|
||||||
|
|||||||
34
tg3_local_teleop/wait_ros_ready.sh
Executable file
34
tg3_local_teleop/wait_ros_ready.sh
Executable file
@@ -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
|
||||||
@@ -21,14 +21,34 @@ Z+C 按键及诊断字段；若 5001 的双侧 `hand.position` 在 250 ms 内有
|
|||||||
xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和腰、头、腿、行走、
|
xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和腰、头、腿、行走、
|
||||||
底盘命令不会进入机器人双臂桥。
|
底盘命令不会进入机器人双臂桥。
|
||||||
|
|
||||||
|
右侧原始按键顺序是 `A/B/C`。按住右 `B` 时，sender 仍合并 5001 的左手
|
||||||
|
目标，但保留 5003 的原始右手开合量，不用 xTELE 处理后的右手目标覆盖。
|
||||||
|
这样右 B 的自定义长按手势还未满 1 秒时，原厂 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，但不发送
|
公网业务数据由 EAI 本地会话门控：服务启动后仍持续读取本机 5003/5001，但不发送
|
||||||
xTELE 帧；连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_id` 并开始发送。
|
xTELE 帧；连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_id` 并开始发送。
|
||||||
再次长按 3 秒时发送最后一帧 `stop`，等待有界 KCP 刷新后关闭 OmniSocket Session；
|
再次长按 3 秒时发送最后一帧 `stop`，等待有界 KCP 刷新后关闭 OmniSocket Session；
|
||||||
之后 `frames_sent/bytes_sent` 不再增长，待机既没有 xTELE 业务帧，也没有该 sender 的
|
之后 `frames_sent/bytes_sent` 不再增长，待机既没有 xTELE 业务帧，也没有该 sender 的
|
||||||
底层注册/心跳。即使 Hub 离线，sender 仍持续读取本机 xTELE 并保持会话关闭；启动连接
|
底层注册/心跳。即使 Hub 离线，sender 仍持续读取本机 xTELE 并保持会话关闭。Z+C 满
|
||||||
失败会使本次会话失效，必须松开组合键后重新长按，不会在 Hub 恢复时续发旧动作。
|
3 秒后先进入 `START_PENDING`，后台连接不会阻塞本机按键采样；本次长按只尝试连接一次。
|
||||||
|
若 Hub 未启动，本次连接失败且不会发出业务帧；若机器人 Peer 不存在或首帧发送失败，
|
||||||
|
机器人不会收到控制数据且 sender 在收到错误后立即关闭。这次请求都必须稳定松开后再做
|
||||||
|
一次新的 3 秒长按；Hub 稍后恢复不会让旧请求自行启动。
|
||||||
|
任一键松开、按键数据畸形或 5003 失联也会立即取消 pending。连接成功后还必须看到两帧
|
||||||
|
时间戳最终递增且仍按住 Z+C 的新鲜 5003 数据，才发送第一帧 START，连接前排队的旧
|
||||||
|
动作不会被续发。
|
||||||
活动期间若 KCP 反馈超过 `500 ms` 未更新，或 `snd_queue+snd_buffer` 超过 100 帧，sender
|
活动期间若 KCP 反馈超过 `500 ms` 未更新，或 `snd_queue+snd_buffer` 超过 100 帧，sender
|
||||||
立即作废会话并关闭 Session，从源头丢弃待发队列，防止网络恢复后回放旧动作。
|
立即作废会话并关闭 Session，从源头丢弃待发队列，防止网络恢复后回放旧动作。
|
||||||
|
5003 的源时间戳在活动期间也必须持续推进；允许相邻毫秒值短暂重复，但超过 `250 ms`
|
||||||
|
不再推进、时间戳倒退、持续畸形 JSON 或超长帧都会立即作废会话。操作员 STOP 无论最终
|
||||||
|
业务帧能否编码或发送，EAI 都会在本地关闭 Session，机器人端由输入超时保护停止并回 Home。
|
||||||
|
|
||||||
每个业务 JSON 的 `tg3_transport` 由 sender 强制覆盖，不能由 5003 输入伪造：
|
每个业务 JSON 的 `tg3_transport` 由 sender 强制覆盖，不能由 5003 输入伪造：
|
||||||
|
|
||||||
@@ -42,9 +62,10 @@ xTELE 帧；连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_i
|
|||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|
||||||
`start` 连续发送 50 个源帧，避免接收端只取最新帧时错过唯一启动事件；机器人对同一
|
当前用户服务将 `start` 连续发送 500 个源帧，避免接收端只取最新帧时错过唯一启动事件；机器人对同一
|
||||||
`session_id` 只做一次启动安全检查。`stop` 是该会话最后一个业务帧。服务重启、源数据
|
`session_id` 只做一次启动安全检查。`stop` 是该会话最后一个业务帧。服务重启、源数据
|
||||||
失联或网络错误都会使会话失效，恢复后必须先松开组合键，再重新长按 3 秒。
|
失联或网络错误都会使会话失效，恢复后必须先松开组合键，再重新长按 3 秒；pending 和
|
||||||
|
活动会话都不会自动重试或自动恢复。
|
||||||
`tg3_local_teleop` 内部直接拒绝非预期发送端、
|
`tg3_local_teleop` 内部直接拒绝非预期发送端、
|
||||||
乱序、格式错误以及相对本次连接最低包龄额外排队超过 300 ms 的数据；相对包龄会消除
|
乱序、格式错误以及相对本次连接最低包龄额外排队超过 300 ms 的数据；相对包龄会消除
|
||||||
两端系统时钟的固定偏差。遥操桥仍有 250 ms 输入失联保护。
|
两端系统时钟的固定偏差。遥操桥仍有 250 ms 输入失联保护。
|
||||||
|
|||||||
821
tg3_omnisocket_transport/omnisocket_xtele_sender.py
Normal file → Executable file
821
tg3_omnisocket_transport/omnisocket_xtele_sender.py
Normal file → Executable file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -7,7 +7,7 @@ Wants=network-online.target
|
|||||||
Type=simple
|
Type=simple
|
||||||
WorkingDirectory=/home/eai/tg3_omnisocket_transport
|
WorkingDirectory=/home/eai/tg3_omnisocket_transport
|
||||||
Environment=PYTHONPATH=/home/eai/OmniSocketGo/python
|
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
|
Restart=always
|
||||||
RestartSec=1
|
RestartSec=1
|
||||||
KillSignal=SIGINT
|
KillSignal=SIGINT
|
||||||
|
|||||||
42
verify.sh
42
verify.sh
@@ -4,13 +4,45 @@ set -euo pipefail
|
|||||||
repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
expected_omnisocket_commit="de3f5c96779dbe1571c10feb22fc7f2331b6b222"
|
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_omnisocket_transport/test_session_gate.py"
|
||||||
python3 "$repo_dir/tg3_local_teleop/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)"
|
with open(sys.argv[1], "rb") as stream:
|
||||||
if [[ "$actual_omnisocket_commit" != "$expected_omnisocket_commit" ]]; then
|
tomllib.load(stream)
|
||||||
echo "OmniSocketGo commit mismatch: $actual_omnisocket_commit" >&2
|
print("config.toml parse passed")
|
||||||
exit 1
|
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
|
fi
|
||||||
|
|
||||||
echo "Offline checks passed; OmniSocketGo is pinned to $actual_omnisocket_commit"
|
echo "All offline TG3 checks passed"
|
||||||
|
|||||||
Reference in New Issue
Block a user