把现场确认能用的对孔方案落成保存点：小孔只用视觉关键点交到工件面、多走多拍闭环；D435 走到点后杀栈冷启动再停稳 3 秒才交 D405，避免赶路时带臂晃。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -590,10 +590,12 @@ servo:
|
||||
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
|
||||
deadzone_z_m: 0.012
|
||||
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
|
||||
# D405 跟住后，法兰 XY 连续这么久位移不超过 lock_still_move_m → /visual_servo/locked
|
||||
# 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
|
||||
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
|
||||
lock_still_sec: 3.0
|
||||
wrist_align_sec: 1.0
|
||||
# MoveL 后杀栈冷启动回来，再叠这么久才交回 D405。流畅度先不管，停稳优先。
|
||||
d435_handback_sec: 3.0
|
||||
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
|
||||
wrist_align_sec: 0.5
|
||||
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
|
||||
e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速
|
||||
v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s
|
||||
|
||||
+5
-4
@@ -577,11 +577,12 @@ servo:
|
||||
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
|
||||
deadzone_z_m: 0.012
|
||||
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
|
||||
# D435 到位之后，法兰 XY 连续这么久位移不超过 lock_still_move_m 才算停稳、交回 D405。
|
||||
# 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
|
||||
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
|
||||
lock_still_sec: 3.0
|
||||
# D405 改追工件框中心之后，连续这么久才进对孔。不要和上面的停稳写成同一个数。
|
||||
wrist_align_sec: 1.0
|
||||
# MoveL 后杀栈冷启动回来，再叠这么久才交回 D405。流畅度先不管，停稳优先。
|
||||
d435_handback_sec: 3.0
|
||||
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
|
||||
wrist_align_sec: 0.5
|
||||
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
|
||||
e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速
|
||||
v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s
|
||||
|
||||
+11
-7
@@ -66,18 +66,22 @@ run_all.sh
|
||||
bash ~/ros2/scripts/run_real.sh --arm 89 # T1 控制栈，一直开着
|
||||
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # T2 相机预览，一直开着
|
||||
bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（默认不拧入）
|
||||
bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
|
||||
```
|
||||
|
||||
一阶段默认 **D405 实时推理**，D435 待命。YOLO-pose 这一路不关。连续
|
||||
`D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
|
||||
的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
|
||||
这一段运动是厂家 `MoveL`：先放开 `ros2_control` 占着的 RDK，XY 对准该点，
|
||||
工具尖高度保持当前（点的 z 是工件表面），不按直线距离拒绝；走完再用
|
||||
`run_real.sh --no-stop` 把控制栈拉回来（不扫感知进程）。走到该点并且法兰 XY
|
||||
连续 `servo.lock_still_sec`（3 s）位移不超过 `lock_still_move_m`（15 mm）之前，
|
||||
D405 的点不下发。停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
|
||||
框心对准连续 `servo.wrist_align_sec`（1 s）→ `/visual_servo/locked` → 二阶段对小孔。
|
||||
对孔只用关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
|
||||
这一段运动是厂家 `MoveL`：脚本先停真机控制栈（感知不停），XY 对准该点，
|
||||
工具尖高度保持当前，再冷启动 `run_real.sh --no-stop`。冷启动回来之后才开始计
|
||||
`servo.d435_handback_sec`（3 s）。赶路和杀栈期间不下发 D405。
|
||||
停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
|
||||
启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
|
||||
会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
|
||||
不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
|
||||
框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。
|
||||
二阶段粗居中工件框，再多拍 k1/k2 闭环对准，轻降后在观测高度补一次 XY。
|
||||
孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
|
||||
`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
|
||||
只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
|
||||
|
||||
|
||||
@@ -903,6 +903,7 @@ def servo_overrides(data: dict) -> dict:
|
||||
"ee_hold_kp", "ee_w_max",
|
||||
"lock_hold_sec",
|
||||
"lock_still_sec",
|
||||
"d435_handback_sec",
|
||||
"wrist_align_sec",
|
||||
"lock_still_move_m",
|
||||
"a_max", "max_qdot", "damping",
|
||||
@@ -934,8 +935,13 @@ def servo_overrides(data: dict) -> dict:
|
||||
out["overhead_hold_s"] = float(oh["oneshot_hold_s"])
|
||||
if "lock_still_sec" not in servo:
|
||||
raise RuntimeError(
|
||||
"servo.lock_still_sec 必须显式写"
|
||||
"（D435 到位后法兰停稳多久才交回 D405）。不许用代码默认 3。"
|
||||
"servo.lock_still_sec 必须显式写。"
|
||||
"不许用代码默认秒数。"
|
||||
)
|
||||
if "d435_handback_sec" not in servo:
|
||||
raise RuntimeError(
|
||||
"servo.d435_handback_sec 必须显式写"
|
||||
"（厂家 MoveL 到位后，再确认多久才交回 D405）。不许用代码默认秒数。"
|
||||
)
|
||||
if "wrist_align_sec" not in servo:
|
||||
raise RuntimeError(
|
||||
@@ -948,6 +954,7 @@ def servo_overrides(data: dict) -> dict:
|
||||
"（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
|
||||
)
|
||||
out["lock_still_sec"] = float(servo["lock_still_sec"])
|
||||
out["d435_handback_sec"] = float(servo["d435_handback_sec"])
|
||||
out["wrist_align_sec"] = float(servo["wrist_align_sec"])
|
||||
out["lock_still_move_m"] = float(servo["lock_still_move_m"])
|
||||
out["overhead_k0_topic"] = "visual_servo/overhead_k0"
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
"""伺服和 flexiv_hardware 之间的 MoveL 交接。
|
||||
|
||||
两边不在一个进程里，硬件接口也没有自己的 ROS 节点。用三个文件：
|
||||
|
||||
* ``/tmp/flexiv_movel_req`` 一行 ``seq x y z keep_z vel``
|
||||
* ``/tmp/flexiv_movel_ack`` 硬件接住请求后立刻写序号
|
||||
* ``/tmp/flexiv_movel_done`` 一行 ``seq ok`` 或 ``seq fail 原因``
|
||||
|
||||
先写临时文件再改名，避免对面读到半行。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
REQ = Path("/tmp/flexiv_movel_req")
|
||||
ACK = Path("/tmp/flexiv_movel_ack")
|
||||
DONE = Path("/tmp/flexiv_movel_done")
|
||||
|
||||
|
||||
def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
|
||||
return (
|
||||
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}\n"
|
||||
)
|
||||
|
||||
|
||||
def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
|
||||
text = format_request(seq, x, y, z, keep_z, vel)
|
||||
tmp = REQ.with_name(REQ.name + ".tmp")
|
||||
tmp.write_text(text, encoding="utf-8")
|
||||
tmp.replace(REQ)
|
||||
|
||||
|
||||
def parse_done(text: str):
|
||||
"""返回 ``(seq, ok, detail)``。行不完整时返回 None。"""
|
||||
parts = text.strip().split(maxsplit=2)
|
||||
if len(parts) < 2:
|
||||
return None
|
||||
try:
|
||||
seq = int(parts[0])
|
||||
except ValueError:
|
||||
return None
|
||||
if parts[1] not in ("ok", "fail"):
|
||||
return None
|
||||
detail = parts[2] if len(parts) > 2 else ""
|
||||
return seq, parts[1] == "ok", detail
|
||||
|
||||
|
||||
def read_done():
|
||||
try:
|
||||
text = DONE.read_text(encoding="utf-8")
|
||||
except OSError:
|
||||
return None
|
||||
return parse_done(text)
|
||||
|
||||
|
||||
def read_ack() -> int:
|
||||
try:
|
||||
text = ACK.read_text(encoding="utf-8").strip()
|
||||
except OSError:
|
||||
return -1
|
||||
try:
|
||||
return int(text.split()[0])
|
||||
except (ValueError, IndexError):
|
||||
return -1
|
||||
+9
-1
@@ -21,6 +21,8 @@
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 # 启动后先到这个法兰高度 [m]
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --hover-z 0.34 # 对孔悬停法兰高度 [m]，不安全会拒绝
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only # 拍一张 K0，厂家 MoveL 把工具尖 XY 对准它（先停 run_real.sh）
|
||||
#
|
||||
# 前置（另开一个终端，先跑起来）：
|
||||
@@ -45,6 +47,8 @@ WITH_TACTILE=true
|
||||
CAMERA_PREVIEW=false
|
||||
NO_DESCENT=false
|
||||
D435_ONLY=false
|
||||
INIT_Z=""
|
||||
HOVER_Z=""
|
||||
|
||||
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
|
||||
# shellcheck disable=SC1091
|
||||
@@ -65,6 +69,8 @@ while [[ $# -gt 0 ]]; do
|
||||
# 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
|
||||
--no-descent) NO_DESCENT=true; shift ;;
|
||||
--d435-only) D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
|
||||
--init-z) INIT_Z="${2:?--init-z 需要法兰高度，单位米}"; shift 2 ;;
|
||||
--hover-z) HOVER_Z="${2:?--hover-z 需要法兰高度，单位米}"; shift 2 ;;
|
||||
--no-preview) PIPELINE_PREVIEW=false; shift ;;
|
||||
--camera-preview) CAMERA_PREVIEW=true; shift ;;
|
||||
--no-camera) WITH_CAMERA=false; shift ;;
|
||||
@@ -303,9 +309,11 @@ ARGS=(
|
||||
"no_descent:=${NO_DESCENT}"
|
||||
"d435_move_only:=${D435_ONLY}"
|
||||
)
|
||||
[[ -n "${INIT_Z}" ]] && ARGS+=("init_z_m:=${INIT_Z}")
|
||||
[[ -n "${HOVER_Z}" ]] && ARGS+=("hover_z_m:=${HOVER_Z}")
|
||||
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
|
||||
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
|
||||
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
|
||||
|
||||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
|
||||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
|
||||
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
|
||||
|
||||
@@ -46,10 +46,13 @@ if [[ "${PREVIEW}" == "1" ]]; then
|
||||
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
|
||||
CAM_PID=$!
|
||||
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
|
||||
# 工件框是 D405 像素，叠到 1280×720 的顶视图上位置全错，看起来像没检出。
|
||||
# 绿框来自 workpiece/detection_overhead（D435 像素）。不要订 workpiece/detection。
|
||||
ros2 run arm_application realsense_view --ros-args \
|
||||
-p camera_name:=d435 \
|
||||
-p overlay_detection:=false
|
||||
-p overlay_detection:=true \
|
||||
-p detection_topic:=workpiece/detection_overhead \
|
||||
-p overlay_hold_sec:=1.5 \
|
||||
-p hud_label:="D435 顶视"
|
||||
else
|
||||
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
|
||||
fi
|
||||
|
||||
@@ -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 的说明）：
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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:
|
||||
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:
|
||||
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
|
||||
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()
|
||||
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,14 +1551,28 @@ 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
|
||||
return
|
||||
raw = self._decode_depth_m(depth)
|
||||
depth_m = depth_raw_to_m(raw, depth.encoding or "")
|
||||
if depth_m is None:
|
||||
|
||||
@@ -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()
|
||||
z = float(Tee[2, 3])
|
||||
if z >= tz - 0.008:
|
||||
return
|
||||
self._state("pre_lift")
|
||||
self.get_logger().info(
|
||||
f"先到锁高：Z {z:.3f} → {tz:.3f}m（跟踪与对孔都停在这个高度，对完孔再下降）"
|
||||
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]),
|
||||
)
|
||||
_, q, Tee = self._goto(
|
||||
if why:
|
||||
self.get_logger().error(why)
|
||||
return False
|
||||
z = float(Tee[2, 3])
|
||||
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")
|
||||
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)
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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.hover_z = _fixed
|
||||
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
|
||||
)
|
||||
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
|
||||
|
||||
@@ -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}）"
|
||||
|
||||
@@ -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),
|
||||
]
|
||||
|
||||
@@ -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 之内
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"]
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 轴向力 —— 这是"停住是碰到还是没跟上"的唯一直接证据。
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
|
||||
#define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
@@ -139,6 +140,16 @@ private:
|
||||
// 让 ros2_control 把栈停下来报错，而不是静默地"成功"。
|
||||
int64_t last_state_stamp_ns_ = 0; // 上一帧的机器人时间戳
|
||||
int64_t last_state_change_ns_ = 0; // 上一帧时间戳发生变化的本地时刻
|
||||
|
||||
// D435 粗定位的 MoveL 走已经连上的这份 RDK。write() 里一步一步推进，
|
||||
// 不另开线程，也不在这段里往关节接口灌点。请求文件是 /tmp/flexiv_movel_req。
|
||||
enum class VendorMovelPhase { kIdle, kRunning };
|
||||
VendorMovelPhase vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
long vendor_movel_seq_ = -1;
|
||||
long vendor_movel_done_seq_ = -1;
|
||||
std::chrono::steady_clock::time_point vendor_movel_deadline_ {};
|
||||
bool poll_vendor_movel();
|
||||
void finish_vendor_movel(long seq, bool ok, const std::string& detail);
|
||||
};
|
||||
|
||||
} /* namespace flexiv_hardware */
|
||||
|
||||
@@ -7,15 +7,21 @@
|
||||
*/
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <exception>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <variant>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "flexiv/rdk/mode.hpp"
|
||||
|
||||
// RDK 网卡白名单的本地核对（见下面 resolve_network_interface_whitelist 的说明）
|
||||
#include <arpa/inet.h>
|
||||
#include <ifaddrs.h>
|
||||
@@ -43,6 +49,76 @@ constexpr double kServoJointAcceleration = 1.00;
|
||||
// 留了很大余量；宁可晚 0.5 s 报错，也不要在运动中误判把栈停掉。
|
||||
constexpr int64_t kStateStaleTimeoutNs = 500'000'000LL;
|
||||
|
||||
// 伺服和硬件不在一个进程里。请求写成一行，原子改名，避免读到半行。
|
||||
constexpr const char* kMovelReq = "/tmp/flexiv_movel_req";
|
||||
constexpr const char* kMovelAck = "/tmp/flexiv_movel_ack";
|
||||
constexpr const char* kMovelDone = "/tmp/flexiv_movel_done";
|
||||
|
||||
long read_movel_seq()
|
||||
{
|
||||
std::ifstream in(kMovelReq);
|
||||
long seq = -1;
|
||||
if (!(in >> seq)) {
|
||||
return -1;
|
||||
}
|
||||
return seq;
|
||||
}
|
||||
|
||||
// 与 scripts/rdk_goto_xyz.py 的 quat_wxyz_to_euler_xyz_deg 同一套。
|
||||
// 不要改用 Eigen eulerAngles：工具朝下接近 180° 时会翻成另一组角，MoveL 会拧一圈。
|
||||
std::array<double, 3> movel_euler_deg(double qw, double qx, double qy, double qz)
|
||||
{
|
||||
const double n = std::sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
|
||||
if (n < 1e-12) {
|
||||
throw std::runtime_error("tcp 四元数是零");
|
||||
}
|
||||
qw /= n;
|
||||
qx /= n;
|
||||
qy /= n;
|
||||
qz /= n;
|
||||
const double xx = qx * qx;
|
||||
const double yy = qy * qy;
|
||||
const double zz = qz * qz;
|
||||
const double xy = qx * qy;
|
||||
const double xz = qx * qz;
|
||||
const double yz = qy * qz;
|
||||
const double wx = qw * qx;
|
||||
const double wy = qw * qy;
|
||||
const double wz = qw * qz;
|
||||
const double r00 = 1.0 - 2.0 * (yy + zz);
|
||||
const double r10 = 2.0 * (xy + wz);
|
||||
const double r20 = 2.0 * (xz - wy);
|
||||
const double r21 = 2.0 * (yz + wx);
|
||||
const double r22 = 1.0 - 2.0 * (xx + yy);
|
||||
const double r12 = 2.0 * (yz - wx);
|
||||
const double r11 = 1.0 - 2.0 * (xx + zz);
|
||||
const double sy = std::sqrt(r00 * r00 + r10 * r10);
|
||||
double x = 0.0;
|
||||
double y = 0.0;
|
||||
double z = 0.0;
|
||||
if (sy > 1e-6) {
|
||||
x = std::atan2(r21, r22);
|
||||
y = std::atan2(-r20, sy);
|
||||
z = std::atan2(r10, r00);
|
||||
} else {
|
||||
x = std::atan2(-r12, r11);
|
||||
y = std::atan2(-r20, sy);
|
||||
z = 0.0;
|
||||
}
|
||||
constexpr double kRad2Deg = 180.0 / 3.14159265358979323846;
|
||||
return {x * kRad2Deg, y * kRad2Deg, z * kRad2Deg};
|
||||
}
|
||||
|
||||
void write_movel_line(const char* path, const std::string& line)
|
||||
{
|
||||
const std::string tmp = std::string(path) + ".tmp";
|
||||
{
|
||||
std::ofstream out(tmp, std::ios::trunc);
|
||||
out << line << "\n";
|
||||
}
|
||||
std::rename(tmp.c_str(), path);
|
||||
}
|
||||
|
||||
struct NrtSendGate
|
||||
{
|
||||
std::vector<double> last_sent;
|
||||
@@ -573,6 +649,9 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate(
|
||||
}
|
||||
|
||||
RCLCPP_INFO(getLogger(), "System successfully started!");
|
||||
// 启动前磁盘上的旧请求不许再执行一遍。新请求用更晚的序号。
|
||||
vendor_movel_done_seq_ = read_movel_seq();
|
||||
vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
|
||||
return hardware_interface::CallbackReturn::SUCCESS;
|
||||
}
|
||||
@@ -662,9 +741,112 @@ hardware_interface::return_type FlexivHardwareInterface::read(
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
void FlexivHardwareInterface::finish_vendor_movel(
|
||||
long seq, bool ok, const std::string& detail)
|
||||
{
|
||||
std::string clean;
|
||||
clean.reserve(detail.size());
|
||||
for (char c : detail) {
|
||||
clean.push_back(c == '\n' || c == '\r' ? ' ' : c);
|
||||
}
|
||||
if (ok) {
|
||||
write_movel_line(kMovelDone, std::to_string(seq) + " ok");
|
||||
} else {
|
||||
write_movel_line(kMovelDone, std::to_string(seq) + " fail " + clean);
|
||||
}
|
||||
vendor_movel_done_seq_ = seq;
|
||||
vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
}
|
||||
|
||||
bool FlexivHardwareInterface::poll_vendor_movel()
|
||||
{
|
||||
// 这一拍正在走 MoveL：只查 reachedTarget，不往关节接口灌点。
|
||||
// read() 仍每拍更新状态，时间戳看门狗不会把这段误判成断连。
|
||||
if (vendor_movel_phase_ == VendorMovelPhase::kRunning) {
|
||||
try {
|
||||
if (std::chrono::steady_clock::now() > vendor_movel_deadline_) {
|
||||
try {
|
||||
robot_->SwitchMode(rdk_control_mode_);
|
||||
} catch (const std::exception&) {
|
||||
}
|
||||
finish_vendor_movel(vendor_movel_seq_, false, "timeout");
|
||||
return true;
|
||||
}
|
||||
const auto states = robot_->primitive_states();
|
||||
const auto it = states.find("reachedTarget");
|
||||
const bool reached = it != states.end()
|
||||
&& std::holds_alternative<int>(it->second)
|
||||
&& std::get<int>(it->second) != 0;
|
||||
if (reached) {
|
||||
robot_->SwitchMode(rdk_control_mode_);
|
||||
finish_vendor_movel(vendor_movel_seq_, true, "");
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
try {
|
||||
robot_->SwitchMode(rdk_control_mode_);
|
||||
} catch (const std::exception&) {
|
||||
}
|
||||
finish_vendor_movel(vendor_movel_seq_, false, e.what());
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
std::ifstream in(kMovelReq);
|
||||
long seq = 0;
|
||||
double x = 0.0;
|
||||
double y = 0.0;
|
||||
double z = 0.0;
|
||||
double vel = 0.0;
|
||||
int keep_z = 0;
|
||||
if (!(in >> seq >> x >> y >> z >> keep_z >> vel)) {
|
||||
return false;
|
||||
}
|
||||
if (seq == vendor_movel_done_seq_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 先应答，伺服才能在 1.5s 内分清「新接口没编进去」和「臂还在走」。
|
||||
write_movel_line(kMovelAck, std::to_string(seq));
|
||||
try {
|
||||
const auto pose = robot_->states().tcp_pose;
|
||||
if (keep_z != 0) {
|
||||
z = pose[2];
|
||||
}
|
||||
const auto euler = movel_euler_deg(pose[3], pose[4], pose[5], pose[6]);
|
||||
if (robot_->fault()) {
|
||||
robot_->ClearFault(3);
|
||||
}
|
||||
robot_->SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
|
||||
const flexiv::rdk::Coord target(
|
||||
std::array<double, 3> {x, y, z},
|
||||
euler,
|
||||
std::array<std::string, 2> {"WORLD", "WORLD_ORIGIN"});
|
||||
robot_->ExecutePrimitive(
|
||||
"MoveL", {{"target", target}, {"vel", vel}}, true);
|
||||
vendor_movel_seq_ = seq;
|
||||
vendor_movel_phase_ = VendorMovelPhase::kRunning;
|
||||
vendor_movel_deadline_ = std::chrono::steady_clock::now() + std::chrono::seconds(90);
|
||||
RCLCPP_INFO(getLogger(),
|
||||
"厂家 MoveL 已下发 xyz=(%.3f, %.3f, %.3f) keep_z=%d vel=%.3f",
|
||||
x, y, z, keep_z, vel);
|
||||
} catch (const std::exception& e) {
|
||||
try {
|
||||
robot_->SwitchMode(rdk_control_mode_);
|
||||
} catch (const std::exception&) {
|
||||
}
|
||||
finish_vendor_movel(seq, false, e.what());
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
hardware_interface::return_type FlexivHardwareInterface::write(
|
||||
const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
// MoveL 期间不能回到关节模式，也不能把旧关节目标再发出去。
|
||||
if (poll_vendor_movel()) {
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// Initialize target vectors to hold position
|
||||
std::vector<double> target_pos(robot_->info().DoF);
|
||||
std::vector<double> target_vel(robot_->info().DoF);
|
||||
|
||||
Reference in New Issue
Block a user