# =============================================================================
# site.example.yaml — 新环境部署模板
#
# ！本文件由 scripts/gen_site_example.py 从 config/site.yaml 自动生成，请勿手改。
#   要改模板内容，请改 config/site.yaml（它是唯一权威）后重跑：
#       python3 scripts/gen_site_example.py
#   或在改完 site.yaml 后跑一次以确认模板同步：
#       python3 scripts/gen_site_example.py --check
#
# 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
#       现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
#
# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
#       部署时请改成实际用户的 home 目录。
# =============================================================================

# =============================================================================
# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制而来）
# 路径：~/ros2/config/site.yaml
#
# 89 与 91 的手眼 / 腕部相机 / 夹爪 / 点位互相独立，禁止混用。
# 切换机械臂：改 robot.sn，或所有终端加同一 --arm 89|91。
# =============================================================================

robot:
  # >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
  sn: "Rizon4-XXXXXX"
  # Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
  tool_name: ""
  # 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
  robots:
    - sn: "Rizon4-XXXXXX"
      arm: left
      ip: "192.168.2.XXX"
      pc_ip: "192.168.2.XXX"
      iface: "enp4s0"
      label: "左臂89"
      # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
      # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
      tactile: true
      camera:
        name: d405_left          # 话题 /d405_left/... 与 91 分开
        serial: "<序列号>"   # 左腕 D405（一直在用的那台）
        auto_exposure: true
        ae_limit_us: 4000
        emitter_enabled: false
      gripper:
        model: "PGEA-100-20-W"
        sn: "<序列号>"
        port: "/dev/ttyUSB0"
        stroke_mm: 20.0
      named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
      paths:
        handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
        handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
        handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
      # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
      # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
      # 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
      servo: &servo_left
        ee_z_m: 0.35           # 跟踪锁高：法兰相对基座 Z [m]
        min_ee_z_m: 0.20
        safe_z_m: 0.45
        z_min_m: 0.22
        xy_track: hover        # 法兰(夹爪中心) XY 压在工件上，不是画面中心
        ee_vertical_down: true # 法兰 +Z 朝世界 −Z
        hover_offset_ee_xy_m: [0.0, 0.0]  # 爪心相对法兰 XY；平行爪同轴保持 0
        # >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
        # 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
        # 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
        hole_hover_z_m: 0.23   # ← 二阶段悬停高度调这一行
    # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
    # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
    #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
    # 名带 `sim_` 前缀（与真机区分，避免仿真数据冒充真机），所以它需要自己的一台。
    # 登记之后，**所有阶段（stage2/stage3/装配…）都不改一行代码**就能跑仿真，
    # 只要 `--arm sim`（`scripts/run_gauge_assembly.sh --sim` 会自动选它）。
    #
    # 真机上永远不会选到它：只有显式 `--arm sim` 才会，而没有任何真机脚本会这么写。
    - sn: "sim"
      arm: sim
      label: "仿真台"
      # 仿真建模的就是左臂，触觉也只有左指（由 sim_tactile_adapter 发）
      tactile: true
      camera:
        # 话题 /sim_d405/... ：与真机的 /d405_left 分开。为什么不用同一个名字：
        # 同域里两个来源会互相覆盖，"台上这台到底是谁"就说不清了。
        # 名字里的 d405 与 URDF 里的模型名一致 → 光学系 sim_d405_color_optical_frame
        # 正好等于 `{camera.name}_color_optical_frame`，所以应用查 TF 不用特判。
        name: sim_d405
      # 仿真不需要手眼标定文件：腕部相机在 URDF 里就装在法兰上，
      # 而真机那份标定文件描述的是"相机相对法兰的实际安装偏差"，
      # 套到仿真上会把相机搬到一个**错误**的位置（投影全错）。
      # 留空 → 启动脚本不发静态 TF（见 gauge_assembly.launch.py 的提示）。
      paths:
        # 仿真不"标定"，而是把 URDF 里的真值**导出**成同格式的文件：
        # scripts/gen_sim_handeye.py（run_gauge_assembly.sh --sim 会自动跑）。
        # 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
        # T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
        #（"手眼标定结果缺失"），S04 跟踪起不来。
        handeye_file: "/home/USER/ros2/handeye/results/sim_eye_in_hand.yaml"
      # 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
      #
      # 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
      # 就等于"法兰相对台面"（hole_hover_z_m=0.23 就是这么来的）。
      # 仿真里机械臂站在**地面**、台面 0.40m 高、工件上表面 0.412m —— 一切 Z
      # 都比真机大 0.412。照抄真实值会让伺服把法兰送到 0.28m，也就是**撞进台面**。
      #
      # 推导（与 scene.yaml 的 operating 段同源，scripts/check_sim_scene.py 会核对）：
      #   相机到工件 0.17（像素门槛反推，不能改）
      #   + 相机比法兰低 0.05（URDF 里 d405_z=0.05）
      #   + 工件上表面 0.412
      #   ⇒ 二阶段悬停法兰高 = 0.632
      #   一阶段跟踪高 = 悬停 + 0.05（通止规离工件）= 0.682
      # 所以工作盒上界必须 ≥ 0.682 —— 真机那套上界 0.65 不够，S04 一跟踪就会被
      # "超出工作盒"截断（现象是"越跟踪越偏"，很难想到是配置差 3cm）。
      servo:
        <<: *servo_left
        ee_z_m: 0.68              # 一阶段跟踪锁高（法兰，世界系）
        hole_hover_z_m: 0.632      # 二阶段悬停高：= 0.412 + 0.17 + 0.05
        safe_z_m: 0.75             # 安全抬升/复位高度（高于跟踪高）
        z_ref_m: 0.632             # 没有测量时的兜底 Z：绝不能是 0.22（会撞工件）
        z_min_m: 0.55              # 伺服硬下限：0.55 = 工件上表面 + 0.14
        min_ee_z_m: 0.55
        workspace_xyz_min: [0.08, -0.55, 0.55]
        workspace_xyz_max: [0.85, 0.55, 0.75]
      # 仿真不跑推理：感知结果由 sim_truth_perception 按已知位姿直接给出。
      # 原因：ros_gz_sim 环境里没有 torch/ultralytics，且本机离线装不上 ——
      # perception_node 一启动就死，表现为"一阶段永远不锁定"。
      # 详见 devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」。
      perception:
        source: "truth"
    - sn: "Rizon4-XXXXXX"
      arm: right
      ip: "192.168.2.XXX"
      pc_ip: "192.168.2.XXX"
      iface: "enp4s0"
      label: "右臂91"
      camera:
        name: d405_right         # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
        serial: "<序列号>"   # 右腕新 D405
        auto_exposure: true
        ae_limit_us: 0           # 0=不封顶，让固件自己调亮
        emitter_enabled: false
        laser_power: 0
      gripper:
        model: "RGI-100-30-O-S-M1"   # 旋转爪：run_gripper.sh --arm 91
        sn: ""
        port: "/dev/ttyUSB1"
        stroke_mm: 30.0
        auto_init: false
      named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
      paths:
        handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml"
        handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml"
        handeye_workdir: "/home/USER/ros2/handeye/tmp/91"
      # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
      # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
      #   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
      servo:
        ee_z_m: 0.25           # 跟踪锁高：法兰相对基座 Z [m]  ← 日常调这一行
        min_ee_z_m: 0.20
        safe_z_m: 0.45
        z_min_m: 0.22
        xy_track: hover        # 用法兰系手眼 T：夹爪中心(法兰原点)压在工件正上方
        ee_vertical_down: true # 法兰/RGI 轴线朝下；相机有 29° 倾角，绿框不必对准十字
        hover_offset_ee_xy_m: [0.0, 0.0]  # RGI 与法兰同轴；爪心偏了再改这两项 [m]
        # 二阶段孔定位悬停锁高（91 装了传感器后同样可用）
        hole_hover_z_m: 0.22
        # --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方

