配置:消灭代码里的硬编码臂 SN,并给"换臂漏改"加一把尺子
## 为什么
"设备 ID 全部可配置"这条要求在代码里只做了一半:site.yaml 能配,
但**代码和静态配置里到处躺着 Rizon4-063989 当兜底**。这类兜底的危害
不是"改起来麻烦",而是它把"配置没生效"伪装成"配置生效了":
* `robot_sn` 默认 `Rizon4-063989` → 换臂后节点去订**上一台臂**的关节话题,
节点起来了、不报错、什么都不动;
* launch 里用 `ee_frame == "Rizon4-063989_flange"` 当"用户没指定"的哨兵 →
换个站点这个判断永远不成立,于是 site.yaml 里的 ee_frame 再也不会被用上,
静态 TF 挂错坐标系、点还照样查得到;
* `if SN == "Rizon4-063989"` 决定要不要拉触觉服务 → 换臂后触觉静默消失。
## 改了什么
1. **删掉所有"兜底成某台臂"的默认值**(15 处脚本 + 6 个 launch + 6 个节点)。
没配就为空 / 直接 `:?` 报错,宁可起不来也不要连错机器:
- `scripts/lib/export_site_env.py`:别名解析改为从 `robots[]` 现算
(`arm_short_id` / `resolve_robot_sn` / `describe_arms`),选不出臂时
抛 `ValueError`;`main()` 捕获后打人话 + 退出码 2(供 shell `eval`);
- 手眼文件/路点/工作目录由当前臂 short id 派生,不再写死 `89_*`;
- `ROBOT_HAS_TACTILE` 改为读 site.yaml 的 `robots[].tactile`;
- `arm_application/site_io.py` 新增 `joint_prefix_default()` /
`base_frame_default()` / `ee_frame_default()`:优先读 `SITE_*`,
退而由 `SITE_ROBOT_SN` 现算,**不写死 SN**;
- `arm.urdf.xacro` 的 `robot_sn` 默认值改成明显的占位符 `UNSET_ARM_SN`
(关节名会带出来,报错信息自解释);
- `ros2_control.launch.py` / `move_group.launch.py` **原先根本不传
robot_sn**,靠 xacro 写死默认值"蒙对"本机,现在都从环境取。
2. **新增 `scripts/check_arm_prefixes.py`**:Flexiv 把 SN 编进关节名,
所以有一批一级配置(SRDF/controllers/joint_names/bringup/servo/...)
必须跟着当前臂走。漏改的症状是"MoveIt 规划成功但机械臂不动",很难查。
这个脚本逐文件逐行列出对不上的地方,并直接给出修法。
`--list` 还会说明 SN 来自 site.yaml 还是被 `ROBOT_SN` 覆盖。
3. **文档**:`FRAMEWORK_RULES.md` 新增 R1.1.1(两类含臂名文件怎么分、
换臂四步)+ 检查清单第 12 条 + 反例 2 条;`HANDBOOK.md` 新增 3.6.1
与症状表 2 行。
4. **顺带修掉的真 bug**(都是这次改动带出来、被 pyflakes + 真跑抓住的):
- `convert_flexiv_traj.py` / `rdk_drag_bridge.py` 用了 `os` 但没 import
→ `--help` 直接 NameError;
- `check_arm_prefixes.py` 第一版正则用 `\b` 收尾,而关节名是
`Rizon4-063989_joint1`(`_` 属于 \w,`\b` 不成立)→ SRDF 里 24 处
硬编码**一处都没报**,还打印 OK。已加负向测试钉住;
- 删掉若干死代码/死 import(`pos_line`/`flat`/`names`、`IfCondition` 等)。
## 验收(都真跑过)
- `python3 -m pyflakes <全部改过的 py>` → 干净
- 逐包 pytest:233 + 69 + 78 + 24 + 236 = **640 passed**
- `env -u ROBOT_SN python3 scripts/check_arm_prefixes.py` → OK(8 文件)
- 换成 SN=Rizon4-777001 的临时 site.yaml → 报出**8 个**文件不一致并给修法(退出码 1)
- 故意不给 SITE_* → 各 launch 的 `robot_sn` / `ee_frame` / `handeye_file`
默认值为 `''`,不再悄悄指向上一台臂
- `ros2 launch ... --show-args` 对 stack/sim/ros2_control/mock_control/
move_group/display/visual_servo **全部通过**
- `convert_flexiv_traj.py --help` 正常;拿 `xunhuan.traj` 真转 5 个路点,
输出 `joint_prefix: "Rizon4-063989_"` 来自 site.yaml
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -35,6 +35,9 @@ robot:
|
|||||||
pc_ip: "192.168.2.XXX"
|
pc_ip: "192.168.2.XXX"
|
||||||
iface: "enp4s0"
|
iface: "enp4s0"
|
||||||
label: "左臂89"
|
label: "左臂89"
|
||||||
|
# 本臂装了 Orisys 视触觉(只有左臂有)。脚本靠这一项决定要不要拉触觉服务,
|
||||||
|
# 而不是靠"SN 等于某个具体值"这种写死的判断(换台臂就会失效)。
|
||||||
|
tactile: true
|
||||||
camera:
|
camera:
|
||||||
name: d405_left # 话题 /d405_left/... 与 91 分开
|
name: d405_left # 话题 /d405_left/... 与 91 分开
|
||||||
serial: "<序列号>" # 左腕 D405(一直在用的那台)
|
serial: "<序列号>" # 左腕 D405(一直在用的那台)
|
||||||
|
|||||||
@@ -61,6 +61,44 @@
|
|||||||
**禁止**在代码里硬编码站点值。真实反例:15 处硬编码的 `Rizon4-063989`
|
**禁止**在代码里硬编码站点值。真实反例:15 处硬编码的 `Rizon4-063989`
|
||||||
(见 `R8` 的清单),换一台臂就会静默连错机器。
|
(见 `R8` 的清单),换一台臂就会静默连错机器。
|
||||||
|
|
||||||
|
#### R1.1.1 含臂名的一级配置分两类,别混
|
||||||
|
|
||||||
|
Flexiv 官方把 SN 编进了关节名(`Rizon4-063989_joint1`),所以"含 SN 的文件"
|
||||||
|
天然分成两类。**新增文件时必须先归类**,归错类就是下一颗哑弹:
|
||||||
|
|
||||||
|
| 类 | 特征 | 换臂时 | 例子 |
|
||||||
|
|----|------|--------|------|
|
||||||
|
| **跟随当前臂** | 描述"这台臂的接口/结构",只可能有一个正确值 | **必须改**,否则控制器报关节不存在 | `arm_control/config/controllers.yaml`、`named_poses.yaml`、`arm_moveit_config/config/moveit_controllers.yaml`、`arm_moveit_config/srdf/arm.srdf`、`arm_bringup/config/bringup.yaml`、`arm_description/config/joint_names.yaml`、`arm_application/config/servo.yaml`、`xunhuan_trajectory.yaml` |
|
||||||
|
| **按臂各留一份** | 记录某台臂的**实测结果**,多台共存是对的 | 不用改,该**新增**一份(如 `named_poses_91.yaml`) | `config/site.yaml`、`named_poses_89/91.yaml`、`arm_application/config/dual_grippers.yaml`、`handeye/waypoints.yaml` |
|
||||||
|
|
||||||
|
第一类**一定要显式检查**,因为漏改的症状有两种,而且都能伪装成别的问题:
|
||||||
|
|
||||||
|
- 控制器起不来 → `Joint 'Rizon4-063989_joint1' not found`(这条还算好查);
|
||||||
|
- URDF 换了名字、SRDF 没换 → MoveIt 规划成功但**机械臂不动**(难查得多)。
|
||||||
|
|
||||||
|
所以有一把专用尺子:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 scripts/check_arm_prefixes.py # 一致则 0,不一致逐条列出 file:line + 修法
|
||||||
|
python3 scripts/check_arm_prefixes.py --list # 只看当前臂的应然前缀,以及它来自哪儿
|
||||||
|
```
|
||||||
|
|
||||||
|
**换臂的完整动作**(少一步就会出上面那种症状):
|
||||||
|
|
||||||
|
1. 改 `config/site.yaml`(`robot.sn` + `robot.robots[]`);
|
||||||
|
2. `source scripts/env.sh --arm <新臂>`;
|
||||||
|
3. `python3 scripts/check_arm_prefixes.py` —— **必须输出 OK**;
|
||||||
|
4. `bash scripts/build.sh`(让 `install/` 同步)。
|
||||||
|
|
||||||
|
> ⚠️ 第 3 步为什么不能省:这个脚本的 `_SN_TOKEN` 正则第一版写成
|
||||||
|
> `\bRizon\d+-\d{6}\b`,结果 SRDF 里 24 处硬编码**一个都没报**(`_joint1` 的
|
||||||
|
> 下划线属于 `\w`,`\b` 不成立)。检查器的漏洞和配置的漏洞长得一模一样,
|
||||||
|
> 所以它自己也必须有负向测试(见 R5.2)。
|
||||||
|
|
||||||
|
> ⚠️ `ROBOT_SN` / `ROBOT_ARM` 环境变量会**覆盖** `site.yaml` 的 `robot.sn`
|
||||||
|
> (为了支持"这个终端临时用另一台臂")。排查"明明改了配置却没生效"时,
|
||||||
|
> 先 `echo $ROBOT_SN` —— `check_arm_prefixes.py --list` 会直接告诉你值从哪来。
|
||||||
|
|
||||||
### R1.2 新增 `site.yaml` 键必须二选一:**接线** 或 **明确告警**
|
### R1.2 新增 `site.yaml` 键必须二选一:**接线** 或 **明确告警**
|
||||||
|
|
||||||
声明了却没接线的键是最难查的一类问题:用户按文档改了 `site.yaml`,行为毫无变化,
|
声明了却没接线的键是最难查的一类问题:用户按文档改了 `site.yaml`,行为毫无变化,
|
||||||
@@ -454,6 +492,7 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0
|
|||||||
| 9 | 提交信息里有"为什么"和实测数据吗? | R6.4 |
|
| 9 | 提交信息里有"为什么"和实测数据吗? | R6.4 |
|
||||||
| 10 | 我写进文档的命令,自己跑过一遍吗? | R6.7 |
|
| 10 | 我写进文档的命令,自己跑过一遍吗? | R6.7 |
|
||||||
| 11 | 这次构建是在**目标环境**里做的吗?(`head -1 install/*/lib/*/*`) | R6.8、R5.7 |
|
| 11 | 这次构建是在**目标环境**里做的吗?(`head -1 install/*/lib/*/*`) | R6.8、R5.7 |
|
||||||
|
| 12 | 我碰的文件属于"跟随当前臂"还是"按臂各留一份"?改完跑 `check_arm_prefixes.py` 了吗? | R1.1.1 |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@@ -485,6 +524,8 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0
|
|||||||
| 脚本说"已停止",进程还在跑 | 进程模式表漏项(漏过 `robot_state_publisher`) | M1、R4.10 |
|
| 脚本说"已停止",进程还在跑 | 进程模式表漏项(漏过 `robot_state_publisher`) | M1、R4.10 |
|
||||||
| `pkill -f gz` 秒返回、什么都没停 | 它先匹配到调用它的 shell | R4.10 |
|
| `pkill -f gz` 秒返回、什么都没停 | 它先匹配到调用它的 shell | R4.10 |
|
||||||
| 换了台臂却连错机器 | 硬编码的 `Rizon4-063989` 默认值 | R1.1 |
|
| 换了台臂却连错机器 | 硬编码的 `Rizon4-063989` 默认值 | R1.1 |
|
||||||
|
| 换了台臂,MoveIt 说规划成功、机械臂不动 | SRDF/controllers 还是旧 SN 的关节名(跟随臂的一级配置漏改) | R1.1.1 |
|
||||||
|
| 前缀检查器"全部通过",其实一处都没查 | 正则用 `\b` 收尾,而关节名是 `Rizon4-063989_joint1` | R5.2、R1.1.1 |
|
||||||
| `assets_root()` 指向从未存在的 `assets/` | 零调用者的死代码,没人发现 | R5.5 |
|
| `assets_root()` 指向从未存在的 `assets/` | 零调用者的死代码,没人发现 | R5.5 |
|
||||||
| 脚本报 `$'\r': 未找到命令`、`set: pipefail: 无效的选项名` | 脚本是 CRLF 行尾,`\r` 被当成标识符的一部分 | R6.6 |
|
| 脚本报 `$'\r': 未找到命令`、`set: pipefail: 无效的选项名` | 脚本是 CRLF 行尾,`\r` 被当成标识符的一部分 | R6.6 |
|
||||||
| `bash -n` 说没问题,真跑立刻死 | `bash -n` 对 CRLF 会漏报 —— 静态检查≠能用 | R6.6 |
|
| `bash -n` 说没问题,真跑立刻死 | `bash -n` 对 CRLF 会漏报 —— 静态检查≠能用 | R6.6 |
|
||||||
|
|||||||
@@ -254,6 +254,36 @@ ros2 run arm_interfaces device_check --list-brands # 看每类现在有哪
|
|||||||
改完不算清楚,孔检测在仿真里会"根本不可能通过",而看起来像算法不行。
|
改完不算清楚,孔检测在仿真里会"根本不可能通过",而看起来像算法不行。
|
||||||
详见[仿真手册 §4.3](../devices/sim/gazebo/README.md)。
|
详见[仿真手册 §4.3](../devices/sim/gazebo/README.md)。
|
||||||
|
|
||||||
|
### 3.6.1 换一台机械臂(最容易漏改,而且很难查)
|
||||||
|
|
||||||
|
`site.yaml` 里写的是**哪台臂**,但有一批一级配置文件里写死了**臂的关节名**
|
||||||
|
(Flexiv 官方把 SN 编进关节名:`Rizon4-063989_joint1`)。这些文件必须跟着改:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 scripts/check_arm_prefixes.py # 逐条列出哪个文件哪一行对不上,并给修法
|
||||||
|
python3 scripts/check_arm_prefixes.py --list # 只看当前臂的应然前缀
|
||||||
|
```
|
||||||
|
|
||||||
|
漏改的症状有两种,**都不像"配置错了"**:
|
||||||
|
|
||||||
|
- 控制器报 `Joint 'Rizon4-063989_joint1' not found`(这条还算好查);
|
||||||
|
- URDF 换了名字、`srdf/arm.srdf` 没换 → MoveIt **规划成功但机械臂不动**。
|
||||||
|
|
||||||
|
换臂四步(少一步就出上面那种症状):
|
||||||
|
|
||||||
|
```bash
|
||||||
|
vim config/site.yaml # 1. robot.sn + robot.robots[]
|
||||||
|
source scripts/env.sh --arm 新臂 # 2. 选定当前臂
|
||||||
|
python3 scripts/check_arm_prefixes.py # 3. 必须输出 OK
|
||||||
|
bash scripts/build.sh # 4. 让 install/ 同步
|
||||||
|
```
|
||||||
|
|
||||||
|
哪些文件该跟着改、哪些是"按臂各留一份"的现场资产,
|
||||||
|
见 [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 的 R1.1.1 表。
|
||||||
|
|
||||||
|
> 排查提示:`ROBOT_SN` / `ROBOT_ARM` 环境变量会**覆盖** `site.yaml` 的 `robot.sn`。
|
||||||
|
> "明明改了配置却没生效"时先 `echo $ROBOT_SN`,或看 `--list` 打印的"来源"。
|
||||||
|
|
||||||
### 3.7 改完必须做的一件事
|
### 3.7 改完必须做的一件事
|
||||||
|
|
||||||
**问自己:我故意弄坏它,哪个检查会红?**
|
**问自己:我故意弄坏它,哪个检查会红?**
|
||||||
@@ -289,6 +319,7 @@ source scripts/sim_env.sh # 仿真环境(独立 con
|
|||||||
# —— 设备与配置 ——
|
# —— 设备与配置 ——
|
||||||
ros2 run arm_interfaces device_check # 设备契约 + site.yaml 必填项
|
ros2 run arm_interfaces device_check # 设备契约 + site.yaml 必填项
|
||||||
bash scripts/show_site_config.sh # site.yaml 解析结果
|
bash scripts/show_site_config.sh # site.yaml 解析结果
|
||||||
|
python3 scripts/check_arm_prefixes.py # 含臂名的配置是否跟着当前臂(换臂必跑)
|
||||||
|
|
||||||
# —— 仿真验收(★ 必做,别靠肉眼)——
|
# —— 仿真验收(★ 必做,别靠肉眼)——
|
||||||
python3 scripts/check_sim_cameras.py # 深度反投影命中已知平面才算过
|
python3 scripts/check_sim_cameras.py # 深度反投影命中已知平面才算过
|
||||||
@@ -376,6 +407,8 @@ python3 -m pytest src/arm_sim_gazebo/test -q
|
|||||||
| 校验代码永远通过 | `sdformat` 用 `__default__` / `__default_topic__` 假值填了缺失字段 | 仿真手册坑 9 |
|
| 校验代码永远通过 | `sdformat` 用 `__default__` / `__default_topic__` 假值填了缺失字段 | 仿真手册坑 9 |
|
||||||
| 脚本报 `$'\r': 未找到命令` | 脚本是 CRLF 行尾 | R6.6 |
|
| 脚本报 `$'\r': 未找到命令` | 脚本是 CRLF 行尾 | R6.6 |
|
||||||
| 改了参数不生效 | 改的是包内 yaml,被 `site.yaml` 覆盖了 | 3.2 |
|
| 改了参数不生效 | 改的是包内 yaml,被 `site.yaml` 覆盖了 | 3.2 |
|
||||||
|
| MoveIt 说规划成功、机械臂不动 | SRDF / controllers 里还是**旧臂**的关节名 | 3.6.1、R1.1.1 |
|
||||||
|
| 控制器报 `Joint 'Rizon4-xxxxxx_joint1' not found` | 换了臂但一级配置没跟着改(或改了两处漏一处) | 3.6.1 |
|
||||||
| 仿真镜头位置不对、只看得见地面 | 相机 `rpy` 复合顺序 | 仿真手册坑 5 |
|
| 仿真镜头位置不对、只看得见地面 | 相机 `rpy` 复合顺序 | 仿真手册坑 5 |
|
||||||
| 脚本说"已停止",进程还在 | 进程模式表漏项 | R4.10 |
|
| 脚本说"已停止",进程还在 | 进程模式表漏项 | R4.10 |
|
||||||
| 孔检测在仿真里怎么都不过 | 图幅与真机不同,像素门槛失效 | 仿真手册 §4.3 |
|
| 孔检测在仿真里怎么都不过 | 图幅与真机不同,像素门槛失效 | 仿真手册 §4.3 |
|
||||||
|
|||||||
@@ -0,0 +1,214 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""检查「静态配置里的关节前缀」是否与 site.yaml 当前选中的臂一致。
|
||||||
|
|
||||||
|
为什么需要这个脚本
|
||||||
|
------------------
|
||||||
|
本框架里有两类文件:
|
||||||
|
|
||||||
|
**一类跟着 site.yaml 走(本脚本负责检查)**
|
||||||
|
``src/arm_control/config/controllers.yaml``、``srdf/arm.srdf``、
|
||||||
|
``moveit_controllers.yaml``、``bringup.yaml``、``joint_names.yaml``、
|
||||||
|
``servo.yaml``、``xunhuan_trajectory.yaml`` ……
|
||||||
|
|
||||||
|
这些文件里写的是**具体臂的关节名**(``Rizon4-063989_joint1``)。Flexiv 官方
|
||||||
|
的关节命名把 SN 编在名字里,所以换一台臂,这些文件必须跟着改。
|
||||||
|
|
||||||
|
**一类按臂各留一份(本脚本只提示、不报错)**
|
||||||
|
``config/site.yaml``、``named_poses_89.yaml`` / ``named_poses_91.yaml``、
|
||||||
|
``dual_grippers.yaml``(两条臂的夹爪都在里面)、``handeye/waypoints.yaml``。
|
||||||
|
|
||||||
|
这些是**现场资产**,本来就该保存多台臂各自的标定/示教结果。
|
||||||
|
|
||||||
|
为什么必须显式检查
|
||||||
|
------------------
|
||||||
|
换臂后如果只改了 ``site.yaml``、忘了改上面第一类文件,症状是:
|
||||||
|
|
||||||
|
* 控制器起不来,报 ``Joint 'Rizon4-063989_joint1' not found``;
|
||||||
|
* 或者更糟——URDF 里关节名变了、SRDF 里没变,MoveIt 规划出的轨迹
|
||||||
|
全部作用在**不存在的关节**上,表现为"规划成功但机械臂不动"。
|
||||||
|
|
||||||
|
这类失败在真机上排查代价很高,所以做成一条命令:**改完 site.yaml 先跑它**。
|
||||||
|
|
||||||
|
用法
|
||||||
|
----
|
||||||
|
python3 scripts/check_arm_prefixes.py # 检查
|
||||||
|
python3 scripts/check_arm_prefixes.py --list # 只列出当前臂的应然前缀
|
||||||
|
|
||||||
|
退出码:0 = 一致;1 = 有文件对不上(会给出逐条 file:line)。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import re
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
_HERE = Path(__file__).resolve().parent
|
||||||
|
_REPO = _HERE.parent
|
||||||
|
sys.path.insert(0, str(_HERE / "lib"))
|
||||||
|
|
||||||
|
from export_site_env import flatten, load_site # noqa: E402 (本仓库自己的加载器)
|
||||||
|
|
||||||
|
# 必须跟着当前臂走的文件(相对仓库根)。
|
||||||
|
REQUIRED_FILES: tuple[str, ...] = (
|
||||||
|
"src/arm_control/config/controllers.yaml",
|
||||||
|
"src/arm_control/config/named_poses.yaml",
|
||||||
|
"src/arm_moveit_config/config/moveit_controllers.yaml",
|
||||||
|
"src/arm_moveit_config/srdf/arm.srdf",
|
||||||
|
"src/arm_bringup/config/bringup.yaml",
|
||||||
|
"src/arm_description/config/joint_names.yaml",
|
||||||
|
"src/arm_application/config/servo.yaml",
|
||||||
|
"src/arm_application/config/xunhuan_trajectory.yaml",
|
||||||
|
)
|
||||||
|
|
||||||
|
# 按臂各留一份的现场资产:含别的臂的 SN 是对的,不报错。
|
||||||
|
PER_ARM_FILES: tuple[str, ...] = (
|
||||||
|
"config/site.yaml",
|
||||||
|
"config/site.example.yaml",
|
||||||
|
"src/arm_control/config/named_poses_89.yaml",
|
||||||
|
"src/arm_control/config/named_poses_91.yaml",
|
||||||
|
"src/arm_application/config/dual_grippers.yaml",
|
||||||
|
"handeye/waypoints.yaml",
|
||||||
|
)
|
||||||
|
|
||||||
|
# Flexiv 关节名里的 SN 形态:Rizon4-063989 / Rizon4s-123456 …
|
||||||
|
#
|
||||||
|
# 注意别用 ``\b`` 收尾:关节名是 ``Rizon4-063989_joint1``,下划线属于 \w,
|
||||||
|
# 写成 ``\b`` 会**一个都匹配不到**(本脚本第一版就踩了这个坑,
|
||||||
|
# 结果 SRDF 里 24 处硬编码全部漏报)。
|
||||||
|
_SN_TOKEN = re.compile(r"Rizon\d+[A-Za-z]*-\d{6}(?!\d)")
|
||||||
|
|
||||||
|
|
||||||
|
def _active_prefix() -> str:
|
||||||
|
"""当前臂的关节前缀(``Rizon4-063989_``)。取不到就报错退出。
|
||||||
|
|
||||||
|
这里刻意**不兜底**成任何具体前缀:读不出 site.yaml 时,
|
||||||
|
"检查通过"是假象,宁可让调用方看到失败(R1.1)。
|
||||||
|
|
||||||
|
三种失败都必须变成"人话 + 退出码 2",不能让 Python traceback 直接糊到脸上
|
||||||
|
—— 这个脚本是换臂流程里的第 3 步,用户往往正处在"配置改到一半"的状态:
|
||||||
|
|
||||||
|
* ``site.yaml`` 不存在(``load_site`` 抛 ``FileNotFoundError``);
|
||||||
|
* ``robot.sn`` 为空、或 ``robots[]`` 里查不到(``flatten`` 抛 ``ValueError``)。
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
data = load_site()
|
||||||
|
except FileNotFoundError as exc:
|
||||||
|
print(
|
||||||
|
f"读不到 site 配置:{exc}\n"
|
||||||
|
"没有当前臂就无从检查。先确认 config/site.yaml 存在,"
|
||||||
|
"或让 SITE_CONFIG 指向它。",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
raise SystemExit(2) from exc
|
||||||
|
|
||||||
|
try:
|
||||||
|
flat = flatten(data)
|
||||||
|
except ValueError as exc:
|
||||||
|
# 典型就是"robot.sn 没填 / 别名对不上"——把原始信息原样带出来,
|
||||||
|
# 它已经写明了现有哪几台臂可选。
|
||||||
|
print(
|
||||||
|
f"解析 site 配置失败,取不到当前臂:\n{exc}",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
raise SystemExit(2) from exc
|
||||||
|
|
||||||
|
prefix = str(flat.get("SITE_JOINT_PREFIX", "") or "").strip()
|
||||||
|
if not prefix or prefix == "_":
|
||||||
|
print(
|
||||||
|
"site.yaml 里没解析出关节前缀(SITE_JOINT_PREFIX 为空)。\n"
|
||||||
|
"请确认 robot.robots[].sn 已填,且该臂就是 robot.sn 选中的那一台。",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
raise SystemExit(2)
|
||||||
|
return prefix
|
||||||
|
|
||||||
|
|
||||||
|
def _scan(path: Path, want_sn: str) -> list[str]:
|
||||||
|
"""返回该文件里所有"不属于 want_sn"的 SN 出现处,格式 ``行号: 内容``。"""
|
||||||
|
hits: list[str] = []
|
||||||
|
try:
|
||||||
|
text = path.read_text(encoding="utf-8")
|
||||||
|
except OSError as exc: # 文件缺失也值得报出来
|
||||||
|
return [f"读不到文件:{exc}"]
|
||||||
|
for lineno, line in enumerate(text.splitlines(), start=1):
|
||||||
|
for token in _SN_TOKEN.findall(line):
|
||||||
|
if token != want_sn:
|
||||||
|
hits.append(f"{lineno}: {line.strip()}")
|
||||||
|
break
|
||||||
|
return hits
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
parser = argparse.ArgumentParser(description=__doc__.splitlines()[0])
|
||||||
|
parser.add_argument(
|
||||||
|
"--list",
|
||||||
|
action="store_true",
|
||||||
|
help="只打印当前臂的应然前缀与 SN,不做检查",
|
||||||
|
)
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
prefix = _active_prefix()
|
||||||
|
want_sn = prefix.rstrip("_")
|
||||||
|
|
||||||
|
if args.list:
|
||||||
|
print(f"当前臂 SN : {want_sn}")
|
||||||
|
print(f"关节前缀 : {prefix}")
|
||||||
|
# 说明这个 SN 是哪来的:环境变量 ROBOT_SN 会**覆盖** site.yaml 的
|
||||||
|
# robot.sn(为了让"当前终端选哪台臂"能临时换),排查时容易看错。
|
||||||
|
import os
|
||||||
|
|
||||||
|
env_sn = os.environ.get("ROBOT_SN", "").strip()
|
||||||
|
if env_sn:
|
||||||
|
print(f"来源 : 环境变量 ROBOT_SN={env_sn}(覆盖 site.yaml 的 robot.sn)")
|
||||||
|
else:
|
||||||
|
print("来源 : config/site.yaml 的 robot.sn")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
problems: dict[str, list[str]] = {}
|
||||||
|
for rel in REQUIRED_FILES:
|
||||||
|
path = _REPO / rel
|
||||||
|
hits = _scan(path, want_sn)
|
||||||
|
if hits:
|
||||||
|
problems[rel] = hits
|
||||||
|
|
||||||
|
if not problems:
|
||||||
|
print(f"OK:{len(REQUIRED_FILES)} 个跟随臂走的一级配置文件都指向 {want_sn}。")
|
||||||
|
return 0
|
||||||
|
|
||||||
|
print(
|
||||||
|
f"发现 {len(problems)} 个配置文件与当前臂 {want_sn} 不一致:\n",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
for rel, hits in problems.items():
|
||||||
|
print(f" {rel}", file=sys.stderr)
|
||||||
|
for hit in hits[:8]:
|
||||||
|
print(f" {hit}", file=sys.stderr)
|
||||||
|
if len(hits) > 8:
|
||||||
|
print(f" …… 另有 {len(hits) - 8} 处", file=sys.stderr)
|
||||||
|
print("", file=sys.stderr)
|
||||||
|
|
||||||
|
old_sns = sorted({tok for rel in problems for hit in problems[rel]
|
||||||
|
for tok in _SN_TOKEN.findall(hit)})
|
||||||
|
print("修法(把旧 SN 文本替换成当前 SN,再重新 build):", file=sys.stderr)
|
||||||
|
for old in old_sns:
|
||||||
|
if old == want_sn:
|
||||||
|
continue
|
||||||
|
print(f" # {old} → {want_sn}", file=sys.stderr)
|
||||||
|
print(
|
||||||
|
" grep -rl " + repr(old) + " " + " ".join(REQUIRED_FILES)
|
||||||
|
+ f" | xargs sed -i 's/{old}/{want_sn}/g'",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"\n注意:SRDF / controllers 里的关节名是**结构**,替换后请跑\n"
|
||||||
|
" python3 scripts/check_arm_prefixes.py # 应输出 OK\n"
|
||||||
|
" bash scripts/build.sh # 重新 build 让 install 同步",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
return 1
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -7,7 +7,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
|||||||
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
||||||
|
|
||||||
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
||||||
ROBOT_SN="${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}"
|
ROBOT_SN="${ROBOT_SN:-${SITE_ROBOT_SN:-}}"
|
||||||
PC_IP="${PC_IP:-${SITE_PC_IP:-192.168.100.10}}"
|
PC_IP="${PC_IP:-${SITE_PC_IP:-192.168.100.10}}"
|
||||||
|
|
||||||
pick_iface() {
|
pick_iface() {
|
||||||
|
|||||||
@@ -5,6 +5,7 @@ from __future__ import annotations
|
|||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
import math
|
import math
|
||||||
|
import os
|
||||||
import re
|
import re
|
||||||
import sys
|
import sys
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
@@ -53,19 +54,15 @@ def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None:
|
|||||||
def to_yaml(
|
def to_yaml(
|
||||||
waypoints: list[dict[str, Any]],
|
waypoints: list[dict[str, Any]],
|
||||||
*,
|
*,
|
||||||
joint_prefix: str = 'Rizon4-063989_',
|
joint_prefix: str = '',
|
||||||
velocity_scale: float | None = None,
|
velocity_scale: float | None = None,
|
||||||
) -> str:
|
) -> str:
|
||||||
if velocity_scale is None:
|
if velocity_scale is None:
|
||||||
velocity_scale = waypoints[0].get('velocity', 0.2) if waypoints else 0.2
|
velocity_scale = waypoints[0].get('velocity', 0.2) if waypoints else 0.2
|
||||||
times = [wp['time'] for wp in waypoints]
|
times = [wp['time'] for wp in waypoints]
|
||||||
flat: list[float] = []
|
# 时间轴拉平成一维;位置在下面的循环里**逐段**写,顺便把该段名字放进行尾注释。
|
||||||
for wp in waypoints:
|
# (这里以前还先算了 pos_line / names 两个字符串再全部丢弃 —— 死代码,已删。)
|
||||||
flat.extend(wp['positions'])
|
|
||||||
pos_line = ', '.join(f'{v:.6f}' for v in flat)
|
|
||||||
time_line = ', '.join(str(t) for t in times)
|
time_line = ', '.join(str(t) for t in times)
|
||||||
names = ', '.join(f' # {wp.get("name", "")}' if i == 0 else '' for i, wp in enumerate(waypoints))
|
|
||||||
_ = names # 名称保留在下面的注释里
|
|
||||||
lines = [
|
lines = [
|
||||||
'# 由 Flexiv Elements .traj 自动转换而来(关节角:度 -> 弧度)',
|
'# 由 Flexiv Elements .traj 自动转换而来(关节角:度 -> 弧度)',
|
||||||
'# ROS 参数:扁平的 waypoint_positions + waypoint_times(不使用嵌套字典)',
|
'# ROS 参数:扁平的 waypoint_positions + waypoint_times(不使用嵌套字典)',
|
||||||
@@ -91,7 +88,11 @@ def main() -> int:
|
|||||||
ap = argparse.ArgumentParser(description=__doc__)
|
ap = argparse.ArgumentParser(description=__doc__)
|
||||||
ap.add_argument('input', type=Path, help='Flexiv Elements 的 .traj 文件')
|
ap.add_argument('input', type=Path, help='Flexiv Elements 的 .traj 文件')
|
||||||
ap.add_argument('-o', '--output', type=Path, help='输出的 YAML 路径')
|
ap.add_argument('-o', '--output', type=Path, help='输出的 YAML 路径')
|
||||||
ap.add_argument('--prefix', default='Rizon4-063989_', help='关节名前缀(现场 SN + _)')
|
ap.add_argument(
|
||||||
|
'--prefix',
|
||||||
|
default=os.environ.get('SITE_JOINT_PREFIX', ''),
|
||||||
|
help='关节名前缀(现场 SN + _);默认取 site.yaml 里该臂的 robot.sn',
|
||||||
|
)
|
||||||
ap.add_argument('--min-seg', type=float, default=3.0, help='每段的最小秒数')
|
ap.add_argument('--min-seg', type=float, default=3.0, help='每段的最小秒数')
|
||||||
args = ap.parse_args()
|
args = ap.parse_args()
|
||||||
|
|
||||||
|
|||||||
@@ -108,9 +108,9 @@ Done. Next steps (Ubuntu 22.04 / Humble + Rizon):
|
|||||||
4) Real robot (site SN, no gripper yet):
|
4) Real robot (site SN, no gripper yet):
|
||||||
ros2 launch arm_bringup real.launch.py
|
ros2 launch arm_bringup real.launch.py
|
||||||
# or explicitly:
|
# or explicitly:
|
||||||
ros2 launch arm_bringup real.launch.py robot_sn:=Rizon4-063989 load_gripper:=false
|
ros2 launch arm_bringup real.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> load_gripper:=false
|
||||||
|
|
||||||
Official fake hardware:
|
Official fake hardware:
|
||||||
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=Rizon4-063989 use_fake_hardware:=true load_gripper:=false
|
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false
|
||||||
|
|
||||||
EOF
|
EOF
|
||||||
|
|||||||
+2
-2
@@ -108,7 +108,7 @@ else:
|
|||||||
m = dict(zip(box[0].name, box[0].position))
|
m = dict(zip(box[0].name, box[0].position))
|
||||||
v = dict(zip(box[0].name, box[0].velocity)) if box[0].velocity else {}
|
v = dict(zip(box[0].name, box[0].velocity)) if box[0].velocity else {}
|
||||||
for i in range(1, 8):
|
for i in range(1, 8):
|
||||||
k = f"{__import__('os').environ.get('SITE_JOINT_PREFIX','Rizon4-063989_')}joint{i}"
|
k = f"{__import__('os').environ['SITE_JOINT_PREFIX']}joint{i}"
|
||||||
if k in m:
|
if k in m:
|
||||||
print(f" joint{i}: {m[k]:+.4f} qd={v.get(k, 0.0):+.4f}")
|
print(f" joint{i}: {m[k]:+.4f} qd={v.get(k, 0.0):+.4f}")
|
||||||
n.destroy_node(); rclpy.shutdown()
|
n.destroy_node(); rclpy.shutdown()
|
||||||
@@ -138,7 +138,7 @@ target = POSES.get(name)
|
|||||||
if target is None:
|
if target is None:
|
||||||
raise SystemExit(f"unknown pose {name}")
|
raise SystemExit(f"unknown pose {name}")
|
||||||
import os
|
import os
|
||||||
PREFIX = os.environ.get("SITE_JOINT_PREFIX", "Rizon4-063989_")
|
PREFIX = os.environ["SITE_JOINT_PREFIX"] # 缺失就报错:前缀错了会驱动错关节
|
||||||
JOINTS = [f"{PREFIX}joint{i}" for i in range(1, 8)]
|
JOINTS = [f"{PREFIX}joint{i}" for i in range(1, 8)]
|
||||||
|
|
||||||
rclpy.init()
|
rclpy.init()
|
||||||
|
|||||||
+190
-62
@@ -62,23 +62,121 @@ def list_robots(data: dict) -> list[dict]:
|
|||||||
return out
|
return out
|
||||||
|
|
||||||
|
|
||||||
# --arm 89|91|left|right 与 SN 别名。禁止把 89 的手眼/相机套到 91。
|
# 侧别词 —— 这是**语言别名**,不是站点数据:任何站点都希望
|
||||||
_ARM_ALIASES = {
|
# `--arm left` / `--arm 左` / `--arm 左臂` 能选中 `robots[]` 里 `arm: left` 的那台。
|
||||||
"89": "Rizon4-063989",
|
#
|
||||||
"left": "Rizon4-063989",
|
# 这里**绝不允许**出现具体 SN(`Rizon4-063989` 之类)。历史上这里是一张
|
||||||
"左": "Rizon4-063989",
|
# `89 -> Rizon4-063989` 的写死映射表:换一台机器人后 `--arm 89` 仍然会选中
|
||||||
"左臂": "Rizon4-063989",
|
# 上一台,而且不报错(R1.1)。现在 `89` 这个写法的来源是
|
||||||
"左臂89": "Rizon4-063989",
|
# **这台机器 SN 的末两位**,由 `_arm_index()` 从 `robots[]` 现算出来。
|
||||||
"91": "Rizon4-063991",
|
_SIDE_WORDS = {
|
||||||
"right": "Rizon4-063991",
|
"left": "left",
|
||||||
"右": "Rizon4-063991",
|
"l": "left",
|
||||||
"右臂": "Rizon4-063991",
|
"左": "left",
|
||||||
"右臂91": "Rizon4-063991",
|
"左臂": "left",
|
||||||
|
"right": "right",
|
||||||
|
"r": "right",
|
||||||
|
"右": "right",
|
||||||
|
"右臂": "right",
|
||||||
}
|
}
|
||||||
|
|
||||||
_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")
|
_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")
|
||||||
|
|
||||||
|
|
||||||
|
def arm_short_id(sn: str) -> str:
|
||||||
|
"""Flexiv 序列号的"臂号"= 末两位数字(``Rizon4-063989`` → ``89``)。
|
||||||
|
|
||||||
|
手眼文件名、结果文件都用这个编号。它是从 SN 派生出来的,不是常量。
|
||||||
|
"""
|
||||||
|
tail = str(sn or "").strip().rsplit("-", 1)[-1]
|
||||||
|
return tail[-2:] if tail.isdigit() and len(tail) >= 2 else ""
|
||||||
|
|
||||||
|
|
||||||
|
def _arm_index(data: dict) -> dict[str, str]:
|
||||||
|
"""从 ``robots[]`` 派生 ``别名 -> SN`` 索引。
|
||||||
|
|
||||||
|
别名包括:完整 SN、``label``(如 ``左臂89``)、侧别词(``left``/``左``)、
|
||||||
|
SN 末两位(``89``)。全部来自 site.yaml —— 这是"换一台机器人只改配置"的前提。
|
||||||
|
"""
|
||||||
|
index: dict[str, str] = {}
|
||||||
|
for item in list_robots(data):
|
||||||
|
sn = str(item.get("sn") or "").strip()
|
||||||
|
if not sn:
|
||||||
|
continue
|
||||||
|
names = {sn.lower()}
|
||||||
|
label = str(item.get("label") or "").strip()
|
||||||
|
if label:
|
||||||
|
names.add(label.lower())
|
||||||
|
arm = str(item.get("arm") or "").strip().lower()
|
||||||
|
if arm:
|
||||||
|
names.add(arm)
|
||||||
|
names.update(word for word, side in _SIDE_WORDS.items() if side == arm)
|
||||||
|
short = arm_short_id(sn)
|
||||||
|
if short:
|
||||||
|
names.add(short)
|
||||||
|
if arm:
|
||||||
|
names.add(f"{arm}{short}")
|
||||||
|
for name in names:
|
||||||
|
if name:
|
||||||
|
index.setdefault(name, sn)
|
||||||
|
return index
|
||||||
|
|
||||||
|
|
||||||
|
def _looks_like_full_sn(token: str) -> bool:
|
||||||
|
"""区分"完整 SN"与"臂别名"。
|
||||||
|
|
||||||
|
``Rizon4-063989`` 这种带连字符的写法允许不在 ``robots[]`` 里
|
||||||
|
(单臂站点可以只写 ``robot.sn`` 不登记清单);而 ``89`` / ``left`` 这类
|
||||||
|
**别名**查不到就必须报错,不能当成 SN 硬用。
|
||||||
|
"""
|
||||||
|
return "-" in token
|
||||||
|
|
||||||
|
|
||||||
|
def normalize_robot_sn(data: dict, want: str) -> str:
|
||||||
|
"""把 89 / left / 左臂 / label / 完整 SN 归一成完整 SN(查不到则原样返回)。"""
|
||||||
|
return resolve_robot_sn(data, want)[0]
|
||||||
|
|
||||||
|
|
||||||
|
def resolve_robot_sn(data: dict, want: str) -> tuple[str, bool]:
|
||||||
|
"""归一化并报告是否命中。
|
||||||
|
|
||||||
|
返回 ``(sn, matched)``。``matched=False`` 表示这个写法在 ``robots[]`` 里
|
||||||
|
查不到 —— 调用方必须**报错**(见 ``resolve_robot``),不能静默拿它当 SN,
|
||||||
|
否则会去连一台不存在的机器人。
|
||||||
|
"""
|
||||||
|
w = (want or "").strip()
|
||||||
|
if not w:
|
||||||
|
return "", False
|
||||||
|
index = _arm_index(data)
|
||||||
|
hit = index.get(w.lower())
|
||||||
|
if hit:
|
||||||
|
return hit, True
|
||||||
|
if _looks_like_full_sn(w):
|
||||||
|
return w, True
|
||||||
|
return w, False
|
||||||
|
|
||||||
|
|
||||||
|
def describe_arms(data: dict) -> str:
|
||||||
|
"""给报错信息用:列出本配置里可选的全部机械臂。"""
|
||||||
|
lines = []
|
||||||
|
for item in list_robots(data):
|
||||||
|
sn = str(item.get("sn") or "").strip()
|
||||||
|
if not sn:
|
||||||
|
continue
|
||||||
|
bits = [f" - {sn}"]
|
||||||
|
arm = str(item.get("arm") or "").strip()
|
||||||
|
label = str(item.get("label") or "").strip()
|
||||||
|
if arm or label:
|
||||||
|
bits.append(f"arm={arm or '?'}")
|
||||||
|
if label:
|
||||||
|
bits.append(f"label={label}")
|
||||||
|
short = arm_short_id(sn)
|
||||||
|
if short:
|
||||||
|
bits.append(f"--arm {short}")
|
||||||
|
lines.append(" ".join(bits))
|
||||||
|
return "\n".join(lines) or " (robot.robots[] 是空的)"
|
||||||
|
|
||||||
|
|
||||||
def _deep_merge(base: dict, overlay: dict) -> dict:
|
def _deep_merge(base: dict, overlay: dict) -> dict:
|
||||||
out = dict(base)
|
out = dict(base)
|
||||||
for k, v in overlay.items():
|
for k, v in overlay.items():
|
||||||
@@ -89,32 +187,38 @@ def _deep_merge(base: dict, overlay: dict) -> dict:
|
|||||||
return out
|
return out
|
||||||
|
|
||||||
|
|
||||||
def normalize_robot_sn(data: dict, want: str) -> str:
|
|
||||||
"""把 89/91/left/right/label 映射成完整的 Flexiv SN。"""
|
|
||||||
w = (want or "").strip()
|
|
||||||
if not w:
|
|
||||||
return w
|
|
||||||
mapped = _ARM_ALIASES.get(w) or _ARM_ALIASES.get(w.lower())
|
|
||||||
if mapped:
|
|
||||||
return mapped
|
|
||||||
for item in list_robots(data):
|
|
||||||
sn = str(item.get("sn") or "").strip()
|
|
||||||
label = str(item.get("label") or "").strip()
|
|
||||||
arm = str(item.get("arm") or "").strip().lower()
|
|
||||||
if w == sn or w == label or w.lower() == arm:
|
|
||||||
return sn
|
|
||||||
if sn.endswith(w) and w.isdigit() and len(w) >= 2:
|
|
||||||
return sn
|
|
||||||
return w
|
|
||||||
|
|
||||||
|
|
||||||
def resolve_robot(data: dict, sn: str | None = None) -> dict:
|
def resolve_robot(data: dict, sn: str | None = None) -> dict:
|
||||||
"""当前生效的机械臂:环境变量 ``ROBOT_SN`` 覆盖 robot.sn;清单补齐 ip/iface/frames。"""
|
"""当前生效的机械臂:环境变量 ``ROBOT_SN`` 覆盖 robot.sn;清单补齐 ip/iface/frames。
|
||||||
|
|
||||||
|
选不出来时**抛 ``ValueError``**,绝不猜:
|
||||||
|
|
||||||
|
- 给了别名(``89`` / ``left``)但 ``robots[]`` 里没有对应的臂;
|
||||||
|
- ``robot.sn`` 没写、也没给 ``--arm``。
|
||||||
|
|
||||||
|
以前这两种情况会静默落回一个写死的 SN 或一个空的 ``sn``,结果就是
|
||||||
|
"配置漏了一键,程序照样启动,连的是哪台看运气"。
|
||||||
|
"""
|
||||||
robot = dict(data.get("robot") or {})
|
robot = dict(data.get("robot") or {})
|
||||||
catalog = list_robots(data)
|
catalog = list_robots(data)
|
||||||
want = normalize_robot_sn(
|
raw = (
|
||||||
data,
|
sn
|
||||||
sn or os.environ.get("ROBOT_SN") or os.environ.get("ROBOT_ARM") or robot.get("sn") or "",
|
or os.environ.get("ROBOT_SN")
|
||||||
|
or os.environ.get("ROBOT_ARM")
|
||||||
|
or robot.get("sn")
|
||||||
|
or ""
|
||||||
|
)
|
||||||
|
want, matched = resolve_robot_sn(data, raw)
|
||||||
|
if str(raw).strip() and not matched:
|
||||||
|
raise ValueError(
|
||||||
|
f"选不出机械臂:{str(raw).strip()!r} 既不是 robot.robots[] 里的别名,"
|
||||||
|
f"也不像完整 SN(完整 SN 形如 Rizon4-063989)。\n"
|
||||||
|
f"config/site.yaml 里现有:\n{describe_arms(data)}\n"
|
||||||
|
f"用法:所有终端加同一个 --arm,或直接写完整 SN。"
|
||||||
|
)
|
||||||
|
if not want:
|
||||||
|
raise ValueError(
|
||||||
|
"config/site.yaml 没有指定机械臂:robot.sn 为空,也没给 --arm。\n"
|
||||||
|
f"可选:\n{describe_arms(data)}"
|
||||||
)
|
)
|
||||||
match: dict = {}
|
match: dict = {}
|
||||||
for item in catalog:
|
for item in catalog:
|
||||||
@@ -124,16 +228,13 @@ def resolve_robot(data: dict, sn: str | None = None) -> dict:
|
|||||||
out = {k: v for k, v in robot.items() if k != "robots"}
|
out = {k: v for k, v in robot.items() if k != "robots"}
|
||||||
out.update({k: v for k, v in match.items() if k != "robots"})
|
out.update({k: v for k, v in match.items() if k != "robots"})
|
||||||
out["sn"] = want
|
out["sn"] = want
|
||||||
if not str(out.get("arm") or "").strip():
|
# 侧别只认 robots[].arm。**不要**从 SN 里猜数字("063989 就是左臂")——
|
||||||
if "063989" in want:
|
# 那是站点数据写进了代码,换个站点就会把左右搞反。
|
||||||
out["arm"] = "left"
|
if not str(out.get("joint_prefix") or "").strip():
|
||||||
elif "063991" in want:
|
|
||||||
out["arm"] = "right"
|
|
||||||
if not str(out.get("joint_prefix") or "").strip() and want:
|
|
||||||
out["joint_prefix"] = f"{want}_"
|
out["joint_prefix"] = f"{want}_"
|
||||||
if not str(out.get("base_frame") or "").strip() and want:
|
if not str(out.get("base_frame") or "").strip():
|
||||||
out["base_frame"] = f"{want}_base_link"
|
out["base_frame"] = f"{want}_base_link"
|
||||||
if not str(out.get("ee_frame") or "").strip() and want:
|
if not str(out.get("ee_frame") or "").strip():
|
||||||
out["ee_frame"] = f"{want}_flange"
|
out["ee_frame"] = f"{want}_flange"
|
||||||
net = data.get("network") or {}
|
net = data.get("network") or {}
|
||||||
if not str(out.get("iface") or "").strip():
|
if not str(out.get("iface") or "").strip():
|
||||||
@@ -200,10 +301,19 @@ def flatten(data: dict) -> dict:
|
|||||||
except Exception:
|
except Exception:
|
||||||
weights_resolved = weights
|
weights_resolved = weights
|
||||||
|
|
||||||
|
# 派生文件名的口径:臂号 = 当前 SN 末两位(见 arm_short_id)。
|
||||||
|
# 用当前臂现算,**不要**钉死 "89" —— 否则换成 91 或另一台臂时,
|
||||||
|
# 两边会共用同一个手眼文件,表现是"标定做了但不生效"。
|
||||||
|
short = arm_short_id(str(robot.get("sn") or ""))
|
||||||
|
he_mode = str(he.get("mode") or "eye_in_hand")
|
||||||
|
he_stem = f"{short}_{he_mode}" if short else he_mode
|
||||||
|
|
||||||
out = {
|
out = {
|
||||||
"SITE_CONFIG": str(site_path()),
|
"SITE_CONFIG": str(site_path()),
|
||||||
"SITE_ROS2_ROOT": str(_root()),
|
"SITE_ROS2_ROOT": str(_root()),
|
||||||
"SITE_ROBOT_SN": str(robot.get("sn") or "Rizon4-063989"),
|
# 必须来自配置。缺了就留空让调用方报错 —— 兜底成某个真实 SN
|
||||||
|
# 等于"配置漏了照样启动,连的是哪台看运气"。
|
||||||
|
"SITE_ROBOT_SN": str(robot.get("sn") or ""),
|
||||||
"SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
|
"SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
|
||||||
"SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
|
"SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
|
||||||
"SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
|
"SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
|
||||||
@@ -212,6 +322,10 @@ def flatten(data: dict) -> dict:
|
|||||||
"SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
|
"SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
|
||||||
"SITE_ROBOT_LABEL": str(robot.get("label") or ""),
|
"SITE_ROBOT_LABEL": str(robot.get("label") or ""),
|
||||||
"SITE_ROBOT_ARM": str(robot.get("arm") or ""),
|
"SITE_ROBOT_ARM": str(robot.get("arm") or ""),
|
||||||
|
"SITE_ROBOT_SHORT_ID": short,
|
||||||
|
# 本臂有没有装 Orisys 触觉。以前脚本靠 `SN == Rizon4-063989` 来判断,
|
||||||
|
# 于是"哪台臂有传感器"变成了代码里的常量。现在由 site.yaml 说了算。
|
||||||
|
"SITE_ROBOT_HAS_TACTILE": "1" if robot.get("tactile") else "0",
|
||||||
"SITE_NAMED_POSES_FILE": named_poses,
|
"SITE_NAMED_POSES_FILE": named_poses,
|
||||||
"SITE_ROBOT_CATALOG": ",".join(
|
"SITE_ROBOT_CATALOG": ",".join(
|
||||||
str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
|
str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
|
||||||
@@ -220,7 +334,7 @@ def flatten(data: dict) -> dict:
|
|||||||
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
|
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
|
||||||
"SITE_HANDEYE_WAYPOINTS": str(
|
"SITE_HANDEYE_WAYPOINTS": str(
|
||||||
paths.get("handeye_waypoints")
|
paths.get("handeye_waypoints")
|
||||||
or str(_root() / "handeye" / "poses" / "89_eye_in_hand.yaml")
|
or str(_root() / "handeye" / "poses" / f"{he_stem}.yaml")
|
||||||
),
|
),
|
||||||
"SITE_HANDEYE_CALIBRATE": str(
|
"SITE_HANDEYE_CALIBRATE": str(
|
||||||
paths.get("handeye_calibrate")
|
paths.get("handeye_calibrate")
|
||||||
@@ -231,13 +345,14 @@ def flatten(data: dict) -> dict:
|
|||||||
or str(_root() / "handeye" / "waypoints.yaml")
|
or str(_root() / "handeye" / "waypoints.yaml")
|
||||||
),
|
),
|
||||||
"SITE_HANDEYE_WORKDIR": str(
|
"SITE_HANDEYE_WORKDIR": str(
|
||||||
paths.get("handeye_workdir") or str(_root() / "handeye" / "tmp")
|
paths.get("handeye_workdir")
|
||||||
|
or str(_root() / "handeye" / "tmp" / short)
|
||||||
),
|
),
|
||||||
"SITE_HANDEYE_FILE": str(
|
"SITE_HANDEYE_FILE": str(
|
||||||
paths.get("handeye_file")
|
paths.get("handeye_file")
|
||||||
or str(_root() / "handeye" / "results" / "89_eye_in_hand.yaml")
|
or str(_root() / "handeye" / "results" / f"{he_stem}.yaml")
|
||||||
),
|
),
|
||||||
"SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"),
|
"SITE_HANDEYE_MODE": he_mode,
|
||||||
# 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
|
# 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
|
||||||
# 这里原先是 11/16/0.011(一张早期用过、现已不存在的板),与 site.yaml 的
|
# 这里原先是 11/16/0.011(一张早期用过、现已不存在的板),与 site.yaml 的
|
||||||
# 8/11/0.015 冲突:一旦 site.yaml 缺这几个键就会静默用错尺寸,
|
# 8/11/0.015 冲突:一旦 site.yaml 缺这几个键就会静默用错尺寸,
|
||||||
@@ -278,8 +393,10 @@ def flatten(data: dict) -> dict:
|
|||||||
"SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
|
"SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
|
||||||
"SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
|
"SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
|
||||||
"SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
|
"SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
|
||||||
"SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or "352122274408"),
|
# 序列号**不给默认值**:兜底成某个真实序列号 = 换站点后悄悄打开别人的相机。
|
||||||
"SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or "260522274442"),
|
# 缺了就让相机节点以"序列号为空"报错,比连错设备好得多(M1)。
|
||||||
|
"SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or ""),
|
||||||
|
"SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or ""),
|
||||||
"SITE_YOLO_CONF": str(yolo.get("conf") or 0.10),
|
"SITE_YOLO_CONF": str(yolo.get("conf") or 0.10),
|
||||||
"SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 640)),
|
"SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 640)),
|
||||||
"SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
|
"SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
|
||||||
@@ -369,14 +486,10 @@ def perception_overrides(data: dict) -> dict:
|
|||||||
cam_ns, cam_frame, depth_topic = camera_frames(data)
|
cam_ns, cam_frame, depth_topic = camera_frames(data)
|
||||||
obj = str(yolo.get("object_frame") or "").strip()
|
obj = str(yolo.get("object_frame") or "").strip()
|
||||||
if not obj or obj == "tracked_object":
|
if not obj or obj == "tracked_object":
|
||||||
arm = str((_robot or {}).get("arm") or "")
|
# 跟踪目标的坐标系按**臂号**派生(SN 末两位),两侧互不干扰。
|
||||||
sn = str((_robot or {}).get("sn") or "")
|
# 以前是从 SN 里找 "063989"/"063991" 子串,换个站点就失去意义了。
|
||||||
if "063991" in sn or arm == "right":
|
short = arm_short_id(str((_robot or {}).get("sn") or ""))
|
||||||
obj = "tracked_object_91"
|
obj = f"tracked_object_{short}" if short else "tracked_object"
|
||||||
elif "063989" in sn or arm == "left":
|
|
||||||
obj = "tracked_object_89"
|
|
||||||
else:
|
|
||||||
obj = "tracked_object"
|
|
||||||
return {
|
return {
|
||||||
"weights": str(paths.get("yolo_weights") or ""),
|
"weights": str(paths.get("yolo_weights") or ""),
|
||||||
"conf": float(yolo.get("conf") or 0.10),
|
"conf": float(yolo.get("conf") or 0.10),
|
||||||
@@ -466,7 +579,14 @@ def main() -> int:
|
|||||||
os.environ["ROBOT_SN"] = str(args.sn)
|
os.environ["ROBOT_SN"] = str(args.sn)
|
||||||
path = Path(args.config).expanduser() if args.config else site_path()
|
path = Path(args.config).expanduser() if args.config else site_path()
|
||||||
data = load_site(path)
|
data = load_site(path)
|
||||||
|
try:
|
||||||
flat = flatten(data)
|
flat = flatten(data)
|
||||||
|
except ValueError as exc:
|
||||||
|
# 调用方是 `eval "$(python3 export_site_env.py)"`,抛栈对使用者毫无意义,
|
||||||
|
# 而且 eval 拿到空字符串后 SITE_* 全是空的,真正失败的地方会离这里很远。
|
||||||
|
# 所以这里必须打印一句人能看懂的原因并明确非零退出。
|
||||||
|
print(f"错误:{exc}", file=sys.stderr)
|
||||||
|
return 2
|
||||||
if args.json:
|
if args.json:
|
||||||
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
|
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
|
||||||
return 0
|
return 0
|
||||||
@@ -535,19 +655,27 @@ def main() -> int:
|
|||||||
if isinstance(he_data, dict) and (
|
if isinstance(he_data, dict) and (
|
||||||
he_data.get("uncalibrated") or "T_4x4" not in he_data
|
he_data.get("uncalibrated") or "T_4x4" not in he_data
|
||||||
):
|
):
|
||||||
|
arm_hint = flat.get("SITE_ROBOT_SHORT_ID") or flat.get("SITE_ROBOT_ARM") or "?"
|
||||||
print(
|
print(
|
||||||
f" 警告:{he_path.name} 没有针对该机械臂标定 "
|
f" 警告:{he_path.name} 没有针对该机械臂标定 "
|
||||||
f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})",
|
f"(run_handeye.sh teach/calibrate --arm {arm_hint})",
|
||||||
file=sys.stderr,
|
file=sys.stderr,
|
||||||
)
|
)
|
||||||
if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"):
|
# 右臂不该用左腕 D405 的序列号 —— 判据是**侧别**,不是 SN 里的数字。
|
||||||
|
# (两台臂的相机装反了不会报错,只会"跟踪一直差几厘米"。)
|
||||||
|
if (
|
||||||
|
flat.get("SITE_ROBOT_ARM") == "right"
|
||||||
|
and flat.get("SITE_CAM_SERIAL")
|
||||||
|
and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL")
|
||||||
|
):
|
||||||
print(
|
print(
|
||||||
" 警告:91 用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
|
" 警告:右臂用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
|
||||||
file=sys.stderr,
|
file=sys.stderr,
|
||||||
)
|
)
|
||||||
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
|
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
|
||||||
|
arm_hint = flat.get("SITE_ROBOT_SHORT_ID") or flat.get("SITE_ROBOT_ARM") or "?"
|
||||||
print(
|
print(
|
||||||
" 提示:本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
|
f" 提示:本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm {arm_hint} --cli open|rotate",
|
||||||
file=sys.stderr,
|
file=sys.stderr,
|
||||||
)
|
)
|
||||||
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
|
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
|
||||||
|
|||||||
@@ -2,19 +2,49 @@
|
|||||||
# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。
|
# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。
|
||||||
# 用法(在其它脚本里):
|
# 用法(在其它脚本里):
|
||||||
# source "${ROOT}/scripts/lib/load_site_config.sh"
|
# source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||||
|
#
|
||||||
|
# 本文件同时负责**在配置有问题时立刻拦下**,因为它的调用方几乎都是
|
||||||
|
# `eval "$(...)"` 或直接引用 `SITE_*`:如果这里不拦,脚本会拿着一堆空变量
|
||||||
|
# 继续往下跑,真正报错的地方会离原因很远(历史上就这么出过"参数静默失效")。
|
||||||
|
#
|
||||||
|
# 两种"没配置好"要区别对待:
|
||||||
|
# 1. site.yaml **不存在** —— 新克隆还没 `cp`。给 WARN 就放行,
|
||||||
|
# 因为有些脚本(网络检查、RDK 自检)在配置前就该能用。
|
||||||
|
# 2. site.yaml **存在但选不出机械臂** —— 配置写错了。直接拦下,
|
||||||
|
# 并且必须给出"改哪个文件的哪个键"。
|
||||||
|
|
||||||
_SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)"
|
_SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)"
|
||||||
_SITE_EXPORTER="${_SITE_ROOT}/scripts/lib/export_site_env.py"
|
_SITE_EXPORTER="${_SITE_ROOT}/scripts/lib/export_site_env.py"
|
||||||
_SITE_FILE="${SITE_CONFIG:-${_SITE_ROOT}/config/site.yaml}"
|
_SITE_FILE="${SITE_CONFIG:-${_SITE_ROOT}/config/site.yaml}"
|
||||||
|
|
||||||
|
# 供调用方判断"是没配置还是配错了"。0 = 正常加载;1 = 文件不存在;2 = 配置有误。
|
||||||
|
SITE_LOAD_STATUS=0
|
||||||
|
|
||||||
if [[ ! -f "${_SITE_FILE}" ]]; then
|
if [[ ! -f "${_SITE_FILE}" ]]; then
|
||||||
echo "[site] WARN: missing ${_SITE_FILE}" >&2
|
echo "[site] WARN: 没有 ${_SITE_FILE}(新克隆请先 cp config/site.example.yaml config/site.yaml)" >&2
|
||||||
|
SITE_LOAD_STATUS=1
|
||||||
|
else
|
||||||
|
_site_env=""
|
||||||
|
if ! _site_env="$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"; then
|
||||||
|
echo "[site] ERROR: 解析 ${_SITE_FILE} 失败,见上面 export_site_env.py 的报错。" >&2
|
||||||
|
echo "[site] 修好后重试;不要带着空的 SITE_* 继续跑。" >&2
|
||||||
|
SITE_LOAD_STATUS=2
|
||||||
else
|
else
|
||||||
# shellcheck disable=SC1090
|
# shellcheck disable=SC1090
|
||||||
eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"
|
eval "${_site_env}"
|
||||||
export SITE_CONFIG="${_SITE_FILE}"
|
export SITE_CONFIG="${_SITE_FILE}"
|
||||||
|
|
||||||
|
if [[ -z "${SITE_ROBOT_SN:-}" ]]; then
|
||||||
|
# 绝不兜底成某个 SN:那等于"配置漏了照样启动,连的是哪台看运气"。
|
||||||
|
echo "[site] ERROR: ${_SITE_FILE} 里没有选出机械臂(robot.sn 为空且没有 --arm)。" >&2
|
||||||
|
echo "[site] 在 config/site.yaml 的 robot.sn 里填完整 SN," >&2
|
||||||
|
echo "[site] 或给命令加 --arm <臂号>(臂号 = 该臂 SN 的末两位)。" >&2
|
||||||
|
SITE_LOAD_STATUS=2
|
||||||
|
else
|
||||||
# 把 89/91/left/right 别名解析成完整的 Flexiv SN,供 RDK 使用。
|
# 把 89/91/left/right 别名解析成完整的 Flexiv SN,供 RDK 使用。
|
||||||
if [[ -n "${SITE_ROBOT_SN:-}" ]]; then
|
|
||||||
export ROBOT_SN="${SITE_ROBOT_SN}"
|
export ROBOT_SN="${SITE_ROBOT_SN}"
|
||||||
fi
|
fi
|
||||||
fi
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
unset _SITE_ROOT _SITE_EXPORTER _SITE_FILE _site_env
|
||||||
|
|||||||
@@ -25,7 +25,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
|
|||||||
行协议(JSON 只写 stdout;RDK 自身日志走 stderr)
|
行协议(JSON 只写 stdout;RDK 自身日志走 stderr)
|
||||||
-------------------------------------------------
|
-------------------------------------------------
|
||||||
{"cmd":"ping"}
|
{"cmd":"ping"}
|
||||||
{"cmd":"connect","sn":"Rizon4-063989","iface":"enp4s0"}
|
{"cmd":"connect","sn":"<site.yaml 的 robot.sn>","iface":"<robot.iface>"}
|
||||||
{"cmd":"enable"} # 使能 + 清故障(幂等)
|
{"cmd":"enable"} # 使能 + 清故障(幂等)
|
||||||
{"cmd":"clear_fault"} # 显式清故障并使能
|
{"cmd":"clear_fault"} # 显式清故障并使能
|
||||||
{"cmd":"switch_tool","name":"tool1"} # 切工具;name 空=只查询
|
{"cmd":"switch_tool","name":"tool1"} # 切工具;name 空=只查询
|
||||||
@@ -41,6 +41,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import json
|
import json
|
||||||
|
import os
|
||||||
import sys
|
import sys
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
@@ -222,7 +223,7 @@ def main() -> int:
|
|||||||
reply({'ok': True, 'py': sys.version.split()[0]})
|
reply({'ok': True, 'py': sys.version.split()[0]})
|
||||||
|
|
||||||
elif cmd == 'connect':
|
elif cmd == 'connect':
|
||||||
sn = str(msg.get('sn', 'Rizon4-063989'))
|
sn = str(msg.get('sn') or os.environ.get('SITE_ROBOT_SN') or '')
|
||||||
iface = str(msg.get('iface') or '')
|
iface = str(msg.get('iface') or '')
|
||||||
robot = None
|
robot = None
|
||||||
last_err = None
|
last_err = None
|
||||||
|
|||||||
@@ -13,7 +13,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
|||||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||||
|
|
||||||
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
|
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
|
||||||
SN="${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}"
|
SN="${ROBOT_SN:-${SITE_ROBOT_SN:-}}"
|
||||||
NAME="${SITE_ROBOT_TOOL_NAME:-}"
|
NAME="${SITE_ROBOT_TOOL_NAME:-}"
|
||||||
IFACE="${SITE_NET_IFACE:-}"
|
IFACE="${SITE_NET_IFACE:-}"
|
||||||
|
|
||||||
|
|||||||
+4
-1
@@ -151,7 +151,10 @@ _tactile_ok() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if [[ "${WITH_TACTILE}" == true ]]; then
|
if [[ "${WITH_TACTILE}" == true ]]; then
|
||||||
if [[ "${SITE_ROBOT_ARM:-}" == "left" || "${SITE_ROBOT_SN:-}" == "Rizon4-063989" ]]; then
|
# 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算。
|
||||||
|
# 以前这里写的是 `SN == Rizon4-063989`:换一台机器人就再也拉不起触觉服务,
|
||||||
|
# 而且失败现象是"搜孔时力阈值全是 0",离真正原因很远。
|
||||||
|
if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
|
||||||
if _tactile_ok; then
|
if _tactile_ok; then
|
||||||
echo "=== 触觉 ${TACTILE_TOPIC} 已在发,跳过拉起 ==="
|
echo "=== 触觉 ${TACTILE_TOPIC} 已在发,跳过拉起 ==="
|
||||||
else
|
else
|
||||||
|
|||||||
@@ -75,7 +75,7 @@ if [[ ${#CLI[@]} -gt 0 ]]; then
|
|||||||
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
|
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
|
||||||
;;
|
;;
|
||||||
esac
|
esac
|
||||||
echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=91 ==="
|
echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=${SITE_ROBOT_SHORT_ID:-?} ==="
|
||||||
exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
|
exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
|
||||||
fi
|
fi
|
||||||
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
|
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
|
||||||
|
|||||||
+10
-6
@@ -68,16 +68,20 @@ if [[ ! -d "${APP_CFG}" ]]; then
|
|||||||
fi
|
fi
|
||||||
|
|
||||||
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下(见 handeye/README.md)。
|
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下(见 handeye/README.md)。
|
||||||
# 这些 SHE 值正常情况下由 site.yaml → export_site_env.py 提供;
|
# 这些值全部由 site.yaml → export_site_env.py 提供,**包括文件名里的臂号**
|
||||||
# 下面只是 shell 侧的兜底,路径必须与 export_site_env.py 的默认值一致。
|
# (臂号 = 该臂 SN 末两位,由 arm_short_id() 现算)。
|
||||||
|
#
|
||||||
|
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`:`_arm_id` 和 `HANDEYE_MODE`
|
||||||
|
# 在下面几行之后才算得出来,在这里拼会拼出 `results/_.yaml` 这种空文件名,
|
||||||
|
# 而症状是"标定写完了、用的时候读不到"。
|
||||||
HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
|
HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
|
||||||
WP="${SITE_HANDEYE_WAYPOINTS:-${HE_DIR}/poses/89_eye_in_hand.yaml}"
|
WP="${SITE_HANDEYE_WAYPOINTS:?site.yaml 没有给出该臂的手眼示教点文件}"
|
||||||
CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
|
CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
|
||||||
TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
|
TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
|
||||||
RES="${SITE_HANDEYE_FILE:-${HE_DIR}/results/89_eye_in_hand.yaml}"
|
RES="${SITE_HANDEYE_FILE:?site.yaml 没有给出该臂的手眼结果文件}"
|
||||||
WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
|
WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
|
||||||
TAUGHT="${WORKDIR}/taught_poses.txt"
|
TAUGHT="${WORKDIR}/taught_poses.txt"
|
||||||
PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}"
|
PREFIX="${SITE_JOINT_PREFIX:?SITE_JOINT_PREFIX 为空 —— site.yaml 里需要该臂的 robot.sn}"
|
||||||
CAM="${SITE_CAM_NAME:-d405}"
|
CAM="${SITE_CAM_NAME:-d405}"
|
||||||
|
|
||||||
# ---- --mode:切换 eye-in-hand / eye-to-hand ----
|
# ---- --mode:切换 eye-in-hand / eye-to-hand ----
|
||||||
@@ -294,7 +298,7 @@ cmd_teach() {
|
|||||||
fi
|
fi
|
||||||
TEACH_ARGS=(
|
TEACH_ARGS=(
|
||||||
-p backend:="${backend}"
|
-p backend:="${backend}"
|
||||||
-p robot_sn:="${SITE_ROBOT_SN:-Rizon4-063989}"
|
-p robot_sn:="${SITE_ROBOT_SN:?手眼标定必须知道是哪台臂}"
|
||||||
-p joint_prefix:="${PREFIX}"
|
-p joint_prefix:="${PREFIX}"
|
||||||
-p camera_source:="${CAM}"
|
-p camera_source:="${CAM}"
|
||||||
-p output_dir:="${WORKDIR}"
|
-p output_dir:="${WORKDIR}"
|
||||||
|
|||||||
+1
-1
@@ -35,7 +35,7 @@ source "${ROOT}/scripts/lib/load_site_config.sh"
|
|||||||
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
|
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
|
||||||
|
|
||||||
# `SITE_ROBOT_SN` 是解析后的 Flexiv 序列号(89/91 别名已经映射好了)。
|
# `SITE_ROBOT_SN` 是解析后的 Flexiv 序列号(89/91 别名已经映射好了)。
|
||||||
ROBOT_SN="${SITE_ROBOT_SN:-${ROBOT_SN:-Rizon4-063989}}"
|
ROBOT_SN="${SITE_ROBOT_SN:?run_real.sh 需要已配置的机械臂 SN —— 见 scripts/lib/load_site_config.sh 的报错}"
|
||||||
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
||||||
NET_IFACE="${SITE_NET_IFACE:-}"
|
NET_IFACE="${SITE_NET_IFACE:-}"
|
||||||
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
|
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
|
||||||
|
|||||||
@@ -98,7 +98,8 @@ fi
|
|||||||
|
|
||||||
# ---- 3) 触觉传感器服务(目前只有左臂 89 装了传感器;hover-only 不需要) ----
|
# ---- 3) 触觉传感器服务(目前只有左臂 89 装了传感器;hover-only 不需要) ----
|
||||||
if [[ "${WITH_TACTILE}" == true && "${HOVER_ONLY}" != true ]]; then
|
if [[ "${WITH_TACTILE}" == true && "${HOVER_ONLY}" != true ]]; then
|
||||||
if [[ "${SITE_ROBOT_ARM}" == "left" || "${SITE_ROBOT_SN}" == "Rizon4-063989" ]]; then
|
# 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算(见 run_all.sh 同名改动)。
|
||||||
|
if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
|
||||||
echo "=== start tactile sensor service (left arm) ==="
|
echo "=== start tactile sensor service (left arm) ==="
|
||||||
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
||||||
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker(占死 /dev/video*)
|
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker(占死 /dev/video*)
|
||||||
|
|||||||
@@ -79,7 +79,8 @@ fi
|
|||||||
|
|
||||||
# ---- 2) 触觉传感器服务(三阶段的核心传感器,必须在线) ----
|
# ---- 2) 触觉传感器服务(三阶段的核心传感器,必须在线) ----
|
||||||
if [[ "${WITH_TACTILE}" == true ]]; then
|
if [[ "${WITH_TACTILE}" == true ]]; then
|
||||||
if [[ "${SITE_ROBOT_ARM}" == "left" || "${SITE_ROBOT_SN}" == "Rizon4-063989" ]]; then
|
# 本臂有没有触觉由 site.yaml 的 robots[].tactile 说了算(见 run_all.sh 同名改动)。
|
||||||
|
if [[ "${SITE_ROBOT_HAS_TACTILE:-0}" == "1" ]]; then
|
||||||
echo "=== start tactile sensor service (left arm) ==="
|
echo "=== start tactile sensor service (left arm) ==="
|
||||||
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
|
||||||
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker(占死 /dev/video*)
|
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker(占死 /dev/video*)
|
||||||
|
|||||||
@@ -70,7 +70,7 @@ if [[ "${CONTROL}" == true ]]; then
|
|||||||
else
|
else
|
||||||
echo "=== launch perception + servo (observe only, no arm motion) ==="
|
echo "=== launch perception + servo (observe only, no arm motion) ==="
|
||||||
fi
|
fi
|
||||||
EE="${SITE_EE_FRAME:-Rizon4-063989_flange}"
|
EE="${SITE_EE_FRAME:?SITE_EE_FRAME 为空 —— site.yaml 的 robot.robots[] 里需要该臂的 SN}"
|
||||||
HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
|
HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
|
||||||
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}"
|
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}"
|
||||||
echo " weights=${SITE_YOLO_WEIGHTS} camera=${SITE_CAM_NAME:-} control=${CONTROL}"
|
echo " weights=${SITE_YOLO_WEIGHTS} camera=${SITE_CAM_NAME:-} control=${CONTROL}"
|
||||||
|
|||||||
@@ -5,7 +5,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
|||||||
# shellcheck disable=SC1091
|
# shellcheck disable=SC1091
|
||||||
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
|
||||||
|
|
||||||
SN="${1:-${ROBOT_SN:-${SITE_ROBOT_SN:-Rizon4-063989}}}"
|
SN="${1:-${ROBOT_SN:-${SITE_ROBOT_SN:-}}}"
|
||||||
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
|
||||||
IFACE="${SITE_NET_IFACE:-}"
|
IFACE="${SITE_NET_IFACE:-}"
|
||||||
PY="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
|
PY="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
|
||||||
|
|||||||
@@ -648,10 +648,13 @@ class RotaryGripper(BaseGripper):
|
|||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class DualArmGripperConfig:
|
class DualArmGripperConfig:
|
||||||
"""双臂夹爪配置。"""
|
"""双臂夹爪配置。
|
||||||
|
|
||||||
|
这里**没有** ``left_arm_sn`` / ``right_arm_sn``:它们曾经存在但从来没有被读过
|
||||||
|
(只写不读的死字段,R5.5),而且默认值是本机的 SN —— 换台臂就会误导读者。
|
||||||
|
哪个侧别是哪台臂,由 ``load_from_site()`` 从 site.yaml 的 ``robots[].arm`` 决定。
|
||||||
|
"""
|
||||||
|
|
||||||
left_arm_sn: str = "Rizon4-063989"
|
|
||||||
right_arm_sn: str = "Rizon4-063991"
|
|
||||||
left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
|
left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
|
||||||
right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
|
right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
|
||||||
|
|
||||||
@@ -742,22 +745,21 @@ def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
|
|||||||
auto_initialize=bool(rotary.get("auto_initialize", True)),
|
auto_initialize=bool(rotary.get("auto_initialize", True)),
|
||||||
)
|
)
|
||||||
return DualArmGripperConfig(
|
return DualArmGripperConfig(
|
||||||
left_arm_sn=left.get("robot_sn", "Rizon4-063989"),
|
|
||||||
right_arm_sn=right.get("robot_sn", "Rizon4-063991"),
|
|
||||||
left_gripper=left_cfg,
|
left_gripper=left_cfg,
|
||||||
right_gripper=right_cfg,
|
right_gripper=right_cfg,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def load_from_site(data: dict) -> DualArmGripperConfig:
|
def load_from_site(data: dict) -> DualArmGripperConfig:
|
||||||
"""从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。"""
|
"""从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。
|
||||||
|
|
||||||
|
左右靠 ``robots[].arm`` 认,**不靠 SN 里的数字**:``"063989 就是左臂"``
|
||||||
|
是站点数据,写进代码后换个站点就会把左右夹爪搞反
|
||||||
|
(症状是"右臂的旋转爪去开左臂的平行爪",串口对不上才报错)。
|
||||||
|
"""
|
||||||
robots = list((data.get("robot") or {}).get("robots") or [])
|
robots = list((data.get("robot") or {}).get("robots") or [])
|
||||||
left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
|
left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
|
||||||
right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
|
right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
|
||||||
if not left:
|
|
||||||
left = next((r for r in robots if "063989" in str(r.get("sn") or "")), {})
|
|
||||||
if not right:
|
|
||||||
right = next((r for r in robots if "063991" in str(r.get("sn") or "")), {})
|
|
||||||
g = data.get("gripper") or {}
|
g = data.get("gripper") or {}
|
||||||
parallel = g.get("parallel") or {}
|
parallel = g.get("parallel") or {}
|
||||||
rotary = g.get("rotary") or {}
|
rotary = g.get("rotary") or {}
|
||||||
@@ -796,8 +798,6 @@ def load_from_site(data: dict) -> DualArmGripperConfig:
|
|||||||
init_timeout=float(rotary.get("init_timeout") or 15.0),
|
init_timeout=float(rotary.get("init_timeout") or 15.0),
|
||||||
)
|
)
|
||||||
return DualArmGripperConfig(
|
return DualArmGripperConfig(
|
||||||
left_arm_sn=str(left.get("sn") or "Rizon4-063989"),
|
|
||||||
right_arm_sn=str(right.get("sn") or "Rizon4-063991"),
|
|
||||||
left_gripper=left_cfg,
|
left_gripper=left_cfg,
|
||||||
right_gripper=right_cfg,
|
right_gripper=right_cfg,
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -31,15 +31,52 @@ def _print_status(label: str, status) -> None:
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def _normalize_arm(raw: str) -> str:
|
def _load_site_exporter():
|
||||||
t = (raw or "").strip().lower()
|
"""复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。
|
||||||
if t in ("89", "left", "l", "063989"):
|
|
||||||
return "left"
|
这里**不自己再抄一份** ``89 -> SN`` 的映射:抄一份就多一处会漂移的地方(M2)。
|
||||||
if t in ("91", "right", "r", "063991"):
|
"""
|
||||||
return "right"
|
root = Path(__file__).resolve().parents[3]
|
||||||
if t in ("both", "dual", "all"):
|
lib = str(root / "scripts" / "lib")
|
||||||
|
if lib not in sys.path:
|
||||||
|
sys.path.insert(0, lib)
|
||||||
|
import export_site_env # type: ignore
|
||||||
|
|
||||||
|
return export_site_env
|
||||||
|
|
||||||
|
|
||||||
|
def _normalize_arm(raw: str, data: dict) -> str:
|
||||||
|
"""把 --arm 的写法归一成 ``left`` / ``right`` / ``both``。
|
||||||
|
|
||||||
|
别名(``89`` 这类)由 ``export_site_env`` 从 site.yaml 的 ``robots[]`` 现算
|
||||||
|
(臂号 = 该臂 SN 末两位)。代码里**不写死** SN,否则换台机器人后会指错臂,
|
||||||
|
而症状是"给右臂发命令,左臂动了"。
|
||||||
|
"""
|
||||||
|
t = (raw or "").strip()
|
||||||
|
if t.lower() in ("both", "dual", "all"):
|
||||||
return "both"
|
return "both"
|
||||||
raise SystemExit(f"未知的 --arm {raw!r}(可用 89|91|left|right|both)")
|
site_env = _load_site_exporter()
|
||||||
|
sn, matched = site_env.resolve_robot_sn(data, t)
|
||||||
|
if not matched:
|
||||||
|
raise SystemExit(
|
||||||
|
f"未知的 --arm {raw!r}:不在 site.yaml 的 robot.robots[] 里。\n"
|
||||||
|
f"可用:\n{site_env.describe_arms(data)}\n"
|
||||||
|
f"也可以直接写 left|right|both。"
|
||||||
|
)
|
||||||
|
for row in (data.get("robot") or {}).get("robots") or []:
|
||||||
|
if str(row.get("sn") or "").strip() != sn:
|
||||||
|
continue
|
||||||
|
arm = str(row.get("arm") or "").strip().lower()
|
||||||
|
if arm in ("left", "right"):
|
||||||
|
return arm
|
||||||
|
raise SystemExit(
|
||||||
|
f"--arm {raw!r} 选中了 {sn},但 site.yaml 里这台臂没写 arm: left|right,"
|
||||||
|
f"无法判断操作哪个夹爪。请补上这个键。"
|
||||||
|
)
|
||||||
|
raise SystemExit(
|
||||||
|
f"--arm {raw!r} 解析出 {sn},但它不在 site.yaml 的 robot.robots[] 里,"
|
||||||
|
f"无法判断是哪个夹爪。"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||||
@@ -93,7 +130,18 @@ def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> Non
|
|||||||
|
|
||||||
def main() -> int:
|
def main() -> int:
|
||||||
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具(串口来自 site.yaml)")
|
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具(串口来自 site.yaml)")
|
||||||
parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
|
parser.add_argument(
|
||||||
|
"--arm",
|
||||||
|
# 默认取当前臂(parse_arm_args.sh / load_site_config.sh 已经把它放进环境)。
|
||||||
|
# **不**兜底成某个具体臂号:那等于"没选臂时也能跑,跑的是哪台看运气"。
|
||||||
|
default=(
|
||||||
|
os.environ.get("SITE_ROBOT_ARM")
|
||||||
|
or os.environ.get("ROBOT_SN")
|
||||||
|
or os.environ.get("SITE_ROBOT_SN")
|
||||||
|
or ""
|
||||||
|
),
|
||||||
|
help="left|right|both,或该臂的臂号(= SN 末两位,由 site.yaml 决定)",
|
||||||
|
)
|
||||||
parser.add_argument(
|
parser.add_argument(
|
||||||
"--test",
|
"--test",
|
||||||
default="status",
|
default="status",
|
||||||
@@ -114,7 +162,7 @@ def main() -> int:
|
|||||||
return 1
|
return 1
|
||||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||||
manager = DualArmGripperManager(load_from_site(data))
|
manager = DualArmGripperManager(load_from_site(data))
|
||||||
arm = _normalize_arm(str(args.arm))
|
arm = _normalize_arm(str(args.arm), data)
|
||||||
print(f"site={site} arm={arm} test={args.test}")
|
print(f"site={site} arm={arm} test={args.test}")
|
||||||
|
|
||||||
try:
|
try:
|
||||||
|
|||||||
@@ -33,6 +33,12 @@ from arm_application.geom_ik import (
|
|||||||
geometric_jacobian,
|
geometric_jacobian,
|
||||||
quat_xyzw_to_R,
|
quat_xyzw_to_R,
|
||||||
)
|
)
|
||||||
|
from arm_application.site_io import (
|
||||||
|
base_frame_default,
|
||||||
|
ee_frame_default,
|
||||||
|
joint_prefix_default,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def _load_ee_cfg() -> dict:
|
def _load_ee_cfg() -> dict:
|
||||||
@@ -45,9 +51,9 @@ def _load_ee_cfg() -> dict:
|
|||||||
robot = resolve_robot(data)
|
robot = resolve_robot(data)
|
||||||
ee_z = float(servo.get("ee_z_m") or 0.24)
|
ee_z = float(servo.get("ee_z_m") or 0.24)
|
||||||
return {
|
return {
|
||||||
"prefix": str(robot.get("joint_prefix") or "Rizon4-063989_"),
|
"prefix": str(robot.get("joint_prefix") or joint_prefix_default()),
|
||||||
"base": str(robot.get("base_frame") or "Rizon4-063989_base_link"),
|
"base": str(robot.get("base_frame") or base_frame_default()),
|
||||||
"ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"),
|
"ee": str(robot.get("ee_frame") or ee_frame_default()),
|
||||||
"z": ee_z,
|
"z": ee_z,
|
||||||
# 回 home 前先竖直升到这个 Z(避免撞到墙面/工装);
|
# 回 home 前先竖直升到这个 Z(避免撞到墙面/工装);
|
||||||
# 然后再回到 Flexiv 的命名初始位姿。
|
# 然后再回到 Flexiv 的命名初始位姿。
|
||||||
|
|||||||
@@ -45,6 +45,8 @@ from arm_application.handeye_realsense import (
|
|||||||
query_realsense_color_intrinsics,
|
query_realsense_color_intrinsics,
|
||||||
resolve_realsense_topics,
|
resolve_realsense_topics,
|
||||||
)
|
)
|
||||||
|
from arm_application.site_io import joint_prefix_default
|
||||||
|
|
||||||
from arm_msgs.action import FollowJointTrajectory
|
from arm_msgs.action import FollowJointTrajectory
|
||||||
from arm_msgs.srv import GetArmState
|
from arm_msgs.srv import GetArmState
|
||||||
|
|
||||||
@@ -82,7 +84,7 @@ class HandeyeCalibrate(Node):
|
|||||||
super().__init__('handeye_calibrate')
|
super().__init__('handeye_calibrate')
|
||||||
|
|
||||||
self.declare_parameter('mode', 'eye_in_hand') # eye_in_hand | eye_to_hand
|
self.declare_parameter('mode', 'eye_in_hand') # eye_in_hand | eye_to_hand
|
||||||
self.declare_parameter('joint_prefix', 'Rizon4-063989_')
|
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||||
self.declare_parameter('velocity_scale', 0.10)
|
self.declare_parameter('velocity_scale', 0.10)
|
||||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||||
|
|||||||
@@ -26,7 +26,6 @@ rdk(软件 Free Drive):
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import sys
|
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
from datetime import datetime, timezone
|
from datetime import datetime, timezone
|
||||||
@@ -249,7 +248,7 @@ class HandeyeTeach(Node):
|
|||||||
self.declare_parameter('height', 720)
|
self.declare_parameter('height', 720)
|
||||||
self.declare_parameter('fps', 15)
|
self.declare_parameter('fps', 15)
|
||||||
self.declare_parameter('backend', 'ros') # ros | rdk
|
self.declare_parameter('backend', 'ros') # ros | rdk
|
||||||
self.declare_parameter('robot_sn', 'Rizon4-063989')
|
self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
|
||||||
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
|
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
|
||||||
self._serial = _param_as_str(self, 'camera_serial', '')
|
self._serial = _param_as_str(self, 'camera_serial', '')
|
||||||
self.declare_parameter('auto_exposure', True)
|
self.declare_parameter('auto_exposure', True)
|
||||||
|
|||||||
@@ -36,6 +36,8 @@ from arm_application.handeye_realsense import (
|
|||||||
is_realsense_source,
|
is_realsense_source,
|
||||||
resolve_realsense_topics,
|
resolve_realsense_topics,
|
||||||
)
|
)
|
||||||
|
from arm_application.site_io import joint_prefix_default
|
||||||
|
|
||||||
from arm_msgs.action import FollowJointTrajectory
|
from arm_msgs.action import FollowJointTrajectory
|
||||||
from arm_msgs.srv import GetArmState
|
from arm_msgs.srv import GetArmState
|
||||||
|
|
||||||
@@ -111,7 +113,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
|
|||||||
class HandeyeWaypoints(Node):
|
class HandeyeWaypoints(Node):
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__('handeye_waypoints')
|
super().__init__('handeye_waypoints')
|
||||||
self.declare_parameter('joint_prefix', 'Rizon4-063989_')
|
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||||
self.declare_parameter('velocity_scale', 0.12)
|
self.declare_parameter('velocity_scale', 0.12)
|
||||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||||
|
|||||||
@@ -4,12 +4,12 @@
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import time
|
import time
|
||||||
from typing import List
|
|
||||||
|
|
||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.action import ActionClient
|
from rclpy.action import ActionClient
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
|
from arm_application.site_io import joint_prefix_default
|
||||||
from arm_msgs.action import FollowJointTrajectory
|
from arm_msgs.action import FollowJointTrajectory
|
||||||
|
|
||||||
DEFAULT_JOINTS = [
|
DEFAULT_JOINTS = [
|
||||||
@@ -28,7 +28,7 @@ DEFAULT_POSITIONS = [
|
|||||||
class RunJointTrajectory(Node):
|
class RunJointTrajectory(Node):
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__('run_joint_trajectory')
|
super().__init__('run_joint_trajectory')
|
||||||
self.declare_parameter('joint_prefix', 'Rizon4-063989_')
|
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||||
self.declare_parameter('velocity_scale', 0.15)
|
self.declare_parameter('velocity_scale', 0.15)
|
||||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||||
|
|||||||
@@ -40,6 +40,11 @@ from arm_application.geom_ik import (
|
|||||||
)
|
)
|
||||||
from arm_application.pid_controller import PidController
|
from arm_application.pid_controller import PidController
|
||||||
from arm_application.kalman_filter import KalmanFilter3D
|
from arm_application.kalman_filter import KalmanFilter3D
|
||||||
|
from arm_application.site_io import (
|
||||||
|
base_frame_default,
|
||||||
|
ee_frame_default,
|
||||||
|
joint_prefix_default,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
DEFAULT_JOINTS = [
|
DEFAULT_JOINTS = [
|
||||||
@@ -62,10 +67,10 @@ class ServoNode(Node):
|
|||||||
f"servo_node 已启动 pid={os.getpid()} thread_id={tid}"
|
f"servo_node 已启动 pid={os.getpid()} thread_id={tid}"
|
||||||
)
|
)
|
||||||
|
|
||||||
self.declare_parameter("joint_prefix", "Rizon4-063989_")
|
self.declare_parameter("joint_prefix", joint_prefix_default())
|
||||||
self.declare_parameter("joint_names", DEFAULT_JOINTS)
|
self.declare_parameter("joint_names", DEFAULT_JOINTS)
|
||||||
self.declare_parameter("base_frame", "Rizon4-063989_base_link")
|
self.declare_parameter("base_frame", base_frame_default())
|
||||||
self.declare_parameter("ee_frame", "Rizon4-063989_flange")
|
self.declare_parameter("ee_frame", ee_frame_default())
|
||||||
self.declare_parameter("camera_frame", "d405_color_optical_frame")
|
self.declare_parameter("camera_frame", "d405_color_optical_frame")
|
||||||
self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
|
self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
|
||||||
self.declare_parameter("point_topic", "visual_servo/filtered_point")
|
self.declare_parameter("point_topic", "visual_servo/filtered_point")
|
||||||
|
|||||||
@@ -40,3 +40,43 @@ def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
|
|||||||
else:
|
else:
|
||||||
out[key] = val
|
out[key] = val
|
||||||
return out
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def joint_prefix_default() -> str:
|
||||||
|
"""当前臂的关节名前缀(``Rizon4-XXXXXX_``),用作 ROS 参数的默认值。
|
||||||
|
|
||||||
|
来源顺序:
|
||||||
|
|
||||||
|
1. ``SITE_JOINT_PREFIX`` —— ``scripts/env.sh`` → ``load_site_config.sh`` 从
|
||||||
|
site.yaml 导出,这就是**权威值**;
|
||||||
|
2. 由 ``SITE_ROBOT_SN`` 现算(``Rizon4-063989`` → ``Rizon4-063989_``);
|
||||||
|
3. 都没有 → 空字符串。
|
||||||
|
|
||||||
|
**绝不写死某个具体 SN。** 以前这里的默认值是 ``"Rizon4-063989_"``,
|
||||||
|
于是换台机器人后 ``ros2 run`` 直接起的节点会去订**上一台臂**的关节话题,
|
||||||
|
表现为"节点起来了、什么都不动、也不报错"(R1.1)。
|
||||||
|
"""
|
||||||
|
prefix = os.environ.get("SITE_JOINT_PREFIX", "").strip()
|
||||||
|
if prefix:
|
||||||
|
return prefix
|
||||||
|
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||||
|
return f"{sn}_" if sn else ""
|
||||||
|
|
||||||
|
|
||||||
|
def base_frame_default() -> str:
|
||||||
|
"""当前臂的基座坐标系(``<SN>_base_link``),用作 ROS 参数默认值。"""
|
||||||
|
frame = os.environ.get("SITE_BASE_FRAME", "").strip()
|
||||||
|
if frame:
|
||||||
|
return frame
|
||||||
|
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||||
|
return f"{sn}_base_link" if sn else ""
|
||||||
|
|
||||||
|
|
||||||
|
def ee_frame_default() -> str:
|
||||||
|
"""当前臂的法兰坐标系(``<SN>_flange``),用作 ROS 参数默认值。"""
|
||||||
|
frame = os.environ.get("SITE_EE_FRAME", "").strip()
|
||||||
|
if frame:
|
||||||
|
return frame
|
||||||
|
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||||
|
return f"{sn}_flange" if sn else ""
|
||||||
|
|
||||||
|
|||||||
@@ -117,9 +117,14 @@ def _launch_setup(context, *args, **kwargs):
|
|||||||
ee_frame = LaunchConfiguration("ee_frame").perform(context).strip()
|
ee_frame = LaunchConfiguration("ee_frame").perform(context).strip()
|
||||||
cam_frame = LaunchConfiguration("camera_frame").perform(context).strip()
|
cam_frame = LaunchConfiguration("camera_frame").perform(context).strip()
|
||||||
yolo = site.get("yolo") or {}
|
yolo = site.get("yolo") or {}
|
||||||
if not ee_frame or ee_frame == "Rizon4-063989_flange":
|
# "没指定"用**空串**表示,不再拿一个具体坐标系名当哨兵:
|
||||||
|
# 之前写的是 `ee_frame == "Rizon4-063989_flange"`,换个站点以后
|
||||||
|
# 这个判断永远不成立,于是 site.yaml 里的 ee_frame 再也不会被用上
|
||||||
|
# (症状是静态 TF 挂到了错的坐标系上,而点还是能查到)。
|
||||||
|
if not ee_frame:
|
||||||
ee_frame = str(robot.get("ee_frame") or ee_frame)
|
ee_frame = str(robot.get("ee_frame") or ee_frame)
|
||||||
if not cam_frame or cam_frame == "d405_color_optical_frame":
|
# 同 ee_frame:用空串表示"没指定",不拿具体坐标系名当哨兵。
|
||||||
|
if not cam_frame:
|
||||||
cam_frame = str(
|
cam_frame = str(
|
||||||
perc_over.get("camera_frame")
|
perc_over.get("camera_frame")
|
||||||
or yolo.get("camera_frame")
|
or yolo.get("camera_frame")
|
||||||
@@ -216,17 +221,20 @@ def _launch_setup(context, *args, **kwargs):
|
|||||||
def generate_launch_description() -> LaunchDescription:
|
def generate_launch_description() -> LaunchDescription:
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
[
|
[
|
||||||
|
# 默认值从环境取(scripts/env.sh → load_site_config.sh 导出的
|
||||||
|
# SITE_HANDEYE_FILE / SITE_EE_FRAME 已经是**当前臂**的值)。
|
||||||
|
# 不写死某个臂的文件名/坐标系:写死的话换臂后这里就悄悄指向上一台。
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"handeye_file",
|
"handeye_file",
|
||||||
default_value="/home/cc/ros2/handeye/results/89_eye_in_hand.yaml",
|
default_value=os.environ.get("SITE_HANDEYE_FILE", ""),
|
||||||
),
|
),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"ee_frame",
|
"ee_frame",
|
||||||
default_value="Rizon4-063989_flange",
|
default_value=os.environ.get("SITE_EE_FRAME", ""),
|
||||||
),
|
),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"camera_frame",
|
"camera_frame",
|
||||||
default_value="d405_color_optical_frame",
|
default_value=os.environ.get("SITE_CAMERA_FRAME", ""),
|
||||||
),
|
),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"enable_control",
|
"enable_control",
|
||||||
|
|||||||
@@ -9,7 +9,9 @@ import os
|
|||||||
|
|
||||||
# 现场机械臂 —— DH PGEA 是独立的 Modbus 节点(run_gripper.sh)。
|
# 现场机械臂 —— DH PGEA 是独立的 Modbus 节点(run_gripper.sh)。
|
||||||
# 保持 Flexiv 的 load_gripper 为 false(该开关在 Studio 里对应 Flexiv-GN01)。
|
# 保持 Flexiv 的 load_gripper 为 false(该开关在 Studio 里对应 Flexiv-GN01)。
|
||||||
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
|
# 必须来自环境(scripts/env.sh → load_site_config.sh 会保证它非空)。
|
||||||
|
# 这里**不兜底**成某个具体 SN:兜底等于"环境没加载好也照样启动,连哪台看运气"。
|
||||||
|
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', '').strip()
|
||||||
|
|
||||||
|
|
||||||
def _launch_setup(context, *args, **kwargs):
|
def _launch_setup(context, *args, **kwargs):
|
||||||
|
|||||||
@@ -4,6 +4,7 @@ from launch.conditions import IfCondition
|
|||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
import os
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
@@ -36,10 +37,13 @@ def generate_launch_description():
|
|||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument('use_rviz', default_value='true'),
|
DeclareLaunchArgument('use_rviz', default_value='true'),
|
||||||
DeclareLaunchArgument('use_moveit', default_value='false'),
|
DeclareLaunchArgument('use_moveit', default_value='false'),
|
||||||
|
# 仿真**故意**不用真机的 SN 前缀:两边同时跑时,同名关节会让 TF 与
|
||||||
|
# 控制器互相打架(症状是 RViz 里模型乱跳)。见 devices/sim/gazebo/README.md。
|
||||||
|
# 所以默认值是仿真自己的前缀,而不是"把真机那台抄过来"。
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
'robot_sn',
|
'robot_sn',
|
||||||
default_value='Rizon4-063989',
|
default_value=os.environ.get('SITE_SIM_JOINT_PREFIX', 'sim'),
|
||||||
description='现场 SN;关节名会变成 Rizon4-063989_joint1..joint7',
|
description='仿真关节前缀;关节名会变成 <前缀>_joint1..joint7(有意与真机不同名)',
|
||||||
),
|
),
|
||||||
mock,
|
mock,
|
||||||
moveit,
|
moveit,
|
||||||
|
|||||||
@@ -8,7 +8,6 @@ import os
|
|||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
|
||||||
from launch.conditions import IfCondition
|
|
||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
@@ -16,7 +15,8 @@ from launch_ros.substitutions import FindPackageShare
|
|||||||
from ament_index_python.packages import PackageNotFoundError, get_package_share_directory
|
from ament_index_python.packages import PackageNotFoundError, get_package_share_directory
|
||||||
|
|
||||||
|
|
||||||
SITE_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
|
# 由 scripts/env.sh → load_site_config.sh 提供;不兜底成具体 SN(见 real.launch.py)。
|
||||||
|
SITE_SN = os.environ.get('SITE_ROBOT_SN', '').strip()
|
||||||
SITE_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_SN}_')
|
SITE_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_SN}_')
|
||||||
SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
|
SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
|
||||||
|
|
||||||
|
|||||||
@@ -9,7 +9,9 @@ from launch_ros.parameter_descriptions import ParameterValue
|
|||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
|
||||||
# 现场机械臂。官方 flexiv_description 要求 SN 前缀里**保留连字符**。
|
# 现场机械臂。官方 flexiv_description 要求 SN 前缀里**保留连字符**。
|
||||||
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
|
# 由 scripts/env.sh → load_site_config.sh 提供;不兜底成具体 SN
|
||||||
|
# (兜底等于"环境没加载好也照样启动,连哪台看运气")。
|
||||||
|
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', '').strip()
|
||||||
SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_')
|
SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_')
|
||||||
SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
|
SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
|
||||||
|
|
||||||
|
|||||||
@@ -1,3 +1,8 @@
|
|||||||
|
# 现场机械臂 SN(由 scripts/env.sh → load_site_config.sh 从 site.yaml 导出)。
|
||||||
|
# 这个 launch 以前**根本不传 robot_sn**,靠 arm.urdf.xacro 里写死的默认值
|
||||||
|
# 正好等于本机 SN 才跑得起来;换台臂就会去驱动上一台臂的关节名。
|
||||||
|
import os
|
||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument
|
from launch.actions import DeclareLaunchArgument
|
||||||
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
||||||
@@ -8,6 +13,7 @@ from launch_ros.substitutions import FindPackageShare
|
|||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
use_fake = LaunchConfiguration('use_fake_hardware')
|
use_fake = LaunchConfiguration('use_fake_hardware')
|
||||||
|
robot_sn = LaunchConfiguration('robot_sn')
|
||||||
|
|
||||||
urdf_path = PathJoinSubstitution(
|
urdf_path = PathJoinSubstitution(
|
||||||
[FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
|
[FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
|
||||||
@@ -17,12 +23,21 @@ def generate_launch_description():
|
|||||||
)
|
)
|
||||||
|
|
||||||
robot_description = ParameterValue(
|
robot_description = ParameterValue(
|
||||||
Command(['xacro ', urdf_path, ' use_fake_hardware:=', use_fake]),
|
Command([
|
||||||
|
'xacro ', urdf_path,
|
||||||
|
' robot_sn:=', robot_sn,
|
||||||
|
' use_fake_hardware:=', use_fake,
|
||||||
|
]),
|
||||||
value_type=str,
|
value_type=str,
|
||||||
)
|
)
|
||||||
|
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument('use_fake_hardware', default_value='true'),
|
DeclareLaunchArgument('use_fake_hardware', default_value='true'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'robot_sn',
|
||||||
|
default_value=os.environ.get('SITE_ROBOT_SN', '').strip(),
|
||||||
|
description='当前臂 SN;关节名会变成 {SN}_joint1..joint7(连字符保留)',
|
||||||
|
),
|
||||||
|
|
||||||
Node(
|
Node(
|
||||||
package='robot_state_publisher',
|
package='robot_state_publisher',
|
||||||
|
|||||||
@@ -5,6 +5,7 @@ from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitut
|
|||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
from launch_ros.parameter_descriptions import ParameterValue
|
from launch_ros.parameter_descriptions import ParameterValue
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
import os
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
@@ -34,7 +35,11 @@ def generate_launch_description():
|
|||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument('use_gui', default_value='true'),
|
DeclareLaunchArgument('use_gui', default_value='true'),
|
||||||
DeclareLaunchArgument('use_rviz', default_value='true'),
|
DeclareLaunchArgument('use_rviz', default_value='true'),
|
||||||
DeclareLaunchArgument('robot_sn', default_value='Rizon4-063989'),
|
# 默认取当前臂(scripts/env.sh 已导出);不再写死某个 SN。
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'robot_sn',
|
||||||
|
default_value=os.environ.get('SITE_ROBOT_SN', '').strip(),
|
||||||
|
),
|
||||||
DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
|
DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
|
||||||
DeclareLaunchArgument('load_gripper', default_value='false'),
|
DeclareLaunchArgument('load_gripper', default_value='false'),
|
||||||
|
|
||||||
|
|||||||
@@ -6,7 +6,11 @@
|
|||||||
<xacro:include filename="$(find flexiv_description)/urdf/common/rizon_robot.xacro"/>
|
<xacro:include filename="$(find flexiv_description)/urdf/common/rizon_robot.xacro"/>
|
||||||
|
|
||||||
<xacro:arg name="arm_prefix" default=""/>
|
<xacro:arg name="arm_prefix" default=""/>
|
||||||
<xacro:arg name="robot_sn" default="Rizon4-063989"/>
|
<!-- 必须由 launch 传入真实 SN(mock_control / ros2_control / display 都会传)。
|
||||||
|
这里刻意留一个**明显是占位符**的默认值,而不是某台臂的真实 SN:
|
||||||
|
默认成真实 SN 的话,忘记传参时会安静地去驱动上一台臂的关节名,
|
||||||
|
而 <prefix> 变成 UNSET_ARM_SN_ 至少能让控制器报错信息里带着它。 -->
|
||||||
|
<xacro:arg name="robot_sn" default="UNSET_ARM_SN"/>
|
||||||
<xacro:arg name="joint_limits_parameters_file"
|
<xacro:arg name="joint_limits_parameters_file"
|
||||||
default="$(find flexiv_description)/config/$(arg rizon_type)/joint_limits.yaml"/>
|
default="$(find flexiv_description)/config/$(arg rizon_type)/joint_limits.yaml"/>
|
||||||
<xacro:arg name="kinematics_parameters_file"
|
<xacro:arg name="kinematics_parameters_file"
|
||||||
|
|||||||
@@ -0,0 +1,135 @@
|
|||||||
|
"""``scripts/check_arm_prefixes.py`` 的自证测试(R5.2)。
|
||||||
|
|
||||||
|
为什么这个检查器必须有测试
|
||||||
|
--------------------------
|
||||||
|
它是"换臂时防止漏改一级配置"的唯一守卫。**守卫自己失效过一次**:
|
||||||
|
|
||||||
|
第一个版本的正则写成 ``\\bRizon\\d+-\\d{6}\\b``,而 Flexiv 的关节名是
|
||||||
|
``Rizon4-063989_joint1`` —— ``_joint1`` 的下划线属于 ``\\w``,所以结尾的
|
||||||
|
``\\b`` 不成立,SRDF 里 24 处硬编码**一处都没报出来**,脚本还打印
|
||||||
|
"OK:8 个配置文件都指向 Rizon4-063989"。
|
||||||
|
|
||||||
|
守卫的漏洞和它要防的漏洞长得一模一样,所以这里逐条钉住:
|
||||||
|
|
||||||
|
* 挑得出 ``SN_joint1`` 这种**后缀带下划线**的形态(就是上面那个 bug);
|
||||||
|
* 不会把 ``Rizon4-06398912`` 这种更长数字误判成长度正确的前缀;
|
||||||
|
* 混着两个 SN 的文件,只报**不属于当前臂**的那些,并带上行号;
|
||||||
|
* 读不出当前臂时**直接失败**,不输出"检查通过"。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import importlib.util
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
_REPO = Path(__file__).resolve().parents[3]
|
||||||
|
_SCRIPT = _REPO / "scripts" / "check_arm_prefixes.py"
|
||||||
|
|
||||||
|
|
||||||
|
def _load():
|
||||||
|
"""按路径加载 ``check_arm_prefixes``(它在 scripts/ 下,不是可导入的包)。"""
|
||||||
|
if not _SCRIPT.is_file(): # pragma: no cover - 仓库被裁剪时的兜底
|
||||||
|
pytest.skip(f"找不到 {_SCRIPT}")
|
||||||
|
spec = importlib.util.spec_from_file_location("_check_arm_prefixes", _SCRIPT)
|
||||||
|
module = importlib.util.module_from_spec(spec)
|
||||||
|
assert spec and spec.loader
|
||||||
|
spec.loader.exec_module(module)
|
||||||
|
return module
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.fixture(scope="module")
|
||||||
|
def mod():
|
||||||
|
return _load()
|
||||||
|
|
||||||
|
|
||||||
|
LEFT = "Rizon4-063989"
|
||||||
|
RIGHT = "Rizon4-063991"
|
||||||
|
|
||||||
|
|
||||||
|
class TestSnToken:
|
||||||
|
"""正则必须挑得出真实的关节名形态。"""
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"text",
|
||||||
|
[
|
||||||
|
LEFT, # 裸 SN
|
||||||
|
f"{LEFT}_joint1", # ★ 就是踩过坑的形态
|
||||||
|
f"{LEFT}_base_link",
|
||||||
|
f"{LEFT}_flange",
|
||||||
|
f'joint_prefix: "{LEFT}_"',
|
||||||
|
f"- {LEFT}_joint7",
|
||||||
|
"Rizon10s-123456_joint3", # 别的型号
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_matches_real_joint_names(self, mod, text: str):
|
||||||
|
found = mod._SN_TOKEN.findall(text)
|
||||||
|
assert found, f"应挑出 SN,但一个都没匹配到:{text!r}"
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
"text",
|
||||||
|
[
|
||||||
|
"Rizon4-0639891", # 多一位数字,不是合法的 6 位 SN 前缀
|
||||||
|
"Rizon4-06398", # 只有 5 位
|
||||||
|
"Rizon4_063989", # 下划线而不是连字符
|
||||||
|
"not a robot at all",
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_rejects_non_sn_text(self, mod, text: str):
|
||||||
|
assert not mod._SN_TOKEN.findall(text), f"不该匹配:{text!r}"
|
||||||
|
|
||||||
|
|
||||||
|
class TestScan:
|
||||||
|
"""混着两台臂的 SN 时,只报不属于当前臂的那些。"""
|
||||||
|
|
||||||
|
def test_reports_only_other_arm(self, mod, tmp_path: Path):
|
||||||
|
f = tmp_path / "controllers.yaml"
|
||||||
|
f.write_text(
|
||||||
|
"\n".join(
|
||||||
|
[
|
||||||
|
f"# 站点机器人 {LEFT}",
|
||||||
|
"joints:",
|
||||||
|
f" - {LEFT}_joint1",
|
||||||
|
f" - {RIGHT}_joint2",
|
||||||
|
]
|
||||||
|
),
|
||||||
|
encoding="utf-8",
|
||||||
|
)
|
||||||
|
hits = mod._scan(f, want_sn=LEFT)
|
||||||
|
assert len(hits) == 1, f"只应报出 {RIGHT} 那一行,实际:{hits}"
|
||||||
|
assert hits[0].startswith("4:"), f"应带行号 4,实际:{hits[0]!r}"
|
||||||
|
assert RIGHT in hits[0]
|
||||||
|
|
||||||
|
def test_all_matching_is_clean(self, mod, tmp_path: Path):
|
||||||
|
f = tmp_path / "srdf.srdf"
|
||||||
|
f.write_text(f'<joint name="{LEFT}_joint1" value="0"/>\n', encoding="utf-8")
|
||||||
|
assert mod._scan(f, want_sn=LEFT) == []
|
||||||
|
|
||||||
|
def test_missing_file_is_reported_not_swallowed(self, mod, tmp_path: Path):
|
||||||
|
"""文件缺失也算不一致 —— 不能当成"没问题"跳过。"""
|
||||||
|
hits = mod._scan(tmp_path / "nope.yaml", want_sn=LEFT)
|
||||||
|
assert hits and "读不到文件" in hits[0]
|
||||||
|
|
||||||
|
|
||||||
|
class TestActivePrefixFailsLoudly:
|
||||||
|
"""读不出当前臂时必须失败,不能打印"检查通过"。"""
|
||||||
|
|
||||||
|
def test_missing_site_config_exits_nonzero(self, mod, monkeypatch, tmp_path: Path):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", str(tmp_path / "不存在.yaml"))
|
||||||
|
monkeypatch.delenv("ROBOT_SN", raising=False)
|
||||||
|
monkeypatch.delenv("ROBOT_ARM", raising=False)
|
||||||
|
with pytest.raises(SystemExit) as exc:
|
||||||
|
mod._active_prefix()
|
||||||
|
assert exc.value.code == 2, "缺配置应当是明确的失败退出码,不是 0"
|
||||||
|
|
||||||
|
def test_empty_sn_exits_nonzero(self, mod, monkeypatch, tmp_path: Path):
|
||||||
|
"""site.yaml 里 robot.sn 为空 → 报错,而不是兜底成某台臂。"""
|
||||||
|
site = tmp_path / "site.yaml"
|
||||||
|
site.write_text("robot:\n sn: ''\n robots: []\n", encoding="utf-8")
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", str(site))
|
||||||
|
monkeypatch.delenv("ROBOT_SN", raising=False)
|
||||||
|
monkeypatch.delenv("ROBOT_ARM", raising=False)
|
||||||
|
with pytest.raises(SystemExit) as exc:
|
||||||
|
mod._active_prefix()
|
||||||
|
assert exc.value.code == 2
|
||||||
@@ -0,0 +1,289 @@
|
|||||||
|
"""证明「换一份 site.yaml,导出的 SITE_* 里就不该残留任何本机站点值」。
|
||||||
|
|
||||||
|
这是 R1.1(禁止硬编码站点值)的机器化执行。与其用 grep 去数 ``Rizon4-063989``
|
||||||
|
出现了几次(数不清、还会误报注释和文档),不如直接问一个**更强**的问题:
|
||||||
|
|
||||||
|
给定一份完全不同的 site.yaml,``flatten()`` 导出的环境里
|
||||||
|
还会不会出现本机的 SN / IP / 相机序列号 / 用户路径?
|
||||||
|
|
||||||
|
只要有一处会,就说明那处的"默认值"其实是**写死的站点数据**。后果不是报错,
|
||||||
|
而是 site.yaml 一旦缺那个键,程序就**静默地去操作另一台机器**:
|
||||||
|
|
||||||
|
- ``SITE_ROBOT_SN`` 兜底成 ``Rizon4-063989`` → 配置漏了也照样连上,连的是谁看运气;
|
||||||
|
- ``SITE_D405_LEFT_SERIAL`` 兜底成某个真实序列号 → 换站点后悄悄打开别人的相机。
|
||||||
|
|
||||||
|
这类兜底比"没有默认值"危险得多(M1:不报错但结果是错的)。所以本测试的思路是
|
||||||
|
**用一份虚构站点的配置跑一遍,断言本机值一个都不许出现**。
|
||||||
|
|
||||||
|
真实工作区不存在时跳过(包可能被单独构建)。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_interfaces import paths
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# 被测模块在 scripts/lib/ 下(launch 文件也是把它加进 sys.path 再 import 的),
|
||||||
|
# 所以这里用同样的方式接进来。
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
|
_LIB = paths.ros2_root() / "scripts" / "lib"
|
||||||
|
if str(_LIB) not in sys.path:
|
||||||
|
sys.path.insert(0, str(_LIB))
|
||||||
|
|
||||||
|
pytestmark = pytest.mark.skipif(
|
||||||
|
not (_LIB / "export_site_env.py").is_file(),
|
||||||
|
reason="scripts/lib/export_site_env.py 不存在(包被单独构建),跳过",
|
||||||
|
)
|
||||||
|
|
||||||
|
export_site_env = pytest.importorskip(
|
||||||
|
"export_site_env", reason="需要 PyYAML 才能加载 site 配置"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# 本机(真实站点)的值 —— 这些**绝不允许**成为任何配置缺键时的兜底
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
|
LOCAL_SN_LEFT = "Rizon4-063989"
|
||||||
|
LOCAL_SN_RIGHT = "Rizon4-063991"
|
||||||
|
LOCAL_CAM_LEFT = "352122274408"
|
||||||
|
LOCAL_CAM_RIGHT = "260522274442"
|
||||||
|
LOCAL_IPS = ("192.168.2.101", "192.168.2.100", "192.168.2.10")
|
||||||
|
|
||||||
|
# 只放**设备身份**:SN、IP、相机序列号。不包含工作区绝对路径 ——
|
||||||
|
# `_root()` 派生出来的 `/home/<用户>/ros2/...` 是仓库内部默认值,
|
||||||
|
# 换站点换的也是仓库位置,不算站点数据(工具说的假错会让人不再信它,R5.3)。
|
||||||
|
LOCAL_VALUES = (LOCAL_SN_LEFT, LOCAL_SN_RIGHT, LOCAL_CAM_LEFT, LOCAL_CAM_RIGHT,
|
||||||
|
*LOCAL_IPS)
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# 一份虚构站点的 site.yaml:SN / IP / 相机序列号全都不一样,
|
||||||
|
# 侧别与 --arm 别名走 robots[].arm 与 robots[].label,而不是 SN 里的数字。
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
|
FAKE_SITE = {
|
||||||
|
"robot": {
|
||||||
|
"sn": "Rizon4-777001",
|
||||||
|
"tool_name": "FakeToolA",
|
||||||
|
"robots": [
|
||||||
|
{
|
||||||
|
"sn": "Rizon4-777001",
|
||||||
|
"arm": "left",
|
||||||
|
"label": "左臂01",
|
||||||
|
"ip": "10.9.9.31",
|
||||||
|
"pc_ip": "10.9.9.30",
|
||||||
|
"iface": "eth9",
|
||||||
|
"camera": {"name": "cam_a", "serial": "99900001"},
|
||||||
|
"gripper": {"model": "FAKE-GRIP-A", "port": "/dev/ttyFAKE0"},
|
||||||
|
"named_poses_file": "/opt/fake/named_a.yaml",
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"sn": "Rizon4-777002",
|
||||||
|
"arm": "right",
|
||||||
|
"label": "右臂02",
|
||||||
|
"ip": "10.9.9.41",
|
||||||
|
"pc_ip": "10.9.9.30",
|
||||||
|
"iface": "eth9",
|
||||||
|
"camera": {"name": "cam_b", "serial": "99900002"},
|
||||||
|
"gripper": {"model": "FAKE-GRIP-B", "port": "/dev/ttyFAKE1"},
|
||||||
|
"named_poses_file": "/opt/fake/named_b.yaml",
|
||||||
|
},
|
||||||
|
],
|
||||||
|
},
|
||||||
|
"paths": {"yolo_weights": "/opt/fake/weights.engine"},
|
||||||
|
"cameras": {
|
||||||
|
"d405_left": {"serial": "99900001"},
|
||||||
|
"d405_right": {"serial": "99900002"},
|
||||||
|
"d435": {"serial": "99900003"},
|
||||||
|
},
|
||||||
|
"handeye": {"mode": "eye_in_hand"},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def _flat(monkeypatch, sn: str | None = None, site: dict | None = None) -> dict:
|
||||||
|
"""用虚构站点配置跑一遍 flatten(),返回导出的环境字典。"""
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml") # 防止误读真配置
|
||||||
|
if sn is None:
|
||||||
|
monkeypatch.delenv("ROBOT_SN", raising=False)
|
||||||
|
monkeypatch.delenv("ROBOT_ARM", raising=False)
|
||||||
|
else:
|
||||||
|
monkeypatch.setenv("ROBOT_SN", sn)
|
||||||
|
return export_site_env.flatten(dict(site if site is not None else FAKE_SITE))
|
||||||
|
|
||||||
|
|
||||||
|
# 一份**最小**配置:只有 robots[].sn / arm,其余键一律不写。
|
||||||
|
# 这样所有 ``or <默认值>`` 的兜底分支都会被走到 —— 泄漏就藏在那些分支里。
|
||||||
|
# 注意它仍然要能跑完 flatten(),所以不能真的删到没法解析。
|
||||||
|
MINIMAL_SITE = {
|
||||||
|
"robot": {
|
||||||
|
"robots": [
|
||||||
|
{"sn": "Rizon4-777001", "arm": "left"},
|
||||||
|
{"sn": "Rizon4-777002", "arm": "right"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class TestNoLocalValuesLeak:
|
||||||
|
"""换站点后,导出结果里不许出现本机的任何设备标识。"""
|
||||||
|
|
||||||
|
def test_flat_env_has_no_local_values(self, monkeypatch):
|
||||||
|
flat = _flat(monkeypatch)
|
||||||
|
leaked = {
|
||||||
|
key: value
|
||||||
|
for key, value in flat.items()
|
||||||
|
if any(token in str(value) for token in LOCAL_VALUES)
|
||||||
|
}
|
||||||
|
assert not leaked, (
|
||||||
|
"这些 SITE_* 在 site.yaml 缺键时兜底成了本机站点值,"
|
||||||
|
"会让程序静默操作另一台设备:\n"
|
||||||
|
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_flat_env_has_no_local_values_for_right_arm(self, monkeypatch):
|
||||||
|
flat = _flat(monkeypatch, sn="Rizon4-777002")
|
||||||
|
leaked = {
|
||||||
|
key: value
|
||||||
|
for key, value in flat.items()
|
||||||
|
if any(token in str(value) for token in LOCAL_VALUES)
|
||||||
|
}
|
||||||
|
assert not leaked, (
|
||||||
|
"切换到虚构站的右臂后仍有本机值残留:\n"
|
||||||
|
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_minimal_config_exercises_every_fallback(self, monkeypatch):
|
||||||
|
"""配置里只有 SN 和侧别时,兜底值也不许是本机设备值。
|
||||||
|
|
||||||
|
这条是上一条的加强版:完整配置会把 ``or <默认值>`` 短路掉,
|
||||||
|
所以泄漏只有用**最小配置**才暴露得出来。
|
||||||
|
"""
|
||||||
|
flat = _flat(monkeypatch, sn="Rizon4-777001", site=MINIMAL_SITE)
|
||||||
|
leaked = {
|
||||||
|
key: value
|
||||||
|
for key, value in flat.items()
|
||||||
|
if any(token in str(value) for token in LOCAL_VALUES)
|
||||||
|
}
|
||||||
|
assert not leaked, (
|
||||||
|
"最小配置(只给 SN/侧别)下,这些 SITE_* 兜底成了本机站点值:\n"
|
||||||
|
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_no_local_serial_fallback_without_cameras_section(self, monkeypatch):
|
||||||
|
"""``cameras:`` 整段缺失时,序列号必须为空,不能补一个真实序列号。"""
|
||||||
|
flat = _flat(monkeypatch, sn="Rizon4-777001", site=MINIMAL_SITE)
|
||||||
|
for key in ("SITE_D405_LEFT_SERIAL", "SITE_D405_RIGHT_SERIAL",
|
||||||
|
"SITE_D435_SERIAL"):
|
||||||
|
value = str(flat.get(key) or "")
|
||||||
|
assert value == "", (
|
||||||
|
f"{key} 应当是空(配置里没写),实际兜底成了 {value!r} —— "
|
||||||
|
"这会让程序悄悄打开别人的相机"
|
||||||
|
)
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("builder", ["gripper_overrides", "perception_overrides",
|
||||||
|
"servo_overrides"])
|
||||||
|
def test_ros_param_dicts_have_no_local_values(self, monkeypatch, builder):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
monkeypatch.delenv("ROBOT_SN", raising=False)
|
||||||
|
monkeypatch.delenv("ROBOT_ARM", raising=False)
|
||||||
|
params = getattr(export_site_env, builder)(dict(FAKE_SITE))
|
||||||
|
leaked = {
|
||||||
|
key: value
|
||||||
|
for key, value in params.items()
|
||||||
|
if any(token in str(value) for token in LOCAL_VALUES)
|
||||||
|
}
|
||||||
|
assert not leaked, (
|
||||||
|
f"{builder}() 返回的 ROS 参数里带上了本机站点值:\n"
|
||||||
|
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_active_arm_switches_camera_with_the_arm(self, monkeypatch):
|
||||||
|
"""换臂必须换相机序列号(这是 89/91 不能混用的核心)。"""
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
monkeypatch.setenv("ROBOT_SN", "Rizon4-777001")
|
||||||
|
left = export_site_env.flatten(dict(FAKE_SITE))
|
||||||
|
monkeypatch.setenv("ROBOT_SN", "Rizon4-777002")
|
||||||
|
right = export_site_env.flatten(dict(FAKE_SITE))
|
||||||
|
|
||||||
|
assert left["SITE_CAM_SERIAL"] == "99900001"
|
||||||
|
assert right["SITE_CAM_SERIAL"] == "99900002"
|
||||||
|
assert left["SITE_CAM_NAME"] != right["SITE_CAM_NAME"]
|
||||||
|
|
||||||
|
|
||||||
|
class TestArmAliasResolution:
|
||||||
|
"""--arm 的别名解析必须**来自 robots[]**,不能来自写死的 SN 表。"""
|
||||||
|
|
||||||
|
def test_resolves_by_side_word(self, monkeypatch):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "left") == "Rizon4-777001"
|
||||||
|
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "right") == "Rizon4-777002"
|
||||||
|
|
||||||
|
def test_resolves_by_chinese_label(self, monkeypatch):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "左臂01") == "Rizon4-777001"
|
||||||
|
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "右臂02") == "Rizon4-777002"
|
||||||
|
|
||||||
|
def test_resolves_by_full_sn(self, monkeypatch):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "Rizon4-777002") == "Rizon4-777002"
|
||||||
|
|
||||||
|
def test_site_alias_scheme_still_works(self, monkeypatch):
|
||||||
|
"""真实站点仍然要支持 --arm 89|91(由 SN 末两位派生,不是查表)。"""
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
site = {
|
||||||
|
"robot": {
|
||||||
|
"robots": [
|
||||||
|
{"sn": "Rizon4-063989", "arm": "left"},
|
||||||
|
{"sn": "Rizon4-063991", "arm": "right"},
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
assert export_site_env.normalize_robot_sn(site, "89") == "Rizon4-063989"
|
||||||
|
assert export_site_env.normalize_robot_sn(site, "91") == "Rizon4-063991"
|
||||||
|
|
||||||
|
|
||||||
|
class TestUnknownArmFailsLoudly:
|
||||||
|
"""给了一个匹配不上的臂别名,必须在启动时就报错,不能静默用错臂。"""
|
||||||
|
|
||||||
|
def test_unknown_numeric_alias_is_rejected(self, monkeypatch):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
with pytest.raises((ValueError, SystemExit)) as exc:
|
||||||
|
export_site_env.resolve_robot(dict(FAKE_SITE), sn="89")
|
||||||
|
assert "89" in str(exc.value)
|
||||||
|
|
||||||
|
def test_unknown_side_word_is_rejected(self, monkeypatch):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
with pytest.raises((ValueError, SystemExit)):
|
||||||
|
export_site_env.resolve_robot(dict(FAKE_SITE), sn="middle")
|
||||||
|
|
||||||
|
def test_error_message_lists_available_arms(self, monkeypatch):
|
||||||
|
"""报错必须告诉人有哪些臂可选 —— 否则使用者只能去读源码。"""
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
with pytest.raises((ValueError, SystemExit)) as exc:
|
||||||
|
export_site_env.resolve_robot(dict(FAKE_SITE), sn="89")
|
||||||
|
message = str(exc.value)
|
||||||
|
assert "Rizon4-777001" in message
|
||||||
|
assert "Rizon4-777002" in message
|
||||||
|
|
||||||
|
|
||||||
|
class TestDerivedPathsFollowTheActiveArm:
|
||||||
|
"""派生路径(手眼文件等)要跟着当前臂走,不能钉死某一台。"""
|
||||||
|
|
||||||
|
def test_handeye_filenames_derived_from_sn(self, monkeypatch):
|
||||||
|
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||||
|
monkeypatch.setenv("ROBOT_SN", "Rizon4-777001")
|
||||||
|
left = export_site_env.flatten(dict(FAKE_SITE))
|
||||||
|
monkeypatch.setenv("ROBOT_SN", "Rizon4-777002")
|
||||||
|
right = export_site_env.flatten(dict(FAKE_SITE))
|
||||||
|
|
||||||
|
assert left["SITE_HANDEYE_FILE"] != right["SITE_HANDEYE_FILE"], (
|
||||||
|
"两台臂的手眼文件路径不能是同一个"
|
||||||
|
)
|
||||||
|
assert left["SITE_HANDEYE_WAYPOINTS"] != right["SITE_HANDEYE_WAYPOINTS"]
|
||||||
|
assert left["SITE_HANDEYE_WORKDIR"] != right["SITE_HANDEYE_WORKDIR"]
|
||||||
@@ -1,9 +1,8 @@
|
|||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||||
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
from launch.substitutions import Command, LaunchConfiguration
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
from launch_ros.parameter_descriptions import ParameterValue
|
from launch_ros.parameter_descriptions import ParameterValue
|
||||||
from launch_ros.substitutions import FindPackageShare
|
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
import os
|
import os
|
||||||
import yaml
|
import yaml
|
||||||
@@ -16,6 +15,8 @@ def _load_yaml(path: str):
|
|||||||
|
|
||||||
def _launch_setup(context, *args, **kwargs):
|
def _launch_setup(context, *args, **kwargs):
|
||||||
use_fake = LaunchConfiguration('use_fake_hardware').perform(context)
|
use_fake = LaunchConfiguration('use_fake_hardware').perform(context)
|
||||||
|
# 同 ros2_control.launch.py:这里以前也不传 robot_sn,靠 xacro 里写死的默认值。
|
||||||
|
robot_sn = LaunchConfiguration('robot_sn').perform(context)
|
||||||
|
|
||||||
desc_share = get_package_share_directory('arm_description')
|
desc_share = get_package_share_directory('arm_description')
|
||||||
moveit_share = get_package_share_directory('arm_moveit_config')
|
moveit_share = get_package_share_directory('arm_moveit_config')
|
||||||
@@ -24,7 +25,11 @@ def _launch_setup(context, *args, **kwargs):
|
|||||||
srdf_path = os.path.join(moveit_share, 'srdf', 'arm.srdf')
|
srdf_path = os.path.join(moveit_share, 'srdf', 'arm.srdf')
|
||||||
|
|
||||||
robot_description = ParameterValue(
|
robot_description = ParameterValue(
|
||||||
Command(['xacro ', urdf_path, ' use_fake_hardware:=', use_fake]),
|
Command([
|
||||||
|
'xacro ', urdf_path,
|
||||||
|
' robot_sn:=', robot_sn,
|
||||||
|
' use_fake_hardware:=', use_fake,
|
||||||
|
]),
|
||||||
value_type=str,
|
value_type=str,
|
||||||
)
|
)
|
||||||
with open(srdf_path, 'r', encoding='utf-8') as f:
|
with open(srdf_path, 'r', encoding='utf-8') as f:
|
||||||
@@ -70,5 +75,11 @@ def _launch_setup(context, *args, **kwargs):
|
|||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument('use_fake_hardware', default_value='true'),
|
DeclareLaunchArgument('use_fake_hardware', default_value='true'),
|
||||||
|
# 从环境取当前臂 SN,避免靠 xacro 里的写死默认值"蒙对"。
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'robot_sn',
|
||||||
|
default_value=os.environ.get('SITE_ROBOT_SN', '').strip(),
|
||||||
|
description='当前臂 SN;关节名会变成 {SN}_joint1..joint7(连字符保留)',
|
||||||
|
),
|
||||||
OpaqueFunction(function=_launch_setup),
|
OpaqueFunction(function=_launch_setup),
|
||||||
])
|
])
|
||||||
|
|||||||
Reference in New Issue
Block a user