ros_contorl/devices/sim/gazebo

devices/sim/gazebo/ —— Gazebo 仿真平台（中文手册）

这份文档是仿真的"使用说明书 + 建模规范 + 避坑清单"。

读法建议：

• 只想跑起来 → 看 第 1 节 使用方法。
• 要加一个工件/夹具/机器人 → 看 第 2 节 建模要求 与 第 3 节 导入格式。
• 要改参数 → 看 第 4 节 配置方式（有一张"改哪里才真的生效"的表，先看它）。
• 改仿真之前必须看完 第 5 节 实测踩到的坑 —— 仿真里绝大多数错误都是静默的： 不报错、能出图、能查到 TF，结果却是错的。每条坑都写了"怎么发现"。

本文档只记录已经跑通的事实。没接线的东西会明确标 ✘，不要凭印象改成 ✔。

---

0. 先读这段：哪个"仿真"是真的

仓库里有两个都叫"仿真"的东西，很容易混：

入口	底层	有物理吗	用途
scripts/run_sim.sh	arm_bringup 的 mock 驱动（mock_arm_driver.py）	✘ 假数据 + RViz	验证话题/流程/UI，不耗资源
scripts/run_sim_gz.sh	Gazebo Fortress（真物理、真接触）	✔	验证接触/力控/几何、装配状态机

要验证装配的接触与力判据，只能用后者。 前者永远不会产生接触力。

判断当前在跑哪个：ros2 topic echo /clock --once —— 有 /clock 且时间在走，是 Gazebo。

---

1. 使用方法

1.1 装环境（只做一次）

bash scripts/install_sim_env.sh     # 建独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
bash scripts/build_sim.sh           # 编仿真用的包（--merge-install）

环境是独立的一份，与真机环境 ros_flexiv 隔离。理由与踩坑见 坑 3 和 scripts/install_sim_env.sh 顶部的长注释。

1.2 启动

bash scripts/run_sim_gz.sh                            # 无头（无显示器环境用这个）
bash scripts/run_sim_gz.sh --gui                      # 带 Gazebo GUI
bash scripts/run_sim_gz.sh --check                    # 只自检描述，不启动任何东西
bash scripts/run_sim_gz.sh --world /abs/other.world   # 换 world
bash scripts/run_sim_gz.sh --no-gripper --no-sensors  # 最省资源，只验运动学

首次启动约 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。启动日志末尾会打印一份 验收命令清单，照它做一遍就能确认仿真真的活了。

1.3 验收（★ 必做，别只看"有没有图像"）

ros2 topic echo /clock --once                  # 仿真时间在走
ros2 control list_controllers                  # 两个控制器 active
ros2 topic echo /joint_states --once           # 7 个 sim_joint
ros2 run tf2_ros tf2_echo world sim_tcp        # 末端位姿可查
ros2 topic echo /tactile/left/status --once    # 触觉三力在发
ign model -m workpiece -p                      # 工件在台面上
python3 scripts/check_sim_cameras.py           # ★ 相机的真正验收
python3 scripts/check_sim_scene.py             # ★ 场景摆放 + 孔检测几何验收

机械臂应当在重力下保持站住。若它瘫下去或消失，说明描述有问题 —— 见 坑 1。

两条 check_*.py 才是验收标准，"有图像"证明不了什么。 它们做的判断分别是：

工具	它凭什么判"通过"	它防的是哪一类错
check_sim_cameras.py	把深度反投影到 world，要求命中已知平面（地面 z=0、台面 z=0.40）	深度单位差 1000 倍、frame_id 盖成本体系、内参是 gz 的错值、相机位姿朝向反了 —— 全都能出图
check_sim_scene.py	摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机完整看得见工件	物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全

1.4 停止

bash scripts/stop_sim_gz.sh      # 稳妥地停掉整套仿真

不要用 pkill -f gz：它会把调用它的那个 shell 一起杀掉（-f 匹配命令行， 而 pkill 自己的命令行里就含 gz），也会误杀同名的其他进程。stop_sim_gz.sh 按进程树精确识别，排除了自己和自己的祖先进程。

---