paths:
  # >>> YOLO 权重（标准放法，不必改代码）<<<
  #   1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
  #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
  #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
  #        bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
  #   4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
  #   5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
  #   6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
  # 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
  yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
  # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
  # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
  # 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
  # 原先指向 hole_insert/best.pt —— 那个文件不存在（悬空引用）。这里改成实际存在的
  # detect_best_1.onnx，这样 backend: yolo 至少是能跑的。查悬空引用：
  #   bash ~/ros2/scripts/weights.sh doctor
  hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
  # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
  handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
  handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
  handeye_calibrate: "/home/USER/ros2/handeye/calibrate.yaml"
  handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
  # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
  fastdds_profile: ""

handeye:
  mode: "eye_in_hand"
  # >>> 标定板尺寸（内角点数 + 方格边长 m）；改这里即可 <<<
  # 例：A5 板 12×17 方格 → 内角点 11×16，方格 11 mm
  board_cols: 8          # 内角点列数 (方格列数 - 1)
  board_rows: 11          # 内角点行数 (方格行数 - 1)
  square_size_m: 0.015    # 方格边长 [m]
  # 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照
  velocity_scale: 0.05
  move_time_sec: 8.0
  settle_sec: 1.5
  settle_qd_max: 0.015     # |qd|_max 低于此值才算静
  settle_stable_sec: 0.6
  settle_timeout_sec: 8.0
  pin_holds: 2
  pin_hold_sec: 0.45
  skip_unstable_poses: true   # 某点看不到棋盘则跳过，不整段中止
  # 点位：paths.handeye_waypoints

