ros_contorl/docs/HANDBOOK.md

新手手册 —— 从「这是干什么的」到「我能自己改」

这份文档记录什么事实 它不是 API 参考，也不是环境安装说明书。它回答三个问题：

1. 这台机器到底在干什么活（业务），代码里哪一段对应哪一步；
2. 我要改一个东西时，改哪个文件、用哪条命令验证；
3. 出问题时，先看哪份文档（不重复写，只给索引）。

怎么读：第 1 节读完你才知道后面在讲什么；第 3 节的决策表是日常最高频的一页； 第 5 节是排障入口。细节一律外链到权威文档，避免两份说法不一致（见 FRAMEWORK_RULES.md 的 M2）。

真值来源：本文所有结论都能用命令复现；凡本文说得含糊的，以 config/site.yaml、各 devices/*/manifest.yaml 和源码为准（R6.1）。

---

1. 这台机器在干什么

1.1 一句话

把一支螺纹通止规自动送进工件上的螺纹孔，旋入到指定深度，用"能不能旋进去、 旋入过程中的力怎么变"来判断这个孔的螺纹加工是否合格。

"通止规"是机械行业检验螺纹的标准量具：通端应当能顺利旋到底， 止端应该旋不进去（或只能进很少几圈）。人来测就是拿规往里旋、靠手感判断； 这个项目要做的是让机械臂拿着规自己旋，并把判断依据（力）记下来。

1.2 为什么要自动化

人手旋规的判断依赖老师傅手感，无法留证据、无法 24 小时连续测。 换成机械臂 + 视触觉传感器之后，每一次旋入都能留下力曲线， "合格 / 不合格"就有了可追溯的数据依据。

1.3 一次完整任务分几步

代码里用 stage1 / stage2 / stage3 三段串起来，一条命令跑完：

run_all.sh
  │
  ├─ stage1_track    视觉跟踪：D405 看住工件，法兰维持悬停高度跟着它走，
  │                  直到"锁定"（连续 N 帧稳定）→ 停跟踪、钉住当前关节
  │
  ├─ stage2_locate   拍照对孔：垂直朝下、降到拍摄高度、拍一张照，
  │                  YOLO 找工件外框 → Hough 找左右两个孔心 →
  │                  深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
  │                  移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
  │
  └─ stage3_search   柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
                     再十字微扫定心，最后在力限内下压（可选按螺距旋入）

三段之间在同一个进程里交接，所以相机、触觉、手眼 TF 只初始化一次。 盯日志就能知道跑到哪了 —— 三行关键输出：

[pipeline] === stage1_track ===      → 在等跟踪锁定
[pipeline] === stage2_locate ===     → 在拍照对孔
[pipeline] === stage3_search ===     → 触面 → ★ 入孔 → 轻压到位

日常入口（三个终端，必须同一个 --arm）：

bash ~/ros2/scripts/run_real.sh --arm 89                    # T1 控制栈，一直开着
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，一直开着
bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）

细节、参数、退出码见 HOLE_DETECT.md。

1.4 现在到哪一步了（如实标注，别信印象）

能力	状态	怎么自己确认
三段串联（跟踪 → 对孔 → 搜孔轻压）	✅ 真机跑通	run_all.sh --arm 89
按螺距旋入（螺纹规）	✅ 有参数，--depth/--pitch 控制	run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0
Orisys 双指触觉（法向 + 两路切向）	⚠️ 能读，但轴向映射尚未实测确认	见 TACTILE.md
Orisys 的深度图 / 力矢量图	⚠️ 消息结构与话题已在，真机值待确认	同上
D435 手外相机（S03 粗定位兜底）	❌ 还没做手眼标定，链路不可用	见 devices/cameras/README.md
自动启动全部服务（一条命令拉起所有进程）	❌ 还没做，目前靠三个终端手工起	见第 6 节待办
「规有没有被夹住」自检	❌ 还没做	见第 6 节待办
装配合格判定（通/止）	❌ 还没做。当前只输出几何与力的原始结果	/tmp/stage3_result.json
Gazebo 仿真	✅ 场景、相机、触觉、夹爪可跑	devices/sim/gazebo/README.md

这张表是承诺书：❌ 的别当成已具备。文档说"已就绪"而实际没有， 是本仓库最忌讳的那类问题（R6.1）。

---

2. 系统长什么样

2.1 三层，各有硬边界

┌─ 现场数值层 ─────────────────────────────────────────────┐
│  config/site.yaml          唯一权威：SN / IP / 串口 / 阈值 / 锁高 │
│  devices/registry.yaml     只用哪个品牌的驱动（不含任何数值）      │
└──────────────────────────────────────────────────────────┘
                          ↓ 启动时注入
┌─ 设备模块层 ─────────────────────────────────────────────┐
│  devices/<类别>/<品牌>/     manifest.yaml + driver/ + 厂商物料   │
│  换品牌 = 加一个目录 + 改 registry 一行，**不改应用代码**         │
└──────────────────────────────────────────────────────────┘
                          ↓ 框架接口（src/arm_interfaces 定义的契约）
┌─ 应用层 ─────────────────────────────────────────────────┐
│  src/arm_application/    感知 / 伺服 / 手眼 / 夹爪 / 入孔 / 触觉  │
│  src/arm_control/        命名位姿、状态、轨迹适配                 │
│  src/arm_bringup/        launch 总装（real | mock | sim）        │
└──────────────────────────────────────────────────────────┘

唯一允许的配置入口是 config/site.yaml。 包内 config/*.yaml 只是缺省值， 改了会被 site 覆盖 —— 这是新手最常踩的一个坑（见 3.2）。

两块硬资源是独占的，同时开会互相咬：

资源	谁占着	冲突表现
RDK（机器人控制接口）	run_real.sh、run_handeye.sh teach	后启动的连不上 / 报错
D405 USB	相机节点、示教、标定	后启动的拿不到图

所以 run_handeye.sh 会先停掉旧相机；go_home.sh 会先杀掉 perception/servo 再回零，否则两条轨迹抢同一个臂。

2.2 从像素到关节：一条链路看懂全栈

D405        YOLO              手眼 T              伺服             ros2_control
出图   →   找工件框   →   像素+深度 →  相机系 3D →  法兰系  →   基座系  →  关节角
                                                              ↓
                                              /visual_servo/filtered_point
                                                        100 Hz

链路里每一步都有一个"口径"，写错任何一处都不会报错，只会让点偏掉：

口径	写错的表现	规矩
深度是米还是毫米	偏 1000 倍	R2.3
frame_id 是光学系还是相机本体系	点转到别处，但 tf2 查得到	R2.2
rpy 复合顺序	相机背对目标，只看得见地面	R2.4
内参从哪来	偏半米量级，但单目预览完全正常	R2.5
手眼 T_4x4 是哪台臂的	始终差几厘米，很难怀疑到标定值上	第 1 节末尾

这些坑全部在 FRAMEWORK_RULES.md 第 8 节有症状表。

2.3 三条设计取舍（理解了才不会误改）

1. 仿真与真机共用同一套参数。 所以你在仿真里调的悬停高、阈值，搬到真机直接生效； 代价是仿真里不能有"仿真专用参数"，只有物理引擎自己的参数（重力、步长）放 sim.*。
2. 设备模块里不许出现现场数值。 数值只有 site.yaml 一个来源。 这样换一台臂就是改 site.yaml，而不是满仓库找硬编码。
3. 校验必须能真的失败。 故意弄坏它、检查会红，才算校验； 不然就是"把没检查伪装成检查通过"（M1）。

---

3. 我要改东西，改哪里

3.1 ★ 决策表（先查这张，再动手）

我想改…	改这里	验证命令	细节
换机械臂 / 换臂号	site.yaml → robot.sn，或所有终端加同一 --arm	scripts/show_site_config.sh	SITE_CONFIG.md
悬停高 / 锁高 / 伺服参数	site.yaml → robots[].servo.*	重启第三终端即可	TRACKING.md
夹爪速度 / 力 / 行程	site.yaml → gripper:	run_gripper.sh --arm 89 --cli open	GRIPPER.md
入孔检测算法参数	site.yaml → hole_detect:	run_hole_detect.sh --arm 89 --preview	HOLE_DETECT.md
触觉灵敏度	site.yaml → hole_detect.stage3: 的 *_delta	看启动日志打印的派生阈值	TACTILE.md
相机曝光 / 图幅 / 视场	site.yaml → cameras:（真机）	list_realsense.sh	SITE_CONFIG.md
仿真场景里放什么、放哪	devices/sim/gazebo/config/scene.yaml	python3 scripts/check_sim_scene.py	仿真手册 §4.1
仿真里开合夹爪（开口 mm）	服务 /gripper/{open,close,move}，与真机同一套	python3 scripts/check_sim_gripper.py	仿真手册 §4.4
仿真相机视场/图幅	仿真手册 §4.3，三处要同时改	python3 scripts/check_sim_cameras.py	同上 §4.3
算法权重（换模型）	models/<分组>/ + site.yaml 的路径	scripts/weights.sh list	models/README.md
接一个新品牌设备	devices/<类别>/<品牌>/ + registry.yaml 一行	ros2 run arm_interfaces device_check	devices/README.md
命名位姿（回零点等）	src/arm_control/config/named_poses_89.yaml / _91.yaml	go_home.sh --arm 89	INTERFACES.md
框架规矩本身	docs/FRAMEWORK_RULES.md，同时改 .cursor/rules/	python3 scripts/check_text_only.py	R6.1

3.2 改一个现场数值（最高频）

唯一入口是 config/site.yaml。 不要改 src/arm_application/config/*.yaml —— 那只是缺省值，会被 site 覆盖，改了不生效而且不报错（典型静默失效）。

cp config/site.example.yaml config/site.yaml   # 只做一次（site.yaml 不进 git）
vim config/site.yaml
bash scripts/show_site_config.sh               # 看解析结果对不对
# 重启对应终端

生效范围不一样，别白改：

改了什么	要重启什么
锁高 / YOLO / 伺服参数	跑伺服的那一个终端
相机分辨率 / 曝光	相机那个终端
夹爪参数	夹爪进程
设备选型（registry.yaml）	对应的启动脚本

3.3 加一个场景物体（仿真）

三步，只改一处配置文件：

1. 在 devices/sim/gazebo/models/<物体名>/ 下建 model.sdf + model.config （建模要求见仿真手册 §2：原点必须落在底面中心， 碰撞体用简单几何，质量与惯性别省）。
2. 在 devices/sim/gazebo/config/scene.yaml 的 objects: 里加一条（位姿、是否 static）。
3. 校验：

source scripts/sim_env.sh
python3 scripts/check_sim_scene.py     # 悬空 / 穿模 / 出界 / 互相嵌入 / 像素几何

3.4 在仿真里开合夹爪

服务名与真机一致，所以脚本两边通用：

source scripts/sim_env.sh
ros2 service call /gripper/move arm_msgs/srv/GripperExecute \
    "{command: {command: move, position: 20.0}}"   # 开口 20 mm（真的 20 mm）
ros2 topic echo /gripper/status --once             # 开口 / 是否夹住 / 是否在动
python3 scripts/check_sim_gripper.py               # ★ 验收：拿两指尖的实际间距说话

position 是开口毫米，仿真里由 URDF 正运动学反解保证是真的毫米 （上游的线性 mimic 近似会让"开 20 mm"变成 21.75 mm，抓不住工件 —— 见仿真手册 §4.4）。

如果启动日志说行程是 99.93 mm：那是对的，是实测值，不是名义的 100.0。

3.5 加一个算法权重

放进去 → 打标签 → 改路径 → 体检。TensorRT 转换也在这条链上：

bash scripts/weights.sh convert <文件> --name <模型名>   # PT→ONNX→TensorRT，自动改名归位
bash scripts/weights.sh labels <模型名> --set "工件,通止规"
bash scripts/weights.sh doctor --check-engines          # 查版本不匹配这类静默失效

布局是 models/<分组>/<模型名>.{pt,onnx,engine} + 旁挂 <模型名>.labels.yaml。 二进制不进 git，标签文件进。规则见 models/README.md。

3.6 接一个新品牌设备

四步（完整说明在 devices/README.md，这里只给顺序）：

1. 把厂商物料丢进 devices/<类别>/<品牌>/（格式与命名按该类别 README.md，必须照做）。
2. 复制同类别的 manifest.yaml，改 id / label / driver.package / config_schema。
3. 在 driver/ 下实现框架接口契约（见 src/arm_interfaces/）。 这一步躲不掉 —— 厂商给的是 C++/Python 库而不是 ROS 2 驱动时， 外面永远需要一层薄适配器；框架能做的是把它写到最薄（只翻译 API，不含业务逻辑）。
4. 改 devices/registry.yaml 一行。

验：

ros2 run arm_interfaces device_check                    # 契约校验
ros2 run arm_interfaces device_check --list-brands      # 看每类现在有哪些可选

校验会读 registry.yaml → 各 manifest.yaml → site.yaml，检查三件事： 选中的品牌目录在不在、assets 里声明为必需的物料文件在不在、 required: true 的配置键在 site.yaml 里有没有值。 任何一项不满足立即报错并指出该去哪个文件补什么，不静默降级。

3.7 改相机分辨率（最容易漏改）

真机改 site.yaml 就行。仿真要改三处，因为像素门槛是绑在图幅上的 —— 改完不算清楚，孔检测在仿真里会"根本不可能通过"，而看起来像算法不行。 详见仿真手册 §4.3。

3.7.1 换一台机械臂（最容易漏改，而且很难查）

site.yaml 里写的是哪台臂，但有一批一级配置文件里写死了臂的关节名 （Flexiv 官方把 SN 编进关节名：Rizon4-063989_joint1）。这些文件必须跟着改：

python3 scripts/check_arm_prefixes.py          # 逐条列出哪个文件哪一行对不上，并给修法
python3 scripts/check_arm_prefixes.py --list   # 只看当前臂的应然前缀

漏改的症状有两种，都不像"配置错了"：

• 控制器报 Joint 'Rizon4-063989_joint1' not found（这条还算好查）；
• URDF 换了名字、srdf/arm.srdf 没换 → MoveIt 规划成功但机械臂不动。

换臂四步（少一步就出上面那种症状）：

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 的 R1.1.1 表。

排查提示：ROBOT_SN / ROBOT_ARM 环境变量会覆盖 site.yaml 的 robot.sn。 "明明改了配置却没生效"时先 echo $ROBOT_SN，或看 --list 打印的"来源"。

3.8 改完必须做的一件事

问自己：我故意弄坏它，哪个检查会红？

如果答不上来，说明这个改动没有任何东西在守着 —— 出问题只会以"结果是错的"形式出现。 这条写在 FRAMEWORK_RULES.md 第 7 节的改动前检查清单里， 逐条过一遍只要两分钟。

另外：只改注释/文档时，跑 python3 scripts/check_text_only.py 自证没碰逻辑（R6.5）。

---

4. 怎么验

4.1 三层，只有后两层算验收

层	判据	够不够
1	不报错、有话题、TF 查得到	❌ 不算验收（R4.9）
2	物理量落在已知真值上（深度反投影命中已知平面、内参与公式吻合）	✅ 模块级
3	任务真的成功（跑出结果、力曲线符合预期）	✅ 端到端

第 1 层是陷阱：仿真里相机背对目标照样出图、create 打印成功但模型没进 Gazebo、 夹爪控制器 active 但手指不动。这些全都不报错。

4.2 命令清单

# —— 环境 ——
source scripts/env.sh                                # 真机环境
source scripts/sim_env.sh                            # 仿真环境（独立 conda）

# —— 设备与配置 ——
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                 # 深度反投影命中已知平面才算过
python3 scripts/check_sim_scene.py                   # 摆放 / 像素门槛漂移 / 成像几何 / 可见性
python3 scripts/check_sim_gripper.py                 # 夹爪：命令开口 mm == 两指实际张开 mm

# —— 只改文字时自证 ——
python3 scripts/check_text_only.py

# —— 权重体检 ——
bash scripts/weights.sh doctor --check-engines

4.3 单元测试怎么跑

每个功能包都有自己的测试，改完对应包就跑一下。当前 pytest 收集 608 项。

source scripts/env.sh                                # 或 sim_env.sh

# 逐包跑（跳过需要厂商 flexivrdk 包的 flexiv_rdk，那个环境里没有）
for d in src/*/test; do
  case "$d" in src/flexiv_rdk/*) continue;; esac
  printf '%-28s ' "$d"; python3 -m pytest "$d" -q -p no:cacheprovider 2>&1 | tail -1
done

# 只跑一个包
python3 -m pytest src/arm_sim_gazebo/test -q

改动前后对比数量是最省事的回归判据 —— 现在应该是（pytest 收集数，共 608 项）：

包	测试数	覆盖什么
arm_application	233	纯逻辑：滤波器、解析器、几何、参数合并
arm_sim_gazebo	236	场景校验、URDF 归一化、相机内参、触觉适配
arm_driver_flexiv	69	驱动适配、下压控制、超时
arm_interfaces	46	路径解析、清单校验、配置守卫
arm_kinematics	24	正逆运动学

src/flexiv_rdk/test 需要厂商的 flexivrdk Python 包，仿真环境里没有， 跑不过属正常，与我们的改动无关。

4.4 验收工具自己也会失效

这是本仓库最贵的教训，单独写一节：

• 校验代码可能永远通过 —— 因为 sdformat 会用 __default__ / __default_topic__ 这种假值把缺失字段填上，"检查是否为空"的代码就查不出来（M1）。
• 解析外部输出可能格式对不上，然后代码"优雅降级成跳过检查" —— 于是"没检查"被伪装成"通过"（R5.3、R5.4）。
• bash -n 说脚本语法 OK，脚本却跑不起来 —— CRLF 行尾会漏报， 只有真跑 --help 才暴露（R6.6）。
• 验收脚本自己算错了单位 —— check_sim_gripper.py 把 TF 的"米"当成"毫米" 去比阈值，于是 127 mm 被显示成 0.13 mm，恰好看起来像夹爪没张开， 让人跑去查夹爪代码（R4.12）。
• 上一轮的残留进程伪造出"代码写错了"的现场 —— 两个同名适配器抢话题/服务， 残留的 static_transform_publisher 把旧外参按在新仿真上，读数全是乱的（R4.11）。

所以：

1. 写完校验，先故意弄坏被校验的对象，确认检查真的会红（负向测试和正向测试 一样重要，R5.2）；
2. 判据要挑与被测实现无关的物理量，单位只在一处换算；
3. 结论不对时，先怀疑判据和环境，再怀疑被测对象 —— 上面后两条都是 "先怀疑了代码，结果是自己错"（R4.12）。

---

5. 出问题按现象查

5.1 三个权威处，按"你在问什么"分工

你在问	去哪	为什么
「这个现象可能是什么原因」	FRAMEWORK_RULES.md 第 8 节	按症状排序的反例表，一条一个真实原因
「这是 Gazebo 的问题」	仿真手册 §5	11 条仿真专属坑，每条写了"怎么发现"
「这个脚本/话题/参数是干嘛的」	WORKSPACE.md / INTERFACES.md	目录、脚本、话题、参数契约
「手眼/跟踪/夹爪/入孔/触觉怎么用」	各自那份 docs/*.md	见下节对应表

先查症状表，再动手改代码。 这个仓库的坑绝大多数"看着像新问题，其实是老问题复发"。

5.2 常见现象 → 先想到什么

现象	先怀疑	详细
跟踪一直差几厘米	用了别的臂的手眼文件，或没重新标定	HANDEYE.md
图像有、但点算错	深度单位 / frame_id / 内参来源	第 2.2 节、R2.2–R2.5
模型"根本没进 Gazebo"，但 create 打印成功	world 里声明了插件，挤掉了默认的 UserCommands	仿真手册坑 2
物体一 spawn 就消失	上游 URDF 缺 <inertial> / 有零质量 link	仿真手册坑 1
夹爪控制器 active 但手指不动	<mimic> 被 sdformat 丢了	仿真手册坑 10
触觉"离开"之后接触标志永远是 True	contact 是事件式消息，离开时不发	仿真手册坑 6
消息定义里有 wrenches 字段，运行时却永远是空的	Fortress 的 contact-system 根本不填力	仿真手册坑 7
触觉"有力、会响应、松开归零"但语义全错	轴向映射是猜的，没实测	R3.3
明明夹住了，fnormal 恒为 0	基线把真实接触吃掉了	R3.1
校验代码永远通过	sdformat 用 __default__ / __default_topic__ 假值填了缺失字段	仿真手册坑 9
脚本报 $'\r': 未找到命令	脚本是 CRLF 行尾	R6.6
改了参数不生效	改的是包内 yaml，被 site.yaml 覆盖了	3.2
MoveIt 说规划成功、机械臂不动	SRDF / controllers 里还是旧臂的关节名	3.7.1、R1.1.1
控制器报 Joint 'Rizon4-xxxxxx_joint1' not found	换了臂但一级配置没跟着改（或改了两处漏一处）	3.7.1
仿真镜头位置不对、只看得见地面	相机 rpy 复合顺序	仿真手册坑 5
脚本说"已停止"，进程还在	进程模式表漏项	R4.10
孔检测在仿真里怎么都不过	图幅与真机不同，像素门槛失效	仿真手册 §4.3

---

6. 术语表与待办

6.1 术语

词	意思
通止规	检验螺纹的量具。通端应能旋到底，止端应旋不进（或只进几圈）
89 / 91	左臂 / 右臂的现场编号，各有独立的手眼、相机、夹爪、锁高，禁止混用
法兰	机械臂末端安装面；手眼标定求的就是相机→法兰的 T_4x4
eye-in-hand / eye-to-hand	相机装在手上 / 装在手上之外（D405 在手上，D435 在外部）
stage1/2/3	跟踪 / 对孔 / 搜孔，见 1.3
S03 / S04 / S07 / S08	整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，没有一一对应，别硬套
基线	触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值
R1.1 / M1 …	FRAMEWORK_RULES.md 里的规则编号，文档和提交信息里直接引用
静默失效	不报错但结果是错的。本仓库的头号敌人，见 M1

6.2 还没做的事（别按已具备来用）

按优先级：

1. 自动启动全部服务 —— 目前靠三个终端手工起，容易漏、 且顺序错了会出"控制器没起来但脚本继续跑"这类问题。
2. 夹爪持规自检 —— 启动后先用触觉判断"规真的被夹着"，没有就报错停机 （现在没有这一步，规掉了会一路跑到撞工件才发现）。
3. D435 手眼标定 —— 标完 S03 粗定位链路才可用（工件不在 D405 视野里时靠它先粗居中）。
4. 装配合格判定 —— 把力曲线转成"通/止"结论。当前只输出原始几何与力， 判定逻辑（含螺纹旋入圈数）还没写。

6.3 怎么往下读

你刚来        → 第 1、2 节，然后照着 1.3 的三个终端跑一次
要动手改      → 第 3 节决策表
要交付        → 第 4 节验收 + FRAMEWORK_RULES 第 7 节检查清单
被 bug 咬了   → 第 5 节
要给 AI 下指令 → FRAMEWORK_RULES 第 9 节