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