camera:
  name: "d405"
  serial: "<序列号>"  # 全局缺省=左腕；真机以 robots[].camera.serial 为准
  # USB3 推荐 848x480@30；USB2 自动降到 640x480@15，深度/右目仍开
  width: 848
  height: 480              # 改宽高/帧率必须重启相机：bash ~/ros2/scripts/run_camera_d405.sh --preview
  fps: 30
  enable_depth: true
  enable_infra: true      # 同一 SDK 进程开左右红外；失败则自动退回彩图+深度
  preview: true
  preview_stereo: true    # 预览同时显示 彩图 | 深度 | 左目 | 右目（不另开相机、不抢 USB）
  overlay_hold_sec: 0.10  # 预览只留很短；长残留会看起来像绿框不跟
  use_site_resolution: true
  auto_exposure: true
  ae_limit_us: 4000       # D405 腕部近距；91 在 robots[] 里更严
  emitter_enabled: false  # 腕部关红外投射，否则彩图发白
  laser_power: 0

# 现场三台 RealSense（按序列号钉死，禁止按「第一台 D405」乱开）
cameras:
  d405_left:
    model: d405
    name: d405_left
    serial: "<序列号>"
    role: left_wrist
    arm: left
    label: 左腕D405（89 平行夹爪）
    width: 848                 # 相机分辨率以本段为准（全局 camera: 段为旧路径，P2 收敛）
    height: 480
    emitter_enabled: false     # 腕部必须关投射器，红外点阵会污染跟踪图像
  d405_right:
    model: d405
    name: d405_right
    serial: "<序列号>"
    role: right_wrist
    arm: right
    label: 右腕D405（91 旋转夹爪）
    width: 848
    height: 480
    emitter_enabled: false
  # >>> 仿真那两台（arm: sim）<<<
  # 为什么仿真也要在这里登记：应用是"按当前臂"去认相机的 —— 腕部相机取
  # robots[].camera.name，手外相机取这里 role=overhead* 且 arm 与当前臂一致的
  # 那一条（见 gauge_assembly_node._d435_ns）。名字必须与
  # devices/sim/gazebo/config/cameras.yaml 的 ros_ns 一模一样。
  # 名字不带 d405_left/d435 那样的真机名：同域里两个来源会互相覆盖，
  # 而 eye-to-hand 标定文件是按相机名命名的 —— 重名会让"跑一次仿真把真机标定盖掉"。
  sim_d405:
    model: d405
    name: sim_d405
    role: sim_wrist
    arm: sim
    label: 仿真腕部D405（= 左臂位置）
    width: 848
    height: 480
    emitter_enabled: false
  sim_d435:
    model: d435
    name: sim_d435
    role: overhead_eye_to_hand
    arm: sim
    label: 仿真手外D435（世界固定）
    width: 640
    height: 480
    fps: 30
    emitter_enabled: true

  d435:
    model: d435
    serial: "<序列号>"
    role: overhead_eye_to_hand
    label: 双臂中间手外D435
    name: d435
    width: 1280
    height: 720
    fps: 30
    auto_exposure: true
    ae_limit_us: 8000
    emitter_enabled: true   # 顶视深度可用投射器；只看彩图可改 false


yolo:
  conf: 0.10               # 检测器门槛：略低于 min_conf，让运动模糊框也能进跟踪
  iou: 0.45
  imgsz: 640                 # 必须等于所用 ONNX 的输入边长（当前 detect_best_0 = 640）
  device: "0"
  half_precision: true
  backend: "tensorrt"        # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh
  mp_infer: true             # YOLO 独立进程，不和 ROS 抢 GIL
  min_conf: 0.12             # 新锁门槛；已锁定的框可略低于此
  vision_hz: 30.0
  infer_max_width: 640       # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵
  box_ema_alpha: 1.00          # 绿框用预测补偿后的框，不再 EMA
  miss_clear_frames: 10        # ~0.33 s @30 Hz；期间绿框按像素速度滑行
  overlay_hold_on_miss: true   # 滑行失败时仍续发上一框
  hold_point_on_miss: false    # 眼在手上：丢检续发旧点会让臂追错位置
  miss_hold_point_frames: 1
  jump_px: 400.0
  jump_z_m: 0.10
  z_ref_m: 0.22
  min_depth_m: 0.04
  max_depth_m: 0.55
  work_z_max_m: 0.50
  meas_blend: 0.85             # 3D 跟手；抖则略降
  z_alpha_min: 0.10
  z_alpha_max: 0.40
  z_alpha_gain: 1.0
  camera_frame: "d405_color_optical_frame"
  object_frame: "tracked_object"
  depth_topic: "/d405/aligned_depth_to_color/image_raw"
  feature_mode: "plane"    # plane=框内近表面立体点（工件本体）；box=单目射线
  plane_inlier_m: 0.008    # 拟合平面内点阈值 [m]
  plane_band_m: 0.06       # 近表面深度带宽 [m]