2. 建模要求

2.1 三条通用规矩（违反哪一条都会"看起来正常"）

规矩	为什么	违反了会怎样
单位必须是米	Gazebo 没有单位系统，SDF 里的数字就是米	用毫米建的 mesh 会变成 1000 倍大的怪物，或者小到看不见
模型原点放在底面中心	放置时 pose.z 就等于"放在台面上的高度"	原点在质心时物体一半埋进台面，或者悬空；sim_scene 会校验并报错
<model name> 必须与推理用的类别名一致	仿真里算出的坐标要与 YOLO 结果对齐	视觉算出的类别名与场景里的名字对不上，坐标静默错位

sim_scene.validate_scene() 会在启动时把上面第 1、2 条自动查一遍 （模型原点、是否穿台面/悬空/出界/互相嵌入），有问题的物体不会被 spawn 出去。

2.2 质量与惯性（<inertial>）

<inertial> 不是可选的。 缺了它 Gazebo 会给一个默认值，物体表现得像另一个东西； 更糟的是惯性为 0 或 NaN 时物体直接消失（见 坑 1）。

• 质量与真实值同量级即可，不必精确。装配里要紧的是质量比： 工件太重会让夹爪"压进去"的读数异常大（见 config/tactile.yaml 的复测记录）。
• inertia 用简化公式算（薄板、圆柱的公式够用），只要量级对。
• 参考 models/workpiece/model.sdf（铝板：mass=0.08）与 models/gauge/model.sdf（钢规：mass=0.02）。

2.3 <collision> 与 <visual> 的分工

	作用	建模要求
<collision>	决定物理：穿不穿、卡不卡、接触力多大	用简单几何体（box/cylinder），不要用 mesh —— mesh 碰撞在接触力计算上又慢又不稳
<visual>	决定"相机看见什么"	可以加入只为人眼/算法准备的细节（亮环、色块），它们不参与物理

这两个可以不一样，而且这个框架有意让它们不一样：

models/workpiece/model.sdf 的 collision 是"一块带三个方孔的平板"， 而 visual 额外加了两个亮环（φ14 外 / φ7 内）来模拟真机上的压入衬套 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔的。若只按 collision 建视觉， 仿真里"孔检测"会因为看不见环而永远失败，而这不是算法问题。

⚠️ 由此带来一条约定：visual 的名字有意义，sim_scene.features_from_workpiece 按名字前缀（insert_*_ring / center_bore）去读出特征尺寸，用来算像素值。 改 visual 的名字必须同步改那个函数（有单测守着）。

2.4 接触参数（<surface>）：只在必要时改，且要写清理由

默认接触参数（kp 很大 = 刚性）对"位置控制 + 刚性夹持"是灾难： 夹爪把工件"压进去"的力会到几百牛，工件被弹飞。

models/gauge/model.sdf 因此对规单独放软了接触刚度（kp/kd 比台面软得多）， 注释里写明了原因。这是有意为之，不是随手抄的参数 —— 照抄前先想清楚你的物体 是不是也要被夹爪夹。

2.5 落地判据：怎么知道模型建对了

ign sdf -p models/<名>/model.sdf        # 先过语法
python3 scripts/check_sim_scene.py      # 再过摆放与成像

两条都过了，才认为"模型建好了"。不要以"Gazebo 里能看到它"为判据。

---

3. 导入格式

3.1 物料放置总表

类型	放哪	格式	命名要求	谁读它
世界文件	worlds/	.world / .sdf	<场景名>.world	site.yaml → sim.world
机器人描述	urdf/	.urdf / .xacro	关节前缀必须与真机不同（仿真用 sim_）	gazebo_sim_launcher
场景物体	models/<模型名>/	<模型名>.sdf + <模型名>.config	目录名 = <model name> = 推理类别名	config/scene.yaml → 启动时 spawn
网格文件	models/<模型名>/meshes/	.stl / .dae	单位必须是米	被 SDF 的 <mesh><uri> 引用
相机声明	config/cameras.yaml	YAML	ros_ns 必须等于应用的 camera_name	桥接 + 适配器 + 静态 TF
场景布局	config/scene.yaml	YAML	—	启动时 spawn + 像素门槛校验
触觉标定	config/tactile.yaml	YAML	话题名必须与 URDF 里的 <topic> 一致	sim_tactile_adapter
控制器	config/controllers.yaml	YAML	控制器名与真机一致	ros2_control

