# =============================================================================
# devices/sim/gazebo/config/scene.yaml
#
# 仿真场景：**谁摆在台面上的哪里**。
#
# 为什么单独一份文件，而不是把位姿写进模型或 world：
#     模型描述"这个东西长什么样"，场景描述"它现在在哪"。
#     分开之后，同一个模型可以在场景里出现多次、可以在不改模型的情况下换位置，
#     也才能在真机现场对照着调整（"现场工件是摆在偏左还是偏右？"）。
#     world 文件里只放**固定不动**的东西（地面、台子、眼在手外的相机）。
#
# ⚠️ 位姿是**绝对**坐标（世界系、米），不是相对台面的偏移。
#    仿真世界没有"台面坐标系"这种东西，写相对量只会多一层换算、多一个出错点。
#    下面的 z 值都是"物体的原点"。本框架约定：模型的局部原点 = 它**底面的中心**
#    （工件的板底、通止规的规尖），所以"放在台面上"就是 z = 台面高度。
#    这条约定让场景文件读起来最直观，代价是建模时必须把原点设在底部 ——
#    用 SolidWorks/Blender 导出网格前先把原点挪到底面中心。
# =============================================================================

# 场景模型所在目录（相对仓库根）。每个子目录一个模型：<名字>/model.sdf + model.config
models_dir: devices/sim/gazebo/models

# ---------------------------------------------------------------------------
# 台面几何 —— 用于校验"模型确实摆在台面上、没悬空也没穿模"
# ---------------------------------------------------------------------------
# 这些值必须与 worlds/assembly.world 里的 work_table 一致。
# 不一致时脚本会报错（scripts/check_sim_scene.py），而不是让你在现场猜。
# 台面：pose(0.55,0,0) + link top 在 z=0.375 + box 厚 0.05 → 顶面 0.40
#       box 尺寸 0.8(x) × 1.2(y)
table_top_z: 0.40
table_bounds:
  x: [0.15, 0.95]
  y: [-0.60, 0.60]

# ---------------------------------------------------------------------------
# 作业条件 —— 校验"仿真里的成像几何与真机一致"
# ---------------------------------------------------------------------------
# 二阶段的孔检测是**像素门槛**（hough_min_radius / hole_distance_min …），
# 它们只在"相机到工件表面距离"合适时才有意义。所以这个距离必须在仿真里
# 显式复现，不能靠"看起来差不多"。
#
# ⚠️ 真机与仿真的**悬停高度不一样**，别照抄：
#     真机 site.yaml：robots[].servo.hole_hover_z_m = 0.23（法兰相对基座）
#     仿真需要：约 0.56（法兰相对世界，算下来见下）
#   原因是两者的"基座与台面的相对关系"不同：真机是机器人装在台面同一高度上
#   （法兰比台面高 23cm 就等于离工件 23cm），而仿真里机械臂站在地面、
#   台面在 0.40m 高。**相机到工件的距离**才是决定成像的东西，绝对高度不是。
#   这个差异目前是**已知且未解决**的（要让两套参数完全通用，得把仿真的台面
#   降到与真机一致，那会连带改 D435 的位姿和相机验收基线）——
#   详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。
operating:
  # 用哪台相机做孔检测（决定用哪组内参算像素尺度）
  camera: sim_d405
  # 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。
  # 这里写的是物体的 name（不是模型名）。
  part_object: workpiece
  # 相机光心到工件**上表面**的距离 [m]。
  # 这个值不是量出来的，是**两条约束夹出来的**：
  #
  #   (a) 像素门槛（来自 hole_detect.yaml，见下面 gates_px）
  #         亮环 φ14（半径 7mm）要落在 10~30px      → 距离 ∈ [0.104, 0.313]
  #         两孔间距 58mm 要 ≥ 90px                 → 距离 ≤ 0.288
  #       取中段 ⇒ 约 0.17~0.25 都行。
  #
  #   (b) 机械臂够得着（site.yaml 的 servo.workspace_xyz_max 是 z=0.65）
  #         仿真里法兰高度 = 工件上表面(0.412) + 本值 + 相机伸出量(0.05)
  #         本值 0.17 → 法兰 0.632，离上界还有 18mm 余量。
  #         本值 0.22 → 法兰 0.682，**超出工作空间**，二阶段会直接拒绝执行。
  #
  # 两条约束交出来的取值，恰好也解释了真机的数：
  #   真机 hole_hover_z_m=0.23（法兰相对基座）、相机比法兰靠下 0.05、
  #   台面与基座同一水平面、板上表面 0.012 ⇒ 相机到工件 = 0.23-0.05-0.012
  #   = 0.168 ≈ 0.17。所以 0.17 不是凑出来的，而是与真机的悬停高度自洽的。
  camera_to_part_m: 0.17
  # 相机相对法兰沿工具轴的伸出量 [m]。arm.sim.sensors.xacro 里 d405_z=0.05。
  # 竖直朝下装配时工具轴朝下，所以相机比法兰更靠近工件这么多。
  camera_offset_below_flange_m: 0.05

