arm_dev
86 lines
2.6 KiB
Python
86 lines
2.6 KiB
Python
"""伺服和 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,
|
||
zone: str = "", toler: int = -1,
|
||
) -> str:
|
||
"""一行请求。不带 ``zone`` 时仍是 6 个数，插孔和粗定位保持厂家默认到位。
|
||
|
||
微升降要带过渡区和到位等级。厂家默认 ``Z50``（50 mm）加等级 3，
|
||
几毫米的直线会在还差约 0.5 mm 时就报到位。合法的最细过渡区是 ``ZFine``，
|
||
最严且仍会检查到位的等级是 1（0 表示不检查）。
|
||
"""
|
||
if bool(zone) != (int(toler) >= 0):
|
||
raise ValueError("MoveL 过渡区和到位等级必须一起给")
|
||
if zone and (zone != "ZFine" or int(toler) != 1):
|
||
raise ValueError(f"MoveL 微升降只接受 ZFine 和到位等级 1，收到 {zone} {toler}")
|
||
line = (
|
||
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
|
||
f"{1 if keep_z else 0} {float(vel):.4f}"
|
||
)
|
||
if zone:
|
||
line += f" {zone} {int(toler)}"
|
||
return line + "\n"
|
||
|
||
|
||
def write_request(
|
||
seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
|
||
zone: str = "", toler: int = -1,
|
||
) -> None:
|
||
text = format_request(seq, x, y, z, keep_z, vel, zone, toler)
|
||
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
|