sensor_plugins/ 目前是空的（只有 .gitkeep）。触觉走的是 Gazebo 内置的 force_torque + contact 传感器 + 本框架的适配器，不需要自己写 C++ 插件。 那个目录留给"以后真要写插件"的场景。

3.2 一个模型目录长什么样

models/workpiece/
├── model.sdf       # 几何 + 惯性 + 接触参数
└── model.config    # 元数据：<name>、版本、指向 model.sdf、描述

model.config 里的 <name> 必须等于目录名，也必须等于 YOLO 的类别名。 models/workpiece/model.config 里写了这条约定，照它抄。

3.3 加一个场景物体：改 scene.yaml 一处即可

物体不在 world 文件里，而在 config/scene.yaml 的 objects 列表里， 启动时由 ros_gz_sim create 动态 spawn：

objects:
  - name: workpiece          # Gazebo 里的实体名（也是 ign model -m 的名字）
    model: workpiece         # models/ 下的目录名
    pose: {x: 0.52, y: 0.0, z: 0.40, yaw: 0.0}   # z = 台面顶面高度（原点在底面）
    note: 为什么摆这儿 —— 请写清，否则半年后没人敢动

这么设计的原因：world 文件是"环境"（地面、台面、重力、固定相机）， 场景是"这一批活要做什么"（工件在哪、规座在哪）。混在一起会导致 "为了挪一下工件而去改 world 文件"，而 world 文件一旦改错（比如丢了 UserCommands 系统）会让所有模型都 spawn 不进去（见 坑 2）。

3.4 URDF → Gazebo：中间隔着 sdformat，会丢东西

flexiv_description 的 xacro 展开后交给 sdformat 转成 SDF。这个转换是有损的：

会被丢/被改的	后果	本框架的应对
<mimic> 标签	夹爪的从动指节不再跟随 → 夹爪"驱动了但不动"	urdf_normalize 把 mimic 关系记下来，改成把全部 7 个夹爪关节都接入 ros2_control，由控制器按记录的关系驱动
缺 <inertial> 的 link	惯性 NaN → 物体消失	启动时补默认惯性
零质量的 link	惯性叠加出 NaN	同上，一律归一化
世界 link（无质量的根 link）	整棵子树被静默丢掉	D435 因此必须放在 world 文件里，不能挂在 URDF 的 world link 上

这些都不是"理论上可能"，是实测踩过的。归一化逻辑在 src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py（37 项单测）。 单独排查：gazebo_sim_launcher --check。

---

4. 配置方式

4.1 ★ 改哪里才真的生效（先看这张表）

想改什么	改哪里	真的生效吗
用哪个世界	site.yaml → sim.world，或命令行 --world	✔ 优先级：--world > site.yaml > 内置默认
场景里有哪些物体、摆在哪	config/scene.yaml	✔ 启动时 spawn，并做摆放/像素校验
相机的视场/坐标系/话题	config/cameras.yaml，并且同步 SDF 里对应的 horizontal_fov	✔ 启动时自动比对，不一致告警
相机图幅（影响像素尺度）	config/cameras.yaml 的 width/height 且 SDF 的 image_width/height	✔ 启动时自动比对（这是最容易漏的一处，见 4.3）
触觉标度/轴向/基线	config/tactile.yaml	✔ 适配器每次启动都读
装配的悬高/速度	复用真机那套（servo.* / hole_detect.stage2.*）	✔ 有意共用
物理开关	site.yaml → sim.physics_enable	✘ 未接线。要真关，改 worlds/assembly.world 的 <gravity>
关节前缀	site.yaml → sim.joint_prefix	✘ 未接线。改要动三处：urdf/arm.sim.urdf.xacro 的 arm_prefix、config/controllers.yaml 的 sim_joint1..7、本项