servo:
  control_hz: 100.0
  # --- 基座坐标系下的末端约束（法兰，单位 m）---
  ee_x_m: 0.25
  ee_y_m: 0.00
  ee_z_m: 0.35             # 全局缺省锁高；真机以 robots[].servo.ee_z_m 为准（89/91 分开调）
  ee_hold_xy: false          # false=跟踪工件 XY；true=锁死 ee_x/ee_y
  xy_track: "hover"          # hover=法兰悬停在目标正上方（任意锁高）  lookat=画面中心对准目标
  use_static_handeye_tf: false  # 只用 handeye_result.yaml，避免 TF 树抢父子帧
  ee_vertical_down: true
  # --- 启动回初始位（已停用：--control 不再调用 goto_ee_plane）---
  safe_z_m: 0.45               # >>> 防撞安全高度：先纯 Z 抬到这里再回 home，避免扫到料框/壁
  home_pose_name: "home"       # 非夕初始点位（/set_named_pose：home | ready | inspect | pos_left）
  home_pose_speed: 0.30        # 回初始位的速度比例 (0,1]，越小越稳
  ee_hold_kp: 2.0
  ee_w_max: 0.60
  z_ref_m: 0.22
  z_min_m: 0.22
  approach_xy_gate_m: 0.040  # --control 先降到锁高，|ee_z-ee_z_m| 小于此值才开始 XY hover
  v_approach_z: 0.025
  watchdog_timeout_sec: 1.2
  lost_timeout_sec: 1.2
  stale_hold_sec: 0.35             # 丢检空窗内用基座 KF 预测继续跟（自运动安全），超过才钉住
  max_extrapolate_sec: 0.35   # 预测时间上限（图像延迟 + 前瞻）
  predict_enable: true        # 基座系恒速预测，补视觉延迟
  predict_sec: 0.16           # 额外前瞻 [s]
  predict_v_max: 0.80         # 工件速度上限 [m/s]，防乱飞
  predict_jump_m: 0.08        # 基座 XY 跳变大于此则重锁速度
  kp_xy: 4.20                # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5
  ki_xy: 0.00
  kd_xy: 0.20
  kp_z: 0.80
  ki_z: 0.00
  kd_z: 0.35
  i_sep_pos_m: 0.008
  deadzone_m: 0.003          # 3 mm；收紧提高对准精度。若抖动调回 0.006
  deadzone_z_m: 0.012
  lock_hold_sec: 0.7        # 死区驻留多久才发 /visual_servo/locked（串联 0.6s 已够）
  e_xy_clip_m: 0.200         # 大误差仍按 v_max 走，靠 a_max 加速
  v_max_xy: 0.45             # 法兰 XY 上限 450 mm/s
  v_max_z: 0.04
  min_ee_z_m: 0.20
  workspace_xyz_min: [0.08, -0.55, 0.18]   # 法兰工作盒；盒外只允许往回走，不再卡死 no traj
  workspace_xyz_max: [0.85, 0.55, 0.65]
  a_max: 3.00                # 加速度上限（真正作用在 hover 指令上）
  vel_lpf_alpha: 0.80        # 速度低通：越大越跟手（1=不滤波）
  dq_window_size: 1          # 1=不做滑窗平均，去掉 ~20ms 滞后
  traj_stream_hz: 50.0       # 轨迹点流率：JTC open_loop+样条，吃得起
  exec_horizon_sec: 0.08     # 每个轨迹点的执行窗口
  max_qdot: 2.00             # 关节速度上限 [rad/s]
  min_cmd_delta_rad: 0.001
  point_topic: "visual_servo/filtered_point"
  traj_topic: "rizon_arm_controller/joint_trajectory"