# ---------------------------------------------------------------------------
# 孔检测的像素门槛 —— 用于在**启动时**就算出"仿真里这些特征能否被检测到"
# ---------------------------------------------------------------------------
# ⚠️ 这里的值必须与真机的检测配置一致：
#       src/arm_application/config/hole_detect.yaml
#       config/site.yaml 里 hole_detect.* 对它的覆盖（site.yaml 优先）
#    抄一份的原因：那些门槛是"像素"量，只有在仿真侧算出像素值才能提前发现
#    "孔间距不够/环太小"，不必等上真机。代价是两份数字可能漂移 ——
#    所以 scripts/check_sim_scene.py 会读那两个文件做**交叉核对**，
#    不一致直接报错。改门槛时改真机那份，然后同步到这里（脚本会提醒）。
#
# 键名必须与模型里 visual 的名字约定对应（见 sim_scene.features_from_workpiece）：
#     insert_ring_radius  嵌件亮环半径（模型里 insert_*_ring 的 radius）
#     hole_spacing        两孔中心距（两个 insert_*_ring 的 pose 距离）
#
# 半径区间取 [6, 30]：这是 config/site.yaml **实际生效**的值
# （site.yaml 覆盖了包内 hole_detect.yaml 的 10/30；生效规则是 site 优先，
#  见 hole_detector.config_from_site）。脚本会读那两个文件做交叉核对，
# 不一致直接失败 —— 所以这里永远等于真机在用的门槛，不需要人工同步。
gates_px:
  insert_ring_radius: [6, 30]
  hole_spacing: [90, 700]

# ---------------------------------------------------------------------------
# 世界模型位姿回读 —— 仿真里唯一能发现"物体被撞走/掉下去了"的手段
# ---------------------------------------------------------------------------
# 为什么需要它（2026-09-24 花了一整轮排查换来的，值得完整读一遍）：
#     `perception.source=truth` 是**纯几何**的：它按下面 objects 里的**名义位姿**
#     把孔投到像素上，**从不问 Gazebo 物体现在到底在哪**。
#     于是工件一旦被碰走，"真值"就开始撒谎，而且每一处现象都指向别的原因：
#
#         实测：工件掉进台面通槽、落到地面（z≈0），而 scene.yaml 仍写着 z=0.40。
#         S05 拿"名义孔位"的像素去查**真实**深度图 → 孔位处看到的是槽底/地面
#         （0.6m，而工件面其实只有 0.21m）→ 三角化出 k1/k2 间距 169mm
#         （真值 58mm）→ 被间距门槛丢掉 → 精拍连丢 9 拍 → S05 超时 ABORT。
#         现场现象像"检测坏了 / 标定坏了 / 阈值调高了"，全都不是。
#         真正的信息只有一句："**工件现在不在 scene.yaml 说的位置**。"
#
# Gazebo 自己会把每个模型的实际位姿发在 /world/<world>/pose/info
# （gz.msgs.Pose_V），桥成 tf2_msgs/TFMessage 之后，漂移监视器
# （sim_truth_perception）就能把"名义位姿 vs 实际位姿"直接对比出来并**指名报错**。
#
# 话题名里的 world 名必须与 worlds/assembly.world 的文件名一致（`assembly`）。
pose_info:
  gz_topic: "/world/assembly/pose/info"
  ros_topic: "/sim/world_pose_info"
  # 类型固定这两条：ros_gz_bridge 原生支持 Pose_V ↔ TFMessage（实测确认，
  # 见 ros_gz_sim_demos 的 tf_bridge.launch.py）。写错类型不会有任何报错，
  # 只会"桥接器起来了但一个消息都没有" —— 所以别改。
  ros_type: "tf2_msgs/msg/TFMessage"
  gz_type: "gz.msgs.Pose_V"
  # 漂移判据：**两个**阈值，别合成一个（2026-09-24 的教训）。
  #
  # drift_tol_m（2mm）= **告警**线：真值几何已经跟着**实测**位姿走
  #     （见 arm_sim_gazebo/sim_live_pose.py），所以 2mm 不再意味着"几何错了"，
  #     只意味着"这个场景跟 scene.yaml 写的不一样了"。而 2~3mm 是**天天都有**的：
  #     spawn 落地就偏 2.6mm，工件骑在通槽上静置十几分钟还会再挪 1.9mm。
  #     2mm 的来历：真值投影在相机到工件 0.17m 处，fx≈447 ⇒ 1mm 位姿误差 ≈ 2.6px，
  #     而 S05 的稳定判据是 2px —— 2mm 是"开始影响二阶段"的量级。
  #     yaw 取 2°：两孔间距 58mm，绕中心转 2° 会让孔横移 1mm（约 2.6px），同量级。
  drift_tol_m: 0.002
  drift_tol_deg: 2.0
  # drift_stop_m 才是**停发**线：到这个量级说明工件掉了/被搬走了，场景前提没了
  # （再发点等于指挥机械臂去装一个不存在的工件）。20mm 的来历：工件一半宽 70mm、
  # 掉下台面是 400mm 量级，而"顺手碰一下"是几毫米 —— 20mm 两边都分得开。
  drift_stop_m: 0.02
  # 启动复位：开局把被盯的物体 set_pose 回名义位姿（仿真专有，默认关，这里开）。
  #    为什么还要它：上面两条线是"容差"，这一条是**消除源头** ——
  #    物体是"在名义位姿上方 spawn、再落下来"的，落地那 2.6mm 偏差纯属 spawn
  #    的落点问题（实测 set_pose 回名义位姿后它就停在那儿不动）。
  #    只在开局 20s 窗口内、且只做一次 —— 运行中的漂移必须报错，不能悄悄搬回去
  #    （那会抹掉"谁把它撞歪了"的现场，还可能把工件从爪里拽走）。
  #    实现见 sim_truth_perception._maybe_startup_reset（含完整理由）。
  reset_on_start: true