设计约束：仿真与真机必须共用同一套 site.yaml 参数。任何"仿真专用参数"都是 未来"仿真里调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数 （重力、步长）放在 sim.* 下。

✘ 的两个键由 arm_sim_gazebo.sim_site.unwired_warnings() 守着：写的值与实际状态 不一致时启动会告警。不要让它们重回静默。

4.2 配置文件之间的分工

config/site.yaml          ← 站点真相：真机有哪些设备、序列号、阈值（仿真与真机共用）
   │
   ├── devices/sim/gazebo/config/cameras.yaml   ← "Gazebo 里有什么相机、桥到哪个 ROS 名字"
   ├── devices/sim/gazebo/config/scene.yaml     ← "这一批活：工件/规座摆在哪"（含像素门槛）
   ├── devices/sim/gazebo/config/tactile.yaml   ← 仿真的力标定口径与轴向映射
   └── devices/sim/gazebo/config/controllers.yaml ← 控制器（名字与真机一致）

一条原则：能派的就别抄。 相机位姿从 world 文件里解析（不在 cameras.yaml 重复写）、 内参从 horizontal_fov 与图幅算出（不手抄 fx/fy）—— 抄一份就多一处能漂移的地方。

唯一的例外是 scene.yaml 里的 gates_px：像素门槛本来只在真机的 hole_detect.yaml/site.yaml 里，但仿真要在启动时就判"能不能看见"， 所以抄了一份。为了不让它漂移，check_sim_scene.py 会读那两个文件做交叉核对， 不一致直接判失败。抄一份可以，但必须有人守着。

4.3 改相机图幅会牵动三处（最常见的漏改）

# 想改 D405 的分辨率，三处必须同时改：
devices/sim/gazebo/config/cameras.yaml        # width / height（内参由此算出）
devices/sim/gazebo/urdf/arm.sim.sensors.xacro # image_width / image_height（真正渲染的）

第三处是下游：hole_detect.yaml 的像素门槛（hough_min_radius、hole_distance_min） 只在特定像素尺度下才对。仿真用了 640x480（fx=337）而不是真机的 848x480（fx=447）时， 58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 —— 孔检测在仿真里根本不可能通过，而报出来的现象会像是"算法不行"。

启动时会比对 cameras.yaml 与 SDF 的视场和图幅，不一致会告警并说明影响。

---

5. 实测踩到的坑

这些坑的共同特征是：不报错。话题在、图像有、TF 查得到、create 打印成功，但结果是错的。 所以每条都写了"怎么发现"。

坑 1：官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉

7 轴本体缺 <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。 只有 Gazebo 会暴露，RViz/MoveIt 完全正常。

→ 启动时归一化（urdf_normalize.py）。单独排查：gazebo_sim_launcher --check。

坑 2：world 里声明了任何 <plugin> 都会挤掉默认系统

world 里只要有一个 <plugin>，Gazebo 就不再自动加载默认的 Physics / UserCommands / SceneBroadcaster。

少了 UserCommands 的后果最阴：/world/<名>/create 服务不存在 → 模型根本没进 Gazebo，而 create 仍然打印 OK creation of entity， 控制器永远起不来。

→ 这三个系统必须在 world 里显式声明（assembly.world 里已有）。启动时会检查并告警。

坑 3：conda 环境里 gz 插件路径缺 $CONDA_PREFIX/lib

ros_gz_sim 的 launch 只拼 LD_LIBRARY_PATH，而 conda 激活不往那里加。 症状是 gz_ros2_control 报 couldn't find shared library，而库其实就在那儿（ldd 全齐）。

→ 必须在 ros2 launch 之前设 GZ_SIM_SYSTEM_PLUGIN_PATH；资源路径同理（否则 mesh 全加载不出来）。两件事都由 scripts/sim_env.sh 负责 —— 不要绕过它直接 ros2 launch。

坑 4：gz 发的 camera_info 是错的

恒为 fx=fy=277, cx=160, cy=120（它的默认 60° 视场 + 半分辨率主点）， 而渲染用的是配置的 87° 视场与 640x480。

照抄它做三维反投影，点会偏半米量级；而单目预览完全正常。

