把现场确认能用的对孔方案落成保存点：小孔只用视觉关键点交到工件面、多走多拍闭环；D435 走到点后杀栈冷启动再停稳 3 秒才交 D405，避免赶路时带臂晃。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user