命令挤在一条队列里,上一条没返回后面的就点不了;软拖拽又被位置环顶回去。相机、夹爪、触觉分开走,停止只杀本界面登记的进程组。左指右指挪到画面外面,标题条改成实底,图像不再把字盖住。
验收:test_command_lanes、test_listed_arms、test_camera_preview 共 26 项通过。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -60,7 +60,10 @@ from arm_application.site_io import arm_short_id, listed_arms, load_site_dict
|
|||||||
from arm_application.operator_console import (
|
from arm_application.operator_console import (
|
||||||
_poll_tactile,
|
_poll_tactile,
|
||||||
catalog,
|
catalog,
|
||||||
|
command_blocked_message,
|
||||||
|
command_lane,
|
||||||
run_action,
|
run_action,
|
||||||
|
stop_started,
|
||||||
run_gripper_tune,
|
run_gripper_tune,
|
||||||
status,
|
status,
|
||||||
)
|
)
|
||||||
@@ -298,9 +301,10 @@ class FrameLabel(QLabel):
|
|||||||
self._clip_round(painter)
|
self._clip_round(painter)
|
||||||
painter.fillRect(self.rect(), QColor("#090c11"))
|
painter.fillRect(self.rect(), QColor("#090c11"))
|
||||||
pix = self._scaled
|
pix = self._scaled
|
||||||
|
frame = self._picture_rect()
|
||||||
painter.drawPixmap(
|
painter.drawPixmap(
|
||||||
(self.width() - pix.width()) // 2,
|
frame.x() + (frame.width() - pix.width()) // 2,
|
||||||
(self.height() - pix.height()) // 2,
|
frame.y() + (frame.height() - pix.height()) // 2,
|
||||||
pix,
|
pix,
|
||||||
)
|
)
|
||||||
self._paint_badge(painter)
|
self._paint_badge(painter)
|
||||||
@@ -315,32 +319,50 @@ class FrameLabel(QLabel):
|
|||||||
def _paint_badge(self, painter: QPainter) -> None:
|
def _paint_badge(self, painter: QPainter) -> None:
|
||||||
if not self._badge_title and not self._badge_detail:
|
if not self._badge_title and not self._badge_detail:
|
||||||
return
|
return
|
||||||
bar = 24
|
bar = self._caption_h()
|
||||||
top = max(0, self.height() - bar)
|
top = max(0, self.height() - bar)
|
||||||
painter.fillRect(0, top, self.width(), bar, QColor(8, 10, 14, 176))
|
painter.fillRect(0, top, self.width(), bar, QColor("#0c1016"))
|
||||||
font = QFont(painter.font())
|
font = QFont(painter.font())
|
||||||
font.setPixelSize(12)
|
font.setPixelSize(12)
|
||||||
painter.setFont(font)
|
painter.setFont(font)
|
||||||
painter.setPen(QColor("#f4f7fb"))
|
painter.setPen(QColor("#f4f7fb"))
|
||||||
|
detail = self._badge_detail
|
||||||
|
if detail:
|
||||||
|
title_w = max(0, self.width() // 2 - 14)
|
||||||
|
detail_w = max(0, self.width() // 2 - 14)
|
||||||
|
else:
|
||||||
|
title_w = max(0, self.width() - 16)
|
||||||
|
detail_w = 0
|
||||||
|
title = QFontMetrics(font).elidedText(self._badge_title, Qt.TextElideMode.ElideRight, title_w)
|
||||||
painter.drawText(
|
painter.drawText(
|
||||||
10, top, max(0, self.width() // 2 - 8), bar,
|
8, top, title_w, bar,
|
||||||
int(Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignLeft),
|
int(Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignLeft),
|
||||||
self._badge_title,
|
title,
|
||||||
)
|
)
|
||||||
detail = QFont("DejaVu Sans Mono")
|
if detail_w:
|
||||||
detail.setPixelSize(11)
|
detail_font = QFont("DejaVu Sans Mono")
|
||||||
painter.setFont(detail)
|
detail_font.setPixelSize(11)
|
||||||
|
painter.setFont(detail_font)
|
||||||
painter.setPen(QColor("#d5deea"))
|
painter.setPen(QColor("#d5deea"))
|
||||||
|
shown = QFontMetrics(detail_font).elidedText(detail, Qt.TextElideMode.ElideLeft, detail_w)
|
||||||
painter.drawText(
|
painter.drawText(
|
||||||
self.width() // 2, top, max(0, self.width() // 2 - 10), bar,
|
self.width() // 2, top, detail_w, bar,
|
||||||
int(Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignRight),
|
int(Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignRight),
|
||||||
self._badge_detail,
|
shown,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
def _caption_h(self) -> int:
|
||||||
|
return 24 if (self._badge_title or self._badge_detail) else 0
|
||||||
|
|
||||||
|
def _picture_rect(self) -> QRect:
|
||||||
|
"""画面停在标题条上面。标题条是实底,图像不再从字下面透出来。"""
|
||||||
|
bar = self._caption_h()
|
||||||
|
return QRect(0, 0, max(1, self.width()), max(1, self.height() - bar))
|
||||||
|
|
||||||
def _draw(self, _smooth: bool) -> None:
|
def _draw(self, _smooth: bool) -> None:
|
||||||
if self._busy or self._source.isNull():
|
if self._busy or self._source.isNull():
|
||||||
return
|
return
|
||||||
size = self.contentsRect().size()
|
size = self._picture_rect().size()
|
||||||
if size.width() < 8 or size.height() < 8 or size == self._drawn:
|
if size.width() < 8 or size.height() < 8 or size == self._drawn:
|
||||||
return
|
return
|
||||||
self._busy = True
|
self._busy = True
|
||||||
@@ -1186,7 +1208,7 @@ class StationApp(QMainWindow):
|
|||||||
self._arm = self._catalog[0]["id"] if self._catalog else ""
|
self._arm = self._catalog[0]["id"] if self._catalog else ""
|
||||||
self._mode = "eye_in_hand"
|
self._mode = "eye_in_hand"
|
||||||
self._snap: dict = {}
|
self._snap: dict = {}
|
||||||
self._job: _Job | None = None
|
self._jobs: dict[str, _Job] = {}
|
||||||
self._teach_sig: tuple | None = None
|
self._teach_sig: tuple | None = None
|
||||||
self._pages: dict[str, QWidget] = {}
|
self._pages: dict[str, QWidget] = {}
|
||||||
self._nav: list[tuple[str, QPushButton]] = []
|
self._nav: list[tuple[str, QPushButton]] = []
|
||||||
@@ -1575,21 +1597,28 @@ class StationApp(QMainWindow):
|
|||||||
pics.setSpacing(6)
|
pics.setSpacing(6)
|
||||||
pics.setContentsMargins(0, 0, 0, 0)
|
pics.setContentsMargins(0, 0, 0, 0)
|
||||||
for side_index, (side, name) in enumerate((("left", "左指"), ("right", "右指"))):
|
for side_index, (side, name) in enumerate((("left", "左指"), ("right", "右指"))):
|
||||||
|
head = QLabel(name)
|
||||||
|
head.setObjectName("finger")
|
||||||
|
head.setAlignment(Qt.AlignmentFlag.AlignCenter)
|
||||||
|
head.setFixedHeight(22)
|
||||||
|
pics.addWidget(head, 0, side_index * 2, 1, 2)
|
||||||
for kind_index, (kind, caption) in enumerate((("shear", "切向"), ("thermal", "热像"))):
|
for kind_index, (kind, caption) in enumerate((("shear", "切向"), ("thermal", "热像"))):
|
||||||
column = side_index * 2 + kind_index
|
column = side_index * 2 + kind_index
|
||||||
shot = FrameLabel("", compact=True)
|
shot = FrameLabel("", compact=True)
|
||||||
shot.set_badge(f"{name}{caption}", "")
|
shot.set_badge(caption, "")
|
||||||
self._pic[f"{side}_{kind}"] = shot
|
self._pic[f"{side}_{kind}"] = shot
|
||||||
pics.addWidget(shot, 0, column)
|
pics.addWidget(shot, 1, column)
|
||||||
force = _fit_text(QLabel("—"))
|
force = _fit_text(QLabel("—"))
|
||||||
force.setObjectName("muted")
|
force.setObjectName("muted")
|
||||||
force.setFixedHeight(22)
|
force.setFixedHeight(22)
|
||||||
force.setAlignment(Qt.AlignmentFlag.AlignCenter)
|
force.setAlignment(Qt.AlignmentFlag.AlignCenter)
|
||||||
self._mini.append(force)
|
self._mini.append(force)
|
||||||
pics.addWidget(force, 1, side_index * 2, 1, 2)
|
pics.addWidget(force, 2, side_index * 2, 1, 2)
|
||||||
for column in range(4):
|
for column in range(4):
|
||||||
pics.setColumnStretch(column, 1)
|
pics.setColumnStretch(column, 1)
|
||||||
pics.setRowStretch(0, 1)
|
pics.setRowStretch(1, 1)
|
||||||
|
pics.setRowMinimumHeight(0, 22)
|
||||||
|
pics.setRowMinimumHeight(2, 22)
|
||||||
tac_box = QWidget()
|
tac_box = QWidget()
|
||||||
tac_box.setLayout(pics)
|
tac_box.setLayout(pics)
|
||||||
tac_box.setMinimumHeight(160)
|
tac_box.setMinimumHeight(160)
|
||||||
@@ -1611,7 +1640,15 @@ class StationApp(QMainWindow):
|
|||||||
ops_lay = QVBoxLayout(ops)
|
ops_lay = QVBoxLayout(ops)
|
||||||
ops_lay.setContentsMargins(8, 6, 8, 8)
|
ops_lay.setContentsMargins(8, 6, 8, 8)
|
||||||
ops_lay.setSpacing(6)
|
ops_lay.setSpacing(6)
|
||||||
ops_lay.addWidget(_section("操作"))
|
head = QHBoxLayout()
|
||||||
|
head.setSpacing(6)
|
||||||
|
head.addWidget(_section("操作"))
|
||||||
|
head.addStretch(1)
|
||||||
|
stop = _btn("停止命令", True)
|
||||||
|
stop.setFixedHeight(36)
|
||||||
|
stop.clicked.connect(lambda: self._start(stop_started, "stop"))
|
||||||
|
head.addWidget(stop)
|
||||||
|
ops_lay.addLayout(head)
|
||||||
grid = QGridLayout()
|
grid = QGridLayout()
|
||||||
grid.setSpacing(6)
|
grid.setSpacing(6)
|
||||||
actions = (
|
actions = (
|
||||||
@@ -1950,7 +1987,8 @@ class StationApp(QMainWindow):
|
|||||||
page = QWidget()
|
page = QWidget()
|
||||||
lay = QVBoxLayout(page)
|
lay = QVBoxLayout(page)
|
||||||
lead = QLabel(
|
lead = QLabel(
|
||||||
"软拖拽占用 RDK,要先停掉控制栈。记录的是当时的 7 个关节角,单位弧度。"
|
"软拖拽会先停下关节轨迹控制器,关掉后再恢复,不再另开一条去抢位置环。"
|
||||||
|
"记录的是当时的 7 个关节角,单位弧度。"
|
||||||
"回到点位会先关掉拖拽,再按下面的关节速度走过去。和 home 复位不是同一条路。"
|
"回到点位会先关掉拖拽,再按下面的关节速度走过去。和 home 复位不是同一条路。"
|
||||||
)
|
)
|
||||||
lead.setWordWrap(True)
|
lead.setWordWrap(True)
|
||||||
@@ -2128,7 +2166,7 @@ class StationApp(QMainWindow):
|
|||||||
return
|
return
|
||||||
captured = dict(numbers)
|
captured = dict(numbers)
|
||||||
arm = self._arm
|
arm = self._arm
|
||||||
self._start(lambda: run_gripper_tune(arm, send_kind, captured))
|
self._start(lambda: run_gripper_tune(arm, send_kind, captured), "grip")
|
||||||
|
|
||||||
def _selected_point(self) -> str:
|
def _selected_point(self) -> str:
|
||||||
item = self._teach_list.currentItem()
|
item = self._teach_list.currentItem()
|
||||||
@@ -2141,14 +2179,14 @@ class StationApp(QMainWindow):
|
|||||||
if not sn:
|
if not sn:
|
||||||
self.statusBar().showMessage("这台臂没有序列号", 4000)
|
self.statusBar().showMessage("这台臂没有序列号", 4000)
|
||||||
return
|
return
|
||||||
self._start(lambda: teach.drag_on(sn))
|
self._start(lambda: teach.drag_on(sn), "teach")
|
||||||
|
|
||||||
def _teach_off(self) -> None:
|
def _teach_off(self) -> None:
|
||||||
sn = self._sn()
|
sn = self._sn()
|
||||||
if not sn:
|
if not sn:
|
||||||
self.statusBar().showMessage("这台臂没有序列号", 4000)
|
self.statusBar().showMessage("这台臂没有序列号", 4000)
|
||||||
return
|
return
|
||||||
self._start(lambda: teach.drag_off(sn))
|
self._start(lambda: teach.drag_off(sn), "teach")
|
||||||
|
|
||||||
def _teach_record(self) -> None:
|
def _teach_record(self) -> None:
|
||||||
sn = self._sn()
|
sn = self._sn()
|
||||||
@@ -2157,7 +2195,7 @@ class StationApp(QMainWindow):
|
|||||||
self.statusBar().showMessage("这台臂没有序列号", 4000)
|
self.statusBar().showMessage("这台臂没有序列号", 4000)
|
||||||
return
|
return
|
||||||
arm = self._arm
|
arm = self._arm
|
||||||
self._start(lambda: teach.record(sn, arm, name))
|
self._start(lambda: teach.record(sn, arm, name), "teach")
|
||||||
|
|
||||||
def _teach_delete(self) -> None:
|
def _teach_delete(self) -> None:
|
||||||
name = self._selected_point()
|
name = self._selected_point()
|
||||||
@@ -2166,7 +2204,7 @@ class StationApp(QMainWindow):
|
|||||||
return
|
return
|
||||||
arm = self._arm
|
arm = self._arm
|
||||||
self._teach_sig = None
|
self._teach_sig = None
|
||||||
self._start(lambda: teach.delete_point(arm, name))
|
self._start(lambda: teach.delete_point(arm, name), "teach")
|
||||||
|
|
||||||
def _teach_goto(self) -> None:
|
def _teach_goto(self) -> None:
|
||||||
sn = self._sn()
|
sn = self._sn()
|
||||||
@@ -2176,7 +2214,7 @@ class StationApp(QMainWindow):
|
|||||||
self.statusBar().showMessage("先选臂和点位", 3000)
|
self.statusBar().showMessage("先选臂和点位", 3000)
|
||||||
return
|
return
|
||||||
arm = self._arm
|
arm = self._arm
|
||||||
self._start(lambda: teach.goto(sn, arm, name, speed))
|
self._start(lambda: teach.goto(sn, arm, name, speed), "teach")
|
||||||
|
|
||||||
def _sn(self) -> str:
|
def _sn(self) -> str:
|
||||||
arms = (self._snap or {}).get("arms") or []
|
arms = (self._snap or {}).get("arms") or []
|
||||||
@@ -2204,22 +2242,25 @@ class StationApp(QMainWindow):
|
|||||||
confirm = bool(spec and spec.get("moves"))
|
confirm = bool(spec and spec.get("moves"))
|
||||||
arm, mode = self._arm, self._mode
|
arm, mode = self._arm, self._mode
|
||||||
extras = {"points": self._he_points.value(), "append": self._he_append.isChecked()}
|
extras = {"points": self._he_points.value(), "append": self._he_append.isChecked()}
|
||||||
self._start(lambda: run_action("handeye_teach", arm, confirm, mode, extras))
|
self._start(lambda: run_action("handeye_teach", arm, confirm, mode, extras), "teach")
|
||||||
|
|
||||||
def _ask(self, action: str) -> None:
|
def _ask(self, action: str) -> None:
|
||||||
"""点下去就下发。动臂仍带 confirm,后台才肯执行,界面不再弹窗。"""
|
"""点下去就下发。动臂仍带 confirm,后台才肯执行,界面不再弹窗。"""
|
||||||
spec = self._spec(action)
|
spec = self._spec(action)
|
||||||
confirm = bool(spec and spec.get("moves"))
|
confirm = bool(spec and spec.get("moves"))
|
||||||
arm, mode = self._arm, self._mode
|
arm, mode = self._arm, self._mode
|
||||||
self._start(lambda: run_action(action, arm, confirm, mode))
|
self._start(lambda: run_action(action, arm, confirm, mode), command_lane(action))
|
||||||
|
|
||||||
def _start(self, fn) -> None:
|
def _start(self, fn, lane: str) -> None:
|
||||||
if self._job is not None and self._job.isRunning():
|
running = {name for name, job in self._jobs.items() if job.isRunning()}
|
||||||
self.statusBar().showMessage("上一个命令还没返回", 3000)
|
blocked = command_blocked_message(running, lane)
|
||||||
|
if blocked:
|
||||||
|
self.statusBar().showMessage(blocked, 3000)
|
||||||
return
|
return
|
||||||
self._job = _Job(fn)
|
job = _Job(fn)
|
||||||
self._job.finished_ok.connect(self._ran)
|
self._jobs[lane] = job
|
||||||
self._job.start()
|
job.finished_ok.connect(self._ran)
|
||||||
|
job.start()
|
||||||
|
|
||||||
def _ran(self, result: dict) -> None:
|
def _ran(self, result: dict) -> None:
|
||||||
text = result.get("message") or ("已发出" if result.get("ok") else "失败")
|
text = result.get("message") or ("已发出" if result.get("ok") else "失败")
|
||||||
@@ -2535,6 +2576,7 @@ QLabel#armLine { font-size: 16px; font-weight: 600; color: #f4f7fb; }
|
|||||||
QLabel#count { color: #f4f7fb; font-weight: 600; }
|
QLabel#count { color: #f4f7fb; font-weight: 600; }
|
||||||
QLabel#cardTitle { color: #8b95a7; font-size: 11px; }
|
QLabel#cardTitle { color: #8b95a7; font-size: 11px; }
|
||||||
QLabel#section { color: #d5dbe6; font-weight: 600; }
|
QLabel#section { color: #d5dbe6; font-weight: 600; }
|
||||||
|
QLabel#finger { color: #f4f7fb; font-weight: 600; background: transparent; }
|
||||||
QFrame#sectionMark { background: #5b9bff; border: none; border-radius: 1px; }
|
QFrame#sectionMark { background: #5b9bff; border: none; border-radius: 1px; }
|
||||||
QLabel#step {
|
QLabel#step {
|
||||||
background: #151a22; color: #9aa3b2; border: 1px solid #2a3240;
|
background: #151a22; color: #9aa3b2; border: 1px solid #2a3240;
|
||||||
|
|||||||
@@ -12,6 +12,7 @@ from __future__ import annotations
|
|||||||
import json
|
import json
|
||||||
import os
|
import os
|
||||||
import re
|
import re
|
||||||
|
import signal
|
||||||
import subprocess
|
import subprocess
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
@@ -36,11 +37,60 @@ else:
|
|||||||
ROOT = Path.home() / "ros2"
|
ROOT = Path.home() / "ros2"
|
||||||
|
|
||||||
LOG_DIR = Path("/tmp/orisys_console")
|
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
|
ASSETS = Path(__file__).resolve().parent
|
||||||
_LOCK = threading.Lock()
|
_LOCK = threading.Lock()
|
||||||
_TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
|
_TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
|
||||||
_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
|
_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
|
||||||
_PROCS: dict[str, int] = {}
|
_PROCS: dict[str, int] = {}
|
||||||
|
_LIVE: dict[str, subprocess.Popen] = {}
|
||||||
|
_PROC_LOCK = threading.Lock()
|
||||||
|
|
||||||
|
|
||||||
def _push_force(side: str, sample: dict) -> None:
|
def _push_force(side: str, sample: dict) -> None:
|
||||||
@@ -529,6 +579,63 @@ def catalog() -> dict:
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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:
|
def _stop_tactile() -> str:
|
||||||
subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
|
subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
|
||||||
subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
|
subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
|
||||||
@@ -625,17 +732,26 @@ def run_action(
|
|||||||
shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
|
shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
|
||||||
try:
|
try:
|
||||||
if spec.get("sync"):
|
if spec.get("sync"):
|
||||||
try:
|
proc = subprocess.Popen(
|
||||||
proc = subprocess.run(
|
|
||||||
["bash", str(script), *args],
|
["bash", str(script), *args],
|
||||||
cwd=str(ROOT),
|
cwd=str(ROOT),
|
||||||
capture_output=True,
|
stdout=subprocess.PIPE,
|
||||||
|
stderr=subprocess.STDOUT,
|
||||||
text=True,
|
text=True,
|
||||||
timeout=float(spec.get("timeout") or 40),
|
start_new_session=True,
|
||||||
)
|
)
|
||||||
|
_remember(action, proc)
|
||||||
|
try:
|
||||||
|
detail, _ = proc.communicate(timeout=float(spec.get("timeout") or 40))
|
||||||
except subprocess.TimeoutExpired:
|
except subprocess.TimeoutExpired:
|
||||||
|
try:
|
||||||
|
os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
|
||||||
|
except OSError:
|
||||||
|
pass
|
||||||
return {"ok": False, "message": "命令超时还没返回", "detail": shown}
|
return {"ok": False, "message": "命令超时还没返回", "detail": shown}
|
||||||
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
|
finally:
|
||||||
|
_forget(action)
|
||||||
|
detail = (detail or "").strip()
|
||||||
log_path.write_text(detail)
|
log_path.write_text(detail)
|
||||||
ok = proc.returncode == 0
|
ok = proc.returncode == 0
|
||||||
return {
|
return {
|
||||||
@@ -652,7 +768,7 @@ def run_action(
|
|||||||
stderr=subprocess.STDOUT,
|
stderr=subprocess.STDOUT,
|
||||||
start_new_session=True,
|
start_new_session=True,
|
||||||
)
|
)
|
||||||
_PROCS[action] = proc.pid
|
_remember(action, proc)
|
||||||
if grip_port:
|
if grip_port:
|
||||||
hold_serial(proc.pid)
|
hold_serial(proc.pid)
|
||||||
if uses_rdk:
|
if uses_rdk:
|
||||||
@@ -725,11 +841,23 @@ def _run_gripper_argv(script, arm: str, force: int, speed: int, cli: list[str],
|
|||||||
"--cli", *cli,
|
"--cli", *cli,
|
||||||
]
|
]
|
||||||
shown = " ".join(argv)
|
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:
|
||||||
proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
|
try:
|
||||||
|
detail, _ = proc.communicate(timeout=90)
|
||||||
except subprocess.TimeoutExpired:
|
except subprocess.TimeoutExpired:
|
||||||
|
try:
|
||||||
|
os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
|
||||||
|
except OSError:
|
||||||
|
pass
|
||||||
return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
|
return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
|
||||||
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
|
finally:
|
||||||
|
_forget(f"gripper_{arm}")
|
||||||
|
detail = (detail or "").strip()
|
||||||
ok = proc.returncode == 0
|
ok = proc.returncode == 0
|
||||||
return {
|
return {
|
||||||
"ok": ok,
|
"ok": ok,
|
||||||
|
|||||||
@@ -8,6 +8,8 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import re
|
||||||
|
import subprocess
|
||||||
import threading
|
import threading
|
||||||
from datetime import datetime, timezone
|
from datetime import datetime, timezone
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
@@ -17,6 +19,8 @@ import yaml
|
|||||||
_LOCK = threading.Lock()
|
_LOCK = threading.Lock()
|
||||||
_BRIDGE = None
|
_BRIDGE = None
|
||||||
_SN = ""
|
_SN = ""
|
||||||
|
_YIELDED = False
|
||||||
|
_ARM_CONTROLLER = "rizon_arm_controller"
|
||||||
|
|
||||||
|
|
||||||
def _file(arm: str) -> Path:
|
def _file(arm: str) -> Path:
|
||||||
@@ -62,19 +66,95 @@ def close() -> None:
|
|||||||
pass
|
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:
|
def drag_on(sn: str) -> dict:
|
||||||
with _LOCK:
|
with _LOCK:
|
||||||
try:
|
try:
|
||||||
|
yielded = _yield_position_stream()
|
||||||
|
if yielded:
|
||||||
|
return {"ok": False, "message": yielded}
|
||||||
bridge = _bridge_for(sn)
|
bridge = _bridge_for(sn)
|
||||||
result = bridge.drag_on(mode="soft")
|
result = bridge.drag_on(mode="soft")
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
return {"ok": False, "message": str(exc)}
|
restore = _restore_position_stream()
|
||||||
|
message = str(exc)
|
||||||
|
if restore:
|
||||||
|
message = f"{message};{restore}"
|
||||||
|
return {"ok": False, "message": message}
|
||||||
if not result.get("ok"):
|
if not result.get("ok"):
|
||||||
return {"ok": False, "message": str(result.get("error") or result)}
|
restore = _restore_position_stream()
|
||||||
return {
|
message = str(result.get("error") or result)
|
||||||
"ok": True,
|
if restore:
|
||||||
"message": "软拖拽已开启。轻推机械臂,再记点。这一步要先停掉控制栈。",
|
message = f"{message};{restore}"
|
||||||
}
|
return {"ok": False, "message": message}
|
||||||
|
note = "软拖拽已开启。轻推机械臂,再记点。"
|
||||||
|
if _YIELDED:
|
||||||
|
note += "关节轨迹控制器已停下,关掉拖拽后会回来。"
|
||||||
|
return {"ok": True, "message": note}
|
||||||
|
|
||||||
|
|
||||||
def drag_off(sn: str) -> dict:
|
def drag_off(sn: str) -> dict:
|
||||||
@@ -83,9 +163,16 @@ def drag_off(sn: str) -> dict:
|
|||||||
bridge = _bridge_for(sn)
|
bridge = _bridge_for(sn)
|
||||||
result = bridge.drag_off()
|
result = bridge.drag_off()
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
return {"ok": False, "message": str(exc)}
|
restore = _restore_position_stream()
|
||||||
|
message = str(exc)
|
||||||
|
if restore:
|
||||||
|
message = f"{message};{restore}"
|
||||||
|
return {"ok": False, "message": message}
|
||||||
if not result.get("ok"):
|
if not result.get("ok"):
|
||||||
return {"ok": False, "message": str(result.get("error") or result)}
|
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": "软拖拽已关闭,臂保持当前位置。"}
|
return {"ok": True, "message": "软拖拽已关闭,臂保持当前位置。"}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,103 @@
|
|||||||
|
"""命令按设备分通道。相机不该等夹爪,软拖拽也不该叠在位置环上。"""
|
||||||
|
|
||||||
|
import subprocess
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from arm_application.operator_console import command_blocked_message, command_lane
|
||||||
|
from arm_application import operator_teach as teach
|
||||||
|
|
||||||
|
|
||||||
|
def test_camera_does_not_wait_for_the_gripper():
|
||||||
|
assert command_lane("camera_d405") == "camera"
|
||||||
|
assert command_lane("gripper_open") == "grip"
|
||||||
|
assert command_lane("tactile_up") == "tactile"
|
||||||
|
assert command_blocked_message({"grip"}, "camera") == ""
|
||||||
|
assert command_blocked_message({"camera"}, "teach") == ""
|
||||||
|
|
||||||
|
|
||||||
|
def test_drag_waits_only_when_this_arm_is_still_moving():
|
||||||
|
assert command_lane("stage_track") == "arm"
|
||||||
|
assert command_lane("handeye_teach") == "teach"
|
||||||
|
assert command_blocked_message({"arm"}, "teach") == "这台臂还在动,软拖拽先不开"
|
||||||
|
assert command_blocked_message({"teach"}, "arm") == "软拖拽占着这台臂,运动先不开"
|
||||||
|
assert command_blocked_message({"teach"}, "teach") == "软拖拽还没返回"
|
||||||
|
assert command_blocked_message(set(), "teach") == ""
|
||||||
|
|
||||||
|
|
||||||
|
def test_stop_is_not_queued_behind_another_command():
|
||||||
|
assert command_lane("stop_commands") == "stop"
|
||||||
|
assert command_blocked_message({"arm", "teach", "grip", "camera"}, "stop") == ""
|
||||||
|
|
||||||
|
|
||||||
|
def test_stop_kills_only_the_recorded_process_group():
|
||||||
|
from arm_application import operator_console as console
|
||||||
|
|
||||||
|
keep = subprocess.Popen(["sleep", "30"], start_new_session=True)
|
||||||
|
victim = subprocess.Popen(["sleep", "30"], start_new_session=True)
|
||||||
|
console._remember("demo", victim)
|
||||||
|
try:
|
||||||
|
result = console.stop_started()
|
||||||
|
assert result["ok"] is True
|
||||||
|
assert "demo" in result["message"]
|
||||||
|
victim.wait(timeout=2)
|
||||||
|
assert keep.poll() is None
|
||||||
|
assert "demo" not in console._LIVE
|
||||||
|
finally:
|
||||||
|
for proc in (keep, victim):
|
||||||
|
if proc.poll() is None:
|
||||||
|
proc.kill()
|
||||||
|
proc.wait(timeout=2)
|
||||||
|
|
||||||
|
|
||||||
|
def test_finger_names_sit_above_the_pictures():
|
||||||
|
import os
|
||||||
|
|
||||||
|
os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
|
||||||
|
pytest.importorskip("PySide6")
|
||||||
|
from PySide6.QtWidgets import QApplication, QLabel, QPushButton
|
||||||
|
|
||||||
|
from arm_application.operator_app import FrameLabel, StationApp
|
||||||
|
|
||||||
|
app = QApplication.instance() or QApplication([])
|
||||||
|
win = StationApp()
|
||||||
|
try:
|
||||||
|
win.resize(1440, 900)
|
||||||
|
win.show()
|
||||||
|
app.processEvents()
|
||||||
|
names = [
|
||||||
|
label for label in win._pages["main"].findChildren(QLabel)
|
||||||
|
if label.text() in ("左指", "右指") and not isinstance(label, FrameLabel)
|
||||||
|
]
|
||||||
|
assert sorted(label.text() for label in names) == ["右指", "左指"]
|
||||||
|
pictures = win._pic
|
||||||
|
assert pictures["left_shear"]._badge_title == "切向"
|
||||||
|
assert pictures["right_thermal"]._badge_title == "热像"
|
||||||
|
for label in names:
|
||||||
|
side = "left" if label.text() == "左指" else "right"
|
||||||
|
shot = pictures[f"{side}_shear"]
|
||||||
|
assert shot.geometry().top() > 0
|
||||||
|
assert label.geometry().bottom() <= shot.geometry().top()
|
||||||
|
shot = pictures["left_shear"]
|
||||||
|
shot.resize(180, 100)
|
||||||
|
assert shot._picture_rect().bottom() <= shot.height() - 24
|
||||||
|
assert any(button.text() == "停止命令" for button in win._pages["main"].findChildren(QPushButton))
|
||||||
|
finally:
|
||||||
|
win.close()
|
||||||
|
app.processEvents()
|
||||||
|
|
||||||
|
|
||||||
|
def test_drag_refuses_before_opening_another_link_when_the_position_loop_stays(monkeypatch):
|
||||||
|
monkeypatch.setattr(
|
||||||
|
"arm_application.operator_arm_state.stack_running",
|
||||||
|
lambda: True,
|
||||||
|
)
|
||||||
|
monkeypatch.setattr(
|
||||||
|
teach,
|
||||||
|
"_switch_arm_controller",
|
||||||
|
lambda _activate: "没能停下关节轨迹控制器:拒绝",
|
||||||
|
)
|
||||||
|
result = teach.drag_on("Rizon4-063989")
|
||||||
|
assert result["ok"] is False
|
||||||
|
assert "位置环还在推" in result["message"]
|
||||||
|
assert teach._BRIDGE is None
|
||||||
Reference in New Issue
Block a user