From f5ed44a98740a2cbe00d4e3eee17854ae3d220ce Mon Sep 17 00:00:00 2001 From: arm_dev Date: Thu, 24 Sep 2026 09:18:44 +0800 Subject: [PATCH] =?UTF-8?q?=E9=85=8D=E7=BD=AE=EF=BC=9A=E6=B6=88=E7=81=AD?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E9=87=8C=E7=9A=84=E7=A1=AC=E7=BC=96=E7=A0=81?= =?UTF-8?q?=E8=87=82=20SN=EF=BC=8C=E5=B9=B6=E7=BB=99"=E6=8D=A2=E8=87=82?= =?UTF-8?q?=E6=BC=8F=E6=94=B9"=E5=8A=A0=E4=B8=80=E6=8A=8A=E5=B0=BA?= =?UTF-8?q?=E5=AD=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## 为什么 "设备 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 --- config/site.example.yaml | 3 + docs/FRAMEWORK_RULES.md | 41 +++ docs/HANDBOOK.md | 33 ++ scripts/check_arm_prefixes.py | 214 +++++++++++++ scripts/check_robot_network.sh | 2 +- scripts/convert_flexiv_traj.py | 17 +- scripts/fetch_flexiv.sh | 4 +- scripts/go_home.sh | 4 +- scripts/lib/export_site_env.py | 254 +++++++++++---- scripts/lib/load_site_config.sh | 44 ++- scripts/rdk_drag_bridge.py | 5 +- scripts/rdk_switch_tool.sh | 2 +- scripts/run_all.sh | 5 +- scripts/run_gripper.sh | 2 +- scripts/run_handeye.sh | 16 +- scripts/run_real.sh | 2 +- scripts/run_stage2_locate.sh | 3 +- scripts/run_stage3_search.sh | 3 +- scripts/run_visual_servo_rt.sh | 2 +- scripts/test_rdk_python.sh | 2 +- .../arm_application/dual_gripper.py | 24 +- .../arm_application/dual_gripper_cli.py | 68 ++++- .../arm_application/goto_ee_plane.py | 12 +- .../arm_application/handeye_calibrate.py | 4 +- .../arm_application/handeye_teach.py | 3 +- .../arm_application/handeye_waypoints.py | 4 +- .../arm_application/run_joint_trajectory.py | 4 +- .../arm_application/servo_node.py | 11 +- .../arm_application/site_io.py | 40 +++ .../launch/visual_servo.launch.py | 18 +- src/arm_bringup/launch/real.launch.py | 4 +- src/arm_bringup/launch/sim.launch.py | 8 +- src/arm_bringup/launch/stack.launch.py | 4 +- src/arm_control/launch/mock_control.launch.py | 4 +- src/arm_control/launch/ros2_control.launch.py | 17 +- src/arm_description/launch/display.launch.py | 7 +- src/arm_description/urdf/arm.urdf.xacro | 6 +- .../test/test_check_arm_prefixes.py | 135 ++++++++ src/arm_interfaces/test/test_site_env.py | 289 ++++++++++++++++++ .../launch/move_group.launch.py | 17 +- 40 files changed, 1187 insertions(+), 150 deletions(-) create mode 100644 scripts/check_arm_prefixes.py create mode 100644 src/arm_interfaces/test/test_check_arm_prefixes.py create mode 100644 src/arm_interfaces/test/test_site_env.py diff --git a/config/site.example.yaml b/config/site.example.yaml index f9ae25c..90b6c3f 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -35,6 +35,9 @@ robot: pc_ip: "192.168.2.XXX" iface: "enp4s0" label: "左臂89" + # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务， + # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。 + tactile: true camera: name: d405_left # 话题 /d405_left/... 与 91 分开 serial: "<序列号>" # 左腕 D405（一直在用的那台） diff --git a/docs/FRAMEWORK_RULES.md b/docs/FRAMEWORK_RULES.md index 3d891a5..7d43d71 100644 --- a/docs/FRAMEWORK_RULES.md +++ b/docs/FRAMEWORK_RULES.md @@ -61,6 +61,44 @@ **禁止**在代码里硬编码站点值。真实反例：15 处硬编码的 `Rizon4-063989` （见 `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` 键必须二选一：**接线** 或 **明确告警** 声明了却没接线的键是最难查的一类问题：用户按文档改了 `site.yaml`，行为毫无变化， @@ -454,6 +492,7 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0 | 9 | 提交信息里有"为什么"和实测数据吗？ | R6.4 | | 10 | 我写进文档的命令，自己跑过一遍吗？ | R6.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 | | `pkill -f gz` 秒返回、什么都没停 | 它先匹配到调用它的 shell | R4.10 | | 换了台臂却连错机器 | 硬编码的 `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 | | 脚本报 `$'\r': 未找到命令`、`set: pipefail: 无效的选项名` | 脚本是 CRLF 行尾，`\r` 被当成标识符的一部分 | R6.6 | | `bash -n` 说没问题，真跑立刻死 | `bash -n` 对 CRLF 会漏报 —— 静态检查≠能用 | R6.6 | diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index 8dd9a72..390d7a3 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -254,6 +254,36 @@ ros2 run arm_interfaces device_check --list-brands # 看每类现在有哪 改完不算清楚，孔检测在仿真里会"根本不可能通过"，而看起来像算法不行。 详见[仿真手册 §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 改完必须做的一件事 **问自己：我故意弄坏它，哪个检查会红？** @@ -289,6 +319,7 @@ source scripts/sim_env.sh # 仿真环境（独立 con # —— 设备与配置 —— ros2 run arm_interfaces device_check # 设备契约 + site.yaml 必填项 bash scripts/show_site_config.sh # site.yaml 解析结果 +python3 scripts/check_arm_prefixes.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 | | 脚本报 `$'\r': 未找到命令` | 脚本是 CRLF 行尾 | R6.6 | | 改了参数不生效 | 改的是包内 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 | | 脚本说"已停止"，进程还在 | 进程模式表漏项 | R4.10 | | 孔检测在仿真里怎么都不过 | 图幅与真机不同，像素门槛失效 | 仿真手册 §4.3 | diff --git a/scripts/check_arm_prefixes.py b/scripts/check_arm_prefixes.py new file mode 100644 index 0000000..2b531c9 --- /dev/null +++ b/scripts/check_arm_prefixes.py @@ -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()) diff --git a/scripts/check_robot_network.sh b/scripts/check_robot_network.sh index 8a3dd93..5be173e 100755 --- a/scripts/check_robot_network.sh +++ b/scripts/check_robot_network.sh @@ -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() { diff --git a/scripts/convert_flexiv_traj.py b/scripts/convert_flexiv_traj.py index 6463a44..746b3b2 100755 --- a/scripts/convert_flexiv_traj.py +++ b/scripts/convert_flexiv_traj.py @@ -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() diff --git a/scripts/fetch_flexiv.sh b/scripts/fetch_flexiv.sh index 78b643a..19f8dca 100755 --- a/scripts/fetch_flexiv.sh +++ b/scripts/fetch_flexiv.sh @@ -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:= 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:= use_fake_hardware:=true load_gripper:=false EOF diff --git a/scripts/go_home.sh b/scripts/go_home.sh index b4624d5..df04eb6 100755 --- a/scripts/go_home.sh +++ b/scripts/go_home.sh @@ -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() diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py index 17ec653..42aa072 100644 --- a/scripts/lib/export_site_env.py +++ b/scripts/lib/export_site_env.py @@ -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() diff --git a/scripts/lib/load_site_config.sh b/scripts/lib/load_site_config.sh index 7d16562..567d107 100755 --- a/scripts/lib/load_site_config.sh +++ b/scripts/lib/load_site_config.sh @@ -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 diff --git a/scripts/rdk_drag_bridge.py b/scripts/rdk_drag_bridge.py index 7d30008..e6e2004 100755 --- a/scripts/rdk_drag_bridge.py +++ b/scripts/rdk_drag_bridge.py @@ -25,7 +25,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。 行协议（JSON 只写 stdout；RDK 自身日志走 stderr） ------------------------------------------------- {"cmd":"ping"} - {"cmd":"connect","sn":"Rizon4-063989","iface":"enp4s0"} + {"cmd":"connect","sn":"","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 diff --git a/scripts/rdk_switch_tool.sh b/scripts/rdk_switch_tool.sh index 8462c30..d441d7c 100755 --- a/scripts/rdk_switch_tool.sh +++ b/scripts/rdk_switch_tool.sh @@ -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:-}" diff --git a/scripts/run_all.sh b/scripts/run_all.sh index 143d5b0..67e8d7d 100755 --- a/scripts/run_all.sh +++ b/scripts/run_all.sh @@ -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 diff --git a/scripts/run_gripper.sh b/scripts/run_gripper.sh index 3d07144..62f7e30 100755 --- a/scripts/run_gripper.sh +++ b/scripts/run_gripper.sh @@ -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}" diff --git a/scripts/run_handeye.sh b/scripts/run_handeye.sh index 0eb4adb..097765d 100755 --- a/scripts/run_handeye.sh +++ b/scripts/run_handeye.sh @@ -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}" diff --git a/scripts/run_real.sh b/scripts/run_real.sh index 2377679..e7342eb 100755 --- a/scripts/run_real.sh +++ b/scripts/run_real.sh @@ -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}" diff --git a/scripts/run_stage2_locate.sh b/scripts/run_stage2_locate.sh index 6b59b91..8642d92 100755 --- a/scripts/run_stage2_locate.sh +++ b/scripts/run_stage2_locate.sh @@ -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*） diff --git a/scripts/run_stage3_search.sh b/scripts/run_stage3_search.sh index eb63204..bbd7a96 100755 --- a/scripts/run_stage3_search.sh +++ b/scripts/run_stage3_search.sh @@ -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*） diff --git a/scripts/run_visual_servo_rt.sh b/scripts/run_visual_servo_rt.sh index ec416e5..af4710d 100755 --- a/scripts/run_visual_servo_rt.sh +++ b/scripts/run_visual_servo_rt.sh @@ -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}" diff --git a/scripts/test_rdk_python.sh b/scripts/test_rdk_python.sh index f4631d7..ba74eb9 100755 --- a/scripts/test_rdk_python.sh +++ b/scripts/test_rdk_python.sh @@ -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}" diff --git a/src/arm_application/arm_application/dual_gripper.py b/src/arm_application/arm_application/dual_gripper.py index 511831d..f576d94 100644 --- a/src/arm_application/arm_application/dual_gripper.py +++ b/src/arm_application/arm_application/dual_gripper.py @@ -648,10 +648,13 @@ class RotaryGripper(BaseGripper): @dataclass 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) 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)), ) return DualArmGripperConfig( - left_arm_sn=left.get("robot_sn", "Rizon4-063989"), - right_arm_sn=right.get("robot_sn", "Rizon4-063991"), left_gripper=left_cfg, right_gripper=right_cfg, ) 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 []) 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"), {}) - 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 {} parallel = g.get("parallel") 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), ) 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, right_gripper=right_cfg, ) diff --git a/src/arm_application/arm_application/dual_gripper_cli.py b/src/arm_application/arm_application/dual_gripper_cli.py index 1041fc5..f4f3402 100644 --- a/src/arm_application/arm_application/dual_gripper_cli.py +++ b/src/arm_application/arm_application/dual_gripper_cli.py @@ -31,15 +31,52 @@ def _print_status(label: str, status) -> None: ) -def _normalize_arm(raw: str) -> str: - t = (raw or "").strip().lower() - if t in ("89", "left", "l", "063989"): - return "left" - if t in ("91", "right", "r", "063991"): - return "right" - if t in ("both", "dual", "all"): +def _load_site_exporter(): + """复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。 + + 这里**不自己再抄一份** ``89 -> SN`` 的映射：抄一份就多一处会漂移的地方（M2）。 + """ + root = Path(__file__).resolve().parents[3] + 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" - 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: @@ -93,7 +130,18 @@ def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> Non def main() -> int: 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( "--test", default="status", @@ -114,7 +162,7 @@ def main() -> int: return 1 data = yaml.safe_load(site.read_text(encoding="utf-8")) or {} 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}") try: diff --git a/src/arm_application/arm_application/goto_ee_plane.py b/src/arm_application/arm_application/goto_ee_plane.py index d3dd39e..8dd0e3d 100644 --- a/src/arm_application/arm_application/goto_ee_plane.py +++ b/src/arm_application/arm_application/goto_ee_plane.py @@ -33,6 +33,12 @@ from arm_application.geom_ik import ( geometric_jacobian, quat_xyzw_to_R, ) +from arm_application.site_io import ( + base_frame_default, + ee_frame_default, + joint_prefix_default, +) + def _load_ee_cfg() -> dict: @@ -45,9 +51,9 @@ def _load_ee_cfg() -> dict: robot = resolve_robot(data) ee_z = float(servo.get("ee_z_m") or 0.24) return { - "prefix": str(robot.get("joint_prefix") or "Rizon4-063989_"), - "base": str(robot.get("base_frame") or "Rizon4-063989_base_link"), - "ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"), + "prefix": str(robot.get("joint_prefix") or joint_prefix_default()), + "base": str(robot.get("base_frame") or base_frame_default()), + "ee": str(robot.get("ee_frame") or ee_frame_default()), "z": ee_z, # 回 home 前先竖直升到这个 Z（避免撞到墙面/工装）； # 然后再回到 Flexiv 的命名初始位姿。 diff --git a/src/arm_application/arm_application/handeye_calibrate.py b/src/arm_application/arm_application/handeye_calibrate.py index d4fe70b..356dcea 100644 --- a/src/arm_application/arm_application/handeye_calibrate.py +++ b/src/arm_application/arm_application/handeye_calibrate.py @@ -45,6 +45,8 @@ from arm_application.handeye_realsense import ( query_realsense_color_intrinsics, resolve_realsense_topics, ) +from arm_application.site_io import joint_prefix_default + from arm_msgs.action import FollowJointTrajectory from arm_msgs.srv import GetArmState @@ -82,7 +84,7 @@ class HandeyeCalibrate(Node): super().__init__('handeye_calibrate') 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('velocity_scale', 0.10) self.declare_parameter('action_name', 'follow_joint_trajectory') diff --git a/src/arm_application/arm_application/handeye_teach.py b/src/arm_application/arm_application/handeye_teach.py index 245682c..a2ee476 100644 --- a/src/arm_application/arm_application/handeye_teach.py +++ b/src/arm_application/arm_application/handeye_teach.py @@ -26,7 +26,6 @@ rdk（软件 Free Drive）： from __future__ import annotations import os -import sys import threading import time from datetime import datetime, timezone @@ -249,7 +248,7 @@ class HandeyeTeach(Node): self.declare_parameter('height', 720) self.declare_parameter('fps', 15) 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._serial = _param_as_str(self, 'camera_serial', '') self.declare_parameter('auto_exposure', True) diff --git a/src/arm_application/arm_application/handeye_waypoints.py b/src/arm_application/arm_application/handeye_waypoints.py index ab1bd1e..a0cbaf4 100644 --- a/src/arm_application/arm_application/handeye_waypoints.py +++ b/src/arm_application/arm_application/handeye_waypoints.py @@ -36,6 +36,8 @@ from arm_application.handeye_realsense import ( is_realsense_source, resolve_realsense_topics, ) +from arm_application.site_io import joint_prefix_default + from arm_msgs.action import FollowJointTrajectory from arm_msgs.srv import GetArmState @@ -111,7 +113,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray: class HandeyeWaypoints(Node): def __init__(self) -> None: 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('velocity_scale', 0.12) self.declare_parameter('action_name', 'follow_joint_trajectory') diff --git a/src/arm_application/arm_application/run_joint_trajectory.py b/src/arm_application/arm_application/run_joint_trajectory.py index f0acd93..6ccad6d 100644 --- a/src/arm_application/arm_application/run_joint_trajectory.py +++ b/src/arm_application/arm_application/run_joint_trajectory.py @@ -4,12 +4,12 @@ from __future__ import annotations import time -from typing import List import rclpy from rclpy.action import ActionClient from rclpy.node import Node +from arm_application.site_io import joint_prefix_default from arm_msgs.action import FollowJointTrajectory DEFAULT_JOINTS = [ @@ -28,7 +28,7 @@ DEFAULT_POSITIONS = [ class RunJointTrajectory(Node): def __init__(self) -> None: 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('velocity_scale', 0.15) self.declare_parameter('action_name', 'follow_joint_trajectory') diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index 3e8dafe..ebf57bd 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -40,6 +40,11 @@ from arm_application.geom_ik import ( ) from arm_application.pid_controller import PidController from arm_application.kalman_filter import KalmanFilter3D +from arm_application.site_io import ( + base_frame_default, + ee_frame_default, + joint_prefix_default, +) DEFAULT_JOINTS = [ @@ -62,10 +67,10 @@ class ServoNode(Node): 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("base_frame", "Rizon4-063989_base_link") - self.declare_parameter("ee_frame", "Rizon4-063989_flange") + self.declare_parameter("base_frame", base_frame_default()) + self.declare_parameter("ee_frame", ee_frame_default()) self.declare_parameter("camera_frame", "d405_color_optical_frame") self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory") self.declare_parameter("point_topic", "visual_servo/filtered_point") diff --git a/src/arm_application/arm_application/site_io.py b/src/arm_application/arm_application/site_io.py index 05f2326..0e7f016 100644 --- a/src/arm_application/arm_application/site_io.py +++ b/src/arm_application/arm_application/site_io.py @@ -40,3 +40,43 @@ def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]: else: out[key] = val 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: + """当前臂的基座坐标系（``_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: + """当前臂的法兰坐标系（``_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 "" + diff --git a/src/arm_application/launch/visual_servo.launch.py b/src/arm_application/launch/visual_servo.launch.py index 4380a3e..a7928fe 100644 --- a/src/arm_application/launch/visual_servo.launch.py +++ b/src/arm_application/launch/visual_servo.launch.py @@ -117,9 +117,14 @@ def _launch_setup(context, *args, **kwargs): ee_frame = LaunchConfiguration("ee_frame").perform(context).strip() cam_frame = LaunchConfiguration("camera_frame").perform(context).strip() 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) - if not cam_frame or cam_frame == "d405_color_optical_frame": + # 同 ee_frame：用空串表示"没指定"，不拿具体坐标系名当哨兵。 + if not cam_frame: cam_frame = str( perc_over.get("camera_frame") or yolo.get("camera_frame") @@ -216,17 +221,20 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ + # 默认值从环境取（scripts/env.sh → load_site_config.sh 导出的 + # SITE_HANDEYE_FILE / SITE_EE_FRAME 已经是**当前臂**的值）。 + # 不写死某个臂的文件名/坐标系：写死的话换臂后这里就悄悄指向上一台。 DeclareLaunchArgument( "handeye_file", - default_value="/home/cc/ros2/handeye/results/89_eye_in_hand.yaml", + default_value=os.environ.get("SITE_HANDEYE_FILE", ""), ), DeclareLaunchArgument( "ee_frame", - default_value="Rizon4-063989_flange", + default_value=os.environ.get("SITE_EE_FRAME", ""), ), DeclareLaunchArgument( "camera_frame", - default_value="d405_color_optical_frame", + default_value=os.environ.get("SITE_CAMERA_FRAME", ""), ), DeclareLaunchArgument( "enable_control", diff --git a/src/arm_bringup/launch/real.launch.py b/src/arm_bringup/launch/real.launch.py index ba82e06..ab05017 100644 --- a/src/arm_bringup/launch/real.launch.py +++ b/src/arm_bringup/launch/real.launch.py @@ -9,7 +9,9 @@ import os # 现场机械臂 —— DH PGEA 是独立的 Modbus 节点（run_gripper.sh）。 # 保持 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): diff --git a/src/arm_bringup/launch/sim.launch.py b/src/arm_bringup/launch/sim.launch.py index ab82d1a..9961860 100644 --- a/src/arm_bringup/launch/sim.launch.py +++ b/src/arm_bringup/launch/sim.launch.py @@ -4,6 +4,7 @@ from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare +import os def generate_launch_description(): @@ -36,10 +37,13 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_rviz', default_value='true'), DeclareLaunchArgument('use_moveit', default_value='false'), + # 仿真**故意**不用真机的 SN 前缀：两边同时跑时，同名关节会让 TF 与 + # 控制器互相打架（症状是 RViz 里模型乱跳）。见 devices/sim/gazebo/README.md。 + # 所以默认值是仿真自己的前缀，而不是"把真机那台抄过来"。 DeclareLaunchArgument( 'robot_sn', - default_value='Rizon4-063989', - description='现场 SN；关节名会变成 Rizon4-063989_joint1..joint7', + default_value=os.environ.get('SITE_SIM_JOINT_PREFIX', 'sim'), + description='仿真关节前缀；关节名会变成 <前缀>_joint1..joint7（有意与真机不同名）', ), mock, moveit, diff --git a/src/arm_bringup/launch/stack.launch.py b/src/arm_bringup/launch/stack.launch.py index 278df13..7b39d13 100644 --- a/src/arm_bringup/launch/stack.launch.py +++ b/src/arm_bringup/launch/stack.launch.py @@ -8,7 +8,6 @@ import os from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction -from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 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 -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_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip() diff --git a/src/arm_control/launch/mock_control.launch.py b/src/arm_control/launch/mock_control.launch.py index 20a1ec6..69fd959 100644 --- a/src/arm_control/launch/mock_control.launch.py +++ b/src/arm_control/launch/mock_control.launch.py @@ -9,7 +9,9 @@ from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare # 现场机械臂。官方 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_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip() diff --git a/src/arm_control/launch/ros2_control.launch.py b/src/arm_control/launch/ros2_control.launch.py index 5c33848..569a561 100644 --- a/src/arm_control/launch/ros2_control.launch.py +++ b/src/arm_control/launch/ros2_control.launch.py @@ -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.actions import DeclareLaunchArgument from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution @@ -8,6 +13,7 @@ from launch_ros.substitutions import FindPackageShare def generate_launch_description(): use_fake = LaunchConfiguration('use_fake_hardware') + robot_sn = LaunchConfiguration('robot_sn') urdf_path = PathJoinSubstitution( [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro'] @@ -17,12 +23,21 @@ def generate_launch_description(): ) 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, ) return LaunchDescription([ 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( package='robot_state_publisher', diff --git a/src/arm_description/launch/display.launch.py b/src/arm_description/launch/display.launch.py index c433e64..e01fb43 100644 --- a/src/arm_description/launch/display.launch.py +++ b/src/arm_description/launch/display.launch.py @@ -5,6 +5,7 @@ from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitut from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare +import os def generate_launch_description(): @@ -34,7 +35,11 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_gui', 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('load_gripper', default_value='false'), diff --git a/src/arm_description/urdf/arm.urdf.xacro b/src/arm_description/urdf/arm.urdf.xacro index 7612415..f69f41a 100644 --- a/src/arm_description/urdf/arm.urdf.xacro +++ b/src/arm_description/urdf/arm.urdf.xacro @@ -6,7 +6,11 @@ - + + \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 diff --git a/src/arm_interfaces/test/test_site_env.py b/src/arm_interfaces/test/test_site_env.py new file mode 100644 index 0000000..0df03c7 --- /dev/null +++ b/src/arm_interfaces/test/test_site_env.py @@ -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"] diff --git a/src/arm_moveit_config/launch/move_group.launch.py b/src/arm_moveit_config/launch/move_group.launch.py index 485969e..6f453bb 100644 --- a/src/arm_moveit_config/launch/move_group.launch.py +++ b/src/arm_moveit_config/launch/move_group.launch.py @@ -1,9 +1,8 @@ from launch import LaunchDescription 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.parameter_descriptions import ParameterValue -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory import os import yaml @@ -16,6 +15,8 @@ def _load_yaml(path: str): def _launch_setup(context, *args, **kwargs): 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') 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') 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, ) with open(srdf_path, 'r', encoding='utf-8') as f: @@ -70,5 +75,11 @@ def _launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription([ 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), ])