arm_dev
b29339e522
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
yolo / truth / external —— truth 就走这条路。
为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
既准又稳，还能当真实检测算法的对照基线。
2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
visual_servo/filtered_point 用，绕开 ONNX。
3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。
4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
宁可用退化模式跑，也不要整个适配器起不来。
5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
与 use_sim_time 向子 launch 的传递。
另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。
Co-authored-by: Cursor <cursoragent@cursor.com>
103 lines
7.0 KiB
YAML
103 lines
7.0 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 侧命名空间。应用按 site.yaml → robots[].camera.name 取话题，所以这里
|
||
# 必须**等于那一项**：仿真那台在 site.yaml 里登记为 robots[] 的 `sn: sim`，
|
||
# 它的 camera.name 就是这里写的 sim_d405。
|
||
# 曾经这里写的是 d405，而 site 写的是 d405_left —— 后果是通止规装配在仿真里
|
||
# S00 报"腕部相机无图像"（图像 13Hz 好好地流着），人只会去怀疑相机。
|
||
# 改这里的名字，site.yaml 里那个 sn: sim 条目的 camera.name 必须同步改。
|
||
ros_ns: sim_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 侧命名空间 = sim_d435（与真机 d435 分开）。为什么不能沿用 d435：
|
||
# eye-to-hand 标定文件是按**相机名**命名的（handeye/results/<相机名>_eye_to_hand.yaml），
|
||
# 仿真自动生成的这份若叫 d435_...，会和真机的手外标定**抢同一个文件名** ——
|
||
# 后果是"跑一次仿真把真机的标定覆盖了"，而标定文件平时没人看。
|
||
# frame 名也必须跟着改（默认 <ros_ns>_color_optical_frame，应用按这个查 TF）。
|
||
ros_ns: sim_d435
|
||
link_frame: sim_d435_link
|
||
optical_frame: sim_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 的说明
|