# 大寰夹爪：官方 Modbus RTU，USB–RS485 插 IPC，不走 Studio/RDK。
# 左 89 PGEA → /dev/ttyUSB0；右 91 RGI → /dev/ttyUSB1。
#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open|close|pos
#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|close|rotate 90
#   bash ~/ros2/scripts/run_dual_gripper.sh
# ROS：/gripper/open|close（PGEA 单爪）或 /dual_gripper_driver/{left,right}/execute
gripper:
  backend: "usb"              # usb=工控机官方Modbus  rdk=Studio设备（不用）
  vendor: "dh"
  model: "PGEA-100-20-W"
  sn: "<序列号>"              # 铭牌，不进协议
  device_name: "GripperDahuanModbus"
  port: "/dev/ttyUSB0"        # 本机 MACROSILICON/pl2303
  baud: 115200                # 官方默认 115200 8N1
  slave_id: 1                 # 官方默认从站 1
  rs485_rts: ""               # 空=不拉RTS；scan 若 OK 在 tx/tx_inv 再填
  stroke_mm: 20.0
  # --- 开合 / 力 / 速（大寰官方寄存器，超出会被驱动钳位）---
  force_pct: 20               # 夹持力  区间 20–100 %（低于 20 会被钳到 20；PGEA-100 约 30–100 N）
  speed_pct: 50               # 开合速度 区间 1–100 %
  open_position: 1000         # 张开目标 区间 0–1000 ‰（1000=全开）
  close_position: 0           # 闭合目标 区间 0–1000 ‰（0=全合；留缝改 50–200）
  auto_init: true
  state_hz: 20.0
  # 双爪共用参数（run_dual_gripper.sh / load_from_site）
  parallel:
    stroke_min: 0.0
    stroke_max: 20.0
    default_speed: 50
    default_force: 20
    force_min: 20
    force_max: 100
    auto_initialize: true
  rotary:
    auto_initialize: false
    angle_min: -32767.0
    angle_max: 32767.0
    max_step_deg: 30000.0
    finger_open_per_mille: 1000
    finger_close_per_mille: 0
    default_speed: 50
    default_torque: 50
    default_finger_speed: 50
    default_finger_force: 50
    torque_min: 20
    torque_max: 100
  # RDK 回退字段（backend: rdk 才用）
  force_n: 50.0
  vel_mps: 0.05

# 入孔检测。算法参数写在这里（覆盖包内 hole_detect.yaml 默认值）。
#   bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
hole_detect:
  backend: "two_stage"   # two_stage | yolo | hough
  # backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。
  weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
  # 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
  workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
  config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
  conf: 0.25
  iou: 0.7
  imgsz: 640
  center_gate_enable: true
  center_max_offset_ratio_x: 0.15
  center_max_offset_ratio_y: 0.18
  center_gate_draw: true
  refine_hole_centers: true
  bbox_padding: 80
  gaussian_ksize: 5
  gaussian_sigma: 1.5
  hough_dp: 1.2
  hough_min_dist: 50
  hough_param1: 100
  hough_param2: 26
  hough_min_radius: 6      # 悬停较高时螺纹孔半径只有几像素
  hough_max_radius: 30
  min_radius: 6
  max_radius: 30
  hole_distance_min: 90    # 悬停 0.23m 两螺纹孔约 170px；过小会配到中心孔
  hole_distance_max: 700
  expected_holes: 2
  min_hole_sep_px: 20
  max_hole_sep_px: 2000
  stabilize_hold_frames: 15
  stabilize_ema_alpha: 0.35
  stabilize_max_jump: 60
  default_depth_m: 0.10
  camera_intrinsics:
    fx: 425.0
    fy: 425.0
    cx: 640.0
    cy: 360.0
  # ===== 二阶段一次性拍照定位（stage2_locate_node，不做实时推理）=====
  # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89            # 完整流程
  # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only  # 只悬停调高度
  stage2:
    target_hole: "k1"        # 对准目标孔：k1=图像左孔 / k2=右孔（见 k1_label）
    k1_label: "left"         # k1 是画面里的左孔；现场相反就改 right
    align_tol_m: 0.0007      # 对准到位判据 [m]
    align_max_iters: 8       # 拍照→修正→复拍 迭代上限
    coarse_max_moves: 2      # 工件粗居中（bbox 中心）移动上限
    # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
    align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
    move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
    final_gap_mm: 2.0        # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
    final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
    final_descent_enable: true  # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
    verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
    settle_sec: 0.45         # 到位后静止多久再拍照
    snap_frames: 5           # 每次精拍取中值的帧数
    min_depth_m: 0.06        # 有效深度范围（防孔底/桌面拉偏 XY）
    max_depth_m: 0.60
    linger_sec: 8.0          # 完成后继续发布结果/叠图的时长

  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89                # 搜孔+落座
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心不插入
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # 螺纹规旋入
  stage3:
    # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
    #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
    touch_delta: 250.0       # 触面判定：fnormal > μ0 + max(4σ0, 本值)
    press_delta: 200.0       # 轻触目标按压量（触面阈值之上再加这么多）
    max_press_delta: 900.0   # 螺旋中允许的最大按压（超过→力闭环抬起）
    safe_press_delta: 2000.0 # 安全上限：fnormal > μ0+本值 立即抬升中止
    shear_delta: 600.0       # 切向拖拽保护：shear > 静止切向均值+本值 → 贴面时微抬 0.15mm（Z 钉在触面）
    insert_press_delta: 1200.0  # 插入力上限（触面阈值之上；超过回退微抖重试）
    kp_z_mm_per_unit: 0.001  # Z 力闭环增益：每单位读数偏差调整的毫米数
    approach_step_mm: 0.8    # 触面前每步下降量（驱动死区已修，0.5mm 微步可跟踪；小步=触面冲击小）
    enter_probe_mm: 2.0      # 触面深于此值→疑似直接入孔，四向微探验证后跳过螺旋
    mouth_lift_mm: 2.0       # 直接入孔后抬到孔口倒角段再定心（深插横探会别死孔壁）
    baseline_min: 8.0        # 触觉基线健康窗下限（<此值=几乎全 0，tare/断流）
    baseline_max: 1500.0     # 上限（>此值=近饱和，接触余量不足）——出窗即中止并提示重启触觉
    approach_ft_touch_n: 6.0 # approach 腕部 |F| 旁路触面判据（标定牛顿；自由空气≈2-3N）
    approach_max_mm: 60.0    # 从当前位姿最多下降这么多（找不到面就中止；悬停被抬高后仍有余量）
    spiral_pitch_mm: 1.0     # 螺旋螺距（< 通止规直径，保证重叠覆盖）
    spiral_step_mm: 0.5      # 沿螺旋的弧长步长
    spiral_r_max_mm: 4.0     # 最大搜索半径（覆盖二阶段残差 1.5mm + 余量）
    spiral_expand_max: 1     # 找不到孔时扩大半径的次数（残差≤1.8mm 时代码不再扩大）
    spiral_expand_step_mm: 2.0
    drop_ratio: 0.45         # 入孔判据：fnormal 相对接触均值跌落比例
    drop_frames: 2           # 跌落需持续的帧数（防毛刺）
    center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
    center_scan_mm: 0.3
    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
    insert_retry: 3
    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
    search_speed: 0.03       # 搜孔笛卡尔速度 [m/s]
    settle_sec: 0.25         # 每个微步后的静止时间
    tactile_timeout_s: 3.0   # 触觉话题断流多久中止
    ft_watchdog: true        # 订阅 /<sn>/flexiv_robot_states 做腕部力看门狗
    ft_safe_n: 35.0          # 腕部 |F| 安全上限（真牛顿，标定过的；连续3拍超限才中止）
    linger_sec: 8.0          # 完成后继续发布结果的时长

