orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

把现场确认能用的对孔方案落成保存点：小孔只用视觉关键点交到工件面、多走多拍闭环；D435 走到点后杀栈冷启动再停稳 3 秒才交 D405，避免赶路时带臂晃。

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-13 14:57:54 +08:00
parent f882c4309c
commit 1b264c0654
24 changed files with 906 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_application/arm_application/gauge_fsm.py
+28
View File
@@ -614,6 +614,34 @@ def hover_z_guard(
return need_z, why
def resolve_hover_z(
hover_z_m: float,
surface_z_m: float,
tool_tip_below_flange_m: float,
want_clearance_m: float,
user_set: bool,
) -> Tuple[float, str]:
"""返回 ``(要用的法兰悬停高, 拒绝原因)``。
启动参数写了高度：不安全就拒绝，原因非空，高度原样返回，调用方不许改成别的数。
没写（用 site）：沿用 :func:`hover_z_guard` 的几何抬高，拒绝原因留空。
"""
fixed, why = hover_z_guard(
hover_z_m, surface_z_m, tool_tip_below_flange_m, want_clearance_m
)
if not why:
return float(hover_z_m), ""
if not user_set:
return float(fixed), ""
clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
return float(hover_z_m), (
f"悬停高度会让工具尖端进入工件：法兰 {float(hover_z_m):.4f} − 工具长 "
f"{float(tool_tip_below_flange_m) * 1000:.1f}mm，工件面 {float(surface_z_m):.4f}，"
f"间隙 {clear_m * 1000:.1f}mm，要求 ≥ {float(want_clearance_m) * 1000:.1f}mm。"
"这个值是启动参数指定的，不会改成别的高度。"
)
#: S05 精对位时判"工件被工具推走"的两级阈值 [m]。
#
# 为什么需要这条判据（2026-09-24 又栽了一次，见下面 drag_verdict 的说明）：
src/arm_application/arm_application/overhead_oneshot.py
+3 -3
View File
@@ -152,9 +152,9 @@ def d405_commands_arm(
) -> bool:
"""D405 这一帧的点能不能下发给左臂。
推理一直开着。正在用 D435 那一个点时，点到并且停稳之前不下发。
停稳沿用一阶段的 ``StillLock``（site 里的 ``lock_still_sec`` /
``lock_still_move_m``），不另写一套计时。
推理一直开着。正在用 D435 那一个点时，杀栈 MoveL 冷启动结束并且停稳之前不下发。
停稳从控制栈回来之后才计，用 ``d435_handback_sec`` 和
``lock_still_move_m``。赶路时 XY 已经进 15 mm 也不算到位。
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
"""
if move_only:
src/arm_application/arm_application/perception_node.py
+94 -28
View File
@@ -77,6 +77,7 @@ from arm_application.ros_node import TolerantNode
# D435 一次性推理用的 seq。必须远离腕部帧序号，结果绝不能当 D405 检测去立体匹配。
_ONESHOT_SEQ = 2_000_000_000
_ONESHOT_VIZ_SEQ = 2_000_000_001
def _bgr_copy(msg: Image) -> np.ndarray:
@@ -245,6 +246,8 @@ class PerceptionNode(TolerantNode):
self._z_locked = False
self._oneshot_img: Optional[Image] = None
self._oneshot_depth: Optional[Image] = None
self._oneshot_viz_only = False
self._oh_viz_t = -1.0e9
if self.overhead_oneshot_enable:
self.wrist_handoff_conf = float(
self.get_parameter("wrist_handoff_conf").value
@@ -499,6 +502,10 @@ class PerceptionNode(TolerantNode):
reliability=ReliabilityPolicy.RELIABLE,
)
self._det_pub = self.create_publisher(Float32MultiArray, "workpiece/detection", det_qos)
# 顶视框单独话题：像素是 D435 的，不能叠到 D405 窗上。
self._oh_det_pub = self.create_publisher(
Float32MultiArray, "workpiece/detection_overhead", det_qos
)
self._tf_br = TransformBroadcaster(self)
vision_hz = max(5.0, min(40.0, float(self.get_parameter("vision_hz").value)))
@@ -601,6 +608,7 @@ class PerceptionNode(TolerantNode):
self.detector.restart()
self._submitted_seq = -1
self._oneshot_inflight = False
self._oneshot_viz_only = False
self._last_gpu_t = time.monotonic()
self.get_logger().info("YOLO 工作进程已重启")
except Exception as exc: # noqa: BLE001
@@ -653,12 +661,17 @@ class PerceptionNode(TolerantNode):
continue
if int(seq) >= _ONESHOT_SEQ:
try:
self._finish_overhead_oneshot(dets, float(self._infer_meta.get(int(seq), 1.0)))
self._finish_overhead_oneshot(
dets,
float(self._infer_meta.get(int(seq), 1.0)),
viz_only=bool(int(seq) == _ONESHOT_VIZ_SEQ),
)
except Exception as exc: # noqa: BLE001
self.get_logger().error(
f"D435 一次性结果处理失败：{exc}", throttle_duration_sec=2.0
)
self._oneshot_inflight = False
self._oneshot_viz_only = False
self._oneshot_t = time.monotonic()
continue
scale = float(self._infer_meta.get(int(seq), 1.0))
@@ -670,7 +683,9 @@ class PerceptionNode(TolerantNode):
self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0)
self._done_seq = int(seq)
if self._need_oneshot and not self._oneshot_inflight:
self._submit_overhead_oneshot()
self._submit_overhead_oneshot(viz_only=False)
elif self._should_submit_overhead_viz() and not self._oneshot_inflight:
self._submit_overhead_oneshot(viz_only=True)
else:
self._submit_latest()
dt_ms = (time.monotonic() - t0) * 1000.0
@@ -1430,68 +1445,105 @@ class PerceptionNode(TolerantNode):
self._overhead_k0_pub.publish(pt)
del is_k0 # 语义写在日志里；话题保持 PointStamped，避免再抄一份类型
def _submit_overhead_oneshot(self) -> None:
def _should_submit_overhead_viz(self) -> bool:
"""低频率给 D435 窗刷绿框；不发控制点。赶路时不抢 GPU。"""
if not self.overhead_oneshot_enable or self.d435_move_only:
return False
if self._d435_commanding or self._need_oneshot:
return False
return (time.monotonic() - self._oh_viz_t) >= 0.5
def _publish_overhead_det(
self, xyxy: np.ndarray, conf: float, cls_id: float, uv: np.ndarray, z: float
) -> None:
det = Float32MultiArray()
det.data = [
float(xyxy[0]), float(xyxy[1]), float(xyxy[2]), float(xyxy[3]),
float(conf), float(cls_id),
float(uv[0]), float(uv[1]), float(z),
]
self._oh_det_pub.publish(det)
def _publish_overhead_det_clear(self) -> None:
msg = Float32MultiArray()
msg.data = []
self._oh_det_pub.publish(msg)
def _submit_overhead_oneshot(self, *, viz_only: bool = False) -> 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:
if (not viz_only) and 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,
)
if not viz_only:
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,
)
if not viz_only:
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:
if (not viz_only) and (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:
if (not viz_only) and self._oh_fx <= 1.0:
self.get_logger().warn(
"D435 内参还没到（camera_info）—— 不反投影",
throttle_duration_sec=1.0,
)
return
if self._T_oh is None:
if (not viz_only) and self._T_oh is None:
self.get_logger().error("眼在手外 T 没加载 —— 一次性定位拒绝")
return
seq = _ONESHOT_VIZ_SEQ if viz_only else _ONESHOT_SEQ
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):
self._infer_meta[seq] = scale
if not self.detector.submit(small, seq):
return
except Exception as exc: # noqa: BLE001
self.get_logger().warn(
f"D435 一次性提交失败：{exc}", throttle_duration_sec=2.0
f"D435 {'预览' if viz_only else '一次性'}提交失败：{exc}",
throttle_duration_sec=2.0,
)
return
self._oneshot_inflight = True
self._oneshot_viz_only = bool(viz_only)
self._oneshot_img = img
self._oneshot_depth = depth
self.get_logger().info("D435 一次性已提交（不打断 D405 队列）")
if viz_only:
self._oh_viz_t = now
else:
self.get_logger().info("D435 一次性已提交（不打断 D405 队列）")
def _finish_overhead_oneshot(self, dets, scale: float) -> None:
"""YOLO 返回的顶视结果 → 基座系 K0。失败不发假点。"""
def _finish_overhead_oneshot(
self, dets, scale: float, *, viz_only: bool = False
) -> None:
"""YOLO 返回的顶视结果。viz_only 只刷绿框；控制路径才发基座点。"""
self._oneshot_inflight = False
viz_only = bool(viz_only or self._oneshot_viz_only)
self._oneshot_viz_only = False
now = time.monotonic()
self._oneshot_t = now
if not viz_only:
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:
if dets is INFER_BUSY or dets is None or img is None:
return
if scale != 1.0:
for d in dets:
@@ -1499,13 +1551,27 @@ class PerceptionNode(TolerantNode):
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)
det = select_oneshot_det(dets, self.overhead_oneshot_conf if not viz_only else 0.25)
if det is None:
self._publish_overhead_det_clear()
if not viz_only:
self._oneshot_t = now
self.get_logger().warn(
f"D435 一次性：画面里没有 conf≥{self.overhead_oneshot_conf:.2f} 的工件",
throttle_duration_sec=1.0,
)
return
xyxy = np.asarray(det.xyxy, dtype=float).reshape(4)
uv = np.array(
[0.5 * (xyxy[0] + xyxy[2]), 0.5 * (xyxy[1] + xyxy[3])], dtype=float
)
self._publish_overhead_det(
xyxy, float(det.conf), float(det.cls_id), uv, 0.0
)
if viz_only:
return
if depth 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 "")
src/arm_application/arm_application/pipeline_node.py
+71 -12
View File
@@ -40,6 +40,36 @@ TRACK_CLEARANCE_M = 0.05
TRACK_LOCK_GAP_M = 0.040
def init_z_refusal(
z_m: float,
tool_len_m: float,
surface_z_m: float,
gap_m: float,
z_min: float,
z_max: float,
) -> str:
"""初始法兰高度能不能用。空串表示可以；非空就是拒绝原因，不许悄悄抬高。"""
z = float(z_m)
if z < float(z_min) - 1e-6 or z > float(z_max) + 1e-6:
return (
f"初始法兰 Z {z:.3f}m 超出工作盒 [{float(z_min):.3f}, {float(z_max):.3f}]，"
"拒绝开跑"
)
tool = float(tool_len_m)
surf = float(surface_z_m)
if tool <= 0.0 or surf <= 0.0:
return "写了初始高度，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
clear = z - tool - surf
if clear < float(gap_m) - 0.001:
return (
f"初始法兰 Z {z:.3f}m 会让工具尖进入工件："
f"尖端 {z - tool:.3f}，工件面 {surf:.3f}，"
f"间隙 {clear * 1000:.1f}mm，要求 ≥ {float(gap_m) * 1000:.1f}mm。"
"拒绝开跑，不会悄悄抬高。"
)
return ""
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
"""一阶段锁高。
@@ -97,6 +127,7 @@ class AssemblyPipeline(Stage3Search):
self.declare_parameter("skip_stage2", False)
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
self.declare_parameter("d435_move_only", False)
self.declare_parameter("init_z_m", -1.0) # >0：启动后先到这个法兰高度
self.declare_parameter("stage1_timeout_sec", 150.0)
self.linger3 = 1.2 # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
@@ -183,37 +214,64 @@ class AssemblyPipeline(Stage3Search):
self._kill_old_stage1()
def _track_z(self) -> float:
"""一阶段锁高：见 :func:`track_lock_z`。对孔完成前不再改这个高度。"""
"""一阶段锁高。写了 ``init_z_m`` 就用它，否则见 :func:`track_lock_z`。"""
z = float(self.get_parameter("init_z_m").value)
if z > 0.0:
return z
return track_lock_z(
float(self.hover_z),
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
float(getattr(self, "surface_z_m", 0.0) or 0.0),
)
def _ensure_clearance(self) -> None:
"""上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
def _ensure_clearance(self) -> bool:
"""没写初始高度：只在低于锁高时抬上去。写了：升或降到那个法兰 Z，不安全就拒绝。"""
user = float(self.get_parameter("init_z_m").value) > 0.0
q, Tee = self._wait_state(4.0)
if q is None or Tee is None:
return
if user:
self.get_logger().error("设了 --init-z，但拿不到当前位姿，拒绝开跑")
return False
return True
tz = self._track_z()
if user:
why = init_z_refusal(
tz,
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
float(getattr(self, "surface_z_m", 0.0) or 0.0),
float(self.final_gap_m),
float(self.ws_min[2]),
float(self.ws_max[2]),
)
if why:
self.get_logger().error(why)
return False
z = float(Tee[2, 3])
if z >= tz - 0.008:
return
if user:
if abs(z - tz) <= 0.008:
self.get_logger().info(f"已在初始高度 Z={z:.3f}m（目标 {tz:.3f}）")
return True
elif z >= tz - 0.008:
return True
self._state("pre_lift")
self.get_logger().info(
f"先到锁高：Z {z:.3f} → {tz:.3f}m（跟踪与对孔都停在这个高度，对完孔再下降）"
)
_, q, Tee = self._goto(
self.get_logger().info(f"先到初始高度：Z {z:.3f} → {tz:.3f}m")
ok, q, Tee = self._goto(
q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0,
tag="pre_lift", v=self.move_speed, max_hops=8, hold_xy=True, settle_s=0.25,
)
q2 = self._q()
self._pin(q2 if q2 is not None else q)
z2 = float(Tee[2, 3])
if user and (not ok or abs(z2 - tz) > 0.008):
self.get_logger().error(f"初始高度没到位：Z={z2:.3f}，目标 {tz:.3f}")
return False
return True
def _run_stage1(self) -> int:
"""拉起 perception+servo，等到 /visual_servo/locked 后冻结并拆跟踪。"""
self._ensure_clearance()
if not self._ensure_clearance():
return 2
self._state("stage1_track")
self._kill_old_stage1()
self._locked = False
@@ -289,7 +347,8 @@ class AssemblyPipeline(Stage3Search):
if code != 0:
return code
else:
self._ensure_clearance()
if not self._ensure_clearance():
return 2
self._state("skip_stage1")
q, Tee = self._wait_state(10.0)
src/arm_application/arm_application/realsense_view.py
+10 -1
View File
@@ -63,11 +63,16 @@ class RealsenseView(TolerantNode):
self.declare_parameter('camera_name', 'd405')
self.declare_parameter('overlay_hold_sec', 0.10)
self.declare_parameter('overlay_detection', True)
# D405 用 workpiece/detection；D435 用 workpiece/detection_overhead（像素坐标系不同）。
self.declare_parameter('detection_topic', 'workpiece/detection')
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._det_topic = str(self.get_parameter('detection_topic').value or '').strip() or (
'workpiece/detection'
)
self._hud = str(self.get_parameter('hud_label').value or '').strip()
self._color: Optional[Image] = None
@@ -106,7 +111,7 @@ class RealsenseView(TolerantNode):
)
if self._overlay_detection:
self.create_subscription(
Float32MultiArray, 'workpiece/detection', self._on_det, det_qos
Float32MultiArray, self._det_topic, self._on_det, det_qos
)
# stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示
self.create_subscription(
@@ -128,6 +133,7 @@ class RealsenseView(TolerantNode):
self.create_timer(1.0 / hz, self._push)
self.get_logger().info(
f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 '
f'检测={self._det_topic if self._overlay_detection else "关"} '
f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。'
)
@@ -190,6 +196,9 @@ class RealsenseView(TolerantNode):
self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
return
if "overlay_hold_sec" in cam:
# 顶视检测频率低，保留 launch 里设的较长 hold，别被 site 的 0.1s 冲掉。
if "overhead" in self._det_topic:
return
hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
if abs(hold - self._hold) > 1e-6:
self.get_logger().info(
src/arm_application/arm_application/servo_node.py
+149 -27
View File
@@ -216,8 +216,10 @@ class ServoNode(TolerantNode):
# 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True
self.declare_parameter("lock_hold_sec", 2.0)
# D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
# lock_still_sec 只用于 D435 到位后的停稳；框心对准进对孔用 wrist_align_sec。
# lock_still_sec 仍须从 site 接上（缺了就报错）。
# 交回 D405：MoveL 结束后杀栈冷启动，再叠 d435_handback_sec（现场 3 s）。
self.declare_parameter("lock_still_sec", -1.0)
self.declare_parameter("d435_handback_sec", -1.0)
self.declare_parameter("wrist_align_sec", -1.0)
self.declare_parameter("lock_still_move_m", -1.0)
self.declare_parameter("wrist_handoff_conf", -1.0)
@@ -305,6 +307,7 @@ class ServoNode(TolerantNode):
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.d435_handback_sec = float(self.get_parameter("d435_handback_sec").value)
self.wrist_align_sec = float(self.get_parameter("wrist_align_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)
@@ -312,21 +315,24 @@ class ServoNode(TolerantNode):
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
self._hybrid = (
self.lock_still_sec >= 0.0
and self.d435_handback_sec >= 0.0
and self.wrist_align_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
)
# 框心对准 1s 进对孔。D435 停稳仍用 lock_still_sec，两段不要合成一个数。
# 框心对准进对孔用 wrist_align_sec。D435 交回只用 d435_handback_sec，两段不要合成一个数。
self._still = StillLock(
still_sec=max(0.0, self.wrist_align_sec),
still_move_m=max(0.0, self.lock_still_move_m),
)
self._arrive = StillLock(
still_sec=max(0.0, self.lock_still_sec),
still_sec=max(0.0, self.d435_handback_sec),
still_move_m=max(0.0, self.lock_still_move_m),
)
self._sw_cli = None
self._d435_vendor_done = False
self._d435_vendor_busy = False
self._d435_vendor_failed = False
self._d435_settled = False
self._d435_exec_until = 0.0
@@ -377,8 +383,8 @@ class ServoNode(TolerantNode):
)
if self.handeye_mode == "eye_in_hand" and not self._hybrid:
raise RuntimeError(
"腕部主跟踪必须从 site.yaml 接入 lock_still_sec / wrist_align_sec / "
"lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
"腕部主跟踪必须从 site.yaml 接入 lock_still_sec / d435_handback_sec / "
"wrist_align_sec / lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
"收到哨兵 -1 = 配置没接线，不许用代码默认秒数假装已经静止判据。"
)
R_he = np.asarray(self.T_cam_to_gripper[:3, :3], dtype=float)
@@ -608,6 +614,13 @@ class ServoNode(TolerantNode):
"""D435 一次性：点已经在基座系。再乘腕部 T 会差几十厘米还看起来合理。"""
if not self._hybrid:
return
if not self.enable_control:
# 仅观察时不占臂：占着 d435_commanding 会把感知的预览框也卡死。
self.get_logger().info(
"仅观察：收到 D435 点，不占臂、不挡 D405 预览",
throttle_duration_sec=2.0,
)
return
fid = str(msg.header.frame_id or "").strip()
if fid and self.base_frame and fid != self.base_frame:
self.get_logger().error(
@@ -649,6 +662,9 @@ class ServoNode(TolerantNode):
self._target_lost_flag = False
self._track_source = "overhead"
self._d435_settled = False
self._d435_vendor_done = False
self._d435_vendor_busy = False
self._d435_vendor_failed = False
self._overhead_t = now_m
self._last_seen_time = now_m
self._stamp_sec = now_sec
@@ -670,6 +686,7 @@ class ServoNode(TolerantNode):
self._d435_full_done = False
self._d435_fix_done = False
self._d435_vendor_done = False
self._d435_vendor_busy = False
self._d435_vendor_failed = False
self._publish_d435_commanding(True)
self.get_logger().info(
@@ -690,9 +707,11 @@ class ServoNode(TolerantNode):
with self._lock:
using_d435 = self._track_source == "overhead"
settled = self._d435_settled
# 到位必须是 MoveL 已经结束，不能把「还在赶路、XY 进了 15mm」当成到位。
# 停稳计时也不许在这段路上累计，否则 D405 会在 D435 没走完时就开始带臂晃。
if self._hybrid and not d405_commands_arm(
using_d435=using_d435,
arrived=settled,
arrived=(self._d435_vendor_done and not self._d435_vendor_busy),
settled=settled,
move_only=self.d435_move_only,
):
@@ -904,6 +923,15 @@ class ServoNode(TolerantNode):
self._tick_still_lock()
if self.enable_control and self._d435_oneshot_holds_the_arm():
return
if (
self.enable_control
and self._hybrid
and self._locked
and not self.d435_move_only
):
# 框心已经对准并停稳。实时预测到此为止，臂钉住，等二阶段精拍一次。
self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True)
return
# 还没有目标、人又停在锁高下面：只抬一次 Z。已经有 D435 点或腕部点之后
# 不再插这一步 —— 平移途中改高度会抖，也会把正在走的 XY 掐掉。
@@ -1443,8 +1471,83 @@ class ServoNode(TolerantNode):
if not ok:
self._d435_give_up = True
def _switch_arm_controller(self, activate: bool) -> bool:
"""停或重新打开关节轨迹控制器。关节状态广播留着，硬件接口不下线。
用节点里的客户端，不走 ``ros2 service call``：那条命令自己就要一两秒，
交回 D405 的预算会被它吃掉。
"""
from controller_manager_msgs.srv import SwitchController
if self._sw_cli is None:
self._sw_cli = self.create_client(
SwitchController,
"/controller_manager/switch_controller",
callback_group=ReentrantCallbackGroup(),
)
t0 = time.monotonic()
while not self._sw_cli.service_is_ready():
if time.monotonic() - t0 > 3.0:
self.get_logger().error("switch_controller 服务不在")
return False
time.sleep(0.02)
req = SwitchController.Request()
if activate:
req.activate_controllers = ["rizon_arm_controller"]
req.deactivate_controllers = []
else:
req.activate_controllers = []
req.deactivate_controllers = ["rizon_arm_controller"]
req.strictness = SwitchController.Request.STRICT
req.activate_asap = True
req.timeout.sec = 3
req.timeout.nanosec = 0
fut = self._sw_cli.call_async(req)
while not fut.done():
if time.monotonic() - t0 > 8.0:
self.get_logger().error(
"switch_controller 超时，"
+ ("没能重新激活" if activate else "没能停掉")
+ " rizon_arm_controller"
)
return False
time.sleep(0.02)
try:
resp = fut.result()
except Exception as exc:
self.get_logger().error(f"switch_controller 失败：{exc}")
return False
if resp is None or not bool(resp.ok):
self.get_logger().error(
"switch_controller 返回失败，"
+ ("控制器没回来" if activate else "控制器还占着关节")
)
return False
return True
def _wait_movel_file(self, seq: int, timeout_s: float, want_done: bool):
lib = ros2_root() / "scripts" / "lib"
if str(lib) not in sys.path:
sys.path.insert(0, str(lib))
from flexiv_movel_ipc import read_ack, read_done
t0 = time.monotonic()
while time.monotonic() - t0 < timeout_s:
if want_done:
got = read_done()
if got is not None and int(got[0]) == int(seq):
return bool(got[1]), str(got[2])
elif read_ack() == int(seq):
return True, ""
time.sleep(0.02)
return False, ""
def _d435_vendor_movel(self) -> bool:
"""把 D435 这一点交给厂家 MoveL。推理不关，只是这期间 D405 的点不下发。"""
"""D435 这一点：停真机栈 → 厂家 MoveL → 冷启动控制栈。
流畅度先不优化。杀栈再拉起大约多几秒，但 RDK 独占，D405 的点
在整段 busy 期间都不会下发。停稳计时从冷启动回来之后才开始。
"""
with self._lock:
goal = None if self._p_feat_base is None else np.asarray(
self._p_feat_base, dtype=float
@@ -1452,6 +1555,7 @@ class ServoNode(TolerantNode):
if goal is None or not np.all(np.isfinite(goal)):
self.get_logger().error("没有 D435 基座点，厂家 MoveL 不下发")
return False
arm = str(os.environ.get("SITE_ROBOT_ARM") or "89").strip() or "89"
script = ros2_root() / "scripts" / "rdk_release_movel.py"
if not script.is_file():
self.get_logger().error(f"找不到 {script}")
@@ -1460,36 +1564,46 @@ class ServoNode(TolerantNode):
sys.executable,
str(script),
"--xyz",
f"{float(goal[0])}",
f"{float(goal[1])}",
f"{float(goal[2])}",
f"{float(goal[0]):.6f}",
f"{float(goal[1]):.6f}",
f"{float(goal[2]):.6f}",
"--arm",
arm,
"--vel",
"0.05",
]
self.get_logger().info(
f"D435 改走厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
"—— 先放开 ros2_control 的 RDK，YOLO 不停，工具尖高度保持当前"
f"D435 厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
"—— 先停控制栈，MoveL 后再冷启动；期间不下发 D405"
)
try:
got = subprocess.run(cmd, check=False, timeout=180)
except subprocess.TimeoutExpired:
self.get_logger().error("厂家 MoveL 超过 180s 还没回来")
return False
if got.returncode != 0:
self.get_logger().error(f"厂家 MoveL 失败，退出码 {got.returncode}")
return False
return True
t0 = time.monotonic()
got = subprocess.run(cmd, check=False)
dt = time.monotonic() - t0
ok = got.returncode == 0
self.get_logger().info(
f"rdk_release_movel {'完成' if ok else '失败'}，耗时 {dt:.2f}s"
+ (f" 退出码 {got.returncode}" if not ok else "")
)
return bool(ok)
def _d435_oneshot_holds_locked(self) -> bool:
# 混合流程这一段只走厂家 MoveL。下面留下的关节折线不再执行：
# 那条路径会在真机上一段一段启停。
if self._d435_vendor_failed:
if self._d435_vendor_failed or self._d435_vendor_busy:
return True
if not self._d435_vendor_done:
self._d435_vendor_done = True
if not self._d435_vendor_movel():
# busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前交回 D405。
self._d435_vendor_busy = True
ok = self._d435_vendor_movel()
self._d435_vendor_busy = False
if not ok:
self._d435_vendor_failed = True
self.get_logger().error(
"厂家 MoveL 没走成 —— 停在原地，D405 的点继续不下发"
)
else:
self._d435_vendor_done = True
self._arrive.reset()
return True
return True
@@ -1658,7 +1772,12 @@ class ServoNode(TolerantNode):
"""走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
if not self._hybrid:
return
if self._d435_inflight or time.monotonic() < self._d435_settle_until:
if (
self._d435_inflight
or self._d435_vendor_busy
or not self._d435_vendor_done
or time.monotonic() < self._d435_settle_until
):
self._arrive.reset()
return
with self._lock:
@@ -1690,7 +1809,7 @@ class ServoNode(TolerantNode):
self._locked = True
self.get_logger().info(
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
f"{self.lock_still_sec:.0f}s —— D435 位移测试结束，"
f"{self.d435_handback_sec:.1f}s —— D435 位移测试结束，"
"不交 D405、不进二阶段"
)
self._lock_pub.publish(Bool(data=True))
@@ -1702,7 +1821,7 @@ class ServoNode(TolerantNode):
self._publish_d435_commanding(False)
self.get_logger().info(
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
f"{self.lock_still_sec:.0f}s —— 改回下发 D405 框心"
f"{self.d435_handback_sec:.1f}s —— 改回下发 D405 框心"
)
def _tick_still_lock(self) -> None:
@@ -1712,6 +1831,9 @@ class ServoNode(TolerantNode):
"""
if not self._hybrid or self.d435_move_only:
return
if self._locked:
# 框心对准并且停稳这一拍已经抓住。后面的实时点不再解开，也不再带臂。
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:
src/arm_application/arm_application/stage2_locate_node.py
+88 -18
View File
@@ -54,12 +54,26 @@ from arm_application.path_profile import (
from arm_application.overhead_oneshot import pixel_on_base_plane
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, wrench_all_zero
from arm_application.gauge_fsm import drag_verdict, resolve_hover_z, 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
from arm_application.ros_node import TolerantNode
def contact_armed_gap_m(observe_gap_m: float) -> float:
"""轻降力接触门槛对应的几何间隙。离面更远时腕部力会漂几牛，不能当碰到。
2026-10-13：悬停 0.42 往下到法兰 0.384 时 ΔFz=+2.87N 超过 2.5N，
几何间隙仍约 138mm。立刻停再交给三阶段抬到 hover+50mm，插孔没做。
"""
return max(float(observe_gap_m), 0.020) + 0.005
def contact_armed_by_gap(gap_m: float, observe_gap_m: float) -> bool:
"""当前几何间隙够近，才许把腕部力突变当成碰到工件。"""
return float(gap_m) <= contact_armed_gap_m(observe_gap_m)
# 一次 hop 之后实测关节变化小于这个值，就认为「臂根本没跟随指令」。
#
# 真机踩到过（2026-09-28）：一次 hover hop 计划要动 `max|Δq|=0.17378rad`，实测
@@ -164,8 +178,10 @@ class Stage2Locate(TolerantNode):
# 悬停锁高：launch 覆盖 > robots[].servo.hole_hover_z_m > stage2.hover_z_m
hover_override = float(self.get_parameter("hover_z_m").value)
self.hover_z_user_set = hover_override > 0.0
self._hover_refused = ""
self.hover_z = (
hover_override if hover_override > 0.0
hover_override if self.hover_z_user_set
else float(servo.get("hole_hover_z_m") or stage2.get("hover_z_m") or 0.30)
)
self.min_ee_z = float(servo.get("min_ee_z_m") or 0.20)
@@ -234,13 +250,27 @@ class Stage2Locate(TolerantNode):
self.surface_z_m = _surface_z
# 对小孔只用关键点像素和已知工件面，不读深度图。缺了就按旧的深度路径。
self.hole_visual_only = _as_bool(stage2.get("hole_visual_only"), False)
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}"
if self.hover_z_user_set and (_tool_len <= 0.0 or _surface_z <= 0.0):
self._hover_refused = (
"写了 --hover-z，但工具长度或工件面未知，无法确认尖端在面之上，拒绝执行"
)
self.get_logger().error(self._hover_refused)
elif _tool_len > 0.0:
_used, _refused = resolve_hover_z(
self.hover_z, _surface_z, _tool_len, self.final_gap_m, self.hover_z_user_set
)
if _refused:
self._hover_refused = _refused
self.get_logger().error(
"启动参数指定的悬停高度不安全，拒绝执行，不会改成别的高度：\n "
+ _refused
)
self.hover_z = _fixed
elif abs(_used - self.hover_z) > 1e-9:
self.get_logger().warn(
f"二阶段悬停高度自检不通过 —— 用几何修正值 {_used:.4f}m"
"（这次没写 --hover-z，沿用 site 的几何抬高）"
)
self.hover_z = _used
# 轻降**最多能降多少**：从悬停高度降到"规尖停在工件面上方 final_gap"为止。
# 这是纯几何量，与深度测量无关 —— 测量的唯一职责是让下降**提前**收工，
# 绝不允许它把下降量**放大**（2026-09-29 实测就是这么撞的：测量出的
@@ -1674,8 +1704,14 @@ class Stage2Locate(TolerantNode):
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):
# 力觉兜底：规尖一碰工件 force.z 突变，立即停并回退 1mm。
# 离面还远时力会漂几牛，不能当碰到。
gap_now = float(Tee[2, 3]) - tip_len - surf
if (
base_fz is not None
and contact_armed_by_gap(gap_now, self.observe_gap_m)
and self._contact_detected(base_fz)
):
self.get_logger().warn("腕部力突变（规尖触到工件），立即停止并回退 1mm（安全优先）")
p = Tee[:3, 3].copy()
ok, q, Tee = self._goto(
@@ -1869,12 +1905,14 @@ class Stage2Locate(TolerantNode):
med_depth = (
np.median(np.stack(depths), axis=0).astype(np.float32) if len(depths) > 1 else depths[0]
)
# 对孔只认像素关键点。把深度图传进去，嵌件对会用深度换像素半径，等于又用上了深度。
det_depth = None if self.hole_visual_only else med_depth
res = self._detect_holes_img(med_bgr, skip_center_gate=skip_center_gate,
depth_m=med_depth, intr=last_intr)
depth_m=det_depth, intr=last_intr)
if not res.get("status"):
for bgr in frames:
r2 = self._detect_holes_img(bgr, skip_center_gate=skip_center_gate,
depth_m=med_depth, intr=last_intr)
depth_m=det_depth, intr=last_intr)
if r2.get("status"):
res = r2
med_bgr = bgr
@@ -1901,7 +1939,12 @@ class Stage2Locate(TolerantNode):
res["snap_frames"] = int(len(frames))
# 把"三维是哪来的"也打出来：这一项错了（真值被当成深度、或反过来）
# 会让孔距从 58mm 变成 169mm，而现象看起来只是"检测不认"。
_src = "真值" if res.get("hole_base") and self._truth_enabled() else "深度反投影"
if self._truth_enabled() and res.get("hole_base"):
_src = "真值"
elif self.hole_visual_only:
_src = "像素交工件面"
else:
_src = "深度反投影"
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})"
@@ -1955,7 +1998,19 @@ class Stage2Locate(TolerantNode):
self.get_logger().error("JTC action 不在 —— 控制器没起来（T1 run_real.sh --arm 89）")
return 2
if not (self.min_ee_z - 1e-6) <= self.hover_z <= (self.safe_z + 0.05):
if self._hover_refused:
self.get_logger().error(self._hover_refused)
return 2
if self.hover_z_user_set:
z_lo = float(self.ws_min[2])
z_hi = float(self.ws_max[2])
if not (z_lo - 1e-6) <= self.hover_z <= (z_hi + 1e-6):
self.get_logger().error(
f"--hover-z {self.hover_z:.3f} 超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝执行"
)
return 2
elif not (self.min_ee_z - 1e-6) <= self.hover_z <= (self.safe_z + 0.05):
self.get_logger().error(
f"hole_hover_z_m={self.hover_z:.3f} 越界 [{self.min_ee_z:.2f}, {self.safe_z + 0.05:.2f}]，拒绝执行"
)
@@ -2000,7 +2055,11 @@ class Stage2Locate(TolerantNode):
max_hops=12, hold_xy=True, settle_s=0.30,
)
self._pin(q)
z_now = float(Tee[:3, 3][2])
self._spin(0.2)
Tee_now = self._lookup(self.base_frame, self.ee_frame)
if Tee_now is not None:
Tee = Tee_now
z_now = float(Tee[2, 3])
_, ang2 = self._w_down(Tee[:3, :3])
z_err = abs(z_now - self.hover_z)
self.get_logger().info(
@@ -2017,12 +2076,21 @@ class Stage2Locate(TolerantNode):
f"悬停闭环未完全收敛，但 Z 差 {z_err*1000:.1f}mm 可接受，继续拍照定位"
)
else:
z_now = float(p0[2])
self._state("hover")
self._pin(q)
self._spin(0.2)
Tee_now = self._lookup(self.base_frame, self.ee_frame)
if Tee_now is not None:
Tee = Tee_now
z_now = float(Tee[2, 3])
if abs(z_now - self.hover_z) > 0.004:
self.get_logger().error(
f"悬停未达容差（Z={z_now:.3f}，目标 {self.hover_z:.3f}），终止"
)
return 2, q, Tee, {}
self.get_logger().info(
f"已在悬停高 Z={z_now:.3f}m（目标 {self.hover_z:.3f}），跳过升降"
)
self._pin(q)
if self.hover_only:
self._state("hover_only_hold")
@@ -2205,7 +2273,9 @@ class Stage2Locate(TolerantNode):
if proj is None:
proj = self._project_holes(depth_m, pts, radii, bbox_l, intr)
if proj is None:
self.get_logger().error("孔深度/手眼投影失败")
self.get_logger().error(
"孔心像素没交到工件面" if self.hole_visual_only else "孔深度/手眼投影失败"
)
if it >= self.align_max_iters:
return 4, q, Tee, {}
continue
src/arm_application/arm_application/stage3_search_node.py
+25 -1
View File
@@ -104,6 +104,18 @@ def tactile_timeout_reason(pairs: Sequence[Any], timeout_s: float) -> str:
)
return f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）"
def should_lift_air_baseline(
z_now_m: float, z_air_m: float, *, tactile_live: bool
) -> bool:
"""空载抬升只在触觉已经在发、并且当前低于空载高度时做。
触觉没数据时先抬到 hover+50mm 再报断流，等于插孔做到一半把臂送回锁高。
"""
if not tactile_live:
return False
return float(z_now_m) < float(z_air_m) - 0.008
def spiral_points(
cx: float, cy: float, pitch_mm: float, step_mm: float, r_max_mm: float
) -> List[Tuple[float, float]]:
@@ -581,9 +593,21 @@ class Stage3Search(Stage2Locate):
result: Dict[str, Any] = {"ok": False, "stage": "stage3", "target_hole": self.target_hole}
try:
# 基线必须悬空测。二阶段结束在 hover≈触面+1mm，带载会把阈值抬飞。
# 触觉没在发就原地中止，不许先抬到锁高再报断流。
z_now = float(Tee[2, 3])
z_air = float(self.hover_z) + 0.05
if z_now < z_air - 0.008:
self._spin(0.05)
tac_live = bool(
live_tactile(time.monotonic(), (self._tac_l, self._tac_r))
)
if not tac_live:
raise SearchAbort(
tactile_timeout_reason(
(self._tac_l, self._tac_r), float(self.tac_timeout)
),
code=7,
)
if should_lift_air_baseline(z_now, z_air, tactile_live=True):
self._state("air_baseline")
self.get_logger().info(
f"抬到 {z_air:.3f}m 测触觉空载基线（当前 Z={z_now:.3f}）"
src/arm_application/launch/pipeline.launch.py
+6
View File
@@ -82,6 +82,8 @@ def _launch_setup(context, *args, **kwargs):
# 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
init_z = LaunchConfiguration("init_z_m").perform(context).strip()
hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -121,6 +123,8 @@ def _launch_setup(context, *args, **kwargs):
"skip_stage2": skip2 or d435_only,
"skip_stage3": skip3 or d435_only,
"d435_move_only": d435_only,
"init_z_m": float(init_z) if init_z else -1.0,
"hover_z_m": float(hover_z) if hover_z else -1.0,
# -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
"final_descent_enable": 0 if no_descent else -1,
}
@@ -160,6 +164,8 @@ def generate_launch_description() -> LaunchDescription:
# 只验证精对准、不轻降（见 _launch_setup 里的说明）
DeclareLaunchArgument("no_descent", default_value="false"),
DeclareLaunchArgument("d435_move_only", default_value="false"),
DeclareLaunchArgument("init_z_m", default_value=""),
DeclareLaunchArgument("hover_z_m", default_value=""),
DeclareLaunchArgument("preview", default_value="true"),
OpaqueFunction(function=_launch_setup),
]
src/arm_application/test/test_gauge_fsm.py
+16
View File
@@ -43,6 +43,7 @@ from arm_application.gauge_fsm import (
points_stable,
hover_z_guard,
hover_z_for_tool_clearance,
resolve_hover_z,
ratchet_cycles,
resolve_next,
rotation_verdict,
@@ -785,6 +786,21 @@ def test_hover_guard_uses_want_clearance_when_configuring():
assert hover_z_for_tool_clearance(0.412, 0.245, 0.005) == pytest.approx(fixed)
def test_user_hover_is_refused_instead_of_rewritten():
"""启动参数写了悬停高：不安全就拒绝，不许改成另一个数再往下走。"""
used, why = resolve_hover_z(0.632, 0.412, 0.245, 0.002, True)
assert used == pytest.approx(0.632)
assert "不会改成别的高度" in why
# 没写启动参数：仍按几何抬高，不当成拒绝。
used_site, why_site = resolve_hover_z(0.632, 0.412, 0.245, 0.002, False)
assert why_site == ""
assert used_site == pytest.approx(0.412 + 0.245 + 0.002)
# 写了且本身安全：原样用。
used_ok, why_ok = resolve_hover_z(0.700, 0.412, 0.245, 0.002, True)
assert why_ok == ""
assert used_ok == pytest.approx(0.700)
def test_hover_guard_tolerates_millimetre_level_shortfall():
"""夹在目标间隙附近（差不到 1mm）不算错：工件面 z 本身就有几毫米不确定度。"""
assert hover_z_guard(0.6580, 0.412, 0.245, 0.002)[1] == "" # 差 1.0mm 之内
src/arm_application/test/test_overhead_oneshot.py
+38 -1
View File
@@ -151,7 +151,7 @@ class TestHandoff:
assert "d435_commanding" in perc
def test_after_d435_wrist_tracks_box_center_then_hole_without_depth(self):
"""交回 D405 追框心；对准 1s 后对孔只用像素交到工件面。"""
"""交回 D405 追框心；二阶段多拍 k1/k2 闭环，孔坐标只交到工件面。"""
perc = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "perception_node.py"
@@ -164,12 +164,49 @@ class TestHandoff:
).read_text(encoding="utf-8")
assert "pixel_on_base_plane(" in stage2
assert "hole_visual_only" in stage2
assert "_trim_xy_after_descent" in stage2
assert "align_max_iters" in stage2
assert "像素交工件面" in stage2
assert "det_depth = None if self.hole_visual_only else med_depth" in stage2
assert "hole_keypoint_once" not in stage2
servo = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "servo_node.py"
).read_text(encoding="utf-8")
hw = (
__import__("pathlib").Path(__file__).resolve().parents[3]
/ "src" / "flexiv_hardware" / "src" / "flexiv_hardware_interface.cpp"
).read_text(encoding="utf-8")
assert "movel_euler_deg" in hw
assert "Quat2EulerZYX" not in hw
assert "sy > 1e-6" in hw
goto = (
__import__("pathlib").Path(__file__).resolve().parents[3]
/ "scripts" / "rdk_goto_xyz.py"
).read_text(encoding="utf-8")
assert "quat_wxyz_to_euler_xyz_deg" in goto
# 工具朝下是绕 X 转 180°。这组角必须还是 180,0,0，不能翻成会让 MoveL 拧一圈的另一组。
sys_path = __import__("sys").path
scripts = __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts"
if str(scripts) not in sys_path:
sys_path.insert(0, str(scripts))
from rdk_goto_xyz import quat_wxyz_to_euler_xyz_deg
euler = quat_wxyz_to_euler_xyz_deg([0.0, 1.0, 0.0, 0.0])
assert abs(euler[0]) == pytest.approx(180.0, abs=1e-6)
assert abs(euler[1]) == pytest.approx(0.0, abs=1e-6)
assert abs(euler[2]) == pytest.approx(0.0, abs=1e-6)
assert "rdk_release_movel.py" in servo
assert "d435_handback_sec" in servo
assert "still_sec=max(0.0, self.d435_handback_sec)" in servo
assert "wrist_align_sec" in servo
# 杀栈 MoveL 还在走时不许把 vendor_done 提前置上，停稳计时也不许在这段路上累计。
call_at = servo.find("ok = self._d435_vendor_movel()")
done_at = servo.find("self._d435_vendor_done = True", call_at)
assert call_at > 0 and done_at > call_at
assert "arrived=(self._d435_vendor_done and not self._d435_vendor_busy)" in servo
assert "or not self._d435_vendor_done" in servo
assert "实时预测到此为止" in servo
assert "workpiece/detection_overhead" in perc
# 相机在工件正上方朝下：主点射线落在相机 XY、工件面高度上。
T = np.eye(4)
T[1, 1] = -1.0
src/arm_application/test/test_overhead_track.py
+12 -3
View File
@@ -42,7 +42,8 @@ def _track_site(**over) -> dict:
site["servo"] = {
"xy_track": "hover",
"lock_still_sec": 3.0,
"wrist_align_sec": 1.0,
"d435_handback_sec": 3.0,
"wrist_align_sec": 0.5,
"lock_still_move_m": 0.015,
}
site["perception"] = {
@@ -153,7 +154,8 @@ class TestPerceptionServoWiring:
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_align_sec"] == pytest.approx(1.0)
assert s["d435_handback_sec"] == pytest.approx(3.0)
assert s["wrist_align_sec"] == pytest.approx(0.5)
assert s["wrist_handoff_conf"] == pytest.approx(0.5)
assert s["overhead_hold_s"] == pytest.approx(8.0)
@@ -169,6 +171,12 @@ class TestPerceptionServoWiring:
with pytest.raises(RuntimeError, match="lock_still_sec"):
export_site_env.servo_overrides(site)
def test_wrist_hybrid_missing_handback_raises(self):
site = _track_site()
del site["servo"]["d435_handback_sec"]
with pytest.raises(RuntimeError, match="d435_handback_sec"):
export_site_env.servo_overrides(site)
def test_overhead_lookat_raises(self):
site = _overhead_ok()
site["servo"] = {"xy_track": "lookat"}
@@ -230,7 +238,8 @@ class TestRealSiteBranchDefault:
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["wrist_align_sec"] == pytest.approx(1.0)
assert s["d435_handback_sec"] == pytest.approx(3.0)
assert s["wrist_align_sec"] == pytest.approx(0.5)
assert s["lock_still_move_m"] == pytest.approx(0.015)
# 负向：跟踪外参不能被换成顶视文件
assert "eye_to_hand" not in s["handeye_file"]
src/arm_application/test/test_pipeline_stage1_diag.py
+25 -1
View File
@@ -13,7 +13,7 @@ from __future__ import annotations
import pytest
from arm_application.pipeline_node import stage1_timeout_reason, track_lock_z
from arm_application.pipeline_node import init_z_refusal, stage1_timeout_reason, track_lock_z
def test_zero_points_blames_perception_not_calibration():
@@ -54,6 +54,30 @@ def test_sim_still_lifts_when_hover_barely_clears_the_part():
assert z == pytest.approx(0.707)
def test_init_z_refuses_a_value_that_would_enter_the_part():
"""用户指定的初始高度会钻进工件时必须拒绝，不许悄悄抬高。"""
why = init_z_refusal(0.20, 0.2245, 0.022, 0.002, 0.18, 0.65)
assert "进入工件" in why
assert "不会悄悄抬高" in why
assert init_z_refusal(0.320, 0.2245, 0.022, 0.002, 0.18, 0.65) == ""
assert "工作盒" in init_z_refusal(0.10, 0.2245, 0.022, 0.002, 0.18, 0.65)
assert "未知" in init_z_refusal(0.320, 0.0, 0.022, 0.002, 0.18, 0.65)
def test_movel_ipc_roundtrip():
lib = __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
import sys
if str(lib) not in sys.path:
sys.path.insert(0, str(lib))
from flexiv_movel_ipc import format_request, parse_done
line = format_request(7, 0.4, -0.2, 0.32, True, 0.05)
assert line.startswith("7 0.400000 -0.200000 0.320000 1 0.0500")
assert parse_done("7 ok\n") == (7, True, "")
assert parse_done("7 fail mode\n") == (7, False, "mode")
assert parse_done("nope\n") is None
def test_missing_geometry_does_not_pretend_the_hover_is_clear():
"""工具长或工件面缺失时必须抬升。缺数还锁在悬停高，等于没检查。"""
assert track_lock_z(0.320, 0.0, 0.022) == pytest.approx(0.370)
src/arm_application/test/test_stage2_descent_clamp.py
+30 -4
View File
@@ -37,7 +37,10 @@ from typing import Any, List, Optional
import numpy as np
from arm_application.stage2_locate_node import Stage2Locate
from arm_application.stage2_locate_node import (
Stage2Locate,
contact_armed_by_gap,
)
# 与真机那次一致的一组数（site.yaml 现行值）
_HOVER_Z = 0.320
@@ -90,6 +93,7 @@ class _StubStage2:
self._logger = _StubLogger(self.logs)
self.final_descent_enable = True
self.final_gap_m = _FINAL_GAP
self.observe_gap_m = 0.015
self.final_descent_speed = 0.005
self.vert_tol_rad = 0.01
self.hover_z = _HOVER_Z
@@ -259,18 +263,40 @@ def test_zero_tool_length_refuses_to_descend():
# 真安全机制仍在：FT 接触、goto 失败
# --------------------------------------------------------------------------- #
def test_ft_contact_stops_and_backs_off():
"""腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。"""
st = _StubStage2(contact_after_steps=2)
"""腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。
只有几何间隙已经贴近工件才认接触。两步（8mm）时还离面 45mm，力门槛不算。
"""
st = _StubStage2(contact_after_steps=8)
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_contact_not_armed_while_still_high_above_the_part():
"""2026-10-13：法兰 0.384、间隙 138mm、ΔFz=+2.87N 不算碰到。"""
assert contact_armed_by_gap(0.1375, 0.015) is False
assert contact_armed_by_gap(0.020, 0.015) is True
def test_ft_drift_far_from_surface_does_not_stop_on_the_first_step():
"""离面还远时力一直超门槛，也要先降到门控间隙附近才许停。"""
class _AlwaysHot(_StubStage2):
def _contact_detected(self, _base_fz: float) -> bool:
return True
ok, _Tee, st = _run(_AlwaysHot())
assert ok is True
assert sum(st.steps_mm) >= 28.0, "离面 53mm 时力超门槛也不该第一步就停"
assert any("腕部力突变" in m for m in st.logs)
def test_every_step_logs_the_ft_force():
"""每一步都要打出 FT 轴向力 —— 这是"停住是碰到还是没跟上"的唯一直接证据。
src/arm_application/test/test_stage3_tactile_live.py
+8
View File
@@ -24,6 +24,7 @@ from dataclasses import dataclass
from arm_application.stage3_search_node import (
TACTILE_MAX_AGE_S,
live_tactile,
should_lift_air_baseline,
tactile_timeout_reason,
)
@@ -77,3 +78,10 @@ def test_timeout_reason_still_reports_stall_when_only_stale_frames_exist():
old = time.monotonic() - 10.0
msg = tactile_timeout_reason([(old, _Msg(connected=True)), None], 3.0)
assert "断流" in msg
def test_no_tactile_does_not_lift_to_air_baseline():
"""触觉没数据时不许先抬到 hover+50mm 再报断流。"""
assert should_lift_air_baseline(0.385, 0.470, tactile_live=False) is False
assert should_lift_air_baseline(0.249, 0.470, tactile_live=True) is True
assert should_lift_air_baseline(0.468, 0.470, tactile_live=True) is False