工位操作改成桌面窗口：夹爪位置、力度、速度和旋转角可以调，阶段能分步或整体跑，并能软拖拽记点后回到该点。相机预览按窗口一次铺满，不再跟着每帧慢慢放大。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -220,6 +220,13 @@ class RdkBridge:
|
||||
return self._last_q
|
||||
return self._last_q
|
||||
|
||||
def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
|
||||
"""按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
|
||||
return self._ask(
|
||||
{'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
|
||||
timeout=180.0,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
try:
|
||||
self._ask({'cmd': 'quit'}, timeout=3.0)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,226 @@
|
||||
"""工位程序里的相机预览。
|
||||
|
||||
只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_STOP = threading.Event()
|
||||
_THREAD: threading.Thread | None = None
|
||||
_JPEG: dict[str, bytes] = {}
|
||||
_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
|
||||
|
||||
|
||||
def targets() -> list[dict]:
|
||||
"""真机腕部相机和手外相机。仿真相机不进预览。"""
|
||||
from arm_application.operator_console import _arms, _site
|
||||
from arm_application.site_io import overhead_camera
|
||||
|
||||
found: list[dict] = []
|
||||
seen: set[str] = set()
|
||||
for arm in _arms():
|
||||
name = str(arm.get("camera") or "").strip().strip("/")
|
||||
if not name or name.startswith("sim") or name in seen:
|
||||
continue
|
||||
seen.add(name)
|
||||
found.append({"id": name, "label": f"{arm.get('label') or name} 腕部"})
|
||||
cam, _others = overhead_camera(_site(), None)
|
||||
name = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if name and not name.startswith("sim") and name not in seen:
|
||||
found.append({"id": name, "label": str((cam or {}).get("label") or "手外相机")})
|
||||
return found
|
||||
|
||||
|
||||
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 _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 _jpeg(img: np.ndarray) -> bytes | None:
|
||||
import cv2
|
||||
|
||||
if img.size == 0:
|
||||
return None
|
||||
view = img
|
||||
if view.shape[1] > 640:
|
||||
scale = 640.0 / float(view.shape[1])
|
||||
view = cv2.resize(view, (640, max(1, int(view.shape[0] * scale))))
|
||||
ok, buf = cv2.imencode(".jpg", view, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
if not ok:
|
||||
return None
|
||||
return buf.tobytes()
|
||||
|
||||
|
||||
def _depth_bgr(depth: np.ndarray) -> np.ndarray:
|
||||
import cv2
|
||||
|
||||
vis = np.zeros((depth.shape[0], depth.shape[1], 3), np.uint8)
|
||||
valid = depth > 0
|
||||
if not bool(valid.any()):
|
||||
return vis
|
||||
values = depth.astype(np.float32)
|
||||
lo, hi = np.percentile(values[valid], [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
gray = (np.clip((values - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
colored[~valid] = 0
|
||||
return colored
|
||||
|
||||
|
||||
def _color_array(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
|
||||
need = int(msg.height) * int(msg.width) * 3
|
||||
if arr.size < need:
|
||||
return None
|
||||
arr = arr[:need].reshape(int(msg.height), int(msg.width), 3)
|
||||
if msg.encoding == "rgb8":
|
||||
arr = arr[:, :, ::-1]
|
||||
return np.ascontiguousarray(arr)
|
||||
|
||||
|
||||
def _depth_array(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
arr = np.frombuffer(bytes(msg.data), dtype=np.uint16)
|
||||
need = int(msg.height) * int(msg.width)
|
||||
if arr.size < need:
|
||||
return None
|
||||
return np.ascontiguousarray(arr[:need].reshape(int(msg.height), int(msg.width)))
|
||||
|
||||
|
||||
def _loop(cams: list[dict]) -> None:
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
latest: dict[str, np.ndarray] = {}
|
||||
lock = threading.Lock()
|
||||
|
||||
class _Node(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("operator_camera_view")
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/color/image_raw",
|
||||
self._color(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/aligned_depth_to_color/image_raw",
|
||||
self._depth(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
|
||||
def _color(self, name: str):
|
||||
def _cb(msg: Image) -> None:
|
||||
arr = _color_array(msg)
|
||||
if arr is None:
|
||||
return
|
||||
with lock:
|
||||
latest[f"{name}_color"] = arr
|
||||
return _cb
|
||||
|
||||
def _depth(self, name: str):
|
||||
def _cb(msg: Image) -> None:
|
||||
arr = _depth_array(msg)
|
||||
if arr is None:
|
||||
return
|
||||
with lock:
|
||||
latest[f"{name}_depth"] = arr
|
||||
return _cb
|
||||
|
||||
rclpy.init()
|
||||
node = _Node()
|
||||
next_encode = 0.0
|
||||
try:
|
||||
while not _STOP.is_set():
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
now = time.monotonic()
|
||||
if now < next_encode:
|
||||
continue
|
||||
next_encode = now + 0.12
|
||||
with lock:
|
||||
snap = dict(latest)
|
||||
latest.clear()
|
||||
frames = {}
|
||||
blobs = {}
|
||||
for key, arr in snap.items():
|
||||
name, _, kind = key.rpartition("_")
|
||||
img = _depth_bgr(arr) if kind == "depth" else arr
|
||||
blob = _jpeg(img)
|
||||
if not blob:
|
||||
continue
|
||||
blobs[key] = blob
|
||||
frames[key] = True
|
||||
if blobs:
|
||||
with _LOCK:
|
||||
_JPEG.update(blobs)
|
||||
_STATE["frames"].update(frames)
|
||||
_STATE["stamp"] = time.time()
|
||||
_STATE["error"] = ""
|
||||
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
|
||||
cams = 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()
|
||||
_THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
|
||||
_THREAD.start()
|
||||
names = "、".join(cam["label"] for cam in cams)
|
||||
return f"预览订了 {names} 的彩色和深度，不含左右红外。"
|
||||
|
||||
|
||||
def stop() -> None:
|
||||
_STOP.set()
|
||||
thread = _THREAD
|
||||
if thread is not None:
|
||||
thread.join(timeout=1.5)
|
||||
@@ -0,0 +1,858 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>ORISYS 通止规工位</title>
|
||||
<style>
|
||||
:root {
|
||||
--bg: #070708;
|
||||
--panel: #101216;
|
||||
--panel-2: #171a20;
|
||||
--line: #2a2e36;
|
||||
--text: #f3f4f6;
|
||||
--muted: #8b929d;
|
||||
--blue: #12315d;
|
||||
--blue-2: #1c4f98;
|
||||
--ok: #3f9d6e;
|
||||
--warn: #c8963e;
|
||||
--bad: #c45c5c;
|
||||
}
|
||||
* { box-sizing: border-box; }
|
||||
html, body { margin: 0; height: 100%; background: var(--bg); color: var(--text); }
|
||||
body {
|
||||
font-family: "Noto Sans SC", "Source Han Sans SC", "PingFang SC", "Segoe UI", sans-serif;
|
||||
display: grid;
|
||||
grid-template-columns: 220px 1fr;
|
||||
grid-template-rows: 72px 1fr;
|
||||
height: 100vh;
|
||||
overflow: hidden;
|
||||
}
|
||||
header {
|
||||
grid-column: 1 / -1;
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 16px;
|
||||
padding: 0 18px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: #0b0c0e;
|
||||
}
|
||||
header img { height: 46px; width: auto; }
|
||||
header .name { display: flex; flex-direction: column; gap: 2px; }
|
||||
header .name b { font-size: 15px; font-weight: 560; letter-spacing: 0.08em; }
|
||||
header .name span { color: var(--muted); font-size: 12px; }
|
||||
header .grow { flex: 1; }
|
||||
.chips { display: flex; gap: 6px; flex-wrap: wrap; }
|
||||
.arms { display: flex; gap: 6px; }
|
||||
.arms button, .btn {
|
||||
border-radius: 8px;
|
||||
border: 1px solid var(--line);
|
||||
background: var(--panel-2);
|
||||
color: var(--text);
|
||||
padding: 8px 12px;
|
||||
cursor: pointer;
|
||||
font-size: 13px;
|
||||
}
|
||||
.arms button.on { background: var(--blue); border-color: #1a4580; }
|
||||
.btn.primary { background: var(--blue-2); border-color: #2a62b0; }
|
||||
.btn.on { background: var(--blue); border-color: #1a4580; }
|
||||
.btn.danger { background: #6e2a2a; border-color: #8a3a3a; }
|
||||
.clock { color: var(--muted); font-variant-numeric: tabular-nums; font-size: 13px; min-width: 72px; text-align: right; }
|
||||
aside {
|
||||
border-right: 1px solid var(--line);
|
||||
background: #0b0c0e;
|
||||
padding: 16px 12px;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 6px;
|
||||
}
|
||||
nav { display: flex; flex-direction: column; gap: 6px; }
|
||||
nav button {
|
||||
display: block;
|
||||
width: 100%;
|
||||
text-align: left;
|
||||
background: transparent;
|
||||
color: var(--text);
|
||||
border: 1px solid transparent;
|
||||
border-radius: 8px;
|
||||
padding: 11px 12px;
|
||||
font-size: 14px;
|
||||
cursor: pointer;
|
||||
}
|
||||
nav button small { display: block; color: var(--muted); font-size: 11px; margin-top: 3px; }
|
||||
nav button.active { background: var(--blue); border-color: #1a4580; }
|
||||
nav button.active small { color: #c5d4ea; }
|
||||
nav button:hover { border-color: var(--line); }
|
||||
.side-note { margin-top: auto; color: var(--muted); font-size: 12px; line-height: 1.55; padding: 0 4px; }
|
||||
main { min-width: 0; min-height: 0; overflow: auto; }
|
||||
section { display: none; height: 100%; padding: 16px 18px; }
|
||||
section.on { display: flex; flex-direction: column; gap: 12px; }
|
||||
h2 { margin: 0; font-size: 18px; font-weight: 560; }
|
||||
.lead { margin: 0; color: var(--muted); font-size: 13px; line-height: 1.55; }
|
||||
.dash {
|
||||
flex: 1;
|
||||
min-height: 0;
|
||||
display: grid;
|
||||
grid-template-columns: 1.35fr 0.95fr;
|
||||
grid-template-rows: minmax(0, 1fr) 168px;
|
||||
gap: 12px;
|
||||
}
|
||||
.devices, .mini-forces { display: grid; gap: 12px; min-height: 0; }
|
||||
.devices { grid-template-columns: 1fr 1fr; grid-template-rows: 1fr 1fr; }
|
||||
.mini-forces { grid-template-rows: 1fr 1fr; }
|
||||
.flow { grid-column: 1 / -1; display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 12px; }
|
||||
.card {
|
||||
background: var(--panel);
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 12px;
|
||||
padding: 14px;
|
||||
min-height: 0;
|
||||
}
|
||||
.card h3 {
|
||||
margin: 0 0 8px;
|
||||
font-size: 12px;
|
||||
color: #9aa3b2;
|
||||
font-weight: 500;
|
||||
letter-spacing: 0.12em;
|
||||
}
|
||||
.stat { font-size: 32px; font-variant-numeric: tabular-nums; letter-spacing: 0.02em; }
|
||||
.sub { color: var(--muted); font-size: 12px; margin-top: 6px; line-height: 1.5; white-space: pre-line; }
|
||||
.pill {
|
||||
display: inline-block;
|
||||
border-radius: 999px;
|
||||
padding: 2px 8px;
|
||||
font-size: 12px;
|
||||
border: 1px solid var(--line);
|
||||
margin-right: 6px;
|
||||
}
|
||||
.pill.ok { color: var(--ok); border-color: #245c40; }
|
||||
.pill.warn { color: var(--warn); border-color: #6d5424; }
|
||||
.pill.bad { color: var(--bad); border-color: #6d3030; }
|
||||
.split { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; flex: 1; min-height: 0; }
|
||||
.split.three { grid-template-columns: 1fr 1fr 1fr; }
|
||||
.instrument { display: flex; flex-direction: column; gap: 10px; height: 100%; }
|
||||
.readout { display: flex; gap: 18px; }
|
||||
.readout div { min-width: 88px; }
|
||||
.readout em { display: block; color: var(--muted); font-style: normal; font-size: 12px; }
|
||||
.readout strong { font-size: 28px; font-weight: 520; font-variant-numeric: tabular-nums; }
|
||||
.meter { display: grid; grid-template-columns: 52px 1fr 72px; align-items: center; gap: 8px; font-size: 12px; color: var(--muted); }
|
||||
.track { height: 8px; background: #1c212b; border-radius: 99px; overflow: hidden; }
|
||||
.track i { display: block; height: 100%; width: 0; background: linear-gradient(90deg, #12315d, #4c86d4); }
|
||||
.finder {
|
||||
flex: 1;
|
||||
min-height: 180px;
|
||||
border: 1px solid #243044;
|
||||
border-radius: 10px;
|
||||
background:
|
||||
linear-gradient(#4c86d4, #4c86d4) left top / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) left top / 2px 18px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right top / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right top / 2px 18px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) left bottom / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) left bottom / 2px 18px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right bottom / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right bottom / 2px 18px no-repeat,
|
||||
#0c0e12;
|
||||
display: flex;
|
||||
align-items: flex-end;
|
||||
padding: 14px;
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
}
|
||||
.rail { display: grid; grid-template-columns: repeat(6, minmax(0, 1fr)); gap: 8px; }
|
||||
.step { background: var(--panel); border: 1px solid var(--line); border-radius: 10px; padding: 12px; min-height: 78px; }
|
||||
.step b { display: block; color: #7f93b5; font-size: 12px; letter-spacing: 0.08em; }
|
||||
.step span { display: block; margin-top: 8px; font-size: 14px; }
|
||||
.ops { display: grid; grid-template-columns: repeat(3, minmax(0, 1fr)); gap: 12px; }
|
||||
.row { display: flex; gap: 8px; flex-wrap: wrap; }
|
||||
.pair { display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
|
||||
.pair img:not([src]) { visibility: hidden; }
|
||||
.pair img {
|
||||
width: 100%;
|
||||
height: 200px;
|
||||
object-fit: contain;
|
||||
background: #0c0e12;
|
||||
border-radius: 8px;
|
||||
border: 1px solid #243044;
|
||||
}
|
||||
.charts { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; }
|
||||
canvas.chart { width: 100%; height: 210px; background: #0c0e12; border-radius: 8px; display: block; }
|
||||
.legend { display: flex; gap: 14px; font-size: 12px; color: var(--muted); margin-bottom: 6px; }
|
||||
.legend i { display: inline-block; width: 18px; height: 3px; margin-right: 6px; vertical-align: middle; }
|
||||
pre.out {
|
||||
margin: 0;
|
||||
min-height: 140px;
|
||||
max-height: 260px;
|
||||
overflow: auto;
|
||||
background: #0c0e12;
|
||||
border-radius: 8px;
|
||||
padding: 10px;
|
||||
color: #c5d4ea;
|
||||
font-size: 12px;
|
||||
white-space: pre-wrap;
|
||||
}
|
||||
dialog {
|
||||
border: 1px solid var(--line);
|
||||
background: #12141a;
|
||||
color: var(--text);
|
||||
border-radius: 12px;
|
||||
padding: 18px;
|
||||
width: min(480px, calc(100vw - 32px));
|
||||
}
|
||||
dialog::backdrop { background: rgba(0, 0, 0, 0.55); }
|
||||
dialog h3 { margin: 0; }
|
||||
dialog code {
|
||||
display: block;
|
||||
margin: 12px 0;
|
||||
padding: 10px;
|
||||
background: #0b0c0e;
|
||||
border-radius: 8px;
|
||||
color: #c5d4ea;
|
||||
font-size: 12px;
|
||||
white-space: pre-wrap;
|
||||
}
|
||||
.toast {
|
||||
position: fixed; right: 18px; bottom: 18px;
|
||||
background: #151922; border: 1px solid var(--line);
|
||||
border-radius: 10px; padding: 12px 14px; max-width: 420px;
|
||||
display: none;
|
||||
}
|
||||
@media (max-width: 1100px) {
|
||||
body { grid-template-columns: 1fr; grid-template-rows: auto auto 1fr; height: auto; overflow: auto; }
|
||||
header { flex-wrap: wrap; height: auto; padding: 12px; }
|
||||
.dash, .split, .split.three, .ops, .rail, .flow { grid-template-columns: 1fr 1fr; }
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<img src="/logo.png" alt="ORISYS 叠动科技" />
|
||||
<div class="name">
|
||||
<b>通止规工位</b>
|
||||
<span id="arm-line">左臂 89</span>
|
||||
</div>
|
||||
<div class="grow"></div>
|
||||
<div class="chips" id="chips"></div>
|
||||
<div class="arms" id="arms"></div>
|
||||
<div class="clock" id="clock">--:--:--</div>
|
||||
</header>
|
||||
<aside>
|
||||
<nav>
|
||||
<button class="active" data-page="overview">总览<small>设备与流程</small></button>
|
||||
<button data-page="tactile">视触觉<small>切向、热成像、曲线</small></button>
|
||||
<button data-page="handeye">手眼<small>眼在手上 / 手外</small></button>
|
||||
<button data-page="camera">相机<small>D405 与 D435</small></button>
|
||||
<button data-page="gripper">夹爪<small>平行爪与旋转爪</small></button>
|
||||
<button data-page="assembly">装配<small>S00 到 S11</small></button>
|
||||
<button data-page="operate">操作<small>启动与确认</small></button>
|
||||
</nav>
|
||||
<div class="side-note">读的是本机 USB、进程和话题。动臂、张合夹爪都要再确认一次。</div>
|
||||
</aside>
|
||||
<main>
|
||||
<section id="overview" class="on">
|
||||
<div>
|
||||
<h2>现场</h2>
|
||||
<p class="lead" id="arm-lead"></p>
|
||||
</div>
|
||||
<div class="dash">
|
||||
<div class="devices" id="usb-cards"></div>
|
||||
<div class="mini-forces" id="mini-forces"></div>
|
||||
<div class="flow" id="flow"></div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="tactile">
|
||||
<div>
|
||||
<h2>视触觉</h2>
|
||||
<p class="lead">切向力是 SDK 的剪切场箭头，热成像是接触散度的伪彩。下面两条曲线是法向、切向 X、切向 Y，同一时间轴。单位是形变读数，不是牛顿。</p>
|
||||
</div>
|
||||
<div class="row">
|
||||
<button class="btn primary" id="view-on" type="button">开始成像</button>
|
||||
<button class="btn" id="view-off" type="button">停止成像</button>
|
||||
<button class="btn" data-act="tactile_up">启动三力节点</button>
|
||||
<button class="btn" data-act="tactile_down">停止触觉节点</button>
|
||||
</div>
|
||||
<p class="sub" id="view-note"></p>
|
||||
<div class="split" id="views"></div>
|
||||
<div class="charts">
|
||||
<div class="card">
|
||||
<h3>左指</h3>
|
||||
<div class="legend"><span><i style="background:#e8e8e8"></i>法向</span><span><i style="background:#4c86d4"></i>切向 X</span><span><i style="background:#d7a15a"></i>切向 Y</span></div>
|
||||
<canvas class="chart" id="chart-left"></canvas>
|
||||
</div>
|
||||
<div class="card">
|
||||
<h3>右指</h3>
|
||||
<div class="legend"><span><i style="background:#e8e8e8"></i>法向</span><span><i style="background:#4c86d4"></i>切向 X</span><span><i style="background:#d7a15a"></i>切向 Y</span></div>
|
||||
<canvas class="chart" id="chart-right"></canvas>
|
||||
</div>
|
||||
</div>
|
||||
<p class="sub" id="tac-note"></p>
|
||||
</section>
|
||||
|
||||
<section id="handeye">
|
||||
<div>
|
||||
<h2>手眼标定</h2>
|
||||
<p class="lead">眼在手上用腕部 D405 和 A5 板，结果是相机到法兰。眼在手外用顶置 D435 和 A4 板，结果是相机到基座。两份文件不能混用。示教和求解会动臂。</p>
|
||||
</div>
|
||||
<div class="row" id="he-modes">
|
||||
<button class="btn on" type="button" data-mode="eye_in_hand">眼在手上</button>
|
||||
<button class="btn" type="button" data-mode="eye_to_hand">眼在手外</button>
|
||||
</div>
|
||||
<div class="card" id="he-card"></div>
|
||||
<div class="row">
|
||||
<button class="btn" data-act="handeye_show">查看配置</button>
|
||||
<button class="btn danger" data-act="handeye_teach">示教采点</button>
|
||||
<button class="btn" data-act="handeye_merge">合并示教点</button>
|
||||
<button class="btn danger" data-act="handeye_calibrate">求解手眼</button>
|
||||
</div>
|
||||
<pre class="out" id="he-out">查看配置后，脚本输出显示在这里。</pre>
|
||||
</section>
|
||||
|
||||
<section id="camera">
|
||||
<div>
|
||||
<h2>相机</h2>
|
||||
<p class="lead">腕部 D405 是 848×480 @ 30，手外 D435 是 1280×720 @ 30。预览开在桌面窗口，这一页只报链路，不造画面。</p>
|
||||
</div>
|
||||
<div class="split three" id="cams"></div>
|
||||
<div class="row">
|
||||
<button class="btn primary" data-act="camera_d405">本臂 D405 预览</button>
|
||||
<button class="btn" data-act="camera_d435">D435 预览</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="gripper">
|
||||
<div>
|
||||
<h2>夹爪</h2>
|
||||
<p class="lead" id="grip-lead"></p>
|
||||
</div>
|
||||
<div class="split" id="grip-body"></div>
|
||||
</section>
|
||||
|
||||
<section id="assembly">
|
||||
<div>
|
||||
<h2>通止规装配</h2>
|
||||
<p class="lead">顺序与状态机一致。干跑只打印计划。真跑会动臂、会拧，而且这台机器上还没跑过完整真机装配。</p>
|
||||
</div>
|
||||
<div class="rail" id="rail"></div>
|
||||
<div class="row">
|
||||
<button class="btn" data-act="gauge_dry">干跑</button>
|
||||
<button class="btn danger" data-act="gauge_run">真跑</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="operate">
|
||||
<div>
|
||||
<h2>操作</h2>
|
||||
<p class="lead" id="op-lead"></p>
|
||||
</div>
|
||||
<div class="ops" id="ops"></div>
|
||||
</section>
|
||||
</main>
|
||||
|
||||
<dialog id="ask">
|
||||
<h3 id="ask-title">确认</h3>
|
||||
<p class="sub" id="ask-text"></p>
|
||||
<code id="ask-cmd"></code>
|
||||
<div class="row">
|
||||
<button class="btn" id="ask-no">取消</button>
|
||||
<button class="btn primary" id="ask-yes">确认执行</button>
|
||||
</div>
|
||||
</dialog>
|
||||
<div class="toast" id="toast"></div>
|
||||
|
||||
<script>
|
||||
const titles = {overview:"总览", tactile:"视触觉", camera:"相机", assembly:"装配", operate:"操作", gripper:"夹爪", handeye:"手眼"};
|
||||
const boot = /*BOOT*/null;
|
||||
let arm = "89";
|
||||
let catalog = (boot && boot.catalog) || {actions:[], stages:[]};
|
||||
let status = (boot && boot.status) || null;
|
||||
let pending = null;
|
||||
let heMode = "eye_in_hand";
|
||||
|
||||
function $(id){ return document.getElementById(id); }
|
||||
function pill(ok, word, warn){
|
||||
const s = document.createElement("span");
|
||||
s.className = "pill " + (ok ? "ok" : warn ? "warn" : "bad");
|
||||
s.textContent = word;
|
||||
return s;
|
||||
}
|
||||
function toast(text){
|
||||
const el = $("toast");
|
||||
el.textContent = text;
|
||||
el.style.display = "block";
|
||||
clearTimeout(toast._t);
|
||||
toast._t = setTimeout(() => { el.style.display = "none"; }, 4200);
|
||||
}
|
||||
async function api(path, body){
|
||||
const res = await fetch(path, body ? {
|
||||
method:"POST", headers:{"Content-Type":"application/json"}, body: JSON.stringify(body)
|
||||
} : undefined);
|
||||
return res.json();
|
||||
}
|
||||
function cmdOf(action){
|
||||
const spec = catalog.actions.find(a => a.id === action);
|
||||
if (!spec) return action;
|
||||
const args = (spec.args || []).map(a => a.replace("{arm}", arm).replace("{mode}", heMode)).join(" ");
|
||||
return "bash scripts/" + ({
|
||||
real_up:"run_real.sh", camera_d405:"run_camera_d405.sh", camera_d435:"run_camera_d435.sh",
|
||||
tactile_up:"run_tactile.sh", gripper_up:"run_gripper.sh", gripper_probe:"run_gripper.sh",
|
||||
gripper_open:"run_gripper.sh", gripper_close:"run_gripper.sh", gripper_rotate:"run_gripper.sh",
|
||||
pipeline:"run_all.sh", gauge_dry:"run_gauge_assembly.sh", gauge_run:"run_gauge_assembly.sh",
|
||||
handeye_show:"run_handeye.sh", handeye_teach:"run_handeye.sh",
|
||||
handeye_merge:"run_handeye.sh", handeye_calibrate:"run_handeye.sh"
|
||||
}[action] || "scripts") + (args ? " " + args : "");
|
||||
}
|
||||
async function fire(action, confirm){
|
||||
const data = await api("/api/action", {action, arm, confirm: !!confirm, mode: heMode});
|
||||
toast(data.message || (data.ok ? "已发出" : "失败"));
|
||||
if (String(action).startsWith("handeye") && $("he-out")) {
|
||||
$("he-out").textContent = data.detail || data.message || "";
|
||||
}
|
||||
}
|
||||
function ask(action){
|
||||
const spec = catalog.actions.find(a => a.id === action);
|
||||
if (action === "gripper_rotate" && arm !== "91"){
|
||||
toast("旋转只对右臂 91");
|
||||
return;
|
||||
}
|
||||
if (spec && spec.moves){
|
||||
pending = action;
|
||||
$("ask-title").textContent = spec.title;
|
||||
$("ask-text").textContent = spec.blurb + " 当前臂 " + arm + "。";
|
||||
$("ask-cmd").textContent = cmdOf(action);
|
||||
$("ask").showModal();
|
||||
return;
|
||||
}
|
||||
fire(action, false);
|
||||
}
|
||||
function svc(id){ return (status?.services || []).find(s => s.id === id); }
|
||||
function meters(card, data){
|
||||
const keys = [["法向", data.fnormal], ["切向 X", data.fshearx], ["切向 Y", data.fsheary]];
|
||||
const peak = Math.max(1, ...keys.map(([,v]) => Math.abs(typeof v === "number" ? v : 0)));
|
||||
keys.forEach(([name, value]) => {
|
||||
const row = document.createElement("div");
|
||||
row.className = "meter";
|
||||
const lab = document.createElement("span");
|
||||
lab.textContent = name;
|
||||
const track = document.createElement("div");
|
||||
track.className = "track";
|
||||
const bar = document.createElement("i");
|
||||
const n = typeof value === "number" ? value : null;
|
||||
bar.style.width = n == null ? "0%" : (Math.abs(n) / peak * 100).toFixed(1) + "%";
|
||||
track.appendChild(bar);
|
||||
const num = document.createElement("span");
|
||||
num.textContent = n == null ? "—" : n.toFixed(1);
|
||||
row.append(lab, track, num);
|
||||
card.appendChild(row);
|
||||
});
|
||||
}
|
||||
function forceCard(title, data, tall){
|
||||
const card = document.createElement("div");
|
||||
card.className = "card instrument";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = title;
|
||||
card.appendChild(h);
|
||||
const has = data && (data.connected !== undefined || data.fnormal !== undefined);
|
||||
if (tall) {
|
||||
const row = document.createElement("div");
|
||||
row.className = "readout";
|
||||
[["法向", data.fnormal], ["切向 X", data.fshearx], ["切向 Y", data.fsheary], ["fps", data.fps]].forEach(([k,v]) => {
|
||||
const d = document.createElement("div");
|
||||
const em = document.createElement("em");
|
||||
em.textContent = k;
|
||||
const st = document.createElement("strong");
|
||||
st.textContent = typeof v === "number" ? (k === "fps" ? v.toFixed(1) : v.toFixed(1)) : "—";
|
||||
d.append(em, st);
|
||||
row.appendChild(d);
|
||||
});
|
||||
card.appendChild(row);
|
||||
}
|
||||
meters(card, data || {});
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = has
|
||||
? ((data.contact ? "有接触" : "无接触") + (typeof data.fps === "number" ? " · " + data.fps.toFixed(1) + " fps" : "") + (data.connected === false ? " · 未连接" : ""))
|
||||
: "话题上还没有这一侧";
|
||||
card.appendChild(sub);
|
||||
return card;
|
||||
}
|
||||
function renderHeader(){
|
||||
const box = $("arms");
|
||||
box.replaceChildren();
|
||||
(status?.arms || [{id:"89", label:"左臂89"}, {id:"91", label:"右臂91"}]).forEach(a => {
|
||||
const b = document.createElement("button");
|
||||
b.textContent = a.label;
|
||||
b.className = a.id === arm ? "on" : "";
|
||||
b.onclick = () => { arm = a.id; render(); };
|
||||
box.appendChild(b);
|
||||
});
|
||||
const chips = $("chips");
|
||||
chips.replaceChildren();
|
||||
(status?.services || []).forEach(s => {
|
||||
chips.appendChild(pill(s.up, s.label + (s.up ? " 在跑" : " 未开")));
|
||||
});
|
||||
const cur = (status?.arms || []).find(a => a.id === arm);
|
||||
$("arm-line").textContent = cur ? cur.label : ("臂 " + arm);
|
||||
$("arm-lead").textContent = cur
|
||||
? cur.label + " · " + (cur.sn || "") + " · 相机 " + (cur.camera || "—") + " · 夹爪 " + (cur.gripper || "—") + (cur.tactile ? " · 本臂有视触觉" : " · 本臂无视触觉")
|
||||
: "";
|
||||
$("op-lead").textContent = "当前臂 " + arm + "。红色按钮会动设备，先弹出命令再执行。";
|
||||
$("grip-lead").textContent = arm === "91"
|
||||
? "右臂 91 是旋转爪 RGI。张开、闭合、转动都要确认。探测只读状态。"
|
||||
: "左臂 89 是平行爪 PGEA，串口以现场为准。张开和闭合要确认。探测只读状态。";
|
||||
}
|
||||
function renderUsb(){
|
||||
const box = $("usb-cards");
|
||||
box.replaceChildren();
|
||||
const ports = status?.grippers || [];
|
||||
const rows = [...(status?.usb || []), {id:"tty", label:"夹爪串口", expect:1, items: ports.map(p => ({port:p, speed:""}))}];
|
||||
rows.forEach(dev => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = dev.label;
|
||||
const n = document.createElement("div");
|
||||
n.className = "stat";
|
||||
n.textContent = dev.id === "tty" ? String(dev.items.length) : (dev.items.length + " / " + dev.expect);
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = dev.items.length
|
||||
? dev.items.map(i => i.speed ? (i.port + " · " + i.speed + " Mbps") : i.port).join("\n")
|
||||
: "未检测到";
|
||||
const ok = dev.id === "tty" ? dev.items.length > 0 : dev.items.length >= dev.expect;
|
||||
const partial = !ok && dev.items.length > 0;
|
||||
card.append(h, n, pill(ok, ok ? "齐" : partial ? "缺一" : "缺失", partial), sub);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
function renderMini(){
|
||||
const box = $("mini-forces");
|
||||
const t = status?.tactile || {};
|
||||
box.replaceChildren(forceCard("左指", t.left, false), forceCard("右指", t.right, false));
|
||||
}
|
||||
function renderFlow(){
|
||||
const box = $("flow");
|
||||
box.replaceChildren();
|
||||
const pipe = svc("pipeline");
|
||||
[["跟踪", "D405 看住工件，锁住悬停"], ["对孔", "拍照找孔，移到孔上方"], ["搜孔轻压", "力闭环入孔，再轻压"]].forEach(([name, desc]) => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = name;
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = desc + (pipe && pipe.up ? "\n进程在跑，具体阶段看终端日志。" : "\n流程未启动。");
|
||||
card.append(h, pill(false, pipe && pipe.up ? "进行中" : "待命", pipe && pipe.up), sub);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
function renderFingers(){
|
||||
const t = status?.tactile || {};
|
||||
const node = svc("tactile");
|
||||
const live = ["left", "right"].some(side => (t[side] || {}).fnormal !== undefined);
|
||||
const note = $("tac-note");
|
||||
if (note) note.textContent = node && !node.up && live && !status?.view?.on
|
||||
? "正式触觉节点没开。曲线若在走，是话题上另一个发布者。"
|
||||
: "";
|
||||
}
|
||||
function camCard(title, desc, dev){
|
||||
const card = document.createElement("div");
|
||||
card.className = "card instrument";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = title;
|
||||
const frame = document.createElement("div");
|
||||
frame.className = "finder";
|
||||
frame.textContent = dev && dev.items.length ? "链路在线 · 预览用下面的按钮挂到桌面" : "未插入";
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
const where = (dev?.items || []).map(i => i.port + " · " + i.speed + " Mbps").join("\n");
|
||||
sub.textContent = desc + (where ? "\n" + where : "");
|
||||
card.append(h, frame, pill(!!(dev && dev.items.length), dev && dev.items.length ? dev.items.length + " 台" : "缺失"), sub);
|
||||
return card;
|
||||
}
|
||||
function renderCams(){
|
||||
const usb = Object.fromEntries((status?.usb || []).map(d => [d.id, d]));
|
||||
const d405 = usb.d405 || {items:[]};
|
||||
const d435 = usb.d435 || {items:[]};
|
||||
const other = d405.items.length >= 2 ? {items: d405.items.slice(1)} : {items: []};
|
||||
$("cams").replaceChildren(
|
||||
camCard(arm === "91" ? "右腕 D405" : "左腕 D405", "848×480 @ 30 · 彩色 + 深度", {items: d405.items.slice(0, 1)}),
|
||||
camCard("另一只腕部", "现场期望两台。缺的那台插上后这里会亮。", other),
|
||||
camCard("手外 D435", "1280×720 @ 30 · 顶视", d435),
|
||||
);
|
||||
}
|
||||
function renderGripper(){
|
||||
const ports = status?.grippers || [];
|
||||
const up = svc("gripper");
|
||||
const box = $("grip-body");
|
||||
box.replaceChildren();
|
||||
[
|
||||
["89", "左臂平行爪", "PGEA-100-20-W", ["gripper_probe", "gripper_up", "gripper_open", "gripper_close"]],
|
||||
["91", "右臂旋转爪", "RGI-100-30-O-S-M1", ["gripper_probe", "gripper_up", "gripper_open", "gripper_close", "gripper_rotate"]],
|
||||
].forEach(([id, title, model, acts]) => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = title;
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = model + "\n串口 " + (ports.join(" ") || "没有 ttyUSB") + (up && up.up ? "\n驱动在跑" : "\n驱动未开");
|
||||
card.append(h, pill(id === arm, id === arm ? "当前臂" : "切换顶栏", id !== arm), sub);
|
||||
const row = document.createElement("div");
|
||||
row.className = "row";
|
||||
acts.forEach(act => {
|
||||
const spec = catalog.actions.find(a => a.id === act);
|
||||
const b = document.createElement("button");
|
||||
b.className = "btn " + (spec && spec.moves ? "danger" : "primary");
|
||||
b.textContent = spec ? spec.title : act;
|
||||
b.onclick = () => { if (arm !== id) arm = id; ask(act); render(); };
|
||||
row.appendChild(b);
|
||||
});
|
||||
card.appendChild(row);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
function renderRail(){
|
||||
const box = $("rail");
|
||||
if (box.childElementCount) return;
|
||||
catalog.stages.forEach(([id, name]) => {
|
||||
const d = document.createElement("div");
|
||||
d.className = "step";
|
||||
const b = document.createElement("b");
|
||||
b.textContent = id;
|
||||
const s = document.createElement("span");
|
||||
s.textContent = name;
|
||||
d.append(b, s);
|
||||
box.appendChild(d);
|
||||
});
|
||||
}
|
||||
function renderOps(){
|
||||
const box = $("ops");
|
||||
box.replaceChildren();
|
||||
const skip = new Set(["gripper_open", "gripper_close", "gripper_rotate", "gripper_probe", "handeye_show", "handeye_teach", "handeye_merge", "handeye_calibrate"]);
|
||||
catalog.actions.filter(spec => !skip.has(spec.id)).forEach(spec => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = spec.title;
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = spec.blurb;
|
||||
const b = document.createElement("button");
|
||||
b.className = "btn " + (spec.moves ? "danger" : "primary");
|
||||
b.textContent = spec.moves ? "确认后执行" : "执行";
|
||||
b.style.marginTop = "10px";
|
||||
b.onclick = () => ask(spec.id);
|
||||
card.append(h, sub, b);
|
||||
box.appendChild(card);
|
||||
});
|
||||
const stop = document.createElement("div");
|
||||
stop.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = "停止视触觉";
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = "关掉触觉节点和占着相机的 worker。";
|
||||
const b = document.createElement("button");
|
||||
b.className = "btn";
|
||||
b.textContent = "停止";
|
||||
b.style.marginTop = "10px";
|
||||
b.onclick = () => ask("tactile_down");
|
||||
stop.append(h, sub, b);
|
||||
box.appendChild(stop);
|
||||
}
|
||||
function shot(side, kind, title){
|
||||
const wrap = document.createElement("div");
|
||||
const h = document.createElement("div");
|
||||
h.className = "sub";
|
||||
h.textContent = title;
|
||||
const img = document.createElement("img");
|
||||
img.alt = "";
|
||||
img.dataset.side = side;
|
||||
img.dataset.kind = kind;
|
||||
const ready = status?.view?.frames?.[side + "_" + kind];
|
||||
if (status?.view?.on && ready) {
|
||||
img.src = "/api/tactile/frame/" + side + "/" + kind + ".jpg?t=" + Math.floor((status.view.stamp || 0) * 5);
|
||||
}
|
||||
wrap.append(h, img);
|
||||
return wrap;
|
||||
}
|
||||
function renderViews(){
|
||||
const box = $("views");
|
||||
if (!box.childElementCount) {
|
||||
[["left", "左指"], ["right", "右指"]].forEach(([side, name]) => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = name;
|
||||
const pair = document.createElement("div");
|
||||
pair.className = "pair";
|
||||
pair.append(shot(side, "shear", "切向力"), shot(side, "thermal", "热成像"));
|
||||
card.append(h, pair);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
const view = status?.view || {};
|
||||
$("view-note").textContent = view.error || view.note || (view.on ? "成像已开，等第一帧。" : "成像没开。点开始成像后这里出切向力和热成像。");
|
||||
}
|
||||
function drawChart(canvas, samples){
|
||||
if (!canvas) return;
|
||||
const dpr = window.devicePixelRatio || 1;
|
||||
const w = canvas.clientWidth || 360;
|
||||
const h = canvas.clientHeight || 210;
|
||||
canvas.width = Math.floor(w * dpr);
|
||||
canvas.height = Math.floor(h * dpr);
|
||||
const ctx = canvas.getContext("2d");
|
||||
ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
|
||||
ctx.clearRect(0, 0, w, h);
|
||||
ctx.fillStyle = "#0c0e12";
|
||||
ctx.fillRect(0, 0, w, h);
|
||||
const rows = (samples || []).filter(p => ["fnormal", "fshearx", "fsheary"].some(k => typeof p[k] === "number"));
|
||||
if (rows.length < 2) {
|
||||
ctx.fillStyle = "#8b929d";
|
||||
ctx.font = "13px sans-serif";
|
||||
ctx.fillText("还没有足够的采样", 16, 28);
|
||||
return;
|
||||
}
|
||||
const keys = [["fnormal", "#e8e8e8"], ["fshearx", "#4c86d4"], ["fsheary", "#d7a15a"]];
|
||||
let lo = Infinity, hi = -Infinity;
|
||||
rows.forEach(p => keys.forEach(([k]) => {
|
||||
if (typeof p[k] === "number") { lo = Math.min(lo, p[k]); hi = Math.max(hi, p[k]); }
|
||||
}));
|
||||
if (!(hi > lo)) { hi = lo + 1; }
|
||||
const pad = (hi - lo) * 0.08;
|
||||
lo -= pad; hi += pad;
|
||||
const t0 = rows[0].t, t1 = rows[rows.length - 1].t;
|
||||
const span = Math.max(t1 - t0, 1e-3);
|
||||
const left = 46, right = 10, top = 12, bottom = 22;
|
||||
ctx.strokeStyle = "#2a2e36";
|
||||
ctx.beginPath();
|
||||
const y0 = top + (hi - 0) / (hi - lo) * (h - top - bottom);
|
||||
if (y0 > top && y0 < h - bottom) {
|
||||
ctx.moveTo(left, y0);
|
||||
ctx.lineTo(w - right, y0);
|
||||
ctx.stroke();
|
||||
}
|
||||
keys.forEach(([k, color]) => {
|
||||
ctx.beginPath();
|
||||
ctx.strokeStyle = color;
|
||||
ctx.lineWidth = 1.6;
|
||||
let started = false;
|
||||
rows.forEach(p => {
|
||||
if (typeof p[k] !== "number") return;
|
||||
const x = left + (p.t - t0) / span * (w - left - right);
|
||||
const y = top + (hi - p[k]) / (hi - lo) * (h - top - bottom);
|
||||
if (!started) { ctx.moveTo(x, y); started = true; }
|
||||
else ctx.lineTo(x, y);
|
||||
});
|
||||
ctx.stroke();
|
||||
});
|
||||
ctx.fillStyle = "#8b929d";
|
||||
ctx.font = "11px sans-serif";
|
||||
ctx.fillText(hi.toFixed(0), 4, top + 10);
|
||||
ctx.fillText(lo.toFixed(0), 4, h - bottom);
|
||||
ctx.fillText((span).toFixed(0) + " 秒", w - 48, h - 6);
|
||||
}
|
||||
function mm(v){
|
||||
if (!Array.isArray(v) || v.length < 3) return "—";
|
||||
return v.slice(0, 3).map(x => (Number(x) * 1000).toFixed(1) + " mm").join(" ");
|
||||
}
|
||||
function renderHandeye(){
|
||||
document.querySelectorAll("#he-modes button").forEach(b => b.classList.toggle("on", b.dataset.mode === heMode));
|
||||
const card = (status?.handeye || {})[arm]?.[heMode];
|
||||
const box = $("he-card");
|
||||
if (!box) return;
|
||||
box.replaceChildren();
|
||||
if (!card) {
|
||||
box.textContent = "没有这一臂的标定记录";
|
||||
return;
|
||||
}
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = heMode === "eye_in_hand" ? "眼在手上" : "眼在手外";
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
const board = card.board || {};
|
||||
sub.textContent = [
|
||||
card.exists ? "已有结果" : "还没有结果文件",
|
||||
card.meaning || "",
|
||||
"文件 " + card.file,
|
||||
"视角 " + (card.views || "—") + " 时间 " + (card.utc || "—"),
|
||||
"相机 " + (card.camera || "—") + " 工具 " + (card.tool || "—"),
|
||||
"平移 X / Y / Z " + mm(card.translation_m),
|
||||
"示教点 " + (card.poses == null ? "没有位姿文件" : card.poses + " 个"),
|
||||
"板 " + (board.cols || "—") + "×" + (board.rows || "—") + " 方格 " + (board.square_m == null ? "—" : board.square_m + " m"),
|
||||
].filter(Boolean).join("\n");
|
||||
box.append(h, pill(!!card.exists, card.exists ? "有结果" : "无结果"), sub);
|
||||
}
|
||||
function render(){
|
||||
renderHeader();
|
||||
renderUsb();
|
||||
renderMini();
|
||||
renderFlow();
|
||||
renderFingers();
|
||||
renderViews();
|
||||
drawChart($("chart-left"), status?.history?.left);
|
||||
drawChart($("chart-right"), status?.history?.right);
|
||||
renderCams();
|
||||
renderGripper();
|
||||
renderHandeye();
|
||||
renderRail();
|
||||
renderOps();
|
||||
}
|
||||
function show(page){
|
||||
if (!titles[page]) page = "overview";
|
||||
document.querySelectorAll("nav button").forEach(b => b.classList.toggle("active", b.dataset.page === page));
|
||||
document.querySelectorAll("main section").forEach(s => s.classList.remove("on"));
|
||||
$(page).classList.add("on");
|
||||
if (location.hash !== "#" + page) history.replaceState(null, "", "#" + page);
|
||||
}
|
||||
document.querySelectorAll("nav button").forEach(btn => { btn.onclick = () => show(btn.dataset.page); });
|
||||
window.addEventListener("hashchange", () => show(location.hash.slice(1)));
|
||||
document.body.addEventListener("click", ev => {
|
||||
const act = ev.target.closest("[data-act]");
|
||||
if (act) ask(act.dataset.act);
|
||||
});
|
||||
document.querySelectorAll("#he-modes button").forEach(btn => {
|
||||
btn.onclick = () => { heMode = btn.dataset.mode; renderHandeye(); };
|
||||
});
|
||||
$("view-on").onclick = async () => {
|
||||
const data = await api("/api/tactile/view", {on: true});
|
||||
toast(data.message || "已请求");
|
||||
};
|
||||
$("view-off").onclick = async () => {
|
||||
const data = await api("/api/tactile/view", {on: false});
|
||||
toast(data.message || "已停止");
|
||||
};
|
||||
setInterval(() => {
|
||||
if (!$("tactile").classList.contains("on") || !status?.view?.on) return;
|
||||
document.querySelectorAll("#views img").forEach(img => {
|
||||
const side = img.dataset.side, kind = img.dataset.kind;
|
||||
if (!status.view.frames?.[side + "_" + kind]) return;
|
||||
img.src = "/api/tactile/frame/" + side + "/" + kind + ".jpg?t=" + Date.now();
|
||||
});
|
||||
}, 200);
|
||||
$("ask-no").onclick = () => $("ask").close();
|
||||
$("ask-yes").onclick = () => { const action = pending; $("ask").close(); if (action) fire(action, true); };
|
||||
async function tick(){
|
||||
try { status = await api("/api/status"); render(); }
|
||||
catch (err) { toast("状态刷新失败"); }
|
||||
paintClock();
|
||||
}
|
||||
function paintClock(){
|
||||
$("clock").textContent = new Date().toLocaleTimeString("zh-CN", {hour12:false});
|
||||
}
|
||||
async function start(){
|
||||
if (!boot) {
|
||||
catalog = await api("/api/catalog");
|
||||
await tick();
|
||||
} else {
|
||||
render();
|
||||
paintClock();
|
||||
}
|
||||
show(location.hash.slice(1) || "overview");
|
||||
setInterval(tick, 2000);
|
||||
}
|
||||
start();
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,725 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Orisys 工位界面。
|
||||
|
||||
只做两件事：把本机真实状态摊开，以及按白名单启动仓库里已有的脚本。
|
||||
会动臂的动作必须带 ``confirm=true``，避免页面上误点一下就上电。
|
||||
|
||||
bash ~/ros2/scripts/run_console.sh
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
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, overhead_camera
|
||||
|
||||
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")
|
||||
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] = {}
|
||||
|
||||
|
||||
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]:
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
out = []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
|
||||
if not arm_no:
|
||||
continue
|
||||
out.append({
|
||||
"id": arm_no,
|
||||
"label": str(robot.get("label") or f"臂{arm_no}"),
|
||||
"sn": sn,
|
||||
"tactile": bool(robot.get("tactile")),
|
||||
"camera": str((robot.get("camera") or {}).get("name") or ""),
|
||||
"gripper": str((robot.get("gripper") or {}).get("model") or ""),
|
||||
})
|
||||
return out or [
|
||||
{"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
|
||||
{"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
|
||||
]
|
||||
|
||||
|
||||
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})
|
||||
return [
|
||||
{"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
|
||||
{"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
|
||||
{"id": "orisys", "label": "视触觉", "expect": 2, "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:
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
|
||||
if short == 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")
|
||||
))
|
||||
return {
|
||||
"eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
|
||||
"eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
|
||||
}
|
||||
|
||||
|
||||
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"]),
|
||||
}
|
||||
return {
|
||||
"arms": _arms(),
|
||||
"usb": _usb(),
|
||||
"services": _services(),
|
||||
"grippers": _grippers(),
|
||||
"tactile": tactile,
|
||||
"history": history,
|
||||
"view": tactile_view.state(),
|
||||
"handeye": {"89": _handeye("89"), "91": _handeye("91")},
|
||||
"now": time.time(),
|
||||
}
|
||||
|
||||
|
||||
# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
|
||||
_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_search": {
|
||||
"title": "只测搜孔",
|
||||
"script": "run_stage3_search.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"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", "12"],
|
||||
"moves": True,
|
||||
"blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
|
||||
},
|
||||
"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 _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 run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> 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 ("89", "91"):
|
||||
return {"ok": False, "message": "臂号只接受 89 或 91"}
|
||||
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}"}
|
||||
args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
|
||||
LOG_DIR.mkdir(parents=True, exist_ok=True)
|
||||
log_path = LOG_DIR / f"{action}_{arm}.log"
|
||||
shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
|
||||
if spec.get("sync"):
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["bash", str(script), *args],
|
||||
cwd=str(ROOT),
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=float(spec.get("timeout") or 40),
|
||||
)
|
||||
except subprocess.TimeoutExpired:
|
||||
return {"ok": False, "message": "命令超时还没返回", "detail": shown}
|
||||
detail = ((proc.stdout or "") + (proc.stderr 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,
|
||||
)
|
||||
_PROCS[action] = proc.pid
|
||||
return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
|
||||
|
||||
|
||||
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 ("89", "91"):
|
||||
return {"ok": False, "message": "臂号只接受 89 或 91"}
|
||||
if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
|
||||
return {"ok": False, "message": "没有这种夹爪调节"}
|
||||
if kind != "pos" and arm != "91":
|
||||
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)}
|
||||
script = ROOT / "scripts" / "run_gripper.sh"
|
||||
argv = [
|
||||
"bash", str(script),
|
||||
"--arm", arm,
|
||||
"--force", str(force),
|
||||
"--speed", str(speed),
|
||||
"--cli", *cli,
|
||||
]
|
||||
shown = " ".join(argv)
|
||||
try:
|
||||
proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
|
||||
except subprocess.TimeoutExpired:
|
||||
return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
|
||||
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
|
||||
ok = proc.returncode == 0
|
||||
return {
|
||||
"ok": ok,
|
||||
"message": summary if ok else f"夹爪退出码 {proc.returncode}",
|
||||
"detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
|
||||
}
|
||||
|
||||
|
||||
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, "89", 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 "89"),
|
||||
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)
|
||||
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()
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 36 KiB |
@@ -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,203 @@
|
||||
"""软拖拽示教：开拖拽、记下当前关节角、再回到该点。
|
||||
|
||||
点位写在 ``handeye/tmp/<臂号>/operator_teach.yaml``。关节角单位是弧度，
|
||||
直接存桥接返回的 7 个数，不另做姿态换算。
|
||||
|
||||
软拖拽占用 RDK，和 ``run_real.sh`` 不能同时连同一台臂。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_BRIDGE = None
|
||||
_SN = ""
|
||||
|
||||
|
||||
def _file(arm: str) -> Path:
|
||||
from arm_application.operator_console import ROOT
|
||||
|
||||
if arm not in ("89", "91"):
|
||||
raise ValueError("臂号只接受 89 或 91")
|
||||
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
|
||||
|
||||
_BRIDGE = RdkBridge(sn)
|
||||
_SN = sn
|
||||
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 drag_on(sn: str) -> dict:
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge.drag_on(mode="soft")
|
||||
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)}
|
||||
return {
|
||||
"ok": True,
|
||||
"message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
|
||||
}
|
||||
|
||||
|
||||
def drag_off(sn: str) -> dict:
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge.drag_off()
|
||||
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)}
|
||||
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}",
|
||||
}
|
||||
Reference in New Issue
Block a user