# Orisys 视触觉（夹爪左右指摄像机，不是腕部 D405）。SDK 单独 pip 安装。
#   bash ~/ros2/scripts/run_tactile.sh --cli
#   bash ~/ros2/scripts/run_tactile.sh
# =============================================================================
# 感知来源：伺服用的"视觉测量"由谁产生
#
#   yolo      本体 perception_node（YOLO/ONNX，需要 torch/ultralytics）—— 真机
#   truth     仿真真值：**整条视觉链都由场景几何算出**，不跑任何推理
#               · 一阶段目标点：arm_sim_gazebo 的 sim_truth_perception
#               · 二阶段孔位  ：arm_sim_gazebo 的 truth_holes（投影 <visual>*_ring）
#   external  由外部节点按**同一话题**（visual_servo/filtered_point）提供，
#             本框架不起感知进程（二阶段仍走检测器）
#
# ⚠️ 别和 yolo.backend 混了：那个是"**怎么**推理"（tensorrt/onnx），
#    这个是"**谁**给出测量"。
#
# 为什么需要这个开关：仿真环境（ros_gz_sim conda）没有 torch/ultralytics，
# 离线也装不上，perception_node 一起就死；但仿真里工件位姿本来就是已知的，
# 没必要也没法做推理。切到 truth 后，流程能在仿真里验"控制与接口"那一半，
# 而**感知那一半留给真机**——这条边界写在 devices/sim/gazebo/README.md 的
# 「仿真里哪些模块用真值替代」一节。
# =============================================================================
perception:
  source: "yolo"
  truth:
    # 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
    # "哪个是工件"是场景的知识，换场景不该改这里）。
    object: ""
    # 目标点相对物体**顶面中心**的偏移 [m]，在物体局部系里（会跟着 yaw 转）。
    # 例：把目标从板中心挪到 k1 孔 → [0.0, -0.029, 0.0]
    offset_m: [0.0, 0.0, 0.0]
