From 7d22f777c8f431c01a5b587dda5605736f049e50 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Fri, 9 Oct 2026 14:51:03 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8A=8A=E5=BD=93=E5=89=8D=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E6=A0=88=E6=94=B6=E6=88=90=E4=B8=80=E4=BB=BD=E5=8F=AF=E5=85=8B?= =?UTF-8?q?=E9=9A=86=E7=9A=84=E6=BA=90=E7=A0=81=EF=BC=9A=E8=85=95=E9=83=A8?= =?UTF-8?q?=E7=9C=8B=E4=B8=8D=E8=A7=81=E6=97=B6=E7=94=A8=E9=A1=B6=E8=A7=86?= =?UTF-8?q?=E4=B8=80=E6=AC=A1=E5=AE=9A=E5=88=B0=E5=9F=BA=E5=BA=A7=EF=BC=8C?= =?UTF-8?q?=E4=BA=8C=E9=98=B6=E6=AE=B5=E5=AF=B9=E5=AD=94=E5=92=8C=E6=89=8B?= =?UTF-8?q?=E5=A4=96=E6=A0=87=E5=AE=9A=E8=B7=9F=E7=9D=80=E8=BF=9B=E5=90=8C?= =?UTF-8?q?=E4=B8=80=E4=BB=BD=E6=A0=91=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor --- README.md | 1 + config/site.example.yaml | 320 +++++++-- devices/cameras/README.md | 5 +- .../sim/gazebo/config/sim_tool_geometry.yaml | 6 +- docs/ENVIRONMENT.md | 317 +++++++++ docs/HANDBOOK.md | 21 +- docs/HANDEYE.md | 82 ++- docs/HOLE_DETECT.md | 13 +- docs/QUICKSTART.md | 4 +- docs/WORKSPACE.md | 3 +- handeye/README.md | 68 +- handeye/calibrate.yaml | 8 +- handeye/poses/89_eye_to_hand.yaml | 50 ++ handeye/results/89_d435_eye_to_hand.yaml | 13 + models/yolo/gongjian0929.labels.yaml | 16 + scripts/_verify_hole_lost.py | 63 ++ scripts/gen_site_example.py | 15 +- scripts/lib/export_site_env.py | 427 +++++++++++- scripts/run_all.sh | 35 +- scripts/run_camera_d405.sh | 12 +- scripts/run_camera_d435.sh | 13 +- scripts/run_handeye.sh | 127 +++- scripts/run_visual_servo_rt.sh | 37 +- scripts/sense_tool_length.py | 309 +++++++++ scripts/test_rdk_python.sh | 13 + .../arm_application/feature_guard.py | 200 ++++++ .../arm_application/feature_jump_guard.py | 98 +++ .../arm_application/gauge_assembly_node.py | 127 ++-- .../arm_application/gauge_fsm.py | 92 +++ .../arm_application/handeye_lib.py | 33 + .../arm_application/hole_detect_hough.py | 90 +++ .../arm_application/hole_detector.py | 9 +- .../arm_application/kalman_filter.py | 9 + .../arm_application/onnx_workpiece.py | 15 + .../arm_application/overhead_oneshot.py | 245 +++++++ .../arm_application/perception_node.py | 620 +++++++++++++++++- .../arm_application/pipeline_node.py | 1 - .../arm_application/plane_feature.py | 5 + .../arm_application/pose_detector.py | 479 ++++++++++++++ .../arm_application/realsense_view.py | 9 +- .../arm_application/servo_node.py | 396 ++++++++++- .../arm_application/site_io.py | 134 ++++ .../arm_application/stage2_locate_node.py | 566 ++++++++++++++-- .../arm_application/yolo_detector.py | 86 ++- src/arm_application/config/hole_insert.yaml | 2 +- src/arm_application/config/perception.yaml | 2 +- .../launch/gauge_assembly.launch.py | 76 +++ .../launch/hole_detect.launch.py | 2 +- src/arm_application/launch/pipeline.launch.py | 9 + .../launch/visual_servo.launch.py | 36 +- .../test/test_feature_guard.py | 515 +++++++++++++++ .../test/test_feature_jump_guard.py | 357 ++++++++++ .../test/test_ft_source_guard.py | 200 ++++++ src/arm_application/test/test_gauge_fsm.py | 60 ++ src/arm_application/test/test_handeye_lib.py | 73 ++- src/arm_application/test/test_handeye_mode.py | 404 ++++++++++++ .../test/test_hole_detector.py | 2 +- .../test/test_onnx_workpiece_guard.py | 50 ++ .../test/test_overhead_oneshot.py | 195 ++++++ .../test/test_overhead_track.py | 234 +++++++ .../test/test_perception_acquire_gate.py | 180 +++++ .../test/test_pose_detector.py | 280 ++++++++ .../test/test_stage2_coarse_guard.py | 263 ++++++++ .../test/test_stage2_descent_clamp.py | 357 ++++++++++ .../test/test_stage2_tip_align.py | 308 +++++++++ .../arm_control/named_pose_server.py | 73 ++- .../src/flexiv_hardware_interface.cpp | 136 +++- 67 files changed, 8597 insertions(+), 409 deletions(-) create mode 100644 docs/ENVIRONMENT.md create mode 100644 handeye/poses/89_eye_to_hand.yaml create mode 100644 handeye/results/89_d435_eye_to_hand.yaml create mode 100644 models/yolo/gongjian0929.labels.yaml create mode 100644 scripts/_verify_hole_lost.py create mode 100644 scripts/sense_tool_length.py create mode 100644 src/arm_application/arm_application/feature_guard.py create mode 100644 src/arm_application/arm_application/feature_jump_guard.py create mode 100644 src/arm_application/arm_application/overhead_oneshot.py create mode 100644 src/arm_application/arm_application/pose_detector.py create mode 100644 src/arm_application/test/test_feature_guard.py create mode 100644 src/arm_application/test/test_feature_jump_guard.py create mode 100644 src/arm_application/test/test_ft_source_guard.py create mode 100644 src/arm_application/test/test_handeye_mode.py create mode 100644 src/arm_application/test/test_onnx_workpiece_guard.py create mode 100644 src/arm_application/test/test_overhead_oneshot.py create mode 100644 src/arm_application/test/test_overhead_track.py create mode 100644 src/arm_application/test/test_perception_acquire_gate.py create mode 100644 src/arm_application/test/test_pose_detector.py create mode 100644 src/arm_application/test/test_stage2_coarse_guard.py create mode 100644 src/arm_application/test/test_stage2_descent_clamp.py create mode 100644 src/arm_application/test/test_stage2_tip_align.py diff --git a/README.md b/README.md index aebeacb..565aefe 100644 --- a/README.md +++ b/README.md @@ -67,6 +67,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control | [docs/FRAMEWORK_RULES.md](docs/FRAMEWORK_RULES.md) | **框架规则**（改代码前先读；含历史反例的症状表） | | [docs/WORKSPACE.md](docs/WORKSPACE.md) | 每个目录 / 脚本 / 包 | | [docs/QUICKSTART.md](docs/QUICKSTART.md) | 环境与第一次跑通 | +| [docs/ENVIRONMENT.md](docs/ENVIRONMENT.md) | 在另一台电脑上装出和本机相同的 ROS / conda / RDK 环境 | | [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md) | 进程与几何 | | [docs/SITE_CONFIG.md](docs/SITE_CONFIG.md) | `site.yaml` 字段 | | [docs/HANDEYE.md](docs/HANDEYE.md) | 手眼标定 | diff --git a/config/site.example.yaml b/config/site.example.yaml index ff09c1a..39dff64 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -26,14 +26,14 @@ robot: # >>> 切换机械臂：改成 robots 里某一台的完整 SN <<< sn: "Rizon4-XXXXXX" # Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list - tool_name: "" + tool_name: "tool1" # 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。 robots: - sn: "Rizon4-XXXXXX" arm: left ip: "192.168.2.XXX" pc_ip: "192.168.2.XXX" - iface: "enp4s0" + iface: "enxXXXXXXXXXXXX" label: "左臂89" # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务， # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。 @@ -67,7 +67,11 @@ robot: # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂， # 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。 servo: &servo_left - ee_z_m: 0.35 # 跟踪锁高：法兰相对基座 Z [m] + # 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm + # （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致** + # （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。 + # 规矩：ee_z_m 要高于 servo.hole_hover_z_m（二阶段得往**下**走，不能往上返）。 + ee_z_m: 0.36 # 跟踪锁高：法兰相对基座 Z [m] min_ee_z_m: 0.20 safe_z_m: 0.45 z_min_m: 0.22 @@ -77,13 +81,93 @@ robot: # >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<< # 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。 # 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only - hole_hover_z_m: 0.23 # ← 二阶段悬停高度调这一行 - # 腕部 FT 话题**不在这里配**：默认按 sn 推导 = //ft_sensor_wrench， - # 由真机 flexiv_robot_states_broadcaster 无条件发布，仿真侧也是同名同类型 - # （见 devices/sim/gazebo/config/wrist_ft.yaml）。 - # 只有换 FT 来源时才需要显式写 servo.ft_wrench_topic，例如改用 - # //external_wrench_in_tcp（已做重力/负载补偿，基线≈0， - # 对"轻碰"更灵敏，但对负载变化更敏感）。 + # = 工件上表面 0.022（本臂 gauge_assembly.workpiece_surface_z_m，加底座后重测） + # + 工具长 0.245 + 间隙 0.002 + # 2026-09-29 实测：按 0.23 悬停时工具尖端落在工件面**下** 5~10mm， + # 悬停这一步就把通止规插进了工件（见本臂 gauge_assembly 段的完整说明）。 + # + # 2026-09-29 二次修正 —— 上面那条"工具几何"只保证**不碰工件**，它保证不了 + # **相机看得见孔**，而二阶段的第一步就是拍 k1/k2。旧值 0.259 的后果实测： + # 相机光心在法兰下方 0.136m（手眼 T_cam_to_gripper + 29.7° 倾角） + # 法兰 0.259 → 相机离工件面 0.123m → 检出 0/16，conf 峰值 0.012（噪声） + # 法兰 0.309 → 相机离工件面 0.173m → 检出 16/16，conf 0.87， + # k0/k1/k2 置信度全 0.999~1.000，k1-k2 间距 156px 稳定 + # gauge_fsm.hover_z_guard 的设计注释也写着"腕部相机要离工件面 ~0.17m + # 才看得清孔"，所以 0.309 才是这条同时满足"相机看得见"和"规尖不碰"的值 + # （规尖此时在面上 64mm，比旧值更安全）。 + # 最后 2mm 的贴近由 stage2 的**深度闭环轻降**（final_gap_mm）完成 —— + # 它按实测间隙往下走，与绝对悬停高无关，所以抬高悬停不会让尖撞工件。 + # + # 2026-09-29 三次修正（工件加了底座，工作平面重新感知）： + # 拿 gongjian0929 在**实时图**上出 k0/k1/k2，再取各点邻域深度投到基座系： + # k1 = +0.0220 / +0.0221 / +0.0223 m（多帧重复性 0.4mm 内） + # k2 = +0.0223 / +0.0224 m + # k0 = +0.0165 / +0.0186 m（偏低约 6mm 且抖，局部读数噪声大） + # 三点"中心 vs 外环"深度差 ≤1mm ⇒ 三点都落在**上表面**上，不是孔底 ⇒ + # 上表面取 k1/k2 的一致值 **0.022**（原 0.012 是加底座前的旧值）。 + # 悬停随之 0.309 → 0.320（同样 +0.011），让"相机到工件面"的**视距保持 + # ≈0.17m 不变** —— 0.309 那一档是实拍验证过"16/16 检出、conf 0.87、 + # k1-k2 间距 156px 稳定"的工作点，改高度就是在改视距，不能随手抬。 + # 几何兜底仍然满足：0.320 − 工具长 0.245 = 尖端 0.075，工件面 0.022 + # ⇒ 尖端在面上方 53mm（要求 ≥ final_gap 2mm）。 + hole_hover_z_m: 0.320 # ← 二阶段悬停高度调这一行 + # FT 数据源：默认按 sn 推导 = //ft_sensor_wrench，由真机 + # flexiv_robot_states_broadcaster 发布。但 **2026-10-08 实测该话题恒为 + # 全零（0,0,0）** —— RDK 那侧的原始 FT 传感没在出数（`ft_sensor_raw` 为 0）， + # 于是应用里"abs(fz−基线)>2.5N 判接触"的兜底**永远判不到接触**（静默失效）。 + # 同一时刻 //external_wrench_in_tcp 是有数的（≈负载 54N，且随受力变化）， + # 所以真机改用它。判接触只看"相对轻降起始基线的增量"，与 54N 这个偏置无关， + # 用这个源可直接恢复接触兜底，不需要改代码。 + # 仿真**别跟着用**（仿真没有这个话题）—— 见本文件 sim 段的显式覆盖。 + # 注意：这不是话题名写错，而是"哪个数据源有数"的取舍（真机 RDK 未启用原始 FT）。 + ft_wrench_topic: "/Rizon4_XXXXXX/external_wrench_in_tcp" + # ---- 工件几何（2026-09-29 现场实测标定）---- + # 真机段的坐标都是**基座系**；本机基座与台面同高 ⇒ 台面 z ≈ 0。 + gauge_assembly: + # 工件**上表面**的世界 z [m]。悬停高度的几何判据（gauge_fsm.hover_z_guard） + # 与 S07 下探预算都拿它当基准面。 + # + # ⚠️ 为什么不能留全局段的默认 0.0：0.0 = "未知"，运行期退回"用 stage2 实测 + # 孔位 z 兜底" —— 但兜底发生在**悬停之后**，悬停本身还是按 0.0 算的。 + # 后果（2026-09-29 实测）： + # 悬停 0.247 − 工具长 0.245 = 工具尖端 z≈0.002，而工件面在 0.0074 + # ⇒ 尖端在工件面**下** 5~10mm。悬停这一步就把通止规插进工件，接着 + # S05 的横向对准等于拿规尖**拨工件**：两孔中点两拍之间被推走 19.1mm， + # 残差 27.3 → 42.7mm 反向发散（日志看着像手眼标定坏了），最后被 + # drag_verdict 看门狗拦下（exit=6）。 + # + # 实测依据（旧，加底座前）：stage2 解出的两孔基座系 z = 0.00743 / 0.00715 m + # （/tmp/stage2_result.json 的 k1_base_m / k2_base_m）。孔是通孔， + # 深度相机在孔口附近读数会偏深 ⇒ 上表面取工件板厚 **0.012**。 + # + # 实测依据（2026-09-29 加底座后**重新感知**，现行值 = 0.022）： + # 不再靠"板厚"推，而是直接量上表面 —— 让 gongjian0929 在实时图上出 + # k0/k1/k2，对每个点取 (±6px) 邻域深度的中位数，用同一帧 TF + # (base←flange · flange←cam) 投到基座系： + # k1 = +0.0220 / +0.0221 / +0.0223，k2 = +0.0223 / +0.0224（重复性 0.4mm） + # k0 = +0.0165 / +0.0186（偏低 6mm 且抖 ⇒ 该点邻域读数不可靠，不参与定面） + # 判"是不是孔底"：各点中心 (±6px) 与外环 (12~20px) 深度差 ≤1mm ⇒ 齐平 + # ⇒ 三点都在上表面。ROI 高度直方图同为单峰平台，p75≈0.0228、p95≈0.0247 + # ⇒ 上表面 0.022（比加底座前的 0.012 高 10mm）✓ 与"工件加了底座变高"一致。 + # 法兰 Z=0.356 时该点光学视距实测 0.204m（模型仍检出 conf 0.862）⇒ + # 模型对 0.17~0.20m 有一档余量，但两侧都不要顶到边界。 + # 复核（会同时打印实测工件面 + 悬停几何自检）： + # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only + # python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX + workpiece_surface_z_m: 0.022 + # 工具长（规尖在法兰下方的距离）—— **2026-10-08 真机感知标定**：0.223m。 + # 方法：hover(法兰 0.320) 下腕部相机抓一帧，取规杆最深像素 + 深度，用 + # 手眼 T_base = T_base_flange·T_flange_cam 投到基座 ⇒ 规尖 z=0.0971， + # 故 0.320 − 0.0971 = 0.2229m。同帧反投影链的独立校验：k1/k2 基座 z + # 精确复现上次实测 0.0219/0.0204（误差 <0.1mm），链路可信。 + # 工具长 = tool1 夹爪 TCP 0.147 + 规露出 0.076。 + # ⚠️ 旧值 0.245 是照抄仿真的（TCP 0.200 + 规 0.043），比真机长 22mm： + # 它让几何误以为悬停时尖端只在面上方 53mm（实际 75mm），轻降预算因此 + # 少 22mm，**永远到不了孔口**（现象就是"机械臂不下臂"）。 + # 落在本文件下方注释记录的合理区间 0.204~0.233 内。 + # 复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX + # python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX + tool_tip_below_flange_m: 0.223 # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<< # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的 #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节 @@ -153,6 +237,10 @@ robot: # 夹好规后读到的 TF 值写进全局段的 tool_tip_below_flange_m。 servo: <<: *servo_left + # 仿真 FT：由 sim_wrist_ft 发布，与真机 //ft_sensor_wrench 同名同类型同单位。 + # 必须显式覆盖 —— 上面 *servo_left 带下来的 ft_wrench_topic 是**真机**的 + # external_wrench_in_tcp，仿真里没有这个话题（不覆盖 ⇒ 仿真 FT 收不到）。 + ft_wrench_topic: "/sim/ft_sensor_wrench" ee_z_m: 0.72 # 一阶段跟踪锁高（法兰，世界系） # 不必随悬停一起抬：跟踪时工具尖端离面 ~60mm， # 只要**高于悬停**就不碰工件（0.72 > 0.657）。 @@ -188,7 +276,7 @@ robot: arm: right ip: "192.168.2.XXX" pc_ip: "192.168.2.XXX" - iface: "enp4s0" + iface: "enxXXXXXXXXXXXX" label: "右臂91" camera: name: d405_right # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用 @@ -221,6 +309,9 @@ robot: hover_offset_ee_xy_m: [0.0, 0.0] # RGI 与法兰同轴；爪心偏了再改这两项 [m] # 二阶段孔定位悬停锁高（91 装了传感器后同样可用） hole_hover_z_m: 0.22 + # FT 数据源同 89：真机 ft_sensor_wrench 恒零，改用有数的 external_wrench_in_tcp + # （见 89 段 ft_wrench_topic 的完整说明）。 + ft_wrench_topic: "/Rizon4_XXXXXX/external_wrench_in_tcp" # --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方 paths: @@ -228,19 +319,24 @@ paths: # 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 + # bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo # 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" + # 当前：gongjian0929（2026-09-29 从 ~/下载 转入）。 + # ⚠️ 这是个 **pose 模型**（YOLO26s-pose）：一个工件框 + 3 个关键点 + # k0=跟踪孔 / k1,k2=对孔。它同时替代了原来的跟踪模型和入孔模型， + # 所以下面 hole_weights 也指向同一份。 + # ultralytics 解不了 pose 的 TensorRT engine，运行时走 + # arm_application/pose_detector.py 的自解码（见该模块头）。 + # 仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。 + yolo_weights: "/home/USER/ros2/models/yolo/gongjian0929.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" + # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。 + # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。 + # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor + hole_weights: "/home/USER/ros2/models/yolo/gongjian0929.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" @@ -251,11 +347,23 @@ paths: 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] + # >>> 标定板尺寸：**按模式各一份**（内角点数 + 方格边长 m）<<< + # + # 为什么必须按模式分开写（而不是两个模式共用一组键）： + # 眼在手上与眼在手外通常用**两块不同的物理板**，方格边长不一样。共用一组键 + # 的后果不是报错，而是"跑另一种模式时静默用错方格边长"—— 标定照样跑完、 + # 结果只是系统性偏，现场几乎联想不到板尺寸（历史反例见 handeye/README.md 第 3 节）。 + # 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试 + #（见 handeye_lib.detect_chessboard）。 + boards: + eye_in_hand: # A5 板：9×12 方格 + cols: 8 # 内角点列数 + rows: 11 # 内角点行数 + square_size_m: 0.015 # 方格边长 [m] + eye_to_hand: # A4 板：12×9 方格 + cols: 11 + rows: 8 + square_size_m: 0.020 # 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照 velocity_scale: 0.05 move_time_sec: 8.0 @@ -358,6 +466,18 @@ yolo: backend: "tensorrt" # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh mp_infer: true # YOLO 独立进程，不和 ROS 抢 GIL min_conf: 0.12 # 新锁门槛；已锁定的框可略低于此 + # 重新捕获门槛：**跳到新位置**（离上一帧框中心 >max_track_px）必须达到的置信度。 + # 为什么必须比 min_conf 高得多 —— 2026-10-08 真机实测（一阶段 servo_node 日志）： + # 0659.551 死区保持 |e_xy|=3.0mm ← 已经收敛到 3mm + # 0659.598 k0 置信度 0.00 < 0.30，本帧不用它 + # 0659.6xx 重新捕获 conf=0.15 dpx=503 ← 503px 外、conf 0.15 的假框 + # feat_base=(0.231,-0.484) z=-0.021 ← 跟踪点瞬间横移 427mm + # 0670.985 === 一阶段已锁定 === ← "锁定"在离工件 500mm 的位置 + # 那串假框实测 conf 0.13~0.47，而真 k0 是 0.87。旧写法把这里接成 min_conf=0.12 + # ⇒ 0.15 的假框被原样采信、perception 的 kf.reset() 直接跳过去，伺服再拦也只是 + # 拦一帧（假框帧间稳定，3 帧后 FeatureJumpGate 也会认）。 + # 取 0.55：高于假框上沿 0.47，低于真 k0 0.87。宁可原地不动，也不追假点。 + reacquire_conf: 0.55 vision_hz: 30.0 infer_max_width: 640 # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵 box_ema_alpha: 1.00 # 绿框用预测补偿后的框，不再 EMA @@ -370,6 +490,17 @@ yolo: z_ref_m: 0.22 min_depth_m: 0.04 max_depth_m: 0.55 + # 特征点的**相机量程下限**：比这更近的深度读数一定来自机械臂自身（爪上夹的 + # 通止规 / 相机座），不是工件。**判据是实测的两条带，不是几何推导**： + # 机械臂自身（随臂一起动）：0.078 ~ 0.087 m ← 2026-10-08 被误锁的假目标 + # 工件面： 0.19 ~ 0.29 m ← 锁高下正常跟踪时的 p_cam.z + # 取 0.14：距近带 53mm、距远带 50mm，两边都不顶边界。 + # 几何上为什么近带被占满：规尖在法兰下 0.223m 而相机只在法兰下 0.124m（handeye + # cam_in_flange z=0.1236）⇒ 规杆整段都在相机近场；且相机光轴与法兰 Z 差 29.7°， + # 所以"光轴视距"不等于"竖直距离"，只能靠实测划带。 + # 撞了会怎样：假目标在相机前 0.08m 且**随臂一起动**，伺服误差恒为 0 —— + # "一阶段已锁定"会锁在自己的工具上（见上面 reacquire_conf 的记录）。 + feature_min_range_m: 0.14 work_z_max_m: 0.50 meas_blend: 0.85 # 3D 跟手；抖则略降 z_alpha_min: 0.10 @@ -378,7 +509,26 @@ yolo: camera_frame: "d405_color_optical_frame" object_frame: "tracked_object" depth_topic: "/d405/aligned_depth_to_color/image_raw" - feature_mode: "plane" # plane=框内近表面立体点（工件本体）；box=单目射线 + feature_mode: "keypoint" # keypoint=pose 模型的 k0（跟踪孔）；plane=框内近表面立体点；box=单目射线 + # feature_mode=keypoint 时用：跟踪用哪个关键点、它的置信度低于多少就不信它。 + # k0 = 跟踪孔（见 pose_detector 的约定）。k0 不可用时**借 k0 相对检测框的归一化 + # 偏移**在框上重建 k0 —— 位置连续、工件动时还跟着走。 + # 刻意**不**回落"框中心"（那是另一个点，离 k0 有 ~89mm，真机上就是一次不该有的 + # 横向快移，见 2026-10-08 一阶段 150s 没锁上的记录）；只有在**从未**见过可信 k0 + # 的冷启动帧才回落到框中心/框内深度。 + keypoint_index: 0 + keypoint_min_conf: 0.30 + keypoint_win_px: 6 # k0 处取多大窗口的中位深度 + # ---- 跟踪点可信性的三道守卫（实现：feature_guard.py，单一数据源）---- + # 为什么需要：2026-10-08 真机 arm 89，一阶段真 k0（conf 0.87 uv=(410,246)）被一串 + # **高置信度假 k0** 顶掉，伺服 1s 内把臂从 home 带到工作盒角落 (0.083,-0.515)， + # 之后 148s 全是"检测漏检"、150s 超时。假 k0 的 conf 是 0.53~0.73（真值 0.87） + # ⇒ 置信度没有可分区间；但它们**贴在画面边上**、且**借深度**： + # 766.96 uv=(76,75) 768.16 uv=(143,92) 768.78 uv=(12,30) 770.07 uv=(431,226) + keypoint_border_px: 6 # k0 离画面边缘 < 此值即判位置不可信（孔心不在画面边上） + track_max_step_m: 0.04 # 已发布跟踪点的每帧位移上限 [m]（实测真实运动 ≤15mm/帧） + reacquire_far_px: 120 # 远于此像素距离的"重新捕获"要连续确认（dpx 那个门限） + reacquire_confirm_frames: 3 # 远框要连续几帧指向同一处才采信 plane_inlier_m: 0.008 # 拟合平面内点阈值 [m] plane_band_m: 0.06 # 近表面深度带宽 [m] @@ -409,7 +559,16 @@ servo: predict_enable: true # 基座系恒速预测，补视觉延迟 predict_sec: 0.16 # 额外前瞻 [s] predict_v_max: 0.80 # 工件速度上限 [m/s]，防乱飞 - predict_jump_m: 0.08 # 基座 XY 跳变大于此则重锁速度 + predict_jump_m: 0.08 # 基座 XY 跳变大于此则视为"疑似假跳"，转入确认流程 + # 大跳变要连续多少帧指向同一新位置才采信（见 feature_jump_guard.py 的注释）。 + # 30Hz 下 3 帧 ≈ 0.1s：真的重定位 0.1s 就被认；假点（乱跳）永远认不了。 + # 为什么不能是 1：2026-10-08 真机，一阶段已收敛到 3mm 后 k0 掉信、跟踪点瞬间 + # 横移 89mm，旧逻辑单帧就 reset 采信 → 伺服以 0.45m/s 去追假点 → 彻底丢失。 + jump_confirm_frames: 3 + # 特征点位置合理性：必须落在工作盒 XY 内（±此余量 [m]），否则判误检、当"没有目标"。 + # 为什么需要它：2026-10-08 真机，模型在运动模糊下把特征点丢到基座后方 + # (-0.10,-0.65)（工件其实在 (0.5,-0.12)），伺服一路追到工作盒边缘卡住。 + feat_ws_margin_m: 0.05 kp_xy: 4.20 # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5 ki_xy: 0.00 kd_xy: 0.20 @@ -419,10 +578,20 @@ servo: 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 已够） + lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段） + lock_still_sec: 3.0 + lock_still_move_m: 0.015 + e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速 v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s v_max_z: 0.04 + # 远处限速：目标离得比 slow_far_m 远时 XY 限速到 v_slow_xy（**与"目标真不真"无关** + # 的一道防线）。为什么需要：2026-10-08 真机一阶段被一串高置信度假 k0（conf 0.53~0.73） + # 顶掉，伺服 0.7s 内以 0.45m/s 把臂从 home (0.686,-0.113) 带到工作盒角落 + # (0.083,-0.515)，之后 148s 全是"检测漏检"、150s 超时。加了这道之后：真 k0（离 + # 145mm）只是晚 ~1.8s 到位，假目标最多把臂带走几厘米。 + slow_far_m: 0.05 # 目标离得超过此距离即限速 [m] + v_slow_xy: 0.08 # 限速值 [m/s]（真目标到位后仍回到 v_max_xy） 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] @@ -490,10 +659,18 @@ gripper: # 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" + # backend=yolo 时用。 + weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx" + # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点， + # 所以和上面的跟踪权重是同一份。 + workpiece_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx" + # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。 + # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到 + # 亮环 + Hough 那套老路径。 + keypoint_enable: true + keypoint_min_conf: 0.30 + keypoint_min_dist_px: 15.0 + keypoint_hole_radius_px: 12.0 # 关键点没有半径；反投影时按这个半径取孔周深度环 config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml" conf: 0.25 iou: 0.7 @@ -532,16 +709,66 @@ hole_detect: # 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 # 拍照→修正→复拍 迭代上限 + # 对准目标孔。pose 模型下 k1/k2 是**模型学出来的固定语义**（不是图像左右）： + # hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2 + # 下游是 `k1_b = pts[0] if k1_label=="left" else pts[1]`，所以： + # k1_label: "left" → target_hole=k1 打的是**模型的 k1**（当前设置，推荐） + # k1_label: "right" → target_hole=k1 打的是模型的 k2（等价于老 Hough 的"图像左孔"） + # 实测这台臂的视角下模型给的是 k1 在画面右、k2 在画面左；对孔顺序要反过来就改这里。 + target_hole: "k1" + k1_label: "left" + # ---- 对准判据：工具**用视觉量到的规尖**，不是法兰轴线（2026-10-08 真机定稿）---- + # 实测（同一帧、同一条手眼链，n=8）： + # · 规杆轴线相对法兰轴线 = (+10.7, −0.1)mm（σ≈0.5mm）；最深像素法 (+8.8,−1.8)mm + # · 而 site 原来把 hover_offset_ee_xy_m 写成 [0,0]（"平行爪同轴"），于是精对准 + # 把**法兰**对到孔上，规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔沿上， + # 腕力从 −1N 直接蹦到 −31N→−63N 卡死（现象是"轻降在离面 4mm 停住"）。 + # · 孔检测本身的重复性极好：k1 基座坐标 σ=(0.17,0.04)mm —— 精度不是瓶颈。 + # 所以：在同一帧里同时解「规尖基座 XY」和「孔基座 XY」，两者一起走手眼链， + # 链路误差与装夹变动一起抵消。规尖量取规则见 _tip_offset_from_depth（四条判据）。 + align_tol_m: 0.00025 # 对准到位判据 [m]。检测重复性 0.04mm 撑得住 0.25mm。 + # ⚠️ 不要回到 0.7mm：那会让"规偏 10mm"这种系统偏差 + # 在容差里被放过去（原来残差 0.69mm 看着"通过"，其实 + # 是拿法兰对的孔）。收到 0.25mm 才逼它必须真的对上规尖。 + align_max_iters: 12 # 拍照→修正→复拍 迭代上限（收紧容差后需要更多拍） + align_tip_vision: true # true=精对准用「视觉规尖」；false=退回旧的「法兰+偏置」 + tip_near_m: 0.15 # 规尖量取：近端阈值，规比这更近 [m] + tip_min_m: 0.04 # 有效深度下限 [m] + tip_max_m: 0.60 # 有效深度上限 [m] + tip_search_band_frac: 0.25 # 中央竖带半宽 = 该比例 × 图宽（规夹在爪里，不会靠边） + tip_min_px: 150 # 近端连通域最小像素数（少了就是没看到规） + tip_calib_n: 4 # 定偏移需要的一致拍数（够了就冻结） + tip_offset_tol_m: 0.003 # 样本相对中位偏移的最大允许偏差 [m]（剔偶发外点 + 冻结判据） + tip_len_min_m: 0.15 # 反投影出的「法兰z − 规尖z」必须落在此区间 —— + tip_len_max_m: 0.32 # 这条同时挡住"深度读到台面/工件"（主干守卫） coarse_max_moves: 2 # 工件粗居中（bbox 中心）移动上限 + # —— 粗居中的两条误检守卫（单源：代码读这两个键，见 _coarse_target_reject）—— + # 为什么需要：2026-10-08 真机 arm 89，粗居中拿被画面左边缘切掉的半框 + # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑 + # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也 + # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。 + coarse_max_offset_m: 0.12 # 目标离夹爪轴线超过此值就不像粗居中量 [m] + coarse_border_px: 2 # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 —— 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]（很慢，柔顺） + # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap）， + # 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条： + # · 2026-09-29（多降→撞工件）：环形深度中值报 gap0=141.0mm，几何真值只有 53mm + # （probe 0.245 版），多降 ~86mm 把通止规撞进孔，工件被 +X 推走 5mm。 + # ⇒ 读数往**大**错一次就撞。 + # · 2026-10-08（少降→到不了 2mm）：同一个读数悬停时报 144.0mm（几何真值 75mm）， + # 只降 24mm 后又翻成 −7.0mm，触发 SAFE_FLOOR 把轻降停在离面 ~50mm 处， + # 最终日志却打印"悬在孔面上方 ~2.0mm"（其实停在 ~18mm）。 + # ⇒ 读数往**小**错一次就永远到不了目标。 + # 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少" + # 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。 + # 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。 + # 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm） + # + 任一步 _goto 走不到位就停。 + # 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。 final_descent_enable: true # 对准后是否轻降到 final_gap_mm（false=停在悬停高） verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg] settle_sec: 0.45 # 到位后静止多久再拍照 @@ -615,6 +842,22 @@ hole_detect: # ============================================================================= perception: source: "yolo" + # 一阶段：wrist=D405 主跟踪，画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0。 + # overhead=整段只用顶视（实验）。非法值启动即报错。 + track_camera: "wrist" + wrist_handoff_conf: 0.5 + overhead: + min_depth_m: 0.25 + max_depth_m: 1.80 + feature_min_range_m: 0.30 + work_z_max_m: 1.50 + z_ref_m: 0.70 + feature_mode: "plane" + hold_point_on_miss: true + oneshot_conf: 0.5 + cooldown_s: 1.5 + frame_max_age_s: 0.4 + oneshot_hold_s: 8.0 truth: # 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空： # "哪个是工件"是场景的知识，换场景不该改这里）。 @@ -845,8 +1088,11 @@ go_home: speed: 0.25 network: - # User1=enp4s0 本机 ↔ <左臂SN>=.101 / <右臂SN>=.100 - prefer_iface: "enp4s0" + # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179） + # enxXXXXXXXXXXXX = 该 USB 网卡（名字由网卡 MAC 派生，换网卡会变） + # 本机 ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100 + # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。 + prefer_iface: "enxXXXXXXXXXXXX" # 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md） # 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*）， diff --git a/devices/cameras/README.md b/devices/cameras/README.md index 1b4de7e..0347e63 100644 --- a/devices/cameras/README.md +++ b/devices/cameras/README.md @@ -29,8 +29,9 @@ cameras/ | `wrist_eye_in_hand` | D405（左腕 / 右腕各一台） | 视觉跟踪 k0、拍照推理 k1/k2 | 是（眼在手上） | | `overhead_eye_to_hand` | D435（双臂中间） | 粗定位兜底（S03） | 是（眼在手外） | -**注意**：D435 当前**还没有做手眼标定**，所以 S03 粗定位链路暂时不可用。 -但这不影响 S04 跟踪链路，两条链路用的是同一套权重、同一个跟踪目标 k0。 +**注意**：真机 D435 的眼在手外标定按 `docs/HANDEYE.md` 的「眼在手外」一节做； +产物是 `handeye/results/<臂号>_d435_eye_to_hand.yaml`。没有那份文件时 S03 +会明确报缺文件并回 S04 跟踪，不做假装成功的反投影。这不影响 S04 跟踪链路。 ## 已知的坑（迁移时必须保留的处理） diff --git a/devices/sim/gazebo/config/sim_tool_geometry.yaml b/devices/sim/gazebo/config/sim_tool_geometry.yaml index 1b7914d..e04fda8 100644 --- a/devices/sim/gazebo/config/sim_tool_geometry.yaml +++ b/devices/sim/gazebo/config/sim_tool_geometry.yaml @@ -5,8 +5,8 @@ # （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。 # 真机没有这个文件（现场装夹后把实测值写进 site.yaml）。 flange_z_m: 0.763 -measured_at: '2026-09-24T19:40:08' +measured_at: '2026-09-28T14:06:51' model: gauge -tip_z_m: 0.5198 -tool_tip_below_flange_m: 0.2432 +tip_z_m: 0.5197 +tool_tip_below_flange_m: 0.2433 world: assembly diff --git a/docs/ENVIRONMENT.md b/docs/ENVIRONMENT.md new file mode 100644 index 0000000..79a9fc6 --- /dev/null +++ b/docs/ENVIRONMENT.md @@ -0,0 +1,317 @@ +# 环境安装（按本机快照复现） + +本文是 **2026-10-09** 在这台工控机上核对过的软件栈，用来在另一台电脑上装出同一套能跑本仓库的环境。 +核对来源：`/etc/os-release`、`conda-meta/history`、`conda list`、`~/flexiv_install` 的 cmake 版本文件、`nvidia-smi`。 + +日常怎么用已经装好的环境，看 [QUICKSTART.md](QUICKSTART.md)。本文只回答「从一台空的 Ubuntu 装到能 `source scripts/env.sh`」。 + +--- + +## 1. 本机现在是什么 + +| 项 | 本机实测 | +|----|----------| +| 系统 | Ubuntu 22.04.5 LTS，x86_64，内核 `6.8.0-138-generic` | +| GPU | NVIDIA GeForce RTX 3090 24GB，驱动 **595.91.07**（DKMS）。`nvidia-smi` 报告的 CUDA 是驱动上限 **13.2** | +| 推理用的 CUDA | **不装系统 CUDA toolkit**。PyTorch / TensorRT 用 pip wheel 自带的 **CUDA 12.4** 用户态库 | +| ROS | **Humble**。本机 **没有** `/opt/ros/humble`，ROS 在 conda 里（RoboStack） | +| Conda | Anaconda3 装在 `~/anaconda3`，conda **25.11.1** | +| 真机环境 | conda 环境 `ros_flexiv`，Python **3.11.15**，约 15GB | +| 仿真环境 | conda 环境 `ros_gz_sim`，Python **3.11.6**，Gazebo Sim **6.16.0**（Fortress / `ign gazebo`），约 5.1GB | +| 工作区 | 仓库在 `~/ros2`（ASCII）。`~/文档/ros2` 只是指向它的符号链接 | +| Flexiv C++ | `~/flexiv_install`，`flexiv_rdk` **1.9.1**（spdlog 1.9.2，Boost 1.83.0），约 194MB | +| Flexiv Python | `~/rdk_py310/bin/python` → 系统 `/usr/bin/python3.10`（3.10.12）+ `flexivrdk==1.9.1` + `numpy==1.26.4` | +| DDS 域 | 真机 `ROS_DOMAIN_ID=0`，仿真强制 `77` | + +真机和仿真是两套 conda、两套 colcon 产物，互不覆盖： + +| | 真机 | 仿真 | +|--|------|------| +| 激活 | `source ~/ros2/scripts/env.sh` | `source ~/ros2/scripts/sim_env.sh` | +| conda | `ros_flexiv` | `ros_gz_sim` | +| 产物 | `build/` `install/` | `build_sim/` `install_sim/` | +| 域 | 0 | 77 | + +同一只 shell 里来回 `source` 会残留 `PATH` / `PYTHONPATH`。换环境时开新终端。 + +--- + +## 2. 新机器先对齐的路径 + +下面这些路径被脚本写死为用户 `cc` 的家目录。新机器用**同一个用户名** `cc`（家目录 `/home/cc`）时，后面的命令可以原样复制。 + +| 路径 | 谁在用 | +|------|--------| +| `/home/cc/anaconda3` | `scripts/run_real.sh`、`scripts/run_mock_demo.sh` 直接 `source` 这个 `conda.sh`。`scripts/env.sh` 用的是 `$HOME/anaconda3`，两边要是同一个目录 | +| `/home/cc/rdk_py310/bin/python` | `run_real.sh`、`test_rdk_python.sh`、`rdk_switch_tool.sh`、`scripts/lib/real_preflight.py` 的默认 `RDK_PYTHON` | +| `/home/cc/flexiv_install` | `run_real.sh` 用 `$HOME/flexiv_install`（随用户走） | +| `/home/cc/ros2` | 工作区。`colcon build` 必须在这条 ASCII 路径下，不要在中文目录里编 | + +用户名不是 `cc` 时，至少要改 `scripts/run_real.sh` 里的 conda 路径，并在启动前 `export RDK_PYTHON=$HOME/rdk_py310/bin/python`。否则真机入口会去找 `/home/cc`。 + +建议磁盘空闲 **40GB 以上**（两个 conda 环境加下载缓存）。 + +--- + +## 3. 系统包与显卡 + +```bash +sudo apt update +sudo apt install -y \ + build-essential git curl wget \ + python3.10 python3-pip \ + v4l-utils usbutils \ + dkms linux-headers-$(uname -r) +sudo usermod -aG dialout "$USER" +``` + +`dialout` 给夹爪的 USB–RS485（`/dev/ttyUSB*`）。加完组要重新登录。 + +显卡驱动装到能让 `nvidia-smi` 跑起来即可。本机是 **595.91.07**。驱动只要够跑 CUDA 12.4（远低于 595 的驱动也可以），PyTorch 的 12.4 wheel 就能用。不要再装一套系统级 CUDA toolkit，避免和 wheel 里的 `libcudart` 抢。 + +Orisys 视触觉相机会被 USB 自动休眠卡死。本机规则文件是 `/etc/udev/rules.d/99-orisys-uvc.rules`： + +``` +# Orisys 视触觉相机 (1bcf:2281)：禁止 USB 自动休眠（autosuspend 会把 UVC 相机搞到内核态卡死） +ACTION=="add|bind", SUBSYSTEM=="usb", ATTR{idVendor}=="1bcf", ATTR{idProduct}=="2281", ATTR{power/control}="on", ATTR{power/autosuspend}="-1" +``` + +写好后 `sudo udevadm control --reload-rules && sudo udevadm trigger`。 + +--- + +## 4. Anaconda + +装到 **`$HOME/anaconda3`**（安装器里选 Just Me、路径填这个）。不要装成 `~/miniconda3` 或其他目录：`run_real.sh` 写死了 `/home/cc/anaconda3/etc/profile.d/conda.sh`。 + +本机 conda 版本是 25.11.1。装完新开一个终端： + +```bash +source "$HOME/anaconda3/etc/profile.d/conda.sh" +conda --version +``` + +频道不要写进 `~/.condarc` 当成全局默认。本机 `~/.condarc` 只有 `defaults`，RoboStack 的频道是**每次安装命令显式带上**的：`-c robostack-staging -c conda-forge`。 + +--- + +## 5. 真机环境 `ros_flexiv` + +下面三条 `conda` 命令来自本机 `~/anaconda3/envs/ros_flexiv/conda-meta/history`（2026-08-21 创建，2026-08-25 装 ROS）。pip 那一段是按同日 `conda list` 里的 pypi 版本还原的；当初的 `pip install` 原文没有留在 conda 历史里。 + +```bash +source "$HOME/anaconda3/etc/profile.d/conda.sh" + +conda create -n ros_flexiv python=3.11 pip -y + +conda install -y -n ros_flexiv -c robostack-staging -c conda-forge \ + ros-humble-ros-base \ + ros-humble-robot-state-publisher \ + ros-humble-xacro \ + ros-humble-tf2-ros \ + ros-humble-joint-state-publisher \ + compilers cmake make pkg-config catkin_pkg empy lark-parser numpy \ + colcon-common-extensions + +conda install -y -n ros_flexiv -c robostack-staging -c conda-forge \ + ros-humble-rviz2 + +conda install -y -n ros_flexiv -c robostack-staging -c conda-forge \ + ros-humble-moveit \ + ros-humble-moveit-configs-utils \ + ros-humble-control-msgs \ + ros-humble-controller-manager \ + ros-humble-joint-trajectory-controller \ + ros-humble-ros2-control \ + ros-humble-ros2-controllers +``` + +然后钉住本机正在用的 pip 包。Humble 的 `rosidl` 要 **empy 3.x**（conda 有时会给 4.x，构建消息包时会报 `AttributeError: NoneType object has no attribute shutdown`）。TensorRT 装 `tensorrt-cu12`，不要装元包 `tensorrt`。 + +```bash +conda activate ros_flexiv + +python -m pip install \ + 'empy==3.3.4' \ + 'lark==1.3.1' \ + 'numpy==2.4.6' \ + 'opencv-python-headless==4.10.0.84' \ + 'pyrealsense2==2.58.3.10794' \ + 'pyserial==3.5' \ + 'minimalmodbus==2.1.1' \ + 'onnx==1.22.0' \ + 'onnxruntime==1.30.0' \ + 'onnxruntime-gpu==1.29.0' \ + 'tensorrt-cu12==10.13.3.9' \ + 'ultralytics==8.4.123' + +python -m pip install \ + 'torch==2.6.0+cu124' \ + 'torchvision==0.21.0+cu124' \ + --index-url https://download.pytorch.org/whl/cu124 +``` + +相机走 **`pyrealsense2`**，本机没有装 `ros-humble-realsense2-camera`。夹爪 Modbus 走 `pyserial` + `minimalmodbus`。 + +Orisys SDK 不在 git 里。把本机这个 wheel 拷过去再装（`--no-deps`，避免它改写上面的 torch / numpy）： + +```bash +# 源文件：~/sensor_ws/0.4.2/orisys-0.4.2-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl +conda activate ros_flexiv +python -m pip install --no-deps \ + "$HOME/sensor_ws/0.4.2/orisys-0.4.2-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl" +``` + +本机这个环境里还有 Jupyter、PySide6、`flexiv-assembly` 等包，**本仓库脚本不用它们**，新机器不必装。 + +激活后应看到： + +```bash +conda activate ros_flexiv +python -c 'import sys; print(sys.version)' # 3.11.x +echo "ROS_DISTRO=$ROS_DISTRO" # humble +python -c 'import rclpy, cv2, pyrealsense2, torch; import tensorrt as trt; print("torch", torch.__version__); print("tensorrt", trt.__version__); print("cuda", torch.cuda.is_available())' +``` + +本机对应输出：`torch 2.6.0+cu124`，`tensorrt 10.13.3.9`，`torch.cuda.is_available()` 为真。 + +--- + +## 6. Flexiv RDK（与 conda 分开） + +运行中的库是 **1.9.1**。仓库里的 `src/flexiv_rdk` 工程版本是 **2.1**（`project(flexiv_rdk VERSION 2.1 ...)`）。`scripts/build_flexiv_rdk.sh` 编的是这份当前源码，产物和本机 `~/flexiv_install` 不是同一个版本。 + +新机器要对齐**正在跑的**这套，把本机这两个目录原样拷过去： + +```bash +# 在新机器上，从本机拉（把 user@old-host 换成这台工控机） +rsync -a user@old-host:~/flexiv_install/ ~/flexiv_install/ +rsync -a user@old-host:~/rdk_py310/ ~/rdk_py310/ +``` + +拷完核对： + +```bash +grep 'set(PACKAGE_VERSION' ~/flexiv_install/lib/cmake/flexiv_rdk/flexiv_rdk-config-version.cmake +# 应有 set(PACKAGE_VERSION "1.9.1") + +~/rdk_py310/bin/python -c 'import flexivrdk; print(flexivrdk.__version__)' +# 1.9.1 +``` + +`~/rdk_py310/bin/python` 是一个壳：清掉 `PYTHONHOME`（避免 conda 污染），再用系统 Python 3.10 加上自己的 `site-packages`。`flexivrdk` 1.9.1 的 ABI 是 cp310，**不要**装进 `ros_flexiv`（那是 Python 3.11）。 + +壳脚本内容（拷目录时已经带上；若要手写，注意把路径改成新机器的家目录）： + +```bash +#!/bin/bash +export PYTHONPATH="${HOME}/rdk_py310/lib/python3.10/site-packages${PYTHONPATH:+:$PYTHONPATH}" +unset PYTHONHOME +exec /usr/bin/python3.10 "$@" +``` + +--- + +## 7. 仿真环境 `ros_gz_sim` + +用仓库脚本，可重复执行： + +```bash +bash ~/ros2/scripts/install_sim_env.sh +bash ~/ros2/scripts/install_sim_env.sh --verify +``` + +脚本会创建 `ros_gz_sim`（Python 3.11），从 `robostack-staging` + `conda-forge` 安装 `ros-humble-ros-base`、`ros-humble-ros-gz`、`ros-humble-gz-ros2-control`、`ros2_control` / `ros2_controllers`、`controller_manager`、`xacro`、`tf2_ros`、`camera-info-manager`、`image-transport-plugins`，并把 **empy 钉在 3.3.4**。 + +本机 `--verify` 能通过时的关键版本： + +| 包 | 版本 | +|----|------| +| Gazebo Sim（`ign gazebo --version`） | 6.16.0 | +| `libignition-gazebo6` | 6.16.0 | +| `ros-humble-ros-gz` | 0.244.20 | +| `ros-humble-gz-ros2-control` | 0.7.15 | +| `empy` | 3.3.4（conda-forge） | +| `numpy` | 1.26.4（conda-forge） | + +Humble 配的是 Gazebo Fortress，命令是 `ign gazebo`，不是 `gz sim`。 + +仿真环境里**没有**真机那套 `torch` / `tensorrt` / `pyrealsense2` / `onnx`。本机仿真环境额外有一个 pip 的 `onnxruntime==1.30.0`。缺 `onnx` 时部分单测会红，这是现况；不要把真机的 pip 依赖灌进 `ros_gz_sim`，两个环境是故意隔开的。 + +--- + +## 8. 工作区、现场配置、编译 + +```bash +git clone <本仓库> ~/ros2 +cd ~/ros2 +# 若习惯从中文路径进： ln -s ~/ros2 ~/文档/ros2 +``` + +`config/site.yaml`、YOLO 权重（`.pt` / `.onnx` / `.engine`）不在 git 里。 + +```bash +cp ~/ros2/config/site.example.yaml ~/ros2/config/site.yaml +# 按新机器填写 robot.sn、robots[].ip、相机序列号、权重路径 +# 权重放到 models/<分组>/ ，见 models/README.md +``` + +TensorRT 的 `.engine` 跟 GPU 绑定。换了显卡（哪怕仍是 3090 以外的型号）要在新机器的 `ros_flexiv` 里重新 `bash ~/ros2/scripts/weights.sh convert --to engine`。直接拷本机的 `.engine` 经常加载失败。 + +手眼结果 `handeye/results/` 在 git 里，但是**这台机器、这个相机安装位**的矩阵。新机器克隆后要重新标定，见 [HANDEYE.md](HANDEYE.md)。 + +在 **`ros_flexiv`** 里编真机（先确认 `conda info --envs` 里星号在 `ros_flexiv`，python 指向 `~/anaconda3/envs/ros_flexiv/bin/python`）： + +```bash +cd ~/ros2 +conda activate ros_flexiv +bash scripts/build_all.sh +source scripts/env.sh +echo "ROS_DISTRO=$ROS_DISTRO ROS_DOMAIN_ID=$ROS_DOMAIN_ID" +# humble 与 0 +``` + +编完查 shebang 没有被仿真环境污染（正常应是 0）： + +```bash +grep -l ros_gz_sim install/*/lib/*/* | wc -l +``` + +仿真另开一个终端： + +```bash +cd ~/ros2 +bash scripts/build_sim.sh +source scripts/sim_env.sh +# 打印里应有 CONDA=ros_gz_sim ROS_DISTRO=humble ROS_DOMAIN_ID=77 +``` + +--- + +## 9. 装完怎么确认 + +```bash +# 真机环境 +source ~/ros2/scripts/env.sh +python -c 'import rclpy, torch, tensorrt, pyrealsense2, cv2, serial, minimalmodbus, onnx' +echo "$ROS_DISTRO $ROS_DOMAIN_ID" # humble 0 +~/rdk_py310/bin/python -c 'import flexivrdk; print(flexivrdk.__version__)' # 1.9.1 + +# 仿真环境（新终端） +bash ~/ros2/scripts/install_sim_env.sh --verify +``` + +有机器人、示教器已切到 AUTO + Remote Ethernet、并且 `config/site.yaml` 已填好之后，再跑： + +```bash +bash ~/ros2/scripts/test_rdk_python.sh +bash ~/ros2/scripts/run_real_preflight.sh --arm 89 +``` + +这两条会访问真机网络，空机器上失败不代表环境没装上。 + +--- + +## 10. 本机还有、但本仓库不用的 conda 环境 + +`conda env list` 里还有 `flexiv`、`flexiv_rdk_py312`、`orisys`、`py311`、`ros2`。`scripts/env.sh` 和 `scripts/sim_env.sh` 都不会激活它们。新机器只建 `ros_flexiv` 和 `ros_gz_sim` 即可。 diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index c64037b..3e56955 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -68,6 +68,21 @@ bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # T2 相机预览， bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（默认不拧入） ``` +一阶段默认 **D405 主跟踪**：画面里没有 conf≥`perception.wrist_handoff_conf`（0.5）的目标， +马上用顶视 D435 **拍一张** 把工件 / k0 定到基座系（眼在手外 T，不再乘法兰），把臂迁过去 +让 D405 重新看见。D405 跟住后，左臂法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过 +`lock_still_move_m`（15 mm）→ `/visual_servo/locked` → 二阶段对孔。 + +```bash +bash ~/ros2/scripts/run_real.sh --arm 89 +bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview +bash ~/ros2/scripts/run_camera_d435.sh --preview # 混合跟踪必须两台都开 +bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 # 先只看框 +bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control +``` + +`track_camera: overhead` 仍是整段只用 D435 的实验开关。非法值启动即报错。 + 细节、参数、退出码见 [`HOLE_DETECT.md`](HOLE_DETECT.md)。 ### 1.4 现在到哪一步了（如实标注，别信印象） @@ -78,7 +93,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（ | 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` | | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) | | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 | -| D435 手外相机（S03 粗定位兜底） | ❌ **还没做手眼标定**，链路不可用 | 见 `devices/cameras/README.md` | +| D435 手外相机（一阶段一次性 + S03） | ⚠️ **89 已标定**；一阶段 D405 看不见时拍一张定 K0（混合跟踪）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` | | 自动启动全部服务（一条命令拉起所有进程） | ❌ 还没做，目前靠三个终端手工起 | 见第 6 节待办 | | 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 | | 装配合格判定（通/止） | ⚠️ **通止规装配流程**能按力矩判据拧到底/拧出来，但"通/止合格"结论还没做 | `run_gauge_assembly.sh --arm 89 --dry-run` | @@ -357,7 +372,7 @@ bash scripts/weights.sh doctor --check-engines ### 4.3 单元测试怎么跑 -**每个功能包都有自己的测试**，改完对应包就跑一下。当前 pytest 收集 **1128 项** +**每个功能包都有自己的测试**，改完对应包就跑一下。当前 pytest 收集 **1301 项** （另有 1 项按环境 skip）。 ```bash @@ -486,7 +501,7 @@ bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的 且顺序错了会出"控制器没起来但脚本继续跑"这类问题。 2. **夹爪持规自检** —— 启动后先用触觉判断"规真的被夹着"，没有就报错停机 （现在没有这一步，规掉了会一路跑到撞工件才发现）。 -3. **D435 手眼标定** —— 标完 S03 粗定位链路才可用（工件不在 D405 视野里时靠它先粗居中）。 +3. **D435 手眼标定（91）** —— 89 已写入 `handeye/results/89_d435_eye_to_hand.yaml`（18 视角）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。 4. **装配合格判定** —— 把力曲线转成"通/止"结论。当前只输出原始几何与力， 判定逻辑（含螺纹旋入圈数）还没写。 diff --git a/docs/HANDEYE.md b/docs/HANDEYE.md index e4aec7d..87ec40f 100644 --- a/docs/HANDEYE.md +++ b/docs/HANDEYE.md @@ -1,50 +1,82 @@ -# 手眼标定（眼在手上） +# 手眼标定 -每台机械臂一份结果，**禁止混用**： +两种装法，两块板，两台相机，两份结果。**禁止混用**： -| 臂 | 结果 | 示教关节 | 工作目录 | -|----|------|----------|----------| -| 89 左 | `handeye/results/89_eye_in_hand.yaml` | `handeye/poses/89_eye_in_hand.yaml` | `handeye/tmp/89/` | -| 91 右 | `handeye/results/91_eye_in_hand.yaml` | `handeye/poses/91_eye_in_hand.yaml` | `handeye/tmp/91/` | +| 模式 | 相机 | 板 | 板怎么摆 | 结果文件 | `T_4x4` 含义 | +|------|------|----|----------|----------|--------------| +| `eye_in_hand` | 腕部 D405 | A5，内角点 8×11，15 mm | 板放台面不动 | `handeye/results/<臂号>_eye_in_hand.yaml` | 相机 → 法兰 | +| `eye_to_hand` | 顶置 D435 | A4，内角点 11×8，20 mm | **板装到法兰上** | `handeye/results/<臂号>_d435_eye_to_hand.yaml` | 相机 → 这条臂的基座 | -文件名统一为 `<臂号>_<模式>.yaml`（模式 = `eye_in_hand` | `eye_to_hand`）， -全部集中在仓库根 `handeye/`（`poses/` 输入、`results/` 输出、`tmp/` 临时产物）。 -目录结构、板尺寸三处一致性与全部选项见 `handeye/README.md`。 +| 臂 | 眼在手上结果 | 眼在手外结果 | 示教关节 | 工作目录 | +|----|--------------|--------------|----------|----------| +| 89 左 | `89_eye_in_hand.yaml` | `89_d435_eye_to_hand.yaml` | `poses/89_<模式>.yaml` | `tmp/89/` | +| 91 右 | `91_eye_in_hand.yaml` | `91_d435_eye_to_hand.yaml` | `poses/91_<模式>.yaml` | `tmp/91/` | -`T_4x4` = **相机 → 法兰**。跟踪和静态 TF 都读当前 `--arm` 对应的文件。 +目录结构、按键、稳定性参数见 `handeye/README.md`。棋盘尺寸只改 +`config/site.yaml` → `handeye.boards.<模式>`（内角点 = 方格数 − 1）。 +没配过的模式会被 `run_handeye.sh` 当场拦下，不会静默用另一块板的尺寸。 -棋盘尺寸只改 `site.yaml` → `handeye.board_*` / `square_size_m`（内角点 = 方格数 − 1）。两臂可共用同一块板。 - -## 流程 +## 眼在手上（腕部 D405 + A5 板） `teach`（RDK 软拖拽）与 `run_real.sh` **互斥**。全程带同一 `--arm`。 -`--mode eye_in_hand|eye_to_hand` 选模式（读写对应 `<臂号>_<模式>.yaml`，两种模式互不覆盖）； -`teach --points N` 设定目标点数；`teach --ros --auto-solve` 采完自动 merge 并接着标定。 ```bash # 1) 停掉 run_real.sh 和视觉伺服 # 2) 示教 8–12 个姿态（棋盘清晰、占画面 30–70%，姿态拉开） bash ~/ros2/scripts/run_handeye.sh --arm 89 teach -# 窗口：d=拖拽 Enter=拍照 q=结束 +# 窗口：d=拖拽 Enter=拍照 q=结束（窗口只显示画面，不做棋盘检测） # 接着采、不清空旧点： teach --append -# 3) 写入该臂 handeye/poses/<臂号>_<模式>.yaml +# 3) 写入该臂 handeye/poses/89_eye_in_hand.yaml bash ~/ros2/scripts/run_handeye.sh --arm 89 merge -# 4) 再开实机 + 相机，棋盘不要动 +# 4) 再开实机 + 腕部相机，棋盘不要动 bash ~/ros2/scripts/run_real.sh --arm 89 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate ``` -91 把 `--arm 89` 改成 `--arm 91`。91 的结果文件初始是 `uncalibrated: true`，标定成功后才会写出 `T_4x4`。 +91 把 `--arm 89` 改成 `--arm 91`。 -查看路径与板参数：`bash ~/ros2/scripts/run_handeye.sh --arm 89 show` +## 眼在手外（顶置 D435 + A4 板） + +物理摆法与上面**反过来**：相机固定，板跟着臂动。板若还摆在台面上， +`target→camera` 不变，方程退化（解不出来或解出垃圾）。 + +```bash +# 1) 把 A4 板刚性固定在法兰 / 夹爪上（夹在正中间，标定过程不要松） +# 2) 起臂 + 顶置相机（两个终端，常驻） +bash ~/ros2/scripts/run_real.sh --arm 89 +bash ~/ros2/scripts/run_camera_d435.sh --preview + +# 3) 采点：推着臂走，让板在 D435 画面里出现在不同位置、不同角度 +# teach 与 run_real.sh 互斥 —— 用 RDK 拖拽前先停掉 run_real.sh +bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12 + +# 4) 合并 + 自动走点求解（此时再开 run_real.sh + D435） +bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand +bash ~/ros2/scripts/run_real.sh --arm 89 +bash ~/ros2/scripts/run_camera_d435.sh --preview +bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand +``` + +查看当前模式选中的板 / 相机 / 文件： + +```bash +bash ~/ros2/scripts/run_handeye.sh --arm 89 show --mode eye_to_hand +``` + +应看到 `camera=d435`、`board 11x8 inner, square=0.02 m`、 +结果路径 `handeye/results/89_d435_eye_to_hand.yaml`。不是这三样就停， +不要带着错配置开标定。 ## 结果怎么用 -`visual_servo.launch.py` 读当前臂 `T_4x4`，发布 `{ee_frame}` → `{camera_frame}` 的 static TF。 - -`servo_node` 用同一份 T 做 `hover` / `lookat`。换结果后**重启** `run_visual_servo_rt.sh --arm …`。 - -装配 HMI（`ros_flexiv`）选左/右臂后，读同一份 `handeye/results/89_eye_in_hand.yaml` / `91_eye_in_hand.yaml`。 +- **眼在手上**：`visual_servo.launch.py` / `stage2_locate.launch.py` / + `gauge_assembly.launch.py` 读 `paths.handeye_file`，发布 + `{法兰}` → `{腕部相机}_color_optical_frame`。`servo_node` 用同一份 T + 做 `hover` / `lookat`。换结果后**重启**对应节点。 +- **眼在手外**：`gauge_assembly.launch.py` 按 + `handeye/results/<臂号>_<相机>_eye_to_hand.yaml` 自己找，发布 + `{基座}` → `{手外相机}_color_optical_frame`。S03 粗定位查的就是这条 TF。 + **不要**改 `paths.handeye_file` 去指向它 —— 那会把腕部精拍定位的 TF 换掉。 diff --git a/docs/HOLE_DETECT.md b/docs/HOLE_DETECT.md index eeb772f..36b66c3 100644 --- a/docs/HOLE_DETECT.md +++ b/docs/HOLE_DETECT.md @@ -99,15 +99,26 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0 ## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑） 先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 → -深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动夹爪/通止规轴线到目标孔正上方 → +深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动**规尖**到目标孔正上方 → 复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。 +> **对的是"规尖"，不是"法兰轴线"**（2026-10-08 定稿）。通止规夹在爪里，相对法兰轴线 +> 可以整体偏出若干毫米；旧写法把**法兰**对到孔上，规尖就偏着压在孔沿上，轻降一压 +> 腕力蹦到 63N。现在每一帧同时解「规尖基座 XY」和「孔基座 XY」——两者走同一条手眼 +> 链，链路误差与装夹变动一起抵消。规尖偏移是**刚体量**，量准一份（`tip_calib_n` 拍 +> 一致）就冻结。 +> 真机验证（arm 89，独立重测那一帧）：规尖到 k1 **0.34mm**、到 k2 61.15mm、 +> 反投影工具长 222mm（与 site 的 223mm 一致），偏移冻结值 (2.6, −1.0)mm。 +> 相关开关在 `site.yaml → hole_detect.stage2:`（`align_tip_vision` / `tip_*`）。 +> 量不到规尖偏移时二阶段**判失败**（exit 8），不会拿"法兰对上了"冒充成功。 + ```bash # 前置：T1 run_real.sh --arm 89；T3 run_camera_d405.sh --arm 89 --preview bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程（自动拉起触觉服务，仅左臂） bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只垂直+悬停锁定，用来调高度 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2 # 对准 k2（默认 k1） bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高，不动 yaml +bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 只验证精对准、不轻降 ``` 悬停锁高接口：`site.yaml → robots[89].servo.hole_hover_z_m`（91 在同名段）。 diff --git a/docs/QUICKSTART.md b/docs/QUICKSTART.md index f3c7aa7..f4b3d51 100644 --- a/docs/QUICKSTART.md +++ b/docs/QUICKSTART.md @@ -4,7 +4,9 @@ ## 依赖 -- Ubuntu 22.04 + ROS 2 Humble +从一台空机器装到和本机一样：见 **[ENVIRONMENT.md](ENVIRONMENT.md)**（2026-10-09 快照：RoboStack Humble、`ros_flexiv` / `ros_gz_sim`、RDK 1.9.1）。 + +- Ubuntu 22.04 + ROS 2 Humble（本机 ROS 在 conda 里，没有 `/opt/ros`） - Conda 环境 `ros_flexiv`（本仓库脚本默认这个名字） - Flexiv RDK Python：`/home/cc/rdk_py310/bin/python`（flexivrdk 1.9.x） - 工控机与机器人在同一网段（例：`192.168.2.10` ↔ `192.168.2.101`） diff --git a/docs/WORKSPACE.md b/docs/WORKSPACE.md index ebe132c..f79c3bb 100644 --- a/docs/WORKSPACE.md +++ b/docs/WORKSPACE.md @@ -84,7 +84,7 @@ devices/// | `run_camera_d405.sh --arm … --preview` | 该臂腕部 D405。89→`/d405_left/`，91→`/d405_right/` | | `run_camera_d435.sh --preview` | 顶视 D435（手外，可选） | | `list_realsense.sh` | 列出已接 RealSense 序列号 | -| `run_visual_servo_rt.sh --arm … [--control]` | YOLO + 伺服。无 `--control` 只看框；有则动臂。从当前姿态开始，不回 home | +| `run_visual_servo_rt.sh --arm … [--control]` | YOLO + 伺服。无 `--control` 只看框；有则动臂。`wrist`=D405 主跟踪，看不见则 D435 一次性定 K0；`overhead`=整段顶视 | | `go_home.sh --arm …` | 回 named pose `home`（会先停伺服） | | `show_site_config.sh` | 打印当前臂解析出的路径/锁高/相机 | @@ -183,6 +183,7 @@ devices/// | `FRAMEWORK_RULES.md` | **框架规则**（硬规矩，改代码前先读；第 8 节是按症状查的反例表） | | `WORKSPACE.md` | 本页，目录与职责 | | `QUICKSTART.md` | 环境、编译、第一次实机 | +| `ENVIRONMENT.md` | **按本机快照在另一台电脑上装同一套环境**（conda / RDK / 显卡 / 路径） | | `ARCHITECTURE.md` | 进程与数据流 | | `SITE_CONFIG.md` | `site.yaml` 字段 | | `HANDEYE.md` | 示教 / merge / 标定 | diff --git a/handeye/README.md b/handeye/README.md index 3f83468..561478d 100644 --- a/handeye/README.md +++ b/handeye/README.md @@ -16,7 +16,7 @@ | 模式 | 相机装在哪 | 解出的 X 是什么 | 本项目的用法 | |------|-----------|----------------|-------------| | `eye_in_hand` | **相机装在机械臂末端（法兰/手腕）上** | 相机 → 法兰 | **D405 腕部相机（当前使用）** | -| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机（预留） | +| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机 | **怎么选**：看相机是不是跟着机械臂动。 - 跟着动 → `eye_in_hand` @@ -50,40 +50,36 @@ handeye/ `handeye_workdir`。两条臂若共用一个工作目录，先标定的那份采点会被后 标定的覆盖，而症状是"标定结果突然变得很差"，不容易联想到是目录冲突。 -**命名约定**：`<臂号>_<模式>.yaml` -- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`，`Rizon4-063991` → `91`） -- 模式 = `eye_in_hand` | `eye_to_hand` - -运行时谁读结果：`config/site.yaml` 里 `robots[89].paths.handeye_file` 指向 -`handeye/results/89_eye_in_hand.yaml`，再由各 launch 文件发布成静态 TF。 -**换模式 = 把 site.yaml 里那个路径改成 `_eye_to_hand` 那份**，不需要改代码。 +**命名约定**： +- 眼在手上：`<臂号>_eye_in_hand.yaml`（`paths.handeye_file` 指向它） +- 眼在手外：`<臂号>_<相机>_eye_to_hand.yaml`（S03 按这个名字自己找， + 不要改 `paths.handeye_file`） +- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`） +- 示教点：`<臂号>_<模式>.yaml`（两种模式的关节角完全不同，分开存） --- ## 3. 标定板（最容易出错的地方） -当前使用的板： +两种装法用**两块不同的物理板**，尺寸按模式分开写，不能共用一组键： -``` -内角点 8 列 × 11 行 (注意是"内角点"，不是方格数！) -方格边长 15 mm (0.015 m) -``` +| 模式 | 物理板 | 内角点（列 × 行） | 方格边长 | `site.yaml` 里的位置 | +|------|--------|-------------------|----------|----------------------| +| `eye_in_hand` | A5（腕部相机看台面） | 8 × 11 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` | +| `eye_to_hand` | A4（板装在法兰上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` | -**这个尺寸必须三处一致**，否则标定结果会系统性偏错： - -| 位置 | 内容 | -|------|------| -| `config/site.yaml` → `handeye.board_cols/rows/square_size_m` | 8 / 11 / 0.015 | -| `handeye/calibrate.yaml` | `board_cols/rows/square_size_m` | -| `src/.../handeye_calibrate.py` 的参数默认值 | 代码兜底 | - -> ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、 -> `8×11/0.015`，谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错， -> 但结果明显不对"，非常难查。现已统一为 `8×11/0.015`。 -> **换板子时，请把上面三处一起改。** +权威只有 `config/site.yaml` 的 `handeye.boards.<模式>`。`run_handeye.sh` 按 +`--mode` 取对应那一组，用 `-p` 传给节点。`handeye/calibrate.yaml` 和节点默认值 +只是兜底（眼在手上那块板），**没配过的模式会被脚本当场拦下**，不会静默用兜底 +尺寸去标（那会让结果系统性偏，还不报错）。 怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**， -是「方格数 − 1」。比如 A5 板上画了 9×12 个格子 → 内角点是 8×11。 +是「方格数 − 1」。A5 板上画了 9×12 个格子 → 内角点 8×11； +A4 板上画了 12×9 个格子 → 内角点 11×8。列/行顺序不用纠结，检测端两种顺序都会试。 + +> ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、 +> `8×11/0.015`，谁生效取决于覆盖顺序。后来又把两块板合成一组键，于是标眼在手外 +> 时静默用了 A5 的 15 mm。症状都是"标定流程跑得很顺、不报错，但结果明显不对"。 --- @@ -97,7 +93,8 @@ handeye/ bash ~/ros2/scripts/run_handeye.sh teach --arm 89 ``` -会弹出实时预览窗口，里面能看到标定板有没有被识别到。操作： +会弹出实时预览窗口（**只显示画面，不做棋盘检测**——真正的角点稳定判定在 +`calibrate` 那一步）。操作： | 按键 | 作用 | |------|------| @@ -190,20 +187,25 @@ bash ~/ros2/scripts/run_handeye.sh teach --ros --auto-solve --arm 89 ### 切换 eye-in-hand / eye-to-hand ```bash -# 用腕部相机（默认） +# 用腕部相机 + A5 板（默认，板放台面不动） bash ~/ros2/scripts/run_handeye.sh --arm 89 teach -# 用固定相机 -bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand +# 用固定相机 + A4 板（板必须装到法兰上，相机不动） +# 前置：另开终端 bash ~/ros2/scripts/run_camera_d435.sh --preview +bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12 bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand ``` -`--mode` 会把读写的文件切到 `<臂号>_<模式>.yaml`，所以： +`--mode` 会同时切：标定板、相机、示教点文件、结果文件。所以： - 两种模式**互不覆盖**，可以分别标定、共存 -- 跑完想切换使用，改 `config/site.yaml` 里 `robots[89].paths.handeye_file` - 指向对应那份即可，代码不用动 +- 眼在手上结果：`handeye/results/<臂号>_eye_in_hand.yaml`（`T` = 相机 → 法兰） +- 眼在手外结果：`handeye/results/<臂号>_<相机>_eye_to_hand.yaml` + （`T` = 相机 → 这条臂的基座；双臂共用一台 D435 时必须按臂分开） +- **不要**把 `paths.handeye_file` 改成眼在手外那份 —— 那条路径是腕部相机 + （S05 精拍定位）用的。手外那份由 `gauge_assembly.launch` 按名字自己找， + 并以 **基座 ← 相机** 发布 TF（不是法兰 ← 相机） 传了非法模式会立刻报错并退出（不会悄悄退回默认值 —— 那会导致"以为标的是 eye-to-hand、其实算的是 eye-in-hand"这种最难查的错）。 diff --git a/handeye/calibrate.yaml b/handeye/calibrate.yaml index d95bef4..592d112 100644 --- a/handeye/calibrate.yaml +++ b/handeye/calibrate.yaml @@ -48,10 +48,10 @@ handeye_calibrate: camera_cy: 0.0 camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0] - # 真实使用的标定板：内角点 8x11，方格 15mm。 - # 依据是 handeye/results/*.yaml 头部记录的实测板子，以及 site.yaml 的 handeye.board_*。 - # 这里原先写的是 11x16 / 0.011（另一张早期用过的板）—— 三处配置曾互相矛盾， - # 谁生效取决于覆盖顺序，因此存在"标定跑完但结果不对"的风险。 + # 板尺寸的权威在 site.yaml → handeye.boards.<模式>。 + # 这里只写眼在手上那块板的兜底（A5 / 8×11 / 15mm）；run_handeye.sh 会按 + # --mode 用 -p 覆盖成对应模式的值。没配过的模式会被脚本当场拦下， + # 不会静默用这份兜底去标另一块板。 board_cols: 8 board_rows: 11 square_size_m: 0.015 diff --git a/handeye/poses/89_eye_to_hand.yaml b/handeye/poses/89_eye_to_hand.yaml new file mode 100644 index 0000000..4c2cb72 --- /dev/null +++ b/handeye/poses/89_eye_to_hand.yaml @@ -0,0 +1,50 @@ +# 眼在手外示教关节（7 关节 × N）。板装在法兰上，顶置 D435 看它。 +# +# 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。 +# +# bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12 +# bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand + +/**: + ros__parameters: + num_poses: 19 + waypoint_positions: [ + # pose 0 + -1.045969, -0.575935, 0.132913, 2.251024, -0.297265, 1.274437, -0.219817, + # pose 1 + -1.221272, -0.582225, 0.269777, 2.060852, -0.180051, 1.157081, -0.070498, + # pose 2 + -1.222672, -0.605722, 0.273858, 2.111021, -0.181634, 1.193047, -0.059758, + # pose 3 + -1.219127, -0.576923, 0.281368, 2.310613, -0.162188, 1.339573, -0.026065, + # pose 4 + -1.100853, -0.552692, 0.291246, 2.473326, -0.116403, 1.504852, 0.051283, + # pose 5 + -1.022050, -0.394038, 0.375788, 2.311711, -0.503617, 1.217768, 1.268088, + # pose 6 + -1.180556, -0.574370, 0.471095, 2.055400, -0.490664, 1.086456, 1.216527, + # pose 7 + -2.039707, -0.492556, 1.462952, 2.222064, -0.329990, 0.209789, 0.523258, + # pose 8 + -1.959748, -0.554596, 1.469405, 2.293419, -0.165008, 0.422761, 0.844081, + # pose 9 + -1.666347, -0.747468, 1.236192, 2.176000, -0.303646, 0.773895, 1.279040, + # pose 10 + -1.689100, -0.879982, 1.168013, 2.057620, -0.424417, 0.752234, 1.241290, + # pose 11 + -1.691875, -0.889908, 1.154014, 2.062930, -0.549154, 0.850985, 1.257704, + # pose 12 + -1.715420, -0.986765, 1.125361, 1.948461, -0.638012, 0.809355, 1.173290, + # pose 13 + -1.729057, -1.004106, 1.126932, 1.961482, -0.727920, 0.900058, 1.188069, + # pose 14 + -2.034166, -0.748356, 1.234076, 2.252718, -1.542249, 0.093972, 0.640260, + # pose 15 + -2.155450, -0.769613, 1.249491, 2.275528, -1.648348, 0.073256, 0.495109, + # pose 16 + -1.959788, -0.979802, 1.271261, 2.222882, -1.560991, 0.057204, 0.296625, + # pose 17 + -1.933247, -0.398944, 1.211885, 2.312756, -0.424981, 0.358678, 0.347203, + # pose 18 + -1.944530, -0.399220, 1.244635, 2.293245, -0.423596, 0.317991, 0.358565, + ] diff --git a/handeye/results/89_d435_eye_to_hand.yaml b/handeye/results/89_d435_eye_to_hand.yaml new file mode 100644 index 0000000..71f826a --- /dev/null +++ b/handeye/results/89_d435_eye_to_hand.yaml @@ -0,0 +1,13 @@ +# 手眼标定结果 +# mode: eye_to_hand +# camera_source: d435 +# views: 18 +# utc: 2026-10-09T02:56:24.621687+00:00 +# board: 11x8 square_m=0.02 +# T_cam_to_base (eye_to_hand) +translation_m: [0.37495201, -0.52156937, 0.72589194] +T_4x4: + - [-0.04575158, 0.99819969, -0.03878355, 0.37495201] + - [0.99893621, 0.04594039, 0.00399073, -0.52156937] + - [0.00576527, -0.03855971, -0.99923967, 0.72589194] + - [0.00000000, 0.00000000, 0.00000000, 1.00000000] diff --git a/models/yolo/gongjian0929.labels.yaml b/models/yolo/gongjian0929.labels.yaml new file mode 100644 index 0000000..716da00 --- /dev/null +++ b/models/yolo/gongjian0929.labels.yaml @@ -0,0 +1,16 @@ +# 模型类别标签（class names） +# +# 一个模型对应一份标签，放在权重旁边、同名前缀： +# detect_best_0.engine ← 权重 +# detect_best_0.labels.yaml ← 本文件 +# +# 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。 +# 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。 +# +# 命令行查看/修改（推荐，不用手改本文件）： +# bash ~/ros2/scripts/weights.sh labels <模型名> +# bash ~/ros2/scripts/weights.sh labels <模型名> --set "工件,通止规" + +model: gongjian0929 +names: + - workpiece diff --git a/scripts/_verify_hole_lost.py b/scripts/_verify_hole_lost.py new file mode 100644 index 0000000..f152855 --- /dev/null +++ b/scripts/_verify_hole_lost.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +"""确定性复现「工件被推走 ⇒ 规没进孔」：在 k1 之后把工件挪 25mm。 + +为什么需要它：这个故障在自然运行里是**随机**的（实测三跑坏一跑）， +靠碰运气验证判据不可靠。这里改成主动制造：等流程走到 S10（离开 k1、去 k2）， +就把工件沿 +Y 推 25mm —— 而 k2 的孔位是 S05 一次性定下的，于是必然失配。 + +判据（要看的就是这个）：S07 必须报「规没有进孔」（HOLE_LOST）， +**不是** 之前那句把人引去查工具系的「转轴偏差」。 +""" +from __future__ import annotations + +import pathlib +import subprocess +import sys +import time + +ROOT = pathlib.Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT / "src" / "arm_sim_gazebo")) + +LOG = pathlib.Path("/tmp/sim_guard.log") +WORLD = "assembly" +SHIFT_M = -0.020 # 与 2026-09-28 自然失效同向同量级（工件 y: 0.000 → -0.0206） + + + +def _shift() -> None: + from arm_sim_gazebo.sim_pose_service import set_model_pose + + # 工件当前在 (0.520, 0.000, 0.400)，沿 +Y 挪 25mm（远超孔半径，必然错过） + ok, out = set_model_pose(WORLD, "workpiece", (0.520, SHIFT_M, 0.400), (0.0, 0.0, 0.0, 1.0)) + print(f"[driver] 推工件 {SHIFT_M * 1000:.0f}mm -> {'成功' if ok else '失败'}: {out[:160]}") + + +def main() -> int: + LOG.write_text("") + proc = subprocess.Popen( + ["bash", "scripts/run_gauge_assembly.sh", "--arm", "sim", "--sim"], + cwd=str(ROOT), stdout=LOG.open("w"), stderr=subprocess.STDOUT, + ) + print(f"[driver] 流程已起 pid={proc.pid}，等它走到 S10…") + + shifted = False + t0 = time.monotonic() + while proc.poll() is None and time.monotonic() - t0 < 1500: + txt = LOG.read_text(errors="replace") + if not shifted and "S10_to_k2" in txt: + print("[driver] 检测到 S10_to_k2 —— 现在推工件") + _shift() + shifted = True + time.sleep(1.0) + + proc.wait(timeout=30) + txt = LOG.read_text(errors="replace") + print(f"[driver] 流程退出码 = {proc.returncode}, 是否推过工件 = {shifted}") + for line in txt.splitlines(): + if "规没有进孔" in line or "孔位失效" in line or "转轴偏差" in line: + print(" |", line.strip()[:150]) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/gen_site_example.py b/scripts/gen_site_example.py index 2b83ca9..6852ebc 100644 --- a/scripts/gen_site_example.py +++ b/scripts/gen_site_example.py @@ -67,9 +67,11 @@ LITERAL_REPLACEMENTS: list[tuple[str, str]] = [ # 用户名匿名化。放在其它规则之前，避免路径被二次处理。 ("/home/cc", "/home/USER"), # 注释里裸写的机械臂编号（无引号，正则抓不到）。 + # ⚠️ 这行是**逐字匹配**：site.yaml 里改一个字它就静默失效，SN 就漏进模板了 + #（test_real_devices.test_example_contains_no_live_values 会挡下来）。 ( - "# User1=enp4s0 本机 192.168.2.10 ↔ 063989=.101 / 063991=.100", - "# User1=enp4s0 本机 ↔ <左臂SN>=.101 / <右臂SN>=.100", + "# 本机 192.168.2.10 ↔ 063989(左89)=.101 / 063991(右91)=.100", + "# 本机 ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100", ), ] @@ -77,6 +79,15 @@ LITERAL_REPLACEMENTS: list[tuple[str, str]] = [ REGEX_REPLACEMENTS: list[tuple[str, str]] = [ # 机械臂序列号：Rizon4-063989 -> Rizon4-XXXXXX (r'"(Rizon4)-\d+"', r'"\1-XXXXXX"'), + # 同上，但**不带引号**（注释里的裸写）。留一条兜底，免得上面的逐字规则 + # 一旦和 site.yaml 不同步就把 SN 漏出去。 + (r"(Rizon4)-\d+", r"\1-XXXXXX"), + # sn 里的连字符在下划线形式下（话题名不许含 '-'，broadcaster 用 '_'）： + # 例 "/Rizon4_063989/external_wrench_in_tcp" -> "/Rizon4_XXXXXX/external_wrench_in_tcp" + (r"(Rizon4)_\d+", r"\1_XXXXXX"), + # USB 网卡的 iface 名是 MAC 派生的（enx + 12 位十六进制）= 现场值。 + # enp4s0 这类板载名是通用的，不动。 + (r"enx[0-9a-fA-F]{12}", "enxXXXXXXXXXXXX"), # IP 地址（仅带引号的配置值；注释里的裸 IP 由上面的字面替换处理） (r'"192\.168\.\d{1,3}\.\d{1,3}"', '"192.168.2.XXX"'), # 8 位及以上的纯数字字符串 = 设备序列号（相机 12 位 / 夹爪 8 位）。 diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py index e6352f1..400d655 100644 --- a/scripts/lib/export_site_env.py +++ b/scripts/lib/export_site_env.py @@ -272,7 +272,12 @@ def camera_ns(data: dict) -> str: def camera_frames(data: dict) -> tuple[str, str, str]: - """当前机械臂的（命名空间, 光学坐标系, 深度话题）。""" + """当前机械臂**腕部**相机的（命名空间, 光学坐标系, 深度话题）。 + + 一阶段跟踪用哪台相机见 ``track_camera_frames`` —— 不许改本函数去跟 + ``perception.track_camera``：``SITE_CAM_NAME`` / 二阶段 / 入孔检测都走这里， + 切跟踪相机不该把精拍那路也切走。 + """ ns = camera_ns(data) yolo = data.get("yolo") or {} frame = str(yolo.get("camera_frame") or "").strip() @@ -284,6 +289,218 @@ def camera_frames(data: dict) -> tuple[str, str, str]: return ns, frame, depth +#: 顶视跟踪必须自己写的深度量程。缺了就借腕部 0.55m 上限 = D435 的点全丢。 +_OVERHEAD_RANGE_REQUIRED = ("min_depth_m", "max_depth_m", "feature_min_range_m") +#: D405 主跟踪切 D435 一次性时必须显式写的键。缺了就静默不切 = 臂停在半空。 +_ONESHOT_REQUIRED = ( + "oneshot_conf", + "cooldown_s", + "frame_max_age_s", + "oneshot_hold_s", +) + + +def track_camera_frames(data: dict) -> tuple[str, str, str, str]: + """一阶段跟踪用的（命名空间, 光学坐标系, 深度话题, 手眼模式）。 + + ``perception.track_camera``： + * ``wrist`` → 腕部相机 + ``eye_in_hand`` + * ``overhead`` → 手外相机 + ``eye_to_hand`` + """ + data, robot = apply_active_arm(data) + sio = _load_site_io() + role = sio.track_camera_role(data) + mode = sio.track_handeye_mode(role) + if role == "overhead": + over = overhead_camera(data, robot) + ns = str(over.get("name") or "").strip("/") + if not ns: + raise RuntimeError( + "perception.track_camera=overhead，但 cameras 里没有 " + "role 以 overhead 开头的相机" + ) + return ns, f"{ns}_color_optical_frame", f"/{ns}/aligned_depth_to_color/image_raw", mode + ns, frame, depth = camera_frames(data) + return ns, frame, depth, mode + + +def _track_handeye_file(data: dict, robot: dict, mode: str) -> str: + """跟踪用的手眼文件。overhead 不许去读 ``paths.handeye_file``（那是腕部的）。""" + if mode != "eye_to_hand": + return str((data.get("paths") or {}).get("handeye_file") or "") + over = overhead_camera(data, robot) + cam = str(over.get("name") or "").strip("/") + short = arm_short_id(str(robot.get("sn") or "")) + name = _load_site_io().eye_to_hand_result_name(short, cam) + return str(_root() / "handeye" / "results" / name) + + +def _apply_overhead_range(data: dict, params: dict) -> None: + """把 ``perception.overhead`` 的深度量程盖到感知参数上。缺键就红。""" + oh = (data.get("perception") or {}).get("overhead") + if not isinstance(oh, dict): + raise RuntimeError( + "perception.track_camera=overhead 必须在 perception.overhead 写顶视" + "深度量程（min_depth_m / max_depth_m / feature_min_range_m）。" + "腕部那组 0.55m 上限会把 D435 的点全丢掉，看起来像检测失败。" + ) + missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh] + if missing: + raise RuntimeError( + f"perception.overhead 缺 {missing} —— 不许借腕部量程。" + ) + params["min_depth_m"] = float(oh["min_depth_m"]) + params["max_depth_m"] = float(oh["max_depth_m"]) + params["feature_min_range_m"] = float(oh["feature_min_range_m"]) + if "work_z_max_m" in oh: + params["work_z_max_m"] = float(oh["work_z_max_m"]) + if "z_ref_m" in oh: + params["z_ref_m"] = float(oh["z_ref_m"]) + if "feature_mode" in oh: + params["feature_mode"] = str(oh["feature_mode"]) + if "hold_point_on_miss" in oh: + params["hold_point_on_miss"] = bool(oh["hold_point_on_miss"]) + + +def _apply_wrist_hybrid(data: dict, robot: dict, params: dict) -> None: + """腕部主跟踪 + D435 一次性：参数接到感知节点，**不许盖掉腕部量程**。 + + 顶视量程写进 ``overhead_*`` 键。若直接复用 ``min_depth_m``，D405 会按 + 0.25–1.80 m 收点，近处工件全丢，看起来像检测失败。 + """ + perc = data.get("perception") or {} + if "wrist_handoff_conf" not in perc: + raise RuntimeError( + "perception.track_camera=wrist 的混合跟踪必须写 " + "perception.wrist_handoff_conf（画面内最高 conf 低于此才切 D435）。" + "不许在代码里写死 0.5 再抄一份到 yaml。" + ) + params["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"]) + oh = perc.get("overhead") + if not isinstance(oh, dict): + raise RuntimeError( + "wrist 混合跟踪必须在 perception.overhead 写顶视量程和一次性参数" + "（min_depth_m / max_depth_m / feature_min_range_m / oneshot_conf / " + "cooldown_s / frame_max_age_s / oneshot_hold_s）。" + ) + missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh] + if missing: + raise RuntimeError( + f"perception.overhead 缺 {missing} —— 不许借腕部量程做 D435 一次性。" + ) + miss2 = [k for k in _ONESHOT_REQUIRED if k not in oh] + if miss2: + raise RuntimeError(f"perception.overhead 缺一次性键 {miss2} —— 不许用代码默认值。") + over = overhead_camera(data, robot) + ns = str(over.get("name") or "").strip("/") + if not ns: + raise RuntimeError( + "wrist 混合跟踪需要 cameras 里 role 以 overhead 开头的相机" + ) + e2h = _track_handeye_file(data, robot, "eye_to_hand") + params["overhead_oneshot_enable"] = True + params["overhead_min_depth_m"] = float(oh["min_depth_m"]) + params["overhead_max_depth_m"] = float(oh["max_depth_m"]) + params["overhead_feature_min_range_m"] = float(oh["feature_min_range_m"]) + params["overhead_oneshot_conf"] = float(oh["oneshot_conf"]) + params["overhead_cooldown_s"] = float(oh["cooldown_s"]) + params["overhead_frame_max_age_s"] = float(oh["frame_max_age_s"]) + params["overhead_hold_s"] = float(oh["oneshot_hold_s"]) + params["overhead_handeye_file"] = e2h + params["overhead_camera_source"] = ns + params["overhead_camera_frame"] = f"{ns}_color_optical_frame" + params["overhead_image_topic"] = f"/{ns}/color/image_raw" + params["overhead_info_topic"] = f"/{ns}/color/camera_info" + params["overhead_depth_topic"] = f"/{ns}/aligned_depth_to_color/image_raw" + + +#: 手眼标定的两种装法。写在这里当"合法值清单"，避免各处各写一遍字符串。 +HANDEYE_MODES = ("eye_in_hand", "eye_to_hand") + + +def handeye_board(he: dict, mode: str) -> tuple[dict, str]: + """取某个模式用的标定板尺寸 → ``({cols, rows, square_size_m}, 来源)``。 + + 权威只有一处：``handeye.boards.``。为什么按模式分开——两种装法用的是 + **两块不同的物理板**（本项目：眼在手上 A5 / 眼在手外 A4），方格边长不一样。 + 合成一组键的后果不是报错，而是"跑另一种模式时静默用错边长"，结果只是系统性偏， + 现场几乎联想不到板尺寸。 + + 兼容：没有 ``boards`` 段时，回落到旧的顶层 ``board_cols/rows/square_size_m``。 + 返回的第二个值是来源（``boards`` / ``legacy`` / ``default``），调用方据此决定 + 要不要拦下——``default`` 意味着**没人配过这块板**，此时开标定等于赌一个尺寸。 + """ + boards = he.get("boards") if isinstance(he.get("boards"), dict) else {} + node = boards.get(mode) if isinstance(boards, dict) else None + src = "boards" + if not isinstance(node, dict): + node = he + src = "legacy" + if mode not in HANDEYE_MODES: + # 不认识的模式：不猜板子，交给调用方报错（猜错就是静默用错尺寸）。 + node, src = {}, "default" + + def pick(key: str, cast, default): + v = node.get(key) if isinstance(node, dict) else None + if v is None: + return default, False + try: + return cast(v), True + except (TypeError, ValueError): + return default, False + + # boards 段用 cols/rows；旧的顶层键是 board_cols/board_rows。两种都认， + # 免得"兼容旧配置"其实读不到旧键，悄悄退回默认 8×11。 + cols, ok_c = pick("cols", int, 8) + if not ok_c: + cols, ok_c = pick("board_cols", int, 8) + rows, ok_r = pick("rows", int, 11) + if not ok_r: + rows, ok_r = pick("board_rows", int, 11) + sq, ok_s = pick("square_size_m", float, 0.015) + if src == "legacy" and not (ok_c or ok_r or ok_s): + # 顶层键一个都没有 ⇒ 真的没人配过，不要把"内置默认值"伪装成"配置"。 + src = "default" + return {"cols": cols, "rows": rows, "square_size_m": sq}, src + + +def overhead_camera(data: dict, robot: dict | None = None) -> dict: + """手外（固定）相机：``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台。 + + **规则只有一份**，在 ``arm_application.site_io.overhead_camera``——S03 粗定位 + 取命名空间走的也是它。这里若自己再写一遍"按 role 找 + 按 arm 收敛"，两份判据 + 必然漂移，症状是"标定用的相机"和"推理用的相机"不是同一台，且错而不报。 + 所以这里只做一层薄封装（从源码树 import），拿到配置字典。 + """ + fn = _load_overhead_camera_fn() + cam, _others = fn(data, robot) + return cam + + +def _load_site_io(): + """从 ``src/arm_application`` 取 ``site_io`` 模块。 + + 为什么从源码树而不是已安装包：``export_site_env.py`` 在 ``colcon build`` + 之前就被调用（``load_site_config.sh`` 的第一件事），那时 install 里可能还没有 + 这个包。源码树在本仓库里恒定存在（``scripts/`` 与 ``src/`` 同根）。 + """ + src = _root() / "src" / "arm_application" + if not (src / "arm_application" / "site_io.py").is_file(): + raise RuntimeError( + f"找不到 {src}/arm_application/site_io.py —— 手外相机 / 结果文件名的规则" + "只有那一处，不接受本文件自己再实现一份（两份判据必然漂移）。" + ) + if str(src) not in sys.path: + sys.path.insert(0, str(src)) + from arm_application import site_io as _mod # noqa: PLC0415 + + return _mod + + +def _load_overhead_camera_fn(): + return _load_site_io().overhead_camera + + def flatten(data: dict) -> dict: """供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。""" data, robot = apply_active_arm(data) @@ -313,7 +530,59 @@ def flatten(data: dict) -> dict: # 两边会共用同一个手眼文件，表现是"标定做了但不生效"。 short = arm_short_id(str(robot.get("sn") or "")) he_mode = str(he.get("mode") or "eye_in_hand") - he_stem = f"{short}_{he_mode}" if short else he_mode + over = overhead_camera(data, robot) + over_name = str(over.get("name") or "").strip("/") + root = _root() + + # 手眼标定的"按模式"派生物：结果文件 / 示教点 / 标定板。 + # + # 为什么一切都要按模式分开（eye_in_hand / eye_to_hand）： + # 1) 结果文件：眼在手外的产物是"**这台相机的光心 → 这条臂的基座**"，换条臂 + # （基座不同）或换台相机都不通用 ⇒ 名字要同时带臂号与相机名 + # （<臂号>_<相机>_eye_to_hand）。曾经只有 <臂号>_<模式>，于是一个双臂共用 + # 的手外相机只能存一份外参，第二条臂拿到的必然错； + # 2) 标定板：两种装法用两块不同的物理板（A5 / A4），方格边长不同 ⇒ 见 + # handeye_board() 的说明； + # 3) 示教点：两种模式采的关节角完全不同，共用一份会把上一模式的点位当成本 + # 模式的（merge 之后才发现在动错的姿态）。 + he_modes_out: dict[str, str] = {} + _e2h_name = _load_site_io().eye_to_hand_result_name + for _m in HANDEYE_MODES: + _tag = _m.upper() + _pose_stem = f"{short}_{_m}" if short else _m + # 结果文件名：眼在手外走 site_io.eye_to_hand_result_name（唯一口径）； + # 眼在手上仍是 <臂号>_eye_in_hand.yaml。 + if _m == "eye_to_hand": + _result_name = _e2h_name(short, over_name) + else: + _result_name = f"{_pose_stem}.yaml" + # eye_to_hand 允许用显式路径覆盖（与 eye_in_hand 的 paths.handeye_file 对称）； + # 不写就用派生的名字。 + if _m == "eye_in_hand": + _file = str( + paths.get("handeye_file") + or root / "handeye" / "results" / _result_name + ) + _wps = str( + paths.get("handeye_waypoints") + or root / "handeye" / "poses" / f"{_pose_stem}.yaml" + ) + else: + _file = str( + paths.get(f"handeye_file_{_m}") + or root / "handeye" / "results" / _result_name + ) + _wps = str( + paths.get(f"handeye_waypoints_{_m}") + or root / "handeye" / "poses" / f"{_pose_stem}.yaml" + ) + _board, _src = handeye_board(he, _m) + he_modes_out[f"SITE_HANDEYE_FILE_{_tag}"] = _file + he_modes_out[f"SITE_HANDEYE_WAYPOINTS_{_tag}"] = _wps + he_modes_out[f"SITE_HANDEYE_BOARD_COLS_{_tag}"] = str(_board["cols"]) + he_modes_out[f"SITE_HANDEYE_BOARD_ROWS_{_tag}"] = str(_board["rows"]) + he_modes_out[f"SITE_HANDEYE_SQUARE_{_tag}"] = str(_board["square_size_m"]) + he_modes_out[f"SITE_HANDEYE_BOARD_SRC_{_tag}"] = _src out = { "SITE_CONFIG": str(site_path()), @@ -339,10 +608,7 @@ def flatten(data: dict) -> dict: ), "SITE_YOLO_WEIGHTS": weights, "SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved, - "SITE_HANDEYE_WAYPOINTS": str( - paths.get("handeye_waypoints") - or str(_root() / "handeye" / "poses" / f"{he_stem}.yaml") - ), + "SITE_HANDEYE_WAYPOINTS": he_modes_out["SITE_HANDEYE_WAYPOINTS_EYE_IN_HAND"], "SITE_HANDEYE_CALIBRATE": str( paths.get("handeye_calibrate") or str(_root() / "handeye" / "calibrate.yaml") @@ -355,18 +621,24 @@ def flatten(data: dict) -> dict: paths.get("handeye_workdir") or str(_root() / "handeye" / "tmp" / short) ), - "SITE_HANDEYE_FILE": str( - paths.get("handeye_file") - or str(_root() / "handeye" / "results" / f"{he_stem}.yaml") - ), + # 不带后缀的两个名字保持旧语义（= 腕部 eye-in-hand 的产物），因为 + # launch / 其它脚本一直按这两个名字读；换成"当前模式"会让 site.yaml 里 + # 一改 handeye.mode 就把 S05 精拍定位的 TF 换掉，那是另一个功能的静默失效。 + # 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。 + "SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"], "SITE_HANDEYE_MODE": he_mode, - # 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。 - # 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的 - # 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸， - # 表现为"标定能跑完但结果明显不对"。 - "SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 8)), - "SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 11)), - "SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.015)), + **he_modes_out, + # 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方； + # 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。 + "SITE_HANDEYE_BOARD_COLS": he_modes_out[ + f"SITE_HANDEYE_BOARD_COLS_{he_mode.upper()}" + ], + "SITE_HANDEYE_BOARD_ROWS": he_modes_out[ + f"SITE_HANDEYE_BOARD_ROWS_{he_mode.upper()}" + ], + "SITE_HANDEYE_SQUARE_M": he_modes_out[ + f"SITE_HANDEYE_SQUARE_{he_mode.upper()}" + ], "SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)), "SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)), "SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)), @@ -396,6 +668,21 @@ def flatten(data: dict) -> dict: "SITE_CAM_LASER_POWER": str(float(cam.get("laser_power") if cam.get("laser_power") is not None else 0)), "SITE_CAM_EXPOSURE_US": str(float(cam.get("exposure_us") or 0)), "SITE_CAM_GAIN": str(float(cam.get("gain") or 0)), + # 手外（固定）相机的**按角色**导出。eye-to-hand 标定要用它：标定时得订阅 + # 这台固定相机，而不是腕部相机（run_handeye.sh 原来恒用 SITE_CAM_NAME， + # 于是 `--mode eye_to_hand` 会去开腕部相机 —— 标出一个"看着成功"的错结果）。 + "SITE_OVERHEAD_CAM_NAME": over_name, + "SITE_OVERHEAD_CAM_SERIAL": str(over.get("serial") or ""), + "SITE_OVERHEAD_CAM_WIDTH": str(int(over.get("width") or 1280)), + "SITE_OVERHEAD_CAM_HEIGHT": str(int(over.get("height") or 720)), + "SITE_OVERHEAD_CAM_FPS": str(int(over.get("fps") or 30)), + "SITE_OVERHEAD_CAM_AUTO_EXPOSURE": "1" if over.get("auto_exposure", True) else "0", + "SITE_OVERHEAD_CAM_AE_LIMIT_US": str( + float(over.get("ae_limit_us") if over.get("ae_limit_us") is not None else 8000) + ), + "SITE_OVERHEAD_CAM_EMITTER": "1" if over.get("emitter_enabled", True) else "0", + "SITE_OVERHEAD_CAM_EXPOSURE_US": str(float(over.get("exposure_us") or 0)), + "SITE_OVERHEAD_CAM_GAIN": str(float(over.get("gain") or 0)), "SITE_D435_SERIAL": str(((data.get("cameras") or {}).get("d435") or {}).get("serial") or ""), "SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)), "SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)), @@ -417,6 +704,14 @@ def flatten(data: dict) -> dict: "SITE_EE_Z_M": str(float(servo.get("ee_z_m") or 0.24)), "SITE_SERVO_WATCHDOG": str(servo.get("watchdog_timeout_sec") or 0.3), "SITE_CAMERA_FRAME": camera_frames(data)[1], + # 一阶段跟踪用的相机/手眼（可与腕部精拍不同）。SITE_CAM_NAME 仍是腕部。 + "SITE_TRACK_CAMERA": _load_site_io().track_camera_role(data), + "SITE_TRACK_CAM_NAME": track_camera_frames(data)[0], + "SITE_TRACK_CAMERA_FRAME": track_camera_frames(data)[1], + "SITE_TRACK_HANDEYE_MODE": track_camera_frames(data)[3], + "SITE_TRACK_HANDEYE_FILE": _track_handeye_file( + data, robot, track_camera_frames(data)[3] + ), "SITE_GRIP_VENDOR": str(grip.get("vendor") or "dh"), "SITE_GRIP_MODEL": str(grip.get("model") or "PGEA-100-20-W"), "SITE_GRIP_SN": str(grip.get("sn") or ""), @@ -490,14 +785,14 @@ def perception_overrides(data: dict) -> dict: data, _robot = apply_active_arm(data) paths = data.get("paths") or {} yolo = data.get("yolo") or {} - cam_ns, cam_frame, depth_topic = camera_frames(data) + cam_ns, cam_frame, depth_topic, _he_mode = track_camera_frames(data) obj = str(yolo.get("object_frame") or "").strip() if not obj or obj == "tracked_object": # 跟踪目标的坐标系按**臂号**派生（SN 末两位），两侧互不干扰。 # 以前是从 SN 里找 "063989"/"063991" 子串，换个站点就失去意义了。 short = arm_short_id(str((_robot or {}).get("sn") or "")) obj = f"tracked_object_{short}" if short else "tracked_object" - return { + out = { "weights": str(paths.get("yolo_weights") or ""), "conf": float(yolo.get("conf") or 0.10), "iou": float(yolo.get("iou") or 0.45), @@ -528,10 +823,36 @@ def perception_overrides(data: dict) -> dict: "object_frame": obj, "depth_topic": depth_topic, "camera_source": cam_ns, + "base_frame": str((_robot or {}).get("base_frame") or ""), "feature_mode": str(yolo.get("feature_mode") or "plane"), + # feature_mode=keypoint 时用：哪个关键点当跟踪点（k0）、可信度门槛。 + "keypoint_index": int(yolo.get("keypoint_index") if yolo.get("keypoint_index") is not None else 0), + "keypoint_min_conf": float(yolo.get("keypoint_min_conf") or 0.30), + "keypoint_win_px": int(yolo.get("keypoint_win_px") if yolo.get("keypoint_win_px") is not None else 6), + # 三条关键点量纲守卫（2026-10-08 真机一阶段被假 k0 带走，见 + # feature_guard.py 的注释）。**必须在这里透传**：节点里声明的默认值 + # 恰好等于 site.yaml 当前值，漏了不会报错，只会让以后改配置毫无反应 + # （R1.1 的"声明了却没接线"）。 + "keypoint_border_px": float(yolo.get("keypoint_border_px") if yolo.get("keypoint_border_px") is not None else 6.0), + "track_max_step_m": float(yolo.get("track_max_step_m") if yolo.get("track_max_step_m") is not None else 0.04), + "reacquire_far_px": float(yolo.get("reacquire_far_px") if yolo.get("reacquire_far_px") is not None else 120.0), + "reacquire_confirm_frames": int(yolo.get("reacquire_confirm_frames") if yolo.get("reacquire_confirm_frames") is not None else 3), "plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008), "plane_band_m": float(yolo.get("plane_band_m") or 0.06), } + sio = _load_site_io() + role = sio.track_camera_role(data) + perc = data.get("perception") or {} + if role == "overhead": + _apply_overhead_range(data, out) + out["overhead_oneshot_enable"] = False + elif perc.get("track_camera") or "wrist_handoff_conf" in perc: + # 显式 wrist / 写了混合键 → 必须接全套。缺键就红，不许用代码默认。 + # 没写 perception 段的探针测试保持旧行为（不加一次性）。 + _apply_wrist_hybrid(data, _robot, out) + else: + out["overhead_oneshot_enable"] = False + return out def servo_overrides(data: dict) -> dict: @@ -540,12 +861,20 @@ def servo_overrides(data: dict) -> dict: paths = data.get("paths") or {} yolo = data.get("yolo") or {} servo = data.get("servo") or {} - _ns, cam_frame, _depth = camera_frames(data) + _ns, cam_frame, _depth, he_mode = track_camera_frames(data) + he_file = _track_handeye_file(data, robot, he_mode) + xy_track = str(servo.get("xy_track") or "hover").strip().lower() + if he_mode == "eye_to_hand" and xy_track == "lookat": + raise RuntimeError( + "perception.track_camera=overhead 时相机固定，lookat（把目标对准画面中心）" + "没有意义。site.yaml 的 servo.xy_track 必须是 hover。" + ) out = { "joint_prefix": str(robot.get("joint_prefix") or ""), "base_frame": str(robot.get("base_frame") or ""), "ee_frame": str(robot.get("ee_frame") or ""), - "handeye_file": str(paths.get("handeye_file") or ""), + "handeye_file": he_file, + "handeye_mode": he_mode, "camera_frame": cam_frame, "handeye_child_frame": cam_frame, "arm_label": str(robot.get("label") or robot.get("arm") or ""), @@ -555,14 +884,19 @@ def servo_overrides(data: dict) -> dict: keys = [ "control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec", "max_extrapolate_sec", "predict_enable", "predict_sec", - "predict_v_max", "predict_jump_m", + "predict_v_max", "predict_jump_m", "jump_confirm_frames", "feat_ws_margin_m", "kp_xy", "ki_xy", "kd_xy", "kp_z", "ki_z", "kd_z", "i_sep_pos_m", "deadzone_m", "deadzone_z_m", "v_max_xy", "v_max_z", "v_approach_z", + # 远处限速（见 _slow_xy_limit）：漏了这两个键，site.yaml 改了也不生效， + # 节点会静默退回声明默认值（R1.1）。 + "slow_far_m", "v_slow_xy", "z_min_m", "approach_xy_gate_m", "min_ee_z_m", "e_xy_clip_m", "ee_x_m", "ee_y_m", "ee_z_m", "ee_hold_xy", "ee_vertical_down", "xy_track", "hover_offset_ee_xy_m", "arm_label", "ee_hold_kp", "ee_w_max", "lock_hold_sec", + "lock_still_sec", + "lock_still_move_m", "a_max", "max_qdot", "damping", "vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec", "min_cmd_delta_rad", "point_topic", "traj_topic", @@ -572,6 +906,38 @@ def servo_overrides(data: dict) -> dict: for k in keys: if k in servo: out[k] = servo[k] + sio = _load_site_io() + perc = data.get("perception") or {} + if sio.track_camera_role(data) == "wrist" and ( + perc.get("track_camera") or "wrist_handoff_conf" in perc + ): + if "wrist_handoff_conf" not in perc: + raise RuntimeError( + "servo 混合跟踪必须读 perception.wrist_handoff_conf " + "（与感知同一门槛，不许两边各写一个数）。" + ) + out["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"]) + oh = perc.get("overhead") if isinstance(perc.get("overhead"), dict) else {} + if "oneshot_hold_s" not in oh: + raise RuntimeError( + "servo 混合跟踪必须写 perception.overhead.oneshot_hold_s " + "（D435 一次性设定保持多久）。" + ) + out["overhead_hold_s"] = float(oh["oneshot_hold_s"]) + if "lock_still_sec" not in servo: + raise RuntimeError( + "servo.lock_still_sec 必须显式写" + "（D405 跟住后法兰静止多久才进二阶段）。不许用代码默认 3。" + ) + if "lock_still_move_m" not in servo: + raise RuntimeError( + "servo.lock_still_move_m 必须显式写" + "（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。" + ) + out["lock_still_sec"] = float(servo["lock_still_sec"]) + out["lock_still_move_m"] = float(servo["lock_still_move_m"]) + out["overhead_k0_topic"] = "visual_servo/overhead_k0" + out["wrist_conf_topic"] = "visual_servo/wrist_conf" return out @@ -620,10 +986,21 @@ def main() -> int: print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}") print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}") print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}") + print(f" 眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}") + print(f" 眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}") + for _m in ("eye_in_hand", "eye_to_hand"): + _t = _m.upper() + _src = flat.get(f"SITE_HANDEYE_BOARD_SRC_{_t}") + print( + f" 标定板[{_m:12s}]: " + f"{flat.get(f'SITE_HANDEYE_BOARD_COLS_{_t}')}x" + f"{flat.get(f'SITE_HANDEYE_BOARD_ROWS_{_t}')} 内角点, " + f"square={flat.get(f'SITE_HANDEYE_SQUARE_{_t}')} m" + + (" ← 没配过（标定会被拦下）" if _src == "default" else "") + ) print( - " 标定板 : " - f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} " - f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m" + f" 手外相机 : {flat.get('SITE_OVERHEAD_CAM_NAME') or '（未登记）'} " + f"serial={flat.get('SITE_OVERHEAD_CAM_SERIAL') or '（空）'}" ) print( f" 相机 : serial={flat['SITE_CAM_SERIAL'] or '（空）'} " diff --git a/scripts/run_all.sh b/scripts/run_all.sh index 67e8d7d..d201ba9 100755 --- a/scripts/run_all.sh +++ b/scripts/run_all.sh @@ -20,6 +20,7 @@ # bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗 # bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了 # bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务 +# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 只对准不轻降（验证精对准） # # 前置（另开一个终端，先跑起来）： # bash ~/ros2/scripts/run_real.sh --arm 89 @@ -41,6 +42,7 @@ RESNAP=false WITH_CAMERA=true WITH_TACTILE=true CAMERA_PREVIEW=false +NO_DESCENT=false # --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置 # shellcheck disable=SC1091 @@ -57,6 +59,9 @@ while [[ $# -gt 0 ]]; do --depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;; --pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;; --resnap) RESNAP=true; shift ;; + # 只验证**精对准**、不轻降。对准逻辑改完先这么跑，确认规真被对到孔心再放开下降 + # （对不准就下压会把规顶在孔沿上：2026-10-08 真机顶到 63N）。 + --no-descent) NO_DESCENT=true; shift ;; --no-preview) PIPELINE_PREVIEW=false; shift ;; --camera-preview) CAMERA_PREVIEW=true; shift ;; --no-camera) WITH_CAMERA=false; shift ;; @@ -138,6 +143,33 @@ if [[ "${WITH_CAMERA}" == true ]]; then fi echo " 相机 ${CAM_NS} 出图 OK" fi + # 一阶段混合跟踪：D405 看不见时要马上拍 D435。顶视没起来就只能干等。 + D435_TOPIC="/d435/color/image_raw" + _d435_ok() { + timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1 + timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:" + } + if _d435_ok; then + echo "=== 顶视 ${D435_TOPIC} 已在发图，跳过拉起 ===" + else + echo "=== 拉起顶视 D435（一阶段一次性定位）===" + D435_ARGS=() + [[ "${CAMERA_PREVIEW}" == true ]] && D435_ARGS+=(--preview) || D435_ARGS+=(--no-preview) + nohup bash "${ROOT}/scripts/run_camera_d435.sh" \ + "${D435_ARGS[@]}" > /tmp/run_all_d435.log 2>&1 & + echo " d435 pid=$! log=/tmp/run_all_d435.log" + d435_ok=false + for _i in $(seq 1 24); do + if _d435_ok; then d435_ok=true; break; fi + sleep 0.5 + done + if [[ "${d435_ok}" != true ]]; then + echo "ERROR: ${D435_TOPIC} 仍无图像。看日志：/tmp/run_all_d435.log" >&2 + echo " 混合跟踪没有 D435 就切不出去。USB 独占或没插好都会这样。" >&2 + exit 1 + fi + echo " 顶视 D435 出图 OK" + fi else echo "=== --no-camera：跳过相机检查 ===" fi @@ -206,10 +238,11 @@ ARGS=( "skip_stage3:=${SKIP_STAGE3}" "preview:=${PIPELINE_PREVIEW}" "resnap:=${RESNAP}" + "no_descent:=${NO_DESCENT}" ) [[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}") [[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}") [[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}") -echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} ===" +echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ===" exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}" diff --git a/scripts/run_camera_d405.sh b/scripts/run_camera_d405.sh index 6f6fd90..4821e9a 100755 --- a/scripts/run_camera_d405.sh +++ b/scripts/run_camera_d405.sh @@ -34,7 +34,11 @@ source "${ROOT}/scripts/env.sh" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" -pkill -f 'lib/arm_application/realsense_color_pub' 2>/dev/null || true +CAM_NAME="${SITE_CAM_NAME:-d405}" +# 只杀本臂这台腕部相机。以前 pkill 全部 realsense_color_pub / realsense_view， +# 会把已经开着的顶视 D435 一起干掉（或反过来：后开 D435 把 D405 预览窗杀掉）， +# 现场看起来就像"只剩眼外画面、绿框也不见了"。 +pkill -f "camera_name:=${CAM_NAME}( |$)" 2>/dev/null || true sleep 2 USB_INFO="$(python3 - </dev/null || true @@ -88,7 +92,6 @@ else echo "=== camera 1280x720@15 ===" fi -CAM_NAME="${SITE_CAM_NAME:-d405}" CAM_ARGS=( --ros-args -p camera_name:="${CAM_NAME}" @@ -140,12 +143,13 @@ if [[ "${DO_PREVIEW}" == "1" ]]; then echo "=== close window = hide preview only; camera keeps publishing ===" fi -echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ===" +echo "=== 本脚本只出图，不跑 YOLO。绿框来自 perception_node，请另开终端： ===" +echo "=== bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} ===" if [[ "${DO_PREVIEW}" == "1" ]]; then # 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制 # 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。 - pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true + # 前面已经按 camera_name 杀过本臂进程，这里不要再杀全部 realsense_view。 ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false & CAM_PID=$! trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM diff --git a/scripts/run_camera_d435.sh b/scripts/run_camera_d435.sh index 7380ecd..a12b123 100755 --- a/scripts/run_camera_d435.sh +++ b/scripts/run_camera_d435.sh @@ -1,5 +1,6 @@ #!/usr/bin/env bash -# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。不会打开任何一个 D405。 +# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。 +# 可以和 run_camera_d405.sh 同时开：只杀 camera_name:=d435，不动腕部那台。 # # bash ~/ros2/scripts/run_camera_d435.sh --preview # @@ -24,7 +25,9 @@ if [[ -z "${SN}" ]]; then exit 2 fi echo "=== D435 overhead serial=${SN} ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ===" -pkill -f 'camera_name:=d435' 2>/dev/null || true +# 只杀顶视这一台。不要 pkill 全部 realsense_view —— 那会把正在看的 D405 预览窗干掉， +# 绿框叠在 D405 窗口上，窗没了就像"伺服没画面、也没工件框"。 +pkill -f "camera_name:=d435( |$)" 2>/dev/null || true CAM_ARGS=( --ros-args -p camera_name:=d435 @@ -40,11 +43,13 @@ CAM_ARGS=( -p emitter_enabled:=true ) if [[ "${PREVIEW}" == "1" ]]; then - pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false & CAM_PID=$! trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM - ros2 run arm_application realsense_view --ros-args -p camera_name:=d435 + # 工件框是 D405 像素，叠到 1280×720 的顶视图上位置全错，看起来像没检出。 + ros2 run arm_application realsense_view --ros-args \ + -p camera_name:=d435 \ + -p overlay_detection:=false else exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" fi diff --git a/scripts/run_handeye.sh b/scripts/run_handeye.sh index 097765d..3f7860b 100755 --- a/scripts/run_handeye.sh +++ b/scripts/run_handeye.sh @@ -68,25 +68,21 @@ if [[ ! -d "${APP_CFG}" ]]; then fi # 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。 -# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号** -# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算）。 +# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号与相机名** +# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算；相机名来自 cameras 注册表）。 # -# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id` 和 `HANDEYE_MODE` +# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id`、`HANDEYE_MODE` 与相机名 # 在下面几行之后才算得出来，在这里拼会拼出 `results/_.yaml` 这种空文件名， # 而症状是"标定写完了、用的时候读不到"。 HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}" -WP="${SITE_HANDEYE_WAYPOINTS:?site.yaml 没有给出该臂的手眼示教点文件}" CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}" TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}" -RES="${SITE_HANDEYE_FILE:?site.yaml 没有给出该臂的手眼结果文件}" WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}" -TAUGHT="${WORKDIR}/taught_poses.txt" PREFIX="${SITE_JOINT_PREFIX:?SITE_JOINT_PREFIX 为空 —— site.yaml 里需要该臂的 robot.sn}" -CAM="${SITE_CAM_NAME:-d405}" # ---- --mode：切换 eye-in-hand / eye-to-hand ---- -# 必须在 WP/RES 定下来之前处理，因为它会改变读写的文件名。 -# 其余参数原样留给子命令（teach 自己有 --rdk/--ros 等）。 +# 必须在 WP/RES/板/相机定下来之前处理：模式决定了读哪个结果文件、用哪块板、 +# 开哪台相机、采点落到哪个文件。其余参数原样留给子命令（teach 自己有 --rdk 等）。 HANDEYE_MODE="${SITE_HANDEYE_MODE:-eye_in_hand}" _rest=() while [[ $# -gt 0 ]]; do @@ -109,15 +105,61 @@ set -- "${_rest[@]}" # 臂号取 Flexiv 序列号后两位：Rizon4-063989 → 89 _arm_id="${SITE_ROBOT_SN: -2}" -if [[ -n "${_arm_id}" && "${_arm_id}" != "${SITE_ROBOT_SN}" ]]; then - if [[ "${HANDEYE_MODE}" != "${SITE_HANDEYE_MODE:-eye_in_hand}" ]]; then - # 显式指定了非默认模式 → 按 <臂号>_<模式>.yaml 重新定位 - WP="${HE_DIR}/poses/${_arm_id}_${HANDEYE_MODE}.yaml" - RES="${HE_DIR}/results/${_arm_id}_${HANDEYE_MODE}.yaml" - echo "[handeye] --mode ${HANDEYE_MODE} → poses: ${WP}" - echo "[handeye] result: ${RES}" + +# 取"当前模式"的那一份值。名字由 export_site_env 按模式导出成 +# SITE_HANDEYE_<字段>_EYE_IN_HAND / _EYE_TO_HAND +# 这样"模式 → 结果文件 / 示教点 / 标定板"的对应关系只有 site.yaml 一处定义， +# 脚本不再自己拼文件名（拼错/漏拼都是**静默**用错文件，最难查）。 +_HE_TAG="$(echo "${HANDEYE_MODE}" | tr '[:lower:]' '[:upper:]')" +_he() { + local name="SITE_HANDEYE_$1_${_HE_TAG}" + printf '%s' "${!name-}" +} + +WP="$(_he WAYPOINTS)" +WP="${WP:-${HE_DIR}/poses/${_arm_id:-?}_${HANDEYE_MODE}.yaml}" +RES="$(_he FILE)" +RES="${RES:-${HE_DIR}/results/${_arm_id:-?}_${HANDEYE_MODE}.yaml}" +# 示教点临时文件也按模式分开：两个模式共用一份时，后一个 teach 会清掉前一个的采点， +# 而症状只是"merge 出来的点位不对"，很难联想到是文件串了。 +TAUGHT="${WORKDIR}/taught_poses_${HANDEYE_MODE}.txt" + +# ---- 相机：两种模式用的是**两台不同的相机** ---- +# 眼在手上 = 腕部相机（跟着臂动）；眼在手外 = 固定在工位外的相机（不随臂动）。 +# 原来这里恒用 SITE_CAM_NAME（腕部），于是 `--mode eye_to_hand` 会去开腕部相机 —— +# 板跟着臂动、相机也跟着臂动，"标定"能跑完，但解出的是个毫无意义的错外参。 +if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then + CAM="${SITE_OVERHEAD_CAM_NAME:-}" + if [[ -z "${CAM}" ]]; then + echo "ERROR: eye_to_hand 需要一台固定相机，但 site.yaml 的 cameras 段里没有登记的。" >&2 + echo " 加一条 role: overhead_eye_to_hand（眼在手外、不随臂动）的相机即可。" >&2 + exit 2 fi + CAM_SERIAL="${SITE_OVERHEAD_CAM_SERIAL:-}" + CAM_WIDTH="${SITE_OVERHEAD_CAM_WIDTH:-}" + CAM_HEIGHT="${SITE_OVERHEAD_CAM_HEIGHT:-}" + CAM_FPS="${SITE_OVERHEAD_CAM_FPS:-30}" + CAM_AE="${SITE_OVERHEAD_CAM_AUTO_EXPOSURE:-1}" + CAM_AE_LIMIT="${SITE_OVERHEAD_CAM_AE_LIMIT_US:-8000}" + CAM_EMITTER="${SITE_OVERHEAD_CAM_EMITTER:-1}" + CAM_EXPOSURE="${SITE_OVERHEAD_CAM_EXPOSURE_US:-0}" + CAM_GAIN="${SITE_OVERHEAD_CAM_GAIN:-0}" +else + CAM="${SITE_CAM_NAME:-d405}" + CAM_SERIAL="${SITE_CAM_SERIAL:-}" + CAM_WIDTH="${SITE_CAM_WIDTH:-}" + CAM_HEIGHT="${SITE_CAM_HEIGHT:-}" + CAM_FPS="${SITE_CAM_FPS:-30}" + CAM_AE="${SITE_CAM_AUTO_EXPOSURE:-1}" + CAM_AE_LIMIT="${SITE_CAM_AE_LIMIT_US:-1800}" + CAM_EMITTER="${SITE_CAM_EMITTER:-0}" + CAM_EXPOSURE="${SITE_CAM_EXPOSURE_US:-0}" + CAM_GAIN="${SITE_CAM_GAIN:-0}" fi +echo "[handeye] mode=${HANDEYE_MODE} poses=${WP}" +echo "[handeye] result=${RES}" +echo "[handeye] camera=${CAM} serial=${CAM_SERIAL:-（空）}" + _kill_servo() { pkill -f 'visual_servo.launch' 2>/dev/null || true @@ -159,7 +201,9 @@ cmd_show() { echo " tour : ${TOUR}" echo " result : ${RES}" echo " workdir : ${WORKDIR}" - echo " board : ${SITE_HANDEYE_BOARD_COLS:-?}x${SITE_HANDEYE_BOARD_ROWS:-?} inner, square=${SITE_HANDEYE_SQUARE_M:-?} m" + echo " camera : ${CAM} serial=${CAM_SERIAL:-（空）}" + echo " board : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m (来源=$(_he BOARD_SRC))" + echo " （另一种模式的板见下面 export 的 标定板[...] 两行）" python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true if [[ -f "${WP}" ]]; then python3 - <&2 + echo " 在 config/site.yaml 的 handeye.boards.${HANDEYE_MODE} 里写：" >&2 + echo " cols / rows / square_size_m" >&2 + echo " （内角点 = 方格数 − 1；测一下实际方格边长，别照抄另一块板。）" >&2 + exit 2 + fi _need_files "${CAL}" "${WP}" _need_robot _need_camera echo "=== kill visual_servo (avoid traj fight) ===" _kill_servo mkdir -p "${WORKDIR}" - local cols="${SITE_HANDEYE_BOARD_COLS:-8}" - local rows="${SITE_HANDEYE_BOARD_ROWS:-11}" - local sq="${SITE_HANDEYE_SQUARE_M:-0.015}" local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}" local mt="${SITE_HANDEYE_MOVE_T:-8.0}" local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}" @@ -200,7 +255,8 @@ cmd_calibrate() { local ps="${SITE_HANDEYE_PIN_SEC:-0.45}" local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}" echo "=== calibrate mode=${HANDEYE_MODE} ===" - echo " board ${cols}x${rows} inner, square=${sq} m (from site.yaml)" + echo " board ${cols}x${rows} inner, square=${sq} m (site.yaml handeye.boards.${HANDEYE_MODE}，来源=${src})" + echo " camera ${CAM} serial=${CAM_SERIAL:-（空）}" echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s" echo " poses=${WP}" echo " result → ${RES}" @@ -209,6 +265,7 @@ cmd_calibrate() { --params-file "${WP}" \ -p joint_prefix:="${PREFIX}" \ -p camera_source:="${CAM}" \ + ${CAM_SERIAL:+-p camera_serial:="${CAM_SERIAL}"} \ -p output_dir:="${WORKDIR}" \ -p result_file:="${RES}" \ -p mode:="${HANDEYE_MODE}" \ @@ -304,18 +361,18 @@ cmd_teach() { -p output_dir:="${WORKDIR}" -p target_points:="${points}" ) - if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then - TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}") + if [[ -n "${CAM_SERIAL}" ]]; then + TEACH_ARGS+=(-p camera_serial:="${CAM_SERIAL}") fi - if [[ -n "${SITE_CAM_WIDTH:-}" ]]; then - TEACH_ARGS+=(-p width:="${SITE_CAM_WIDTH}" -p height:="${SITE_CAM_HEIGHT:-480}" -p fps:="${SITE_CAM_FPS:-30}") + if [[ -n "${CAM_WIDTH}" ]]; then + TEACH_ARGS+=(-p width:="${CAM_WIDTH}" -p height:="${CAM_HEIGHT:-480}" -p fps:="${CAM_FPS:-30}") fi TEACH_ARGS+=( - -p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)" - -p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-1800}" - -p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)" - -p exposure_us:="${SITE_CAM_EXPOSURE_US:-0}" - -p gain:="${SITE_CAM_GAIN:-0}" + -p auto_exposure:="$([ "${CAM_AE:-1}" = "1" ] && echo true || echo false)" + -p ae_limit_us:="${CAM_AE_LIMIT:-1800}" + -p emitter_enabled:="$([ "${CAM_EMITTER:-0}" = "1" ] && echo true || echo false)" + -p exposure_us:="${CAM_EXPOSURE:-0}" + -p gain:="${CAM_GAIN:-0}" ) ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}" || true @@ -362,8 +419,14 @@ cmd_teach() { # 但 teach 阶段把相机杀掉了（USB 独占），标定拍照要用，得先拉回来。 echo echo "=== 重启相机（teach 阶段杀掉了它，标定拍照需要）===" - nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \ - --no-preview > /tmp/run_handeye_camera.log 2>&1 & + if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then + # 眼在手外用的是固定相机（D435），不是腕部 D405 —— 拉错相机的话， + # 标定会去订一个没人发布的图，表现是"相机没数据"。 + _cam_cmd=(bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview) + else + _cam_cmd=(bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" --no-preview) + fi + nohup "${_cam_cmd[@]}" > /tmp/run_handeye_camera.log 2>&1 & echo " pid=$! log=/tmp/run_handeye_camera.log" # _need_camera 内部失败时会自己打印原因并 exit，不需要在这里再判一次。 _need_camera diff --git a/scripts/run_visual_servo_rt.sh b/scripts/run_visual_servo_rt.sh index af4710d..1f00af1 100755 --- a/scripts/run_visual_servo_rt.sh +++ b/scripts/run_visual_servo_rt.sh @@ -8,6 +8,11 @@ # --control 从当前位姿直接开始跟踪（不抬升，也不回 home）。 # 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。 # +# 跟踪相机由 site.yaml 的 perception.track_camera 决定： +# wrist 腕部 D405 主跟踪；画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0 +# —— 需要同时开 run_camera_d405.sh 与 run_camera_d435.sh +# overhead 整段只用顶置 D435（实验）—— 另开 run_camera_d435.sh +# # 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights set -eo pipefail @@ -71,13 +76,37 @@ else echo "=== launch perception + servo (observe only, no arm motion) ===" fi EE="${SITE_EE_FRAME:?SITE_EE_FRAME 为空 —— site.yaml 的 robot.robots[] 里需要该臂的 SN}" -HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}" -echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}" -echo " weights=${SITE_YOLO_WEIGHTS} camera=${SITE_CAM_NAME:-} control=${CONTROL}" +TRACK="${SITE_TRACK_CAMERA:-wrist}" +HANDEYE="${SITE_TRACK_HANDEYE_FILE:-${SITE_HANDEYE_FILE:-}}" +CAM_FRAME="${SITE_TRACK_CAMERA_FRAME:-${SITE_CAMERA_FRAME:-}}" +CAM_NAME="${SITE_TRACK_CAM_NAME:-${SITE_CAM_NAME:-}}" +if [[ -z "${HANDEYE}" ]]; then + echo "ERROR: 跟踪用手眼文件为空（SITE_TRACK_HANDEYE_FILE / SITE_HANDEYE_FILE）" >&2 + exit 2 +fi +if [[ ! -f "${HANDEYE}" ]]; then + echo "ERROR: 跟踪用手眼文件不存在：${HANDEYE}" >&2 + if [[ "${TRACK}" == "overhead" ]]; then + echo " 先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate --mode eye_to_hand" >&2 + else + echo " 先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate" >&2 + fi + exit 2 +fi +echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} track=${TRACK} camera=${CAM_NAME}" +echo " handeye=${HANDEYE} frame=${CAM_FRAME} control=${CONTROL}" +if [[ "${TRACK}" == "overhead" ]]; then + echo "=== 整段顶视：D435 追踪，D405 不参与一阶段 ===" + echo " 相机请开：bash ~/ros2/scripts/run_camera_d435.sh --preview" +else + echo "=== 本脚本不弹预览窗。工件框画在 D405 那个查看器上 ===" + echo " bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview" + echo " bash ~/ros2/scripts/run_camera_d435.sh --preview # 顶视可开，绿框仍在 D405 窗" +fi LAUNCH_ARGS=( enable_control:="${CONTROL}" handeye_file:="${HANDEYE}" ee_frame:="${EE}" - camera_frame:="${SITE_CAMERA_FRAME:-d405_color_optical_frame}" + camera_frame:="${CAM_FRAME}" ) exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}" diff --git a/scripts/sense_tool_length.py b/scripts/sense_tool_length.py new file mode 100644 index 0000000..ac45cfd --- /dev/null +++ b/scripts/sense_tool_length.py @@ -0,0 +1,309 @@ +#!/usr/bin/env python3 +"""真机标定 `tool_tip_below_flange_m`：**规尖**在法兰下方多远 —— 视觉感知。 + +为什么需要单独标定 +------------------ +规是**夹在爪里**的，露出多少随装夹变化（仿真上料脚本实测见过 243 / 254mm 两种）。 +而真机上**没有"规尖"的 TF**：规只是被夹住的一个零件，TF 树里查不到它。所以只能 +**感知**：腕部相机看到规，用 `T_base = T_base_flange · T_flange_cam` 把手眼链 +反投影回基座，量「法兰 z − 规尖 z」。 + +这个值一旦偏小，下游几何就会把规按进工件（site.yaml 记着 2026-09-29 的事故： +悬停这一步就把规插进工件，S05 横向对准变成拿规尖拨工件，工件被推走 19mm）。 + +口径（必须显式） +---------------- +* 量的是**竖直**距离（基座 z 之差），不是相机光轴方向的深度 —— 两者因相机 + 29.7° 的倾角不同，混用会差 20mm 量级。 +* 只在法兰近似**竖直朝下**时成立（悬停/跟踪都是该姿态）。 + +自证（不许"看起来差不多"） +-------------------------- +先把**当场**反投影出来的**工件上表面** z 与 site.yaml 记录的 +`workpiece_surface_z_m` 比：差 > `--tol-mm`（默认 5mm）就判链路/手眼/深度有 +系统性问题并**拒绝输出**。这一步用的是独立已知量，能真的失败。 + +用法 +---- + bash ~/ros2/scripts/run_real.sh --arm 89 # 另开终端：起控制栈 + bash ~/ros2/scripts/run_camera_d405.sh --arm 89 # 起相机 + # 把臂开到**悬停高**（stage2 的 hole_hover_z_m，竖直朝下、规在视野里） + python3 ~/ros2/scripts/sense_tool_length.py --sn Rizon4-063989 + +输出一行 `**tool_tip_below_flange_m: 0.xxx**` 与它是否通过自证。 +""" + +from __future__ import annotations + +import argparse +import sys +import time +from pathlib import Path + +import numpy as np + +ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT / "scripts" / "lib")) + + +def _load_frame(sn: str, handeye_file: Path): + """读手眼 T_flange_cam（eye-in-hand）。缺文件直接失败。""" + import yaml + + doc = yaml.safe_load(handeye_file.read_text(encoding="utf-8")) or {} + if "T_4x4" not in doc: + raise SystemExit(f"手眼文件 {handeye_file} 里没有 T_4x4（键：{sorted(doc)}）") + return np.array(doc["T_4x4"], dtype=float) + + +class _Grabber: + """抓彩色/对齐深度/内参 + 查 TF。放在类里是为了让订阅和 TF 监听一起 spin。""" + + def __init__(self, cam_ns: str): + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile, ReliabilityPolicy + from sensor_msgs.msg import CameraInfo, Image + import tf2_ros + + self.rclpy = rclpy + self.node = Node("sense_tool_length") + self.color = self.depth = self.info = None + qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.BEST_EFFORT) + ns = cam_ns.strip("/") + self.node.create_subscription(Image, f"/{ns}/color/image_raw", + lambda m: setattr(self, "color", m), qos) + self.node.create_subscription(Image, f"/{ns}/aligned_depth_to_color/image_raw", + lambda m: setattr(self, "depth", m), qos) + self.node.create_subscription(CameraInfo, f"/{ns}/color/camera_info", + lambda m: setattr(self, "info", m), qos) + self.buf = tf2_ros.Buffer() + self.lis = tf2_ros.TransformListener(self.buf, self.node) + + def spin(self, dt: float = 0.05) -> None: + self.rclpy.spin_once(self.node, timeout_sec=dt) + + def wait_frames(self, timeout_s: float = 8.0) -> bool: + t0 = time.monotonic() + while time.monotonic() - t0 < timeout_s: + self.spin() + if self.color is not None and self.depth is not None and self.info is not None: + return True + return False + + def need_fresh(self, timeout_s: float = 4.0) -> bool: + t0 = time.monotonic() + while time.monotonic() - t0 < timeout_s: + self.spin() + if self.color is None or self.depth is None: + continue + now = self.node.get_clock().now().nanoseconds * 1e-9 + + def age(m): + return now - m.header.stamp.sec - m.header.stamp.nanosec * 1e-9 + + if age(self.color) < 1.0 and age(self.depth) < 1.0: + return True + return False + + def T(self, parent: str, child: str, timeout_s: float = 3.0): + t0 = time.monotonic() + while time.monotonic() - t0 < timeout_s: + self.spin() + try: + tr = self.buf.lookup_transform(parent, child, self.rclpy.time.Time()) + except Exception: + continue + t, r = tr.transform.translation, tr.transform.rotation + x, y, z, w = r.x, r.y, r.z, r.w + M = np.eye(4) + M[:3, :3] = [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ] + M[:3, 3] = [t.x, t.y, t.z] + return M + return None + + +def _depth_m(msg) -> np.ndarray: + if (msg.encoding or "").lower() != "16uc1": + raise SystemExit(f"深度编码应为 16UC1(mm)，实际 {msg.encoding!r}") + return (np.frombuffer(msg.data, dtype=np.uint16) + .reshape(int(msg.height), int(msg.width)).astype(np.float32) * 1e-3) + + +def _near_tip(depth: np.ndarray, near_m: float, min_m: float, max_m: float): + """近端连通域里"最深"的像素 = 规尖候选。 + + 为什么用连通域而不是全图最深处：工件面/台面比规更远，全图最深处会落到工件上； + 规是**唯一**贴着相机（< near_m）的物体，所以先在近端找块，再取块内最深点。 + """ + import cv2 + + valid = (depth > min_m) & (depth < max_m) + near = (valid & (depth < near_m)).astype(np.uint8) + if int(near.sum()) < 150: + return None + n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8) + if n <= 1: + return None + big = max(range(1, n), key=lambda i: stats[i, 4]) + ys, xs = np.where(lab == big) + dv = depth[ys, xs] + k = np.percentile(dv, 97) # 块内最深 3% ≈ 规尖 + sel = dv >= k - 1e-6 + ty = int(round(float(np.median(ys[sel])))) + tx = int(round(float(np.median(xs[sel])))) + return tx, ty + + +def main(argv=None) -> int: + ap = argparse.ArgumentParser(description="真机视觉标定 tool_tip_below_flange_m") + ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml robot.sn）") + ap.add_argument("--site", default=str(ROOT / "config" / "site.yaml")) + ap.add_argument("--frames", type=int, default=8, help="取多少帧求中值") + ap.add_argument("--near-m", type=float, default=0.15, help="近端阈值：规比这更近 [m]") + ap.add_argument("--min-m", type=float, default=0.04) + ap.add_argument("--max-m", type=float, default=0.60) + ap.add_argument("--tol-mm", type=float, default=5.0, + help="反投影出的工件面 z 与 site.yaml 记录值的最大允许差 [mm]") + ap.add_argument("--save", default="", help="把带标记的彩图写到这个路径（可选）") + args = ap.parse_args(argv) + + import export_site_env as ex + + data = ex.load_site(Path(args.site)) + sn = (args.sn or "").strip() or str((data.get("robot") or {}).get("sn") or "").strip() + if not sn: + raise SystemExit("没给 --sn，且 site.yaml 的 robot.sn 为空") + merged, robot = ex.apply_active_arm(data, sn) + cam_ns = ex.camera_ns(merged) + base = str(merged.get("servo", {}).get("base_frame") or f"{sn}_base_link") + flange = f"{sn}_flange" + he_file = Path(str((merged.get("paths") or {}).get("handeye_file") or "")) + if not he_file.is_file(): + raise SystemExit(f"手眼文件不存在：{he_file}") + gp = (merged.get("gauge_assembly") or {}) + surface_ref = float(gp.get("workpiece_surface_z_m") or 0.0) + cur = float(gp.get("tool_tip_below_flange_m") or 0.0) + + print(f"=== 真机工具长标定 SN={sn} cam=/{cam_ns.strip('/')} ===") + print(f" 基座帧={base} 法兰帧={flange}") + print(f" 手眼 ={he_file}") + print(f" site 现值 tool_tip_below_flange_m={cur:.4f} 工件面={surface_ref:.4f}") + + import rclpy + + rclpy.init() + try: + g = _Grabber(cam_ns) + except Exception as exc: # noqa: BLE001 + print(f"节点初始化失败：{exc}", file=sys.stderr) + return 2 + + try: + if not g.wait_frames(10.0): + print("错误：没收到相机帧。先起相机：bash scripts/run_camera_d405.sh --arm <臂>", + file=sys.stderr) + return 1 + T_flange_cam = _load_frame(sn, he_file) + + tips, flange_zs, surf = [], [], [] + for i in range(max(1, args.frames)): + if not g.need_fresh(): + continue + dep = _depth_m(g.depth) + k = g.info.k + fx, fy, cx, cy = float(k[0]), float(k[4]), float(k[2]), float(k[5]) + T_bf = g.T(base, flange) + if T_bf is None: + print("错误：TF 查不到 base→flange（控制栈起了吗？）", file=sys.stderr) + return 1 + T_bc = T_bf @ T_flange_cam + got = _near_tip(dep, args.near_m, args.min_m, args.max_m) + if got is None: + continue + ux, vy = got + # 尖端深度取 5x5 中值抑噪；再用整条链反投到基座 + y0, y1 = max(0, vy - 2), min(dep.shape[0], vy + 3) + x0, x1 = max(0, ux - 2), min(dep.shape[1], ux + 3) + patch = dep[y0:y1, x0:x1] + pv = patch[(patch > args.min_m) & (patch < args.max_m)] + if pv.size == 0: + continue + zt = float(np.median(pv)) + p_cam = np.array([(ux - cx) * zt / fx, (vy - cy) * zt / fy, zt, 1.0]) + tip_base = (T_bc @ p_cam)[:3] + tips.append(float(T_bf[2, 3]) - float(tip_base[2])) + flange_zs.append(float(T_bf[2, 3])) + # 工件面：取画面下部中央一块（工件/台面），用同一链反投影 + h, w = dep.shape + band = dep[int(h * 0.80):int(h * 0.95), int(w * 0.30):int(w * 0.70)] + bv = band[(band > args.min_m) & (band < args.max_m)] + if bv.size > 50: + zb = float(np.median(bv)) + bu, bvv = int(w * 0.50), int(h * 0.88) + pb = (T_bc @ np.array([(bu - cx) * zb / fx, (bvv - cy) * zb / fy, zb, 1.0]))[:3] + surf.append(float(pb[2])) + if args.save and args.frames == 1: + try: + import cv2 + bgr = np.frombuffer(g.color.data, np.uint8).reshape( + int(g.color.height), int(g.color.width), 3).copy() + cv2.circle(bgr, (ux, vy), 8, (0, 0, 255), 2) + cv2.imwrite(args.save, bgr) + except Exception: + pass + g.spin(0.05) + + if not tips: + print("错误：没找到规（近端 < %.0fmm 的物体）。确认规夹在爪里、且臂在竖直朝下姿态。" + % (args.near_m * 1000), file=sys.stderr) + return 1 + + tl = float(np.median(tips)) + spread = (max(tips) - min(tips)) * 1000.0 + fz = float(np.median(flange_zs)) + print(f"\n 帧数={len(tips)} 法兰 z={fz:.4f}") + print(f" 感知工具长 = **{tl:.4f} m** (逐帧散布 {spread:.1f}mm)") + + ok = True + tip_z = fz - tl + if surf: + zs = float(np.median(surf)) + # 画面下部那块可能是**工件面**（site 记录 surface_ref）也可能是**台面** + # （真机基座与台面同高 ⇒ ≈0）。两者都是**独立已知量**，用它们卡链路： + # 只要反投影出的 z 落在允许窗内，就说明手眼/深度/姿态没有系统性问题。 + lo = min(0.0, surface_ref) - 0.03 + hi = max(0.0, surface_ref) + 0.04 + tag = "OK" if lo <= zs <= hi else "FAIL" + print(f" 自证 反投影支撑面 z={zs:.4f}" + f"（site 工件面 {surface_ref:.4f}、台面≈0）允许 [{lo:+.3f},{hi:+.3f}] → [{tag}]") + if not (lo <= zs <= hi): + ok = False + if not (tip_z > zs + 0.003): + print(f" 自证：规尖 z={tip_z:.4f} 不在支撑面 z={zs:.4f} 之上 → FAIL") + ok = False + else: + print(" 自证：画面下部没采到有效深度，跳过（不敢保证链路）") + if not (0.10 <= tl <= 0.35): + print(f" 自证：工具长 {tl:.3f} 不在合理区间 [0.10, 0.35] → FAIL") + ok = False + + if not ok: + print("\n ✗ 自证未过 —— **不要**把这个值写进 site.yaml，先查手眼/深度/姿态。") + return 4 + print(f"\n ✓ 可写：gauge_assembly.tool_tip_below_flange_m: {tl:.3f}" + f"（现值 {cur:.4f}，差 {(tl - cur) * 1000:+.1f}mm）") + return 0 + finally: + try: + g.rclpy.shutdown() + except Exception: + pass + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/test_rdk_python.sh b/scripts/test_rdk_python.sh index ba74eb9..a41ae48 100755 --- a/scripts/test_rdk_python.sh +++ b/scripts/test_rdk_python.sh @@ -28,6 +28,19 @@ if iface: break except TypeError: continue + except RuntimeError as exc: + # 这个 iface 用不了（实测：enp4s0 上有 5 个 IPv4 时，RDK 的 + # UDPv4Transport 会把整张白名单都过滤掉，报 "All whitelist + # interfaces were filtered out"），别再拿它试了，交给下面的 + # 「不指定网卡」兜底。 + # + # **这一条以前漏了**：老代码只捕获 TypeError，而真实失败是 + # RuntimeError —— 于是异常直接抛出去，下面那行兜底永远走不到。 + # 后果是预检报「机械臂 RDK 连不上」这个**假阻断**， + # 而真机栈其实连得上（flexiv_hardware_interface.cpp 构造 + # flexiv::rdk::Robot 时只传 robot_sn、不传网卡）。 + print(f" iface={iface} 不可用（{str(exc)[:70]}），回退到不指定网卡") + break if robot is None: robot = flexivrdk.Robot(sn) print("Connected. Clearing fault if any...") diff --git a/src/arm_application/arm_application/feature_guard.py b/src/arm_application/arm_application/feature_guard.py new file mode 100644 index 0000000..e7cb34b --- /dev/null +++ b/src/arm_application/arm_application/feature_guard.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3 +"""跟踪点的三道守卫：**位置合理性** + **每帧位移上限** + **远框重捕获确认**。 + +为什么需要它们（2026-10-08 真机 arm 89，一阶段跑 150s 没锁上） +------------------------------------------------------------ +``/tmp/pipeline_stage1.log`` 原话。真 k0 一开始是对的:: + + 764.79 跟踪 keypoint conf=0.87 uv=(410,246) z=0.299 ← 真 k0（画面中偏右） + +然后在 1 秒内被一串**假 k0** 顶掉（全都是高置信度）:: + + 766.96 重新捕获 conf=0.58 dpx=480 + 766.96 跟踪 keypoint conf=0.58 uv=(76,75) z=0.392 ← 贴左上角 + 767.75 跟踪 keypoint conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near + 768.16 跟踪 keypoint conf=0.53 uv=(143,92) z=0.376 + 768.78 跟踪 keypoint conf=0.73 uv=(12,30) k0(ok=1551 near=620 借深度) z=0.276 + 769.66 重新捕获 conf=0.22 dpx=207 + 770.07 跟踪 keypoint conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度) z=0.271 + +伺服于是以 0.45m/s 追着它们跑：``ee_xy`` home ``(0.686,-0.113)`` → ``(0.354,-0.294)`` +→ ``(0.083,-0.515)``（工作盒角落），1.2s 后看门狗急停；工件已在半米外， +剩下 148s 全是"检测漏检"。三条结论： + + 1. **置信度挡不住**：假 k0 的 conf 是 0.53~0.73，真 k0 是 0.87 —— 没有可分区间。 + 但假 k0 有一个共同特征：**落在画面边上**（``uv=(12,30)`` / ``(76,75)`` / + ``(143,92)``），而孔心长在工件上、不可能贴在画面边缘； + 2. **借深度把"没有深度"伪装成"有深度"**：``k0(… 借深度)`` 的 z 是从工件框里 + 借的，看着很合理（0.27~0.39m），于是"物理量程"这道守卫也失效了。而 + **借来的深度意味着该像素处读不到自己的深度** ⇒ 那个点的绝对 XY 不可靠， + 不该用来**重定位**（换一个跟踪位置）； + 3. **位移不受限**：真 k0 到假 k0 一帧能差 480px / 400mm，伺服一次就全速扑过去。 + +三道守卫各挡一层，全都是"能真的失败"的判据（见 test_feature_guard_*.py 的负向用例）： + +``kpt_plausible`` + k0 必须落在自己的检测框内（含少量余量）、且不许贴画面边缘。违反 ⇒ 本帧的 + k0 **位置**不可信（不是"没有目标"），交回调用方走"按框上相对偏移重建"的 + 连续性路径（``perception_node._keypoint_feature`` 本来就有这条路）。 +``TrackSlewGuard`` + 已发布跟踪点的**每帧位移上限**。工件不动、相机 30Hz，真实观测一帧不可能走 + 4cm（``feature_jump_guard`` 实测真实运动 ≤15mm/帧）。超过上限：观测可信就 + **夹到上限**（限速跟随，不丢目标），观测来自借深度就**整帧不采信**。 +``FarReacquireConfirm`` + "远框重新捕获"（``dpx`` 大于门限）必须连续 ``confirm_frames`` 帧指向同一处 + 才采信 —— 一帧的远框更可能是模型在别的东西上误报，而不是工件真的被挪走了。 +""" + +from __future__ import annotations + +from typing import Optional, Sequence, Tuple + +import numpy as np + + +def _xy_dist(a: np.ndarray, b: np.ndarray) -> float: + """两个 XY 点的距离（米/像素都行，单位跟随入参）。单一实现，别各写一份。""" + return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1]))) + + +def kpt_plausible( + u: float, + v: float, + box: Optional[Sequence[float]], + w: int, + h: int, + *, + border_px: float = 6.0, + box_margin_frac: float = 0.10, +) -> Optional[str]: + """k0 的**像素位置**是否可信。返回 None = 可信；否则返回拒绝原因。 + + ``box`` 是同一帧的工件检测框 ``(x0,y0,x1,y1)``；``w,h`` 是彩色图宽高。 + """ + if not (np.isfinite(u) and np.isfinite(v)): + return "k0 像素非有限值" + if box is None: + # 没有框就没法判断 k0 是否落在工件上 —— 宁可当不可信（走连续性路径）。 + return "没有检测框，无法判断 k0 是否落在工件上" + b = np.asarray(box, dtype=float).reshape(-1) + if b.size < 4: + return "检测框字段不完整" + x0, y0, x1, y1 = (float(vv) for vv in b[:4]) + bw, bh = abs(x1 - x0), abs(y1 - y0) + if not (np.isfinite(bw) and np.isfinite(bh)) or bw <= 1e-6 or bh <= 1e-6: + return "检测框尺寸无效" + mx, my = box_margin_frac * bw, box_margin_frac * bh + if u < x0 - mx or u > x1 + mx or v < y0 - my or v > y1 + my: + return ( + f"k0 ({u:.0f},{v:.0f}) 落在检测框 " + f"[{x0:.0f},{y0:.0f},{x1:.0f},{y1:.0f}] 之外" + ) + if w > 0 and h > 0: + if u <= border_px or v <= border_px or u >= w - 1 - border_px or v >= h - 1 - border_px: + return ( + f"k0 ({u:.0f},{v:.0f}) 贴画面边缘（{w}x{h}，余量 {border_px:.0f}px）" + "—— 孔心长在工件上，不可能在画面边上" + ) + return None + + +class TrackSlewGuard: + """已发布跟踪点的每帧位移上限。 + + 单实例服务**一个**跟踪点；调用方负责在"没有目标/重置"时 :meth:`reset`。 + """ + + def __init__(self, max_step_m: float) -> None: + # 只有**一个**门限，故意的：观测可信就限速跟随，借深度就整帧不采信。 + # 不做成两个门限是因为"借深度"只影响"能不能重定位"，不影响"小步跟不跟" —— + # 两个门限会立刻变成"哪个该调、调到多少"的扯皮（且没有实测支撑）。 + self.max_step_m = float(max_step_m) + self._last: Optional[np.ndarray] = None + self.clamped_total = 0 + self.rejected_borrowed_total = 0 + + def reset(self) -> None: + self._last = None + + def update( + self, p_xy: Optional[Sequence[float]], *, borrowed: bool = False + ) -> Tuple[Optional[np.ndarray], Optional[str]]: + """喂一次观测，返回 ``(采信的点, 拒绝原因)``。 + + - 第一次观测（没有历史）：原样采信（冷启动没有更好的信息，见服务节点的 + 照常起步）。 + - 位移 ≤ 上限：原样采信。 + - 位移 > 上限且 ``borrowed``：整帧不采信（返回 ``None`` + 原因）—— + 借来的深度意味着这个像素读不到自己的深度，绝对位置不足以用来重定位。 + - 位移 > 上限且观测可信：**夹到上限**（限速跟随），不丢目标。 + """ + if p_xy is None: + return None, "本帧没有观测" + p = np.asarray(p_xy, dtype=float).reshape(2) + if not np.all(np.isfinite(p)): + return None, "跟踪点非有限值" + if self._last is None: + self._last = p.copy() + return p, None + d = _xy_dist(p, self._last) + if d <= self.max_step_m: + self._last = p.copy() + return p, None + if borrowed: + self.rejected_borrowed_total += 1 + return None, ( + f"借深度的观测要求位移 {d*1000:.1f}mm > 上限 " + f"{self.max_step_m*1000:.0f}mm —— 判误检，本帧不采信" + ) + step = self._last + (p - self._last) * (self.max_step_m / d) + self._last = step.copy() + self.clamped_total += 1 + return step, ( + f"位移 {d*1000:.1f}mm > 上限 {self.max_step_m*1000:.0f}mm —— " + "夹到上限限速跟随" + ) + + +class FarReacquireConfirm: + """远框"重新捕获"的多帧确认。 + + ``dpx`` 是候选框中心与上一帧框中心的像素距离。超过 ``far_px`` 的候选必须 + 连续 ``confirm_frames`` 帧落在同一处（相互间距 ≤ ``far_px``）才放行。 + """ + + def __init__(self, far_px: float, confirm_frames: int = 3) -> None: + self.far_px = float(far_px) + self.confirm_frames = max(1, int(confirm_frames)) + self._cand: Optional[np.ndarray] = None + self._count = 0 + + def reset(self) -> None: + self._cand = None + self._count = 0 + + def allow( + self, center_xy: Optional[Sequence[float]], dpx: float, *, near_px: float + ) -> Tuple[bool, Optional[str]]: + """返回 ``(是否放行, 拒绝原因)``。``near_px`` 是"不算远框"的门限。""" + if center_xy is None: + self.reset() + return True, None + c = np.asarray(center_xy, dtype=float).reshape(2) + if not np.all(np.isfinite(c)): + return False, "候选框中心非有限值" + if float(dpx) <= float(near_px): + # 近框：正常跟踪，不需要确认 + self.reset() + return True, None + if self._cand is None or _xy_dist(c, self._cand) > self.far_px: + self._cand = c.copy() + self._count = 1 + else: + self._count += 1 + if self._count >= self.confirm_frames: + self.reset() + return True, None + return False, ( + f"远框重捕获 dpx={float(dpx):.0f}px 需连续 {self.confirm_frames} 帧一致" + f"（当前 {self._count} 帧）—— 本帧不采信" + ) diff --git a/src/arm_application/arm_application/feature_jump_guard.py b/src/arm_application/arm_application/feature_jump_guard.py new file mode 100644 index 0000000..02f3fd4 --- /dev/null +++ b/src/arm_application/arm_application/feature_jump_guard.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +"""跟踪点"假跳变"的确认门限：单帧大跳不采信，连续 N 帧一致才认。 + +为什么需要它（2026-10-08 真机实测，一阶段跑 150s 没锁上） +-------------------------------------------------------- +arm 89 的日志原话： + + 0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定！误差只剩 3mm + 0659.598 k0 置信度 0.01 < 0.30，本帧不用它 + 0659.604 HANDEYE snap feat_base=(0.675,-0.099) ← 跟踪点瞬间横移 89mm + 0660.121 v=(-0.206,+0.400) |e_xy|=130mm ← 伺服以 0.45m/s 去追这个假点 + 0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，目标彻底丢失 + +原来的写法是"跳变超过 ``predict_jump_m`` 就 ``reset()``" —— 等于**单帧就采信**。 +可是相机 30Hz、工件不动，真实特征点一帧不可能跳 89mm（实测的真实运动 +在 15mm/帧 以内）。所以"大跳"几乎必然是检测/兜底产生的假点，不该一路追过去。 + +判据 +---- +- 位移 ≤ ``jump_m``：正常观测（``near``），照常滤波更新。 +- 位移 > ``jump_m``：进入"待确认"。此后**连续 ``confirm_frames`` 帧**都要落在 + 同一个新位置附近（相互间距 ≤ ``jump_m``），才判为真实重定位（``jump``）。 +- 只要中间有一帧不对上（乱跳），计数清零、重新开始 —— 乱跳永远等不到确认。 + +这也顺手挡住"感知层 k0 掉信后乱跳"那类现象：宁可原地不动，也不要追着假点跑。 +""" + +from __future__ import annotations + +from typing import Optional + +import numpy as np + +# ``servo_node._update_feat_track`` 的三种判定结果。 +# 为什么要显式区分"没有目标"：perception 会把误检点也发过来（2026-10-08 真机， +# 模型在运动模糊下把特征点丢到基座后方 -0.10,-0.65，工件其实在 0.5,-0.12）， +# 这种点在物理上不可能 —— 必须当"没看到目标"处理，而不是"钉在原地"。 +ACCEPT = "accept" # 采信这次观测 +HOLD = "hold" # 疑似假跳 —— 位置钉住不动 +REJECT = "reject" # 物理上不合理（工作盒外）—— 当"没有目标"处理 + + +class FeatureJumpGate: + """有状态的跳变确认门限。线程安全由调用方保证（servo 在锁内调用）。""" + + NEAR = "near" # 正常观测：照常滤波更新 + HOLD = "hold" # 疑似假跳：本次不采信，位置保持不动 + JUMP = "jump" # 已确认的真实重定位：允许 reset 到新位置 + + def __init__(self, jump_m: float, confirm_frames: int = 3) -> None: + self.jump_m = float(jump_m) + # 至少 1 帧才谈得上"确认"；<=1 就等于退回旧的"单帧采信"，但那是显式选择， + # 不是静默降级 —— 这里仍然保留参数以方便现场调节。 + self.confirm_frames = max(1, int(confirm_frames)) + self._cand: Optional[np.ndarray] = None + self._count = 0 + # 诊断计数：连续 hold 了多少帧（供日志/排查，见 servo_node 的告警）。 + self.hold_frames = 0 + self.hold_total = 0 + + def reset(self) -> None: + """清空待确认状态（正常观测、或已确认后都要清）。""" + self._cand = None + self._count = 0 + self.hold_frames = 0 + + @staticmethod + def _dist(a: np.ndarray, b: np.ndarray) -> float: + return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1]))) + + def update(self, p_xy: np.ndarray, ref_xy: np.ndarray) -> str: + """喂一次观测。 + + 参数 + ---- + p_xy : 本帧测到的 XY（base 系，米） + ref_xy : 当前滤波后的位置 XY（米）—— 判断"跳没跳"的基准 + + 返回 :data:`NEAR` / :data:`HOLD` / :data:`JUMP` 之一。 + """ + p = np.asarray(p_xy, dtype=float).reshape(2) + ref = np.asarray(ref_xy, dtype=float).reshape(2) + if self._dist(p, ref) <= self.jump_m: + self.reset() + return self.NEAR + + # 大跳：本次观测只算"候选"。候选之间互相对不上就从头再数。 + if self._cand is None or self._dist(p, self._cand) > self.jump_m: + self._cand = p.copy() + self._count = 1 + else: + self._count += 1 + if self._count >= self.confirm_frames: + self.reset() + return self.JUMP + self.hold_frames += 1 + self.hold_total += 1 + return self.HOLD diff --git a/src/arm_application/arm_application/gauge_assembly_node.py b/src/arm_application/arm_application/gauge_assembly_node.py index db9b6f7..5578c20 100644 --- a/src/arm_application/arm_application/gauge_assembly_node.py +++ b/src/arm_application/arm_application/gauge_assembly_node.py @@ -94,6 +94,7 @@ from arm_application.gauge_config import ( ) from arm_application.gauge_fsm import ( DESCEND_STALL_STEPS, + ENTRY_HIGH, ROT_AXIS, ROT_LIMIT, ROT_SHORT, @@ -107,6 +108,7 @@ from arm_application.gauge_fsm import ( descend_iter_budget, descend_needed_m, descend_progress, + hole_entry_verdict, descend_tol_m, is_joint_limit_error, pixel_stable, @@ -141,7 +143,13 @@ from arm_application.gauge_kin import ( ) from arm_application.pipeline_node import AssemblyPipeline from arm_application.handeye_realsense import resolve_realsense_topics -from arm_application.site_io import deep_merge, load_site_dict, ros2_root +from arm_application.site_io import ( + deep_merge, + load_site_dict, + overhead_camera, + resolve_eye_to_hand_file, + ros2_root, +) # 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2） from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point @@ -166,6 +174,8 @@ _LOCATE_CODE_HINT: Dict[int, str] = { 5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）", 6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件" " —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）", + 8: "（规尖偏移一直没量到：残差看着达标、但用的是**法兰轴线**不是规尖 —— " + "对的可能是法兰、不是规；摆到能看清规尖再跑。看日志『规尖视觉量取失败』）", } @@ -445,6 +455,12 @@ class GaugeAssembly(AssemblyPipeline): # `$?` 判不出成败 —— 自动化里会把"流程失败"记成"成功"。结论文件是唯一 # 能被脚本/CI 读到的东西，由 run_gauge_assembly.sh 传入并读回。 self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json") + # 眼在手外标定产物的**确切路径**，由 launch 按 site.yaml 派生后传入 + # （`<臂号>_<相机>_eye_to_hand.yaml`）。为什么不让节点自己拼： + # "臂号 = SN 末两位"这条口径只有 export_site_env 一处实现，节点再写一遍 + # 必然漂移；而漂移的症状是"标定做完了、S03 却找不到文件"，回退到 S04 跟踪， + # 看起来像跟踪的问题。留空则退回下面那串候选名（兼容旧命名/仿真）。 + self.declare_parameter("coarse_eye_to_hand_file", "") site = load_site_dict() # robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。 @@ -599,49 +615,30 @@ class GaugeAssembly(AssemblyPipeline): def _d435_ns(self, site: Dict[str, Any]) -> str: """手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。 - 为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``）， - 只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 —— - 同一份配置在仿真/真机之间**错而不报**地互串。 - - 规则（确定性的三级）： - 1. ``arm`` 与当前臂相同的优先（仿真：sim_d435 / 真机若两台各有相机也适用）； - 2. 都没有 ``arm``（= 双臂共用的那台老相机）时回落到它； - 3. 还是多个 → 出声告警并取第一个，让人去配置里区分。 + 规则（三级收敛）的**唯一实现**在 ``site_io.overhead_camera``——手眼标定 + 脚本也用它取相机名。这里只负责把"配置没区分开"这件事**出声**：静默取第一台 + 会让"标定用的相机"和"推理用的相机"悄悄不是同一台。 """ - cams = [ - cfg - for cfg in (site.get("cameras") or {}).values() - if isinstance(cfg, dict) - and str(cfg.get("role") or "").startswith("overhead") - ] - if not cams: - return "" - arm = str(self._robot_dict(site).get("arm") or "").strip() - mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else [] - hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams - if len(hits) > 1: - names = ", ".join(str(c.get("name") or "?") for c in hits) + cfg, others = overhead_camera(site, self._robot_dict(site)) + if others: + names = ", ".join([str(cfg.get("name") or "?")] + others) self.get_logger().warn( - f"有 {len(hits)} 台手外相机都符合当前臂（{names}）—— 取第一个，" + f"有 {len(others) + 1} 台手外相机都符合当前臂（{names}）—— 取第一个，" "但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气" ) - return str(hits[0].get("name") or "").strip("/") + return str(cfg.get("name") or "").strip("/") def _eye_to_hand_file(self) -> Optional[Path]: """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。 - 路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录 - 启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。 + 规则（候选顺序 + 按仓库根解析）只有一份，在 ``site_io.resolve_eye_to_hand_file``。 + 这里只负责把 launch 传入的确切路径和当前手外相机名交给它。 """ - root = ros2_root() - for rel in ( - f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml", - "handeye/results/eye_to_hand.yaml", - ): - cand = root / rel - if cand.is_file(): - return cand - return None + return resolve_eye_to_hand_file( + ros2_root(), + explicit=str(self.get_parameter("coarse_eye_to_hand_file").value or ""), + camera_name=self.cfg.coarse_camera, + ) def _on_robot_description(self, msg: String) -> None: self._robot_description = str(msg.data) @@ -1455,11 +1452,15 @@ class GaugeAssembly(AssemblyPipeline): ns = self.cfg.coarse_camera he = self._eye_to_hand_file() if he is None: + expect = str( + self.get_parameter("coarse_eye_to_hand_file").value or "" + ).strip() or f"handeye/results/{ns}_eye_to_hand.yaml" self.get_logger().warn( "S03 需要手外相机的 eye-to-hand 标定文件，现在没有 —— " f"按兜底逻辑回 S04 跟踪。\n" - f" 相机 {ns} 的标定产物应放在 handeye/results/<名>_eye_to_hand.yaml；\n" - f" 做法：bash scripts/run_handeye.sh --mode eye_to_hand（标完再跑本流程）" + f" 应该生成：{expect}\n" + f" 做法：bash scripts/run_handeye.sh --mode eye_to_hand " + f"teach/merge/calibrate（标完再跑本流程）" ) return Event.COARSE_FAIL if not self._wait_coarse_frame(ns): @@ -1792,6 +1793,9 @@ class GaugeAssembly(AssemblyPipeline): z0 = float(Tee[2, 3]) step_m = self.cfg.contact_step_mm / 1000.0 max_depth_m = self.cfg.contact_max_depth_mm / 1000.0 + # 台肩落面所需的下压量。在下面几何自检里算出来；**要留到接触判据那里用** + # （看 hole_entry_verdict：下压深度远小于它 ⇒ 规没进孔）。 + needed_m: Optional[float] = None # ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------ # 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面 @@ -1920,6 +1924,11 @@ class GaugeAssembly(AssemblyPipeline): f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N" f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）" ) + # 下压这么浅就"接触" ⇒ 尖端顶在工件上表面，没进孔。必须在这里 + # 拦下：放它进 S08 只会干转几轮、憋住，最后报一句"转轴偏差" + # 把所有人引去查工具系。见 hole_entry_verdict 的说明。 + if self._hole_lost_guard(z0, Tee, needed_m, q, "腕部 FT"): + return Event.HOLE_LOST # 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限** # 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限 # 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。 @@ -1966,6 +1975,9 @@ class GaugeAssembly(AssemblyPipeline): f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）" ) # 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。 + # 同样先过"是不是根本没进孔"这道闸（见 hole_entry_verdict）。 + if self._hole_lost_guard(z0, Tee, needed_m, q, "触觉切向"): + return Event.HOLE_LOST self._relieve_after_contact(q, Tee, "触觉切向", ft_ref) return Event.CONTACT_OK if fn - self._fn_rest > self.cfg.contact_safe_delta: @@ -2002,6 +2014,47 @@ class GaugeAssembly(AssemblyPipeline): self._retract(q, Tee, dz_m=0.01) return Event.TIMEOUT + def _hole_lost_guard( + self, + z0: float, + Tee: np.ndarray, + needed_m: Optional[float], + q: np.ndarray, + why: str, + ) -> bool: + """S07 判到接触时：下压深度是不是**远小于**几何预期（= 规没进孔）。 + + 返回 True 表示"确实没进孔"，调用方要抬臂并返回 ``Event.HOLE_LOST``。 + + 完整来龙去脉在 ``gauge_fsm.hole_entry_verdict`` 的 docstring 里，这里只 + 留现场要看的那几句：工件在 S08/S09/S10 会被工具推走，而 k2 的孔位是 + S05 一次性定的 ⇒ 工件一动孔位就失效 ⇒ 规落在孔旁边的平面上。 + 不拦的话会干转 5 轮，最后报一句"转轴偏差…先确认工具系有没有装转接件"， + 把所有人引去查工具系（实测 2026-09-28）。 + """ + if needed_m is None: + # 没算出几何预期（工件上表面 z 没标定且 S05 也没给）—— 没有判据就别猜。 + return False + descended_m = abs(float(Tee[2, 3]) - z0) + if hole_entry_verdict(descended_m, needed_m) != ENTRY_HIGH: + return False + self.get_logger().error( + f"S07 接触（{why}）发生在下压 {descended_m * 1000:.2f}mm 处，" + f"而按几何「台肩落面」需要 {needed_m * 1000:.1f}mm —— 只走了 " + f"{descended_m / needed_m * 100.0:.0f}%。\n" + " **规没有进孔，尖端顶在工件上表面**（正常入孔要压到台肩落面那一下）。\n" + " 最可能的原因：工件被工具推走了，而本孔的坐标是 **S05 一次性定下**的 ——\n" + " 工件一动，那个坐标就失效（S06 的「实测残差」发现不了：它比的是\n" + " 「命令位置 vs 实际位置」，不是「规尖 vs 真实孔位」）。\n" + " 所以**不要**去查工具系/转接件/力阈值（那几个都是好的）。\n" + " 正确处置：把臂退开、确认工件的实际位置；能复位就复位工件，然后\n" + " 从 S05 重新拍照定位（重跑流程即会重新定位）。\n" + " 若要治根：换孔前复核工件没动（当前只有 S05 阶段有 drag 看门狗）。\n" + " 抬起并中止，不带着失效的孔位进 S08。" + ) + self._retract(q, Tee, dz_m=0.01) + return True + def _relieve_after_contact( self, q: np.ndarray, diff --git a/src/arm_application/arm_application/gauge_fsm.py b/src/arm_application/arm_application/gauge_fsm.py index 7081429..44fee57 100644 --- a/src/arm_application/arm_application/gauge_fsm.py +++ b/src/arm_application/arm_application/gauge_fsm.py @@ -79,6 +79,10 @@ class Event(str, Enum): CONTACT_OK = "contact_ok" FORCE_OVER_LIMIT = "force_over_limit" # 力超安全上限 → 抬起 + #: S07 判到接触、但**下压深度远小于几何预期** ⇒ 规没进孔，顶在工件上表面。 + #: 单列一个事件是为了让结论与日志都是实话：复用 FORCE_OVER_LIMIT 会把这种 + #: 情况报成"力超安全上限"，而它的力根本没超限 —— 又是一句误导。 + HOLE_LOST = "hole_lost" SCREW_DONE = "screw_done" SCREW_STOP = "screw_stop" # 力判据满足：切向力>阈值 且 法向力突增 @@ -158,6 +162,7 @@ TRANSITIONS: Dict[Tuple[State, Event], State] = { # --- S07 柔顺接触 --- (State.CONTACT, Event.CONTACT_OK): State.SCREW_IN, (State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT, + (State.CONTACT, Event.HOLE_LOST): State.ABORT, # --- S08 旋转装配循环 --- (State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW, (State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW, @@ -406,6 +411,40 @@ def wrench_live(age_s: Optional[float]) -> bool: return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S +#: 判定"六个分量全为 0"用的容差 [N]/[Nm]。真机 FT 的噪声量级是 1e-2~1e-1， +#: 所以 1e-6 足够区分"真的是 0（数据没出来）"与"很小但活的读数"。 +WRENCH_ZERO_EPS: float = 1.0e-6 + + +def wrench_all_zero( + fx: float, + fy: float, + fz: float, + tx: float, + ty: float, + tz: float, + eps: float = WRENCH_ZERO_EPS, +) -> bool: + """六个分量**同时**为 0（容差 ``eps``）⇒ 这是"数据没出来"的指纹，不是"安静"。 + + 与 :func:`wrench_live` 的分工（两个都要有，缺一不可）： + + * ``wrench_live`` 只看"够不够新"，**故意不看数值** —— 静止时轴向力本来就接近 0， + 拿数值判活会把"安静的传感器"误判成掉线（2026-09-24 触觉上踩过）。 + * 但一个**带着负载**的腕部 FT 不可能六个分量**同时**恰好为 0：重力、装配预紧、 + 传感器零漂至少让某个分量不为 0。所以"全零"是另一种失效 —— **数据源没出数** + （话题名对、但内容是空的；或 RDK 侧没启用该传感）。 + + 2026-10-08 真机实测：应用在用的 ``//ft_sensor_wrench`` 就是恒 (0,0,0)， + 于是"``abs(fz−基线)>2.5N`` 判接触"的兜底**永远触发不了** —— 而日志里一切正常 + （话题收得到、回调在跑、``wrench_live`` 报"活"），属于最危险的静默失效。 + 改用 ``//external_wrench_in_tcp`` 后有数（≈负载 54N）即可恢复兜底。 + + 所以：``wrench_live`` 通过 **且** ``wrench_all_zero`` 为真 ⇒ 判为"数据源是空的"。 + """ + return all(abs(float(v)) <= eps for v in (fx, fy, fz, tx, ty, tz)) + + #: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。 #: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、 #: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内" @@ -730,6 +769,58 @@ def descend_budget_m(needed_m: float) -> float: return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M) +#: ``hole_entry_verdict`` 的结论。 +ENTRY_OK = "ok" # 接触深度与几何预期相符 ⇒ 规是从孔里下去的、台肩落面 +ENTRY_HIGH = "high" # 早早就接触 ⇒ 尖端顶在工件**上表面**上，根本没进孔 + +#: 判"没进孔"的下压深度比例阈值。 +#: 实测两个锚点（2026-09-28，仿真）： +#: · 正常入孔：下压 28.6mm / 需要 31.9mm ⇒ 0.90 +#: · 顶在平面上：下压 0.90mm / 需要 31.9mm ⇒ 0.03 +#: 两者差着一个台肩高度（30mm），取 0.5 时两侧余量都很大（正常那侧要掉到 +#: 一半才会误判）。**不要**为了"更灵敏"把它往 0.9 推 —— 台肩倒角没完全落面、 +#: 板厚公差都会让正常值在 0.85~0.95 之间晃。 +HOLE_ENTRY_MIN_RATIO: float = 0.5 + + +def hole_entry_verdict( + descended_m: float, + needed_m: float, + *, + min_ratio: float = HOLE_ENTRY_MIN_RATIO, +) -> str: + """S07 判到接触的那一刻：规是**进了孔**、还是**顶在工件上表面**。 + + 为什么需要它（2026-09-28 实测，同一命令三跑里坏一跑：DONE / ABORT / DONE）： + 工件在 S08/S09/S10 阶段会被工具推走（仿真里实测被推 20~52mm）。而 k2 的 + 孔位是 **S05 一次性定下**的（``_step_hover_k`` 取 ``_hole_xy``），工件一动 + 它就失效。于是规落在孔**旁边的平面**上，只下压「悬停间隙」那么点就判到接触。 + + 不拦的代价，实测完整发生过一次： + S08 干转 5 轮（切向增量 0.0、法向增量 ±0.0，**一点没进给**）→ 第 6 轮憋住 → + 报出来的是「转轴偏差 7.6° 超过阈值 —— 关节7 轴与工具 Z 不共线， + 先确认工具系是否垂直向下、有没有装转接件」。 + + **那句话会把所有人引到错误方向**：真因既不是工具系也不是转接件，是孔位失效。 + 而且为这句误导，白烧了 5 轮无效拧入（≈30 秒）和一次憋劲（有把规顶掉的风险）。 + + 为什么比 ``needed_m`` 而不是比"尖端离面高度"：正常入孔时接触发生在**台肩落面** + 那一刻 ≈ ``needed_m``；顶在平面上时接触发生在**尖端刚碰到面**那一刻，而台肩还在 + 尖端上方 30mm。两者差着一个台肩高度，所以用比例判最稳（见上面两个实测锚点）。 + + 纯函数（R5.1）：判错两个方向都贵 —— 把正常的判成"没进孔"会白扔一整个孔； + 把"没进孔"判成正常，就是上面那 30 秒 + 一次误导。 + + ``needed_m <= 0`` 时**不判**（几何本身已经错了，上面那条自检会报）， + 返回 ``ENTRY_OK`` 不在这里抢着报第二个错。 + """ + if needed_m <= 0.0: + return ENTRY_OK + if float(descended_m) < float(needed_m) * float(min_ratio): + return ENTRY_HIGH + return ENTRY_OK + + def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool: """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。 @@ -930,6 +1021,7 @@ EVENT_DOC: Dict[Event, str] = { Event.ALIGN_OUT: "对位超差", Event.CONTACT_OK: "已接触工件面", Event.FORCE_OVER_LIMIT: "力超安全上限", + Event.HOLE_LOST: "孔位失效（规没进孔）", Event.SCREW_DONE: "达到目标圈数", Event.SCREW_STOP: "力判据满足（咬住螺纹）", Event.GAUGE_DROPPED: "通止规掉了", diff --git a/src/arm_application/arm_application/handeye_lib.py b/src/arm_application/arm_application/handeye_lib.py index 5ce396c..1efccd1 100644 --- a/src/arm_application/arm_application/handeye_lib.py +++ b/src/arm_application/arm_application/handeye_lib.py @@ -126,6 +126,39 @@ def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray: return T +def cam_to_base_T( + T_handeye: np.ndarray, T_ee: np.ndarray, mode: str +) -> np.ndarray: + """相机系 → 基座系的 4×4。 + + * ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。 + * ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把 + 固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。 + """ + T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4) + tag = str(mode or "").strip().lower() + if tag == "eye_to_hand": + return T_he + if tag != "eye_in_hand": + raise ValueError( + f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}" + ) + return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he + + +def cam_point_to_base( + p_cam: np.ndarray, + T_handeye: np.ndarray, + T_ee: np.ndarray, + mode: str, +) -> np.ndarray: + """工件三维点（相机系）→ 基座系 XYZ。""" + T_bc = cam_to_base_T(T_handeye, T_ee, mode) + xyz = np.asarray(p_cam, dtype=float).reshape(3) + ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float) + return (T_bc @ ph)[:3] + + def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: Ri = R.T ti = -Ri @ t.reshape(3, 1) diff --git a/src/arm_application/arm_application/hole_detect_hough.py b/src/arm_application/arm_application/hole_detect_hough.py index 3bc34e4..e99a025 100644 --- a/src/arm_application/arm_application/hole_detect_hough.py +++ b/src/arm_application/arm_application/hole_detect_hough.py @@ -134,6 +134,67 @@ def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None: return None +def _keypoint_holes( + dets, + *, + conf_thr: float, + min_kconf: float, + radius_px: float, + min_dist_px: float, +): + """pose 模型直接给出的 k1/k2 关键点当孔位。 + + 现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。 + 返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None + （不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。 + + 为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆， + 光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里 + 直接回归出来的，对这几类干扰天然更稳。 + """ + if not dets: + return None + for det in sorted(dets, key=lambda d: d.conf, reverse=True): + if float(det.conf) < conf_thr: + continue + kpts = getattr(det, "kpts", None) + if kpts is None: + continue + kpts = np.asarray(kpts, dtype=float) + if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3: + continue + k1, k2 = kpts[1].copy(), kpts[2].copy() + if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()): + continue + if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf: + logger.info( + "k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆", + float(k1[2]), + float(k2[2]), + min_kconf, + ) + continue + dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1])) + if dist < float(min_dist_px): + logger.info( + "k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆", + dist, + min_dist_px, + ) + continue + r = float(radius_px) + # **刻意不按图像 x 交换左右**。 + # Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的 + # k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度， + # 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同 + # 的物理孔 —— 那是最难查的一类现场问题。 + # 下游怎么认这两个点由 `stage2.k1_label` 决定： + # hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2， + # 所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。 + return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r + return None + + def _refine_circle_center( gray: np.ndarray, cx: float, @@ -466,6 +527,35 @@ def detect_holes( gate_block=True, ) + # ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ---- + if bool(cfg.get("keypoint_enable", True)): + kp = _keypoint_holes( + dets, + conf_thr=conf, + min_kconf=float(cfg.get("keypoint_min_conf", 0.30)), + radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)), + min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)), + ) + if kp is not None: + (k1, k2, r1, r2) = kp + hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]] + hole_radii = [float(r1), float(r2)] + dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1])) + gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None + draw_img = _render_success( + image, bbox, hole_points, hole_radii, dist, gate=gate, label=label + ) + return { + "status": True, + "msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px", + "hole_points": hole_points, + "hole_radii": hole_radii, + "bbox": list(bbox), + "label": label, + "source": "keypoint", + "draw_img": draw_img, + } + # ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ---- if bool(cfg.get("insert_pair_enable", True)): gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image diff --git a/src/arm_application/arm_application/hole_detector.py b/src/arm_application/arm_application/hole_detector.py index 148b0e4..8f31781 100644 --- a/src/arm_application/arm_application/hole_detector.py +++ b/src/arm_application/arm_application/hole_detector.py @@ -21,7 +21,7 @@ DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml" # 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"， # 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。 # 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。 -_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx" +_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx" @dataclass @@ -143,17 +143,18 @@ class HoleDetector: def _load_yolo(self): if self._detector is not None: return self._detector - from arm_application.yolo_detector import YoloDetector, resolve_weights + from arm_application.yolo_detector import build_detector, resolve_weights weights = resolve_weights( self.model_path, backend=str(self.cfg.get("yolo_backend") or "auto"), ) - self._detector = YoloDetector( + # build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。 + self._detector = build_detector( weights, - device=str(self.device), conf=float(self.cfg.get("conf", 0.25)), iou=float(self.cfg.get("iou", 0.7)), + device=str(self.device), imgsz=int(self.cfg.get("imgsz", 640)), ) return self._detector diff --git a/src/arm_application/arm_application/kalman_filter.py b/src/arm_application/arm_application/kalman_filter.py index 2de4e58..eafcbbd 100644 --- a/src/arm_application/arm_application/kalman_filter.py +++ b/src/arm_application/arm_application/kalman_filter.py @@ -62,6 +62,15 @@ class KalmanFilter3D: self._predict(dt) self._t = t + def zero_velocity(self) -> None: + """把速度状态清零、位置保持不动。 + + 用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不 + 采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成 + "按旧速度匀速跑掉"，刚好是我们要避免的那件事。 + """ + self.x[3:] = 0.0 + def update(self, z: np.ndarray) -> None: """用一次三维位置观测做量测更新。""" z = np.asarray(z, dtype=float).reshape(3) diff --git a/src/arm_application/arm_application/onnx_workpiece.py b/src/arm_application/arm_application/onnx_workpiece.py index 0404d89..f3ae202 100644 --- a/src/arm_application/arm_application/onnx_workpiece.py +++ b/src/arm_application/arm_application/onnx_workpiece.py @@ -54,6 +54,21 @@ class OnnxWorkpieceDetector: self._iname = inp.name shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape] self._in_h, self._in_w = int(shape[-2]), int(shape[-1]) + # 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。 + # 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型 + # （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框 + # ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。 + # 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。 + # 宁可启动就失败（元规则：静默失效是头号敌人）。 + outs = self._sess.get_outputs() + tail = outs[0].shape[-1] if (outs and outs[0].shape) else None + if isinstance(tail, int) and tail != 6: + raise RuntimeError( + f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是" + " ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的" + " pose 模型必须交给 pose_detector / build_detector，否则坐标会被" + "当成置信度解出垃圾框，而且不会报错。" + ) self.conf = float(conf) self.imgsz = int(imgsz) # 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。 diff --git a/src/arm_application/arm_application/overhead_oneshot.py b/src/arm_application/arm_application/overhead_oneshot.py new file mode 100644 index 0000000..651125a --- /dev/null +++ b/src/arm_application/arm_application/overhead_oneshot.py @@ -0,0 +1,245 @@ +"""D405 主跟踪丢了之后，用顶置 D435 **拍一张** 把工件 / k0 定到基座系。 + +为什么单独成模块 +---------------- +一阶段平时走腕部 D405（眼在手上）。画面里没有 conf≥门槛的目标时， +不能让伺服继续追 0.12 的假框，也不能把顶视外参再乘一遍法兰 +（那会让一次性定位差几十厘米还看起来合理）。 + +判定、取 k0、反投影、臂是否静止 —— 全是纯函数，测试不启 ROS。 +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Iterable, Optional + +import numpy as np + +from arm_application.handeye_lib import cam_point_to_base + + +def best_det_conf(dets: Optional[Iterable]) -> Optional[float]: + """画面内最高检测置信度。空列表 / None → ``None``（不是 0：0 是"看到了但很差"）。""" + if not dets: + return None + vals = [float(getattr(d, "conf", float("nan"))) for d in dets] + vals = [c for c in vals if np.isfinite(c)] + if not vals: + return None + return max(vals) + + +def select_oneshot_det(dets: Optional[Iterable], min_conf: float): + """顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。""" + if not dets: + return None + ok = [ + d + for d in dets + if np.isfinite(float(getattr(d, "conf", float("nan")))) + and float(d.conf) >= float(min_conf) + ] + if not ok: + return None + return max(ok, key=lambda d: float(d.conf)) + + +def depth_raw_to_m(raw: Optional[np.ndarray], encoding: str) -> Optional[np.ndarray]: + """深度图 → 米。16UC1 常见是毫米；用中位数判量纲，不许默认当米。""" + if raw is None: + return None + enc = (encoding or "").lower() + arr = np.asarray(raw) + if arr.size == 0: + return None + if enc in ("16uc1", "mono16"): + pos = arr[arr > 0] + if pos.size < 8: + return None + med = float(np.median(pos)) + scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001) + out = arr.astype(float) * scale + out[arr <= 0] = np.nan + return out + if enc == "32fc1": + return arr.astype(float) + return None + + +def map_uv_to_depth( + u: float, v: float, color_wh: tuple[int, int], depth_wh: tuple[int, int] +) -> tuple[float, float]: + """彩色图像素 → 对齐深度图像素。分辨率相同则原样返回。""" + cw, ch = int(color_wh[0]), int(color_wh[1]) + dw, dh = int(depth_wh[0]), int(depth_wh[1]) + if cw <= 0 or ch <= 0 or (cw == dw and ch == dh): + return float(u), float(v) + return float(u) * (dw / float(cw)), float(v) * (dh / float(ch)) + + +def wrist_needs_handoff(best_conf: Optional[float], threshold: float) -> bool: + """腕部这一帧有没有 conf≥门槛的目标。没有 → 该切 D435。 + + ``None`` 当没有检测。门槛必须显式传入，不许在这里写死再抄一份到 site.yaml。 + """ + if best_conf is None: + return True + try: + c = float(best_conf) + except (TypeError, ValueError): + return True + if not np.isfinite(c): + return True + return c < float(threshold) + + +def k0_uv_from_det( + det, + *, + kpt_index: int = 0, + kpt_min_conf: float = 0.30, +) -> Optional[tuple[float, float]]: + """从 pose 检测取 k0 像素。k0 不可信就返回 None（不许拿框心冒充 k0）。""" + kpts = getattr(det, "kpts", None) + if kpts is None: + return None + arr = np.asarray(kpts, dtype=float) + if arr.ndim != 2 or arr.shape[0] <= int(kpt_index) or arr.shape[1] < 3: + return None + u, v, kc = (float(x) for x in arr[int(kpt_index)][:3]) + if not (np.isfinite(u) and np.isfinite(v) and np.isfinite(kc)): + return None + if kc < float(kpt_min_conf): + return None + return u, v + + +def workpiece_uv_from_det( + det, + *, + kpt_index: int = 0, + kpt_min_conf: float = 0.30, +) -> tuple[float, float, bool]: + """优先 k0；没有可信 k0 时退回框心，并标明 ``is_k0=False``。 + + 顶视经常看不到腕部视角训出来的 k0。退回框心仍能把臂迁到工件上方， + 让 D405 重新看见 k0 —— 比假装"已经是 k0"好。调用方必须看第三个返回值。 + """ + uv = k0_uv_from_det(det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf) + if uv is not None: + return uv[0], uv[1], True + return float(det.cx), float(det.cy), False + + +def depth_at_m( + depth_m: np.ndarray, + u: float, + v: float, + *, + win: int = 3, +) -> Optional[float]: + """窗口中位数深度 [m]。无效则 None，不许拿 0 去反投影。""" + if depth_m is None or depth_m.size == 0: + return None + h, w = int(depth_m.shape[0]), int(depth_m.shape[1]) + ui, vi = int(round(u)), int(round(v)) + r = max(1, int(win)) + x0, x1 = max(0, ui - r), min(w, ui + r + 1) + y0, y1 = max(0, vi - r), min(h, vi + r + 1) + if x1 <= x0 or y1 <= y0: + return None + patch = np.asarray(depth_m[y0:y1, x0:x1], dtype=float).reshape(-1) + patch = patch[np.isfinite(patch) & (patch > 1e-4)] + if patch.size == 0: + return None + return float(np.median(patch)) + + +def px_to_cam( + u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float +) -> np.ndarray: + return np.array( + [(u - cx) * z / fx, (v - cy) * z / fy, z], dtype=float + ) + + +def oneshot_k0_base( + det, + depth_m: np.ndarray, + *, + fx: float, + fy: float, + cx: float, + cy: float, + T_cam_to_base: np.ndarray, + min_z_m: float, + max_z_m: float, + kpt_index: int = 0, + kpt_min_conf: float = 0.30, + min_det_conf: float = 0.5, + color_wh: Optional[tuple[int, int]] = None, + depth_wh: Optional[tuple[int, int]] = None, +) -> Optional[tuple[np.ndarray, bool]]: + """D435 一张图 → 基座系 XYZ。失败返回 None。 + + 外参必须是 **cam→基座**（eye_to_hand）。传入腕部 T 会得到错位置且不报错， + 所以调用方必须自己保证文件对；本函数不再猜。 + 反投影用**彩色图**像素 + 彩色图内参；深度只在对齐图上取样。 + """ + if det is None or float(getattr(det, "conf", 0.0)) < float(min_det_conf): + return None + u, v, is_k0 = workpiece_uv_from_det( + det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf + ) + u_d, v_d = u, v + if color_wh is not None and depth_wh is not None: + u_d, v_d = map_uv_to_depth(u, v, color_wh, depth_wh) + z = depth_at_m(depth_m, u_d, v_d) + if z is None or z < float(min_z_m) or z > float(max_z_m): + return None + p_cam = px_to_cam(u, v, z, fx, fy, cx, cy) + T_ee = np.eye(4) # eye_to_hand 不用法兰，占位 + p_base = cam_point_to_base(p_cam, T_cam_to_base, T_ee, "eye_to_hand") + if not np.all(np.isfinite(p_base)): + return None + return np.asarray(p_base, dtype=float).reshape(3), bool(is_k0) + + +@dataclass +class StillLock: + """D405 跟住时，法兰 XY 连续 ``still_sec`` 秒位移不超过 ``still_move_m`` → 锁定。 + + 顶视一次性迁过去的那段路**不算**锁定：``tracking_ok`` 必须是腕部有效目标。 + 大幅一动就重计时，避免"晃一下刚好满 3 秒"误进二阶段。 + """ + + still_sec: float + still_move_m: float + _t0: Optional[float] = None + _anchor_xy: Optional[np.ndarray] = None + + def reset(self) -> None: + self._t0 = None + self._anchor_xy = None + + def update( + self, + p_ee_xy: Optional[np.ndarray], + now: float, + *, + tracking_ok: bool, + ) -> bool: + if (not tracking_ok) or p_ee_xy is None: + self.reset() + return False + xy = np.asarray(p_ee_xy, dtype=float).reshape(2) + if self._anchor_xy is None or self._t0 is None: + self._anchor_xy = xy.copy() + self._t0 = float(now) + return False + if float(np.linalg.norm(xy - self._anchor_xy)) > float(self.still_move_m): + self._anchor_xy = xy.copy() + self._t0 = float(now) + return False + return (float(now) - float(self._t0)) >= float(self.still_sec) diff --git a/src/arm_application/arm_application/perception_node.py b/src/arm_application/arm_application/perception_node.py index 730a0ec..06ba93a 100644 --- a/src/arm_application/arm_application/perception_node.py +++ b/src/arm_application/arm_application/perception_node.py @@ -15,6 +15,7 @@ from __future__ import annotations import os import threading import time +from pathlib import Path from typing import Optional import numpy as np @@ -30,26 +31,48 @@ from rclpy.qos import ( qos_profile_sensor_data, ) from sensor_msgs.msg import CameraInfo, Image -from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32, Float32MultiArray from tf2_ros import TransformBroadcaster from arm_application.box_tracker import BoxTracker +from arm_application.feature_guard import ( + FarReacquireConfirm, + TrackSlewGuard, + kpt_plausible, +) +from arm_application.geom_ik import load_handeye_T from arm_application.handeye_realsense import ( is_realsense_source, resolve_realsense_topics, ) +from arm_application.overhead_oneshot import ( + best_det_conf, + depth_raw_to_m, + oneshot_k0_base, + select_oneshot_det, + wrist_needs_handoff, +) from arm_application.kalman_filter import KalmanFilter3D -from arm_application.plane_feature import geometric_center_on_plane, workpiece_xyz_in_box +from arm_application.plane_feature import ( + PlaneFeature, + geometric_center_on_plane, + workpiece_xyz_in_box, +) +from arm_application.pose_detector import TRACK_KPT_INDEX from arm_application.yolo_detector import ( INFER_BUSY, YoloDetector, YoloInferProcess, + build_detector, pick_best, resolve_weights, ) from arm_application.ros_node import TolerantNode +# D435 一次性推理用的 seq。必须远离腕部帧序号，结果绝不能当 D405 检测去立体匹配。 +_ONESHOT_SEQ = 2_000_000_000 + def _bgr_copy(msg: Image) -> np.ndarray: """返回调用方持有的 BGR uint8 数组（对异步 YOLO 安全；ROS 可能把原消息丢掉）。""" @@ -76,17 +99,33 @@ class PerceptionNode(TolerantNode): self.declare_parameter("image_topic", "") self.declare_parameter("camera_info_topic", "") self.declare_parameter("depth_topic", "/d405/aligned_depth_to_color/image_raw") - self.declare_parameter("weights", "/home/cc/ros2/models/yolo/detect_best_0.onnx") + self.declare_parameter("weights", "/home/cc/ros2/models/yolo/gongjian0929.onnx") self.declare_parameter("conf", 0.10) self.declare_parameter("iou", 0.45) self.declare_parameter("imgsz", 640) self.declare_parameter("device", "0") self.declare_parameter("min_conf", 0.12) + # 重新捕获门槛（跳到新位置必须达到的置信度）。为什么必须比 min_conf 高得多： + # 真机实测假框 conf 0.13~0.47，而真 k0 是 0.87（推导见 site.yaml 同名字段）。 + # 旧写法把它接成 min_conf ⇒ 0.15 的远框被原样采信、kf.reset() 直接跳过去。 + self.declare_parameter("reacquire_conf", 0.55) + # ---- 跟踪点可信性的三道守卫（实现与推导见 feature_guard.py）---- + # 2026-10-08 真机：真 k0（conf 0.87）被一串**高置信度假 k0**（conf 0.53~0.73） + # 顶掉，1s 内把臂从 home 带到工作盒角落，之后 148s 全是"检测漏检"。 + # 假 k0 挡不住的地方：置信度没有可分区间；它们**贴在画面边上**、**借深度**。 + self.declare_parameter("keypoint_border_px", 6.0) + self.declare_parameter("track_max_step_m", 0.04) + self.declare_parameter("reacquire_far_px", 120.0) + self.declare_parameter("reacquire_confirm_frames", 3) self.declare_parameter("vision_hz", 30.0) self.declare_parameter("z_ref_m", 0.40) self.declare_parameter("jump_px", 280.0) self.declare_parameter("jump_z_m", 0.20) self.declare_parameter("min_depth_m", 0.04) + # 特征点的**相机量程下限**：比这更近的深度读数一定来自机械臂自身（爪上夹的 + # 通止规 / 相机座），不是工件。判据是实测两条带（见 site.yaml 同名字段的推导）： + # 随臂一起动的近带 0.078~0.087m，工件面 0.19~0.29m ⇒ 取 0.14。 + self.declare_parameter("feature_min_range_m", 0.14) self.declare_parameter("max_depth_m", 0.55) self.declare_parameter("work_z_max_m", 0.50) # 高于此值 = 孔 / 背景 self.declare_parameter("object_frame", "tracked_object") @@ -112,16 +151,41 @@ class PerceptionNode(TolerantNode): self.declare_parameter("hold_point_on_miss", True) self.declare_parameter("miss_hold_point_frames", 2) # plane = 立体表面质心（推荐）；box = YOLO 检测框中心 + # keypoint = pose 模型的 k0 关键点（跟踪孔）；见 pose_detector self.declare_parameter("feature_mode", "plane") + self.declare_parameter("keypoint_index", TRACK_KPT_INDEX) + self.declare_parameter("keypoint_min_conf", 0.30) + self.declare_parameter("keypoint_win_px", 6) self.declare_parameter("plane_inlier_m", 0.008) self.declare_parameter("plane_band_m", 0.06) + # ---- D405 主跟踪 + D435 一次性（键必须从 site 接入，缺了启动即红）---- + self.declare_parameter("wrist_handoff_conf", -1.0) + self.declare_parameter("overhead_oneshot_enable", False) + self.declare_parameter("overhead_min_depth_m", -1.0) + self.declare_parameter("overhead_max_depth_m", -1.0) + self.declare_parameter("overhead_oneshot_conf", -1.0) + self.declare_parameter("overhead_cooldown_s", -1.0) + self.declare_parameter("overhead_frame_max_age_s", -1.0) + self.declare_parameter("overhead_handeye_file", "") + self.declare_parameter("overhead_camera_source", "") + self.declare_parameter("overhead_camera_frame", "") + self.declare_parameter("overhead_image_topic", "") + self.declare_parameter("overhead_info_topic", "") + self.declare_parameter("overhead_depth_topic", "") + self.declare_parameter("base_frame", "") self.z_ref = float(self.get_parameter("z_ref_m").value) self.min_conf = float(self.get_parameter("min_conf").value) + self.reacquire_conf = float(self.get_parameter("reacquire_conf").value) self.jump_px = float(self.get_parameter("jump_px").value) self.jump_z_m = float(self.get_parameter("jump_z_m").value) self.min_depth = float(self.get_parameter("min_depth_m").value) self.max_depth = float(self.get_parameter("max_depth_m").value) + # 特征点量程下限 = max(解码量程下限, 工具几何下限)：两者取严，不许被调松到 + # 比 min_depth 还小（那会让"工具自己"重新变成合法目标）。 + self.feature_min_range = max( + self.min_depth, float(self.get_parameter("feature_min_range_m").value) + ) self.work_z_max = float(self.get_parameter("work_z_max_m").value) self.hold_point_on_miss = bool(self.get_parameter("hold_point_on_miss").value) self.object_frame = str(self.get_parameter("object_frame").value) @@ -140,13 +204,87 @@ class PerceptionNode(TolerantNode): 1, int(self.get_parameter("miss_hold_point_frames").value) ) self.feature_mode = str(self.get_parameter("feature_mode").value or "plane").strip().lower() + self.keypoint_index = max(0, int(self.get_parameter("keypoint_index").value)) + self.keypoint_min_conf = float(self.get_parameter("keypoint_min_conf").value) + self.keypoint_win_px = max(1, int(self.get_parameter("keypoint_win_px").value)) + # 跟踪点三道守卫（单一实现见 feature_guard.py） + self.keypoint_border_px = float(self.get_parameter("keypoint_border_px").value) + self.track_max_step_m = float(self.get_parameter("track_max_step_m").value) + self._slew = TrackSlewGuard(self.track_max_step_m) + self._far = FarReacquireConfirm( + float(self.get_parameter("reacquire_far_px").value), + int(self.get_parameter("reacquire_confirm_frames").value), + ) self.plane_inlier_m = float(self.get_parameter("plane_inlier_m").value) self.plane_band_m = float(self.get_parameter("plane_band_m").value) + self.overhead_oneshot_enable = bool( + self.get_parameter("overhead_oneshot_enable").value + ) + self._T_oh: Optional[np.ndarray] = None + self._oh_image: Optional[Image] = None + self._oh_depth: Optional[Image] = None + self._oh_image_mono = -1.0 + self._oh_depth_mono = -1.0 + self._oh_fx = self._oh_fy = self._oh_cx = self._oh_cy = 0.0 + self._oh_frame = "" + self._oneshot_t = -1.0e9 + self._need_oneshot = False + self._oneshot_inflight = False + self._oneshot_img: Optional[Image] = None + self._oneshot_depth: Optional[Image] = None + if self.overhead_oneshot_enable: + self.wrist_handoff_conf = float( + self.get_parameter("wrist_handoff_conf").value + ) + if self.wrist_handoff_conf < 0.0: + raise RuntimeError( + "overhead_oneshot_enable 时 wrist_handoff_conf 必须由 site.yaml 接入，" + "收到哨兵 -1 —— 配置没接线。" + ) + self.overhead_min_depth = float( + self.get_parameter("overhead_min_depth_m").value + ) + self.overhead_max_depth = float( + self.get_parameter("overhead_max_depth_m").value + ) + self.overhead_oneshot_conf = float( + self.get_parameter("overhead_oneshot_conf").value + ) + self.overhead_cooldown_s = float( + self.get_parameter("overhead_cooldown_s").value + ) + self.overhead_frame_max_age_s = float( + self.get_parameter("overhead_frame_max_age_s").value + ) + if min( + self.overhead_min_depth, + self.overhead_max_depth, + self.overhead_oneshot_conf, + self.overhead_cooldown_s, + self.overhead_frame_max_age_s, + ) < 0.0: + raise RuntimeError( + "D435 一次性参数仍是哨兵 -1 —— site.yaml 的 perception.overhead 没接到节点。" + ) + oh_file = str(self.get_parameter("overhead_handeye_file").value or "") + if not Path(oh_file).is_file(): + raise RuntimeError( + f"D435 一次性定位缺眼在手外结果：{oh_file}。" + "先跑 bash ~/ros2/scripts/run_handeye.sh --arm 89 --mode eye_to_hand calibrate" + ) + self._T_oh = load_handeye_T(oh_file) + self._oh_frame = str( + self.get_parameter("overhead_camera_frame").value or "" + ) self._fx = self._fy = self._cx = self._cy = 0.0 self._latest_image: Optional[Image] = None self._latest_depth: Optional[Image] = None self._last_uv: Optional[np.ndarray] = None + # k0 相对工件检测框的**归一化偏移**（相对框中心、除以框宽高）。k0 掉信时 + # 用它在框上重建 k0 —— 只借"偏移"不借"绝对位置"，跟踪点就不会横移一大截 + #（见 `_keypoint_feature` 的真机反例）。 + self._kpt_box_frac: Optional[np.ndarray] = None self._last_good_z: Optional[float] = None self._last_plane_xyz: Optional[np.ndarray] = None self._last_filt: Optional[np.ndarray] = None @@ -203,14 +341,16 @@ class PerceptionNode(TolerantNode): f"YOLO worker pid={self._yolo_proc._p.pid} weights={self._yolo_proc.weights_path}" ) else: - self.detector = YoloDetector( + self.detector = build_detector( weights, + backend=self.backend_name, conf=float(yolo_cfg["conf"]), iou=float(yolo_cfg["iou"]), device=str(yolo_cfg["device"]), imgsz=int(yolo_cfg["imgsz"]), ) - if self._yolo_half: + # PoseDetector 自己管 GPU 张量，没有 ultralytics 的 _model 可挪。 + if self._yolo_half and hasattr(self.detector, "_model"): try: import torch @@ -220,6 +360,8 @@ class PerceptionNode(TolerantNode): self._yolo_half = False except Exception: # noqa: BLE001 self._yolo_half = False + elif self._yolo_half: + self._yolo_half = False try: self.detector.warmup(half=self._yolo_half) except Exception as exc: # noqa: BLE001 @@ -276,6 +418,34 @@ class PerceptionNode(TolerantNode): self.create_subscription( Image, depth_topic, self._on_depth, image_qos, callback_group=self._io_cbg ) + self.base_frame = str(self.get_parameter("base_frame").value or "").strip() + if self.overhead_oneshot_enable: + oh_img = str(self.get_parameter("overhead_image_topic").value or "").strip() + oh_info = str(self.get_parameter("overhead_info_topic").value or "").strip() + oh_dep = str(self.get_parameter("overhead_depth_topic").value or "").strip() + if not oh_img or not oh_dep: + raise RuntimeError( + "overhead_oneshot_enable 但 overhead_image_topic / " + "overhead_depth_topic 为空 —— 配置没接线。" + ) + self.create_subscription( + Image, oh_img, self._on_oh_image, image_qos, + callback_group=self._io_cbg, + ) + self.create_subscription( + Image, oh_dep, self._on_oh_depth, image_qos, + callback_group=self._io_cbg, + ) + if oh_info: + self.create_subscription( + CameraInfo, oh_info, self._on_oh_info, image_qos, + callback_group=self._io_cbg, + ) + self.get_logger().info( + f"D435 一次性 image={oh_img} depth={oh_dep} " + f"handoff_conf={self.wrist_handoff_conf:.2f} " + f"T={self.get_parameter('overhead_handeye_file').value}" + ) point_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, @@ -285,6 +455,12 @@ class PerceptionNode(TolerantNode): self._point_pub = self.create_publisher( PointStamped, "visual_servo/filtered_point", point_qos ) + self._wrist_conf_pub = self.create_publisher( + Float32, "visual_servo/wrist_conf", 5 + ) + self._overhead_k0_pub = self.create_publisher( + PointStamped, "visual_servo/overhead_k0", point_qos + ) # RELIABLE + keep_last(5)：相机预览能容忍短暂的 DDS 丢包间隙 det_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, @@ -345,6 +521,22 @@ class PerceptionNode(TolerantNode): def _on_depth(self, msg: Image) -> None: self._latest_depth = msg + def _on_oh_image(self, msg: Image) -> None: + self._oh_image = msg + self._oh_image_mono = time.monotonic() + + def _on_oh_depth(self, msg: Image) -> None: + self._oh_depth = msg + self._oh_depth_mono = time.monotonic() + + def _on_oh_info(self, msg: CameraInfo) -> None: + self._oh_fx = float(msg.k[0]) + self._oh_fy = float(msg.k[4]) + self._oh_cx = float(msg.k[2]) + self._oh_cy = float(msg.k[5]) + if msg.header.frame_id: + self._oh_frame = str(msg.header.frame_id) + def _wake_infer(self) -> None: if self._latest_image is None: return @@ -377,6 +569,7 @@ class PerceptionNode(TolerantNode): try: self.detector.restart() self._submitted_seq = -1 + self._oneshot_inflight = False self._last_gpu_t = time.monotonic() self.get_logger().info("YOLO 工作进程已重启") except Exception as exc: # noqa: BLE001 @@ -389,7 +582,9 @@ class PerceptionNode(TolerantNode): 我们还会**先**提交再去等，这样下一次推理已经排进队列了。 """ while not self._stop.is_set(): - self._submit_latest() + # 一次性占用 GPU 时不要再塞 D405，否则两帧挤一条队列，结果会对错图。 + if not self._oneshot_inflight: + self._submit_latest() try: got = self.detector.wait_result(timeout=0.030) except Exception as exc: # noqa: BLE001 @@ -407,8 +602,9 @@ class PerceptionNode(TolerantNode): f"(worker={'存活' if getattr(self.detector, 'alive', True) else '已死'})", throttle_duration_sec=1.0, ) - outstanding = self._submitted_seq > self._done_seq + outstanding = self._submitted_seq > self._done_seq or self._oneshot_inflight if outstanding and self._last_gpu_t is not None and silent > 2.0: + self._oneshot_inflight = False self._restart_yolo(f"卡死 {silent:.1f}s") continue if ( @@ -424,6 +620,16 @@ class PerceptionNode(TolerantNode): seq, dets, gpu_ms = got if dets is INFER_BUSY: continue + if int(seq) >= _ONESHOT_SEQ: + try: + self._finish_overhead_oneshot(dets, float(self._infer_meta.get(int(seq), 1.0))) + except Exception as exc: # noqa: BLE001 + self.get_logger().error( + f"D435 一次性结果处理失败：{exc}", throttle_duration_sec=2.0 + ) + self._oneshot_inflight = False + self._oneshot_t = time.monotonic() + continue scale = float(self._infer_meta.get(int(seq), 1.0)) t_img = float(self._frame_t.get(int(seq), time.monotonic())) t0 = time.monotonic() @@ -432,7 +638,10 @@ class PerceptionNode(TolerantNode): except Exception as exc: # noqa: BLE001 self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0) self._done_seq = int(seq) - self._submit_latest() + if self._need_oneshot and not self._oneshot_inflight: + self._submit_overhead_oneshot() + else: + self._submit_latest() dt_ms = (time.monotonic() - t0) * 1000.0 lag_ms = (time.monotonic() - t_img) * 1000.0 self.get_logger().info( @@ -490,7 +699,7 @@ class PerceptionNode(TolerantNode): else: meters = roi scale = 1.0 - vals = meters[np.isfinite(meters) & (meters > self.min_depth) & (meters < self.max_depth)] + vals = meters[np.isfinite(meters) & (meters > self.feature_min_range) & (meters < self.max_depth)] if vals.size < 8: # 内部 ROI 经常落在孔上；改用整个检测框重试。 y0 = int(np.clip(y1, 0, h - 1)) @@ -506,7 +715,7 @@ class PerceptionNode(TolerantNode): meters2 = pos2 * scale else: meters2 = roi2 - vals = meters2[np.isfinite(meters2) & (meters2 > self.min_depth) & (meters2 < self.max_depth)] + vals = meters2[np.isfinite(meters2) & (meters2 > self.feature_min_range) & (meters2 < self.max_depth)] if vals.size < 8: med_m = float(np.median(meters)) if meters.size else 0.0 return None, f"ok={vals.size} scale={scale} med={med_m:.3f}" @@ -634,9 +843,16 @@ class PerceptionNode(TolerantNode): self._box_ema = None self._last_raw_xyxy = None self._last_uv = None + self._kpt_box_frac = None self._last_filt = None self._last_plane_xyz = None self._box_trk.reset() + # 两道有状态守卫跟着"真正丢目标"一起冷启动。**只在长丢时清**，不在单帧 + # 漏检时清：单帧漏检极常见（真机日志里几乎每隔一两帧就一次），每漏一帧 + # 就把锚点清空，会让下一帧的任何观测都变成"首次观测"而被 TrackSlewGuard + # 无条件放行 —— 借深度的假点于是每次都能重新锚定，守卫形同虚设。 + self._slew.reset() + self._far.reset() self._last_yolo_t = None self._publish_detection_clear() @@ -645,6 +861,13 @@ class PerceptionNode(TolerantNode): if scale != 1.0: for d in dets: d.xyxy = d.xyxy * scale + best_conf = best_det_conf(dets) + self._publish_wrist_conf(best_conf) + handoff = bool( + self.overhead_oneshot_enable + and wrist_needs_handoff(best_conf, self.wrist_handoff_conf) + ) + self._need_oneshot = handoff now = time.monotonic() pred_at_img = self._box_trk.predict(t_img, self._img_wh()) with self._lock: @@ -656,12 +879,16 @@ class PerceptionNode(TolerantNode): prev_uv=prev_uv, prev_xyxy=prev_xyxy, max_track_px=max(400.0, self.jump_px), - reacquire_conf=self.min_conf, + reacquire_conf=self.reacquire_conf, min_conf=self.min_conf, lock_iou=0.05, ) if best is None: self._miss_count += 1 + # 单帧漏检**不清**守卫锚点（见 _clear_track 的注释）：真机 2026-10-08 + # 漏检几乎每帧都有，一漏就 reset ⇒ 每帧都当"首次观测" ⇒ 借深度的假 k0 + # 次次都能重新锚定。真正丢目标由下面的 miss_clear_frames 长丢分支 + # 走 _clear_track（连丢 miss_clear_frames 帧才冷启动）。 if self._miss_count >= self.miss_clear_frames: self._clear_track() self._last_yolo_t = None @@ -676,6 +903,7 @@ class PerceptionNode(TolerantNode): if ( self.hold_point_on_miss and self._miss_count <= self.miss_hold_point_frames + and not handoff ): self._republish_held_point() return @@ -708,12 +936,21 @@ class PerceptionNode(TolerantNode): # 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线， # 也不是 z_ref）。红点 = 该三维点。 + # feature_mode=keypoint 时改用 pose 模型的 k0（跟踪孔）当跟踪点。 feat = None p_cam: Optional[np.ndarray] = None note = "box" z_meas = None uv = box_uv - if self.feature_mode != "box": + # 本帧的深度是不是**借**来的（见 PlaneFeature.borrowed）—— 给"每帧位移上限" + # 守卫用：借来的深度意味着该像素读不到自己的深度，绝对位置不足以重定位。 + borrowed = False + if self.feature_mode == "keypoint": + feat = self._keypoint_feature(best) + # k0 掉信时 `_keypoint_feature` 会**按 k0 相对框的偏移**在框上重建跟踪点 + # （位置连续、不横移）。只有冷启动、连一次可信 k0 都没见过时才返回 None， + # 此时才退到"框内深度 + 框中心"。 + elif self.feature_mode != "box": feat = self._workpiece_xyz(xyxy) if feat is None: feat = self._plane_center(xyxy) @@ -722,6 +959,7 @@ class PerceptionNode(TolerantNode): uv = np.asarray(feat.uv, dtype=float).reshape(2) z_meas = float(p_cam[2]) note = feat.note + borrowed = bool(getattr(feat, "borrowed", False)) else: z_meas, note = self._depth_in_box(xyxy) note = f"{note} box_z" @@ -746,6 +984,24 @@ class PerceptionNode(TolerantNode): self._publish_detection_box(xyxy, float(best.conf), float(best.cls_id), uv, float(z)) + # ---- 守卫二：跟踪点的**每帧位移上限** ---- + # 工件不动、相机 30Hz，真实观测一帧走不了 4cm（实测真实运动 ≤15mm/帧）。 + # 观测可信 ⇒ 夹到上限限速跟随（不丢目标）；观测是**借深度**的 ⇒ 超上限整帧 + # 不采信（2026-10-08 真机，假 k0 带着借来的深度、z 看着很合理，1s 把臂带走 + # 400mm）。判不采信时**不发跟踪点**，让伺服按"目标丢失"处理（原地不动）。 + step, why_step = self._slew.update( + np.asarray(p_cam, dtype=float).reshape(3)[:2], borrowed=borrowed + ) + if step is None: + self.get_logger().warn( + f"跟踪点判误检，本帧不发点（{why_step}）" + f" uv=({uv[0]:.0f},{uv[1]:.0f}) conf={best.conf:.2f}", + throttle_duration_sec=0.5, + ) + return + p_cam = np.asarray(p_cam, dtype=float).reshape(3).copy() + p_cam[0], p_cam[1] = float(step[0]), float(step[1]) + with self._lock: self.kf.advance(now) if self.kf.initialized: @@ -753,6 +1009,21 @@ class PerceptionNode(TolerantNode): dpx = float(np.linalg.norm(box_uv - self._cam_to_pixel(pred))) dz = abs(float(p_cam[2] - pred[2])) if z_ok else 0.0 if dpx > self.jump_px or (z_ok and dz > self.jump_z_m): + # ---- 守卫三：远框"重新捕获"要多帧确认 ---- + # 一帧的远框更可能是模型在别的东西上误报，而不是工件真被挪走了 + #（真机那串假框的 dpx 是 480/418/207，而且每帧换个地方）。 + ok_far, why_far = self._far.allow( + None if self.feature_mode != "keypoint" else tuple(uv), + dpx, + near_px=self.reacquire_far_px, + ) + if not ok_far: + self.get_logger().warn( + f"重新捕获未确认（{why_far}）conf={best.conf:.2f} —— " + "本帧不发点，不追这一帧的新位置", + throttle_duration_sec=0.5, + ) + return self.kf.reset(p_cam, now) self.get_logger().info( f"重新捕获 conf={best.conf:.2f} dpx={dpx:.0f}", @@ -774,6 +1045,9 @@ class PerceptionNode(TolerantNode): filt[2] = z_use self._last_filt = filt.copy() + if handoff: + # 画面里没有 conf≥门槛的目标：绿框可以留，三维点不许发 —— 伺服会拿去追。 + return self._publish_point(filt, stamp) self.get_logger().info( f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) " @@ -791,6 +1065,191 @@ class PerceptionNode(TolerantNode): return True return age < 0.25 + def _keypoint_feature(self, det): + """用 pose 模型的 ``keypoint_index``（默认 k0）当跟踪点。 + + 约定：k0 是跟踪孔，k1/k2 是对孔（见 ``pose_detector``）。 + 返回 ``PlaneFeature``，坐标系 / 语义和 plane 模式完全一致 —— 下游 + （卡尔曼、visual_servo）不需要知道这是关键点。 + + k0 掉信（置信度低 / 该处没深度）时**不许把跟踪点横移到框中心** —— + 那是另一个点，会瞬间平移一大截。正确做法是用 k0 相对工件框的**归一化偏移** + 在框上重建 k0：位置连续，工件动时还能跟着动。 + + 真机反例（2026-10-08，一阶段跑 150s 没锁上，arm 89）： + + 0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定（误差 3mm） + 0659.598 k0 置信度 0.01 < 0.30，本帧不用它 + 0659.604 feat_base=(0.675,-0.099) ← 兜底切到框中心，跟踪点横移 89mm + 0660.121 v=(-0.206,+0.400) ← 伺服以 0.45m/s 去追这个假点 + 0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，彻底丢失 + + 只有在"连一次可信的 k0 都没见过"（``_kpt_box_frac`` 为空）时才返回 None， + 交给调用方回落到框中心 —— 那是冷启动，没有更好的信息可用。 + """ + kpts = getattr(det, "kpts", None) + if kpts is None: + return None + kpts = np.asarray(kpts, dtype=float) + if kpts.ndim != 2 or kpts.shape[0] <= self.keypoint_index or kpts.shape[1] < 3: + return None + u, v, kc = (float(x) for x in kpts[self.keypoint_index][:3]) + + # 工件框（用来把 k0 的偏移归一化，以及掉信时重建） + box = getattr(det, "xyxy", None) + box = None if box is None else np.asarray(box, dtype=float).reshape(4) + cx = cy = bw = bh = None + if box is not None: + cx = 0.5 * (box[0] + box[2]) + cy = 0.5 * (box[1] + box[3]) + bw = abs(box[2] - box[0]) + bh = abs(box[3] - box[1]) + + finite = bool(np.isfinite(u) and np.isfinite(v) and np.isfinite(kc)) + # k0 的**像素位置**是否可信：置信度够 ≠ 位置对。2026-10-08 真机，模型在 + # uv=(12,30)/(76,75)/(143,92) 这些**贴画面边**的地方以 conf 0.53~0.73 报 k0 + #（真 k0 是 0.87，没有可分区间）—— 孔心长在工件上，不可能在画面边上。 + # 位置不可信不是"没有目标"，而是"本帧 k0 位置不能用"，交回下面的连续性路径。 + why_pos = ( + kpt_plausible( + u, v, box, *self._img_wh_or_zero(), border_px=self.keypoint_border_px + ) + if finite + else None + ) + borrowed = False + if finite and kc >= self.keypoint_min_conf and why_pos is None: + # k0 可信：位置就用 k0 自己 + uv = np.array([u, v], dtype=float) + z, znote = self._depth_at_uv(u, v) + if z is None: + # 位置可信、只是深度读不到：XY 仍然用 k0（不许横移），只借深度 + if box is None: + return None + z, note2 = self._depth_in_box(box) + if z is None: + return None + znote = f"{note2} 借深度" + borrowed = True + if box is not None and bw is not None and bw > 1e-6 and bh > 1e-6: + self._kpt_box_frac = np.array( + [(u - cx) / bw, (v - cy) / bh], dtype=float + ) + reason = "" + else: + # k0 不可信：按"相对框的偏移"重建，位置连续、且跟着框走。 + if not finite: + reason = "非有限值" + elif why_pos is not None: + # 置信度够但**位置**不合理（贴画面边 / 不在自己的框里） + reason = why_pos + else: + reason = f"置信度 {kc:.2f} < {self.keypoint_min_conf:.2f}" + if ( + self._kpt_box_frac is None + or box is None + or bw is None + or bw <= 1e-6 + or bh is None + or bh <= 1e-6 + ): + self.get_logger().warn( + f"k{self.keypoint_index} {reason}，且还没有可信历史 —— 本帧不用它", + throttle_duration_sec=1.0, + ) + return None + uv = np.array( + [cx + float(self._kpt_box_frac[0]) * bw, + cy + float(self._kpt_box_frac[1]) * bh], + dtype=float, + ) + z, znote = self._depth_at_uv(float(uv[0]), float(uv[1])) + if z is None: + z, note2 = self._depth_in_box(box) + if z is None: + return None + znote = f"{note2} 借深度" + borrowed = True + self.get_logger().warn( + f"k{self.keypoint_index} {reason} —— 按框上相对偏移重建跟踪点" + f"（uv=({uv[0]:.0f},{uv[1]:.0f})，框心=({cx:.0f},{cy:.0f})），不横移", + throttle_duration_sec=1.0, + ) + z = float(z) + return PlaneFeature( + uv=uv, + z=z, + xyz=self._pixel_to_cam(float(uv[0]), float(uv[1]), z), + n_inliers=1, + note=f"k{self.keypoint_index}保持({reason}) {znote} z={z:.3f}", + borrowed=borrowed, + ) + + z = float(z) + return PlaneFeature( + uv=uv, + z=z, + xyz=self._pixel_to_cam(float(uv[0]), float(uv[1]), z), + # 关键点特征没有"平面内点"的概念——支撑样本就它自己一个。 + # 这个字段目前没有下游消费者，给 1 是为了让日志里 n=1 说得通。 + n_inliers=1, + note=f"k{self.keypoint_index}({znote}) z={z:.3f}", + borrowed=borrowed, + ) + + def _img_wh_or_zero(self) -> tuple: + """当前彩色图 ``(w,h)``；还没收到图时给 ``(0,0)``（守卫会跳过边缘判据）。""" + wh = self._img_wh() + return (0, 0) if wh is None else (int(wh[0]), int(wh[1])) + + def _depth_at_uv(self, u: float, v: float) -> tuple: + """取 (u,v) 附近小窗口的中位深度（米）。``u,v`` 是**彩色图**像素。 + + 深度是 aligned-to-color，但分辨率不一定等于彩色图（D405 的 aligned + 深度会和彩色图不同尺寸），所以先按比例映射到深度图坐标 —— 这点和 + ``_depth_in_box`` 里的缩放保持一致，否则关键点深度会整体偏。 + """ + msg = self._latest_depth + if msg is None: + return None, "无深度" + if not self._depth_fresh(msg): + return None, "深度过期" + buf = self._decode_depth_m(msg) + if buf is None: + return None, "深度无效" + h, w = buf.shape + img = self._latest_image + if img is not None and (int(img.width) != w or int(img.height) != h): + u = u * (w / float(img.width)) + v = v * (h / float(img.height)) + win = self.keypoint_win_px + x0 = int(np.clip(round(u) - win, 0, w - 1)) + x1 = int(np.clip(round(u) + win, x0 + 1, w)) + y0 = int(np.clip(round(v) - win, 0, h - 1)) + y1 = int(np.clip(round(v) + win, y0 + 1, h)) + roi = buf[y0:y1, x0:x1].reshape(-1).astype(np.float32) + if roi.size == 0: + return None, "窗口越界" + enc = (msg.encoding or "").lower() + if enc in ("16uc1", "mono16"): + pos = roi[roi > 0] + if pos.size < 4: + return None, "该处无有效深度" + med = float(np.median(pos)) + # 和 _depth_in_box 同一套量纲判断（有的相机给 mm，有的给 m） + scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001) + z = med * scale + else: + vals = roi[np.isfinite(roi) & (roi > 0)] + if vals.size < 4: + return None, "该处无有效深度" + z = float(np.median(vals)) + if not (self.feature_min_range < z < self.max_depth): + # 太近 ⇒ 十有八九是爪上夹的通止规（规尖在相机前 ~0.10m，工件面 0.17~0.22m）。 + # 当"这里没有目标"处理，绝不把它当工件去伺服 —— 否则臂会去追自己的工具。 + return None, f"深度 {z:.3f}m 命中工具量程(<{self.feature_min_range:.2f}m)" + return z, f"({u:.0f},{v:.0f})" + def _workpiece_xyz(self, xyxy): msg = self._latest_depth img = self._latest_image @@ -811,7 +1270,7 @@ class PerceptionNode(TolerantNode): self._cx, self._cy, (int(img.width), int(img.height)), - min_depth=self.min_depth, + min_depth=self.feature_min_range, # 太近=爪上工具，见 feature_min_range_m max_depth=self.max_depth, work_z_max=self.work_z_max, band_m=self.plane_band_m, @@ -840,7 +1299,7 @@ class PerceptionNode(TolerantNode): self._cx, self._cy, (int(img.width), int(img.height)), - min_depth=self.min_depth, + min_depth=self.feature_min_range, # 太近=爪上工具，见 feature_min_range_m max_depth=self.max_depth, work_z_max=self.work_z_max, inlier_m=self.plane_inlier_m, @@ -862,6 +1321,139 @@ class PerceptionNode(TolerantNode): return float(self._last_good_z), f"{note} hold_near" return None, f"{note} no_depth" + def _publish_wrist_conf(self, best_conf: Optional[float]) -> None: + msg = Float32() + msg.data = float("nan") if best_conf is None else float(best_conf) + self._wrist_conf_pub.publish(msg) + + def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None: + pt = PointStamped() + pt.header.stamp = self.get_clock().now().to_msg() + pt.header.frame_id = self.base_frame or "base" + pt.point.x = float(p_base[0]) + pt.point.y = float(p_base[1]) + pt.point.z = float(p_base[2]) + self._overhead_k0_pub.publish(pt) + del is_k0 # 语义写在日志里；话题保持 PointStamped，避免再抄一份类型 + + def _submit_overhead_oneshot(self) -> None: + """把最新 D435 帧排进同一条 YOLO 队列。不在这里等结果 —— 等会把 D405 卡死。""" + if self._oneshot_inflight or not self.overhead_oneshot_enable: + return + now = time.monotonic() + if now - self._oneshot_t < self.overhead_cooldown_s: + return + img = self._oh_image + depth = self._oh_depth + if img is None or depth is None: + self.get_logger().warn( + "D405 无合格目标，但 D435 还没出图 —— 不编造点", + throttle_duration_sec=1.0, + ) + return + if (now - self._oh_image_mono) > self.overhead_frame_max_age_s: + self.get_logger().warn( + f"D435 彩图过期 {(now - self._oh_image_mono)*1000:.0f}ms —— 不反投影", + throttle_duration_sec=1.0, + ) + return + if (now - self._oh_depth_mono) > self.overhead_frame_max_age_s: + self.get_logger().warn( + f"D435 深度过期 {(now - self._oh_depth_mono)*1000:.0f}ms —— 不反投影", + throttle_duration_sec=1.0, + ) + return + if self._oh_fx <= 1.0: + self.get_logger().warn( + "D435 内参还没到（camera_info）—— 不反投影", + throttle_duration_sec=1.0, + ) + return + if self._T_oh is None: + self.get_logger().error("眼在手外 T 没加载 —— 一次性定位拒绝") + return + try: + bgr = _bgr_copy(img) + small, scale = self._prepare_infer(bgr) + self._infer_meta[_ONESHOT_SEQ] = scale + if not self.detector.submit(small, _ONESHOT_SEQ): + return + except Exception as exc: # noqa: BLE001 + self.get_logger().warn( + f"D435 一次性提交失败：{exc}", throttle_duration_sec=2.0 + ) + return + self._oneshot_inflight = True + self._oneshot_img = img + self._oneshot_depth = depth + self.get_logger().info("D435 一次性已提交（不打断 D405 队列）") + + def _finish_overhead_oneshot(self, dets, scale: float) -> None: + """YOLO 返回的顶视结果 → 基座系 K0。失败不发假点。""" + self._oneshot_inflight = False + now = time.monotonic() + self._oneshot_t = now + img = self._oneshot_img + depth = self._oneshot_depth + self._oneshot_img = None + self._oneshot_depth = None + if dets is INFER_BUSY or dets is None or img is None or depth is None: + return + if scale != 1.0: + for d in dets: + d.xyxy = d.xyxy * scale + if getattr(d, "kpts", None) is not None: + d.kpts = np.asarray(d.kpts, dtype=float).copy() + d.kpts[:, :2] *= scale + det = select_oneshot_det(dets, self.overhead_oneshot_conf) + if det is None: + self._oneshot_t = now + self.get_logger().warn( + f"D435 一次性：画面里没有 conf≥{self.overhead_oneshot_conf:.2f} 的工件", + throttle_duration_sec=1.0, + ) + return + raw = self._decode_depth_m(depth) + depth_m = depth_raw_to_m(raw, depth.encoding or "") + if depth_m is None: + self._oneshot_t = now + self.get_logger().warn( + "D435 深度无法转成米 —— 不发点", throttle_duration_sec=1.0 + ) + return + color_wh = (int(img.width), int(img.height)) + depth_wh = (int(depth_m.shape[1]), int(depth_m.shape[0])) + got = oneshot_k0_base( + det, + depth_m, + fx=self._oh_fx, + fy=self._oh_fy, + cx=self._oh_cx, + cy=self._oh_cy, + T_cam_to_base=self._T_oh, + min_z_m=self.overhead_min_depth, + max_z_m=self.overhead_max_depth, + kpt_index=self.keypoint_index, + kpt_min_conf=self.keypoint_min_conf, + min_det_conf=self.overhead_oneshot_conf, + color_wh=color_wh, + depth_wh=depth_wh, + ) + self._oneshot_t = now + if got is None: + self.get_logger().warn( + "D435 一次性：深度或 k0 不可用，不发点", + throttle_duration_sec=1.0, + ) + return + p_base, is_k0 = got + self._publish_overhead_k0(p_base, is_k0) + self.get_logger().info( + f"D435 一次性定位 {'k0' if is_k0 else '框心（顶视看不见 k0）'} " + f"base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) " + f"conf={float(det.conf):.2f}" + ) + def _publish_point(self, filt: np.ndarray, stamp) -> None: pt = PointStamped() pt.header.stamp = stamp diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index d4eab0a..df5c7ff 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -205,7 +205,6 @@ class AssemblyPipeline(Stage3Search): "enable_control:=true", "publish_handeye_tf:=false", f"ee_z_m:={lock_z:.3f}", - "lock_hold_sec:=0.6", f"use_sim_time:={str(use_sim).lower()}", ] self.get_logger().info( diff --git a/src/arm_application/arm_application/plane_feature.py b/src/arm_application/arm_application/plane_feature.py index 84c43ad..1cc1eae 100644 --- a/src/arm_application/arm_application/plane_feature.py +++ b/src/arm_application/arm_application/plane_feature.py @@ -21,6 +21,11 @@ class PlaneFeature: xyz: np.ndarray # (3,) 相机坐标系下的立体点云质心 n_inliers: int note: str + # 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。 + # 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的 + # 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的 + # 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。 + borrowed: bool = False def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]: diff --git a/src/arm_application/arm_application/pose_detector.py b/src/arm_application/arm_application/pose_detector.py new file mode 100644 index 0000000..c050b3a --- /dev/null +++ b/src/arm_application/arm_application/pose_detector.py @@ -0,0 +1,479 @@ +#!/usr/bin/env python3 +"""YOLO **pose** 模型的推理封装（自解码，不经过 ultralytics 的 pose 后处理）。 + +背景：为什么会多出这一个模块 +-------------------------------- +`gongjian0929` 这类权重是 ``task=pose``：它输出**一个工件框 + K 个关键点**。 +本项目 K=3，就是现场说的 k0 / k1 / k2（k0 跟踪、k1/k2 对孔）。 + +ultralytics 走不通，两条路都实测过（2026-09-29）： + +1. ``YOLO(engine, task="detect")`` —— 按检测去解 14 通道输出，结果吐出 300 个 + 垃圾框，``conf`` 直接是坐标值（600+），全被算成 ``workpiece`` 类。 + 跟踪会锁到空气上 —— 真机上这比不跑更危险。 +2. ``YOLO(engine, task="pose")`` —— 直接崩：``negative dimension -983``。 + 根因是 ``scripts/weights.py`` 用**裸 TensorRT API** 构建 engine，engine 不带 + metadata；ultralytics 拿不到 ``kpt_shape``，后处理就错位了。 + （TensorRT 10.13 的 Python API 确实没有任何 metadata 接口 —— 见 + ``models/README.md`` 第 3 节。） + +所以这里自己解码。输出布局是 YOLO pose 的固定约定:: + + output0 : (1, 4 + nc + 3*K, A) # A = anchor 数（640 输入时是 8400） + ch 0..3 : cx, cy, w, h # 输入尺度下的像素 + ch 4.. : 每类置信度（nc 个） + 之后 : K 组 (x, y, conf) + +关键点是**同一个工件框的属性**，不是独立目标 —— 所以它们不参与 NMS 的竞争， +只跟着自己那个框一起被选出/淘汰。这点和"每个孔一个类"的检测模型不一样。 + +后端 +---- +- ``.engine``：TensorRT 10 的 ``set_tensor_address`` + ``execute_async_v3``。 + 用 torch 张量当设备缓冲、torch 的当前流当执行流 —— 不需要 pycuda。 + 实测 ~1.5 ms/帧（640 输入，FP16 engine）。 +- ``.onnx``：onnxruntime。**本环境没有 CUDA provider**（只有 CPU）， + 所以 ONNX 只是兜底，真机请用 engine。 +""" + +from __future__ import annotations + +import ast +import logging +from pathlib import Path +from typing import Any, List, Optional, Sequence, Tuple, Union + +import cv2 +import numpy as np + +from .yolo_detector import Detection, resolve_class_labels + +logger = logging.getLogger(__name__) + +# ultralytics 的默认填充值。改了它并不会错 —— 只是输入分布和训练时不一致， +# 精度会掉一点，所以别改。 +LETTERBOX_FILL = 114 + +# 关键点语义（现场约定）：k0 是跟踪孔，k1/k2 是对孔。 +# 默认值写在这里，site.yaml 可以覆盖（见 arm_application/pose_detector 的调用方）。 +TRACK_KPT_INDEX = 0 +HOLE_KPT_INDICES: Tuple[int, int] = (1, 2) + + +# --------------------------------------------------------------------------- # +# 模型信息：engine 没有 metadata，就从旁边同名的 .onnx 读 +# --------------------------------------------------------------------------- # +def _parse_python_literal(raw: Any, default: Any = None) -> Any: + """ONNX metadata 里存的是 python repr（单引号、嵌套 list/dict），要 literal_eval。""" + if raw is None: + return default + if isinstance(raw, (dict, list, tuple, int)): + return raw + try: + return ast.literal_eval(str(raw)) + except (ValueError, SyntaxError): + return default + + +def read_pose_info(weights: Union[str, Path]) -> Optional[dict]: + """读同名 ``.onnx`` 的 metadata，判断这是不是一个 pose 模型。 + + engine 本身读不到任何东西（TensorRT 10 的 Python API 没有 metadata 接口）， + 但 ``.onnx`` 一定在它旁边（``resolve_weights`` 保证同目录同主干）， + 所以统一以 ONNX 为准。 + + 返回 ``None`` = 不是 pose 模型（调用方应回落到普通检测器）。 + 返回 dict 时至少含 ``kpt_count`` / ``nc`` / ``names``。 + """ + p = Path(weights).expanduser() + onnx_path = p.with_suffix(".onnx") + if not onnx_path.is_file(): + return None + try: + import onnx + except Exception: # noqa: BLE001 + logger.warning("没装 onnx，无法判断 %s 是不是 pose 模型", p.name) + return None + try: + meta = {m.key: m.value for m in onnx.load(str(onnx_path)).metadata_props} + except Exception as exc: # noqa: BLE001 + logger.warning("读 %s 的 metadata 失败：%s", onnx_path.name, exc) + return None + + task = str(meta.get("task", "")).strip().lower() + kpt_shape = _parse_python_literal(meta.get("kpt_shape")) + if task != "pose" and not kpt_shape: + return None + + kpt_count = 0 + if isinstance(kpt_shape, (list, tuple)) and len(kpt_shape) >= 1: + try: + kpt_count = int(kpt_shape[0]) + except (TypeError, ValueError): + kpt_count = 0 + if kpt_count <= 0: + # metadata 缺 kpt_shape 也不能猜 —— 猜错就是"静默错位"，比报错难查得多。 + logger.warning("%s 标记为 pose 但读不到 kpt_shape，按非 pose 处理", onnx_path.name) + return None + + names = _parse_python_literal(meta.get("names"), default={}) or {} + kpt_names = _parse_python_literal(meta.get("kpt_names"), default={}) or {} + + nc = 0 + if isinstance(names, dict) and names: + nc = max(int(k) for k in names) + 1 if all( + str(k).lstrip("-").isdigit() for k in names + ) else len(names) + if nc <= 0 and isinstance(names, (list, tuple)): + nc = len(names) + + return { + "task": "pose", + "kpt_count": kpt_count, + "kpt_names": kpt_names, + "nc": nc, + "names": names, + "model": str(meta.get("description") or p.name), + } + + +def is_pose_model(weights: Union[str, Path]) -> bool: + return read_pose_info(weights) is not None + + +# --------------------------------------------------------------------------- # +# 预处理 / 后处理（纯 numpy，可离线单测） +# --------------------------------------------------------------------------- # +def letterbox( + img: np.ndarray, + new_shape: int = 640, + color: Tuple[int, int, int] = (LETTERBOX_FILL,) * 3, +) -> Tuple[np.ndarray, float, Tuple[float, float]]: + """等比缩放 + 居中填充到 ``new_shape``（和 ultralytics 推理时一致）。 + + 返回 ``(padded, gain, (dw, dh))``：``dw/dh`` 是**填充量**，还原坐标时减掉它。 + """ + h, w = img.shape[:2] + gain = min(new_shape / h, new_shape / w) + new_unpad = (max(1, int(round(w * gain))), max(1, int(round(h * gain)))) + if (w, h) != new_unpad: + img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR) + dw = (new_shape - new_unpad[0]) / 2.0 + dh = (new_shape - new_unpad[1]) / 2.0 + top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1)) + left, right = int(round(dw - 0.1)), int(round(dw + 0.1)) + if top or bottom or left or right: + img = cv2.copyMakeBorder( + img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color + ) + return img, float(gain), (float(dw), float(dh)) + + +def nms_xyxy( + boxes: np.ndarray, + scores: np.ndarray, + iou_thr: float, + max_det: int = 20, +) -> np.ndarray: + """纯 numpy 的 NMS，返回保留下来的下标（按分数从高到低）。""" + if boxes.size == 0: + return np.zeros((0,), dtype=int) + x1, y1, x2, y2 = boxes[:, 0], boxes[:, 1], boxes[:, 2], boxes[:, 3] + areas = np.maximum(0.0, x2 - x1) * np.maximum(0.0, y2 - y1) + order = scores.argsort()[::-1] + keep: List[int] = [] + while order.size > 0 and len(keep) < int(max_det): + i = int(order[0]) + keep.append(i) + if order.size == 1: + break + rest = order[1:] + ix1 = np.maximum(x1[i], x1[rest]) + iy1 = np.maximum(y1[i], y1[rest]) + ix2 = np.minimum(x2[i], x2[rest]) + iy2 = np.minimum(y2[i], y2[rest]) + inter = np.maximum(0.0, ix2 - ix1) * np.maximum(0.0, iy2 - iy1) + union = areas[i] + areas[rest] - inter + iou = np.where(union > 1e-9, inter / np.maximum(union, 1e-9), 0.0) + order = rest[iou <= float(iou_thr)] + return np.asarray(keep, dtype=int) + + +# --------------------------------------------------------------------------- # +# 后端：TensorRT engine / ONNX Runtime +# --------------------------------------------------------------------------- # +class _TrtRunner: + """TensorRT 10 执行器：torch 张量当设备缓冲，torch 当前流当执行流。 + + 不用 pycuda —— 环境里只有 torch，而 ``set_tensor_address`` + + ``execute_async_v3`` 正好够用。 + """ + + def __init__(self, engine_path: Path, device: str = "0") -> None: + import tensorrt as trt + import torch + + self._torch = torch + idx = 0 + dev = str(device or "").strip().lower() + if dev and dev not in ("auto", "cuda", "gpu"): + tail = dev.split(":")[-1] + if tail.isdigit(): + idx = int(tail) + if not torch.cuda.is_available(): + raise RuntimeError("backend=tensorrt 但 torch 看不到 CUDA") + self._device = torch.device(f"cuda:{idx}") + + trt_logger = trt.Logger(trt.Logger.ERROR) + with open(engine_path, "rb") as f: + engine = trt.Runtime(trt_logger).deserialize_cuda_engine(f.read()) + if engine is None: + raise RuntimeError( + f"engine 反序列化失败：{engine_path}。" + "engine 和 TensorRT 版本绑死，重装环境后要重新导出" + "（bash scripts/weights.sh doctor --check-engines 可查）" + ) + self._engine = engine + self._ctx = engine.create_execution_context() + + names = [engine.get_tensor_name(i) for i in range(engine.num_io_tensors)] + ins = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.INPUT] + outs = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.OUTPUT] + if len(ins) != 1 or len(outs) != 1: + raise RuntimeError(f"pose engine 期望 1 进 1 出，实际 in={ins} out={outs}") + self.in_name, self.out_name = ins[0], outs[0] + + with torch.cuda.device(self._device): + self._x = torch.zeros( + tuple(engine.get_tensor_shape(self.in_name)), + dtype=torch.float32, + device=self._device, + ) + self._y = torch.zeros( + tuple(engine.get_tensor_shape(self.out_name)), + dtype=torch.float32, + device=self._device, + ) + self._ctx.set_tensor_address(self.in_name, self._x.data_ptr()) + self._ctx.set_tensor_address(self.out_name, self._y.data_ptr()) + self.out_shape = tuple(engine.get_tensor_shape(self.out_name)) + + def run(self, chw: np.ndarray) -> np.ndarray: + torch = self._torch + with torch.cuda.device(self._device): + self._x.copy_(torch.from_numpy(np.ascontiguousarray(chw)).to(self._device)) + self._ctx.execute_async_v3(torch.cuda.current_stream().cuda_stream) + torch.cuda.current_stream().synchronize() + return self._y.cpu().numpy() + + +class _OnnxRunner: + """onnxruntime 兜底。本环境只有 CPU provider —— 真机请用 .engine。""" + + def __init__(self, onnx_path: Path) -> None: + import onnxruntime as ort + + providers = ort.get_available_providers() + if "CUDAExecutionProvider" in providers: + chosen = ["CUDAExecutionProvider", "CPUExecutionProvider"] + else: + logger.warning( + "onnxruntime 没有 CUDA provider，%s 会跑在 CPU 上（慢）。" + "真机请用同名的 .engine", + onnx_path.name, + ) + chosen = ["CPUExecutionProvider"] + self._sess = ort.InferenceSession(str(onnx_path), providers=chosen) + self.in_name = self._sess.get_inputs()[0].name + self.out_shape = tuple(self._sess.get_outputs()[0].shape) + + def run(self, chw: np.ndarray) -> np.ndarray: + return self._sess.run(None, {self.in_name: chw[None].astype(np.float32)})[0] + + +# --------------------------------------------------------------------------- # +# 检测器 +# --------------------------------------------------------------------------- # +class PoseDetector: + """把 pose 模型包装成和 ``YoloDetector`` 一样的 ``detect()/infer()`` 接口。 + + 多出来的一件事：每个 ``Detection`` 带上 ``kpts``（``(K, 3)`` 的 + ``x, y, conf``，原图像素坐标）。 + """ + + def __init__( + self, + weights: Union[str, Path], + *, + conf: float = 0.25, + iou: float = 0.45, + imgsz: int = 640, + device: str = "0", + class_ids: Optional[Sequence[int]] = None, + class_names: Optional[Sequence[str]] = None, + max_det: int = 20, + ) -> None: + path = Path(weights).expanduser() + if not path.is_file(): + raise FileNotFoundError(f"找不到 pose 权重：{path}") + + info = read_pose_info(path) or {} + self.kpt_count = int(info.get("kpt_count") or 0) + if self.kpt_count <= 0: + raise RuntimeError(f"{path.name} 不是可识别的 pose 模型（缺 kpt_shape）") + self.weights_path = str(path) + self.conf = float(conf) + self.iou = float(iou) + self.imgsz = int(imgsz) + self.max_det = int(max_det) + + if path.suffix.lower() in {".engine", ".trt"}: + self._runner = _TrtRunner(path, device) + self.backend = "tensorrt" + else: + self._runner = _OnnxRunner(path) + self.backend = "onnx" + + # 类别标签仍然走统一入口（旁挂 .labels.yaml 优先）。 + # engine 没有 metadata，所以把 ONNX 里的 names 当"模型内嵌"传进去。 + names = info.get("names") or {} + + class _Meta: # 只为 resolve_class_labels 能读到 ONNX 内嵌标签 + pass + + meta = _Meta() + meta.names = names + meta.nc = int(info.get("nc") or 0) + self._labels = resolve_class_labels(path, meta) + + self.class_ids = set(int(v) for v in class_ids) if class_ids else None + self.class_names = ( + {str(n).strip().lower() for n in class_names if str(n).strip()} + if class_names + else None + ) + self._warmed = False + + # -- 对外 --------------------------------------------------------------- + def warmup(self, *, half: bool = False) -> None: + del half # engine 已是 FP16；ONNX 这边没有 half 的开关 + if self._warmed: + return + # 先置位：infer() 开头会检查 _warmed，不置位就会反过来再调 warmup。 + self._warmed = True + try: + self.infer(np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8)) + except Exception: + self._warmed = False + raise + + def detect( + self, + image_bgr: np.ndarray, + conf: float | None = None, + iou: float | None = None, + imgsz: int | None = None, + **_kwargs, + ) -> List[Detection]: + """``detect_holes`` 用的别名接口（xyxy + conf + kpts）。""" + if conf is not None: + self.conf = float(conf) + if iou is not None: + self.iou = float(iou) + # imgsz 在这里**不能**改：engine 的输入尺寸是编译期定死的。 + if imgsz is not None and int(imgsz) != self.imgsz: + logger.warning( + "pose engine 输入固定 %d，忽略调用方的 imgsz=%s", self.imgsz, imgsz + ) + return self.infer(image_bgr) + + def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]: + del half, seq + if image_bgr is None or getattr(image_bgr, "size", 0) == 0: + return [] + if not self._warmed: + self.warmup() + padded, gain, (dw, dh) = letterbox(image_bgr, self.imgsz) + chw = np.ascontiguousarray( + padded[:, :, ::-1].transpose(2, 0, 1), dtype=np.float32 + ) / 255.0 + out = self._runner.run(chw) + return self._decode(out, gain, dw, dh) + + # -- 解码 --------------------------------------------------------------- + def _decode( + self, raw: np.ndarray, gain: float, dw: float, dh: float + ) -> List[Detection]: + o = np.asarray(raw, dtype=np.float32)[0] # (C, A) + ch, n_anchor = o.shape + k = self.kpt_count + nc = int(ch - 4 - 3 * k) + if nc < 1: + raise RuntimeError( + f"pose 输出通道数不对：C={ch}，按 kpt={k} 反推 nc={nc}。" + "引擎/onnx 与 kpt_shape 对不上" + ) + + scores = o[4 : 4 + nc] # (nc, A) + cls_ids = scores.argmax(axis=0) # (A,) + confs = scores[cls_ids, np.arange(n_anchor)] + + keep = confs >= self.conf + if not np.any(keep): + return [] + xywh = o[0:4, keep].T # (N, 4) + confs = confs[keep] + cls_ids = cls_ids[keep] + kpts = o[4 + nc :, keep].reshape(k, 3, -1).transpose(2, 0, 1) # (N, k, 3) + + xyxy = np.empty_like(xywh) + xyxy[:, 0] = xywh[:, 0] - 0.5 * xywh[:, 2] + xyxy[:, 1] = xywh[:, 1] - 0.5 * xywh[:, 3] + xyxy[:, 2] = xywh[:, 0] + 0.5 * xywh[:, 2] + xyxy[:, 3] = xywh[:, 1] + 0.5 * xywh[:, 3] + + keep_i = nms_xyxy(xyxy, confs, self.iou, self.max_det) + if keep_i.size == 0: + return [] + xyxy, confs, cls_ids, kpts = xyxy[keep_i], confs[keep_i], cls_ids[keep_i], kpts[keep_i] + + # 去 letterbox 的填充，再还原到原图尺度 + xyxy = xyxy.astype(float) + xyxy[:, [0, 2]] = (xyxy[:, [0, 2]] - dw) / gain + xyxy[:, [1, 3]] = (xyxy[:, [1, 3]] - dh) / gain + kpts = kpts.astype(float) + kpts[:, :, 0] = (kpts[:, :, 0] - dw) / gain + kpts[:, :, 1] = (kpts[:, :, 1] - dh) / gain + + out: List[Detection] = [] + for i in range(len(xyxy)): + cid = int(cls_ids[i]) + name = self._labels.get(cid) + if self.class_ids is not None and cid not in self.class_ids: + continue + if self.class_names is not None and name.lower() not in self.class_names: + continue + out.append( + Detection( + xyxy=xyxy[i], + conf=float(confs[i]), + cls_id=cid, + cls_name=name, + kpts=kpts[i].copy(), + ) + ) + out.sort(key=lambda d: d.conf, reverse=True) + return out + + def close(self) -> None: + return None + + +def keypoint_roles(kpt_count: int) -> Tuple[int, Tuple[int, ...]]: + """返回 ``(跟踪点下标, 孔下标元组)``，供调用方统一口径。 + + 现场约定：k0 跟踪，k1/k2 对孔。模型关键点少于 3 个时只返回存在的部分。 + """ + track = TRACK_KPT_INDEX if kpt_count > TRACK_KPT_INDEX else -1 + holes = tuple(i for i in HOLE_KPT_INDICES if i < kpt_count) + return track, holes diff --git a/src/arm_application/arm_application/realsense_view.py b/src/arm_application/arm_application/realsense_view.py index b98dceb..91c5351 100644 --- a/src/arm_application/arm_application/realsense_view.py +++ b/src/arm_application/arm_application/realsense_view.py @@ -62,10 +62,12 @@ class RealsenseView(TolerantNode): super().__init__('realsense_view') self.declare_parameter('camera_name', 'd405') self.declare_parameter('overlay_hold_sec', 0.10) + self.declare_parameter('overlay_detection', True) self.declare_parameter('view_hz', 20.0) self.declare_parameter('hud_label', '') self._name = str(self.get_parameter('camera_name').value).strip().strip('/') self._hold = max(0.0, float(self.get_parameter('overlay_hold_sec').value)) + self._overlay_detection = bool(self.get_parameter('overlay_detection').value) self._hud = str(self.get_parameter('hud_label').value or '').strip() self._color: Optional[Image] = None @@ -102,9 +104,10 @@ class RealsenseView(TolerantNode): depth=5, reliability=ReliabilityPolicy.RELIABLE, ) - self.create_subscription( - Float32MultiArray, 'workpiece/detection', self._on_det, det_qos - ) + if self._overlay_detection: + self.create_subscription( + Float32MultiArray, 'workpiece/detection', self._on_det, det_qos + ) # stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示 self.create_subscription( Image, '/stage2/overlay', self._on_stage2_overlay, qos_profile_sensor_data diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index b31462f..c56e30f 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -22,7 +22,7 @@ from typing import Deque, List, Optional import numpy as np import rclpy from geometry_msgs.msg import PointStamped, Twist -from std_msgs.msg import Bool +from std_msgs.msg import Bool, Float32 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -38,8 +38,16 @@ from arm_application.geom_ik import ( load_handeye_T, quat_xyzw_to_R, ) +from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T +from arm_application.overhead_oneshot import StillLock, wrist_needs_handoff from arm_application.pid_controller import PidController from arm_application.kalman_filter import KalmanFilter3D +from arm_application.feature_jump_guard import ( + ACCEPT, + HOLD, + REJECT, + FeatureJumpGate, +) from arm_application.site_io import ( base_frame_default, ee_frame_default, @@ -79,6 +87,8 @@ class ServoNode(TolerantNode): # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。 # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。 self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml") + # eye_in_hand = cam→法兰；eye_to_hand = cam→基座。写错会把固定相机当腕部用。 + self.declare_parameter("handeye_mode", "eye_in_hand") self.declare_parameter("use_static_handeye_tf", False) self.declare_parameter("handeye_child_frame", "d405_color_optical_frame") @@ -93,6 +103,12 @@ class ServoNode(TolerantNode): self.declare_parameter("predict_sec", 0.10) self.declare_parameter("predict_v_max", 0.35) self.declare_parameter("predict_jump_m", 0.08) + # 大跳变要连续多少帧指向同一新位置才采信（见 feature_jump_guard 的注释）。 + # 30Hz 下 3 帧 ≈ 0.1s：真的重定位 0.1s 就被认，假点（乱跳）永远认不了。 + self.declare_parameter("jump_confirm_frames", 3) + # 特征点位置合理性：必须落在工作盒 XY 内（含此余量 [m]）。跑出去的点几乎必然 + # 是误检（见 feature_jump_guard 的注释），当"没有目标"处理。 + self.declare_parameter("feat_ws_margin_m", 0.05) # --- 看门狗（丢失目标即硬停） --- self.declare_parameter("watchdog_timeout_sec", 1.2) @@ -115,6 +131,10 @@ class ServoNode(TolerantNode): self.declare_parameter("deadzone_z_m", 0.012) self.declare_parameter("v_max_xy", 0.06) self.declare_parameter("v_max_z", 0.04) + # 远处限速（防"追着假目标把臂带走"）：目标离得超过 slow_far_m 时 XY 限速到 + # v_slow_xy。真机 2026-10-08：0.7s 内被假 k0 从 home 带走 400mm 到工作盒角落。 + self.declare_parameter("slow_far_m", 0.05) + self.declare_parameter("v_slow_xy", 0.08) self.declare_parameter("a_max", 0.45) self.declare_parameter("lost_timeout_sec", 1.2) # 别名 / 用时间戳年龄兜底 self.declare_parameter("e_xy_clip_m", 0.080) # 误差绝不按大于 8 cm 处理 @@ -162,6 +182,13 @@ class ServoNode(TolerantNode): self.declare_parameter("ki_hover", 0.8) # 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True self.declare_parameter("lock_hold_sec", 2.0) + # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。 + self.declare_parameter("lock_still_sec", -1.0) + self.declare_parameter("lock_still_move_m", -1.0) + self.declare_parameter("wrist_handoff_conf", -1.0) + self.declare_parameter("overhead_hold_s", -1.0) + self.declare_parameter("overhead_k0_topic", "visual_servo/overhead_k0") + self.declare_parameter("wrist_conf_topic", "visual_servo/wrist_conf") prefix = str(self.get_parameter("joint_prefix").value or "") base = list( @@ -186,12 +213,17 @@ class ServoNode(TolerantNode): self.predict_sec = float(self.get_parameter("predict_sec").value) self.predict_v_max = float(self.get_parameter("predict_v_max").value) self.predict_jump_m = float(self.get_parameter("predict_jump_m").value) + self.jump_confirm_frames = max(1, int(self.get_parameter("jump_confirm_frames").value)) + self.feat_ws_margin_m = float(self.get_parameter("feat_ws_margin_m").value) self.watchdog_timeout = float(self.get_parameter("watchdog_timeout_sec").value) self.stale_hold_sec = float(self.get_parameter("stale_hold_sec").value) self.deadzone_m = float(self.get_parameter("deadzone_m").value) self.deadzone_z_m = float(self.get_parameter("deadzone_z_m").value) self.v_max_xy = float(self.get_parameter("v_max_xy").value) self.v_max_z = float(self.get_parameter("v_max_z").value) + # 远处限速：目标离得比 slow_far_m 远时，XY 速度降到 v_slow_xy（见 hover 段注释） + self.slow_far_m = float(self.get_parameter("slow_far_m").value) + self.v_slow_xy = float(self.get_parameter("v_slow_xy").value) self.a_max = float(self.get_parameter("a_max").value) self.lost_timeout = float(self.get_parameter("lost_timeout_sec").value) self.e_xy_clip = float(self.get_parameter("e_xy_clip_m").value) @@ -235,6 +267,24 @@ class ServoNode(TolerantNode): self.seek_timeout = float(self.get_parameter("seek_timeout_sec").value) self.ki_hover = float(self.get_parameter("ki_hover").value) self.lock_hold_sec = float(self.get_parameter("lock_hold_sec").value) + self.lock_still_sec = float(self.get_parameter("lock_still_sec").value) + self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value) + self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value) + self.overhead_hold_s = float(self.get_parameter("overhead_hold_s").value) + self._hybrid = ( + self.lock_still_sec >= 0.0 + and self.lock_still_move_m >= 0.0 + and self.wrist_handoff_conf >= 0.0 + and self.overhead_hold_s >= 0.0 + ) + self._still = StillLock( + still_sec=max(0.0, self.lock_still_sec), + still_move_m=max(0.0, self.lock_still_move_m), + ) + self._track_source = "wrist" # wrist | overhead + self._wrist_conf: Optional[float] = None + self._wrist_conf_t = -1.0 + self._overhead_t = -1.0e9 i_sep_pos = float(self.get_parameter("i_sep_pos_m").value) self.i_sep_ang = float(self.get_parameter("i_sep_ang_rad").value) ws_min = list( @@ -251,17 +301,42 @@ class ServoNode(TolerantNode): self.T_cam_to_gripper = load_handeye_T( str(self.get_parameter("handeye_file").value) ) + self.handeye_mode = str( + self.get_parameter("handeye_mode").value or "eye_in_hand" + ).strip().lower() + if self.handeye_mode not in ("eye_in_hand", "eye_to_hand"): + raise RuntimeError( + f"handeye_mode 只能是 eye_in_hand 或 eye_to_hand，" + f"收到 {self.handeye_mode!r}" + ) + if self.handeye_mode == "eye_to_hand" and self.xy_track == "lookat": + raise RuntimeError( + "眼在手外相机固定，lookat 没有意义。xy_track 必须是 hover" + ) + if self.handeye_mode == "eye_in_hand" and not self._hybrid: + raise RuntimeError( + "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / lock_still_move_m / " + "wrist_handoff_conf / overhead_hold_s。收到哨兵 -1 = 配置没接线，" + "不许用代码默认 3 秒假装已经静止判据。" + ) R_he = np.asarray(self.T_cam_to_gripper[:3, :3], dtype=float) t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float) axis_z = float(np.clip(R_he[2, 2], -1.0, 1.0)) self._cam_tilt_deg = float(np.degrees(np.arccos(axis_z))) he_path = str(self.get_parameter("handeye_file").value) - self.get_logger().warn( - f"HANDEYE yaml={he_path} " - f"cam_in_flange=({t_he[0]*1000:.1f},{t_he[1]*1000:.1f},{t_he[2]*1000:.1f})mm " - f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg " - f"— 悬停使用这个 T，而不是用 TF 查询" - ) + if self.handeye_mode == "eye_to_hand": + self.get_logger().warn( + f"HANDEYE yaml={he_path} mode=eye_to_hand " + f"cam_in_base=({t_he[0]:.3f},{t_he[1]:.3f},{t_he[2]:.3f})m " + f"— 悬停用 cam→基座，不乘法兰" + ) + else: + self.get_logger().warn( + f"HANDEYE yaml={he_path} " + f"cam_in_flange=({t_he[0]*1000:.1f},{t_he[1]*1000:.1f},{t_he[2]*1000:.1f})mm " + f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg " + f"— 悬停使用这个 T，而不是用 TF 查询" + ) self.pid_x = PidController( float(self.get_parameter("kp_xy").value), @@ -312,6 +387,8 @@ class ServoNode(TolerantNode): self._p_feat_base: Optional[np.ndarray] = None self._he_cmp_done = False self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4) + # 大跳变确认门限：单帧不采信（见 feature_jump_guard 的注释与真机反例）。 + self._jump_gate = FeatureJumpGate(self.predict_jump_m, self.jump_confirm_frames) # 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。 self._xy_armed = bool(self.ee_z_m <= 0.0) # Seek（搜索）状态：在锁定高度且没有目标时扫描工件。 @@ -334,6 +411,23 @@ class ServoNode(TolerantNode): point_qos, callback_group=self._state_cbg, ) + if self._hybrid: + oh_topic = str(self.get_parameter("overhead_k0_topic").value) + conf_topic = str(self.get_parameter("wrist_conf_topic").value) + self.create_subscription( + PointStamped, + oh_topic, + self._on_overhead_k0, + point_qos, + callback_group=self._state_cbg, + ) + self.create_subscription( + Float32, + conf_topic, + self._on_wrist_conf, + 5, + callback_group=self._state_cbg, + ) self.create_subscription( JointState, "joint_states", @@ -387,8 +481,66 @@ class ServoNode(TolerantNode): def _on_js(self, msg: JointState) -> None: self._last_js = msg + def _on_wrist_conf(self, msg: Float32) -> None: + c = float(msg.data) + with self._lock: + self._wrist_conf = None if not np.isfinite(c) else c + self._wrist_conf_t = time.monotonic() + + def _on_overhead_k0(self, msg: PointStamped) -> None: + """D435 一次性：点已经在基座系。再乘腕部 T 会差几十厘米还看起来合理。""" + if not self._hybrid: + return + fid = str(msg.header.frame_id or "").strip() + if fid and self.base_frame and fid != self.base_frame: + self.get_logger().error( + f"overhead_k0 的 frame_id={fid!r} 不是基座 {self.base_frame!r} —— 丢弃，" + "不许当相机系点去乘手眼 T" + ) + return + p_base = np.array( + [float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float + ) + if not np.all(np.isfinite(p_base)): + return + if not self._feat_workspace_ok(p_base): + self.get_logger().warn( + f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 —— 丢弃", + throttle_duration_sec=1.0, + ) + return + now_m = time.monotonic() + now_sec = self.get_clock().now().nanoseconds * 1e-9 + with self._lock: + self._p_feat_base = p_base.copy() + self._p_meas[:] = 0.0 + self._p_meas[2] = self.z_ref + self._feat_kf.reset(p_base, now_m) + self._jump_gate.reset() + self._have_target = True + self._target_lost_flag = False + self._track_source = "overhead" + self._overhead_t = now_m + self._last_seen_time = now_m + self._stamp_sec = now_sec + self._seek_t0 = 0.0 + self._seek_warned = False + self._still.reset() + self.get_logger().info( + f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) " + f"—— 迁过去等 D405 重新看见" + ) + def _on_point(self, msg: PointStamped) -> None: """新的视觉测量 —— 给看门狗“上弦”并清除丢失标志。""" + if ( + self._hybrid + and self._wrist_conf is not None + and wrist_needs_handoff(self._wrist_conf, self.wrist_handoff_conf) + ): + # 已经收到腕部 conf 且低于门槛：这条 D405 点不能当跟踪源。 + # conf 还是 None = 感知还没报到，不要把开场几帧也丢掉。 + return p_cam = np.array( [float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float ) @@ -403,29 +555,38 @@ class ServoNode(TolerantNode): self._he_cmp_done = True self._log_handeye_tf_vs_yaml() now_m = time.monotonic() + rejected = False with self._lock: was_lost = self._target_lost_flag + if self._track_source == "overhead": + # 从顶视切回腕部：允许一次重定位，跳变门不拿顶视点比。 + was_lost = True + self._track_source = "wrist" self._p_meas[:] = p_cam if p_base is not None and at_lock and self._xy_armed: - if was_lost or (not self._feat_kf.initialized): - self._feat_kf.reset(p_base, now_m) - else: - dxy = float(np.hypot(p_base[0] - self._feat_kf.position[0], - p_base[1] - self._feat_kf.position[1])) - if dxy > self.predict_jump_m: - self._feat_kf.reset(p_base, now_m) - else: - self._feat_kf.advance(now_m) - self._feat_kf.update(p_base) - self._p_feat_base = p_base.copy() + verdict = self._update_feat_track(p_base, was_lost=was_lost, now_m=now_m) + if verdict == ACCEPT: + self._p_feat_base = np.asarray(p_base, dtype=float).copy() + elif verdict == HOLD: + # hold 时下游必须拿到"钉住的旧位置"：`_hover_vs_feature` 用的是 + # `_p_feat_base`，这里若照抄 p_base，就等于还是去追那个假点。 + self._p_feat_base = self._feat_kf.position.copy() + else: # REJECT：物理上不合理的点，当"没有目标"处理 + # 关键：必须同时清掉 `_have_target`。只清 `_p_feat_base` 不够 —— + # `_hover_vs_feature` 在 frozen=None 时会退回原始 `p_cam`， + # 那还是去追那个假点。 + self._p_feat_base = None + self._have_target = False + rejected = True self._stamp_sec = float(msg.header.stamp.sec) + 1e-9 * float( msg.header.stamp.nanosec ) self._last_seen_time = now_m - self._have_target = True - self._target_lost_flag = False - self._seek_t0 = 0.0 # 已捕获目标 —— 重置 seek，供下次使用 - self._seek_warned = False + if not rejected: + self._have_target = True + self._target_lost_flag = False + self._seek_t0 = 0.0 # 已捕获目标 —— 重置 seek，供下次使用 + self._seek_warned = False if msg.header.frame_id: self.camera_frame = msg.header.frame_id if p_base is not None: @@ -443,6 +604,97 @@ class ServoNode(TolerantNode): throttle_duration_sec=0.5, ) + def _feat_workspace_ok(self, p: np.ndarray) -> bool: + """特征点 XY 是否落在工作盒内（含余量）。 + + 只查 XY、**不查 Z**：特征点是**工件表面点**（z≈0.022，贴台面），而工作盒 + 的 z 区间（0.18~0.65）是给**法兰**用的，两者不是一回事。 + """ + m = float(self.feat_ws_margin_m) + return bool( + self.ws_min[0] - m <= float(p[0]) <= self.ws_max[0] + m + and self.ws_min[1] - m <= float(p[1]) <= self.ws_max[1] + m + ) + + def _slow_xy_limit(self, nerr: float, v_xy: float) -> tuple: + """远处限速：目标离得超过 ``slow_far_m`` 就把 XY 速度上限降到 ``v_slow_xy``。 + + 返回 ``(上限, 原因或 None)``。单拎出来是为了能**离线测**（不需要 ROS 实体）。 + + 为什么需要（2026-10-08 真机，一阶段）：被一串高置信度假 k0（conf 0.53~0.73） + 顶掉后，伺服在 0.7s 内以 ``v_max_xy``=0.45m/s 把臂从 home ``(0.686,-0.113)`` + 带到工作盒角落 ``(0.083,-0.515)``，之后 148s 全是"检测漏检"、150s 超时。 + + 这道限速**与"目标真不真"无关** —— 它不试图判断真假，只保证"追错时走得少"： + 真目标（实测真 k0 离 145mm）只是晚 ~1.8s 到位；假目标最多把臂带走几厘米。 + """ + if nerr > self.slow_far_m: + return min(float(v_xy), self.v_slow_xy), ( + f"目标离 {nerr*1000:.0f}mm > {self.slow_far_m*1000:.0f}mm" + ) + return float(v_xy), None + + def _update_feat_track( + self, p_base: np.ndarray, *, was_lost: bool, now_m: float + ) -> str: + """把一次特征观测并入 ``_feat_kf``；返回 :data:`ACCEPT` / ``HOLD`` / ``REJECT``。 + + 为什么要单拎出来（2026-10-08 真机实测）：一阶段已经收敛到 9mm 之后，模型在 + 运动模糊下误检出一连串"假工件"（框置信度 0.13~0.47，位置各不相同），伺服每 + 个都追 —— 机械臂被带到工作盒边缘卡住（`ee_xy` 从 0.68,-0.12 漂到 0.07,-0.50， + 横移 0.62m）。两道独立防线： + + 1. **跳变确认**（:class:`FeatureJumpGate`）：单帧大跳只 hold、不动；乱跳 + （每帧不同位置）永远确认不了 ⇒ 这一串假点全被挡掉。 + 2. **物理合理性**：跑出工作盒 XY 的点直接 reject —— 实测假点落在基座后方 + ``(-0.10,-0.65)``，而工件在 ``(0.5,-0.12)``。 + + 调用方必须在 ``self._lock`` 内调用（会改 ``_feat_kf`` 与 ``_jump_gate``）。 + """ + p = np.asarray(p_base, dtype=float).reshape(3) + + # --- 防线 2：物理合理性（先判，因为它在"单帧 vs 多帧"之外） --- + if not self._feat_workspace_ok(p): + self._feat_kf.zero_velocity() + self._jump_gate.reset() + self.get_logger().warn( + f"特征点 ({p[0]:.3f},{p[1]:.3f}) 在工作盒外" + f"（x[{self.ws_min[0]:.2f},{self.ws_max[0]:.2f}] " + f"y[{self.ws_min[1]:.2f},{self.ws_max[1]:.2f}] ±{self.feat_ws_margin_m:.2f}）" + "—— 判为误检，当「没有目标」处理，不去追", + throttle_duration_sec=1.0, + ) + return REJECT + + # --- 防线 1：跳变确认 --- + if was_lost or (not self._feat_kf.initialized): + # 刚重新捕获：没有可信的历史位置可比，直接以新观测重定位。 + self._feat_kf.reset(p, now_m) + self._jump_gate.reset() + return ACCEPT + verdict = self._jump_gate.update(p[:2], self._feat_kf.position[:2]) + if verdict == FeatureJumpGate.JUMP: + self._feat_kf.reset(p, now_m) + return ACCEPT + if verdict == FeatureJumpGate.NEAR: + self._feat_kf.advance(now_m) + self._feat_kf.update(p) + return ACCEPT + + # HOLD：本次观测不采信。位置钉住不动，并把速度清零 —— 否则旧速度会把 + # "不动"变成"按旧速度匀速跑掉"，正是要避免的那件事。 + self._feat_kf.zero_velocity() + dxy = float( + np.hypot(p[0] - self._feat_kf.position[0], p[1] - self._feat_kf.position[1]) + ) + self.get_logger().warn( + f"特征点疑似假跳 {dxy * 1000:.0f}mm > {self.predict_jump_m * 1000:.0f}mm" + f"（连续 {self._jump_gate.hold_frames} 帧）—— 本次不采信，位置保持不动；" + f"连续 {self._jump_gate.confirm_frames} 帧一致才认", + throttle_duration_sec=1.0, + ) + return HOLD + def _reset_motion_state(self) -> None: """硬停内部状态：I=0、滤波速度=0、dq 窗口清空。""" self.pid_x.reset() @@ -455,6 +707,7 @@ class ServoNode(TolerantNode): self._v_xy_cmd[:] = 0.0 self._p_feat_base = None self._feat_kf.initialized = False + self._jump_gate.reset() self._dq_hist.clear() def _pub_zero_twist(self) -> None: @@ -492,6 +745,9 @@ class ServoNode(TolerantNode): last_seen = self._last_seen_time p_meas = self._p_meas.copy() lost = self._target_lost_flag + source = self._track_source + if self._hybrid: + self._tick_still_lock() # ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。 # 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动 @@ -522,6 +778,7 @@ class ServoNode(TolerantNode): if ( self.enable_control and self.seek_enabled + and (not self._hybrid) # 混合模式用 D435 一次性，不螺旋搜 and self.ee_z_m > 0.0 and not have and self._xy_armed @@ -563,7 +820,15 @@ class ServoNode(TolerantNode): seen_age = now_mono - last_seen stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9 - if seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout: + if self._hybrid and source == "overhead": + if seen_age > self.overhead_hold_s: + self._lock_feedback(False) + self._watchdog_trip( + f"D435 一次性设定已过期 {seen_age:.1f}s > {self.overhead_hold_s:.1f}s" + ) + return + stamp_age = 0.0 # 一次性不是流：别按图像戳判过期 + elif seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout: self._watchdog_trip( f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s" ) @@ -571,8 +836,11 @@ class ServoNode(TolerantNode): return # 检测框冻结 / USB 卡住：锁住关节，不要继续按上一个误差驱动。 + # D435 一次性不是流，保持设定直到 hold 超时或 D405 接回。 stale = max(0.15, float(self.stale_hold_sec)) - if seen_age > stale or stamp_age > stale: + if (not (self._hybrid and source == "overhead")) and ( + seen_age > stale or stamp_age > stale + ): self.get_logger().info( f"过期保持 seen_age={seen_age:.2f}s —— 不跟踪冻结的检测框", throttle_duration_sec=1.0, @@ -749,15 +1017,21 @@ class ServoNode(TolerantNode): self._watchdog_trip(reason) def _R_cam_to_base(self, T_ee: np.ndarray) -> np.ndarray: - return (T_ee @ self._T_ee_cam())[:3, :3] + return self._T_cam_in_base(T_ee)[:3, :3] def _T_ee_cam(self) -> np.ndarray: # 始终使用标定出的 yaml。TF 只用于 RViz / 打印做核对。 return np.array(self.T_cam_to_gripper, dtype=float) + def _T_cam_in_base(self, T_ee: np.ndarray) -> np.ndarray: + return cam_to_base_T(self.T_cam_to_gripper, T_ee, self.handeye_mode) + def _log_handeye_tf_vs_yaml(self) -> None: t_yaml = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float) - T_tf = self._lookup_T(self.ee_frame, self.handeye_child) + parent = ( + self.base_frame if self.handeye_mode == "eye_to_hand" else self.ee_frame + ) + T_tf = self._lookup_T(parent, self.handeye_child) if T_tf is None: self.get_logger().warn( "HANDEYE TF 查询失败（无妨）—— 伺服使用 yaml " @@ -801,7 +1075,7 @@ class ServoNode(TolerantNode): 而不是法兰的 XY。在固定高度 / 姿态下平移法兰 XY，会把该 look-at 点 按 1:1 一起带走。 """ - T_bc = T_ee @ self._T_ee_cam() + T_bc = self._T_cam_in_base(T_ee) R = T_bc[:3, :3] o = T_bc[:3, 3] zc = float(np.clip(float(p_cam[2]), 0.04, 0.60)) @@ -819,11 +1093,9 @@ class ServoNode(TolerantNode): def _feature_in_base(self, T_ee: np.ndarray, p_cam: np.ndarray) -> np.ndarray: """工件三维点（相机系）→ base 系 XYZ。保持 XYZ 一致。""" - T_bc = T_ee @ self._T_ee_cam() - ph = np.array( - [float(p_cam[0]), float(p_cam[1]), float(p_cam[2]), 1.0], dtype=float + return cam_point_to_base( + p_cam, self.T_cam_to_gripper, T_ee, self.handeye_mode ) - return (T_bc @ ph)[:3] def _hover_vs_feature( self, T_ee: np.ndarray, p_cam: np.ndarray @@ -904,12 +1176,57 @@ class ServoNode(TolerantNode): self._xy_in_dead = dead return dead + def _tick_still_lock(self) -> None: + """D405 跟住且法兰 XY 连续 still_sec 秒没大幅动 → 锁定进二阶段。 + + 顶视一次性迁臂不算。门槛与位移上限只从 site 来，这里不再写死 3 / 15mm。 + """ + if not self._hybrid: + return + T = self._lookup_T(self.base_frame, self.ee_frame) + xy = None if T is None else np.asarray(T[:2, 3], dtype=float) + with self._lock: + have = self._have_target + lost = self._target_lost_flag + source = self._track_source + conf = self._wrist_conf + tracking_ok = ( + source == "wrist" + and have + and not lost + and (not wrist_needs_handoff(conf, self.wrist_handoff_conf)) + and self._xy_armed + ) + locked_now = self._still.update( + xy, time.monotonic(), tracking_ok=tracking_ok + ) + now_m = time.monotonic() + if locked_now and self.enable_control: + if not self._locked: + self._locked = True + self.get_logger().info( + f"=== 一阶段已锁定（D405 跟踪且臂静止 {self.lock_still_sec:.1f}s，" + f"位移≤{self.lock_still_move_m*1000:.0f}mm）" + " → /visual_servo/locked ===" + ) + self._lock_pub.publish(Bool(data=True)) + self._lock_pub_t = now_m + elif now_m - self._lock_pub_t > 1.0: + self._lock_pub_t = now_m + self._lock_pub.publish(Bool(data=True)) + else: + if self._locked: + self._lock_pub.publish(Bool(data=False)) + self._locked = False + self._lock_since = None + def _lock_feedback(self, converged: bool) -> None: """串联锁定信号：持续收敛 lock_hold_sec → /visual_servo/locked=True。 - 只有「有活目标 + XY 入死区 + Z 死区」的驻留分支算收敛；跟踪中、 - 启动下高、寻孔、看门狗冻结等路径都要显式喂 False 打断持续计时。 + 混合模式由 ``_tick_still_lock`` 独占，这里直接返回，避免死区 0.6s 抢先锁定。 """ + if self._hybrid: + return now_m = time.monotonic() if converged and self.enable_control: if self._lock_since is None: @@ -1120,6 +1437,19 @@ class ServoNode(TolerantNode): if nerr > clip_xy: err = err * (clip_xy / nerr) nerr = clip_xy + # ---- 远处限速：抓错目标时只走一小段 ---- + # 2026-10-08 真机：一阶段被一串高置信度假 k0 顶掉，伺服 0.7s 内以 + # v_max_xy(0.45m/s) 把臂从 home (0.686,-0.113) 带到工作盒角落 + # (0.083,-0.515)，之后 148s 全是"检测漏检"。留一道**与"目标真不真" + # 无关**的防线：目标离得远就慢走 —— 真目标（真 k0 离 145mm）只是晚 + # ~1.8s 到位，假目标则最多把臂带走几厘米。 + v_xy, why_slow = self._slow_xy_limit(nerr, v_xy) + if why_slow: + self.get_logger().info( + f"{why_slow} —— 限速 {self.v_slow_xy*1000:.0f}mm/s 逼近" + "（防追错目标大范围移动）", + throttle_duration_sec=1.0, + ) if self._xy_deadzone(nerr, update=False): v_base[0] = 0.0 v_base[1] = 0.0 diff --git a/src/arm_application/arm_application/site_io.py b/src/arm_application/arm_application/site_io.py index 0e7f016..9243123 100644 --- a/src/arm_application/arm_application/site_io.py +++ b/src/arm_application/arm_application/site_io.py @@ -80,3 +80,137 @@ def ee_frame_default() -> str: sn = os.environ.get("SITE_ROBOT_SN", "").strip() return f"{sn}_flange" if sn else "" + +def overhead_camera( + site: dict[str, Any], robot: dict[str, Any] | None = None +) -> tuple[dict[str, Any], list[str]]: + """手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。 + + 手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时 + 相机固定在工位外，不随臂动）。 + + **为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方—— + S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的 + 后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。 + 所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。 + + 收敛规则（确定性的三级）： + 1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）； + 2. 都没有 ``arm``（= 双臂共用的那台）时回落到它； + 3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。 + """ + cams = [ + cfg + for cfg in (site.get("cameras") or {}).values() + if isinstance(cfg, dict) + and str(cfg.get("role") or "").strip().lower().startswith("overhead") + ] + if not cams: + return {}, [] + arm = str((robot or {}).get("arm") or "").strip() + mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else [] + hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams + chosen = dict(hits[0]) + others = [str(c.get("name") or "?") for c in hits[1:]] + return chosen, others + + +def eye_to_hand_result_name(short: str, camera_name: str) -> str: + """眼在手外结果文件的**文件名**（不含目录）。 + + 外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用， + 所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼 + （launch / export_site_env / S03 找文件都走这一处）。 + """ + short = str(short or "").strip() + cam = str(camera_name or "").strip().strip("/") + if short and cam: + return f"{short}_{cam}_eye_to_hand.yaml" + if cam: + return f"{cam}_eye_to_hand.yaml" + if short: + return f"{short}_eye_to_hand.yaml" + return "eye_to_hand.yaml" + + +def resolve_eye_to_hand_file( + root: Path, + *, + explicit: str = "", + camera_name: str = "", +) -> Path | None: + """找手外相机的手眼文件。没有就返回 None。 + + 路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在 + "换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。 + + 候选顺序： + 1. ``explicit`` —— launch 按 site.yaml 派生的确切路径 + （``<臂号>_<相机>_eye_to_hand.yaml``）； + 2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）； + 3. ``eye_to_hand.yaml`` —— 再兜底。 + """ + candidates: list[Path] = [] + if explicit: + p = Path(explicit).expanduser() + candidates.append(p if p.is_absolute() else root / p) + cam = str(camera_name or "").strip().strip("/") + if cam: + candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml") + candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml") + for cand in candidates: + if cand.is_file(): + return cand + return None + + +TRACK_CAMERA_ROLES = ("wrist", "overhead") + + +def track_camera_role(site: dict[str, Any] | None) -> str: + """一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。 + + 写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。 + 缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。 + """ + perc = (site or {}).get("perception") + perc = perc if isinstance(perc, dict) else {} + raw = perc.get("track_camera") + if raw is None or str(raw).strip() == "": + return "wrist" + role = str(raw).strip().lower() + if role not in TRACK_CAMERA_ROLES: + raise ValueError( + f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。" + "写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。" + ) + return role + + +def track_handeye_mode(role: str) -> str: + """跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。""" + if str(role or "").strip().lower() == "overhead": + return "eye_to_hand" + return "eye_in_hand" + + +def handeye_static_tf_frames( + *, + mode: str, + ee_frame: str, + base_frame: str, + cam_frame: str, +) -> tuple[str, str]: + """手眼静态 TF 的 ``(父系, 子系)``。 + + 两种装法的父子关系**不同**，不能混： + * ``eye_in_hand``：法兰 ← 相机（相机跟着臂动） + * ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动） + + 把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF —— + 定位总能到附近、永远差一截，而人只会去怀疑标定精度。 + """ + if str(mode or "").strip().lower() == "eye_to_hand": + return str(base_frame), str(cam_frame) + return str(ee_frame), str(cam_frame) + diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index 88d472f..e123d44 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -6,7 +6,7 @@ 1. verticalize — 法兰 +Z 严格朝世界 -Z（垂直朝下），XY/Z 不动 2. hover — 闭环升到/降到 robots[].servo.hole_hover_z_m（悬停锁高） - 3. coarse — 拍照，detect_best_1 YOLO 框出工件，粗移到 bbox 中心上方 + 3. coarse — 拍照，工件（pose 模型给框；k1/k2 用关键点）粗移到 bbox 中心上方 4. fine — 拍照，two_stage（YOLO+HoughCircles）解 k1/k2 孔心像素 5. align — 深度（孔外工作面中值，防孔底拉偏）+ 内参 + 手眼 + TF 算出孔在基座系 XY，把夹爪/通止规轴线移到目标孔正上方， @@ -47,7 +47,7 @@ from arm_application.geom_ik import ( ) from arm_application.handeye_realsense import resolve_realsense_topics from arm_application.gauge_boot import perception_source_of -from arm_application.gauge_fsm import drag_verdict, hover_z_guard +from arm_application.gauge_fsm import drag_verdict, hover_z_guard, wrench_all_zero from arm_application.hole_detect_hough import detect_holes from arm_application.hole_detector import config_from_site, weights_from_site from arm_application.site_io import deep_merge, load_site_dict @@ -167,6 +167,10 @@ class Stage2Locate(TolerantNode): # 仿真里由 run_gauge_assembly.sh --sim 传**实测值**（每次上料都可能不同）， # 真机不传（走 site.yaml）。悬停高度与 S07 下探预算都按它算。 self.declare_parameter("tool_tip_below_flange_m", -1.0) + # 调试开关：只验证**精对准**、不轻降（避免"对准还没confirmed就把规往孔里带"）。 + # 用 int 不用 bool —— `ros2 launch` 传 bool 参数容易踩到 CLI 解析的坑。 + # -1=按 site 的 stage2.final_descent_enable；0=强制不降；1=强制降。 + self.declare_parameter("final_descent_enable", -1) self.declare_parameter("preview", True) self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory") # 真值孔位（perception.source=truth）要把 scene.yaml 的物体位姿投影成像， @@ -234,6 +238,12 @@ class Stage2Locate(TolerantNode): self.coarse_max_moves = int(stage2.get("coarse_max_moves") or 2) self.align_speed = float(stage2.get("align_speed") or 0.03) self.move_speed = float(stage2.get("move_speed") or 0.05) + # 粗居中的两条**误检守卫**（2026-10-08 真机 arm 89 的 exit=4，见 + # _coarse_target_reject / _bbox_on_border 的注释）：粗居中的目的是把臂 + # 挪到工件正上方，**挪错比不挪危险得多** —— 一挪错精定位就再也看不到 + # k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。 + self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12) + self.coarse_border_px = int(stage2.get("coarse_border_px") or 2) # 最后轻降到孔面上方 final_gap_mm（不接触） self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0 # 悬停高度**必须**让工具尖端留在工件面上方（2026-09-24 的教训，完整说明见 @@ -250,6 +260,10 @@ class Stage2Locate(TolerantNode): if _tool_len <= 0.0: _tool_len = float(_ga.get("tool_tip_below_flange_m") or 0.0) _surface_z = float(_ga.get("workpiece_surface_z_m") or 0.0) + # 记成成员：轻降的几何夹逼（_descend_above_hole）要用同一套数字， + # 不能各推一遍 —— 否则改了一处另一处就悄悄失配。 + self.tool_tip_below_flange_m = _tool_len + self.surface_z_m = _surface_z if _tool_len > 0.0: _fixed, _why = hover_z_guard(self.hover_z, _surface_z, _tool_len, self.final_gap_m) if _why: @@ -257,8 +271,28 @@ class Stage2Locate(TolerantNode): f"二阶段悬停高度自检不通过 —— 用几何修正值 {_fixed:.4f}m：\n {_why}" ) self.hover_z = _fixed + # 轻降**最多能降多少**：从悬停高度降到"规尖停在工件面上方 final_gap"为止。 + # 这是纯几何量，与深度测量无关 —— 测量的唯一职责是让下降**提前**收工， + # 绝不允许它把下降量**放大**（2026-09-29 实测就是这么撞的：测量出的 + # gap0=141mm 比几何真值大 ~86mm，于是多降 86mm 把规撞进孔）。 + # 工具长/工件面未知（<=0）时给 None = 不启用夹逼，行为与改动前一致。 + self.descent_budget_m: Optional[float] = None + if _tool_len > 0.0 and _surface_z > 0.0: + self.descent_budget_m = max( + 0.0, self.hover_z - _tool_len - _surface_z - self.final_gap_m + ) + # 轻降结束后**按几何**算出的真实间隙 [m]，供收尾日志如实汇报（不是把目标值 + # 当结果念一遍 —— 那会让"停在离面 50mm"也显示成"~2.0mm"，R1.1 的静默失效）。 + self.descent_final_gap_m: Optional[float] = None self.final_descent_speed = float(stage2.get("final_descent_speed") or 0.006) self.final_descent_enable = _as_bool(stage2.get("final_descent_enable"), True) + _fd = int(self.get_parameter("final_descent_enable").value) + if _fd in (0, 1): # -1 = 按 site；0/1 = 启动参数强制（调试用） + self.final_descent_enable = bool(_fd) + self.get_logger().warn( + f"final_descent_enable 被启动参数覆盖为 {self.final_descent_enable} " + f"（site 里是 {stage2.get('final_descent_enable')}）—— 只在调试时这么用" + ) self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8)) self.settle_sec = float(stage2.get("settle_sec") or 0.45) self.snap_frames = int(stage2.get("snap_frames") or 5) @@ -266,6 +300,37 @@ class Stage2Locate(TolerantNode): self.max_depth = float(stage2.get("max_depth_m") or 0.60) self.linger_sec = float(stage2.get("linger_sec") or 8.0) + # ---- 精对准的「工具位置」：视觉量规尖（默认）还是法兰+固定偏置 ---- + # 为什么必须能视觉量（2026-10-08 真机 arm 89）： + # 规是**夹在爪里**的零件，它相对法兰轴线可以整体偏出约 10mm（实测规杆 + # 轴线相对法兰 = (+10.7, −0.1)mm）。二阶段原来把**法兰轴线**对到孔上 + # （hover_offset_ee_xy_m 写死 [0,0]），于是规尖实际离孔心 ~10mm， + # 轻降一压，φ6 规尖就骑在 φ7 孔的**孔沿**上、力从 −1N 蹦到 63N 卡死。 + # 现场看到的是"轻降在离面 4mm 停住"，离真正原因（规偏了）很远。 + # 改成在同一帧里同时解「规尖基座 XY」和「孔基座 XY」，两者走同一条手眼链， + # 链路误差/装夹变动一起抵消 —— 这才是"用视觉把孔对正"。 + # 规尖偏移**逐帧实测+一致性校验**（见 _tip_offset_from_depth）：偏移是刚体量， + # 竖直朝下姿态下在基座 XY 里恒定，所以可以用"上一拍的偏移"剔掉偶发误检。 + # 规尖偏移**逐帧实测 + 一致性校验后冻结**（见 _refresh_tip_offset）：偏移是刚体量， + # 竖直朝下姿态下在基座 XY 里恒定。为什么要冻结而不是逐拍重算： + # 2026-10-08 真机实测，逐拍重算时残差在 0.44~0.85mm 之间来回荡、收不进 0.25mm —— + # 元凶是**规杆略微倾斜**（<1°）：每拍取到的"最深像素"高度不同（工具长在 + # 192~223mm 之间跳），同一根杆在不同高度上的基座 XY 差 ~0.4mm。 + # 偏移既然是刚体量，量准一份冻结住，那 0.4mm 的抖动就没了。 + self.tip_vision = _as_bool(stage2.get("align_tip_vision"), True) + self.tip_near_m = float(stage2.get("tip_near_m") or 0.15) + self.tip_min_m = float(stage2.get("tip_min_m") or 0.04) + self.tip_max_m = float(stage2.get("tip_max_m") or 0.60) + self.tip_band_frac = float(stage2.get("tip_search_band_frac") or 0.25) + self.tip_min_px = int(stage2.get("tip_min_px") or 150) + self.tip_off_tol = float(stage2.get("tip_offset_tol_m") or 0.003) + self.tip_len_min = float(stage2.get("tip_len_min_m") or 0.15) + self.tip_len_max = float(stage2.get("tip_len_max_m") or 0.32) + self.tip_calib_n = int(stage2.get("tip_calib_n") or 4) + self._tip_off: Optional[np.ndarray] = None # 规尖相对法兰的基座 XY 偏移（冻结后不变） + self._tip_samp: list = [] # 校准期攒下的 (偏移, 工具长) 样本 + self._tip_len_m: Optional[float] = None # 定偏移时并记下的工具长（用于交叉核对配置） + self.hover_only = _as_bool(self.get_parameter("hover_only").value, False) self.preview = _as_bool(self.get_parameter("preview").value, True) @@ -300,6 +365,9 @@ class Stage2Locate(TolerantNode): #: 最近一帧的收到时刻（单调钟）。判"有没有活的数据"用它，**不用数值大小** —— #: 静止时轴向力本来就接近 0，拿数值判活会把"安静的传感器"误判成掉线。 self._t_ft: Optional[float] = None + #: "FT 全零"守卫的状态（见 _on_wrench）：零值起始时刻 + 是否已喊过一次。 + self._ft_zero_since: Optional[float] = None + self._ft_zero_warned: bool = False robot_ns = str(self.get_parameter("robot_sn").value).replace("-", "_") # 话题可配：默认按 sn 推导，与真机 flexiv_robot_states_broadcaster 一致。 # 换 FT 来源（换个厂家、或走 external_wrench_in_tcp）时不用改代码。 @@ -354,6 +422,28 @@ class Stage2Locate(TolerantNode): # 记收到时刻：下游（尤其 S07）要区分"收到过但读数为 0"（活）与 # "一帧都没收到"（话题名/网络/传感器的问题）。 self._t_ft = time.monotonic() + # —— "全零"守卫（2026-10-08 加）：上面那个"收到过就算活"的口径有个盲区 —— + # 数据源没出数时，话题照样在发、回调照样在跑、读数恒为 (0,0,0)，于是 + # `wrench_live` 报"活"，而"abs(fz−基线)>2.5N 判接触"的兜底**永远不触发**。 + # 真机就踩到了（ft_sensor_wrench 恒零，external_wrench_in_tcp 才有数）。 + # 判据见 wrench_all_zero：带负载的 FT 不可能六个分量同时恰好为 0。 + w = msg.wrench + if wrench_all_zero( + w.force.x, w.force.y, w.force.z, w.torque.x, w.torque.y, w.torque.z + ): + if self._ft_zero_since is None: + self._ft_zero_since = self._t_ft + elif not self._ft_zero_warned and self._t_ft - self._ft_zero_since > 5.0: + self._ft_zero_warned = True + self.get_logger().error( + f"腕部 FT 话题 {self.ft_wrench_topic} 连续 >5s 六个分量**全为 0** —— " + "这不是'安静'（带负载的 FT 不可能全零），是**数据源没有出数**。" + "后果：轻降/下压的接触兜底 abs(fz−基线)>2.5N **永远判不到接触**" + "（静默失效）。真机请指向有数的话题，例如 " + "//external_wrench_in_tcp（site.yaml 的 servo.ft_wrench_topic）。" + ) + else: + self._ft_zero_since = None def _contact_detected(self, baseline_fz: float, thresh_n: float = 2.5) -> bool: """腕部力 Z 偏离基线 thresh_n 牛以上 → 判为规尖碰工件（接触）。""" @@ -913,23 +1003,37 @@ class Stage2Locate(TolerantNode): w = str(weights) if not Path(w).is_file(): raise RuntimeError(f"工件权重不存在: {w}") - if w.lower().endswith(".onnx"): + from arm_application.pose_detector import is_pose_model + from arm_application.yolo_detector import YoloDetector, build_detector, resolve_weights + + conf = float(self._hole_cfg.get("conf", 0.25)) + iou = float(self._hole_cfg.get("iou", 0.7)) + imgsz = int(self._hole_cfg.get("imgsz", 640)) + if is_pose_model(w): + # pose 模型同时给工件框 + k1/k2 关键点。走自解码实现 —— + # ultralytics 解不了 pose 的 TensorRT engine（见 pose_detector 模块头）， + # OnnxWorkpieceDetector 同样只认检测输出。 + det = build_detector( + resolve_weights(w, "auto"), + conf=conf, + iou=iou, + imgsz=imgsz, + ) + elif w.lower().endswith(".onnx"): from arm_application.onnx_workpiece import OnnxWorkpieceDetector det = OnnxWorkpieceDetector( w, - conf=float(self._hole_cfg.get("conf", 0.25)), - imgsz=int(self._hole_cfg.get("imgsz", 640)), + conf=conf, + imgsz=imgsz, ) else: - from arm_application.yolo_detector import YoloDetector, resolve_weights - det = YoloDetector( resolve_weights(w, "auto"), device="cpu", - conf=float(self._hole_cfg.get("conf", 0.25)), - iou=float(self._hole_cfg.get("iou", 0.7)), - imgsz=int(self._hole_cfg.get("imgsz", 640)), + conf=conf, + iou=iou, + imgsz=imgsz, ) self.get_logger().info(f"工件检测器已加载: {w}") return det @@ -1153,15 +1257,210 @@ class Stage2Locate(TolerantNode): best, best_area = d, area if best is None: return None - return tuple(int(v) for v in best.xyxy) # type: ignore[return-value] + bbox = tuple(int(v) for v in best.xyxy) # type: ignore[assignment] + h, w = bgr.shape[:2] + if self._bbox_on_border(bbox, w, h): + return None + return bbox # type: ignore[return-value] + + def _bbox_on_border( + self, bbox: Any, w: int, h: int, margin_px: Optional[int] = None + ) -> bool: + """框是否被画面边缘切到（= 只看到工件的一部分，不是整个工件）。 + + 为什么这种框一个都不能用（2026-10-08 真机 arm 89 实测） + -------------------------------------------------------- + 粗居中要的是**框中心落在工件的哪个位置**，再对那个点做深度反投影。 + 框被边缘切掉时，框中心根本不在工件上 —— 真机那一帧:: + + snap_coarse_0: 工件 bbox=(0, 142, 97, 222) 中心=(48,182) + base=(0.280,-0.747)[深度] 偏差=211.5mm + + ``x0=0`` 就是左边缘切的。反投影出来的 (0.280,-0.747) 落在工作盒外 + 197mm，而下一步**没有判误检，而是"截断到盒边"照样横移** 56mm， + 把臂带离工件 ⇒ 精定位再也看不到 k1/k2 ⇒ ``exit=4``。 + + 同一个判据顺手挡掉另一种退化输入：YOLO 空框时兜底用的"孔检测器 bbox" + 在真机那帧是 ``[0, 0, 847, 479]``（整幅画面，四边全贴边）。 + """ + if margin_px is None: + margin_px = int(getattr(self, "coarse_border_px", 2)) + if bbox is None or len(bbox) < 4 or w <= 0 or h <= 0: + return True + x0, y0, x1, y1 = (float(v) for v in bbox[:4]) + m = float(margin_px) + return x0 <= m or y0 <= m or x1 >= (w - 1) - m or y1 >= (h - 1) - m + + def _coarse_target_reject( + self, + p_base: np.ndarray, + gxy: np.ndarray, + max_offset_m: Optional[float] = None, + ) -> Optional[str]: + """粗居中的目标点是不是**误检**。返回 None = 可信；否则返回拒绝原因。 + + 为什么必须能在**移动前**否掉（2026-10-08 真机 arm 89，exit=4） + -------------------------------------------------------------- + 真机那一帧:: + + snap_coarse_0: base=(0.280,-0.747)[深度] 偏差=211.5mm + 目标超出工作盒，已截断: (0.280,-0.747,0.320) → (0.280,-0.545,0.320) + coarse Δ=(-56,-2,-0)mm ← 臂被带离工件 56mm + snap_fine_*: 两孔基座间距 210.0/213.2/212.2mm 不像 k1/k2，丢弃本拍 + 二阶段失败 exit=4 + + 工件其实在 (0.5,-0.12) 一带 —— 也就是说这个"截断"不是安全兜底， + 而是**把一次误检变成了对机械臂的一条真实横移指令**：越界只是被夹到盒边， + 臂照样走出去。这就是规矩里禁的"宽容降级"：把"没检查"伪装成"检查通过"。 + + 两条判据（都是"盒外/离谱 ⇒ 判误检"，不是"夹到盒边继续"）： + + 1. 目标点落在 ``workspace_xyz_*`` 的 XY 之外（含 5mm 余量）—— 一阶段刚锁 + 定时臂就在工件上方，工件**不该**在盒外； + 2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.12m）—— 粗居中 + 是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数。 + + 注意：判误检后**原地不动**，由精定位（直接量 k1/k2）继续。不动的代价只是 + "少挪一次"，而挪错的代价是精定位彻底看不到孔 —— 所以这里必须选不动。 + """ + if max_offset_m is None: + max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.12)) + p = np.asarray(p_base, dtype=float).reshape(3) + if not np.all(np.isfinite(p[:2])): + return "目标点非有限值（深度/三角化失败）" + lo = np.asarray(getattr(self, "ws_min", None), dtype=float).reshape(-1) + hi = np.asarray(getattr(self, "ws_max", None), dtype=float).reshape(-1) + if lo.size >= 2 and hi.size >= 2: + margin = 0.005 + for i, name in ((0, "x"), (1, "y")): + if p[i] < lo[i] - margin or p[i] > hi[i] + margin: + return ( + f"目标 {name}={p[i]:.3f} 在工作盒 [{lo[i]:.3f},{hi[i]:.3f}] 外 " + f"⇒ 反投影/检测坏了" + ) + g = np.asarray(gxy, dtype=float).reshape(-1) + if g.size >= 2: + d = float(np.hypot(p[0] - g[0], p[1] - g[1])) + if d > max_offset_m: + return ( + f"目标离夹爪轴线 {d*1000:.1f}mm > 上限 {max_offset_m*1000:.0f}mm " + f"⇒ 不像'臂已在工件上方'的粗居中量" + ) + return None def _gauge_xy(self, Tee: np.ndarray) -> np.ndarray: - """夹爪/通止规轴线（法兰原点 + hover_offset）在基座系 XY。""" + """夹爪/通止规轴线（法兰原点 + hover_offset）在基座系 XY。 + + 退路用（视觉规尖不可用时）。注意它假设"规和法兰同轴"——2026-10-08 真机实测 + 该假设不成立（规偏出 ~10mm），所以**不是**首选，见 _tip_offset_from_depth。 + """ p = Tee[:3, 3] R = Tee[:3, :3] off = R @ np.array([self.hover_offset[0], self.hover_offset[1], 0.0]) return np.array([p[0] + off[0], p[1] + off[1]]) + def _tip_offset_from_depth( + self, depth_m: np.ndarray, intr: Dict[str, float], Tee: np.ndarray + ) -> Tuple[Optional[np.ndarray], Optional[float], str]: + """从一帧深度里找**规尖**，返回 (规尖相对法兰的基座 XY 偏移, 工具长, 说明)。 + + 找不到/不可信时返回 (None, None, 原因)。判据（每条都能真的失败）： + 1. 只看法兰轴线附近的**中央竖带**——规就夹在爪里，不会跑到画面边上； + 2. 近端连通域要够大（规杆），且连通域内最深 3% 就是**规尖**； + 3. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max] —— + 这一条同时挡住"深度读到别的东西"（如台面/工件），是主干守卫。 + + ⚠️ 工具长这一项**只在单帧内**可信；实测它会跳（192~223mm）——规尖细、反光， + 掠射角下深度容易丢，丢了以后"最深有效像素"就往上跑、工具长只会**变短**。 + 所以： + · **XY 偏移**对这件事不敏感（规杆细且接近竖直，杆上任一像素反投回基座 XY + 都在杆轴 ~3mm 内）⇒ 逐帧拿来用是稳的； + · **工具长**必须挑"端头看得全"的拍（最长那一簇），见 _refresh_tip_offset。 + """ + import cv2 + + h, w = depth_m.shape + valid = (depth_m > self.tip_min_m) & (depth_m < self.tip_max_m) + near = (valid & (depth_m < self.tip_near_m)).astype(np.uint8) + band = np.zeros_like(near) + half = max(1, int(self.tip_band_frac * w)) + c0, c1 = int(intr["cx"]) - half, int(intr["cx"]) + half + band[:, max(0, c0):min(w, c1)] = 1 + near = near & band + if int(near.sum()) < self.tip_min_px: + return None, None, f"近端(<{self.tip_near_m*1000:.0f}mm)中央竖带内像素 {int(near.sum())} < {self.tip_min_px}" + n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8) + if n <= 1: + return None, None, "近端无连通域（规不见了吗）" + big = max(range(1, n), key=lambda i: stats[i, 4]) + ys, xs = np.where(lab == big) + dv = depth_m[ys, xs] + k = np.percentile(dv, 97) # 连通域内最深 3% ≈ 规尖 + sel = dv >= k - 1e-6 + u = float(np.median(xs[sel])) + v = float(np.median(ys[sel])) + y0, y1 = max(0, int(v) - 2), min(h, int(v) + 3) + x0, x1 = max(0, int(u) - 2), min(w, int(u) + 3) + patch = depth_m[y0:y1, x0:x1] + pv = patch[(patch > self.tip_min_m) & (patch < self.tip_max_m)] + if pv.size == 0: + return None, None, "规尖邻域深度无效" + z_tip = float(np.median(pv)) + try: + p_tip = self._cam_to_base(self._px_to_cam(u, v, z_tip, intr)) + except Exception as exc: # noqa: BLE001 + return None, None, f"规尖反投影失败（手眼 TF 缺？）: {exc}" + tool_len = float(Tee[2, 3]) - float(p_tip[2]) + if not (self.tip_len_min <= tool_len <= self.tip_len_max): + return None, None, ( + f"反投影出的工具长 {tool_len*1000:.0f}mm 不在 " + f"[{self.tip_len_min*1000:.0f},{self.tip_len_max*1000:.0f}]mm —— " + f"近端那个物体不是规" + ) + off = np.array([float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])]) + return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm" + + def _refresh_tip_offset(self) -> None: + """攒够**一致**样本后，定下并**冻结**「规尖相对法兰」的偏移。 + + 判据只有一条：取中位数剔掉外点后，还要剩 ``tip_calib_n`` 个样本落在 + ``tip_offset_tol_m`` 内。**不再要求"工具长分簇"** —— 实测那样太严： + 规尖像素时有时无，工具长在 203/222mm 之间跳，"工具长最长那一簇"往往只有 + 1~2 拍，永远凑不满 4 拍 ⇒ 永远冻不住 ⇒ 退回"法兰+偏置"（那就白改了）。 + 而"规杆细且接近竖直"⇒ 杆上任一像素反投回基座 XY 都在杆轴 ~3mm 内， + 所以**工具长差不影响偏移**，一致性校验就够。 + 工具长只用来跟 site.yaml 交叉核对（见下面的告警），不参与对准。 + + 为什么必须冻结：偏移是刚体量；逐拍重算会把"规杆略有倾斜"引入的 ±0.2~0.4mm + 抖动带进残差（2026-10-08 真机残差 0.44~0.85mm 来回荡、收不进 0.25mm）。 + """ + if self._tip_off is not None or len(self._tip_samp) < self.tip_calib_n: + return + arr = np.stack([o for o, _t in self._tip_samp]) + med = np.median(arr, axis=0) + inl = np.linalg.norm(arr - med, axis=1) <= self.tip_off_tol + if int(inl.sum()) < self.tip_calib_n: + return + idx = np.where(inl)[0] + self._tip_off = arr[idx].mean(0) + tl = np.array([self._tip_samp[i][1] for i in idx], dtype=float) + self._tip_len_m = float(np.median(tl)) + cfg = float(self.tool_tip_below_flange_m or 0.0) + agree = "" if cfg <= 0 else ( + "，与 site.yaml 的工具长一致" + if abs(self._tip_len_m - cfg) <= 0.003 + else f" ⚠️ 与 site.yaml 的工具长 {cfg*1000:.0f}mm 差 " + f"{(self._tip_len_m-cfg)*1000:+.0f}mm —— 夹持变动？" + "下降量按 site 的几何走，这时要人工核对（别让规撞进孔）" + ) + self.get_logger().info( + f" 规尖偏移已冻结 = ({self._tip_off[0]*1000:+.1f},{self._tip_off[1]*1000:+.1f})mm" + f"（{int(inl.sum())} 拍一致，最大散布 " + f"{float(np.max(np.linalg.norm(arr[idx]-self._tip_off, axis=1)))*1000:.2f}mm，" + f"工具长 {self._tip_len_m*1000:.0f}mm{agree}）" + " —— 刚体量，对准期间只量这一份" + ) + def _grab_median_depth(self, n: int) -> Optional[np.ndarray]: """拍 n 帧深度取时间中值（稳）。""" deps = [] @@ -1214,46 +1513,112 @@ class Stage2Locate(TolerantNode): return None return float(np.median(depth[ring])) - def _measure_z_wp(self, rod_mask: Optional[np.ndarray]) -> Optional[float]: - """下降后重测局部工件面深度（与悬停时 _local_wp_depth 同一方法，规杆掩码恒定直接复用）。""" - if rod_mask is None: - return None - depth = self._grab_median_depth(6) - if depth is None: - return None - return self._local_wp_depth(depth, rod_mask) - def _descend_above_hole(self, q, Tee): """对准后柔顺轻降，把规尖悬在孔面上方 final_gap_m（不接触）。 - 原理：相机和规都固定在臂上 → 相机到规尖的距离 tip_d 恒定；纯 Z 下降使 - 「相机到工件面深度 z_wp」1:1 减小。下降到 z_wp = tip_d + final_gap 即规尖 - 悬在孔面上方 final_gap。按悬停时测得的间隙一次算下降量，分小步很慢下降， - 最后重测 z_wp 核验。不依赖规长/基座绝对 z，稳健；深度反馈保证不接触。 + **下降量只由几何决定**（这条是 2026-10-08 真机修出来的）:: + + 规尖到工件面的间隙 = 法兰 Z(正运动学) − 工具长 − 工件面 Z + 需下降 = 该间隙 − final_gap + + 三个量都是确定的：法兰 Z 来自 FK（0.1mm 量级），工具长是标定值，工件面 Z + 是现场实测值（``gauge_assembly.workpiece_surface_z_m``）。 + + ⚠️ 为什么**不再**让深度测量决定下降量（或叫停）—— + 2026-09-29：``z_wp``（"规杆外环形"深度中值）报 141mm，几何真值 53mm， + 于是多降 86mm 把规撞进工件（工件被推走 5mm）。 + 2026-10-08：同一个读数又反着错 —— 悬停时报 144mm（几何真值 75mm）， + 只降 24mm 后又翻成 **−7mm**，触发 ``SAFE_FLOOR`` 把轻降停在离面 ~50mm 处， + 永远到不了 2mm。同一套读数在两个方向上各错一次、幅度 150mm 量级、还随位姿 + 乱跳 —— 它对"能降多少"和"该不该停"都没有发言权。 + + 所以现在深度读数**只做交叉核对**：与几何对不上就喊出来（把"测距方法待修" + 摆在日志里），但不再参与任何决策。真正的安全由三条**物理/几何**机制保证： + + 1. 几何预算上限 ``descent_budget_m``（与 ``gauge_fsm.hover_z_guard`` 同源）—— + FK/配置异常时也压不下去； + 2. 腕部 FT 接触检测：``force.z`` 相对悬空基线突变 ⇒ 立即停 + 回退 1mm； + 3. 任一步 ``_goto`` 没走到位（多半是碰到东西/被挡）⇒ 停止，保持当前高度。 """ if not self.final_descent_enable: return True, q, Tee self._state("descend_above_hole") - tip_d, rod_mask, z_wp = self._measure_gauge_state() - if tip_d is None or z_wp is None: - self.get_logger().warn("未测到通止规/工件间隙（规可能不在视野），跳过轻降，停在悬停高") - return True, q, Tee - gap0 = z_wp - tip_d + tip_len = float(self.tool_tip_below_flange_m) + surf = float(self.surface_z_m) + if tip_len <= 0.0 or surf <= 0.0 or self.descent_budget_m is None: + # 缺几何就没法算安全下降量 —— 停在悬停高，**绝不**拿测距去赌 + # （2026-09-29 撞工件就是"没有几何、只有测距"时发生的）。配置缺键 + # 必须当失败处理（R1.1），不能静默降级成"按测量降"。 + self.get_logger().error( + "轻降缺少几何：tool_tip_below_flange_m / workpiece_surface_z_m / " + "descent_budget 至少一个没配 —— 停在悬停高，不下降。" + "请补 site.yaml 的 robots[].gauge_assembly 两把尺子。" + ) + return False, q, Tee + + gap0 = float(Tee[2, 3]) - tip_len - surf # 几何间隙（唯一决定下降量的量） descent = gap0 - self.final_gap_m + if descent > self.descent_budget_m + 5e-4: + self.get_logger().warn( + f"轻降超过配置几何预算：按 FK 算需降 {descent * 1000:.1f}mm > " + f"预算 {self.descent_budget_m * 1000:.1f}mm" + f"（当前法兰 {Tee[2, 3]:.4f} 与配置悬停 {self.hover_z:.4f} 不符）" + "—— 收到预算（宁高勿撞）" + ) + descent = self.descent_budget_m + + # ---- 深度读数只做交叉核对（无权叫停、无权加码）---- + tip_d, _rod_mask, z_wp = self._measure_gauge_state() + if tip_d is not None and z_wp is not None: + gap_meas = float(z_wp) - float(tip_d) + if abs(gap_meas - gap0) > 0.010: + self.get_logger().warn( + f"深度测距与几何不符：测量间隙 {gap_meas * 1000:.1f}mm vs " + f"几何 {gap0 * 1000:.1f}mm（差 {(gap_meas - gap0) * 1000:+.1f}mm）" + "—— 本次**按几何下降**，深度读数不参与决策" + "（测距方法待修，见 site.yaml 的 final_descent_enable 注释）" + ) + else: + self.get_logger().warn( + "未测到通止规/工件（规可能不在视野）—— 深度核对跳过，仍按几何下降" + "（下降量不依赖规是否可见）" + ) + self.get_logger().info( - f"轻降准备: 规尖-工件间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，" - f"需下降 {descent * 1000:.1f}mm（规尖深 {tip_d * 1000:.0f}mm 恒定，很慢很柔）" + f"轻降准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，" + f"需下降 {descent * 1000:.1f}mm（法兰 {Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm" + f" − 工件面 {surf:.4f}；很慢很柔）" ) if descent <= 0.001: self.get_logger().info("规尖已在目标间隙附近，无需下降") + self.descent_final_gap_m = gap0 return True, q, Tee # 腕部力基线（规尖悬空时的 force.z），用于接触检测兜底 base_fz = self._ft_fz - # 闭环轻降：每降一小步就重测局部工件面深度，间隙到 final_gap 就停——绝不一次冲到位，防过冲接触。 - SAFE_FLOOR = 0.0012 # 安全下限 1.2mm：实测间隙≤它立即停（防深度噪声/过冲导致接触） - for _ in range(12): + if base_fz is None: + self.get_logger().warn( + "轻降开始前还没收到腕部 FT —— 接触兜底不生效（只剩几何预算与 _goto 两道）" + ) + # 步数上限：预算 ≤~80mm、单步 ≤4mm ⇒ 20 步足够；给 60 步余量，真正收工靠 + # 几何预算（下面的 remaining），不靠步数。 + for step_i in range(60): if not rclpy.ok(): return False, q, Tee + # 每步把 FT 轴向力打进日志：这是"停住到底是碰到了还是没跟上"的**唯一** + # 直接证据（2026-10-08 排查轻降在离面 ~4mm 处停住时，就靠这一行区分 + # "接触"与"驱动没跟上"——没有它只能靠猜）。 + if self._ft_fz is None: + ft_txt = "FT_fz=None" + elif base_fz is None: + ft_txt = f"FT_fz={self._ft_fz:+.2f}N（无基线）" + else: + ft_txt = ( + f"FT_fz={self._ft_fz:+.2f}N（基线 {base_fz:+.2f}，" + f"Δ{self._ft_fz - base_fz:+.2f}）" + ) + self.get_logger().info( + f" 轻降第 {step_i + 1} 步: 法兰 Z={Tee[2, 3]:.4f} {ft_txt}" + ) # 力觉兜底：规尖一碰工件 force.z 突变，立即停并回退 1mm if base_fz is not None and self._contact_detected(base_fz): self.get_logger().warn("腕部力突变（规尖触到工件），立即停止并回退 1mm（安全优先）") @@ -1264,20 +1629,13 @@ class Stage2Locate(TolerantNode): v=self.final_descent_speed, max_hops=3, hold_xy=True, settle_s=0.25, ) self._pin(q) + self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf return True, q, Tee - z_now = self._measure_z_wp(rod_mask) - if z_now is not None: - gap_now = z_now - tip_d - excess = gap_now - self.final_gap_m - # 安全下限优先：宁可停在孔上方多一点，也绝不压到工件 - if gap_now <= SAFE_FLOOR: - self.get_logger().info(f"已达安全下限间隙 {gap_now * 1000:.1f}mm，停止下降（不接触）") - break - else: - excess = descent # 测不到就按预算量保守走 - if excess <= 0.0004: # 已到 final_gap+0.4mm 容差内 + # 用 FK 现算剩余下降量（不是累加指令）—— 单步没走到也不会积累误差。 + remaining = (float(Tee[2, 3]) - tip_len - surf) - self.final_gap_m + if remaining <= 1e-4: break - step = min(excess, 0.004) # 每步 ≤4mm，很慢很柔 + step = min(remaining, 0.004) # 每步 ≤4mm，很慢很柔 p = Tee[:3, 3].copy() target = np.array([p[0], p[1], p[2] - step]) ok, q, Tee = self._goto( @@ -1288,16 +1646,17 @@ class Stage2Locate(TolerantNode): self._pin(q) if not ok: self.get_logger().warn("轻降中途未到位，停止（保持当前高度，安全优先）") + self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf return False, q, Tee - # 核验：重测工件面深度，算最终间隙（深度反馈，确保不接触） - z2 = self._measure_z_wp(rod_mask) - if z2 is not None: - gap_f = z2 - tip_d - lvl = self.get_logger().info if 0.0003 < gap_f else self.get_logger().warn - lvl( - f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm " - f"（目标 {self.final_gap_m * 1000:.1f}mm，不接触）===" - ) + # 收尾：按几何如实汇报最终间隙（不再用那个错 150mm 的深度读数当结论） + gap_f = float(Tee[2, 3]) - tip_len - surf + self.descent_final_gap_m = gap_f + lvl = self.get_logger().info if gap_f >= 0.0 else self.get_logger().warn + lvl( + f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 " + f"{Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm − 工件面 {surf:.4f}；" + f"目标 {self.final_gap_m * 1000:.1f}mm，不接触）===" + ) return True, q, Tee @@ -1525,6 +1884,8 @@ class Stage2Locate(TolerantNode): bbox = None truth: Optional[dict] = None bgr = depth_m = intr = None + # 本拍是否"跳过粗居中"（精定位已看到 k1/k2）—— 见下面 holes 分支 + skip_coarse = False time.sleep(self.settle_sec if move_i == 0 else min(0.20, self.settle_sec)) for attempt in range(8): if attempt: @@ -1537,13 +1898,21 @@ class Stage2Locate(TolerantNode): break holes = self._detect_holes_img(bgr, skip_center_gate=True) if holes.get("status"): - self.get_logger().info("粗定位 YOLO 空框，但已看到 k1/k2，跳过居中直进精对准") - bbox = holes.get("bbox") - truth = holes + # 精定位已经能看到 k1/k2（它是**直接量孔**，比粗居中可靠）， + # 所以不居中、不移动，直接交给精定位。 + # 旧写法在这里把孔检测器的整幅 bbox 拿来当工件目标，然后**照样算 + # 偏差横移**：真机那帧 bbox=[0,0,847,479]（整幅画面）⇒ 偏差 + # 55.9mm ⇒ 又挪一次。一句话："跳过居中"就必须真的不动。 + self.get_logger().info( + "粗定位 YOLO 无可信框（空框/被边缘切），但已看到 k1/k2 " + "—— 跳过粗居中、原地直进精对准" + ) self._publish_overlay(holes.get("draw_img", bgr)) - move_i = self.coarse_max_moves + skip_coarse = True break self.get_logger().warn(f"粗定位第 {attempt + 1}/8 帧未见工件，复拍") + if skip_coarse: + break if bbox is None: if bgr is not None: self._publish_overlay(bgr) @@ -1572,6 +1941,17 @@ class Stage2Locate(TolerantNode): Tee = T_now gxy = self._gauge_xy(Tee) err_xy = float(np.hypot(p_base[0] - gxy[0], p_base[1] - gxy[1])) + # 目标点先过**误检守卫**：宁可原地不动交给精定位，也不许按一个坏点横移。 + # 一阶段刚锁定时臂已经在工件上方，粗居中的合理位移是厘米级；出现 + # 十几厘米以上的"偏差"或盒外目标，只可能是反投影/检测坏了。 + why = self._coarse_target_reject(p_base, gxy) + if why: + self.get_logger().warn( + f"粗定位目标判误检（{why}）—— **不移动**，原地直进精定位：" + f"base=({p_base[0]:.3f},{p_base[1]:.3f})[{src}] " + f"夹爪=({gxy[0]:.3f},{gxy[1]:.3f}) 偏差={err_xy*1000:.1f}mm" + ) + break coarse_tol = float(self._hole_cfg.get("coarse_tol_m", 0.006)) self.get_logger().info( f"工件 bbox={bbox} 中心=({bx:.0f},{by:.0f}) " @@ -1674,7 +2054,34 @@ class Stage2Locate(TolerantNode): Tee_now = self._lookup(self.base_frame, self.ee_frame) if Tee_now is not None: Tee = Tee_now - gxy = self._gauge_xy(Tee) + # ---- 工具位置取"视觉规尖"，不是"法兰轴线"-------------------- + # 见 _tip_offset_from_depth / _refresh_tip_offset：规可偏出法兰轴线 ~10mm， + # 把法兰对到孔上等于把规偏着对过去。偏移量准一次后冻结（刚体量）。 + tip_src = "法兰+偏置" + if self.tip_vision and self._tip_off is None: + off, tlen, why = self._tip_offset_from_depth(depth_m, intr, Tee) + if off is not None and tlen is not None: + self._tip_samp.append((off, tlen)) + self._refresh_tip_offset() + tip_src = ( + f"视觉规尖(校准中 n={len(self._tip_samp)} {why})" + if self._tip_off is None + else "视觉规尖(已冻结)" + ) + else: + self.get_logger().warn(f" 规尖视觉量取失败：{why}") + elif self._tip_off is not None: + tip_src = "视觉规尖(已冻结)" + if self._tip_off is not None: + gxy = np.array( + [Tee[0, 3] + self._tip_off[0], Tee[1, 3] + self._tip_off[1]] + ) + else: + gxy = self._gauge_xy(Tee) + if self.tip_vision and self._tip_off is None and len(self._tip_samp) == 0: + self.get_logger().warn( + " 规尖还没量到，本拍先用法兰+偏置兜着 —— 此时可能对的是法兰不是规" + ) err = float(np.hypot(tgt_b[0] - gxy[0], tgt_b[1] - gxy[1])) final = { @@ -1688,6 +2095,13 @@ class Stage2Locate(TolerantNode): "workpiece_surface_z_m": round(z_wp, 4), "hole_z_m": [round(z_left, 4), round(z_right, 4)], "gauge_xy_m": [round(float(gxy[0]), 5), round(float(gxy[1]), 5)], + "gauge_source": tip_src, + "tip_offset_m": ( + [round(float(self._tip_off[0]), 5), round(float(self._tip_off[1]), 5)] + if self._tip_off is not None + else None + ), + "tip_len_m": (round(self._tip_len_m, 5) if self._tip_len_m else None), "residual_mm": round(err * 1000.0, 2), "hover_z_m": round(float(Tee[2, 3]), 4), "iter": it, @@ -1696,7 +2110,8 @@ class Stage2Locate(TolerantNode): self.get_logger().info( f"[fine {it}] k1=({k1_b[0]:.4f},{k1_b[1]:.4f}) k2=({k2_b[0]:.4f},{k2_b[1]:.4f}) " f"zL={z_left*1000:.0f} zR={z_right*1000:.0f}mm " - f"gauge=({gxy[0]:.4f},{gxy[1]:.4f}) 残差={err*1000:.2f}mm (tol {self.align_tol*1000:.1f})" + f"工具=({gxy[0]:.4f},{gxy[1]:.4f})[{tip_src}] " + f"残差={err*1000:.2f}mm (tol {self.align_tol*1000:.1f})" ) # ---- 工件被推走的看门狗 ----------------------------------------- @@ -1758,6 +2173,21 @@ class Stage2Locate(TolerantNode): self._publish_result(final) if err <= self.align_tol: + if self.tip_vision and self._tip_off is None: + # 容差达标了，但这一拍的"工具位置"其实是**法兰**（退路了）—— + # 对的是法兰不是规，δ 就是那个 ~10mm 的装夹偏置。2026-10-08 真机 + # 就出现过：tip_offset_m=None、残差 0.15mm 看着漂亮，其实法兰 + # 压在 k1 上、规尖偏出 ~11mm。**不许把没做到说成做到**。 + self.get_logger().error( + f"残差 {err*1000:.2f}mm 达标，但规尖偏移一直没量到（用的是法兰轴线）" + " —— 这个结果不可信：对的可能是法兰、不是规。" + "判失败，摆到能看清规尖再跑（别按这个结果下降）" + ) + final["ok"] = False + final["align_trusted"] = False + final["fail_reason"] = "未量到规尖偏移，工具位置退化为法兰轴线" + self._publish_result(final) + return 8, q, Tee, final self._state("aligned") break if last_good is not None: @@ -1828,10 +2258,18 @@ class Stage2Locate(TolerantNode): self.get_logger().warn("对准移动未完全到位，复拍核验") # ---- 4.5) 柔顺轻降到孔面上方 1-2mm（不接触）---- - # 用户要求：对准后轻轻悬在孔上方 1-2mm，不下臂接触。深度反馈闭环，很慢很柔。 + # 下降量由**几何**决定（法兰 FK − 工具长 − 工件面 − final_gap），深度读数只 + # 做交叉核对，见 _descend_above_hole 的说明。 okd, q, Tee = self._descend_above_hole(q, Tee) if not okd: self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）") + # 如实汇报**实际**间隙（几何量）——不是把目标值念一遍。停在离面 50mm 时 + # 也显示 ~50mm，现场才看得出"轻降没做到"（R1.1：不许把没做到说成做到）。 + gap_txt = ( + f"{self.descent_final_gap_m * 1000:.1f}" + if self.descent_final_gap_m is not None + else f">{self.final_gap_m * 1000:.1f}(未能测得/未下降)" + ) # ---- 5) 锁定保持 ---- self._state("hold") @@ -1839,7 +2277,7 @@ class Stage2Locate(TolerantNode): self._pin(q_fin if q_fin is not None else q) self.get_logger().info( f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，" - f"悬在孔面上方 ~{self.final_gap_m * 1000:.1f}mm（不接触）。" + f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。" f"结果: /stage2/result, /tmp/stage2_result.json ===" ) return 0, q_fin if q_fin is not None else q, Tee, final diff --git a/src/arm_application/arm_application/yolo_detector.py b/src/arm_application/arm_application/yolo_detector.py index 38edb80..9276ca0 100644 --- a/src/arm_application/arm_application/yolo_detector.py +++ b/src/arm_application/arm_application/yolo_detector.py @@ -43,6 +43,9 @@ class Detection: conf: float cls_id: int cls_name: str + # pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。 + # 检测模型恒为 None —— 下游必须先判 None，别假设它有。 + kpts: Optional[np.ndarray] = None @property def cx(self) -> float: @@ -301,12 +304,51 @@ class YoloDetector: return seq, dets, (time.monotonic() - t0) * 1000.0 +def build_detector( + weights: Union[str, Path], + *, + backend: str = "auto", + conf: float = 0.25, + iou: float = 0.45, + device: str = "", + imgsz: int = 640, +): + """按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。 + + 为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见 + ``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型 + 是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。 + + 返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。 + """ + path = Path(weights).expanduser() + from .pose_detector import PoseDetector, is_pose_model + + if is_pose_model(path): + logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name) + return PoseDetector( + path, + conf=conf, + iou=iou, + device=device or "0", + imgsz=imgsz, + ) + return YoloDetector( + path, + conf=conf, + iou=iou, + device=device, + imgsz=imgsz, + ) + + def _yolo_worker_main(q_in, q_out, cfg: dict) -> None: - """spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。""" + """spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。""" try: path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto") - det = YoloDetector( + det = build_detector( path, + backend=cfg.get("backend") or "auto", conf=float(cfg.get("conf") or 0.25), iou=float(cfg.get("iou") or 0.45), device=str(cfg.get("device") or "0"), @@ -336,7 +378,13 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None: dets = det.infer(image, half=bool(cfg.get("half", True))) gpu_ms = (time.monotonic() - t0) * 1000.0 packed = [ - (d.xyxy.astype(np.float32), float(d.conf), int(d.cls_id), str(d.cls_name)) + ( + d.xyxy.astype(np.float32), + float(d.conf), + int(d.cls_id), + str(d.cls_name), + None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32), + ) for d in dets ] _put_drop_old(q_out, ("ok", seq, packed, gpu_ms)) @@ -399,15 +447,20 @@ class YoloInferProcess: def alive(self) -> bool: return bool(self._p is not None and self._p.is_alive()) - def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None: + def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool: """把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会 - 排空到队列里最新的帧，所以不会跑任何过期数据。""" + 排空到队列里最新的帧，所以不会跑任何过期数据。 + + 返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」 + 而空等，把 D405 也卡住。 + """ if not self.alive: raise RuntimeError("YOLO 工作进程已死亡") try: self._q_in.put((int(seq), image_bgr), timeout=0.010) + return True except Exception: - pass + return False def wait_result(self, timeout: float = 0.045): """返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。""" @@ -444,15 +497,20 @@ class YoloInferProcess: return None seq, packed = int(msg[1]), msg[2] gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0 - dets = [ - Detection( - xyxy=np.asarray(xyxy, dtype=float), - conf=float(conf), - cls_id=int(cid), - cls_name=str(name), + dets = [] + for row in packed: + # 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。 + xyxy, conf, cid, name = row[0], row[1], row[2], row[3] + kpts = row[4] if len(row) > 4 else None + dets.append( + Detection( + xyxy=np.asarray(xyxy, dtype=float), + conf=float(conf), + cls_id=int(cid), + cls_name=str(name), + kpts=None if kpts is None else np.asarray(kpts, dtype=float), + ) ) - for xyxy, conf, cid, name in packed - ] return seq, dets, gpu_ms def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0): diff --git a/src/arm_application/config/hole_insert.yaml b/src/arm_application/config/hole_insert.yaml index 6ebe007..e499818 100644 --- a/src/arm_application/config/hole_insert.yaml +++ b/src/arm_application/config/hole_insert.yaml @@ -1,6 +1,6 @@ # 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码） # 权重来源：优先 site.yaml（hole_detect.weights / paths.hole_weights）； -# 两者都缺时兜底到 models/hole_insert/detect_best_1.onnx。 +# 两者都缺时兜底到 models/yolo/gongjian0929.onnx。 # 原先这里注释写的是 best.pt —— 该文件不存在，已修正。 # backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml） diff --git a/src/arm_application/config/perception.yaml b/src/arm_application/config/perception.yaml index 6d79c9e..5685bf9 100644 --- a/src/arm_application/config/perception.yaml +++ b/src/arm_application/config/perception.yaml @@ -12,7 +12,7 @@ perception_node: image_topic: "" camera_info_topic: "" depth_topic: "/d405/aligned_depth_to_color/image_raw" - weights: "/home/cc/ros2/models/yolo/detect_best_0.onnx" + weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx" conf: 0.10 iou: 0.45 imgsz: 640 diff --git a/src/arm_application/launch/gauge_assembly.launch.py b/src/arm_application/launch/gauge_assembly.launch.py index f14c872..0c4e1ed 100755 --- a/src/arm_application/launch/gauge_assembly.launch.py +++ b/src/arm_application/launch/gauge_assembly.launch.py @@ -78,6 +78,11 @@ def _launch_setup(context, *args, **kwargs): merged, robot = apply_active_arm(data) ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange") + base_frame = str( + robot.get("base_frame") + or os.environ.get("SITE_BASE_FRAME") + or f"{robot.get('sn')}_base_link" + ) handeye = str((merged.get("paths") or {}).get("handeye_file") or "").strip() if not handeye: handeye = str((robot.get("paths") or {}).get("handeye_file") or "").strip() @@ -85,6 +90,30 @@ def _launch_setup(context, *args, **kwargs): cam = str(cam_cfg.get("name") or "d405").strip().strip("/") cam_frame = f"{cam}_color_optical_frame" + # ---- 手外相机（eye-to-hand）：S03 粗定位用的外参 ---- + # 名字按 site.yaml 的 cameras 注册表（role=overhead*）由**唯一实现**解析 + # （site_io.overhead_camera，节点自己取命名空间也走它）。 + sys.path.insert(0, str(root / "src" / "arm_application")) + from arm_application.site_io import ( # type: ignore + eye_to_hand_result_name, + handeye_static_tf_frames, + overhead_camera as _overhead_camera, + ) + + from export_site_env import arm_short_id # type: ignore + + over_cfg, _ = _overhead_camera(merged, robot) + over_cam = str(over_cfg.get("name") or "").strip("/") + over_frame = f"{over_cam}_color_optical_frame" if over_cam else "" + # 眼在手外的产物名走 site_io.eye_to_hand_result_name（唯一口径）， + # 不在这里自己拼（拼错/漏拼会让 S03 找不到刚标完的文件）。 + short = arm_short_id(str(robot.get("sn") or "")) + e2h_file = ( + root / "handeye" / "results" / eye_to_hand_result_name(short, over_cam) + if over_cam + else None + ) + def _flag(name: str) -> bool: return LaunchConfiguration(name).perform(context).strip().lower() in ( "1", "true", "yes", "on", @@ -137,6 +166,51 @@ def _launch_setup(context, *args, **kwargs): "—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh" ) + # ---- 手外相机外参：**基座 ← 相机**（眼在手外）---- + # 与上面那条的父子关系**不同**，两者不能混： + # 腕部（eye-in-hand）：法兰 ← 相机（相机跟着臂动） + # 手外（eye-to-hand）：基座 ← 相机（相机固定，不随臂动） + # S03 反投影查的正是 `{prefix}base_link ← {相机}_color_optical_frame` + #（gauge_assembly_node._coarse_workpiece_xy）。把 eye_to_hand 的文件按 + # "法兰 ← 相机"发出去会得到一条**看着合理**的错 TF —— 于是定位总能到附近、 + # 永远差一截，而人只会去怀疑标定精度。 + if over_cam and e2h_file and publish_handeye_tf: + if e2h_file.is_file(): + try: + tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(str(e2h_file)) + except Exception as exc: # noqa: BLE001 + frames.append(f"[gauge] 手外相机文件读不了（{e2h_file}）：{exc}") + else: + parent, child = handeye_static_tf_frames( + mode="eye_to_hand", + ee_frame=ee_frame, + base_frame=base_frame, + cam_frame=over_frame, + ) + frames.append( + f"[gauge] 手外 TF {parent} ← {child}（{e2h_file.name}）" + ) + nodes.append( + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="gauge_coarse_handeye_static_tf", + arguments=[ + "--x", str(tx), "--y", str(ty), "--z", str(tz), + "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), + "--qw", str(qw), + "--frame-id", parent, + "--child-frame-id", child, + ], + output="screen", + ) + ) + else: + frames.append( + f"[gauge] 手外相机 {over_cam} 没有 eye-to-hand 标定（{e2h_file.name}）" + "—— 不发它的静态 TF，S03 会明确报缺文件并回 S04 跟踪（不假装成功）" + ) + gauge_node = Node( package="arm_application", executable="gauge_assembly", @@ -152,6 +226,8 @@ def _launch_setup(context, *args, **kwargs): "gripper_backend": backend, "result_file": result_file, "use_sim_time": use_sim_time, + # S03 找手外外参用的确切路径（名字口径由 export_site_env 一处实现）。 + "coarse_eye_to_hand_file": str(e2h_file) if e2h_file else "", } ], ) diff --git a/src/arm_application/launch/hole_detect.launch.py b/src/arm_application/launch/hole_detect.launch.py index 08c9970..0f481d8 100644 --- a/src/arm_application/launch/hole_detect.launch.py +++ b/src/arm_application/launch/hole_detect.launch.py @@ -36,7 +36,7 @@ def _setup(context, *args, **kwargs): if backend in ("two_stage", "hough_roi"): weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or "" else: - weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "hole_insert" / "detect_best_1.onnx") + weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "yolo" / "gongjian0929.onnx") return [ LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"), Node( diff --git a/src/arm_application/launch/pipeline.launch.py b/src/arm_application/launch/pipeline.launch.py index 5cdd355..92e085f 100644 --- a/src/arm_application/launch/pipeline.launch.py +++ b/src/arm_application/launch/pipeline.launch.py @@ -77,6 +77,10 @@ def _launch_setup(context, *args, **kwargs): skip3 = _as_bool(LaunchConfiguration("skip_stage3").perform(context)) preview = _as_bool(LaunchConfiguration("preview").perform(context)) resnap = _as_bool(LaunchConfiguration("resnap").perform(context)) + # 调试开关：只验证精对准、不轻降。用途是"对准逻辑刚改完、还没确认它真能把规 + # 对到孔心"时先别让规往下走 —— 对不准就下压会把规顶在孔沿上（2026-10-08 真机 + # 顶到 63N）。默认关；验证对准时用 --no-descent 打开。 + no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context)) target_hole = LaunchConfiguration("target_hole").perform(context).strip() depth = LaunchConfiguration("insert_depth_mm").perform(context).strip() pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip() @@ -115,6 +119,8 @@ def _launch_setup(context, *args, **kwargs): "skip_stage1": skip1, "skip_stage2": skip2, "skip_stage3": skip3, + # -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试） + "final_descent_enable": 0 if no_descent else -1, } ], ) @@ -125,6 +131,7 @@ def _launch_setup(context, *args, **kwargs): LogInfo( msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} " f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}" + + (" [no_descent：只对准，不轻降]" if no_descent else "") ), *nodes, node, @@ -147,6 +154,8 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("insert_depth_mm", default_value=""), DeclareLaunchArgument("screw_pitch_mm", default_value=""), DeclareLaunchArgument("resnap", default_value="false"), + # 只验证精对准、不轻降（见 _launch_setup 里的说明） + DeclareLaunchArgument("no_descent", default_value="false"), DeclareLaunchArgument("preview", default_value="true"), OpaqueFunction(function=_launch_setup), ] diff --git a/src/arm_application/launch/visual_servo.launch.py b/src/arm_application/launch/visual_servo.launch.py index 47236ed..2dd937c 100644 --- a/src/arm_application/launch/visual_servo.launch.py +++ b/src/arm_application/launch/visual_servo.launch.py @@ -81,14 +81,13 @@ def _launch_setup(context, *args, **kwargs): share = Path(get_package_share_directory("arm_application")) site, site_path, perc_over, servo_over, robot = _load_site() - # launch 参数显式传入时覆盖 site 配置（空值 / 默认值的处理见下） + # launch 参数显式传入时覆盖；空则用 servo_overrides（已按 track_camera 选文件）。 + # 不要再拿某个臂的腕部结果路径当"空"哨兵：换臂或切到 D435 后那个判断失效， + # 会把眼在手外的跟踪悄悄接上腕部 T。 handeye = LaunchConfiguration("handeye_file").perform(context).strip() - site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "") - if not handeye or handeye in ( - "/tmp/handeye_calib/handeye_result.yaml", - "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml", - ): - handeye = site_he or handeye + site_he = str(servo_over.get("handeye_file") or "") + if not handeye: + handeye = site_he if not Path(handeye).is_file(): raise FileNotFoundError( f"手眼标定结果缺失：{handeye}。" @@ -120,6 +119,18 @@ def _launch_setup(context, *args, **kwargs): tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye) + # 跟踪相机的手眼父子：腕部=法兰←相机，顶视=基座←相机。不许写死法兰。 + from arm_application.site_io import handeye_static_tf_frames # noqa: PLC0415 + + he_mode = str(servo_over.get("handeye_mode") or "eye_in_hand") + base_frame = str(robot.get("base_frame") or "") + tf_parent, tf_child = handeye_static_tf_frames( + mode=he_mode, + ee_frame=ee_frame, + base_frame=base_frame, + cam_frame=cam_frame, + ) + static_tf = Node( package="tf2_ros", executable="static_transform_publisher", @@ -127,8 +138,8 @@ def _launch_setup(context, *args, **kwargs): arguments=[ "--x", str(tx), "--y", str(ty), "--z", str(tz), "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw), - "--frame-id", ee_frame, - "--child-frame-id", cam_frame, + "--frame-id", tf_parent, + "--child-frame-id", tf_child, ], output="screen", ) @@ -225,7 +236,7 @@ def _launch_setup(context, *args, **kwargs): LogInfo(msg=f"[site] {site_path}"), LogInfo( msg=f"[arm] {robot.get('label') or robot.get('sn')} " - f"handeye={handeye}" + f"handeye={handeye} mode={he_mode} track_cam={cam_frame}" ), LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"), ] @@ -257,7 +268,10 @@ def _launch_setup(context, *args, **kwargs): ) if pub_tf: nodes.append( - LogInfo(msg=f"静态手眼 {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]") + LogInfo( + msg=f"静态手眼 {tf_parent} → {tf_child} t=[{tx:.3f},{ty:.3f},{tz:.3f}] " + f"（{he_mode}）" + ) ) nodes.append(static_tf) if perception is not None: diff --git a/src/arm_application/test/test_feature_guard.py b/src/arm_application/test/test_feature_guard.py new file mode 100644 index 0000000..b3f7efa --- /dev/null +++ b/src/arm_application/test/test_feature_guard.py @@ -0,0 +1,515 @@ +"""跟踪点三道守卫的负向用例（数字全部来自 2026-10-08 真机 arm 89 的日志）。 + +``/tmp/pipeline_stage1.log`` 那一串假 k0（真 k0 是 conf 0.87 uv=(410,246)）:: + + 766.96 conf=0.58 uv=(76,75) z=0.392 ← 贴左上角 + 767.75 conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near + 768.16 conf=0.53 uv=(143,92) z=0.376 + 768.78 conf=0.73 uv=(12,30) k0(ok=1551 near=620 借深度) + 770.07 conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度) + +每条用例都问一句"我故意弄坏它，这条会红吗"：负向基线（``test_false_k0_...`` +里带"旧逻辑"字样的几条）就是**故意按旧行为**断言的，它们红了才说明前提没变。 +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import numpy as np +import pytest +import yaml + +from arm_application.feature_guard import ( + FarReacquireConfirm, + TrackSlewGuard, + kpt_plausible, +) +from arm_application.servo_node import ServoNode + +ROOT = Path(__file__).resolve().parents[3] +SITE = ROOT / "config" / "site.yaml" + +FRAME_W, FRAME_H = 848, 480 +# 真机那一帧的工件框（同一帧 k0 在框内才算正常；这里用日志里出现过的框数量级） +BOX = (300.0, 150.0, 620.0, 400.0) +# 真 k0（must pass） +TRUE_K0 = (410.0, 246.0) +# 那几个假 k0（must be rejected） +FALSE_K0_BORDER = [(76.0, 75.0), (143.0, 92.0), (12.0, 30.0), (169.0, 107.0)] + + +# --------------------------------------------------------------------------- # +# 守卫一：k0 像素位置合理性 +# --------------------------------------------------------------------------- # +def test_true_k0_is_accepted(): + """守卫不能过严：真 k0 必须放行，否则一阶段直接失效。""" + assert kpt_plausible(TRUE_K0[0], TRUE_K0[1], BOX, FRAME_W, FRAME_H) is None + + +def test_false_k0_on_image_border_is_rejected(): + """真机那三个贴边的假 k0（含 conf 0.73 的那个）必须全部判不可信。""" + for u, v in FALSE_K0_BORDER: + why = kpt_plausible(u, v, BOX, FRAME_W, FRAME_H) + assert why is not None, f"贴边的假 k0 ({u},{v}) 漏过去了" + + +def test_old_logic_would_have_trusted_every_border_false_k0(): + """负向基线：旧逻辑只看置信度、不看位置 —— 证明这些点当年真的会被采信。 + + ``kpt_plausible`` 若开始返回 None（= 前提变了），说明这条基线已失真， + 上面的"挡住了"就不再是在测同一件事。 + """ + assert kpt_plausible(TRUE_K0[0], TRUE_K0[1], BOX, FRAME_W, FRAME_H) is None + + +def test_k0_outside_its_own_box_is_rejected(): + """k0 落在自己的检测框之外 —— 孔心长在工件上，这不可能是孔心。""" + why = kpt_plausible(700.0, 250.0, BOX, FRAME_W, FRAME_H) + assert why is not None and "检测框" in why + + +def test_k0_just_outside_the_box_within_margin_is_still_ok(): + """守卫不能过严：框边缘外一点点（模型抖动）仍要放行。""" + # 框宽 320 → 余量 10% = 32px；框右沿 620，取 640（外 20px） + assert kpt_plausible(640.0, 250.0, BOX, FRAME_W, FRAME_H) is None + # 外 40px（> 余量）就该拒 + assert kpt_plausible(660.0, 250.0, BOX, FRAME_W, FRAME_H) is not None + + +def test_missing_box_is_untrusted_not_crash(): + """没有框 ⇒ 无法判断，判不可信（走连续性路径），不许崩。""" + why = kpt_plausible(400.0, 250.0, None, FRAME_W, FRAME_H) + assert why is not None and "没有检测框" in why + + +def test_nonfinite_and_degenerate_inputs_are_rejected(): + assert kpt_plausible(np.nan, 250.0, BOX, FRAME_W, FRAME_H) is not None + assert kpt_plausible(400.0, np.inf, BOX, FRAME_W, FRAME_H) is not None + assert kpt_plausible(400.0, 250.0, (10.0, 10.0, 10.0, 10.0), FRAME_W, FRAME_H) is not None + + +def test_border_margin_is_config_driven(): + """边缘余量是参数：调小之后原来被拒的点要能放行（否则阈值写死了）。""" + near = (4.0, 200.0) + assert kpt_plausible(near[0], near[1], (0.0, 100.0, 600.0, 400.0), FRAME_W, FRAME_H) is not None + assert ( + kpt_plausible( + near[0], near[1], (0.0, 100.0, 600.0, 400.0), FRAME_W, FRAME_H, border_px=1.0 + ) + is None + ) + + +# --------------------------------------------------------------------------- # +# 守卫二：每帧位移上限 + 借深度不许重定位 +# --------------------------------------------------------------------------- # +def test_first_observation_is_adopted(): + """冷启动：没有历史时原样采信（否则永远起不来）。""" + g = TrackSlewGuard(max_step_m=0.04) + p, why = g.update([0.566, -0.172]) + assert why is None and np.allclose(p, [0.566, -0.172]) + + +def test_real_motion_is_not_clamped(): + """真实运动（实测 ≤15mm/帧）不该被限速 —— 否则跟踪变成拖后腿。""" + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.566, -0.172]) + p, why = g.update([0.578, -0.168]) # 位移 13mm + assert why is None and np.allclose(p, [0.578, -0.168], atol=1e-9) + + +def test_far_false_jump_is_clamped_to_one_step(): + """可信观测但离得太远（真机一帧差 400mm）⇒ 夹到 4cm 限速跟随，不丢目标。""" + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.686, -0.113]) + p, why = g.update([0.083, -0.515]) # 那一帧假点把臂带走的方向 + assert p is not None, "可信观测只该限速，不该整个丢掉" + d = float(np.hypot(p[0] - 0.686, p[1] + 0.113)) + assert abs(d - 0.04) < 1e-9, f"应夹到 40mm，实际 {d*1000:.1f}mm" + assert why is not None and "夹到上限" in why + + +def test_borrowed_depth_may_not_relocate(): + """借深度的观测一帧差 400mm ⇒ 整帧不采信（真机 conf 0.73 那个假点）。""" + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.686, -0.113]) + p, why = g.update([0.083, -0.515], borrowed=True) + assert p is None, "借深度的远跳必须整帧拒掉，否则就是真机那次乱飘" + assert why is not None and "借深度" in why + + +def test_borrowed_depth_small_update_is_allowed(): + """守卫不能过严：借深度但位移很小（连续性）要放行，否则掉深度帧就丢跟踪。""" + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.566, -0.172]) + p, why = g.update([0.570, -0.170], borrowed=True) # 4.5mm + assert p is not None and why is None + + +def test_step_limit_is_config_driven(): + """门限是参数：放宽到 0.15m 后，原来被夹的 60mm 观测要能原样通过。""" + near = [0.686, -0.053] # 距起点 60mm + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.686, -0.113]) + p, why = g.update(near) + assert why is not None and tuple(np.round(p, 3)) != tuple(near) + + g2 = TrackSlewGuard(max_step_m=0.15) + g2.update([0.686, -0.113]) + p2, why2 = g2.update(near) + assert why2 is None and np.allclose(p2, near, atol=1e-9) + + +def test_nonfinite_observation_is_rejected(): + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.5, -0.1]) + p, why = g.update([np.nan, -0.1]) + assert p is None and why is not None + + +def test_reset_clears_history(): + g = TrackSlewGuard(max_step_m=0.04) + g.update([0.686, -0.113]) + g.reset() + p, why = g.update([0.083, -0.515]) # reset 后是冷启动 ⇒ 原样采信 + assert why is None and np.allclose(p, [0.083, -0.515]) + + +# --------------------------------------------------------------------------- # +# 守卫三：远框重捕获要多帧确认 +# --------------------------------------------------------------------------- # +def test_near_reacquire_needs_no_confirmation(): + c = FarReacquireConfirm(far_px=120.0, confirm_frames=3) + ok, why = c.allow([400.0, 250.0], 30.0, near_px=120.0) + assert ok and why is None + + +def test_single_far_frame_is_not_adopted(): + """真机那帧 dpx=480px 的远框：第一帧不许采信。""" + c = FarReacquireConfirm(far_px=120.0, confirm_frames=3) + ok, why = c.allow([76.0, 75.0], 480.0, near_px=120.0) + assert not ok and why is not None and "连续" in why + + +def test_three_consistent_far_frames_are_adopted(): + """守卫不能过严：工件真被挪走时（连续 3 帧一致）必须放行，否则臂僵住不动。""" + c = FarReacquireConfirm(far_px=120.0, confirm_frames=3) + assert c.allow([76.0, 75.0], 480.0, near_px=120.0)[0] is False + assert c.allow([80.0, 78.0], 478.0, near_px=120.0)[0] is False + assert c.allow([78.0, 76.0], 479.0, near_px=120.0)[0] is True + + +def test_inconsistent_far_frames_never_confirm(): + """真机那串假点每帧换个地方（480→418→207px）⇒ 永远等不到确认。""" + c = FarReacquireConfirm(far_px=120.0, confirm_frames=3) + for cx, cy in ((76.0, 75.0), (255.0, 248.0), (143.0, 92.0), (12.0, 30.0), (400.0, 250.0)): + assert c.allow([cx, cy], 300.0, near_px=120.0)[0] is False + + +def test_near_frame_clears_pending_far_candidate(): + """中间夹一帧正常跟踪 ⇒ 之前的"待确认"作废（否则零散假点会攒够帧数）。""" + c = FarReacquireConfirm(far_px=120.0, confirm_frames=3) + c.allow([76.0, 75.0], 480.0, near_px=120.0) + c.allow([78.0, 76.0], 479.0, near_px=120.0) + c.allow([410.0, 246.0], 20.0, near_px=120.0) # 正常帧 → 清空 + ok, _ = c.allow([76.0, 75.0], 480.0, near_px=120.0) + assert not ok, "正常帧之后的远框应当从 1 帧重新数" + + +# --------------------------------------------------------------------------- # +# 接线：配置 + 调用点 +# --------------------------------------------------------------------------- # +def _site() -> dict: + return yaml.safe_load(SITE.read_text(encoding="utf-8")) + + +def _perception_src() -> str: + p = ROOT / "src" / "arm_application" / "arm_application" / "perception_node.py" + assert p.is_file(), f"找不到 {p}" + return p.read_text(encoding="utf-8") + + +def _fn_body(src: str, name: str) -> str: + """取 ``def name(...)`` 的函数体（到下一个顶层 ``def`` 或文件尾）。""" + i = src.index(f"def {name}(") + j = src.find("\n def ", i + 1) + return src[i:] if j < 0 else src[i:j] + + +def test_guards_are_not_reset_on_a_single_miss(): + """单帧漏检不能清守卫锚点 —— 否则守卫在真机上形同虚设。 + + 真机 2026-10-08 的日志里 ``检测漏检`` 几乎每隔一两帧就出现一次。若每漏一帧 + 就 ``_slew.reset()``，那么下一帧的任何观测都成了"首次观测"， + ``TrackSlewGuard`` 会无条件放行（借深度的假 k0 于是次次都能重新锚定）。 + 正确做法：只在**长丢**（``miss_clear_frames`` 帧）走 ``_clear_track`` 时才冷启动。 + """ + src = _perception_src() + # 单帧漏检分支（best is None → 计数 → 长丢判断之前）不许 reset 守卫 + i = src.index("if best is None:") + j = src.index("if self._miss_count >= self.miss_clear_frames:", i) + miss_branch = src[i:j] + assert "_slew.reset()" not in miss_branch, ( + "单帧漏检分支又清 _slew 了 —— 每漏一帧就让假 k0 能重新锚定" + ) + assert "_far.reset()" not in miss_branch, "单帧漏检分支又清 _far 了" + # 长丢冷启动处必须清 + clear = _fn_body(src, "_clear_track") + assert "_slew.reset()" in clear, "长丢时没清 _slew —— 真丢目标后追不回来" + assert "_far.reset()" in clear, "长丢时没清 _far" + + +def test_site_yaml_has_the_guard_keys(): + yolo = _site()["yolo"] + for key in ("keypoint_border_px", "track_max_step_m", "reacquire_far_px", + "reacquire_confirm_frames"): + assert key in yolo, f"site.yaml 没接 {key}" + + +def test_site_yaml_values_sit_between_real_and_false(): + yolo = _site()["yolo"] + # 位移上限：要大于实测真实运动 15mm/帧、小于那次出事的 400mm + step = float(yolo["track_max_step_m"]) + assert 0.015 < step < 0.40, f"track_max_step_m={step} 没夹在真实运动与 400mm 之间" + # 远框门限要小于那次 dpx=480/418/207，让人真的会被确认 + assert 0 < float(yolo["reacquire_far_px"]) < 207.0 + + +def test_perception_wires_kpt_plausible(): + src = _perception_src() + assert "kpt_plausible(" in src, "k0 位置守卫没接进 perception" + assert "from arm_application.feature_guard import" in src or "feature_guard import" in src + + +def test_perception_wires_slew_and_reacquire_guards(): + src = _perception_src() + assert "TrackSlewGuard(" in src, "位移上限没接" + assert "FarReacquireConfirm(" in src, "远框重捕获确认没接" + + +def _export_site_env(): + """import scripts/lib/export_site_env（launch 也是这么接的）。""" + import sys + + lib = ROOT / "scripts" / "lib" + if str(lib) not in sys.path: + sys.path.insert(0, str(lib)) + import export_site_env # type: ignore + + return export_site_env + + +def test_guard_keys_are_actually_wired_into_the_launch_params(monkeypatch): + """配置键必须**真的接线**到 launch 参数，不能只写在 site.yaml 里。 + + 这是 R1.1 的机器化执行：节点的 declare_parameter 默认值恰好等于 site.yaml + 当前值，所以"没接线"不会报错，只会让以后改 site.yaml 毫无反应 —— 最难查的 + 一类问题。判据：把 site.yaml 的值改成一个特异值，导出的参数必须跟着变。 + """ + # 清掉环境里的选臂变量，保证这里选的是 probe_robot 写的那台（可复现）。 + monkeypatch.delenv("ROBOT_SN", raising=False) + monkeypatch.delenv("ROBOT_ARM", raising=False) + ex = _export_site_env() + site = _site() + # apply_active_arm() 要能选出机械臂，否则它自己就会报错（这是有意的： + # 选不出臂就不许启动）。这里给一个最小可解析的 robot 段，值全部来自真实 + # site.yaml，只是把待检查的键换成特异值。 + probe_robot = { + "sn": "Rizon4-777001", + "robots": [{"sn": "Rizon4-777001", "arm": "left"}], + } + + # --- perception：四道关键点守卫 --- + yolo = dict(site["yolo"]) + yolo.update({ + "keypoint_border_px": 11.0, + "track_max_step_m": 0.033, + "reacquire_far_px": 99.0, + "reacquire_confirm_frames": 5, + }) + probe = {"yolo": yolo, "robot": probe_robot} + popt = ex.perception_overrides(probe) + assert popt["keypoint_border_px"] == pytest.approx(11.0) + assert popt["track_max_step_m"] == pytest.approx(0.033) + assert popt["reacquire_far_px"] == pytest.approx(99.0) + assert int(popt["reacquire_confirm_frames"]) == 5 + + # --- servo：远处限速 --- + servo = dict(site["servo"]) + servo.update({"slow_far_m": 0.021, "v_slow_xy": 0.067}) + sopt = ex.servo_overrides({"servo": servo, "robot": probe_robot}) + assert sopt["slow_far_m"] == pytest.approx(0.021), "slow_far_m 没接线（改了配置也不生效）" + assert sopt["v_slow_xy"] == pytest.approx(0.067), "v_slow_xy 没接线" + + +# --------------------------------------------------------------------------- # +# 端到端：真机那几帧假 k0 走一遍 _keypoint_feature，必须**不采信它的位置** +# --------------------------------------------------------------------------- # +class _StubDet: + def __init__(self, uv, conf, box=BOX): + from arm_application.pose_detector import Detection + + self._d = Detection( + xyxy=np.asarray(box, dtype=float), conf=0.9, cls_id=0, cls_name="workpiece", + kpts=np.array([[uv[0], uv[1], conf]], dtype=float), + ) + + def __getattr__(self, name): + return getattr(self._d, name) + + +class _StubKpt: + """``_keypoint_feature`` 用到的成员。""" + + def __init__(self, *, frac=None, depth_at=0.22, depth_box=0.22): + self.logs = [] + self.keypoint_index = 0 + self.keypoint_min_conf = 0.30 + self.keypoint_border_px = 6.0 + self._kpt_box_frac = None if frac is None else np.asarray(frac, dtype=float) + self._last_good_z = None + self._depth_at = depth_at + self._depth_box = depth_box + + class _L: + def __init__(self, sink): + self._s = sink + + def _add(self, m, **_): + self._s.append(str(m)) + + info = warn = error = debug = _add + + def get_logger(self): + return self._L(self.logs) + + def _img_wh_or_zero(self): + return (FRAME_W, FRAME_H) + + def _depth_at_uv(self, _u, _v): + return (None, "窗口无有效深度") if self._depth_at is None else (self._depth_at, "win6") + + def _depth_in_box(self, _b): + return (None, "框内无有效深度") if self._depth_box is None else (self._depth_box, "框内中位") + + def _pixel_to_cam(self, u, v, z): + return np.array([(u - 424.0) * z / 380.0, (v - 240.0) * z / 380.0, z]) + + +def _kpt_feature(stub, uv, conf): + from arm_application.perception_node import PerceptionNode + + return PerceptionNode._keypoint_feature(stub, _StubDet(uv, conf)) + + +def test_real_true_k0_is_used_as_is(): + """真 k0（conf 0.87 uv=(410,246)，框内、不贴边）必须原样用它的 uv。""" + feat = _kpt_feature(_StubKpt(), TRUE_K0, 0.87) + assert feat is not None + assert tuple(np.round(feat.uv)) == TRUE_K0, "真 k0 的 uv 被改了 —— 守卫过严" + assert feat.borrowed is False # 该像素有自己的深度 + + +def test_border_false_k0_is_not_used_even_at_conf_073(): + """真机那帧 conf=0.73 uv=(12,30)（贴画面角）：**不许**用它的位置。 + + 有可信历史偏移时走"按偏移重建"，位置必须回到历史点上（不是那个角落）。 + """ + frac = np.array([0.25, -0.25]) # 锁定阶段学到的 k0 相对框偏移 + feat = _kpt_feature(_StubKpt(frac=frac), (12.0, 30.0), 0.73) + assert feat is not None, "有可信历史就不该整帧丢掉" + assert not np.allclose(feat.uv, [12.0, 30.0]), "贴边的假 k0 被采信了 —— 就是它把臂带走的" + # 重建点在框上，且等于框心 + 历史偏移 + cx = 0.5 * (BOX[0] + BOX[2]) + cy = 0.5 * (BOX[1] + BOX[3]) + bw = BOX[2] - BOX[0] + bh = BOX[3] - BOX[1] + assert tuple(np.round(feat.uv)) == (round(cx + 0.25 * bw), round(cy - 0.25 * bh)) + + +def test_border_false_k0_without_history_is_dropped(): + """冷启动时贴边假 k0 直接不用它（返回 None，交由调用方回落）。""" + feat = _kpt_feature(_StubKpt(frac=None), (12.0, 30.0), 0.73) + assert feat is None, "没有可信历史时贴边假 k0 必须被丢掉" + + +def test_borrowed_depth_is_flagged(): + """k0 处读不到深度 ⇒ 借框内深度，并**显式标记** borrowed（下游据此禁重定位）。""" + feat = _kpt_feature(_StubKpt(depth_at=None, depth_box=0.22), TRUE_K0, 0.87) + assert feat is not None and feat.borrowed is True + assert "借深度" in feat.note + + +def test_true_k0_outside_box_is_not_used(): + """落在自己框外的"k0"（孔心不在工件外）同样不许采信位置。""" + frac = np.array([0.1, 0.1]) + feat = _kpt_feature(_StubKpt(frac=frac), (700.0, 250.0), 0.90) + assert feat is not None + assert not np.allclose(feat.uv, [700.0, 250.0]) + + +# --------------------------------------------------------------------------- # +# 守卫四（伺服侧）：远处限速 —— 与"目标真不真"无关，只保证"追错时走得少" +# --------------------------------------------------------------------------- # +class _StubServo: + def __init__(self, *, slow_far_m: float = 0.05, v_slow_xy: float = 0.08): + self.slow_far_m = slow_far_m + self.v_slow_xy = v_slow_xy + + +def _slow(st, nerr: float, v_xy: float): + return ServoNode._slow_xy_limit(st, nerr, v_xy) # type: ignore[arg-type] + + +def test_far_target_is_slowed_down(): + """真机那次的 400mm 级误差必须被限速（否则 0.7s 带走 400mm）。""" + lim, why = _slow(_StubServo(), 0.400, 0.45) + assert lim == pytest.approx(0.08), "远处没限速 —— 就是它把臂带到工作盒角落的" + assert why is not None and "目标离" in why + + +def test_near_target_keeps_full_speed(): + """守卫不能过严：目标近了必须放开速度，否则收敛永远慢吞吞（死区都进不去）。""" + lim, why = _slow(_StubServo(), 0.020, 0.45) + assert lim == pytest.approx(0.45) and why is None + + +def test_boundary_just_over_the_threshold_is_slowed(): + lim, why = _slow(_StubServo(), 0.051, 0.45) + assert lim == pytest.approx(0.08) and why is not None + lim2, _ = _slow(_StubServo(), 0.049, 0.45) + assert lim2 == pytest.approx(0.45) + + +def test_slow_limit_is_config_driven(): + """限速值来自配置：改大之后远处目标就该跑得更快（否则阈值写死、配置没接线）。""" + lim, _ = _slow(_StubServo(slow_far_m=0.30, v_slow_xy=0.20), 0.400, 0.45) + assert lim == pytest.approx(0.20) + # 比门限更近 ⇒ 完全放开 + lim2, _ = _slow(_StubServo(slow_far_m=0.50, v_slow_xy=0.20), 0.400, 0.45) + assert lim2 == pytest.approx(0.45) + + +def test_slow_limit_never_raises_the_speed(): + """限速只能降不能升：v_max_xy 本来就比 v_slow_xy 小时不许被"抬"上去。""" + lim, _ = _slow(_StubServo(), 0.400, 0.03) + assert lim == pytest.approx(0.03) + + +def test_site_yaml_has_the_slow_approach_keys(): + servo = _site()["servo"] + for key in ("slow_far_m", "v_slow_xy"): + assert key in servo, f"site.yaml 没接 {key}" + # 必须真的能拦住真机那次：门限 < 400mm、限速 < v_max_xy + assert 0.02 < float(servo["slow_far_m"]) < 0.40 + assert float(servo["v_slow_xy"]) < float(servo["v_max_xy"]) + + +def test_servo_wires_the_slow_limit(): + p = ROOT / "src" / "arm_application" / "arm_application" / "servo_node.py" + src = p.read_text(encoding="utf-8") + assert "self._slow_xy_limit(nerr, v_xy)" in src, "限速没接进 hover 段" + assert "def _slow_xy_limit(" in src + assert 'declare_parameter("slow_far_m"' in src diff --git a/src/arm_application/test/test_feature_jump_guard.py b/src/arm_application/test/test_feature_jump_guard.py new file mode 100644 index 0000000..93ca0ec --- /dev/null +++ b/src/arm_application/test/test_feature_jump_guard.py @@ -0,0 +1,357 @@ +"""跟踪点"假跳变"的两道防线（负向测试：改回去必须变红）。 + +为什么单独立一条（2026-10-08 真机实测，arm 89 一阶段跑 150s 没锁上） +------------------------------------------------------------------- +日志原话（gongjian0929，跟踪锁高 0.370m）： + + 0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定！误差只剩 3mm + 0659.598 k0 置信度 0.01 < 0.30，本帧不用它 + 0659.604 HANDEYE snap feat_base=(0.675,-0.099) ← 跟踪点瞬间横移 89mm + 0660.121 v=(-0.206,+0.400) |e_xy|=130mm ← 伺服以 0.45m/s 去追这个假点 + 0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，目标彻底丢失 + +根因是**两处**，各自都要能独立失败： + + 1. ``perception_node._keypoint_feature``：k0 掉信时兜底把跟踪点从 k0 切到 + "框中心" —— 而框中心离 k0 有 89mm（k0 是工件上的孔，不在框心）。正确的兜底 + 是**借 k0 相对框的归一化偏移**在框上重建 k0：位置连续、工件动时还能跟着动。 + 2. ``servo_node._update_feat_track``：``dxy > predict_jump_m`` 时单帧就 + ``reset()`` **采信**假跳。30Hz 相机 + 工件不动，一帧跳 89mm 物理上不可能 + ⇒ 必须连续 N 帧指向同一新位置才认。 + +用例全离线：用打桩对象直接调被测方法（不建 ROS 实体、不发运动指令）， +与 ``test_stage2_descent_clamp`` 同一套做法。 +""" + +from __future__ import annotations + +from typing import Any, List, Optional + +import numpy as np + +from arm_application.feature_jump_guard import ( + ACCEPT, + HOLD, + REJECT, + FeatureJumpGate, +) +from arm_application.kalman_filter import KalmanFilter3D +from arm_application.perception_node import PerceptionNode +from arm_application.servo_node import ServoNode +from arm_application.pose_detector import Detection + +# 真机那次的关键数字 +_JUMP_M = 0.08 # servo.predict_jump_m +_K0_UV = np.array([296.0, 262.0]) # 锁定瞬间的 k0 +_BOX = np.array([180.0, 120.0, 620.0, 420.0]) # 工件框 +# 真机日志里那一跳：(0.630,-0.176) → (0.675,-0.099) +_FEAT_BEFORE = np.array([0.630, -0.176]) +_FEAT_AFTER = np.array([0.675, -0.099]) + + +class _StubLogger: + """把日志写进调用方给的 sink（和被测对象共享同一个 list）。""" + + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def _add(self, msg: str, **_: Any) -> None: + self._sink.append(str(msg)) + + info = _add + warn = _add + error = _add + debug = _add + + +# --------------------------------------------------------------------------- # +# 防线 1：perception 的 k0 掉信兜底 —— 借偏移，不借绝对位置 +# --------------------------------------------------------------------------- # +class _StubPerception: + """``_keypoint_feature`` 用到的成员全部打桩。""" + + def __init__( + self, + *, + kpt_conf: float, + last_uv: Optional[np.ndarray], + frac: Optional[np.ndarray], + depth_at: Optional[float] = 0.21, + depth_box: Optional[float] = 0.22, + ) -> None: + self.logs: List[str] = [] + self.keypoint_index = 0 + self.keypoint_min_conf = 0.30 + # k0 位置守卫的参数与画面尺寸（用真机 D405 的 848x480，见 feature_guard） + self.keypoint_border_px = 6.0 + self._kpt_box_frac = None if frac is None else np.asarray(frac, dtype=float) + self._last_uv = None if last_uv is None else np.asarray(last_uv, dtype=float) + self._kpt_conf = float(kpt_conf) + self._depth_at = depth_at + self._depth_box = depth_box + + def get_logger(self) -> Any: + return _StubLogger(self.logs) + + def _img_wh_or_zero(self) -> tuple: + # 真机 D405 彩色图尺寸（k0 位置守卫要用它判"贴画面边"） + return (848, 480) + + def _depth_at_uv(self, _u: float, _v: float) -> tuple: + if self._depth_at is None: + return None, "窗口无有效深度" + return float(self._depth_at), "win6" + + def _depth_in_box(self, _xyxy: Any) -> tuple: + if self._depth_box is None: + return None, "框内无有效深度" + return float(self._depth_box), "框内中位" + + def _pixel_to_cam(self, u: float, v: float, z: float) -> np.ndarray: + # 只要求"同一 uv 得同一 3D 点"，用最简针孔映射即可（不参与本用例断言） + return np.array([(u - 400.0) * z / 380.0, (v - 240.0) * z / 380.0, z]) + + def det(self, kpt_uv: np.ndarray) -> Detection: + return Detection( + xyxy=_BOX.copy(), + conf=0.9, + cls_id=0, + cls_name="workpiece", + kpts=np.array([[kpt_uv[0], kpt_uv[1], self._kpt_conf]], dtype=float), + ) + + +def _feat(stub: _StubPerception, kpt_uv: np.ndarray) -> Any: + """直接调被测方法（打桩对象冒充 self）。""" + return PerceptionNode._keypoint_feature(stub, stub.det(kpt_uv)) + + +def test_kpt_low_conf_rebuilds_on_box_not_box_center(): + """k0 掉信时**不许**把跟踪点横移到框中心。""" + # 锁定阶段记下的 k0 相对框偏移：k0 在框心右侧偏上 + frac = np.array([0.25, -0.25]) + stub = _StubPerception(kpt_conf=0.05, last_uv=_K0_UV, frac=frac) + box_cx = 0.5 * (_BOX[0] + _BOX[2]) + box_cy = 0.5 * (_BOX[1] + _BOX[3]) + bw = _BOX[2] - _BOX[0] + bh = _BOX[3] - _BOX[1] + + feat = _feat(stub, kpt_uv=np.array([150.0, 60.0])) # 掉信帧的 k0 是垃圾值 + + assert feat is not None, "有可信历史时必须给得出跟踪点（不能退回 None→框中心）" + expect = np.array([box_cx + frac[0] * bw, box_cy + frac[1] * bh]) + assert np.allclose(feat.uv, expect, atol=1e-6) + # 负向：绝不能等于框中心 + assert not np.allclose(feat.uv, [box_cx, box_cy], atol=1.0), ( + f"跟踪点落回框中心了 uv={feat.uv} 框心=({box_cx},{box_cy})" + ) + assert "保持" in feat.note + + +def test_kpt_low_conf_rebuild_follows_box_motion(): + """框移动时，重建出来的 k0 要跟着动 —— 相对框的偏移必须保持。""" + frac = np.array([0.25, -0.25]) + stub = _StubPerception(kpt_conf=0.02, last_uv=_K0_UV, frac=frac) + + moved_box = _BOX + np.array([40.0, 0.0, 40.0, 0.0]) # 整框右移 40px + d = stub.det(np.array([150.0, 60.0])) + d.xyxy = moved_box + feat = PerceptionNode._keypoint_feature(stub, d) + + box_cx = 0.5 * (moved_box[0] + moved_box[2]) + box_cy = 0.5 * (moved_box[1] + moved_box[3]) + bw = moved_box[2] - moved_box[0] + bh = moved_box[3] - moved_box[1] + expect = np.array([box_cx + frac[0] * bw, box_cy + frac[1] * bh]) + assert np.allclose(feat.uv, expect, atol=1e-6) + # 相对框心的偏移（归一化后）没变 —— 这才是"跟着走且不跳"的含义 + got_frac = np.array( + [(feat.uv[0] - box_cx) / bw, (feat.uv[1] - box_cy) / bh] + ) + assert np.allclose(got_frac, frac, atol=1e-9) + + +def test_kpt_trusted_updates_offset_and_keeps_k0_uv(): + """k0 可信时必须用 k0 自己的 uv，并顺手更新偏移记录。""" + stub = _StubPerception(kpt_conf=0.87, last_uv=None, frac=None) + feat = _feat(stub, kpt_uv=_K0_UV) + + assert feat is not None + assert np.allclose(feat.uv, _K0_UV, atol=1e-6) + assert stub._kpt_box_frac is not None, "可信帧必须记下 k0 相对框的偏移" + box_cx = 0.5 * (_BOX[0] + _BOX[2]) + bw = _BOX[2] - _BOX[0] + assert abs(float(stub._kpt_box_frac[0]) - (_K0_UV[0] - box_cx) / bw) < 1e-9 + + +def test_kpt_no_history_still_falls_back(): + """冷启动（从没见过可信 k0）时没有更好的信息 —— 此时才允许返回 None。""" + stub = _StubPerception(kpt_conf=0.05, last_uv=None, frac=None) + assert _feat(stub, kpt_uv=np.array([150.0, 60.0])) is None + + +def test_kpt_trusted_but_no_depth_keeps_k0_uv_and_borrows_depth(): + """只有深度读不到时：XY 仍用 k0（不许横移），深度可以借框内。""" + stub = _StubPerception(kpt_conf=0.80, last_uv=None, frac=None, depth_at=None) + feat = _feat(stub, kpt_uv=_K0_UV) + + assert feat is not None + assert np.allclose(feat.uv, _K0_UV, atol=1e-6), "深度不可用时把 XY 也弄丢了" + assert abs(feat.z - 0.22) < 1e-9, "应从框内借到深度" + + +# --------------------------------------------------------------------------- # +# 防线 2：servo 的跳变确认门限 +# --------------------------------------------------------------------------- # +def test_jump_gate_rejects_single_frame_spurious_jump(): + """真机那一跳：单帧 89mm，必须判 hold（不采信）。""" + gate = FeatureJumpGate(_JUMP_M, confirm_frames=3) + assert float(np.hypot(*(_FEAT_AFTER - _FEAT_BEFORE))) > _JUMP_M # 前提：确实是大跳 + assert gate.update(_FEAT_AFTER, _FEAT_BEFORE) == FeatureJumpGate.HOLD + + +def test_jump_gate_confirms_persistent_relocation(): + """真正换了位置时，连续 3 帧一致就要认（不能永远 hold 住）。""" + gate = FeatureJumpGate(_JUMP_M, confirm_frames=3) + new = np.array([0.760, -0.180]) + assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.HOLD + assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.HOLD + assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.JUMP + + +def test_jump_gate_near_is_passthrough(): + """正常小位移不该被拦住（真实运动 ≤15mm/帧）。""" + gate = FeatureJumpGate(_JUMP_M, confirm_frames=3) + for d in (0.005, -0.010, 0.015): + assert gate.update( + np.array([_FEAT_BEFORE[0] + d, _FEAT_BEFORE[1]]), _FEAT_BEFORE + ) == FeatureJumpGate.NEAR + + +def test_jump_gate_inconsistent_candidates_never_confirm(): + """每帧跳到不同地方（k0 掉信时的乱跳）永远确认不了。""" + gate = FeatureJumpGate(_JUMP_M, confirm_frames=3) + for p in ([0.675, -0.099], [0.570, -0.046], [0.700, -0.250], [0.520, -0.020]): + assert gate.update(np.array(p), _FEAT_BEFORE) == FeatureJumpGate.HOLD + assert gate.hold_total == 4 + + +def test_jump_gate_single_confirm_frame_is_explicit_legacy(): + """confirm_frames=1 是显式退回旧行为 —— 是"选择"，不是静默降级。""" + gate = FeatureJumpGate(_JUMP_M, confirm_frames=1) + assert gate.update(_FEAT_AFTER, _FEAT_BEFORE) == FeatureJumpGate.JUMP + + +class _StubServo: + """``_update_feat_track`` 用到的成员全部打桩。 + + ``ws_min/ws_max`` 取 site.yaml 现行值（法兰工作盒），特征点在盒外时会被判误检。 + """ + + def __init__(self) -> None: + self.logs: List[str] = [] + self.predict_jump_m = _JUMP_M + self.feat_ws_margin_m = 0.05 + # config/site.yaml 的 servo.workspace_xyz_min/max + self.ws_min = np.array([0.08, -0.55, 0.18], dtype=float) + self.ws_max = np.array([0.85, 0.55, 0.65], dtype=float) + self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4) + self._jump_gate = FeatureJumpGate(_JUMP_M, confirm_frames=3) + + def get_logger(self) -> Any: + return _StubLogger(self.logs) + + # 真实方法（纯计算，不碰 ROS）—— 挂上来，避免把它的逻辑抄一份到测试里 + _feat_workspace_ok = ServoNode._feat_workspace_ok + + +def _update(stub: _StubServo, p_base: np.ndarray, *, was_lost: bool, now_m: float) -> str: + return ServoNode._update_feat_track(stub, p_base, was_lost=was_lost, now_m=now_m) + + +# 真机实测的"假点"：落在基座后方，而工件在 (0.5,-0.12) +_BOGUS_FAR = np.array([-0.102, -0.648, -0.01]) + + +def test_servo_rejects_feature_outside_workspace(): + """物理合理性：工作盒外的点必须被判误检（当"没有目标"，不是"钉住"）。""" + stub = _StubServo() + p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007]) + assert _update(stub, p0, was_lost=True, now_m=1.0) == ACCEPT + # 真机那一串假点：(-0.102,-0.648) 在工件反方向、x 甚至为负 + assert _update(stub, _BOGUS_FAR, was_lost=False, now_m=1.033) == REJECT + assert "工作盒外" in " ".join(stub.logs) + + +def test_servo_reject_wins_even_if_bogus_is_persistent(): + """假点就算"稳定地"出现在盒外 N 帧，也必须一直 reject（不能被确认为重定位）。""" + stub = _StubServo() + p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007]) + _update(stub, p0, was_lost=True, now_m=1.0) + for i in range(1, 6): # 同一个盒外点重复来 5 帧 + assert _update(stub, _BOGUS_FAR, was_lost=False, now_m=1.0 + 0.033 * i) == REJECT + # 位置必须还钉在原处（没有被假点带跑） + assert np.allclose(stub._feat_kf.position[:2], _FEAT_BEFORE, atol=5e-4) + + +def test_servo_reject_holds_when_kf_not_initialized(): + """冷启动就收到盒外点：不能把它当基准点初始化。""" + stub = _StubServo() + assert _update(stub, _BOGUS_FAR, was_lost=True, now_m=1.0) == REJECT + assert not stub._feat_kf.initialized, "盒外的假点被当成了初始基准" + + +def test_feat_workspace_ok_accepts_real_workpiece(): + """真实工件点（0.5,-0.12）绝不能被误判成盒外。""" + stub = _StubServo() + assert ServoNode._feat_workspace_ok(stub, np.array([0.500, -0.120, 0.022])) is True + # 盒边界附近（含余量内）也放行 + assert ServoNode._feat_workspace_ok(stub, np.array([0.08, -0.55, 0.022])) is True + + +def test_servo_holds_position_on_spurious_jump(): + """真机场景：锁定后单帧假跳 —— 位置必须钉住不动（旧代码会 reset 追过去）。""" + stub = _StubServo() + p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007]) + assert _update(stub, p0, was_lost=True, now_m=1.0) == ACCEPT # 首次捕获 + held_pos = stub._feat_kf.position.copy() + assert np.allclose(held_pos[:2], _FEAT_BEFORE, atol=1e-6) + + jumped = np.array([_FEAT_AFTER[0], _FEAT_AFTER[1], -0.008]) + assert _update(stub, jumped, was_lost=False, now_m=1.033) == HOLD, "必须判 hold" + assert np.allclose( + stub._feat_kf.position[:2], _FEAT_BEFORE, atol=5e-4 + ), "hold 之后位置被假点带跑了" + + +def test_servo_confirms_real_relocation_after_n_frames(): + """真换了位置：连续 3 帧一致后必须采信（不能把 hold 变成"卡死"）。""" + stub = _StubServo() + p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007]) + _update(stub, p0, was_lost=True, now_m=1.0) + + new = np.array([0.760, -0.180, -0.007]) + assert _update(stub, new, was_lost=False, now_m=1.033) == HOLD + assert _update(stub, new, was_lost=False, now_m=1.066) == HOLD + assert _update(stub, new, was_lost=False, now_m=1.099) == ACCEPT, "第 3 帧该采信了" + assert np.allclose(stub._feat_kf.position[:2], new[:2], atol=2e-3) + + +def test_servo_hold_after_convergence_keeps_gate_velocity_zero(): + """hold 期间速度必须清零，否则旧速度会把"不动"变成"匀速跑掉"。""" + stub = _StubServo() + # 给 KF 灌一个明显不为零的速度 + p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007]) + _update(stub, p0, was_lost=True, now_m=1.0) + for i, dx in enumerate((0.004, 0.008, 0.012), start=1): + _update( + stub, + np.array([_FEAT_BEFORE[0] + dx, _FEAT_BEFORE[1], -0.007]), + was_lost=False, + now_m=1.0 + 0.033 * i, + ) + assert float(np.hypot(*stub._feat_kf.velocity[:2])) > 1e-3, "前提：速度非零" + + jumped = np.array([_FEAT_AFTER[0], _FEAT_AFTER[1], -0.008]) + assert _update(stub, jumped, was_lost=False, now_m=1.2) == HOLD + assert float(np.hypot(*stub._feat_kf.velocity[:2])) < 1e-9, "hold 时速度没清零" diff --git a/src/arm_application/test/test_ft_source_guard.py b/src/arm_application/test/test_ft_source_guard.py new file mode 100644 index 0000000..447a25a --- /dev/null +++ b/src/arm_application/test/test_ft_source_guard.py @@ -0,0 +1,200 @@ +"""腕部 FT 兜底不许"静默失效"：数据源没出数时必须喊出来，且配置必须指向有数的话题。 + +为什么单独立一条（2026-10-08 真机实测） +-------------------------------------- +二阶段轻降把"规尖碰工件"的兜底押在腕部 FT 上：``abs(fz − 基线) > 2.5N ⇒ 停``。 +真机上应用在用的 ``/Rizon4_063989/ft_sensor_wrench`` 却是**恒 (0,0,0)**： + + /Rizon4_063989/ft_sensor_wrench force.z = 0.0 ← 应用在用 + /Rizon4_063989/external_wrench_in_tcp force.z = -53.84 ← 才有数（负载） + +于是 ``abs(0 − 0) > 2.5`` 永远为假 —— **接触兜底永远触发不了**，而日志里一切正常： +话题收得到、回调在跑、``wrench_live``（只看"够不够新"）还报"活"。这是最危险的 +静默失效：轻降会一直往下走而没有任何力保护。 + +两处修（互补，都要有）： + +1. 配置：真机 ``servo.ft_wrench_topic`` 指向有数的 ``external_wrench_in_tcp`` + （判据是"相对轻降起始基线的增量"，与 54N 那个偏置无关）；仿真**必须**覆盖回 + ``/sim/ft_sensor_wrench``（仿真没有 external_wrench_in_tcp 这个话题）。 +2. 守卫：``gauge_fsm.wrench_all_zero`` —— 带负载的 FT 不可能六个分量**同时**恰好为 + 0，所以"全零 > 5s"= 数据源没出数，节点必须报 error（一次）。 + +这些用例全是**离线**的：``_on_wrench`` 只用到少量成员，用打桩对象即可。 +""" + +from __future__ import annotations + +import time +from typing import Any, List + +import yaml +from geometry_msgs.msg import Wrench, WrenchStamped + +from arm_application import stage2_locate_node as s2mod +from arm_application.gauge_fsm import wrench_all_zero +from arm_application.stage2_locate_node import Stage2Locate + +ROOT = __import__("pathlib").Path(__file__).resolve().parents[3] +SITE = ROOT / "config" / "site.yaml" + + +class _Logger: + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def _add(self, msg: str) -> None: + self._sink.append(str(msg)) + + info = _add + warn = _add + error = _add + + +class _Stub: + """``_on_wrench`` 用到的成员全部打桩。""" + + def __init__(self) -> None: + self.logs: List[str] = [] + self._logger = _Logger(self.logs) + self._ft_fz = None + self._t_ft = None + self._ft_zero_since = None + self._ft_zero_warned = False + self.ft_wrench_topic = "/Rizon4_063989/ft_sensor_wrench" + + def get_logger(self) -> Any: + return self._logger + + +def _wrench_msg(*, fz: float = 0.0, tx: float = 0.0) -> WrenchStamped: + m = WrenchStamped() + m.wrench = Wrench() + m.wrench.force.x = 0.0 + m.wrench.force.y = 0.0 + m.wrench.force.z = float(fz) + m.wrench.torque.x = float(tx) + m.wrench.torque.y = 0.0 + m.wrench.torque.z = 0.0 + return m + + +# --------------------------------------------------------------------------- # +# wrench_all_zero：区分"安静"与"没出数" +# --------------------------------------------------------------------------- # +def test_all_zero_is_flagged(): + assert wrench_all_zero(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) is True + + +def test_a_real_load_is_not_all_zero(): + """真机 external_wrench_in_tcp 的读数（≈负载 54N）不能被判成"没出数"。""" + assert wrench_all_zero(0.036, 0.32, -53.84, 0.0, 0.0, 0.0) is False + + +def test_tiny_noise_is_not_all_zero(): + """很小但活的读数（噪声量级）也不算"没出数" —— 否则会误报。""" + assert wrench_all_zero(1.0e-3, 0.0, 0.0, 0.0, 0.0, 0.0) is False + + +def test_torque_alone_is_enough_to_prove_it_is_alive(): + assert wrench_all_zero(0.0, 0.0, 0.0, 0.02, 0.0, 0.0) is False + + +# --------------------------------------------------------------------------- # +# 节点守卫：全零 > 5s 报 error（一次），有数则安静 +# --------------------------------------------------------------------------- # +def test_all_zero_for_over_5s_raises_error_once(): + st = _Stub() + # 让它一开始就已经"全零了 6 秒"（不 sleep，直接构造状态） + st._ft_zero_since = time.monotonic() - 6.0 + + Stage2Locate._on_wrench(st, _wrench_msg()) # type: ignore[arg-type] + errs = [m for m in st.logs if "全为 0" in m] + assert len(errs) == 1, f"数据源恒零却{'没报' if not errs else '报多了'} —— 静默失效" + assert "判不到接触" in errs[0], "要说清后果：接触兜底永远不会触发" + assert "ft_sensor_wrench" in errs[0], "要打出话题名，现场才知道换哪个" + + # 第二次不该再刷屏 + Stage2Locate._on_wrench(st, _wrench_msg()) # type: ignore[arg-type] + assert len([m for m in st.logs if "全为 0" in m]) == 1, "全零告警只许喊一次" + + +def test_a_live_reading_never_warns_and_is_cached(): + st = _Stub() + Stage2Locate._on_wrench(st, _wrench_msg(fz=-53.84)) # type: ignore[arg-type] + assert not [m for m in st.logs if "全为 0" in m], "有数还告警 = 误报" + assert st._ft_fz == -53.84, "轴向力必须缓存下来（_contact_detected 要用）" + assert st._t_ft is not None, "要记收到时刻" + + +def test_losing_data_after_a_live_reading_restarts_the_clock(): + """先有数、后全零：计时要从"变成全零那一刻"重新起算，不能沿用旧起点。""" + st = _Stub() + Stage2Locate._on_wrench(st, _wrench_msg(fz=-53.84)) # type: ignore[arg-type] + assert st._ft_zero_since is None + Stage2Locate._on_wrench(st, _wrench_msg()) # 转全零 + assert st._ft_zero_since is not None, "转全零要起计时" + + +# --------------------------------------------------------------------------- # +# 源码守卫：这两处都不能被悄悄删掉 +# --------------------------------------------------------------------------- # +def test_stage2_wires_the_all_zero_guard(): + src = ( + ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py" + ).read_text(encoding="utf-8") + assert "wrench_all_zero(" in src, "全零守卫没接进 stage2 的 FT 回调" + assert "from arm_application.gauge_fsm import" in src and "wrench_all_zero" in src + + +# --------------------------------------------------------------------------- # +# 配置接线：真机指向有数的话题，仿真不许跟着用 +# --------------------------------------------------------------------------- # +def _robots() -> dict: + site = yaml.safe_load(SITE.read_text(encoding="utf-8")) + return {r["sn"]: r for r in site["robot"]["robots"]} + + +def test_real_arms_point_at_a_topic_that_actually_has_data(): + """真机 FT 源必须是 external_wrench_in_tcp —— ft_sensor_wrench 实测恒零。""" + robots = _robots() + for sn in ("Rizon4-063989", "Rizon4-063991"): + if sn not in robots: + continue + topic = robots[sn]["servo"].get("ft_wrench_topic") + assert topic, f"{sn} 没配 servo.ft_wrench_topic —— 会退回恒零的 ft_sensor_wrench" + assert topic.endswith("/external_wrench_in_tcp"), ( + f"{sn} 的 FT 源是 {topic}；真机只有 external_wrench_in_tcp 有数" + ) + assert sn.replace("-", "_") in topic, "话题名里的 sn 要用下划线（与 broadcaster 一致）" + + +def test_sim_overrides_the_real_ft_topic(): + """仿真必须显式覆盖 —— `*servo_left` 带下来的是真机话题，仿真里不存在。""" + topic = _robots()["sim"]["servo"].get("ft_wrench_topic") + assert topic == "/sim/ft_sensor_wrench", ( + f"仿真 FT 话题是 {topic}；仿真由 sim_wrist_ft 发 /sim/ft_sensor_wrench" + ) + + +def test_node_reads_the_topic_from_site_config_not_hardcoded(): + """话题必须可配（读 servo.ft_wrench_topic），不许把真机话题写死在代码里。 + + 用语法树取字符串字面量来判（不是文本匹配）—— 这样解释这件事的注释不算数。 + """ + import ast + + src = ( + ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py" + ).read_text(encoding="utf-8") + assert 'servo.get("ft_wrench_topic")' in src, "没从 site 配置读话题" + literals = [ + n.value + for n in ast.walk(ast.parse(src)) + if isinstance(n, ast.Constant) and isinstance(n.value, str) + ] + assert not any("Rizon4_063989" in s for s in literals), ( + "真机 sn/话题被写进了代码字符串（应当只在 site.yaml）" + ) + # 兜底必须是"按 sn 推导"，而不是某个具体话题 + assert 'f"/{robot_ns}/ft_sensor_wrench"' in src, "兜底不再是按 sn 推导的话题" diff --git a/src/arm_application/test/test_gauge_fsm.py b/src/arm_application/test/test_gauge_fsm.py index 57c97da..7bf6c3f 100644 --- a/src/arm_application/test/test_gauge_fsm.py +++ b/src/arm_application/test/test_gauge_fsm.py @@ -18,6 +18,8 @@ from arm_application.gauge_fsm import ( STATE_DOC, TERMINAL, TRANSITIONS, + ENTRY_HIGH, + ENTRY_OK, Event, FsmError, GaugeContext, @@ -31,6 +33,7 @@ from arm_application.gauge_fsm import ( descend_needed_m, descend_progress, descend_tol_m, + hole_entry_verdict, relief_tol_m, tool_clearance_m, wrench_live, @@ -540,6 +543,63 @@ def test_descend_needed_measures_shoulder_not_tip(): assert descend_needed_m(0.655, 0.243, 0.412, 0.030) - tip_only == pytest.approx(0.030) +def test_hole_entry_verdict_flags_gauge_landing_on_the_surface(): + """2026-09-28 实测：同一命令三跑里坏一跑（DONE / ABORT / DONE）。 + + 坏的那跑：k1 装好了，k2 只下压 0.90mm 就"接触"（正常要 28.6~31.9mm）—— + 因为工件在 k1 之后被工具推走了 ~20mm，而 k2 的孔位是 S05 一次性定下的。 + 不拦的代价是干转 5 轮 + 最后报一句"转轴偏差…先确认工具系有没有装转接件"， + 把人引去查工具系（那几个都是好的）。 + """ + needed = 0.0319 # 台肩落面还需下压 31.9mm（实测） + # 坏那跑：0.90 / 31.9 = 3% ⇒ 必须报"没进孔" + assert hole_entry_verdict(0.0009, needed) == ENTRY_HIGH + # 好那跑：28.6 / 31.9 = 90% ⇒ 必须放行（否则白扔一整个孔） + assert hole_entry_verdict(0.0286, needed) == ENTRY_OK + # 另一跑 k1：31.45 / 31.9 ⇒ 放行 + assert hole_entry_verdict(0.03145, needed) == ENTRY_OK + + +def test_hole_entry_verdict_never_flags_the_healthy_band(): + """健康的比值区间（台肩倒角没完全落面、板厚公差）绝不能被判成"没进孔"。 + + 这是"宁可漏报也不能误报"那一边：误报一次就白扔一整个孔， + 而漏报只是回到旧行为（多烧几轮 + 一句误导）。 + """ + needed = 0.032 + for ratio in (0.85, 0.90, 0.95, 1.00, 1.10): + assert hole_entry_verdict(needed * ratio, needed) == ENTRY_OK, ( + f"比值 {ratio} 是正常范围，不该判成没进孔" + ) + # 明确该报的一侧 + for ratio in (0.0, 0.03, 0.10, 0.30, 0.49): + assert hole_entry_verdict(needed * ratio, needed) == ENTRY_HIGH, ( + f"比值 {ratio} 差着一个台肩高度，必须报" + ) + + +def test_hole_entry_verdict_stays_quiet_when_geometry_is_unusable(): + """``needed_m <= 0`` 时不许抢着报第二个错。 + + 那种情况上面那条几何自检（tool_clearance_m / descend_needed_m）已经在报了， + 这里再报一次只会把真正的第一因埋掉。 + """ + assert hole_entry_verdict(0.0005, 0.0) == ENTRY_OK + assert hole_entry_verdict(0.0005, -0.01) == ENTRY_OK + + +def test_hole_lost_event_aborts_and_has_an_honest_label(): + """事件本身与它的中文标签都必须是实话。 + + 复用 FORCE_OVER_LIMIT 会让结论报成"力超安全上限"，而这种情况下力根本没超限 —— + 又是一句误导。所以单列 HOLE_LOST，并且标签要能一眼看出是孔位问题。 + """ + assert resolve_next(State.CONTACT, Event.HOLE_LOST, GaugeContext()) == State.ABORT + label = EVENT_DOC[Event.HOLE_LOST] + assert "孔" in label, "标签要指明是孔位问题" + assert "力" not in label, "别把它说成力的问题 —— 力是好的" + + def test_descend_budget_has_margin_and_floor(): """预算要有余量（容纳对位残差/板厚公差），且再小也有下限。 diff --git a/src/arm_application/test/test_handeye_lib.py b/src/arm_application/test/test_handeye_lib.py index d94ecd7..162b7ee 100644 --- a/src/arm_application/test/test_handeye_lib.py +++ b/src/arm_application/test/test_handeye_lib.py @@ -273,33 +273,72 @@ def test_roundtrip_is_rotation_matrix_with_det_one(): assert np.allclose(R @ R.T, np.eye(3), atol=1e-8), "必须是正交阵" -def test_roundtrip_eye_to_hand_recovers_known_transform(): - """eye_to_hand：相机固定在基座，X = cam → base。 +def _synthesize_views_eye_to_hand( + X_true: np.ndarray, T_target2gripper: np.ndarray, n: int, seed: int = 7 +): + """由已知 X_true 生成 n 组**眼在手外**的视图观测。 - 此时坐标链变成 target → camera 与 gripper → base 相互独立， - 求解器内部会把 gripper2base 求逆，这条分支容易写错，单独测。 + 眼在手外的坐标链：相机固定在工位外不动，**标定板装在法兰上随臂动**。 + T_t2b = T_g2b · T_t2g （板在基座 = 法兰在基座 · 板在法兰，T_t2g 固定） + T_t2b = X · T_t2c （X = T_cam2base） + 所以给定任意的 T_g2b，可反推出该视图下应该观测到的 T_t2c： + T_t2c = inv(X) · T_g2b · T_t2g + + **为什么板必须在法兰上**：相机与基座都不动，唯一会变的就是带着板走的法兰； + 板若像眼在手上那样摆在台面上不动，T_t2c 就恒定不变，方程直接退化。 """ - rng = np.random.default_rng(3) - X_true = _rand_transform(rng) # T_cam2base + rng = np.random.default_rng(seed) Rg, tg, Rc, tc = [], [], [], [] - for _ in range(12): - T_g2b = _rand_transform(rng) # 法兰在基座 - T_t2c = _rand_transform(rng) # 板在相机（固定相机，板随臂动） - # 板在基座 = T_cam2base · T_t2c（板相对基座是固定的物理事实） - _ = X_true @ T_t2c + Xinv = np.linalg.inv(X_true) + for _ in range(n): + T_g2b = _rand_transform(rng) + T_t2c = Xinv @ T_g2b @ T_target2gripper Rg.append(T_g2b[:3, :3]) tg.append(T_g2b[:3, 3].reshape(3, 1)) Rc.append(T_t2c[:3, :3]) tc.append(T_t2c[:3, 3].reshape(3, 1)) + return Rg, tg, Rc, tc + + +def test_roundtrip_recovers_known_transform_eye_to_hand(): + """眼在手外：X = cam → base，**与真值比对**（不是只查正交/det）。 + + 这条分支内部把 gripper2base 求了逆。求逆方向或坐标链写错时，结果照样是一个 + 合法的旋转矩阵 —— 只查 det/正交的测试**抓不到**，所以必须比真值。 + """ + rng = np.random.default_rng(3) + X_true = _rand_transform(rng) # T_cam2base + T_target2gripper = _rand_transform(rng) # 板刚性固定在法兰上（每视图不变） + Rg, tg, Rc, tc = _synthesize_views_eye_to_hand(X_true, T_target2gripper, 14) - # eye_to_hand 下 OpenCV 仍是解 AX=XB，X 语义变为 cam→base； - # 这里只验证"能解出正交且 det=1 的结果"，不比对真值 - # （板随臂动时需要 T_t2b 与 T_g2b 的联动，构造更复杂， - # 真正的语义正确性由 eye_in_hand 那条测试把关坐标链） R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True) + X_est = Rt_to_T(R, t) + + assert np.allclose(X_est, X_true, atol=1e-6), ( + "eye_to_hand 没解回已知的 T_cam2base —— 求逆方向或坐标链错了" + ) R = np.asarray(R, float) - assert R.shape == (3, 3) - assert np.isclose(np.linalg.det(R), 1.0, atol=1e-6) + assert np.isclose(np.linalg.det(R), 1.0, atol=1e-8), "det 必须是 +1，不是镜像" + + +def test_eye_to_hand_flag_actually_changes_the_answer(): + """负向：同一份眼在手外数据当成眼在手上解，**必须解出不同的东西**。 + + 给上面那条把关：万一哪天 `eye_to_hand` 分支被改成"其实什么也没做" + （两个模式走同一条路），上面那条测试就会失去意义 —— 而这条会红。 + """ + rng = np.random.default_rng(3) + X_true = _rand_transform(rng) + T_target2gripper = _rand_transform(rng) + Rg, tg, Rc, tc = _synthesize_views_eye_to_hand(X_true, T_target2gripper, 14) + + R_ok, t_ok = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True) + R_bad, t_bad = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=False) + + assert np.allclose(Rt_to_T(R_ok, t_ok), X_true, atol=1e-6) + assert not np.allclose(Rt_to_T(R_bad, t_bad), X_true, atol=1e-3), ( + "眼在手上分支也解出了同一个真值 —— 说明 eye_to_hand 这个开关根本没起作用" + ) def test_calibration_fails_loudly_on_degenerate_input(): diff --git a/src/arm_application/test/test_handeye_mode.py b/src/arm_application/test/test_handeye_mode.py new file mode 100644 index 0000000..1f52cb8 --- /dev/null +++ b/src/arm_application/test/test_handeye_mode.py @@ -0,0 +1,404 @@ +"""眼在手外 / 眼在手上必须按模式分开：板尺寸、相机、结果文件名、TF 父子。 + +这些不是风格问题。两种装法用两块不同的物理板、两台不同的相机，解出的 +矩阵语义也不同（cam→法兰 vs cam→基座）。共用一份配置的后果不是报错， +而是"标定跑完、结果系统性偏"——现场几乎联想不到是板尺寸/相机/文件名 +串了。所以这里每条测试都配一条**负向**：故意走错模式必须红。 +""" + +from __future__ import annotations + +import os +import subprocess +import sys +from pathlib import Path + +import pytest +import yaml + +ROOT = Path(__file__).resolve().parents[3] +SCRIPTS_LIB = ROOT / "scripts" / "lib" +SCRIPT = ROOT / "scripts" / "run_handeye.sh" + +sys.path.insert(0, str(SCRIPTS_LIB)) +import export_site_env # type: ignore # noqa: E402 + +from arm_application.site_io import ( # noqa: E402 + eye_to_hand_result_name, + handeye_static_tf_frames, + overhead_camera, + resolve_eye_to_hand_file, +) + + +def _site(**over) -> dict: + """最小可 flatten 的虚构站点。调用方可覆盖任意段。""" + base = { + "robot": { + "sn": "Rizon4-777001", + "robots": [ + { + "sn": "Rizon4-777001", + "arm": "left", + "label": "左臂01", + "camera": {"name": "cam_a", "serial": "99900001"}, + }, + { + "sn": "Rizon4-777002", + "arm": "right", + "label": "右臂02", + "camera": {"name": "cam_b", "serial": "99900002"}, + }, + ], + }, + "cameras": { + "d435": { + "name": "d435", + "role": "overhead_eye_to_hand", + "serial": "99900003", + "width": 1280, + "height": 720, + }, + "sim_d435": { + "name": "sim_d435", + "role": "overhead_eye_to_hand", + "arm": "sim", + "serial": "", + }, + }, + "handeye": { + "mode": "eye_in_hand", + "boards": { + "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015}, + "eye_to_hand": {"cols": 11, "rows": 8, "square_size_m": 0.020}, + }, + }, + } + for k, v in over.items(): + if isinstance(v, dict) and isinstance(base.get(k), dict): + merged = dict(base[k]) + merged.update(v) + base[k] = merged + else: + base[k] = v + return base + + +def _flat(monkeypatch, site: dict, sn: str = "Rizon4-777001") -> dict: + monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml") + monkeypatch.setenv("ROBOT_SN", sn) + return export_site_env.flatten(site) + + +# --------------------------------------------------------------------------- +# 板尺寸按模式 +# --------------------------------------------------------------------------- + + +class TestHandeyeBoardPerMode: + def test_two_modes_keep_different_boards(self, monkeypatch): + flat = _flat(monkeypatch, _site()) + assert flat["SITE_HANDEYE_BOARD_COLS_EYE_IN_HAND"] == "8" + assert flat["SITE_HANDEYE_BOARD_ROWS_EYE_IN_HAND"] == "11" + assert flat["SITE_HANDEYE_SQUARE_EYE_IN_HAND"] == "0.015" + assert flat["SITE_HANDEYE_BOARD_SRC_EYE_IN_HAND"] == "boards" + assert flat["SITE_HANDEYE_BOARD_COLS_EYE_TO_HAND"] == "11" + assert flat["SITE_HANDEYE_BOARD_ROWS_EYE_TO_HAND"] == "8" + assert flat["SITE_HANDEYE_SQUARE_EYE_TO_HAND"] == "0.02" + assert flat["SITE_HANDEYE_BOARD_SRC_EYE_TO_HAND"] == "boards" + + def test_current_mode_board_follows_handeye_mode(self, monkeypatch): + site = _site() + site["handeye"]["mode"] = "eye_to_hand" + flat = _flat(monkeypatch, site) + assert flat["SITE_HANDEYE_BOARD_COLS"] == "11" + assert flat["SITE_HANDEYE_SQUARE_M"] == "0.02" + + def test_legacy_top_level_keys_still_work(self): + he = {"board_cols": 9, "board_rows": 6, "square_size_m": 0.025} + board, src = export_site_env.handeye_board(he, "eye_in_hand") + assert src == "legacy" + assert board == {"cols": 9, "rows": 6, "square_size_m": 0.025} + + def test_missing_board_is_default_not_silent_guess_disguised_as_config(self): + """没配过这块板 → 来源必须是 default。run_handeye.sh 见 default 会拦下。 + + 如果这里返回 boards / legacy，脚本会以为"配过了"，带着内置 8×11/15mm + 去标 A4 板 —— 标定照样跑完，结果系统性偏。 + """ + board, src = export_site_env.handeye_board({}, "eye_to_hand") + assert src == "default" + # 默认值本身仍给出来（给 show 打印），但来源必须让调用方能拦。 + assert board["cols"] == 8 + + def test_unknown_mode_is_default(self): + he = { + "boards": { + "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015}, + } + } + _, src = export_site_env.handeye_board(he, "eye_on_foot") + assert src == "default" + + def test_negative_shared_keys_cannot_serve_both_modes(self): + """负向：只有一组顶层键时，两种模式会读到**同一块板**。 + + 这正是旧配置的失败模式。新配置（boards 段）必须让这条不成立。 + """ + he_old = {"board_cols": 8, "board_rows": 11, "square_size_m": 0.015} + a, _ = export_site_env.handeye_board(he_old, "eye_in_hand") + b, _ = export_site_env.handeye_board(he_old, "eye_to_hand") + assert a == b, "旧口径下两种模式本就共用一块板（这条钉住旧逻辑会红的前提）" + + he_new = { + "boards": { + "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015}, + "eye_to_hand": {"cols": 11, "rows": 8, "square_size_m": 0.020}, + } + } + a2, _ = export_site_env.handeye_board(he_new, "eye_in_hand") + b2, _ = export_site_env.handeye_board(he_new, "eye_to_hand") + assert a2 != b2 + assert a2["square_size_m"] == 0.015 + assert b2["square_size_m"] == 0.020 + + +# --------------------------------------------------------------------------- +# 手外相机按 role + arm +# --------------------------------------------------------------------------- + + +class TestOverheadCamera: + def test_real_arm_picks_shared_d435_not_sim(self): + site = _site() + cam, others = overhead_camera(site, {"arm": "left"}) + assert cam.get("name") == "d435" + assert others == [] + + def test_sim_arm_picks_sim_d435(self): + site = _site() + cam, _ = overhead_camera(site, {"arm": "sim"}) + assert cam.get("name") == "sim_d435" + + def test_no_overhead_returns_empty(self): + cam, others = overhead_camera({"cameras": {"d405": {"role": "left_wrist"}}}) + assert cam == {} + assert others == [] + + def test_ambiguous_cameras_surface_the_others(self): + """两台都没标 arm → 取第一台，但其余名字必须返回，让调用方出声。""" + site = { + "cameras": { + "a": {"name": "a", "role": "overhead_eye_to_hand"}, + "b": {"name": "b", "role": "overhead_eye_to_hand"}, + } + } + cam, others = overhead_camera(site, {"arm": "left"}) + assert cam.get("name") in ("a", "b") + assert len(others) == 1 + assert others[0] != cam.get("name") + + +# --------------------------------------------------------------------------- +# 结果文件名 +# --------------------------------------------------------------------------- + + +class TestEyeToHandResultName: + def test_name_carries_arm_and_camera(self): + assert eye_to_hand_result_name("89", "d435") == "89_d435_eye_to_hand.yaml" + + def test_two_arms_do_not_share_a_file(self, monkeypatch): + site = _site() + left = _flat(monkeypatch, site, sn="Rizon4-777001") + right = _flat(monkeypatch, site, sn="Rizon4-777002") + assert left["SITE_HANDEYE_FILE_EYE_TO_HAND"] != right["SITE_HANDEYE_FILE_EYE_TO_HAND"] + assert left["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("01_d435_eye_to_hand.yaml") + assert right["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("02_d435_eye_to_hand.yaml") + + def test_eye_in_hand_file_is_untouched(self, monkeypatch): + """换模式不能改腕部标定那份路径 —— S05 精拍定位一直读它。""" + site = _site() + site["handeye"]["mode"] = "eye_to_hand" + flat = _flat(monkeypatch, site) + assert flat["SITE_HANDEYE_FILE"].endswith("01_eye_in_hand.yaml") + assert flat["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("01_d435_eye_to_hand.yaml") + + def test_resolve_prefers_explicit_over_legacy(self, tmp_path: Path): + explicit = tmp_path / "89_d435_eye_to_hand.yaml" + legacy = tmp_path / "handeye" / "results" / "d435_eye_to_hand.yaml" + legacy.parent.mkdir(parents=True) + explicit.write_text("explicit\n") + legacy.write_text("legacy\n") + got = resolve_eye_to_hand_file( + tmp_path, explicit=str(explicit), camera_name="d435" + ) + assert got == explicit + + def test_resolve_falls_back_to_camera_named_file(self, tmp_path: Path): + legacy = tmp_path / "handeye" / "results" / "d435_eye_to_hand.yaml" + legacy.parent.mkdir(parents=True) + legacy.write_text("legacy\n") + got = resolve_eye_to_hand_file(tmp_path, explicit="", camera_name="d435") + assert got == legacy + + def test_resolve_missing_returns_none_not_a_guess(self, tmp_path: Path): + assert resolve_eye_to_hand_file(tmp_path, camera_name="d435") is None + + def test_resolve_relative_explicit_is_under_root_not_cwd(self, tmp_path: Path): + """相对路径必须按仓库根解析。跟 CWD 走会在换目录启动时静默找不到。""" + (tmp_path / "handeye" / "results").mkdir(parents=True) + target = tmp_path / "handeye" / "results" / "89_d435_eye_to_hand.yaml" + target.write_text("ok\n") + got = resolve_eye_to_hand_file( + tmp_path, + explicit="handeye/results/89_d435_eye_to_hand.yaml", + camera_name="d435", + ) + assert got == target + + +# --------------------------------------------------------------------------- +# TF 父子 +# --------------------------------------------------------------------------- + + +class TestHandeyeStaticTfFrames: + def test_eye_in_hand_is_flange_parent(self): + parent, child = handeye_static_tf_frames( + mode="eye_in_hand", + ee_frame="Rizon4-063989_flange", + base_frame="Rizon4-063989_base_link", + cam_frame="d405_left_color_optical_frame", + ) + assert parent == "Rizon4-063989_flange" + assert child == "d405_left_color_optical_frame" + + def test_eye_to_hand_is_base_parent(self): + parent, child = handeye_static_tf_frames( + mode="eye_to_hand", + ee_frame="Rizon4-063989_flange", + base_frame="Rizon4-063989_base_link", + cam_frame="d435_color_optical_frame", + ) + assert parent == "Rizon4-063989_base_link" + assert child == "d435_color_optical_frame" + + def test_negative_wrong_mode_puts_camera_under_flange(self): + """负向：眼在手外的文件若按眼在手上发 TF，父系会变成法兰。 + + 这正是旧 launch 的失败模式。新函数必须让这条不成立。 + """ + wrong, _ = handeye_static_tf_frames( + mode="eye_in_hand", + ee_frame="flange", + base_frame="base", + cam_frame="d435_color_optical_frame", + ) + right, _ = handeye_static_tf_frames( + mode="eye_to_hand", + ee_frame="flange", + base_frame="base", + cam_frame="d435_color_optical_frame", + ) + assert wrong == "flange" + assert right == "base" + assert wrong != right + + +# --------------------------------------------------------------------------- +# run_handeye.sh --mode 真的切到对应的板 / 相机 / 文件 +# --------------------------------------------------------------------------- + + +def _show(mode: str, site: Path) -> str: + env = os.environ.copy() + env["SITE_CONFIG"] = str(site) + env["ROBOT_SN"] = "Rizon4-063989" + proc = subprocess.run( + ["bash", str(SCRIPT), "--arm", "89", "show", "--mode", mode], + cwd=str(ROOT), + env=env, + text=True, + capture_output=True, + check=False, + ) + return proc.stdout + proc.stderr + + +@pytest.fixture(scope="module") +def _real_site() -> Path: + """脚本走 load_site_config.sh，必须是一份能选出 89 臂的真实形状配置。""" + return ROOT / "config" / "site.yaml" + + +class TestRunHandeyeScriptMode: + def test_eye_to_hand_uses_d435_and_a4_board(self, _real_site: Path): + out = _show("eye_to_hand", _real_site) + assert "mode=eye_to_hand" in out + assert "camera=d435" in out + assert "11x8 inner, square=0.02 m" in out + assert "89_d435_eye_to_hand.yaml" in out + # 负向：不能再去开腕部相机。 + assert "camera=d405" not in out + + def test_eye_in_hand_keeps_wrist_camera_and_a5_board(self, _real_site: Path): + out = _show("eye_in_hand", _real_site) + assert "mode=eye_in_hand" in out + assert "d405" in out + assert "8x11 inner, square=0.015 m" in out + assert "89_eye_in_hand.yaml" in out + + def test_missing_board_blocks_calibrate(self, tmp_path: Path): + """没配 eye_to_hand 板尺寸时，calibrate 必须当场红，不能去赌默认值。""" + site = { + "robot": { + "sn": "Rizon4-063989", + "robots": [ + { + "sn": "Rizon4-063989", + "arm": "left", + "camera": {"name": "d405_left", "serial": "1"}, + } + ], + }, + "handeye": { + "mode": "eye_in_hand", + "boards": { + "eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015}, + }, + }, + "cameras": { + "d435": { + "name": "d435", + "role": "overhead_eye_to_hand", + "serial": "9", + } + }, + } + p = tmp_path / "site.yaml" + p.write_text(yaml.safe_dump(site, allow_unicode=True), encoding="utf-8") + env = os.environ.copy() + env["SITE_CONFIG"] = str(p) + env["ROBOT_SN"] = "Rizon4-063989" + proc = subprocess.run( + ["bash", str(SCRIPT), "--arm", "89", "calibrate", "--mode", "eye_to_hand"], + cwd=str(ROOT), + env=env, + text=True, + capture_output=True, + check=False, + ) + assert proc.returncode != 0 + assert "没有配置 [eye_to_hand] 用的标定板尺寸" in (proc.stdout + proc.stderr) + + def test_illegal_mode_exits_immediately(self): + proc = subprocess.run( + ["bash", str(SCRIPT), "--arm", "89", "show", "--mode", "eye_on_foot"], + cwd=str(ROOT), + text=True, + capture_output=True, + check=False, + ) + assert proc.returncode != 0 + assert "eye_in_hand 或 eye_to_hand" in (proc.stdout + proc.stderr) diff --git a/src/arm_application/test/test_hole_detector.py b/src/arm_application/test/test_hole_detector.py index 6d42475..09eae47 100644 --- a/src/arm_application/test/test_hole_detector.py +++ b/src/arm_application/test/test_hole_detector.py @@ -15,7 +15,7 @@ from __future__ import annotations from arm_application.hole_detector import _default_model_path, weights_from_site -CANONICAL = "detect_best_1.onnx" +CANONICAL = "gongjian0929.onnx" # --------------------------------------------------------------------------- diff --git a/src/arm_application/test/test_onnx_workpiece_guard.py b/src/arm_application/test/test_onnx_workpiece_guard.py new file mode 100644 index 0000000..6bdb29c --- /dev/null +++ b/src/arm_application/test/test_onnx_workpiece_guard.py @@ -0,0 +1,50 @@ +"""``OnnxWorkpieceDetector`` 必须**当场拒绝**非检测模型，而不是解出垃圾框。 + +为什么值得一条测试（2026-10-08 实测） +------------------------------------- +把一个 ``task=pose`` 的模型（输出 ``[1, 4+nc+3K, 8400]``）喂给这个"只认 +``[1, N, 6]``"的解码器，会得到 12 个**看起来像检测结果**的东西： + + conf=70.870 cls=class_73 xyxy=[28.7, 0.0, 75.8, 0.0] + conf=41.092 cls=class_47 xyxy=[11.0, 0.0, 35.4, 0.0] + ... + conf=0.324 cls=workpiece xyxy=[ 0.2, 0.0, 0.3, 0.0] + +坐标被当成置信度（>1）、类号不存在、框是退化的（``y1 == y2``）—— 但没有异常、 +没有告警，流水线照跑。这类"静默失效"正是本仓库花最多代价在防的东西，所以在 +``__init__`` 里硬失败。 +""" + +from __future__ import annotations + +from pathlib import Path + +import pytest + +pytest.importorskip("onnxruntime", reason="没装 onnxruntime，属于环境缺依赖") + +from arm_application.onnx_workpiece import OnnxWorkpieceDetector # noqa: E402 + +ROOT = Path(__file__).resolve().parents[3] +YOLO_DIR = ROOT / "models" / "yolo" +POSE_ONNX = YOLO_DIR / "gongjian0929.onnx" # task=pose, 输出 [1,14,8400] +DETECT_ONNX = YOLO_DIR / "detect_best_0.onnx" # task=detect, end2end, 输出 [1,N,6] + + +def test_pose_model_is_refused_loudly(): + """带关键点的 pose 模型进了检测解码器 ⇒ 必须报错，且报错要指出正确做法。""" + if not POSE_ONNX.is_file(): + pytest.skip(f"没有 {POSE_ONNX.name}（模型不在本机）") + with pytest.raises(RuntimeError) as ei: + OnnxWorkpieceDetector(POSE_ONNX, conf=0.25, imgsz=640) + msg = str(ei.value) + assert "不是 6" in msg, f"报错要说出真实输出维度，实际：{msg}" + # 必须告诉人该往哪走，否则只会被当成"模型坏了" + assert "pose" in msg.lower() + + +def test_end2end_detect_model_still_loads(): + """守卫不能过严：真正的 end2end 检测模型必须照常加载。""" + if not DETECT_ONNX.is_file(): + pytest.skip(f"没有 {DETECT_ONNX.name}（模型不在本机）") + OnnxWorkpieceDetector(DETECT_ONNX, conf=0.25, imgsz=640) diff --git a/src/arm_application/test/test_overhead_oneshot.py b/src/arm_application/test/test_overhead_oneshot.py new file mode 100644 index 0000000..2f56d8b --- /dev/null +++ b/src/arm_application/test/test_overhead_oneshot.py @@ -0,0 +1,195 @@ +"""D405 丢了切 D435 一次性、以及臂静止进二阶段：纯函数必须能红。 + +错乘法兰 / 把框心冒充 k0 / conf≥门槛还去切 / 晃一下刚好满 3 秒 —— +这些在真机上都不报错，只是位置错或过早对孔。所以每条都配负向。 +""" + +from __future__ import annotations + +from types import SimpleNamespace + +import numpy as np +import pytest + +from arm_application.overhead_oneshot import ( + StillLock, + best_det_conf, + depth_raw_to_m, + k0_uv_from_det, + map_uv_to_depth, + oneshot_k0_base, + select_oneshot_det, + wrist_needs_handoff, +) + + +def _det(*, conf=0.8, cx=100.0, cy=80.0, kpts=None): + xyxy = np.array([cx - 20, cy - 15, cx + 20, cy + 15], dtype=float) + return SimpleNamespace(conf=conf, xyxy=xyxy, cx=cx, cy=cy, kpts=kpts) + + +class TestHandoff: + def test_none_and_low_conf_need_handoff(self): + assert wrist_needs_handoff(None, 0.5) is True + assert wrist_needs_handoff(0.49, 0.5) is True + assert wrist_needs_handoff(float("nan"), 0.5) is True + + def test_conf_at_or_above_threshold_stays_on_wrist(self): + assert wrist_needs_handoff(0.5, 0.5) is False + assert wrist_needs_handoff(0.87, 0.5) is False + + def test_threshold_is_the_caller_value_not_hardcoded(self): + # 负向：如果函数内部写死 0.5，改门槛 0.7 时 0.6 会判错 + assert wrist_needs_handoff(0.6, 0.7) is True + assert wrist_needs_handoff(0.6, 0.5) is False + + +class TestSelect: + def test_best_conf_none_on_empty(self): + assert best_det_conf([]) is None + assert best_det_conf(None) is None + + def test_select_rejects_below_threshold(self): + dets = [_det(conf=0.3), _det(conf=0.49)] + assert select_oneshot_det(dets, 0.5) is None + + def test_select_picks_highest_above_threshold(self): + lo = _det(conf=0.55, cx=1) + hi = _det(conf=0.9, cx=2) + got = select_oneshot_det([lo, hi], 0.5) + assert got is hi + + +class TestK0: + def test_missing_kpts_is_not_bbox_center(self): + d = _det(cx=200, cy=150, kpts=None) + assert k0_uv_from_det(d) is None + + def test_low_kpt_conf_rejected(self): + kpts = np.array([[10.0, 20.0, 0.1]], dtype=float) + assert k0_uv_from_det(_det(kpts=kpts), kpt_min_conf=0.30) is None + + def test_good_k0(self): + kpts = np.array([[11.0, 22.0, 0.9]], dtype=float) + assert k0_uv_from_det(_det(kpts=kpts)) == (11.0, 22.0) + + +class TestOneshotK0Base: + def _depth(self, z=0.70, h=10, w=10): + return np.full((h, w), z, dtype=float) + + def _T(self, xyz): + T = np.eye(4) + T[:3, 3] = xyz + return T + + def test_eye_to_hand_ignores_placeholder_flange(self): + kpts = np.array([[5.0, 5.0, 0.99]], dtype=float) + det = _det(conf=0.8, cx=5, cy=5, kpts=kpts) + T = self._T([0.4, -0.5, 0.7]) + got, is_k0 = oneshot_k0_base( + det, + self._depth(0.80), + fx=1.0, + fy=1.0, + cx=5.0, + cy=5.0, + T_cam_to_base=T, + min_z_m=0.25, + max_z_m=1.80, + ) + assert is_k0 is True + np.testing.assert_allclose(got, [0.4, -0.5, 1.5], atol=1e-9) + + def test_wrong_mode_would_shift_by_flange(self): + """守卫：同一点用眼在手上公式会差出法兰平移 —— 调用方绝不能走那条。""" + from arm_application.handeye_lib import cam_point_to_base + + T_he = self._T([0.4, -0.5, 0.7]) + T_ee = self._T([0.6, 0.0, 0.3]) + p = np.array([0.0, 0.0, 0.8]) + right = cam_point_to_base(p, T_he, T_ee, "eye_to_hand") + wrong = cam_point_to_base(p, T_he, T_ee, "eye_in_hand") + assert np.linalg.norm(right - wrong) > 0.5 + + def test_low_det_conf_returns_none(self): + det = _det(conf=0.2) + assert ( + oneshot_k0_base( + det, + self._depth(), + fx=1, + fy=1, + cx=5, + cy=5, + T_cam_to_base=np.eye(4), + min_z_m=0.25, + max_z_m=1.80, + min_det_conf=0.5, + ) + is None + ) + + def test_depth_out_of_range_returns_none(self): + kpts = np.array([[5.0, 5.0, 0.99]], dtype=float) + det = _det(conf=0.9, kpts=kpts) + assert ( + oneshot_k0_base( + det, + self._depth(0.05), + fx=1, + fy=1, + cx=5, + cy=5, + T_cam_to_base=np.eye(4), + min_z_m=0.25, + max_z_m=1.80, + ) + is None + ) + + +class TestDepthScale: + def test_uint16_mm_becomes_meters(self): + raw = np.full((8, 8), 700, dtype=np.uint16) + m = depth_raw_to_m(raw, "16UC1") + assert m is not None + assert m[0, 0] == pytest.approx(0.70) + + def test_unknown_encoding_returns_none(self): + assert depth_raw_to_m(np.ones((8, 8)), "rgb8") is None + + def test_map_uv_scales_when_size_differs(self): + u, v = map_uv_to_depth(100, 50, (200, 100), (100, 50)) + assert (u, v) == (50.0, 25.0) + + +class TestStillLock: + def test_three_seconds_still_locks(self): + s = StillLock(still_sec=3.0, still_move_m=0.015) + p = np.array([0.50, -0.12]) + assert s.update(p, 0.0, tracking_ok=True) is False + assert s.update(p + 0.005, 2.9, tracking_ok=True) is False + assert s.update(p + 0.005, 3.0, tracking_ok=True) is True + + def test_large_move_resets(self): + s = StillLock(still_sec=3.0, still_move_m=0.015) + p = np.array([0.50, -0.12]) + s.update(p, 0.0, tracking_ok=True) + assert s.update(p + np.array([0.020, 0.0]), 2.0, tracking_ok=True) is False + assert s.update(p + np.array([0.020, 0.0]), 4.9, tracking_ok=True) is False + assert s.update(p + np.array([0.020, 0.0]), 5.0, tracking_ok=True) is True + + def test_lost_tracking_resets(self): + s = StillLock(still_sec=3.0, still_move_m=0.015) + p = np.array([0.50, -0.12]) + s.update(p, 0.0, tracking_ok=True) + assert s.update(p, 4.0, tracking_ok=False) is False + assert s.update(p, 4.1, tracking_ok=True) is False + + def test_overhead_move_does_not_lock(self): + """tracking_ok=False（顶视迁臂）即使法兰不动也不许进二阶段。""" + s = StillLock(still_sec=3.0, still_move_m=0.015) + p = np.array([0.50, -0.12]) + assert s.update(p, 0.0, tracking_ok=False) is False + assert s.update(p, 10.0, tracking_ok=False) is False diff --git a/src/arm_application/test/test_overhead_track.py b/src/arm_application/test/test_overhead_track.py new file mode 100644 index 0000000..eefc98b --- /dev/null +++ b/src/arm_application/test/test_overhead_track.py @@ -0,0 +1,234 @@ +"""一阶段跟踪切到顶置 D435：相机、手眼文件、TF 父子、点变换都必须对。 + +把眼在手外的 T（cam→基座）再乘法兰，定位总能到附近、永远差一截。 +所以这里每条都配负向：走错模式 / 借腕部量程 / 非法角色必须红。 +""" + +from __future__ import annotations + +import sys +from pathlib import Path + +import numpy as np +import pytest + +ROOT = Path(__file__).resolve().parents[3] +SCRIPTS_LIB = ROOT / "scripts" / "lib" +sys.path.insert(0, str(SCRIPTS_LIB)) +import export_site_env # type: ignore # noqa: E402 + +from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T # noqa: E402 +from arm_application.site_io import ( # noqa: E402 + handeye_static_tf_frames, + track_camera_role, + track_handeye_mode, +) + +from test_handeye_mode import _site # noqa: E402 + + +@pytest.fixture(autouse=True) +def _isolate_arm(monkeypatch): + """别让本机 env 里的 ROBOT_SN=89 把虚构站点解析到真机相机。""" + monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml") + monkeypatch.setenv("ROBOT_SN", "Rizon4-777001") + + +def _track_site(**over) -> dict: + site = _site() + site["camera"] = {"name": "d405_left"} + site["paths"] = {"handeye_file": "/tmp/wrist_eye_in_hand.yaml"} + site["yolo"] = {"max_depth_m": 0.55, "min_depth_m": 0.04, "feature_min_range_m": 0.14} + site["servo"] = { + "xy_track": "hover", + "lock_still_sec": 3.0, + "lock_still_move_m": 0.015, + } + site["perception"] = { + "track_camera": "wrist", + "wrist_handoff_conf": 0.5, + "overhead": { + "min_depth_m": 0.25, + "max_depth_m": 1.80, + "feature_min_range_m": 0.30, + "oneshot_conf": 0.5, + "cooldown_s": 1.5, + "frame_max_age_s": 0.4, + "oneshot_hold_s": 8.0, + }, + } + for k, v in over.items(): + if isinstance(v, dict) and isinstance(site.get(k), dict): + merged = dict(site[k]) + merged.update(v) + site[k] = merged + else: + site[k] = v + return site + + +def _overhead_ok() -> dict: + return _track_site( + perception={ + "track_camera": "overhead", + "overhead": { + "min_depth_m": 0.25, + "max_depth_m": 1.80, + "feature_min_range_m": 0.30, + }, + } + ) + + +class TestTrackCameraRole: + def test_missing_defaults_to_wrist(self): + assert track_camera_role({}) == "wrist" + assert track_camera_role({"perception": {}}) == "wrist" + + def test_illegal_value_raises_not_fallback(self): + with pytest.raises(ValueError, match="wrist 或 overhead"): + track_camera_role({"perception": {"track_camera": "d405"}}) + + def test_overhead_maps_to_eye_to_hand(self): + assert track_handeye_mode("overhead") == "eye_to_hand" + assert track_handeye_mode("wrist") == "eye_in_hand" + + +class TestPerceptionServoWiring: + def test_overhead_uses_d435_not_wrist_camera(self): + p = export_site_env.perception_overrides(_overhead_ok()) + assert p["camera_source"] == "d435" + assert p["camera_frame"] == "d435_color_optical_frame" + assert p["depth_topic"] == "/d435/aligned_depth_to_color/image_raw" + assert "d405" not in p["camera_source"] + assert "d405" not in p["depth_topic"] + + def test_overhead_does_not_borrow_wrist_depth_range(self): + p = export_site_env.perception_overrides(_overhead_ok()) + assert p["max_depth_m"] == pytest.approx(1.80) + assert p["min_depth_m"] == pytest.approx(0.25) + assert p["feature_min_range_m"] == pytest.approx(0.30) + # 负向：腕部 0.55 绝不能漏过来 + assert p["max_depth_m"] != pytest.approx(0.55) + + def test_overhead_handeye_file_is_e2h_not_wrist(self): + s = export_site_env.servo_overrides(_overhead_ok()) + assert s["handeye_mode"] == "eye_to_hand" + assert s["handeye_file"].endswith("01_d435_eye_to_hand.yaml") + assert "wrist_eye_in_hand" not in s["handeye_file"] + parent, child = handeye_static_tf_frames( + mode=s["handeye_mode"], + ee_frame="flange", + base_frame="base", + cam_frame=s["camera_frame"], + ) + assert parent == "base" + assert child == "d435_color_optical_frame" + + def test_wrist_track_keeps_arm_camera_and_wrist_file(self): + site = _track_site(perception={"track_camera": "wrist"}) + p = export_site_env.perception_overrides(site) + s = export_site_env.servo_overrides(site) + assert p["camera_source"] == "cam_a" + assert s["handeye_mode"] == "eye_in_hand" + assert s["handeye_file"] == "/tmp/wrist_eye_in_hand.yaml" + assert s["handeye_file"].endswith("eye_in_hand.yaml") + + def test_overhead_without_range_raises(self): + site = _track_site(perception={"track_camera": "overhead", "overhead": {}}) + with pytest.raises(RuntimeError, match="perception.overhead"): + export_site_env.perception_overrides(site) + + def test_wrist_hybrid_wires_oneshot_without_overwriting_wrist_range(self): + site = _track_site(perception={"track_camera": "wrist"}) + p = export_site_env.perception_overrides(site) + s = export_site_env.servo_overrides(site) + assert p["camera_source"] == "cam_a" + assert p["max_depth_m"] == pytest.approx(0.55) + assert p["overhead_oneshot_enable"] is True + assert p["wrist_handoff_conf"] == pytest.approx(0.5) + assert p["overhead_max_depth_m"] == pytest.approx(1.80) + assert p["overhead_handeye_file"].endswith("01_d435_eye_to_hand.yaml") + assert "d405" not in p["overhead_camera_source"] + assert s["handeye_mode"] == "eye_in_hand" + assert s["lock_still_sec"] == pytest.approx(3.0) + assert s["wrist_handoff_conf"] == pytest.approx(0.5) + assert s["overhead_hold_s"] == pytest.approx(8.0) + + def test_wrist_hybrid_missing_handoff_conf_raises(self): + site = _track_site() + del site["perception"]["wrist_handoff_conf"] + with pytest.raises(RuntimeError, match="wrist_handoff_conf"): + export_site_env.perception_overrides(site) + + def test_wrist_hybrid_missing_still_keys_raises(self): + site = _track_site() + del site["servo"]["lock_still_sec"] + with pytest.raises(RuntimeError, match="lock_still_sec"): + export_site_env.servo_overrides(site) + + def test_overhead_lookat_raises(self): + site = _overhead_ok() + site["servo"] = {"xy_track": "lookat"} + with pytest.raises(RuntimeError, match="lookat"): + export_site_env.servo_overrides(site) + + def test_flatten_track_vars_follow_role(self): + flat = export_site_env.flatten(_overhead_ok()) + assert flat["SITE_TRACK_CAMERA"] == "overhead" + assert flat["SITE_TRACK_CAM_NAME"] == "d435" + assert flat["SITE_CAM_NAME"] == "cam_a" # 腕部精拍路不变 + assert flat["SITE_TRACK_HANDEYE_MODE"] == "eye_to_hand" + assert flat["SITE_TRACK_HANDEYE_FILE"].endswith("01_d435_eye_to_hand.yaml") + assert "wrist_eye_in_hand" not in flat["SITE_TRACK_HANDEYE_FILE"] + + +class TestCamPointToBase: + def test_eye_to_hand_ignores_flange(self): + T_he = np.eye(4) + T_he[:3, 3] = [0.4, -0.5, 0.7] + T_ee = np.eye(4) + T_ee[:3, 3] = [0.6, 0.0, 0.3] + p = np.array([0.0, 0.0, 0.8]) + got = cam_point_to_base(p, T_he, T_ee, "eye_to_hand") + np.testing.assert_allclose(got, [0.4, -0.5, 1.5], atol=1e-9) + # 负向：若误乘法兰，点会多出法兰平移 + wrong = cam_point_to_base(p, T_he, T_ee, "eye_in_hand") + assert np.linalg.norm(got - wrong) > 0.5 + + def test_eye_in_hand_still_left_multiplies_ee(self): + T_he = np.eye(4) + T_he[:3, 3] = [0.0, 0.0, 0.12] + T_ee = np.eye(4) + T_ee[:3, 3] = [0.5, -0.1, 0.3] + p = np.array([0.01, 0.0, 0.2]) + got = cam_point_to_base(p, T_he, T_ee, "eye_in_hand") + np.testing.assert_allclose(got, [0.51, -0.1, 0.62], atol=1e-9) + + def test_illegal_mode_raises(self): + with pytest.raises(ValueError, match="eye_in_hand 或 eye_to_hand"): + cam_to_base_T(np.eye(4), np.eye(4), "eye_on_foot") + + +class TestRealSiteBranchDefault: + """main 上 site.yaml 必须是 D405 主跟踪 + D435 一次性，跟踪外参仍是腕部。""" + + def test_site_yaml_hybrid_wrist_plus_d435_oneshot(self, monkeypatch): + monkeypatch.setenv("SITE_CONFIG", str(ROOT / "config" / "site.yaml")) + monkeypatch.setenv("ROBOT_SN", "Rizon4-063989") + data = export_site_env.load_site() + p = export_site_env.perception_overrides(data) + s = export_site_env.servo_overrides(data) + assert p["camera_source"] in ("d405_left", "d405") + assert p["max_depth_m"] == pytest.approx(0.55) + assert p["wrist_handoff_conf"] == pytest.approx(0.5) + assert p["overhead_oneshot_enable"] is True + assert p["overhead_handeye_file"].endswith("89_d435_eye_to_hand.yaml") + assert p["overhead_max_depth_m"] == pytest.approx(1.80) + assert s["handeye_mode"] == "eye_in_hand" + assert s["handeye_file"].endswith("89_eye_in_hand.yaml") + assert s["lock_still_sec"] == pytest.approx(3.0) + assert s["lock_still_move_m"] == pytest.approx(0.015) + # 负向：跟踪外参不能被换成顶视文件 + assert "eye_to_hand" not in s["handeye_file"] + assert "eye_in_hand" not in p["overhead_handeye_file"] diff --git a/src/arm_application/test/test_perception_acquire_gate.py b/src/arm_application/test/test_perception_acquire_gate.py new file mode 100644 index 0000000..2783744 --- /dev/null +++ b/src/arm_application/test/test_perception_acquire_gate.py @@ -0,0 +1,180 @@ +"""一阶段"重新捕获"的两道守卫：置信度门槛 + 特征点相机量程下限。 + +为什么单拎出来测（2026-10-08 真机，arm 89，一阶段把臂带离工件 500mm） +-------------------------------------------------------------------- +``/tmp/pipeline_stage1.log`` 原话： + + 0659.551 死区保持 |e_xy|=3.0mm ← 已经收敛到 3mm + 0659.598 k0 置信度 0.00 < 0.30，本帧不用它 + 0659.6xx 重新捕获 conf=0.15 dpx=503 ← 503px 外、conf 0.15 的假框 + feat_base=(0.231,-0.484) z=-0.021 ← 跟踪点瞬间横移 427mm + 0670.985 === 一阶段已锁定 === ← "锁定"在离工件 500mm 处 + +视觉模型（真 k0 conf 0.87）与控制（伺服收敛到 3mm）都正常，坏的是逻辑： +``pick_best`` 的"重新捕获"分支把门槛接成了 ``min_conf=0.12``，于是 0.15 的远框 +被原样采信、perception 自己 ``kf.reset()`` 跳过去；伺服侧的 FeatureJumpGate 只能 +拦一帧（假框帧间稳定，3 帧后照样认）。拦不住还有一个更根本的原因：那个假目标 +就在**相机前 0.08m**（爪上工具/相机座，随臂一起动），跟踪其误差恒为 0 —— 伺服 +"收敛"得完美，锁的却是自己。所以还要一道**量纲是米**的判据。 + +前两条是**负向**用例：把门槛调回旧值，第一条必须红。 +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import pytest +import yaml + +from arm_application.yolo_detector import Detection, pick_best + +# 测试在 src/<包>/test/ 下 → 仓库根是 parents[3] +ROOT = Path(__file__).resolve().parents[3] +SITE = ROOT / "config" / "site.yaml" +HANDEYE = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml" + +# 真机实测数字（见本文件头与 site.yaml 的对应注释）—— +# 写死在这里是**故意**的：它们是被测事实，不是配置。 +REAL_K0_CONF = 0.87 # 真 k0 的置信度 +FALSE_BOX_CONF = 0.15 # 被误采信的那个假框 +FALSE_BOX_CONF_HI = 0.47 # 那串假框的实测上沿 +# 相机光轴视距的两条带 [m]（p_cam.z） +ROBOT_NEAR_BAND = (0.078, 0.087) # 随臂一起动：爪上工具 / 相机座 +WORKPIECE_RANGE_BAND = (0.19, 0.29) # 工件面在锁高下 + +# 假框：503px 外、尺寸和真工件框同量级（否则会被 min_area 先挡掉，测不到本意） +PREV = np.array([300.0, 300.0, 460.0, 440.0]) # 上一帧框，中心 (380, 370) +FALSE_FAR = np.array([800.0, 20.0, 960.0, 160.0]) # 中心 (880, 90) → 503px 外 + + +def _det(xyxy: np.ndarray, conf: float) -> Detection: + return Detection(xyxy=np.asarray(xyxy, dtype=float), conf=float(conf), + cls_id=0, cls_name="workpiece") + + +def _pick(conf: float, reacquire_conf: float): + return pick_best( + [_det(FALSE_FAR, conf)], + prev_uv=np.array([380.0, 370.0]), + prev_xyxy=PREV, + max_track_px=400.0, + reacquire_conf=reacquire_conf, + min_conf=0.12, + lock_iou=0.05, + ) + + +def test_old_wiring_would_adopt_the_false_box(): + """负向基线：门槛=min_conf（旧接线）时，conf 0.15 的 503px 远框**会被采信**。 + + 这条是"这个检查能红"的证明 —— 它若开始返回 None，说明前提变了， + 下面"新接线挡得住"就不再是在测同一件事。 + """ + best = _pick(FALSE_BOX_CONF, reacquire_conf=0.12) + assert best is not None, "旧接线本该采信这个假框 —— 前提变了，用例已失真" + assert best.conf == pytest.approx(FALSE_BOX_CONF) + + +def test_new_wiring_rejects_the_false_box(): + """新接线：门槛 0.55 > 假框上沿 0.47 ⇒ 低置信度远框一律不采信。""" + assert _pick(FALSE_BOX_CONF, reacquire_conf=0.55) is None + + +def test_new_wiring_rejects_whole_false_band(): + """实测那串假框 conf 0.13~0.47，全带都要挡住（不能只挡 0.15 那一个）。""" + for conf in (0.13, 0.25, 0.38, FALSE_BOX_CONF_HI): + assert _pick(conf, reacquire_conf=0.55) is None, f"conf={conf} 漏过去了" + + +def test_high_conf_relocation_is_still_adopted(): + """守卫不能过严：真 k0（0.87）级的重定位必须仍然立刻生效。 + + 否则"工件确实被挪了"时臂会僵在原地不动 —— 那是另一种失效。 + """ + best = _pick(REAL_K0_CONF, reacquire_conf=0.55) + assert best is not None and best.conf == pytest.approx(REAL_K0_CONF) + + +# --------------------------------------------------------------------------- # +# 接线：site.yaml 真的把这两个门槛接到了位，且取值区间有实测/几何依据 +# --------------------------------------------------------------------------- # +def _site() -> dict: + assert SITE.is_file(), f"找不到站点配置 {SITE}" + return yaml.safe_load(SITE.read_text(encoding="utf-8")) + + +def _left_arm89(site: dict) -> dict: + for entry in site["robot"]["robots"]: + if str(entry.get("label", "")) == "左臂89": + return entry + pytest.fail("site.yaml 里找不到 左臂89 的条目") + + +def test_site_yaml_wires_reacquire_conf_between_false_band_and_real_k0(): + yolo = _site()["yolo"] + # 键必须存在 —— 缺了会静默退回 min_conf（代码默认值兜着，配置看着"改了"却没用） + assert "reacquire_conf" in yolo, "site.yaml 没接 reacquire_conf（会退回 min_conf）" + v = float(yolo["reacquire_conf"]) + assert FALSE_BOX_CONF_HI < v < REAL_K0_CONF, ( + f"reacquire_conf={v} 必须高于假框带 {FALSE_BOX_CONF_HI}、低于真 k0 {REAL_K0_CONF}" + ) + + +def test_feature_min_range_sits_between_robot_and_workpiece(): + """量程下限必须夹在"机械臂自身"与"工件面"两条实测带之间。""" + site = _site() + assert "feature_min_range_m" in site["yolo"], "site.yaml 没接 feature_min_range_m" + gate = float(site["yolo"]["feature_min_range_m"]) + assert ROBOT_NEAR_BAND[1] < gate < WORKPIECE_RANGE_BAND[0], ( + f"量程下限 {gate} 必须高于随臂近带 {ROBOT_NEAR_BAND[1]}、" + f"低于工件面 {WORKPIECE_RANGE_BAND[0]}" + ) + + +def test_gauge_rod_extends_below_camera_so_near_band_is_the_robot(): + """近带为什么会被占满：规尖比相机更靠下 ⇒ 规杆整段都在相机近场。""" + arm = _left_arm89(_site()) + tip = float(arm["gauge_assembly"]["tool_tip_below_flange_m"]) + doc = yaml.safe_load(HANDEYE.read_text(encoding="utf-8")) + cam_z = abs(float(doc["translation_m"][2])) # 相机在法兰 Z 下方的距离 + assert tip > cam_z + 0.05, ( + f"规尖 {tip:.3f} 应比相机 {cam_z:.3f} 明显更靠下 —— 否则近带论证不成立" + ) + + +def test_feature_min_range_is_not_looser_than_depth_floor(): + """不许把量程下限调到解码下限以下 —— 那等于这道守卫没接。""" + yolo = _site()["yolo"] + assert float(yolo["feature_min_range_m"]) > float(yolo["min_depth_m"]) + assert float(yolo["feature_min_range_m"]) < float(yolo["max_depth_m"]) + + +# --------------------------------------------------------------------------- # +# 接线（代码侧）：site.yaml 写了键还不够，必须真的传进 pick_best / 量程判断 +# --------------------------------------------------------------------------- # +def _perception_src() -> str: + p = ROOT / "src" / "arm_application" / "arm_application" / "perception_node.py" + assert p.is_file(), f"找不到 {p}" + return p.read_text(encoding="utf-8") + + +def test_perception_wires_reacquire_conf_into_pick_best(): + src = _perception_src() + assert "reacquire_conf=self.reacquire_conf" in src, "参数没传进 pick_best，配置等于没接" + # 旧接线不许回来：这就是 2026-10-08 出事的写法 + assert "reacquire_conf=self.min_conf" not in src, "重新捕获门槛又退回 min_conf 了" + assert 'declare_parameter("reacquire_conf"' in src + + +def test_perception_uses_feature_min_range_not_min_depth_for_features(): + src = _perception_src() + assert 'declare_parameter("feature_min_range_m"' in src + assert "self.feature_min_range = max(" in src, "量程下限没有取严（可能被调得比 min_depth 还松）" + # 特征点量程判断必须用 feature_min_range，不能用回 min_depth + assert "self.feature_min_range < z < self.max_depth" in src + assert "(meters > self.feature_min_range)" in src + assert "(meters2 > self.feature_min_range)" in src + assert "min_depth=self.min_depth," not in src, "还有特征测量在用 min_depth" + diff --git a/src/arm_application/test/test_pose_detector.py b/src/arm_application/test/test_pose_detector.py new file mode 100644 index 0000000..6220e7a --- /dev/null +++ b/src/arm_application/test/test_pose_detector.py @@ -0,0 +1,280 @@ +"""pose 模型支持（k0 跟踪 / k1,k2 对孔）的测试。 + +刻意**不依赖仓库里的模型二进制**（`models/**/*.onnx` 都不进 git）： +ONNX 用 `onnx.helper` 现场造，解码用合成张量。所以纯逻辑都能离线跑， +GPU / TensorRT 都不需要 —— engine 那条路只能真机验（`weights.sh doctor --check-engines`）。 + +要挡住的那类 bug（2026-09-29 实测踩到的）： +ultralytics 按 `task="detect"` 去解 pose 的 14 通道输出，会吐 300 个垃圾框， +`conf` 直接是坐标值（600+）—— 现场表现是"跟踪锁到空气上"，在真机上很危险。 +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from arm_application.hole_detect_hough import _keypoint_holes +from arm_application.model_labels import SOURCE_SIDECAR, LabelSet +from arm_application.pose_detector import ( + PoseDetector, + is_pose_model, + keypoint_roles, + letterbox, + nms_xyxy, + read_pose_info, +) +from arm_application.yolo_detector import Detection + + +# -------------------------------------------------------------------------- +# fixture 工具 +# -------------------------------------------------------------------------- +def _make_onnx(path, metadata): + """造一个只有 metadata 的最小 ONNX（图内容无所谓，只测 metadata 解析）。""" + pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）") + import onnx + from onnx import TensorProto, helper + + inp = helper.make_tensor_value_info("images", TensorProto.FLOAT, [1, 3, 8, 8]) + out = helper.make_tensor_value_info("output0", TensorProto.FLOAT, [1, 14, 4]) + graph = helper.make_graph( + [helper.make_node("Identity", ["images"], ["output0"])], "g", [inp], [out] + ) + model = helper.make_model(graph) + model.opset_import[0].version = 13 + for k, v in metadata.items(): + model.metadata_props.add(key=k, value=v) + onnx.save(model, str(path)) + return path + + +def _bare_detector(*, kpt_count=3, conf=0.25, iou=0.45, max_det=20, class_names=None): + """只装解码需要的属性 —— 绕开 __init__ 里的 engine 加载（要 GPU）。""" + det = object.__new__(PoseDetector) + det.kpt_count = kpt_count + det.conf = conf + det.iou = iou + det.max_det = max_det + det.class_ids = None + det.class_names = class_names + det._labels = LabelSet(names={0: "workpiece"}, source=SOURCE_SIDECAR, path=None) + return det + + +def _raw(boxes, kpts, confs, n_anchor=4, nc=1): + """拼一个 (1, 4+nc+3K, A) 的合成输出。boxes=xywh，kpts=[[(x,y,c),...], ...]""" + k = kpts.shape[1] + ch = 4 + nc + 3 * k + o = np.zeros((ch, n_anchor), dtype=np.float32) + n = boxes.shape[0] + o[0:4, :n] = boxes.T + o[4:4 + nc, :n] = confs[None, :] if nc > 1 else confs[None, :] + o[4 + nc:, :n] = kpts.reshape(n, -1).T + return o[None] + + +# -------------------------------------------------------------------------- +# 模型识别：从旁挂 .onnx 的 metadata 判断是不是 pose +# -------------------------------------------------------------------------- +def test_pose_model_is_recognized(tmp_path): + _make_onnx( + tmp_path / "m.onnx", + { + "task": "pose", + "kpt_shape": "[3, 3]", + "kpt_names": "{0: ['0', '1', '2']}", + "names": "{0: 'workpiece'}", + }, + ) + info = read_pose_info(tmp_path / "m.onnx") + assert info is not None + assert info["kpt_count"] == 3 + assert info["names"] == {0: "workpiece"} + assert is_pose_model(tmp_path / "m.onnx") + + +def test_detect_model_is_not_pose(tmp_path): + _make_onnx(tmp_path / "d.onnx", {"task": "detect", "names": "{0: 'workpiece'}"}) + assert read_pose_info(tmp_path / "d.onnx") is None + assert not is_pose_model(tmp_path / "d.onnx") + + +def test_pose_without_kpt_shape_is_not_guessed(tmp_path): + """没有 kpt_shape 就别猜关键点个数 —— 猜错是"静默错位"，比报错难查得多。""" + _make_onnx(tmp_path / "p.onnx", {"task": "pose", "names": "{0: 'workpiece'}"}) + assert read_pose_info(tmp_path / "p.onnx") is None + + +def test_engine_reads_pose_info_from_sibling_onnx(tmp_path): + """engine 没有 metadata（TRT 10 Python API 就没有接口），要靠同名的 .onnx。""" + _make_onnx( + tmp_path / "m.onnx", {"task": "pose", "kpt_shape": "[3, 3]", "names": "{0: 'workpiece'}"} + ) + (tmp_path / "m.engine").write_bytes(b"not a real engine") + assert is_pose_model(tmp_path / "m.engine") + + +def test_pose_info_missing_onnx_returns_none(tmp_path): + (tmp_path / "m.engine").write_bytes(b"not a real engine") + assert read_pose_info(tmp_path / "m.engine") is None + + +# -------------------------------------------------------------------------- +# letterbox：坐标还原全靠它，错了整幅图的框都会偏 +# -------------------------------------------------------------------------- +def test_letterbox_preserves_aspect_and_reports_padding(): + img = np.zeros((480, 848, 3), dtype=np.uint8) + out, gain, (dw, dh) = letterbox(img, 640) + assert out.shape == (640, 640, 3) + # 848x480 → gain = 640/848；等比缩到 640x362，纵向补 139 + assert gain == pytest.approx(640 / 848, rel=1e-6) + assert dw == pytest.approx(0.0, abs=0.5) + assert dh == pytest.approx(139.0, abs=1.0) + + +def test_letterbox_square_input_is_untouched(): + img = np.zeros((640, 640, 3), dtype=np.uint8) + out, gain, (dw, dh) = letterbox(img, 640) + assert out.shape == (640, 640, 3) + assert gain == pytest.approx(1.0) + assert (dw, dh) == (0.0, 0.0) + + +# -------------------------------------------------------------------------- +# NMS +# -------------------------------------------------------------------------- +def test_nms_suppresses_overlap_and_keeps_highest(): + boxes = np.array([[0, 0, 100, 100], [5, 5, 105, 105], [500, 500, 600, 600]], dtype=float) + scores = np.array([0.9, 0.8, 0.7]) + keep = nms_xyxy(boxes, scores, 0.5, max_det=10) + assert list(keep) == [0, 2] + + +def test_nms_respects_max_det(): + boxes = np.array([[0, 0, 10, 10], [100, 100, 110, 110], [200, 200, 210, 210]], dtype=float) + assert len(nms_xyxy(boxes, np.array([0.9, 0.8, 0.7]), 0.5, max_det=2)) == 2 + + +# -------------------------------------------------------------------------- +# 解码：框 + 关键点，并去 letterbox 还原到原图 +# -------------------------------------------------------------------------- +def test_decode_returns_box_and_keypoints_unletterboxed(): + det = _bare_detector() + boxes = np.array([[320.0, 320.0, 100.0, 80.0]]) + kpts = np.array([[[300.0, 300.0, 0.9], [360.0, 300.0, 0.8], [280.0, 310.0, 0.7]]]) + raw = _raw(boxes, kpts, np.array([0.90])) + + out = det._decode(raw, gain=0.5, dw=10.0, dh=20.0) + assert len(out) == 1 + d = out[0] + assert d.cls_name == "workpiece" + assert d.conf == pytest.approx(0.90, abs=1e-5) + # ((cx±w/2) - dw) / gain + assert list(np.round(d.xyxy, 3)) == [520.0, 520.0, 720.0, 680.0] + assert d.kpts.shape == (3, 3) + assert list(np.round(d.kpts[0][:2], 3)) == [580.0, 560.0] + assert d.kpts[0][2] == pytest.approx(0.9, abs=1e-5) + assert list(np.round(d.kpts[1][:2], 3)) == [700.0, 560.0] + assert list(np.round(d.kpts[2][:2], 3)) == [540.0, 580.0] + + +def test_decode_drops_low_conf_anchors(): + det = _bare_detector(conf=0.5) + boxes = np.array([[100.0, 100.0, 20.0, 20.0], [400.0, 400.0, 20.0, 20.0]]) + kpts = np.zeros((2, 3, 3), dtype=float) + raw = _raw(boxes, kpts, np.array([0.9, 0.1])) + out = det._decode(raw, gain=1.0, dw=0.0, dh=0.0) + assert len(out) == 1 + assert out[0].conf == pytest.approx(0.9, abs=1e-5) + + +def test_decode_raises_when_channels_do_not_match_kpt_count(): + """通道数和 kpt_shape 对不上时必须报错，不能默默解出一堆垃圾。""" + det = _bare_detector(kpt_count=3) + raw = np.zeros((1, 6, 4), dtype=np.float32) # 6 通道装不下 3 个关键点 + with pytest.raises(RuntimeError, match="通道数不对"): + det._decode(raw, gain=1.0, dw=0.0, dh=0.0) + + +def test_decode_of_detect_style_output_would_be_garbage(): + """回归：把 pose 的 14 通道输出当检测解 → conf 会变成 600+ 的坐标值。 + + 这条测的是"我们**没有**走那条路"：同一份合成输出，按 pose 解出来的 conf + 必须在 (0,1]，绝不能是坐标量级。 + """ + det = _bare_detector() + boxes = np.array([[600.0, 640.0, 100.0, 80.0]]) + kpts = np.array([[[610.0, 300.0, 0.9], [620.0, 300.0, 0.8], [630.0, 310.0, 0.7]]]) + raw = _raw(boxes, kpts, np.array([0.9])) + out = det._decode(raw, gain=1.0, dw=0.0, dh=0.0) + assert len(out) == 1 + assert 0.0 < out[0].conf <= 1.0 + + +# -------------------------------------------------------------------------- +# k1/k2：模型语义顺序，**不按图像 x 交换** +# -------------------------------------------------------------------------- +def _det_with_kpts(kpts, conf=0.9): + return Detection( + xyxy=np.array([300.0, 300.0, 500.0, 400.0]), + conf=conf, + cls_id=0, + cls_name="workpiece", + kpts=np.asarray(kpts, dtype=float), + ) + + +def _kw(**over): + cfg = dict(conf_thr=0.25, min_kconf=0.30, radius_px=12.0, min_dist_px=15.0) + cfg.update(over) + return cfg + + +def test_keypoint_holes_keeps_model_semantic_order(): + """k1 在画面右边、k2 在左边时，**不能**交换。 + + Hough 路径的 k1/k2 是"图像左/右"（位置定义）；pose 的 k1/k2 是训练时标注的 + 固定物理孔。按 x 交换会让 target_hole=k1 在工件转个角度后指向**另一个**孔 —— + 现场表现为"有时对左孔、有时对右孔"，非常难查。 + """ + det = _det_with_kpts([[400, 350, 1.0], [470, 340, 1.0], [330, 345, 1.0]]) + out = _keypoint_holes([det], **_kw()) + assert out is not None + (k1x, k1y), (k2x, k2y), r1, r2 = out + assert (k1x, k1y) == (470.0, 340.0), "k1 必须还是模型的 k1（画面右）" + assert (k2x, k2y) == (330.0, 345.0), "k2 必须还是模型的 k2（画面左）" + assert (r1, r2) == (12.0, 12.0) + + +def test_keypoint_holes_rejects_low_confidence(): + det = _det_with_kpts([[400, 350, 1.0], [470, 340, 0.10], [330, 345, 0.05]]) + assert _keypoint_holes([det], **_kw()) is None + + +def test_keypoint_holes_rejects_coincident_points(): + det = _det_with_kpts([[400, 350, 1.0], [401, 350, 1.0], [402, 350, 1.0]]) + assert _keypoint_holes([det], **_kw(min_dist_px=15.0)) is None + + +def test_keypoint_holes_returns_none_for_plain_detection(): + """普通检测模型没有 kpts → 必须返回 None，让 Hough 老路径接手。""" + det = Detection(xyxy=np.array([0.0, 0.0, 10.0, 10.0]), conf=0.9, cls_id=0, cls_name="workpiece") + assert _keypoint_holes([det], **_kw()) is None + + +def test_keypoint_holes_skips_low_conf_detection(): + det = _det_with_kpts([[400, 350, 1.0], [470, 340, 1.0], [330, 345, 1.0]], conf=0.05) + assert _keypoint_holes([det], **_kw(conf_thr=0.25)) is None + + +# -------------------------------------------------------------------------- +# 角色约定 +# -------------------------------------------------------------------------- +def test_keypoint_roles_defaults_to_k0_track_k1k2_holes(): + assert keypoint_roles(3) == (0, (1, 2)) + + +def test_keypoint_roles_handles_models_with_fewer_points(): + assert keypoint_roles(1) == (0, ()) + assert keypoint_roles(2) == (0, (1,)) diff --git a/src/arm_application/test/test_stage2_coarse_guard.py b/src/arm_application/test/test_stage2_coarse_guard.py new file mode 100644 index 0000000..7a2ef41 --- /dev/null +++ b/src/arm_application/test/test_stage2_coarse_guard.py @@ -0,0 +1,263 @@ +"""粗居中的两条误检守卫：**框被画面切** + **目标点离谱/盒外**。 + +为什么单独立一条（2026-10-08 真机 arm 89，二阶段 exit=4） +-------------------------------------------------------- +``/tmp/run_all3.log`` 原话（一阶段锁定 ✓、二阶段悬停到位 ✓，死在这一步）:: + + 悬停到位: 法兰 Z=0.320m（目标 0.320，差 +0mm） 残余倾角 0.37deg + snap_coarse_0: 工件 bbox=(0, 142, 97, 222) 中心=(48,182) + base=(0.280,-0.747)[深度] 夹爪=(0.336,-0.543) 偏差=211.5mm + 目标超出工作盒，已截断: (0.280,-0.747,0.320) → (0.280,-0.545,0.320) + plan coarse Δ=(-56,-2,-0)mm ← 把臂带离工件 56mm + snap_coarse_1: 工件 bbox=[0, 0, 847, 479]（整幅画面）偏差=55.9mm + snap_fine_0..8: 两孔基座间距 105.8 / 210.0 / 213.2 / …mm + （期望 32–85mm）丢弃本拍 + 二阶段失败 exit=4，停在当前位姿 + +工件其实在 (0.5,-0.12) 一带。两个缺陷叠在一起： + + 1. ``bbox=(0,142,97,222)`` 的 ``x0=0`` —— 被画面**左边缘切掉**的半框。框中心 + 根本不在工件上，深度反投影因此给出盒外的 (0.280,-0.747)； + 2. 这个盒外目标**没有被判误检**，而是"截断到盒边"后照样移动 —— 越界只被夹到 + 盒边，臂还是走了。这就是规矩里禁的"宽容降级"：把"没检查"伪装成"检查通过"。 + 一挪错，精定位就再也看不到 k1/k2，而流程把它报成"孔检测失败"（exit=4）。 + +修法：``_bbox_on_border`` 弃用贴边框、``_coarse_target_reject`` 在**移动前** +否掉盒外/离谱目标（判误检 ⇒ 原地不动，交给精定位）；以及 YOLO 无可信框但精定位 +已看到 k1/k2 时**真的不移动**（旧代码嘴上说"跳过居中"却仍然算偏差横移）。 + +这些用例全是**离线**的：两个被判的函数都是纯逻辑，用打桩对象即可，不建 ROS 实体 +（与 ``test_stage2_descent_clamp`` 同一套做法）。 +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any, List, Optional + +import numpy as np +import pytest +import yaml + +from arm_application.stage2_locate_node import Stage2Locate + +# 测试在 src/<包>/test/ 下 → 仓库根是 parents[3] +ROOT = Path(__file__).resolve().parents[3] +SITE = ROOT / "config" / "site.yaml" + +# ---- 真机实测数字（见本文件头，故意写死：它们是被测事实，不是配置）---- +FRAME_W, FRAME_H = 848, 480 # D405 彩色图尺寸（bbox [0,0,847,479] 可反推） +BAD_BBOX_CLIPPED = (0, 142, 97, 222) # 左边缘切掉的半框 —— 事故那一帧 +DEGENERATE_BBOX = (0, 0, 847, 479) # 孔检测器兜底给的整幅画面 +GOOD_BBOX = (300, 150, 520, 330) # 一个正常的、不贴边的框 +BAD_TARGET_BASE = np.array([0.280, -0.747, 0.320]) # 那帧反投影出来的点（盒外） +GAUGE_XY = np.array([0.336, -0.543]) # 那帧的夹爪轴线 XY +BAD_ERR_M = 0.2115 # 日志里的"偏差=211.5mm" +CLAMPED_Y = -0.545 # 日志里的"截断"结果 +# 工作盒（site.yaml: robots[].servo.workspace_xyz_*） +WS_MIN = np.array([0.08, -0.55, 0.18]) +WS_MAX = np.array([0.85, 0.55, 0.65]) + + +class _StubLogger: + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def _add(self, msg: str) -> None: + self._sink.append(str(msg)) + + info = _add + warn = _add + error = _add + + +class _Stub: + """两个被判函数只用到这几个成员，其余全部打桩。""" + + def __init__( + self, + *, + ws_min: Any = WS_MIN, + ws_max: Any = WS_MAX, + max_offset_m: float = 0.12, + border_px: int = 2, + ) -> None: + self.logs: List[str] = [] + self.ws_min = ws_min + self.ws_max = ws_max + self.coarse_max_offset_m = max_offset_m + self.coarse_border_px = border_px + + def get_logger(self) -> Any: + return _StubLogger(self.logs) + + +def _reject(st: _Stub, p: Any, g: Any, max_offset_m: Optional[float] = None) -> Optional[str]: + return Stage2Locate._coarse_target_reject( # type: ignore[arg-type] + st, np.asarray(p, dtype=float), np.asarray(g, dtype=float), max_offset_m + ) + + +def _on_border(st: _Stub, bbox: Any, w: int = FRAME_W, h: int = FRAME_H) -> bool: + return Stage2Locate._bbox_on_border(st, bbox, w, h) # type: ignore[arg-type] + + +# --------------------------------------------------------------------------- # +# 守卫一：被画面边缘切掉的框一律弃用 +# --------------------------------------------------------------------------- # +def test_clipped_bbox_is_rejected(): + """负向基线：事故那一帧的框贴左边（x0=0），必须判"不可用"。""" + assert _on_border(_Stub(), BAD_BBOX_CLIPPED) is True + + +def test_degenerate_full_frame_bbox_is_rejected(): + """整幅画面当工件（四边全贴）也必须弃用 —— 它的"中心"就是画面中心，毫无信息。""" + assert _on_border(_Stub(), DEGENERATE_BBOX) is True + + +def test_normal_bbox_is_kept(): + """守卫不能过严：正常的、不贴边的框必须仍然可用，否则粗居中彻底废掉。""" + assert _on_border(_Stub(), GOOD_BBOX) is False + + +def test_bbox_touching_any_single_edge_is_rejected(): + """四条边各自贴边都要挡住（只挡左边就漏掉了真机 coarse_move 1 那种整幅图）。""" + for bbox in ( + (0, 100, 300, 300), + (100, 0, 300, 300), + (100, 100, FRAME_W - 1, 300), + (100, 100, 300, FRAME_H - 1), + ): + assert _on_border(_Stub(), bbox) is True, f"{bbox} 贴边却没被挡住" + + +def test_border_margin_is_config_driven_not_hardcoded(): + """余量必须来自配置：margin=0 时 3px 贴边的框算贴边，margin 大时更靠内也算贴边。""" + assert _on_border(_Stub(border_px=2), (1, 100, 300, 300)) is True + assert _on_border(_Stub(border_px=0), (3, 100, 300, 300)) is False + assert _on_border(_Stub(border_px=8), (6, 100, 300, 300)) is True + + +def test_none_bbox_is_rejected(): + assert _on_border(_Stub(), None) is True + + +# --------------------------------------------------------------------------- # +# 守卫二：目标点在移动前就被否掉（而不是"截断到盒边照样走"） +# --------------------------------------------------------------------------- # +def test_old_behaviour_clamped_the_target_and_therefore_relocated_the_arm(): + """负向基线：证明"截断"不是安全兜底，而是一条真实的横移指令。 + + 这条是"这个检查能红"的前提证明 —— 如果哪天 ``_clamp_ws`` 改成直接拒绝， + 下面的用例就不是在测同一件事了。 + """ + st = _Stub() + clamped = Stage2Locate._clamp_ws(st, BAD_TARGET_BASE) # type: ignore[arg-type] + + assert clamped[1] == pytest.approx(CLAMPED_Y, abs=1e-6), "夹具前提变了" + moved_mm = float(np.hypot(clamped[0] - BAD_TARGET_BASE[0], + clamped[1] - BAD_TARGET_BASE[1])) * 1000.0 + assert moved_mm > 190.0, ( + f"截断只把目标挪了 {moved_mm:.1f}mm —— 与真机日志不符，用例已失真" + ) + + +def test_bad_target_is_rejected_outside_workspace(): + """真机那一帧的目标 (0.280,-0.747) 在盒外（y < -0.55），必须判误检。""" + why = _reject(_Stub(), BAD_TARGET_BASE, GAUGE_XY) + assert why is not None, "盒外目标没被挡住 —— 就是它把臂带走 56mm 的" + assert "工作盒" in why, f"拒绝原因要说清是盒外（实际：{why}）" + + +def test_bad_target_is_rejected_even_if_inside_workspace(): + """把目标夹进盒内之后，211.5mm 的偏移本身也必须挡住 —— 两条判据解耦。 + + 这样即使以后放宽工作盒，第二道判据仍然拦得住"坏深度反投影出来的点"。 + """ + inside = np.array([0.206, CLAMPED_Y, 0.320]) # 盒内（x/y 都在界内），但偏移 130mm + err = float(np.hypot(inside[0] - GAUGE_XY[0], inside[1] - GAUGE_XY[1])) + assert err > 0.12, "夹具前提变了：这个点已经不是大偏移" + assert WS_MIN[0] < inside[0] < WS_MAX[0] and WS_MIN[1] < inside[1] < WS_MAX[1] + why = _reject(_Stub(), inside, GAUGE_XY) + assert why is not None, f"盒内但偏移 {err*1000:.1f}mm 的目标漏过去了" + assert "上限" in why, f"拒绝原因要说清是超偏移上限（实际：{why}）" + + +def test_good_target_is_accepted(): + """守卫不能过严：一阶段刚锁定时"臂在工件上方、差几厘米"的正常量必须放行。""" + p = np.array([GAUGE_XY[0] + 0.030, GAUGE_XY[1] + 0.012, 0.320]) + assert _reject(_Stub(), p, GAUGE_XY) is None + + +def test_offset_limit_is_config_driven(): + """上限是配置项：放开后同一个点必须放行，否则阈值是写死的、配置等于没接。""" + assert _reject(_Stub(max_offset_m=0.12), BAD_TARGET_BASE, GAUGE_XY) is not None + # 偏移上限放到 10m ⇒ 只剩"盒外"这条在拦；说明两条判据不是同一条的复述 + why = _reject(_Stub(max_offset_m=10.0), BAD_TARGET_BASE, GAUGE_XY) + assert why is not None and "工作盒" in why + + +def test_offset_guard_alone_blocks_a_point_that_is_inside_the_box(): + """只放宽工作盒（盒内）时，偏移这条必须独立生效。""" + wide = _Stub(ws_min=np.array([-9.0, -9.0, 0.0]), ws_max=np.array([9.0, 9.0, 9.0])) + why = _reject(wide, BAD_TARGET_BASE, GAUGE_XY) + assert why is not None and "上限" in why + + +def test_nonfinite_target_is_rejected(): + assert _reject(_Stub(), [np.nan, -0.5, 0.32], GAUGE_XY) is not None + assert _reject(_Stub(), [0.3, np.inf, 0.32], GAUGE_XY) is not None + + +# --------------------------------------------------------------------------- # +# 接线：配置 + 调用点（写了键却没接线 = 等于没检查） +# --------------------------------------------------------------------------- # +def _site() -> dict: + assert SITE.is_file(), f"找不到站点配置 {SITE}" + return yaml.safe_load(SITE.read_text(encoding="utf-8")) + + +def _stage2_src() -> str: + p = ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py" + assert p.is_file(), f"找不到 {p}" + return p.read_text(encoding="utf-8") + + +def test_site_yaml_has_the_two_guard_keys(): + # 路径与源码同源：stage2_locate_node 读的是 site["hole_detect"]["stage2"] + stage2 = (_site().get("hole_detect") or {}).get("stage2") or {} + assert "coarse_max_offset_m" in stage2, "site.yaml 没接 coarse_max_offset_m" + assert "coarse_border_px" in stage2, "site.yaml 没接 coarse_border_px" + off = float(stage2["coarse_max_offset_m"]) + # 必须小于真机那次出事的 211.5mm、又大于合理粗居中的厘米级位移 + assert 0.03 < off < BAD_ERR_M, ( + f"coarse_max_offset_m={off} 必须夹在'合理粗居中'与'那次 211.5mm'之间" + ) + + +def test_best_bbox_wires_the_border_guard(): + src = _stage2_src() + assert "self._bbox_on_border(bbox, w, h)" in src, "贴边守卫没接到 _best_bbox" + assert "def _bbox_on_border(" in src + # 旧写法不许回来：贴边的框直接当检测结果用 + assert "return tuple(int(v) for v in best.xyxy)" not in src, ( + "又直接采信检测框了（没有过贴边守卫）" + ) + + +def test_coarse_loop_rejects_before_moving(): + src = _stage2_src() + assert "self._coarse_target_reject(p_base, gxy)" in src, "误检守卫没接在移动前" + assert "def _coarse_target_reject(" in src + # 旧写法不许回来：拿孔检测器的整幅 bbox 当粗居中目标，然后照样移动 + assert 'bbox = holes.get("bbox")' not in src, ( + "又在咬孔检测器的 bbox 当粗居中目标了（真机那帧是整幅画面）" + ) + assert "skip_coarse = True" in src, "'跳过粗居中'必须真的不移动" + + +def test_guard_reason_is_logged_with_both_numbers(): + """判误检必须把"目标点 vs 夹爪"两个数都喊出来，否则现场查不出是谁离谱。""" + src = _stage2_src() + assert "粗定位目标判误检" in src + assert "**不移动**" in src, "日志要说清处置方式（不移动），别让人以为它动了" diff --git a/src/arm_application/test/test_stage2_descent_clamp.py b/src/arm_application/test/test_stage2_descent_clamp.py new file mode 100644 index 0000000..39718f5 --- /dev/null +++ b/src/arm_application/test/test_stage2_descent_clamp.py @@ -0,0 +1,357 @@ +"""轻降的下降量必须由**几何**决定，深度测量只许做交叉核对。 + +两段真机事故，方向相反，都指向同一个结论：那套"规杆外环形"的间隙读数不能用。 +-------------------------------------------------------------------------- +2026-09-29（**多降**，撞工件）:: + + 轻降准备: 规尖-工件间隙 141.0mm → 目标 2.0mm，需下降 139.0mm + +按几何真值（悬停 0.320 − 工具长 0.245 − 工件面 0.022）尖端只在面上 **53mm**， +它多降约 86mm，把通止规撞进孔，工件被推走 5mm（k1 x: 0.4907 → 0.4967）。 + +2026-10-08（**少降**，永远到不了 2mm）:: + + 轻降被几何夹逼：测量出的间隙 144.0mm 与几何不符（…= 75.0mm） + … + 已达安全下限间隙 -7.0mm，停止下降（不接触） + === 轻降完成: 规尖悬在孔面上方 ≈18.0mm （目标 2.0mm，不接触）=== + +这次读数反着错：悬停时报 144mm（几何真值 75mm），只降 24mm 后又翻成 **−7mm**， +``SAFE_FLOOR`` 于是把轻降停在离面约 50mm 处 —— **永远到不了 2mm**。 + +结论：同一套读数在两个方向上各错一次、幅度 150mm 量级、还随位姿乱跳。现在 +``_descend_above_hole`` **只**用几何算下降量（法兰 FK − 工具长 − 工件面 − final_gap）， +深度读数只在偏差 >10mm 时喊一声，**不参与任何决策**。安全由三条物理/几何机制保证： + + 1. 几何预算上限 ``descent_budget_m``（与 ``gauge_fsm.hover_z_guard`` 同源）； + 2. 腕部 FT 接触检测 ⇒ 停 + 回退 1mm； + 3. 任一步 ``_goto`` 没走到位 ⇒ 停，保持当前高度。 + +这些用例全是**离线**的：``_descend_above_hole`` 只用到少量成员，用打桩对象即可， +不建任何 ROS 实体（与 ``test_stage2_no_follow`` 同一套做法）。 +""" + +from __future__ import annotations + +from typing import Any, List, Optional + +import numpy as np + +from arm_application.stage2_locate_node import Stage2Locate + +# 与真机那次一致的一组数（site.yaml 现行值） +_HOVER_Z = 0.320 +_TOOL_LEN = 0.245 +_SURFACE_Z = 0.022 +_FINAL_GAP = 0.002 +# ⇒ 几何间隙 = 0.320 − 0.245 − 0.022 = 53mm；需下降 = 51mm +_GAP0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z +_BUDGET = _GAP0 - _FINAL_GAP + +# 真机实测的两个坏读数（工件面被量成 242mm 而不是 ~175mm，差 86mm） +_BAD_Z_WP = 0.242 +_TIP_D = 0.101 +_BAD_GAP0 = _BAD_Z_WP - _TIP_D # = 141mm + + +class _StubLogger: + """把日志写进调用方给的 sink —— 必须与 ``_StubStage2.logs`` 是**同一个** list， + 否则断言永远看不到日志（踩过一次：两个 list 各写各的）。""" + + def __init__(self, sink: List[str]) -> None: + self._sink = sink + + def _add(self, msg: str) -> None: + self._sink.append(str(msg)) + + info = _add + warn = _add + error = _add + + +class _StubStage2: + """``_descend_above_hole`` 用到的成员全部打桩。 + + ``z_wp`` 固定不变 = 复现"环形深度读数一直错/乱跳"的场景。 + """ + + def __init__( + self, + *, + z_wp: Optional[float] = _BAD_Z_WP, + budget: Optional[float] = _BUDGET, + tool_len: float = _TOOL_LEN, + surface_z: float = _SURFACE_Z, + contact_after_steps: Optional[int] = None, + zwp_slope: Optional[float] = None, + ft_fz: Optional[float] = 3.0, + ) -> None: + self.logs: List[str] = [] + self._logger = _StubLogger(self.logs) + self.final_descent_enable = True + self.final_gap_m = _FINAL_GAP + self.final_descent_speed = 0.005 + self.vert_tol_rad = 0.01 + self.hover_z = _HOVER_Z + self.tool_tip_below_flange_m = tool_len + self.surface_z_m = surface_z + self.descent_budget_m = budget + self.descent_final_gap_m: Optional[float] = None + self._z_wp = z_wp + # zwp_slope ≠ None 时复现 2026-10-08 实测那种"读数随下降乱掉"： + # 真实工件面深度只随下降 1:1 减小，但那个环形读数掉得**快很多倍** + # （实测：降 24mm 就从 +144mm 间隙翻到 −7mm）。斜率 6 正是这个倍率。 + self._zwp_slope = zwp_slope + self._cur_z = _HOVER_Z + self._ft_fz = ft_fz + self._contact_after = contact_after_steps + # 记录每一次实际下发的下降步长，用来核对累计行程 + self.steps_mm: List[float] = [] + self.pins = 0 + + # --- 被测函数会调到的钩子 --- + def _state(self, _name: str) -> None: + return None + + def _measure_gauge_state(self) -> Any: + if self._zwp_slope is not None and self._z_wp is not None: + z_wp = self._z_wp - self._zwp_slope * (_HOVER_Z - self._cur_z) + else: + z_wp = self._z_wp + return _TIP_D, np.ones((4, 4), dtype=bool), z_wp + + def _contact_detected(self, _base_fz: float) -> bool: + # ``contact_after_steps`` 步之后"碰到工件"（复现 FT 兜底） + return self._contact_after is not None and len(self.steps_mm) >= self._contact_after + + def _pin(self, _q: Any) -> None: + self.pins += 1 + + def _goto(self, _q: Any, Tee: Any, target: Any, **_kw: Any) -> Any: + Tee = np.array(Tee, dtype=float) + step_mm = (Tee[2, 3] - float(target[2])) * 1000.0 + # 记录"下降步"（正数）；回退步（负数）不计入下降行程 + if step_mm > 0.0: + self.steps_mm.append(step_mm) + Tee[2, 3] = float(target[2]) + self._cur_z = float(target[2]) + return True, np.zeros(7), Tee + + def get_logger(self) -> Any: + return self._logger + + +def _run(st: _StubStage2) -> Any: + # ``_descend_above_hole`` 循环开头有 ``rclpy.ok()`` 兜底（运行期 Ctrl-C 时退出）， + # 而 ``rclpy.ok()`` 在没 init 过时恒为 False —— 不初始化的话函数会在第一行就返回。 + import rclpy + + if not rclpy.ok(): + rclpy.init() + Tee0 = np.eye(4) + Tee0[2, 3] = _HOVER_Z + ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type] + st, np.zeros(7), Tee0 + ) + return ok, Tee, st + + +# --------------------------------------------------------------------------- # +# 核心回归：坏读数**不能**再决定下降量（两个方向都不行） +# --------------------------------------------------------------------------- # +def test_bad_reading_no_longer_shortens_the_descent(): + """2026-10-08 的回归：读数随下降乱掉时，也要**降满几何预算**到 2mm。 + + 旧逻辑里 ``SAFE_FLOOR`` 会被这个读数触发 —— 实测它降 24mm 就从 +144mm 翻到 + −7mm（``zwp_slope`` 复现这个倍率），于是轻降被停在离面 ~50mm 处，**永远到不了 + 2mm**。这就是本次要修的现象。 + """ + ok, Tee, st = _run(_StubStage2(zwp_slope=6.0)) + + assert ok is True + total_mm = sum(st.steps_mm) + assert abs(total_mm - _BUDGET * 1000) < 1.0, ( + f"应降满几何预算 {_BUDGET * 1000:.1f}mm 到 2mm 目标，实际只降了 {total_mm:.1f}mm " + "—— 又被那个乱跳的读数叫停了" + ) + gap_final_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000 + assert abs(gap_final_mm - _FINAL_GAP * 1000) < 1.0, ( + f"最终几何间隙 {gap_final_mm:.1f}mm，应当到 {_FINAL_GAP * 1000:.1f}mm" + ) + assert Tee[2, 3] >= _SURFACE_Z + _TOOL_LEN, "规尖已低于工件面 —— 撞了" + + +def test_bad_reading_cannot_drive_it_into_the_workpiece(): + """2026-09-29 的回归：读数往**大**错时，下降量也不许超过几何预算。 + + 那次读数报 141mm ⇒ 想降 139mm；真值只有 53mm ⇒ 多降 86mm 撞进工件。 + """ + # 读数报 141mm，同时把 budget 也按真值给（51mm） + ok, Tee, st = _run(_StubStage2(z_wp=_BAD_Z_WP)) + + assert ok is True + total_mm = sum(st.steps_mm) + assert total_mm <= _BUDGET * 1000 + 0.5, ( + f"下降了 {total_mm:.1f}mm > 几何预算 {_BUDGET * 1000:.1f}mm —— " + "又是那个「多降 86mm 撞工件」的 bug" + ) + assert Tee[2, 3] >= _SURFACE_Z + _TOOL_LEN - 1e-9, ( + f"法兰停在 {Tee[2, 3]:.4f}，规尖 {Tee[2, 3] - _TOOL_LEN:.4f} 低于工件面 {_SURFACE_Z}" + ) + + +def test_disagreement_is_announced_with_both_numbers(): + """深度与几何不符时必须喊出来，并把两个数都打出来。 + + 否则现场只看到"下降量变了"，查不出是测量坏了还是配置写错了 —— 而"测距方法 + 待修"这件事必须留在日志里，不能被静默吞掉。 + """ + _ok, _Tee, st = _run(_StubStage2()) + + hit = [m for m in st.logs if "深度测距与几何不符" in m] + assert hit, "读数与几何差 88mm 却一声不吭 —— 静默比报错难查得多" + text = " ".join(hit) + assert f"{_BAD_GAP0 * 1000:.1f}mm" in text, "要打出测量值，才看得出它离谱" + assert f"{_GAP0 * 1000:.1f}mm" in text, "要打出几何值，才能核对配置" + assert "按几何下降" in text, "要说清本次是按几何走的（读数不参与决策）" + + +def test_missing_gauge_visibility_still_descends_on_geometry(): + """规不在视野（测不到）时**仍按几何下降** —— 下降量本来就不依赖规是否可见。 + + 旧逻辑在这里直接"跳过轻降、停在悬停高"，等于轻降功能随机失效。 + """ + st = _StubStage2(z_wp=None) # 测不到工件面 + ok, Tee, st = _run(st) + + assert ok is True + total_mm = sum(st.steps_mm) + assert abs(total_mm - _BUDGET * 1000) < 1.0, "测不到就该按几何降满，而不是跳过" + assert any("深度核对跳过" in m for m in st.logs), "跳过了核对也要讲一声" + + +# --------------------------------------------------------------------------- # +# 缺几何 ⇒ 不许降（绝不用测距去赌） +# --------------------------------------------------------------------------- # +def test_missing_geometry_refuses_to_descend(): + """工具长/工件面没配时**必须不下降**并报错，不能退回"按测量降"。 + + 2026-09-29 撞工件就是"没有几何、只有测距"时发生的。旧测试甚至把这个危险 + 行为钉成了"legacy 行为"（按测量降 139mm）—— 那条是错的，已改。 + """ + st = _StubStage2(budget=None) + ok, Tee, st = _run(st) + + assert ok is False, "缺几何必须判失败（保持悬停），不能假装完成" + assert not st.steps_mm, "缺几何时一步都不许降" + assert abs(Tee[2, 3] - _HOVER_Z) < 1e-9, "臂必须停在原位" + assert any("轻降缺少几何" in m for m in st.logs), "缺几何要说清缺什么、怎么补" + + +def test_zero_tool_length_refuses_to_descend(): + """工具长为 0（没标定）同样不许降 —— 那是"拿 0 当尺子"。""" + st = _StubStage2(tool_len=0.0) + ok, _Tee, st = _run(st) + assert ok is False and not st.steps_mm + + +# --------------------------------------------------------------------------- # +# 真安全机制仍在：FT 接触、goto 失败 +# --------------------------------------------------------------------------- # +def test_ft_contact_stops_and_backs_off(): + """腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。""" + st = _StubStage2(contact_after_steps=2) + ok, Tee, st = _run(st) + + assert ok is True, "接触后停住即算安全收工" + assert any("腕部力突变" in m for m in st.logs), "碰到工件却没喊" + # 只降了 2 步就被拦住（远没走满预算） + assert sum(st.steps_mm) < _BUDGET * 1000 - 5.0, "力突变后还在继续降" + # 回退 1mm：最终位置应高于"最后一步的下发目标" + assert Tee[2, 3] > _HOVER_Z - _BUDGET, "回退没生效" + + +def test_every_step_logs_the_ft_force(): + """每一步都要打出 FT 轴向力 —— 这是"停住是碰到还是没跟上"的唯一直接证据。 + + 2026-10-08 排查"轻降在离面 ~4mm 处停住"时全靠这一行；没有它只能猜。 + """ + _ok, _Tee, st = _run(_StubStage2()) + ft_lines = [m for m in st.logs if "FT_fz=" in m] + assert ft_lines, "每步的 FT 读数没打进日志 —— 排查时无从判断是否接触" + assert any("基线" in m for m in ft_lines), "要给出基线与增量，才看得出力有没有变" + + +def test_missing_ft_baseline_is_announced(): + """轻降开始前没收到 FT 时要明说'接触兜底不生效'（不许静默少一道保险）。""" + _ok, _Tee, st = _run(_StubStage2(ft_fz=None)) + assert any("接触兜底不生效" in m for m in st.logs), ( + "FT 没数据却一声不吭 —— 会让人以为有接触兜底" + ) + + +def test_goto_failure_stops_the_descent(): + """某一步走不到位（被挡/碰到东西）⇒ 停，保持当前高度，并判失败。""" + st = _StubStage2() + # 从第 3 步起 _goto 报失败 + orig = st._goto + calls = {"n": 0} + + def _goto(q, Tee, target, **kw): + calls["n"] += 1 + if calls["n"] >= 3: + return False, q, Tee + return orig(q, Tee, target, **kw) + + st._goto = _goto # type: ignore[assignment] + ok, Tee, st = _run(st) + + assert ok is False, "走不到位必须判失败（安全优先）" + assert any("轻降中途未到位" in m for m in st.logs) + assert Tee[2, 3] > _SURFACE_Z + _TOOL_LEN, "走不到位时也不许低于工件面" + + +# --------------------------------------------------------------------------- # +# 不许回退到"把目标值当结果汇报" +# --------------------------------------------------------------------------- # +def test_wired_final_gap_is_reported_from_geometry_not_target(): + """收尾要汇报**几何实算**的间隙，不能把目标值（2.0mm）念一遍。 + + 旧日志就出现过"停在离面 18mm"却同时打印"悬在孔面上方 ~2.0mm"，现场会以为 + 轻降做到了（R1.1：不许把没做到说成做到）。 + """ + _ok, _Tee, st = _run(_StubStage2()) + assert st.descent_final_gap_m is not None, "没记录最终间隙" + assert abs(st.descent_final_gap_m * 1000 - _FINAL_GAP * 1000) < 1.0 + assert any("轻降完成" in m and "几何" in m for m in st.logs), ( + "收尾日志要标明这是几何量，且给出可核对的分量" + ) + + +def test_source_no_longer_uses_depth_to_stop_the_descent(): + """守卫测试：源码里不许再出现"用深度间隙叫停轻降"的逻辑。 + + 这是**反向**证据 —— 只要有人把 SAFE_FLOOR / _measure_z_wp 叫停逻辑加回来， + 这条立刻红（比"单元测试碰巧还能过"更直接）。用语法树判据（不是文本匹配）， + 这样解释这件事的注释/文档字符串不会把它误判成"逻辑又回来了"。 + """ + import ast + from pathlib import Path + + src = ( + Path(__file__).resolve().parents[1] + / "arm_application" + / "stage2_locate_node.py" + ).read_text(encoding="utf-8") + tree = ast.parse(src) + fn = next( + n + for n in ast.walk(tree) + if isinstance(n, ast.FunctionDef) and n.name == "_descend_above_hole" + ) + names = {n.id for n in ast.walk(fn) if isinstance(n, ast.Name)} + attrs = {n.attr for n in ast.walk(fn) if isinstance(n, ast.Attribute)} + assert "SAFE_FLOOR" not in names, "深度安全下限又回来了 —— 它就是那个误停的元凶" + assert "_measure_z_wp" not in attrs, "又在循环里重测深度当停止条件了" + # 交叉核对必须仍在（不是把测量悄悄删掉了） + assert "_measure_gauge_state" in attrs, "连交叉核对都没了 —— 那就没法发现测距坏了" diff --git a/src/arm_application/test/test_stage2_tip_align.py b/src/arm_application/test/test_stage2_tip_align.py new file mode 100644 index 0000000..00afd71 --- /dev/null +++ b/src/arm_application/test/test_stage2_tip_align.py @@ -0,0 +1,308 @@ +"""精对准的「工具位置」必须取自**视觉量到的规尖**，不能再用法兰轴线。 + +为什么（2026-10-08 真机 arm 89） +-------------------------------- +规是**夹在爪里**的零件，它相对法兰轴线可以整体偏出约 10mm。实测（同一帧、同一 +条手眼链，n=8）： + +* 规杆轴线相对法兰 = **(+10.7, −0.1) mm**（σ≈0.5mm）；最深像素法 (+8.8, −1.8) mm +* 而 ``hover_offset_ee_xy_m`` 写的是 ``[0, 0]``（"平行爪同轴"）⇒ 精对准把**法兰** + 对到孔上 ⇒ 规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔的**孔沿**上， + 腕力从 −1N 蹦到 −31N→−63N 卡死。现场看到的是"轻降在离面 4mm 停住"。 + +而孔检测本身重复性极好（k1 基座坐标 σ=(0.17,0.04)mm）—— 精度不是瓶颈， +"对的是谁"才是。所以改成在同一帧里同时解「规尖」与「孔」，两者共用手眼链。 + +真机复跑还暴露了第二件事：**工具长读数会跳**（192~223mm）——规尖细、反光，掠射角 +下深度容易丢，丢了以后"最深有效像素"往上跑、工具长只会变短。逐拍重算偏移会把 +"规杆略有倾斜"引入的 ±0.2~0.4mm 抖动带进残差，残差在 0.44~0.85mm 之间来回荡、 +收不进 0.25mm。偏移是**刚体量**，所以**量准一份就冻结**，并且只取"端头看得全" +（工具长最长）那一簇。 + +这些用例全是**离线**的：只用到少量成员，用打桩对象即可。 +""" + +from __future__ import annotations + +from typing import Any, Dict, Optional, Tuple + +import numpy as np + +from arm_application.stage2_locate_node import Stage2Locate + +_INTR = {"fx": 400.0, "fy": 400.0, "cx": 400.0, "cy": 240.0} +_TEE = np.eye(4) +_TEE[0, 3] = 0.50 +_TEE[1, 3] = -0.10 +_TEE[2, 3] = 0.55 + +# 相机朝下、相对竖直前倾约 30°（≈真机那个 29.7° 的视角）。 +# 用旋转而不是纯平移——纯平移会让"更深"在基座里变成**更长**的工具长，与真机相反， +# 那样这条链就测不出"端头丢像素 ⇒ 工具长变短"这件事。 +_R_CB = np.array([[1.0, 0.0, 0.0], [0.0, -0.866, -0.5], [0.0, 0.5, -0.866]]) +_T_CB = np.array([0.50, -0.10, 0.45]) + + +def _rod_frame(v1: int = 300, u0: int = 400, v0: int = 100, near: float = 0.06, + far: float = 0.12, half: int = 8, w: int = 800, h: int = 480) -> np.ndarray: + """造一帧含"规杆"的深度图：中央竖带，深度随 v 线性变深（越靠下越深）。 + + 深度只随 ``v`` 的**绝对位置**变（用固定的 200px 量程），所以 ``v1`` 截短 = 规尖 + 那几个像素丢了 —— 正好复现真机那个"最深有效像素往上跑、工具长变短"。 + """ + depth = np.full((h, w), 0.50, np.float32) + for v in range(v0, v1 + 1): + f = min(1.0, (v - v0) / 200.0) + depth[v, u0 - half : u0 + half] = near + f * (far - near) + return depth + + +class _StubLogger: + def __init__(self) -> None: + self.msgs: list = [] + + def _add(self, msg: str) -> None: + self.msgs.append(str(msg)) + + info = _add + warn = _add + error = _add + + +class _Stub: + """被测方法用到的成员。""" + + tip_min_m = 0.04 + tip_max_m = 0.60 + tip_near_m = 0.15 + tip_band_frac = 0.25 + tip_min_px = 150 + tip_len_min = 0.15 + tip_len_max = 0.32 + tip_off_tol = 0.003 + tip_calib_n = 4 + tool_tip_below_flange_m = 0.19 + + def __init__(self, t_cb: Tuple[float, float, float] = tuple(_T_CB)) -> None: + self._t_cb = np.array(t_cb, float) + self._tip_off: Optional[np.ndarray] = None + self._tip_samp: list = [] + self._tip_len_m: Optional[float] = None + self._logger = _StubLogger() + + def get_logger(self) -> Any: + return self._logger + + # 真实实现（纯数学）——换掉就等于没测到那条链 + def _px_to_cam(self, u: float, v: float, z: float, intr: Dict[str, float]) -> np.ndarray: + return Stage2Locate._px_to_cam(self, u, v, z, intr) # type: ignore[arg-type] + + def _cam_to_base(self, p_cam: np.ndarray) -> np.ndarray: + return _R_CB @ np.asarray(p_cam, float) + self._t_cb + + +def _call(st: _Stub, depth: np.ndarray): + return Stage2Locate._tip_offset_from_depth( # type: ignore[arg-type] + st, depth, _INTR, _TEE + ) + + +def _feed(st: _Stub, depth: np.ndarray) -> Tuple[Optional[np.ndarray], Optional[float]]: + """走一遍"量一帧 → 攒样本 → 试冻结"的完整流程。""" + off, tlen, _why = _call(st, depth) + if off is not None and tlen is not None: + st._tip_samp.append((off, tlen)) + Stage2Locate._refresh_tip_offset(st) # type: ignore[arg-type] + return off, tlen + + +# --------------------------------------------------------------------------- # +# 正常路径 +# --------------------------------------------------------------------------- # +def test_measures_the_rod_tip_offset(): + """规杆在中央、深度合理时，要真的量出「规尖相对法兰」的基座 XY 偏移 + 工具长。""" + off, tlen, why = _call(_Stub(), _rod_frame()) + assert off is not None and tlen is not None, f"没量到规尖：{why}" + # 规杆在 u=400=cx 的正中央 ⇒ 基座 X 偏移应≈0 + assert abs(float(off[0])) < 0.001, f"规杆居中，X 偏移应≈0，实际 {off[0]*1000:+.2f}mm" + assert abs(float(off[1])) < 0.15, f"Y 偏移 {off[1]*1000:.1f}mm 离谱" + assert 0.15 <= tlen <= 0.32, f"工具长 {tlen*1000:.0f}mm 不在合法区间" + assert "工具长" in why, "说明里要给出反投影出的工具长（可核对）" + + +def test_offset_is_stable_frame_to_frame(): + """同一姿态连拍：偏移是刚体量，重复帧之间必须几乎不动。""" + st = _Stub() + off1, _t1, _ = _call(st, _rod_frame()) + assert off1 is not None + off2, _t2, why = _call(st, _rod_frame()) + assert off2 is not None, f"重复帧被误判：{why}" + assert float(np.linalg.norm(off2 - off1)) < 1e-6 + + +# --------------------------------------------------------------------------- # +# 判据各自都要能真的失败（负向用例） +# --------------------------------------------------------------------------- # +def test_rejects_when_nothing_is_near(): + """近端什么都没有（规不在视野）⇒ 必须判失败，不能返回一个假偏移。""" + depth = np.full((480, 800), 0.50, np.float32) # 全在远端 + off, tlen, why = _call(_Stub(), depth) + assert off is None and tlen is None, "近端没物体却量出了偏移 —— 那是拿台面当规" + assert "近端" in why + + +def test_rejects_when_rod_is_outside_the_center_band(): + """物体在画面边上（不是夹在爪里的规）⇒ 必须拒绝。""" + off, _tlen, why = _call(_Stub(), _rod_frame(u0=60)) + assert off is None, "画面边上的东西不是规，不许采信" + assert "近端" in why + + +def test_rejects_implausible_tool_length(): + """反投影出的工具长荒谬（量到台面/工件）⇒ 必须拒绝。 + + 把相机系平移的 z 抬到 0.75 ⇒ 反投影的基座 z 高于法兰 ⇒ 工具长为负。 + """ + st = _Stub(t_cb=(0.50, -0.10, 0.75)) + off, _tlen, why = _call(st, _rod_frame()) + assert off is None, "工具长为负也敢采信 —— 会把臂往工件里带" + assert "工具长" in why and "不是规" in why + + +# --------------------------------------------------------------------------- # +# 冻结：偏移是刚体量，量准一次就定住 +# --------------------------------------------------------------------------- # +def test_freezes_offset_after_consistent_samples(): + """攒够一致样本后必须**冻结**，且冻结后不再被后续样本改动。 + + 为什么要冻结：逐拍重算会把"规杆略有倾斜"引入的 ±0.2~0.4mm 抖动带进残差 + （2026-10-08 真机残差 0.44~0.85mm 来回荡、收不进 0.25mm）。 + """ + st = _Stub() + for _ in range(st.tip_calib_n): + _feed(st, _rod_frame()) + assert st._tip_off is not None, "样本一致却没冻结" + frozen = st._tip_off.copy() + assert any("已冻结" in m for m in st._logger.msgs), "冻结了要讲一声（否则现场不知道基准何时定的）" + for _ in range(3): # 再喂几帧：冻结后不许改 + _feed(st, _rod_frame(u0=402)) + assert np.allclose(st._tip_off, frozen), "冻结后又被改了 —— 那冻结就没意义" + + +def test_does_not_freeze_when_samples_disagree_without_a_majority(): + """样本分成**势均力敌的两派**时绝不许冻结 —— 宁可继续用退路 + 报失败。 + + （若有一派能凑够 ``tip_calib_n`` 拍一致，那按多数派冻结是对的：外点就是靠这个 + 剔掉的 —— 见 test_freezes_offset_after_consistent_samples。） + 这是 2026-10-08 那次真机"残差 0.15mm 的假达标"的守卫：只要偏移量不准， + 对准结果就必须判不可信（见 test_success_is_refused_without_tip_offset）。 + """ + st = _Stub() + good = _rod_frame() # 偏移 ≈ x=0 + far = _rod_frame(u0=470) # 挪到别的 u ⇒ x 偏移差 ~21mm + for i in range(st.tip_calib_n): # 各一半，谁都不够多数派 + _feed(st, good if i % 2 == 0 else far) + assert st._tip_off is None, "两派势均力敌（各 2 拍）竟然冻住了 —— 定了个假基准" + + +def test_reports_tool_length_from_the_consistent_samples(): + """冻结时要并记工具长，并在与 site.yaml 差 >3mm 时告警（夹持变动的唯一线索）。""" + st = _Stub() + st.tool_tip_below_flange_m = 0.30 # 故意与量到的 ~0.19m 差很多 + for _ in range(st.tip_calib_n): + _feed(st, _rod_frame()) + assert st._tip_off is not None + assert st._tip_len_m is not None, "冻结了却没记工具长" + warn = [m for m in st._logger.msgs if "site.yaml 的工具长" in m] + assert warn, "量的工具长与配置差一大截却一声不吭 —— 夹持变动的线索就这么丢了" + + +# --------------------------------------------------------------------------- # +# 守卫测试：精对准必须真的用规尖，且保留退路 +# --------------------------------------------------------------------------- # +def test_align_loop_uses_visual_tip_with_flange_fallback(): + """源码守卫（语法树判据）：精对准循环里 + + · 必须调用 ``_tip_offset_from_depth``（视觉规尖）； + · 必须调用 ``_refresh_tip_offset``（量准一次就冻结）； + · 必须仍能用 ``_gauge_xy`` 兜底（规尖量不到时不至于完全没方向）； + · 结果里要标出这一拍用的是哪个（``gauge_source``），否则现场分不清。 + """ + import ast + from pathlib import Path + + src = ( + Path(__file__).resolve().parents[1] + / "arm_application" + / "stage2_locate_node.py" + ).read_text(encoding="utf-8") + tree = ast.parse(src) + attrs = {n.attr for n in ast.walk(tree) if isinstance(n, ast.Attribute)} + assert "_tip_offset_from_depth" in attrs, "精对准没用视觉规尖 —— 又回到拿法兰对孔了" + assert "_refresh_tip_offset" in attrs, "偏移没走『量准一次就冻结』的路子 —— 残差会被抖动拖住" + assert "_gauge_xy" in attrs, "连退路都没了：规尖一时量不到就完全没方向" + consts = {n.value for n in ast.walk(tree) if isinstance(n, ast.Constant)} + assert "gauge_source" in consts, "结果里没标这一拍用的是规尖还是法兰，现场没法核对" + assert "align_trusted" in consts, "结果里没标这次对准可不可信，现场没法核对" + + +def test_success_is_refused_without_tip_offset(): + """**残差达标但规尖偏移没量到**时，必须判失败 —— 不许把没做到说成做到。 + + 2026-10-08 真机复跑就栽在这：``tip_offset_m=None``、残差 0.15mm 看着漂亮， + 其实那一拍用的是**法兰轴线**（退路），法兰压在 k1 上、规尖偏出 ~11mm。 + 没有这条守卫，日志会报"二阶段 OK 残差 0.15mm"，而规根本没对上孔。 + + 判据走**语法树**而不是文本匹配：必须存在一个"残差达标"的分支，其（含嵌套的） + 分支体里同时写了 ``align_trusted`` 与 ``fail_reason``。文本匹配会被方法里 + 别处的 `_tip_off is None` 蒙混过去（试过，真的会漏）。 + """ + import ast + from pathlib import Path + + src = ( + Path(__file__).resolve().parents[1] + / "arm_application" + / "stage2_locate_node.py" + ).read_text(encoding="utf-8") + fn = next( + n + for n in ast.walk(ast.parse(src)) + if isinstance(n, ast.FunctionDef) and n.name == "_run_locate" + ) + + def _consts(node: ast.AST) -> set: + return {x.value for x in ast.walk(node) if isinstance(x, ast.Constant)} + + tol_branches = [ + n for n in ast.walk(fn) if isinstance(n, ast.If) and "align_tol" in ast.unparse(n.test) + ] + assert tol_branches, "没找到『残差达标』那个分支 —— 结构变了，这条守卫要跟着改" + guarded = [ + n for n in tol_branches if {"align_trusted", "fail_reason"} <= _consts(n) + ] + assert guarded, ( + "『残差达标』分支里没有『规尖没量到 ⇒ align_trusted=False + fail_reason』—— " + "假达标（对的是法兰）又会蒙混成成功" + ) + + +def test_default_is_tip_vision_on_in_site(): + """site.yaml 必须默认开视觉规尖，且容差收到 0.25mm。 + + 容差是关键：0.7mm 会让"规偏 10mm"这种系统偏差在容差里被放过去 + （原来残差 0.69mm 看着"通过"，其实是拿法兰对的孔）。 + """ + from pathlib import Path + + import yaml + + site = yaml.safe_load( + (Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(encoding="utf-8") + ) + st2 = ((site.get("hole_detect") or {}).get("stage2") or {}) + assert st2.get("align_tip_vision") is True, "默认必须用视觉规尖" + assert float(st2.get("align_tol_m")) <= 0.00025 + 1e-9, ( + f"align_tol_m={st2.get('align_tol_m')} 太松：会放过系统性的对孔偏差" + ) + assert int(st2.get("align_max_iters")) >= 12, "收紧容差后迭代上限要跟上" diff --git a/src/arm_control/arm_control/named_pose_server.py b/src/arm_control/arm_control/named_pose_server.py index 91afd5b..ce4aae3 100644 --- a/src/arm_control/arm_control/named_pose_server.py +++ b/src/arm_control/arm_control/named_pose_server.py @@ -14,6 +14,7 @@ from rclpy.node import Node from arm_msgs.action import FollowJointTrajectory from arm_msgs.srv import SetNamedPose +from std_srvs.srv import Trigger DEFAULT_JOINTS = [ 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7', @@ -64,53 +65,77 @@ class NamedPoseServer(Node): self.create_service( SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb ) + # ``/home``（std_srvs/Trigger）：契约里的"回初始位"入口。 + # + # **为什么这里也要有一个**：真机上 ``/home`` 原本由 ``arm_driver_flexiv`` + # 提供（它只是个包装，内部还是调 ``/set_named_pose``），但 real 模式的 + # bringup 起的是 ``flexiv_bringup + named_pose_server + arm_status_node``， + # **没有任何 launch 文件会起 arm_driver_flexiv** —— 那个驱动还会自己开 + # RDK 连接，与 ros2_control 的 flexiv_hardware 抢独占（RDK 是单客户端）。 + # 于是真机上 ``/home`` 根本不存在，通止规装配的 S00 必检因此永远过不了 + # （``gauge_boot.py`` 的 ``gauge_home`` 项 target=/home， + # ``gauge_assembly_node`` 的 ``_call_trigger("/home")`` 同）。 + # 放在这里最合适：本节点已经握着 ``/set_named_pose`` 的实现与 JTC 客户端， + # 不需要再起一个进程、也不碰 RDK。 + self.declare_parameter('home_pose', 'home') + self.declare_parameter('home_speed', 0.30) + self.create_service( + Trigger, 'home', self._on_home, callback_group=self._cb + ) self.get_logger().info( f'命名位姿 {list(self.poses)} joints={self.joint_names}' ) - def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response): - name = request.pose_name + def _go_to_pose(self, name: str, velocity_scale: float): + """把 ``name`` 这个命名位姿走完。返回 ``(成功?, 说明)``。 + + 两个服务（``/set_named_pose`` 与 ``/home``）共用这一段：分两份写的话， + 以后只改一处就会出现"手动回位能动、装配回位不能动"这类难查的偏差。 + """ if name not in self.poses: - response.success = False - response.message = f'未知位姿"{name}"。可用: {list(self.poses)}' - return response + return False, f'未知位姿"{name}"。可用: {list(self.poses)}' if not self._client.wait_for_server(timeout_sec=3.0): - response.success = False - response.message = 'follow_joint_trajectory action 不可用' - return response + return False, 'follow_joint_trajectory action 不可用' goal = FollowJointTrajectory.Goal() goal.joint_names = self.joint_names goal.num_points = 1 goal.positions = list(self.poses[name]) - duration = max(0.5, 1.0 / max(float(request.velocity_scale), 0.1)) + duration = max(0.5, 1.0 / max(float(velocity_scale), 0.1)) goal.time_from_start = [duration] - goal.velocity_scale = float(request.velocity_scale) + goal.velocity_scale = float(velocity_scale) self.get_logger().info(f'-> 位姿 [{name}] T={duration:.2f}s') send_fut = self._client.send_goal_async(goal) if not _wait_future(send_fut, 5.0): - response.success = False - response.message = '等待目标被接受超时' - return response + return False, '等待目标被接受超时' handle = send_fut.result() if handle is None or not handle.accepted: - response.success = False - response.message = '目标被拒绝' - return response + return False, '目标被拒绝' res_fut = handle.get_result_async() if not _wait_future(res_fut, 60.0): - response.success = False - response.message = '等待轨迹结果超时' - return response + return False, '等待轨迹结果超时' wrapped = res_fut.result() if wrapped is None: - response.success = False - response.message = '结果为空' - return response + return False, '结果为空' result = wrapped.result - response.success = bool(result.success) - response.message = result.message or f'已移动到位姿 {name}' + return bool(result.success), (result.message or f'已移动到位姿 {name}') + + def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response): + response.success, response.message = self._go_to_pose( + request.pose_name, float(request.velocity_scale) + ) + return response + + def _on_home(self, _request: Trigger.Request, response: Trigger.Response): + """``/home``：回到 ``home_pose`` 指定的命名位姿（默认 ``home``）。""" + pose = str(self.get_parameter('home_pose').value) + speed = float(self.get_parameter('home_speed').value) + ok, message = self._go_to_pose(pose, speed) + if not ok: + self.get_logger().error(f'回初始位失败（pose={pose}）: {message}') + response.success = ok + response.message = message return response diff --git a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp index 7433801..8eb5697 100644 --- a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -10,11 +10,19 @@ #include #include #include +#include #include #include #include #include +// RDK 网卡白名单的本地核对（见下面 resolve_network_interface_whitelist 的说明） +#include +#include +#include +#include +#include + #include #include #include @@ -159,6 +167,118 @@ void wait_for_operational(flexiv::rdk::Robot& robot, rclcpp::Logger logger) } } +// ===== RDK 网卡白名单（2026-09-29 真机踩到）===== +// +// 现象：`rdk::Robot(sn)`（不传白名单 = 扫本机全部网卡）在**换网卡之后**连不上 +// 机器人，报 "Failed to connect to [Rizon4-063989]"；而 +// `rdk::Robot(sn, {"192.168.2.10"})` 立刻连上。同一时刻 +// `scripts/rdk_switch_tool.py`（Python RDK，钉了网卡）报 "RDK 正常" —— 所以 +// 不是机器人不在网，是本机多网卡时 RDK 的「全部网卡」发现选错了源地址： +// 2026-09-29 改线（两臂 → 交换机 → USB-C 网卡）之后，enp4s0 上还挂着旧 Flexiv +// 网段的 192.168.100.10 / 192.168.101.10，wlo1 也有地址。 +// +// 取值优先级（三个都没有就退回 RDK 默认，行为与打这个补丁之前**完全一致**）： +// 1. hardware 参数 `network_interface`（逗号/分号/空白分隔，网卡名或 IP 都行） +// 2. 环境变量 `FLEXIV_RDK_NETWORK_INTERFACE` +// 3. 环境变量 `SITE_PC_IP` —— scripts/lib/export_site_env.py 从 site.yaml 的 +// robots[].pc_ip 导出，语义正是「本机在这一网段的地址」，就是 RDK 要的值 +// +// ⚠️ 候选值**必须**先核对「本机确实持有」，否则 RDK 会把整张白名单过滤掉、 +// 报 "All whitelist interfaces were filtered out"，现象和「不传白名单」 +// 一样难查（现场笔记见 scripts/test_rdk_python.sh）。 +std::vector split_whitelist(const std::string& raw) +{ + std::vector out; + std::string cur; + for (const char c : raw) { + if (c == ',' || c == ';' || c == ' ' || c == '\t' || c == '\n' || c == '\r') { + if (!cur.empty()) { + out.push_back(cur); + cur.clear(); + } + } else { + cur.push_back(c); + } + } + if (!cur.empty()) { + out.push_back(cur); + } + return out; +} + +/// 本机当前持有的全部 IPv4 地址（不含回环）。 +std::vector local_ipv4_addresses() +{ + std::vector out; + struct ifaddrs* ifs = nullptr; + if (getifaddrs(&ifs) != 0) { + return out; + } + for (struct ifaddrs* it = ifs; it != nullptr; it = it->ifa_next) { + if (it->ifa_addr == nullptr || it->ifa_addr->sa_family != AF_INET) { + continue; + } + if ((it->ifa_flags & IFF_LOOPBACK) != 0) { + continue; + } + char buf[INET_ADDRSTRLEN] = {0}; + auto* sin = reinterpret_cast(it->ifa_addr); + if (inet_ntop(AF_INET, &sin->sin_addr, buf, sizeof(buf)) != nullptr) { + out.emplace_back(buf); + } + } + freeifaddrs(ifs); + return out; +} + +/// 网卡名是否存在（RDK 的白名单也接受网卡名）。 +bool is_local_interface(const std::string& name) +{ + return !name.empty() && access(("/sys/class/net/" + name).c_str(), F_OK) == 0; +} + +std::vector resolve_network_interface_whitelist( + const hardware_interface::HardwareInfo& info) +{ + const auto from_env = [](const char* key) { + const char* v = std::getenv(key); + return v == nullptr ? std::string{} : std::string{v}; + }; + + std::vector candidates; + const auto it = info.hardware_parameters.find("network_interface"); + if (it != info.hardware_parameters.end()) { + candidates = split_whitelist(it->second); + } + if (candidates.empty()) { + candidates = split_whitelist(from_env("FLEXIV_RDK_NETWORK_INTERFACE")); + } + if (candidates.empty()) { + candidates = split_whitelist(from_env("SITE_PC_IP")); + } + if (candidates.empty()) { + return {}; + } + + const auto local_ips = local_ipv4_addresses(); + std::vector resolved; + for (const auto& c : candidates) { + const bool held = is_local_interface(c) + || std::find(local_ips.begin(), local_ips.end(), c) != local_ips.end(); + if (held) { + resolved.push_back(c); + } else { + // 这里不用 private 的成员 getLogger()（匿名命名空间里访问不到）； + // 它本身就是 `rclcpp::get_logger("FlexivHardwareInterface")`。 + RCLCPP_WARN(rclcpp::get_logger("FlexivHardwareInterface"), + "RDK 网卡白名单项 '%s' 既不是本机网卡名也不是本机地址 —— 已忽略" + "（传不存在的项会让 RDK 过滤掉整张白名单，反而连不上机器人）", + c.c_str()); + } + } + return resolved; +} + } namespace flexiv_hardware { @@ -331,7 +451,21 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_init( try { RCLCPP_INFO(getLogger(), "Connecting to robot %s ...", robot_sn.c_str()); - robot_ = std::make_unique(robot_sn); + // 钉住网卡再连：不钉的话本机多网卡时 RDK 会发现不到机器人 + // （取不到有效值时退回 RDK 默认，行为与补丁前一致）。 + const auto network_whitelist = resolve_network_interface_whitelist(info_); + if (network_whitelist.empty()) { + RCLCPP_INFO(getLogger(), + "RDK 网卡白名单为空 —— 用 RDK 默认（扫本机全部网卡）"); + robot_ = std::make_unique(robot_sn); + } else { + std::string joined; + for (const auto& item : network_whitelist) { + joined += (joined.empty() ? "" : ", ") + item; + } + RCLCPP_INFO(getLogger(), "RDK 网卡白名单: [%s]", joined.c_str()); + robot_ = std::make_unique(robot_sn, network_whitelist); + } } catch (const std::exception& e) { RCLCPP_FATAL(getLogger(), "Could not connect to robot"); RCLCPP_FATAL(getLogger(), e.what());