配置:消灭代码里的硬编码臂 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
@@ -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,
)
@@ -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:
@@ -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 的命名初始位姿。
@@ -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')
@@ -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)
@@ -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')
@@ -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')
@@ -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")
@@ -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:
"""当前臂的基座坐标系(``<SN>_base_link``),用作 ROS 参数默认值。"""
frame = os.environ.get("SITE_BASE_FRAME", "").strip()
if frame:
return frame
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
return f"{sn}_base_link" if sn else ""
def ee_frame_default() -> str:
"""当前臂的法兰坐标系(``<SN>_flange``),用作 ROS 参数默认值。"""
frame = os.environ.get("SITE_EE_FRAME", "").strip()
if frame:
return frame
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
return f"{sn}_flange" if sn else ""
@@ -117,9 +117,14 @@ def _launch_setup(context, *args, **kwargs):
ee_frame = LaunchConfiguration("ee_frame").perform(context).strip()
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",
+3 -1
View File
@@ -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):
+6 -2
View File
@@ -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,
+2 -2
View File
@@ -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()
@@ -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()
+16 -1
View File
@@ -1,3 +1,8 @@
# 现场机械臂 SN(由 scripts/env.sh → load_site_config.sh 从 site.yaml 导出)。
# 这个 launch 以前**根本不传 robot_sn**,靠 arm.urdf.xacro 里写死的默认值
# 正好等于本机 SN 才跑得起来;换台臂就会去驱动上一台臂的关节名。
import os
from launch import LaunchDescription
from launch.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',
+6 -1
View File
@@ -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'),
+5 -1
View File
@@ -6,7 +6,11 @@
<xacro:include filename="$(find flexiv_description)/urdf/common/rizon_robot.xacro"/>
<xacro:arg name="arm_prefix" default=""/>
<xacro:arg name="robot_sn" default="Rizon4-063989"/>
<!-- 必须由 launch 传入真实 SN(mock_control / ros2_control / display 都会传)。
这里刻意留一个**明显是占位符**的默认值,而不是某台臂的真实 SN:
默认成真实 SN 的话,忘记传参时会安静地去驱动上一台臂的关节名,
而 <prefix> 变成 UNSET_ARM_SN_ 至少能让控制器报错信息里带着它。 -->
<xacro:arg name="robot_sn" default="UNSET_ARM_SN"/>
<xacro:arg name="joint_limits_parameters_file"
default="$(find flexiv_description)/config/$(arg rizon_type)/joint_limits.yaml"/>
<xacro:arg name="kinematics_parameters_file"
@@ -0,0 +1,135 @@
"""``scripts/check_arm_prefixes.py`` 的自证测试(R5.2)。
为什么这个检查器必须有测试
--------------------------
它是"换臂时防止漏改一级配置"的唯一守卫。**守卫自己失效过一次**:
第一个版本的正则写成 ``\\bRizon\\d+-\\d{6}\\b``,而 Flexiv 的关节名是
``Rizon4-063989_joint1`` —— ``_joint1`` 的下划线属于 ``\\w``,所以结尾的
``\\b`` 不成立,SRDF 里 24 处硬编码**一处都没报出来**,脚本还打印
"OK:8 个配置文件都指向 Rizon4-063989"。
守卫的漏洞和它要防的漏洞长得一模一样,所以这里逐条钉住:
* 挑得出 ``SN_joint1`` 这种**后缀带下划线**的形态(就是上面那个 bug);
* 不会把 ``Rizon4-06398912`` 这种更长数字误判成长度正确的前缀;
* 混着两个 SN 的文件,只报**不属于当前臂**的那些,并带上行号;
* 读不出当前臂时**直接失败**,不输出"检查通过"。
"""
from __future__ import annotations
import importlib.util
from pathlib import Path
import pytest
_REPO = Path(__file__).resolve().parents[3]
_SCRIPT = _REPO / "scripts" / "check_arm_prefixes.py"
def _load():
"""按路径加载 ``check_arm_prefixes``(它在 scripts/ 下,不是可导入的包)。"""
if not _SCRIPT.is_file(): # pragma: no cover - 仓库被裁剪时的兜底
pytest.skip(f"找不到 {_SCRIPT}")
spec = importlib.util.spec_from_file_location("_check_arm_prefixes", _SCRIPT)
module = importlib.util.module_from_spec(spec)
assert spec and spec.loader
spec.loader.exec_module(module)
return module
@pytest.fixture(scope="module")
def mod():
return _load()
LEFT = "Rizon4-063989"
RIGHT = "Rizon4-063991"
class TestSnToken:
"""正则必须挑得出真实的关节名形态。"""
@pytest.mark.parametrize(
"text",
[
LEFT, # 裸 SN
f"{LEFT}_joint1", # ★ 就是踩过坑的形态
f"{LEFT}_base_link",
f"{LEFT}_flange",
f'joint_prefix: "{LEFT}_"',
f"- {LEFT}_joint7",
"Rizon10s-123456_joint3", # 别的型号
],
)
def test_matches_real_joint_names(self, mod, text: str):
found = mod._SN_TOKEN.findall(text)
assert found, f"应挑出 SN,但一个都没匹配到:{text!r}"
@pytest.mark.parametrize(
"text",
[
"Rizon4-0639891", # 多一位数字,不是合法的 6 位 SN 前缀
"Rizon4-06398", # 只有 5 位
"Rizon4_063989", # 下划线而不是连字符
"not a robot at all",
],
)
def test_rejects_non_sn_text(self, mod, text: str):
assert not mod._SN_TOKEN.findall(text), f"不该匹配:{text!r}"
class TestScan:
"""混着两台臂的 SN 时,只报不属于当前臂的那些。"""
def test_reports_only_other_arm(self, mod, tmp_path: Path):
f = tmp_path / "controllers.yaml"
f.write_text(
"\n".join(
[
f"# 站点机器人 {LEFT}",
"joints:",
f" - {LEFT}_joint1",
f" - {RIGHT}_joint2",
]
),
encoding="utf-8",
)
hits = mod._scan(f, want_sn=LEFT)
assert len(hits) == 1, f"只应报出 {RIGHT} 那一行,实际:{hits}"
assert hits[0].startswith("4:"), f"应带行号 4,实际:{hits[0]!r}"
assert RIGHT in hits[0]
def test_all_matching_is_clean(self, mod, tmp_path: Path):
f = tmp_path / "srdf.srdf"
f.write_text(f'<joint name="{LEFT}_joint1" value="0"/>\n', encoding="utf-8")
assert mod._scan(f, want_sn=LEFT) == []
def test_missing_file_is_reported_not_swallowed(self, mod, tmp_path: Path):
"""文件缺失也算不一致 —— 不能当成"没问题"跳过。"""
hits = mod._scan(tmp_path / "nope.yaml", want_sn=LEFT)
assert hits and "读不到文件" in hits[0]
class TestActivePrefixFailsLoudly:
"""读不出当前臂时必须失败,不能打印"检查通过"。"""
def test_missing_site_config_exits_nonzero(self, mod, monkeypatch, tmp_path: Path):
monkeypatch.setenv("SITE_CONFIG", str(tmp_path / "不存在.yaml"))
monkeypatch.delenv("ROBOT_SN", raising=False)
monkeypatch.delenv("ROBOT_ARM", raising=False)
with pytest.raises(SystemExit) as exc:
mod._active_prefix()
assert exc.value.code == 2, "缺配置应当是明确的失败退出码,不是 0"
def test_empty_sn_exits_nonzero(self, mod, monkeypatch, tmp_path: Path):
"""site.yaml 里 robot.sn 为空 → 报错,而不是兜底成某台臂。"""
site = tmp_path / "site.yaml"
site.write_text("robot:\n sn: ''\n robots: []\n", encoding="utf-8")
monkeypatch.setenv("SITE_CONFIG", str(site))
monkeypatch.delenv("ROBOT_SN", raising=False)
monkeypatch.delenv("ROBOT_ARM", raising=False)
with pytest.raises(SystemExit) as exc:
mod._active_prefix()
assert exc.value.code == 2
+289
View File
@@ -0,0 +1,289 @@
"""证明「换一份 site.yaml,导出的 SITE_* 里就不该残留任何本机站点值」。
这是 R1.1(禁止硬编码站点值)的机器化执行。与其用 grep 去数 ``Rizon4-063989``
出现了几次(数不清、还会误报注释和文档),不如直接问一个**更强**的问题:
给定一份完全不同的 site.yaml,``flatten()`` 导出的环境里
还会不会出现本机的 SN / IP / 相机序列号 / 用户路径?
只要有一处会,就说明那处的"默认值"其实是**写死的站点数据**。后果不是报错,
而是 site.yaml 一旦缺那个键,程序就**静默地去操作另一台机器**:
- ``SITE_ROBOT_SN`` 兜底成 ``Rizon4-063989`` → 配置漏了也照样连上,连的是谁看运气;
- ``SITE_D405_LEFT_SERIAL`` 兜底成某个真实序列号 → 换站点后悄悄打开别人的相机。
这类兜底比"没有默认值"危险得多(M1:不报错但结果是错的)。所以本测试的思路是
**用一份虚构站点的配置跑一遍,断言本机值一个都不许出现**。
真实工作区不存在时跳过(包可能被单独构建)。
"""
from __future__ import annotations
import sys
import pytest
from arm_interfaces import paths
# ---------------------------------------------------------------------------
# 被测模块在 scripts/lib/ 下(launch 文件也是把它加进 sys.path 再 import 的),
# 所以这里用同样的方式接进来。
# ---------------------------------------------------------------------------
_LIB = paths.ros2_root() / "scripts" / "lib"
if str(_LIB) not in sys.path:
sys.path.insert(0, str(_LIB))
pytestmark = pytest.mark.skipif(
not (_LIB / "export_site_env.py").is_file(),
reason="scripts/lib/export_site_env.py 不存在(包被单独构建),跳过",
)
export_site_env = pytest.importorskip(
"export_site_env", reason="需要 PyYAML 才能加载 site 配置"
)
# ---------------------------------------------------------------------------
# 本机(真实站点)的值 —— 这些**绝不允许**成为任何配置缺键时的兜底
# ---------------------------------------------------------------------------
LOCAL_SN_LEFT = "Rizon4-063989"
LOCAL_SN_RIGHT = "Rizon4-063991"
LOCAL_CAM_LEFT = "352122274408"
LOCAL_CAM_RIGHT = "260522274442"
LOCAL_IPS = ("192.168.2.101", "192.168.2.100", "192.168.2.10")
# 只放**设备身份**:SN、IP、相机序列号。不包含工作区绝对路径 ——
# `_root()` 派生出来的 `/home/<用户>/ros2/...` 是仓库内部默认值,
# 换站点换的也是仓库位置,不算站点数据(工具说的假错会让人不再信它,R5.3)。
LOCAL_VALUES = (LOCAL_SN_LEFT, LOCAL_SN_RIGHT, LOCAL_CAM_LEFT, LOCAL_CAM_RIGHT,
*LOCAL_IPS)
# ---------------------------------------------------------------------------
# 一份虚构站点的 site.yaml:SN / IP / 相机序列号全都不一样,
# 侧别与 --arm 别名走 robots[].arm 与 robots[].label,而不是 SN 里的数字。
# ---------------------------------------------------------------------------
FAKE_SITE = {
"robot": {
"sn": "Rizon4-777001",
"tool_name": "FakeToolA",
"robots": [
{
"sn": "Rizon4-777001",
"arm": "left",
"label": "左臂01",
"ip": "10.9.9.31",
"pc_ip": "10.9.9.30",
"iface": "eth9",
"camera": {"name": "cam_a", "serial": "99900001"},
"gripper": {"model": "FAKE-GRIP-A", "port": "/dev/ttyFAKE0"},
"named_poses_file": "/opt/fake/named_a.yaml",
},
{
"sn": "Rizon4-777002",
"arm": "right",
"label": "右臂02",
"ip": "10.9.9.41",
"pc_ip": "10.9.9.30",
"iface": "eth9",
"camera": {"name": "cam_b", "serial": "99900002"},
"gripper": {"model": "FAKE-GRIP-B", "port": "/dev/ttyFAKE1"},
"named_poses_file": "/opt/fake/named_b.yaml",
},
],
},
"paths": {"yolo_weights": "/opt/fake/weights.engine"},
"cameras": {
"d405_left": {"serial": "99900001"},
"d405_right": {"serial": "99900002"},
"d435": {"serial": "99900003"},
},
"handeye": {"mode": "eye_in_hand"},
}
def _flat(monkeypatch, sn: str | None = None, site: dict | None = None) -> dict:
"""用虚构站点配置跑一遍 flatten(),返回导出的环境字典。"""
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml") # 防止误读真配置
if sn is None:
monkeypatch.delenv("ROBOT_SN", raising=False)
monkeypatch.delenv("ROBOT_ARM", raising=False)
else:
monkeypatch.setenv("ROBOT_SN", sn)
return export_site_env.flatten(dict(site if site is not None else FAKE_SITE))
# 一份**最小**配置:只有 robots[].sn / arm,其余键一律不写。
# 这样所有 ``or <默认值>`` 的兜底分支都会被走到 —— 泄漏就藏在那些分支里。
# 注意它仍然要能跑完 flatten(),所以不能真的删到没法解析。
MINIMAL_SITE = {
"robot": {
"robots": [
{"sn": "Rizon4-777001", "arm": "left"},
{"sn": "Rizon4-777002", "arm": "right"},
]
},
}
class TestNoLocalValuesLeak:
"""换站点后,导出结果里不许出现本机的任何设备标识。"""
def test_flat_env_has_no_local_values(self, monkeypatch):
flat = _flat(monkeypatch)
leaked = {
key: value
for key, value in flat.items()
if any(token in str(value) for token in LOCAL_VALUES)
}
assert not leaked, (
"这些 SITE_* 在 site.yaml 缺键时兜底成了本机站点值,"
"会让程序静默操作另一台设备:\n"
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
)
def test_flat_env_has_no_local_values_for_right_arm(self, monkeypatch):
flat = _flat(monkeypatch, sn="Rizon4-777002")
leaked = {
key: value
for key, value in flat.items()
if any(token in str(value) for token in LOCAL_VALUES)
}
assert not leaked, (
"切换到虚构站的右臂后仍有本机值残留:\n"
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
)
def test_minimal_config_exercises_every_fallback(self, monkeypatch):
"""配置里只有 SN 和侧别时,兜底值也不许是本机设备值。
这条是上一条的加强版:完整配置会把 ``or <默认值>`` 短路掉,
所以泄漏只有用**最小配置**才暴露得出来。
"""
flat = _flat(monkeypatch, sn="Rizon4-777001", site=MINIMAL_SITE)
leaked = {
key: value
for key, value in flat.items()
if any(token in str(value) for token in LOCAL_VALUES)
}
assert not leaked, (
"最小配置(只给 SN/侧别)下,这些 SITE_* 兜底成了本机站点值:\n"
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
)
def test_no_local_serial_fallback_without_cameras_section(self, monkeypatch):
"""``cameras:`` 整段缺失时,序列号必须为空,不能补一个真实序列号。"""
flat = _flat(monkeypatch, sn="Rizon4-777001", site=MINIMAL_SITE)
for key in ("SITE_D405_LEFT_SERIAL", "SITE_D405_RIGHT_SERIAL",
"SITE_D435_SERIAL"):
value = str(flat.get(key) or "")
assert value == "", (
f"{key} 应当是空(配置里没写),实际兜底成了 {value!r} —— "
"这会让程序悄悄打开别人的相机"
)
@pytest.mark.parametrize("builder", ["gripper_overrides", "perception_overrides",
"servo_overrides"])
def test_ros_param_dicts_have_no_local_values(self, monkeypatch, builder):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
monkeypatch.delenv("ROBOT_SN", raising=False)
monkeypatch.delenv("ROBOT_ARM", raising=False)
params = getattr(export_site_env, builder)(dict(FAKE_SITE))
leaked = {
key: value
for key, value in params.items()
if any(token in str(value) for token in LOCAL_VALUES)
}
assert not leaked, (
f"{builder}() 返回的 ROS 参数里带上了本机站点值:\n"
+ "\n".join(f" {k} = {v!r}" for k, v in sorted(leaked.items()))
)
def test_active_arm_switches_camera_with_the_arm(self, monkeypatch):
"""换臂必须换相机序列号(这是 89/91 不能混用的核心)。"""
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
monkeypatch.setenv("ROBOT_SN", "Rizon4-777001")
left = export_site_env.flatten(dict(FAKE_SITE))
monkeypatch.setenv("ROBOT_SN", "Rizon4-777002")
right = export_site_env.flatten(dict(FAKE_SITE))
assert left["SITE_CAM_SERIAL"] == "99900001"
assert right["SITE_CAM_SERIAL"] == "99900002"
assert left["SITE_CAM_NAME"] != right["SITE_CAM_NAME"]
class TestArmAliasResolution:
"""--arm 的别名解析必须**来自 robots[]**,不能来自写死的 SN 表。"""
def test_resolves_by_side_word(self, monkeypatch):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "left") == "Rizon4-777001"
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "right") == "Rizon4-777002"
def test_resolves_by_chinese_label(self, monkeypatch):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "左臂01") == "Rizon4-777001"
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "右臂02") == "Rizon4-777002"
def test_resolves_by_full_sn(self, monkeypatch):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
assert export_site_env.normalize_robot_sn(dict(FAKE_SITE), "Rizon4-777002") == "Rizon4-777002"
def test_site_alias_scheme_still_works(self, monkeypatch):
"""真实站点仍然要支持 --arm 89|91(由 SN 末两位派生,不是查表)。"""
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
site = {
"robot": {
"robots": [
{"sn": "Rizon4-063989", "arm": "left"},
{"sn": "Rizon4-063991", "arm": "right"},
]
}
}
assert export_site_env.normalize_robot_sn(site, "89") == "Rizon4-063989"
assert export_site_env.normalize_robot_sn(site, "91") == "Rizon4-063991"
class TestUnknownArmFailsLoudly:
"""给了一个匹配不上的臂别名,必须在启动时就报错,不能静默用错臂。"""
def test_unknown_numeric_alias_is_rejected(self, monkeypatch):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
with pytest.raises((ValueError, SystemExit)) as exc:
export_site_env.resolve_robot(dict(FAKE_SITE), sn="89")
assert "89" in str(exc.value)
def test_unknown_side_word_is_rejected(self, monkeypatch):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
with pytest.raises((ValueError, SystemExit)):
export_site_env.resolve_robot(dict(FAKE_SITE), sn="middle")
def test_error_message_lists_available_arms(self, monkeypatch):
"""报错必须告诉人有哪些臂可选 —— 否则使用者只能去读源码。"""
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
with pytest.raises((ValueError, SystemExit)) as exc:
export_site_env.resolve_robot(dict(FAKE_SITE), sn="89")
message = str(exc.value)
assert "Rizon4-777001" in message
assert "Rizon4-777002" in message
class TestDerivedPathsFollowTheActiveArm:
"""派生路径(手眼文件等)要跟着当前臂走,不能钉死某一台。"""
def test_handeye_filenames_derived_from_sn(self, monkeypatch):
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
monkeypatch.setenv("ROBOT_SN", "Rizon4-777001")
left = export_site_env.flatten(dict(FAKE_SITE))
monkeypatch.setenv("ROBOT_SN", "Rizon4-777002")
right = export_site_env.flatten(dict(FAKE_SITE))
assert left["SITE_HANDEYE_FILE"] != right["SITE_HANDEYE_FILE"], (
"两台臂的手眼文件路径不能是同一个"
)
assert left["SITE_HANDEYE_WAYPOINTS"] != right["SITE_HANDEYE_WAYPOINTS"]
assert left["SITE_HANDEYE_WORKDIR"] != right["SITE_HANDEYE_WORKDIR"]
@@ -1,9 +1,8 @@
from launch import LaunchDescription
from launch.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),
])