→ 用已知地面平面最小二乘反解内参，得 fy=337.2 / cy=239.5，与 640/(2·tan(87°/2))=337.2、480/2=240 吻合（残差中位 0.2mm）。 已改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。

代价：cameras.yaml 的 horizontal_fov 必须与 SDF 真正渲染用的值一致， 启动时自动比对告警（sim_cameras.check_camera_consistency）。

坑 5：相机位姿的 rpy 复合顺序

URDF 相机是"X 朝前"约定，视线是本体 X 轴；而 rpy 的复合顺序是 Rz·Ry·Rx。 "先俯 50° 再偏航 180°"会把视线翻到背面。

D435 就这么翻过：背对台面、只看得见地面。什么都不报，而且几何自洽 （点确实落在真实地面上），只查数值发现不了。

→ 用 scripts/check_sim_cameras.py 检查"该看见的平面有没有看见"。已修正 + 加回归测试。

坑 6：接触传感器只发"接触"，不发"离开"

Gazebo 的 contact 是事件式的：接触时发消息，离开时一条都不发。

若写成"收到消息=接触 / 没收到=不管"，那么离开工件后 contact 会永远停在 True。

→ 判据必须是"最近 contact_timeout_s 内收到过消息"（config/tactile.yaml）。 config/tactile.yaml 里对这点有专门说明。

坑 7：contact-system 发的 Contacts 消息里没有力

ros_gz_interfaces/msg/Contact 的 wrenches 字段在 Fortress 的默认 contact-system 下永远是空的（实测 ign topic -e -t /tactile/left/raw -n 1， 只有几何名与接触点，没有任何力）。

这是这套链路里最耗时的一个坑：消息结构里有 wrenches 字段，看起来"应该有"。

→ 三向力改由 <sensor type="force_torque"> 提供（挂在指尖关节上，读该关节传递的约束力）。 接触布尔仍用 contact 传感器（它只有这个用途）。

附带一个教训：光看消息定义判断不了运行时会填什么，必须 ign topic -e 实测。

坑 8：forcetorque-system 也必须显式声明

与坑 2 同源。只声明了 contact-system 时，force_torque 传感器话题根本不出现， 而 URDF 里写得好好的。

→ assembly.world 里显式声明 libignition-gazebo6-forcetorque-system.so。

坑 9：sdformat 用占位值填它不认识/缺失的东西

这是坑 1 的"隐藏变体"，也是验收检查本身最容易失效的地方：

你写的	sdformat 的输出
有 <contact> 但没 <collision>	__default__
有 <contact> 但没 <topic>	__default_topic__
<topic> 写在 <sensor> 里而不是 <contact> 里	话题跑到默认作用域，订阅方永远收不到

于是"检查这个字段是不是空"的代码永远通过 —— 因为它不是空的，它是个占位字符串。

→ robot_description.check_tactile_collisions() 现在显式判这两个占位值 （__default__ / __default_topic__），并检查 <topic> 的位置对不对。 有负向单测守着（test_robot_description.py）。

通用教训：校验代码自己会静默失效。凡是"检查 X 是否为空"的地方， 都要问一句"它会不会被填成一个假值"。

坑 10：<mimic> 标签被 sdformat 丢掉

夹爪的从动指节在 URDF 里靠 <mimic> 定义跟随关系，而 sdformat 转换时把它丢了。

症状是"夹爪控制器 active，但手指不动" —— 控制器在驱动一个不存在的关节。

→ urdf_normalize 在转换前把 mimic 关系记录成一张表（mimic_relations()）， 去掉标签，并把全部 7 个夹爪关节都接入 ros2_control，由适配器按关系驱动。

顺带：从动关节原本 effort="0"，归一化时一并改成可驱动的值， 否则它在控制器里是个"只能看不能动"的关节。

坑 11：gz model -m <名> -p 的输出格式没有稳定契约

不同版本的文本格式不同。验收脚本因此宽松解析，解析不出来时降级成 "跳过位姿核对"，而不是报一个假错。

这是有意的取舍：宁可漏报，不可误报。误报会让人不再相信工具。