# =============================================================================
# 通止规装配（看板 S00–S11）：bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89
#
# 这一段全是**会动的量**，也是现场最常改的地方。改完不必改代码：
# 解析在 arm_application/gauge_config.py（键名、单位、类型都在一张表里校验），
# 拼错键名会直接报错并给出最相近的候选名，不会静默退回默认值。
#
# 单位体现在键名后缀上（_deg/_mm/_m/_s/_pct/_delta），三张表逐条校验；
# 带 _delta 的是**力增量**（Orisys 读数是形变任意单位且基线会漂移，
# 绝对值没有意义），必须现场标定：
#     bash ~/ros2/scripts/run_tactile.sh --cli     # 看读数量级
# 启动时节点会打印本段**实际生效**的值（含派生结论），先核对再上电。
# =============================================================================
gauge_assembly:
  # ---- 总开关 ----
  holes_total: 2             # 一次任务装几个孔（k1/k2）；--holes N 可临时覆盖
  dry_run: false             # true = 只验编排与自检，不发任何运动指令
  # E1（掉件）重放时等人工确认 /gauge/reload_ok 的最长时间 [s]。超时=中止。
  # 不是"再等等"：臂停在 home，让人来处理比继续跑安全。
  reload_timeout_s: 180.0
  # S00 自检每项（相机/触觉/夹爪状态）等"第一帧真数据"的最长秒数。
  # 只看话题名会假阳性（发布者不在也算在），所以必须等真数据。
  boot_timeout_s: 30.0

  # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
  # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
  extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
  extreme_speed_deg_s: 20.0  # 关节角速度 [deg/s]，慢一点防冲击
  extreme_direction: 1       # +1=逆时针 / -1=顺时针（改符号，别去改代码）
  extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                             # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"

  # ---- S02 夹持确认（预留，默认跳过）----
  # 现在的触觉判据与 D435 手眼都还没就绪，先留接口：开成 true 时，
  # 夹爪状态字必须明确报"夹住了"，否则 S02 判失败并中止。
  grip_check: false
  grip_hold_s: 0.5           # 合爪后等多久再读状态 [s]

  # ---- S03 手外粗定位（S04 跟踪的兜底：跟踪丢了才来）----
  # 前提是手外相机做过 eye-to-hand 标定：handeye/results/<相机>_eye_to_hand.yaml。
  # 没有那个文件时 S03 会明确报"缺哪个文件、怎么生成"并回 S04，不做假装成功的反投影。
  coarse_enabled: true
  # 手外相机名：**以 cameras 段里 role=overhead* 且 arm 与本臂一致的那条为准**
  #（就是 cameras 注册表，见 gauge_assembly_node._d435_ns）。这一项只在
  # "注册表里没有手外相机"时当兜底；两者不一致会在 S00 打告警并以注册表为准。
  # 所以真机/仿真各跑各的时候**不用**改这里 —— 曾经写死 d435 让仿真去查真机的相机。
  coarse_camera: "d435"
  coarse_hover_m: 0.25       # 粗定位后悬停在工件上方多高 [m]（可调）
  coarse_conf: 0.35          # 工件检测置信度阈值（0~1）
  coarse_timeout_s: 20.0     # 等手外相机出图的最长秒数（相机刚起来要一两秒）

  # ---- S05 精拍定位：等画面稳了再拍 ----
  # 判据是"所有采样点到均值的距离 ≤ 容差"，不是"相邻两帧差 ≤ 容差"：
  # 跟踪抖动是零均值的，相邻帧之差会在画面明明在晃时判成稳定，拍出糊图。
  snap_stable_s: 1.0         # 连续观察多久 [s]
  snap_tol_px: 2.0           # 允许的抖动半径 [px]（越小越严，越难满）
  snap_tol_mm: 0.4           # 同上，但特征=法兰位移时的容差 [mm]
  snap_min_frames: 6         # 至少采到多少帧才敢说"稳定"
  # 用哪个特征判"画面稳定了"：
  #   auto   = 按 perception.source 自动选（真机 yolo→bbox；仿真真值/外部→motion）
  #   bbox   = 工件检测框中心（像素）。需要检测器真的出框：仿真里合成图过不了
  #            真机训练出来的模型（实测最高分 0.017），选它会让 S05 空转到超时
  #   motion = 法兰位移（mm）。相机装在法兰上，法兰不动画面就不动 —— 不依赖模型
  snap_stable_feature: "auto"

  # ---- S06 正悬停（悬停高度不在这里：见 servo.hole_hover_z_m）----
  align_tol_mm: 1.0          # 对位残差容差 [mm]；超了回 S05 重拍（不是硬往下走）
                             # 孔口径 mm 级时别放大这个值：放大=直接蹭孔壁

  # ---- S07 柔顺接触：步进下压，切向力突变即停 ----
  contact_speed_m_s: 0.02    # 每步下压速度 [m/s]；这一步必须慢，快了就是一次冲击
  contact_step_mm: 0.5       # 每步下压量 [mm]（越大越快，越小越不会过冲）
  contact_max_depth_mm: 35.0 # 最多下探多少 [mm]；到了还没接触判失败（不会一直压）
                             # 为什么是 35 而不是 25：判"到底了"的是**台肩落到工件
                             # 上表面**那一下，而台肩在规尖上方 30mm（通端长 30，
                             # 见 models/gauge/model.sdf）。工件板厚 12mm，所以要
                             # 下压 30mm 台肩才落面 —— 25mm 的上限根本到不了，
                             # 只会报"下探 25mm 都没触发接触判据"然后 ABORT。
                             # 35 = 30（台肩落面）+ 5（对位残差/工件厚度公差余量）。
  contact_tangential_delta: 300.0  # 接触判据：两路切向任一相对基线增量超过它 [任意单位]
  contact_safe_delta: 2000.0       # 法向安全上限 [任意单位]：超了立刻抬起并中止(E2)
                                   # 必须 > contact_tangential_delta，否则自检就报矛盾

  # ---- S08 棘轮拧入（慢转 → 松爪 → 快回转 → 夹紧，循环）----
  # 停止判据：切向力 > screw_shear_delta **且** 法向力突增 > screw_normal_delta。
  screw_step_deg: 180.0      # 每次慢转多少度；要 ≤ 预绕角（否则第一轮就贴限位）
  screw_speed_deg_s: 30.0    # 慢转速度 [deg/s]
  screw_back_speed_deg_s: 120.0  # 松爪后的空转回转速度 [deg/s]（爪里没东西，可以快）
  screw_max_cycles: 12       # 最多几轮（12×180°=2160°，够 M6 螺纹规走完）
  screw_normal_delta: 400.0  # 拧到位时的法向力突增 [任意单位]（螺纹到底/顶住）
  screw_shear_delta: 600.0   # 拧到位的切向力增量 [任意单位]
  screw_grip_force_pct: 50.0 # 夹紧力百分比 [%]（**只有契约后端生效**：仿真/新驱动）
  screw_grip_speed_pct: 50.0 # 夹紧速度百分比 [%]（同上；真机 dh_pgea 驱动不支持这两个参数）
  screw_release_wait_s: 0.3  # 松爪后等多久再回转 [s]（等爪真的张开，否则会带着规磨）

  # ---- S09 反向拧出 ----
  unscrew_extra_cycles: 1     # 比拧入多转几轮，确保螺纹完全脱开

  # ---- 力判据怎么调（现场最常问）----
  # 1) 先看量级：bash ~/ros2/scripts/run_tactile.sh --cli，手压一下看读数变化；
  # 2) S07 的接触判据是"相对基线的增量"，基线在 S06 悬停时（还没碰工件）测；
  # 3) 太灵敏（刚靠近就停）→ 调大 contact_tangential_delta；
  #    不灵敏（压很深才停）→ 调小；同时看启动日志里的基线/实测值再定。

