配置:消灭代码里的硬编码臂 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:
arm_dev
2026-09-24 09:18:44 +08:00
parent fac2d5384d
commit f5ed44a987
40 changed files with 1187 additions and 150 deletions
+3
View File
@@ -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(一直在用的那台)
+41
View File
@@ -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 |
+33
View File
@@ -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 |
+214
View File
@@ -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())
+1 -1
View File
@@ -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() {
+9 -8
View File
@@ -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()
+2 -2
View File
@@ -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
View File
@@ -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()
+191 -63
View File
@@ -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,33 +187,39 @@ 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:
if str(item.get("sn") or "").strip() == want: if str(item.get("sn") or "").strip() == want:
@@ -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)
flat = flatten(data) try:
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()
+37 -7
View File
@@ -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 else
# shellcheck disable=SC1090 _site_env=""
eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")" if ! _site_env="$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"; then
export SITE_CONFIG="${_SITE_FILE}" echo "[site] ERROR: 解析 ${_SITE_FILE} 失败,见上面 export_site_env.py 的报错。" >&2
# 把 89/91/left/right 别名解析成完整的 Flexiv SN,供 RDK 使用。 echo "[site] 修好后重试;不要带着空的 SITE_* 继续跑。" >&2
if [[ -n "${SITE_ROBOT_SN:-}" ]]; then SITE_LOAD_STATUS=2
export ROBOT_SN="${SITE_ROBOT_SN}" else
# shellcheck disable=SC1090
eval "${_site_env}"
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 使用。
export ROBOT_SN="${SITE_ROBOT_SN}"
fi
fi fi
fi fi
unset _SITE_ROOT _SITE_EXPORTER _SITE_FILE _site_env
+3 -2
View File
@@ -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
+1 -1
View File
@@ -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
View File
@@ -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
+1 -1
View File
@@ -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
View File
@@ -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
View File
@@ -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}"
+2 -1
View File
@@ -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*)
+2 -1
View File
@@ -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*)
+1 -1
View File
@@ -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}"
+1 -1
View File
@@ -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",
+3 -1
View File
@@ -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):
+6 -2
View File
@@ -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,
+2 -2
View File
@@ -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()
+16 -1
View File
@@ -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',
+6 -1
View File
@@ -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'),
+5 -1
View File
@@ -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
+289
View File
@@ -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),
]) ])