---

6. 现状与进度

已就绪（端到端验证过）

部分	位置	说明
运行环境	scripts/install_sim_env.sh / sim_env.sh	独立 conda 环境 ros_gz_sim（Gazebo Fortress 6.16 + ROS 2 Humble）
世界	worlds/assembly.world	地面 + 工作台（顶面 z=0.40）+ D435（世界相机）
机器人描述	urdf/*.xacro	复用官方 flexiv_description，启动时归一化
控制器	config/controllers.yaml	控制器名与真机一致
启动器	src/arm_sim_gazebo 的 gazebo_sim_launcher	起 Gazebo → 发归一化 URDF → spawn → 拉控制器 → spawn 场景
相机	config/cameras.yaml + sim_camera_adapter	D405/D435 桥接到 ROS，内参/单位/frame 都已校正
触觉	config/tactile.yaml + sim_tactile_adapter	force_torque 三向力 + contact 布尔 → arm_msgs/TactileStatus
场景物体	models/ + config/scene.yaml	工件（带亮环的孔板）、通止规、规座，可校验摆放
夹爪	7 关节接入 ros2_control	实测开合 27→127mm

还没做

部分	缺什么	影响
视触觉图像	仿真的深度图/力矢量图只能是占位（空/NaN）	TactileStatus 的图字段在仿真里不可信；三力是可信的
夹具/料框	目前只有工件、规、规座	上料流程（若有）没法验
夹爪服务适配器	`~/open	~/close

---

7. 有意保留的规格差异（不是 bug，但会让人困惑）

项	真机	仿真	后果
镜头畸变	有（d 非 0）	无（d 全 0、R 单位阵）	"仿真里用畸变矫正调出来的参数"搬到真机仍需重调
D435 图幅	1280x720	640x480	像素尺度不同，hole_detect 的像素门槛搬过去要重算
触觉单位	Orisys 形变任意单位（非牛顿）	牛顿 × force_scale	见 config/tactile.yaml 头部的"单位口径"；选错不报错，只是阈值不触发或一碰就炸
触觉图像	有真实深度图/力矢量图	占位	下游不用改代码，但图内容不可信

---

8. 文件一览

devices/sim/gazebo/
├── manifest.yaml            # 设备清单：进度、配置声明、放置要求（与本文档同源）
├── README.md                # 本文档
├── config/
│   ├── cameras.yaml         # ★ 两台相机的唯一真相（话题/坐标系/视场/图幅）
│   ├── controllers.yaml     # 控制器配置（名字与真机一致）
│   ├── scene.yaml           # ★ 场景布局（工件/规/规座）+ 像素门槛
│   └── tactile.yaml         # ★ 触觉标定：单位口径、轴向映射（实测）、基线策略
├── urdf/
│   ├── arm.sim.urdf.xacro        # 顶层：拼装 + 前缀
│   ├── arm.sim.gazebo.xacro      # Gazebo 扩展（接触/摩擦参数）
│   ├── arm.sim.ros2_control.xacro# 控制器接口（7 轴 + 7 个夹爪关节）
│   └── arm.sim.sensors.xacro     # D405 + 触觉传感器（contact + force_torque）
├── worlds/assembly.world    # 环境：地面 + 台面 + D435 + 显式声明的系统插件
├── models/
│   ├── workpiece/           # 工件：带三个孔的铝板 + 两个检测用亮环
│   ├── gauge/               # 通止规：通端/套环/握柄/止端，接触参数刻意放软
│   └── gauge_stand/         # 规座：static，把规立住
└── sensor_plugins/          # （空）触觉走内置传感器 + 适配器，不需要自定义插件

配套代码：src/arm_sim_gazebo/（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑） 配套脚本：scripts/run_sim_gz.sh（入口）、scripts/stop_sim_gz.sh（停止）、 scripts/check_sim_cameras.py（相机验收）、scripts/check_sim_scene.py（场景验收） 单测：src/arm_sim_gazebo/test/（test_urdf_normalize / test_sim_cameras / test_sim_site / test_sim_tactile / test_robot_description / test_check_sim_scene）