tactile:
  backend: "orisys"          # orisys | mock
  # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
  model: "dd03"
  # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
  config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json"
  # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
  # 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
  left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
  right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
  cuda: false                # 两路进程抢 GPU 上下文会互相拖慢；CPU 解算仅 ~9ms/帧，足够
  isstitch: true
  infer_backend: "cpu"       # cupy | opencl | cpu；双路并行用 cpu 最稳
  baseline_seconds: 2.0
  filter_alpha: 0.25
  motion_threshold: 0.0
  target_fps: 30.0
  publish_hz: 20.0
  # 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。
  # 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列，
  # 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。
  image_hz: 0.0
  rotation_config: ""        # 左右共用；有分指文件则用下面两项
  left_rotation: ""          # 例：orisys SDK 的 rotation_config_0.json
  right_rotation: ""

go_home:
  pose: "home"
  speed: 0.25

network:
  # User1=enp4s0  本机 <PC_IP>  ↔  <左臂SN>=.101  /  <右臂SN>=.100
  prefer_iface: "enp4s0"

# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
sim:
  # 这一段由 arm_sim_gazebo.sim_site 真正读取（此前是"声明了但没人读"，
  # 改了毫无反应也不报错）。三个键的接线状况**不一样**，务必看清：
  #
  #   world           ✔ 已接线。优先级：命令行 --world > 这里 > 内置默认。
  #                     相对路径按**仓库根**解析。
  #   physics_enable  ✘ 未接线。世界文件里的重力/步长是写死的。若这里写的值
  #                     与世界的实际状态不符，启动时会打印告警（不再静默）。
  #   joint_prefix    ✘ 未接线。前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix
  #                     决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
  #                     写了个不同的值会告警。要真改，三处必须一起改。
  #
  # 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真；
  # 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
  platform: "gazebo"
  world: "devices/sim/gazebo/worlds/assembly.world"
  physics_enable: true
  joint_prefix: "sim_"       # 仿真关节前缀，必须与真实臂前缀区分