# ---------------------------------------------------------------------------
# 场景物体
# ---------------------------------------------------------------------------
# name   = 进 Gazebo 后的实体名（也是 `ign model -m <name>` 用的名字）。
# model  = models_dir 下的子目录名 / model.config 里的 <name>。
# yaw    = 绕世界 Z 的旋转 [rad]。工件的 yaw 决定两个孔的连线方向，
#          而两孔连线方向会直接影响 stage2 的 k1/k2 判定（k1_label: left/right）
#          —— 换 yaw 后要重新确认哪边是"画面里的左孔"。
objects:
  - name: workpiece
    model: workpiece
    # 摆在工作台长度方向的中间偏左（靠近机械臂一侧）：
    #   太靠外（x 大）→ 手臂要伸直，接近工作空间边缘，柔顺下压时余量小
    #   太靠里（x 小）→ 会被手臂本体挡住 D435 的视线（粗定位就瞎了）
    # x=0.52 是两者折中；校验脚本会把工件四角反投影到 D435 画面里确认没被裁掉。
    pose: {x: 0.52, y: 0.00, z: 0.40, yaw: 0.0}
    note: >
      工件。z=0.40 = 台面顶面，板底贴台面。
      工件**上表面**在 0.40+0.012=0.412，二阶段的对孔基准面就是它。

  - name: gauge_stand
    model: gauge_stand
    # 与规**同心**：规座就在规的下面，规插在它的方孔里。
    # 座高 32mm，所以规的 z = 台面 + 32 - 30（通端长 30）= 0.402，
    # 也就是"台肩正好坐在座顶面上、规尖离台面 2mm"。
    # 这个 30mm 不是随手写的：它来自 gauge/model.sdf 里通端的长度，
    # 改模型时必须一起改（改错了的表现是规穿进台面或悬在座里没坐实）。
    pose: {x: 0.30, y: 0.28, z: 0.40, yaw: 0.0}
    note: >
      规座（工装，静态）。**必须在规之前 spawn**（objects 里的顺序就是 spawn 顺序）：
      否则规先落到台面上、规座再出现在它身体里 —— 两个刚体互相嵌入，
      求解器会把规弹飞，而现象看起来像"规自己会跳"。

  - name: gauge
    model: gauge
    # 立在规座里、工件旁边偏机械臂一侧，方便夹爪从上往下夹住夹持段。
    # y=0.28 是避开工件与 D435 视线的一侧（工件在 y=0 附近）。
    # z 见 gauge_stand 的说明：0.402 = 座顶(0.432) - 通端长(0.030)。
    pose: {x: 0.30, y: 0.28, z: 0.402, yaw: 0.0}
    note: >
      通止规。规尖在 z=0.402（原点在规尖），夹持段在 0.435~0.480。
      本阶段它只是"插在规座里立着"；把它夹起来是 P6 的活（P6 会用
      GripperExecute 指令夹爪开到 ~35mm、伸到夹持段、再闭合到 ~29mm）。
      真机上规是**人工放进夹爪**的（P6 里有提示交互），仿真里先立在座上，
      便于确认夹爪能不能够到、开合范围够不够。