旋转操作加速版本
This commit is contained in:
arm_dev
@@ -40,7 +40,19 @@ arm_application/kalman_filter.py
|
||||
arm_application/model_labels.py
|
||||
arm_application/move_to_pose_client.py
|
||||
arm_application/onnx_workpiece.py
|
||||
arm_application/operator_app.py
|
||||
arm_application/operator_arm_mesh.py
|
||||
arm_application/operator_arm_state.py
|
||||
arm_application/operator_camera_view.py
|
||||
arm_application/operator_console.py
|
||||
arm_application/operator_gripper_mesh.py
|
||||
arm_application/operator_gripper_state.py
|
||||
arm_application/operator_handeye_board.py
|
||||
arm_application/operator_tactile_view.py
|
||||
arm_application/operator_teach.py
|
||||
arm_application/orisys_tactile.py
|
||||
arm_application/overhead_oneshot.py
|
||||
arm_application/path_profile.py
|
||||
arm_application/perception_node.py
|
||||
arm_application/pid_controller.py
|
||||
arm_application/pipeline_node.py
|
||||
|
||||
@@ -10,8 +10,10 @@ from rclpy.node import Node
|
||||
|
||||
from arm_msgs.srv import SetNamedPose
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
class DemoNamedPoses(Node):
|
||||
|
||||
class DemoNamedPoses(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('demo_named_poses')
|
||||
self.declare_parameter(
|
||||
|
||||
@@ -201,21 +201,36 @@ class DHModbusClient:
|
||||
|
||||
def set_position_per_mille(self, position: float | int) -> int:
|
||||
value = _clamp_int(position, 0, 1000)
|
||||
# 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
|
||||
# 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
|
||||
from arm_application.dh_pgea import latched_position_break
|
||||
|
||||
try:
|
||||
cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
|
||||
except Exception:
|
||||
cmd = value
|
||||
if cmd == value:
|
||||
try:
|
||||
cur = self.get_position_per_mille()
|
||||
except Exception:
|
||||
cur = value
|
||||
nudge = cur + (40 if value >= cur else -40)
|
||||
nudge = _clamp_int(nudge, 0, 1000)
|
||||
if nudge == value:
|
||||
nudge = 0 if value != 0 else 40
|
||||
self.write_register(REG_POSITION, nudge)
|
||||
time.sleep(0.06)
|
||||
try:
|
||||
cur = self.get_position_per_mille()
|
||||
except Exception:
|
||||
cur = value
|
||||
bridge = latched_position_break(cmd, cur, value)
|
||||
if bridge is not None:
|
||||
self.write_register(REG_POSITION, bridge)
|
||||
deadline = time.monotonic() + 0.6
|
||||
echoed = False
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
|
||||
echoed = True
|
||||
break
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.05)
|
||||
if not echoed:
|
||||
raise DHModbusError(
|
||||
f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
|
||||
)
|
||||
time.sleep(0.15)
|
||||
self.write_register(REG_POSITION, value)
|
||||
return value
|
||||
|
||||
|
||||
@@ -104,6 +104,44 @@ INIT_STATE_NAME = {
|
||||
}
|
||||
|
||||
|
||||
def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
|
||||
"""命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
|
||||
|
||||
大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
|
||||
开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
|
||||
先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
|
||||
固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
|
||||
来不及看到中间值，最终仍是旧命令，手指不动。
|
||||
"""
|
||||
cmd = int(max(0, min(1000, command)))
|
||||
cur = int(max(0, min(1000, current)))
|
||||
pos = int(max(0, min(1000, target)))
|
||||
if cmd != pos or abs(cur - pos) <= 25:
|
||||
return None
|
||||
if cur != cmd:
|
||||
return cur
|
||||
return 0 if cmd != 0 else 40
|
||||
|
||||
|
||||
def grip_motion_settled(
|
||||
grip_state: int, position: int, target: int, seen_motion: bool
|
||||
) -> bool:
|
||||
"""这一拍能不能算「这句位置命令已经结束」。
|
||||
|
||||
张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
|
||||
闭合夹住物体：动过之后停在比目标更开的位置，算完成。
|
||||
"""
|
||||
near = abs(int(position) - int(target)) <= 25
|
||||
if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
|
||||
return True
|
||||
if grip_state == S_GRIP_CAUGHT and seen_motion:
|
||||
if int(target) + 25 < int(position):
|
||||
return True
|
||||
if near:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def crc16_modbus(data: bytes) -> int:
|
||||
"""官方 DH CRC16（Modbus，线路上按小端发送）。"""
|
||||
crc = 0xFFFF
|
||||
@@ -175,6 +213,34 @@ WIRING_HINT = (
|
||||
)
|
||||
|
||||
|
||||
def open_failure_text(port: str, exc: BaseException) -> str:
|
||||
"""串口打不开时把「被占用」和「没应答」分开。
|
||||
|
||||
独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
|
||||
界面上看起来像缺自动上电，实际是别的进程还握着这个口。
|
||||
"""
|
||||
text = str(exc).lower()
|
||||
errno = getattr(exc, "errno", None)
|
||||
busy = errno in (11, 16) or any(
|
||||
token in text
|
||||
for token in (
|
||||
"temporarily unavailable",
|
||||
"device or resource busy",
|
||||
"could not exclusively lock",
|
||||
"resource busy",
|
||||
)
|
||||
)
|
||||
if busy:
|
||||
return (
|
||||
f"无法独占打开 {port}：{exc}。"
|
||||
"串口已被别的进程占用（夹爪驱动或状态读取）。"
|
||||
)
|
||||
return (
|
||||
f"无法打开 {port}：{exc}。"
|
||||
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
|
||||
)
|
||||
|
||||
|
||||
class DhPgeaGripper:
|
||||
"""面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
|
||||
|
||||
@@ -219,10 +285,7 @@ class DhPgeaGripper:
|
||||
exclusive=True,
|
||||
)
|
||||
except Exception as exc:
|
||||
raise DhPgeaError(
|
||||
f"无法打开 {self.port}：{exc}。"
|
||||
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
|
||||
) from exc
|
||||
raise DhPgeaError(open_failure_text(self.port, exc)) from exc
|
||||
try:
|
||||
self._ser.dtr = True
|
||||
except Exception:
|
||||
@@ -319,17 +382,28 @@ class DhPgeaGripper:
|
||||
def initialization(self) -> bool:
|
||||
return self.write_register(REG_INIT, INIT_CMD)
|
||||
|
||||
def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
|
||||
"""写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
|
||||
def initialize(
|
||||
self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
|
||||
) -> bool:
|
||||
"""写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
|
||||
|
||||
``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
|
||||
命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
|
||||
手指根本不会回零，看起来像初始化失效。
|
||||
"""
|
||||
state = self.get_init_state()
|
||||
if state == S_INIT_FINISHED:
|
||||
if state == S_INIT_FINISHED and not force:
|
||||
return True
|
||||
if not self.initialization():
|
||||
raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
# force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
|
||||
saw_busy = not force
|
||||
while time.monotonic() < deadline:
|
||||
state = self.get_init_state()
|
||||
if state == S_INIT_FINISHED:
|
||||
if state != S_INIT_FINISHED:
|
||||
saw_busy = True
|
||||
elif saw_busy:
|
||||
return True
|
||||
time.sleep(poll_s)
|
||||
raise DhPgeaError(
|
||||
@@ -345,42 +419,58 @@ class DhPgeaGripper:
|
||||
speed_pct = int(max(1, min(100, speed_pct)))
|
||||
return self.write_register(REG_SPEED, speed_pct)
|
||||
|
||||
def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
|
||||
"""等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
if self.get_target_position(retries=1) == int(value):
|
||||
return True
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.05)
|
||||
return False
|
||||
|
||||
def set_target_position(self, pos: int) -> bool:
|
||||
pos = int(max(0, min(1000, pos)))
|
||||
# 大寰固件：0x0103 写入与当前命令相同的值不会启动新运动。
|
||||
# 开/关卡在 caught 时最常见——命令已是 1000，手指却停在夹持位。
|
||||
try:
|
||||
cmd = self.get_target_position(retries=2)
|
||||
except Exception:
|
||||
cmd = pos
|
||||
if cmd == pos:
|
||||
try:
|
||||
cur = self.get_current_position(retries=2)
|
||||
except Exception:
|
||||
cur = pos
|
||||
nudge = cur + (40 if pos >= cur else -40)
|
||||
nudge = int(max(0, min(1000, nudge)))
|
||||
if nudge == pos:
|
||||
nudge = 0 if pos != 0 else 40
|
||||
self.write_register(REG_POSITION, nudge)
|
||||
time.sleep(0.06)
|
||||
try:
|
||||
cur = self.get_current_position(retries=2)
|
||||
except Exception:
|
||||
cur = pos
|
||||
bridge = latched_position_break(cmd, cur, pos)
|
||||
if bridge is not None:
|
||||
if not self.write_register(REG_POSITION, bridge):
|
||||
return False
|
||||
if not self._wait_command_echo(bridge):
|
||||
return False
|
||||
# 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
|
||||
time.sleep(0.15)
|
||||
return self.write_register(REG_POSITION, pos)
|
||||
|
||||
def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
|
||||
"""等到到位/夹住/松开，或超时返回最后一帧。"""
|
||||
"""等到到位或夹住，或超时返回最后一帧。
|
||||
|
||||
张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
|
||||
手指还停在夹持位。
|
||||
"""
|
||||
t0 = time.monotonic()
|
||||
last = None
|
||||
seen_motion = False
|
||||
st = self.get_run_states()
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
st = self.get_run_states()
|
||||
moved = last is not None and st.position != last
|
||||
if last is not None and st.position != last:
|
||||
seen_motion = True
|
||||
last = st.position
|
||||
if st.grip_state == S_GRIP_MOVING or moved:
|
||||
if st.grip_state == S_GRIP_MOVING:
|
||||
seen_motion = True
|
||||
time.sleep(0.08)
|
||||
continue
|
||||
if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
|
||||
return st
|
||||
if st.grip_state == S_GRIP_CAUGHT:
|
||||
if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
|
||||
return st
|
||||
time.sleep(0.08)
|
||||
return st
|
||||
@@ -497,6 +587,36 @@ def _scan_bus(port: str) -> int:
|
||||
return 1
|
||||
|
||||
|
||||
def ctrl_values(
|
||||
cmd: str,
|
||||
value: str = "",
|
||||
force: int = 50,
|
||||
speed: int = 50,
|
||||
) -> Optional[tuple[float, float, float, float]]:
|
||||
"""节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
|
||||
|
||||
顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
|
||||
"""
|
||||
name = str(cmd or "").strip().lower()
|
||||
if name in ("probe", "status"):
|
||||
return None
|
||||
if name == "init":
|
||||
return (1.0, 0.0, float(force), float(speed))
|
||||
if name == "open":
|
||||
pos = 1000
|
||||
elif name == "close":
|
||||
pos = 0
|
||||
elif name in ("pos", "move"):
|
||||
if str(value).strip() == "":
|
||||
raise ValueError("pos 需要 0–1000")
|
||||
pos = int(round(float(value)))
|
||||
if pos < 0 or pos > 1000:
|
||||
raise ValueError("位置只接受 0–1000")
|
||||
else:
|
||||
raise ValueError(f"节点在线时不支持 {name}")
|
||||
return (0.0, float(pos), float(force), float(speed))
|
||||
|
||||
|
||||
def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
site = _site_defaults()
|
||||
ap = argparse.ArgumentParser(
|
||||
@@ -541,8 +661,8 @@ def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
print(st.summary())
|
||||
return 0
|
||||
if args.cmd == "init":
|
||||
print("正在初始化（手指回零）…")
|
||||
grip.initialize()
|
||||
print("正在初始化（手指回零，已初始化也会再做一次）…")
|
||||
grip.initialize(force=True)
|
||||
print(grip.get_run_states().summary())
|
||||
return 0
|
||||
if args.cmd == "status":
|
||||
@@ -551,12 +671,25 @@ def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
if args.cmd == "open":
|
||||
if not grip.open_jaw(args.force, args.speed):
|
||||
raise DhPgeaError("张开写入失败")
|
||||
print(grip.wait_settle(1000).summary())
|
||||
st = grip.wait_settle(1000)
|
||||
print(st.summary())
|
||||
if abs(st.position - 1000) > 25:
|
||||
print(
|
||||
"张开失败：手指没有离开夹持位。"
|
||||
"若仍是 grip=caught，先取下工件再执行 init。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
return 0
|
||||
if args.cmd == "close":
|
||||
if not grip.close_jaw(args.force, args.speed):
|
||||
raise DhPgeaError("闭合写入失败")
|
||||
print(grip.wait_settle(0).summary())
|
||||
st = grip.wait_settle(0)
|
||||
print(st.summary())
|
||||
# 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
|
||||
if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
|
||||
print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
|
||||
return 1
|
||||
return 0
|
||||
if args.cmd == "pos":
|
||||
if args.value == "":
|
||||
@@ -565,7 +698,15 @@ def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
pos = int(float(args.value))
|
||||
if not grip.move(pos, args.force, args.speed):
|
||||
raise DhPgeaError("位置写入失败")
|
||||
print(grip.wait_settle(pos).summary())
|
||||
st = grip.wait_settle(pos)
|
||||
print(st.summary())
|
||||
grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
|
||||
if abs(st.position - pos) > 25 and not grasped_short:
|
||||
print(
|
||||
f"位置 {pos} 没有到位（仍在 {st.position}）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
return 0
|
||||
except DhPgeaError as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
|
||||
@@ -33,15 +33,7 @@ from arm_application.dh_pgea import (
|
||||
GripperState,
|
||||
WIRING_HINT,
|
||||
)
|
||||
|
||||
|
||||
def _param_as_str(node: Node, name: str, default: str = "") -> str:
|
||||
"""字符串参数，同时允许命令行用数字覆盖（例如 SN 26610799）。"""
|
||||
node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
|
||||
val = node.get_parameter(name).value
|
||||
if val is None or val == "":
|
||||
return ""
|
||||
return str(val).strip()
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
|
||||
def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
|
||||
@@ -51,7 +43,7 @@ def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
|
||||
return float(data[0]), float(data[1]), float(data[2]), float(data[3])
|
||||
|
||||
|
||||
class DhPgeaNode(Node):
|
||||
class DhPgeaNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dh_pgea")
|
||||
self.declare_parameter("port", "/dev/ttyUSB0")
|
||||
@@ -64,9 +56,9 @@ class DhPgeaNode(Node):
|
||||
self.declare_parameter("state_hz", 20.0)
|
||||
self.declare_parameter("open_position", 1000)
|
||||
self.declare_parameter("close_position", 0)
|
||||
self._model = _param_as_str(self, "model", "PGEA-100-20-W")
|
||||
self._sn = _param_as_str(self, "serial_number", "")
|
||||
self._rts = _param_as_str(self, "rs485_rts", "")
|
||||
self._model = param_as_str(self, "model", "PGEA-100-20-W")
|
||||
self._sn = param_as_str(self, "serial_number", "")
|
||||
self._rts = param_as_str(self, "rs485_rts", "")
|
||||
|
||||
self._port = str(self.get_parameter("port").value)
|
||||
self._baud = int(self.get_parameter("baud").value)
|
||||
|
||||
@@ -22,6 +22,8 @@ from arm_msgs.msg import GripperCommand as GripperCommandMsg
|
||||
from arm_msgs.msg import GripperStatus as GripperStatusMsg
|
||||
from arm_msgs.srv import GripperExecute
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
@@ -38,7 +40,7 @@ def _as_bool(val, default: bool = True) -> bool:
|
||||
return bool(val)
|
||||
|
||||
|
||||
class DualGripperNode(Node):
|
||||
class DualGripperNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dual_gripper_driver")
|
||||
self.declare_parameter("config_file", "")
|
||||
|
||||
@@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪点的三道守卫：**位置合理性** + **每帧位移上限** + **远框重捕获确认**。
|
||||
|
||||
为什么需要它们（2026-10-08 真机 arm 89，一阶段跑 150s 没锁上）
|
||||
------------------------------------------------------------
|
||||
``/tmp/pipeline_stage1.log`` 原话。真 k0 一开始是对的::
|
||||
|
||||
764.79 跟踪 keypoint conf=0.87 uv=(410,246) z=0.299 ← 真 k0（画面中偏右）
|
||||
|
||||
然后在 1 秒内被一串**假 k0** 顶掉（全都是高置信度）::
|
||||
|
||||
766.96 重新捕获 conf=0.58 dpx=480
|
||||
766.96 跟踪 keypoint conf=0.58 uv=(76,75) z=0.392 ← 贴左上角
|
||||
767.75 跟踪 keypoint conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near
|
||||
768.16 跟踪 keypoint conf=0.53 uv=(143,92) z=0.376
|
||||
768.78 跟踪 keypoint conf=0.73 uv=(12,30) k0(ok=1551 near=620 借深度) z=0.276
|
||||
769.66 重新捕获 conf=0.22 dpx=207
|
||||
770.07 跟踪 keypoint conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度) z=0.271
|
||||
|
||||
伺服于是以 0.45m/s 追着它们跑：``ee_xy`` home ``(0.686,-0.113)`` → ``(0.354,-0.294)``
|
||||
→ ``(0.083,-0.515)``（工作盒角落），1.2s 后看门狗急停；工件已在半米外，
|
||||
剩下 148s 全是"检测漏检"。三条结论：
|
||||
|
||||
1. **置信度挡不住**：假 k0 的 conf 是 0.53~0.73，真 k0 是 0.87 —— 没有可分区间。
|
||||
但假 k0 有一个共同特征：**落在画面边上**（``uv=(12,30)`` / ``(76,75)`` /
|
||||
``(143,92)``），而孔心长在工件上、不可能贴在画面边缘；
|
||||
2. **借深度把"没有深度"伪装成"有深度"**：``k0(… 借深度)`` 的 z 是从工件框里
|
||||
借的，看着很合理（0.27~0.39m），于是"物理量程"这道守卫也失效了。而
|
||||
**借来的深度意味着该像素处读不到自己的深度** ⇒ 那个点的绝对 XY 不可靠，
|
||||
不该用来**重定位**（换一个跟踪位置）；
|
||||
3. **位移不受限**：真 k0 到假 k0 一帧能差 480px / 400mm，伺服一次就全速扑过去。
|
||||
|
||||
三道守卫各挡一层，全都是"能真的失败"的判据（见 test_feature_guard_*.py 的负向用例）：
|
||||
|
||||
``kpt_plausible``
|
||||
k0 必须落在自己的检测框内（含少量余量）、且不许贴画面边缘。违反 ⇒ 本帧的
|
||||
k0 **位置**不可信（不是"没有目标"），交回调用方走"按框上相对偏移重建"的
|
||||
连续性路径（``perception_node._keypoint_feature`` 本来就有这条路）。
|
||||
``TrackSlewGuard``
|
||||
已发布跟踪点的**每帧位移上限**。工件不动、相机 30Hz，真实观测一帧不可能走
|
||||
4cm（``feature_jump_guard`` 实测真实运动 ≤15mm/帧）。超过上限：观测可信就
|
||||
**夹到上限**（限速跟随，不丢目标），观测来自借深度就**整帧不采信**。
|
||||
``FarReacquireConfirm``
|
||||
"远框重新捕获"（``dpx`` 大于门限）必须连续 ``confirm_frames`` 帧指向同一处
|
||||
才采信 —— 一帧的远框更可能是模型在别的东西上误报，而不是工件真的被挪走了。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _xy_dist(a: np.ndarray, b: np.ndarray) -> float:
|
||||
"""两个 XY 点的距离（米/像素都行，单位跟随入参）。单一实现，别各写一份。"""
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
|
||||
def kpt_plausible(
|
||||
u: float,
|
||||
v: float,
|
||||
box: Optional[Sequence[float]],
|
||||
w: int,
|
||||
h: int,
|
||||
*,
|
||||
border_px: float = 6.0,
|
||||
box_margin_frac: float = 0.10,
|
||||
) -> Optional[str]:
|
||||
"""k0 的**像素位置**是否可信。返回 None = 可信；否则返回拒绝原因。
|
||||
|
||||
``box`` 是同一帧的工件检测框 ``(x0,y0,x1,y1)``；``w,h`` 是彩色图宽高。
|
||||
"""
|
||||
if not (np.isfinite(u) and np.isfinite(v)):
|
||||
return "k0 像素非有限值"
|
||||
if box is None:
|
||||
# 没有框就没法判断 k0 是否落在工件上 —— 宁可当不可信（走连续性路径）。
|
||||
return "没有检测框，无法判断 k0 是否落在工件上"
|
||||
b = np.asarray(box, dtype=float).reshape(-1)
|
||||
if b.size < 4:
|
||||
return "检测框字段不完整"
|
||||
x0, y0, x1, y1 = (float(vv) for vv in b[:4])
|
||||
bw, bh = abs(x1 - x0), abs(y1 - y0)
|
||||
if not (np.isfinite(bw) and np.isfinite(bh)) or bw <= 1e-6 or bh <= 1e-6:
|
||||
return "检测框尺寸无效"
|
||||
mx, my = box_margin_frac * bw, box_margin_frac * bh
|
||||
if u < x0 - mx or u > x1 + mx or v < y0 - my or v > y1 + my:
|
||||
return (
|
||||
f"k0 ({u:.0f},{v:.0f}) 落在检测框 "
|
||||
f"[{x0:.0f},{y0:.0f},{x1:.0f},{y1:.0f}] 之外"
|
||||
)
|
||||
if w > 0 and h > 0:
|
||||
if u <= border_px or v <= border_px or u >= w - 1 - border_px or v >= h - 1 - border_px:
|
||||
return (
|
||||
f"k0 ({u:.0f},{v:.0f}) 贴画面边缘（{w}x{h}，余量 {border_px:.0f}px）"
|
||||
"—— 孔心长在工件上，不可能在画面边上"
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
class TrackSlewGuard:
|
||||
"""已发布跟踪点的每帧位移上限。
|
||||
|
||||
单实例服务**一个**跟踪点；调用方负责在"没有目标/重置"时 :meth:`reset`。
|
||||
"""
|
||||
|
||||
def __init__(self, max_step_m: float) -> None:
|
||||
# 只有**一个**门限，故意的：观测可信就限速跟随，借深度就整帧不采信。
|
||||
# 不做成两个门限是因为"借深度"只影响"能不能重定位"，不影响"小步跟不跟" ——
|
||||
# 两个门限会立刻变成"哪个该调、调到多少"的扯皮（且没有实测支撑）。
|
||||
self.max_step_m = float(max_step_m)
|
||||
self._last: Optional[np.ndarray] = None
|
||||
self.clamped_total = 0
|
||||
self.rejected_borrowed_total = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
self._last = None
|
||||
|
||||
def update(
|
||||
self, p_xy: Optional[Sequence[float]], *, borrowed: bool = False
|
||||
) -> Tuple[Optional[np.ndarray], Optional[str]]:
|
||||
"""喂一次观测，返回 ``(采信的点, 拒绝原因)``。
|
||||
|
||||
- 第一次观测（没有历史）：原样采信（冷启动没有更好的信息，见服务节点的
|
||||
照常起步）。
|
||||
- 位移 ≤ 上限：原样采信。
|
||||
- 位移 > 上限且 ``borrowed``：整帧不采信（返回 ``None`` + 原因）——
|
||||
借来的深度意味着这个像素读不到自己的深度，绝对位置不足以用来重定位。
|
||||
- 位移 > 上限且观测可信：**夹到上限**（限速跟随），不丢目标。
|
||||
"""
|
||||
if p_xy is None:
|
||||
return None, "本帧没有观测"
|
||||
p = np.asarray(p_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(p)):
|
||||
return None, "跟踪点非有限值"
|
||||
if self._last is None:
|
||||
self._last = p.copy()
|
||||
return p, None
|
||||
d = _xy_dist(p, self._last)
|
||||
if d <= self.max_step_m:
|
||||
self._last = p.copy()
|
||||
return p, None
|
||||
if borrowed:
|
||||
self.rejected_borrowed_total += 1
|
||||
return None, (
|
||||
f"借深度的观测要求位移 {d*1000:.1f}mm > 上限 "
|
||||
f"{self.max_step_m*1000:.0f}mm —— 判误检，本帧不采信"
|
||||
)
|
||||
step = self._last + (p - self._last) * (self.max_step_m / d)
|
||||
self._last = step.copy()
|
||||
self.clamped_total += 1
|
||||
return step, (
|
||||
f"位移 {d*1000:.1f}mm > 上限 {self.max_step_m*1000:.0f}mm —— "
|
||||
"夹到上限限速跟随"
|
||||
)
|
||||
|
||||
|
||||
class FarReacquireConfirm:
|
||||
"""远框"重新捕获"的多帧确认。
|
||||
|
||||
``dpx`` 是候选框中心与上一帧框中心的像素距离。超过 ``far_px`` 的候选必须
|
||||
连续 ``confirm_frames`` 帧落在同一处（相互间距 ≤ ``far_px``）才放行。
|
||||
"""
|
||||
|
||||
def __init__(self, far_px: float, confirm_frames: int = 3) -> None:
|
||||
self.far_px = float(far_px)
|
||||
self.confirm_frames = max(1, int(confirm_frames))
|
||||
self._cand: Optional[np.ndarray] = None
|
||||
self._count = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
self._cand = None
|
||||
self._count = 0
|
||||
|
||||
def allow(
|
||||
self, center_xy: Optional[Sequence[float]], dpx: float, *, near_px: float
|
||||
) -> Tuple[bool, Optional[str]]:
|
||||
"""返回 ``(是否放行, 拒绝原因)``。``near_px`` 是"不算远框"的门限。"""
|
||||
if center_xy is None:
|
||||
self.reset()
|
||||
return True, None
|
||||
c = np.asarray(center_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(c)):
|
||||
return False, "候选框中心非有限值"
|
||||
if float(dpx) <= float(near_px):
|
||||
# 近框：正常跟踪，不需要确认
|
||||
self.reset()
|
||||
return True, None
|
||||
if self._cand is None or _xy_dist(c, self._cand) > self.far_px:
|
||||
self._cand = c.copy()
|
||||
self._count = 1
|
||||
else:
|
||||
self._count += 1
|
||||
if self._count >= self.confirm_frames:
|
||||
self.reset()
|
||||
return True, None
|
||||
return False, (
|
||||
f"远框重捕获 dpx={float(dpx):.0f}px 需连续 {self.confirm_frames} 帧一致"
|
||||
f"（当前 {self._count} 帧）—— 本帧不采信"
|
||||
)
|
||||
@@ -0,0 +1,98 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪点"假跳变"的确认门限：单帧大跳不采信，连续 N 帧一致才认。
|
||||
|
||||
为什么需要它（2026-10-08 真机实测，一阶段跑 150s 没锁上）
|
||||
--------------------------------------------------------
|
||||
arm 89 的日志原话：
|
||||
|
||||
0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定！误差只剩 3mm
|
||||
0659.598 k0 置信度 0.01 < 0.30，本帧不用它
|
||||
0659.604 HANDEYE snap feat_base=(0.675,-0.099) ← 跟踪点瞬间横移 89mm
|
||||
0660.121 v=(-0.206,+0.400) |e_xy|=130mm ← 伺服以 0.45m/s 去追这个假点
|
||||
0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，目标彻底丢失
|
||||
|
||||
原来的写法是"跳变超过 ``predict_jump_m`` 就 ``reset()``" —— 等于**单帧就采信**。
|
||||
可是相机 30Hz、工件不动，真实特征点一帧不可能跳 89mm（实测的真实运动
|
||||
在 15mm/帧 以内）。所以"大跳"几乎必然是检测/兜底产生的假点，不该一路追过去。
|
||||
|
||||
判据
|
||||
----
|
||||
- 位移 ≤ ``jump_m``：正常观测（``near``），照常滤波更新。
|
||||
- 位移 > ``jump_m``：进入"待确认"。此后**连续 ``confirm_frames`` 帧**都要落在
|
||||
同一个新位置附近（相互间距 ≤ ``jump_m``），才判为真实重定位（``jump``）。
|
||||
- 只要中间有一帧不对上（乱跳），计数清零、重新开始 —— 乱跳永远等不到确认。
|
||||
|
||||
这也顺手挡住"感知层 k0 掉信后乱跳"那类现象：宁可原地不动，也不要追着假点跑。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
# ``servo_node._update_feat_track`` 的三种判定结果。
|
||||
# 为什么要显式区分"没有目标"：perception 会把误检点也发过来（2026-10-08 真机，
|
||||
# 模型在运动模糊下把特征点丢到基座后方 -0.10,-0.65，工件其实在 0.5,-0.12），
|
||||
# 这种点在物理上不可能 —— 必须当"没看到目标"处理，而不是"钉在原地"。
|
||||
ACCEPT = "accept" # 采信这次观测
|
||||
HOLD = "hold" # 疑似假跳 —— 位置钉住不动
|
||||
REJECT = "reject" # 物理上不合理（工作盒外）—— 当"没有目标"处理
|
||||
|
||||
|
||||
class FeatureJumpGate:
|
||||
"""有状态的跳变确认门限。线程安全由调用方保证（servo 在锁内调用）。"""
|
||||
|
||||
NEAR = "near" # 正常观测：照常滤波更新
|
||||
HOLD = "hold" # 疑似假跳：本次不采信，位置保持不动
|
||||
JUMP = "jump" # 已确认的真实重定位：允许 reset 到新位置
|
||||
|
||||
def __init__(self, jump_m: float, confirm_frames: int = 3) -> None:
|
||||
self.jump_m = float(jump_m)
|
||||
# 至少 1 帧才谈得上"确认"；<=1 就等于退回旧的"单帧采信"，但那是显式选择，
|
||||
# 不是静默降级 —— 这里仍然保留参数以方便现场调节。
|
||||
self.confirm_frames = max(1, int(confirm_frames))
|
||||
self._cand: Optional[np.ndarray] = None
|
||||
self._count = 0
|
||||
# 诊断计数：连续 hold 了多少帧（供日志/排查，见 servo_node 的告警）。
|
||||
self.hold_frames = 0
|
||||
self.hold_total = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清空待确认状态（正常观测、或已确认后都要清）。"""
|
||||
self._cand = None
|
||||
self._count = 0
|
||||
self.hold_frames = 0
|
||||
|
||||
@staticmethod
|
||||
def _dist(a: np.ndarray, b: np.ndarray) -> float:
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
def update(self, p_xy: np.ndarray, ref_xy: np.ndarray) -> str:
|
||||
"""喂一次观测。
|
||||
|
||||
参数
|
||||
----
|
||||
p_xy : 本帧测到的 XY（base 系，米）
|
||||
ref_xy : 当前滤波后的位置 XY（米）—— 判断"跳没跳"的基准
|
||||
|
||||
返回 :data:`NEAR` / :data:`HOLD` / :data:`JUMP` 之一。
|
||||
"""
|
||||
p = np.asarray(p_xy, dtype=float).reshape(2)
|
||||
ref = np.asarray(ref_xy, dtype=float).reshape(2)
|
||||
if self._dist(p, ref) <= self.jump_m:
|
||||
self.reset()
|
||||
return self.NEAR
|
||||
|
||||
# 大跳：本次观测只算"候选"。候选之间互相对不上就从头再数。
|
||||
if self._cand is None or self._dist(p, self._cand) > self.jump_m:
|
||||
self._cand = p.copy()
|
||||
self._count = 1
|
||||
else:
|
||||
self._count += 1
|
||||
if self._count >= self.confirm_frames:
|
||||
self.reset()
|
||||
return self.JUMP
|
||||
self.hold_frames += 1
|
||||
self.hold_total += 1
|
||||
return self.HOLD
|
||||
@@ -94,6 +94,10 @@ from arm_application.gauge_config import (
|
||||
)
|
||||
from arm_application.gauge_fsm import (
|
||||
DESCEND_STALL_STEPS,
|
||||
ENTRY_HIGH,
|
||||
ROT_AXIS,
|
||||
ROT_LIMIT,
|
||||
ROT_SHORT,
|
||||
Event,
|
||||
FsmError,
|
||||
GaugeContext,
|
||||
@@ -104,12 +108,16 @@ from arm_application.gauge_fsm import (
|
||||
descend_iter_budget,
|
||||
descend_needed_m,
|
||||
descend_progress,
|
||||
hole_entry_verdict,
|
||||
descend_tol_m,
|
||||
is_joint_limit_error,
|
||||
pixel_stable,
|
||||
points_stable,
|
||||
ratchet_cycles,
|
||||
ratchet_delta_rad,
|
||||
ratchet_travel_ok,
|
||||
relief_tol_m,
|
||||
rotation_verdict,
|
||||
screw_stop,
|
||||
tangential_jump,
|
||||
tool_clearance_m,
|
||||
@@ -135,16 +143,41 @@ from arm_application.gauge_kin import (
|
||||
)
|
||||
from arm_application.pipeline_node import AssemblyPipeline
|
||||
from arm_application.handeye_realsense import resolve_realsense_topics
|
||||
from arm_application.site_io import deep_merge, load_site_dict, ros2_root
|
||||
from arm_application.site_io import (
|
||||
deep_merge,
|
||||
load_site_dict,
|
||||
overhead_camera,
|
||||
resolve_eye_to_hand_file,
|
||||
ros2_root,
|
||||
)
|
||||
# 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2）
|
||||
from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point
|
||||
|
||||
#: 允许的"绕轴验证"误差 [deg]。超过就说明这根关节轴不是工具 Z 轴，
|
||||
#: 后面所有以"转多少度"为单位的动作都不成立 —— 必须报错而不是继续。
|
||||
AXIS_VERIFY_TOL_DEG = 5.0
|
||||
#: 棘轮转腕时距离关节限位保留的余量 [deg]。
|
||||
#: 3° 不够用：实测轨迹会**自己**少走 ~5°（比如命令 180° 只到位 175.1°，
|
||||
#: 而关节7 的限位正好是 175.06° —— 两者叠在一起，判断"是被限位截住还是
|
||||
#: 没走完"就分不清了）。留 8° 让这两种情况都还没有贴住。
|
||||
RATCHET_LIMIT_MARGIN_DEG = 8.0
|
||||
#: 限制"跟踪丢失 → 粗定位 → 跟踪"的来回次数，防死循环。
|
||||
MAX_TRACK_RETRIES = 3
|
||||
|
||||
#: ``stage2_locate`` 返回码 → 人话。为什么要这张表：日志里只有 ``code=6`` 时，
|
||||
#: 现场只能去翻源码；而每个码对应的**下一步动作完全不同**（垂直化/粗定位/
|
||||
#: 检测/对准/工件被推走），写在这里就不用来回翻了。
|
||||
_LOCATE_CODE_HINT: Dict[int, str] = {
|
||||
2: "（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）",
|
||||
3: "（粗定位失败：视野里没框到工件 —— 看相机/光照/权重）",
|
||||
4: "（k1/k2 解不出来：孔被挡、曝光不对、或孔根本没在视野里）",
|
||||
5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）",
|
||||
6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件"
|
||||
" —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）",
|
||||
8: "（规尖偏移一直没量到：残差看着达标、但用的是**法兰轴线**不是规尖 —— "
|
||||
"对的可能是法兰、不是规；摆到能看清规尖再跑。看日志『规尖视觉量取失败』）",
|
||||
}
|
||||
|
||||
|
||||
class GaugeAbort(RuntimeError):
|
||||
"""阶段内部判定要中止。带上 exit code，方便脚本区分原因。"""
|
||||
@@ -194,12 +227,11 @@ class GripperClient:
|
||||
if math.isfinite(pos):
|
||||
self._last_aperture = pos
|
||||
self._last_gripped = bool(msg.is_gripped)
|
||||
if self._last_gripped:
|
||||
if not self._ever_gripped:
|
||||
# 记住"夹住时的开口"：之后的掉件判据是**相对它的塌陷量**，
|
||||
# 换一把规、换一个夹持开口都不用改阈值。
|
||||
self._grip_aperture = self._last_aperture
|
||||
self._ever_gripped = True
|
||||
# ⚠️ 这里**故意不**记"夹住时的开口"。曾经记过，代价是一个假 E1：
|
||||
# 本后端的 is_gripped 是**推断值**，夹爪**全开**时也会闪成 True
|
||||
# （实测 99.29mm，见 dropped 的说明），于是基准被记成 99.29mm、
|
||||
# 塌陷量算成 77mm ⇒ "通止规掉了(E1)"，而规好好地在爪里。
|
||||
# 基准只由 :meth:`adopt_current_grip` 在**明确知道夹住了**的时候记。
|
||||
|
||||
def _on_legacy(self, msg: Float32MultiArray) -> None:
|
||||
fields = legacy_states_to_fields(list(msg.data))
|
||||
@@ -210,6 +242,8 @@ class GripperClient:
|
||||
if got is not None:
|
||||
self._last_gripped = got
|
||||
if got:
|
||||
# 老后端只有状态字、没有开口，掉件判据本来就吃状态字（见 dropped），
|
||||
# 所以这里记"曾经夹住过"是对的。
|
||||
self._ever_gripped = True
|
||||
|
||||
# -- 查询 -------------------------------------------------------------
|
||||
@@ -224,6 +258,75 @@ class GripperClient:
|
||||
"""``True``/``False``/``None``（后端给不出明确答案）。"""
|
||||
return self._last_gripped
|
||||
|
||||
def aperture(self) -> Optional[float]:
|
||||
"""最近一帧的**开口** [mm]（只有契约后端的 ``position`` 有；没有返回 None）。"""
|
||||
return self._last_aperture
|
||||
|
||||
def grip_aperture(self) -> Optional[float]:
|
||||
"""第一次判到"夹住"时的开口 [mm]（掉件判据的基准，见 :meth:`dropped`）。"""
|
||||
return self._grip_aperture
|
||||
|
||||
def collapse_mm(self) -> Optional[float]:
|
||||
"""相对夹住时的**开口塌陷量** [mm]（正 = 比夹住时更小）。拿不到返回 None。
|
||||
|
||||
S08/S09 每轮都要"松爪 → 回转 → 夹紧"，日志里必须看得到这个数：
|
||||
它一路变大就说明每轮夹紧都比上一轮更往里挤，或者规在往下滑。
|
||||
"""
|
||||
if self._grip_aperture is None or self._last_aperture is None:
|
||||
return None
|
||||
return float(self._grip_aperture) - float(self._last_aperture)
|
||||
|
||||
# -- 开口的"等稳定"与"记基准" ------------------------------------------
|
||||
def settle(self, *, settle_s: float = 1.0, tol_mm: float = 0.05) -> Optional[float]:
|
||||
"""等开口**稳定**下来，返回稳定后的开口 [mm]（拿不到读数返回 None）。
|
||||
|
||||
为什么必须有这个（2026-09-24 用日志抓到的）：
|
||||
``close``/``open`` 服务**返回时动作还没做完**（JTC 轨迹是异步的）。
|
||||
立刻读开口会读到**松手前的旧值**——实测 S08 夹紧后立刻读，读到的还是
|
||||
全开 99.93mm，于是日志与基准全错（"夹住时 99.29mm"）。谁要在动作之后
|
||||
判断开口，都必须先过这里。
|
||||
"""
|
||||
spin = getattr(self._node, "_spin", None)
|
||||
deadline = time.monotonic() + max(0.0, settle_s)
|
||||
prev = self._last_aperture
|
||||
stable = 0
|
||||
while True:
|
||||
if spin is not None:
|
||||
spin(0.05)
|
||||
else: # pragma: no cover - 只有测试桩走这里
|
||||
time.sleep(0.05)
|
||||
now = self._last_aperture
|
||||
if now is not None and prev is not None and abs(float(now) - float(prev)) <= tol_mm:
|
||||
stable += 1
|
||||
if stable >= 2:
|
||||
return float(now)
|
||||
else:
|
||||
stable = 0
|
||||
prev = now
|
||||
if time.monotonic() >= deadline:
|
||||
return None if now is None else float(now)
|
||||
|
||||
def adopt_current_grip(self, why: str, *, settle_s: float = 1.0) -> Optional[float]:
|
||||
"""把**当前**开口记为"夹住时的开口"（掉件判据的基准），并记"曾经夹住过"。
|
||||
|
||||
只在**明确知道爪里夹着规**的时刻调用（S00 自检通过后、以及每一次
|
||||
确认夹紧之后）——它是掉件判据唯一的参照，随手记会让判据失去意义
|
||||
（见 :meth:`dropped` 与 :meth:`_on_contract` 的说明）。
|
||||
"""
|
||||
ap = self.settle(settle_s=settle_s)
|
||||
if ap is None:
|
||||
# 拿不到开口（老固件/别的夹爪）也要记"曾经夹住过"：否则 dropped()
|
||||
# 会永远返回 False，那比"会抖的旧判据"更糟（真掉件也看不见）。
|
||||
self._ever_gripped = True
|
||||
self._node.get_logger().warn(
|
||||
f"拿不到开口读数（{why}）—— 掉件判据退回状态字，会抖但不会漏"
|
||||
)
|
||||
return None
|
||||
self._grip_aperture = float(ap)
|
||||
self._ever_gripped = True
|
||||
self._node.get_logger().info(f"夹持基准：开口 {ap:.2f}mm（{why}）")
|
||||
return float(ap)
|
||||
|
||||
def dropped(self) -> bool:
|
||||
"""E1 掉件：**曾经夹住过**，现在报"掉落"。
|
||||
|
||||
@@ -238,6 +341,9 @@ class GripperClient:
|
||||
的指令落到与实际开口同一档，``is_gripped`` 就翻成 False。于是
|
||||
**S08 第 2/12 轮直接报 E1**（爪里其实好好的，是判据在抖），
|
||||
然后整条流程去等人工"放好通止规"，而仿真里根本没人能做这件事。
|
||||
更坏的是这个推断值在**全开**时也会闪成 True（实测 99.29mm）——
|
||||
拿它当基准就会得到"塌陷 77mm"的假 E1。所以基准只由
|
||||
:meth:`adopt_current_grip` 明确记录（见 :meth:`_on_contract`）。
|
||||
|
||||
真正"掉了"的物理表现是**两指合到底**：开口从"工件宽度"塌到几毫米，
|
||||
是几十毫米的变化，不会被指令切换带偏。所以判据用
|
||||
@@ -349,6 +455,12 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
# `$?` 判不出成败 —— 自动化里会把"流程失败"记成"成功"。结论文件是唯一
|
||||
# 能被脚本/CI 读到的东西，由 run_gauge_assembly.sh 传入并读回。
|
||||
self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json")
|
||||
# 眼在手外标定产物的**确切路径**，由 launch 按 site.yaml 派生后传入
|
||||
# （`<臂号>_<相机>_eye_to_hand.yaml`）。为什么不让节点自己拼：
|
||||
# "臂号 = SN 末两位"这条口径只有 export_site_env 一处实现，节点再写一遍
|
||||
# 必然漂移；而漂移的症状是"标定做完了、S03 却找不到文件"，回退到 S04 跟踪，
|
||||
# 看起来像跟踪的问题。留空则退回下面那串候选名（兼容旧命名/仿真）。
|
||||
self.declare_parameter("coarse_eye_to_hand_file", "")
|
||||
|
||||
site = load_site_dict()
|
||||
# robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。
|
||||
@@ -369,6 +481,13 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
holes = int(self.get_parameter("holes_total").value)
|
||||
if holes > 0:
|
||||
overrides["holes_total"] = holes
|
||||
# 工具长：仿真里由启动脚本传**实测值**（上料脚本刚量的），真机不传（走 site）。
|
||||
# 为什么必须能覆盖：这个值决定悬停高度与 S07 下探预算，而仿真里规会在指间
|
||||
# 下滑到落座位置、下滑量随装夹变化（实测见过 243 / 254mm，差 11mm）。
|
||||
# 差 11mm 的后果见 gauge_fsm.hover_z_guard / drag_verdict 的说明。
|
||||
tool_len = float(self.get_parameter("tool_tip_below_flange_m").value)
|
||||
if tool_len > 0.0:
|
||||
overrides["tool_tip_below_flange_m"] = tool_len
|
||||
try:
|
||||
self.cfg: GaugeConfig = from_site(site, overrides=overrides or None)
|
||||
except GaugeConfigError as exc:
|
||||
@@ -496,49 +615,30 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
def _d435_ns(self, site: Dict[str, Any]) -> str:
|
||||
"""手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。
|
||||
|
||||
为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``），
|
||||
只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 ——
|
||||
同一份配置在仿真/真机之间**错而不报**地互串。
|
||||
|
||||
规则（确定性的三级）：
|
||||
1. ``arm`` 与当前臂相同的优先（仿真：sim_d435 / 真机若两台各有相机也适用）；
|
||||
2. 都没有 ``arm``（= 双臂共用的那台老相机）时回落到它；
|
||||
3. 还是多个 → 出声告警并取第一个，让人去配置里区分。
|
||||
规则（三级收敛）的**唯一实现**在 ``site_io.overhead_camera``——手眼标定
|
||||
脚本也用它取相机名。这里只负责把"配置没区分开"这件事**出声**：静默取第一台
|
||||
会让"标定用的相机"和"推理用的相机"悄悄不是同一台。
|
||||
"""
|
||||
cams = [
|
||||
cfg
|
||||
for cfg in (site.get("cameras") or {}).values()
|
||||
if isinstance(cfg, dict)
|
||||
and str(cfg.get("role") or "").startswith("overhead")
|
||||
]
|
||||
if not cams:
|
||||
return ""
|
||||
arm = str(self._robot_dict(site).get("arm") or "").strip()
|
||||
mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
|
||||
hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
|
||||
if len(hits) > 1:
|
||||
names = ", ".join(str(c.get("name") or "?") for c in hits)
|
||||
cfg, others = overhead_camera(site, self._robot_dict(site))
|
||||
if others:
|
||||
names = ", ".join([str(cfg.get("name") or "?")] + others)
|
||||
self.get_logger().warn(
|
||||
f"有 {len(hits)} 台手外相机都符合当前臂（{names}）—— 取第一个，"
|
||||
f"有 {len(others) + 1} 台手外相机都符合当前臂（{names}）—— 取第一个，"
|
||||
"但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气"
|
||||
)
|
||||
return str(hits[0].get("name") or "").strip("/")
|
||||
return str(cfg.get("name") or "").strip("/")
|
||||
|
||||
def _eye_to_hand_file(self) -> Optional[Path]:
|
||||
"""找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。
|
||||
|
||||
路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录
|
||||
启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
|
||||
规则（候选顺序 + 按仓库根解析）只有一份，在 ``site_io.resolve_eye_to_hand_file``。
|
||||
这里只负责把 launch 传入的确切路径和当前手外相机名交给它。
|
||||
"""
|
||||
root = ros2_root()
|
||||
for rel in (
|
||||
f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml",
|
||||
"handeye/results/eye_to_hand.yaml",
|
||||
):
|
||||
cand = root / rel
|
||||
if cand.is_file():
|
||||
return cand
|
||||
return None
|
||||
return resolve_eye_to_hand_file(
|
||||
ros2_root(),
|
||||
explicit=str(self.get_parameter("coarse_eye_to_hand_file").value or ""),
|
||||
camera_name=self.cfg.coarse_camera,
|
||||
)
|
||||
|
||||
def _on_robot_description(self, msg: String) -> None:
|
||||
self._robot_description = str(msg.data)
|
||||
@@ -959,6 +1059,13 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
return Event.BOOT_OK
|
||||
self._publish_gauge_result({"ok": False, "state": "S00", "failures": blockers})
|
||||
return Event.BOOT_FAIL
|
||||
# 记"夹住时的开口"（掉件判据的基准）。**放在这里而不是等到 S08**：
|
||||
# · 此刻爪里确实夹着规（仿真上料脚本刚装好，真机是人工装好才开始跑）；
|
||||
# · 基准必须是**明确知道夹住**时记的 —— 本后端的 is_gripped 是全开时
|
||||
# 都会闪 True 的推断值（实测 99.29mm），拿它当基准会造出假 E1
|
||||
# （见 GripperClient.dropped）。
|
||||
if self.grip is not None:
|
||||
self.grip.adopt_current_grip("S00 自检通过：爪里已装好通止规")
|
||||
return Event.BOOT_OK
|
||||
|
||||
# =====================================================================
|
||||
@@ -1140,20 +1247,25 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
f"{tag}: 目标 {delta_deg:+.1f}°，实测绕工具 Z {got:+.1f}°"
|
||||
f"（转轴偏差 {mis:.1f}°，阈值 {AXIS_VERIFY_TOL_DEG:.1f}°）"
|
||||
)
|
||||
if abs(got) < 3.0 and abs(delta_deg) > 10.0:
|
||||
# 完全没动：最可能是撞上关节极限（用户要求的"转到极限角"就是这种情形）
|
||||
verdict = rotation_verdict(delta_deg, got, mis, AXIS_VERIFY_TOL_DEG)
|
||||
if verdict == ROT_LIMIT:
|
||||
# 完全没动、或只走了一部分：都是**关节限位**把这段截住了
|
||||
# （2026-09-24 实测：棘轮回转每轮顶到上限 ±175°，于是下一步反向
|
||||
# 只剩 ~16° 余量）。交给调用方让余量重试（见 _ratchet_step）；
|
||||
# 调用方不处理时 FSM 会按未定义转移报错，不会静默走下去。
|
||||
self.get_logger().warn(
|
||||
f"{tag}: 指令 {delta_deg:+.1f}° 但几乎没动 —— 判为已贴关节极限"
|
||||
f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°"
|
||||
f"（差 {abs(delta_deg) - abs(got):.1f}°）—— 判为**贴到关节限位**"
|
||||
)
|
||||
return Event.JOINT_LIMIT
|
||||
if mis > AXIS_VERIFY_TOL_DEG:
|
||||
if verdict == ROT_AXIS:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 转轴偏差 {mis:.1f}° 超过阈值 —— 关节7 轴与工具 Z 不共线，"
|
||||
"以『度』为单位的动作（S01/S08/S09）都不可信，中止。"
|
||||
"先确认工具系是否垂直向下、有没有装转接件"
|
||||
)
|
||||
return Event.ABORT
|
||||
if abs(abs(got) - abs(delta_deg)) > max(AXIS_VERIFY_TOL_DEG, 0.1 * abs(delta_deg)):
|
||||
if verdict == ROT_SHORT:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°，偏差过大，中止"
|
||||
)
|
||||
@@ -1161,6 +1273,146 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
self._axis_verified = True
|
||||
return fsm_event
|
||||
|
||||
def _ratchet_step(
|
||||
self, target_rad: float, *, tag: str, fsm_event: Event, speed_deg_s: Optional[float],
|
||||
direction: int = 0,
|
||||
) -> Event:
|
||||
"""棘轮的一步"转腕"：行程不够时**张开爪让出余量**再转。
|
||||
|
||||
:param target_rad: 关节 7 的**目标角（绝对值）**，与
|
||||
:meth:`_rotate_joint_and_verify` 同一口径（那边算的就是
|
||||
``target_rad - q_now``）。**不是增量。**
|
||||
:param direction: 这一步的**工艺转向**：``+1`` 正转、``-1`` 反转、
|
||||
``0`` 交给"最短路径"。棘轮的四步一律显式给：步长接近 180° 时
|
||||
"最短路径"会被 0.1° 的读数差翻方向（详见
|
||||
:func:`arm_application.gauge_fsm.ratchet_delta_rad`）。
|
||||
|
||||
⚠️ 为什么要把口径写在这里（2026-09-24 栽过一次，S09 直接 ABORT）：
|
||||
调用方（S08/S09）给的本来就是绝对值（``q[-1] ± screw_step_rad``），
|
||||
而本函数此前把它当**增量**用（``prewind(want_deg=abs(deg(target)))``、
|
||||
让余量时又按 ``q - target`` 算新目标）。于是 S09 第一步被算成
|
||||
"要转 338.9°"（其实是目标 338.9°、增量只有 180°）⇒ 误判"行程不够"
|
||||
⇒ 让余量那一步算到限位**外面**（``158.9 − 338.9 = −180°``，比
|
||||
−175° 的限位还低 5°）⇒ 报"期望 −338.9°、实测 +26.0°，偏差过大"
|
||||
⇒ 整条流程在**最不该失败的一步** ABORT（规在爪里好好的、什么坏事都没发生）。
|
||||
两个函数各用一种口径，编译器不会说话，日志还会把"目标"和"增量"都印成
|
||||
一个数 —— 所以口径写进参数名和 docstring。
|
||||
|
||||
为什么需要"让余量"这一步：
|
||||
棘轮要求"一正一反各 180°"，而关节 7 的行程只有 ±175°（3.0543 rad）。
|
||||
拧入（S08）的**回转**那一步每轮都正好顶到上限（实测 +175.1°），
|
||||
于是 S09 要做的**反方向**第一步只剩 ~16° 余量，被限位截住。
|
||||
而这一轮其实什么坏事都没发生。
|
||||
|
||||
为什么"让余量"是免费的：
|
||||
指间**张开**时规在打滑（这正是棘轮成立的原理），所以手腕空转既不会
|
||||
把已经拧进去的螺纹转回去，也不会漏掉进度 —— 只是把行程重新分配。
|
||||
|
||||
让多少：整步（不去算亏欠了多少）。反正张开状态下这一步不产生任何装配
|
||||
效果，整步最省事也最不容易算错；重试时的那一步才是"用力"的那一步。
|
||||
|
||||
两条路：**先算**（限位已知时，提前让余量，干净）＋ **兜底**（真被限位
|
||||
截住时再让，覆盖"限位没读到 / 轨迹自己没走完"这两种情况）。
|
||||
|
||||
两条路**最后都汇到同一次让余量**：只有一次，避免"先算发现不够、
|
||||
让完还不够、再让一次"这种把行程越绕越少的做法。
|
||||
"""
|
||||
limits = self._wait_joint_limits()
|
||||
q = None
|
||||
if limits is not None:
|
||||
q, _Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
# 目标角可能落在限位**外面**（这正是棘轮要解决的问题），先折成"最近一圈"
|
||||
# 的增量（gauge_fsm.ratchet_delta_rad，含完整说明与单测）：
|
||||
# 关节在 +158.9°、目标 +338.9° ⇒ 增量 +180°（而不是 +338.9°）。
|
||||
# **方向按工艺意图给**：步长贴着 180° 时"最短路径"会被读数差翻向（R2.10）。
|
||||
delta_rad = ratchet_delta_rad(target_rad, float(q[-1]), direction)
|
||||
why = ""
|
||||
if limits is not None and abs(math.degrees(delta_rad)) > 1e-6:
|
||||
plan = prewind(
|
||||
q_now_rad=float(q[-1]), want_deg=abs(math.degrees(delta_rad)),
|
||||
direction=1 if delta_rad > 0 else -1,
|
||||
limits=limits, margin_deg=RATCHET_LIMIT_MARGIN_DEG,
|
||||
)
|
||||
if plan.clamped:
|
||||
why = (
|
||||
f"行程不够（关节7 在 {math.degrees(float(q[-1])):+.1f}°，"
|
||||
f"这一步要转 {math.degrees(delta_rad):+.1f}°"
|
||||
f"（目标 {math.degrees(float(target_rad)):+.1f}°），限位 "
|
||||
f"{math.degrees(limits[0]):+.0f}~{math.degrees(limits[1]):+.0f}°）"
|
||||
)
|
||||
if not why:
|
||||
ev = self._rotate_joint_and_verify(
|
||||
target_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s
|
||||
)
|
||||
if ev is not Event.JOINT_LIMIT:
|
||||
return ev
|
||||
why = "轨迹被关节限位截住（算着够、实际没走完）"
|
||||
return self._ratchet_rewind_then(
|
||||
delta_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s, why=why,
|
||||
)
|
||||
|
||||
def _ratchet_rewind_then(
|
||||
self, delta_rad: float, *, tag: str, fsm_event: Event,
|
||||
speed_deg_s: Optional[float], why: str,
|
||||
) -> Event:
|
||||
"""张开爪 → 反向让一步 → 夹紧 → 转原来那一步。
|
||||
|
||||
:param delta_rad: **要转的增量**（不是目标角，见 :meth:`_ratchet_step`）。
|
||||
让余量时按它反着退一步，重试时再按它前进 —— 两次都以**当时的**
|
||||
关节角为基准重新算目标，所以让完余量后关节会回到原处、工具净转
|
||||
``delta_rad``。把"目标"固定成一开始那个绝对值是错的：让完余量之后
|
||||
那个目标已经在另一圈上了（差 360°）。
|
||||
"""
|
||||
if self.grip is None:
|
||||
self.get_logger().error(f"{tag}: {why}，但没有夹爪客户端，无法让余量 → 中止")
|
||||
return Event.ABORT
|
||||
self.get_logger().info(
|
||||
f"{tag}: {why} —— 张开爪让出一步（此刻规在打滑，不会动到螺纹）再转"
|
||||
)
|
||||
if not self.grip.open():
|
||||
self.get_logger().error(f"{tag}: 需要张爪让余量，但开爪失败 → 中止")
|
||||
return Event.ABORT
|
||||
time.sleep(self.cfg.screw_release_wait_s)
|
||||
q, _Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
float(q[-1]) - delta_rad, tag=f"{tag}_rewind",
|
||||
fsm_event=fsm_event, speed_deg_s=speed_deg_s,
|
||||
)
|
||||
if ev is not fsm_event:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 让余量那一步也没走开（{ev}）—— 关节7 的行程真用尽了，中止。\n"
|
||||
f" 排查：site.yaml 的 gauge_assembly.screw_step_deg 太接近关节行程了"
|
||||
f"（Rizon4 的关节7 是 ±175°），棘轮一步要 {math.degrees(abs(delta_rad)):.0f}°，"
|
||||
f"留给'回转'的余量本来就不够。\n"
|
||||
f" 改法：把 screw_step_deg 调小（例如 120°），并保证 "
|
||||
f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍** —— "
|
||||
f"累计必须转整数圈，否则规会停在螺纹中间（S09 拧出也就少转几分之一圈）。"
|
||||
)
|
||||
return Event.ABORT
|
||||
if not self.grip.close():
|
||||
self.get_logger().error(f"{tag}: 让完余量后夹紧失败 → 中止")
|
||||
return Event.ABORT
|
||||
# 重试目标**按让完余量后的位置重算**：让余量退了一步，原来那个绝对目标
|
||||
# 现在差了 360°（正好一整圈），照原样下发会让关节反向绕一圈回去。
|
||||
q2, _Tee2 = self._wait_state(3.0)
|
||||
if q2 is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
float(q2[-1]) + delta_rad, tag=f"{tag}_retry",
|
||||
fsm_event=fsm_event, speed_deg_s=speed_deg_s,
|
||||
)
|
||||
if ev is not fsm_event:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 让完余量重试仍然没走到位（{ev}）—— 中止"
|
||||
f"（不是限位问题，先查是不是有东西卡住/螺纹咬死）"
|
||||
)
|
||||
return Event.ABORT
|
||||
return fsm_event
|
||||
|
||||
# =====================================================================
|
||||
# S02 夹持确认（预留）
|
||||
# =====================================================================
|
||||
@@ -1200,11 +1452,15 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
ns = self.cfg.coarse_camera
|
||||
he = self._eye_to_hand_file()
|
||||
if he is None:
|
||||
expect = str(
|
||||
self.get_parameter("coarse_eye_to_hand_file").value or ""
|
||||
).strip() or f"handeye/results/{ns}_eye_to_hand.yaml"
|
||||
self.get_logger().warn(
|
||||
"S03 需要手外相机的 eye-to-hand 标定文件，现在没有 —— "
|
||||
f"按兜底逻辑回 S04 跟踪。\n"
|
||||
f" 相机 {ns} 的标定产物应放在 handeye/results/<名>_eye_to_hand.yaml；\n"
|
||||
f" 做法：bash scripts/run_handeye.sh --mode eye_to_hand（标完再跑本流程）"
|
||||
f" 应该生成：{expect}\n"
|
||||
f" 做法：bash scripts/run_handeye.sh --mode eye_to_hand "
|
||||
f"teach/merge/calibrate（标完再跑本流程）"
|
||||
)
|
||||
return Event.COARSE_FAIL
|
||||
if not self._wait_coarse_frame(ns):
|
||||
@@ -1354,7 +1610,9 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
return Event.TIMEOUT
|
||||
code, q, Tee, final = self._run_locate(q, Tee)
|
||||
if code != 0 or not final:
|
||||
self.get_logger().error(f"S05 精拍定位失败 code={code}")
|
||||
self.get_logger().error(
|
||||
f"S05 精拍定位失败 code={code}{_LOCATE_CODE_HINT.get(code, '')}"
|
||||
)
|
||||
return Event.TIMEOUT
|
||||
self._snap_final = dict(final)
|
||||
self._snap_pose = Tee.copy()
|
||||
@@ -1535,6 +1793,9 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
z0 = float(Tee[2, 3])
|
||||
step_m = self.cfg.contact_step_mm / 1000.0
|
||||
max_depth_m = self.cfg.contact_max_depth_mm / 1000.0
|
||||
# 台肩落面所需的下压量。在下面几何自检里算出来；**要留到接触判据那里用**
|
||||
# （看 hole_entry_verdict：下压深度远小于它 ⇒ 规没进孔）。
|
||||
needed_m: Optional[float] = None
|
||||
|
||||
# ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------
|
||||
# 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面
|
||||
@@ -1663,6 +1924,11 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N"
|
||||
f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
|
||||
)
|
||||
# 下压这么浅就"接触" ⇒ 尖端顶在工件上表面，没进孔。必须在这里
|
||||
# 拦下：放它进 S08 只会干转几轮、憋住，最后报一句"转轴偏差"
|
||||
# 把所有人引去查工具系。见 hole_entry_verdict 的说明。
|
||||
if self._hole_lost_guard(z0, Tee, needed_m, q, "腕部 FT"):
|
||||
return Event.HOLE_LOST
|
||||
# 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限**
|
||||
# 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限
|
||||
# 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。
|
||||
@@ -1709,6 +1975,9 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
|
||||
)
|
||||
# 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。
|
||||
# 同样先过"是不是根本没进孔"这道闸（见 hole_entry_verdict）。
|
||||
if self._hole_lost_guard(z0, Tee, needed_m, q, "触觉切向"):
|
||||
return Event.HOLE_LOST
|
||||
self._relieve_after_contact(q, Tee, "触觉切向", ft_ref)
|
||||
return Event.CONTACT_OK
|
||||
if fn - self._fn_rest > self.cfg.contact_safe_delta:
|
||||
@@ -1745,6 +2014,47 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return Event.TIMEOUT
|
||||
|
||||
def _hole_lost_guard(
|
||||
self,
|
||||
z0: float,
|
||||
Tee: np.ndarray,
|
||||
needed_m: Optional[float],
|
||||
q: np.ndarray,
|
||||
why: str,
|
||||
) -> bool:
|
||||
"""S07 判到接触时：下压深度是不是**远小于**几何预期（= 规没进孔）。
|
||||
|
||||
返回 True 表示"确实没进孔"，调用方要抬臂并返回 ``Event.HOLE_LOST``。
|
||||
|
||||
完整来龙去脉在 ``gauge_fsm.hole_entry_verdict`` 的 docstring 里，这里只
|
||||
留现场要看的那几句：工件在 S08/S09/S10 会被工具推走，而 k2 的孔位是
|
||||
S05 一次性定的 ⇒ 工件一动孔位就失效 ⇒ 规落在孔旁边的平面上。
|
||||
不拦的话会干转 5 轮，最后报一句"转轴偏差…先确认工具系有没有装转接件"，
|
||||
把所有人引去查工具系（实测 2026-09-28）。
|
||||
"""
|
||||
if needed_m is None:
|
||||
# 没算出几何预期（工件上表面 z 没标定且 S05 也没给）—— 没有判据就别猜。
|
||||
return False
|
||||
descended_m = abs(float(Tee[2, 3]) - z0)
|
||||
if hole_entry_verdict(descended_m, needed_m) != ENTRY_HIGH:
|
||||
return False
|
||||
self.get_logger().error(
|
||||
f"S07 接触（{why}）发生在下压 {descended_m * 1000:.2f}mm 处，"
|
||||
f"而按几何「台肩落面」需要 {needed_m * 1000:.1f}mm —— 只走了 "
|
||||
f"{descended_m / needed_m * 100.0:.0f}%。\n"
|
||||
" **规没有进孔，尖端顶在工件上表面**（正常入孔要压到台肩落面那一下）。\n"
|
||||
" 最可能的原因：工件被工具推走了，而本孔的坐标是 **S05 一次性定下**的 ——\n"
|
||||
" 工件一动，那个坐标就失效（S06 的「实测残差」发现不了：它比的是\n"
|
||||
" 「命令位置 vs 实际位置」，不是「规尖 vs 真实孔位」）。\n"
|
||||
" 所以**不要**去查工具系/转接件/力阈值（那几个都是好的）。\n"
|
||||
" 正确处置：把臂退开、确认工件的实际位置；能复位就复位工件，然后\n"
|
||||
" 从 S05 重新拍照定位（重跑流程即会重新定位）。\n"
|
||||
" 若要治根：换孔前复核工件没动（当前只有 S05 阶段有 drag 看门狗）。\n"
|
||||
" 抬起并中止，不带着失效的孔位进 S08。"
|
||||
)
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return True
|
||||
|
||||
def _relieve_after_contact(
|
||||
self,
|
||||
q: np.ndarray,
|
||||
@@ -1879,6 +2189,29 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
# =====================================================================
|
||||
# S08 旋转装配循环（棘轮）
|
||||
# =====================================================================
|
||||
def _log_regrip(self, tag: str) -> None:
|
||||
"""把"这一轮夹紧后的开口"打出来（含相对夹住时的塌陷量）。
|
||||
|
||||
为什么每轮都要打（2026-09-24 实测）：
|
||||
S08/S09 每轮都是"慢转 → 松爪 → 回转 → 夹紧"，而 `close` 在仿真里
|
||||
是"位置控制走到全合 0mm" —— **真机走到力上限就停，仿真会一路挤进规里**。
|
||||
只报一句"通止规掉了(E1)"时，人分不出"真掉了"和"被挤小了"。
|
||||
把开口与塌陷量打出来一眼就能分辨：
|
||||
· 塌陷量逐轮变大、开口还在十几毫米 → 是**被挤**（规还在指间）
|
||||
· 开口塌到接近 0（两指合到底） → 是**真掉了**
|
||||
"""
|
||||
if self.grip is None:
|
||||
return
|
||||
ap = self.grip.settle(settle_s=1.0) # 服务返回时动作还没做完，等它停
|
||||
if ap is None:
|
||||
return
|
||||
gp = self.grip.grip_aperture()
|
||||
col = self.grip.collapse_mm()
|
||||
detail = ""
|
||||
if gp is not None and col is not None:
|
||||
detail = f"（夹住时 {gp:.2f}mm，塌陷 {col:+.2f}mm）"
|
||||
self.get_logger().info(f"{tag}：开口 {ap:.2f}mm{detail}")
|
||||
|
||||
def _step_screw_in(self) -> Event:
|
||||
hole = self._hole_name()
|
||||
self._state(f"S08_screw_in_{hole}")
|
||||
@@ -1895,6 +2228,20 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
f"停止判据「切向 >+{self.cfg.screw_shear_delta:.0f} "
|
||||
f"且法向 >+{self.cfg.screw_normal_delta:.0f}」"
|
||||
)
|
||||
# 步长 vs 关节行程：**在这里算一次**，不要等第 3 轮贴限位再炸。
|
||||
# 2026-09-24 实测：180° 的步长配 ±175° 的关节 7，第 3 轮 ABORT，
|
||||
# 日志是一长串"贴限位 → 让余量 → 还是贴限位"，真因（配置）藏在最后。
|
||||
travel = ratchet_travel_ok(
|
||||
self.cfg.screw_step_deg, self._wait_joint_limits(), RATCHET_LIMIT_MARGIN_DEG
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"S08 行程核算：一步棘轮要 {travel.need_deg:.0f}°，"
|
||||
f"关节7 行程 {travel.travel_deg:.0f}°"
|
||||
f"（最大可用步长 {travel.max_step_deg:.0f}°）—— {'放得下' if travel.ok else '放不下'}"
|
||||
)
|
||||
if not travel.ok:
|
||||
self.get_logger().error(f"S08 配置自检不通过（先改配置，别硬跑）：{travel.why}")
|
||||
return Event.ABORT
|
||||
for cycle in range(self.cfg.screw_max_cycles):
|
||||
q, Tee = self._wait_state(5.0)
|
||||
if q is None or Tee is None:
|
||||
@@ -1909,9 +2256,10 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
# --- 四步原子动作，顺序固定（慢转 → 松爪 → 快回转 → 夹紧）---
|
||||
# 慢转是**顺时针**：与 S01 的逆时针预绕相反，这样预绕行程就是
|
||||
# 这一整段拧入的回转空间（S01 不预绕，转两圈就贴限位）。
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) - self.cfg.screw_step_rad, tag=f"S08_slow_{cycle}",
|
||||
fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
|
||||
direction=-1, # 顺时针拧入（与 S01 的预绕相反）
|
||||
)
|
||||
if ev is not Event.SCREW_DONE:
|
||||
return ev
|
||||
@@ -1924,15 +2272,17 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
q, Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) + self.cfg.screw_step_rad, tag=f"S08_back_{cycle}",
|
||||
fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
|
||||
direction=+1, # 回转：与慢转正好相反
|
||||
)
|
||||
if ev is not Event.SCREW_DONE:
|
||||
return ev
|
||||
if not self.grip.close():
|
||||
self.get_logger().error("S08 夹紧失败 —— 下一轮慢转会打滑磨螺纹")
|
||||
return Event.ABORT
|
||||
self._log_regrip(f"S08 第 {cycle + 1} 轮夹紧后")
|
||||
else:
|
||||
self.get_logger().error("S08 没有夹爪客户端，无法松爪/夹紧，中止")
|
||||
return Event.ABORT
|
||||
@@ -1961,9 +2311,17 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
self._progress(f"{hole} 拧入到位（{cycle + 1} 轮）")
|
||||
return Event.SCREW_STOP
|
||||
if self.grip is not None and self.grip.dropped():
|
||||
ap = self.grip.aperture()
|
||||
gp = self.grip.grip_aperture()
|
||||
self.get_logger().error(
|
||||
"S08 通止规掉了（E1）—— 回 S01 流程：张开爪、提示人工放好，"
|
||||
"确认后继续（按看板：该孔记为完成，换下一个孔）"
|
||||
"确认后继续（按看板：该孔记为完成，换下一个孔）\n"
|
||||
f" 开口：夹住时 {gp if gp is None else f'{gp:.2f}mm'}"
|
||||
f" → 现在 {ap if ap is None else f'{ap:.2f}mm'}"
|
||||
f"（判据：塌陷 ≥ {GripperClient.DROP_COLLAPSE_MM:.1f}mm）\n"
|
||||
" 分辨真掉还是被挤：开口塌到接近 0 = 两指合到底（真掉）；\n"
|
||||
" 开口还有十几毫米、只是比夹住时小了几毫米 = 被**挤**"
|
||||
"（仿真位置控制的「全合」不知道「挡住就停」，见 GripperClient.dropped）"
|
||||
)
|
||||
self._e1_pending = True
|
||||
return Event.GAUGE_DROPPED
|
||||
@@ -1994,9 +2352,10 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
# 反向：慢转是逆时针（与 S08 相反），空手回转是顺时针
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) + self.cfg.screw_step_rad, tag=f"S09_slow_{cycle}",
|
||||
fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
|
||||
direction=+1, # 逆时针拧出
|
||||
)
|
||||
if ev is not Event.UNSCREW_DONE:
|
||||
return ev
|
||||
@@ -2008,17 +2367,64 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
q, Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) - self.cfg.screw_step_rad, tag=f"S09_back_{cycle}",
|
||||
fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
|
||||
direction=-1, # 回转：与慢转相反
|
||||
)
|
||||
if ev is not Event.UNSCREW_DONE:
|
||||
return ev
|
||||
if not self.grip.close():
|
||||
return Event.ABORT
|
||||
self._log_regrip(f"S09 第 {cycle + 1} 轮夹紧后")
|
||||
self._lift_clear_of_hole(hole)
|
||||
self._progress(f"{hole} 已拧出")
|
||||
return Event.UNSCREW_DONE
|
||||
|
||||
def _lift_clear_of_hole(self, hole: str) -> None:
|
||||
"""拧出后**先竖直把规提出孔**，之后才允许任何横向动作。
|
||||
|
||||
**为什么必须有这一步**（2026-09-24 仿真实测，两次栽在同一个地方）：
|
||||
1. 仿真里**没有螺纹几何**：S09 的"拧出"只是让规在孔里转，规**仍然插在
|
||||
孔里**（真机会沿螺纹退出来几个毫米，也没退出多少）。
|
||||
2. 于是紧跟着的横向动作（S10 换孔 / S11 回 home）会把规从孔里**拽**
|
||||
出来 —— 实测抓取锁被拽断、规掉在台面上，而下游只表现为
|
||||
"下一孔 S07 下探 35mm 判不到接触"（工具上已经没规了），
|
||||
日志完全指不到真因。
|
||||
3. 真机也怕这个动作：螺纹刚脱开时横向别住最容易啃伤螺纹。标准做法就是
|
||||
**先竖直退出来，再走 XY**。
|
||||
|
||||
高度提到**跟踪高**（``_track_z()`` = 孔悬停高 + 50mm）而不是写死毫米数：
|
||||
跟踪高是这条流程里"工具已离开工件"的既有定义（``_ensure_clearance``
|
||||
也用它），拿它当判据不用再维护第二套口径。实测：不提出时规尖比工件面
|
||||
**低 30mm**，而 S10 的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和
|
||||
"干净退出"的分界，所以必须是**先后有序**的两个动作，不能靠斜线一起走。
|
||||
|
||||
失败只告警（不失败）：这一刻孔里已经没有规要保护了，硬失败会把一次成功
|
||||
的装配变成失败；后面的 S10/S11 仍会走到安全位。
|
||||
"""
|
||||
try:
|
||||
q, Tee = self._wait_state(3.0)
|
||||
if q is None or Tee is None:
|
||||
return
|
||||
tz = self._track_z()
|
||||
z = float(Tee[2, 3])
|
||||
if z >= tz - 0.008:
|
||||
return
|
||||
self.get_logger().info(
|
||||
f"S09 {hole} 拧出后竖直提出：Z {z:.3f} → {tz:.3f}m"
|
||||
"（先离开孔口，再做横向动作）"
|
||||
)
|
||||
_, q2, _Tee2 = 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=f"S09_exit_{hole}", v=self.move_speed, max_hops=8,
|
||||
hold_xy=True, settle_s=0.25,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f"S09 {hole} 提出孔口失败（继续走 S10/S11）：{exc}")
|
||||
|
||||
# =====================================================================
|
||||
# S10 换孔 / S11 复位
|
||||
# =====================================================================
|
||||
|
||||
@@ -254,7 +254,9 @@ DEFAULTS: Dict[str, Any] = {
|
||||
"contact_hold_n": 5.0,
|
||||
"contact_safe_delta": 2000.0,
|
||||
# S08
|
||||
"screw_step_deg": 180.0,
|
||||
# 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
|
||||
# 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
|
||||
"screw_step_deg": 120.0,
|
||||
"screw_speed_deg_s": 30.0,
|
||||
"screw_back_speed_deg_s": 120.0,
|
||||
"screw_max_cycles": 12,
|
||||
|
||||
@@ -79,6 +79,10 @@ class Event(str, Enum):
|
||||
|
||||
CONTACT_OK = "contact_ok"
|
||||
FORCE_OVER_LIMIT = "force_over_limit" # 力超安全上限 → 抬起
|
||||
#: S07 判到接触、但**下压深度远小于几何预期** ⇒ 规没进孔，顶在工件上表面。
|
||||
#: 单列一个事件是为了让结论与日志都是实话：复用 FORCE_OVER_LIMIT 会把这种
|
||||
#: 情况报成"力超安全上限"，而它的力根本没超限 —— 又是一句误导。
|
||||
HOLE_LOST = "hole_lost"
|
||||
|
||||
SCREW_DONE = "screw_done"
|
||||
SCREW_STOP = "screw_stop" # 力判据满足：切向力>阈值 且 法向力突增
|
||||
@@ -158,6 +162,7 @@ TRANSITIONS: Dict[Tuple[State, Event], State] = {
|
||||
# --- S07 柔顺接触 ---
|
||||
(State.CONTACT, Event.CONTACT_OK): State.SCREW_IN,
|
||||
(State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT,
|
||||
(State.CONTACT, Event.HOLE_LOST): State.ABORT,
|
||||
# --- S08 旋转装配循环 ---
|
||||
(State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW,
|
||||
(State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW,
|
||||
@@ -406,6 +411,40 @@ def wrench_live(age_s: Optional[float]) -> bool:
|
||||
return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S
|
||||
|
||||
|
||||
#: 判定"六个分量全为 0"用的容差 [N]/[Nm]。真机 FT 的噪声量级是 1e-2~1e-1，
|
||||
#: 所以 1e-6 足够区分"真的是 0（数据没出来）"与"很小但活的读数"。
|
||||
WRENCH_ZERO_EPS: float = 1.0e-6
|
||||
|
||||
|
||||
def wrench_all_zero(
|
||||
fx: float,
|
||||
fy: float,
|
||||
fz: float,
|
||||
tx: float,
|
||||
ty: float,
|
||||
tz: float,
|
||||
eps: float = WRENCH_ZERO_EPS,
|
||||
) -> bool:
|
||||
"""六个分量**同时**为 0（容差 ``eps``）⇒ 这是"数据没出来"的指纹，不是"安静"。
|
||||
|
||||
与 :func:`wrench_live` 的分工（两个都要有，缺一不可）：
|
||||
|
||||
* ``wrench_live`` 只看"够不够新"，**故意不看数值** —— 静止时轴向力本来就接近 0，
|
||||
拿数值判活会把"安静的传感器"误判成掉线（2026-09-24 触觉上踩过）。
|
||||
* 但一个**带着负载**的腕部 FT 不可能六个分量**同时**恰好为 0：重力、装配预紧、
|
||||
传感器零漂至少让某个分量不为 0。所以"全零"是另一种失效 —— **数据源没出数**
|
||||
（话题名对、但内容是空的；或 RDK 侧没启用该传感）。
|
||||
|
||||
2026-10-08 真机实测：应用在用的 ``/<sn>/ft_sensor_wrench`` 就是恒 (0,0,0)，
|
||||
于是"``abs(fz−基线)>2.5N`` 判接触"的兜底**永远触发不了** —— 而日志里一切正常
|
||||
（话题收得到、回调在跑、``wrench_live`` 报"活"），属于最危险的静默失效。
|
||||
改用 ``/<sn>/external_wrench_in_tcp`` 后有数（≈负载 54N）即可恢复兜底。
|
||||
|
||||
所以：``wrench_live`` 通过 **且** ``wrench_all_zero`` 为真 ⇒ 判为"数据源是空的"。
|
||||
"""
|
||||
return all(abs(float(v)) <= eps for v in (fx, fy, fz, tx, ty, tz))
|
||||
|
||||
|
||||
#: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
|
||||
#: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
|
||||
#: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
|
||||
@@ -513,6 +552,209 @@ def tool_clearance_m(
|
||||
return tip_z - float(surface_z_m)
|
||||
|
||||
|
||||
def hover_z_for_tool_clearance(
|
||||
surface_z_m: float,
|
||||
tool_tip_below_flange_m: float,
|
||||
clearance_m: float,
|
||||
) -> float:
|
||||
"""让工具尖端停在工件面上方 ``clearance_m`` 所需的**法兰**高度 [m]。
|
||||
|
||||
就是 ``tool_clearance_m`` 的逆运算 —— 写成一个函数是为了让"该用多高"这个
|
||||
结论只有一个来源（下面 ``hover_z_guard`` 与调用方都用它，不各推一遍）。
|
||||
"""
|
||||
return float(surface_z_m) + float(tool_tip_below_flange_m) + float(clearance_m)
|
||||
|
||||
|
||||
def hover_z_guard(
|
||||
hover_z_m: float,
|
||||
surface_z_m: float,
|
||||
tool_tip_below_flange_m: float,
|
||||
want_clearance_m: float,
|
||||
) -> Tuple[float, str]:
|
||||
"""二阶段悬停高度的自检 → ``(该用的悬停高度, 说明; 不用改时说明是空串)``。
|
||||
|
||||
**为什么必须有这条**（2026-09-24 实测栽在这上面）：
|
||||
|
||||
悬停高度是**按相机**定的（腕部相机要离工件面 ~0.17m 才看得清孔），
|
||||
而工具（夹爪 + 通止规）可能比相机长得多 —— 仿真里实测
|
||||
``tool_tip_below_flange_m=0.245``，而悬停算的是
|
||||
``0.412 + 0.17 + 0.05 = 0.632`` ⇒ 工具尖端在工件上表面**下面 25mm**。
|
||||
|
||||
后果不在 S07（S07 有自己的几何自检），而在**更早的 S05**：
|
||||
"悬停"这一步本身就把工具捅进工件里了，于是 S05 的横向对准等于
|
||||
拿规尖去拨工件 —— 实测把工件从 x=0.520 拖到 0.4284（**92mm**）。
|
||||
而真值孔位是按工件的**实测**位姿算的（``sim_live_pose``，这是对的），
|
||||
工件一走，孔位就跟着工具走 ⇒ 残差恒定在 13mm、迭代 9 次永不收敛
|
||||
（日志：``[fine 3..8] 残差=13.25/13.19/…/11.79mm``）。
|
||||
现象看起来像"手眼标定坏了 / 检测不认孔"。
|
||||
|
||||
真机同参数同结果：这不是仿真专有，只要"悬停按相机定、工具比相机长"就会这样。
|
||||
|
||||
判据取"明显低于目标间隙"而不是"一毫米都不许低"：配置的悬停与实测的
|
||||
工件面 z 都有几毫米的不确定度，拿 0 当界会把正常配置判成错的。
|
||||
"""
|
||||
clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
|
||||
if clear_m >= float(want_clearance_m) - 0.001:
|
||||
return float(hover_z_m), ""
|
||||
need_z = hover_z_for_tool_clearance(surface_z_m, tool_tip_below_flange_m, want_clearance_m)
|
||||
why = (
|
||||
f"悬停高度会让工具尖端钻进工件：法兰 {float(hover_z_m):.4f} − 工具长 "
|
||||
f"{float(tool_tip_below_flange_m) * 1000:.1f}mm = 尖端 "
|
||||
f"{float(hover_z_m) - float(tool_tip_below_flange_m):.4f}，工件上表面 "
|
||||
f"{float(surface_z_m):.4f} → 尖端在面{'下' if clear_m < 0 else '上仅'} "
|
||||
f"{abs(clear_m) * 1000:.1f}mm（要求 ≥ {float(want_clearance_m) * 1000:.1f}mm）。\n"
|
||||
f" 悬停高度通常是按**相机**视距定的，而工具比相机长 —— 两者冲突时必须"
|
||||
f"先保证不碰工件。\n"
|
||||
f" 已按几何抬到 {need_z:.4f}（= {float(surface_z_m):.3f} 上表面 + "
|
||||
f"{float(tool_tip_below_flange_m) * 1000:.0f}mm 工具 + "
|
||||
f"{float(want_clearance_m) * 1000:.1f}mm 间隙）。\n"
|
||||
f" 要固定这个值：改 site.yaml 的 robots[<sn>].servo.hole_hover_z_m = "
|
||||
f"{need_z:.3f}（以及同组件的 ee_z_m ≥ 它 +0.05）。"
|
||||
)
|
||||
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 的说明）：
|
||||
# S05 的对准是"看着孔走"的闭环，而孔位是**工件上**的。工具一旦碰到工件，
|
||||
# 闭环追的就不再是孔，而是"自己推着走的工件" —— 残差恒定（本轮 11mm）、
|
||||
# 迭代用满，日志报的是"检查手眼/相机安装"，与真因（工具比相机长、
|
||||
# 悬停时已经插进工件）差着十万八千里。
|
||||
# 取 3mm：工件是刚体，两拍之间它整体挪动 >3mm 只可能是被推/被撞（正常加工
|
||||
# 过程里它一动不动）。同一对孔的两拍测量噪声在 0.2mm 量级，离阈值很远。
|
||||
SNAP_DRAG_TOL_M: float = 0.003
|
||||
#: 单拍就这么大 ⇒ 不必等第二拍确认（刚体不会自己"挪回去"）。
|
||||
SNAP_DRAG_HARD_M: float = 0.006
|
||||
|
||||
|
||||
#: "已经到位"的容差：避免 ±1e-9 的浮点噪声变成"再转一整圈"。
|
||||
_RATCHET_FULL_TURN_EPS: float = 1e-9
|
||||
|
||||
|
||||
def ratchet_delta_rad(target_rad: float, q_now_rad: float, direction: int = 0) -> float:
|
||||
""""目标角 − 当前角"，折成**一次**能转到的增量 [rad]。
|
||||
|
||||
**为什么要有这个函数**（2026-09-24 实测：棘轮把整条流程 ABORT 在 S09 第一步）：
|
||||
|
||||
S08/S09 给棘轮的是**关节 7 的目标角（绝对值）**（``q[-1] ± step``），
|
||||
而棘轮此前把它当**增量**用。于是"关节在 +158.9°、目标 +338.9°"被算成
|
||||
"要转 338.9°"（实际增量只有 180°）⇒ 误判行程不够 ⇒ 让余量那一步算到
|
||||
限位外面（``158.9 − 338.9 = −180°``，比 −175° 还低）⇒ 报
|
||||
"期望 −338.9°、实测 +26.0°，偏差过大" ⇒ ABORT。现场什么坏事都没发生。
|
||||
|
||||
差值必须折一圈：目标常常**故意**超出限位（这正是要"让余量"的原因），
|
||||
``+338.9°`` 与 ``−21.1°`` 是同一个关节位置。
|
||||
|
||||
**为什么还要 direction**（2026-09-24 又一次实测：S08 第 3 轮 ABORT）：
|
||||
|
||||
不给方向时这里取"最短的那条路"，而名义步长**正好是 180°** 时，
|
||||
哪条路更短取决于 0.1° 级的读数差：调用方按自己读到的 ``q`` 算出目标
|
||||
``−18.0°``，棘轮再用**稍晚一点**读到的 ``q=+162.1°`` 去算差值，
|
||||
``−180.1°`` 折一圈就成了 ``+179.9°`` —— 方向**翻了个个儿**。
|
||||
于是棘轮朝螺纹的反方向走，接着贴限位、让余量、重试，净位移 0 ⇒ ABORT。
|
||||
|
||||
棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给
|
||||
"谁更短"这种数值巧合决定，所以由调用方明确给出 ``direction``。
|
||||
|
||||
注：``direction=0``（默认）保留旧的"最短路径"语义，给"目标本来就明确
|
||||
在某一侧"的调用方用；棘轮的四个原子步一律显式给方向。
|
||||
|
||||
:param target_rad: 目标角（绝对值，可以是限位外的角度）。
|
||||
:param q_now_rad: 当前关节角。
|
||||
:param direction: 想要的转向：``+1`` 正转、``-1`` 反转、``0`` 取最短路径。
|
||||
:returns: 增量 [rad]。``direction=0`` 时落在 ``(−π, π]``；给了方向时落在
|
||||
``[0, 2π)``（正转）或 ``(−2π, 0]``（反转）——**一定**沿指定方向，
|
||||
且是能落到目标角的最小那一步。**同一个目标、同一个当前位置**只会有一个
|
||||
增量 —— 棘轮的"让余量"与"重试"都按它算，两次不会互相矛盾。
|
||||
"""
|
||||
base = float(target_rad) - float(q_now_rad)
|
||||
two_pi = 2.0 * math.pi
|
||||
if direction == 0:
|
||||
return math.remainder(base, two_pi)
|
||||
# 先折到 [0, 2π)，再按想要的转向取那一侧（与目标角差正好一整圈，是同一个位置）。
|
||||
pos = math.fmod(base, two_pi)
|
||||
if pos < 0.0:
|
||||
pos += two_pi
|
||||
if pos >= two_pi - _RATCHET_FULL_TURN_EPS:
|
||||
pos = 0.0 # 已经（几乎）在目标上：不要为了凑方向白转一整圈
|
||||
if direction > 0:
|
||||
return pos
|
||||
return pos - two_pi if pos > 0.0 else 0.0
|
||||
|
||||
|
||||
|
||||
def drag_verdict(
|
||||
mid_now: Tuple[float, float],
|
||||
mid_prev: Optional[Tuple[float, float]],
|
||||
mid_prev2: Optional[Tuple[float, float]] = None,
|
||||
*,
|
||||
tol_m: float = SNAP_DRAG_TOL_M,
|
||||
hard_m: float = SNAP_DRAG_HARD_M,
|
||||
) -> Tuple[bool, float]:
|
||||
"""两孔中点是否在两拍之间"整体挪动" → ``(是否判定为被推走, 本拍位移 m)``。
|
||||
|
||||
**为什么看"中点"而不是看目标孔**：目标孔是**追**的对象，工具一动它就该动
|
||||
（对准本意如此），看它分不出"正常对准"和"推着工件走"。而两孔**中点**在
|
||||
对准过程中不该动 —— 它只有工件整体平移时才会动。所以中点位移干净地对应
|
||||
"工件被搬动了"。
|
||||
|
||||
**为什么看"两孔中点"而不是单孔**：单孔在相机里会因分辨率/遮挡抖动，且
|
||||
目标孔本来就在被逼近；中点是两者的平均，噪声更小、语义更明确。
|
||||
|
||||
:param mid_now: 本拍两孔中点 (x, y)[m]（基座系）。
|
||||
:param mid_prev: 上一拍中点；``None`` = 还没有上一拍（第一拍不判）。
|
||||
:param mid_prev2: 上上拍中点。用来确认"位移是持续的"（单拍可能是误检）。
|
||||
:returns: ``(判定, 本拍位移 m)``。
|
||||
"""
|
||||
if mid_prev is None:
|
||||
return False, 0.0
|
||||
dx = float(mid_now[0]) - float(mid_prev[0])
|
||||
dy = float(mid_now[1]) - float(mid_prev[1])
|
||||
d1 = math.hypot(dx, dy)
|
||||
if d1 >= float(hard_m):
|
||||
return True, d1
|
||||
if d1 < float(tol_m) or mid_prev2 is None:
|
||||
return False, d1
|
||||
px = float(mid_prev[0]) - float(mid_prev2[0])
|
||||
py = float(mid_prev[1]) - float(mid_prev2[1])
|
||||
d2 = math.hypot(px, py)
|
||||
# 第二拍确认：上一拍也挪了（≥ 阈值的一半），且方向一致（点积 > 0）。
|
||||
# 为什么允许方向只"大致一致"：工件被推着走时前后两拍的推动量本来就不同
|
||||
# （工件一让开，工具就少推一点），拿夹角严格相等会漏判。
|
||||
if d2 >= float(tol_m) * 0.5 and (dx * px + dy * py) > 0.0:
|
||||
return True, d1
|
||||
return False, d1
|
||||
|
||||
|
||||
def descend_needed_m(
|
||||
flange_z_m: float,
|
||||
tool_tip_below_flange_m: float,
|
||||
@@ -555,6 +797,58 @@ def descend_budget_m(needed_m: float) -> float:
|
||||
return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M)
|
||||
|
||||
|
||||
#: ``hole_entry_verdict`` 的结论。
|
||||
ENTRY_OK = "ok" # 接触深度与几何预期相符 ⇒ 规是从孔里下去的、台肩落面
|
||||
ENTRY_HIGH = "high" # 早早就接触 ⇒ 尖端顶在工件**上表面**上，根本没进孔
|
||||
|
||||
#: 判"没进孔"的下压深度比例阈值。
|
||||
#: 实测两个锚点（2026-09-28，仿真）：
|
||||
#: · 正常入孔：下压 28.6mm / 需要 31.9mm ⇒ 0.90
|
||||
#: · 顶在平面上：下压 0.90mm / 需要 31.9mm ⇒ 0.03
|
||||
#: 两者差着一个台肩高度（30mm），取 0.5 时两侧余量都很大（正常那侧要掉到
|
||||
#: 一半才会误判）。**不要**为了"更灵敏"把它往 0.9 推 —— 台肩倒角没完全落面、
|
||||
#: 板厚公差都会让正常值在 0.85~0.95 之间晃。
|
||||
HOLE_ENTRY_MIN_RATIO: float = 0.5
|
||||
|
||||
|
||||
def hole_entry_verdict(
|
||||
descended_m: float,
|
||||
needed_m: float,
|
||||
*,
|
||||
min_ratio: float = HOLE_ENTRY_MIN_RATIO,
|
||||
) -> str:
|
||||
"""S07 判到接触的那一刻：规是**进了孔**、还是**顶在工件上表面**。
|
||||
|
||||
为什么需要它（2026-09-28 实测，同一命令三跑里坏一跑：DONE / ABORT / DONE）：
|
||||
工件在 S08/S09/S10 阶段会被工具推走（仿真里实测被推 20~52mm）。而 k2 的
|
||||
孔位是 **S05 一次性定下**的（``_step_hover_k`` 取 ``_hole_xy``），工件一动
|
||||
它就失效。于是规落在孔**旁边的平面**上，只下压「悬停间隙」那么点就判到接触。
|
||||
|
||||
不拦的代价，实测完整发生过一次：
|
||||
S08 干转 5 轮（切向增量 0.0、法向增量 ±0.0，**一点没进给**）→ 第 6 轮憋住 →
|
||||
报出来的是「转轴偏差 7.6° 超过阈值 —— 关节7 轴与工具 Z 不共线，
|
||||
先确认工具系是否垂直向下、有没有装转接件」。
|
||||
|
||||
**那句话会把所有人引到错误方向**：真因既不是工具系也不是转接件，是孔位失效。
|
||||
而且为这句误导，白烧了 5 轮无效拧入（≈30 秒）和一次憋劲（有把规顶掉的风险）。
|
||||
|
||||
为什么比 ``needed_m`` 而不是比"尖端离面高度"：正常入孔时接触发生在**台肩落面**
|
||||
那一刻 ≈ ``needed_m``；顶在平面上时接触发生在**尖端刚碰到面**那一刻，而台肩还在
|
||||
尖端上方 30mm。两者差着一个台肩高度，所以用比例判最稳（见上面两个实测锚点）。
|
||||
|
||||
纯函数（R5.1）：判错两个方向都贵 —— 把正常的判成"没进孔"会白扔一整个孔；
|
||||
把"没进孔"判成正常，就是上面那 30 秒 + 一次误导。
|
||||
|
||||
``needed_m <= 0`` 时**不判**（几何本身已经错了，上面那条自检会报），
|
||||
返回 ``ENTRY_OK`` 不在这里抢着报第二个错。
|
||||
"""
|
||||
if needed_m <= 0.0:
|
||||
return ENTRY_OK
|
||||
if float(descended_m) < float(needed_m) * float(min_ratio):
|
||||
return ENTRY_HIGH
|
||||
return ENTRY_OK
|
||||
|
||||
|
||||
def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
|
||||
"""S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
|
||||
|
||||
@@ -584,6 +878,115 @@ def ratchet_cycles(total_deg: float, step_deg: float) -> int:
|
||||
return int(math.ceil(total_deg / step_deg))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RatchetTravel:
|
||||
"""棘轮步长与关节行程的**可用性核算**（S08 开头自检，别等到第 3 轮才炸）。"""
|
||||
|
||||
ok: bool
|
||||
step_deg: float # 配置的一步转角
|
||||
travel_deg: float # 关节行程（上限 − 下限）
|
||||
need_deg: float # 一步棘轮真正需要的行程
|
||||
max_step_deg: float # 行程允许的最大步长
|
||||
why: str # 不 ok 时给人看的说明（ok 时为空串）
|
||||
|
||||
|
||||
def ratchet_travel_ok(
|
||||
step_deg: float, limits_rad: Optional[Tuple[float, float]], margin_deg: float
|
||||
) -> RatchetTravel:
|
||||
"""一步棘轮 ``step_deg`` 放得进关节行程吗？
|
||||
|
||||
**为什么需要**：2026-09-24 实测，``screw_step_deg=180`` 配 Rizon4 的
|
||||
关节 7（±175°，行程 350°）在第 3 轮 ABORT —— 慢转那一步要转 180°、
|
||||
回转那一步还要 180°，两次加起来 360° > 350°，关节**必然**有一头贴限位；
|
||||
更糟的是"让余量"也腾不出地方（要腾就得往反向再让一整步，还是 360°）。
|
||||
日志里那一长串"贴限位 → 让余量 → 还是贴限位"就是这么来的。
|
||||
|
||||
判据取 ``2×step + 2×margin ≤ 行程``：
|
||||
- 2×step：棘轮一个来回（慢转 + 回转）要的行程；
|
||||
- 2×margin：两头都要留余量 —— 贴死限位再让余量时**一点空间都没有**，
|
||||
而实测每轮回转还会比指令少走 4~5°（速度/加减速限制），
|
||||
这点亏欠要靠余量吃，否则 7~8 轮后累计偏出限位、再也转不回来。
|
||||
|
||||
:param limits_rad: ``(lower, upper)`` [rad]；``None`` = 拿不到限位，放行
|
||||
（返回 ``ok=True``，理由写清楚：真机靠驱动器报 JOINT_LIMIT 兜底）。
|
||||
"""
|
||||
need = 2.0 * float(step_deg) + 2.0 * float(margin_deg)
|
||||
if limits_rad is None:
|
||||
return RatchetTravel(
|
||||
ok=True, step_deg=float(step_deg), travel_deg=float("inf"),
|
||||
need_deg=need, max_step_deg=float("inf"),
|
||||
why="",
|
||||
)
|
||||
travel = math.degrees(float(limits_rad[1]) - float(limits_rad[0]))
|
||||
max_step = 0.5 * (travel - 2.0 * float(margin_deg))
|
||||
if need <= travel:
|
||||
return RatchetTravel(
|
||||
ok=True, step_deg=float(step_deg), travel_deg=travel,
|
||||
need_deg=need, max_step_deg=max_step, why="",
|
||||
)
|
||||
# 给个能直接照着改的步长：往整十度取整（现场习惯填 120/150/180），
|
||||
# 但不许把步长压到 30° 以下 —— 那样拧进/拧出的轮数会多到失控。
|
||||
suggest = max(30.0, math.floor(max_step / 10.0) * 10.0)
|
||||
why = (
|
||||
f"棘轮一步 {step_deg:.0f}° 放不进关节 7 的行程：一步棘轮要 "
|
||||
f"2×{step_deg:.0f}° + 2×{margin_deg:.0f}°(余量) = {need:.0f}°，"
|
||||
f"而行程只有 {travel:.0f}°（上限 {math.degrees(limits_rad[1]):.0f}° − "
|
||||
f"下限 {math.degrees(limits_rad[0]):.0f}°）。\n"
|
||||
f" 改法：site.yaml 的 gauge_assembly.screw_step_deg 改到 "
|
||||
f"{max_step:.0f}° 以内（例如 {suggest:.0f}°），并保证 "
|
||||
f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍**（"
|
||||
f"累计必须转整数圈，否则规停在螺纹中间、S09 也就少拧出几分之一圈）。"
|
||||
)
|
||||
return RatchetTravel(
|
||||
ok=False, step_deg=float(step_deg), travel_deg=travel,
|
||||
need_deg=need, max_step_deg=max_step, why=why,
|
||||
)
|
||||
|
||||
|
||||
#: ``rotation_verdict`` 的结论。
|
||||
ROT_OK = "ok"
|
||||
ROT_LIMIT = "limit" # 转了一部分、方向对、但没走完 ⇒ 最可能是贴到关节限位
|
||||
ROT_AXIS = "axis" # 转轴与工具 Z 不共线 ⇒ 以「度」为单位的一切动作都不成立
|
||||
ROT_SHORT = "short" # 其它偏差（方向不对/过冲）⇒ 真有问题，该停
|
||||
|
||||
|
||||
def rotation_verdict(
|
||||
delta_deg: float,
|
||||
got_deg: float,
|
||||
mis_deg: float,
|
||||
tol_deg: float,
|
||||
) -> str:
|
||||
"""绕工具 Z 转 ``delta_deg``、实测转 ``got_deg`` 的验收判据。
|
||||
|
||||
**纯函数**：这条判据 2026-09-24 把整条 S09 判死过一次
|
||||
（"期望 +180.0°、实测 +16.4°，偏差过大" → ABORT），而真因只是
|
||||
**关节7 的行程被前面的棘轮回转顶到了上限**（±175°，而棘轮一步就要 180°）。
|
||||
判错两个方向都贵：
|
||||
* 把"贴限位"判成"short" ⇒ 白扔一整轮（还可能把规撞歪）；
|
||||
* 把"真有问题"判成"limit" ⇒ 该停的没停，继续拧。
|
||||
|
||||
还有一条**曾经漏判**的：只比绝对值时，"让它 +180、它 -170"会算成
|
||||
10° 误差、落在 10% 容差里 ⇒ 判成功，而实际上一个孔都没拧。所以方向必须比。
|
||||
所以它必须能脱离节点被钉住。
|
||||
"""
|
||||
if abs(got_deg) < 3.0 and abs(delta_deg) > 10.0:
|
||||
return ROT_LIMIT # 几乎没动：与下面的"只走了一部分"同因不同余量
|
||||
if mis_deg > tol_deg:
|
||||
return ROT_AXIS
|
||||
if abs(got_deg) >= 3.0 and abs(delta_deg) >= 3.0 and got_deg * delta_deg < 0:
|
||||
# 方向反了。这条以前**漏判**：老判据只比绝对值
|
||||
# （``abs(abs(got) - abs(delta))``），所以"让它 +180、它 -170"会算 10° 误差
|
||||
# 落在 10% 容差里 → 判成功，而实际上一个孔都没拧。
|
||||
return ROT_SHORT
|
||||
slack = max(tol_deg, 0.1 * abs(delta_deg))
|
||||
short = abs(delta_deg) - abs(got_deg)
|
||||
if short > slack:
|
||||
return ROT_LIMIT # 方向对、只是没走完 ⇒ 被硬止点截住
|
||||
if short < -slack:
|
||||
return ROT_SHORT # 过冲：转多了，同样不该当成到位
|
||||
return ROT_OK
|
||||
|
||||
|
||||
def is_joint_limit_error(text: str) -> bool:
|
||||
"""S01：把驱动/规划报的字符串认成"撞到关节极限"。
|
||||
|
||||
@@ -646,6 +1049,7 @@ EVENT_DOC: Dict[Event, str] = {
|
||||
Event.ALIGN_OUT: "对位超差",
|
||||
Event.CONTACT_OK: "已接触工件面",
|
||||
Event.FORCE_OVER_LIMIT: "力超安全上限",
|
||||
Event.HOLE_LOST: "孔位失效（规没进孔）",
|
||||
Event.SCREW_DONE: "达到目标圈数",
|
||||
Event.SCREW_STOP: "力判据满足（咬住螺纹）",
|
||||
Event.GAUGE_DROPPED: "通止规掉了",
|
||||
|
||||
@@ -26,6 +26,9 @@ import numpy as np
|
||||
#: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
|
||||
DEFAULT_LIMIT_MARGIN_DEG = 3.0
|
||||
|
||||
#: 厂家 A7。``Robot::SendJointPosition`` 的关节向量按 A1..A7 排列，A7 是最后一轴。
|
||||
A7_INDEX = 6
|
||||
|
||||
_JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
|
||||
_ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
|
||||
|
||||
@@ -178,6 +181,43 @@ def prewind(
|
||||
)
|
||||
|
||||
|
||||
def a7_limit_target(
|
||||
q: Sequence[float],
|
||||
limits: Optional[Tuple[float, float]],
|
||||
direction: int,
|
||||
margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
|
||||
) -> Optional[np.ndarray]:
|
||||
"""只把 A7 收到软件限位内侧。前六轴原样拷回，不许在这里改。
|
||||
|
||||
厂家接口是 ``Robot::SendJointPosition``（模式 ``NRT_JOINT_POSITION``）。
|
||||
向量按 A1..A7 排，A7 是下标 ``A7_INDEX``。限位是 ``RobotInfo::q_min/q_max``
|
||||
的那一对，真机上与 URDF ``joint7`` 的 lower/upper 是同一组数。
|
||||
|
||||
``direction`` 与 ``prewind`` 相同：``+1`` 关节角增大 = 逆时针，目标取上限；
|
||||
``-1`` 关节角减小 = 顺时针，目标取下限。现场若发现正负反了，改
|
||||
``site.yaml`` 的 ``extreme_direction``，不要改这里的符号。
|
||||
|
||||
没有限位、方向不是 ±1、或者余量把上下限挤没了，返回 None。
|
||||
调用方必须停，不许猜一个角度发出去。
|
||||
"""
|
||||
if direction not in (-1, 1):
|
||||
raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
|
||||
arr = np.asarray(q, dtype=float).reshape(-1)
|
||||
if arr.size != 7:
|
||||
raise ValueError(f"A7 目标要 7 个关节角，收到 {arr.size}")
|
||||
if limits is None:
|
||||
return None
|
||||
lo = float(limits[0])
|
||||
up = float(limits[1])
|
||||
margin = math.radians(float(margin_deg))
|
||||
if not math.isfinite(lo) or not math.isfinite(up) or up - lo <= 2.0 * margin:
|
||||
return None
|
||||
target = (up - margin) if direction > 0 else (lo + margin)
|
||||
out = arr.copy()
|
||||
out[A7_INDEX] = target
|
||||
return out
|
||||
|
||||
|
||||
def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||
"""从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
|
||||
|
||||
|
||||
@@ -39,6 +39,8 @@ from arm_application.site_io import (
|
||||
joint_prefix_default,
|
||||
)
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
|
||||
def _load_ee_cfg() -> dict:
|
||||
@@ -78,7 +80,7 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
|
||||
return pt
|
||||
|
||||
|
||||
class GotoEePlane(Node):
|
||||
class GotoEePlane(TolerantNode):
|
||||
def __init__(self, cfg: dict) -> None:
|
||||
super().__init__("goto_ee_plane")
|
||||
self.cfg = cfg
|
||||
|
||||
@@ -33,8 +33,11 @@ from arm_application.handeye_lib import (
|
||||
BoardConfig,
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
T_from_flexiv_tcp,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
detect_chessboard,
|
||||
format_matrix_yaml,
|
||||
format_transform_yaml,
|
||||
is_pose_filled,
|
||||
pose_to_Rt,
|
||||
@@ -50,6 +53,8 @@ from arm_application.site_io import joint_prefix_default
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
@@ -79,7 +84,7 @@ def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
|
||||
return CameraIntrinsics(K=K, dist=dist)
|
||||
|
||||
|
||||
class HandeyeCalibrate(Node):
|
||||
class HandeyeCalibrate(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_calibrate')
|
||||
|
||||
@@ -111,11 +116,10 @@ class HandeyeCalibrate(Node):
|
||||
|
||||
# 棋盘（可自定义）
|
||||
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
|
||||
# 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
|
||||
# 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
|
||||
# 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
|
||||
self.declare_parameter('board_cols', 8) # 内角点
|
||||
self.declare_parameter('board_rows', 11)
|
||||
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
|
||||
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
|
||||
self.declare_parameter('board_cols', 11) # 内角点
|
||||
self.declare_parameter('board_rows', 8)
|
||||
self.declare_parameter('square_size_m', 0.015)
|
||||
|
||||
self.declare_parameter('stable_window', 8)
|
||||
@@ -136,6 +140,11 @@ class HandeyeCalibrate(Node):
|
||||
self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
|
||||
self.declare_parameter('result_file', '')
|
||||
self.declare_parameter('cleanup_images', True)
|
||||
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
|
||||
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
|
||||
self.declare_parameter('gripper_tool', '')
|
||||
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
|
||||
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
@@ -150,6 +159,21 @@ class HandeyeCalibrate(Node):
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.min_poses = int(self.get_parameter('min_poses').value)
|
||||
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
|
||||
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
|
||||
tcp_xyz = list(
|
||||
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
|
||||
or [0.0, 0.0, 0.0]
|
||||
)
|
||||
tcp_q = list(
|
||||
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
|
||||
or [1.0, 0.0, 0.0, 0.0]
|
||||
)
|
||||
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
|
||||
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
|
||||
raise RuntimeError(
|
||||
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
|
||||
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
|
||||
)
|
||||
|
||||
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
|
||||
@@ -263,10 +287,12 @@ class HandeyeCalibrate(Node):
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
eye_to_hand = self.mode == 'eye_to_hand'
|
||||
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
|
||||
self.get_logger().info(
|
||||
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
|
||||
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale}'
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale} '
|
||||
f'gripper_tool={grip}'
|
||||
)
|
||||
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
|
||||
|
||||
@@ -366,12 +392,58 @@ class HandeyeCalibrate(Node):
|
||||
|
||||
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
|
||||
name = (
|
||||
'T_cam_to_base (eye_to_hand)'
|
||||
if eye_to_hand
|
||||
else 'T_cam_to_gripper (eye_in_hand)'
|
||||
)
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = ''
|
||||
if eye_to_hand:
|
||||
name = 'T_cam_to_base (eye_to_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
if self.gripper_tool:
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
|
||||
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, :3] = R
|
||||
T_cam_tool[:3, 3] = t.reshape(3)
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
|
||||
if tool_len > 0.05 and delta < 1e-4:
|
||||
self.get_logger().error(
|
||||
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
|
||||
'折算没生效。结果作废。'
|
||||
)
|
||||
return 11
|
||||
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
|
||||
if (
|
||||
self.T_flange_to_tool[2, 3] > 0.05
|
||||
and float(T_cam_flange[2, 3]) < 0.0
|
||||
):
|
||||
self.get_logger().error(
|
||||
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
|
||||
'在法兰后方，乘序反了。结果作废。'
|
||||
)
|
||||
return 12
|
||||
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
|
||||
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：'
|
||||
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
|
||||
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
|
||||
)
|
||||
else:
|
||||
name = 'T_cam_to_gripper (eye_in_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
header = (
|
||||
f'# 手眼标定结果\n'
|
||||
f'# mode: {self.mode}\n'
|
||||
@@ -381,8 +453,20 @@ class HandeyeCalibrate(Node):
|
||||
f'# board: {self.board.cols}x{self.board.rows} '
|
||||
f'square_m={self.board.square_size}\n'
|
||||
)
|
||||
if self.gripper_tool and eye_to_hand:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
|
||||
f'# 旧结果备份在 results/backup/\n'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
|
||||
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
|
||||
)
|
||||
self.result_file.parent.mkdir(parents=True, exist_ok=True)
|
||||
self.result_file.write_text(header + yaml_txt, encoding='utf-8')
|
||||
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
|
||||
(self.output_dir / 'captures.json').write_text(
|
||||
json.dumps(meta, indent=2), encoding='utf-8'
|
||||
)
|
||||
|
||||
@@ -126,6 +126,39 @@ def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
return T
|
||||
|
||||
|
||||
def cam_to_base_T(
|
||||
T_handeye: np.ndarray, T_ee: np.ndarray, mode: str
|
||||
) -> np.ndarray:
|
||||
"""相机系 → 基座系的 4×4。
|
||||
|
||||
* ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。
|
||||
* ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把
|
||||
固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。
|
||||
"""
|
||||
T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4)
|
||||
tag = str(mode or "").strip().lower()
|
||||
if tag == "eye_to_hand":
|
||||
return T_he
|
||||
if tag != "eye_in_hand":
|
||||
raise ValueError(
|
||||
f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}"
|
||||
)
|
||||
return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he
|
||||
|
||||
|
||||
def cam_point_to_base(
|
||||
p_cam: np.ndarray,
|
||||
T_handeye: np.ndarray,
|
||||
T_ee: np.ndarray,
|
||||
mode: str,
|
||||
) -> np.ndarray:
|
||||
"""工件三维点（相机系）→ 基座系 XYZ。"""
|
||||
T_bc = cam_to_base_T(T_handeye, T_ee, mode)
|
||||
xyz = np.asarray(p_cam, dtype=float).reshape(3)
|
||||
ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
|
||||
return (T_bc @ ph)[:3]
|
||||
|
||||
|
||||
def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
Ri = R.T
|
||||
ti = -Ri @ t.reshape(3, 1)
|
||||
@@ -309,6 +342,38 @@ def calibrate_hand_eye(
|
||||
return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
|
||||
|
||||
|
||||
def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
|
||||
"""Studio ``tcp_location`` → 法兰→工具 4×4。
|
||||
|
||||
Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
|
||||
和本仓库其余地方的 xyzw 不一样。只在这里换一次。
|
||||
"""
|
||||
xyz = np.asarray(xyz_m, dtype=float).reshape(3)
|
||||
q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
|
||||
if q.size == 7:
|
||||
qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
|
||||
else:
|
||||
qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
|
||||
R = quat_xyzw_to_R((qx, qy, qz, qw))
|
||||
return Rt_to_T(R, xyz.reshape(3, 1))
|
||||
|
||||
|
||||
def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
|
||||
"""AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
|
||||
|
||||
``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
|
||||
OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
|
||||
所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
|
||||
|
||||
乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
|
||||
扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
|
||||
两种都不报错。
|
||||
"""
|
||||
T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
|
||||
T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
|
||||
return T_ft @ T_ct
|
||||
|
||||
|
||||
def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
|
||||
T = Rt_to_T(R, t)
|
||||
lines = [
|
||||
@@ -321,6 +386,14 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def format_matrix_yaml(T: np.ndarray, key: str) -> str:
|
||||
arr = np.asarray(T, dtype=float).reshape(4, 4)
|
||||
lines = [f'{key}:']
|
||||
for row in arr:
|
||||
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def is_pose_filled(joints: Sequence[float]) -> bool:
|
||||
"""若全为零 / NaN 占位则返回 False。"""
|
||||
arr = np.asarray(joints, dtype=float)
|
||||
|
||||
@@ -37,6 +37,8 @@ import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
HELP = """
|
||||
================================================
|
||||
手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
|
||||
@@ -94,14 +96,6 @@ def _setup_matplotlib_font():
|
||||
return chosen
|
||||
|
||||
|
||||
def _param_as_str(node: Node, name: str, default: str = '') -> str:
|
||||
node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
|
||||
val = node.get_parameter(name).value
|
||||
if val is None or val == '':
|
||||
return ''
|
||||
return str(val).strip()
|
||||
|
||||
|
||||
class RdkBridge:
|
||||
"""通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
|
||||
|
||||
@@ -128,6 +122,8 @@ class RdkBridge:
|
||||
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
|
||||
'LD_LIBRARY_PATH': flexiv_lib,
|
||||
'LANG': 'C.UTF-8',
|
||||
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
|
||||
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
|
||||
}
|
||||
# 不要继承 conda 的 PYTHONPATH。
|
||||
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
|
||||
@@ -240,7 +236,7 @@ class RdkBridge:
|
||||
pass
|
||||
|
||||
|
||||
class HandeyeTeach(Node):
|
||||
class HandeyeTeach(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_teach')
|
||||
self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
|
||||
@@ -250,7 +246,7 @@ class HandeyeTeach(Node):
|
||||
self.declare_parameter('backend', 'ros') # ros | rdk
|
||||
self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
|
||||
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
|
||||
self._serial = _param_as_str(self, 'camera_serial', '')
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('auto_exposure', True)
|
||||
self.declare_parameter('ae_limit_us', 1800.0)
|
||||
self.declare_parameter('emitter_enabled', False)
|
||||
@@ -260,6 +256,9 @@ class HandeyeTeach(Node):
|
||||
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
|
||||
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
|
||||
self.declare_parameter('target_points', 0)
|
||||
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
|
||||
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
|
||||
self.declare_parameter('poses_file', '')
|
||||
|
||||
self.backend = str(self.get_parameter('backend').value).strip().lower()
|
||||
self.robot_sn = str(self.get_parameter('robot_sn').value)
|
||||
@@ -267,8 +266,12 @@ class HandeyeTeach(Node):
|
||||
|
||||
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.images_dir = self.output_dir / 'images'
|
||||
self.poses_file = self.output_dir / 'taught_poses.txt'
|
||||
_pf = str(self.get_parameter('poses_file').value).strip()
|
||||
self.poses_file = (
|
||||
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
|
||||
)
|
||||
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
|
||||
self._last_capture_t = 0.0
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self._captures: List[dict] = []
|
||||
@@ -537,6 +540,20 @@ class HandeyeTeach(Node):
|
||||
if q is None:
|
||||
self.get_logger().error('读取关节失败')
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self._last_capture_t < 0.8:
|
||||
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
|
||||
return
|
||||
if self._captures:
|
||||
last = self._captures[-1]['joints']
|
||||
delta = max(abs(a - b) for a, b in zip(q, last))
|
||||
if delta < 0.015:
|
||||
self.get_logger().warn(
|
||||
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
|
||||
'先拖到另一个姿态再按 Enter。'
|
||||
)
|
||||
return
|
||||
self._last_capture_t = now
|
||||
|
||||
idx = len(self._captures)
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
|
||||
@@ -41,6 +41,8 @@ from arm_application.site_io import joint_prefix_default
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
@@ -110,7 +112,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
class HandeyeWaypoints(Node):
|
||||
class HandeyeWaypoints(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_waypoints')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
|
||||
@@ -134,6 +134,67 @@ def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
|
||||
return None
|
||||
|
||||
|
||||
def _keypoint_holes(
|
||||
dets,
|
||||
*,
|
||||
conf_thr: float,
|
||||
min_kconf: float,
|
||||
radius_px: float,
|
||||
min_dist_px: float,
|
||||
):
|
||||
"""pose 模型直接给出的 k1/k2 关键点当孔位。
|
||||
|
||||
现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。
|
||||
返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None
|
||||
（不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。
|
||||
|
||||
为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆，
|
||||
光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里
|
||||
直接回归出来的，对这几类干扰天然更稳。
|
||||
"""
|
||||
if not dets:
|
||||
return None
|
||||
for det in sorted(dets, key=lambda d: d.conf, reverse=True):
|
||||
if float(det.conf) < conf_thr:
|
||||
continue
|
||||
kpts = getattr(det, "kpts", None)
|
||||
if kpts is None:
|
||||
continue
|
||||
kpts = np.asarray(kpts, dtype=float)
|
||||
if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3:
|
||||
continue
|
||||
k1, k2 = kpts[1].copy(), kpts[2].copy()
|
||||
if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()):
|
||||
continue
|
||||
if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf:
|
||||
logger.info(
|
||||
"k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆",
|
||||
float(k1[2]),
|
||||
float(k2[2]),
|
||||
min_kconf,
|
||||
)
|
||||
continue
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
if dist < float(min_dist_px):
|
||||
logger.info(
|
||||
"k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆",
|
||||
dist,
|
||||
min_dist_px,
|
||||
)
|
||||
continue
|
||||
r = float(radius_px)
|
||||
# **刻意不按图像 x 交换左右**。
|
||||
# Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的
|
||||
# k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度，
|
||||
# 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同
|
||||
# 的物理孔 —— 那是最难查的一类现场问题。
|
||||
# 下游怎么认这两个点由 `stage2.k1_label` 决定：
|
||||
# hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2，
|
||||
# 所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。
|
||||
return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r
|
||||
return None
|
||||
|
||||
|
||||
def _refine_circle_center(
|
||||
gray: np.ndarray,
|
||||
cx: float,
|
||||
@@ -466,6 +527,35 @@ def detect_holes(
|
||||
gate_block=True,
|
||||
)
|
||||
|
||||
# ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ----
|
||||
if bool(cfg.get("keypoint_enable", True)):
|
||||
kp = _keypoint_holes(
|
||||
dets,
|
||||
conf_thr=conf,
|
||||
min_kconf=float(cfg.get("keypoint_min_conf", 0.30)),
|
||||
radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)),
|
||||
min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)),
|
||||
)
|
||||
if kp is not None:
|
||||
(k1, k2, r1, r2) = kp
|
||||
hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
|
||||
hole_radii = [float(r1), float(r2)]
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(
|
||||
image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
|
||||
)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"source": "keypoint",
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
|
||||
# ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
|
||||
if bool(cfg.get("insert_pair_enable", True)):
|
||||
gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
|
||||
@@ -25,6 +25,8 @@ from arm_application.site_io import load_site_dict
|
||||
from arm_msgs.msg import HoleDetection as HoleDetectionMsg
|
||||
from arm_msgs.msg import HoleDetections
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _as_bool(val, default: bool = False) -> bool:
|
||||
if val is None:
|
||||
@@ -57,7 +59,7 @@ def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
|
||||
return msg
|
||||
|
||||
|
||||
class HoleDetectNode(Node):
|
||||
class HoleDetectNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("hole_detect")
|
||||
self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")
|
||||
|
||||
@@ -21,7 +21,7 @@ DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
|
||||
# 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
|
||||
# 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
|
||||
# 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
|
||||
_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
|
||||
_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx"
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -143,17 +143,18 @@ class HoleDetector:
|
||||
def _load_yolo(self):
|
||||
if self._detector is not None:
|
||||
return self._detector
|
||||
from arm_application.yolo_detector import YoloDetector, resolve_weights
|
||||
from arm_application.yolo_detector import build_detector, resolve_weights
|
||||
|
||||
weights = resolve_weights(
|
||||
self.model_path,
|
||||
backend=str(self.cfg.get("yolo_backend") or "auto"),
|
||||
)
|
||||
self._detector = YoloDetector(
|
||||
# build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。
|
||||
self._detector = build_detector(
|
||||
weights,
|
||||
device=str(self.device),
|
||||
conf=float(self.cfg.get("conf", 0.25)),
|
||||
iou=float(self.cfg.get("iou", 0.7)),
|
||||
device=str(self.device),
|
||||
imgsz=int(self.cfg.get("imgsz", 640)),
|
||||
)
|
||||
return self._detector
|
||||
|
||||
@@ -62,6 +62,15 @@ class KalmanFilter3D:
|
||||
self._predict(dt)
|
||||
self._t = t
|
||||
|
||||
def zero_velocity(self) -> None:
|
||||
"""把速度状态清零、位置保持不动。
|
||||
|
||||
用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不
|
||||
采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成
|
||||
"按旧速度匀速跑掉"，刚好是我们要避免的那件事。
|
||||
"""
|
||||
self.x[3:] = 0.0
|
||||
|
||||
def update(self, z: np.ndarray) -> None:
|
||||
"""用一次三维位置观测做量测更新。"""
|
||||
z = np.asarray(z, dtype=float).reshape(3)
|
||||
|
||||
@@ -15,8 +15,10 @@ from geometry_msgs.msg import PoseStamped
|
||||
|
||||
from arm_msgs.action import MoveToPose
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
class MoveToPoseClient(Node):
|
||||
|
||||
class MoveToPoseClient(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('move_to_pose_client')
|
||||
self._client = ActionClient(self, MoveToPose, 'move_to_pose')
|
||||
|
||||
@@ -54,6 +54,21 @@ class OnnxWorkpieceDetector:
|
||||
self._iname = inp.name
|
||||
shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape]
|
||||
self._in_h, self._in_w = int(shape[-2]), int(shape[-1])
|
||||
# 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。
|
||||
# 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型
|
||||
# （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框
|
||||
# ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。
|
||||
# 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。
|
||||
# 宁可启动就失败（元规则：静默失效是头号敌人）。
|
||||
outs = self._sess.get_outputs()
|
||||
tail = outs[0].shape[-1] if (outs and outs[0].shape) else None
|
||||
if isinstance(tail, int) and tail != 6:
|
||||
raise RuntimeError(
|
||||
f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是"
|
||||
" ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的"
|
||||
" pose 模型必须交给 pose_detector / build_detector，否则坐标会被"
|
||||
"当成置信度解出垃圾框，而且不会报错。"
|
||||
)
|
||||
self.conf = float(conf)
|
||||
self.imgsz = int(imgsz)
|
||||
# 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,339 @@
|
||||
"""用非夕给的 Rizon4 外壳网格摆出当前姿态。
|
||||
|
||||
网格和安装位姿来自 ``flexiv_description`` 的 ``visual_parameters.yaml``
|
||||
与 ``meshes/Rizon4/visual/*.obj``，不另画一套简图。面数按固定步长抽稀，
|
||||
只为了界面里能转得动，几何仍是厂家的。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from arm_application.operator_arm_state import link_frames
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
# 单个连杆原始面数大约 5000。再按 700 抽稀会在壳上留出三角洞，
|
||||
# 白壳和灰蓝关节分不出来。这里保留全部面。
|
||||
_FACES_PER_LINK = 8000
|
||||
_MODELS: dict[str, list[tuple[list[tuple[float, float, float]], tuple]]] | None = None
|
||||
|
||||
|
||||
def posed_triangles(q: list[float]) -> list[tuple]:
|
||||
"""返回基座系下的三角面 ``(a, b, c, 明暗, rgb)``。
|
||||
|
||||
``rgb`` 来自同目录 ``.mtl`` 的 ``Kd``。外壳是白的，关节壳体是灰蓝，
|
||||
这是厂家导出的两种材质，不是另涂的颜色。
|
||||
"""
|
||||
world, shade, rgb = _posed_arrays(q)
|
||||
return [
|
||||
(
|
||||
(float(world[i, 0, 0]), float(world[i, 0, 1]), float(world[i, 0, 2])),
|
||||
(float(world[i, 1, 0]), float(world[i, 1, 1]), float(world[i, 1, 2])),
|
||||
(float(world[i, 2, 0]), float(world[i, 2, 1]), float(world[i, 2, 2])),
|
||||
float(shade[i]),
|
||||
(float(rgb[i, 0]), float(rgb[i, 1]), float(rgb[i, 2])),
|
||||
)
|
||||
for i in range(shade.shape[0])
|
||||
]
|
||||
|
||||
|
||||
def posed_gl(q: list[float]) -> tuple[bytes, int, tuple[float, float, float], float]:
|
||||
"""给显卡的一份网格：交错 ``xyzrgb``、顶点数、中心、半径。
|
||||
|
||||
拖视角只改观察矩阵，不再把四万个面在界面线程里重投影。
|
||||
"""
|
||||
world, shade, rgb = _posed_arrays(q)
|
||||
light = (0.55 + 0.45 * shade)[:, None]
|
||||
colored = (rgb * light).astype(np.float32)
|
||||
pos = world.reshape(-1, 3).astype(np.float32)
|
||||
col = np.repeat(colored, 3, axis=0)
|
||||
center = pos.mean(axis=0)
|
||||
radius = float(np.linalg.norm(pos - center, axis=1).max())
|
||||
packed = np.concatenate([pos, col], axis=1).astype(np.float32, copy=False)
|
||||
return (
|
||||
packed.tobytes(),
|
||||
int(pos.shape[0]),
|
||||
(float(center[0]), float(center[1]), float(center[2])),
|
||||
max(radius, 1e-4),
|
||||
)
|
||||
|
||||
|
||||
def _posed_arrays(q: list[float]):
|
||||
"""每个连杆做一次矩阵乘法，不再对每个顶点走 Python 循环。"""
|
||||
frames = link_frames(q)
|
||||
worlds: list[np.ndarray] = []
|
||||
shades: list[np.ndarray] = []
|
||||
colors: list[np.ndarray] = []
|
||||
for link, parts in _models().items():
|
||||
frame = frames.get(link)
|
||||
if frame is None:
|
||||
continue
|
||||
rot = np.asarray(frame[0], dtype=np.float64)
|
||||
origin = np.asarray(frame[1], dtype=np.float64)
|
||||
for part in parts:
|
||||
local = np.einsum("ij,fvj->fvi", part["erot"], part["verts"]) + part["epos"]
|
||||
world = np.einsum("ij,fvj->fvi", rot, local) + origin
|
||||
ab = world[:, 1] - world[:, 0]
|
||||
ac = world[:, 2] - world[:, 0]
|
||||
normal = np.cross(ab, ac)
|
||||
norm = np.linalg.norm(normal, axis=1)
|
||||
norm = np.where(norm > 0.0, norm, 1.0)
|
||||
facing = np.abs(normal[:, 0] + normal[:, 1]) / (norm * math.sqrt(2.0))
|
||||
worlds.append(world)
|
||||
shades.append(0.28 + 0.72 * facing)
|
||||
colors.append(part["rgb"])
|
||||
if not worlds:
|
||||
empty = np.zeros((0, 3, 3), dtype=np.float64)
|
||||
return empty, np.zeros((0,), dtype=np.float64), np.zeros((0, 3), dtype=np.float64)
|
||||
return np.concatenate(worlds), np.concatenate(shades), np.concatenate(colors)
|
||||
|
||||
|
||||
def view_rotation(yaw: float, pitch: float):
|
||||
"""先绕竖直轴转，再俯仰。拖拽视角用这一份，不在界面里另写一套。"""
|
||||
return _mul(_rx(pitch), _rz(yaw))
|
||||
|
||||
|
||||
def view_scale(radius: float, width: int, height: int) -> tuple[float, float]:
|
||||
"""和着色器里的 ``u_sx`` / ``u_sy`` 同一套缩放。宽屏时横向再缩一档。"""
|
||||
fit = 0.85 / max(float(radius), 1e-4)
|
||||
width = max(int(width), 1)
|
||||
height = max(int(height), 1)
|
||||
if width >= height:
|
||||
return fit * height / width, fit
|
||||
return fit, fit * width / height
|
||||
|
||||
|
||||
def screen_point(
|
||||
point: tuple[float, float, float],
|
||||
yaw: float,
|
||||
pitch: float,
|
||||
center: tuple[float, float, float],
|
||||
radius: float,
|
||||
width: int,
|
||||
height: int,
|
||||
) -> tuple[float, float]:
|
||||
"""基座系的点投到控件像素。纵向和着色器里的 ``p.z`` 是同一套，+Z 朝画面上方。"""
|
||||
shifted = (
|
||||
float(point[0]) - float(center[0]),
|
||||
float(point[1]) - float(center[1]),
|
||||
float(point[2]) - float(center[2]),
|
||||
)
|
||||
viewed = _apply(view_rotation(yaw, pitch), shifted)
|
||||
sx, sy = view_scale(radius, width, height)
|
||||
ndc_x = viewed[0] * sx
|
||||
# 着色器写的是 ``p.z * u_sy``。OpenGL 的 y 朝上，这里再换成 Qt 的像素 y。
|
||||
ndc_y = viewed[2] * sy
|
||||
width = max(int(width), 1)
|
||||
height = max(int(height), 1)
|
||||
return (ndc_x * 0.5 + 0.5) * width, (1.0 - (ndc_y * 0.5 + 0.5)) * height
|
||||
|
||||
|
||||
def ground_grid(extent: float = 1.0, step: float = 0.25) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]:
|
||||
"""地平面格子，z = 0，单位米。以前这张格子在 RViz 里，现在画在工位仿真上。"""
|
||||
if step <= 0 or extent <= 0:
|
||||
raise ValueError("地平面格子的间距和范围都要大于 0")
|
||||
count = int(round(extent / step))
|
||||
lines = []
|
||||
for index in range(-count, count + 1):
|
||||
coord = index * step
|
||||
lines.append(((-extent, coord, 0.0), (extent, coord, 0.0)))
|
||||
lines.append(((coord, -extent, 0.0), (coord, extent, 0.0)))
|
||||
return lines
|
||||
|
||||
|
||||
def axis_tip(
|
||||
origin: tuple[float, float, float],
|
||||
rot: tuple,
|
||||
axis: int,
|
||||
length: float,
|
||||
) -> tuple[float, float, float]:
|
||||
"""坐标系某一轴的端点。``rot`` 把连杆坐标变到基座坐标，列向量就是三根轴。"""
|
||||
basis = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))[axis]
|
||||
direction = _apply(rot, basis)
|
||||
return (
|
||||
origin[0] + direction[0] * length,
|
||||
origin[1] + direction[1] * length,
|
||||
origin[2] + direction[2] * length,
|
||||
)
|
||||
|
||||
|
||||
def _place(rot, origin, erot, epos, vertex):
|
||||
local = _add(epos, _apply(erot, vertex))
|
||||
return _add(origin, _apply(rot, local))
|
||||
|
||||
|
||||
def _shade(a, b, c) -> float:
|
||||
ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
|
||||
vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
|
||||
nx = uy * vz - uz * vy
|
||||
ny = uz * vx - ux * vz
|
||||
nz = ux * vy - uy * vx
|
||||
norm = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
|
||||
facing = abs(nx + ny) / (norm * math.sqrt(2.0))
|
||||
return 0.28 + 0.72 * facing
|
||||
|
||||
|
||||
def _models() -> dict[str, list]:
|
||||
global _MODELS
|
||||
if _MODELS is not None:
|
||||
return _MODELS
|
||||
root = _description_root()
|
||||
visual = yaml.safe_load((root / "config/Rizon4/visual_parameters.yaml").read_text(encoding="utf-8"))
|
||||
params = (visual or {}).get("visual_parameters") or {}
|
||||
loaded: dict[str, list] = {}
|
||||
for link, spec in params.items():
|
||||
shell = ((spec or {}).get("visual") or {})
|
||||
parts = []
|
||||
for kind in ("shell", "ring"):
|
||||
item = shell.get(kind)
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
mesh = item.get("mesh") or {}
|
||||
path = root / str(mesh.get("path") or "")
|
||||
if not path.is_file():
|
||||
continue
|
||||
scale = _scale(mesh.get("scale"))
|
||||
origin = item.get("origin") or {}
|
||||
erot = _rpy(
|
||||
float(origin.get("roll") or 0.0),
|
||||
float(origin.get("pitch") or 0.0),
|
||||
float(origin.get("yaw") or 0.0),
|
||||
)
|
||||
epos = (
|
||||
float(origin.get("x") or 0.0),
|
||||
float(origin.get("y") or 0.0),
|
||||
float(origin.get("z") or 0.0),
|
||||
)
|
||||
parts.append(_part_arrays(_load_obj(path, scale, _materials(path)), erot, epos))
|
||||
if parts:
|
||||
loaded[str(link)] = parts
|
||||
if "base_link" not in loaded or "link7" not in loaded:
|
||||
raise FileNotFoundError(f"Rizon4 外壳网格不完整：{root}")
|
||||
_MODELS = loaded
|
||||
return loaded
|
||||
|
||||
|
||||
def _part_arrays(faces: list[tuple], erot, epos) -> dict:
|
||||
"""把一个零件收成数组，姿态更新时整块相乘。"""
|
||||
if not faces:
|
||||
verts = np.zeros((0, 3, 3), dtype=np.float64)
|
||||
rgb = np.zeros((0, 3), dtype=np.float64)
|
||||
else:
|
||||
verts = np.asarray([[a, b, c] for a, b, c, _color in faces], dtype=np.float64)
|
||||
rgb = np.asarray([color for _a, _b, _c, color in faces], dtype=np.float64)
|
||||
return {
|
||||
"verts": verts,
|
||||
"rgb": rgb,
|
||||
"erot": np.asarray(erot, dtype=np.float64),
|
||||
"epos": np.asarray(epos, dtype=np.float64),
|
||||
}
|
||||
|
||||
|
||||
def _description_root() -> Path:
|
||||
root = ros2_root()
|
||||
candidates = (
|
||||
root / "src/flexiv_description",
|
||||
root / "install/flexiv_description/share/flexiv_description",
|
||||
)
|
||||
for item in candidates:
|
||||
if (item / "config/Rizon4/visual_parameters.yaml").is_file():
|
||||
return item
|
||||
raise FileNotFoundError("找不到 flexiv_description 的 Rizon4 外观")
|
||||
|
||||
|
||||
def _materials(path: Path) -> dict[str, tuple[float, float, float]]:
|
||||
mtl = path.with_suffix(".mtl")
|
||||
colors: dict[str, tuple[float, float, float]] = {}
|
||||
name = ""
|
||||
if not mtl.is_file():
|
||||
return colors
|
||||
for line in mtl.read_text(encoding="utf-8", errors="replace").splitlines():
|
||||
if line.startswith("newmtl "):
|
||||
name = line.split(None, 1)[1].strip()
|
||||
elif name and line.startswith("Kd "):
|
||||
parts = line.split()
|
||||
if len(parts) >= 4:
|
||||
colors[name] = (float(parts[1]), float(parts[2]), float(parts[3]))
|
||||
return colors
|
||||
|
||||
|
||||
def _load_obj(path: Path, scale: tuple[float, float, float], materials: dict) -> list[tuple]:
|
||||
fallback = (0.75, 0.76, 0.78)
|
||||
rgb = fallback
|
||||
vertices: list[tuple[float, float, float]] = []
|
||||
faces: list[tuple[int, int, int, tuple]] = []
|
||||
for line in path.read_text(encoding="utf-8", errors="replace").splitlines():
|
||||
if line.startswith("usemtl "):
|
||||
rgb = materials.get(line.split(None, 1)[1].strip(), fallback)
|
||||
elif line.startswith("v "):
|
||||
_v, x, y, z, *_rest = line.split()
|
||||
vertices.append((float(x) * scale[0], float(y) * scale[1], float(z) * scale[2]))
|
||||
elif line.startswith("f "):
|
||||
idx = []
|
||||
for token in line.split()[1:]:
|
||||
raw = token.split("/")[0]
|
||||
if not raw:
|
||||
continue
|
||||
number = int(raw)
|
||||
idx.append(number - 1 if number > 0 else number)
|
||||
for offset in range(1, len(idx) - 1):
|
||||
faces.append((idx[0], idx[offset], idx[offset + 1], rgb))
|
||||
if not faces or not vertices:
|
||||
return []
|
||||
step = max(1, len(faces) // _FACES_PER_LINK)
|
||||
chosen = faces[::step]
|
||||
out = []
|
||||
count = len(vertices)
|
||||
for ia, ib, ic, color in chosen:
|
||||
if not (0 <= ia < count and 0 <= ib < count and 0 <= ic < count):
|
||||
continue
|
||||
out.append((vertices[ia], vertices[ib], vertices[ic], color))
|
||||
return out
|
||||
|
||||
|
||||
def _scale(raw) -> tuple[float, float, float]:
|
||||
if raw is None:
|
||||
return (1.0, 1.0, 1.0)
|
||||
if isinstance(raw, (int, float)):
|
||||
value = float(raw)
|
||||
return (value, value, value)
|
||||
text = str(raw).replace(",", " ").split()
|
||||
nums = [float(item) for item in text[:3]]
|
||||
while len(nums) < 3:
|
||||
nums.append(nums[-1] if nums else 1.0)
|
||||
return (nums[0], nums[1], nums[2])
|
||||
|
||||
|
||||
def _rpy(roll: float, pitch: float, yaw: float):
|
||||
return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
|
||||
|
||||
|
||||
def _rx(angle: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
|
||||
|
||||
|
||||
def _ry(angle: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
|
||||
|
||||
|
||||
def _rz(angle: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
|
||||
|
||||
|
||||
def _mul(a, b):
|
||||
return tuple(tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)) for i in range(3))
|
||||
|
||||
|
||||
def _apply(rot, vec):
|
||||
return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))
|
||||
|
||||
|
||||
def _add(a, b):
|
||||
return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
|
||||
@@ -0,0 +1,595 @@
|
||||
"""工位界面用的机械臂状态：只读使能 / Remote，并用关节角画出姿态。
|
||||
|
||||
不会调用 Enable，也不会在控制栈或拖拽桥已经占着 RDK 时再连一次。
|
||||
``robot.mode()`` 看不出 Manual，必须看 ``operational_status``。
|
||||
示教器上的 Auto（Remote）没有远程切换接口，这里只负责把这句话摆在界面上。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import re
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application.site_io import listed_arms, ros2_root
|
||||
|
||||
# 和 scripts/lib/rdk_status.py 的 MANUAL_HINT 同一件事，界面上始终可见。
|
||||
REMOTE_PROMPT = (
|
||||
"请在示教器上打开 Auto（Remote）。"
|
||||
"RDK 没有远程拨这个开关的接口，必须人到现场拨。"
|
||||
)
|
||||
|
||||
_AXES = (
|
||||
(0.0, 0.0, 1.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
)
|
||||
_JOINTS = tuple(f"joint{i}" for i in range(1, 8))
|
||||
_LOCK = threading.Lock()
|
||||
_CACHE: dict[str, dict] = {}
|
||||
_STOP = False
|
||||
_STARTED = False
|
||||
_HOLD = 0
|
||||
_HOLD_LOCK = threading.Lock()
|
||||
|
||||
|
||||
def _empty(arm: str) -> dict:
|
||||
return {
|
||||
"arm": arm,
|
||||
"level": "warn",
|
||||
"enabled": False,
|
||||
"summary": "还没读到这台臂的状态",
|
||||
"prompt": REMOTE_PROMPT,
|
||||
"prompt_hot": True,
|
||||
"mode": "",
|
||||
"op_status": "",
|
||||
"fault": None,
|
||||
"estop_released": None,
|
||||
"q": None,
|
||||
"points": [],
|
||||
"source": "",
|
||||
"stack_up": False,
|
||||
"stamp": 0.0,
|
||||
}
|
||||
|
||||
|
||||
def snapshot(arm: str) -> dict:
|
||||
with _LOCK:
|
||||
row = _CACHE.get(arm)
|
||||
return dict(row) if row else _empty(arm)
|
||||
|
||||
|
||||
def present(
|
||||
arm: str,
|
||||
*,
|
||||
stack_up: bool,
|
||||
bridge_up: bool,
|
||||
probe: dict | None,
|
||||
joints: list[float] | None,
|
||||
error: str = "",
|
||||
) -> dict:
|
||||
"""把一次读数收成界面直接能画的字典。纯函数，不连机器人。"""
|
||||
probe = probe or {}
|
||||
q = _seven(joints) or _seven(probe.get("q"))
|
||||
op_status = str(probe.get("op_status") or "")
|
||||
mode = str(probe.get("mode") or "")
|
||||
manual = "MANUAL" in op_status.upper()
|
||||
if stack_up and q is not None:
|
||||
enabled = True
|
||||
level = "ok"
|
||||
summary = "控制栈在发关节角，这台臂已经接上。"
|
||||
if probe.get("_shared_stack"):
|
||||
summary = "控制栈在发关节角，但看不出是 89 还是 91，两台显示的是同一份。"
|
||||
source = "控制栈 /joint_states"
|
||||
elif stack_up:
|
||||
enabled = False
|
||||
level = "fail"
|
||||
summary = "控制栈在跑，但没有关节数据。通常是还没使能，一直在等 operational。"
|
||||
source = "控制栈"
|
||||
elif bridge_up and not probe:
|
||||
enabled = False
|
||||
level = "warn"
|
||||
summary = "软拖拽桥占着 RDK，界面不再另开一条连接。"
|
||||
source = "拖拽桥"
|
||||
q = None
|
||||
elif probe.get("connected"):
|
||||
enabled = bool(probe.get("operational"))
|
||||
level = str(probe.get("level") or ("ok" if enabled else "fail"))
|
||||
summary = str(probe.get("summary") or ("已使能" if enabled else "未使能"))
|
||||
source = "RDK 直读（未使能，只查询）"
|
||||
else:
|
||||
enabled = False
|
||||
level = "fail" if error else "warn"
|
||||
summary = error or str(probe.get("summary") or probe.get("error") or "还没连上 RDK")
|
||||
source = "RDK 直读" if probe else ""
|
||||
hot = (not enabled) or manual
|
||||
return {
|
||||
"arm": arm,
|
||||
"level": level,
|
||||
"enabled": enabled,
|
||||
"summary": summary,
|
||||
"prompt": REMOTE_PROMPT,
|
||||
"prompt_hot": hot,
|
||||
"mode": mode,
|
||||
"op_status": op_status,
|
||||
"fault": probe.get("fault") if probe else None,
|
||||
"estop_released": probe.get("estop_released") if probe else None,
|
||||
"q": q,
|
||||
"points": fk_points(q) if q else fk_points([0.0] * 7),
|
||||
"pose_live": q is not None,
|
||||
"source": source,
|
||||
"stack_up": stack_up,
|
||||
"stamp": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def link_frames(q: list[float]) -> dict[str, tuple]:
|
||||
"""各连杆坐标系，和 ``rizon_arm.xacro`` 的关节顺序一致。
|
||||
|
||||
返回 ``base_link`` 与 ``link1``…``link7``。每个值是 ``(旋转, 原点)``，
|
||||
旋转把连杆坐标变到基座坐标。厂家网格就画在这些坐标系里。
|
||||
"""
|
||||
kin = _kinematics()
|
||||
rot = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
|
||||
pos = (0.0, 0.0, 0.0)
|
||||
frames: dict[str, tuple] = {"base_link": (rot, pos)}
|
||||
for index, angle in enumerate(q[:7]):
|
||||
spec = kin[_JOINTS[index]]
|
||||
pos = _add(pos, _apply(rot, (spec["x"], spec["y"], spec["z"])))
|
||||
rot = _mul(rot, _rpy(spec["roll"], spec["pitch"], spec["yaw"]))
|
||||
rot = _mul(rot, _axis_rot(_AXES[index], float(angle)))
|
||||
frames[f"link{index + 1}"] = (rot, pos)
|
||||
return frames
|
||||
|
||||
|
||||
def fk_points(q: list[float]) -> list[tuple[float, float, float]]:
|
||||
"""按 Rizon4 的 default_kinematics 做正运动学，返回基座到法兰的点（米）。
|
||||
|
||||
关节轴与 ``rizon_arm.xacro`` 一致：1/3/5/7 绕 z，2/4/6 绕 y。
|
||||
URDF 顺序是先平移（父坐标系），再 rpy，再绕关节轴转 q。
|
||||
"""
|
||||
frames = link_frames(q)
|
||||
points = [frames["base_link"][1]]
|
||||
points.extend(frames[f"link{index}"][1] for index in range(1, 8))
|
||||
_rot, pos = _flange_pose(frames)
|
||||
points.append(pos)
|
||||
return points
|
||||
|
||||
|
||||
def display_frames(q: list[float], workpiece: tuple | None = None) -> list[dict]:
|
||||
"""界面要画的坐标系，都在基座系里。
|
||||
|
||||
第一套是基座。第二套默认是法兰（旋转与 ``link7`` 相同，原点再加
|
||||
``link7_to_flange``）。传入工件系时，第二套改画在工件上，不再画法兰。
|
||||
"""
|
||||
frames = link_frames(q)
|
||||
base_rot, base_pos = frames["base_link"]
|
||||
shown = [{"name": "基座", "rot": base_rot, "pos": base_pos}]
|
||||
if workpiece is None:
|
||||
rot, pos = _flange_pose(frames)
|
||||
shown.append({"name": "法兰", "rot": rot, "pos": pos})
|
||||
return shown
|
||||
rot, pos = workpiece
|
||||
if not isinstance(rot, tuple) or len(rot) != 3 or not isinstance(pos, tuple) or len(pos) != 3:
|
||||
raise ValueError("工件坐标系要有旋转和原点")
|
||||
shown.append({"name": "工件", "rot": rot, "pos": pos})
|
||||
return shown
|
||||
|
||||
|
||||
def workpiece_frame(robot: dict | None) -> tuple | None:
|
||||
"""有完整工件系时返回 ``(旋转, 原点)``。没有就返回 None。
|
||||
|
||||
``workpiece_surface_z_m`` 只是上表面高度，没有原点的 XY，也没有朝向，
|
||||
不能当成坐标系。配了 ``gauge_assembly.workpiece_frame`` 却缺 ``xyz`` 或
|
||||
``rpy`` 时直接报错，不用单位阵把缺的朝向补上。
|
||||
``xyz`` 单位米，``rpy`` 单位弧度，都写在基座系，旋转顺序与连杆 ``rpy`` 相同。
|
||||
"""
|
||||
if not isinstance(robot, dict):
|
||||
return None
|
||||
gauge = robot.get("gauge_assembly")
|
||||
if not isinstance(gauge, dict) or "workpiece_frame" not in gauge:
|
||||
return None
|
||||
raw = gauge.get("workpiece_frame")
|
||||
if not isinstance(raw, dict):
|
||||
raise ValueError("工件坐标系要写成 xyz 和 rpy，各三个数")
|
||||
xyz = raw.get("xyz")
|
||||
rpy = raw.get("rpy")
|
||||
if not _three_floats(xyz) or not _three_floats(rpy):
|
||||
raise ValueError("工件坐标系要有 xyz 和 rpy，各三个数，单位是米和弧度，写在基座系里")
|
||||
return _rpy(float(rpy[0]), float(rpy[1]), float(rpy[2])), (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
)
|
||||
|
||||
|
||||
def _flange_pose(frames: dict) -> tuple:
|
||||
"""法兰位姿。旋转就是 ``link7`` 的旋转，平移来自 ``link7_to_flange``。"""
|
||||
rot, pos = frames["link7"]
|
||||
flange = _kinematics()["link7_to_flange"]
|
||||
pos = _add(pos, _apply(rot, (float(flange["x"]), float(flange["y"]), float(flange["z"]))))
|
||||
return rot, pos
|
||||
|
||||
|
||||
def _three_floats(raw: Any) -> bool:
|
||||
if not isinstance(raw, (list, tuple)) or len(raw) != 3:
|
||||
return False
|
||||
return all(isinstance(item, (int, float)) and not isinstance(item, bool) for item in raw)
|
||||
|
||||
|
||||
def stack_running() -> bool:
|
||||
return _stack_arm() is not None or _alive("ros2_control_node")
|
||||
|
||||
|
||||
def control_holds_rdk() -> bool:
|
||||
"""有控制命令在用 RDK 时，状态轮询必须让开。"""
|
||||
with _HOLD_LOCK:
|
||||
return _HOLD > 0
|
||||
|
||||
|
||||
def release_for_control() -> None:
|
||||
"""停掉正在读状态的 RDK 子进程，把机器人交给控制命令。"""
|
||||
with _HOLD_LOCK:
|
||||
global _HOLD
|
||||
_HOLD += 1
|
||||
_stop_status_probes()
|
||||
deadline = time.time() + 3.0
|
||||
while time.time() < deadline and _status_probe_running():
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
def resume_after_control() -> None:
|
||||
with _HOLD_LOCK:
|
||||
global _HOLD
|
||||
_HOLD = max(0, _HOLD - 1)
|
||||
|
||||
|
||||
def hold_until_idle(pid: int, *, until_stack: bool = False) -> None:
|
||||
"""后台脚本还在时继续占着 RDK。进程退出，或控制栈已经起来，就放开。"""
|
||||
def _watch() -> None:
|
||||
while True:
|
||||
if until_stack and stack_running():
|
||||
break
|
||||
if not _pid_alive(pid):
|
||||
break
|
||||
time.sleep(0.3)
|
||||
resume_after_control()
|
||||
|
||||
threading.Thread(target=_watch, daemon=True, name="arm-rdk-hold").start()
|
||||
|
||||
|
||||
def motion_blocked(action: str, *, stack_up: bool, op_status: str) -> str:
|
||||
"""控制栈没起来时，动臂按钮给一句能执行的原因，而不是把脚本挂在那儿。"""
|
||||
needs_stack = {
|
||||
"pipeline", "stage_track", "stage_locate", "stage_screw",
|
||||
"reset_home", "gauge_run", "handeye_calibrate",
|
||||
}
|
||||
if action not in needs_stack or stack_up:
|
||||
return ""
|
||||
manual = "MANUAL" in str(op_status or "").upper()
|
||||
tail = "示教器现在是 Manual，要先拨到 Auto（Remote）。" if manual else ""
|
||||
return f"控制栈没起来，这个动作动不了臂。{tail}请先在操作页执行「上电控制栈」。"
|
||||
|
||||
|
||||
def start() -> None:
|
||||
global _STARTED
|
||||
with _LOCK:
|
||||
if _STARTED:
|
||||
return
|
||||
_STARTED = True
|
||||
threading.Thread(target=_loop, daemon=True, name="arm-state").start()
|
||||
|
||||
|
||||
def retain_pose(fresh: dict, previous: dict | None, *, now: float | None = None, hold_s: float = 15.0) -> dict:
|
||||
"""这一拍没读到关节角时，短时间沿用上一拍，避免模型被打回零位。
|
||||
|
||||
超过 ``hold_s`` 仍没有新读数就放开，界面回到“没连上”，不把过期姿态一直当成当前。
|
||||
"""
|
||||
if fresh.get("q") is not None or not previous or previous.get("q") is None:
|
||||
return fresh
|
||||
moment = time.time() if now is None else now
|
||||
if moment - float(previous.get("stamp") or 0.0) > hold_s:
|
||||
return fresh
|
||||
kept = dict(fresh)
|
||||
kept["q"] = list(previous["q"])
|
||||
kept["points"] = list(previous.get("points") or [])
|
||||
kept["pose_live"] = True
|
||||
note = "关节角沿用上次读数。"
|
||||
summary = str(kept.get("summary") or "")
|
||||
if note not in summary:
|
||||
kept["summary"] = f"{summary} {note}".strip()
|
||||
return kept
|
||||
|
||||
|
||||
def _loop() -> None:
|
||||
while not _STOP:
|
||||
if control_holds_rdk():
|
||||
time.sleep(0.2)
|
||||
continue
|
||||
try:
|
||||
arms = _arms()
|
||||
if not arms:
|
||||
time.sleep(2.0)
|
||||
continue
|
||||
stack_arm = _stack_arm()
|
||||
stack_up = stack_arm is not None or _alive("ros2_control_node")
|
||||
bridge_up = _alive("rdk_drag_bridge.py")
|
||||
joints = _read_joints() if stack_up else None
|
||||
if stack_up:
|
||||
targets = [row for row in arms if stack_arm is None or row["id"] == stack_arm]
|
||||
if not targets:
|
||||
targets = arms
|
||||
shared = stack_arm is None
|
||||
for row in targets:
|
||||
fresh = present(
|
||||
row["id"],
|
||||
stack_up=True,
|
||||
bridge_up=bridge_up,
|
||||
probe={"_shared_stack": True} if shared else None,
|
||||
joints=joints,
|
||||
)
|
||||
_store(retain_pose(fresh, snapshot(row["id"])))
|
||||
else:
|
||||
for row in arms:
|
||||
probe: dict | None = None
|
||||
error = ""
|
||||
if control_holds_rdk():
|
||||
break
|
||||
if not bridge_up:
|
||||
probe, error = _probe(row["sn"], row["iface"], row["pc_ip"])
|
||||
fresh = present(
|
||||
row["id"],
|
||||
stack_up=False,
|
||||
bridge_up=bridge_up,
|
||||
probe=probe,
|
||||
joints=None,
|
||||
error=error,
|
||||
)
|
||||
_store(retain_pose(fresh, snapshot(row["id"])))
|
||||
except Exception as exc: # noqa: BLE001 —— 读状态失败不能把界面线程打死
|
||||
for row in _arms():
|
||||
_store(present(
|
||||
row["id"], stack_up=False, bridge_up=False, probe=None, joints=None, error=str(exc),
|
||||
))
|
||||
time.sleep(4.0)
|
||||
|
||||
|
||||
def _stop_status_probes() -> None:
|
||||
try:
|
||||
subprocess.run(
|
||||
["pkill", "-f", "scripts/lib/rdk_status.py"],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return
|
||||
|
||||
|
||||
def _status_probe_running() -> bool:
|
||||
return "rdk_status.py" in _pgrep("scripts/lib/rdk_status.py")
|
||||
|
||||
|
||||
def _pid_alive(pid: int) -> bool:
|
||||
if pid <= 0:
|
||||
return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def _store(row: dict) -> None:
|
||||
with _LOCK:
|
||||
_CACHE[str(row.get("arm") or "")] = row
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
path = ros2_root() / "config" / "site.yaml"
|
||||
if not path.is_file():
|
||||
return []
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
return [
|
||||
{"id": row["id"], "sn": row["sn"], "iface": row["iface"], "pc_ip": row["pc_ip"]}
|
||||
for row in listed_arms(data)
|
||||
]
|
||||
|
||||
|
||||
def _stack_arm() -> str | None:
|
||||
text = _pgrep("run_real.sh")
|
||||
ids = {row["id"] for row in _arms()}
|
||||
for match in re.finditer(r"--arm(?:=|\s+)(\S+)", text):
|
||||
token = match.group(1)
|
||||
if token in ids:
|
||||
return token
|
||||
return None
|
||||
|
||||
|
||||
def _alive(pattern: str) -> bool:
|
||||
return bool(_pgrep(pattern).strip())
|
||||
|
||||
|
||||
def _pgrep(pattern: str) -> str:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["pgrep", "-af", pattern],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return ""
|
||||
lines = []
|
||||
for line in proc.stdout.splitlines():
|
||||
if "pgrep" in line:
|
||||
continue
|
||||
lines.append(line)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def _read_joints() -> list[float] | None:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["timeout", "4", "ros2", "topic", "echo", "/joint_states", "--once"],
|
||||
capture_output=True, text=True, timeout=6,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return None
|
||||
return _parse_joint_echo(proc.stdout)
|
||||
|
||||
|
||||
def _parse_joint_echo(text: str) -> list[float] | None:
|
||||
for chunk in text.split("---"):
|
||||
if "position:" not in chunk:
|
||||
continue
|
||||
try:
|
||||
data = yaml.safe_load(chunk)
|
||||
except yaml.YAMLError:
|
||||
continue
|
||||
if not isinstance(data, dict):
|
||||
continue
|
||||
names = list(data.get("name") or [])
|
||||
pos = list(data.get("position") or [])
|
||||
if names and len(names) == len(pos):
|
||||
by = {str(name): float(value) for name, value in zip(names, pos)}
|
||||
q = []
|
||||
for joint in _JOINTS:
|
||||
key = next((name for name in by if name == joint or name.endswith(joint)), None)
|
||||
if key is None:
|
||||
q = []
|
||||
break
|
||||
q.append(by[key])
|
||||
if len(q) == 7:
|
||||
return q
|
||||
if len(pos) >= 7:
|
||||
try:
|
||||
return [float(v) for v in pos[:7]]
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
return None
|
||||
|
||||
|
||||
def _probe(sn: str, iface: str, pc_ip: str = "") -> tuple[dict | None, str]:
|
||||
python = os.environ.get("RDK_PYTHON", "").strip() or str(Path.home() / "rdk_py310" / "bin" / "python")
|
||||
script = ros2_root() / "scripts" / "lib" / "rdk_status.py"
|
||||
if not Path(python).is_file() or not script.is_file():
|
||||
return None, "找不到 RDK 解释器或 rdk_status.py，无法读使能状态"
|
||||
cmd = _probe_cmd(python, script, sn, iface, pc_ip)
|
||||
try:
|
||||
proc = subprocess.run(cmd, capture_output=True, text=True, timeout=8)
|
||||
except subprocess.TimeoutExpired:
|
||||
return None, "读 RDK 超时。若控制栈没开，检查网线，并确认示教器已开 Remote。"
|
||||
except OSError as exc:
|
||||
return None, f"读 RDK 失败：{exc}"
|
||||
payload = _json_object(proc.stdout) or _json_object(proc.stderr)
|
||||
if not payload:
|
||||
tail = (proc.stdout + proc.stderr).strip()[-240:]
|
||||
return None, tail or "RDK 没有返回状态"
|
||||
return payload, ""
|
||||
|
||||
|
||||
def _probe_cmd(python: str, script: Path, sn: str, iface: str, pc_ip: str) -> list[str]:
|
||||
"""RDK 白名单要本机 IPv4。只传网卡名 ``eth0`` 时，内核改名叫 enx… 就永远连不上。"""
|
||||
cmd = [python, str(script), "--sn", sn, "--json"]
|
||||
if iface:
|
||||
cmd += ["--iface", iface]
|
||||
if pc_ip:
|
||||
cmd += ["--pc-ip", pc_ip]
|
||||
return cmd
|
||||
|
||||
|
||||
def _json_object(text: str) -> dict:
|
||||
decoder = json.JSONDecoder()
|
||||
for index, char in enumerate(text):
|
||||
if char != "{":
|
||||
continue
|
||||
try:
|
||||
data, _end = decoder.raw_decode(text[index:])
|
||||
except json.JSONDecodeError:
|
||||
continue
|
||||
if isinstance(data, dict) and "connected" in data:
|
||||
return data
|
||||
return {}
|
||||
|
||||
|
||||
_KIN: dict[str, dict[str, float]] | None = None
|
||||
|
||||
|
||||
def _kinematics() -> dict[str, dict[str, float]]:
|
||||
global _KIN
|
||||
if _KIN is not None:
|
||||
return _KIN
|
||||
root = ros2_root()
|
||||
candidates = (
|
||||
root / "src/flexiv_description/config/Rizon4/default_kinematics.yaml",
|
||||
root / "install/flexiv_description/share/flexiv_description/config/Rizon4/default_kinematics.yaml",
|
||||
)
|
||||
path = next((item for item in candidates if item.is_file()), None)
|
||||
if path is None:
|
||||
raise FileNotFoundError("找不到 Rizon4 default_kinematics.yaml")
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
kin = data.get("kinematics") or {}
|
||||
_KIN = kin
|
||||
return kin
|
||||
|
||||
|
||||
def _rpy(roll: float, pitch: float, yaw: float) -> tuple:
|
||||
return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
|
||||
|
||||
|
||||
def _rx(angle: float) -> tuple:
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
|
||||
|
||||
|
||||
def _ry(angle: float) -> tuple:
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
|
||||
|
||||
|
||||
def _rz(angle: float) -> tuple:
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
|
||||
|
||||
|
||||
def _axis_rot(axis: tuple[float, float, float], angle: float) -> tuple:
|
||||
x, y, z = axis
|
||||
if abs(z) > 0.5:
|
||||
return _rz(angle if z > 0 else -angle)
|
||||
if abs(y) > 0.5:
|
||||
return _ry(angle if y > 0 else -angle)
|
||||
return _rx(angle if x > 0 else -angle)
|
||||
|
||||
|
||||
def _mul(a: tuple, b: tuple) -> tuple:
|
||||
return tuple(
|
||||
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
|
||||
for i in range(3)
|
||||
)
|
||||
|
||||
|
||||
def _apply(rot: tuple, vec: tuple[float, float, float]) -> tuple[float, float, float]:
|
||||
return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3)) # type: ignore[return-value]
|
||||
|
||||
|
||||
def _add(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]:
|
||||
return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
|
||||
|
||||
|
||||
def _seven(raw: Any) -> list[float] | None:
|
||||
if not isinstance(raw, list) or len(raw) < 7:
|
||||
return None
|
||||
try:
|
||||
q = [float(v) for v in raw[:7]]
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if any(v != v or abs(v) > 12.6 for v in q):
|
||||
return None
|
||||
return q
|
||||
@@ -0,0 +1,675 @@
|
||||
"""工位程序里的相机预览。
|
||||
|
||||
只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
|
||||
|
||||
回调里先丢掉当前页没在看的流。彩色按相机原分辨率收进来，不按时钟抽帧，
|
||||
也不在收图的线程里做颜色转换。另一条线只画最新的一张，所以帧率跟相机走，
|
||||
848 和 1280 都不会为了帧率被缩成 480。深度是伪彩色，仍然抽稀，
|
||||
而且没打开深度时不订阅，避免大图挤占彩色。
|
||||
手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 彩色上限。D405 是 848×480，D435 是 1280×720，都不超过它，所以不会被缩小。
|
||||
_COLOR_WIDTH = 1280
|
||||
# 深度上色不是要看的画质。全幅每帧上色会把刷新占满，所以这一路仍抽稀。
|
||||
_DEPTH_WIDTH = 480
|
||||
# 手眼只在正在看的那一页编码，宽度跟彩色预览一样，不另缩一档。
|
||||
_JPEG_WIDTH = 1280
|
||||
# 只留给还在用 ``accept_frame`` 的调用。彩色实况不再用它抽帧：
|
||||
# 相机大约 30 帧，再按这个间隔丢一次，画面会掉到十几帧。
|
||||
_INTERVAL = 1.0 / 30.0
|
||||
_DET_HOLD_S = 1.5
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_STOP = threading.Event()
|
||||
_THREAD: threading.Thread | None = None
|
||||
_JPEG: dict[str, bytes] = {}
|
||||
_PREVIEW: dict[str, tuple[bytes, int, int]] = {}
|
||||
_PREVIEW_WANT: set[str] = set()
|
||||
_JPEG_WANT: set[str] = set()
|
||||
_NEXT: dict[str, float] = {}
|
||||
_DET: dict[str, tuple[float, list[float]]] = {}
|
||||
# 没画框的预览。检测一到就在这张上重画，不必等下一帧相机。
|
||||
_CLEAN: dict[str, dict] = {}
|
||||
# 每路最近一秒的到达时刻，用来算帧率。值是 (宽, 高, 帧/秒)，宽高是相机原图。
|
||||
_META: dict[str, tuple[int, int, float]] = {}
|
||||
_STAMPS: dict[str, list[float]] = {}
|
||||
# 每路最新原图。收图线程只往这里放，画图线程只取最新的一张。
|
||||
_RAW: dict[str, dict] = {}
|
||||
_SLOTS: dict[str, list] = {}
|
||||
_SLOT_AT: dict[str, int] = {}
|
||||
_PAINTED: dict[str, int] = {}
|
||||
_WAKE = threading.Event()
|
||||
_PAINT_THREAD: threading.Thread | None = None
|
||||
_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
|
||||
|
||||
|
||||
def preview_targets(site: dict) -> list[dict]:
|
||||
"""主界面只要左臂腕部相机和共用的手外相机。
|
||||
|
||||
右臂腕部不进这一屏。仿真相机名字以 ``sim`` 开头，也不进。
|
||||
推理框的角色仍是 ``wrist`` / ``overhead``，跟话题对得上。
|
||||
"""
|
||||
from arm_application.site_io import listed_arms, overhead_camera
|
||||
|
||||
found: list[dict] = []
|
||||
seen: set[str] = set()
|
||||
left = next((row for row in listed_arms(site) if row.get("side") == "left"), None)
|
||||
name = str((left or {}).get("camera") or "").strip().strip("/")
|
||||
if name and not name.startswith("sim"):
|
||||
cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
|
||||
cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
|
||||
tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
|
||||
seen.add(name)
|
||||
found.append({"id": name, "label": f"左腕 {tag}", "role": "wrist"})
|
||||
cam, _others = overhead_camera(site or {}, None)
|
||||
oname = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if oname and not oname.startswith("sim") and oname not in seen:
|
||||
tag = str((cam or {}).get("model") or "D435").strip().upper() or "D435"
|
||||
found.append({"id": oname, "label": f"手外 {tag}", "role": "overhead"})
|
||||
return found
|
||||
|
||||
|
||||
def sim_preview_targets(site: dict) -> list[dict]:
|
||||
"""仿真腕部和仿真手外。画面进主界面，不另开 Gazebo 的相机窗。"""
|
||||
from arm_application.site_io import overhead_camera
|
||||
|
||||
found: list[dict] = []
|
||||
robots = ((site or {}).get("robot") or {}).get("robots") or []
|
||||
cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
if str(robot.get("sn") or "").strip().lower() != "sim":
|
||||
continue
|
||||
cam = robot.get("camera") if isinstance(robot.get("camera"), dict) else {}
|
||||
name = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if not name:
|
||||
continue
|
||||
cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
|
||||
tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
|
||||
found.append({"id": name, "label": f"仿真左腕 {tag}", "role": "wrist", "source": "sim"})
|
||||
break
|
||||
overhead, _others = overhead_camera(site or {}, {"arm": "sim"})
|
||||
oname = str((overhead or {}).get("name") or "").strip().strip("/")
|
||||
if oname.startswith("sim"):
|
||||
tag = str((overhead or {}).get("model") or "D435").strip().upper() or "D435"
|
||||
found.append({"id": oname, "label": f"仿真手外 {tag}", "role": "overhead", "source": "sim"})
|
||||
return found
|
||||
|
||||
|
||||
def stream_targets(site: dict | None = None) -> list[dict]:
|
||||
"""真机和仿真都订上。主界面切换看哪一组，没在看的那组回调里直接丢掉。"""
|
||||
if site is None:
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
site = _site()
|
||||
real = preview_targets(site)
|
||||
for item in real:
|
||||
item["source"] = "real"
|
||||
seen = {item["id"] for item in real}
|
||||
return real + [item for item in sim_preview_targets(site) if item["id"] not in seen]
|
||||
|
||||
|
||||
def targets() -> list[dict]:
|
||||
"""当前站点的主界面真机相机。规则在 ``preview_targets``。"""
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
return preview_targets(_site())
|
||||
|
||||
|
||||
def sim_targets() -> list[dict]:
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
return sim_preview_targets(_site())
|
||||
|
||||
|
||||
def state() -> dict:
|
||||
with _LOCK:
|
||||
return {
|
||||
"on": _STATE["on"],
|
||||
"error": _STATE["error"],
|
||||
"stamp": _STATE["stamp"],
|
||||
"frames": dict(_STATE["frames"]),
|
||||
}
|
||||
|
||||
|
||||
def frame(name: str, kind: str) -> bytes | None:
|
||||
with _LOCK:
|
||||
return _JPEG.get(f"{name}_{kind}")
|
||||
|
||||
|
||||
def previews() -> dict[str, tuple[bytes, int, int]]:
|
||||
"""当前预览。值是 ``(rgb888, 宽, 高)``，同一帧反复取到的是同一块字节。"""
|
||||
with _LOCK:
|
||||
return dict(_PREVIEW)
|
||||
|
||||
|
||||
def preview_meta() -> dict[str, tuple[int, int, float]]:
|
||||
"""每路相机原图的宽、高，以及最近一秒的帧率。还没来帧就是空。"""
|
||||
with _LOCK:
|
||||
return dict(_META)
|
||||
|
||||
|
||||
def _mark_rate(key: str, width: int, height: int) -> None:
|
||||
"""记下这一帧的原图尺寸，并用最近一秒的到达间隔算帧率。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
stamps = _STAMPS.setdefault(key, [])
|
||||
stamps.append(now)
|
||||
cut = now - 1.0
|
||||
while len(stamps) > 1 and stamps[0] < cut:
|
||||
stamps.pop(0)
|
||||
if len(stamps) >= 2 and stamps[-1] > stamps[0]:
|
||||
fps = (len(stamps) - 1) / (stamps[-1] - stamps[0])
|
||||
else:
|
||||
fps = 0.0
|
||||
_META[key] = (int(width), int(height), float(fps))
|
||||
|
||||
|
||||
def watch(preview: set[str], jpeg: set[str] | None = None) -> None:
|
||||
"""当前页要哪几路。没点名的流在回调里直接丢掉，不拷图像。
|
||||
|
||||
``preview`` 走原分辨率 RGB，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。
|
||||
"""
|
||||
global _PREVIEW_WANT, _JPEG_WANT
|
||||
jpeg = set(jpeg or ())
|
||||
preview = set(preview)
|
||||
with _LOCK:
|
||||
if preview == _PREVIEW_WANT and jpeg == _JPEG_WANT:
|
||||
return
|
||||
_PREVIEW_WANT = preview
|
||||
_JPEG_WANT = jpeg
|
||||
for key in preview | jpeg:
|
||||
_NEXT.pop(key, None)
|
||||
|
||||
|
||||
def wants(key: str) -> tuple[bool, bool]:
|
||||
"""返回 ``(要预览, 要 JPEG)``。"""
|
||||
with _LOCK:
|
||||
return key in _PREVIEW_WANT, key in _JPEG_WANT
|
||||
|
||||
|
||||
def offer_color(key: str, image: np.ndarray, *, bgr: bool, role: str) -> None:
|
||||
"""收一帧彩色原图。没人看就丢掉。有人看就整幅留下，不按时钟抽帧。"""
|
||||
preview, jpeg = wants(key)
|
||||
if not preview and not jpeg:
|
||||
return
|
||||
if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
|
||||
return
|
||||
_offer(key, image, bgr=bgr, role=role, kind="color")
|
||||
|
||||
|
||||
def offer_depth(key: str, image: np.ndarray) -> None:
|
||||
"""收一帧深度原图。伪彩色仍在画图线程里做，不堵收图。"""
|
||||
preview, _jpeg = wants(key)
|
||||
if not preview:
|
||||
return
|
||||
if image.ndim != 2 or image.shape[0] < 1 or image.shape[1] < 1:
|
||||
return
|
||||
_offer(key, image, bgr=False, role="", kind="depth")
|
||||
|
||||
|
||||
def _offer(key: str, image: np.ndarray, *, bgr: bool, role: str, kind: str) -> None:
|
||||
height, width = int(image.shape[0]), int(image.shape[1])
|
||||
with _LOCK:
|
||||
slots = _SLOTS.setdefault(key, [None, None])
|
||||
index = _SLOT_AT.get(key, 0)
|
||||
slot = slots[index]
|
||||
if slot is None or slot.shape != image.shape or slot.dtype != image.dtype:
|
||||
slot = np.empty(image.shape, dtype=image.dtype)
|
||||
slots[index] = slot
|
||||
np.copyto(slot, image)
|
||||
seq = int(_RAW.get(key, {}).get("seq", 0)) + 1
|
||||
_RAW[key] = {
|
||||
"seq": seq,
|
||||
"frame": slot,
|
||||
"bgr": bool(bgr),
|
||||
"role": role,
|
||||
"kind": kind,
|
||||
"w": width,
|
||||
"h": height,
|
||||
}
|
||||
_SLOT_AT[key] = 1 - index
|
||||
_mark_rate(key, width, height)
|
||||
_WAKE.set()
|
||||
|
||||
|
||||
def publish_latest() -> None:
|
||||
"""把各路还没画的最新原图做成预览。中间帧直接跳过，避免越画越落后。"""
|
||||
with _LOCK:
|
||||
jobs = []
|
||||
for key, item in _RAW.items():
|
||||
if _PAINTED.get(key) == item["seq"]:
|
||||
continue
|
||||
jobs.append((
|
||||
key,
|
||||
int(item["seq"]),
|
||||
np.array(item["frame"], copy=True),
|
||||
bool(item["bgr"]),
|
||||
str(item["role"]),
|
||||
str(item["kind"]),
|
||||
int(item["w"]),
|
||||
int(item["h"]),
|
||||
))
|
||||
for key, seq, frame, bgr, role, kind, width, height in jobs:
|
||||
try:
|
||||
preview, jpeg = wants(key)
|
||||
if kind == "depth":
|
||||
if preview:
|
||||
colored = depth_preview(frame)
|
||||
_store(key, colored, preview=colored, jpeg=False)
|
||||
elif preview or jpeg:
|
||||
limit = _JPEG_WIDTH if jpeg else _COLOR_WIDTH
|
||||
rgb = rgb_preview(frame, bgr=bgr, limit=limit)
|
||||
shown = rgb
|
||||
if preview and int(rgb.shape[1]) > _COLOR_WIDTH:
|
||||
shown = rgb_preview(rgb, bgr=False, limit=_COLOR_WIDTH)
|
||||
if preview:
|
||||
_remember_clean(key, shown, role, width, height)
|
||||
shown = _overlay_detection(shown, role, width, height)
|
||||
_store(key, rgb, preview=shown if preview else None, jpeg=jpeg)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，下一帧继续
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
finally:
|
||||
with _LOCK:
|
||||
if _PAINTED.get(key, 0) < seq:
|
||||
_PAINTED[key] = seq
|
||||
|
||||
|
||||
def _paint_loop() -> None:
|
||||
while not _STOP.is_set():
|
||||
_WAKE.wait(0.005)
|
||||
_WAKE.clear()
|
||||
if _STOP.is_set():
|
||||
return
|
||||
try:
|
||||
publish_latest()
|
||||
except Exception as exc: # noqa: BLE001 —— 画图线程不能因为一帧坏掉就退出
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
|
||||
|
||||
def accept_frame(key: str) -> bool:
|
||||
"""这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。
|
||||
|
||||
彩色实况不再走这里。固定间隔会把相机自己的帧率再砍掉一截。
|
||||
"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
if key not in _PREVIEW_WANT and key not in _JPEG_WANT:
|
||||
return False
|
||||
if now < _NEXT.get(key, 0.0):
|
||||
return False
|
||||
_NEXT[key] = now + _INTERVAL
|
||||
return True
|
||||
|
||||
|
||||
def _close_external_viewers() -> None:
|
||||
"""关掉独立的四窗格查看器。它会把左右红外挂出来，本程序不再用它。"""
|
||||
subprocess.run(["pkill", "-f", "lib/arm_application/realsense_view"], check=False)
|
||||
subprocess.run(["pkill", "-f", "arm_application.realsense_view"], check=False)
|
||||
|
||||
|
||||
def paint_detection(
|
||||
rgb: np.ndarray,
|
||||
xyxy: list[float] | tuple,
|
||||
src_width: int,
|
||||
src_height: int,
|
||||
) -> np.ndarray:
|
||||
"""把推理结果画到相机预览上：绿框、画面中心、跟踪红点、置信度和距离。
|
||||
|
||||
``xyxy`` 前四项是原图像素的框。够长时第 5 项是置信度，第 7、8 项是跟踪点，
|
||||
第 9 项是距离（米）。空框原样返回。
|
||||
"""
|
||||
import cv2
|
||||
|
||||
if rgb.ndim != 3 or src_width <= 0 or src_height <= 0 or len(xyxy) < 4:
|
||||
return rgb
|
||||
scale_x = rgb.shape[1] / float(src_width)
|
||||
scale_y = rgb.shape[0] / float(src_height)
|
||||
x1 = int(round(float(xyxy[0]) * scale_x))
|
||||
y1 = int(round(float(xyxy[1]) * scale_y))
|
||||
x2 = int(round(float(xyxy[2]) * scale_x))
|
||||
y2 = int(round(float(xyxy[3]) * scale_y))
|
||||
height, width = rgb.shape[:2]
|
||||
x1 = max(0, min(width - 1, x1))
|
||||
x2 = max(0, min(width - 1, x2))
|
||||
y1 = max(0, min(height - 1, y1))
|
||||
y2 = max(0, min(height - 1, y2))
|
||||
if x2 <= x1 or y2 <= y1:
|
||||
return rgb
|
||||
out = np.array(rgb, copy=True)
|
||||
thick = max(2, width // 180)
|
||||
cv2.rectangle(out, (x1, y1), (x2, y2), (80, 220, 120), thick)
|
||||
cv2.drawMarker(
|
||||
out,
|
||||
(width // 2, height // 2),
|
||||
(255, 220, 60),
|
||||
cv2.MARKER_CROSS,
|
||||
max(8, width // 24),
|
||||
1,
|
||||
)
|
||||
if len(xyxy) >= 8:
|
||||
px = int(round(float(xyxy[6]) * scale_x))
|
||||
py = int(round(float(xyxy[7]) * scale_y))
|
||||
px = max(0, min(width - 1, px))
|
||||
py = max(0, min(height - 1, py))
|
||||
cv2.circle(out, (px, py), max(3, width // 80), (255, 48, 48), -1)
|
||||
if len(xyxy) >= 5:
|
||||
conf = float(xyxy[4])
|
||||
dist = f" z={float(xyxy[8]):.3f}m" if len(xyxy) >= 9 else ""
|
||||
cv2.putText(
|
||||
out,
|
||||
f"{conf:.2f}{dist}",
|
||||
(x1, max(14, y1 - 6)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.45,
|
||||
(80, 220, 120),
|
||||
1,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def inference_status() -> str:
|
||||
"""腕部和手外最近一次推理框。超过保持时间就当没有。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
items = dict(_DET)
|
||||
parts = []
|
||||
for role, title in (("wrist", "腕部"), ("overhead", "手外")):
|
||||
item = items.get(role)
|
||||
if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 5:
|
||||
parts.append(f"{title} 没有框")
|
||||
continue
|
||||
parts.append(f"{title} 置信 {float(item[1][4]):.2f}")
|
||||
return "推理 " + " ".join(parts)
|
||||
|
||||
|
||||
def _remember_detection(role: str, data: list[float]) -> None:
|
||||
with _LOCK:
|
||||
if len(data) < 4:
|
||||
_DET.pop(role, None)
|
||||
else:
|
||||
_DET[role] = (time.monotonic(), data)
|
||||
_stamp_role(role)
|
||||
|
||||
|
||||
def _remember_clean(key: str, rgb: np.ndarray, role: str, src_w: int, src_h: int) -> None:
|
||||
with _LOCK:
|
||||
_CLEAN[key] = {
|
||||
"rgb": np.array(rgb, copy=True),
|
||||
"role": role,
|
||||
"src_w": int(src_w),
|
||||
"src_h": int(src_h),
|
||||
}
|
||||
|
||||
|
||||
def _overlay_detection(rgb: np.ndarray, role: str, src_width: int, src_height: int) -> np.ndarray:
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
item = _DET.get(role)
|
||||
if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 4:
|
||||
return rgb
|
||||
return paint_detection(rgb, item[1], src_width, src_height)
|
||||
|
||||
|
||||
def _stamp_role(role: str) -> None:
|
||||
"""检测更新后，把框画回这一路相机已经在显示的画面上。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
frames = [(key, dict(meta)) for key, meta in _CLEAN.items() if meta.get("role") == role]
|
||||
item = _DET.get(role)
|
||||
fresh = item is not None and now - item[0] <= _DET_HOLD_S and len(item[1]) >= 4
|
||||
for key, meta in frames:
|
||||
shown = meta["rgb"]
|
||||
if fresh:
|
||||
shown = paint_detection(shown, item[1], meta["src_w"], meta["src_h"])
|
||||
with _LOCK:
|
||||
_PREVIEW[key] = (np.ascontiguousarray(shown).tobytes(), int(shown.shape[1]), int(shown.shape[0]))
|
||||
_STATE["frames"][key] = True
|
||||
_STATE["stamp"] = time.time()
|
||||
|
||||
|
||||
def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _COLOR_WIDTH) -> np.ndarray:
|
||||
"""缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。"""
|
||||
import cv2
|
||||
|
||||
if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
|
||||
raise ValueError("彩色预览要的是高×宽×3")
|
||||
view = image
|
||||
width = int(view.shape[1])
|
||||
if width > limit:
|
||||
height = max(1, int(round(int(view.shape[0]) * limit / float(width))))
|
||||
view = cv2.resize(view, (limit, height), interpolation=cv2.INTER_AREA)
|
||||
if bgr:
|
||||
return cv2.cvtColor(view, cv2.COLOR_BGR2RGB)
|
||||
if not view.flags["OWNDATA"] or not view.flags["C_CONTIGUOUS"]:
|
||||
return np.ascontiguousarray(view)
|
||||
return view
|
||||
|
||||
|
||||
def depth_preview(depth: np.ndarray, limit: int = _DEPTH_WIDTH) -> np.ndarray:
|
||||
"""uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。"""
|
||||
import cv2
|
||||
|
||||
if depth.ndim != 2 or depth.shape[0] < 1 or depth.shape[1] < 1:
|
||||
raise ValueError("深度预览要的是高×宽")
|
||||
step = max(1, int(depth.shape[1]) // limit)
|
||||
small = depth[::step, ::step]
|
||||
valid = small > 0
|
||||
gray = np.zeros(small.shape, np.uint8)
|
||||
if bool(valid.any()):
|
||||
sample = small[valid]
|
||||
if sample.size > 8000:
|
||||
sample = sample[:: max(1, sample.size // 8000)]
|
||||
lo, hi = np.percentile(sample.astype(np.float32, copy=False), [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
norm = np.clip((small.astype(np.float32) - float(lo)) / (hi - float(lo)), 0.0, 1.0)
|
||||
gray = (norm * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
colored[~valid] = 0
|
||||
return rgb_preview(colored, bgr=True, limit=limit)
|
||||
|
||||
|
||||
def _jpeg_bgr(rgb: np.ndarray) -> bytes | None:
|
||||
import cv2
|
||||
|
||||
if rgb.size == 0:
|
||||
return None
|
||||
ok, buf = cv2.imencode(
|
||||
".jpg",
|
||||
cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR),
|
||||
[int(cv2.IMWRITE_JPEG_QUALITY), 70],
|
||||
)
|
||||
if not ok:
|
||||
return None
|
||||
return buf.tobytes()
|
||||
|
||||
|
||||
def _as_array(data, dtype, count: int) -> np.ndarray:
|
||||
"""优先直接看消息缓冲。缓冲不支持时才拷一份。"""
|
||||
try:
|
||||
arr = np.frombuffer(data, dtype=dtype, count=count)
|
||||
except (TypeError, BufferError, ValueError):
|
||||
arr = np.frombuffer(bytes(data), dtype=dtype, count=count)
|
||||
if arr.size < count:
|
||||
raise ValueError("图像字节数不够")
|
||||
return arr
|
||||
|
||||
|
||||
def _color_view(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
need = int(msg.height) * int(msg.width) * 3
|
||||
arr = _as_array(msg.data, np.uint8, need)
|
||||
return arr.reshape(int(msg.height), int(msg.width), 3)
|
||||
|
||||
|
||||
def _depth_view(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
need = int(msg.height) * int(msg.width)
|
||||
arr = _as_array(msg.data, np.uint16, need)
|
||||
return arr.reshape(int(msg.height), int(msg.width))
|
||||
|
||||
|
||||
def _store(key: str, rgb: np.ndarray, *, preview: np.ndarray | None, jpeg: bool) -> None:
|
||||
encoded = _jpeg_bgr(rgb) if jpeg else None
|
||||
with _LOCK:
|
||||
if preview is not None:
|
||||
_PREVIEW[key] = (preview.tobytes(), int(preview.shape[1]), int(preview.shape[0]))
|
||||
if encoded:
|
||||
_JPEG[key] = encoded
|
||||
if preview is not None or encoded:
|
||||
_STATE["frames"][key] = True
|
||||
_STATE["stamp"] = time.time()
|
||||
_STATE["error"] = ""
|
||||
|
||||
|
||||
def _loop(cams: list[dict]) -> None:
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
class _Node(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("operator_camera_view")
|
||||
det_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray, "workpiece/detection", self._det("wrist"), det_qos
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray,
|
||||
"workpiece/detection_overhead",
|
||||
self._det("overhead"),
|
||||
det_qos,
|
||||
)
|
||||
self._depth_subs: dict[str, object] = {}
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
role = str(cam.get("role") or "wrist")
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/color/image_raw",
|
||||
self._color(name, role),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
|
||||
def sync_depth(self) -> None:
|
||||
"""深度没打开就不订。订上之后每一帧都会整幅拷进进程，彩色帧率会被拖下去。"""
|
||||
with _LOCK:
|
||||
want = {
|
||||
cam["id"]
|
||||
for cam in cams
|
||||
if f"{cam['id']}_depth" in _PREVIEW_WANT
|
||||
}
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
if name in want and name not in self._depth_subs:
|
||||
self._depth_subs[name] = self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/aligned_depth_to_color/image_raw",
|
||||
self._depth(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
elif name not in want and name in self._depth_subs:
|
||||
self.destroy_subscription(self._depth_subs.pop(name))
|
||||
|
||||
def _det(self, role: str):
|
||||
def _cb(msg: Float32MultiArray) -> None:
|
||||
_remember_detection(role, [float(v) for v in msg.data])
|
||||
return _cb
|
||||
|
||||
def _color(self, name: str, role: str):
|
||||
key = f"{name}_color"
|
||||
|
||||
def _cb(msg: Image) -> None:
|
||||
# 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
|
||||
try:
|
||||
view = _color_view(msg)
|
||||
if view is None:
|
||||
return
|
||||
offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
return _cb
|
||||
|
||||
def _depth(self, name: str):
|
||||
key = f"{name}_depth"
|
||||
|
||||
def _cb(msg: Image) -> None:
|
||||
try:
|
||||
view = _depth_view(msg)
|
||||
if view is None:
|
||||
return
|
||||
offer_depth(key, view)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
return _cb
|
||||
|
||||
rclpy.init()
|
||||
node = _Node()
|
||||
try:
|
||||
while not _STOP.is_set():
|
||||
node.sync_depth()
|
||||
rclpy.spin_once(node, timeout_sec=0.01)
|
||||
except Exception as exc:
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
with _LOCK:
|
||||
_STATE["on"] = False
|
||||
|
||||
|
||||
def start() -> str:
|
||||
global _THREAD, _PAINT_THREAD
|
||||
cams = stream_targets()
|
||||
if not cams:
|
||||
with _LOCK:
|
||||
_STATE["error"] = "site 里没有可预览的真机相机"
|
||||
return _STATE["error"]
|
||||
_close_external_viewers()
|
||||
with _LOCK:
|
||||
if _STATE["on"]:
|
||||
return "相机预览已经在程序里"
|
||||
_STATE["on"] = True
|
||||
_STATE["error"] = ""
|
||||
_STOP.clear()
|
||||
_WAKE.clear()
|
||||
_THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
|
||||
_THREAD.start()
|
||||
_PAINT_THREAD = threading.Thread(target=_paint_loop, daemon=True, name="cam-paint")
|
||||
_PAINT_THREAD.start()
|
||||
names = "、".join(cam["label"] for cam in cams)
|
||||
return f"预览订了 {names} 的彩色原图，深度点开才订，不含左右红外。"
|
||||
|
||||
|
||||
def stop() -> None:
|
||||
_STOP.set()
|
||||
_WAKE.set()
|
||||
for thread in (_THREAD, _PAINT_THREAD):
|
||||
if thread is not None:
|
||||
thread.join(timeout=1.5)
|
||||
@@ -0,0 +1,967 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Orisys 工位界面。
|
||||
|
||||
只做两件事：把本机真实状态摊开，以及按白名单启动仓库里已有的脚本。
|
||||
会动臂的动作必须带 ``confirm=true``，避免页面上误点一下就上电。
|
||||
|
||||
bash ~/ros2/scripts/run_console.sh
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import signal
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
from urllib.parse import urlparse
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application import operator_tactile_view as tactile_view
|
||||
from arm_application.site_io import eye_to_hand_result_name, listed_arms, overhead_camera, usb_expectations
|
||||
|
||||
HOST = "127.0.0.1"
|
||||
PORT = 8766
|
||||
_ROOT = Path(__file__).resolve()
|
||||
for _p in _ROOT.parents:
|
||||
if (_p / "scripts" / "run_real.sh").is_file():
|
||||
ROOT = _p
|
||||
break
|
||||
else:
|
||||
ROOT = Path.home() / "ros2"
|
||||
|
||||
LOG_DIR = Path("/tmp/orisys_console")
|
||||
|
||||
# 动臂和软拖拽都要这台臂，不能一边下发位置一边再开拖拽。
|
||||
# 相机、夹爪、触觉不碰这条，也不该排在它后面等。
|
||||
_ROBOT_LANES = frozenset({"arm", "teach"})
|
||||
_ARM_ACTIONS = frozenset({
|
||||
"pipeline", "stage_track", "stage_locate", "stage_screw",
|
||||
"reset_home", "gauge_run", "real_up", "handeye_calibrate",
|
||||
})
|
||||
_LANE_WAIT = {
|
||||
"grip": "夹爪上一条还没到位",
|
||||
"camera": "相机命令还没返回",
|
||||
"tactile": "触觉命令还没返回",
|
||||
"arm": "这台臂上一条运动还没结束",
|
||||
"teach": "软拖拽还没返回",
|
||||
}
|
||||
|
||||
|
||||
def command_lane(action: str) -> str:
|
||||
"""每类命令一条通道。同一条还没返回才拒绝，不让相机去等夹爪。"""
|
||||
if action == "stop_commands":
|
||||
return "stop"
|
||||
if action in ("camera_d405", "camera_d435"):
|
||||
return "camera"
|
||||
if action.startswith("tactile"):
|
||||
return "tactile"
|
||||
if action.startswith("gripper"):
|
||||
return "grip"
|
||||
if action == "handeye_teach":
|
||||
return "teach"
|
||||
if action in _ARM_ACTIONS:
|
||||
return "arm"
|
||||
return action
|
||||
|
||||
|
||||
def command_blocked_message(running: set[str], lane: str) -> str:
|
||||
"""这条通道，或同一台臂上的另一条运动，还没结束时返回要显示的句子。"""
|
||||
if lane == "stop":
|
||||
return ""
|
||||
if lane in running:
|
||||
return _LANE_WAIT.get(lane, "这一条命令还没返回")
|
||||
if lane in _ROBOT_LANES:
|
||||
other = next((name for name in _ROBOT_LANES if name != lane and name in running), "")
|
||||
if other == "arm":
|
||||
return "这台臂还在动，软拖拽先不开"
|
||||
if other == "teach":
|
||||
return "软拖拽占着这台臂，运动先不开"
|
||||
return ""
|
||||
ASSETS = Path(__file__).resolve().parent
|
||||
_LOCK = threading.Lock()
|
||||
_TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
|
||||
_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
|
||||
_PROCS: dict[str, int] = {}
|
||||
_LIVE: dict[str, subprocess.Popen] = {}
|
||||
_PROC_LOCK = threading.Lock()
|
||||
|
||||
|
||||
def _push_force(side: str, sample: dict) -> None:
|
||||
if side not in _HIST:
|
||||
return
|
||||
point = {
|
||||
"t": float(sample.get("t") or time.time()),
|
||||
"fnormal": sample.get("fnormal"),
|
||||
"fshearx": sample.get("fshearx"),
|
||||
"fsheary": sample.get("fsheary"),
|
||||
}
|
||||
if not any(isinstance(point[k], (int, float)) for k in ("fnormal", "fshearx", "fsheary")):
|
||||
return
|
||||
with _LOCK:
|
||||
_HIST[side].append(point)
|
||||
_TACTILE[side] = {
|
||||
"fnormal": point["fnormal"],
|
||||
"fshearx": point["fshearx"],
|
||||
"fsheary": point["fsheary"],
|
||||
"connected": True,
|
||||
}
|
||||
|
||||
|
||||
tactile_view.set_sample_hook(_push_force)
|
||||
|
||||
|
||||
def _site() -> dict:
|
||||
path = ROOT / "config" / "site.yaml"
|
||||
if not path.is_file():
|
||||
return {}
|
||||
data = yaml.safe_load(path.read_text()) or {}
|
||||
return data if isinstance(data, dict) else {}
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
"""真机臂来自 site.yaml。没有清单就空着，不补本机的 89/91。"""
|
||||
return [
|
||||
{
|
||||
"id": row["id"],
|
||||
"label": row["label"],
|
||||
"sn": row["sn"],
|
||||
"tactile": row["tactile"],
|
||||
"camera": row["camera"],
|
||||
"gripper": row["gripper"],
|
||||
"rotary": row["rotary"],
|
||||
}
|
||||
for row in listed_arms(_site())
|
||||
]
|
||||
|
||||
|
||||
def _default_arm() -> str:
|
||||
rows = _arms()
|
||||
return str(rows[0]["id"]) if rows else ""
|
||||
|
||||
|
||||
def _usb() -> list[dict]:
|
||||
found = {"d405": [], "d435": [], "orisys": []}
|
||||
want = {
|
||||
("8086", "0b5b"): "d405",
|
||||
("8086", "0b07"): "d435",
|
||||
("1bcf", "2281"): "orisys",
|
||||
}
|
||||
base = Path("/sys/bus/usb/devices")
|
||||
if not base.is_dir():
|
||||
return []
|
||||
for dev in base.iterdir():
|
||||
vid = dev / "idVendor"
|
||||
pid = dev / "idProduct"
|
||||
if not vid.is_file() or not pid.is_file():
|
||||
continue
|
||||
key = want.get((vid.read_text().strip(), pid.read_text().strip()))
|
||||
if not key:
|
||||
continue
|
||||
speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
|
||||
found[key].append({"port": dev.name, "speed": speed})
|
||||
expect = usb_expectations(_site())
|
||||
return [
|
||||
{"id": "d405", "label": "腕部 D405", "expect": expect["d405"], "items": found["d405"]},
|
||||
{"id": "d435", "label": "手外 D435", "expect": expect["d435"], "items": found["d435"]},
|
||||
{"id": "orisys", "label": "视触觉", "expect": expect["orisys"], "items": found["orisys"]},
|
||||
]
|
||||
|
||||
|
||||
def _listeners(pattern: str) -> bool:
|
||||
try:
|
||||
out = subprocess.run(
|
||||
["pgrep", "-af", pattern],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
).stdout
|
||||
except Exception:
|
||||
return False
|
||||
for line in out.splitlines():
|
||||
if "pgrep" in line or "operator_console" in line:
|
||||
continue
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def _services() -> list[dict]:
|
||||
rows = [
|
||||
("real", "控制栈", "ros2_control_node"),
|
||||
("tactile", "视触觉节点", "lib/arm_application/tactile"),
|
||||
("camera", "相机", "realsense_color_pub"),
|
||||
("gripper", "夹爪", "dh_pgea|dual_gripper_driver"),
|
||||
("pipeline", "三段流程", "assembly_pipeline"),
|
||||
("gauge", "通止规装配", "gauge_assembly"),
|
||||
]
|
||||
out = []
|
||||
for sid, label, pattern in rows:
|
||||
if "|" in pattern:
|
||||
up = any(_listeners(part) for part in pattern.split("|"))
|
||||
else:
|
||||
up = _listeners(pattern)
|
||||
out.append({"id": sid, "label": label, "up": up})
|
||||
return out
|
||||
|
||||
|
||||
def _grippers() -> list[str]:
|
||||
return sorted(p.name for p in Path("/dev").glob("ttyUSB*"))
|
||||
|
||||
|
||||
def _parse_tactile(text: str) -> dict:
|
||||
got: dict[str, object] = {}
|
||||
for line in text.splitlines():
|
||||
if ":" not in line:
|
||||
continue
|
||||
key, _, raw = line.partition(":")
|
||||
key = key.strip()
|
||||
raw = raw.strip()
|
||||
if key in ("fnormal", "fshearx", "fsheary", "fps"):
|
||||
try:
|
||||
got[key] = float(raw)
|
||||
except ValueError:
|
||||
continue
|
||||
elif key in ("connected", "contact"):
|
||||
got[key] = raw.lower() == "true"
|
||||
return got
|
||||
|
||||
|
||||
def _poll_tactile() -> None:
|
||||
while True:
|
||||
snap = {}
|
||||
for side in ("left", "right"):
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["timeout", "1.2", "ros2", "topic", "echo", f"/tactile/{side}/status", "--once"],
|
||||
capture_output=True, text=True, timeout=2.5,
|
||||
)
|
||||
snap[side] = _parse_tactile(proc.stdout) if proc.returncode == 0 else {}
|
||||
except Exception:
|
||||
snap[side] = {}
|
||||
view_on = bool(tactile_view.state().get("on"))
|
||||
with _LOCK:
|
||||
_TACTILE["stamp"] = time.time()
|
||||
if not view_on:
|
||||
_TACTILE["left"] = snap.get("left") or {}
|
||||
_TACTILE["right"] = snap.get("right") or {}
|
||||
now = time.time()
|
||||
for side in ("left", "right"):
|
||||
row = snap.get(side) or {}
|
||||
if row:
|
||||
_HIST[side].append({
|
||||
"t": now,
|
||||
"fnormal": row.get("fnormal"),
|
||||
"fshearx": row.get("fshearx"),
|
||||
"fsheary": row.get("fsheary"),
|
||||
})
|
||||
time.sleep(0.35)
|
||||
|
||||
|
||||
def _robot(arm: str) -> dict:
|
||||
from arm_application.site_io import arm_short_id
|
||||
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
if sn.lower() == "sim":
|
||||
continue
|
||||
if arm_short_id(sn) == arm:
|
||||
return robot
|
||||
return {}
|
||||
|
||||
|
||||
def _comment(text: str, key: str) -> str:
|
||||
match = re.search(rf"^#\s*{re.escape(key)}:\s*(.+)$", text, re.M)
|
||||
return match.group(1).strip() if match else ""
|
||||
|
||||
|
||||
def _pose_count(path: Path) -> int | None:
|
||||
if not path.is_file():
|
||||
return None
|
||||
match = re.search(r"num_poses:\s*(\d+)", path.read_text(errors="replace"))
|
||||
return int(match.group(1)) if match else None
|
||||
|
||||
|
||||
def _result_card(path: Path, pose_path: Path, board: dict) -> dict:
|
||||
text = path.read_text(errors="replace") if path.is_file() else ""
|
||||
trans = None
|
||||
tool = ""
|
||||
if text:
|
||||
try:
|
||||
data = yaml.safe_load(text) or {}
|
||||
except yaml.YAMLError:
|
||||
data = {}
|
||||
if isinstance(data, dict):
|
||||
raw = data.get("translation_m")
|
||||
if isinstance(raw, list) and len(raw) >= 3:
|
||||
trans = [float(raw[0]), float(raw[1]), float(raw[2])]
|
||||
tool = str(data.get("gripper_tool") or "")
|
||||
meaning = ""
|
||||
for line in text.splitlines():
|
||||
if line.startswith("# T_4x4"):
|
||||
meaning = line[2:].strip()
|
||||
break
|
||||
return {
|
||||
"file": str(path),
|
||||
"exists": path.is_file(),
|
||||
"views": _comment(text, "views"),
|
||||
"utc": _comment(text, "utc"),
|
||||
"board_note": _comment(text, "board"),
|
||||
"camera": _comment(text, "camera_source"),
|
||||
"tool": tool,
|
||||
"meaning": meaning,
|
||||
"translation_m": trans,
|
||||
"poses": _pose_count(pose_path),
|
||||
"pose_file": str(pose_path),
|
||||
"board": {
|
||||
"cols": board.get("cols"),
|
||||
"rows": board.get("rows"),
|
||||
"square_m": board.get("square_size_m"),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _handeye(arm: str) -> dict:
|
||||
site = _site()
|
||||
robot = _robot(arm)
|
||||
paths = robot.get("paths") if isinstance(robot.get("paths"), dict) else {}
|
||||
shared = site.get("paths") if isinstance(site.get("paths"), dict) else {}
|
||||
boards = ((site.get("handeye") or {}).get("boards") or {})
|
||||
wrist_file = Path(str(
|
||||
paths.get("handeye_file")
|
||||
or shared.get("handeye_file")
|
||||
or (ROOT / "handeye" / "results" / f"{arm}_eye_in_hand.yaml")
|
||||
))
|
||||
wrist_pose = Path(str(
|
||||
paths.get("handeye_waypoints")
|
||||
or shared.get("handeye_waypoints")
|
||||
or (ROOT / "handeye" / "poses" / f"{arm}_eye_in_hand.yaml")
|
||||
))
|
||||
cam, _others = overhead_camera(site, robot or None)
|
||||
cam_name = str((cam or {}).get("name") or "")
|
||||
e2h_name = eye_to_hand_result_name(arm, cam_name)
|
||||
e2h_file = Path(str(
|
||||
paths.get("handeye_file_eye_to_hand")
|
||||
or shared.get("handeye_file_eye_to_hand")
|
||||
or (ROOT / "handeye" / "results" / e2h_name)
|
||||
))
|
||||
e2h_pose = Path(str(
|
||||
paths.get("handeye_waypoints_eye_to_hand")
|
||||
or shared.get("handeye_waypoints_eye_to_hand")
|
||||
or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
|
||||
))
|
||||
wrist = _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {})
|
||||
wrist_name = ""
|
||||
if isinstance(robot.get("camera"), dict):
|
||||
wrist_name = str(robot["camera"].get("name") or "").strip().strip("/")
|
||||
wrist["preview_camera"] = wrist_name
|
||||
wrist["preview_label"] = "腕部相机"
|
||||
outside = _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {})
|
||||
outside["preview_camera"] = cam_name.strip().strip("/")
|
||||
outside["preview_label"] = "手外相机"
|
||||
return {
|
||||
"eye_in_hand": wrist,
|
||||
"eye_to_hand": outside,
|
||||
}
|
||||
|
||||
|
||||
def _by_arm(reader) -> dict:
|
||||
return {row["id"]: reader(row["id"]) for row in _arms()}
|
||||
|
||||
|
||||
def _gripper_live() -> dict:
|
||||
from arm_application.operator_gripper_state import snapshot as grip_snapshot
|
||||
return _by_arm(grip_snapshot)
|
||||
|
||||
|
||||
def status() -> dict:
|
||||
with _LOCK:
|
||||
tactile = {
|
||||
"left": dict(_TACTILE["left"]),
|
||||
"right": dict(_TACTILE["right"]),
|
||||
"stamp": _TACTILE["stamp"],
|
||||
}
|
||||
history = {
|
||||
"left": list(_HIST["left"]),
|
||||
"right": list(_HIST["right"]),
|
||||
}
|
||||
from arm_application.operator_arm_state import snapshot as arm_snapshot
|
||||
return {
|
||||
"arms": _arms(),
|
||||
"arm_live": _by_arm(arm_snapshot),
|
||||
"gripper_live": _gripper_live(),
|
||||
"usb": _usb(),
|
||||
"services": _services(),
|
||||
"grippers": _grippers(),
|
||||
"tactile": tactile,
|
||||
"history": history,
|
||||
"view": tactile_view.state(),
|
||||
"handeye": _by_arm(_handeye),
|
||||
"now": time.time(),
|
||||
}
|
||||
|
||||
|
||||
# 会动真机的动作必须 confirm。臂号必须是 site.yaml 里某台真机的末两位。
|
||||
_ACTIONS = {
|
||||
"real_up": {
|
||||
"title": "上电控制栈",
|
||||
"script": "run_real.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "连接机械臂并拉起 ros2_control。",
|
||||
},
|
||||
"camera_d405": {
|
||||
"title": "本臂腕部相机",
|
||||
"script": "run_camera_d405.sh",
|
||||
"args": ["--arm", "{arm}", "--no-preview"],
|
||||
"moves": False,
|
||||
"blurb": "只拉起本臂腕部相机。彩色和深度显示在程序里，不看左右红外，不另开窗口。",
|
||||
},
|
||||
"camera_d435": {
|
||||
"title": "手外 D435",
|
||||
"script": "run_camera_d435.sh",
|
||||
"args": ["--no-preview"],
|
||||
"moves": False,
|
||||
"blurb": "只拉起顶视 D435。彩色和深度显示在程序里，不另开窗口。",
|
||||
},
|
||||
"tactile_up": {
|
||||
"title": "视触觉三力",
|
||||
"script": "run_tactile.sh",
|
||||
"args": [],
|
||||
"moves": False,
|
||||
"blurb": "发布左右指法向力与两路切向力。",
|
||||
},
|
||||
"gripper_up": {
|
||||
"title": "夹爪节点",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": False,
|
||||
"blurb": "只拉起夹爪驱动，不主动张合。",
|
||||
},
|
||||
"gripper_probe": {
|
||||
"title": "夹爪探测",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "probe"],
|
||||
"moves": False,
|
||||
"blurb": "读夹爪是否在线，不动手指。驱动没在跑时先拉起，不回零。",
|
||||
},
|
||||
"gripper_open": {
|
||||
"title": "夹爪张开",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "open"],
|
||||
"moves": True,
|
||||
"blurb": "张开当前臂夹爪。驱动没在跑时先拉起，不回零。",
|
||||
},
|
||||
"gripper_close": {
|
||||
"title": "夹爪闭合",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "close"],
|
||||
"moves": True,
|
||||
"blurb": "闭合当前臂夹爪。驱动没在跑时先拉起，不回零。",
|
||||
},
|
||||
"gripper_rotate": {
|
||||
"title": "右爪转 90°",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "rotate", "90"],
|
||||
"moves": True,
|
||||
"blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
|
||||
},
|
||||
"pipeline": {
|
||||
"title": "跟踪并对孔",
|
||||
"script": "run_all.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "跟踪和对孔一次跑完。搜孔轻压已取消，这一步不含旋入。会动臂。",
|
||||
},
|
||||
"stage_track": {
|
||||
"title": "只测跟踪",
|
||||
"script": "run_visual_servo_rt.sh",
|
||||
"args": ["--arm", "{arm}", "--control"],
|
||||
"moves": True,
|
||||
"blurb": "只跑视觉跟踪。从当前位姿开始，会动臂。要先有控制栈和相机。",
|
||||
},
|
||||
"stage_locate": {
|
||||
"title": "只测对孔",
|
||||
"script": "run_stage2_locate.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
|
||||
},
|
||||
"stage_screw": {
|
||||
"title": "只测旋转装配",
|
||||
"script": "run_stage3_search.sh",
|
||||
"args": ["--arm", "{arm}", "--screw"],
|
||||
"moves": True,
|
||||
"blurb": "按现场配置的拧入深度和螺距把螺纹规旋进去。会动臂。臂应已在孔口。",
|
||||
},
|
||||
"reset_home": {
|
||||
"title": "复位到 home",
|
||||
"script": "go_home.sh",
|
||||
"args": ["--arm", "{arm}", "--pose", "home"],
|
||||
"moves": True,
|
||||
"sync": True,
|
||||
"timeout": 120,
|
||||
"blurb": "先停跟踪，再回到命名位姿 home。要控制栈已经起来。会动臂。",
|
||||
},
|
||||
"gauge_dry": {
|
||||
"title": "装配干跑",
|
||||
"script": "run_gauge_assembly.sh",
|
||||
"args": ["--arm", "{arm}", "--dry-run"],
|
||||
"moves": False,
|
||||
"blurb": "S00 到 S11 只打印计划，不发运动指令。",
|
||||
},
|
||||
"gauge_run": {
|
||||
"title": "装配真跑",
|
||||
"script": "run_gauge_assembly.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "通止规状态机。会动臂、会拧。",
|
||||
},
|
||||
"handeye_show": {
|
||||
"title": "查看标定配置",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "show", "--mode", "{mode}"],
|
||||
"moves": False,
|
||||
"sync": True,
|
||||
"blurb": "打印这块板、结果文件和已有点数，不动臂。",
|
||||
},
|
||||
"handeye_teach": {
|
||||
"title": "示教采点",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"],
|
||||
"moves": True,
|
||||
"blurb": "软拖拽示教。点数和是否接着采由手眼页上的控件决定。要先停控制栈，会动臂。",
|
||||
},
|
||||
"handeye_merge": {
|
||||
"title": "合并示教点",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "merge", "--mode", "{mode}"],
|
||||
"moves": False,
|
||||
"sync": True,
|
||||
"blurb": "把刚采的点写入该模式的位姿文件，不动臂。",
|
||||
},
|
||||
"handeye_calibrate": {
|
||||
"title": "求解手眼",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "calibrate", "--mode", "{mode}"],
|
||||
"moves": True,
|
||||
"blurb": "按点位走一遍并写结果。需要控制栈和对应相机，会动臂。",
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def catalog() -> dict:
|
||||
return {
|
||||
"actions": [
|
||||
{"id": key, **{k: v for k, v in spec.items() if k not in ("script", "sync")}}
|
||||
for key, spec in _ACTIONS.items()
|
||||
],
|
||||
"stages": [
|
||||
("S00", "自检"),
|
||||
("S01", "回零预绕"),
|
||||
("S02", "夹持确认"),
|
||||
("S03", "手外粗定位"),
|
||||
("S04", "视觉跟踪"),
|
||||
("S05", "精拍"),
|
||||
("S06", "正悬停"),
|
||||
("S07", "柔顺接触"),
|
||||
("S08", "棘轮拧入"),
|
||||
("S09", "反向拧出"),
|
||||
("S10", "换孔"),
|
||||
("S11", "回零结束"),
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def _remember(action: str, proc: subprocess.Popen) -> None:
|
||||
with _PROC_LOCK:
|
||||
_LIVE[action] = proc
|
||||
_PROCS[action] = proc.pid
|
||||
|
||||
|
||||
def _forget(action: str) -> None:
|
||||
with _PROC_LOCK:
|
||||
_LIVE.pop(action, None)
|
||||
|
||||
|
||||
def _pid_alive(pid: int) -> bool:
|
||||
if pid <= 0:
|
||||
return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def stop_started() -> dict:
|
||||
"""停掉本界面拉起的命令。只向登记过的进程组发信号，不扫全机。"""
|
||||
with _PROC_LOCK:
|
||||
items = list(_LIVE.items())
|
||||
if not items:
|
||||
return {"ok": True, "message": "没有正在跑的命令"}
|
||||
names: list[str] = []
|
||||
for action, proc in items:
|
||||
pid = int(getattr(proc, "pid", 0) or 0)
|
||||
if not _pid_alive(pid):
|
||||
_forget(action)
|
||||
continue
|
||||
try:
|
||||
os.killpg(os.getpgid(pid), signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
_forget(action)
|
||||
continue
|
||||
except OSError as exc:
|
||||
return {"ok": False, "message": f"停不了 {action}：{exc}"}
|
||||
names.append(action)
|
||||
time.sleep(0.3)
|
||||
for action, proc in items:
|
||||
pid = int(getattr(proc, "pid", 0) or 0)
|
||||
if not _pid_alive(pid):
|
||||
_forget(action)
|
||||
continue
|
||||
try:
|
||||
os.killpg(os.getpgid(pid), signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
_forget(action)
|
||||
if not names:
|
||||
return {"ok": True, "message": "没有正在跑的命令"}
|
||||
return {"ok": True, "message": "已停止：" + "、".join(names)}
|
||||
|
||||
|
||||
def _stop_tactile() -> str:
|
||||
subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
|
||||
subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
|
||||
subprocess.run(["pkill", "orisys_tac_w"], check=False)
|
||||
return "已发送停止视触觉节点"
|
||||
|
||||
|
||||
def render_action_args(
|
||||
template: list[str], arm: str, mode: str, extras: dict | None = None,
|
||||
) -> list[str]:
|
||||
"""把动作模板里的臂号、模式、点数换成这次界面上的值。"""
|
||||
extra = extras or {}
|
||||
needs_points = any("{points}" in str(token) for token in template)
|
||||
points_text = ""
|
||||
if needs_points:
|
||||
try:
|
||||
points = int(extra.get("points", 12))
|
||||
except (TypeError, ValueError):
|
||||
raise ValueError("目标点数不是整数") from None
|
||||
if points < 4 or points > 30:
|
||||
raise ValueError("目标点数只接受 4–30")
|
||||
points_text = str(points)
|
||||
repl = {"{arm}": arm, "{mode}": mode, "{points}": points_text}
|
||||
out: list[str] = []
|
||||
for token in template:
|
||||
text = str(token)
|
||||
for key, value in repl.items():
|
||||
text = text.replace(key, value)
|
||||
out.append(text)
|
||||
if extra.get("append"):
|
||||
if "teach" not in out:
|
||||
raise ValueError("只有示教采点能接着采")
|
||||
out.append("--append")
|
||||
return out
|
||||
|
||||
|
||||
def run_action(
|
||||
action: str, arm: str, confirm: bool, mode: str = "eye_in_hand",
|
||||
extras: dict | None = None,
|
||||
) -> dict:
|
||||
if action == "tactile_down":
|
||||
tactile_view.stop()
|
||||
return {"ok": True, "message": _stop_tactile()}
|
||||
if action == "tactile_view_on":
|
||||
with _LOCK:
|
||||
_HIST["left"].clear()
|
||||
_HIST["right"].clear()
|
||||
return {"ok": True, "message": tactile_view.start(_site())}
|
||||
if action == "tactile_view_off":
|
||||
return {"ok": True, "message": tactile_view.stop()}
|
||||
spec = _ACTIONS.get(action)
|
||||
if spec is None:
|
||||
return {"ok": False, "message": "没有这个动作"}
|
||||
if arm not in {row["id"] for row in _arms()}:
|
||||
return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
|
||||
if mode not in ("eye_in_hand", "eye_to_hand"):
|
||||
return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
|
||||
if spec["moves"] and not confirm:
|
||||
return {"ok": False, "message": "这个动作会动真机，需要确认"}
|
||||
script = ROOT / "scripts" / spec["script"]
|
||||
if not script.is_file():
|
||||
return {"ok": False, "message": f"找不到 {script}"}
|
||||
from arm_application.operator_arm_state import (
|
||||
hold_until_idle,
|
||||
motion_blocked,
|
||||
release_for_control,
|
||||
resume_after_control,
|
||||
snapshot as arm_snapshot,
|
||||
stack_running,
|
||||
)
|
||||
blocked = motion_blocked(
|
||||
action,
|
||||
stack_up=stack_running(),
|
||||
op_status=str((arm_snapshot(arm) or {}).get("op_status") or ""),
|
||||
)
|
||||
if blocked:
|
||||
return {"ok": False, "message": blocked}
|
||||
uses_rdk = action in {
|
||||
"real_up", "handeye_teach", "pipeline", "stage_track", "stage_locate",
|
||||
"stage_screw", "reset_home", "gauge_run", "handeye_calibrate",
|
||||
}
|
||||
if uses_rdk:
|
||||
release_for_control()
|
||||
from arm_application.operator_gripper_state import hold_serial
|
||||
grip_port = action in {
|
||||
"gripper_probe", "gripper_open", "gripper_close", "gripper_rotate", "gripper_up",
|
||||
}
|
||||
try:
|
||||
args = render_action_args(list(spec["args"]), arm, mode, extras)
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
LOG_DIR.mkdir(parents=True, exist_ok=True)
|
||||
log_path = LOG_DIR / f"{action}_{arm}.log"
|
||||
shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
|
||||
try:
|
||||
if spec.get("sync"):
|
||||
proc = subprocess.Popen(
|
||||
["bash", str(script), *args],
|
||||
cwd=str(ROOT),
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
_remember(action, proc)
|
||||
try:
|
||||
detail, _ = proc.communicate(timeout=float(spec.get("timeout") or 40))
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
return {"ok": False, "message": "命令超时还没返回", "detail": shown}
|
||||
finally:
|
||||
_forget(action)
|
||||
detail = (detail or "").strip()
|
||||
log_path.write_text(detail)
|
||||
ok = proc.returncode == 0
|
||||
return {
|
||||
"ok": ok,
|
||||
"message": shown if ok else f"退出码 {proc.returncode}",
|
||||
"detail": detail[-4000:],
|
||||
"log": str(log_path),
|
||||
}
|
||||
log_f = log_path.open("ab")
|
||||
proc = subprocess.Popen(
|
||||
["bash", str(script), *args],
|
||||
cwd=str(ROOT),
|
||||
stdout=log_f,
|
||||
stderr=subprocess.STDOUT,
|
||||
start_new_session=True,
|
||||
)
|
||||
_remember(action, proc)
|
||||
if grip_port:
|
||||
hold_serial(proc.pid)
|
||||
if uses_rdk:
|
||||
hold_until_idle(proc.pid, until_stack=(action == "real_up"))
|
||||
uses_rdk = False
|
||||
return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
|
||||
finally:
|
||||
if uses_rdk:
|
||||
resume_after_control()
|
||||
|
||||
|
||||
def _bounded(name: str, raw, low: float, high: float) -> float:
|
||||
try:
|
||||
value = float(raw)
|
||||
except (TypeError, ValueError):
|
||||
raise ValueError(f"{name} 不是数字") from None
|
||||
if value != value or value < low or value > high:
|
||||
raise ValueError(f"{name} 只接受 {low:g}–{high:g}")
|
||||
return value
|
||||
|
||||
|
||||
def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
|
||||
"""按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
|
||||
if arm not in {row["id"] for row in _arms()}:
|
||||
return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
|
||||
if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
|
||||
return {"ok": False, "message": "没有这种夹爪调节"}
|
||||
if kind != "pos" and not any(row["id"] == arm and row.get("rotary") for row in _arms()):
|
||||
return {"ok": False, "message": "这台夹爪没有旋转轴"}
|
||||
try:
|
||||
if kind == "pos":
|
||||
pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
|
||||
force = int(round(_bounded("力度", numbers.get("force"), 20, 100)))
|
||||
speed = int(round(_bounded("速度", numbers.get("speed"), 1, 100)))
|
||||
cli = ["pos", str(pos)]
|
||||
summary = f"位置 {pos}（0 合，1000 开） 力度 {force}% 速度 {speed}%"
|
||||
else:
|
||||
force = int(round(_bounded("力矩", numbers.get("torque"), 20, 100)))
|
||||
speed = int(round(_bounded("旋转速度", numbers.get("rot_speed"), 1, 100)))
|
||||
if kind == "rotate":
|
||||
angle = _bounded("绝对角", numbers.get("angle"), -32767, 32767)
|
||||
cli = ["rotate", f"{angle:.3f}"]
|
||||
summary = f"绝对角 {angle:.3f}° 力矩 {force}% 速度 {speed}%"
|
||||
elif kind == "rotate_by":
|
||||
angle = _bounded("相对角", numbers.get("delta"), -32767, 32767)
|
||||
cli = ["rotate_by", f"{angle:.3f}"]
|
||||
summary = f"相对转 {angle:.3f}° 力矩 {force}% 速度 {speed}%"
|
||||
else:
|
||||
turns = _bounded("圈数", numbers.get("revolutions"), -50, 50)
|
||||
cli = ["revolutions", f"{turns:.3f}"]
|
||||
summary = f"转 {turns:.3f} 圈 力矩 {force}% 速度 {speed}%"
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
from arm_application.operator_gripper_state import pause_serial, resume_serial
|
||||
|
||||
script = ROOT / "scripts" / "run_gripper.sh"
|
||||
pause_serial()
|
||||
try:
|
||||
return _run_gripper_argv(script, arm, force, speed, cli, summary)
|
||||
finally:
|
||||
resume_serial()
|
||||
|
||||
|
||||
def _run_gripper_argv(script, arm: str, force: int, speed: int, cli: list[str], summary: str) -> dict:
|
||||
argv = [
|
||||
"bash", str(script),
|
||||
"--arm", arm,
|
||||
"--force", str(force),
|
||||
"--speed", str(speed),
|
||||
"--cli", *cli,
|
||||
]
|
||||
shown = " ".join(argv)
|
||||
proc = subprocess.Popen(
|
||||
argv, cwd=str(ROOT), stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
|
||||
text=True, start_new_session=True,
|
||||
)
|
||||
_remember(f"gripper_{arm}", proc)
|
||||
try:
|
||||
try:
|
||||
detail, _ = proc.communicate(timeout=90)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
|
||||
finally:
|
||||
_forget(f"gripper_{arm}")
|
||||
detail = (detail or "").strip()
|
||||
ok = proc.returncode == 0
|
||||
return {
|
||||
"ok": ok,
|
||||
"message": summary if ok else _grip_failure_message(proc.returncode, detail),
|
||||
"detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
|
||||
}
|
||||
|
||||
|
||||
def _grip_failure_message(code: int, detail: str) -> str:
|
||||
"""退出码旁边带上脚本打出来的原因，别只剩一个数字。"""
|
||||
for line in reversed(detail.splitlines()):
|
||||
text = line.strip()
|
||||
if text.startswith("ERROR:") or text.startswith("无法"):
|
||||
return text
|
||||
return f"夹爪退出码 {code}"
|
||||
|
||||
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
def log_message(self, fmt: str, *args) -> None:
|
||||
return
|
||||
|
||||
def _send(self, code: int, body: bytes, content_type: str) -> None:
|
||||
self.send_response(code)
|
||||
self.send_header("Content-Type", content_type)
|
||||
self.send_header("Content-Length", str(len(body)))
|
||||
self.end_headers()
|
||||
self.wfile.write(body)
|
||||
|
||||
def _json(self, code: int, payload: dict) -> None:
|
||||
self._send(code, json.dumps(payload, ensure_ascii=False).encode(), "application/json; charset=utf-8")
|
||||
|
||||
def do_GET(self) -> None:
|
||||
path = urlparse(self.path).path
|
||||
if path in ("/", "/index.html"):
|
||||
page = (ASSETS / "operator_console.html").read_text(encoding="utf-8")
|
||||
payload = json.dumps(
|
||||
{"status": status(), "catalog": catalog()},
|
||||
ensure_ascii=False,
|
||||
).replace("<", "\\u003c")
|
||||
page = page.replace("/*BOOT*/null", payload, 1)
|
||||
self._send(200, page.encode(), "text/html; charset=utf-8")
|
||||
return
|
||||
if path == "/logo.png":
|
||||
self._send(200, (ASSETS / "operator_console_logo.png").read_bytes(), "image/png")
|
||||
return
|
||||
if path == "/api/status":
|
||||
self._json(200, status())
|
||||
return
|
||||
if path == "/api/catalog":
|
||||
self._json(200, catalog())
|
||||
return
|
||||
parts = path.strip("/").split("/")
|
||||
if len(parts) == 5 and parts[:3] == ["api", "tactile", "frame"] and parts[4].endswith(".jpg"):
|
||||
side, kind = parts[3], parts[4][:-4]
|
||||
blob = tactile_view.frame(side, kind) if side in ("left", "right") and kind in ("shear", "thermal") else None
|
||||
if not blob:
|
||||
self._json(404, {"ok": False, "message": "还没有这一帧"})
|
||||
return
|
||||
self.send_response(200)
|
||||
self.send_header("Content-Type", "image/jpeg")
|
||||
self.send_header("Cache-Control", "no-store")
|
||||
self.send_header("Content-Length", str(len(blob)))
|
||||
self.end_headers()
|
||||
self.wfile.write(blob)
|
||||
return
|
||||
self._json(404, {"ok": False, "message": "没有这个路径"})
|
||||
|
||||
def do_POST(self) -> None:
|
||||
path = urlparse(self.path).path
|
||||
if path not in ("/api/action", "/api/tactile/view"):
|
||||
self._json(404, {"ok": False, "message": "没有这个路径"})
|
||||
return
|
||||
length = int(self.headers.get("Content-Length") or 0)
|
||||
try:
|
||||
body = json.loads(self.rfile.read(length) or b"{}")
|
||||
except json.JSONDecodeError:
|
||||
self._json(400, {"ok": False, "message": "请求不是 JSON"})
|
||||
return
|
||||
if path == "/api/tactile/view":
|
||||
action = "tactile_view_on" if body.get("on") else "tactile_view_off"
|
||||
result = run_action(action, _default_arm(), False)
|
||||
self._json(200 if result.get("ok") else 400, result)
|
||||
return
|
||||
result = run_action(
|
||||
str(body.get("action") or ""),
|
||||
str(body.get("arm") or _default_arm()),
|
||||
bool(body.get("confirm")),
|
||||
str(body.get("mode") or "eye_in_hand"),
|
||||
)
|
||||
self._json(200 if result.get("ok") else 400, result)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
import atexit
|
||||
atexit.register(tactile_view.stop)
|
||||
from arm_application.operator_arm_state import start as start_arm_state
|
||||
from arm_application.operator_gripper_state import start as start_gripper_state
|
||||
start_arm_state()
|
||||
start_gripper_state()
|
||||
threading.Thread(target=_poll_tactile, daemon=True).start()
|
||||
server = ThreadingHTTPServer((HOST, PORT), Handler)
|
||||
print(f"Orisys 工位界面 http://{HOST}:{PORT}", flush=True)
|
||||
server.serve_forever()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,138 @@
|
||||
"""大寰夹爪的一比一外形。
|
||||
|
||||
左臂按下载包 ``PGEA-3D-Model-and-2D-Drawing-4`` 里的
|
||||
``PGEA-100-26-W-F`` 尺寸图。图签就是这个型号，总长 138、宽 55、厚 29，
|
||||
安装端抱闸约 20，主体 78，手指伸出约 40，闭合缝 1.2。
|
||||
现场装的是 ``PGEA-100-20-W``，这包里没有 100-20 的图，开口仍用
|
||||
``site.yaml`` 的 20 mm，不用图上的 26。
|
||||
|
||||
同包另一张完整的带抱闸图是 ``PGEA-100-40-W-F``（手指更长，张开约 40.8）。
|
||||
右臂是 ``RGI-100-30``，这包里没有旋转爪的图，仍按手册外形。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
# 米。来源见模块说明。长指向指尖，宽是开合方向，厚是图上的 29。
|
||||
PGEA_SIZE = (0.138, 0.055, 0.029)
|
||||
PGEA_STROKE_MM = 20.0
|
||||
PGEA_CLOSED_GAP_M = 0.0012
|
||||
PGEA_FINGER_M = 0.040
|
||||
RGI_SIZE = (0.158, 0.0755, 0.047)
|
||||
RGI_STROKE_MM = 30.0
|
||||
|
||||
# 深色底上要能分开：银壳体、蓝抱闸、橙手指、绿导轨。
|
||||
# 手指以前是 0.10 的黑，乘上未连接时的 0.55 就沉进背景里。
|
||||
_BODY = (0.86, 0.88, 0.91)
|
||||
_BRAKE = (0.16, 0.42, 0.90)
|
||||
_FINGER = (0.95, 0.46, 0.10)
|
||||
_RAIL = (0.18, 0.78, 0.28)
|
||||
|
||||
|
||||
def triangles(
|
||||
rotary: bool,
|
||||
permille: float | None,
|
||||
rotation_deg: float | None,
|
||||
stroke_mm: float | None,
|
||||
) -> tuple[list[tuple], str]:
|
||||
"""返回 ``(三角面, 说明)``。三角面是 ``(a, b, c, 明暗占位, rgb)``，明暗由界面再乘。"""
|
||||
if rotary:
|
||||
length, width, thick = RGI_SIZE
|
||||
stroke = (float(stroke_mm) if stroke_mm else RGI_STROKE_MM) / 1000.0
|
||||
note = (
|
||||
f"RGI-100-30 外形 {length*1000:.0f}×{width*1000:.1f}×{thick*1000:.0f} mm"
|
||||
f" 行程 {stroke*1000:.0f} mm。这包图纸里没有旋转爪，外形仍按手册。"
|
||||
)
|
||||
body_end = 0.118
|
||||
brake = None
|
||||
else:
|
||||
length, width, thick = PGEA_SIZE
|
||||
stroke = (float(stroke_mm) if stroke_mm else PGEA_STROKE_MM) / 1000.0
|
||||
note = (
|
||||
f"PGEA-100-26-W-F 尺寸图 外形 {length*1000:.0f}×{width*1000:.0f}×{thick*1000:.0f} mm"
|
||||
f" 现场行程 {stroke*1000:.0f} mm。包里没有 100-20 的图，开口不用图上的 26。"
|
||||
)
|
||||
body_end = length - PGEA_FINGER_M
|
||||
brake = (0.0, 0.020)
|
||||
gap = stroke * (0.5 if permille is None else max(0.0, min(1.0, float(permille) / 1000.0)))
|
||||
if not rotary:
|
||||
gap += PGEA_CLOSED_GAP_M
|
||||
parts = _solid(length, width, thick, body_end, gap, brake, rotary)
|
||||
if rotary and rotation_deg:
|
||||
parts = _spin_fingers(parts, math.radians(float(rotation_deg)), body_end)
|
||||
out = []
|
||||
for a, b, c, rgb in parts:
|
||||
out.append((a, b, c, _face_shade(a, b, c), rgb))
|
||||
if permille is None:
|
||||
note = "没有读数。下面是半开，不是当前开口。 " + note
|
||||
return out, note
|
||||
|
||||
|
||||
def _solid(length, width, thick, body_end, gap, brake, rotary: bool):
|
||||
hx, hy = thick / 2.0, width / 2.0
|
||||
faces = []
|
||||
z0 = 0.0
|
||||
if brake is not None:
|
||||
z0 = brake[1]
|
||||
faces.extend(_box(0.0, brake[1], -hy, hy, -hx, hx, _BRAKE))
|
||||
faces.extend(_box(z0, body_end, -hy, hy, -hx, hx, _BODY))
|
||||
if rotary:
|
||||
finger_t = min(0.010, hy * 0.4)
|
||||
finger_h = thick * 0.72
|
||||
else:
|
||||
# 尺寸图端视图：指厚 6.5，指高 16，都落在 55×29 的外形里面。
|
||||
finger_t = 0.0065
|
||||
finger_h = 0.016
|
||||
rail = 0.003
|
||||
faces.extend(_box(z0, body_end, -hy * 0.62, -hy * 0.62 + rail, hx - rail, hx, _RAIL))
|
||||
faces.extend(_box(z0, body_end, hy * 0.62 - rail, hy * 0.62, hx - rail, hx, _RAIL))
|
||||
half = gap / 2.0
|
||||
faces.extend(_box(body_end, length, -(half + finger_t), -half, -finger_h / 2, finger_h / 2, _FINGER))
|
||||
faces.extend(_box(body_end, length, half, half + finger_t, -finger_h / 2, finger_h / 2, _FINGER))
|
||||
return faces
|
||||
|
||||
|
||||
def _spin_fingers(parts, angle: float, body_end: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
spun = []
|
||||
for a, b, c0, rgb in parts:
|
||||
if a[0] >= body_end - 1e-6 and b[0] >= body_end - 1e-6 and c0[0] >= body_end - 1e-6:
|
||||
a, b, c0 = (_rot_z(a, c, s), _rot_z(b, c, s), _rot_z(c0, c, s))
|
||||
spun.append((a, b, c0, rgb))
|
||||
return spun
|
||||
|
||||
|
||||
def _face_shade(a, b, c) -> float:
|
||||
"""顶面亮、底面暗。零件颜色已经分开，明暗再把面分出来。"""
|
||||
ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
|
||||
vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
|
||||
nx = uy * vz - uz * vy
|
||||
ny = uz * vx - ux * vz
|
||||
nz = ux * vy - uy * vx
|
||||
length = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
|
||||
dot = (nx * 0.15 + ny * -0.25 + nz * 0.95) / length
|
||||
return 0.42 + 0.58 * max(0.0, min(1.0, dot))
|
||||
|
||||
|
||||
def _rot_z(point, c, s):
|
||||
x, y, z = point
|
||||
return (x, y * c - z * s, y * s + z * c)
|
||||
|
||||
|
||||
def _box(x0, x1, y0, y1, z0, z1, rgb) -> list[tuple]:
|
||||
"""轴向盒子。x 指向指尖，y 是开合方向，z 是厚度。"""
|
||||
p = (
|
||||
(x0, y0, z0), (x1, y0, z0), (x1, y1, z0), (x0, y1, z0),
|
||||
(x0, y0, z1), (x1, y0, z1), (x1, y1, z1), (x0, y1, z1),
|
||||
)
|
||||
quads = (
|
||||
(0, 1, 2, 3), (4, 7, 6, 5),
|
||||
(0, 4, 5, 1), (3, 2, 6, 7),
|
||||
(0, 3, 7, 4), (1, 5, 6, 2),
|
||||
)
|
||||
out = []
|
||||
for a, b, c, d in quads:
|
||||
out.append((p[a], p[b], p[c], rgb))
|
||||
out.append((p[a], p[c], p[d], rgb))
|
||||
return out
|
||||
@@ -0,0 +1,436 @@
|
||||
"""工位界面用的夹爪状态：只读开口和转角，并交给界面画出来。
|
||||
|
||||
夹爪节点已经占着串口时，只订它发布的话题，不再自己打开串口。
|
||||
没有节点时才短连一次。连上之后只读寄存器，不写初始化，避免手指回零把工件松开。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application.site_io import listed_arms, ros2_root
|
||||
|
||||
_GRIP = {0: "运动中", 1: "已到位", 2: "已夹住", 3: "已掉落"}
|
||||
_ROT = {0: "转动中", 1: "已到位", 2: "受阻", 3: "曾经受阻"}
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_CACHE: dict[str, dict] = {}
|
||||
_STARTED = False
|
||||
_PORT_HOLD = 0
|
||||
_PORT_BUSY = 0
|
||||
_PORT_LOCK = threading.Lock()
|
||||
|
||||
|
||||
def snapshot(arm: str) -> dict:
|
||||
with _LOCK:
|
||||
row = _CACHE.get(arm)
|
||||
return dict(row) if row else _empty(arm)
|
||||
|
||||
|
||||
def _empty(arm: str) -> dict:
|
||||
row = next((item for item in _arms() if item["id"] == arm), None)
|
||||
return present(
|
||||
arm,
|
||||
rotary=bool(row and row["rotary"]),
|
||||
stroke_mm=float(row["stroke_mm"]) if row else 20.0,
|
||||
reading=None,
|
||||
error="还没读到夹爪",
|
||||
)
|
||||
|
||||
|
||||
def present(
|
||||
arm: str,
|
||||
*,
|
||||
rotary: bool,
|
||||
stroke_mm: float,
|
||||
reading: dict | None,
|
||||
error: str = "",
|
||||
) -> dict:
|
||||
"""把一次读数收成界面能画的字典。不打开串口。"""
|
||||
stroke = float(stroke_mm) if stroke_mm and stroke_mm > 0 else 20.0
|
||||
row = reading or {}
|
||||
permille = _unit(row.get("permille"), 0, 1000)
|
||||
connected = bool(row.get("connected")) and permille is not None
|
||||
opening = None if permille is None else permille / 1000.0 * stroke
|
||||
rotation = _finite(row.get("rotation_deg")) if rotary else 0.0
|
||||
grip_state = row.get("grip_state")
|
||||
rot_state = row.get("rotation_state")
|
||||
if not connected:
|
||||
summary = error or str(row.get("error_msg") or "夹爪没连上")
|
||||
else:
|
||||
bits = [_GRIP.get(int(grip_state), "状态未知") if _finite(grip_state) is not None else "状态未知"]
|
||||
bits.append(f"开口 {opening:.1f} mm" if opening is not None else "")
|
||||
if rotary and rotation is not None:
|
||||
bits.append(f"转角 {rotation:.1f}°")
|
||||
if _finite(rot_state) is not None:
|
||||
bits.append(_ROT.get(int(rot_state), ""))
|
||||
summary = " ".join(bit for bit in bits if bit)
|
||||
return {
|
||||
"arm": arm,
|
||||
"rotary": rotary,
|
||||
"connected": connected,
|
||||
"initialized": bool(row.get("initialized")) if connected else False,
|
||||
"permille": permille,
|
||||
"opening_mm": opening,
|
||||
"stroke_mm": stroke,
|
||||
"rotation_deg": rotation if rotary else None,
|
||||
"force": _finite(row.get("force")),
|
||||
"speed": _finite(row.get("speed")),
|
||||
"grip_state": int(grip_state) if _finite(grip_state) is not None else None,
|
||||
"grip_text": _GRIP.get(int(grip_state), "—") if _finite(grip_state) is not None else "—",
|
||||
"rotation_text": _ROT.get(int(rot_state), "—") if rotary and _finite(rot_state) is not None else "—",
|
||||
"moving": bool(row.get("moving")) if connected else False,
|
||||
"gripped": bool(row.get("gripped")) or (_finite(grip_state) == 2) if connected else False,
|
||||
"summary": summary,
|
||||
"error": "" if connected else (error or str(row.get("error_msg") or "")),
|
||||
"source": str(row.get("source") or ""),
|
||||
"pose_live": connected,
|
||||
"stamp": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def retain_reading(
|
||||
fresh: dict,
|
||||
previous: dict | None,
|
||||
*,
|
||||
now: float | None = None,
|
||||
hold_s: float = 15.0,
|
||||
) -> dict:
|
||||
"""这一拍没读到开口时，短时间沿用上一拍。
|
||||
|
||||
和机械臂同一件事：话题 echo 偶发超时会把刚读到的开口清成“没连上”，
|
||||
模型和顶栏就停在那里。超过 ``hold_s`` 仍没有新读数才放开。
|
||||
"""
|
||||
if fresh.get("connected") or not previous or not previous.get("connected"):
|
||||
return fresh
|
||||
moment = time.time() if now is None else now
|
||||
if moment - float(previous.get("stamp") or 0.0) > hold_s:
|
||||
return fresh
|
||||
kept = dict(fresh)
|
||||
for key in (
|
||||
"connected", "initialized", "permille", "opening_mm", "stroke_mm",
|
||||
"rotation_deg", "force", "speed", "grip_state", "grip_text",
|
||||
"rotation_text", "moving", "gripped", "pose_live", "rotary", "source",
|
||||
):
|
||||
if key in previous:
|
||||
kept[key] = previous[key]
|
||||
kept["summary"] = str(previous.get("summary") or fresh.get("summary") or "")
|
||||
note = "沿用上次读数。"
|
||||
if note not in kept["summary"]:
|
||||
kept["summary"] = f"{kept['summary']} {note}".strip()
|
||||
return kept
|
||||
|
||||
|
||||
def pause_serial() -> None:
|
||||
"""控制命令要自己开串口，或把驱动拉起来。先拦住状态线程，等它把口放开。
|
||||
|
||||
一次短读要过好几个寄存器，1.5 秒经常还没结束。提前返回会让新驱动
|
||||
去抢还被占着的口，然后以「需要 24 V」失败。
|
||||
"""
|
||||
global _PORT_HOLD
|
||||
with _PORT_LOCK:
|
||||
_PORT_HOLD += 1
|
||||
deadline = time.time() + 6.0
|
||||
while time.time() < deadline:
|
||||
with _PORT_LOCK:
|
||||
busy = _PORT_BUSY
|
||||
if not busy:
|
||||
return
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
def hold_serial(pid: int) -> None:
|
||||
"""后台夹爪脚本还活着时，状态线程不要去开串口。"""
|
||||
pause_serial()
|
||||
|
||||
def _watch() -> None:
|
||||
while pid > 0:
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
break
|
||||
time.sleep(0.2)
|
||||
resume_serial()
|
||||
|
||||
threading.Thread(target=_watch, daemon=True, name="gripper-port-hold").start()
|
||||
|
||||
|
||||
def resume_serial() -> None:
|
||||
global _PORT_HOLD
|
||||
with _PORT_LOCK:
|
||||
_PORT_HOLD = max(0, _PORT_HOLD - 1)
|
||||
|
||||
|
||||
def serial_paused() -> bool:
|
||||
with _PORT_LOCK:
|
||||
return _PORT_HOLD > 0
|
||||
|
||||
|
||||
def start() -> None:
|
||||
global _STARTED
|
||||
with _LOCK:
|
||||
if _STARTED:
|
||||
return
|
||||
_STARTED = True
|
||||
threading.Thread(target=_loop, daemon=True, name="gripper-state").start()
|
||||
|
||||
|
||||
def _loop() -> None:
|
||||
while True:
|
||||
try:
|
||||
arms = _arms()
|
||||
nodes = _nodes()
|
||||
for arm in arms:
|
||||
reading, error = _read_one(arm, nodes)
|
||||
fresh = present(
|
||||
arm["id"],
|
||||
rotary=arm["rotary"],
|
||||
stroke_mm=arm["stroke_mm"],
|
||||
reading=reading,
|
||||
error=error,
|
||||
)
|
||||
_store(retain_reading(fresh, snapshot(arm["id"])))
|
||||
except Exception as exc: # noqa: BLE001 —— 读夹爪失败不能拖死界面
|
||||
for row in _arms():
|
||||
_store(present(
|
||||
row["id"], rotary=bool(row.get("rotary")), stroke_mm=float(row.get("stroke_mm") or 20.0),
|
||||
reading=None, error=str(exc),
|
||||
))
|
||||
time.sleep(2.5)
|
||||
|
||||
|
||||
def _store(row: dict) -> None:
|
||||
with _LOCK:
|
||||
_CACHE[str(row.get("arm") or "")] = row
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
path = ros2_root() / "config" / "site.yaml"
|
||||
if not path.is_file():
|
||||
return []
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
shared = data.get("gripper") if isinstance(data.get("gripper"), dict) else {}
|
||||
baud = int(shared.get("baud") or 115200)
|
||||
slave = int(shared.get("slave_id") or 1)
|
||||
rts = str(shared.get("rs485_rts") or "")
|
||||
out = []
|
||||
for row in listed_arms(data):
|
||||
item = dict(row)
|
||||
item["baud"] = baud
|
||||
item["slave"] = slave
|
||||
item["rts"] = rts
|
||||
out.append(item)
|
||||
return out
|
||||
|
||||
|
||||
def _nodes() -> dict[str, bool]:
|
||||
text = _pgrep("dh_pgea_node|dual_gripper")
|
||||
return {
|
||||
"pgea": "dh_pgea_node" in text or "lib/arm_application/dh_pgea" in text,
|
||||
"dual": "dual_gripper" in text,
|
||||
}
|
||||
|
||||
|
||||
def _read_one(arm: dict, nodes: dict[str, bool]) -> tuple[dict | None, str]:
|
||||
if nodes.get("dual") and arm.get("side") in ("left", "right"):
|
||||
topic = f"/dual_gripper_driver/{arm['side']}/status"
|
||||
parsed = _parse_status_echo(_echo(topic))
|
||||
if parsed is not None:
|
||||
parsed["source"] = f"话题 {topic}"
|
||||
return parsed, ""
|
||||
return None, "夹爪节点在跑，但这侧话题没有数据"
|
||||
if not arm.get("rotary") and nodes.get("pgea"):
|
||||
parsed = _parse_states_echo(_echo("/gripper/states"))
|
||||
if parsed is not None:
|
||||
parsed["source"] = "话题 /gripper/states"
|
||||
return parsed, ""
|
||||
return None, "平行爪节点在跑，但 /gripper/states 没有数据"
|
||||
if not arm.get("port"):
|
||||
return None, "site.yaml 里这台夹爪没有串口"
|
||||
if serial_paused():
|
||||
return None, "控制命令正在用串口"
|
||||
return _read_serial(arm)
|
||||
|
||||
|
||||
def _read_serial(arm: dict) -> tuple[dict | None, str]:
|
||||
"""短连串口只读。auto_initialize 必须关掉，否则旋转爪一连上就会找零。"""
|
||||
global _PORT_BUSY
|
||||
if serial_paused():
|
||||
return None, "控制命令正在用串口"
|
||||
with _PORT_LOCK:
|
||||
_PORT_BUSY += 1
|
||||
try:
|
||||
return _read_serial_open(arm)
|
||||
finally:
|
||||
with _PORT_LOCK:
|
||||
_PORT_BUSY = max(0, _PORT_BUSY - 1)
|
||||
|
||||
|
||||
def _read_serial_open(arm: dict) -> tuple[dict | None, str]:
|
||||
try:
|
||||
if arm["rotary"]:
|
||||
from arm_application.dual_gripper import RotaryGripper, RotaryGripperConfig
|
||||
grip = RotaryGripper(RotaryGripperConfig(
|
||||
port=arm["port"],
|
||||
slave_id=int(arm["slave"]),
|
||||
baudrate=int(arm["baud"]),
|
||||
timeout=0.25,
|
||||
auto_initialize=False,
|
||||
))
|
||||
grip.connect()
|
||||
try:
|
||||
status = grip.get_status()
|
||||
finally:
|
||||
grip.disconnect()
|
||||
if not status.connected:
|
||||
return None, status.error_msg or "旋转爪没有应答"
|
||||
return {
|
||||
"connected": True,
|
||||
"initialized": bool(status.initialized),
|
||||
"permille": float(status.finger_position_per_mille),
|
||||
"rotation_deg": float(status.rotation_deg),
|
||||
"force": float(status.force),
|
||||
"speed": float(status.velocity),
|
||||
"grip_state": int(status.gripper_state),
|
||||
"rotation_state": int(status.rotation_state),
|
||||
"moving": bool(status.is_moving),
|
||||
"gripped": bool(status.is_gripped),
|
||||
"error_msg": status.error_msg or "",
|
||||
"source": f"串口 {arm['port']} 只读",
|
||||
}, ""
|
||||
from arm_application.dh_pgea import DhPgeaError, DhPgeaGripper
|
||||
grip = DhPgeaGripper(
|
||||
port=arm["port"],
|
||||
baudrate=int(arm["baud"]),
|
||||
slave_id=int(arm["slave"]),
|
||||
stroke_mm=float(arm["stroke_mm"]),
|
||||
rs485_rts=str(arm["rts"]),
|
||||
)
|
||||
grip.open()
|
||||
try:
|
||||
state = grip.get_run_states(retries=1)
|
||||
finally:
|
||||
grip.close()
|
||||
return {
|
||||
"connected": True,
|
||||
"initialized": bool(state.initialized),
|
||||
"permille": float(state.position),
|
||||
"rotation_deg": 0.0,
|
||||
"force": float(state.target_force),
|
||||
"speed": None,
|
||||
"grip_state": int(state.grip_state),
|
||||
"rotation_state": None,
|
||||
"moving": int(state.grip_state) == 0,
|
||||
"gripped": int(state.grip_state) == 2,
|
||||
"error_msg": "",
|
||||
"source": f"串口 {arm['port']} 只读",
|
||||
}, ""
|
||||
except Exception as exc: # noqa: BLE001 —— 串口被节点占着、没上电，都要变成一句人话
|
||||
text = str(exc).strip() or exc.__class__.__name__
|
||||
if "DhPgeaError" in exc.__class__.__name__ or "Permission" in text or "busy" in text.lower():
|
||||
return None, text
|
||||
return None, text
|
||||
|
||||
|
||||
def _echo(topic: str) -> str:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["timeout", "3", "ros2", "topic", "echo", topic, "--once"],
|
||||
capture_output=True, text=True, timeout=5,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return ""
|
||||
return proc.stdout
|
||||
|
||||
|
||||
def _parse_status_echo(text: str) -> dict | None:
|
||||
for chunk in text.split("---"):
|
||||
if "finger_position_per_mille:" not in chunk and "connected:" not in chunk:
|
||||
continue
|
||||
try:
|
||||
data = yaml.safe_load(chunk)
|
||||
except yaml.YAMLError:
|
||||
continue
|
||||
if not isinstance(data, dict) or "connected" not in data:
|
||||
continue
|
||||
return {
|
||||
"connected": bool(data.get("connected")),
|
||||
"initialized": bool(data.get("initialized")),
|
||||
"permille": data.get("finger_position_per_mille"),
|
||||
"rotation_deg": data.get("rotation_deg"),
|
||||
"force": data.get("force"),
|
||||
"speed": data.get("velocity"),
|
||||
"grip_state": data.get("gripper_state"),
|
||||
"rotation_state": data.get("rotation_state"),
|
||||
"moving": bool(data.get("is_moving")),
|
||||
"gripped": bool(data.get("is_gripped")),
|
||||
"error_msg": str(data.get("error_msg") or ""),
|
||||
}
|
||||
return None
|
||||
|
||||
|
||||
def _parse_states_echo(text: str) -> dict | None:
|
||||
for chunk in text.split("---"):
|
||||
if "data:" not in chunk:
|
||||
continue
|
||||
try:
|
||||
data = yaml.safe_load(chunk)
|
||||
except yaml.YAMLError:
|
||||
continue
|
||||
if not isinstance(data, dict):
|
||||
continue
|
||||
raw = data.get("data")
|
||||
if not isinstance(raw, list) or len(raw) < 5:
|
||||
continue
|
||||
try:
|
||||
init, grip, pos, _target, force = (float(v) for v in raw[:5])
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
return {
|
||||
"connected": True,
|
||||
"initialized": init >= 0.5,
|
||||
"permille": pos,
|
||||
"rotation_deg": 0.0,
|
||||
"force": force,
|
||||
"speed": None,
|
||||
"grip_state": int(grip),
|
||||
"rotation_state": None,
|
||||
"moving": int(grip) == 0,
|
||||
"gripped": int(grip) == 2,
|
||||
"error_msg": "",
|
||||
}
|
||||
return None
|
||||
|
||||
|
||||
def _pgrep(pattern: str) -> str:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["pgrep", "-af", pattern],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return ""
|
||||
return "\n".join(line for line in proc.stdout.splitlines() if "pgrep" not in line)
|
||||
|
||||
|
||||
def _finite(raw: Any) -> float | None:
|
||||
try:
|
||||
value = float(raw)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if value != value:
|
||||
return None
|
||||
return value
|
||||
|
||||
|
||||
def _unit(raw: Any, low: float, high: float) -> float | None:
|
||||
value = _finite(raw)
|
||||
if value is None:
|
||||
return None
|
||||
return max(low, min(high, value))
|
||||
@@ -0,0 +1,91 @@
|
||||
"""手眼页的棋盘预览。只在已有彩色画面上画角点，不打开相机，也不动臂。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.handeye_lib import BoardConfig, detect_chessboard
|
||||
|
||||
|
||||
def coverage_fraction(corners: np.ndarray, width: int, height: int) -> float:
|
||||
"""角点外接框占画面的比例。宽或高为 0 时返回 0。"""
|
||||
if width <= 0 or height <= 0:
|
||||
return 0.0
|
||||
pts = np.asarray(corners, dtype=float).reshape(-1, 2)
|
||||
if pts.size == 0:
|
||||
return 0.0
|
||||
area = float(pts[:, 0].max() - pts[:, 0].min()) * float(pts[:, 1].max() - pts[:, 1].min())
|
||||
return area / float(width * height)
|
||||
|
||||
|
||||
def coverage_words(fraction: float) -> str:
|
||||
"""30%–70% 是示教时棋盘该占的画面。再小对不准，再大姿态拉不开。"""
|
||||
if fraction < 0.30:
|
||||
return "偏小，再靠近一点"
|
||||
if fraction > 0.70:
|
||||
return "偏大，再离远一点"
|
||||
return "大小合适"
|
||||
|
||||
|
||||
def annotate_board(jpeg: bytes, cols: int, rows: int) -> tuple[bytes, str]:
|
||||
"""返回（可能画了角点的 JPEG，一行中文状态）。检测失败时 JPEG 原样返回。"""
|
||||
import cv2
|
||||
|
||||
if cols <= 0 or rows <= 0:
|
||||
return jpeg, "配置里没有这块板的内角点"
|
||||
buf = np.frombuffer(jpeg, dtype=np.uint8)
|
||||
image = cv2.imdecode(buf, cv2.IMREAD_COLOR)
|
||||
if image is None:
|
||||
return jpeg, "这一帧解不出来"
|
||||
found = detect_chessboard(image, BoardConfig(int(cols), int(rows), 1.0))
|
||||
if found is None:
|
||||
return jpeg, "画面里没有这块板"
|
||||
corners, pattern = found
|
||||
drawn = image.copy()
|
||||
cv2.drawChessboardCorners(drawn, pattern, corners, True)
|
||||
fraction = coverage_fraction(corners, drawn.shape[1], drawn.shape[0])
|
||||
ok, out = cv2.imencode(".jpg", drawn, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
blob = out.tobytes() if ok else jpeg
|
||||
percent = int(round(fraction * 100))
|
||||
return blob, f"棋盘已找到，约占画面 {percent}%。{coverage_words(fraction)}"
|
||||
|
||||
|
||||
class BoardWatch:
|
||||
"""后台画角点。界面线程只提交最新一帧，检测再慢也不卡住窗口。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._lock = threading.Lock()
|
||||
self._pending: tuple | None = None
|
||||
self._result = {"token": 0, "jpeg": b"", "text": ""}
|
||||
self._stop = threading.Event()
|
||||
self._thread = threading.Thread(target=self._loop, daemon=True, name="handeye-board")
|
||||
self._thread.start()
|
||||
|
||||
def submit(self, token: int, jpeg: bytes, cols: int, rows: int) -> None:
|
||||
with self._lock:
|
||||
if self._result["token"] == token and not self._pending:
|
||||
return
|
||||
self._pending = (token, jpeg, int(cols), int(rows))
|
||||
|
||||
def latest(self) -> tuple[bytes, str, int]:
|
||||
with self._lock:
|
||||
return self._result["jpeg"], self._result["text"], int(self._result["token"])
|
||||
|
||||
def _loop(self) -> None:
|
||||
while not self._stop.is_set():
|
||||
with self._lock:
|
||||
job = self._pending
|
||||
self._pending = None
|
||||
if job is None:
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
token, jpeg, cols, rows = job
|
||||
try:
|
||||
blob, text = annotate_board(jpeg, cols, rows)
|
||||
except Exception as exc: # noqa: BLE001 —— 检测失败不能拖死预览
|
||||
blob, text = jpeg, f"棋盘检测失败：{exc}"
|
||||
with self._lock:
|
||||
self._result = {"token": token, "jpeg": blob, "text": text}
|
||||
@@ -0,0 +1,353 @@
|
||||
"""工位界面用的视触觉成像。
|
||||
|
||||
切向力是 SDK 的 ``VSHEAR`` 箭头，热成像是 ``DEPTH`` 的 JET 伪彩。
|
||||
相机独占，所以只有页面点了「开始成像」才打开，失败把原因交回去，不造图。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import re
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_STOP = threading.Event()
|
||||
_THREADS: list[threading.Thread] = []
|
||||
_ON_SAMPLE = None
|
||||
_STATE = {
|
||||
"on": False,
|
||||
"error": "",
|
||||
"stamp": 0.0,
|
||||
"note": "",
|
||||
"frames": {},
|
||||
}
|
||||
_JPEG: dict[str, bytes] = {}
|
||||
|
||||
|
||||
def set_sample_hook(fn) -> None:
|
||||
global _ON_SAMPLE
|
||||
_ON_SAMPLE = fn
|
||||
|
||||
|
||||
def state() -> dict:
|
||||
with _LOCK:
|
||||
return {
|
||||
"on": _STATE["on"],
|
||||
"error": _STATE["error"],
|
||||
"stamp": _STATE["stamp"],
|
||||
"note": _STATE["note"],
|
||||
"frames": dict(_STATE["frames"]),
|
||||
}
|
||||
|
||||
|
||||
def frame(side: str, kind: str) -> bytes | None:
|
||||
with _LOCK:
|
||||
return _JPEG.get(f"{side}_{kind}")
|
||||
|
||||
|
||||
def _videos() -> list[str]:
|
||||
found = []
|
||||
by_path = Path("/dev/v4l/by-path")
|
||||
if not by_path.is_dir():
|
||||
return found
|
||||
for link in sorted(by_path.iterdir()):
|
||||
if not link.name.endswith("-video-index0"):
|
||||
continue
|
||||
real = os.path.realpath(link)
|
||||
base = os.path.basename(real)
|
||||
sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device")
|
||||
dev = ""
|
||||
for part in reversed(sysdev.split("/")):
|
||||
if re.fullmatch(r"\d+-\d+(\.\d+)*", part):
|
||||
dev = part
|
||||
break
|
||||
if not dev:
|
||||
continue
|
||||
vid = Path(f"/sys/bus/usb/devices/{dev}/idVendor")
|
||||
pid = Path(f"/sys/bus/usb/devices/{dev}/idProduct")
|
||||
if (
|
||||
vid.is_file()
|
||||
and pid.is_file()
|
||||
and vid.read_text().strip() == "1bcf"
|
||||
and pid.read_text().strip() == "2281"
|
||||
):
|
||||
found.append(str(link))
|
||||
return found
|
||||
|
||||
|
||||
def _resolve_cfg(site: dict) -> dict:
|
||||
tactile = site.get("tactile") if isinstance(site.get("tactile"), dict) else {}
|
||||
left = str(tactile.get("left_video") or "")
|
||||
right = str(tactile.get("right_video") or "")
|
||||
if not Path(left).exists() or not Path(right).exists():
|
||||
found = _videos()
|
||||
if len(found) >= 2:
|
||||
left, right = found[0], found[1]
|
||||
elif len(found) == 1:
|
||||
left = right = found[0]
|
||||
return {
|
||||
"cfg": str(tactile.get("config_file") or ""),
|
||||
"backend": str(tactile.get("infer_backend") or "cpu"),
|
||||
"cuda": bool(tactile.get("cuda", False)),
|
||||
"isstitch": bool(tactile.get("isstitch", True)),
|
||||
"baseline": float(tactile.get("baseline_seconds") or 1.5),
|
||||
"left": left,
|
||||
"right": right,
|
||||
}
|
||||
|
||||
|
||||
def _as_bgr(img):
|
||||
if not isinstance(img, np.ndarray) or img.size == 0:
|
||||
return None
|
||||
import cv2
|
||||
|
||||
if img.ndim == 2:
|
||||
return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
if img.ndim == 3 and img.shape[2] >= 3:
|
||||
return np.ascontiguousarray(img[:, :, :3])
|
||||
return None
|
||||
|
||||
|
||||
def _flat(arr) -> bool:
|
||||
if not isinstance(arr, np.ndarray) or arr.size == 0 or arr.ndim < 2:
|
||||
return True
|
||||
a = arr.reshape(-1, arr.shape[-1]) if arr.ndim == 3 else arr.reshape(-1)
|
||||
finite = np.isfinite(a)
|
||||
if not bool(finite.any()):
|
||||
return True
|
||||
vals = a[finite]
|
||||
return float(np.max(vals) - np.min(vals)) < 1e-6
|
||||
|
||||
|
||||
def _thermal(depth):
|
||||
import cv2
|
||||
|
||||
if depth is None:
|
||||
return None
|
||||
a = np.asarray(depth)
|
||||
if a.size == 0:
|
||||
return None
|
||||
if a.ndim == 3 and a.shape[2] == 2:
|
||||
a = np.linalg.norm(a.astype(np.float32), axis=2)
|
||||
if a.ndim != 2:
|
||||
return None
|
||||
finite = np.isfinite(a)
|
||||
if not finite.any():
|
||||
return None
|
||||
vals = a[finite]
|
||||
lo, hi = float(vals.min()), float(vals.max())
|
||||
if hi <= lo:
|
||||
norm = np.zeros(a.shape, np.uint8)
|
||||
else:
|
||||
norm = np.clip((a - lo) / (hi - lo) * 255.0, 0, 255).astype(np.uint8)
|
||||
return cv2.applyColorMap(norm, cv2.COLORMAP_JET)
|
||||
|
||||
|
||||
def _views(sensor, gray):
|
||||
import cv2
|
||||
import orisys
|
||||
|
||||
bgr = _as_bgr(gray)
|
||||
h, w = (bgr.shape[0], bgr.shape[1]) if bgr is not None else (450, 420)
|
||||
try:
|
||||
sensor.compute_deformation(decompose=True, check_motion=False)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
sensor.compute_contact()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
vshear, vnormal, depth = sensor.read_info(
|
||||
sensor.info.VSHEAR, sensor.info.VNORMAL, sensor.info.DEPTH
|
||||
)
|
||||
except Exception:
|
||||
vshear, vnormal, depth = None, None, None
|
||||
canvas = np.zeros((h, w, 3), np.uint8)
|
||||
shear = canvas
|
||||
if isinstance(vshear, np.ndarray) and vshear.ndim == 3 and vshear.shape[2] >= 2 and vshear.size:
|
||||
field = np.ascontiguousarray(vshear[:, :, :2], dtype=np.float32)
|
||||
if field.shape[0] != h or field.shape[1] != w:
|
||||
field = cv2.resize(field, (w, h))
|
||||
mag = np.linalg.norm(field, axis=2)
|
||||
peak = float(np.percentile(mag, 99)) if mag.size else 0.0
|
||||
arrow_scale = (2.0 * peak / 12.0) if peak > 1e-6 else 1.0
|
||||
try:
|
||||
shear = orisys.util.draw_arrows(
|
||||
canvas,
|
||||
field,
|
||||
threshold=peak * 0.15 if peak > 1e-6 else 1.0,
|
||||
grid_spacing=16,
|
||||
arrow_scale=max(arrow_scale, 1e-6),
|
||||
below_threshold_color=(40, 40, 40),
|
||||
)
|
||||
except Exception:
|
||||
shear = canvas
|
||||
heat = None if _flat(depth) else _thermal(depth)
|
||||
if heat is None:
|
||||
heat = _thermal(vnormal)
|
||||
if not isinstance(heat, np.ndarray) or heat.ndim != 3:
|
||||
heat = np.zeros((h, w, 3), np.uint8)
|
||||
elif heat.shape[0] != h or heat.shape[1] != w:
|
||||
heat = cv2.resize(heat, (w, h))
|
||||
return shear, heat
|
||||
|
||||
|
||||
def _jpeg(img) -> bytes | None:
|
||||
import cv2
|
||||
|
||||
if not isinstance(img, np.ndarray) or img.size == 0:
|
||||
return None
|
||||
ok, buf = cv2.imencode(".jpg", img, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
if not ok:
|
||||
return None
|
||||
return buf.tobytes()
|
||||
|
||||
|
||||
def _store(side: str, shear, heat, sample: dict | None) -> None:
|
||||
frames = {}
|
||||
blobs = {}
|
||||
for kind, img in (("shear", shear), ("thermal", heat)):
|
||||
blob = _jpeg(img)
|
||||
if blob:
|
||||
blobs[f"{side}_{kind}"] = blob
|
||||
frames[f"{side}_{kind}"] = True
|
||||
with _LOCK:
|
||||
_JPEG.update(blobs)
|
||||
_STATE["frames"].update(frames)
|
||||
_STATE["stamp"] = time.time()
|
||||
if _STATE["error"].startswith(side):
|
||||
_STATE["error"] = ""
|
||||
if sample and _ON_SAMPLE is not None:
|
||||
_ON_SAMPLE(side, sample)
|
||||
|
||||
|
||||
def _open_sensor(cfg: dict, side: str):
|
||||
import orisys
|
||||
|
||||
return orisys.Sensor(
|
||||
cfg[side],
|
||||
cuda=cfg["cuda"],
|
||||
isstitch=cfg["isstitch"],
|
||||
verbose=False,
|
||||
config_name=cfg["cfg"],
|
||||
backend=cfg["backend"],
|
||||
)
|
||||
|
||||
|
||||
def _worker(side: str, cfg: dict) -> None:
|
||||
if not cfg.get(side):
|
||||
with _LOCK:
|
||||
_STATE["error"] = f"{side} 没有视频节点"
|
||||
return
|
||||
sensor = None
|
||||
try:
|
||||
sensor = _open_sensor(cfg, side)
|
||||
except Exception as exc:
|
||||
with _LOCK:
|
||||
prev = _STATE["error"]
|
||||
_STATE["error"] = (prev + "；" if prev else "") + f"{side} 打开失败：{exc}"
|
||||
return
|
||||
buckets = {"n": [], "x": [], "y": []}
|
||||
deadline = time.perf_counter() + float(cfg["baseline"])
|
||||
while time.perf_counter() < deadline and not _STOP.is_set():
|
||||
try:
|
||||
img = sensor.get_img()
|
||||
sensor.compute_deformation(check_motion=False)
|
||||
n, x, y = sensor.read_info(
|
||||
sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY
|
||||
)
|
||||
buckets["n"].append(float(n))
|
||||
buckets["x"].append(float(x))
|
||||
buckets["y"].append(float(y))
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.02)
|
||||
base = {
|
||||
"n": float(np.mean(buckets["n"])) if buckets["n"] else 0.0,
|
||||
"x": float(np.mean(buckets["x"])) if buckets["x"] else 0.0,
|
||||
"y": float(np.mean(buckets["y"])) if buckets["y"] else 0.0,
|
||||
}
|
||||
with _LOCK:
|
||||
base_note = "曲线已减去打开后短暂基线，单位仍是形变读数。"
|
||||
if "右指" in str(_STATE["note"]):
|
||||
_STATE["note"] = _STATE["note"] + " " + base_note
|
||||
else:
|
||||
_STATE["note"] = base_note
|
||||
last_sample = 0.0
|
||||
while not _STOP.is_set():
|
||||
try:
|
||||
gray = sensor.get_img()
|
||||
except Exception:
|
||||
gray = None
|
||||
if gray is None:
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
shear, heat = _views(sensor, gray)
|
||||
sample = None
|
||||
now = time.time()
|
||||
if now - last_sample >= 0.1:
|
||||
try:
|
||||
n, x, y = sensor.read_info(
|
||||
sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY
|
||||
)
|
||||
sample = {
|
||||
"t": now,
|
||||
"fnormal": float(n) - base["n"],
|
||||
"fshearx": float(x) - base["x"],
|
||||
"fsheary": float(y) - base["y"],
|
||||
}
|
||||
last_sample = now
|
||||
except Exception:
|
||||
sample = None
|
||||
_store(side, shear, heat, sample)
|
||||
time.sleep(0.04)
|
||||
try:
|
||||
sensor.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def start(site: dict) -> str:
|
||||
global _THREADS
|
||||
with _LOCK:
|
||||
if _STATE["on"]:
|
||||
return "成像已经开着"
|
||||
_STATE["on"] = True
|
||||
_STATE["error"] = ""
|
||||
_STATE["frames"] = {}
|
||||
_STOP.clear()
|
||||
cfg = _resolve_cfg(site)
|
||||
if not cfg["left"] and not cfg["right"]:
|
||||
with _LOCK:
|
||||
_STATE["on"] = False
|
||||
_STATE["error"] = "没有找到 Orisys 相机"
|
||||
return _STATE["error"]
|
||||
sides = ["left"]
|
||||
if cfg.get("right") and cfg.get("right") != cfg.get("left"):
|
||||
sides.append("right")
|
||||
elif not cfg.get("right") or cfg.get("right") == cfg.get("left"):
|
||||
with _LOCK:
|
||||
_STATE["note"] = "只找到一路 Orisys，右指没有画面。插上后重新开始成像。"
|
||||
_THREADS = [
|
||||
threading.Thread(target=_worker, args=(side, cfg), daemon=True, name=f"tac-{side}")
|
||||
for side in sides
|
||||
]
|
||||
for thread in _THREADS:
|
||||
thread.start()
|
||||
return "正在打开两只视触觉。切向力和热成像出来之前会先采一小段基线。"
|
||||
|
||||
|
||||
def stop() -> str:
|
||||
_STOP.set()
|
||||
for thread in list(_THREADS):
|
||||
thread.join(timeout=2.0)
|
||||
with _LOCK:
|
||||
_STATE["on"] = False
|
||||
_STATE["frames"] = {}
|
||||
_JPEG.clear()
|
||||
return "已停止视触觉成像"
|
||||
@@ -0,0 +1,297 @@
|
||||
"""软拖拽示教：开拖拽、记下当前关节角、再回到该点。
|
||||
|
||||
点位写在 ``handeye/tmp/<臂号>/operator_teach.yaml``。关节角单位是弧度，
|
||||
直接存桥接返回的 7 个数，不另做姿态换算。
|
||||
|
||||
软拖拽占用 RDK，和 ``run_real.sh`` 不能同时连同一台臂。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import subprocess
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_BRIDGE = None
|
||||
_SN = ""
|
||||
_YIELDED = False
|
||||
_ARM_CONTROLLER = "rizon_arm_controller"
|
||||
|
||||
|
||||
def _file(arm: str) -> Path:
|
||||
from arm_application.operator_console import ROOT
|
||||
from arm_application.site_io import listed_arms, load_site_dict
|
||||
|
||||
known = {row["id"] for row in listed_arms(load_site_dict())}
|
||||
if arm not in known:
|
||||
raise ValueError(f"臂号 {arm} 不在 site.yaml 里")
|
||||
return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
|
||||
|
||||
|
||||
def _bridge_for(sn: str):
|
||||
global _BRIDGE, _SN
|
||||
sn = str(sn or "").strip()
|
||||
if not sn:
|
||||
raise RuntimeError("site.yaml 里这台臂没有序列号，拒绝连接")
|
||||
if _BRIDGE is not None and _SN == sn and _BRIDGE._proc.poll() is None:
|
||||
return _BRIDGE
|
||||
close()
|
||||
from arm_application.handeye_teach import RdkBridge
|
||||
from arm_application.operator_arm_state import release_for_control, resume_after_control
|
||||
|
||||
release_for_control()
|
||||
try:
|
||||
_BRIDGE = RdkBridge(sn)
|
||||
_SN = sn
|
||||
finally:
|
||||
resume_after_control()
|
||||
return _BRIDGE
|
||||
|
||||
|
||||
def close() -> None:
|
||||
global _BRIDGE, _SN
|
||||
bridge = _BRIDGE
|
||||
_BRIDGE = None
|
||||
_SN = ""
|
||||
if bridge is None:
|
||||
return
|
||||
try:
|
||||
bridge.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def _switch_arm_controller(activate: bool) -> str:
|
||||
"""停或恢复关节轨迹控制器。硬件还在，只是不再下发关节位置。
|
||||
|
||||
控制栈在 ``NRT_JOINT_POSITION`` 里持续 ``SendJointPosition``，
|
||||
模式一旦被改走，写循环两秒内又切回去。软拖拽要低阻抗，必须先停这条控制器。
|
||||
"""
|
||||
if activate:
|
||||
payload = (
|
||||
"{activate_controllers: ['%s'], deactivate_controllers: [], "
|
||||
"strictness: 2, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
|
||||
% _ARM_CONTROLLER
|
||||
)
|
||||
else:
|
||||
payload = (
|
||||
"{activate_controllers: [], deactivate_controllers: ['%s'], "
|
||||
"strictness: 1, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
|
||||
% _ARM_CONTROLLER
|
||||
)
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
[
|
||||
"ros2", "service", "call",
|
||||
"/controller_manager/switch_controller",
|
||||
"controller_manager_msgs/srv/SwitchController",
|
||||
payload,
|
||||
],
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=15,
|
||||
)
|
||||
except FileNotFoundError:
|
||||
return "找不到 ros2，停不了关节轨迹控制器"
|
||||
except subprocess.TimeoutExpired:
|
||||
return "关节轨迹控制器没回来" if activate else "停关节轨迹控制器超时"
|
||||
text = ((proc.stdout or "") + (proc.stderr or "")).strip()
|
||||
if proc.returncode == 0 and re.search(r"ok\s*[:=]\s*true", text, re.IGNORECASE):
|
||||
return ""
|
||||
tail = text.splitlines()[-1] if text else f"退出码 {proc.returncode}"
|
||||
verb = "恢复" if activate else "停下"
|
||||
return f"没能{verb}关节轨迹控制器：{tail}"
|
||||
|
||||
|
||||
def _yield_position_stream() -> str:
|
||||
"""控制栈没起来就什么都不做。起来了就先把位置控制器让开。"""
|
||||
global _YIELDED
|
||||
from arm_application.operator_arm_state import stack_running
|
||||
|
||||
if not stack_running():
|
||||
return ""
|
||||
err = _switch_arm_controller(False)
|
||||
if err:
|
||||
return err + "。位置环还在推，软拖拽切不过去。"
|
||||
_YIELDED = True
|
||||
return ""
|
||||
|
||||
|
||||
def _restore_position_stream() -> str:
|
||||
global _YIELDED
|
||||
if not _YIELDED:
|
||||
return ""
|
||||
err = _switch_arm_controller(True)
|
||||
_YIELDED = False
|
||||
return err
|
||||
|
||||
|
||||
def drag_on(sn: str) -> dict:
|
||||
with _LOCK:
|
||||
try:
|
||||
yielded = _yield_position_stream()
|
||||
if yielded:
|
||||
return {"ok": False, "message": yielded}
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge.drag_on(mode="soft")
|
||||
except Exception as exc:
|
||||
restore = _restore_position_stream()
|
||||
message = str(exc)
|
||||
if restore:
|
||||
message = f"{message}；{restore}"
|
||||
return {"ok": False, "message": message}
|
||||
if not result.get("ok"):
|
||||
restore = _restore_position_stream()
|
||||
message = str(result.get("error") or result)
|
||||
if restore:
|
||||
message = f"{message}；{restore}"
|
||||
return {"ok": False, "message": message}
|
||||
note = "软拖拽已开启。轻推机械臂，再记点。"
|
||||
if _YIELDED:
|
||||
note += "关节轨迹控制器已停下，关掉拖拽后会回来。"
|
||||
return {"ok": True, "message": note}
|
||||
|
||||
|
||||
def drag_off(sn: str) -> dict:
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge.drag_off()
|
||||
except Exception as exc:
|
||||
restore = _restore_position_stream()
|
||||
message = str(exc)
|
||||
if restore:
|
||||
message = f"{message}；{restore}"
|
||||
return {"ok": False, "message": message}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or result)}
|
||||
restore = _restore_position_stream()
|
||||
if restore:
|
||||
return {"ok": False, "message": f"软拖拽已关闭，但{restore}"}
|
||||
return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"}
|
||||
|
||||
|
||||
def list_points(arm: str) -> dict:
|
||||
try:
|
||||
path = _file(arm)
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc), "points": []}
|
||||
if not path.is_file():
|
||||
return {"ok": True, "points": [], "file": str(path)}
|
||||
try:
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": f"示教文件读不了：{exc}", "points": [], "file": str(path)}
|
||||
points = []
|
||||
for row in data.get("points") or []:
|
||||
if not isinstance(row, dict):
|
||||
continue
|
||||
joints = row.get("joints")
|
||||
if not isinstance(joints, list) or len(joints) < 7:
|
||||
continue
|
||||
try:
|
||||
values = [float(v) for v in joints[:7]]
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
if any(v != v for v in values):
|
||||
continue
|
||||
points.append({
|
||||
"name": str(row.get("name") or "").strip(),
|
||||
"utc": str(row.get("utc") or ""),
|
||||
"joints": values,
|
||||
})
|
||||
points = [row for row in points if row["name"]]
|
||||
return {"ok": True, "points": points, "file": str(path)}
|
||||
|
||||
|
||||
def _write(arm: str, points: list[dict]) -> Path:
|
||||
path = _file(arm)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text(
|
||||
yaml.safe_dump({"points": points}, allow_unicode=True, sort_keys=False),
|
||||
encoding="utf-8",
|
||||
)
|
||||
return path
|
||||
|
||||
|
||||
def record(sn: str, arm: str, name: str) -> dict:
|
||||
title = str(name or "").strip()
|
||||
if not title or "/" in title or "\\" in title or title in (".", ".."):
|
||||
return {"ok": False, "message": "点名不能为空，也不能带路径"}
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge._ask({"cmd": "joints"}, timeout=2.0)
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or "没有读到关节，点位没写下去")}
|
||||
raw = result.get("q") or []
|
||||
if len(raw) < 7:
|
||||
return {"ok": False, "message": "关节数不够 7 个，点位没写下去"}
|
||||
try:
|
||||
values = [round(float(v), 6) for v in raw[:7]]
|
||||
except (TypeError, ValueError):
|
||||
return {"ok": False, "message": "关节角不是数字，点位没写下去"}
|
||||
if any(v != v for v in values):
|
||||
return {"ok": False, "message": "关节角不是有限值，点位没写下去"}
|
||||
listed = list_points(arm)
|
||||
if not listed.get("ok"):
|
||||
return listed
|
||||
points = [row for row in listed["points"] if row["name"] != title]
|
||||
points.append({
|
||||
"name": title,
|
||||
"utc": datetime.now(timezone.utc).strftime("%Y-%m-%dT%H:%M:%SZ"),
|
||||
"joints": values,
|
||||
})
|
||||
path = _write(arm, points)
|
||||
text = " ".join(f"{v:.4f}" for v in values)
|
||||
return {"ok": True, "message": f"已记录 {title}", "detail": f"{path}\n关节 rad：{text}", "points": points}
|
||||
|
||||
|
||||
def delete_point(arm: str, name: str) -> dict:
|
||||
title = str(name or "").strip()
|
||||
listed = list_points(arm)
|
||||
if not listed.get("ok"):
|
||||
return listed
|
||||
kept = [row for row in listed["points"] if row["name"] != title]
|
||||
if len(kept) == len(listed["points"]):
|
||||
return {"ok": False, "message": f"没有名为 {title} 的点"}
|
||||
path = _write(arm, kept)
|
||||
return {"ok": True, "message": f"已删除 {title}", "detail": str(path), "points": kept}
|
||||
|
||||
|
||||
def goto(sn: str, arm: str, name: str, speed: float) -> dict:
|
||||
title = str(name or "").strip()
|
||||
listed = list_points(arm)
|
||||
if not listed.get("ok"):
|
||||
return listed
|
||||
hit = next((row for row in listed["points"] if row["name"] == title), None)
|
||||
if hit is None:
|
||||
return {"ok": False, "message": f"没有名为 {title} 的点"}
|
||||
try:
|
||||
speed_f = float(speed)
|
||||
except (TypeError, ValueError):
|
||||
return {"ok": False, "message": "关节速度不是数字"}
|
||||
if not 0.05 <= speed_f <= 0.35:
|
||||
return {"ok": False, "message": "关节速度只接受 0.05–0.35 rad/s"}
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
bridge.drag_off()
|
||||
result = bridge.move_joints(hit["joints"], speed_f)
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or result)}
|
||||
text = " ".join(f"{float(v):.4f}" for v in hit["joints"])
|
||||
return {
|
||||
"ok": True,
|
||||
"message": f"已回到 {title}",
|
||||
"detail": f"速度 {speed_f:.2f} rad/s\n关节 rad：{text}",
|
||||
}
|
||||
@@ -0,0 +1,836 @@
|
||||
"""D405 主跟踪丢了之后，用顶置 D435 **拍一张** 把工件 / k0 定到基座系。
|
||||
|
||||
为什么单独成模块
|
||||
----------------
|
||||
一阶段平时走腕部 D405（眼在手上）。画面里没有 conf≥门槛的目标时，
|
||||
不能让伺服继续追 0.12 的假框，也不能把顶视外参再乘一遍法兰
|
||||
（那会让一次性定位差几十厘米还看起来合理）。
|
||||
|
||||
判定、取 k0、反投影、臂是否静止 —— 全是纯函数，测试不启 ROS。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.handeye_lib import cam_point_to_base
|
||||
from arm_application.path_profile import ( # 伺服和测试从本模块再导出，不另抄一份
|
||||
D435_FULL_ACC_M,
|
||||
D435_FULL_CRUISE_MPS,
|
||||
D435_FULL_DEC_M,
|
||||
D435_MID_HOP_MAX_M,
|
||||
D435_MID_LEFTOVER_M,
|
||||
D435_MOVE_ACC_MPS2,
|
||||
D435_MOVE_SPEED_SCALE,
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_NEAR_HOP_MAX_M,
|
||||
D435_NEAR_LEFTOVER_M,
|
||||
D435_VENDOR_MOVEL_ACC_MPS2,
|
||||
D435_VENDOR_MOVEL_VEL_MPS,
|
||||
D435_VERT_TILT_RAD,
|
||||
D435_SETTLE_S,
|
||||
FROZEN_J_HOP_MAX_M,
|
||||
build_arc_xy_move,
|
||||
flange_down_w,
|
||||
flange_tilt_rad,
|
||||
hybrid_cruise_vxy,
|
||||
oneshot_hop_limits,
|
||||
smooth_joint_schedule,
|
||||
trap_acc_for_hop,
|
||||
translate_hold_ori_waypoints,
|
||||
)
|
||||
|
||||
# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
|
||||
D435_MIN_PROGRESS_FRAC = 0.40
|
||||
# SwitchMode 返回不等于已经进 primitive。连试仍失败必须让一阶段立刻中止，
|
||||
# 不许空等 150s 装成"还在粗定位"（2026-10-16 17:27）。
|
||||
D435_MOVEL_RETRY = 3
|
||||
D435_MOVEL_RETRY_S = 0.3
|
||||
D435_MOVEL_GAVE_UP = "厂家 MoveL 连试 3 次没走成"
|
||||
|
||||
|
||||
def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
|
||||
"""画面内最高检测置信度。空列表 / None → ``None``（不是 0：0 是"看到了但很差"）。"""
|
||||
if not dets:
|
||||
return None
|
||||
vals = [float(getattr(d, "conf", float("nan"))) for d in dets]
|
||||
vals = [c for c in vals if np.isfinite(c)]
|
||||
if not vals:
|
||||
return None
|
||||
return max(vals)
|
||||
|
||||
|
||||
def z_locked_qos():
|
||||
"""``visual_servo/z_locked`` 的 QoS。发布端和订阅端必须用这一份。
|
||||
|
||||
感知比伺服晚启动。默认 volatile 会丢掉伺服启动时已经发出的「已到锁高」，
|
||||
感知就停在初值 False，D435 永远不拍。
|
||||
"""
|
||||
from rclpy.qos import ( # noqa: PLC0415 — 纯函数测试不需要在导入时拉起 ROS
|
||||
DurabilityPolicy,
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
)
|
||||
|
||||
return QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
|
||||
|
||||
def at_lock_height(z_m: float, lock_z_m: float, gate_m: float) -> bool:
|
||||
"""法兰是否在锁高带内。
|
||||
|
||||
低于锁高**不算**到位。上一轮观测下降停在 0.26 m 时，若把「不高于锁高」
|
||||
当成到位，XY 会在低位开跑，D435 又因高度不对拒绝拍照，两边都不再动。
|
||||
``lock_z_m <= 0`` 表示没启用锁高。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return True
|
||||
gate = max(0.02, float(gate_m))
|
||||
return abs(float(z_m) - lock) <= gate
|
||||
|
||||
|
||||
def lock_z_velocity(z_m: float, lock_z_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""回到锁高的竖直速度 [m/s]，基座 +Z 朝上。
|
||||
|
||||
低于锁高为正（上抬），高于为负（下降）。4 mm 内视为到位。
|
||||
不许在低于锁高时把速度写成 0：那会和「没到锁高不拍 D435」卡死。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return 0.0
|
||||
ez = lock - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
def hold_translation_vz(z_m: float, z_hold_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""平移时把法兰钉在**这一段开始时的高度**。
|
||||
|
||||
只写 v_z=0 不够：大范围平移加姿态校正时，阻尼求逆会把竖直速度吃掉，
|
||||
2026-10-09 真机从 0.327 m 漂到 0.307 m 又抬到 0.337 m。这里用开始那一
|
||||
帧的高度当设定，4 mm 内不动，避免来回抖。不追 site 里的锁高数字，
|
||||
也不因此停掉 XY。下臂仍只在二阶段。
|
||||
"""
|
||||
ez = float(z_hold_m) - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
# D405 连续这么久没有高置信度目标，才让待命的 D435 拍一张。
|
||||
# 单帧漏检就拍，会在平移半路把目标换掉。
|
||||
D405_MISS_BEFORE_D435_S = 0.5
|
||||
# 拍了但伺服没 ``d435_commanding``（工作盒拒收等）：超过这时限就允许再拍。
|
||||
# 不清的话 ``_oneshot_shot`` 会永远真，臂停着、感知还以为在赶路。
|
||||
ONESHOT_ACCEPT_TIMEOUT_S = 0.8
|
||||
|
||||
|
||||
def oneshot_shot_timed_out(
|
||||
*,
|
||||
shot: bool,
|
||||
commanding: bool,
|
||||
age_s: float,
|
||||
timeout_s: float = ONESHOT_ACCEPT_TIMEOUT_S,
|
||||
) -> bool:
|
||||
"""拍过一张、伺服却没接住 → True。已经在赶路不许清。"""
|
||||
if not bool(shot) or bool(commanding):
|
||||
return False
|
||||
return float(age_s) >= float(timeout_s)
|
||||
|
||||
|
||||
def d405_commands_arm(
|
||||
*,
|
||||
using_d435: bool,
|
||||
arrived: bool,
|
||||
settled: bool,
|
||||
move_only: bool = False,
|
||||
) -> bool:
|
||||
"""D405 这一帧的点能不能下发给左臂。
|
||||
|
||||
推理从启动就开着。没有高置信度目标、或 D435 厂家 MoveL 还没结束、
|
||||
法兰还没连续停稳时，这帧**不下发**（输出截断，不许拿去控臂）。
|
||||
厂家移动结束并且法兰停稳（``settled``，时长是 ``d435_handback_sec``）
|
||||
之后才放行，伺服用它追工件框的矩形中心。框心对准停稳后才进精拍下臂。
|
||||
``move_only``：只测 D435 位移，D405 点一律不下发。
|
||||
"""
|
||||
if move_only:
|
||||
return False
|
||||
if using_d435:
|
||||
return bool(arrived) and bool(settled)
|
||||
return True
|
||||
|
||||
|
||||
def retract_goal_z(z_now: float, z_target: float) -> float:
|
||||
"""收臂目标 Z：已经高于目标就钉在当前高度，不许为了追 5 mm 往下补。
|
||||
|
||||
2026-10-16：贴面 MoveL 之后 JTC 还握着悬停关节，收臂 hop1 计划 +5 mm
|
||||
实际抬了 18–25 mm；hop2 把「抬过头」当成误差 ``Δz=−13 mm`` 往回压，
|
||||
看起来就是进孔再弹出。抬过了 = 成功。
|
||||
"""
|
||||
return max(float(z_now), float(z_target))
|
||||
|
||||
|
||||
def oneshot_needs_verticalize(
|
||||
tilt_rad: float,
|
||||
leftover_m: float,
|
||||
arrive_m: float,
|
||||
*,
|
||||
tilt_lim: float = D435_VERT_TILT_RAD,
|
||||
already_done: bool = False,
|
||||
) -> bool:
|
||||
"""XY 已经进到位门槛、倾角还大，才单独收垂直。不许和平移捆在一起。"""
|
||||
if already_done:
|
||||
return False
|
||||
if float(leftover_m) > float(arrive_m):
|
||||
return False
|
||||
return float(tilt_rad) > float(tilt_lim)
|
||||
|
||||
|
||||
def oneshot_next_hop_m(
|
||||
dist_m: float, hop_max_m: float = FROZEN_J_HOP_MAX_M
|
||||
) -> float:
|
||||
"""这一跳最多走 ``hop_max_m``。冻结 J 一次规划 400 mm 会少走一大截。"""
|
||||
return min(max(0.0, float(dist_m)), float(hop_max_m))
|
||||
|
||||
|
||||
def oneshot_cartesian_dp(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
hop_max_m: Optional[float] = None,
|
||||
leftover_xy_m: Optional[float] = None,
|
||||
):
|
||||
"""这一跳的笛卡尔位移。没到锁高只收 Z；远处 XY 带锁高；近处只走 XY。
|
||||
|
||||
工件上方最后一跳若再把 7 mm 的 Z 和 60 mm 的 XY 捆在一起，冻结 J 会把腕部带偏。
|
||||
远处仍带 ``ee_z_m``，避免长跳把高度抬出锁高带。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3)
|
||||
z_lock = float(lock_z)
|
||||
leftover = (
|
||||
float(leftover_xy_m)
|
||||
if leftover_xy_m is not None
|
||||
else float(
|
||||
np.hypot(float(goal_xy[0]) - float(p[0]), float(goal_xy[1]) - float(p[1]))
|
||||
)
|
||||
)
|
||||
hop_cap = (
|
||||
float(hop_max_m)
|
||||
if hop_max_m is not None
|
||||
else oneshot_hop_limits(leftover)[0]
|
||||
)
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
mag = oneshot_next_hop_m(abs(dz), hop_cap)
|
||||
if mag < 1e-6:
|
||||
return np.zeros(3, dtype=float)
|
||||
return np.array([0.0, 0.0, float(np.sign(dz)) * mag], dtype=float)
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[
|
||||
float(goal_xy[0]) - float(p[0]),
|
||||
float(goal_xy[1]) - float(p[1]),
|
||||
dz,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
n = float(np.linalg.norm(dp))
|
||||
hop = oneshot_next_hop_m(n, hop_cap)
|
||||
if n > hop > 1e-9:
|
||||
dp = dp * (hop / n)
|
||||
return dp
|
||||
|
||||
|
||||
def oneshot_full_segments(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
):
|
||||
"""整段一次走完的笛卡尔分段。先收齐锁高，再走全部 XY，不切 80/15 mm。
|
||||
|
||||
近处（剩差 ≤80 mm）仍然只走 XY，不混 Z。收垂直不在这里。
|
||||
返回 ``[(dp, tag), ...]``，``tag`` 是 ``lift_z`` 或 ``xy``。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3).copy()
|
||||
z_lock = float(lock_z)
|
||||
goal = np.asarray(goal_xy, dtype=float).reshape(2)
|
||||
out = []
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
if abs(dz) >= 1e-6:
|
||||
out.append((np.array([0.0, 0.0, dz], dtype=float), "lift_z"))
|
||||
p[2] = z_lock
|
||||
leftover = float(np.hypot(float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1])))
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1]), dz],
|
||||
dtype=float,
|
||||
)
|
||||
if float(np.linalg.norm(dp)) >= 1e-6:
|
||||
out.append((dp, "xy"))
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
|
||||
"""把分段 ``dp`` 用同一份冻结 J 串成一条关节折线。中途不回零速。"""
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
wps = [q.copy()]
|
||||
planned = 0.0
|
||||
asked = 0.0
|
||||
tags = []
|
||||
for dp, tag in segments:
|
||||
hop = float(np.linalg.norm(np.asarray(dp, dtype=float).reshape(3)))
|
||||
if hop < 1e-6:
|
||||
continue
|
||||
asked += hop
|
||||
part, remain = translate_hold_ori_waypoints(
|
||||
q, J, dp, R=R, verticalize=False
|
||||
)
|
||||
if len(part) < 2:
|
||||
break
|
||||
wps.extend(part[1:])
|
||||
q = wps[-1]
|
||||
planned += hop - float(remain)
|
||||
tags.append(str(tag))
|
||||
tag = "full"
|
||||
if tags == ["lift_z"]:
|
||||
tag = "lift_z"
|
||||
elif "lift_z" in tags and "xy" in tags:
|
||||
tag = "full"
|
||||
elif tags == ["xy"]:
|
||||
tag = "full"
|
||||
return wps, planned, asked, tag
|
||||
|
||||
|
||||
def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
|
||||
"""整段巡航：工件上方 45 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到远处 hop 上限。"""
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
|
||||
return min(D435_NEAR_CRUISE_MPS, far)
|
||||
return min(D435_FULL_CRUISE_MPS, far)
|
||||
|
||||
|
||||
def oneshot_may_send_another(
|
||||
leftover_m: float,
|
||||
last_dist_m: Optional[float],
|
||||
*,
|
||||
arrive_m: float,
|
||||
hop_max_m: float = FROZEN_J_HOP_MAX_M,
|
||||
min_progress_frac: float = D435_MIN_PROGRESS_FRAC,
|
||||
) -> bool:
|
||||
"""上一跳走完后还要不要再发一条。
|
||||
|
||||
剩差已经进到位门槛 → 钉住。
|
||||
上一跳几乎没缩短剩差 → 不许按同一距离再刷（真机 28 mm 死循环）。
|
||||
"""
|
||||
if float(leftover_m) <= float(arrive_m):
|
||||
return False
|
||||
if last_dist_m is None:
|
||||
return True
|
||||
progressed = float(last_dist_m) - float(leftover_m)
|
||||
planned = min(float(last_dist_m), float(hop_max_m))
|
||||
if progressed < float(min_progress_frac) * max(planned, 1e-6):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def oneshot_should_submit(
|
||||
*,
|
||||
handoff: bool,
|
||||
low_for_s: float,
|
||||
already_shot: bool,
|
||||
wait_s: float = D405_MISS_BEFORE_D435_S,
|
||||
) -> bool:
|
||||
"""腕部连续 ``wait_s`` 秒没有高置信度目标、这一轮还没拍成，才让 D435 拍一张。
|
||||
|
||||
高度不在这里再判一次。进一阶段之前已经抬到锁高；再等伺服报「已到锁高」
|
||||
会和「没到锁高就只收 Z、不平移」卡死。拍成一张之后不许连拍，顶视不是第二套跟踪。
|
||||
"""
|
||||
return (
|
||||
bool(handoff)
|
||||
and (not already_shot)
|
||||
and float(low_for_s) >= float(wait_s)
|
||||
)
|
||||
|
||||
|
||||
def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
|
||||
"""顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
|
||||
if not dets:
|
||||
return None
|
||||
ok = [
|
||||
d
|
||||
for d in dets
|
||||
if np.isfinite(float(getattr(d, "conf", float("nan"))))
|
||||
and float(d.conf) >= float(min_conf)
|
||||
]
|
||||
if not ok:
|
||||
return None
|
||||
return max(ok, key=lambda d: float(d.conf))
|
||||
|
||||
|
||||
def depth_raw_to_m(raw: Optional[np.ndarray], encoding: str) -> Optional[np.ndarray]:
|
||||
"""深度图 → 米。16UC1 常见是毫米；用中位数判量纲，不许默认当米。"""
|
||||
if raw is None:
|
||||
return None
|
||||
enc = (encoding or "").lower()
|
||||
arr = np.asarray(raw)
|
||||
if arr.size == 0:
|
||||
return None
|
||||
if enc in ("16uc1", "mono16"):
|
||||
pos = arr[arr > 0]
|
||||
if pos.size < 8:
|
||||
return None
|
||||
med = float(np.median(pos))
|
||||
scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001)
|
||||
out = arr.astype(float) * scale
|
||||
out[arr <= 0] = np.nan
|
||||
return out
|
||||
if enc == "32fc1":
|
||||
return arr.astype(float)
|
||||
return None
|
||||
|
||||
|
||||
def map_uv_to_depth(
|
||||
u: float, v: float, color_wh: tuple[int, int], depth_wh: tuple[int, int]
|
||||
) -> tuple[float, float]:
|
||||
"""彩色图像素 → 对齐深度图像素。分辨率相同则原样返回。"""
|
||||
cw, ch = int(color_wh[0]), int(color_wh[1])
|
||||
dw, dh = int(depth_wh[0]), int(depth_wh[1])
|
||||
if cw <= 0 or ch <= 0 or (cw == dw and ch == dh):
|
||||
return float(u), float(v)
|
||||
return float(u) * (dw / float(cw)), float(v) * (dh / float(ch))
|
||||
|
||||
|
||||
def wrist_needs_handoff(best_conf: Optional[float], threshold: float) -> bool:
|
||||
"""腕部这一帧有没有 conf≥门槛的目标。没有 → 该切 D435。
|
||||
|
||||
``None`` 当没有检测。门槛必须显式传入，不许在这里写死再抄一份到 site.yaml。
|
||||
"""
|
||||
if best_conf is None:
|
||||
return True
|
||||
try:
|
||||
c = float(best_conf)
|
||||
except (TypeError, ValueError):
|
||||
return True
|
||||
if not np.isfinite(c):
|
||||
return True
|
||||
return c < float(threshold)
|
||||
|
||||
|
||||
def k0_uv_from_det(
|
||||
det,
|
||||
*,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
) -> Optional[tuple[float, float]]:
|
||||
"""从 pose 检测取 k0 像素。k0 不可信就返回 None（不许拿框心冒充 k0）。"""
|
||||
kpts = getattr(det, "kpts", None)
|
||||
if kpts is None:
|
||||
return None
|
||||
arr = np.asarray(kpts, dtype=float)
|
||||
if arr.ndim != 2 or arr.shape[0] <= int(kpt_index) or arr.shape[1] < 3:
|
||||
return None
|
||||
u, v, kc = (float(x) for x in arr[int(kpt_index)][:3])
|
||||
if not (np.isfinite(u) and np.isfinite(v) and np.isfinite(kc)):
|
||||
return None
|
||||
if kc < float(kpt_min_conf):
|
||||
return None
|
||||
return u, v
|
||||
|
||||
|
||||
def workpiece_uv_from_det(
|
||||
det,
|
||||
*,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
) -> tuple[float, float, bool]:
|
||||
"""优先 k0；没有可信 k0 时退回框心，并标明 ``is_k0=False``。
|
||||
|
||||
顶视经常看不到腕部视角训出来的 k0。退回框心仍能把臂迁到工件上方，
|
||||
让 D405 重新看见 k0 —— 比假装"已经是 k0"好。调用方必须看第三个返回值。
|
||||
"""
|
||||
uv = k0_uv_from_det(det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf)
|
||||
if uv is not None:
|
||||
return uv[0], uv[1], True
|
||||
return float(det.cx), float(det.cy), False
|
||||
|
||||
|
||||
def depth_at_m(
|
||||
depth_m: np.ndarray,
|
||||
u: float,
|
||||
v: float,
|
||||
*,
|
||||
win: int = 3,
|
||||
) -> Optional[float]:
|
||||
"""窗口中位数深度 [m]。无效则 None，不许拿 0 去反投影。"""
|
||||
if depth_m is None or depth_m.size == 0:
|
||||
return None
|
||||
h, w = int(depth_m.shape[0]), int(depth_m.shape[1])
|
||||
ui, vi = int(round(u)), int(round(v))
|
||||
r = max(1, int(win))
|
||||
x0, x1 = max(0, ui - r), min(w, ui + r + 1)
|
||||
y0, y1 = max(0, vi - r), min(h, vi + r + 1)
|
||||
if x1 <= x0 or y1 <= y0:
|
||||
return None
|
||||
patch = np.asarray(depth_m[y0:y1, x0:x1], dtype=float).reshape(-1)
|
||||
patch = patch[np.isfinite(patch) & (patch > 1e-4)]
|
||||
if patch.size == 0:
|
||||
return None
|
||||
return float(np.median(patch))
|
||||
|
||||
|
||||
def pixel_on_base_plane(
|
||||
u: float,
|
||||
v: float,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
T_cam_to_base: np.ndarray,
|
||||
plane_z_m: float,
|
||||
) -> Optional[np.ndarray]:
|
||||
"""彩色像素射线和基座水平面 ``z=plane_z_m`` 的交点 [m]。不用深度图。
|
||||
|
||||
相机系方向是 ``((u-cx)/fx, (v-cy)/fy, 1)``，乘外参旋转到基座。
|
||||
交点在相机后面，或射线几乎贴着平面，返回 None。
|
||||
"""
|
||||
T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
|
||||
direction = np.array(
|
||||
[(float(u) - float(cx)) / float(fx), (float(v) - float(cy)) / float(fy), 1.0],
|
||||
dtype=float,
|
||||
)
|
||||
d_base = T[:3, :3] @ direction
|
||||
if abs(float(d_base[2])) < 1e-9:
|
||||
return None
|
||||
scale = (float(plane_z_m) - float(T[2, 3])) / float(d_base[2])
|
||||
if not np.isfinite(scale) or scale <= 0.0:
|
||||
return None
|
||||
point = T[:3, 3] + scale * d_base
|
||||
if not np.all(np.isfinite(point)):
|
||||
return None
|
||||
return np.asarray(point, dtype=float).reshape(3)
|
||||
|
||||
|
||||
def cam_xy_bias_axes(T_cam_to_base: np.ndarray):
|
||||
"""基座水平面上两个单位向量：``(视线前方, 画面右)``。
|
||||
|
||||
视线 = 相机 +Z 投到基座 XY（前倾 30° 时的交平面敏感轴）；
|
||||
画面右 = 相机 +X 投到基座 XY（立体基线 / 图像 u 方向）。
|
||||
水平投影退化时返回 None，不许拿 (0,0) 当轴。
|
||||
"""
|
||||
T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
|
||||
look = T[:3, :3] @ np.array([0.0, 0.0, 1.0], dtype=float)
|
||||
right = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
look_xy = np.asarray(look[:2], dtype=float)
|
||||
right_xy = np.asarray(right[:2], dtype=float)
|
||||
n_l = float(np.linalg.norm(look_xy))
|
||||
n_r = float(np.linalg.norm(right_xy))
|
||||
if n_l < 1e-6 or n_r < 1e-6:
|
||||
return None
|
||||
return look_xy / n_l, right_xy / n_r
|
||||
|
||||
|
||||
# 单次精拍这一步必须能走完「框心 → k1」。两孔间距约 60mm，半距 30mm。
|
||||
# 多走多拍的 12mm 封顶在这里会把一半孔距留下（2026-10-13 真机：指令 29mm 只走 12mm）。
|
||||
ONESHOT_ALIGN_CAP_M = 0.040
|
||||
# 下降后补 XY：一跳上限。2026-10-13 两跳合计 4.3mm 把近距检测带崩。
|
||||
ONCE_TRIM_MAX_STEP_M = 0.003
|
||||
ONCE_TRIM_REFUSE_M = 0.008
|
||||
HOLE_PAIR_TRIM_MIN_M = 0.050
|
||||
HOLE_PAIR_TRIM_MAX_M = 0.070
|
||||
# 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。
|
||||
# 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。
|
||||
ONCE_APPROACH_STEP_M = 0.020
|
||||
# 悬停已对上时 ΔXY 必须为 0。site ``once_ff_look_per_m`` 同源。
|
||||
# 2026-10-14 下午：框心改平面交点后两条链路下臂都仍 +1.3mm X 过补插不进。
|
||||
# k=0.018 是深度框心时代的对冲，不是下降几何。笛卡尔 MoveL 只降 Z。
|
||||
ONCE_FF_LOOK_PER_M = 0.0
|
||||
ONCE_FF_MOVEL_VEL_MPS = 0.05
|
||||
|
||||
|
||||
def once_look_ff_xy(look_xy, descent_m: float, k: float):
|
||||
"""视线前馈 ΔXY [m]。k≤0 是笛卡尔锁 XY：返回 (0,0)，不是 None。
|
||||
|
||||
k>0 时沿 -look 走 k×下降。下降≤0 / 视线退化 → None。
|
||||
"""
|
||||
if float(descent_m) <= 1e-9:
|
||||
return None
|
||||
if float(k) <= 1e-9:
|
||||
return np.array([0.0, 0.0], dtype=float)
|
||||
look = np.asarray(look_xy, dtype=float).reshape(2)
|
||||
n = float(np.linalg.norm(look))
|
||||
if n < 1e-6 or not np.all(np.isfinite(look)):
|
||||
return None
|
||||
look = look / n
|
||||
dxy = -look * float(k) * float(descent_m)
|
||||
if not np.all(np.isfinite(dxy)):
|
||||
return None
|
||||
return dxy
|
||||
|
||||
|
||||
def once_ff_tcp_xyz(
|
||||
flange_xy,
|
||||
tip_off_xy,
|
||||
look_xy,
|
||||
descent_m: float,
|
||||
k: float,
|
||||
surface_z_m: float,
|
||||
gap_m: float,
|
||||
):
|
||||
"""规尖（厂家 MoveL 的 TCP）目标 [m]。Z 是面 + 间隙，不是法兰。"""
|
||||
dxy = once_look_ff_xy(look_xy, descent_m, k)
|
||||
if dxy is None:
|
||||
return None
|
||||
off = np.asarray(tip_off_xy, dtype=float).reshape(2)
|
||||
fl = np.asarray(flange_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(off)) or not np.all(np.isfinite(fl)):
|
||||
return None
|
||||
tcp = np.array(
|
||||
[
|
||||
float(fl[0]) + float(off[0]) + float(dxy[0]),
|
||||
float(fl[1]) + float(off[1]) + float(dxy[1]),
|
||||
float(surface_z_m) + float(gap_m),
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(tcp)):
|
||||
return None
|
||||
return tcp, dxy
|
||||
|
||||
|
||||
def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
|
||||
"""D435 粗定位 MoveL 目标 [m]：当前厂家 TCP + (K0−法兰) 的 ROS XY。
|
||||
|
||||
keep_z 由硬件用当前 TCP 的 Z 覆盖。ROS 绝对 K0 不能当 WORLD 目标
|
||||
（2026-10-14 贴面 MoveL 已踩过）。缺数 / 非有限 → None。
|
||||
"""
|
||||
try:
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
fl = np.asarray(flange_xy, dtype=float).reshape(2)
|
||||
k0 = np.asarray(k0_xy, dtype=float).reshape(2)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if t.size != 3 or fl.size != 2 or k0.size != 2:
|
||||
return None
|
||||
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(fl)) or not np.all(np.isfinite(k0)):
|
||||
return None
|
||||
out = np.array(
|
||||
[float(t[0]) + float(k0[0]) - float(fl[0]),
|
||||
float(t[1]) + float(k0[1]) - float(fl[1]),
|
||||
float(t[2])],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(out)):
|
||||
return None
|
||||
return out
|
||||
|
||||
|
||||
def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
|
||||
"""厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
|
||||
|
||||
2026-10-14：把 ROS 规尖绝对坐标发给 MoveL，k=0.018 与 0.035 都落到
|
||||
WORLD (0.595, −0.468, 0.024)，法兰实走约 (+9, −8)mm。WORLD 原点与
|
||||
ROS 基座不是同一点，绝对坐标不能用。相对位移才跟 k 有关。
|
||||
"""
|
||||
if float(descent_m) <= 1e-9:
|
||||
return None
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
d = np.asarray(dxy, dtype=float).reshape(2)
|
||||
if t.size != 3 or d.size != 2:
|
||||
return None
|
||||
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(d)):
|
||||
return None
|
||||
out = np.array(
|
||||
[float(t[0]) + float(d[0]), float(t[1]) + float(d[1]), float(t[2]) - float(descent_m)],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(out)):
|
||||
return None
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_align_step_m(err_m: float) -> float:
|
||||
"""单次精拍允许的步长 [m]。不超过半孔距余量；超过 cap 才截断。"""
|
||||
e = abs(float(err_m))
|
||||
cap = float(ONESHOT_ALIGN_CAP_M)
|
||||
if e <= cap:
|
||||
return e
|
||||
return cap
|
||||
|
||||
|
||||
def hole_pair_span_m(p0, p1) -> float:
|
||||
"""两孔基座 XY 间距 [m]。"""
|
||||
a = np.asarray(p0, dtype=float).reshape(-1)
|
||||
b = np.asarray(p1, dtype=float).reshape(-1)
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
|
||||
def hole_pair_span_ok(span_m: float) -> bool:
|
||||
"""下降后补孔用的间距窗。真机 k1/k2 约 60mm；45mm 是近距检测漂了。"""
|
||||
s = float(span_m)
|
||||
return HOLE_PAIR_TRIM_MIN_M <= s <= HOLE_PAIR_TRIM_MAX_M
|
||||
|
||||
|
||||
def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float:
|
||||
"""规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。"""
|
||||
return float(tip_len_m) + float(surface_z_m) + float(gap_m)
|
||||
|
||||
|
||||
def final_gap_refusal(
|
||||
gap_m: float,
|
||||
tool_len_m: float,
|
||||
surface_z_m: float,
|
||||
hover_z_m: float,
|
||||
z_min_m: float,
|
||||
z_max_m: float,
|
||||
) -> str:
|
||||
"""笛卡尔锁 XY 的贴面间隙能不能用。空串表示可以；非空就是拒绝原因。
|
||||
|
||||
参数是规尖离面 [m]，不是法兰 Z：法兰 = 工具长 + 工件面 + 间隙，只在这里算一次。
|
||||
"""
|
||||
gap = float(gap_m)
|
||||
if not np.isfinite(gap):
|
||||
return "贴面间隙不是有限数，拒绝开跑"
|
||||
if gap <= 1e-9:
|
||||
return (
|
||||
f"贴面间隙 {gap * 1000:.1f}mm ≤ 0，规尖会进工件/贴面接触，拒绝开跑"
|
||||
)
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
if tool <= 0.0 or surf <= 0.0:
|
||||
return "写了贴面间隙，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
|
||||
hover_clear = float(hover_z_m) - tool - surf
|
||||
if gap > hover_clear + 1e-6:
|
||||
return (
|
||||
f"贴面间隙 {gap * 1000:.1f}mm 比悬停离面 {hover_clear * 1000:.1f}mm 还高，"
|
||||
"锁 XY 只会抬臂，拒绝开跑"
|
||||
)
|
||||
flange = flange_z_for_gap_m(tool, surf, gap)
|
||||
z_lo = float(z_min_m)
|
||||
z_hi = float(z_max_m)
|
||||
if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
|
||||
return (
|
||||
f"贴面法兰 Z {flange:.3f}m（规尖离面 {gap * 1000:.1f}mm）"
|
||||
f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray):
|
||||
"""把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。"""
|
||||
axes = cam_xy_bias_axes(T_cam_to_base)
|
||||
if axes is None:
|
||||
return None
|
||||
look_xy, right_xy = axes
|
||||
d = np.asarray(delta_xy, dtype=float).reshape(2)
|
||||
return {
|
||||
"along_look_m": float(np.dot(d, look_xy)),
|
||||
"along_right_m": float(np.dot(d, right_xy)),
|
||||
"look_xy": [float(look_xy[0]), float(look_xy[1])],
|
||||
"right_xy": [float(right_xy[0]), float(right_xy[1])],
|
||||
}
|
||||
|
||||
|
||||
def px_to_cam(
|
||||
u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
|
||||
) -> np.ndarray:
|
||||
return np.array(
|
||||
[(u - cx) * z / fx, (v - cy) * z / fy, z], dtype=float
|
||||
)
|
||||
|
||||
|
||||
def oneshot_k0_base(
|
||||
det,
|
||||
depth_m: np.ndarray,
|
||||
*,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
T_cam_to_base: np.ndarray,
|
||||
min_z_m: float,
|
||||
max_z_m: float,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
min_det_conf: float = 0.5,
|
||||
color_wh: Optional[tuple[int, int]] = None,
|
||||
depth_wh: Optional[tuple[int, int]] = None,
|
||||
) -> Optional[tuple[np.ndarray, bool]]:
|
||||
"""D435 一张图 → 基座系 XYZ。失败返回 None。
|
||||
|
||||
外参必须是 **cam→基座**（eye_to_hand）。传入腕部 T 会得到错位置且不报错，
|
||||
所以调用方必须自己保证文件对；本函数不再猜。
|
||||
反投影用**彩色图**像素 + 彩色图内参；深度只在对齐图上取样。
|
||||
"""
|
||||
if det is None or float(getattr(det, "conf", 0.0)) < float(min_det_conf):
|
||||
return None
|
||||
u, v, is_k0 = workpiece_uv_from_det(
|
||||
det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf
|
||||
)
|
||||
u_d, v_d = u, v
|
||||
if color_wh is not None and depth_wh is not None:
|
||||
u_d, v_d = map_uv_to_depth(u, v, color_wh, depth_wh)
|
||||
z = depth_at_m(depth_m, u_d, v_d)
|
||||
if z is None or z < float(min_z_m) or z > float(max_z_m):
|
||||
return None
|
||||
p_cam = px_to_cam(u, v, z, fx, fy, cx, cy)
|
||||
T_ee = np.eye(4) # eye_to_hand 不用法兰，占位
|
||||
p_base = cam_point_to_base(p_cam, T_cam_to_base, T_ee, "eye_to_hand")
|
||||
if not np.all(np.isfinite(p_base)):
|
||||
return None
|
||||
return np.asarray(p_base, dtype=float).reshape(3), bool(is_k0)
|
||||
|
||||
|
||||
@dataclass
|
||||
class StillLock:
|
||||
"""D405 跟住时，法兰 XY 连续 ``still_sec`` 秒位移不超过 ``still_move_m`` → 锁定。
|
||||
|
||||
顶视一次性迁过去的那段路**不算**锁定：``tracking_ok`` 必须是腕部有效目标。
|
||||
大幅一动就重计时，避免"晃一下刚好满 3 秒"误进二阶段。
|
||||
"""
|
||||
|
||||
still_sec: float
|
||||
still_move_m: float
|
||||
_t0: Optional[float] = None
|
||||
_anchor_xy: Optional[np.ndarray] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self._t0 = None
|
||||
self._anchor_xy = None
|
||||
|
||||
def update(
|
||||
self,
|
||||
p_ee_xy: Optional[np.ndarray],
|
||||
now: float,
|
||||
*,
|
||||
tracking_ok: bool,
|
||||
) -> bool:
|
||||
if (not tracking_ok) or p_ee_xy is None:
|
||||
self.reset()
|
||||
return False
|
||||
xy = np.asarray(p_ee_xy, dtype=float).reshape(2)
|
||||
if self._anchor_xy is None or self._t0 is None:
|
||||
self._anchor_xy = xy.copy()
|
||||
self._t0 = float(now)
|
||||
return False
|
||||
if float(np.linalg.norm(xy - self._anchor_xy)) > float(self.still_move_m):
|
||||
self._anchor_xy = xy.copy()
|
||||
self._t0 = float(now)
|
||||
return False
|
||||
return (float(now) - float(self._t0)) >= float(self.still_sec)
|
||||
@@ -0,0 +1,556 @@
|
||||
"""路径弧长上的梯形速度，以及 XY 圆弧。
|
||||
|
||||
二阶段下臂和一阶段 D435 迁臂共用这一份。不许再抄一套加减速。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.5 → 120 mm/s（远处 hop 上限）。
|
||||
D435_MOVE_SPEED_SCALE = 1.5
|
||||
# D435 厂家 MoveL：SDK ``vel`` [m/s]、``acc`` [m/s²]。
|
||||
# 厂家示例自由空间 0.6 / 近接触 0.2；本档 0.3 和 1.5，短行程也能爬到巡航。
|
||||
# 只改 vel、不给 acc 时，短 hop 仍被默认加速度卡住，肉眼看不出。
|
||||
D435_VENDOR_MOVEL_VEL_MPS = 0.3
|
||||
D435_VENDOR_MOVEL_ACC_MPS2 = 1.5
|
||||
# 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
|
||||
D435_MOVE_ACC_MPS2 = 0.1875
|
||||
# action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
|
||||
D435_SETTLE_S = 0.25
|
||||
# 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
|
||||
# 400 mm 一次规划，真机剩 117 mm，随后 28 mm 死循环刷轨迹。
|
||||
FROZEN_J_HOP_MAX_M = 0.08
|
||||
# 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。
|
||||
D435_NEAR_LEFTOVER_M = 0.08
|
||||
D435_NEAR_HOP_MAX_M = 0.015
|
||||
D435_NEAR_CRUISE_MPS = 0.045
|
||||
# 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
|
||||
D435_VERT_TILT_RAD = 0.015
|
||||
D435_MID_LEFTOVER_M = 0.15
|
||||
D435_MID_HOP_MAX_M = 0.040
|
||||
# 弦长的 12% 当矢高，封顶 50 mm，避免大圆弧甩出工作盒。
|
||||
D435_ARC_SAGITTA_FRAC = 0.12
|
||||
D435_ARC_SAGITTA_MAX_M = 0.05
|
||||
# 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射
|
||||
# （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。
|
||||
# 峰值加速度 a_max = π v² / (4 d_acc)；150 mm @ 90 mm/s → 约 0.042 m/s²。
|
||||
# 巡航相对原 60 mm/s 加 50%；加减速路程同比加长，起步时间差不多。
|
||||
D435_FULL_ACC_M = 0.150
|
||||
D435_FULL_DEC_M = 0.120
|
||||
D435_FULL_CRUISE_MPS = 0.090
|
||||
|
||||
|
||||
def hybrid_cruise_vxy(
|
||||
v_slow_xy: float, scale: float = D435_MOVE_SPEED_SCALE
|
||||
) -> float:
|
||||
"""远处巡航。近处由 ``oneshot_hop_limits`` 另给一档，不许再抄 15 mm/s。"""
|
||||
return max(1e-4, float(v_slow_xy) * float(scale))
|
||||
|
||||
|
||||
def oneshot_hop_limits(
|
||||
leftover_m: float, far_cruise_mps: float = 0.08
|
||||
) -> Tuple[float, float]:
|
||||
"""这一跳的 ``(hop_max_m, cruise_mps)``。近处必须比 80 mm / 80 mm/s 短且慢。"""
|
||||
d = max(0.0, float(leftover_m))
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if d <= D435_NEAR_LEFTOVER_M:
|
||||
return D435_NEAR_HOP_MAX_M, min(D435_NEAR_CRUISE_MPS, far)
|
||||
if d <= D435_MID_LEFTOVER_M:
|
||||
return D435_MID_HOP_MAX_M, far
|
||||
return FROZEN_J_HOP_MAX_M, far
|
||||
|
||||
|
||||
def flange_down_w(
|
||||
R,
|
||||
*,
|
||||
dead_rad: float = 0.006,
|
||||
gain: float = 2.0,
|
||||
mag_lim: float = 0.5,
|
||||
) -> Tuple[np.ndarray, float]:
|
||||
"""让法兰 +Z 指向世界 −Z 的角速度 + 当前倾角。
|
||||
|
||||
和二阶段 ``_w_down`` 同一份。死区 0.006 rad（约 0.34°）以下不再修。
|
||||
"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(math.acos(c))
|
||||
if ang < float(dead_rad):
|
||||
return np.zeros(3, dtype=float), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
mag = float(np.clip(float(gain) * ang, -float(mag_lim), float(mag_lim)))
|
||||
return axis * mag, ang
|
||||
|
||||
|
||||
def flange_tilt_rad(R) -> float:
|
||||
"""法兰 +Z 相对世界 −Z 的倾角 [rad]。只读角度，不带死区。"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
return float(math.acos(c))
|
||||
|
||||
|
||||
def trap_acc_for_hop(dist: float, v: float) -> float:
|
||||
"""二阶段 ``_plan_and_send`` 同一条刹车：路程越长刹车越远，不许另写一套。"""
|
||||
v_cmd = max(min(float(v), 0.08), 1e-3)
|
||||
d_brake = min(0.030, max(0.006, 0.32 * max(float(dist), 1e-6)))
|
||||
return max(v_cmd * v_cmd / (2.0 * d_brake), 0.030)
|
||||
|
||||
|
||||
def trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
|
||||
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
a = max(float(a), 1e-3)
|
||||
d_acc = v * v / (2.0 * a)
|
||||
if 2.0 * d_acc >= dist:
|
||||
v_peak = math.sqrt(max(a * dist, 1e-12))
|
||||
t_acc = v_peak / a
|
||||
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
|
||||
t_acc = v / a
|
||||
t_cruise = (dist - 2.0 * d_acc) / v
|
||||
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
|
||||
|
||||
|
||||
def trap_sv(
|
||||
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的路径弧长 s 与切向速度。"""
|
||||
t = max(0.0, float(t))
|
||||
t_dec0 = t_acc + t_cruise
|
||||
t_end = t_dec0 + t_acc
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
return 0.5 * a * t * t, a * t
|
||||
if t < t_dec0:
|
||||
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_dec0
|
||||
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
|
||||
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
|
||||
|
||||
|
||||
def smooth_path_params(
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
) -> Tuple[float, float, float, float, float, float]:
|
||||
"""整段一次走完的余弦加减速：(T, v_peak, t_acc, t_cruise, d_acc, d_dec)。
|
||||
|
||||
加速/减速段速度按半个余弦走，两端加速度为 0，没有梯形那种加速度台阶。
|
||||
路程不够巡航时整段一条余弦，峰值不超过 ``v``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
acc = min(max(float(d_acc), 1e-4), 0.45 * dist)
|
||||
dec = min(max(float(d_dec), 1e-4), 0.45 * dist)
|
||||
if acc + dec >= dist - 1e-6:
|
||||
# 整段一条正弦速度：v = v_peak sin(π t / T)，s(T)=2 v_peak T / π。
|
||||
T = math.pi * dist / (2.0 * v)
|
||||
return T, v, 0.0, 0.0, 0.0, dist
|
||||
t_acc = 2.0 * acc / v
|
||||
t_dec = 2.0 * dec / v
|
||||
t_cruise = (dist - acc - dec) / v
|
||||
T = t_acc + t_cruise + t_dec
|
||||
return T, v, t_acc, t_cruise, acc, dec
|
||||
|
||||
|
||||
def cosine_peak_acc(v: float, d_acc: float) -> float:
|
||||
"""余弦加速段的峰值切向加速度 [m/s²]。``d_acc`` 越长，起步越不弹。"""
|
||||
d = max(float(d_acc), 1e-6)
|
||||
return math.pi * float(v) * float(v) / (4.0 * d)
|
||||
|
||||
|
||||
def smooth_path_sv(
|
||||
t: float,
|
||||
dist: float,
|
||||
v_peak: float,
|
||||
t_acc: float,
|
||||
t_cruise: float,
|
||||
d_acc: float,
|
||||
d_dec: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的弧长与切向速度。头尾速度为 0。"""
|
||||
t = max(0.0, float(t))
|
||||
dist = max(float(dist), 1e-6)
|
||||
v_peak = max(float(v_peak), 1e-4)
|
||||
if t_acc <= 1e-9 and t_cruise <= 1e-9:
|
||||
T = math.pi * dist / (2.0 * v_peak)
|
||||
if t >= T:
|
||||
return dist, 0.0
|
||||
u = math.pi * t / T
|
||||
s = (v_peak * T / math.pi) * (1.0 - math.cos(u))
|
||||
vs = v_peak * math.sin(u)
|
||||
return min(s, dist), max(0.0, vs)
|
||||
t_dec = 2.0 * float(d_dec) / v_peak if v_peak > 1e-9 else 0.0
|
||||
t_end = float(t_acc) + float(t_cruise) + t_dec
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
u = math.pi * t / t_acc
|
||||
s = 0.5 * v_peak * (t - t_acc / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 - math.cos(u))
|
||||
return s, vs
|
||||
if t < t_acc + t_cruise:
|
||||
return float(d_acc) + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_acc - t_cruise
|
||||
u = math.pi * td / t_dec if t_dec > 1e-9 else math.pi
|
||||
s0 = float(d_acc) + v_peak * float(t_cruise)
|
||||
s = s0 + 0.5 * v_peak * (td + t_dec / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 + math.cos(u))
|
||||
return min(s, dist), max(0.0, vs)
|
||||
|
||||
|
||||
def _circle_3(
|
||||
a: np.ndarray, b: np.ndarray, c: np.ndarray
|
||||
) -> Optional[Tuple[np.ndarray, float]]:
|
||||
ax, ay = float(a[0]), float(a[1])
|
||||
bx, by = float(b[0]), float(b[1])
|
||||
cx, cy = float(c[0]), float(c[1])
|
||||
d = 2.0 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by))
|
||||
if abs(d) < 1e-9:
|
||||
return None
|
||||
a2 = ax * ax + ay * ay
|
||||
b2 = bx * bx + by * by
|
||||
c2 = cx * cx + cy * cy
|
||||
ux = (a2 * (by - cy) + b2 * (cy - ay) + c2 * (ay - by)) / d
|
||||
uy = (a2 * (cx - bx) + b2 * (ax - cx) + c2 * (bx - ax)) / d
|
||||
center = np.array([ux, uy], dtype=float)
|
||||
r = float(np.hypot(ax - ux, ay - uy))
|
||||
if r < 1e-4:
|
||||
return None
|
||||
return center, r
|
||||
|
||||
|
||||
def _ccw(a0: float, a1: float) -> float:
|
||||
return (a1 - a0) % (2.0 * math.pi)
|
||||
|
||||
|
||||
def _xy_in_box(xy: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> bool:
|
||||
return bool(
|
||||
float(lo[0]) <= float(xy[0]) <= float(hi[0])
|
||||
and float(lo[1]) <= float(xy[1]) <= float(hi[1])
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArcXyMove:
|
||||
"""从 p0 走到 p1 的 XY 圆弧（或退化成直线），弧长上跑梯形速度。"""
|
||||
|
||||
p0: np.ndarray
|
||||
p1: np.ndarray
|
||||
length: float
|
||||
v_cruise: float
|
||||
T: float
|
||||
v_peak: float
|
||||
t_acc: float
|
||||
t_cruise: float
|
||||
a: float
|
||||
t0: float
|
||||
linear: bool
|
||||
center: Optional[np.ndarray] = None
|
||||
radius: float = 0.0
|
||||
ang0: float = 0.0
|
||||
sweep: float = 0.0
|
||||
s_last: float = 0.0
|
||||
_dir: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0]))
|
||||
|
||||
def sample(self, now: float) -> Tuple[np.ndarray, np.ndarray, bool]:
|
||||
"""返回 (计划 XY, 切向速度, 是否走完)。"""
|
||||
t = max(0.0, float(now) - float(self.t0))
|
||||
s, sdot = trap_sv(
|
||||
t, self.length, self.v_peak, self.t_acc, self.t_cruise, self.a
|
||||
)
|
||||
s = min(float(s), float(self.length))
|
||||
self.s_last = s
|
||||
done = t >= float(self.T) - 1e-4 or s >= float(self.length) - 1e-5
|
||||
if self.linear or self.radius <= 1e-6 or self.center is None:
|
||||
L = max(float(self.length), 1e-6)
|
||||
u = s / L
|
||||
xy = (1.0 - u) * self.p0 + u * self.p1
|
||||
vxy = self._dir * float(sdot)
|
||||
return xy, vxy, done
|
||||
frac = 0.0 if abs(self.sweep) < 1e-9 else s / (self.radius * abs(self.sweep))
|
||||
frac = min(1.0, max(0.0, frac))
|
||||
ang = self.ang0 + self.sweep * frac
|
||||
xy = self.center + self.radius * np.array(
|
||||
[math.cos(ang), math.sin(ang)], dtype=float
|
||||
)
|
||||
tang = np.array([-math.sin(ang), math.cos(ang)], dtype=float)
|
||||
if self.sweep < 0.0:
|
||||
tang = -tang
|
||||
return xy, tang * float(sdot), done
|
||||
|
||||
|
||||
def build_arc_xy_move(
|
||||
p0,
|
||||
p1,
|
||||
*,
|
||||
v: float,
|
||||
a: float,
|
||||
now: float,
|
||||
ws_min=None,
|
||||
ws_max=None,
|
||||
sag_frac: float = D435_ARC_SAGITTA_FRAC,
|
||||
sag_max: float = D435_ARC_SAGITTA_MAX_M,
|
||||
) -> ArcXyMove:
|
||||
"""p0→p1 的圆弧。工作盒装不下就换侧，再不行走直线。速度整段同一档。"""
|
||||
p0 = np.asarray(p0, dtype=float).reshape(2)
|
||||
p1 = np.asarray(p1, dtype=float).reshape(2)
|
||||
chord = p1 - p0
|
||||
L = float(np.hypot(chord[0], chord[1]))
|
||||
direction = (
|
||||
chord / L if L > 1e-6 else np.array([1.0, 0.0], dtype=float)
|
||||
)
|
||||
cruise = max(float(v), 1e-4)
|
||||
acc = max(float(a), 1e-3)
|
||||
|
||||
def _linear() -> ArcXyMove:
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(max(L, 1e-6), cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=max(L, 1e-6),
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=True,
|
||||
_dir=direction,
|
||||
)
|
||||
|
||||
if L < 0.003:
|
||||
return _linear()
|
||||
|
||||
n = np.array([-direction[1], direction[0]], dtype=float)
|
||||
h = min(float(sag_max), float(sag_frac) * L)
|
||||
mid = 0.5 * (p0 + p1)
|
||||
lo = None if ws_min is None else np.asarray(ws_min, dtype=float).reshape(-1)[:2]
|
||||
hi = None if ws_max is None else np.asarray(ws_max, dtype=float).reshape(-1)[:2]
|
||||
|
||||
def _try(sign: float) -> Optional[ArcXyMove]:
|
||||
apex = mid + n * (h * sign)
|
||||
cir = _circle_3(p0, apex, p1)
|
||||
if cir is None:
|
||||
return None
|
||||
center, r = cir
|
||||
a0 = math.atan2(p0[1] - center[1], p0[0] - center[0])
|
||||
a1 = math.atan2(p1[1] - center[1], p1[0] - center[0])
|
||||
a_via = math.atan2(apex[1] - center[1], apex[0] - center[0])
|
||||
ccw_end = _ccw(a0, a1)
|
||||
ccw_via = _ccw(a0, a_via)
|
||||
if 1e-6 < ccw_via < ccw_end - 1e-6:
|
||||
sweep = ccw_end
|
||||
else:
|
||||
sweep = ccw_end - 2.0 * math.pi
|
||||
if abs(sweep) < 1e-6:
|
||||
return None
|
||||
length = r * abs(sweep)
|
||||
if lo is not None and hi is not None:
|
||||
for k in range(9):
|
||||
frac = k / 8.0
|
||||
ang = a0 + sweep * frac
|
||||
xy = center + r * np.array([math.cos(ang), math.sin(ang)])
|
||||
if not _xy_in_box(xy, lo, hi):
|
||||
return None
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(length, cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=length,
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=False,
|
||||
center=center,
|
||||
radius=r,
|
||||
ang0=a0,
|
||||
sweep=sweep,
|
||||
)
|
||||
|
||||
for sign in (1.0, -1.0):
|
||||
got = _try(sign)
|
||||
if got is not None:
|
||||
return got
|
||||
return _linear()
|
||||
|
||||
|
||||
def path_interp(waypoints, u: float):
|
||||
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
|
||||
nseg = len(waypoints) - 1
|
||||
if nseg <= 0:
|
||||
z = np.zeros_like(waypoints[0])
|
||||
return waypoints[0].copy(), z
|
||||
u = float(min(1.0, max(0.0, u)))
|
||||
x = u * nseg
|
||||
i = min(nseg - 1, int(math.floor(x)))
|
||||
a = x - i
|
||||
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
|
||||
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
|
||||
return q, dq_du
|
||||
|
||||
|
||||
def translate_hold_ori_waypoints(
|
||||
q0,
|
||||
J,
|
||||
dp,
|
||||
*,
|
||||
damping: float = 0.08,
|
||||
max_dq: float = 0.06,
|
||||
R=None,
|
||||
verticalize: bool = False,
|
||||
):
|
||||
"""冻结当前雅可比，把 ``dp`` 拆成关节路点。
|
||||
|
||||
和二阶段 ``_plan_and_send`` 同一份逆解。``verticalize`` 时顺带收法兰朝下，
|
||||
不许另抄一份 ``_w_down``。返回 ``(路点, 还剩多少米)``。
|
||||
"""
|
||||
from arm_application.geom_ik import dls_translate_hold_ori, dls_twist
|
||||
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
remain = np.asarray(dp, dtype=float).reshape(3).copy()
|
||||
dist = float(np.linalg.norm(remain))
|
||||
wps = [q.copy()]
|
||||
inner_tol = min(0.00012, max(0.00005, 0.05 * max(dist, 1e-6)))
|
||||
n_max = int(math.ceil(dist / 0.008)) + 16
|
||||
Jj = np.asarray(J, dtype=float)
|
||||
w_step = np.zeros(3, dtype=float)
|
||||
ang = 0.0
|
||||
if verticalize:
|
||||
if R is None:
|
||||
raise ValueError("verticalize 必须传入当前法兰 R，不许假装已经朝下")
|
||||
w_step, ang = flange_down_w(R)
|
||||
for _ in range(n_max):
|
||||
if float(np.linalg.norm(remain)) < inner_tol and (
|
||||
(not verticalize) or ang < 0.006
|
||||
):
|
||||
break
|
||||
nseg = max(1, int(math.ceil(float(np.linalg.norm(remain)) / 0.012)))
|
||||
step = remain / nseg
|
||||
if verticalize and ang > 0.008:
|
||||
qdot = dls_twist(Jj, step / 0.12, w_base=w_step, damping=damping, max_qdot=0.45)
|
||||
dq = np.clip(qdot * 0.12, -float(max_dq), float(max_dq))
|
||||
else:
|
||||
dq = dls_translate_hold_ori(Jj, step, damping=damping, max_dq=max_dq)
|
||||
if float(np.max(np.abs(dq))) < 1e-5:
|
||||
break
|
||||
q = q + dq
|
||||
wps.append(q.copy())
|
||||
remain = remain - (Jj[:3, :] @ dq)
|
||||
ang = max(0.0, ang - float(np.linalg.norm(w_step)) * 0.12)
|
||||
return wps, float(np.linalg.norm(remain))
|
||||
|
||||
|
||||
def trap_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
a: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
):
|
||||
"""路点 + 梯形速度 → ``[(q, qdot, t), ...]``。头尾速度为 0，时间严格递增。
|
||||
|
||||
一次下发整段。中途再发下一条会把这条掐掉，臂就重新加速 —— 那就是抖。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, a_use = trap_params(dist, v, a)
|
||||
n_samp = max(8, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = trap_sv(t_local, dist, v_peak, t_acc, t_cruise, a_use)
|
||||
u = s / dist
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append((np.asarray(qi, dtype=float).copy(), np.asarray(qdot, dtype=float).copy(), t))
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
|
||||
|
||||
def smooth_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
):
|
||||
"""路点 + 余弦加减速 → ``[(q, qdot, t), ...]``。头尾速度为 0。
|
||||
|
||||
D435 整段一次下发用这一份。二阶段短跳仍走 ``trap_joint_schedule``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, acc_m, dec_m = smooth_path_params(
|
||||
dist, v, d_acc=d_acc, d_dec=d_dec
|
||||
)
|
||||
n_samp = max(12, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = smooth_path_sv(
|
||||
t_local, dist, v_peak, t_acc, t_cruise, acc_m, dec_m
|
||||
)
|
||||
u = min(1.0, s / dist)
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append(
|
||||
(
|
||||
np.asarray(qi, dtype=float).copy(),
|
||||
np.asarray(qdot, dtype=float).copy(),
|
||||
t,
|
||||
)
|
||||
)
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
File diff suppressed because it is too large
Load Diff
@@ -11,31 +11,116 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import time
|
||||
from typing import Optional
|
||||
from pathlib import Path
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from std_msgs.msg import Bool, String
|
||||
from trajectory_msgs.msg import JointTrajectory
|
||||
|
||||
from arm_application.gauge_kin import (
|
||||
a7_limit_target,
|
||||
match_limits,
|
||||
parse_joint_limits,
|
||||
)
|
||||
from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
|
||||
from arm_application.site_io import load_site_dict
|
||||
from arm_application.stage2_locate_node import _pt
|
||||
from arm_application.stage3_search_node import Stage3Search
|
||||
|
||||
|
||||
#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
|
||||
#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
|
||||
#:
|
||||
#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
|
||||
#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
|
||||
#: 小幅下降（只需 5cm 以内的行程）。
|
||||
#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
|
||||
#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
|
||||
#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
|
||||
#:
|
||||
#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
|
||||
#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
|
||||
#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
|
||||
TRACK_CLEARANCE_M = 0.05
|
||||
#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
|
||||
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 hold_at_insert_z(final, *, skip3: bool) -> bool:
|
||||
"""贴面已经按 ``final_gap_mm`` 停下时，不再进三阶段。
|
||||
|
||||
三阶段开局会抬到悬高上方测触觉，再落回 ``hole_hover_z_m``。
|
||||
没有接触传感器。到位高度就是这次的贴面间隙，不另设一个 Z。
|
||||
"""
|
||||
if skip3:
|
||||
return True
|
||||
if not isinstance(final, dict):
|
||||
return False
|
||||
return bool(final.get("insert_held"))
|
||||
|
||||
|
||||
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
|
||||
"""一阶段锁高。
|
||||
|
||||
悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
|
||||
对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
|
||||
否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
|
||||
"""
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
hover = float(hover_z)
|
||||
if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
|
||||
return hover
|
||||
return hover + TRACK_CLEARANCE_M
|
||||
|
||||
|
||||
def stage1_log_fatal(log_path: str) -> str:
|
||||
"""一阶段日志里已经写明失败时，不许再空等锁定。空串 = 还没到这一步。"""
|
||||
try:
|
||||
text = Path(log_path).read_text(encoding="utf-8", errors="replace")
|
||||
except OSError:
|
||||
return ""
|
||||
if D435_MOVEL_GAVE_UP in text:
|
||||
return (
|
||||
f"D435 {D435_MOVEL_GAVE_UP}（RDK 控制模式没切到 primitive），"
|
||||
f"一阶段中止。不是视野/标定。看 {log_path}"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
|
||||
@@ -52,6 +137,9 @@ def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) ->
|
||||
以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
|
||||
因为缺依赖早退了"被读成"算法不收敛"，白查半天。
|
||||
"""
|
||||
fatal = stage1_log_fatal(log_path)
|
||||
if fatal:
|
||||
return fatal
|
||||
if points_seen <= 0:
|
||||
return (
|
||||
f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
|
||||
@@ -79,11 +167,26 @@ class AssemblyPipeline(Stage3Search):
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
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
|
||||
|
||||
self._s1: Optional[subprocess.Popen] = None
|
||||
self._locked = False
|
||||
self._robot_description: Optional[str] = None
|
||||
# robot_state_publisher 用 TRANSIENT_LOCAL 只发一次。默认 VOLATILE
|
||||
# 订阅在它之后才起来，3s 内一条都收不到，A7 会因“没有限位”拒绝旋转。
|
||||
self.create_subscription(
|
||||
String,
|
||||
"/robot_description",
|
||||
self._on_robot_description,
|
||||
QoSProfile(
|
||||
depth=1,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
),
|
||||
)
|
||||
self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
|
||||
# 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
|
||||
# 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
|
||||
@@ -113,9 +216,11 @@ class AssemblyPipeline(Stage3Search):
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
|
||||
self.get_logger().info(
|
||||
f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3} insert={self.insert_depth_mm:.1f}mm"
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3} "
|
||||
f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
|
||||
)
|
||||
|
||||
def _state(self, s: str) -> None:
|
||||
@@ -125,6 +230,113 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._state_pub.publish(msg)
|
||||
self._pipe_pub.publish(msg)
|
||||
|
||||
def _on_robot_description(self, msg: String) -> None:
|
||||
self._robot_description = str(msg.data)
|
||||
|
||||
def _a7_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]:
|
||||
"""A7 软件限位。来自 ``/robot_description`` 的 joint7，与厂家 ``q_min/q_max`` 同一对。"""
|
||||
t0 = time.monotonic()
|
||||
while self._robot_description is None and time.monotonic() - t0 < timeout_s:
|
||||
self._spin(0.05)
|
||||
if not self._robot_description:
|
||||
self.get_logger().error("拿不到 /robot_description，A7 没有限位，拒绝旋转")
|
||||
return None
|
||||
got = match_limits(
|
||||
parse_joint_limits(self._robot_description), [self.joint_names[-1]]
|
||||
)
|
||||
if got is None:
|
||||
self.get_logger().error(
|
||||
f"/robot_description 里没有 {self.joint_names[-1]} 的限位，拒绝旋转"
|
||||
)
|
||||
return got
|
||||
|
||||
def _send_a7_only(self, a7_rad: float, speed_deg_s: float, tag: str) -> bool:
|
||||
"""只下发 A7。前六轴用这一拍的当前角，厂家 ``SendJointPosition`` 收到的也是这一组。"""
|
||||
q, Tee = self._wait_state(5.0)
|
||||
if q is None or Tee is None or int(q.size) != 7:
|
||||
self.get_logger().error(f"{tag}: 拿不到 7 轴关节，不下发")
|
||||
return False
|
||||
z0 = float(Tee[2, 3])
|
||||
q_hold = q.copy()
|
||||
q_hold[6] = float(a7_rad)
|
||||
delta_deg = abs(math.degrees(float(q_hold[6] - q[6])))
|
||||
dur = max(0.8, delta_deg / max(float(speed_deg_s), 1.0))
|
||||
zeros = np.zeros(q.size)
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
traj.points = [_pt(q, zeros, 0.2), _pt(q_hold, zeros, 0.2 + dur)]
|
||||
self.get_logger().info(
|
||||
f"{tag}: A7 {math.degrees(float(q[6])):+.1f}° → "
|
||||
f"{math.degrees(float(q_hold[6])):+.1f}°，"
|
||||
f"{speed_deg_s:.0f}°/s，前六轴不动，法兰 Z={z0:.4f}m"
|
||||
)
|
||||
self._send_action(traj, wait_s=dur + 8.0)
|
||||
q2, Tee2 = self._wait_state(5.0)
|
||||
if q2 is None or Tee2 is None:
|
||||
self.get_logger().error(f"{tag}: 走完读不到关节")
|
||||
return False
|
||||
others = float(np.max(np.abs(q2[:6] - q[:6])))
|
||||
dz = abs(float(Tee2[2, 3]) - z0)
|
||||
a7_err = abs(float(q2[6] - q_hold[6]))
|
||||
self.get_logger().info(
|
||||
f"{tag}: 前六轴最大偏差 {others:.4f} rad，"
|
||||
f"法兰 ΔZ={dz * 1000:.1f}mm，A7 残差 {math.degrees(a7_err):.1f}°"
|
||||
)
|
||||
if others > 0.02:
|
||||
self.get_logger().error(f"{tag}: 前六轴动了 {others:.3f} rad，中止")
|
||||
return False
|
||||
if dz > 0.003:
|
||||
self.get_logger().error(f"{tag}: 法兰 Z 变了 {dz * 1000:.1f}mm，中止")
|
||||
return False
|
||||
if a7_err > 0.05:
|
||||
self.get_logger().error(
|
||||
f"{tag}: A7 还差 {math.degrees(a7_err):.1f}°，没到限位内侧"
|
||||
)
|
||||
return False
|
||||
self._pin(q2)
|
||||
return True
|
||||
|
||||
def _run_a7_seat(self) -> bool:
|
||||
"""贴面后咬螺纹：A7 先逆时针到限位，再顺时针到限位。不改 Z。"""
|
||||
limits = self._a7_limits()
|
||||
if limits is None:
|
||||
return False
|
||||
ga = load_site_dict().get("gauge_assembly") or {}
|
||||
try:
|
||||
margin = float(ga["extreme_margin_deg"])
|
||||
speed = float(ga["extreme_speed_deg_s"])
|
||||
ccw = int(ga["extreme_direction"])
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
self.get_logger().error(
|
||||
f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转"
|
||||
)
|
||||
return False
|
||||
if ccw not in (-1, 1):
|
||||
self.get_logger().error(
|
||||
f"extreme_direction 只能是 +1/-1，收到 {ccw}，拒绝旋转"
|
||||
)
|
||||
return False
|
||||
# +1 = 关节角增大 = 逆时针。第一步逆时针出孔让出牙，第二步顺时针进孔咬合。
|
||||
steps = (
|
||||
(ccw, "A7 逆时针至极限（出孔）"),
|
||||
(-ccw, "A7 顺时针至极限（进孔）"),
|
||||
)
|
||||
for direction, tag in steps:
|
||||
q, _Tee = self._wait_state(5.0)
|
||||
if q is None:
|
||||
self.get_logger().error(f"{tag}: 没有当前关节")
|
||||
return False
|
||||
target = a7_limit_target(q, limits, direction, margin)
|
||||
if target is None:
|
||||
self.get_logger().error(f"{tag}: 算不出 A7 限位目标")
|
||||
return False
|
||||
if not self._send_a7_only(float(target[6]), speed, tag):
|
||||
return False
|
||||
self.get_logger().info("A7 两步完成：逆时针到限位，再顺时针到限位。法兰高度未改。")
|
||||
return True
|
||||
|
||||
def _on_lock(self, msg: Bool) -> None:
|
||||
if bool(msg.data):
|
||||
self._locked = True
|
||||
@@ -163,33 +375,64 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._kill_old_stage1()
|
||||
|
||||
def _track_z(self) -> float:
|
||||
"""跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
|
||||
return float(self.hover_z) + TRACK_CLEARANCE_M
|
||||
"""一阶段锁高。写了 ``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（跟踪高=hover+50mm）"
|
||||
)
|
||||
_, 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
|
||||
@@ -205,12 +448,17 @@ class AssemblyPipeline(Stage3Search):
|
||||
"enable_control:=true",
|
||||
"publish_handeye_tf:=false",
|
||||
f"ee_z_m:={lock_z:.3f}",
|
||||
"lock_hold_sec:=0.6",
|
||||
f"use_sim_time:={str(use_sim).lower()}",
|
||||
f"d435_move_only:={str(self.d435_move_only).lower()}",
|
||||
]
|
||||
self.get_logger().info(
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
self._s1 = subprocess.Popen(
|
||||
cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
|
||||
)
|
||||
@@ -221,6 +469,11 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._spin(0.1)
|
||||
if self._locked:
|
||||
break
|
||||
fatal = stage1_log_fatal(log_path)
|
||||
if fatal:
|
||||
self._stop_stage1()
|
||||
self.get_logger().error(fatal)
|
||||
return 1
|
||||
if self._s1.poll() is not None:
|
||||
self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
|
||||
return 1
|
||||
@@ -230,7 +483,12 @@ class AssemblyPipeline(Stage3Search):
|
||||
stage1_timeout_reason(self._point_msgs, timeout, log_path)
|
||||
)
|
||||
return 1
|
||||
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
self._stop_stage1()
|
||||
time.sleep(0.35)
|
||||
q = self._q()
|
||||
@@ -242,6 +500,9 @@ class AssemblyPipeline(Stage3Search):
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
if self.d435_move_only:
|
||||
skip2 = True
|
||||
skip3 = True
|
||||
if not self._cli.wait_for_server(timeout_sec=5.0):
|
||||
self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
|
||||
return 2
|
||||
@@ -252,7 +513,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)
|
||||
@@ -267,19 +529,54 @@ class AssemblyPipeline(Stage3Search):
|
||||
if code != 0:
|
||||
self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
|
||||
return code
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
+ (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
|
||||
)
|
||||
stay = hold_at_insert_z(final, skip3=skip3)
|
||||
if stay and final.get("insert_held"):
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — "
|
||||
f"插孔停在法兰 Z={final.get('flange_z_m')}m，"
|
||||
f"规尖离面 {final.get('tip_gap_mm')}mm"
|
||||
f"（目标 {final.get('commanded_gap_mm')}mm，来自 final_gap_mm）。"
|
||||
"没有接触传感器，不另设 Z，不抬回悬高。"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
+ (
|
||||
" — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
|
||||
if skip3
|
||||
else " — 立刻下压搜孔并轻压（不拧入）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
self._state("skip_stage2")
|
||||
stay = hold_at_insert_z(final, skip3=skip3)
|
||||
|
||||
if skip3:
|
||||
if stay:
|
||||
self._state("skip_stage3")
|
||||
self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
elif isinstance(final, dict) and final.get("insert_held"):
|
||||
if q is not None:
|
||||
self._pin(q)
|
||||
self.get_logger().info(
|
||||
"插孔已停在贴面高度，不进搜孔、不抬回悬高。"
|
||||
"接着只转 A7：先逆时针到限位，再顺时针到限位。"
|
||||
)
|
||||
if not self._run_a7_seat():
|
||||
self.get_logger().error(
|
||||
"A7 旋转没走完，臂停在当前关节。不抬 Z。"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
"跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
if final:
|
||||
self._publish_result(final)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=0.8)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=15.0)
|
||||
payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
|
||||
try:
|
||||
payload["stage2"] = json.loads(
|
||||
|
||||
@@ -21,6 +21,11 @@ class PlaneFeature:
|
||||
xyz: np.ndarray # (3,) 相机坐标系下的立体点云质心
|
||||
n_inliers: int
|
||||
note: str
|
||||
# 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。
|
||||
# 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的
|
||||
# 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的
|
||||
# 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。
|
||||
borrowed: bool = False
|
||||
|
||||
|
||||
def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:
|
||||
|
||||
@@ -0,0 +1,479 @@
|
||||
#!/usr/bin/env python3
|
||||
"""YOLO **pose** 模型的推理封装（自解码，不经过 ultralytics 的 pose 后处理）。
|
||||
|
||||
背景：为什么会多出这一个模块
|
||||
--------------------------------
|
||||
`gongjian0929` 这类权重是 ``task=pose``：它输出**一个工件框 + K 个关键点**。
|
||||
本项目 K=3，就是现场说的 k0 / k1 / k2（k0 跟踪、k1/k2 对孔）。
|
||||
|
||||
ultralytics 走不通，两条路都实测过（2026-09-29）：
|
||||
|
||||
1. ``YOLO(engine, task="detect")`` —— 按检测去解 14 通道输出，结果吐出 300 个
|
||||
垃圾框，``conf`` 直接是坐标值（600+），全被算成 ``workpiece`` 类。
|
||||
跟踪会锁到空气上 —— 真机上这比不跑更危险。
|
||||
2. ``YOLO(engine, task="pose")`` —— 直接崩：``negative dimension -983``。
|
||||
根因是 ``scripts/weights.py`` 用**裸 TensorRT API** 构建 engine，engine 不带
|
||||
metadata；ultralytics 拿不到 ``kpt_shape``，后处理就错位了。
|
||||
（TensorRT 10.13 的 Python API 确实没有任何 metadata 接口 —— 见
|
||||
``models/README.md`` 第 3 节。）
|
||||
|
||||
所以这里自己解码。输出布局是 YOLO pose 的固定约定::
|
||||
|
||||
output0 : (1, 4 + nc + 3*K, A) # A = anchor 数（640 输入时是 8400）
|
||||
ch 0..3 : cx, cy, w, h # 输入尺度下的像素
|
||||
ch 4.. : 每类置信度（nc 个）
|
||||
之后 : K 组 (x, y, conf)
|
||||
|
||||
关键点是**同一个工件框的属性**，不是独立目标 —— 所以它们不参与 NMS 的竞争，
|
||||
只跟着自己那个框一起被选出/淘汰。这点和"每个孔一个类"的检测模型不一样。
|
||||
|
||||
后端
|
||||
----
|
||||
- ``.engine``：TensorRT 10 的 ``set_tensor_address`` + ``execute_async_v3``。
|
||||
用 torch 张量当设备缓冲、torch 的当前流当执行流 —— 不需要 pycuda。
|
||||
实测 ~1.5 ms/帧（640 输入，FP16 engine）。
|
||||
- ``.onnx``：onnxruntime。**本环境没有 CUDA provider**（只有 CPU），
|
||||
所以 ONNX 只是兜底，真机请用 engine。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import Any, List, Optional, Sequence, Tuple, Union
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from .yolo_detector import Detection, resolve_class_labels
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# ultralytics 的默认填充值。改了它并不会错 —— 只是输入分布和训练时不一致，
|
||||
# 精度会掉一点，所以别改。
|
||||
LETTERBOX_FILL = 114
|
||||
|
||||
# 关键点语义（现场约定）：k0 是跟踪孔，k1/k2 是对孔。
|
||||
# 默认值写在这里，site.yaml 可以覆盖（见 arm_application/pose_detector 的调用方）。
|
||||
TRACK_KPT_INDEX = 0
|
||||
HOLE_KPT_INDICES: Tuple[int, int] = (1, 2)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 模型信息：engine 没有 metadata，就从旁边同名的 .onnx 读
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _parse_python_literal(raw: Any, default: Any = None) -> Any:
|
||||
"""ONNX metadata 里存的是 python repr（单引号、嵌套 list/dict），要 literal_eval。"""
|
||||
if raw is None:
|
||||
return default
|
||||
if isinstance(raw, (dict, list, tuple, int)):
|
||||
return raw
|
||||
try:
|
||||
return ast.literal_eval(str(raw))
|
||||
except (ValueError, SyntaxError):
|
||||
return default
|
||||
|
||||
|
||||
def read_pose_info(weights: Union[str, Path]) -> Optional[dict]:
|
||||
"""读同名 ``.onnx`` 的 metadata，判断这是不是一个 pose 模型。
|
||||
|
||||
engine 本身读不到任何东西（TensorRT 10 的 Python API 没有 metadata 接口），
|
||||
但 ``.onnx`` 一定在它旁边（``resolve_weights`` 保证同目录同主干），
|
||||
所以统一以 ONNX 为准。
|
||||
|
||||
返回 ``None`` = 不是 pose 模型（调用方应回落到普通检测器）。
|
||||
返回 dict 时至少含 ``kpt_count`` / ``nc`` / ``names``。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
onnx_path = p.with_suffix(".onnx")
|
||||
if not onnx_path.is_file():
|
||||
return None
|
||||
try:
|
||||
import onnx
|
||||
except Exception: # noqa: BLE001
|
||||
logger.warning("没装 onnx，无法判断 %s 是不是 pose 模型", p.name)
|
||||
return None
|
||||
try:
|
||||
meta = {m.key: m.value for m in onnx.load(str(onnx_path)).metadata_props}
|
||||
except Exception as exc: # noqa: BLE001
|
||||
logger.warning("读 %s 的 metadata 失败：%s", onnx_path.name, exc)
|
||||
return None
|
||||
|
||||
task = str(meta.get("task", "")).strip().lower()
|
||||
kpt_shape = _parse_python_literal(meta.get("kpt_shape"))
|
||||
if task != "pose" and not kpt_shape:
|
||||
return None
|
||||
|
||||
kpt_count = 0
|
||||
if isinstance(kpt_shape, (list, tuple)) and len(kpt_shape) >= 1:
|
||||
try:
|
||||
kpt_count = int(kpt_shape[0])
|
||||
except (TypeError, ValueError):
|
||||
kpt_count = 0
|
||||
if kpt_count <= 0:
|
||||
# metadata 缺 kpt_shape 也不能猜 —— 猜错就是"静默错位"，比报错难查得多。
|
||||
logger.warning("%s 标记为 pose 但读不到 kpt_shape，按非 pose 处理", onnx_path.name)
|
||||
return None
|
||||
|
||||
names = _parse_python_literal(meta.get("names"), default={}) or {}
|
||||
kpt_names = _parse_python_literal(meta.get("kpt_names"), default={}) or {}
|
||||
|
||||
nc = 0
|
||||
if isinstance(names, dict) and names:
|
||||
nc = max(int(k) for k in names) + 1 if all(
|
||||
str(k).lstrip("-").isdigit() for k in names
|
||||
) else len(names)
|
||||
if nc <= 0 and isinstance(names, (list, tuple)):
|
||||
nc = len(names)
|
||||
|
||||
return {
|
||||
"task": "pose",
|
||||
"kpt_count": kpt_count,
|
||||
"kpt_names": kpt_names,
|
||||
"nc": nc,
|
||||
"names": names,
|
||||
"model": str(meta.get("description") or p.name),
|
||||
}
|
||||
|
||||
|
||||
def is_pose_model(weights: Union[str, Path]) -> bool:
|
||||
return read_pose_info(weights) is not None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 预处理 / 后处理（纯 numpy，可离线单测）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def letterbox(
|
||||
img: np.ndarray,
|
||||
new_shape: int = 640,
|
||||
color: Tuple[int, int, int] = (LETTERBOX_FILL,) * 3,
|
||||
) -> Tuple[np.ndarray, float, Tuple[float, float]]:
|
||||
"""等比缩放 + 居中填充到 ``new_shape``（和 ultralytics 推理时一致）。
|
||||
|
||||
返回 ``(padded, gain, (dw, dh))``：``dw/dh`` 是**填充量**，还原坐标时减掉它。
|
||||
"""
|
||||
h, w = img.shape[:2]
|
||||
gain = min(new_shape / h, new_shape / w)
|
||||
new_unpad = (max(1, int(round(w * gain))), max(1, int(round(h * gain))))
|
||||
if (w, h) != new_unpad:
|
||||
img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR)
|
||||
dw = (new_shape - new_unpad[0]) / 2.0
|
||||
dh = (new_shape - new_unpad[1]) / 2.0
|
||||
top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
|
||||
left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
|
||||
if top or bottom or left or right:
|
||||
img = cv2.copyMakeBorder(
|
||||
img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color
|
||||
)
|
||||
return img, float(gain), (float(dw), float(dh))
|
||||
|
||||
|
||||
def nms_xyxy(
|
||||
boxes: np.ndarray,
|
||||
scores: np.ndarray,
|
||||
iou_thr: float,
|
||||
max_det: int = 20,
|
||||
) -> np.ndarray:
|
||||
"""纯 numpy 的 NMS，返回保留下来的下标（按分数从高到低）。"""
|
||||
if boxes.size == 0:
|
||||
return np.zeros((0,), dtype=int)
|
||||
x1, y1, x2, y2 = boxes[:, 0], boxes[:, 1], boxes[:, 2], boxes[:, 3]
|
||||
areas = np.maximum(0.0, x2 - x1) * np.maximum(0.0, y2 - y1)
|
||||
order = scores.argsort()[::-1]
|
||||
keep: List[int] = []
|
||||
while order.size > 0 and len(keep) < int(max_det):
|
||||
i = int(order[0])
|
||||
keep.append(i)
|
||||
if order.size == 1:
|
||||
break
|
||||
rest = order[1:]
|
||||
ix1 = np.maximum(x1[i], x1[rest])
|
||||
iy1 = np.maximum(y1[i], y1[rest])
|
||||
ix2 = np.minimum(x2[i], x2[rest])
|
||||
iy2 = np.minimum(y2[i], y2[rest])
|
||||
inter = np.maximum(0.0, ix2 - ix1) * np.maximum(0.0, iy2 - iy1)
|
||||
union = areas[i] + areas[rest] - inter
|
||||
iou = np.where(union > 1e-9, inter / np.maximum(union, 1e-9), 0.0)
|
||||
order = rest[iou <= float(iou_thr)]
|
||||
return np.asarray(keep, dtype=int)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 后端：TensorRT engine / ONNX Runtime
|
||||
# --------------------------------------------------------------------------- #
|
||||
class _TrtRunner:
|
||||
"""TensorRT 10 执行器：torch 张量当设备缓冲，torch 当前流当执行流。
|
||||
|
||||
不用 pycuda —— 环境里只有 torch，而 ``set_tensor_address`` +
|
||||
``execute_async_v3`` 正好够用。
|
||||
"""
|
||||
|
||||
def __init__(self, engine_path: Path, device: str = "0") -> None:
|
||||
import tensorrt as trt
|
||||
import torch
|
||||
|
||||
self._torch = torch
|
||||
idx = 0
|
||||
dev = str(device or "").strip().lower()
|
||||
if dev and dev not in ("auto", "cuda", "gpu"):
|
||||
tail = dev.split(":")[-1]
|
||||
if tail.isdigit():
|
||||
idx = int(tail)
|
||||
if not torch.cuda.is_available():
|
||||
raise RuntimeError("backend=tensorrt 但 torch 看不到 CUDA")
|
||||
self._device = torch.device(f"cuda:{idx}")
|
||||
|
||||
trt_logger = trt.Logger(trt.Logger.ERROR)
|
||||
with open(engine_path, "rb") as f:
|
||||
engine = trt.Runtime(trt_logger).deserialize_cuda_engine(f.read())
|
||||
if engine is None:
|
||||
raise RuntimeError(
|
||||
f"engine 反序列化失败：{engine_path}。"
|
||||
"engine 和 TensorRT 版本绑死，重装环境后要重新导出"
|
||||
"（bash scripts/weights.sh doctor --check-engines 可查）"
|
||||
)
|
||||
self._engine = engine
|
||||
self._ctx = engine.create_execution_context()
|
||||
|
||||
names = [engine.get_tensor_name(i) for i in range(engine.num_io_tensors)]
|
||||
ins = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.INPUT]
|
||||
outs = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.OUTPUT]
|
||||
if len(ins) != 1 or len(outs) != 1:
|
||||
raise RuntimeError(f"pose engine 期望 1 进 1 出，实际 in={ins} out={outs}")
|
||||
self.in_name, self.out_name = ins[0], outs[0]
|
||||
|
||||
with torch.cuda.device(self._device):
|
||||
self._x = torch.zeros(
|
||||
tuple(engine.get_tensor_shape(self.in_name)),
|
||||
dtype=torch.float32,
|
||||
device=self._device,
|
||||
)
|
||||
self._y = torch.zeros(
|
||||
tuple(engine.get_tensor_shape(self.out_name)),
|
||||
dtype=torch.float32,
|
||||
device=self._device,
|
||||
)
|
||||
self._ctx.set_tensor_address(self.in_name, self._x.data_ptr())
|
||||
self._ctx.set_tensor_address(self.out_name, self._y.data_ptr())
|
||||
self.out_shape = tuple(engine.get_tensor_shape(self.out_name))
|
||||
|
||||
def run(self, chw: np.ndarray) -> np.ndarray:
|
||||
torch = self._torch
|
||||
with torch.cuda.device(self._device):
|
||||
self._x.copy_(torch.from_numpy(np.ascontiguousarray(chw)).to(self._device))
|
||||
self._ctx.execute_async_v3(torch.cuda.current_stream().cuda_stream)
|
||||
torch.cuda.current_stream().synchronize()
|
||||
return self._y.cpu().numpy()
|
||||
|
||||
|
||||
class _OnnxRunner:
|
||||
"""onnxruntime 兜底。本环境只有 CPU provider —— 真机请用 .engine。"""
|
||||
|
||||
def __init__(self, onnx_path: Path) -> None:
|
||||
import onnxruntime as ort
|
||||
|
||||
providers = ort.get_available_providers()
|
||||
if "CUDAExecutionProvider" in providers:
|
||||
chosen = ["CUDAExecutionProvider", "CPUExecutionProvider"]
|
||||
else:
|
||||
logger.warning(
|
||||
"onnxruntime 没有 CUDA provider，%s 会跑在 CPU 上（慢）。"
|
||||
"真机请用同名的 .engine",
|
||||
onnx_path.name,
|
||||
)
|
||||
chosen = ["CPUExecutionProvider"]
|
||||
self._sess = ort.InferenceSession(str(onnx_path), providers=chosen)
|
||||
self.in_name = self._sess.get_inputs()[0].name
|
||||
self.out_shape = tuple(self._sess.get_outputs()[0].shape)
|
||||
|
||||
def run(self, chw: np.ndarray) -> np.ndarray:
|
||||
return self._sess.run(None, {self.in_name: chw[None].astype(np.float32)})[0]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 检测器
|
||||
# --------------------------------------------------------------------------- #
|
||||
class PoseDetector:
|
||||
"""把 pose 模型包装成和 ``YoloDetector`` 一样的 ``detect()/infer()`` 接口。
|
||||
|
||||
多出来的一件事：每个 ``Detection`` 带上 ``kpts``（``(K, 3)`` 的
|
||||
``x, y, conf``，原图像素坐标）。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.25,
|
||||
iou: float = 0.45,
|
||||
imgsz: int = 640,
|
||||
device: str = "0",
|
||||
class_ids: Optional[Sequence[int]] = None,
|
||||
class_names: Optional[Sequence[str]] = None,
|
||||
max_det: int = 20,
|
||||
) -> None:
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"找不到 pose 权重：{path}")
|
||||
|
||||
info = read_pose_info(path) or {}
|
||||
self.kpt_count = int(info.get("kpt_count") or 0)
|
||||
if self.kpt_count <= 0:
|
||||
raise RuntimeError(f"{path.name} 不是可识别的 pose 模型（缺 kpt_shape）")
|
||||
self.weights_path = str(path)
|
||||
self.conf = float(conf)
|
||||
self.iou = float(iou)
|
||||
self.imgsz = int(imgsz)
|
||||
self.max_det = int(max_det)
|
||||
|
||||
if path.suffix.lower() in {".engine", ".trt"}:
|
||||
self._runner = _TrtRunner(path, device)
|
||||
self.backend = "tensorrt"
|
||||
else:
|
||||
self._runner = _OnnxRunner(path)
|
||||
self.backend = "onnx"
|
||||
|
||||
# 类别标签仍然走统一入口（旁挂 .labels.yaml 优先）。
|
||||
# engine 没有 metadata，所以把 ONNX 里的 names 当"模型内嵌"传进去。
|
||||
names = info.get("names") or {}
|
||||
|
||||
class _Meta: # 只为 resolve_class_labels 能读到 ONNX 内嵌标签
|
||||
pass
|
||||
|
||||
meta = _Meta()
|
||||
meta.names = names
|
||||
meta.nc = int(info.get("nc") or 0)
|
||||
self._labels = resolve_class_labels(path, meta)
|
||||
|
||||
self.class_ids = set(int(v) for v in class_ids) if class_ids else None
|
||||
self.class_names = (
|
||||
{str(n).strip().lower() for n in class_names if str(n).strip()}
|
||||
if class_names
|
||||
else None
|
||||
)
|
||||
self._warmed = False
|
||||
|
||||
# -- 对外 ---------------------------------------------------------------
|
||||
def warmup(self, *, half: bool = False) -> None:
|
||||
del half # engine 已是 FP16；ONNX 这边没有 half 的开关
|
||||
if self._warmed:
|
||||
return
|
||||
# 先置位：infer() 开头会检查 _warmed，不置位就会反过来再调 warmup。
|
||||
self._warmed = True
|
||||
try:
|
||||
self.infer(np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8))
|
||||
except Exception:
|
||||
self._warmed = False
|
||||
raise
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: float | None = None,
|
||||
iou: float | None = None,
|
||||
imgsz: int | None = None,
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
"""``detect_holes`` 用的别名接口（xyxy + conf + kpts）。"""
|
||||
if conf is not None:
|
||||
self.conf = float(conf)
|
||||
if iou is not None:
|
||||
self.iou = float(iou)
|
||||
# imgsz 在这里**不能**改：engine 的输入尺寸是编译期定死的。
|
||||
if imgsz is not None and int(imgsz) != self.imgsz:
|
||||
logger.warning(
|
||||
"pose engine 输入固定 %d，忽略调用方的 imgsz=%s", self.imgsz, imgsz
|
||||
)
|
||||
return self.infer(image_bgr)
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
|
||||
del half, seq
|
||||
if image_bgr is None or getattr(image_bgr, "size", 0) == 0:
|
||||
return []
|
||||
if not self._warmed:
|
||||
self.warmup()
|
||||
padded, gain, (dw, dh) = letterbox(image_bgr, self.imgsz)
|
||||
chw = np.ascontiguousarray(
|
||||
padded[:, :, ::-1].transpose(2, 0, 1), dtype=np.float32
|
||||
) / 255.0
|
||||
out = self._runner.run(chw)
|
||||
return self._decode(out, gain, dw, dh)
|
||||
|
||||
# -- 解码 ---------------------------------------------------------------
|
||||
def _decode(
|
||||
self, raw: np.ndarray, gain: float, dw: float, dh: float
|
||||
) -> List[Detection]:
|
||||
o = np.asarray(raw, dtype=np.float32)[0] # (C, A)
|
||||
ch, n_anchor = o.shape
|
||||
k = self.kpt_count
|
||||
nc = int(ch - 4 - 3 * k)
|
||||
if nc < 1:
|
||||
raise RuntimeError(
|
||||
f"pose 输出通道数不对：C={ch}，按 kpt={k} 反推 nc={nc}。"
|
||||
"引擎/onnx 与 kpt_shape 对不上"
|
||||
)
|
||||
|
||||
scores = o[4 : 4 + nc] # (nc, A)
|
||||
cls_ids = scores.argmax(axis=0) # (A,)
|
||||
confs = scores[cls_ids, np.arange(n_anchor)]
|
||||
|
||||
keep = confs >= self.conf
|
||||
if not np.any(keep):
|
||||
return []
|
||||
xywh = o[0:4, keep].T # (N, 4)
|
||||
confs = confs[keep]
|
||||
cls_ids = cls_ids[keep]
|
||||
kpts = o[4 + nc :, keep].reshape(k, 3, -1).transpose(2, 0, 1) # (N, k, 3)
|
||||
|
||||
xyxy = np.empty_like(xywh)
|
||||
xyxy[:, 0] = xywh[:, 0] - 0.5 * xywh[:, 2]
|
||||
xyxy[:, 1] = xywh[:, 1] - 0.5 * xywh[:, 3]
|
||||
xyxy[:, 2] = xywh[:, 0] + 0.5 * xywh[:, 2]
|
||||
xyxy[:, 3] = xywh[:, 1] + 0.5 * xywh[:, 3]
|
||||
|
||||
keep_i = nms_xyxy(xyxy, confs, self.iou, self.max_det)
|
||||
if keep_i.size == 0:
|
||||
return []
|
||||
xyxy, confs, cls_ids, kpts = xyxy[keep_i], confs[keep_i], cls_ids[keep_i], kpts[keep_i]
|
||||
|
||||
# 去 letterbox 的填充，再还原到原图尺度
|
||||
xyxy = xyxy.astype(float)
|
||||
xyxy[:, [0, 2]] = (xyxy[:, [0, 2]] - dw) / gain
|
||||
xyxy[:, [1, 3]] = (xyxy[:, [1, 3]] - dh) / gain
|
||||
kpts = kpts.astype(float)
|
||||
kpts[:, :, 0] = (kpts[:, :, 0] - dw) / gain
|
||||
kpts[:, :, 1] = (kpts[:, :, 1] - dh) / gain
|
||||
|
||||
out: List[Detection] = []
|
||||
for i in range(len(xyxy)):
|
||||
cid = int(cls_ids[i])
|
||||
name = self._labels.get(cid)
|
||||
if self.class_ids is not None and cid not in self.class_ids:
|
||||
continue
|
||||
if self.class_names is not None and name.lower() not in self.class_names:
|
||||
continue
|
||||
out.append(
|
||||
Detection(
|
||||
xyxy=xyxy[i],
|
||||
conf=float(confs[i]),
|
||||
cls_id=cid,
|
||||
cls_name=name,
|
||||
kpts=kpts[i].copy(),
|
||||
)
|
||||
)
|
||||
out.sort(key=lambda d: d.conf, reverse=True)
|
||||
return out
|
||||
|
||||
def close(self) -> None:
|
||||
return None
|
||||
|
||||
|
||||
def keypoint_roles(kpt_count: int) -> Tuple[int, Tuple[int, ...]]:
|
||||
"""返回 ``(跟踪点下标, 孔下标元组)``，供调用方统一口径。
|
||||
|
||||
现场约定：k0 跟踪，k1/k2 对孔。模型关键点少于 3 个时只返回存在的部分。
|
||||
"""
|
||||
track = TRACK_KPT_INDEX if kpt_count > TRACK_KPT_INDEX else -1
|
||||
holes = tuple(i for i in HOLE_KPT_INDICES if i < kpt_count)
|
||||
return track, holes
|
||||
@@ -32,16 +32,21 @@ from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
def _param_as_str(node: Node, name: str, default: str = '') -> str:
|
||||
"""字符串参数，同时兼容命令行传数字的情况（例如序列号）。"""
|
||||
node.declare_parameter(
|
||||
name, default, ParameterDescriptor(dynamic_typing=True)
|
||||
)
|
||||
val = node.get_parameter(name).value
|
||||
if val is None or val == '':
|
||||
return ''
|
||||
return str(val).strip()
|
||||
# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"：
|
||||
# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。
|
||||
# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）：
|
||||
# D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途
|
||||
# `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失）
|
||||
# 之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦
|
||||
# 接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**，
|
||||
# 表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。
|
||||
USB_RESEAT_HINT = (
|
||||
'拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— '
|
||||
'只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。'
|
||||
'看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。'
|
||||
)
|
||||
|
||||
|
||||
def _fill_camera_info(
|
||||
@@ -68,23 +73,17 @@ def _fill_camera_info(
|
||||
|
||||
|
||||
def _site_yaml_path() -> Path:
|
||||
env = str(os.environ.get("SITE_CONFIG") or "").strip()
|
||||
if env:
|
||||
p = Path(env).expanduser()
|
||||
if p.is_file():
|
||||
return p
|
||||
home = Path.home() / "ros2" / "config" / "site.yaml"
|
||||
if home.is_file():
|
||||
return home
|
||||
return Path("/home/cc/ros2/config/site.yaml")
|
||||
from arm_application.site_io import site_path
|
||||
|
||||
return site_path()
|
||||
|
||||
|
||||
class RealsenseColorPub(Node):
|
||||
class RealsenseColorPub(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('realsense_color_pub')
|
||||
self.declare_parameter('camera_name', 'd405') # 话题命名空间
|
||||
self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
|
||||
self._serial = _param_as_str(self, 'camera_serial', '')
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('width', 1280)
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
@@ -177,6 +176,10 @@ class RealsenseColorPub(Node):
|
||||
self._overlay: Optional[list] = None
|
||||
self._overlay_stamp = 0.0
|
||||
self._capture_started = False
|
||||
self._cap_lock = threading.Lock()
|
||||
self._cap_event = threading.Event()
|
||||
self._pending = None
|
||||
self._last_frame_mono = 0.0
|
||||
self._usb_desc = ''
|
||||
self._last_ok = None
|
||||
self._site_yaml = _site_yaml_path()
|
||||
@@ -310,23 +313,26 @@ class RealsenseColorPub(Node):
|
||||
self._info_pub.publish(info)
|
||||
|
||||
if depth_u16 is not None:
|
||||
dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
|
||||
dmsg = Image()
|
||||
dmsg.header.stamp = stamp
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
dmsg.encoding = '16UC1'
|
||||
dmsg.is_bigendian = 0
|
||||
dmsg.step = dw * 2
|
||||
dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
|
||||
self._depth_pub.publish(dmsg)
|
||||
if not self._logged_depth:
|
||||
self._logged_depth = True
|
||||
self.get_logger().info(
|
||||
f'首个对齐深度 {dw}x{dh} → '
|
||||
f'/{self._name}/aligned_depth_to_color/image_raw'
|
||||
)
|
||||
self._publish_depth(depth_u16, stamp)
|
||||
|
||||
def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
|
||||
dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
|
||||
dmsg = Image()
|
||||
dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
dmsg.encoding = '16UC1'
|
||||
dmsg.is_bigendian = 0
|
||||
dmsg.step = dw * 2
|
||||
dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
|
||||
self._depth_pub.publish(dmsg)
|
||||
if not self._logged_depth:
|
||||
self._logged_depth = True
|
||||
self.get_logger().info(
|
||||
f'首个对齐深度 {dw}x{dh} → '
|
||||
f'/{self._name}/aligned_depth_to_color/image_raw'
|
||||
)
|
||||
|
||||
def _start_pipeline(self, reconnect: bool = False) -> None:
|
||||
try:
|
||||
@@ -348,8 +354,8 @@ class RealsenseColorPub(Node):
|
||||
if not devices:
|
||||
raise RuntimeError(
|
||||
'没有连接 RealSense（多次重试后 USB 仍为空）。'
|
||||
'请把 D405 插到**本机**的 USB3（蓝色）口，拔插一次，'
|
||||
'并关闭 realsense-viewer / 192.168.0.108 上占用该相机的采集端。'
|
||||
'拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；'
|
||||
+ USB_RESEAT_HINT
|
||||
)
|
||||
|
||||
found = self._pick_device(devices, rs)
|
||||
@@ -374,9 +380,12 @@ class RealsenseColorPub(Node):
|
||||
f'requested {self._width}x{self._height}@{self._fps}Hz'
|
||||
)
|
||||
if usb2:
|
||||
# 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
|
||||
# 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
|
||||
# （SuperSpeed 没协商上）。
|
||||
self.get_logger().warn(
|
||||
f'{cam_name} 接在 USB {usb_desc} 上 —— 深度/IR 将保持 '
|
||||
'640x480@15。把 D405 插到 USB3（蓝色）口可获得 848@30。'
|
||||
f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 '
|
||||
f'640x480@15Hz。' + USB_RESEAT_HINT
|
||||
)
|
||||
self._width, self._height, self._fps = 640, 480, 15
|
||||
|
||||
@@ -455,7 +464,9 @@ class RealsenseColorPub(Node):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 2, w, h, rs.format.y8, fps
|
||||
)
|
||||
profile = pipe.start(trial)
|
||||
# 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
|
||||
# 场景一动红外对不齐，彩色帧就在这里被丢掉。
|
||||
profile = pipe.start(trial, self._on_sdk_frame)
|
||||
have_depth = with_depth
|
||||
have_infra = infra_mode != 'none'
|
||||
self._infra_mode = infra_mode
|
||||
@@ -501,11 +512,18 @@ class RealsenseColorPub(Node):
|
||||
break
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'D405 没有可用的彩色配置。最后的错误：{last_err}'
|
||||
f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
|
||||
)
|
||||
|
||||
self._pipe = pipe
|
||||
self._profile = profile
|
||||
try:
|
||||
sensor = profile.get_device().first_color_sensor()
|
||||
if sensor.supports(rs.option.frames_queue_size):
|
||||
# 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
|
||||
sensor.set_option(rs.option.frames_queue_size, 1)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self._have_depth = have_depth
|
||||
self._have_infra = have_infra
|
||||
self._logged_depth = False
|
||||
@@ -607,7 +625,7 @@ class RealsenseColorPub(Node):
|
||||
|
||||
if not getattr(self, '_capture_started', False):
|
||||
self._capture_started = True
|
||||
self._thread = threading.Thread(target=self._capture_loop, daemon=True)
|
||||
self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish")
|
||||
self._thread.start()
|
||||
|
||||
def _enable_color_ae_fast(self, profile, rs) -> None:
|
||||
@@ -700,18 +718,60 @@ class RealsenseColorPub(Node):
|
||||
)
|
||||
return False
|
||||
|
||||
def _capture_loop(self) -> None:
|
||||
def _on_sdk_frame(self, frame) -> None:
|
||||
"""SDK 回调。只拷走彩色并握住这一组帧，立刻返回。
|
||||
|
||||
对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取，
|
||||
动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。
|
||||
"""
|
||||
try:
|
||||
frames = frame.as_frameset()
|
||||
color = frames.get_color_frame()
|
||||
if not color:
|
||||
return
|
||||
bgr = np.array(np.asanyarray(color.get_data()), copy=True)
|
||||
frames.keep()
|
||||
with self._cap_lock:
|
||||
self._pending = (bgr, frames)
|
||||
self._last_frame_mono = time.monotonic()
|
||||
self._cap_event.set()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
try:
|
||||
self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _publish_loop(self) -> None:
|
||||
got = False
|
||||
misses = 0
|
||||
while self._running and rclpy.ok():
|
||||
if self._pipe is None:
|
||||
if not self._reconnect():
|
||||
time.sleep(2.0)
|
||||
continue
|
||||
if not self._cap_event.wait(0.2):
|
||||
silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0
|
||||
if silent:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— '
|
||||
'请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
self._last_frame_mono = 0.0
|
||||
got = False
|
||||
self._reconnect()
|
||||
continue
|
||||
self._cap_event.clear()
|
||||
with self._cap_lock:
|
||||
item = self._pending
|
||||
self._pending = None
|
||||
if item is None:
|
||||
continue
|
||||
bgr, frames = item
|
||||
try:
|
||||
t_wait0 = time.monotonic()
|
||||
frames = self._pipe.wait_for_frames(timeout_ms=2000)
|
||||
self._t_wait = time.monotonic() - t_wait0
|
||||
t_pub = time.monotonic()
|
||||
# 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。
|
||||
self._publish_frame(bgr, None)
|
||||
self._t_pub = time.monotonic() - t_pub
|
||||
ir_l = (
|
||||
frames.get_infrared_frame(1)
|
||||
if self._have_infra and self._infra_mode in ('both', 'left')
|
||||
@@ -722,44 +782,45 @@ class RealsenseColorPub(Node):
|
||||
if self._have_infra and self._infra_mode in ('both', 'right')
|
||||
else None
|
||||
)
|
||||
aligned = frames
|
||||
if self._align is not None:
|
||||
t_al = time.monotonic()
|
||||
frames = self._align.process(frames)
|
||||
aligned = self._align.process(frames)
|
||||
self._t_align = time.monotonic() - t_al
|
||||
depth = aligned.get_depth_frame()
|
||||
color = frames.get_color_frame()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
misses += 1
|
||||
text = str(exc)
|
||||
dropped = (
|
||||
'disconnect' in text.lower()
|
||||
or 'ioctl' in text.lower()
|
||||
or 'errno=5' in text.lower()
|
||||
)
|
||||
if dropped or misses >= 4:
|
||||
if dropped:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机数据流中断（USB={usb}）：{text[:120]}。'
|
||||
'正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
got = False
|
||||
misses = 0
|
||||
self._last_frame_mono = 0.0
|
||||
self._reconnect()
|
||||
continue
|
||||
if misses == 1:
|
||||
self.get_logger().warn(f'wait_for_frames：{exc}')
|
||||
else:
|
||||
self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
|
||||
continue
|
||||
if not color:
|
||||
misses += 1
|
||||
continue
|
||||
misses = 0
|
||||
bgr = np.asanyarray(color.get_data())
|
||||
depth = frames.get_depth_frame()
|
||||
depth_u16 = np.asanyarray(depth.get_data()) if depth else None
|
||||
if depth_u16 is not None and self._depth_to_mm != 1.0:
|
||||
# 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
|
||||
depth_u16 = np.clip(
|
||||
depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
|
||||
).astype(np.uint16)
|
||||
if depth_u16 is not None:
|
||||
try:
|
||||
self._publish_depth(depth_u16)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
if not self._running:
|
||||
break
|
||||
self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
|
||||
self._frame_i += 1
|
||||
self._fps_meter(color, depth, depth_u16)
|
||||
# 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
|
||||
@@ -792,7 +853,6 @@ class RealsenseColorPub(Node):
|
||||
)
|
||||
try:
|
||||
t_pub = time.monotonic()
|
||||
self._publish_frame(bgr, depth_u16)
|
||||
if self._ir1_pub is not None and ir_l is not None:
|
||||
self._publish_ir(ir_l, self._ir1_pub)
|
||||
if self._ir2_pub is not None and ir_r is not None:
|
||||
|
||||
@@ -27,6 +27,7 @@ from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.realsense_color_pub import _site_yaml_path
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
|
||||
@@ -56,15 +57,22 @@ def _mono_to_bgr(mono: np.ndarray) -> np.ndarray:
|
||||
return np.ascontiguousarray(np.stack([mono, mono, mono], axis=-1))
|
||||
|
||||
|
||||
class RealsenseView(Node):
|
||||
class RealsenseView(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('realsense_view')
|
||||
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
|
||||
@@ -101,9 +109,10 @@ class RealsenseView(Node):
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray, 'workpiece/detection', self._on_det, det_qos
|
||||
)
|
||||
if self._overlay_detection:
|
||||
self.create_subscription(
|
||||
Float32MultiArray, self._det_topic, self._on_det, det_qos
|
||||
)
|
||||
# stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示
|
||||
self.create_subscription(
|
||||
Image, '/stage2/overlay', self._on_stage2_overlay, qos_profile_sensor_data
|
||||
@@ -124,6 +133,7 @@ class RealsenseView(Node):
|
||||
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）。关闭窗口 = 仅隐藏。'
|
||||
)
|
||||
|
||||
@@ -186,6 +196,9 @@ class RealsenseView(Node):
|
||||
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(
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
"""应用层节点的统一基类：把「参数类型的宽窄」问题收在一处。
|
||||
|
||||
为什么需要这个基类（2026-09-28 真机落地实测，代价是顶置相机起不来）
|
||||
------------------------------------------------------------------
|
||||
现场 `config/site.yaml` 写的是:
|
||||
|
||||
cameras:
|
||||
d435:
|
||||
ae_limit_us: 8000 # ← YAML 整数
|
||||
|
||||
脚本把它当 `-p ae_limit_us:=8000` 传给节点，而节点里是:
|
||||
|
||||
self.declare_parameter('ae_limit_us', 3000.0) # ← double
|
||||
|
||||
rclpy **严格**比对类型，于是节点**一起就死**:
|
||||
|
||||
InvalidParameterTypeException: Trying to set parameter 'ae_limit_us'
|
||||
to '8000' of type 'INTEGER', expecting type 'DOUBLE'
|
||||
|
||||
现场看到的是「顶置 D435 起不来」，而 site.yaml 那一行看起来毫无问题 ——
|
||||
`8000` 和 `8000.0` 在运维眼里是同一个数，在 rclpy 眼里是两种类型。
|
||||
同一族的雷还有：`width: 848.0`（浮点）喂给 int 参数、`emitter_enabled: 1`
|
||||
喂给 bool 参数、`"26610799"` 与 `26610799`（带不带引号）等等。
|
||||
|
||||
**为什么不让现场人员"写对一点"**：那等于要求每个人记住「这个数必须带小数点」，
|
||||
而记错的表现是节点启动即崩、报错里还完全不提 site.yaml。规则要靠人记住的就
|
||||
一定会破（`FRAMEWORK_RULES.md` 的总纲）。
|
||||
|
||||
**为什么不在每个参数上加个 cast**：异常发生在**声明阶段**，
|
||||
`float(self.get_parameter(...).value)` 根本执行不到 —— 看着宽容，其实没救。
|
||||
|
||||
做法
|
||||
----
|
||||
用 ``ParameterDescriptor(dynamic_typing=True)`` 声明：参数类型由**传进来的值**
|
||||
决定，不做匹配检查；取值时由业务代码 cast（`float(...)` / `int(...)`），
|
||||
语义仍然由声明处的默认值表达。于是 ``8000`` / ``8000.0`` / ``"8000"`` 都能用。
|
||||
|
||||
**代价与边界**：动态类型意味着 rclpy 不再替我们拦「类型写错」。
|
||||
本项目接受这个代价，因为这些参数**全部**来自 `config/site.yaml` 这个运维手改的
|
||||
文件（不是程序内部传参），宽容比严格更符合现场；而"名字写错"仍然会被
|
||||
rclpy 拦下来（参数名是静态的，与类型无关）。
|
||||
|
||||
新增节点一律继承 ``TolerantNode``（有守卫测试
|
||||
``test_ros_node.py::test_no_node_subclasses_rclpy_node_directly`` 钉住）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Optional, Sequence, Union
|
||||
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
__all__ = ["TolerantNode", "tolerant_descriptor", "param_as_str"]
|
||||
|
||||
|
||||
def tolerant_descriptor() -> ParameterDescriptor:
|
||||
"""声明用的描述符：类型跟着**传进来的值**走，不做宽窄匹配。"""
|
||||
return ParameterDescriptor(dynamic_typing=True)
|
||||
|
||||
|
||||
def param_as_str(node: Node, name: str, default: str = "") -> str:
|
||||
"""读一个"本质是字符串"的参数，容忍传进来的是数字。
|
||||
|
||||
为什么需要：现场 `site.yaml` 里 `serial_number: 26610799`（不加引号）就是
|
||||
个 YAML 整数，而节点里想要的是字符串。rclpy 同样会因类型不匹配在**声明**
|
||||
阶段抛异常 —— 而字符串参数在运维手上最容易"看着是数字"。
|
||||
|
||||
以前这段逻辑在三个节点里各抄了一份（`dh_pgea_node` / `handeye_teach` /
|
||||
`realsense_color_pub`），相同的东西抄三份迟早歪（`FRAMEWORK_RULES.md` R5.4）。
|
||||
现在只此一处；取不到就返回 `default`。
|
||||
"""
|
||||
node.declare_parameter(name, default, tolerant_descriptor())
|
||||
value = node.get_parameter(name).value
|
||||
if value is None:
|
||||
return default
|
||||
return str(value).strip()
|
||||
|
||||
|
||||
class TolerantNode(Node):
|
||||
"""`rclpy.node.Node` + 「参数类型宽容」。
|
||||
|
||||
只改 `declare_parameter` 一件事：默认描述符换成动态类型。
|
||||
调用方若自己传了 `descriptor`（例如只想放宽某几个），以调用方为准。
|
||||
"""
|
||||
|
||||
def declare_parameter( # type: ignore[override]
|
||||
self,
|
||||
name: str,
|
||||
value: Any = None,
|
||||
descriptor: Optional[ParameterDescriptor] = None,
|
||||
ignore_override: bool = False,
|
||||
) -> Any:
|
||||
if descriptor is None:
|
||||
descriptor = tolerant_descriptor()
|
||||
return super().declare_parameter(name, value, descriptor, ignore_override)
|
||||
|
||||
# 列表版本是 rclpy 的另一个入口，同样要放宽（`workspace_xyz_min: [1, 2, 3]`
|
||||
# 这种"整数字面量的列表"喂给 DOUBLE_ARRAY 一样会崩）。
|
||||
def declare_parameters( # type: ignore[override]
|
||||
self,
|
||||
namespace: str,
|
||||
parameters: Sequence[
|
||||
Union[tuple, Any]
|
||||
],
|
||||
ignore_override: bool = False,
|
||||
) -> Any:
|
||||
fixed = []
|
||||
for item in parameters:
|
||||
if (
|
||||
isinstance(item, tuple)
|
||||
and len(item) == 2
|
||||
and not isinstance(item[1], ParameterDescriptor)
|
||||
):
|
||||
fixed.append((item[0], item[1], tolerant_descriptor()))
|
||||
else:
|
||||
fixed.append(item)
|
||||
return super().declare_parameters(namespace, fixed, ignore_override)
|
||||
@@ -12,6 +12,8 @@ from rclpy.node import Node
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
@@ -25,7 +27,7 @@ DEFAULT_POSITIONS = [
|
||||
]
|
||||
|
||||
|
||||
class RunJointTrajectory(Node):
|
||||
class RunJointTrajectory(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('run_joint_trajectory')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -80,3 +80,137 @@ def ee_frame_default() -> str:
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_flange" if sn else ""
|
||||
|
||||
|
||||
def overhead_camera(
|
||||
site: dict[str, Any], robot: dict[str, Any] | None = None
|
||||
) -> tuple[dict[str, Any], list[str]]:
|
||||
"""手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。
|
||||
|
||||
手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时
|
||||
相机固定在工位外，不随臂动）。
|
||||
|
||||
**为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方——
|
||||
S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的
|
||||
后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。
|
||||
所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。
|
||||
|
||||
收敛规则（确定性的三级）：
|
||||
1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）；
|
||||
2. 都没有 ``arm``（= 双臂共用的那台）时回落到它；
|
||||
3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。
|
||||
"""
|
||||
cams = [
|
||||
cfg
|
||||
for cfg in (site.get("cameras") or {}).values()
|
||||
if isinstance(cfg, dict)
|
||||
and str(cfg.get("role") or "").strip().lower().startswith("overhead")
|
||||
]
|
||||
if not cams:
|
||||
return {}, []
|
||||
arm = str((robot or {}).get("arm") or "").strip()
|
||||
mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
|
||||
hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
|
||||
chosen = dict(hits[0])
|
||||
others = [str(c.get("name") or "?") for c in hits[1:]]
|
||||
return chosen, others
|
||||
|
||||
|
||||
def eye_to_hand_result_name(short: str, camera_name: str) -> str:
|
||||
"""眼在手外结果文件的**文件名**（不含目录）。
|
||||
|
||||
外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用，
|
||||
所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼
|
||||
（launch / export_site_env / S03 找文件都走这一处）。
|
||||
"""
|
||||
short = str(short or "").strip()
|
||||
cam = str(camera_name or "").strip().strip("/")
|
||||
if short and cam:
|
||||
return f"{short}_{cam}_eye_to_hand.yaml"
|
||||
if cam:
|
||||
return f"{cam}_eye_to_hand.yaml"
|
||||
if short:
|
||||
return f"{short}_eye_to_hand.yaml"
|
||||
return "eye_to_hand.yaml"
|
||||
|
||||
|
||||
def resolve_eye_to_hand_file(
|
||||
root: Path,
|
||||
*,
|
||||
explicit: str = "",
|
||||
camera_name: str = "",
|
||||
) -> Path | None:
|
||||
"""找手外相机的手眼文件。没有就返回 None。
|
||||
|
||||
路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在
|
||||
"换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
|
||||
|
||||
候选顺序：
|
||||
1. ``explicit`` —— launch 按 site.yaml 派生的确切路径
|
||||
（``<臂号>_<相机>_eye_to_hand.yaml``）；
|
||||
2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）；
|
||||
3. ``eye_to_hand.yaml`` —— 再兜底。
|
||||
"""
|
||||
candidates: list[Path] = []
|
||||
if explicit:
|
||||
p = Path(explicit).expanduser()
|
||||
candidates.append(p if p.is_absolute() else root / p)
|
||||
cam = str(camera_name or "").strip().strip("/")
|
||||
if cam:
|
||||
candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml")
|
||||
candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml")
|
||||
for cand in candidates:
|
||||
if cand.is_file():
|
||||
return cand
|
||||
return None
|
||||
|
||||
|
||||
TRACK_CAMERA_ROLES = ("wrist", "overhead")
|
||||
|
||||
|
||||
def track_camera_role(site: dict[str, Any] | None) -> str:
|
||||
"""一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。
|
||||
|
||||
写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。
|
||||
缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。
|
||||
"""
|
||||
perc = (site or {}).get("perception")
|
||||
perc = perc if isinstance(perc, dict) else {}
|
||||
raw = perc.get("track_camera")
|
||||
if raw is None or str(raw).strip() == "":
|
||||
return "wrist"
|
||||
role = str(raw).strip().lower()
|
||||
if role not in TRACK_CAMERA_ROLES:
|
||||
raise ValueError(
|
||||
f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。"
|
||||
"写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。"
|
||||
)
|
||||
return role
|
||||
|
||||
|
||||
def track_handeye_mode(role: str) -> str:
|
||||
"""跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。"""
|
||||
if str(role or "").strip().lower() == "overhead":
|
||||
return "eye_to_hand"
|
||||
return "eye_in_hand"
|
||||
|
||||
|
||||
def handeye_static_tf_frames(
|
||||
*,
|
||||
mode: str,
|
||||
ee_frame: str,
|
||||
base_frame: str,
|
||||
cam_frame: str,
|
||||
) -> tuple[str, str]:
|
||||
"""手眼静态 TF 的 ``(父系, 子系)``。
|
||||
|
||||
两种装法的父子关系**不同**，不能混：
|
||||
* ``eye_in_hand``：法兰 ← 相机（相机跟着臂动）
|
||||
* ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动）
|
||||
|
||||
把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF ——
|
||||
定位总能到附近、永远差一截，而人只会去怀疑标定精度。
|
||||
"""
|
||||
if str(mode or "").strip().lower() == "eye_to_hand":
|
||||
return str(base_frame), str(cam_frame)
|
||||
return str(ee_frame), str(cam_frame)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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]]:
|
||||
@@ -514,7 +526,8 @@ class Stage3Search(Stage2Locate):
|
||||
tgt[2] += dz_m
|
||||
self._goto(q, Tee, tgt, verticalize=True, tol_m=0.001,
|
||||
tol_tilt=self.vert_tol_rad * 3.0, tag="retract",
|
||||
v=0.03, max_hops=3, settle_s=0.2, min_dur=0.6)
|
||||
v=0.03, max_hops=2, settle_s=0.2, min_dur=0.6,
|
||||
hold_xy=True, z_up_only=True)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
@@ -581,9 +594,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}）"
|
||||
|
||||
@@ -22,8 +22,10 @@ from arm_application.site_io import load_site_dict, site_path
|
||||
from arm_msgs.msg import TactileStatus
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
class TactileNode(Node):
|
||||
|
||||
class TactileNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("tactile")
|
||||
self.declare_parameter("publish_hz", 20.0)
|
||||
|
||||
@@ -176,6 +176,51 @@ def staging_dir(root: Union[str, Path]) -> Path:
|
||||
return Path(root) / STAGING_DIRNAME
|
||||
|
||||
|
||||
def infer_group(
|
||||
root: Union[str, Path],
|
||||
source: Union[str, Path],
|
||||
fallback: str = "yolo",
|
||||
) -> str:
|
||||
"""转换产物该落到**哪个分组**：默认沿用源文件所在的分组。
|
||||
|
||||
为什么不能一律落到 ``yolo``（2026-09-28 真机踩到，代价是一次白转）
|
||||
----------------------------------------------------------------
|
||||
现场把 ``models/hole_insert/detect_best_1.onnx`` 转 engine，工具默认
|
||||
``--group yolo``，于是产物落到 ``models/yolo/detect_best_1.engine`` ——
|
||||
而运行时找 engine 的规则是**同级同名**（``yolo_detector.resolve_weights``：
|
||||
``p.with_suffix(".engine")``，即 site.yaml 指的 ``.onnx`` 旁边）。两边
|
||||
一分家，engine 就等于不存在：现象是"启动即报缺 engine"，而文件明明躺着。
|
||||
这正是本项目最忌讳的**静默失效** —— 工具没报错，位置也"合理"。
|
||||
|
||||
规则：
|
||||
- 源文件已在权重根目录下的某个分组里（例如 ``models/hole_insert/x.onnx``）
|
||||
→ 沿用 ``hole_insert``。这样"就地转换"永远落在原地，运行时必然找得到；
|
||||
- 源文件在 ``staging/``、权重根目录之外或根目录正下方 → ``fallback``
|
||||
（默认 ``yolo``）：那才是"新来的模型，还没决定分组"的正常情形。
|
||||
|
||||
Args:
|
||||
root: 权重根目录（``models/``）。
|
||||
source: 输入文件。
|
||||
fallback: 无法沿用时的分组名。
|
||||
|
||||
Returns:
|
||||
分组名（不带路径）。
|
||||
"""
|
||||
r = Path(root)
|
||||
try:
|
||||
rel = Path(source).expanduser().resolve().relative_to(r.resolve())
|
||||
except ValueError:
|
||||
# 不在权重根目录下（/tmp、外部磁盘、staging 之类）→ 没有"原地"可言
|
||||
return fallback
|
||||
if len(rel.parts) < 2:
|
||||
# 直接躺在 models/ 正下方：没有分组信息，用默认分组
|
||||
return fallback
|
||||
group = rel.parts[0]
|
||||
if is_staging(r / group) or group.startswith("."):
|
||||
return fallback
|
||||
return group
|
||||
|
||||
|
||||
def staging_files(root: Union[str, Path]) -> List[Path]:
|
||||
"""staging 里的待处理文件。
|
||||
|
||||
|
||||
@@ -43,6 +43,9 @@ class Detection:
|
||||
conf: float
|
||||
cls_id: int
|
||||
cls_name: str
|
||||
# pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。
|
||||
# 检测模型恒为 None —— 下游必须先判 None，别假设它有。
|
||||
kpts: Optional[np.ndarray] = None
|
||||
|
||||
@property
|
||||
def cx(self) -> float:
|
||||
@@ -301,12 +304,51 @@ class YoloDetector:
|
||||
return seq, dets, (time.monotonic() - t0) * 1000.0
|
||||
|
||||
|
||||
def build_detector(
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
backend: str = "auto",
|
||||
conf: float = 0.25,
|
||||
iou: float = 0.45,
|
||||
device: str = "",
|
||||
imgsz: int = 640,
|
||||
):
|
||||
"""按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。
|
||||
|
||||
为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见
|
||||
``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型
|
||||
是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。
|
||||
|
||||
返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。
|
||||
"""
|
||||
path = Path(weights).expanduser()
|
||||
from .pose_detector import PoseDetector, is_pose_model
|
||||
|
||||
if is_pose_model(path):
|
||||
logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name)
|
||||
return PoseDetector(
|
||||
path,
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
device=device or "0",
|
||||
imgsz=imgsz,
|
||||
)
|
||||
return YoloDetector(
|
||||
path,
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
device=device,
|
||||
imgsz=imgsz,
|
||||
)
|
||||
|
||||
|
||||
def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
|
||||
"""spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。"""
|
||||
"""spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。"""
|
||||
try:
|
||||
path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
|
||||
det = YoloDetector(
|
||||
det = build_detector(
|
||||
path,
|
||||
backend=cfg.get("backend") or "auto",
|
||||
conf=float(cfg.get("conf") or 0.25),
|
||||
iou=float(cfg.get("iou") or 0.45),
|
||||
device=str(cfg.get("device") or "0"),
|
||||
@@ -336,7 +378,13 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
|
||||
dets = det.infer(image, half=bool(cfg.get("half", True)))
|
||||
gpu_ms = (time.monotonic() - t0) * 1000.0
|
||||
packed = [
|
||||
(d.xyxy.astype(np.float32), float(d.conf), int(d.cls_id), str(d.cls_name))
|
||||
(
|
||||
d.xyxy.astype(np.float32),
|
||||
float(d.conf),
|
||||
int(d.cls_id),
|
||||
str(d.cls_name),
|
||||
None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32),
|
||||
)
|
||||
for d in dets
|
||||
]
|
||||
_put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
|
||||
@@ -399,15 +447,20 @@ class YoloInferProcess:
|
||||
def alive(self) -> bool:
|
||||
return bool(self._p is not None and self._p.is_alive())
|
||||
|
||||
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
|
||||
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool:
|
||||
"""把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
|
||||
排空到队列里最新的帧，所以不会跑任何过期数据。"""
|
||||
排空到队列里最新的帧，所以不会跑任何过期数据。
|
||||
|
||||
返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」
|
||||
而空等，把 D405 也卡住。
|
||||
"""
|
||||
if not self.alive:
|
||||
raise RuntimeError("YOLO 工作进程已死亡")
|
||||
try:
|
||||
self._q_in.put((int(seq), image_bgr), timeout=0.010)
|
||||
return True
|
||||
except Exception:
|
||||
pass
|
||||
return False
|
||||
|
||||
def wait_result(self, timeout: float = 0.045):
|
||||
"""返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
|
||||
@@ -444,15 +497,20 @@ class YoloInferProcess:
|
||||
return None
|
||||
seq, packed = int(msg[1]), msg[2]
|
||||
gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
|
||||
dets = [
|
||||
Detection(
|
||||
xyxy=np.asarray(xyxy, dtype=float),
|
||||
conf=float(conf),
|
||||
cls_id=int(cid),
|
||||
cls_name=str(name),
|
||||
dets = []
|
||||
for row in packed:
|
||||
# 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。
|
||||
xyxy, conf, cid, name = row[0], row[1], row[2], row[3]
|
||||
kpts = row[4] if len(row) > 4 else None
|
||||
dets.append(
|
||||
Detection(
|
||||
xyxy=np.asarray(xyxy, dtype=float),
|
||||
conf=float(conf),
|
||||
cls_id=int(cid),
|
||||
cls_name=str(name),
|
||||
kpts=None if kpts is None else np.asarray(kpts, dtype=float),
|
||||
)
|
||||
)
|
||||
for xyxy, conf, cid, name in packed
|
||||
]
|
||||
return seq, dets, gpu_ms
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -1 +1 @@
|
||||
../../src/arm_application/setup.py
|
||||
/home/cc/ros2/src/arm_application/setup.py
|
||||
File diff suppressed because it is too large
Load Diff
@@ -22,283 +22,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/class_loader/class_loader/visibility_control.hpp
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/console_bridge/console.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/console_bridge/console_bridge_export.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Cholesky
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Core
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Dense
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Eigen
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Eigenvalues
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Geometry
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Householder
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/IterativeLinearSolvers
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Jacobi
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/LU
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/OrderingMethods
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/QR
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SVD
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Sparse
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseCholesky
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseCore
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseLU
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseQR
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Version
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/LDLT.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/LLT.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArithmeticSequence.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Array.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArrayBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArrayWrapper.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Assign.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/AssignEvaluator.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/BandMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Block.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CommaInitializer.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ConditionEstimator.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CoreEvaluators.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CoreIterators.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseBinaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseNullaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseTernaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseUnaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseUnaryView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseStorage.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DeviceWrapper.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Diagonal.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DiagonalMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DiagonalProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Dot.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/EigenBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Fill.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/FindCoeff.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Fuzzy.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GeneralProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GenericPacketMath.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GlobalFunctions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/IO.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/IndexedView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/InnerProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Inverse.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Map.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MapBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MathFunctions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MathFunctionsImpl.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Matrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MatrixBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NestByValue.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NoAlias.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NumTraits.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PartialReduxEvaluator.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PermutationMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PlainObjectBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Product.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ProductEvaluators.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Random.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/RandomImpl.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/RealView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Redux.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Ref.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Replicate.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Reshaped.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ReturnByValue.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Reverse.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Select.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SelfAdjointView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SelfCwiseBinaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SkewSymmetricMatrix3.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Solve.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SolveTriangular.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SolverBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/StableNorm.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/StlIterators.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Stride.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Swap.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Transpose.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Transpositions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/TriangularMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/VectorBlock.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/VectorwiseOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Visitor.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/BFloat16.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/ConjHelper.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/Half.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/Settings.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/Complex.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/MathFunctions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/Reductions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/TypeCasting.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/AssignmentFunctors.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/BinaryFunctors.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/NullaryFunctors.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/StlFunctors.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/TernaryFunctors.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/UnaryFunctors.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralBlockPanelKernel.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixVector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/Parallelizer.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointMatrixVector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointRank2Update.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularMatrixMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularMatrixVector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularSolverMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularSolverVector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Assert.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/BlasUtil.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Constants.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/DisableStupidWarnings.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/EmulateArray.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ForwardDeclarations.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/IndexedViewHelper.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/IntegralConstant.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/MKL_support.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Macros.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Memory.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Meta.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/MoreMeta.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ReenableStupidWarnings.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ReshapedHelper.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Serializer.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/StaticAssert.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/SymbolicIndex.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/XprHelper.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/ComplexEigenSolver.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/ComplexSchur.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/EigenSolver.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/HessenbergDecomposition.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/RealQZ.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/RealSchur.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/Tridiagonalization.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/AlignedBox.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/AngleAxis.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/EulerAngles.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Homogeneous.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Hyperplane.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/OrthoMethods.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/ParametrizedLine.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Quaternion.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Rotation2D.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/RotationBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Scaling.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Transform.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Translation.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Umeyama.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/arch/Geometry_SIMD.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/BlockHouseholder.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/Householder.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/HouseholderSequence.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Jacobi/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Jacobi/Jacobi.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/Determinant.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/FullPivLU.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/InverseImpl.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/PartialPivLU.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/arch/InverseSize4.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Amd.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Eigen_Colamd.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Ordering.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/ColPivHouseholderQR.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/CompleteOrthogonalDecomposition.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/FullPivHouseholderQR.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/HouseholderQR.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/BDCSVD.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/JacobiSVD.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/SVDBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/UpperBidiagonalization.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/SimplicialCholesky.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/AmbiVector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/CompressedStorage.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseAssign.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseBlock.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseColEtree.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCompressedBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDenseProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDiagonalProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDot.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseFuzzy.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMap.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMatrixBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseProduct.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseRedux.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseRef.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSelfAdjointView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSolverBase.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseTranspose.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseTriangularView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseUtil.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseVector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseView.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/TriangularSolver.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLUImpl.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Memory.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Structs.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Utils.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_column_bmod.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_column_dfs.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_panel_bmod.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_panel_dfs.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_pivotL.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_pruneL.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_relax_snode.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseQR/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseQR/SparseQR.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/Image.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/InternalHeaderCheck.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/Kernel.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/RealSvd2x2.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ArrayCwiseBinaryOps.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ArrayCwiseUnaryOps.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/BlockMethods.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/CommonCwiseBinaryOps.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/CommonCwiseUnaryOps.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/IndexedViewMethods.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/MatrixCwiseBinaryOps.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ReshapedMethods.inc
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/handle.hpp
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/hardware_info.hpp
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/macros.hpp
|
||||
@@ -787,12 +510,10 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cctype
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cerrno
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/chrono
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/climits
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/clocale
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cmath
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/codecvt
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/compare
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/complex
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/concepts
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/condition_variable
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/csignal
|
||||
@@ -872,7 +593,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/utility
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/variant
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/vector
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/version
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/atomic_word.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/basic_file.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/c++allocator.h
|
||||
@@ -889,17 +609,11 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/messages_members.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/os_defines.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/time_members.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/emmintrin.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/limits.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/mm_malloc.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/mmintrin.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdalign.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdarg.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdbool.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stddef.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdint.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/syslimits.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/xmmintrin.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/alloca.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/arpa/inet.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/asm-generic/bitsperlong.h
|
||||
@@ -933,15 +647,12 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/iscanonical.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/libc-header-start.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/libm-simd-decl-stubs.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/local_lim.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/locale.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/long-double.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/math-vector.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls-helper-functions.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls-narrow.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix1_lim.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix2_lim.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix_opt.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/pthread_stack_min-dynamic.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/pthreadtypes-arch.h
|
||||
@@ -1014,14 +725,12 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/types/wint_t.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/typesizes.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/uintn-identity.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/uio_lim.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/unistd_ext.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/waitflags.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/waitstatus.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wchar.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wctype-wchar.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wordsize.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/xopen_lim.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/ctype.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/endian.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/errno.h
|
||||
@@ -1031,10 +740,8 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/gnu/stubs.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/ifaddrs.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/libintl.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/limits.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/close_range.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/errno.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/limits.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/posix_types.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/stddef.h
|
||||
/home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/locale.h
|
||||
@@ -1063,7 +770,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
|
||||
/home/cc/flexiv_install/include/flexiv/rdk/data.hpp
|
||||
/home/cc/flexiv_install/include/flexiv/rdk/mode.hpp
|
||||
/home/cc/flexiv_install/include/flexiv/rdk/robot.hpp
|
||||
/home/cc/flexiv_install/include/flexiv/rdk/utility.hpp
|
||||
/home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
|
||||
/home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/visibility_control.h
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
BIN
Binary file not shown.
+7
-7
@@ -237,10 +237,16 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/basic_file.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/c++io.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/fstream.tcc \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/map \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_tree.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/ext/aligned_buffer.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/node_handle.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_map.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multimap.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/erase_if.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/variant \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/enable_special_members.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/monostate.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/ext/aligned_buffer.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/thread \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/std_thread.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/unique_ptr.h \
|
||||
@@ -328,11 +334,8 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/std_function.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/set \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_tree.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/node_handle.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_set.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multiset.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/erase_if.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/unordered_map \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/unordered_map.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/hashtable.h \
|
||||
@@ -344,9 +347,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/list \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_list.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/list.tcc \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/map \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_map.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multimap.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/rcl/rcl/guard_condition.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/rcl/rcl/allocator.h \
|
||||
/home/cc/anaconda3/envs/ros_flexiv/include/rcutils/rcutils/allocator.h \
|
||||
|
||||
@@ -1 +1 @@
|
||||
['-DCMAKE_BUILD_TYPE=Release']
|
||||
['-DAMENT_CMAKE_SYMLINK_INSTALL=1']
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
Reference in New Issue
Block a user