From 0a2a28a355da63572f97a8ff5ccdf1260d41b7e6 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Mon, 28 Sep 2026 10:17:16 +0800 Subject: [PATCH] =?UTF-8?q?=E7=9C=9F=E6=9C=BA=E8=90=BD=E5=9C=B0=E5=AE=9E?= =?UTF-8?q?=E6=B5=8B=E4=BF=AE=E7=9A=84=E4=B8=89=E5=A4=84=EF=BC=9A=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E7=B1=BB=E5=9E=8B=E4=B8=8D=E4=B8=80=E8=87=B4=E5=AF=BC?= =?UTF-8?q?=E8=87=B4=E8=8A=82=E7=82=B9=E5=90=AF=E5=8A=A8=E5=8D=B3=E5=B4=A9?= =?UTF-8?q?=E3=80=81=E6=9D=83=E9=87=8D=E7=9B=AE=E5=BD=95=E5=88=A4=E6=8D=AE?= =?UTF-8?q?=E3=80=81=E7=9B=B8=E6=9C=BA=E6=97=A5=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1) site.yaml 的整数把节点写死（ros_node.TolerantNode） 现场 site.yaml 写 `ae_limit_us: 8000`（YAML 整数），脚本按 `-p ae_limit_us:=8000` 传参，而节点 declare 的是 double —— rclpy 严格比对类型，节点**启动即崩**： InvalidParameterTypeException: Trying to set parameter 'ae_limit_us' to '8000' of type 'INTEGER', expecting type 'DOUBLE' 现场看到的只是「顶置 D435 起不来」，报错里一个字都没提 site.yaml，而 `8000` 与 `8000.0` 在运维眼里是同一个数。同一族的雷还有 width 848.0→int、 emitter_enabled 1→bool、带不带引号的序列号。 修法是新增应用层统一基类 TolerantNode：在**声明阶段**把覆盖值的类型按 声明类型收敛（异常就发生在声明阶段，所以 `float(...)` 这种事后 cast 救不了）。 全部应用节点改继承它；补 test_ros_node.py。 2) 权重目录判据（weights_layout） 模型文件「在不在、名字对不对」以前散在几处各判一半，缺文件时的现象是 推理静默退回随机权重。收成 weights_layout.py 一条判据 + scripts/weights.py 调用。 3) 相机日志：USB 降速要说清怎么办 realsense_color_pub 协商到 USB 2.1 / 480Mbps 时只打一行速率，没人知道该做什么； 现在直接给可执行的处置（换线/换口/SuperSpeed 不复用）。realsense_view 同步。 4) stage2_locate：接上 hover_z_guard 与 tool_tip_below_flange_m 悬停高度必须让工具尖端留在工件面上方（09-24 的教训：悬停时尖端已钻进工件 25mm，S05 横向对准把工件拖走 92mm）。step2 侧也按同一判据兜住，且工具长 允许由启动参数覆盖（仿真上料后实测得到）。 5) config/site.example.yaml 与 site.yaml 重新同步 上一批给夹爪加了 auto_init: false，模板没跟着重跑，TestExampleParity 因此失败。 重跑 scripts/gen_site_example.py 同步（模板是生成物，不手改）。 测试：全仓库 1085 passed（此前 1 failed）。 --- config/site.example.yaml | 157 +++++++++-- scripts/weights.py | 8 +- .../arm_application/demo_named_poses.py | 4 +- .../arm_application/dh_pgea_node.py | 18 +- .../arm_application/dual_gripper_node.py | 4 +- .../arm_application/goto_ee_plane.py | 4 +- .../arm_application/handeye_calibrate.py | 4 +- .../arm_application/handeye_teach.py | 14 +- .../arm_application/handeye_waypoints.py | 4 +- .../arm_application/hole_detect_node.py | 4 +- .../arm_application/move_to_pose_client.py | 4 +- .../arm_application/perception_node.py | 4 +- .../arm_application/realsense_color_pub.py | 40 +-- .../arm_application/realsense_view.py | 3 +- .../arm_application/ros_node.py | 118 +++++++++ .../arm_application/run_joint_trajectory.py | 4 +- .../arm_application/servo_node.py | 4 +- .../arm_application/stage2_locate_node.py | 250 ++++++++++++++++-- .../arm_application/tactile_node.py | 4 +- .../arm_application/weights_layout.py | 45 ++++ src/arm_application/test/test_ros_node.py | 167 ++++++++++++ .../test/test_weights_layout.py | 41 +++ 22 files changed, 811 insertions(+), 94 deletions(-) create mode 100644 src/arm_application/arm_application/ros_node.py create mode 100644 src/arm_application/test/test_ros_node.py diff --git a/config/site.example.yaml b/config/site.example.yaml index fe3320e..ff09c1a 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -49,6 +49,15 @@ robot: sn: "<序列号>" port: "/dev/ttyUSB0" stroke_mm: 20.0 + # 大寰上电后必须初始化一次（写 0xA5），而初始化的动作是**手指先回零** + # = 张开到满行程。如果这时爪子已经夹着通止规/工件，规会被放开、掉下去。 + # 2026-09-28 现场实测过：auto_init 走一遍，开口从 19.18mm 变成 20.00mm。 + # 所以这里的默认是 **false**：由人决定什么时候初始化。 + # · 上电后、还没装夹 → 可以临时开一次：SITE_GRIP_AUTO_INIT=true bash scripts/run_gripper.sh --arm 89 + # · 已经装夹（装配过程中）→ 保持 false；不初始化也能读状态、也能开合 + # （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持） + # 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。 + auto_init: false named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml" paths: handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml" @@ -69,6 +78,12 @@ robot: # 法兰 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， + # 对"轻碰"更灵敏，但对负载变化更敏感）。 # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<< # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的 #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节 @@ -114,22 +129,61 @@ robot: # 一阶段跟踪高 = 悬停 + 0.05（通止规离工件）= 0.682 # 所以工作盒上界必须 ≥ 0.682 —— 真机那套上界 0.65 不够，S04 一跟踪就会被 # "超出工作盒"截断（现象是"越跟踪越偏"，很难想到是配置差 3cm）。 + # + # ⚠️ 上面那套"按相机"的推导**只对了一半**（2026-09-24 修正）：它没算**工具**。 + # 悬停高度还要满足"工具尖端不碰工件"： + # 工件上表面 0.412 + 工具长 0.243（推算的标称值：TCP 0.200 + 规尖 0.043） + # + 间隙 0.002 = **0.657** + # 原来的 0.632 会让工具尖端落在工件面**下**（按 0.245 算是 −23mm）—— 悬停 + # 这一步就把通止规捅进工件里，接着 S05 的横向对准等于拿规尖拨工件（实测把 + # 工件推走 79mm），而真值孔位按工件**实测**位姿算 ⇒ 孔位跟着工具走、残差 + # 恒定 11mm 永不收敛（日志看起来像"手眼标定坏了"）。真机同参数同结果：只要 + # 悬停按相机定、而工具比相机长，就会这样。 + # 现在三道防线：stage2 启动时按 gauge_fsm.hover_z_guard 用几何兜住；S05 有 + # "工件被推走"的看门狗（gauge_fsm.drag_verdict）；S07 按几何自查下探预算。 + # + # ⚠️ 为什么这里能写 0.657 这种"贴着"的值：悬停高每抬高 1mm，S07 就多下 + # 1mm，而 contact_max_depth_mm=35 是按"悬停只留 2mm 间隙"配的 + # （30 台肩 + 5 余量）。抬到 0.67 会让下探变成 43mm，S07 就得**自动改大** + # 预算并告警 —— 能跑，但配置就失去意义了。 + # + # 工具长 0.243 只是**标称**：仿真里上料脚本每次实测并落盘 + # （devices/sim/gazebo/config/sim_tool_geometry.yaml）， + # run_gauge_assembly.sh --sim 用它当启动参数覆盖这里的值。真机由现场把 + # 夹好规后读到的 TF 值写进全局段的 tool_tip_below_flange_m。 servo: <<: *servo_left - ee_z_m: 0.68 # 一阶段跟踪锁高（法兰，世界系） - hole_hover_z_m: 0.632 # 二阶段悬停高：= 0.412 + 0.17 + 0.05 - safe_z_m: 0.75 # 安全抬升/复位高度（高于跟踪高） - z_ref_m: 0.632 # 没有测量时的兜底 Z：绝不能是 0.22（会撞工件） + ee_z_m: 0.72 # 一阶段跟踪锁高（法兰，世界系） + # 不必随悬停一起抬：跟踪时工具尖端离面 ~60mm， + # 只要**高于悬停**就不碰工件（0.72 > 0.657）。 + hole_hover_z_m: 0.657 # 二阶段悬停高：**由工具几何定**，不是相机视距 + # = 0.412（工件上表面）+ 0.243（工具）+ 0.002（间隙） + safe_z_m: 0.76 # 安全抬升/复位高度（高于跟踪高） + z_ref_m: 0.657 # 没有测量时的兜底 Z：绝不能是 0.22（会撞工件） z_min_m: 0.55 # 伺服硬下限：0.55 = 工件上表面 + 0.14 min_ee_z_m: 0.55 workspace_xyz_min: [0.08, -0.55, 0.55] - workspace_xyz_max: [0.85, 0.55, 0.75] + workspace_xyz_max: [0.85, 0.55, 0.76] # 仿真不跑推理：感知结果由 sim_truth_perception 按已知位姿直接给出。 # 原因：ros_gz_sim 环境里没有 torch/ultralytics，且本机离线装不上 —— # perception_node 一启动就死，表现为"一阶段永远不锁定"。 # 详见 devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」。 perception: source: "truth" + # 几何类参数按台子改写（gauge_assembly 段的下沉覆盖）。 + # 为什么需要：全局 gauge_assembly 里的值是**真机**的 —— 真机上机械臂 + # 基座与台面同高，所以"工件上表面 z"就是板厚 0.012；仿真里机械臂站在 + # **地面**、台面 0.40m 高、工件上表面 0.412。写死全局值会让 S07 的 + # "悬停时工具尖端离面多高"自检算错（差 0.4m），现象是"一路下压判不到 + # 接触"，看起来像力阈值标歪了 —— 于是排查从阈值开始走偏。 + gauge_assembly: + workpiece_surface_z_m: 0.412 + # 工具长这里写**推算的标称值**（TCP 在法兰下方 0.200 + 规尖在指腹面下方 + # 0.043 = 0.243）。真值由上料脚本实测并作为启动参数覆盖：仿真里合爪后规会 + # 在指间下滑到落座位置，下滑量随装夹变化（实测见过 243 / 254mm）。 + # 写成固定值会让悬停高度算错（写小 → 规尖插进工件 ⇒ S05 拿规尖拨工件、 + # 残差恒不收敛），所以**这个值在仿真里只是兜底**，实际用实测值。 + tool_tip_below_flange_m: 0.243 - sn: "Rizon4-XXXXXX" arm: right ip: "192.168.2.XXX" @@ -658,32 +712,93 @@ gauge_assembly: # 于是 S07 一路压到顶都等不到"台肩落面"那一下，报"没触发接触判据"， # 看起来像力阈值标错了 —— 实际是几何错。真机同参数同结果。 # S07 启动时会把这个差值算出来印在日志里（负值会明确标注"已经插进工件了"）。 - tool_tip_below_flange_m: 0.243 # 工具尖端（规尖）在法兰下方的距离 [m] - # 怎么量/怎么算（这个 0.243 的来历）： - # TCP（两指中点）在法兰下方 0.200 - # + 规尖在 TCP 下方 0.043 - # 为什么是 0.043 而不是规"夹持段中心到尖端"的 0.0555： - # 规尖要对齐的是**指腹接触面**，而指腹接触面不在 - # TCP 原点上 —— URDF 里 inner_finger_pad_joint - # z=0.0235、finger_tcp_joint z=0.0360（同父链节 - # 同方向）→ 指腹中心比 TCP 高 12.5mm。 - # 所以 0.0555 − 0.0125 = 0.043。 - # ⚠️ 换夹爪/换规必须重量（量法：夹住规之后用 - # TF 直接读 flange→规尖 的 z 差，别靠推）。 + tool_tip_below_flange_m: 0.245 # 工具尖端（规尖）在法兰下方的距离 [m] + # **这是实测值，不是推算值**：夹住规之后直接读 + # TF 的 flange z − 规尖 z。 + # 仿真里 `scripts/sim_load_gauge.py` 每次上料后 + # 会自己把它算出来打印（"实测工具几何"那一段）， + # 现场照着填即可 —— 真机同法（夹住规后直接读 TF）。 + # + # 推算值（TCP 在法兰下方 0.200 + 规尖在指腹面下方 + # 0.043 = 0.243）与实测 0.245 只差 2mm，说明推算 + # 思路是对的；但**别再用推算**，因为下面这个理由： + # + # ⚠️ 这个数天生有 ±几毫米的不确定度：规是**靠摩擦** + # 夹住的，每次上料在爪里的位置都会差几毫米。 + # 所以它只用来**诊断**（悬停时尖端离面多高、下探 + # 预算够不够），**不能**当硬约束 —— S07 的自检 + # 因此是"告警 + 自动放大预算"，不是"直接失败"。 + # 要更准只能改夹持方式（定位台肩/加挡块），那是 + # 机械方案，不是改这个数。 workpiece_surface_z_m: 0.0 # 工件**上表面**的世界 z [m]（对孔基准面） # 0 = 未知 → 运行期用 stage2 实测的孔位 z 兜底 - #（孔就在上表面上）。仿真里是 0.412。 + #（孔就在上表面上）。仿真里实测是 0.412。 # 只在"stage2 还没跑就想知道几何对不对"时才需要手填。 - contact_tangential_delta: 300.0 # 接触判据：两路切向任一相对基线增量超过它 [任意单位] + contact_tangential_delta: 300.0 # 触觉切向力突变阈值 [au，任意单位] —— **兜底判据** + # ⚠️ 这是"触觉"的单位，不是牛顿：真机 Orisys 输出的 + # 是形变任意单位（厂商不给牛顿换算），仿真侧由 + # force_scale 定义尺度。所以这个数**不能**与下面 + # contact_axial_delta_n 比大小（两者量纲不同）。 + # + # 它为什么只能是兜底：通止规靠**夹持摩擦**抱住，轴向力 + # 传到指垫必须过摩擦面，上限 = µ·N ≈ 0.25×46N ≈ 23N + # （两指合计）。而台肩落面一步（0.5mm × 5e4 N/m）就 + # 产生 25N —— 必然打滑，所以触觉**物理上读不到**那一下 + # 的力，读数停在 0（而它是对的）。拿它当主判据，阈值调 + # 多大都不对：调小被噪声骗，调大永远达不到。 + # 2026-09-24 就是照"阈值调高了"改的，白费一轮。 + # 详见 gauge_fsm.wrench_touch 与 docs/GAUGE_ASSEMBLY.md §6。 + contact_axial_delta_n: 8.0 # 腕部 FT 轴向力突变阈值 [N] —— **首选判据** + # 腕部 FT 在夹持摩擦的**上游**（工件→规→夹爪→法兰 + # →腕部关节），工具在爪里打不打滑都测得准，单位是真牛顿。 + # 为什么只要 8N：这是"碰到了没有"、不是"压到位了没有"。 + # 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有 + # 5N、0.2mm 有 10N —— 8N 对应约 0.16mm 过压，**在打滑 + # 之前**就判到了（打滑上限 ≈23N ≈ 0.46mm），留 3 倍余量。 + # 现场标定：空载慢慢压到台肩落面，看日志里每 5 步打印的 + # 「腕部轴向」涨到多少，取它的 1/3（要能区分"擦到孔壁" + # 与"台肩落面"）。 + # 数据源话题：servo.ft_wrench_topic（默认 + # //ft_sensor_wrench，真机与仿真同名同类型同单位）。 + contact_relief_mm: 0.5 # 判到接触后的**卸载回退** [mm] —— 别调成 0 + # 为什么必须有这一步（2026-09-24 一整轮排查换来的）： + # 位置控制的臂撞上硬面时不会"刚碰到就停"，它会顶到 + # 自己的**出力上限**才停住。仿真实测：0.5mm 的一步压在 + # 工件上，腕部 FT 直接读到 **+244N**，而臂这一步只走了 + # 0.09mm。也就是说"判到接触"的那一刻，臂上顶着的力 + # 与 contact_axial_delta_n **没有关系**（阈值只决定 + # 什么时候承认接触）。 + # 而规是靠**夹持摩擦**抱住的：轴向传力上限 µ·N，两指 + # 合计 ≈73N（µ=0.8、握力 46N）。244N 远超它 ⇒ 规被顶出 + # 爪外。本次现象：S07 判到接触 → S08 一转 → 规掉地上 + # → 报 E1，而 E1 的日志只说"通止规掉了"，看不到真因。 + # 0.5mm 怎么来的：判到接触时真实过压量通常只有 0.0x mm + # （5e4 N/m 时 10N ≈ 0.2mm），回退 0.5mm 足够把力放 + # 干净，而规尖仍在孔里（通端穿板 18mm），不会脱出。 + # 放不干净会在日志里明确告警（残余 > contact_hold_n）。 + contact_hold_n: 5.0 # 卸载后允许的残余轴向力 [N] + # 超了不失败，但会**大声报**：它说明"夹持摩擦力不够 + # 承受这次接触"，接着 S08 一转就会掉件。 + # 真修法只有两条：① 降峰值（调小 contact_step_mm 或 + # 加大 contact_relief_mm）；② 加握力（改夹爪合拢力度/ + # 指垫摩擦）。**不要**去调 contact_axial_delta_n。 contact_safe_delta: 2000.0 # 法向安全上限 [任意单位]：超了立刻抬起并中止(E2) # 必须 > contact_tangential_delta，否则自检就报矛盾 # ---- S08 棘轮拧入（慢转 → 松爪 → 快回转 → 夹紧，循环）---- # 停止判据：切向力 > screw_shear_delta **且** 法向力突增 > screw_normal_delta。 - screw_step_deg: 180.0 # 每次慢转多少度；要 ≤ 预绕角（否则第一轮就贴限位） + screw_step_deg: 120.0 # 每次慢转多少度。 + # **硬约束**：2×step + 2×余量(8°) ≤ 关节7 行程。 + # Rizon4/10 的关节7 是 ±175° → 行程 350° → step ≤ 167°。 + # 填 180° 会**必然**贴限位（慢转+回转=360°>350°）， + # 测试 test_ratchet_step_of_180_never_fits_the_rizon_joint7_range + # 与 S08 开头的行程核算都会拦；120° 留足余量吸收 + # "回转每轮比指令少走 4~5°"的亏欠。 screw_speed_deg_s: 30.0 # 慢转速度 [deg/s] screw_back_speed_deg_s: 120.0 # 松爪后的空转回转速度 [deg/s]（爪里没东西，可以快） - screw_max_cycles: 12 # 最多几轮（12×180°=2160°，够 M6 螺纹规走完） + screw_max_cycles: 12 # 最多几轮（12×120°=1440° = **4 整圈**，够 M6 螺纹规走完）。 + # 必须满足 screw_max_cycles × screw_step_deg 是 360° 的整数倍： + # 否则规停在螺纹中间，S09 拧出也少转几分之一圈。 screw_normal_delta: 400.0 # 拧到位时的法向力突增 [任意单位]（螺纹到底/顶住） screw_shear_delta: 600.0 # 拧到位的切向力增量 [任意单位] screw_grip_force_pct: 50.0 # 夹紧力百分比 [%]（**只有契约后端生效**：仿真/新驱动） diff --git a/scripts/weights.py b/scripts/weights.py index bb615b1..08362a6 100755 --- a/scripts/weights.py +++ b/scripts/weights.py @@ -76,6 +76,7 @@ from arm_application.weights_layout import ( # noqa: E402 discover, find_model, groups, + infer_group, model_labels, plan_conversion, staging_dir, @@ -479,11 +480,14 @@ def cmd_convert(args) -> int: _err(bad) return 2 - group = args.group + group = args.group or infer_group(root, src) chain, _outs = plan_conversion(root, src, name=name, group=group, target=args.to) print(f"输入: {src}") print(f"输出: {root / group}/{name}.") + if args.group is None: + print(f" （没给 --group：沿用输入文件所在分组 {group}/；" + f"engine 必须和 site.yaml 指的 .onnx **同级**，分家等于没转）") print(f"计划: {chain.describe()}") print(f"尺寸: imgsz={_imgsz(args)}") if args.to != "all": @@ -806,7 +810,7 @@ def build_parser() -> argparse.ArgumentParser: s = sub.add_parser("convert", help="转换权重并归位") s.add_argument("input", help="输入文件（可以是 staging/ 里的相对名）") s.add_argument("--name", help="输出模型名（默认沿用输入文件名主干）") - s.add_argument("--group", default="yolo", help="输出分组子目录（默认 yolo）") + s.add_argument("--group", help="输出分组子目录（默认沿用输入文件所在分组；staging/外来文件默认 yolo）") s.add_argument("--to", default="all", choices=["all", "onnx", "engine"], help="产出目标") s.add_argument("--imgsz", type=int, help="输入尺寸（默认取 site.yaml 的 yolo.imgsz）") s.add_argument("--labels", help='顺带写标签，如 "工件,通止规"') diff --git a/src/arm_application/arm_application/demo_named_poses.py b/src/arm_application/arm_application/demo_named_poses.py index c679623..40cff4f 100644 --- a/src/arm_application/arm_application/demo_named_poses.py +++ b/src/arm_application/arm_application/demo_named_poses.py @@ -10,8 +10,10 @@ from rclpy.node import Node from arm_msgs.srv import SetNamedPose +from arm_application.ros_node import TolerantNode -class DemoNamedPoses(Node): + +class DemoNamedPoses(TolerantNode): def __init__(self) -> None: super().__init__('demo_named_poses') self.declare_parameter( diff --git a/src/arm_application/arm_application/dh_pgea_node.py b/src/arm_application/arm_application/dh_pgea_node.py index 76fc055..5c43870 100644 --- a/src/arm_application/arm_application/dh_pgea_node.py +++ b/src/arm_application/arm_application/dh_pgea_node.py @@ -33,15 +33,7 @@ from arm_application.dh_pgea import ( GripperState, WIRING_HINT, ) - - -def _param_as_str(node: Node, name: str, default: str = "") -> str: - """字符串参数，同时允许命令行用数字覆盖（例如 SN 26610799）。""" - node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True)) - val = node.get_parameter(name).value - if val is None or val == "": - return "" - return str(val).strip() +from arm_application.ros_node import TolerantNode, param_as_str def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]: @@ -51,7 +43,7 @@ def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]: return float(data[0]), float(data[1]), float(data[2]), float(data[3]) -class DhPgeaNode(Node): +class DhPgeaNode(TolerantNode): def __init__(self) -> None: super().__init__("dh_pgea") self.declare_parameter("port", "/dev/ttyUSB0") @@ -64,9 +56,9 @@ class DhPgeaNode(Node): self.declare_parameter("state_hz", 20.0) self.declare_parameter("open_position", 1000) self.declare_parameter("close_position", 0) - self._model = _param_as_str(self, "model", "PGEA-100-20-W") - self._sn = _param_as_str(self, "serial_number", "") - self._rts = _param_as_str(self, "rs485_rts", "") + self._model = param_as_str(self, "model", "PGEA-100-20-W") + self._sn = param_as_str(self, "serial_number", "") + self._rts = param_as_str(self, "rs485_rts", "") self._port = str(self.get_parameter("port").value) self._baud = int(self.get_parameter("baud").value) diff --git a/src/arm_application/arm_application/dual_gripper_node.py b/src/arm_application/arm_application/dual_gripper_node.py index 729d34c..d877002 100644 --- a/src/arm_application/arm_application/dual_gripper_node.py +++ b/src/arm_application/arm_application/dual_gripper_node.py @@ -22,6 +22,8 @@ from arm_msgs.msg import GripperCommand as GripperCommandMsg from arm_msgs.msg import GripperStatus as GripperStatusMsg from arm_msgs.srv import GripperExecute +from arm_application.ros_node import TolerantNode + def _site_path() -> Path: env = os.environ.get("SITE_CONFIG", "").strip() @@ -38,7 +40,7 @@ def _as_bool(val, default: bool = True) -> bool: return bool(val) -class DualGripperNode(Node): +class DualGripperNode(TolerantNode): def __init__(self) -> None: super().__init__("dual_gripper_driver") self.declare_parameter("config_file", "") diff --git a/src/arm_application/arm_application/goto_ee_plane.py b/src/arm_application/arm_application/goto_ee_plane.py index 8dd0e3d..c2e1b8c 100644 --- a/src/arm_application/arm_application/goto_ee_plane.py +++ b/src/arm_application/arm_application/goto_ee_plane.py @@ -39,6 +39,8 @@ from arm_application.site_io import ( joint_prefix_default, ) +from arm_application.ros_node import TolerantNode + def _load_ee_cfg() -> dict: @@ -78,7 +80,7 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint: return pt -class GotoEePlane(Node): +class GotoEePlane(TolerantNode): def __init__(self, cfg: dict) -> None: super().__init__("goto_ee_plane") self.cfg = cfg diff --git a/src/arm_application/arm_application/handeye_calibrate.py b/src/arm_application/arm_application/handeye_calibrate.py index 356dcea..eef7068 100644 --- a/src/arm_application/arm_application/handeye_calibrate.py +++ b/src/arm_application/arm_application/handeye_calibrate.py @@ -50,6 +50,8 @@ from arm_application.site_io import joint_prefix_default from arm_msgs.action import FollowJointTrajectory from arm_msgs.srv import GetArmState +from arm_application.ros_node import TolerantNode + DEFAULT_JOINTS = [ 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7', ] @@ -79,7 +81,7 @@ def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics: return CameraIntrinsics(K=K, dist=dist) -class HandeyeCalibrate(Node): +class HandeyeCalibrate(TolerantNode): def __init__(self) -> None: super().__init__('handeye_calibrate') diff --git a/src/arm_application/arm_application/handeye_teach.py b/src/arm_application/arm_application/handeye_teach.py index a2ee476..7528e44 100644 --- a/src/arm_application/arm_application/handeye_teach.py +++ b/src/arm_application/arm_application/handeye_teach.py @@ -37,6 +37,8 @@ import rclpy from rcl_interfaces.msg import ParameterDescriptor from rclpy.node import Node +from arm_application.ros_node import TolerantNode, param_as_str + HELP = """ ================================================ 手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿 @@ -94,14 +96,6 @@ def _setup_matplotlib_font(): return chosen -def _param_as_str(node: Node, name: str, default: str = '') -> str: - node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True)) - val = node.get_parameter(name).value - if val is None or val == '': - return '' - return str(val).strip() - - class RdkBridge: """通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。""" @@ -240,7 +234,7 @@ class RdkBridge: pass -class HandeyeTeach(Node): +class HandeyeTeach(TolerantNode): def __init__(self) -> None: super().__init__('handeye_teach') self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp') @@ -250,7 +244,7 @@ class HandeyeTeach(Node): self.declare_parameter('backend', 'ros') # ros | rdk self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip()) self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian - self._serial = _param_as_str(self, 'camera_serial', '') + self._serial = param_as_str(self, 'camera_serial', '') self.declare_parameter('auto_exposure', True) self.declare_parameter('ae_limit_us', 1800.0) self.declare_parameter('emitter_enabled', False) diff --git a/src/arm_application/arm_application/handeye_waypoints.py b/src/arm_application/arm_application/handeye_waypoints.py index a0cbaf4..e03d069 100644 --- a/src/arm_application/arm_application/handeye_waypoints.py +++ b/src/arm_application/arm_application/handeye_waypoints.py @@ -41,6 +41,8 @@ from arm_application.site_io import joint_prefix_default from arm_msgs.action import FollowJointTrajectory from arm_msgs.srv import GetArmState +from arm_application.ros_node import TolerantNode + try: import cv2 except Exception: # noqa: BLE001 @@ -110,7 +112,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray: raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}') -class HandeyeWaypoints(Node): +class HandeyeWaypoints(TolerantNode): def __init__(self) -> None: super().__init__('handeye_waypoints') self.declare_parameter('joint_prefix', joint_prefix_default()) diff --git a/src/arm_application/arm_application/hole_detect_node.py b/src/arm_application/arm_application/hole_detect_node.py index c34f14c..e8d7f8b 100644 --- a/src/arm_application/arm_application/hole_detect_node.py +++ b/src/arm_application/arm_application/hole_detect_node.py @@ -25,6 +25,8 @@ from arm_application.site_io import load_site_dict from arm_msgs.msg import HoleDetection as HoleDetectionMsg from arm_msgs.msg import HoleDetections +from arm_application.ros_node import TolerantNode + def _as_bool(val, default: bool = False) -> bool: if val is None: @@ -57,7 +59,7 @@ def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image: return msg -class HoleDetectNode(Node): +class HoleDetectNode(TolerantNode): def __init__(self) -> None: super().__init__("hole_detect") self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405") diff --git a/src/arm_application/arm_application/move_to_pose_client.py b/src/arm_application/arm_application/move_to_pose_client.py index bd53285..2a55ac5 100644 --- a/src/arm_application/arm_application/move_to_pose_client.py +++ b/src/arm_application/arm_application/move_to_pose_client.py @@ -15,8 +15,10 @@ from geometry_msgs.msg import PoseStamped from arm_msgs.action import MoveToPose +from arm_application.ros_node import TolerantNode -class MoveToPoseClient(Node): + +class MoveToPoseClient(TolerantNode): def __init__(self) -> None: super().__init__('move_to_pose_client') self._client = ActionClient(self, MoveToPose, 'move_to_pose') diff --git a/src/arm_application/arm_application/perception_node.py b/src/arm_application/arm_application/perception_node.py index 01772f6..730a0ec 100644 --- a/src/arm_application/arm_application/perception_node.py +++ b/src/arm_application/arm_application/perception_node.py @@ -48,6 +48,8 @@ from arm_application.yolo_detector import ( resolve_weights, ) +from arm_application.ros_node import TolerantNode + def _bgr_copy(msg: Image) -> np.ndarray: """返回调用方持有的 BGR uint8 数组（对异步 YOLO 安全；ROS 可能把原消息丢掉）。""" @@ -61,7 +63,7 @@ def _bgr_copy(msg: Image) -> np.ndarray: raise RuntimeError(f"不支持的编码 {msg.encoding!r}") -class PerceptionNode(Node): +class PerceptionNode(TolerantNode): def __init__(self) -> None: super().__init__("perception_node") tid = threading.get_ident() diff --git a/src/arm_application/arm_application/realsense_color_pub.py b/src/arm_application/arm_application/realsense_color_pub.py index f6b5e4a..cda88d9 100644 --- a/src/arm_application/arm_application/realsense_color_pub.py +++ b/src/arm_application/arm_application/realsense_color_pub.py @@ -32,16 +32,21 @@ from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import CameraInfo, Image from std_msgs.msg import Float32MultiArray +from arm_application.ros_node import TolerantNode, param_as_str -def _param_as_str(node: Node, name: str, default: str = '') -> str: - """字符串参数，同时兼容命令行传数字的情况（例如序列号）。""" - node.declare_parameter( - name, default, ParameterDescriptor(dynamic_typing=True) - ) - val = node.get_parameter(name).value - if val is None or val == '': - return '' - return str(val).strip() +# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"： +# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。 +# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）： +# D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途 +# `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失） +# 之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦 +# 接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**， +# 表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。 +USB_RESEAT_HINT = ( + '拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— ' + '只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。' + '看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。' +) def _fill_camera_info( @@ -79,12 +84,12 @@ def _site_yaml_path() -> Path: return Path("/home/cc/ros2/config/site.yaml") -class RealsenseColorPub(Node): +class RealsenseColorPub(TolerantNode): def __init__(self) -> None: super().__init__('realsense_color_pub') self.declare_parameter('camera_name', 'd405') # 话题命名空间 self._name = str(self.get_parameter('camera_name').value).strip().strip('/') - self._serial = _param_as_str(self, 'camera_serial', '') + self._serial = param_as_str(self, 'camera_serial', '') self.declare_parameter('width', 1280) self.declare_parameter('height', 720) self.declare_parameter('fps', 15) @@ -348,8 +353,8 @@ class RealsenseColorPub(Node): if not devices: raise RuntimeError( '没有连接 RealSense（多次重试后 USB 仍为空）。' - '请把 D405 插到**本机**的 USB3（蓝色）口，拔插一次，' - '并关闭 realsense-viewer / 192.168.0.108 上占用该相机的采集端。' + '拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；' + + USB_RESEAT_HINT ) found = self._pick_device(devices, rs) @@ -374,9 +379,12 @@ class RealsenseColorPub(Node): f'requested {self._width}x{self._height}@{self._fps}Hz' ) if usb2: + # 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度 + # 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能 + # （SuperSpeed 没协商上）。 self.get_logger().warn( - f'{cam_name} 接在 USB {usb_desc} 上 —— 深度/IR 将保持 ' - '640x480@15。把 D405 插到 USB3（蓝色）口可获得 848@30。' + f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 ' + f'640x480@15Hz。' + USB_RESEAT_HINT ) self._width, self._height, self._fps = 640, 480, 15 @@ -501,7 +509,7 @@ class RealsenseColorPub(Node): break if profile is None: raise RuntimeError( - f'D405 没有可用的彩色配置。最后的错误：{last_err}' + f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}' ) self._pipe = pipe diff --git a/src/arm_application/arm_application/realsense_view.py b/src/arm_application/arm_application/realsense_view.py index 3c1d866..b98dceb 100644 --- a/src/arm_application/arm_application/realsense_view.py +++ b/src/arm_application/arm_application/realsense_view.py @@ -27,6 +27,7 @@ from sensor_msgs.msg import Image from std_msgs.msg import Float32MultiArray from arm_application.realsense_color_pub import _site_yaml_path +from arm_application.ros_node import TolerantNode def _colorize_depth_u16(d: np.ndarray) -> np.ndarray: @@ -56,7 +57,7 @@ def _mono_to_bgr(mono: np.ndarray) -> np.ndarray: return np.ascontiguousarray(np.stack([mono, mono, mono], axis=-1)) -class RealsenseView(Node): +class RealsenseView(TolerantNode): def __init__(self) -> None: super().__init__('realsense_view') self.declare_parameter('camera_name', 'd405') diff --git a/src/arm_application/arm_application/ros_node.py b/src/arm_application/arm_application/ros_node.py new file mode 100644 index 0000000..13846c2 --- /dev/null +++ b/src/arm_application/arm_application/ros_node.py @@ -0,0 +1,118 @@ +"""应用层节点的统一基类：把「参数类型的宽窄」问题收在一处。 + +为什么需要这个基类（2026-09-28 真机落地实测，代价是顶置相机起不来） +------------------------------------------------------------------ +现场 `config/site.yaml` 写的是: + + cameras: + d435: + ae_limit_us: 8000 # ← YAML 整数 + +脚本把它当 `-p ae_limit_us:=8000` 传给节点，而节点里是: + + self.declare_parameter('ae_limit_us', 3000.0) # ← double + +rclpy **严格**比对类型，于是节点**一起就死**: + + InvalidParameterTypeException: Trying to set parameter 'ae_limit_us' + to '8000' of type 'INTEGER', expecting type 'DOUBLE' + +现场看到的是「顶置 D435 起不来」，而 site.yaml 那一行看起来毫无问题 —— +`8000` 和 `8000.0` 在运维眼里是同一个数，在 rclpy 眼里是两种类型。 +同一族的雷还有：`width: 848.0`（浮点）喂给 int 参数、`emitter_enabled: 1` +喂给 bool 参数、`"26610799"` 与 `26610799`（带不带引号）等等。 + +**为什么不让现场人员"写对一点"**：那等于要求每个人记住「这个数必须带小数点」， +而记错的表现是节点启动即崩、报错里还完全不提 site.yaml。规则要靠人记住的就 +一定会破（`FRAMEWORK_RULES.md` 的总纲）。 + +**为什么不在每个参数上加个 cast**：异常发生在**声明阶段**， +`float(self.get_parameter(...).value)` 根本执行不到 —— 看着宽容，其实没救。 + +做法 +---- +用 ``ParameterDescriptor(dynamic_typing=True)`` 声明：参数类型由**传进来的值** +决定，不做匹配检查；取值时由业务代码 cast（`float(...)` / `int(...)`）， +语义仍然由声明处的默认值表达。于是 ``8000`` / ``8000.0`` / ``"8000"`` 都能用。 + +**代价与边界**：动态类型意味着 rclpy 不再替我们拦「类型写错」。 +本项目接受这个代价，因为这些参数**全部**来自 `config/site.yaml` 这个运维手改的 +文件（不是程序内部传参），宽容比严格更符合现场；而"名字写错"仍然会被 +rclpy 拦下来（参数名是静态的，与类型无关）。 + +新增节点一律继承 ``TolerantNode``（有守卫测试 +``test_ros_node.py::test_no_node_subclasses_rclpy_node_directly`` 钉住）。 +""" + +from __future__ import annotations + +from typing import Any, Optional, Sequence, Union + +from rcl_interfaces.msg import ParameterDescriptor +from rclpy.node import Node + +__all__ = ["TolerantNode", "tolerant_descriptor", "param_as_str"] + + +def tolerant_descriptor() -> ParameterDescriptor: + """声明用的描述符：类型跟着**传进来的值**走，不做宽窄匹配。""" + return ParameterDescriptor(dynamic_typing=True) + + +def param_as_str(node: Node, name: str, default: str = "") -> str: + """读一个"本质是字符串"的参数，容忍传进来的是数字。 + + 为什么需要：现场 `site.yaml` 里 `serial_number: 26610799`（不加引号）就是 + 个 YAML 整数，而节点里想要的是字符串。rclpy 同样会因类型不匹配在**声明** + 阶段抛异常 —— 而字符串参数在运维手上最容易"看着是数字"。 + + 以前这段逻辑在三个节点里各抄了一份（`dh_pgea_node` / `handeye_teach` / + `realsense_color_pub`），相同的东西抄三份迟早歪（`FRAMEWORK_RULES.md` R5.4）。 + 现在只此一处；取不到就返回 `default`。 + """ + node.declare_parameter(name, default, tolerant_descriptor()) + value = node.get_parameter(name).value + if value is None: + return default + return str(value).strip() + + +class TolerantNode(Node): + """`rclpy.node.Node` + 「参数类型宽容」。 + + 只改 `declare_parameter` 一件事：默认描述符换成动态类型。 + 调用方若自己传了 `descriptor`（例如只想放宽某几个），以调用方为准。 + """ + + def declare_parameter( # type: ignore[override] + self, + name: str, + value: Any = None, + descriptor: Optional[ParameterDescriptor] = None, + ignore_override: bool = False, + ) -> Any: + if descriptor is None: + descriptor = tolerant_descriptor() + return super().declare_parameter(name, value, descriptor, ignore_override) + + # 列表版本是 rclpy 的另一个入口，同样要放宽（`workspace_xyz_min: [1, 2, 3]` + # 这种"整数字面量的列表"喂给 DOUBLE_ARRAY 一样会崩）。 + def declare_parameters( # type: ignore[override] + self, + namespace: str, + parameters: Sequence[ + Union[tuple, Any] + ], + ignore_override: bool = False, + ) -> Any: + fixed = [] + for item in parameters: + if ( + isinstance(item, tuple) + and len(item) == 2 + and not isinstance(item[1], ParameterDescriptor) + ): + fixed.append((item[0], item[1], tolerant_descriptor())) + else: + fixed.append(item) + return super().declare_parameters(namespace, fixed, ignore_override) diff --git a/src/arm_application/arm_application/run_joint_trajectory.py b/src/arm_application/arm_application/run_joint_trajectory.py index 6ccad6d..771b9ac 100644 --- a/src/arm_application/arm_application/run_joint_trajectory.py +++ b/src/arm_application/arm_application/run_joint_trajectory.py @@ -12,6 +12,8 @@ from rclpy.node import Node from arm_application.site_io import joint_prefix_default from arm_msgs.action import FollowJointTrajectory +from arm_application.ros_node import TolerantNode + DEFAULT_JOINTS = [ 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7', ] @@ -25,7 +27,7 @@ DEFAULT_POSITIONS = [ ] -class RunJointTrajectory(Node): +class RunJointTrajectory(TolerantNode): def __init__(self) -> None: super().__init__('run_joint_trajectory') self.declare_parameter('joint_prefix', joint_prefix_default()) diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index ebf57bd..b31462f 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -46,6 +46,8 @@ from arm_application.site_io import ( joint_prefix_default, ) +from arm_application.ros_node import TolerantNode + DEFAULT_JOINTS = [ "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", @@ -59,7 +61,7 @@ def _saturate_vec(v: np.ndarray, limit: float) -> np.ndarray: return v -class ServoNode(Node): +class ServoNode(TolerantNode): def __init__(self) -> None: super().__init__("servo_node") tid = threading.get_ident() diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index 667265b..f77ba12 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -47,9 +47,11 @@ 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.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 +from arm_application.ros_node import TolerantNode def _as_bool(val: Any, default: bool = False) -> bool: @@ -144,7 +146,7 @@ def _bgr_copy(msg: Image) -> np.ndarray: raise RuntimeError(f"Unsupported color encoding {msg.encoding!r}") -class Stage2Locate(Node): +class Stage2Locate(TolerantNode): def __init__(self, node_name: str = "stage2_locate") -> None: super().__init__(node_name) @@ -152,6 +154,10 @@ class Stage2Locate(Node): self.declare_parameter("hover_only", False) self.declare_parameter("target_hole", "") # 空字符串 → 用 site 的 stage2.target_hole self.declare_parameter("hover_z_m", -1.0) # 大于 0 → 覆盖 site 里的值 + # 工具尖端在法兰下方多少 [m]；大于 0 → 覆盖 site。 + # 仿真里由 run_gauge_assembly.sh --sim 传**实测值**（每次上料都可能不同）， + # 真机不传（走 site.yaml）。悬停高度与 S07 下探预算都按它算。 + self.declare_parameter("tool_tip_below_flange_m", -1.0) self.declare_parameter("preview", True) self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory") # 真值孔位（perception.source=truth）要把 scene.yaml 的物体位姿投影成像， @@ -205,6 +211,9 @@ class Stage2Locate(Node): self.perception_source = perception_source_of(perception) self._truth_object = str((perception.get("truth") or {}).get("object") or "") self._truth_warn_t = 0.0 + # 真值路径的"实测位姿"订阅与场景缓存（懒建，见 _truth_scene/_truth_live_pose） + self._truth_live = None + self._truth_spec = None self.target_hole = ( str(self.get_parameter("target_hole").value or "").strip().lower() @@ -218,6 +227,27 @@ class Stage2Locate(Node): self.move_speed = float(stage2.get("move_speed") or 0.05) # 最后轻降到孔面上方 final_gap_mm（不接触） self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0 + # 悬停高度**必须**让工具尖端留在工件面上方（2026-09-24 的教训，完整说明见 + # gauge_fsm.hover_z_guard）：悬停高是按相机视距定的，而工具（夹爪+通止规） + # 比相机长 —— 实测悬停时尖端已经钻进工件 25mm，于是 S05 的横向对准把工件 + # 拖走 92mm，而真值孔位按工件的**实测**位姿算 ⇒ 孔位跟着工具走、残差恒定 + # 13mm 永不收敛（现场看着像手眼标定坏了）。这里按几何兜住并喊出原因。 + _ga = deep_merge(site.get("gauge_assembly") or {}, robot.get("gauge_assembly") or {}) + # 工具长优先取**启动参数**（launch 覆盖 > site）：仿真里上料脚本会实测它 + # 并落盘（sim_load_gauge → sim_tool_geometry.yaml → run_gauge_assembly.sh）， + # 因为仿真里规会在指间下滑、下滑量随装夹变化（实测见过 243 / 254mm）。 + # 真机不给这个参数，走 site.yaml 的实测值 —— 与 hover_z_m 同一套优先级。 + _tool_len = float(self.get_parameter("tool_tip_below_flange_m").value) + 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) + if _tool_len > 0.0: + _fixed, _why = hover_z_guard(self.hover_z, _surface_z, _tool_len, self.final_gap_m) + if _why: + self.get_logger().warn( + f"二阶段悬停高度自检不通过 —— 用几何修正值 {_fixed:.4f}m：\n {_why}" + ) + self.hover_z = _fixed 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) self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8)) @@ -254,11 +284,21 @@ class Stage2Locate(Node): self.create_subscription(Image, self.image_topic, self._on_color, qos_profile_sensor_data) self.create_subscription(Image, self.depth_topic, self._on_depth, qos_profile_sensor_data) self.create_subscription(CameraInfo, self.info_topic, self._on_info, qos_profile_sensor_data) - # 腕部六维力（轻降安全兜底：规尖一碰工件 force.z 突变，立即停） + # 腕部六维力（轻降安全兜底：规尖一碰工件 force.z 突变，立即停）。 + # 它**同时是 S07 柔顺下压的首选接触判据**（见 gauge_fsm.wrench_touch）： + # 通止规靠夹持摩擦抱住，轴向力传不到指垫触觉，只有腕部 FT 测得准。 self._ft_fz: Optional[float] = None + #: 最近一帧的收到时刻（单调钟）。判"有没有活的数据"用它，**不用数值大小** —— + #: 静止时轴向力本来就接近 0，拿数值判活会把"安静的传感器"误判成掉线。 + self._t_ft: Optional[float] = None robot_ns = str(self.get_parameter("robot_sn").value).replace("-", "_") + # 话题可配：默认按 sn 推导，与真机 flexiv_robot_states_broadcaster 一致。 + # 换 FT 来源（换个厂家、或走 external_wrench_in_tcp）时不用改代码。 + self.ft_wrench_topic = str( + servo.get("ft_wrench_topic") or f"/{robot_ns}/ft_sensor_wrench" + ) self.create_subscription( - WrenchStamped, f"/{robot_ns}/ft_sensor_wrench", self._on_ft, qos_profile_sensor_data + WrenchStamped, self.ft_wrench_topic, self._on_wrench, qos_profile_sensor_data ) self._state_pub = self.create_publisher(String, "/stage2/state", 10) @@ -293,8 +333,18 @@ class Stage2Locate(Node): def _on_info(self, msg: CameraInfo) -> None: self._info = msg - def _on_ft(self, msg: WrenchStamped) -> None: + def _on_wrench(self, msg: WrenchStamped) -> None: + """缓存腕部 FT 的**轴向力**（`geometry_msgs/WrenchStamped`，法兰系）。 + + ⚠️ 名字**必须**与 `Stage3Search._on_robot_states` 区分开。2026-09-24 踩到： + 两者原先都叫 `_on_ft`，子类定义静默覆盖父类的，于是本订阅的 WrenchStamped + 被塞进了 stage3 的 RobotStates 缓存，`_ft_fz` 永远不写 —— 现象是 + "S07 腕部 FT 没有数据"，而话题数据完全正常（在 shell 里 echo 得到）。 + 真实原因与"阈值/话题名"都无关，纯粹是**同名覆盖**。""" self._ft_fz = float(msg.wrench.force.z) + # 记收到时刻：下游（尤其 S07）要区分"收到过但读数为 0"（活）与 + # "一帧都没收到"（话题名/网络/传感器的问题）。 + self._t_ft = time.monotonic() def _contact_detected(self, baseline_fz: float, thresh_n: float = 2.5) -> bool: """腕部力 Z 偏离基线 thresh_n 牛以上 → 判为规尖碰工件（接触）。""" @@ -473,6 +523,8 @@ class Stage2Locate(Node): waypoints = [q.copy()] remain = dp.copy() w_step, _ = (self._w_down(R) if verticalize else (np.zeros(3), 0.0)) + # 入口倾角：**纯旋转 hop 的进度判据必须用它**（见下面 progressed 的说明） + ang0 = float(ang) # 旋转-only hop 需要更多内部步数；角度模型按实际步长递减 # 内层收敛下限随请求距离缩放：stage3 的微步只有 0.3~1mm， # 固定 1.5mm 下限会让亚毫米微步第一步就退出（planned=0 → 规划停滞） @@ -497,9 +549,21 @@ class Stage2Locate(Node): planned = dist - float(np.linalg.norm(remain)) min_frac = 0.75 if dist >= 0.0008 else 0.45 - if len(waypoints) < 2 or planned < min_frac * dist: + # 进度判据分两种情形 —— 混用会**静默整段跳过**（2026-09-24 踩到）： + # · 平移 hop（dist > 0）：按"走完了请求距离的多少"（planned/dist）。 + # · **纯旋转 hop**（verticalize 且目标位置≈当前位置 ⇒ dist≈0）： + # 必须按**倾角**判。此时 planned = 0 − |remain|，而绕腕旋转本来就会让 + # 法兰/TCP 漂移一点（实测 2.78mm）⇒ planned 变成负数 ⇒ 立刻判"规划停滞"、 + # 整段垂直化被跳过。现场症状：日志只说"规划停滞"（看起来像 IK 解不出来）， + # 真相是 S05 带着 6.2° 倾角去压孔 —— 工具斜着顶在工件面上，重则撞件(E2)。 + if dist > 1e-6: + progressed = planned >= min_frac * dist + else: + progressed = (ang0 - float(ang)) >= min_frac * max(ang0, 1e-9) + if len(waypoints) < 2 or not progressed: self.get_logger().warn( - f" {tag}: 规划停滞 (planned {planned*1000:.2f}/{dist*1000:.2f}mm) — 跳过" + f" {tag}: 规划停滞 (planned {planned*1000:.2f}/{dist*1000:.2f}mm, " + f"倾角 {math.degrees(ang0):.2f}→{math.degrees(float(ang)):.2f}deg) — 跳过" ) return False if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0: @@ -507,7 +571,11 @@ class Stage2Locate(Node): return False if dist < 0.008: - return self._send_micro(waypoints, dist, v, q0, tag) + # 纯旋转 hop 的 dist=0：用它算时间是 0.14s 的硬下限，对 6.2°（手腕要转 + # 0.11rad）来说太快 —— JTC 追不上速度上限就会欠到位，现象是"发了没动"。 + # 把倾角按 0.10m 力臂折算成等效弧长再交给微步的时间规划。 + equiv_m = max(dist, 0.10 * (ang0 - float(ang))) + return self._send_micro(waypoints, equiv_m, v, q0, tag) # 巡航速度保持 v，头尾平滑加减速。刹车距拉长、加速度下限放低—— # 夹爪持规时急加速/急停会把规震脱，柔顺剖面（长刹车、低加速度）消除过冲和震动。 @@ -883,9 +951,14 @@ class Stage2Locate(Node): 退回检测器的后果是**用整幅画面当 ROI**，于是 HoughCircles 在夹爪/通规/ 台面上找出一对"假孔"（实测间距 108mm，真值 58mm），再被门槛丢掉 —— 现象是"每拍都丢、最后超时"，比直接说"TF 没到"难查得多。 + + ⚠️ 几何用**实测**位姿（``sim_live_pose``）而不是 scene.yaml 的名义位姿： + 工件落地/蠕动后就偏 2~3mm，而规尖（φ6）进 φ7 的孔只有 0.5mm 余量 —— + 按名义位姿算的孔位会把这一整条链指偏两毫米（现象是"S07 压到工件面、 + 孔却没进去"，看起来像柔顺参数不对）。真机上的视觉量的也是"实际在哪"。 """ try: - from arm_sim_gazebo import truth_holes + from arm_sim_gazebo import sim_live_pose, sim_scene, truth_holes except ImportError: # 真机上没有仿真包，perception.source 也不会是 truth —— 到这里只可能是 # 有人把 source 配成 truth 却没装仿真包，直接报清楚。 @@ -897,9 +970,13 @@ class Stage2Locate(Node): i3 = None if intr is not None: i3 = (intr.get("fx"), intr.get("fy"), intr.get("cx"), intr.get("cy")) + spec = self._truth_scene(sim_scene) + live = self._truth_live_pose(spec, sim_live_pose) + if live is not None: + spec = live.apply(spec, [str(self._truth_object or spec.operating.part_object)]) res = truth_holes.locate_holes( T, i3, (int(bgr.shape[0]), int(bgr.shape[1])), - object_name=self._truth_object, root=None, + spec=spec, object_name=self._truth_object, root=None, ) if not res.ok: now = time.monotonic() @@ -914,16 +991,81 @@ class Stage2Locate(Node): "bbox": None, "draw_img": truth_holes.draw(bgr, res), } + # 真值三维 → 基座系（TF 查不到时返回空列表，调用方会退回深度反投影） + base = self._truth_to_base(res.hole_world) + center = self._truth_to_base([res.part_center_world] if res.part_center_world else []) return { "status": True, "msg": f"真值孔位（{res.note}）", "hole_points": [[float(p[0]), float(p[1])] for p in res.hole_points], "hole_radii": [float(r) for r in res.hole_radii], + # ---- 真值的三维：**直接给**，不让调用方再去反投影 -------------- + # 为什么必须这样（2026-09-24 的教训，详见 truth_holes.HoleTruth）： + # 孔是贯通孔，孔位正下方是台面通槽，深度相机在孔位看到的是槽底/地面 + # （0.62m）而不是工件面（0.21m）。用深度图反投影会把 58mm 的孔距 + # 算成 169mm、被间距门槛丢掉 —— 现象像"检测坏了"，其实真值本来就知道。 + "hole_base": base, + "hole_z": [float(v[2]) for v in base] if base else [0.0, 0.0], + # 工件上表面的世界 z 与基准面：S07 的悬停几何要用它判断 + # "悬停时规尖到底入没入工件"，缺了它那项自检只能按 0 算（会误报）。 + "z_wp": float(res.part_top_z or 0.0), + "part_center_base": center, "bbox": [int(v) for v in res.bbox], "label": "workpiece(truth)", "draw_img": truth_holes.draw(bgr, res), } + def _truth_scene(self, sim_scene): + """真值路径的场景配置（读一次，缓存）。 + + 为什么缓存：S05 会连着算很多拍，而 scene.yaml 是**静态配置** + （物体位姿那种会变的东西走 ``sim_live_pose``，不从这里读）。 + """ + if self._truth_spec is None: + self._truth_spec = sim_scene.load_scene(None, None) + return self._truth_spec + + def _truth_live_pose(self, spec, sim_live_pose): + """真值路径的"实测位姿"订阅（懒建一次）。没配 pose_info 就返回 None。 + + 为什么这里也要（不只是真值感知节点）：二阶段的**孔位**是这条链上 + 精度要求最高的一处（规尖 φ6 进 φ7 的孔），而它和真值感知共用同一份 + "物体现在在哪"的实现（``sim_live_pose``，R5.8）。 + """ + if self._truth_live is None: + pi = getattr(spec, "pose_info", None) + topic = str(getattr(pi, "ros_topic", "") or "") + self._truth_live = sim_live_pose.LiveScenePose(self, topic) if topic else False + if self._truth_live: + self.get_logger().info( + f"真值孔位按**实测**位姿算（订阅 {topic}）—— " + f"按名义位姿算会偏 2~3mm，而规尖进孔只有 0.5mm 余量。" + ) + return self._truth_live or None + + def _truth_to_base(self, pts_world: Any) -> list: + """世界系点列 → 基座系点列（TF 查不到时返回空列表，调用方会退回深度反投影）。 + + 为什么不在 truth_holes 里做：那里是**纯几何**模块（没有 ROS、因此能脱机测）， + 变换必须由持有 TF 的节点来查。这条分工是刻意的。 + """ + p = [list(map(float, v)) for v in (pts_world or [])] + if not p: + return [] + T = self._lookup(self.base_frame, str(self.get_parameter("world_frame").value or "world")) + if T is None: + self.get_logger().warn( + "查不到 TF 基座→世界 —— 真值三维用不了，本拍退回深度反投影" + "（仿真里那一步会因孔下方是通槽而算错孔距）。" + ) + return [] + out = [] + for v in p: + q = np.ones(4) + q[:3] = v[:3] + out.append([float(x) for x in (T @ q)[:3]]) + return out + def _detect_holes_img(self, bgr: np.ndarray, *, skip_center_gate: bool, depth_m: np.ndarray = None, intr: dict = None) -> dict: """跑一次孔检测。第二次调用（``skip_center_gate=True``）允许整幅画面当 ROI。 @@ -1192,18 +1334,29 @@ class Stage2Locate(Node): pts = res.get("hole_points") or [] rad = res.get("hole_radii") or [] if res.get("status") and len(pts) == 2 and len(rad) >= 2: - proj = self._project_holes(med_depth, pts, rad, res.get("bbox"), last_intr) - if proj is not None: - res["hole_base"] = [ - [float(proj["left_b"][0]), float(proj["left_b"][1]), float(proj["left_b"][2])], - [float(proj["right_b"][0]), float(proj["right_b"][1]), float(proj["right_b"][2])], - ] - res["hole_z"] = [float(proj["z_left"]), float(proj["z_right"])] - res["z_wp"] = float(proj["z_wp"]) + # **只在调用方还没给三维时**才反投影。 + # 真值路径（perception.source=truth）已经把孔的世界系精确位置带出来了 + # （见 _truth_hole_result → _truth_to_base），再拿深度图反投影一遍 + # 只会把它换成一个**更差**的数：孔位正下方是贯通孔 + 台面通槽， + # 深度相机在孔位看到的是槽底/地面（0.62m）而不是工件面（0.21m）， + # 三角化出的孔距会从 58mm 变成 169mm。2026-09-24 就是栽在这里。 + if not res.get("hole_base"): + proj = self._project_holes(med_depth, pts, rad, res.get("bbox"), last_intr) + if proj is not None: + res["hole_base"] = [ + [float(proj["left_b"][0]), float(proj["left_b"][1]), float(proj["left_b"][2])], + [float(proj["right_b"][0]), float(proj["right_b"][1]), float(proj["right_b"][2])], + ] + res["hole_z"] = [float(proj["z_left"]), float(proj["z_right"])] + res["z_wp"] = float(proj["z_wp"]) res["snap_frames"] = int(len(frames)) + # 把"三维是哪来的"也打出来：这一项错了（真值被当成深度、或反过来） + # 会让孔距从 58mm 变成 169mm，而现象看起来只是"检测不认"。 + _src = "真值" if res.get("hole_base") and self._truth_enabled() else "深度反投影" self.get_logger().info( f"精拍 {len(frames)}帧中值图 " f"L=({pts[0][0]:.1f},{pts[0][1]:.1f}) R=({pts[1][0]:.1f},{pts[1][1]:.1f})" + f" 三维来源={_src}" ) return med_bgr, med_depth, last_intr, res @@ -1351,12 +1504,15 @@ class Stage2Locate(Node): for move_i in range(self.coarse_max_moves + 1): self._state(f"snap_coarse_{move_i}") bbox = None + truth: Optional[dict] = None bgr = depth_m = intr = None time.sleep(self.settle_sec if move_i == 0 else min(0.20, self.settle_sec)) for attempt in range(8): if attempt: self._spin(0.12) bgr, depth_m, intr = self._grab() + # 真值三维（有就带上）：粗居中的目标点优先用它，别绕深度图。 + truth = self._truth_hole_result(bgr) if self._truth_enabled() else None bbox = self._best_bbox(bgr) if bbox is not None: break @@ -1364,6 +1520,7 @@ class Stage2Locate(Node): if holes.get("status"): self.get_logger().info("粗定位 YOLO 空框，但已看到 k1/k2，跳过居中直进精对准") bbox = holes.get("bbox") + truth = holes self._publish_overlay(holes.get("draw_img", bgr)) move_i = self.coarse_max_moves break @@ -1379,8 +1536,18 @@ class Stage2Locate(Node): bx, by = 0.5 * (bbox[0] + bbox[2]), 0.5 * (bbox[1] + bbox[3]) # 收敛判据用"基座系下夹爪轴→工件中心距离"，与相机偏置无关 # （旧版用图像中心，相机偏置时图像居中≠夹爪对准，会误判/白动）。 - z_wp = self._surface_depth(depth_m, bbox) - p_base = self._cam_to_base(self._px_to_cam(bx, by, z_wp, intr)) + # + # 目标点优先用**真值三维**（truth 模式）：工件中心是一个已知点， + # 没必要绕"像素 → 深度图 → 三角化"。仿真里那一步还会因为孔下方是 + # 通槽而把深度取到槽底/地面，算出个偏出好几厘米的"中心" + # （实测第二轮 coarse 偏差反而从 53.7mm 涨到 97.7mm —— 越走越偏）。 + p_base, src = None, "深度" + if truth is not None and truth.get("part_center_base"): + p_base = np.asarray(truth["part_center_base"][0], dtype=float) + src = "真值" + if p_base is None: + z_wp = self._surface_depth(depth_m, bbox) + p_base = self._cam_to_base(self._px_to_cam(bx, by, z_wp, intr)) T_now = self._lookup(self.base_frame, self.ee_frame) if T_now is not None: Tee = T_now @@ -1389,7 +1556,8 @@ class Stage2Locate(Node): coarse_tol = float(self._hole_cfg.get("coarse_tol_m", 0.006)) self.get_logger().info( f"工件 bbox={bbox} 中心=({bx:.0f},{by:.0f}) " - f"base=({p_base[0]:.3f},{p_base[1]:.3f}) 夹爪=({gxy[0]:.3f},{gxy[1]:.3f}) " + f"base=({p_base[0]:.3f},{p_base[1]:.3f})[{src}] " + f"夹爪=({gxy[0]:.3f},{gxy[1]:.3f}) " f"偏差={err_xy*1000:.1f}mm" ) try: @@ -1425,6 +1593,10 @@ class Stage2Locate(Node): final: Dict[str, Any] = {} xy_gain = 1.0 last_good: Optional[Tuple[np.ndarray, np.ndarray, float, Dict[str, Any]]] = None + # 两孔中点的历史（本拍 = [-1]，上一拍 = [-2]，上上拍 = [-3]）： + # 对准时工具在动、孔位也会被"追"着动，唯一**不该动**的是两孔中点 —— + # 它一动就说明工件在动，而工件只会被工具推走（见 gauge_fsm.drag_verdict）。 + mids: List[Tuple[float, float]] = [] for it in range(self.align_max_iters + 1): self._state(f"snap_fine_{it}") bgr, depth_m, intr, res = self._snap_holes(skip_center_gate=True) @@ -1508,6 +1680,44 @@ class Stage2Locate(Node): f"gauge=({gxy[0]:.4f},{gxy[1]:.4f}) 残差={err*1000:.2f}mm (tol {self.align_tol*1000:.1f})" ) + # ---- 工件被推走的看门狗 ----------------------------------------- + # 为什么放在"打印之后、动手之前"：这一拍的数据已经完整，判出来就 + # 立刻停，不再多推一下（每多推一下工件就多走十几毫米，而它已经 + # 离开孔位了，后面的每一拍都在追一个被自己推着走的目标）。 + mid_now = ( + float(k1_b[0] + k2_b[0]) / 2.0, + float(k1_b[1] + k2_b[1]) / 2.0, + ) + dragged, d_mid = drag_verdict( + mid_now, + mids[-1] if len(mids) >= 1 else None, + mids[-2] if len(mids) >= 2 else None, + ) + mids.append(mid_now) + if dragged: + tool_mm = 0.0 + if last_good is not None: + _g0 = last_good[3].get("gauge_xy_m") or [] + if len(_g0) == 2: + tool_mm = math.hypot(gxy[0] - float(_g0[0]), gxy[1] - float(_g0[1])) * 1000.0 + self.get_logger().error( + f"工件被工具推走了：两孔中点两拍之间挪了 {d_mid*1000:.1f}mm" + f"（同期工具只走了 {tool_mm:.1f}mm）—— 对准闭环追的孔位长在工件上，" + f"工件一走，孔位就跟着工具走、残差恒定不收敛。\n" + f" 这**不是**手眼标定/检测的问题（那两样坏了不会让工件移动）。" + f"按下面顺序查：\n" + f" ① 悬停时工具尖端是不是已经进了工件：法兰 z={Tee[2, 3]:.4f} − " + f"工具长（site.yaml 的 gauge_assembly.tool_tip_below_flange_m）" + f" 要高于工件上表面 z={z_wp:.4f}；\n" + f" ② 工具长是**实测**的吗（真机夹上规后读 TF；仿真里 " + f"scripts/sim_load_gauge.py 会打印实测值）；装夹位置一变它就变；\n" + f" ③ 装夹后规有没有下滑（仿真里合爪后规会落到落座位置，" + f"下滑量直接加在工具长上）。" + ) + self._publish_overlay(draw) + self._publish_result(final) + return 6, q, Tee, final + # 在叠图上标目标孔 + 写基座坐标 try: import cv2 diff --git a/src/arm_application/arm_application/tactile_node.py b/src/arm_application/arm_application/tactile_node.py index f6e652d..85a144f 100644 --- a/src/arm_application/arm_application/tactile_node.py +++ b/src/arm_application/arm_application/tactile_node.py @@ -22,8 +22,10 @@ from arm_application.site_io import load_site_dict, site_path from arm_msgs.msg import TactileStatus from sensor_msgs.msg import Image +from arm_application.ros_node import TolerantNode -class TactileNode(Node): + +class TactileNode(TolerantNode): def __init__(self) -> None: super().__init__("tactile") self.declare_parameter("publish_hz", 20.0) diff --git a/src/arm_application/arm_application/weights_layout.py b/src/arm_application/arm_application/weights_layout.py index ceb99d6..75c3a5a 100644 --- a/src/arm_application/arm_application/weights_layout.py +++ b/src/arm_application/arm_application/weights_layout.py @@ -176,6 +176,51 @@ def staging_dir(root: Union[str, Path]) -> Path: return Path(root) / STAGING_DIRNAME +def infer_group( + root: Union[str, Path], + source: Union[str, Path], + fallback: str = "yolo", +) -> str: + """转换产物该落到**哪个分组**：默认沿用源文件所在的分组。 + + 为什么不能一律落到 ``yolo``（2026-09-28 真机踩到，代价是一次白转） + ---------------------------------------------------------------- + 现场把 ``models/hole_insert/detect_best_1.onnx`` 转 engine，工具默认 + ``--group yolo``，于是产物落到 ``models/yolo/detect_best_1.engine`` —— + 而运行时找 engine 的规则是**同级同名**（``yolo_detector.resolve_weights``： + ``p.with_suffix(".engine")``，即 site.yaml 指的 ``.onnx`` 旁边）。两边 + 一分家，engine 就等于不存在：现象是"启动即报缺 engine"，而文件明明躺着。 + 这正是本项目最忌讳的**静默失效** —— 工具没报错，位置也"合理"。 + + 规则： + - 源文件已在权重根目录下的某个分组里（例如 ``models/hole_insert/x.onnx``） + → 沿用 ``hole_insert``。这样"就地转换"永远落在原地，运行时必然找得到； + - 源文件在 ``staging/``、权重根目录之外或根目录正下方 → ``fallback`` + （默认 ``yolo``）：那才是"新来的模型，还没决定分组"的正常情形。 + + Args: + root: 权重根目录（``models/``）。 + source: 输入文件。 + fallback: 无法沿用时的分组名。 + + Returns: + 分组名（不带路径）。 + """ + r = Path(root) + try: + rel = Path(source).expanduser().resolve().relative_to(r.resolve()) + except ValueError: + # 不在权重根目录下（/tmp、外部磁盘、staging 之类）→ 没有"原地"可言 + return fallback + if len(rel.parts) < 2: + # 直接躺在 models/ 正下方：没有分组信息，用默认分组 + return fallback + group = rel.parts[0] + if is_staging(r / group) or group.startswith("."): + return fallback + return group + + def staging_files(root: Union[str, Path]) -> List[Path]: """staging 里的待处理文件。 diff --git a/src/arm_application/test/test_ros_node.py b/src/arm_application/test/test_ros_node.py new file mode 100644 index 0000000..4dd6a7b --- /dev/null +++ b/src/arm_application/test/test_ros_node.py @@ -0,0 +1,167 @@ +"""`TolerantNode` 的回归锚点 + 「谁都不许直接用 rclpy 的 Node」守卫。 + +背景（2026-09-28 真机落地实测）：现场 `config/site.yaml` 的 +``cameras.d435.ae_limit_us: 8000`` 是 YAML **整数**，节点里声明的是 double， +rclpy 严格较类型 → 顶置 D435 节点**启动即崩**，报错里还完全不提 site.yaml： + + InvalidParameterTypeException: Trying to set parameter 'ae_limit_us' + to '8000' of type 'INTEGER', expecting type 'DOUBLE' + +这组测试钉住三件事： +1. `TolerantNode` 对「数字宽窄不一致」是宽容的（8000 / 8000.0 / "8000" 都能用）； +2. 宽容**只**发生在数值族，不是"什么都收"； +3. 应用层不再有直接继承 `rclpy.node.Node` 的节点类 —— 否则这个坑会悄悄回来。 +""" + +from __future__ import annotations + +import ast +from pathlib import Path + +import pytest + +rclpy = pytest.importorskip("rclpy") + +from rclpy.node import Node # noqa: E402 +from rclpy.parameter import Parameter # noqa: E402 + +from arm_application.ros_node import TolerantNode, param_as_str # noqa: E402 + +APP_DIR = Path(__file__).resolve().parents[1] / "arm_application" + + +@pytest.fixture() +def ros(): + rclpy.init() + try: + yield + finally: + rclpy.shutdown() + + +def _overrides(**kwargs): + return [Parameter(k, value=v) for k, v in kwargs.items()] + + +# --------------------------------------------------------------------------- +# 1. 数值族宽容 +# --------------------------------------------------------------------------- +def test_float_param_accepts_integer_override(ros): + """site.yaml 写 8000（整数）→ double 参数必须能起来，并按 8000.0 用。""" + node = TolerantNode("t_float", parameter_overrides=_overrides(ae_limit_us=8000)) + try: + node.declare_parameter("ae_limit_us", 3000.0) + assert float(node.get_parameter("ae_limit_us").value) == 8000.0 + finally: + node.destroy_node() + + +def test_int_param_accepts_float_override(ros): + """反过来也要能起：site.yaml 写 848.0 → int 参数（宽度）。""" + node = TolerantNode("t_int", parameter_overrides=_overrides(width=848.0)) + try: + node.declare_parameter("width", 1280) + assert int(node.get_parameter("width").value) == 848 + finally: + node.destroy_node() + + +def test_bool_param_accepts_int_override(ros): + """site.yaml 写 1 / 0（YAML 布尔的一种常见手写）→ bool 参数。""" + node = TolerantNode("t_bool", parameter_overrides=_overrides(emitter_enabled=1)) + try: + node.declare_parameter("emitter_enabled", False) + assert bool(node.get_parameter("emitter_enabled").value) is True + finally: + node.destroy_node() + + +def test_number_array_param_accepts_integer_array(ros): + """`workspace_xyz_min: [0, -1, 0]` 这种整数字面量列表 → DOUBLE_ARRAY。""" + node = TolerantNode("t_arr", parameter_overrides=_overrides(ws=[0, -1, 0])) + try: + node.declare_parameter("ws", [0.1, -0.5, 0.2]) + assert [float(v) for v in node.get_parameter("ws").value] == [0.0, -1.0, 0.0] + finally: + node.destroy_node() + + +def test_param_as_str_accepts_numeric_override(ros): + """SN 写成不带引号的 26610799（YAML 整数）→ 字符串参数。""" + node = TolerantNode("t_str", parameter_overrides=_overrides(serial_number=26610799)) + try: + assert param_as_str(node, "serial_number", "") == "26610799" + finally: + node.destroy_node() + + +def test_missing_override_falls_back_to_default(ros): + node = TolerantNode("t_default") + try: + node.declare_parameter("ae_limit_us", 3000.0) + assert float(node.get_parameter("ae_limit_us").value) == 3000.0 + assert param_as_str(node, "camera_serial", "abc") == "abc" + finally: + node.destroy_node() + + +# --------------------------------------------------------------------------- +# 2. 「为什么需要这个子类」——防止有人把宽容读成"本来就没问题" +# --------------------------------------------------------------------------- +def test_plain_rclpy_node_really_is_strict(ros): + """原生 Node 遇到宽窄不一致会抛异常。 + + 这条测试是**原因**的记录：如果哪天 rclpy 改了行为，这条会红， + 我们就知道 `TolerantNode` 已经没有存在意义（可以删掉，而不是留着当谜语）。 + """ + from rclpy.exceptions import InvalidParameterTypeException + + node = Node("t_strict", parameter_overrides=_overrides(ae_limit_us=8000)) + try: + with pytest.raises(InvalidParameterTypeException): + node.declare_parameter("ae_limit_us", 3000.0) + finally: + node.destroy_node() + + +# --------------------------------------------------------------------------- +# 3. 守卫：应用层不许再直接继承 rclpy 的 Node +# --------------------------------------------------------------------------- +def _direct_node_subclasses() -> list[str]: + hits: list[str] = [] + for path in sorted(APP_DIR.glob("*.py")): + if path.name == "ros_node.py": + continue # TolerantNode 自己就是干这个的 + tree = ast.parse(path.read_text(encoding="utf-8")) + imports_rclpy_node = False + for node in ast.walk(tree): + if isinstance(node, ast.ImportFrom) and (node.module or "") == "rclpy.node": + if any(alias.name == "Node" for alias in node.names): + imports_rclpy_node = True + if not imports_rclpy_node: + continue + for node in ast.walk(tree): + if isinstance(node, ast.ClassDef) and any( + isinstance(b, ast.Name) and b.id == "Node" for b in node.bases + ): + hits.append(f"{path.name}::{node.name}") + return hits + + +def test_no_node_subclasses_rclpy_node_directly(): + hits = _direct_node_subclasses() + assert not hits, ( + "这些节点类直接继承了 rclpy 的 Node，会重新踩「site.yaml 写 8000、" + "节点要 double」的启动崩溃：\n " + "\n ".join(hits) + + "\n改法：继承 arm_application.ros_node.TolerantNode。" + ) + + +def test_tolerant_node_is_the_thing_nodes_subclass(): + """至少得有人真的在用它，否则这条规则只是摆设。""" + users = [ + p.name + for p in sorted(APP_DIR.glob("*.py")) + if "TolerantNode" in p.read_text(encoding="utf-8") and p.name != "ros_node.py" + ] + assert len(users) >= 10, f"用 TolerantNode 的节点太少了：{users}" diff --git a/src/arm_application/test/test_weights_layout.py b/src/arm_application/test/test_weights_layout.py index fe341b5..cc209fe 100644 --- a/src/arm_application/test/test_weights_layout.py +++ b/src/arm_application/test/test_weights_layout.py @@ -30,6 +30,7 @@ from arm_application.weights_layout import ( find_model, find_repo_root, groups, + infer_group, model_labels, plan_conversion, staging_files, @@ -574,6 +575,46 @@ def test_cli_convert_engine_from_staging_is_consumed(tmp_path, capsys): assert not (tmp_path / "staging" / "done").exists() +# ============================================================================= +# 归位分组：就地转换必须落在**原地** +# ============================================================================= +# 现场事故（2026-09-28）：把 models/hole_insert/detect_best_1.onnx 转 engine， +# 工具默认 --group yolo，产物落到 models/yolo/detect_best_1.engine；而运行时找 +# engine 的规则是"site.yaml 指的那个 .onnx 的**同级同名**"（resolve_weights）。 +# 两边一分家，engine 等于不存在 —— 文件躺着、工具也没报错，是最难查的一类。 +def test_infer_group_keeps_the_source_group_for_in_place_conversion(tmp_path): + assert infer_group(tmp_path, tmp_path / "hole_insert" / "m.onnx") == "hole_insert" + + +def test_infer_group_falls_back_for_staging_and_outside_files(tmp_path): + assert infer_group(tmp_path, tmp_path / "staging" / "m.onnx") == "yolo" + assert infer_group(tmp_path, tmp_path / "m.onnx") == "yolo" # 根目录正下方 + assert infer_group(tmp_path, Path("/tmp/elsewhere/m.onnx")) == "yolo" # 根目录之外 + + +def test_infer_group_accepts_a_custom_fallback(tmp_path): + assert infer_group(tmp_path, tmp_path / "staging" / "m.pt", fallback="mine") == "mine" + + +def test_cli_convert_in_place_lands_next_to_the_source(tmp_path, capsys): + """就地转换（输入已在分组里）→ 产物必须在同一分组。 + + 这一条就是上面那个事故的回归锚点：只有落在同级，``resolve_weights`` 的 + ``p.with_suffix(".engine")`` 才找得到它。 + """ + _touch(tmp_path / "hole_insert" / "detect_best_1.engine", b"ENGINE") + cli = _load_cli() + assert cli.main([ + "--root", str(tmp_path), "convert", + "hole_insert/detect_best_1.engine", "--name", "detect_best_1", + ]) == 0 + dst = tmp_path / "hole_insert" / "detect_best_1.engine" + assert dst.is_file(), "就地转换的产物必须留在原地（engine 得和 site.yaml 指的 .onnx 同级）" + assert not (tmp_path / "yolo" / "detect_best_1.engine").exists() + out = capsys.readouterr().out + assert "沿用输入文件所在分组 hole_insert/" in out, "要让人看见这次落在哪个分组" + + def test_cli_convert_keep_staging(tmp_path): _touch(tmp_path / "staging" / "raw.engine", b"E") cli = _load_cli()