arm_dev
d32e6ae53f
一、场景物料（新增）
devices/sim/gazebo/models/workpiece/ 带三个孔的铝板。碰撞体是"带方孔的平板"，
视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔，
只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。
devices/sim/gazebo/models/gauge/ 通止规：通端/套环/握柄/止端四段，各段异色
便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"，
用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。
devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒，
加个方孔底座把它立住。
三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。
二、场景配置与校验（新增）
config/scene.yaml 物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。
理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。
动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。
含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。
sim_scene.py 摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移），
检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。
重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 ——
正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报，
而误报的后果是人把检查整个关掉，之后真重叠也抓不到。
另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。
scripts/check_sim_scene.py 验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 +
悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。
三、修掉两处静默失效（都是"看起来一切正常"）
1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析，
而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 →
调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**，
而日志只说"跳过"，看起来像正常降级。
现在两种格式都支持，并用真实抓下来的输出样本做回归测试。
教训：宽容降级会把"没检查"伪装成"检查通过"。
2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）：
脚本报"已停止"，它却还在发上一份 /robot_description 与 TF，
下一次启动就会"改了描述却不生效"。
顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报
一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。
现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**：
有残留就报出来并非零退出 —— 下次再有漏项，不再静默。
实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。
四、相机：D405 分辨率对齐真机
仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是
58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 ——
孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。
已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场"
扩成"视场 + 图幅"（sim_cameras.check_camera_consistency），
不一致时说明它对像素门槛的影响。
五、文档
devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 /
配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。
manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。
scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。
cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。
六、测试
新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是
位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条），
含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、
center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。
全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。
同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步，
它的 parity 测试是红的）。
Co-authored-by: Cursor <cursoragent@cursor.com>
93 lines
6.1 KiB
YAML
93 lines
6.1 KiB
YAML
# =============================================================================
|
||
# devices/sim/gazebo/config/cameras.yaml
|
||
#
|
||
# 仿真相机的**唯一数据源**：gazebo_sim.launch.py 里的 ros_gz_bridge 参数、
|
||
# sim_camera_adapter 节点、以及为世界相机补的静态 TF，全部从这份文件派生。
|
||
# 想加一台仿真相机，改这里一处即可（前提是它在 URDF / world 里已经有 <sensor>）。
|
||
#
|
||
# 与 site.yaml 的分工：
|
||
# 这里只描述"Gazebo 里有什么、要桥到 ROS 的哪个名字"。
|
||
# site.yaml 描述"真机上有哪些相机、序列号是多少"。仿真不读序列号 ——
|
||
# Gazebo 里没有 USB，序列号无从绑定。
|
||
# 两者的**话题命名约定必须一致**，这是应用层能不改代码跑仿真的前提。
|
||
#
|
||
# 话题命名约定（与真机 realsense 完全对齐，见 arm_application/handeye_realsense.py
|
||
# 的 REALSENSE_TOPIC_DEFAULTS 与 realsense_color_pub.py）：
|
||
# /<ros_ns>/color/image_raw rgb8
|
||
# /<ros_ns>/color/camera_info plumb_bob
|
||
# /<ros_ns>/aligned_depth_to_color/image_raw 16UC1，单位毫米，0 = 无读数
|
||
#
|
||
# ⚠️ 单位与编码是关键差异，不能只做话题改名就完事：
|
||
# Gazebo 的深度相机发的是 **32FC1、单位米**（本机实测 step=2560 即 4 字节/像素），
|
||
# 而真机 realsense 的 aligned_depth 是 **16UC1、单位毫米**。
|
||
# 直接把 32FC1 换个名字发出去，应用侧按毫米解释会整体差 1000 倍，
|
||
# 且 numpy 的 frombuffer 会因 dtype 不符报错或读出垃圾。
|
||
# 所以深度必须**真正转换**，由 sim_camera_adapter 负责。
|
||
#
|
||
# ⚠️⚠️ 更要紧的一条：**gz 发布的 camera_info 是错的，不能直接用。**
|
||
# 本机实测（Fortress 6.16 / RoboStack）：无论怎么配，它都给
|
||
# fx=fy=277, cx=160, cy=120
|
||
# 这组值对应的是 **gz 的默认值** —— 60° 视场 + 半分辨率图幅(320x240)，
|
||
# 而实际渲染用的是配置里的 87° 视场与 640x480。
|
||
# 用已知地面平面反解真实内参得到 fy=337.2、cy=239.5，
|
||
# 与 640/2/tan(1.5184/2)=337.2、480/2=240 完全吻合（残差中位 0.2mm）。
|
||
# 照抄 gz 的 camera_info 去做三维反投影，点位会错 0.5 米量级 ——
|
||
# 而单目预览里**完全看不出来**，只会让"仿真里调好的参数搬到真机"整个失效。
|
||
# 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。
|
||
#
|
||
# 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。
|
||
# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_camera_consistency）：
|
||
# 腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov
|
||
# 世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 <sensor> 的 horizontal_fov
|
||
# =============================================================================
|
||
|
||
cameras:
|
||
# --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------
|
||
- gz_ns: /d405 # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 <topic>）
|
||
ros_ns: d405 # ROS 侧命名空间。应用按 camera_name 参数用这个名字取话题，
|
||
# 所以它必须等于启动应用时给的 camera_name（默认 d405）
|
||
link_frame: sim_d405_link
|
||
# 图像上盖的坐标系必须是**光学系**，不能是本体系。
|
||
# gz 与 URDF 里的相机本体遵循"X 朝前 / Y 朝左 / Z 朝上"（相机机身约定），
|
||
# 而 ROS 的 image / camera_info 必须用光学约定"Z 朝前 / X 朝右 / Y 朝下"
|
||
# （REP-103/105）。两者差一个 rpy=(-90°, 0, -90°)。
|
||
# 若直接把 frame_id 盖成本体系，应用做 tf2 查询能查到、不报错，但
|
||
# 深度反投影出的三维点会整体转错 —— 这类错误在单目预览里完全看不出来。
|
||
optical_frame: sim_d405_color_optical_frame
|
||
depth: true
|
||
# 水平视场（弧度）。真机 D405 彩色的水平视场约 87° = 1.5184 rad，
|
||
# 仿真的传感器也按这个建，这样两边的像素尺度接近。
|
||
# 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。
|
||
horizontal_fov: 1.5184
|
||
# 图像尺寸必须与真机一致：真机 D405 跑 848x480@30（site.yaml 的 camera.width/height）。
|
||
# 这**不是**显示偏好 —— 阶段二的孔检测在原始分辨率图上跑 HoughCircles，
|
||
# hole_detect.yaml 的像素门槛（hough_min_radius / hole_distance_min）只在这个
|
||
# 尺度下才对。曾经用 640x480（fx 337 而非真机 447），58mm 的两孔间距
|
||
# 成像只剩 85px，低于 hole_distance_min=90 → 孔检测在仿真里根本不可能通过。
|
||
# 启动时会比对这里与 xacro 宏默认值，不一致会告警（sim_cameras）。
|
||
width: 848
|
||
height: 480
|
||
note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供
|
||
|
||
# --- 手外相机（eye-to-hand），固定在世界里看工作台 -------------------------
|
||
# ⚠️ 这台相机在 world 文件里，不在 URDF 里 —— 因为它的正确物理归属就是"世界"，
|
||
# 而不是机器人本体。（曾经把它挂在 URDF 的 world link 上，结果 sdformat 把
|
||
# 无质量的根 link 连同整棵子树静默丢掉，相机与它的话题一起消失。详见
|
||
# devices/sim/gazebo/README.md。）
|
||
# 代价是：gz 不会为世界模型发布 TF，所以必须由启动方补一条静态 TF
|
||
# world → d435_link。位姿从 world 文件里**解析**得到（不在这里重复配置），
|
||
# 避免"改了一处忘了另一处"。
|
||
- gz_ns: /d435
|
||
ros_ns: d435
|
||
link_frame: d435_link
|
||
optical_frame: d435_color_optical_frame
|
||
depth: true
|
||
horizontal_fov: 1.5184
|
||
width: 640
|
||
height: 480
|
||
# 非空 = 这台相机挂在世界模型上，需要补静态 TF。
|
||
# world_model 是 world 文件里的 <model name>，world_link 是模型里的 <link name>。
|
||
world_model: d435
|
||
world_link: link
|
||
note: height 也与真机不同（真机 1280x720），同 D405 的说明
|