arm_dev
47ad9d9a8b
相机链路（devices/sim/gazebo/config/cameras.yaml 是唯一真相）
- sim_cameras.py 纯逻辑：话题映射 / 深度换算 / 内参 / 视场一致性比对
- sim_camera_adapter.py 改光学系 frame_id、32FC1 米 → 16UC1 毫米、覆盖内参
- gazebo_sim.launch.py 桥接 + 适配器 + 世界相机的静态 TF（位姿从 world 解析）
本轮修掉两个**静默**的几何错误（都能出图、都能查到 TF，但点位是错的）
1. gz 发的 camera_info 是错的：恒为 fx=fy=277, cx=160, cy=120
（等于它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
照抄它做三维反投影，点会偏半米量级；单目预览完全正常。
用已知地面平面做最小二乘反解，得 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 一致，启动时自动比对告警。
2. assembly.world 里 D435 的 rpy 写成 (0, 50°, 180°)，
而 URDF 相机的视线是**本体 X 轴**、rpy 复合顺序又是 Rz·Ry·Rx，
于是"先俯 50° 再偏航 180°"把视线翻到了背面：相机背对台面、只看地面。
它不报错，而且几何自洽（点确实落在真实地面上），只查数值发现不了。
改为 (0, 50°, 0)，并加回归测试锁住。
新增验收工具 + 测试
- scripts/check_sim_cameras.py 反投影深度、要求命中地面/台面才算通过。
"有图像"不是验收标准；实测 d435 台面 11.9%/地面 75.3%、d405 台面 37.8%。
- 97 项单测（新增 sim_cameras 22 项、sim_site 16 项）
顺带修掉一处"改了配置静默无效"
- sim.world 此前硬编码在 robot_description._WORLD_REL，而 site.yaml 与
manifest.yaml 都宣称改它就生效。现在真的读了：
优先级 --world > site.yaml:sim.world > 内置默认（相对路径按仓库根解析）。
- physics_enable / joint_prefix 仍未接线，但若其值与实际状态冲突，
启动时会告警（sim_site.unwired_warnings），不再静默忽略。
文档
- devices/sim/gazebo/README.md 重写：原文还写着"仿真不是 Gazebo"、
相机插件用 libgazebo_ros_camera.so，与实现早已不符。现分三部分：
哪个"仿真"是真的 / 现状与缺什么 / 五条静默失效的坑及怎么验。
Co-authored-by: Cursor <cursoragent@cursor.com>
161 lines
9.1 KiB
YAML
161 lines
9.1 KiB
YAML
# =============================================================================
|
||
# devices/sim/gazebo/manifest.yaml
|
||
#
|
||
# Gazebo 仿真平台清单。
|
||
#
|
||
# 进度（截至 P5c-4，如实记录，不要凭印象改）：
|
||
# ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
|
||
# 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh
|
||
# ✔ 世界文件 worlds/assembly.world（sdformat 校验通过）
|
||
# ✔ 机器人描述 urdf/（复用官方 flexiv_description，已通过 sdformat 校验）
|
||
# ✔ 控制器 config/controllers.yaml（控制器名与真机一致）
|
||
# ✔ 描述归一化 arm_sim_gazebo 包的 urdf_normalize（37 项单测）
|
||
# ✔ 启动器 gazebo_sim_launcher 已实现并端到端跑通
|
||
# 起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active）
|
||
# 入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh
|
||
# 与 build_sim.sh 的提示语引用，照做会报文件不存在）
|
||
# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做）
|
||
# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做）
|
||
# ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证**
|
||
# 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑：
|
||
# 话题映射 / 深度换算 / 内参，单测覆盖）
|
||
# → sim_camera_adapter（改光学系 frame_id、米→毫米、覆盖内参）
|
||
# 验收工具：scripts/check_sim_cameras.py（反投影必须命中地面/台面）
|
||
# 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0%
|
||
# ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效）
|
||
#
|
||
# 所以：D405/D435 可用，触觉还要等 P5c-5，装配状态机（P6）依赖它。
|
||
#
|
||
# 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）：
|
||
# 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺
|
||
# <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。两者都只有
|
||
# Gazebo 会暴露，RViz/MoveIt 完全正常。已在启动时归一化解决。
|
||
# 2. world 文件里只要声明了**任何一个** <plugin>，Gazebo 就不再加载默认的
|
||
# Physics/UserCommands/SceneBroadcaster。少了 UserCommands，
|
||
# /world/<名>/create 服务不存在 → 模型根本没进 Gazebo，而 create 仍打印
|
||
# "OK creation of entity"。所以这三个系统必须在 world 里显式声明。
|
||
# 3. gz 的插件搜索路径不含 $CONDA_PREFIX/lib（ros_gz_sim 的 launch 只拼
|
||
# LD_LIBRARY_PATH，而 conda 激活不往那里加），于是 gz_ros2_control 报
|
||
# "couldn't find shared library" 而库其实就在那儿。必须在 ros2 launch 之前设
|
||
# GZ_SIM_SYSTEM_PLUGIN_PATH；资源路径同理（否则 mesh 全加载不出来）。
|
||
# 两者都由 scripts/sim_env.sh 负责。详见该文件里的长注释。
|
||
# 4. **gz 发的 camera_info 是错的**：恒为 fx=fy=277, cx=160, cy=120
|
||
# （等于它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
|
||
# 照抄它做三维反投影，点会偏半米量级，而单目预览完全看不出来。
|
||
# 已改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。
|
||
# 代价：cameras.yaml 的 horizontal_fov 必须与 SDF 一致，启动时会自动比对告警。
|
||
# 5. **相机位姿的 rpy 顺序**：URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；
|
||
# 而 rpy 复合顺序是 Rz·Ry·Rx，"先俯 50° 再偏航 180°"会把视线翻到背面。
|
||
# d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报，
|
||
# 而且几何自洽（点确实落在真实地面上），只查数值发现不了。
|
||
# 已修正并加回归测试（test_robot_description 的 d435 位姿用例）。
|
||
# =============================================================================
|
||
|
||
id: gazebo
|
||
label: Gazebo 仿真
|
||
kind: sim
|
||
vendor: openrobotics
|
||
# 实测版本。注意：Humble 配的是 Fortress（ignition-gazebo6），不是现代 gz-sim ——
|
||
# 后者要 libprotobuf>=6.31，与 RoboStack 给 Humble 钉的 5.29.3 解不开依赖。
|
||
version: "Fortress 6.16 (ignition-gazebo6, ros_gz + gz_ros2_control 0.7.15)"
|
||
|
||
description: >
|
||
物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证
|
||
（起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。
|
||
D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。
|
||
场景模型（工件/通止规）与 Orisys 触觉插件尚未实现；
|
||
sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线，
|
||
写了不一致的值会告警而非静默忽略，见文件头 config_schema）。
|
||
注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前，
|
||
仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。
|
||
|
||
# --- 物料放置要求 ----------------------------------------------------------
|
||
assets:
|
||
- path: worlds
|
||
required: false
|
||
what: Gazebo 世界文件（.world / .sdf）
|
||
format: Gazebo 原生格式
|
||
naming: 场景名.world，如 assembly.world
|
||
where_used: 由 site.yaml 的 sim.world 指向
|
||
|
||
- path: models
|
||
required: false
|
||
what: 工件 / 夹具 / 通止规模型
|
||
format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/
|
||
naming: >
|
||
模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标
|
||
与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。
|
||
where_used: Gazebo 场景加载
|
||
|
||
- path: urdf
|
||
required: false
|
||
what: 仿真版机器人描述
|
||
format: .urdf / .xacro，需含 gazebo 与 ros2_control 标签
|
||
naming: 关节前缀必须以 sim_ 开头，与真实臂（Rizon4-xxxxxx_）区分
|
||
where_used: 仿真机器人加载
|
||
|
||
- path: sensor_plugins
|
||
required: false
|
||
what: 传感器插件（含 Orisys 触觉自定义插件）
|
||
format: 插件源码 + CMakeLists.txt
|
||
naming: 插件类名以 Orisys 开头
|
||
where_used: 仿真传感器数据发布
|
||
|
||
# --- 适配器 ----------------------------------------------------------------
|
||
driver:
|
||
package: arm_sim_gazebo
|
||
executable: gazebo_sim_launcher
|
||
build: colcon
|
||
notes: >
|
||
已实现并在 setup.py 注册。它只做"准备描述（展开 + 归一化）+ 转发给
|
||
launch/gazebo_sim.launch.py"，启动逻辑不在可执行文件里 —— ros2 launch 的
|
||
日志/退出码/中断处理都更成熟，自己重造一份只会更难调试。
|
||
入口建议走 scripts/run_sim_gz.sh（它会先 source scripts/sim_env.sh，
|
||
而 Gazebo 的插件/资源搜索路径必须在 ros2 launch 之前设好，见 sim_env.sh）。
|
||
单独排查描述用：gazebo_sim_launcher --check / --print-urdf。
|
||
|
||
# --- 配置项声明 ------------------------------------------------------------
|
||
# 设计约束：仿真与真机共用同一套 site.yaml 参数（servo.* / hole_detect.stage2.*），
|
||
# 避免「仿真调好的值搬到真机不生效」。只有物理引擎自身的参数放在 sim.* 下。
|
||
#
|
||
# ⚠️ 每个键都必须如实标注"是否真的接线"（wired）。声明了却没接线的键是最难查的
|
||
# 一类问题：用户按文档改了 site.yaml，行为毫无变化，也不报错。
|
||
# 目前由 arm_sim_gazebo.sim_site 读取的是 world（已接线）；
|
||
# physics_enable / joint_prefix 尚未接线，若其值与世界/描述的实际状态冲突，
|
||
# 启动时会打印告警 —— 见 sim_site.unwired_warnings()。
|
||
config_schema:
|
||
- key: world
|
||
site_path: sim.world
|
||
type: str
|
||
required: false
|
||
default: ""
|
||
wired: true
|
||
doc: >
|
||
仿真世界文件路径。留空 = 用内置默认（devices/sim/gazebo/worlds/assembly.world）。
|
||
相对路径按仓库根解析。优先级：命令行 --world > 本项 > 内置默认。
|
||
实现在 arm_sim_gazebo/sim_site.resolve_world()。
|
||
|
||
- key: physics_enable
|
||
site_path: sim.physics_enable
|
||
type: bool
|
||
required: false
|
||
default: true
|
||
wired: false
|
||
doc: >
|
||
是否启用物理引擎。**尚未接线**：世界文件里的 <physics>（重力/步长）是写死的，
|
||
本项不会传导过去。若这里写的值与世界实际状态不符，启动时告警而不是静默忽略。
|
||
要真正生效，需改 world 文件里的 <gravity>，或把本项接进启动流程。
|
||
|
||
- key: joint_prefix
|
||
site_path: sim.joint_prefix
|
||
type: str
|
||
required: false
|
||
default: sim_
|
||
wired: false
|
||
doc: >
|
||
仿真关节名前缀。必须与真实臂的前缀（Rizon4-xxxxxx_）不同，
|
||
否则仿真与真机的 TF 会互相污染。
|
||
**尚未接线**：实际前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix 决定，
|
||
且 config/controllers.yaml 里硬编码了 sim_joint1..7。写了个不同的值会告警。
|
||
要真改，这三处必须一起改。
|