配置:消灭代码里的硬编码臂 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
+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
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}}"
pick_iface() {
+9 -8
View File
@@ -5,6 +5,7 @@ from __future__ import annotations
import argparse
import math
import os
import re
import sys
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(
waypoints: list[dict[str, Any]],
*,
joint_prefix: str = 'Rizon4-063989_',
joint_prefix: str = '',
velocity_scale: float | None = None,
) -> str:
if velocity_scale is None:
velocity_scale = waypoints[0].get('velocity', 0.2) if waypoints else 0.2
times = [wp['time'] for wp in waypoints]
flat: list[float] = []
for wp in waypoints:
flat.extend(wp['positions'])
pos_line = ', '.join(f'{v:.6f}' for v in flat)
# 时间轴拉平成一维;位置在下面的循环里**逐段**写,顺便把该段名字放进行尾注释。
# (这里以前还先算了 pos_line / names 两个字符串再全部丢弃 —— 死代码,已删。)
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 = [
'# 由 Flexiv Elements .traj 自动转换而来(关节角:度 -> 弧度)',
'# ROS 参数:扁平的 waypoint_positions + waypoint_times(不使用嵌套字典)',
@@ -91,7 +88,11 @@ def main() -> int:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument('input', type=Path, help='Flexiv Elements 的 .traj 文件')
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='每段的最小秒数')
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):
ros2 launch arm_bringup real.launch.py
# 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:
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
+2 -2
View File
@@ -108,7 +108,7 @@ else:
m = dict(zip(box[0].name, box[0].position))
v = dict(zip(box[0].name, box[0].velocity)) if box[0].velocity else {}
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:
print(f" joint{i}: {m[k]:+.4f} qd={v.get(k, 0.0):+.4f}")
n.destroy_node(); rclpy.shutdown()
@@ -138,7 +138,7 @@ target = POSES.get(name)
if target is None:
raise SystemExit(f"unknown pose {name}")
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)]
rclpy.init()
+191 -63
View File
@@ -62,23 +62,121 @@ def list_robots(data: dict) -> list[dict]:
return out
# --arm 89|91|left|right 与 SN 别名。禁止把 89 的手眼/相机套到 91。
_ARM_ALIASES = {
"89": "Rizon4-063989",
"left": "Rizon4-063989",
"左": "Rizon4-063989",
"左臂": "Rizon4-063989",
"左臂89": "Rizon4-063989",
"91": "Rizon4-063991",
"right": "Rizon4-063991",
"右": "Rizon4-063991",
"右臂": "Rizon4-063991",
"右臂91": "Rizon4-063991",
# 侧别词 —— 这是**语言别名**,不是站点数据:任何站点都希望
# `--arm left` / `--arm 左` / `--arm 左臂` 能选中 `robots[]` 里 `arm: left` 的那台。
#
# 这里**绝不允许**出现具体 SN(`Rizon4-063989` 之类)。历史上这里是一张
# `89 -> Rizon4-063989` 的写死映射表:换一台机器人后 `--arm 89` 仍然会选中
# 上一台,而且不报错(R1.1)。现在 `89` 这个写法的来源是
# **这台机器 SN 的末两位**,由 `_arm_index()` 从 `robots[]` 现算出来。
_SIDE_WORDS = {
"left": "left",
"l": "left",
"左": "left",
"左臂": "left",
"right": "right",
"r": "right",
"右": "right",
"右臂": "right",
}
_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:
out = dict(base)
for k, v in overlay.items():
@@ -89,33 +187,39 @@ def _deep_merge(base: dict, overlay: dict) -> dict:
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:
"""当前生效的机械臂:环境变量 ``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 {})
catalog = list_robots(data)
want = normalize_robot_sn(
data,
sn or os.environ.get("ROBOT_SN") or os.environ.get("ROBOT_ARM") or robot.get("sn") or "",
raw = (
sn
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 = {}
for item in catalog:
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.update({k: v for k, v in match.items() if k != "robots"})
out["sn"] = want
if not str(out.get("arm") or "").strip():
if "063989" in want:
out["arm"] = "left"
elif "063991" in want:
out["arm"] = "right"
if not str(out.get("joint_prefix") or "").strip() and want:
# 侧别只认 robots[].arm。**不要**从 SN 里猜数字("063989 就是左臂")——
# 那是站点数据写进了代码,换个站点就会把左右搞反。
if not str(out.get("joint_prefix") or "").strip():
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"
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"
net = data.get("network") or {}
if not str(out.get("iface") or "").strip():
@@ -200,10 +301,19 @@ def flatten(data: dict) -> dict:
except Exception:
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 = {
"SITE_CONFIG": str(site_path()),
"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_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
"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_LABEL": str(robot.get("label") 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_ROBOT_CATALOG": ",".join(
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_HANDEYE_WAYPOINTS": str(
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(
paths.get("handeye_calibrate")
@@ -231,13 +345,14 @@ def flatten(data: dict) -> dict:
or str(_root() / "handeye" / "waypoints.yaml")
),
"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(
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 / 结果文件头里的实际板子一致。
# 这里原先是 11/16/0.011(一张早期用过、现已不存在的板),与 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_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_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_IMGSZ": str(int(yolo.get("imgsz") or 640)),
"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)
obj = str(yolo.get("object_frame") or "").strip()
if not obj or obj == "tracked_object":
arm = str((_robot or {}).get("arm") or "")
sn = str((_robot or {}).get("sn") or "")
if "063991" in sn or arm == "right":
obj = "tracked_object_91"
elif "063989" in sn or arm == "left":
obj = "tracked_object_89"
else:
obj = "tracked_object"
# 跟踪目标的坐标系按**臂号**派生(SN 末两位),两侧互不干扰。
# 以前是从 SN 里找 "063989"/"063991" 子串,换个站点就失去意义了。
short = arm_short_id(str((_robot or {}).get("sn") or ""))
obj = f"tracked_object_{short}" if short else "tracked_object"
return {
"weights": str(paths.get("yolo_weights") or ""),
"conf": float(yolo.get("conf") or 0.10),
@@ -466,7 +579,14 @@ def main() -> int:
os.environ["ROBOT_SN"] = str(args.sn)
path = Path(args.config).expanduser() if args.config else 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:
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
return 0
@@ -535,19 +655,27 @@ def main() -> int:
if isinstance(he_data, dict) and (
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(
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,
)
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(
" 警告:91 用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
" 警告:右臂用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
file=sys.stderr,
)
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(
" 提示:本臂是 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,
)
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
+37 -7
View File
@@ -2,19 +2,49 @@
# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。
# 用法(在其它脚本里):
# 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_EXPORTER="${_SITE_ROOT}/scripts/lib/export_site_env.py"
_SITE_FILE="${SITE_CONFIG:-${_SITE_ROOT}/config/site.yaml}"
# 供调用方判断"是没配置还是配错了"。0 = 正常加载;1 = 文件不存在;2 = 配置有误。
SITE_LOAD_STATUS=0
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
# shellcheck disable=SC1090
eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"
export SITE_CONFIG="${_SITE_FILE}"
# 把 89/91/left/right 别名解析成完整的 Flexiv SN,供 RDK 使用。
if [[ -n "${SITE_ROBOT_SN:-}" ]]; then
export ROBOT_SN="${SITE_ROBOT_SN}"
_site_env=""
if ! _site_env="$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"; then
echo "[site] ERROR: 解析 ${_SITE_FILE} 失败,见上面 export_site_env.py 的报错。" >&2
echo "[site] 修好后重试;不要带着空的 SITE_* 继续跑。" >&2
SITE_LOAD_STATUS=2
else
# 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
unset _SITE_ROOT _SITE_EXPORTER _SITE_FILE _site_env
+3 -2
View File
@@ -25,7 +25,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
行协议(JSON 只写 stdout;RDK 自身日志走 stderr)
-------------------------------------------------
{"cmd":"ping"}
{"cmd":"connect","sn":"Rizon4-063989","iface":"enp4s0"}
{"cmd":"connect","sn":"<site.yaml 的 robot.sn>","iface":"<robot.iface>"}
{"cmd":"enable"} # 使能 + 清故障(幂等)
{"cmd":"clear_fault"} # 显式清故障并使能
{"cmd":"switch_tool","name":"tool1"} # 切工具;name 空=只查询
@@ -41,6 +41,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
from __future__ import annotations
import json
import os
import sys
import threading
import time
@@ -222,7 +223,7 @@ def main() -> int:
reply({'ok': True, 'py': sys.version.split()[0]})
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 '')
robot = 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"
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:-}"
IFACE="${SITE_NET_IFACE:-}"
+4 -1
View File
@@ -151,7 +151,10 @@ _tactile_ok() {
}
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
echo "=== 触觉 ${TACTILE_TOPIC} 已在发,跳过拉起 ==="
else
+1 -1
View File
@@ -75,7 +75,7 @@ if [[ ${#CLI[@]} -gt 0 ]]; then
if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
;;
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[@]}"
fi
PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
+10 -6
View File
@@ -68,16 +68,20 @@ if [[ ! -d "${APP_CFG}" ]]; then
fi
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下(见 handeye/README.md)。
# 这些 SHE 值正常情况下由 site.yaml → export_site_env.py 提供;
# 下面只是 shell 侧的兜底,路径必须与 export_site_env.py 的默认值一致。
# 这些值全部由 site.yaml → export_site_env.py 提供,**包括文件名里的臂号**
# (臂号 = 该臂 SN 末两位,由 arm_short_id() 现算)。
#
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`:`_arm_id` 和 `HANDEYE_MODE`
# 在下面几行之后才算得出来,在这里拼会拼出 `results/_.yaml` 这种空文件名,
# 而症状是"标定写完了、用的时候读不到"。
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}"
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}"
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}"
# ---- --mode:切换 eye-in-hand / eye-to-hand ----
@@ -294,7 +298,7 @@ cmd_teach() {
fi
TEACH_ARGS=(
-p backend:="${backend}"
-p robot_sn:="${SITE_ROBOT_SN:-Rizon4-063989}"
-p robot_sn:="${SITE_ROBOT_SN:?手眼标定必须知道是哪台臂}"
-p joint_prefix:="${PREFIX}"
-p camera_source:="${CAM}"
-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:-}"
# `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}}"
NET_IFACE="${SITE_NET_IFACE:-}"
RDK_PYTHON="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"
+2 -1
View File
@@ -98,7 +98,8 @@ fi
# ---- 3) 触觉传感器服务(目前只有左臂 89 装了传感器;hover-only 不需要) ----
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) ==="
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker(占死 /dev/video*)
+2 -1
View File
@@ -79,7 +79,8 @@ fi
# ---- 2) 触觉传感器服务(三阶段的核心传感器,必须在线) ----
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) ==="
pkill -f 'lib/arm_application/tactile' 2>/dev/null || true
pkill orisys_tac_w 2>/dev/null || true # 清理孤儿 worker(占死 /dev/video*)
+1 -1
View File
@@ -70,7 +70,7 @@ if [[ "${CONTROL}" == true ]]; then
else
echo "=== launch perception + servo (observe only, no arm motion) ==="
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}"
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}"
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
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}}"
IFACE="${SITE_NET_IFACE:-}"
PY="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"