配置：消灭代码里的硬编码臂 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
@@ -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())
|
||||
Reference in New Issue
Block a user