orisys/orisys_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial import of ORISYS arm workstation

Browse Source
This commit is contained in:
cheng
2026-10-16 17:27:30 +08:00
commit 2b74c50ba8
745 changed files with 1199101 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
station/README.md
+97
View File
@@ -0,0 +1,97 @@
# 工位前端与设备后端
启动：`bash station/run_station.sh`。旧入口 `desk/run_desk.sh` 也转到这里。
离线打开界面：`bash station/run_station.sh --offline`。
回归测试：`bash station/run_station.sh --self-test`。
## 界面
侧边导航只保留“首页”和“标定”，顶部保留“设置”按钮及设置界面。顶部左右臂按钮切换当前臂，悬停显示对应设备 IP。
首页左侧为机器人状态、机器人实时预览和夹爪 3D 预留区；右侧为当前臂腕部及共用
手外相机、对应臂左右指光流预留区和三层任务按钮。开始全部流程暂未接入。
标定页预留相机预览、采点、求解和保存区域，后续接入 `handeye/`，当前不运行标定命令。
原手动控制、自动任务、相机与视觉、工艺参数页面均已移除。
没有可靠反馈的使能/TCP 状态显示未知；光流、夹爪 3D 和软件急停尚未接入。
拍照保存当前 RGB 预览至 `~/Pictures/ORISYS/`；放大和全屏预览使用同一图像缓存。
没有查看需求的相机订阅会撤销。离线运行不连接 ROS 或设备，也不下发控制。
## 首页动作接口
- 检测 → `stage_track`，运行现有第一阶段跟踪：D405 优先、丢失目标后 D435 补位；会移动机械臂，不进入对孔或旋入。
- 对孔 → `stage_locate`，只运行第二阶段，明确关闭最终下压和后续旋入。
- 回零 → 所选臂命名点文件中的 `servo.home_pose_name`（默认 `home`），经原生轨迹 Action 返回初始姿态，不是关节全零。
- 停止 → 取消本站任务、通过控制器管理服务暂停对应臂的运动控制器（硬件接口调用 `Robot::Stop`），停止本站流程，再等待新鲜关节反馈持续稳定。失败时明确报告停止未确认，并阻止继续运动。仅下一次显式动作恢复本站暂停的控制器。
有本站任务正在运行时，停止始终指向正在执行的臂，不受界面左右臂切换影响。
没有指定机械臂的后台关闭请求只取消本站任务，不暂停外部控制器。软件停止不替代硬件急停。
外部跟踪/对孔节点存在时拒绝重复启动，不批量结束外部节点；第一阶段子 launch 留在受管进程组内。
动作依赖对应臂关节反馈、控制器归属、相机和手眼配置，检查不通过则拒绝执行。
## 边界与设备归属
Qt 前端只负责控件、绘图和本地状态缓存。`runtime_client.py` 在后台线程通过私有 Unix
socket 提交命令、读取任务结果和状态，绘图/定时刷新不会等待 ROS、串口或脚本。
同一工作区只允许一个工位窗口。后端失联后不重发命令，旧状态会标成过期。
`control_service.py` 是独立后台进程，`tasks.py` 保留任务编号、排队/执行/完成/失败/取消状态，
并按设备锁定资源；界面线程结束不等于动作完成。`ros_gateway.py` 用持续 ROS 订阅读取
关节、夹爪和触觉数据，用原生服务和轨迹 Action 执行操作。工位不另开 RDK 连接，也不读串口。
机械臂连接由现有 ros2_control 硬件驱动持有，串口由对应的常驻夹爪驱动独占。
回点与 home 通过 `/rizon_arm_controller/follow_joint_trajectory` 执行，取消使用目标句柄的回执，
不取消其他客户端的目标。发送前检查关节状态、控制器激活与关节归属；有外部轨迹执行时拒绝抢占。
仅有位置反馈时显示“使能未知”，不把关节消息冒充使能/Remote 状态。
PGEA 的常驻驱动提供：
- `/station/grippers/arm_<臂号>/execute`：`arm_msgs/srv/GripperExecute`。
- `/station/grippers/arm_<臂号>/status`：`arm_msgs/msg/GripperStatus`。
位置字段沿用消息契约，单位为 mm；前端的千分比在后端转换。连接/探测不会初始化、回零。
驱动拒绝非有限值、越界位置、非法力/速度和不支持的旋转操作。写入成功和实际到位分别判断；
结果未知时阻止继续发夹爪运动，要求先探测确认。
## 图像链路
RealSense 采集节点只在显式请求时启动。已有发布者会被复用，工位不会按进程名终止它。
采集配置继续来自 `site.yaml`，算法使用的彩色/深度独立于界面预览。
`cam_bridge.py` 负责 ROS 图像接收与共享内存输出。当前 Qt 界面只请求真机彩色流；
没有查看需求的图像与检测订阅会撤销。预览处理最高 15 fps、彩色宽度上限 848，
算法输入不受影响，采集端现有原图话题的发布速率不会因此降低。
图像或桥进程超过 2 秒未更新时清除旧显示。共享内存由生产者释放，读取者只解除映射。
深度、仿真及标定流的后端能力保留给独立工具，不再提供首页切换入口。
触觉三力和预览由同一个常驻 `arm_application.tactile_node` 提供。后端订阅结构化力反馈，
仅在开启预览时订阅 `/tactile/<side>/image`、`/vector`，不会另外调用传感器 SDK。
关闭预览不停止力反馈。只有两指反馈新鲜时才允许旋入。已有外部触觉节点会被复用；
若其 `image_hz=0`，需在原启动入口启用预览输出。
## 进程与关闭
日常夹爪操作和示教回点不调用 shell。跟踪/对孔使用 Python 生成的 ROS launch 参数，
默认不自动拉额外查看器、红外或触觉流。上电的网络/工具准备与手眼标定保留原维护脚本，
由后端作为明确的系统/标定工具运行。`operator_console.py` 的旧 HTTP/脚本控制路径已经退役。
`processes.py` 只登记并停止自己启动的进程组，不扫描和终止外部进程。Linux 子进程设置
父进程退出信号，后端也监测界面退出，避免留下预览或控制孤儿进程。
“停止动作”取消工位任务，保持采集和驱动；窗口关闭时取消任务并退出自己持有的进程。
软件流程停止不能证明控制器已停稳；轨迹、夹爪停止未确认时会明确报告结果未知。
## 现有工程的边界
- 当前 ROS 控制栈的 controller/action 名称是全局的，因此机械臂动作统一串行；没有假装支持双臂同时控制。
- 本项目未包含 RGI 旋转夹爪实现。可接入同契约的常驻驱动，当前不会猜寄存器或调用缺失的模块。
- 软拖拽只接受 `/station/arms/arm_<臂号>/soft_drag` 常驻控制驱动服务；未提供时按钮禁用，
不再建立第二条 RDK 连接。记录点和轨迹回点不依赖软拖拽桥。
- 缺失的通止规整套状态机入口明确禁用，不再调用不存在的脚本。
- 已运行的旧夹爪节点可以继续提供只读状态；要迁移到新命令服务，需先在其原启动终端停止旧节点，
再用“系统设置 → 连接夹爪”启动新驱动。工位不会擅自抢占串口或杀掉外部节点。
- 仿真预览要求相应 DDS 域可达；切换显示来源不会改写真机控制后端的 DDS 域。
测试使用独立 DDS 域与模拟传输，覆盖 Qt 设备隔离、消息/共享内存图像、命令参数边界、
到位等待、资源互斥、轨迹取消、状态过期、父进程退出和正常关闭。没有以测试名义下发真机运动。
每次后端启动会生成一份私有站点配置快照，所有子进程使用同一份设备身份与参数。
现场 `site.yaml` 发生变化后拒绝新操作并提示重启，已执行任务继续使用原快照，避免运行中换臂或换相机。
station/requirements-qt.txt
+2
View File
@@ -0,0 +1,2 @@
# 已验证 Python 3.11 下 Qt void 调用不会递减 None 引用计数。
PySide6==6.8.3
station/run_station.sh
Executable
+99
View File
@@ -0,0 +1,99 @@
#!/usr/bin/env bash
# Orisys 通止规工位桌面程序（窗口，不是网页）。
#
# bash ~/桌面/orisys_arm/station/run_station.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
case "${1:-}" in
-h|--help)
echo "用法：bash station/run_station.sh [--offline | --self-test]"
echo "默认启动工位和独立后端；--offline 禁止设备访问；--self-test 运行离线回归。"
exit 0
;;
esac
# 这份搬迁的 symlink-install 在 /tmp/orisys_mnt 构建；只在私有挂载空间恢复它，
# 否则工位虽然能开，ROS 消息和相机桥的安装链接却失效。
_arm_hook="${ROOT}/install/arm_msgs/share/arm_msgs/local_setup.bash"
if [[ "${ROOT}" != /tmp/orisys_mnt && -L "${_arm_hook}" && ! -e "${_arm_hook}" && "$(readlink "${_arm_hook}")" == /tmp/orisys_mnt/* ]]; then
exec unshare -rm bash -c '
set -e
mkdir -p /tmp/orisys_mnt
mount --bind "$1" /tmp/orisys_mnt
shift
exec bash /tmp/orisys_mnt/station/run_station.sh "$@"
' bash "${ROOT}" "$@"
fi
unset _arm_hook
export XDG_CACHE_HOME="${XDG_CACHE_HOME:-${HOME}/.cache}"
# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# 源码在最前。conda 的 site-packages 要排在 Humble 的 python3.10 之前，
# 否则 3.11 的解释器会去加载 cpython-310 的 rclpy，相机预览线程直接失败。
_conda_site=""
if [[ -n "${CONDA_PREFIX:-}" ]]; then
_py_mm="$(python3 -c 'import sys; print(f"{sys.version_info.major}.{sys.version_info.minor}")')"
if [[ -d "${CONDA_PREFIX}/lib/python${_py_mm}/site-packages" ]]; then
_conda_site="${CONDA_PREFIX}/lib/python${_py_mm}/site-packages"
fi
fi
export PYTHONPATH="${ROOT}/station/src:${ROOT}/src/arm_application${_conda_site:+:${_conda_site}}${PYTHONPATH:+:${PYTHONPATH}}"
unset _conda_site _py_mm
# 屏幕跟当前登录会话走。这台工控机是 :1，别的 Linux 桌面常常是 :0。
if [[ -n "${DISPLAY:-}" ]]; then
_disp="${DISPLAY#:}"
_disp="${_disp%%.*}"
if [[ ! -S "/tmp/.X11-unix/X${_disp}" ]]; then
unset DISPLAY
fi
unset _disp
fi
if [[ -z "${DISPLAY:-}" ]]; then
_seat="$(who | awk -v u="$(id -un)" '$1==u && $2 ~ /^:/ {print $2; exit}')"
_seat="${_seat%%.*}"
_n="${_seat#:}"
if [[ -n "${_n}" && -S "/tmp/.X11-unix/X${_n}" ]]; then
export DISPLAY="${_seat}"
else
for _sock in /tmp/.X11-unix/X*; do
if [[ -S "${_sock}" ]]; then
export DISPLAY=":${_sock##*/X}"
break
fi
done
fi
unset _seat _n _sock
fi
# 有真实屏幕时不要留在 offscreen，否则 OpenGL 窗口起不来，模型也不会画到显卡上。
if [[ "${QT_QPA_PLATFORM:-}" == "offscreen" && -n "${DISPLAY:-}" ]]; then
_disp="${DISPLAY#:}"
_disp="${_disp%%.*}"
if [[ -S "/tmp/.X11-unix/X${_disp}" ]]; then
unset QT_QPA_PLATFORM
fi
unset _disp
fi
if [[ -z "${XAUTHORITY:-}" ]]; then
if [[ -r "${HOME}/.Xauthority" ]]; then
export XAUTHORITY="${HOME}/.Xauthority"
elif [[ -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority"
fi
fi
# Qt 6.5 起 xcb 插件依赖 libxcb-cursor。系统没装这个包时，用用户目录里单独放的那一份，
# 不要把整个 conda lib 塞进 LD_LIBRARY_PATH（会和系统的 glib/pango 撞符号）。
_cursor_dir="${HOME}/.local/lib/xcb-cursor"
if [[ ! -e /usr/lib/x86_64-linux-gnu/libxcb-cursor.so.0 && -e "${_cursor_dir}/libxcb-cursor.so.0" ]]; then
export LD_LIBRARY_PATH="${_cursor_dir}${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}"
fi
unset _cursor_dir
# 工位单独固定 Qt 绑定版本，不修改 ROS/conda 内的包。
source "${ROOT}/scripts/lib/qt_env.sh"
orisys_qt_env
if [[ "${1:-}" == "--self-test" ]]; then
export QT_QPA_PLATFORM=offscreen
exec python3 -m unittest discover -s "${ROOT}/station/test" -v
fi
exec python3 -m station.operator_app "$@"
station/src/station/__init__.py
+1
View File
@@ -0,0 +1 @@
"""工位桌面窗口。"""
station/src/station/assets/icons/lucide/LICENSE
+43
View File
@@ -0,0 +1,43 @@
ISC License
Copyright (c) 2026 Lucide Icons and Contributors
Permission to use, copy, modify, and/or distribute this software for any
purpose with or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
---
The following Lucide icons are derived from the Feather project:
airplay, alert-circle, alert-octagon, alert-triangle, aperture, arrow-down-circle, arrow-down-left, arrow-down-right, arrow-down, arrow-left-circle, arrow-left, arrow-right-circle, arrow-right, arrow-up-circle, arrow-up-left, arrow-up-right, arrow-up, at-sign, calendar, cast, check, chevron-down, chevron-left, chevron-right, chevron-up, chevrons-down, chevrons-left, chevrons-right, chevrons-up, circle, clipboard, clock, code, columns, command, compass, corner-down-left, corner-down-right, corner-left-down, corner-left-up, corner-right-down, corner-right-up, corner-up-left, corner-up-right, crosshair, database, divide-circle, divide-square, dollar-sign, download, external-link, feather, frown, hash, headphones, help-circle, info, italic, key, layout, life-buoy, link-2, link, loader, lock, log-in, log-out, maximize, meh, minimize, minimize-2, minus-circle, minus-square, minus, monitor, moon, more-horizontal, more-vertical, move, music, navigation-2, navigation, octagon, pause-circle, percent, plus-circle, plus-square, plus, power, radio, rss, search, server, share, shopping-bag, sidebar, smartphone, smile, square, table-2, tablet, target, terminal, trash-2, trash, triangle, tv, type, upload, x-circle, x-octagon, x-square, x, zoom-in, zoom-out
The MIT License (MIT) (for the icons listed above)
Copyright (c) 2013-present Cole Bemis
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
station/src/station/assets/icons/lucide/SOURCE.txt
+3
View File
@@ -0,0 +1,3 @@
Source: https://github.com/lucide-icons/lucide/tree/main/icons
Downloaded: 2026-10-16
License: see LICENSE
station/src/station/assets/icons/lucide/bot.svg
+18
View File
@@ -0,0 +1,18 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M12 8V4H8" />
<rect width="16" height="12" x="4" y="8" rx="2" />
<path d="M2 14h2" />
<path d="M20 14h2" />
<path d="M15 13v2" />
<path d="M9 13v2" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 380 B

station/src/station/assets/icons/lucide/camera.svg
+14
View File
@@ -0,0 +1,14 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M13.997 4a2 2 0 0 1 1.76 1.05l.486.9A2 2 0 0 0 18.003 7H20a2 2 0 0 1 2 2v9a2 2 0 0 1-2 2H4a2 2 0 0 1-2-2V9a2 2 0 0 1 2-2h1.997a2 2 0 0 0 1.759-1.048l.489-.904A2 2 0 0 1 10.004 4z" />
<circle cx="12" cy="13" r="3" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 437 B

station/src/station/assets/icons/lucide/circle-play.svg
+14
View File
@@ -0,0 +1,14 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M9 9.003a1 1 0 0 1 1.517-.859l4.997 2.997a1 1 0 0 1 0 1.718l-4.997 2.997A1 1 0 0 1 9 14.996z" />
<circle cx="12" cy="12" r="10" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 352 B

station/src/station/assets/icons/lucide/hand.svg
+16
View File
@@ -0,0 +1,16 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M18 11V6a2 2 0 0 0-2-2a2 2 0 0 0-2 2" />
<path d="M14 10V4a2 2 0 0 0-2-2a2 2 0 0 0-2 2v2" />
<path d="M10 10.5V6a2 2 0 0 0-2-2a2 2 0 0 0-2 2v8" />
<path d="M18 8a2 2 0 1 1 4 0v6a8 8 0 0 1-8 8h-2c-2.8 0-4.5-.86-5.99-2.34l-3.6-3.6a2 2 0 0 1 2.83-2.82L7 15" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 483 B

station/src/station/assets/icons/lucide/house.svg
+14
View File
@@ -0,0 +1,14 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M15 21v-8a1 1 0 0 0-1-1h-4a1 1 0 0 0-1 1v8" />
<path d="M3 10a2 2 0 0 1 .709-1.528l7-6a2 2 0 0 1 2.582 0l7 6A2 2 0 0 1 21 10v9a2 2 0 0 1-2 2H5a2 2 0 0 1-2-2z" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 383 B

station/src/station/assets/icons/lucide/network.svg
+17
View File
@@ -0,0 +1,17 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<rect x="16" y="16" width="6" height="6" rx="1" />
<rect x="2" y="16" width="6" height="6" rx="1" />
<rect x="9" y="2" width="6" height="6" rx="1" />
<path d="M5 16v-3a1 1 0 0 1 1-1h12a1 1 0 0 1 1 1v3" />
<path d="M12 12V8" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 445 B

station/src/station/assets/icons/lucide/octagon-pause.svg
+15
View File
@@ -0,0 +1,15 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M10 15V9" />
<path d="M14 15V9" />
<path d="M2.586 16.726A2 2 0 0 1 2 15.312V8.688a2 2 0 0 1 .586-1.414l4.688-4.688A2 2 0 0 1 8.688 2h6.624a2 2 0 0 1 1.414.586l4.688 4.688A2 2 0 0 1 22 8.688v6.624a2 2 0 0 1-.586 1.414l-4.688 4.688a2 2 0 0 1-1.414.586H8.688a2 2 0 0 1-1.414-.586z" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 506 B

station/src/station/assets/icons/lucide/panel-left-close.svg
+15
View File
@@ -0,0 +1,15 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<rect width="18" height="18" x="3" y="3" rx="2" />
<path d="M9 3v18" />
<path d="m16 15-3-3 3-3" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 314 B

station/src/station/assets/icons/lucide/panel-left-open.svg
+15
View File
@@ -0,0 +1,15 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<rect width="18" height="18" x="3" y="3" rx="2" />
<path d="M9 3v18" />
<path d="m14 9 3 3-3 3" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 313 B

station/src/station/assets/icons/lucide/power.svg
+14
View File
@@ -0,0 +1,14 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M12 2v10" />
<path d="M18.4 6.6a9 9 0 1 1-12.77.04" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 276 B

station/src/station/assets/icons/lucide/scan-eye.svg
+18
View File
@@ -0,0 +1,18 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M3 7V5a2 2 0 0 1 2-2h2" />
<path d="M17 3h2a2 2 0 0 1 2 2v2" />
<path d="M21 17v2a2 2 0 0 1-2 2h-2" />
<path d="M7 21H5a2 2 0 0 1-2-2v-2" />
<circle cx="12" cy="12" r="1" />
<path d="M18.944 12.33a1 1 0 0 0 0-.66 7.5 7.5 0 0 0-13.888 0 1 1 0 0 0 0 .66 7.5 7.5 0 0 0 13.888 0" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 508 B

station/src/station/assets/icons/lucide/settings.svg
+14
View File
@@ -0,0 +1,14 @@
<svg
xmlns="http://www.w3.org/2000/svg"
width="24"
height="24"
viewBox="0 0 24 24"
fill="none"
stroke="currentColor"
stroke-width="2"
stroke-linecap="round"
stroke-linejoin="round"
>
<path d="M9.671 4.136a2.34 2.34 0 0 1 4.659 0 2.34 2.34 0 0 0 3.319 1.915 2.34 2.34 0 0 1 2.33 4.033 2.34 2.34 0 0 0 0 3.831 2.34 2.34 0 0 1-2.33 4.033 2.34 2.34 0 0 0-3.319 1.915 2.34 2.34 0 0 1-4.659 0 2.34 2.34 0 0 0-3.32-1.915 2.34 2.34 0 0 1-2.33-4.033 2.34 2.34 0 0 0 0-3.831A2.34 2.34 0 0 1 6.35 6.051a2.34 2.34 0 0 0 3.319-1.915" />
<circle cx="12" cy="12" r="3" />
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 586 B

station/src/station/cam_bridge.py
+192
View File
@@ -0,0 +1,192 @@
"""相机预览进程。
界面进程不进这个文件。这里自己订彩色、深度和推理框，画好以后放进共享内存。
界面只读共享内存，这样 `rclpy` 不会和 Qt 待在同一个进程里。
"""
from __future__ import annotations
import json
import os
import signal
import sys
import time
import traceback
from multiprocessing import shared_memory
from pathlib import Path
from station.operator_camera_view import (
_DET,
_JPEG,
_LOCK,
_META,
_PREVIEW,
_SHM_HEAD,
_SHM_MAX,
_STATE,
_STAMPS,
_open_node,
publish_latest,
stream_targets,
watch,
)
_QUIT = False
def _on_term(_signum, _frame) -> None:
global _QUIT
_QUIT = True
def _write_json(path: Path, payload: dict) -> None:
tmp = path.with_suffix(path.suffix + ".tmp")
tmp.write_text(json.dumps(payload, ensure_ascii=False), encoding="utf-8")
tmp.replace(path)
def _apply_want(path: Path) -> None:
try:
payload = json.loads(path.read_text(encoding="utf-8"))
except (OSError, json.JSONDecodeError):
return
if not isinstance(payload, dict):
return
preview = payload.get("preview") if isinstance(payload.get("preview"), list) else []
jpeg = payload.get("jpeg") if isinstance(payload.get("jpeg"), list) else []
watch({str(item) for item in preview}, {str(item) for item in jpeg})
def _safe(text: str) -> str:
return "".join(ch if ch.isalnum() else "_" for ch in text)[:40]
def _slot(shms: dict, key: str) -> tuple[str, shared_memory.SharedMemory]:
found = shms.get(key)
if found is not None:
return found
name = f"ocv{os.getpid()}_{_safe(key)}"
try:
shm = shared_memory.SharedMemory(name=name, create=True, size=_SHM_HEAD.size + _SHM_MAX)
except FileExistsError:
stale = shared_memory.SharedMemory(name=name, create=False)
stale.close()
stale.unlink()
shm = shared_memory.SharedMemory(name=name, create=True, size=_SHM_HEAD.size + _SHM_MAX)
shms[key] = (name, shm)
return name, shm
def _shm_write(shm: shared_memory.SharedMemory, blob: bytes, width: int, height: int) -> None:
if len(blob) > _SHM_MAX:
raise ValueError(f"画面 {len(blob)} 字节，超过共享内存")
buf = shm.buf
seq = _SHM_HEAD.unpack_from(buf, 0)[0]
if seq % 2:
seq += 1
writing = seq + 1
_SHM_HEAD.pack_into(buf, 0, writing, int(width), int(height), len(blob))
buf[_SHM_HEAD.size:_SHM_HEAD.size + len(blob)] = blob
_SHM_HEAD.pack_into(buf, 0, writing + 1, int(width), int(height), len(blob))
def _export(work: Path, shms: dict, seen: dict) -> None:
with _LOCK:
previews = dict(_PREVIEW)
jpegs = dict(_JPEG)
meta = {key: [int(val[0]), int(val[1]), float(val[2])] for key, val in _META.items()}
now = time.monotonic()
ages = {key: now-stamps[-1] for key,stamps in _STAMPS.items() if stamps}
det = {
role: {"age_s": now - stamp, "data": list(data)}
for role, (stamp, data) in _DET.items()
}
error = str(_STATE.get("error") or "")
frames: dict[str, dict] = {}
for key, item in previews.items():
blob, width, height = item
if seen.get(("rgb", key)) is blob:
name = shms[f"rgb:{key}"][0]
else:
name, shm = _slot(shms, f"rgb:{key}")
_shm_write(shm, blob, width, height)
seen[("rgb", key)] = blob
frames[key] = {"name": name, "kind": "rgb", "store": key}
for key, blob in jpegs.items():
if seen.get(("jpg", key)) is blob:
name = shms[f"jpg:{key}"][0]
else:
name, shm = _slot(shms, f"jpg:{key}")
_shm_write(shm, blob, 1, 1)
seen[("jpg", key)] = blob
frames[f"{key}#jpeg"] = {"name": name, "kind": "jpeg", "store": key}
_write_json(work / "meta.json", {"error": error, "meta": meta, "det": det, "frames": frames, "ages": ages, "exported_monotonic": time.monotonic()})
def _serve(work: Path) -> None:
import rclpy
from arm_application.site_io import load_site_dict
signal.signal(signal.SIGTERM, _on_term)
signal.signal(signal.SIGINT, _on_term)
cams = stream_targets(load_site_dict())
if not cams:
_write_json(work / "meta.json", {"error": "site 里没有可预览的真机相机", "meta": {}, "det": {}, "frames": {}})
return
names = "、".join(str(cam.get("label") or cam.get("id")) for cam in cams)
print(f"可预览相机目录：{names}；实际仅订当前页面所需流", flush=True)
node = _open_node(cams)
shms: dict = {}
seen: dict = {}
try:
while not _QUIT:
_apply_want(work / "want.json")
try:
node.sync_depth()
for _ in range(8):
rclpy.spin_once(node, timeout_sec=0.0)
publish_latest()
_export(work, shms, seen)
except Exception as exc: # noqa: BLE001 —— 一帧失败留在状态里，进程继续
traceback.print_exc()
with _LOCK:
_STATE["error"] = str(exc)
if _QUIT:
break
time.sleep(0.02 if node._streams.items else 0.1)
finally:
try:
node.destroy_node()
except Exception:
traceback.print_exc()
if rclpy.ok():
rclpy.shutdown()
for _name, shm in shms.values():
try:
shm.close()
shm.unlink()
except Exception:
traceback.print_exc()
def main() -> int:
if len(sys.argv) < 2:
print("用法：python3 -m station.cam_bridge 目录", file=sys.stderr)
return 2
work = Path(sys.argv[1])
work.mkdir(parents=True, exist_ok=True)
try:
_serve(work)
except Exception:
traceback.print_exc()
last = traceback.format_exc().strip().splitlines()[-1]
try:
_write_json(work / "meta.json", {"error": last, "meta": {}, "det": {}, "frames": {}})
except OSError:
pass
return 1
return 0
if __name__ == "__main__":
sys.exit(main())
station/src/station/child_process.py
+19
View File
@@ -0,0 +1,19 @@
"""Linux child launcher: stop a managed process if its station backend disappears."""
import ctypes
import os
import signal
import sys
def main():
parent=int(sys.argv[1]);argv=sys.argv[2:]
if not argv: raise SystemExit('missing executable')
if sys.platform.startswith('linux'):
libc=ctypes.CDLL(None,use_errno=True)
if libc.prctl(1,signal.SIGINT,0,0,0)!=0:
raise OSError(ctypes.get_errno(),'PR_SET_PDEATHSIG failed')
if os.getppid()!=parent: raise SystemExit('station backend exited before child startup')
os.execvpe(argv[0],argv,os.environ)
if __name__=='__main__': main()
station/src/station/control_service.py
+491
View File
@@ -0,0 +1,491 @@
"""Station backend: owns task state, ROS clients and launched processes. No Qt."""
from __future__ import annotations
import argparse
import base64
import copy
import math
import os
import signal
import sys
import threading
import tempfile
import yaml
import time
from pathlib import Path
from arm_application.site_io import listed_arms, load_site_dict, ros2_root, site_path
from station import site_catalog as metadata
from station import operator_teach as points
from station.launch_plans import environment, launch, camera_plan, gripper_plan
from station.processes import ProcessSupervisor
from station.rpc import Server, Handler
from station.tasks import TaskManager, Cancelled
class StationService:
def __init__(self, root=None, gateway=None, offline=False):
self.root = Path(root or ros2_root())
self.site = load_site_dict()
self.config_path = site_path()
self._config_signature = self._signature()
self._config_changed = False
self._config_directory = tempfile.TemporaryDirectory(prefix="orisys_site_")
self._frozen_config = Path(self._config_directory.name)/"site.yaml"
self._frozen_config.write_text(yaml.safe_dump(self.site,allow_unicode=True),encoding="utf-8")
self._frozen_config.chmod(0o600)
self.arms = {a['id']:a for a in listed_arms(self.site)}
self.tasks = TaskManager()
self.processes = ProcessSupervisor(self.root/'log/station')
self._lock = threading.RLock()
self._snapshot = {}
self._stop = threading.Event()
self.gateway = gateway
self.error = ''
self._gripper_unknown = set()
self._stopping = threading.Event()
self._stop_error = ""
self._stop_lock = threading.Lock()
self._submit_lock = threading.RLock()
self._motion_arm = None
if self.gateway is None and not offline:
try:
from station.ros_gateway import RosGateway
self.gateway = RosGateway(self.site)
except Exception as exc:
self.error = f'ROS 后端未就绪：{exc}'
if offline:
self.error = '离线验证模式，所有设备命令禁用'
self._thread = threading.Thread(target=self._monitor, daemon=True, name='station-state')
self._thread.start()
def _signature(self):
stat=self.config_path.stat()
return stat.st_ino,stat.st_mtime_ns,stat.st_size
def _check_configuration(self):
try: changed=self._signature()!=self._config_signature
except OSError: changed=True
self._config_changed=self._config_changed or changed
if self._config_changed:
raise RuntimeError('站点配置已变化，请重启工位；运行中的任务继续使用原配置快照')
def _environment(self, aid):
return environment(self.root,aid,self._frozen_config)
def catalog(self):
value = metadata.catalog()
available = {'real_up','camera_d405','camera_d435','tactile_up','tactile_down',
'tactile_view_on','tactile_view_off','gripper_up','gripper_probe','gripper_open',
'gripper_close','gripper_rotate','pipeline','stage_track','stage_locate','stage_screw',
'reset_home','handeye_show','handeye_teach','handeye_merge','handeye_calibrate'}
for spec in value['actions']:
spec['available'] = spec['id'] in available
if not spec['available']:
spec['unavailable_reason'] = '当前项目未提供此流程实现'
return value
def _monitor(self):
while not self._stop.is_set():
started = time.monotonic()
try:
try: self._check_configuration()
except RuntimeError as exc: self.error=str(exc)
arm_live, grip_live = self.gateway.states() if self.gateway else ({},{})
graph = self.gateway.graph() if self.gateway else {'nodes':[],'topics':[],'services':[]}
nodes = graph['nodes']
with metadata._LOCK:
tactile_status = {'left':dict(metadata._TACTILE['left']), 'right':dict(metadata._TACTILE['right']),
'stamp': metadata._TACTILE['stamp']}
history = {key:list(metadata._HIST[key]) for key in ('left','right')}
gauge = metadata._PASS.snapshot()
for side in ('left','right'):
if time.time()-tactile_status[side].get('stamp',0)>2:
tactile_status[side]['connected']=False
snap = {'arms':list(self.arms.values()),'arm_live':arm_live,'gripper_live':grip_live,
'usb':metadata._usb(), 'services':[
{'id':'real','label':'控制栈','up':'controller_manager' in nodes},
{'id':'camera','label':'相机','up':any(n.startswith('realsense') for n in nodes)},
{'id':'tactile','label':'视触觉','up':any(value.get('connected') for value in tactile_status.values() if isinstance(value,dict))},
{'id':'gripper','label':'夹爪','up':any(r.get('connected') for r in grip_live.values())},
{'id':'pipeline','label':'跟踪对孔','up':any(r['running'] for k,r in self.processes.snapshot().items() if k.startswith('motion:'))}],
'handeye':{aid:metadata._handeye(aid) for aid in self.arms},
'teach_points':{aid:points.list_points(aid) for aid in self.arms},
'grippers':metadata._grippers(), 'tactile':tactile_status, 'history':history,
'view':getattr(self.gateway,'tactile_state',lambda:{})(), 'gauge_pass':gauge, 'now':time.time(),
'backend':{'error':self.error,'pid':os.getpid(),'protocol':1,'graph':graph},
'tasks':self.tasks.snapshot()}
with self._lock: self._snapshot = snap
except Exception as exc:
with self._lock:
self._snapshot.setdefault('backend',{})['error'] = f'状态读取失败：{exc}'
self._stop.wait(max(.05,.5-(time.monotonic()-started)))
def snapshot(self):
with self._lock:
result = copy.deepcopy(self._snapshot)
result['tasks'] = self.tasks.snapshot()
return result
def _arm(self, aid):
if aid not in self.arms: raise ValueError('臂号不在站点配置中')
return self.arms[aid]
def submit(self, action, arm, mode='eye_in_hand', extras=None):
with self._submit_lock:
return self._submit(action, arm, mode, extras)
def _submit(self, action, arm, mode='eye_in_hand', extras=None):
self._check_configuration()
self._arm(arm)
if self._stopping.is_set(): raise RuntimeError('正在停止机械臂，请等待停止结果')
if self._stop_error and action in ('pipeline','stage_track','stage_locate','stage_screw','reset_home','teach_goto'):
raise RuntimeError('上次停止未确认，请先再次停止并确认状态：' + self._stop_error)
extras = extras or {}
spec = next((s for s in self.catalog()['actions'] if s['id']==action),None)
special = {'camera_refresh','gripper_tune','teach_record','teach_delete','teach_goto','teach_drag_on','teach_drag_off'}
if not spec and action not in special: raise ValueError('未注册的操作')
if spec and not spec['available']: raise ValueError(spec['unavailable_reason'])
if mode not in ('eye_in_hand','eye_to_hand'): raise ValueError('手眼模式不合法')
if action.startswith('gripper'): resources = {f'gripper:{arm}'}
elif action == 'camera_refresh': resources = {f'camera:{arm}', 'camera:overhead'}
elif action.startswith('camera_'):
resources = {'camera:overhead' if action=='camera_d435' else f'camera:{arm}'}
elif action.startswith('tactile'): resources = {'tactile'}
elif action in ('handeye_show','handeye_merge','teach_delete'): resources = {f'points:{arm}'}
else:
# Existing ROS stack has global controller/action names; never allow two arm commands concurrently.
resources = {'robot-control',f'arm:{arm}'}
if action=='teach_record': resources.add(f'points:{arm}')
if action in ('pipeline','stage_track','stage_locate','stage_screw','reset_home'): resources.add(f'gripper:{arm}')
if action in ('pipeline','stage_track','stage_locate','stage_screw'):
resources.update({f'camera:{arm}', 'camera:overhead'})
if action.startswith('handeye_') and action!='handeye_show': resources.add(f'calibration:{arm}')
return self.tasks.submit(action, resources, lambda event:self.execute(action,arm,mode,extras,event))
def execute(self, action, aid, mode, extras, event):
motion = action in ('pipeline','stage_track','stage_locate','stage_screw','reset_home','teach_goto')
if motion:
with self._lock: self._motion_arm = aid
try:
return self._execute(action, aid, mode, extras, event)
finally:
if motion:
with self._lock: self._motion_arm = None
def _execute(self, action, aid, mode, extras, event):
arm = self._arm(aid)
if event.is_set(): raise Cancelled('操作已取消')
if action=='teach_delete': return points.delete_point(aid, str(extras.get('name') or ''))
if action in ('tactile_view_on','tactile_up'):
if self.gateway is None: raise RuntimeError(self.error)
if not arm['tactile']: raise RuntimeError('当前臂未配置触觉设备')
if not self.gateway.publisher_count('/tactile/left/status'):
existing=self.processes.snapshot().get('tactile')
if not existing or not existing['running']:
self.processes.start('tactile',[sys.executable,'-m','arm_application.tactile_node',
'--ros-args','-p','image_hz:=5.0'],env=self._environment(aid),cwd=str(self.root))
deadline=time.monotonic()+12
while not self.gateway.publisher_count('/tactile/left/status'):
if event.wait(.1): raise Cancelled('触觉启动等待已取消')
if time.monotonic()>deadline: raise RuntimeError('触觉驱动未就绪，请查看 log/station/tactile.log')
self.gateway.watch_tactile(True)
return {'ok':True,'message':'已连接常驻触觉驱动；仅订阅其力反馈与预览，不重复打开传感器'}
if action in ('tactile_view_off','tactile_down'):
if self.gateway: self.gateway.watch_tactile(False)
if action=='tactile_down': self.processes.stop('tactile')
return {'ok':True,'message':'触觉预览已关闭；外部驱动不受影响'}
if action=='handeye_show':
return {'ok':True,'message':'手眼配置已读取','detail':str(metadata._handeye(aid).get(mode) or {})}
if action=='handeye_merge':
return self._tool(action,aid,mode,extras,event)
if self.gateway is None: raise RuntimeError(self.error or 'ROS 后端不可用')
if action in ('pipeline','stage_track','stage_locate','stage_screw','reset_home','teach_goto'):
nodes = self.gateway.graph()['nodes']
if any(name in nodes for name in ('assembly_pipeline','servo_node','stage2_locate','stage3_search')):
raise RuntimeError('已有外部跟踪/对孔节点在运行，请先在其原入口停止，工位不会抢占')
if action.startswith('gripper'):
return self._gripper(action, arm, extras, event)
if action == 'camera_refresh':
results = []
for overhead, name in ((False, 'D405'), (True, 'D435')):
if event.is_set(): raise Cancelled('相机刷新已取消')
try:
result = self._camera(arm, overhead, event)
results.append({'camera': name, **result})
except Cancelled: raise
except Exception as exc: results.append({'camera':name,'ok':False,'message':str(exc)})
return {'ok': all(row['ok'] for row in results),
'message': '；'.join(f"{row['camera']}：{row['message']}" for row in results), 'cameras':results}
if action.startswith('camera_'):
return self._camera(arm, action=='camera_d435',event)
if action=='real_up':
# Network/tool preparation remains a bringup concern, never used for daily motion commands.
if 'controller_manager' in self.gateway.graph()['nodes']:
self.gateway.require_arm(aid,event)
return {'ok':True,'message':'本臂控制器已就绪；已检查连接并恢复本站暂停的控制器，不重复启动驱动'}
env = self._environment(aid)
self.gateway.active_arm = aid
proc, log = self.processes.start(f'stack:{aid}', ['bash',str(self.root/'scripts/run_real.sh'),'--arm',aid],env,str(self.root))
deadline = time.monotonic()+45
while time.monotonic()<deadline:
if event.wait(.2):
self.processes.stop(f'stack:{aid}');raise Cancelled('控制栈启动已取消')
if proc.poll() is not None: raise RuntimeError(f'控制栈启动失败，见 {log}')
try:
self.gateway.require_arm(aid,event)
return {'ok':True,'message':'控制栈已就绪','log':str(log)}
except RuntimeError: pass
self.processes.stop(f'stack:{aid}')
raise RuntimeError(f'控制栈未在期限内就绪，见 {log}')
if action=='teach_record':
live = self.gateway.states()[0].get(aid,{})
q = live.get('q') if live.get('pose_live') else None
title = str(extras.get('name') or '').strip()
if not title or '/' in title or '\\' in title or title in ('.','..'): raise ValueError('点名无效')
if not q: raise RuntimeError('没有新鲜的本臂关节状态，拒绝记录')
listed = points.list_points(aid)
if not listed.get('ok'): return listed
from datetime import datetime,timezone
values = [row for row in listed['points'] if row['name']!=title]
values.append({'name':title,'joints':q,'utc':datetime.now(timezone.utc).isoformat()})
path = points._write(aid,values)
return {'ok':True,'message':f'已记录 {title}','detail':str(path)}
if action in ('teach_goto','reset_home'):
if action=='teach_goto':
listed = points.list_points(aid)
hit = next((row for row in listed.get('points',[]) if row['name']==extras.get('name')),None)
if not hit: raise ValueError('示教点不存在')
q=hit['joints']; speed=self._number(extras,'speed',.05,.35)
else:
import yaml
robot=next(r for r in self.site['robot']['robots'] if r['sn']==arm['sn'])
path=Path(robot.get('named_poses_file') or '')
if not path.is_absolute(): path=self.root/path
config=yaml.safe_load(path.read_text())['named_pose_server']['ros__parameters']
servo = {**(self.site.get('servo') or {}), **(robot.get('servo') or {})}
pose_name = str(servo.get('home_pose_name') or 'home')
q=config['named_poses.' + pose_name];speed=.15
return self.gateway.move_joints(arm,q,speed,event)
if action.startswith('teach_drag_'):
# Only an explicitly provided persistent driver may own drag mode; never open a second RDK connection.
from std_srvs.srv import SetBool
self.gateway.require_arm(aid,event)
endpoint=f"/station/arms/arm_{aid}/soft_drag"
if endpoint not in self.gateway.graph()['services']:
raise RuntimeError('当前控制栈没有本臂 soft_drag 服务；未连接第二个 RDK 客户端')
req=SetBool.Request();req.data=action=='teach_drag_on'
response=self.gateway.call(SetBool,endpoint,req,event,10)
return {'ok':response.success,'message':response.message}
if action.startswith('handeye_'):
# Calibration still uses the repository's calibration tool, isolated from daily control.
if action=='handeye_calibrate': self.gateway.require_arm(aid,event)
elif 'controller_manager' in self.gateway.graph()['nodes']:
raise RuntimeError('控制栈运行时不能启动独占 RDK 的标定采点工具')
return self._tool(action,aid,mode,extras,event)
if action in ('pipeline','stage_track','stage_locate','stage_screw'):
self.gateway.require_arm(aid,event)
if action=='stage_track':
from arm_application.site_io import track_camera_role
if track_camera_role(self.site) != 'wrist':
raise RuntimeError('检测要求 D405 优先，请将 perception.track_camera 配置为 wrist')
if action=='stage_screw' and not arm['tactile']:
raise RuntimeError('当前臂没有配置触觉，拒绝旋入')
self._camera(arm,False,event);self._camera(arm,True,event)
env=self._environment(aid)
if action in ('pipeline','stage_track','stage_locate'):
argv=launch(self.root,'pipeline.launch.py',skip_stage1=action=='stage_locate',
skip_stage2=action=='stage_track',skip_stage3=True,preview=False,
resnap=False,no_descent=action=='stage_locate',d435_move_only=False)
else:
with metadata._LOCK:
force_ready=all(metadata._TACTILE[side].get('connected') and time.time()-metadata._TACTILE[side].get('stamp',0)<2 for side in ('left','right'))
if not force_ready: raise RuntimeError('两指触觉反馈未就绪，拒绝旋入')
argv=launch(self.root,'stage3_search.launch.py',preview=False)
return self.processes.run(f'motion:{aid}',argv,event,env,str(self.root))
raise ValueError('操作未实现')
@staticmethod
def _number(values,key,low,high):
value=float(values.get(key))
if not math.isfinite(value) or not low<=value<=high: raise ValueError(f'{key} 只接受 {low}–{high}')
return value
def _camera(self,arm,overhead,event):
env=self._environment(arm['id'])
name,argv=camera_plan(env,overhead)
topic=f'/{name}/color/image_raw'
key=f'camera:{name}'
if self.gateway.publisher_count(topic): return {'ok':True,'message':f'{name} 已有采集节点，复用'}
existing=self.processes.snapshot().get(key)
created=not existing or not existing['running']
if created:
proc,log=self.processes.start(key,argv,env,str(self.root))
deadline=time.monotonic()+15
while time.monotonic()<deadline:
if event.wait(.1):
if created: self.processes.stop(key)
raise Cancelled('相机启动等待已取消')
if self.gateway.publisher_count(topic): return {'ok':True,'message':f'{name} 已启动'}
if not self.processes.snapshot()[key]['running']: raise RuntimeError(f'{name} 采集进程退出，请看 log/station')
self.processes.stop(key)
raise RuntimeError(f'{name} 相机未就绪')
def _tool(self,action,aid,mode,extras,event):
command={'handeye_teach':'teach','handeye_merge':'merge','handeye_calibrate':'calibrate'}[action]
argv=['bash',str(self.root/'handeye/run_handeye.sh'),'--arm',aid,command,'--mode',mode]
if command=='teach':
count=int(extras.get('points',12))
if not 0<=count<=40: raise ValueError('采点数超出范围')
argv+=['--points',str(count)]
if extras.get('append'): argv+=['--append']
return self.processes.run(f'tool:{aid}',argv,event,self._environment(aid),str(self.root))
def _gripper(self,action,arm,values,event):
from arm_msgs.srv import GripperExecute
aid=arm['id'];endpoint=f'/station/grippers/arm_{aid}/execute'
if endpoint not in self.gateway.graph()['services']:
if action not in ('gripper_up','gripper_probe'):
raise RuntimeError('请先启动本臂常驻夹爪驱动')
if self.gateway.publisher_count('/gripper/states') and aid==self.gateway.active_arm:
raise RuntimeError('旧夹爪驱动正在持有设备；仅显示其状态，请先手动停旧驱动再启用新驱动')
argv=gripper_plan(arm,self.site.get('gripper') or {})
self.processes.start(f'gripper:{aid}',argv,env=self._environment(aid),cwd=str(self.root))
request=GripperExecute.Request()
request.command.command='connect' if action in ('gripper_up','gripper_probe') else 'move'
if action=='gripper_open': values={**values,'pos':1000,'force':33,'speed':25}
if action=='gripper_close': values={**values,'pos':0,'force':33,'speed':25}
kind=values.get('kind','pos')
if action=='gripper_rotate': values={**values,'angle':90,'torque':33,'rot_speed':25};kind='rotate'
if request.command.command!='connect':
if aid in self._gripper_unknown: raise RuntimeError('上次夹爪执行结果未知；请先探测设备状态再发新命令')
if kind=='pos':
request.command.position=self._number(values,'pos',0,1000)/1000*arm['stroke_mm']
elif arm['rotary']:
names={'rotate':'rotate_to','rotate_by':'rotate_by','revolutions':'rotate_revolutions','rotate_home':'home_rotation'}
if kind not in names: raise ValueError('未知旋转操作')
request.command.command=names[kind]
request.command.position=0. if kind=='rotate_home' else self._number(values,{'rotate':'angle','rotate_by':'delta','revolutions':'revolutions'}[kind],-50 if kind=='revolutions' else -32767,50 if kind=='revolutions' else 32767)
else: raise ValueError('平行夹爪没有旋转轴')
request.command.force=self._number(values,'force' if kind=='pos' else 'torque',20,100)
request.command.speed=self._number(values,'speed' if kind=='pos' else 'rot_speed',1,100)
request.command.wait_complete=False;request.command.timeout=6.
before=self.gateway.states()[1].get(aid,{})
if request.command.command!='connect' and not before.get('connected'):
raise RuntimeError('夹爪没有新鲜的在线状态，拒绝运动')
if request.command.command!='connect' and before.get('moving'):
raise RuntimeError('夹爪仍在执行运动，拒绝覆盖上一条命令')
target_angle = None
if kind!='pos' and request.command.command!='connect':
current=before.get('rotation_deg')
if kind in ('rotate_by','revolutions') and current is None:
raise RuntimeError('相对旋转需要新鲜的当前转角')
target_angle = request.command.position if kind=='rotate' else 0. if kind=='rotate_home' else float(current)+request.command.position*(360. if kind=='revolutions' else 1.)
sent_at=time.monotonic()
try:
response=self.gateway.call(GripperExecute,endpoint,request,event,15)
if not response.success: return {'ok':False,'message':response.message}
if request.command.command=='connect':
if getattr(getattr(response,'status',None),'is_moving',False):
return {'ok':False,'message':'夹爪仍在运动，尚不能确认上次任务结束'}
self._gripper_unknown.discard(aid)
return {'ok':True,'message':response.message}
# The driver's response acknowledges register writes, not physical completion.
started=time.monotonic(); observed=False
while time.monotonic()-started<90:
live=self.gateway.states()[1].get(aid,{})
if event.is_set():
self._gripper_unknown.add(aid)
raise Cancelled('已停止等待；夹爪尚未确认停机，请检查设备并重新探测')
if live.get('connected'):
observed = observed or bool(live.get('moving'))
reached = live.get('grip_state') in (1,2) if kind=='pos' else live.get('rotation_text')=='已到位'
target=values.get('pos')
near=kind=='pos' and target is not None and abs(float(live.get('permille',-10000))-float(target))<10
if kind!='pos' and target_angle is not None and live.get('rotation_deg') is not None:
near=abs(float(live['rotation_deg'])-target_angle)<1.
caught=kind=='pos' and live.get('grip_state')==2 and live.get('received_monotonic',0)>sent_at
if reached and (observed or near or caught) and time.monotonic()-started>.1:
return {'ok':True,'message':'夹爪已到位' if not live.get('gripped') else '夹爪已夹住'}
time.sleep(.1)
self._gripper_unknown.add(aid)
raise RuntimeError('夹爪到位等待超时，执行结果未知；请检查设备')
except Exception:
if request.command.command!='connect': self._gripper_unknown.add(aid)
raise
def stop_commands(self, arm=None):
if arm is not None: self._arm(arm)
with self._stop_lock:
with self._submit_lock:
self._stopping.set()
with self._lock: arm = self._motion_arm or arm
self.tasks.cancel()
try:
failure = None
if arm is not None:
try:
if self.gateway is None: raise RuntimeError(self.error or 'ROS 后端不可用')
self.gateway.stop_arm(self._arm(arm))
except Exception as exc:
failure = exc
for key in self.processes.snapshot():
if key.startswith(('motion:', 'tool:')): self.processes.stop(key)
if arm is None:
return {'ok': True, 'message': '已取消本站任务；未请求停止外部机械臂控制器'}
if failure:
raise failure
result = self.gateway.confirm_stopped(self._arm(arm))
self._stop_error = ''
return result
except Exception as exc:
self._stop_error = str(exc)
return {'ok': False, 'message': '停止未确认：' + str(exc)}
finally:
self._stopping.clear()
def dispatch(self,method,params):
if method=='ping': return {'protocol':1,'pid':os.getpid()}
if method=='catalog': return self.catalog()
if method=='snapshot': return self.snapshot()
if method=='submit': return {'task_id':self.submit(**params)}
if method=='task': return self.tasks.snapshot(params['task_id'])
if method=='cancel': self.tasks.cancel(params.get('task_id'));return {'ok':True}
if method=='stop': return self.stop_commands(params.get('arm'))
if method=='gauge_finished': return metadata.notify_gauge_finished()
if method=='gauge_reset': return metadata.reset_gauge_pass()
if method=='tactile_frames':
frames=self.gateway.tactile_frames() if self.gateway else {}
return {key:base64.b64encode(blob).decode() for key,blob in frames.items()}
raise ValueError('未知接口')
def close(self):
self._stop.set();self.tasks.cancel()
self.processes.close()
self.tasks.close()
self._thread.join(timeout=3)
if self.gateway: self.gateway.close()
self._config_directory.cleanup()
def main():
parser=argparse.ArgumentParser();parser.add_argument('--socket',required=True);parser.add_argument('--offline',action='store_true');parser.add_argument('--parent-pid',type=int)
args=parser.parse_args()
service=StationService(offline=args.offline)
server=Server(args.socket,Handler);os.chmod(args.socket,0o600);server.dispatch=service.dispatch
def stop(signum,frame):
threading.Thread(target=server.shutdown,daemon=True).start()
signal.signal(signal.SIGTERM,stop);signal.signal(signal.SIGINT,stop)
if args.parent_pid:
def watch_parent():
while not service._stop.wait(.5):
if os.getppid()!=args.parent_pid:
server.shutdown()
return
threading.Thread(target=watch_parent,daemon=True,name='station-owner-watch').start()
try: server.serve_forever(poll_interval=.1)
finally:
server.server_close();service.close();Path(args.socket).unlink(missing_ok=True)
if __name__=='__main__': main()
station/src/station/image_preview.py
+41
View File
@@ -0,0 +1,41 @@
"""Render ROS image buffers for display; never change force measurements."""
import numpy as np
def jpeg_preview(msg, limit=640):
import cv2
types={'rgb8':(np.uint8,3),'bgr8':(np.uint8,3),'rgba8':(np.uint8,4),'bgra8':(np.uint8,4),
'mono8':(np.uint8,1),'mono16':(np.uint16,1),'16UC1':(np.uint16,1),
'32FC1':(np.float32,1),'32FC2':(np.float32,2)}
if msg.encoding not in types or msg.width<1 or msg.height<1: return None
dtype,channels=types[msg.encoding]
dtype=np.dtype(dtype).newbyteorder('>' if msg.is_bigendian else '<')
row_bytes=msg.width*channels*dtype.itemsize
if msg.step<row_bytes or len(msg.data)<msg.step*msg.height: return None
data=np.frombuffer(bytes(msg.data),dtype=np.uint8).reshape(msg.height,msg.step)[:,:row_bytes].copy()
array=data.view(dtype).reshape(msg.height,msg.width,channels)
if channels==2:
x,y=array[:,:,0].astype(np.float32),array[:,:,1].astype(np.float32)
if not np.isfinite(array).all(): return None
magnitude,angle=cv2.cartToPolar(x,y,angleInDegrees=True)
hsv=np.zeros((msg.height,msg.width,3),np.uint8)
hsv[:,:,0]=(angle/2).astype(np.uint8);hsv[:,:,1]=255
scale=max(float(np.percentile(magnitude,98)),1e-6)
hsv[:,:,2]=np.clip(magnitude*255/scale,0,255).astype(np.uint8)
image=cv2.cvtColor(hsv,cv2.COLOR_HSV2BGR)
elif channels==1:
values=array[:,:,0].astype(np.float32)
valid=np.isfinite(values)
if not valid.any(): return None
lo,hi=np.percentile(values[valid],[2,98]);span=max(float(hi-lo),1e-6)
gray=np.clip(np.nan_to_num((values-lo)/span)*255,0,255).astype(np.uint8)
image=cv2.applyColorMap(gray,cv2.COLORMAP_JET)
else:
image=array.astype(np.uint8,copy=False)
if msg.encoding=='rgb8': image=cv2.cvtColor(image,cv2.COLOR_RGB2BGR)
elif msg.encoding=='rgba8': image=cv2.cvtColor(image,cv2.COLOR_RGBA2BGR)
elif msg.encoding=='bgra8': image=cv2.cvtColor(image,cv2.COLOR_BGRA2BGR)
if image.shape[1]>limit:
image=cv2.resize(image,(limit,max(1,round(image.shape[0]*limit/image.shape[1]))))
ok,blob=cv2.imencode('.jpg',image,[int(cv2.IMWRITE_JPEG_QUALITY),75])
return blob.tobytes() if ok else None
station/src/station/launch_plans.py
+66
View File
@@ -0,0 +1,66 @@
"""Typed launch plans. Shell is retained only for system bringup / calibration tooling."""
from __future__ import annotations
import json
import os
import subprocess
import sys
from pathlib import Path
def environment(root, arm, config_path=None):
result = subprocess.run([sys.executable, str(root/'scripts/lib/export_site_env.py'),
'--config', str(config_path or root/'config/site.yaml'), '--arm', arm, '--json'],
capture_output=True, text=True, timeout=10, check=True)
env = dict(os.environ)
env.update({key: str(value) for key,value in json.loads(result.stdout)['env'].items()})
env['SITE_CONFIG'] = str(config_path or root/'config/site.yaml')
env['ROBOT_SN'] = env['SITE_ROBOT_SN']
env['ROS2_WS'] = str(root)
profile=env.get('SITE_FASTDDS_PROFILE','')
if profile and Path(profile).is_file():
env['FASTRTPS_DEFAULT_PROFILES_FILE']=profile
env.setdefault('RMW_IMPLEMENTATION','rmw_fastrtps_cpp')
return env
def launch(root, filename, **args):
return ['ros2','launch',str(root/'src/arm_application/launch'/filename),
*[f'{k}:={str(v).lower() if isinstance(v,bool) else v}' for k,v in args.items()]]
def camera_plan(env, overhead=False):
prefix = 'SITE_D435_' if overhead else 'SITE_CAM_'
serial = env.get(prefix+'SERIAL', '')
if not serial:
raise ValueError('相机序列号未配置，拒绝按枚举顺序选择设备')
params = {
'camera_name': env.get('SITE_D435_NAME','d435') if overhead else env['SITE_CAM_NAME'],
'camera_serial': serial,
'width': env.get(prefix+'WIDTH', '1280' if overhead else '848'),
'height': env.get(prefix+'HEIGHT', '720' if overhead else '480'),
'fps': env.get(prefix+'FPS','30'),
# Depth belongs to perception; preview demand must not disable it.
'enable_depth': 'true', 'enable_infra': 'false', 'preview_stereo': 'false', 'show_preview': 'false',
'auto_exposure': 'true' if overhead else ('true' if env.get(prefix+'AUTO_EXPOSURE','1')=='1' else 'false'),
'ae_limit_us': '8000' if overhead else env.get(prefix+'AE_LIMIT_US','4000'),
'emitter_enabled': 'true' if overhead else ('true' if env.get(prefix+'EMITTER','0')=='1' else 'false'),
}
if not overhead:
params['enable_depth'] = 'true' if env.get(prefix+'ENABLE_DEPTH','1')=='1' else 'false'
for key, export in [('laser_power','LASER_POWER'),('exposure_us','EXPOSURE_US'),('gain','GAIN')]:
value=env.get(prefix+export)
if value and value not in ('0','0.0'): params[key]=value
argv = [sys.executable, '-m', 'arm_application.realsense_color_pub', '--ros-args']
for key,value in params.items(): argv.extend(['-p',f'{key}:={value}'])
return params['camera_name'], argv
def gripper_plan(arm, shared):
if arm['rotary']:
raise RuntimeError('本项目缺少旋转夹爪驱动实现；请提供匹配的 GripperExecute 服务，不能猜测寄存器')
params = {'port':arm['port'], 'baud':shared.get('baud',115200), 'slave_id':shared.get('slave_id',1),
'stroke_mm':arm['stroke_mm'], 'auto_init':'false', 'rs485_rts':shared.get('rs485_rts','')}
argv=[sys.executable,'-m','arm_application.dh_pgea_node','--ros-args','-r',f'__ns:=/station/grippers/arm_{arm["id"]}']
for key,value in params.items():
if value != '': argv.extend(['-p',f'{key}:={value}'])
return argv
station/src/station/operator_app.py
+2126
View File
File diff suppressed because it is too large Load Diff
station/src/station/operator_arm_mesh.py
+339
View File
@@ -0,0 +1,339 @@
"""用非夕给的 Rizon4 外壳网格摆出当前姿态。
网格和安装位姿来自 ``flexiv_description`` 的 ``visual_parameters.yaml``
与 ``meshes/Rizon4/visual/*.obj``，不另画一套简图。面数按固定步长抽稀，
只为了界面里能转得动，几何仍是厂家的。
"""
from __future__ import annotations
import math
from pathlib import Path
import numpy as np
import yaml
from station.operator_arm_state import link_frames
from arm_application.site_io import ros2_root
# 单个连杆原始面数大约 5000。再按 700 抽稀会在壳上留出三角洞，
# 白壳和灰蓝关节分不出来。这里保留全部面。
_FACES_PER_LINK = 8000
_MODELS: dict[str, list[tuple[list[tuple[float, float, float]], tuple]]] | None = None
def posed_triangles(q: list[float]) -> list[tuple]:
"""返回基座系下的三角面 ``(a, b, c, 明暗, rgb)``。
``rgb`` 来自同目录 ``.mtl`` 的 ``Kd``。外壳是白的，关节壳体是灰蓝，
这是厂家导出的两种材质，不是另涂的颜色。
"""
world, shade, rgb = _posed_arrays(q)
return [
(
(float(world[i, 0, 0]), float(world[i, 0, 1]), float(world[i, 0, 2])),
(float(world[i, 1, 0]), float(world[i, 1, 1]), float(world[i, 1, 2])),
(float(world[i, 2, 0]), float(world[i, 2, 1]), float(world[i, 2, 2])),
float(shade[i]),
(float(rgb[i, 0]), float(rgb[i, 1]), float(rgb[i, 2])),
)
for i in range(shade.shape[0])
]
def posed_gl(q: list[float]) -> tuple[bytes, int, tuple[float, float, float], float]:
"""给显卡的一份网格：交错 ``xyzrgb``、顶点数、中心、半径。
拖视角只改观察矩阵，不再把四万个面在界面线程里重投影。
"""
world, shade, rgb = _posed_arrays(q)
light = (0.55 + 0.45 * shade)[:, None]
colored = (rgb * light).astype(np.float32)
pos = world.reshape(-1, 3).astype(np.float32)
col = np.repeat(colored, 3, axis=0)
center = pos.mean(axis=0)
radius = float(np.linalg.norm(pos - center, axis=1).max())
packed = np.concatenate([pos, col], axis=1).astype(np.float32, copy=False)
return (
packed.tobytes(),
int(pos.shape[0]),
(float(center[0]), float(center[1]), float(center[2])),
max(radius, 1e-4),
)
def _posed_arrays(q: list[float]):
"""每个连杆做一次矩阵乘法，不再对每个顶点走 Python 循环。"""
frames = link_frames(q)
worlds: list[np.ndarray] = []
shades: list[np.ndarray] = []
colors: list[np.ndarray] = []
for link, parts in _models().items():
frame = frames.get(link)
if frame is None:
continue
rot = np.asarray(frame[0], dtype=np.float64)
origin = np.asarray(frame[1], dtype=np.float64)
for part in parts:
local = np.einsum("ij,fvj->fvi", part["erot"], part["verts"]) + part["epos"]
world = np.einsum("ij,fvj->fvi", rot, local) + origin
ab = world[:, 1] - world[:, 0]
ac = world[:, 2] - world[:, 0]
normal = np.cross(ab, ac)
norm = np.linalg.norm(normal, axis=1)
norm = np.where(norm > 0.0, norm, 1.0)
facing = np.abs(normal[:, 0] + normal[:, 1]) / (norm * math.sqrt(2.0))
worlds.append(world)
shades.append(0.28 + 0.72 * facing)
colors.append(part["rgb"])
if not worlds:
empty = np.zeros((0, 3, 3), dtype=np.float64)
return empty, np.zeros((0,), dtype=np.float64), np.zeros((0, 3), dtype=np.float64)
return np.concatenate(worlds), np.concatenate(shades), np.concatenate(colors)
def view_rotation(yaw: float, pitch: float):
"""先绕竖直轴转，再俯仰。拖拽视角用这一份，不在界面里另写一套。"""
return _mul(_rx(pitch), _rz(yaw))
def view_scale(radius: float, width: int, height: int) -> tuple[float, float]:
"""和着色器里的 ``u_sx`` / ``u_sy`` 同一套缩放。宽屏时横向再缩一档。"""
fit = 0.85 / max(float(radius), 1e-4)
width = max(int(width), 1)
height = max(int(height), 1)
if width >= height:
return fit * height / width, fit
return fit, fit * width / height
def screen_point(
point: tuple[float, float, float],
yaw: float,
pitch: float,
center: tuple[float, float, float],
radius: float,
width: int,
height: int,
) -> tuple[float, float]:
"""基座系的点投到控件像素。纵向和着色器里的 ``p.z`` 是同一套，+Z 朝画面上方。"""
shifted = (
float(point[0]) - float(center[0]),
float(point[1]) - float(center[1]),
float(point[2]) - float(center[2]),
)
viewed = _apply(view_rotation(yaw, pitch), shifted)
sx, sy = view_scale(radius, width, height)
ndc_x = viewed[0] * sx
# 着色器写的是 ``p.z * u_sy``。OpenGL 的 y 朝上，这里再换成 Qt 的像素 y。
ndc_y = viewed[2] * sy
width = max(int(width), 1)
height = max(int(height), 1)
return (ndc_x * 0.5 + 0.5) * width, (1.0 - (ndc_y * 0.5 + 0.5)) * height
def ground_grid(extent: float = 1.0, step: float = 0.25) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]:
"""地平面格子，z = 0，单位米。以前这张格子在 RViz 里，现在画在工位仿真上。"""
if step <= 0 or extent <= 0:
raise ValueError("地平面格子的间距和范围都要大于 0")
count = int(round(extent / step))
lines = []
for index in range(-count, count + 1):
coord = index * step
lines.append(((-extent, coord, 0.0), (extent, coord, 0.0)))
lines.append(((coord, -extent, 0.0), (coord, extent, 0.0)))
return lines
def axis_tip(
origin: tuple[float, float, float],
rot: tuple,
axis: int,
length: float,
) -> tuple[float, float, float]:
"""坐标系某一轴的端点。``rot`` 把连杆坐标变到基座坐标，列向量就是三根轴。"""
basis = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))[axis]
direction = _apply(rot, basis)
return (
origin[0] + direction[0] * length,
origin[1] + direction[1] * length,
origin[2] + direction[2] * length,
)
def _place(rot, origin, erot, epos, vertex):
local = _add(epos, _apply(erot, vertex))
return _add(origin, _apply(rot, local))
def _shade(a, b, c) -> float:
ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
nx = uy * vz - uz * vy
ny = uz * vx - ux * vz
nz = ux * vy - uy * vx
norm = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
facing = abs(nx + ny) / (norm * math.sqrt(2.0))
return 0.28 + 0.72 * facing
def _models() -> dict[str, list]:
global _MODELS
if _MODELS is not None:
return _MODELS
root = _description_root()
visual = yaml.safe_load((root / "config/Rizon4/visual_parameters.yaml").read_text(encoding="utf-8"))
params = (visual or {}).get("visual_parameters") or {}
loaded: dict[str, list] = {}
for link, spec in params.items():
shell = ((spec or {}).get("visual") or {})
parts = []
for kind in ("shell", "ring"):
item = shell.get(kind)
if not isinstance(item, dict):
continue
mesh = item.get("mesh") or {}
path = root / str(mesh.get("path") or "")
if not path.is_file():
continue
scale = _scale(mesh.get("scale"))
origin = item.get("origin") or {}
erot = _rpy(
float(origin.get("roll") or 0.0),
float(origin.get("pitch") or 0.0),
float(origin.get("yaw") or 0.0),
)
epos = (
float(origin.get("x") or 0.0),
float(origin.get("y") or 0.0),
float(origin.get("z") or 0.0),
)
parts.append(_part_arrays(_load_obj(path, scale, _materials(path)), erot, epos))
if parts:
loaded[str(link)] = parts
if "base_link" not in loaded or "link7" not in loaded:
raise FileNotFoundError(f"Rizon4 外壳网格不完整：{root}")
_MODELS = loaded
return loaded
def _part_arrays(faces: list[tuple], erot, epos) -> dict:
"""把一个零件收成数组，姿态更新时整块相乘。"""
if not faces:
verts = np.zeros((0, 3, 3), dtype=np.float64)
rgb = np.zeros((0, 3), dtype=np.float64)
else:
verts = np.asarray([[a, b, c] for a, b, c, _color in faces], dtype=np.float64)
rgb = np.asarray([color for _a, _b, _c, color in faces], dtype=np.float64)
return {
"verts": verts,
"rgb": rgb,
"erot": np.asarray(erot, dtype=np.float64),
"epos": np.asarray(epos, dtype=np.float64),
}
def _description_root() -> Path:
root = ros2_root()
candidates = (
root / "src/flexiv_description",
root / "install/flexiv_description/share/flexiv_description",
)
for item in candidates:
if (item / "config/Rizon4/visual_parameters.yaml").is_file():
return item
raise FileNotFoundError("找不到 flexiv_description 的 Rizon4 外观")
def _materials(path: Path) -> dict[str, tuple[float, float, float]]:
mtl = path.with_suffix(".mtl")
colors: dict[str, tuple[float, float, float]] = {}
name = ""
if not mtl.is_file():
return colors
for line in mtl.read_text(encoding="utf-8", errors="replace").splitlines():
if line.startswith("newmtl "):
name = line.split(None, 1)[1].strip()
elif name and line.startswith("Kd "):
parts = line.split()
if len(parts) >= 4:
colors[name] = (float(parts[1]), float(parts[2]), float(parts[3]))
return colors
def _load_obj(path: Path, scale: tuple[float, float, float], materials: dict) -> list[tuple]:
fallback = (0.75, 0.76, 0.78)
rgb = fallback
vertices: list[tuple[float, float, float]] = []
faces: list[tuple[int, int, int, tuple]] = []
for line in path.read_text(encoding="utf-8", errors="replace").splitlines():
if line.startswith("usemtl "):
rgb = materials.get(line.split(None, 1)[1].strip(), fallback)
elif line.startswith("v "):
_v, x, y, z, *_rest = line.split()
vertices.append((float(x) * scale[0], float(y) * scale[1], float(z) * scale[2]))
elif line.startswith("f "):
idx = []
for token in line.split()[1:]:
raw = token.split("/")[0]
if not raw:
continue
number = int(raw)
idx.append(number - 1 if number > 0 else number)
for offset in range(1, len(idx) - 1):
faces.append((idx[0], idx[offset], idx[offset + 1], rgb))
if not faces or not vertices:
return []
step = max(1, len(faces) // _FACES_PER_LINK)
chosen = faces[::step]
out = []
count = len(vertices)
for ia, ib, ic, color in chosen:
if not (0 <= ia < count and 0 <= ib < count and 0 <= ic < count):
continue
out.append((vertices[ia], vertices[ib], vertices[ic], color))
return out
def _scale(raw) -> tuple[float, float, float]:
if raw is None:
return (1.0, 1.0, 1.0)
if isinstance(raw, (int, float)):
value = float(raw)
return (value, value, value)
text = str(raw).replace(",", " ").split()
nums = [float(item) for item in text[:3]]
while len(nums) < 3:
nums.append(nums[-1] if nums else 1.0)
return (nums[0], nums[1], nums[2])
def _rpy(roll: float, pitch: float, yaw: float):
return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
def _rx(angle: float):
c, s = math.cos(angle), math.sin(angle)
return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
def _ry(angle: float):
c, s = math.cos(angle), math.sin(angle)
return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
def _rz(angle: float):
c, s = math.cos(angle), math.sin(angle)
return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
def _mul(a, b):
return tuple(tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)) for i in range(3))
def _apply(rot, vec):
return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))
def _add(a, b):
return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
station/src/station/operator_arm_state.py
+235
View File
@@ -0,0 +1,235 @@
"""Pure robot presentation and forward kinematics; no device connections."""
from __future__ import annotations
import math
from typing import Any
import yaml
from arm_application.site_io import ros2_root
import time
REMOTE_PROMPT = (
"请在示教器上打开 Auto（Remote）。"
"RDK 没有远程拨这个开关的接口，必须人到现场拨。"
)
_AXES = (
(0.0, 0.0, 1.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
)
_JOINTS = tuple(f"joint{i}" for i in range(1, 8))
_KIN: dict[str, dict[str, float]] | None = None
def present(
arm: str,
*,
stack_up: bool,
bridge_up: bool,
probe: dict | None,
joints: list[float] | None,
error: str = "",
) -> dict:
"""把一次读数收成界面直接能画的字典。纯函数，不连机器人。"""
probe = probe or {}
q = _seven(joints) or _seven(probe.get("q"))
op_status = str(probe.get("op_status") or "")
mode = str(probe.get("mode") or "")
manual = "MANUAL" in op_status.upper()
if stack_up and q is not None:
enabled = True
level = "ok"
summary = "控制栈在发关节角，这台臂已经接上。"
if probe.get("_shared_stack"):
summary = "控制栈在发关节角，但看不出是 89 还是 91，两台显示的是同一份。"
source = "控制栈 /joint_states"
elif stack_up:
enabled = False
level = "fail"
summary = "控制栈在跑，但没有关节数据。通常是还没使能，一直在等 operational。"
source = "控制栈"
elif bridge_up and not probe:
enabled = False
level = "warn"
summary = "软拖拽桥占着 RDK，界面不再另开一条连接。"
source = "拖拽桥"
q = None
elif probe.get("connected"):
enabled = bool(probe.get("operational"))
level = str(probe.get("level") or ("ok" if enabled else "fail"))
summary = str(probe.get("summary") or ("已使能" if enabled else "未使能"))
source = "RDK 直读（未使能，只查询）"
else:
enabled = False
level = "fail" if error else "warn"
summary = error or str(probe.get("summary") or probe.get("error") or "还没连上 RDK")
source = "RDK 直读" if probe else ""
hot = (not enabled) or manual
return {
"arm": arm,
"level": level,
"enabled": enabled,
"summary": summary,
"prompt": REMOTE_PROMPT,
"prompt_hot": hot,
"mode": mode,
"op_status": op_status,
"fault": probe.get("fault") if probe else None,
"estop_released": probe.get("estop_released") if probe else None,
"q": q,
"points": fk_points(q) if q else fk_points([0.0] * 7),
"pose_live": q is not None,
"source": source,
"stack_up": stack_up,
"stamp": time.time(),
}
def link_frames(q: list[float]) -> dict[str, tuple]:
"""各连杆坐标系，和 ``rizon_arm.xacro`` 的关节顺序一致。
返回 ``base_link`` 与 ``link1``…``link7``。每个值是 ``(旋转, 原点)``，
旋转把连杆坐标变到基座坐标。厂家网格就画在这些坐标系里。
"""
kin = _kinematics()
rot = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
pos = (0.0, 0.0, 0.0)
frames: dict[str, tuple] = {"base_link": (rot, pos)}
for index, angle in enumerate(q[:7]):
spec = kin[_JOINTS[index]]
pos = _add(pos, _apply(rot, (spec["x"], spec["y"], spec["z"])))
rot = _mul(rot, _rpy(spec["roll"], spec["pitch"], spec["yaw"]))
rot = _mul(rot, _axis_rot(_AXES[index], float(angle)))
frames[f"link{index + 1}"] = (rot, pos)
return frames
def fk_points(q: list[float]) -> list[tuple[float, float, float]]:
"""按 Rizon4 的 default_kinematics 做正运动学，返回基座到法兰的点（米）。
关节轴与 ``rizon_arm.xacro`` 一致：1/3/5/7 绕 z，2/4/6 绕 y。
URDF 顺序是先平移（父坐标系），再 rpy，再绕关节轴转 q。
"""
frames = link_frames(q)
points = [frames["base_link"][1]]
points.extend(frames[f"link{index}"][1] for index in range(1, 8))
_rot, pos = _flange_pose(frames)
points.append(pos)
return points
def display_frames(q: list[float], workpiece: tuple | None = None) -> list[dict]:
"""界面要画的坐标系，都在基座系里。
第一套是基座。第二套默认是法兰（旋转与 ``link7`` 相同，原点再加
``link7_to_flange``）。传入工件系时，第二套改画在工件上，不再画法兰。
"""
frames = link_frames(q)
base_rot, base_pos = frames["base_link"]
shown = [{"name": "基座", "rot": base_rot, "pos": base_pos}]
if workpiece is None:
rot, pos = _flange_pose(frames)
shown.append({"name": "法兰", "rot": rot, "pos": pos})
return shown
rot, pos = workpiece
if not isinstance(rot, tuple) or len(rot) != 3 or not isinstance(pos, tuple) or len(pos) != 3:
raise ValueError("工件坐标系要有旋转和原点")
shown.append({"name": "工件", "rot": rot, "pos": pos})
return shown
def workpiece_frame(robot: dict | None) -> tuple | None:
"""有完整工件系时返回 ``(旋转, 原点)``。没有就返回 None。
``workpiece_surface_z_m`` 只是上表面高度，没有原点的 XY，也没有朝向，
不能当成坐标系。配了 ``gauge_assembly.workpiece_frame`` 却缺 ``xyz`` 或
``rpy`` 时直接报错，不用单位阵把缺的朝向补上。
``xyz`` 单位米，``rpy`` 单位弧度，都写在基座系，旋转顺序与连杆 ``rpy`` 相同。
"""
if not isinstance(robot, dict):
return None
gauge = robot.get("gauge_assembly")
if not isinstance(gauge, dict) or "workpiece_frame" not in gauge:
return None
raw = gauge.get("workpiece_frame")
if not isinstance(raw, dict):
raise ValueError("工件坐标系要写成 xyz 和 rpy，各三个数")
xyz = raw.get("xyz")
rpy = raw.get("rpy")
if not _three_floats(xyz) or not _three_floats(rpy):
raise ValueError("工件坐标系要有 xyz 和 rpy，各三个数，单位是米和弧度，写在基座系里")
return _rpy(float(rpy[0]), float(rpy[1]), float(rpy[2])), (
float(xyz[0]), float(xyz[1]), float(xyz[2]),
)
def _flange_pose(frames: dict) -> tuple:
"""法兰位姿。旋转就是 ``link7`` 的旋转，平移来自 ``link7_to_flange``。"""
rot, pos = frames["link7"]
flange = _kinematics()["link7_to_flange"]
pos = _add(pos, _apply(rot, (float(flange["x"]), float(flange["y"]), float(flange["z"]))))
return rot, pos
def _three_floats(raw: Any) -> bool:
if not isinstance(raw, (list, tuple)) or len(raw) != 3:
return False
return all(isinstance(item, (int, float)) and not isinstance(item, bool) for item in raw)
def _kinematics() -> dict[str, dict[str, float]]:
global _KIN
if _KIN is not None:
return _KIN
root = ros2_root()
candidates = (
root / "src/flexiv_description/config/Rizon4/default_kinematics.yaml",
root / "install/flexiv_description/share/flexiv_description/config/Rizon4/default_kinematics.yaml",
)
path = next((item for item in candidates if item.is_file()), None)
if path is None:
raise FileNotFoundError("找不到 Rizon4 default_kinematics.yaml")
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
kin = data.get("kinematics") or {}
_KIN = kin
return kin
def _rpy(roll: float, pitch: float, yaw: float) -> tuple:
return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
def _rx(angle: float) -> tuple:
c, s = math.cos(angle), math.sin(angle)
return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
def _ry(angle: float) -> tuple:
c, s = math.cos(angle), math.sin(angle)
return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
def _rz(angle: float) -> tuple:
c, s = math.cos(angle), math.sin(angle)
return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
def _axis_rot(axis: tuple[float, float, float], angle: float) -> tuple:
x, y, z = axis
if abs(z) > 0.5:
return _rz(angle if z > 0 else -angle)
if abs(y) > 0.5:
return _ry(angle if y > 0 else -angle)
return _rx(angle if x > 0 else -angle)
def _mul(a: tuple, b: tuple) -> tuple:
return tuple(
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
for i in range(3)
)
def _apply(rot: tuple, vec: tuple[float, float, float]) -> tuple[float, float, float]:
return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))
def _add(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]:
return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
def _seven(raw: Any) -> list[float] | None:
if not isinstance(raw, list) or len(raw) < 7:
return None
try:
q = [float(v) for v in raw[:7]]
except (TypeError, ValueError):
return None
if any(v != v or abs(v) > 12.6 for v in q):
return None
return q
station/src/station/operator_camera_view.py
+928
View File
@@ -0,0 +1,928 @@
"""工位程序里的相机预览。
只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
收图、颜色转换和画框都在单独的 `station.cam_bridge` 进程里。界面进程不 `import rclpy`。
预览是独立的消费端，不打开物理相机；只订当前页面所需的流。
深度是伪彩色，没打开深度时不订阅。手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。
"""
from __future__ import annotations
import json
import os
import signal
import struct
import subprocess
import sys
import tempfile
import threading
import time
from multiprocessing import shared_memory
from pathlib import Path
import numpy as np
# 界面预览与算法原图分开：显示限制到 848 像素宽，算法仍使用原始分辨率。
_COLOR_WIDTH = 848
# 深度上色不是要看的画质。全幅每帧上色会把刷新占满，所以这一路仍抽稀。
_DEPTH_WIDTH = 480
# 手眼只在正在看的那一页编码，宽度跟彩色预览一样，不另缩一档。
_JPEG_WIDTH = 1280
# 只留给还在用 ``accept_frame`` 的调用。彩色实况不再用它抽帧：
# 相机大约 30 帧，再按这个间隔丢一次，画面会掉到十几帧。
_INTERVAL = 1.0 / 30.0
_DET_HOLD_S = 1.5
_LOCK = threading.Lock()
_STOP = threading.Event()
_NODE = None
_TIMER = None
_JPEG: dict[str, bytes] = {}
_PREVIEW: dict[str, tuple[bytes, int, int]] = {}
_PREVIEW_WANT: set[str] = set()
_JPEG_WANT: set[str] = set()
_NEXT: dict[str, float] = {}
_DET: dict[str, tuple[float, list[float]]] = {}
# 没画框的预览。检测一到就在这张上重画，不必等下一帧相机。
_CLEAN: dict[str, dict] = {}
# 每路最近一秒的到达时刻，用来算帧率。值是 (宽, 高, 帧/秒)，宽高是相机原图。
_META: dict[str, tuple[int, int, float]] = {}
_STAMPS: dict[str, list[float]] = {}
# 每路最新原图。收图线程只往这里放，画图线程只取最新的一张。
_RAW: dict[str, dict] = {}
_SLOTS: dict[str, list] = {}
_SLOT_AT: dict[str, int] = {}
_PAINTED: dict[str, int] = {}
_WAKE = threading.Event()
_PAINT_THREAD: threading.Thread | None = None
_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
# 和 cam_bridge 约定的共享内存头：序号、宽、高、字节数。序号为奇数表示正在写。
_SHM_MAX = 1280 * 720 * 3
_SHM_HEAD = struct.Struct("<QIII")
_BRIDGE: subprocess.Popen | None = None
_BRIDGE_DIR: Path | None = None
_POLL_THREAD: threading.Thread | None = None
_WANT_PENDING: dict | None = None
_SHM_OPEN: dict[str, shared_memory.SharedMemory] = {}
_SHM_SEQ: dict[str, int] = {}
def preview_targets(site: dict) -> list[dict]:
"""登记各臂腕部和共享手外相机；当前臂由界面选择。"""
from arm_application.site_io import listed_arms, overhead_camera
found: list[dict] = []
seen: set[str] = set()
for arm in listed_arms(site):
name = str(arm.get("camera") or "").strip().strip("/")
if not name or name.startswith("sim") or name in seen:
continue
cams = site.get("cameras") or {}
cfg = cams.get(name) or {}
tag = str(cfg.get("model") or "D405").upper()
seen.add(name)
found.append({"id": name, "label": f"{arm['label']}腕部 {tag}", "role": "wrist", "arm": arm['id']})
cam, _others = overhead_camera(site or {}, None)
oname = str((cam or {}).get("name") or "").strip().strip("/")
if oname and not oname.startswith("sim") and oname not in seen:
tag = str((cam or {}).get("model") or "D435").strip().upper() or "D435"
found.append({"id": oname, "label": f"手外 {tag}", "role": "overhead"})
return found
def sim_preview_targets(site: dict) -> list[dict]:
"""仿真腕部和仿真手外。画面进主界面，不另开 Gazebo 的相机窗。"""
from arm_application.site_io import overhead_camera
found: list[dict] = []
robots = ((site or {}).get("robot") or {}).get("robots") or []
cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
for robot in robots:
if not isinstance(robot, dict):
continue
if str(robot.get("sn") or "").strip().lower() != "sim":
continue
cam = robot.get("camera") if isinstance(robot.get("camera"), dict) else {}
name = str((cam or {}).get("name") or "").strip().strip("/")
if not name:
continue
cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
found.append({"id": name, "label": f"仿真左腕 {tag}", "role": "wrist", "source": "sim"})
break
overhead, _others = overhead_camera(site or {}, {"arm": "sim"})
oname = str((overhead or {}).get("name") or "").strip().strip("/")
if oname.startswith("sim"):
tag = str((overhead or {}).get("model") or "D435").strip().upper() or "D435"
found.append({"id": oname, "label": f"仿真手外 {tag}", "role": "overhead", "source": "sim"})
return found
def stream_targets(site: dict | None = None) -> list[dict]:
"""列出可用流；实际订阅由当前消费者需求决定。"""
if site is None:
from arm_application.site_io import load_site_dict as _site
site = _site()
real = preview_targets(site)
for item in real:
item["source"] = "real"
seen = {item["id"] for item in real}
return real + [item for item in sim_preview_targets(site) if item["id"] not in seen]
def targets() -> list[dict]:
"""当前站点的主界面真机相机。规则在 ``preview_targets``。"""
from arm_application.site_io import load_site_dict as _site
return preview_targets(_site())
def sim_targets() -> list[dict]:
from arm_application.site_io import load_site_dict as _site
return sim_preview_targets(_site())
def state() -> dict:
with _LOCK:
return {
"on": _STATE["on"],
"error": _STATE["error"],
"stamp": _STATE["stamp"],
"frames": dict(_STATE["frames"]),
}
def frame(name: str, kind: str) -> bytes | None:
with _LOCK:
return _JPEG.get(f"{name}_{kind}")
def previews() -> dict[str, tuple[bytes, int, int]]:
"""当前预览。值是 ``(rgb888, 宽, 高)``，同一帧反复取到的是同一块字节。"""
with _LOCK:
return dict(_PREVIEW)
def preview_meta() -> dict[str, tuple[int, int, float]]:
"""每路相机原图的宽、高，以及最近一秒的帧率。还没来帧就是空。"""
with _LOCK:
return dict(_META)
def _mark_rate(key: str, width: int, height: int) -> None:
"""记下这一帧的原图尺寸，并用最近一秒的到达间隔算帧率。"""
now = time.monotonic()
with _LOCK:
stamps = _STAMPS.setdefault(key, [])
stamps.append(now)
cut = now - 1.0
while len(stamps) > 1 and stamps[0] < cut:
stamps.pop(0)
if len(stamps) >= 2 and stamps[-1] > stamps[0]:
fps = (len(stamps) - 1) / (stamps[-1] - stamps[0])
else:
fps = 0.0
_META[key] = (int(width), int(height), float(fps))
def watch(preview: set[str], jpeg: set[str] | None = None) -> None:
"""声明页面需求；两边都空时撤销图像与检测订阅。"""
global _PREVIEW_WANT, _JPEG_WANT
jpeg = set(jpeg or ())
preview = set(preview)
with _LOCK:
if preview == _PREVIEW_WANT and jpeg == _JPEG_WANT:
return
_PREVIEW_WANT = preview
_JPEG_WANT = jpeg
for mapping, wanted in ((_PREVIEW, preview), (_JPEG, jpeg), (_META, preview | jpeg)):
for key in set(mapping) - wanted:
mapping.pop(key, None)
for key in preview | jpeg:
_NEXT.pop(key, None)
payload = {"preview": sorted(_PREVIEW_WANT), "jpeg": sorted(_JPEG_WANT)}
_publish_want(payload)
def wants(key: str) -> tuple[bool, bool]:
"""返回 ``(要预览, 要 JPEG)``。"""
with _LOCK:
return key in _PREVIEW_WANT, key in _JPEG_WANT
def offer_color(key: str, image: np.ndarray, *, bgr: bool, role: str) -> None:
"""收一帧彩色原图。没人看就丢掉。有人看就整幅留下，不按时钟抽帧。"""
preview, jpeg = wants(key)
if not preview and not jpeg:
return
if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
return
_offer(key, image, bgr=bgr, role=role, kind="color")
def offer_depth(key: str, image: np.ndarray) -> None:
"""收一帧深度原图。伪彩色仍在画图线程里做，不堵收图。"""
preview, _jpeg = wants(key)
if not preview:
return
if image.ndim != 2 or image.shape[0] < 1 or image.shape[1] < 1:
return
_offer(key, image, bgr=False, role="", kind="depth")
def _offer(key: str, image: np.ndarray, *, bgr: bool, role: str, kind: str) -> None:
height, width = int(image.shape[0]), int(image.shape[1])
with _LOCK:
slots = _SLOTS.setdefault(key, [None, None])
index = _SLOT_AT.get(key, 0)
slot = slots[index]
if slot is None or slot.shape != image.shape or slot.dtype != image.dtype:
slot = np.empty(image.shape, dtype=image.dtype)
slots[index] = slot
np.copyto(slot, image)
seq = int(_RAW.get(key, {}).get("seq", 0)) + 1
_RAW[key] = {
"seq": seq,
"frame": slot,
"bgr": bool(bgr),
"role": role,
"kind": kind,
"w": width,
"h": height,
}
_SLOT_AT[key] = 1 - index
_mark_rate(key, width, height)
_WAKE.set()
def publish_latest() -> None:
"""把各路还没画的最新原图做成预览。中间帧直接跳过，避免越画越落后。"""
with _LOCK:
jobs = []
for key, item in _RAW.items():
if _PAINTED.get(key) == item["seq"]:
continue
jobs.append((
key,
int(item["seq"]),
np.array(item["frame"], copy=True),
bool(item["bgr"]),
str(item["role"]),
str(item["kind"]),
int(item["w"]),
int(item["h"]),
))
for key, seq, frame, bgr, role, kind, width, height in jobs:
try:
preview, jpeg = wants(key)
if kind == "depth":
if preview:
colored = depth_preview(frame)
_store(key, colored, preview=colored, jpeg=False)
elif preview or jpeg:
limit = _JPEG_WIDTH if jpeg else _COLOR_WIDTH
rgb = rgb_preview(frame, bgr=bgr, limit=limit)
shown = rgb
if preview and int(rgb.shape[1]) > _COLOR_WIDTH:
shown = rgb_preview(rgb, bgr=False, limit=_COLOR_WIDTH)
if preview:
_remember_clean(key, shown, role, width, height)
shown = _overlay_detection(shown, role, width, height)
_store(key, rgb, preview=shown if preview else None, jpeg=jpeg)
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，下一帧继续
with _LOCK:
_STATE["error"] = str(exc)
finally:
with _LOCK:
if _PAINTED.get(key, 0) < seq:
_PAINTED[key] = seq
def _paint_loop() -> None:
while not _STOP.is_set():
_WAKE.wait(0.005)
_WAKE.clear()
if _STOP.is_set():
return
try:
publish_latest()
except Exception as exc: # noqa: BLE001 —— 画图线程不能因为一帧坏掉就退出
with _LOCK:
_STATE["error"] = str(exc)
def accept_frame(key: str) -> bool:
"""这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。
彩色实况不再走这里。固定间隔会把相机自己的帧率再砍掉一截。
"""
now = time.monotonic()
with _LOCK:
if key not in _PREVIEW_WANT and key not in _JPEG_WANT:
return False
if now < _NEXT.get(key, 0.0):
return False
_NEXT[key] = now + _INTERVAL
return True
def paint_detection(
rgb: np.ndarray,
xyxy: list[float] | tuple,
src_width: int,
src_height: int,
) -> np.ndarray:
"""把推理结果画到相机预览上：绿框、画面中心、跟踪红点、置信度和距离。
``xyxy`` 前四项是原图像素的框。够长时第 5 项是置信度，第 7、8 项是跟踪点，
第 9 项是距离（米）。空框原样返回。
"""
import cv2
if rgb.ndim != 3 or src_width <= 0 or src_height <= 0 or len(xyxy) < 4:
return rgb
scale_x = rgb.shape[1] / float(src_width)
scale_y = rgb.shape[0] / float(src_height)
x1 = int(round(float(xyxy[0]) * scale_x))
y1 = int(round(float(xyxy[1]) * scale_y))
x2 = int(round(float(xyxy[2]) * scale_x))
y2 = int(round(float(xyxy[3]) * scale_y))
height, width = rgb.shape[:2]
x1 = max(0, min(width - 1, x1))
x2 = max(0, min(width - 1, x2))
y1 = max(0, min(height - 1, y1))
y2 = max(0, min(height - 1, y2))
if x2 <= x1 or y2 <= y1:
return rgb
out = np.array(rgb, copy=True)
thick = max(2, width // 180)
cv2.rectangle(out, (x1, y1), (x2, y2), (80, 220, 120), thick)
cv2.drawMarker(
out,
(width // 2, height // 2),
(255, 220, 60),
cv2.MARKER_CROSS,
max(8, width // 24),
1,
)
if len(xyxy) >= 8:
px = int(round(float(xyxy[6]) * scale_x))
py = int(round(float(xyxy[7]) * scale_y))
px = max(0, min(width - 1, px))
py = max(0, min(height - 1, py))
cv2.circle(out, (px, py), max(3, width // 80), (255, 48, 48), -1)
if len(xyxy) >= 5:
conf = float(xyxy[4])
dist = f" z={float(xyxy[8]):.3f}m" if len(xyxy) >= 9 else ""
cv2.putText(
out,
f"{conf:.2f}{dist}",
(x1, max(14, y1 - 6)),
cv2.FONT_HERSHEY_SIMPLEX,
0.45,
(80, 220, 120),
1,
cv2.LINE_AA,
)
return out
def inference_status() -> str:
"""腕部和手外最近一次推理框。超过保持时间就当没有。"""
now = time.monotonic()
with _LOCK:
items = dict(_DET)
parts = []
for role, title in (("wrist", "腕部"), ("overhead", "手外")):
item = items.get(role)
if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 5:
parts.append(f"{title} 没有框")
continue
parts.append(f"{title} 置信 {float(item[1][4]):.2f}")
return "推理 " + " ".join(parts)
def _remember_detection(role: str, data: list[float]) -> None:
with _LOCK:
if len(data) < 4:
_DET.pop(role, None)
else:
_DET[role] = (time.monotonic(), data)
_stamp_role(role)
def _remember_clean(key: str, rgb: np.ndarray, role: str, src_w: int, src_h: int) -> None:
with _LOCK:
_CLEAN[key] = {
"rgb": np.array(rgb, copy=True),
"role": role,
"src_w": int(src_w),
"src_h": int(src_h),
}
def _overlay_detection(rgb: np.ndarray, role: str, src_width: int, src_height: int) -> np.ndarray:
now = time.monotonic()
with _LOCK:
item = _DET.get(role)
if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 4:
return rgb
return paint_detection(rgb, item[1], src_width, src_height)
def _stamp_role(role: str) -> None:
"""检测更新后，把框画回这一路相机已经在显示的画面上。"""
now = time.monotonic()
with _LOCK:
frames = [(key, dict(meta)) for key, meta in _CLEAN.items() if meta.get("role") == role]
item = _DET.get(role)
fresh = item is not None and now - item[0] <= _DET_HOLD_S and len(item[1]) >= 4
for key, meta in frames:
shown = meta["rgb"]
if fresh:
shown = paint_detection(shown, item[1], meta["src_w"], meta["src_h"])
with _LOCK:
_PREVIEW[key] = (np.ascontiguousarray(shown).tobytes(), int(shown.shape[1]), int(shown.shape[0]))
_STATE["frames"][key] = True
_STATE["stamp"] = time.time()
def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _COLOR_WIDTH) -> np.ndarray:
"""缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。"""
import cv2
if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
raise ValueError("彩色预览要的是高×宽×3")
view = image
width = int(view.shape[1])
if width > limit:
height = max(1, int(round(int(view.shape[0]) * limit / float(width))))
view = cv2.resize(view, (limit, height), interpolation=cv2.INTER_AREA)
if bgr:
return cv2.cvtColor(view, cv2.COLOR_BGR2RGB)
if not view.flags["OWNDATA"] or not view.flags["C_CONTIGUOUS"]:
return np.ascontiguousarray(view)
return view
def depth_preview(depth: np.ndarray, limit: int = _DEPTH_WIDTH) -> np.ndarray:
"""uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。"""
import cv2
if depth.ndim != 2 or depth.shape[0] < 1 or depth.shape[1] < 1:
raise ValueError("深度预览要的是高×宽")
step = max(1, int(depth.shape[1]) // limit)
small = depth[::step, ::step]
valid = small > 0
gray = np.zeros(small.shape, np.uint8)
if bool(valid.any()):
sample = small[valid]
if sample.size > 8000:
sample = sample[:: max(1, sample.size // 8000)]
lo, hi = np.percentile(sample.astype(np.float32, copy=False), [5, 95])
hi = max(float(hi), float(lo) + 1.0)
norm = np.clip((small.astype(np.float32) - float(lo)) / (hi - float(lo)), 0.0, 1.0)
gray = (norm * 255.0).astype(np.uint8)
gray[~valid] = 0
colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
colored[~valid] = 0
return rgb_preview(colored, bgr=True, limit=limit)
def _jpeg_bgr(rgb: np.ndarray) -> bytes | None:
import cv2
if rgb.size == 0:
return None
ok, buf = cv2.imencode(
".jpg",
cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR),
[int(cv2.IMWRITE_JPEG_QUALITY), 70],
)
if not ok:
return None
return buf.tobytes()
def _as_array(data, dtype, count: int) -> np.ndarray:
"""显式复制消息缓冲，让异步绘图不依赖 ROS 回调的生命周期。"""
raw = bytes(data)
arr = np.frombuffer(raw, dtype=dtype, count=count)
if arr.size < count:
raise ValueError("图像字节数不够")
return arr.copy()
def _color_view(msg) -> np.ndarray | None:
if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
return None
need = int(msg.height) * int(msg.width) * 3
arr = _as_array(msg.data, np.uint8, need)
return arr.reshape(int(msg.height), int(msg.width), 3)
def _depth_view(msg) -> np.ndarray | None:
if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
return None
need = int(msg.height) * int(msg.width)
arr = _as_array(msg.data, np.uint16, need)
return arr.reshape(int(msg.height), int(msg.width))
def _store(key: str, rgb: np.ndarray, *, preview: np.ndarray | None, jpeg: bool) -> None:
encoded = _jpeg_bgr(rgb) if jpeg else None
with _LOCK:
if preview is not None:
_PREVIEW[key] = (preview.tobytes(), int(preview.shape[1]), int(preview.shape[0]))
if encoded:
_JPEG[key] = encoded
if preview is not None or encoded:
_STATE["frames"][key] = True
_STATE["stamp"] = time.time()
_STATE["error"] = ""
def pump() -> None:
"""在 Qt 主线程收一批已经到的图。不等待，避免把界面卡住。"""
node = _NODE
if node is None or _STOP.is_set():
return
import rclpy
try:
node.sync_depth()
for _ in range(8):
rclpy.spin_once(node, timeout_sec=0.0)
except Exception as exc: # noqa: BLE001 —— 一帧失败不能把窗口打掉
with _LOCK:
_STATE["error"] = str(exc)
def _open_node(cams: list[dict]):
import rclpy
from rclpy.node import Node
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
from sensor_msgs.msg import Image
from std_msgs.msg import Float32MultiArray
from arm_application.site_io import arm_short_id, load_site_dict
configured_arm=arm_short_id(os.environ.get("SITE_ROBOT_SN") or (load_site_dict().get('robot') or {}).get('sn'))
def camera_role(cam):
if cam.get('arm') and cam['arm'] != configured_arm:
return '' # Legacy global detections have no arm identity; never overlay another arm's result.
return str(cam.get('role') or 'wrist')
class _Node(Node):
def __init__(self) -> None:
super().__init__("operator_camera_view")
det_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=5,
reliability=ReliabilityPolicy.RELIABLE,
)
from station.subscriptions import SubscriptionSet
self._streams = SubscriptionSet()
self._detections = SubscriptionSet()
self._last_frame = {}
self._det_qos = det_qos
def sync_streams(self) -> None:
from station.subscriptions import demanded_streams
with _LOCK:
preview, jpeg = set(_PREVIEW_WANT), set(_JPEG_WANT)
wanted = demanded_streams(cams, preview, jpeg)
by_id = {cam['id']: cam for cam in cams}
def create(key):
name, kind = key.rsplit('_', 1)
cam = by_id[name]
topic = f"/{name}/color/image_raw" if kind == 'color' else f"/{name}/aligned_depth_to_color/image_raw"
callback = self._color(name, camera_role(cam)) if kind == 'color' else self._depth(name)
return self.create_subscription(Image, topic, callback, qos_profile_sensor_data)
self._streams.sync(wanted, create, self.destroy_subscription)
roles = {camera_role(by_id[key.rsplit('_',1)[0]]) for key in wanted if key.endswith('_color')} - {''}
def detection(role):
topic = 'workpiece/detection_overhead' if role == 'overhead' else 'workpiece/detection'
return self.create_subscription(Float32MultiArray, topic, self._det(role), self._det_qos)
self._detections.sync(roles, detection, self.destroy_subscription)
with _LOCK:
for role in set(_DET) - roles: _DET.pop(role, None)
# Purge old frames so returning to a page cannot label stale images as live.
with _LOCK:
for mapping in (_PREVIEW, _JPEG, _RAW, _META, _SLOTS, _SLOT_AT, _CLEAN, _PAINTED, _STAMPS):
for key in set(mapping) - wanted:
mapping.pop(key, None)
for key in set(_STATE['frames']) - wanted:
_STATE['frames'].pop(key, None)
def sync_depth(self) -> None:
self.sync_streams()
def _accept(self, key):
preview, jpeg = wants(key)
if not preview and not jpeg:
return False
now = time.monotonic()
# UI preview is independent of perception rate. Avoid copying full-resolution frames we won't render.
fps = 10.0 if jpeg else 15.0
if now - self._last_frame.get(key, 0) < 1.0/fps:
return False
self._last_frame[key] = now
return True
def _det(self, role: str):
def _cb(msg: Float32MultiArray) -> None:
_remember_detection(role, [float(v) for v in msg.data])
return _cb
def _color(self, name: str, role: str):
key = f"{name}_color"
def _cb(msg: Image) -> None:
# 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
try:
if not self._accept(key):
return
view = _color_view(msg)
if view is None:
return
offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
with _LOCK:
_STATE["error"] = str(exc)
return _cb
def _depth(self, name: str):
key = f"{name}_depth"
def _cb(msg: Image) -> None:
try:
if not self._accept(key):
return
view = _depth_view(msg)
if view is None:
return
offer_depth(key, view)
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
with _LOCK:
_STATE["error"] = str(exc)
return _cb
if not rclpy.ok():
rclpy.init()
return _Node()
def _publish_want(payload: dict) -> None:
"""Queue demand; the preview I/O thread writes it outside the GUI thread."""
global _WANT_PENDING
with _LOCK:
_WANT_PENDING = payload
def _flush_want() -> None:
global _WANT_PENDING
with _LOCK:
payload = _WANT_PENDING
directory = _BRIDGE_DIR
if directory is None or payload is None:
return
path = directory / "want.json"
temporary = directory / "want.json.tmp"
temporary.write_text(json.dumps(payload, ensure_ascii=False), encoding="utf-8")
temporary.replace(path)
with _LOCK:
if _WANT_PENDING is payload:
_WANT_PENDING = None
def _shm_read(shm: shared_memory.SharedMemory) -> tuple[int, int, int, bytes] | None:
"""读一帧。写到一半或者对不上号就丢掉，界面继续用上一张。"""
buf = shm.buf
seq, width, height, nbytes = _SHM_HEAD.unpack_from(buf, 0)
if seq % 2 or nbytes <= 0 or nbytes > _SHM_MAX or width <= 0 or height <= 0:
return None
blob = bytes(buf[_SHM_HEAD.size:_SHM_HEAD.size + nbytes])
seq2, width2, height2, nbytes2 = _SHM_HEAD.unpack_from(buf, 0)
if seq2 != seq or width2 != width or height2 != height or nbytes2 != nbytes:
return None
return int(seq), int(width), int(height), blob
def _attach(name: str) -> shared_memory.SharedMemory | None:
opened = _SHM_OPEN.get(name)
if opened is not None:
return opened
try:
opened = shared_memory.SharedMemory(name=name, create=False)
from multiprocessing import resource_tracker
resource_tracker.unregister(opened._name, "shared_memory")
except FileNotFoundError:
return None
_SHM_OPEN[name] = opened
return opened
def _ingest() -> None:
"""把预览进程写好的画面抄进界面用的字典。这里不碰 `rclpy`。"""
directory = _BRIDGE_DIR
if directory is None:
return
path = directory / "meta.json"
try:
payload = json.loads(path.read_text(encoding="utf-8"))
except (OSError, json.JSONDecodeError):
return
if not isinstance(payload, dict):
return
now = time.monotonic()
with _LOCK:
_STATE["error"] = str(payload.get("error") or "")
expired = now - float(payload.get("exported_monotonic", now)) > 2
ages = payload.get("ages") or {}
wanted = _PREVIEW_WANT | _JPEG_WANT
current = {key for key in wanted if not expired and float(ages.get(key, 0)) < 2}
frames = payload.get("frames") if isinstance(payload.get("frames"), dict) else {}
stores = {spec.get("store") for spec in frames.values() if isinstance(spec, dict)} & current
for mapping in (_PREVIEW, _JPEG, _META):
for key in set(mapping) - stores:
mapping.pop(key, None)
if expired: _STATE["error"] = "预览进程超过 2 秒未更新"
meta = payload.get("meta") if isinstance(payload.get("meta"), dict) else {}
for key, triple in meta.items():
if key not in stores: continue
if isinstance(triple, list) and len(triple) >= 3:
_META[str(key)] = (int(triple[0]), int(triple[1]), float(triple[2]))
fresh: dict[str, tuple[float, list]] = {}
det = payload.get("det") if isinstance(payload.get("det"), dict) else {}
for role, item in det.items():
if not isinstance(item, dict):
continue
data = item.get("data")
if not isinstance(data, list):
continue
prev = _DET.get(str(role))
if prev is not None and list(prev[1]) == list(data):
fresh[str(role)] = prev
else:
fresh[str(role)] = (now - float(item.get("age_s") or 0.0), [float(v) for v in data])
_DET.clear()
_DET.update(fresh)
frames = payload.get("frames") if isinstance(payload.get("frames"), dict) else {}
for key, spec in frames.items():
if not isinstance(spec, dict):
continue
name = str(spec.get("name") or "")
kind = str(spec.get("kind") or "")
store = str(spec.get("store") or key)
if store not in stores:
continue
shm = _attach(name) if name else None
if shm is None:
continue
got = _shm_read(shm)
if got is None or _SHM_SEQ.get(name) == got[0]:
continue
_SHM_SEQ[name] = got[0]
_seq, width, height, blob = got
with _LOCK:
if kind == "jpeg":
_JPEG[store] = blob
else:
_PREVIEW[store] = (blob, width, height)
_STATE["frames"][store] = True
_STATE["stamp"] = time.time()
def _bridge_died(code: int) -> str:
directory = _BRIDGE_DIR
tail = ""
if directory is not None:
log = directory / "bridge.log"
try:
lines = log.read_text(encoding="utf-8", errors="replace").splitlines()
except OSError:
lines = []
tail = next((line for line in reversed(lines) if line.strip()), "")
if tail:
return f"相机预览进程退出了（{code}）：{tail}"
return f"相机预览进程退出了（{code}）"
def _poll_loop() -> None:
while not _STOP.wait(0.02):
proc = _BRIDGE
if proc is None:
return
code = proc.poll()
if code is not None:
text = _bridge_died(code)
with _LOCK:
_STATE["on"] = False
_STATE["error"] = text
return
try:
_flush_want()
_ingest()
except Exception as exc: # noqa: BLE001 —— 抄一帧失败不能把窗口打掉
with _LOCK:
_STATE["error"] = str(exc)
def _drop_shm() -> None:
for shm in list(_SHM_OPEN.values()):
try:
shm.close()
except Exception:
pass
_SHM_OPEN.clear()
_SHM_SEQ.clear()
def _stop_bridge() -> None:
"""停掉预览进程。界面进程里没有要销毁的 `rclpy` 节点。"""
global _BRIDGE, _BRIDGE_DIR, _POLL_THREAD
_STOP.set()
proc = _BRIDGE
_BRIDGE = None
if proc is not None and proc.poll() is None:
try:
os.killpg(proc.pid, signal.SIGTERM)
except OSError:
proc.terminate()
try:
proc.wait(timeout=1.5)
except subprocess.TimeoutExpired:
try:
os.killpg(proc.pid, signal.SIGKILL)
except OSError:
proc.kill()
proc.wait(timeout=1.5)
poll = _POLL_THREAD
_POLL_THREAD = None
if poll is not None and poll is not threading.current_thread():
poll.join(timeout=1.5)
_drop_shm()
directory = _BRIDGE_DIR
_BRIDGE_DIR = None
if directory is not None:
for child in directory.glob("*"):
try:
child.unlink()
except OSError:
pass
try:
directory.rmdir()
except OSError:
pass
with _LOCK:
_STATE["on"] = False
def start() -> str:
"""拉起预览进程。本进程只读它写好的画面，不 `import rclpy`。"""
global _BRIDGE, _BRIDGE_DIR, _POLL_THREAD
cams = stream_targets()
if not cams:
with _LOCK:
_STATE["error"] = "site 里没有可预览的真机相机"
return _STATE["error"]
if _BRIDGE is not None and _BRIDGE.poll() is None:
return "相机预览已经在程序里"
_stop_bridge()
directory = Path(tempfile.mkdtemp(prefix="orisys_cam_"))
_BRIDGE_DIR = directory
with _LOCK:
payload = {"preview": sorted(_PREVIEW_WANT), "jpeg": sorted(_JPEG_WANT)}
_STATE["on"] = True
_STATE["error"] = ""
_publish_want(payload)
_STOP.clear()
log_f = open(directory / "bridge.log", "ab", buffering=0)
try:
try:
_BRIDGE = subprocess.Popen(
[sys.executable, "-m", "station.child_process", str(os.getpid()),
sys.executable, "-m", "station.cam_bridge", str(directory)],
start_new_session=True,
stdin=subprocess.DEVNULL,
stdout=log_f,
stderr=subprocess.STDOUT,
)
except OSError as exc:
with _LOCK:
_STATE["on"] = False
_STATE["error"] = str(exc)
return str(exc)
finally:
log_f.close()
_POLL_THREAD = threading.Thread(target=_poll_loop, daemon=True, name="cam-poll")
_POLL_THREAD.start()
names = "、".join(cam["label"] for cam in cams)
return "预览已启动；仅订当前页所需彩色/深度，不含红外。"
def stop() -> None:
_stop_bridge()
station/src/station/operator_console.html
+858
View File
@@ -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>
station/src/station/operator_console.py
+7
View File
@@ -0,0 +1,7 @@
"""Compatibility entry point. All station commands now go through the backend."""
from station.runtime_client import catalog, status, run_action, run_gripper_tune, stop_started
from arm_application.site_io import ros2_root
ROOT = ros2_root()
if __name__ == "__main__":
raise SystemExit("旧 HTTP/脚本控制台已退役，请运行 bash station/run_station.sh")
station/src/station/operator_console_logo.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

station/src/station/operator_gripper_mesh.py
+138
View File
@@ -0,0 +1,138 @@
"""大寰夹爪的一比一外形。
左臂按下载包 ``PGEA-3D-Model-and-2D-Drawing-4`` 里的
``PGEA-100-26-W-F`` 尺寸图。图签就是这个型号，总长 138、宽 55、厚 29，
安装端抱闸约 20，主体 78，手指伸出约 40，闭合缝 1.2。
现场装的是 ``PGEA-100-20-W``，这包里没有 100-20 的图，开口仍用
``site.yaml`` 的 20 mm，不用图上的 26。
同包另一张完整的带抱闸图是 ``PGEA-100-40-W-F``（手指更长，张开约 40.8）。
右臂是 ``RGI-100-30``，这包里没有旋转爪的图，仍按手册外形。
"""
from __future__ import annotations
import math
# 米。来源见模块说明。长指向指尖，宽是开合方向，厚是图上的 29。
PGEA_SIZE = (0.138, 0.055, 0.029)
PGEA_STROKE_MM = 20.0
PGEA_CLOSED_GAP_M = 0.0012
PGEA_FINGER_M = 0.040
RGI_SIZE = (0.158, 0.0755, 0.047)
RGI_STROKE_MM = 30.0
# 深色底上要能分开：银壳体、蓝抱闸、橙手指、绿导轨。
# 手指以前是 0.10 的黑，乘上未连接时的 0.55 就沉进背景里。
_BODY = (0.86, 0.88, 0.91)
_BRAKE = (0.16, 0.42, 0.90)
_FINGER = (0.95, 0.46, 0.10)
_RAIL = (0.18, 0.78, 0.28)
def triangles(
rotary: bool,
permille: float | None,
rotation_deg: float | None,
stroke_mm: float | None,
) -> tuple[list[tuple], str]:
"""返回 ``(三角面, 说明)``。三角面是 ``(a, b, c, 明暗占位, rgb)``，明暗由界面再乘。"""
if rotary:
length, width, thick = RGI_SIZE
stroke = (float(stroke_mm) if stroke_mm else RGI_STROKE_MM) / 1000.0
note = (
f"RGI-100-30 外形 {length*1000:.0f}×{width*1000:.1f}×{thick*1000:.0f} mm"
f" 行程 {stroke*1000:.0f} mm。这包图纸里没有旋转爪，外形仍按手册。"
)
body_end = 0.118
brake = None
else:
length, width, thick = PGEA_SIZE
stroke = (float(stroke_mm) if stroke_mm else PGEA_STROKE_MM) / 1000.0
note = (
f"PGEA-100-26-W-F 尺寸图 外形 {length*1000:.0f}×{width*1000:.0f}×{thick*1000:.0f} mm"
f" 现场行程 {stroke*1000:.0f} mm。包里没有 100-20 的图，开口不用图上的 26。"
)
body_end = length - PGEA_FINGER_M
brake = (0.0, 0.020)
gap = stroke * (0.5 if permille is None else max(0.0, min(1.0, float(permille) / 1000.0)))
if not rotary:
gap += PGEA_CLOSED_GAP_M
parts = _solid(length, width, thick, body_end, gap, brake, rotary)
if rotary and rotation_deg:
parts = _spin_fingers(parts, math.radians(float(rotation_deg)), body_end)
out = []
for a, b, c, rgb in parts:
out.append((a, b, c, _face_shade(a, b, c), rgb))
if permille is None:
note = "没有读数。下面是半开，不是当前开口。 " + note
return out, note
def _solid(length, width, thick, body_end, gap, brake, rotary: bool):
hx, hy = thick / 2.0, width / 2.0
faces = []
z0 = 0.0
if brake is not None:
z0 = brake[1]
faces.extend(_box(0.0, brake[1], -hy, hy, -hx, hx, _BRAKE))
faces.extend(_box(z0, body_end, -hy, hy, -hx, hx, _BODY))
if rotary:
finger_t = min(0.010, hy * 0.4)
finger_h = thick * 0.72
else:
# 尺寸图端视图：指厚 6.5，指高 16，都落在 55×29 的外形里面。
finger_t = 0.0065
finger_h = 0.016
rail = 0.003
faces.extend(_box(z0, body_end, -hy * 0.62, -hy * 0.62 + rail, hx - rail, hx, _RAIL))
faces.extend(_box(z0, body_end, hy * 0.62 - rail, hy * 0.62, hx - rail, hx, _RAIL))
half = gap / 2.0
faces.extend(_box(body_end, length, -(half + finger_t), -half, -finger_h / 2, finger_h / 2, _FINGER))
faces.extend(_box(body_end, length, half, half + finger_t, -finger_h / 2, finger_h / 2, _FINGER))
return faces
def _spin_fingers(parts, angle: float, body_end: float):
c, s = math.cos(angle), math.sin(angle)
spun = []
for a, b, c0, rgb in parts:
if a[0] >= body_end - 1e-6 and b[0] >= body_end - 1e-6 and c0[0] >= body_end - 1e-6:
a, b, c0 = (_rot_z(a, c, s), _rot_z(b, c, s), _rot_z(c0, c, s))
spun.append((a, b, c0, rgb))
return spun
def _face_shade(a, b, c) -> float:
"""顶面亮、底面暗。零件颜色已经分开，明暗再把面分出来。"""
ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
nx = uy * vz - uz * vy
ny = uz * vx - ux * vz
nz = ux * vy - uy * vx
length = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
dot = (nx * 0.15 + ny * -0.25 + nz * 0.95) / length
return 0.42 + 0.58 * max(0.0, min(1.0, dot))
def _rot_z(point, c, s):
x, y, z = point
return (x, y * c - z * s, y * s + z * c)
def _box(x0, x1, y0, y1, z0, z1, rgb) -> list[tuple]:
"""轴向盒子。x 指向指尖，y 是开合方向，z 是厚度。"""
p = (
(x0, y0, z0), (x1, y0, z0), (x1, y1, z0), (x0, y1, z0),
(x0, y0, z1), (x1, y0, z1), (x1, y1, z1), (x0, y1, z1),
)
quads = (
(0, 1, 2, 3), (4, 7, 6, 5),
(0, 4, 5, 1), (3, 2, 6, 7),
(0, 3, 7, 4), (1, 5, 6, 2),
)
out = []
for a, b, c, d in quads:
out.append((p[a], p[b], p[c], rgb))
out.append((p[a], p[c], p[d], rgb))
return out
station/src/station/operator_gripper_state.py
+104
View File
@@ -0,0 +1,104 @@
"""Pure gripper presentation; transport belongs to the persistent ROS driver."""
from __future__ import annotations
import time
import math
from typing import Any
_GRIP = {0: "运动中", 1: "已到位", 2: "已夹住", 3: "已掉落"}
_ROT = {0: "转动中", 1: "已到位", 2: "受阻", 3: "曾经受阻"}
def present(
arm: str,
*,
rotary: bool,
stroke_mm: float,
reading: dict | None,
error: str = "",
) -> dict:
"""把一次读数收成界面能画的字典。不打开串口。"""
stroke = float(stroke_mm) if stroke_mm and stroke_mm > 0 else 20.0
row = reading or {}
permille = _unit(row.get("permille"), 0, 1000)
connected = bool(row.get("connected")) and permille is not None
opening = None if permille is None else permille / 1000.0 * stroke
rotation = _finite(row.get("rotation_deg")) if rotary else 0.0
grip_state = row.get("grip_state")
rot_state = row.get("rotation_state")
if not connected:
summary = error or str(row.get("error_msg") or "夹爪没连上")
else:
bits = [_GRIP.get(int(grip_state), "状态未知") if _finite(grip_state) is not None else "状态未知"]
bits.append(f"开口 {opening:.1f} mm" if opening is not None else "")
if rotary and rotation is not None:
bits.append(f"转角 {rotation:.1f}°")
if _finite(rot_state) is not None:
bits.append(_ROT.get(int(rot_state), ""))
summary = " ".join(bit for bit in bits if bit)
return {
"arm": arm,
"rotary": rotary,
"connected": connected,
"initialized": bool(row.get("initialized")) if connected else False,
"permille": permille,
"opening_mm": opening,
"stroke_mm": stroke,
"rotation_deg": rotation if rotary else None,
"force": _finite(row.get("force")),
"speed": _finite(row.get("speed")),
"grip_state": int(grip_state) if _finite(grip_state) is not None else None,
"grip_text": _GRIP.get(int(grip_state), "—") if _finite(grip_state) is not None else "—",
"rotation_text": _ROT.get(int(rot_state), "—") if rotary and _finite(rot_state) is not None else "—",
"moving": bool(row.get("moving")) if connected else False,
"gripped": bool(row.get("gripped")) or (_finite(grip_state) == 2) if connected else False,
"summary": summary,
"error": "" if connected else (error or str(row.get("error_msg") or "")),
"source": str(row.get("source") or ""),
"pose_live": connected,
"stamp": time.time(),
}
def retain_reading(
fresh: dict,
previous: dict | None,
*,
now: float | None = None,
hold_s: float = 15.0,
) -> dict:
"""这一拍没读到开口时，短时间沿用上一拍。
和机械臂同一件事：话题 echo 偶发超时会把刚读到的开口清成“没连上”，
模型和顶栏就停在那里。超过 ``hold_s`` 仍没有新读数才放开。
"""
if fresh.get("connected") or not previous or not previous.get("connected"):
return fresh
moment = time.time() if now is None else now
if moment - float(previous.get("stamp") or 0.0) > hold_s:
return fresh
kept = dict(fresh)
for key in (
"connected", "initialized", "permille", "opening_mm", "stroke_mm",
"rotation_deg", "force", "speed", "grip_state", "grip_text",
"rotation_text", "moving", "gripped", "pose_live", "rotary", "source",
):
if key in previous:
kept[key] = previous[key]
kept["summary"] = str(previous.get("summary") or fresh.get("summary") or "")
note = "沿用上次读数。"
if note not in kept["summary"]:
kept["summary"] = f"{kept['summary']} {note}".strip()
return kept
def _finite(raw: Any) -> float | None:
try:
value = float(raw)
except (TypeError, ValueError):
return None
if not math.isfinite(value):
return None
return value
def _unit(raw: Any, low: float, high: float) -> float | None:
value = _finite(raw)
if value is None:
return None
return max(low, min(high, value))
station/src/station/operator_handeye_board.py
+91
View File
@@ -0,0 +1,91 @@
"""手眼页的棋盘预览。只在已有彩色画面上画角点，不打开相机，也不动臂。"""
from __future__ import annotations
import threading
import time
import numpy as np
from arm_application.handeye_lib import BoardConfig, detect_chessboard
def coverage_fraction(corners: np.ndarray, width: int, height: int) -> float:
"""角点外接框占画面的比例。宽或高为 0 时返回 0。"""
if width <= 0 or height <= 0:
return 0.0
pts = np.asarray(corners, dtype=float).reshape(-1, 2)
if pts.size == 0:
return 0.0
area = float(pts[:, 0].max() - pts[:, 0].min()) * float(pts[:, 1].max() - pts[:, 1].min())
return area / float(width * height)
def coverage_words(fraction: float) -> str:
"""30%–70% 是示教时棋盘该占的画面。再小对不准，再大姿态拉不开。"""
if fraction < 0.30:
return "偏小，再靠近一点"
if fraction > 0.70:
return "偏大，再离远一点"
return "大小合适"
def annotate_board(jpeg: bytes, cols: int, rows: int) -> tuple[bytes, str]:
"""返回（可能画了角点的 JPEG，一行中文状态）。检测失败时 JPEG 原样返回。"""
import cv2
if cols <= 0 or rows <= 0:
return jpeg, "配置里没有这块板的内角点"
buf = np.frombuffer(jpeg, dtype=np.uint8)
image = cv2.imdecode(buf, cv2.IMREAD_COLOR)
if image is None:
return jpeg, "这一帧解不出来"
found = detect_chessboard(image, BoardConfig(int(cols), int(rows), 1.0))
if found is None:
return jpeg, "画面里没有这块板"
corners, pattern = found
drawn = image.copy()
cv2.drawChessboardCorners(drawn, pattern, corners, True)
fraction = coverage_fraction(corners, drawn.shape[1], drawn.shape[0])
ok, out = cv2.imencode(".jpg", drawn, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
blob = out.tobytes() if ok else jpeg
percent = int(round(fraction * 100))
return blob, f"棋盘已找到，约占画面 {percent}%。{coverage_words(fraction)}"
class BoardWatch:
"""后台画角点。界面线程只提交最新一帧，检测再慢也不卡住窗口。"""
def __init__(self) -> None:
self._lock = threading.Lock()
self._pending: tuple | None = None
self._result = {"token": 0, "jpeg": b"", "text": ""}
self._stop = threading.Event()
self._thread = threading.Thread(target=self._loop, daemon=True, name="handeye-board")
self._thread.start()
def submit(self, token: int, jpeg: bytes, cols: int, rows: int) -> None:
with self._lock:
if self._result["token"] == token and not self._pending:
return
self._pending = (token, jpeg, int(cols), int(rows))
def latest(self) -> tuple[bytes, str, int]:
with self._lock:
return self._result["jpeg"], self._result["text"], int(self._result["token"])
def _loop(self) -> None:
while not self._stop.is_set():
with self._lock:
job = self._pending
self._pending = None
if job is None:
time.sleep(0.05)
continue
token, jpeg, cols, rows = job
try:
blob, text = annotate_board(jpeg, cols, rows)
except Exception as exc: # noqa: BLE001 —— 检测失败不能拖死预览
blob, text = jpeg, f"棋盘检测失败：{exc}"
with self._lock:
self._result = {"token": token, "jpeg": blob, "text": text}
station/src/station/operator_tactile_view.py
+13
View File
@@ -0,0 +1,13 @@
"""Legacy preview facade. Sensor ownership belongs to the ROS tactile driver."""
from station.runtime_client import tactile_view
state = tactile_view.state
frame = tactile_view.frame
def start(site=None):
raise RuntimeError("请通过工位后端的 tactile_view_on 操作启动常驻驱动，不再单独打开传感器")
def stop():
raise RuntimeError("请通过工位后端的 tactile_view_off 操作关闭预览")
station/src/station/operator_teach.py
+68
View File
@@ -0,0 +1,68 @@
"""Teaching point persistence. Motion is owned by the backend trajectory Action client."""
from __future__ import annotations
from pathlib import Path
import yaml
import math
def _file(arm: str) -> Path:
from station.site_catalog import ROOT, _site
from arm_application.site_io import listed_arms
known = {row["id"] for row in listed_arms(_site())}
if arm not in known:
raise ValueError(f"臂号 {arm} 不在 site.yaml 里")
return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
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 not all(math.isfinite(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)
temporary = path.with_suffix(".yaml.tmp")
temporary.write_text(
yaml.safe_dump({"points": points}, allow_unicode=True, sort_keys=False),
encoding="utf-8",
)
temporary.replace(path)
return path
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}
station/src/station/processes.py
+69
View File
@@ -0,0 +1,69 @@
"""Own only processes launched by this service. Never pgrep/pkill external processes."""
from __future__ import annotations
import subprocess
import os
import sys
import threading
import time
from pathlib import Path
from station.tasks import Cancelled
class ProcessSupervisor:
def __init__(self, log_dir):
self.log_dir = Path(log_dir)
self.log_dir.mkdir(parents=True, exist_ok=True)
self._lock = threading.RLock()
self._items = {}
def start(self, key, argv, env=None, cwd=None):
with self._lock:
old = self._items.get(key)
if old and old.poll() is None:
raise RuntimeError(f"{key} 已在运行")
log = self.log_dir / (key.replace('/', '_').replace(':', '_') + '.log')
with log.open('ab') as output:
managed = [sys.executable, "-m", "station.child_process", str(os.getpid()), *argv]
proc = subprocess.Popen(managed, env=env, cwd=cwd, stdout=output, stderr=subprocess.STDOUT,
stdin=subprocess.DEVNULL, start_new_session=True)
self._items[key] = proc
return proc, log
def run(self, key, argv, event, env=None, cwd=None, timeout=None):
if event.is_set():
raise Cancelled("任务已取消")
proc, log = self.start(key, argv, env, cwd)
started = time.monotonic()
try:
while proc.poll() is None:
if event.wait(.1):
raise Cancelled("已取消工位流程；控制器停止状态仍需确认")
if timeout and time.monotonic() - started > timeout:
raise RuntimeError(f"{key} 执行超时")
code = proc.returncode
with log.open('rb') as stream:
stream.seek(max(0, log.stat().st_size - 6000))
detail = stream.read().decode('utf-8', errors='replace')
return {"ok": code == 0, "message": f"{key} 已结束（退出码 {code}）", "detail": detail, "log": str(log)}
finally:
if proc.poll() is None:
self.stop(key)
def snapshot(self):
with self._lock:
return {key: {"pid": proc.pid, "running": proc.poll() is None, "exit_code": proc.poll()}
for key, proc in self._items.items()}
def stop(self, key):
with self._lock:
proc = self._items.get(key)
if proc is None or proc.poll() is not None:
return
from arm_application.process_lifecycle import stop_process_tree
stop_process_tree(proc)
def close(self):
for key in self.snapshot():
self.stop(key)
station/src/station/ros_gateway.py
+381
View File
@@ -0,0 +1,381 @@
"""One persistent ROS client for station state, services and trajectory actions.
No CLI parsing, no RDK connection, no serial access. Import this only in the backend.
"""
from __future__ import annotations
import math
from collections import deque
import threading
import time
from station.tasks import Cancelled
class RosGateway:
def __init__(self, site):
import rclpy
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from sensor_msgs.msg import JointState
from arm_msgs.msg import GripperStatus, ArmStatus
from std_msgs.msg import Float32MultiArray
from arm_application.site_io import listed_arms, arm_short_id
self.rclpy = rclpy
rclpy.init(args=[])
self.node = Node('orisys_station_backend')
self.executor = MultiThreadedExecutor(num_threads=3)
self.executor.add_node(self.node)
self._lock = threading.RLock()
self._joints = {}
self._joint_history = {}
self._controller_lock = threading.RLock()
self._paused_by_station = set()
self._last_joint_sample = 0.
self._grippers = {}
self._arm_status = {}
self._clients = {}
self._goals = {}
self._known_goals = set()
self._active_goals = set()
self._uncertain_since = None
self._status_received = 0.
self._late_clients = set()
from station.subscriptions import SubscriptionSet
self._tactile_images = SubscriptionSet()
self._tactile_on = False
self._tactile_frames = {}
self.arms = listed_arms(site)
self.active_arm = arm_short_id((site.get('robot') or {}).get('sn'))
from rclpy.qos import qos_profile_sensor_data
self.node.create_subscription(JointState, '/joint_states', self._joint, qos_profile_sensor_data)
from action_msgs.msg import GoalStatusArray
from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy
self.node.create_subscription(GoalStatusArray, '/rizon_arm_controller/follow_joint_trajectory/_action/status',
self._goal_status, QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL,
reliability=ReliabilityPolicy.RELIABLE))
self.node.create_subscription(ArmStatus, '/arm_status', lambda msg: self._arm(self.active_arm, msg), 10)
for arm in self.arms:
aid = arm['id']
self.node.create_subscription(GripperStatus, f'/station/grippers/arm_{aid}/status',
lambda msg, a=aid: self._grip(a, msg), 10)
self.node.create_subscription(ArmStatus, f"/{arm['sn'].replace(chr(45), chr(95))}/arm_status", lambda msg, a=aid: self._arm(a, msg), 10)
if arm.get('side'):
self.node.create_subscription(GripperStatus, f"/dual_gripper_driver/{arm['side']}/status",
lambda msg, a=aid: self._grip(a, msg, 'ROS 旧驱动（只读）'), 10)
self.node.create_subscription(Float32MultiArray, '/gripper/states', self._legacy_grip, 10)
from arm_msgs.msg import TactileStatus
for side in ('left', 'right'):
self.node.create_subscription(TactileStatus, f'/tactile/{side}/status',
lambda msg, side=side: self._tactile(side, msg), 10)
self._thread = threading.Thread(target=self.executor.spin, daemon=True, name='station-ros')
self._thread.start()
def _goal_status(self, msg):
with self._lock:
self._active_goals={bytes(item.goal_info.goal_id.uuid) for item in msg.status_list if item.status in (1,2,3)}
self._known_goals.intersection_update(self._active_goals)
self._status_received=time.monotonic()
def _joint(self, msg):
now=time.monotonic()
if now-self._last_joint_sample<.025: return
self._last_joint_sample=now
by = dict(zip(msg.name, msg.position))
with self._lock:
for arm in self.arms:
names = [f"{arm['sn']}_joint{i}" for i in range(1,8)]
if all(name in by for name in names):
q = [float(by[name]) for name in names]
elif arm['id'] == self.active_arm and all(f'joint{i}' in by for i in range(1,8)):
q = [float(by[f'joint{i}']) for i in range(1,8)]
else:
continue
if all(math.isfinite(x) for x in q):
self._joints[arm['id']] = (now, q)
self._joint_history.setdefault(arm['id'], deque(maxlen=80)).append((now, q))
def _arm(self, arm, msg):
with self._lock:
self._arm_status[arm] = (time.monotonic(), {'mode': msg.mode, 'fault': msg.mode == 'error',
'error_message': msg.error_message})
def _grip(self, arm, msg, source="ROS 常驻驱动"):
with self._lock:
self._grippers[arm] = (time.monotonic(), {
'connected': bool(msg.connected), 'initialized': bool(msg.initialized),
'permille': float(msg.finger_position_per_mille), 'rotation_deg': float(msg.rotation_deg),
'force': float(msg.force), 'speed': float(msg.velocity), 'grip_state': int(msg.gripper_state),
'rotation_state': int(msg.rotation_state), 'moving': bool(msg.is_moving),
'gripped': bool(msg.is_gripped), 'error_msg': msg.error_msg, 'source': source})
def _legacy_grip(self, msg):
if len(msg.data) < 5 or not self.active_arm:
return
# Parse the known typed layout directly; never invoke ros2 topic echo.
init, state, pos, target, force = msg.data[:5]
with self._lock:
current = self._grippers.get(self.active_arm)
if current and time.monotonic() - current[0] < 2 and current[1].get('source') != 'ROS 旧驱动（只读）':
return
self._grippers[self.active_arm] = (time.monotonic(), {
'connected': True, 'initialized': init >= .5, 'permille': pos, 'force': force,
'grip_state': int(state), 'moving': int(state) == 0, 'gripped': int(state) == 2,
'source': 'ROS 旧驱动（只读）'})
def _tactile(self, side, msg):
from station import site_catalog
if msg.connected:
site_catalog._push_force(side, {'t':time.time(), 'fnormal':msg.fnormal,
'fshearx':msg.fshearx, 'fsheary':msg.fsheary})
def watch_tactile(self, on):
from sensor_msgs.msg import Image
from rclpy.qos import qos_profile_sensor_data
self._tactile_on=bool(on)
wanted={f'{side}_{kind}' for side in ('left','right') for kind in ('shear','thermal')} if on else set()
def create(key):
side,kind=key.split('_')
topic=f'/tactile/{side}/'+('vector' if kind=='shear' else 'image')
return self.node.create_subscription(Image,topic,lambda msg,key=key:self._tactile_image(key,msg),qos_profile_sensor_data)
self._tactile_images.sync(wanted,create,self.node.destroy_subscription)
if not on:
with self._lock: self._tactile_frames.clear()
def _tactile_image(self, key, msg):
if not self._tactile_on: return
from station.image_preview import jpeg_preview
try:
blob=jpeg_preview(msg)
if blob:
with self._lock: self._tactile_frames[key]=(time.monotonic(),blob)
except (ValueError,TypeError):
return
def tactile_frames(self):
now=time.monotonic()
with self._lock:
return {key:blob for key,(stamp,blob) in self._tactile_frames.items() if now-stamp<2}
def tactile_state(self):
frames=self.tactile_frames()
return {'on':self._tactile_on,'frames':{key:True for key in frames},'stamp':time.time(),
'error':'等待触觉驱动预览（image_hz > 0）' if self._tactile_on and not frames else ''}
def states(self):
from station import operator_arm_state as arm_state, operator_gripper_state as grip_state
now = time.monotonic()
arms, grips = {}, {}
with self._lock:
for arm in self.arms:
aid = arm['id']
entry = self._joints.get(aid)
live = bool(entry and now - entry[0] < 2)
q = entry[1] if live else None
row = arm_state.present(aid, stack_up=False, bridge_up=False, probe=None, joints=q)
# Joint messages prove position freshness, not enabled/Remote state.
state = self._arm_status.get(aid)
state_live = bool(state and now - state[0] < 2)
row.update(source='ROS /joint_states' if live else '', stack_up=live,
enabled=None, summary='关节状态在线；使能状态未知' if live else '未收到本臂关节状态',
mode=state[1]['mode'] if state_live else '', pose_live=live, q=q,
prompt_hot=False, prompt='使能/Remote 状态由控制驱动确认，工位不另开 RDK 连接')
arms[aid] = row
entry = self._grippers.get(aid)
fresh = bool(entry and now-entry[0] < 2)
grips[aid] = grip_state.present(aid, rotary=arm['rotary'], stroke_mm=arm['stroke_mm'],
reading=entry[1] if fresh else None, error='夹爪状态超时或驱动未启动')
grips[aid]['received_monotonic'] = entry[0] if entry else 0.
return arms, grips
def graph(self):
return {'nodes': self.node.get_node_names(),
'services': [name for name, _ in self.node.get_service_names_and_types()],
'topics': [name for name, _ in self.node.get_topic_names_and_types()]}
def publisher_count(self, topic):
return self.node.count_publishers(topic)
def call(self, service_type, name, request, event, timeout=10):
key = (service_type, name)
with self._lock:
client = self._clients.get(key)
if client is None:
client = self.node.create_client(service_type, name)
self._clients[key] = client
deadline = time.monotonic()+timeout
while not client.wait_for_service(timeout_sec=.1):
if event.is_set():
raise Cancelled('请求发送前已取消')
if time.monotonic() >= deadline:
raise RuntimeError(f'服务未就绪：{name}')
if event.is_set():
raise Cancelled('请求发送前已取消')
future = client.call_async(request)
while not future.done():
# A service has no cancellation handshake: keep ownership until its reply/timeout.
if time.monotonic() >= deadline:
raise RuntimeError(f'服务响应超时，执行结果未知，请检查设备：{name}')
time.sleep(.02)
return future.result()
def require_arm(self, arm, event):
with self._controller_lock:
if event.is_set(): raise Cancelled('运动已取消')
return self._require_arm(arm, event)
def _require_arm(self, arm, event):
from controller_manager_msgs.srv import ListControllers
with self._lock:
if self._uncertain_since is not None:
if self._status_received>self._uncertain_since and not self._active_goals and not self._late_clients:
self._uncertain_since=None
else:
raise RuntimeError('上次轨迹结果未知，尚未收到控制器空闲状态，拒绝新运动')
if self._active_goals-self._known_goals:
raise RuntimeError('控制器正在执行其他客户端的轨迹，工位不会抢占')
arms, _ = self.states()
if not arms.get(arm, {}).get('pose_live'):
raise RuntimeError('本臂关节状态未就绪，拒绝发送运动命令')
result = self.call(ListControllers, '/controller_manager/list_controllers', ListControllers.Request(), event, 3)
controller = next((c for c in result.controller if c.name == 'rizon_arm_controller'), None)
if controller and controller.state == 'inactive' and arm in self._paused_by_station:
self._switch_controller(True, event)
result = self.call(ListControllers, '/controller_manager/list_controllers', ListControllers.Request(), event, 3)
controller = next((c for c in result.controller if c.name == 'rizon_arm_controller'), None)
if controller is None or controller.state != 'active':
raise RuntimeError('rizon_arm_controller 未激活')
selected = next(a for a in self.arms if a['id']==arm)
claimed = {name.split('/')[0] for name in controller.claimed_interfaces}
expected = {f"{selected['sn']}_joint{i}" for i in range(1,8)}
if claimed:
if not expected.issubset(claimed):
raise RuntimeError('控制器占用的关节不属于当前臂，拒绝发送运动命令')
elif arm != self.active_arm:
raise RuntimeError('控制器没有报告关节归属，当前臂与启动配置不一致')
def _switch_controller(self, activate, event):
from controller_manager_msgs.srv import SwitchController
request = SwitchController.Request()
if activate: request.activate_controllers = ['rizon_arm_controller']
else: request.deactivate_controllers = ['rizon_arm_controller']
request.strictness = SwitchController.Request.STRICT
request.timeout.sec = 3
response = self.call(SwitchController, '/controller_manager/switch_controller', request, event, 5)
if not response.ok: raise RuntimeError('控制器' + ('恢复' if activate else '停止') + '失败')
def stop_arm(self, arm):
from controller_manager_msgs.srv import ListControllers
aid = arm['id']
event = threading.Event()
with self._controller_lock:
result = self.call(ListControllers, '/controller_manager/list_controllers', ListControllers.Request(), event, 3)
controller = next((c for c in result.controller if c.name == 'rizon_arm_controller'), None)
if controller is None: raise RuntimeError('未找到本臂控制器，停止未确认')
expected = {f"{arm['sn']}_joint{i}" for i in range(1, 8)}
claimed = {name.split('/')[0] for name in controller.claimed_interfaces}
if claimed and not expected.issubset(claimed):
raise RuntimeError('控制器关节不属于当前臂，拒绝停止另一台机械臂')
if not claimed and aid != self.active_arm and aid not in self._paused_by_station:
raise RuntimeError('无法确认控制器属于所选机械臂，停止未确认')
if controller.state == 'active':
self._switch_controller(False, event)
elif controller.state != 'inactive':
raise RuntimeError(f'控制器状态 {controller.state}，停止未确认')
self._paused_by_station.add(aid)
# Deactivation invokes FlexivHardwareInterface::perform_command_mode_switch -> Robot::Stop.
return {'ok': True, 'message': '已停用本臂运动控制器，等待停稳反馈'}
def confirm_stopped(self, arm, timeout=5.):
from controller_manager_msgs.srv import ListControllers
aid = arm['id']; since = time.monotonic(); deadline = since + timeout
while time.monotonic() < deadline:
with self._lock:
samples = [(t, q) for t, q in self._joint_history.get(aid, []) if t >= since]
if samples and time.monotonic() - samples[-1][0] < .3:
recent = [(t, q) for t, q in samples if t >= samples[-1][0] - .4]
if len(recent) >= 5 and recent[-1][0] - recent[0][0] >= .25:
stable = all(max(q[i] for _, q in recent) - min(q[i] for _, q in recent) < .001 for i in range(7))
if stable:
result = self.call(ListControllers, '/controller_manager/list_controllers', ListControllers.Request(), threading.Event(), 3)
controller = next((c for c in result.controller if c.name == 'rizon_arm_controller'), None)
if not controller or controller.state != 'inactive':
raise RuntimeError('控制器未保持停止状态，停止未确认')
with self._lock:
if not self._late_clients: self._uncertain_since = None
return {'ok': True, 'message': '机械臂已停止；控制器保持暂停，下一次动作时恢复'}
time.sleep(.03)
raise RuntimeError('未收到持续停稳的关节反馈，停止结果未知，请检查机械臂')
def move_joints(self, arm, joints, speed, event):
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from rclpy.action import ActionClient
self.require_arm(arm['id'], event)
if len(joints) != 7 or not all(math.isfinite(float(x)) for x in joints):
raise ValueError('需要 7 个有限的关节角')
client = ActionClient(self.node, FollowJointTrajectory, '/rizon_arm_controller/follow_joint_trajectory')
retained = False
command_since=time.monotonic()
try:
if not client.wait_for_server(timeout_sec=3):
raise RuntimeError('轨迹 Action 未就绪')
current = self.states()[0][arm['id']]['q']
if not math.isfinite(speed) or not .05 <= speed <= .35:
raise ValueError('关节速度只接受 0.05–0.35 rad/s')
seconds = max(2., max(abs(a-b) for a,b in zip(current,joints))/speed)
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = [f"{arm['sn']}_joint{i}" for i in range(1,8)]
point = JointTrajectoryPoint();point.positions = [float(v) for v in joints]
point.time_from_start.sec = int(seconds);point.time_from_start.nanosec = int((seconds-int(seconds))*1e9)
goal.trajectory.points = [point]
with self._controller_lock:
if event.is_set(): raise Cancelled('轨迹发送前已取消')
future = client.send_goal_async(goal)
deadline = time.monotonic()+10
while not future.done():
if time.monotonic()>deadline:
retained=True
self._uncertain_since=time.monotonic()
self._late_clients.add(client)
def cleanup(done=None):
self._late_clients.discard(client)
client.destroy()
def cancel_late(done):
handle=done.result()
if handle and handle.accepted:
handle.cancel_goal_async()
handle.get_result_async().add_done_callback(cleanup)
else:
cleanup()
future.add_done_callback(cancel_late)
raise RuntimeError('轨迹接收响应超时，结果未知；迟到的接收回执将触发取消')
time.sleep(.02)
handle = future.result()
if not handle.accepted: raise RuntimeError('轨迹被控制器拒绝')
self._goals[arm['id']] = handle
self._known_goals.add(bytes(handle.goal_id.uuid))
result = handle.get_result_async()
deadline = time.monotonic()+seconds+15
cancel = None
while not result.done():
if event.is_set() or time.monotonic()>deadline:
if cancel is None: cancel = handle.cancel_goal_async()
if time.monotonic()>deadline+5:
self._uncertain_since=command_since
raise RuntimeError('轨迹停止未确认，请检查设备')
time.sleep(.02)
value = result.result().result
if event.is_set(): raise Cancelled('轨迹已取消')
return {'ok': value.error_code == 0, 'message': value.error_string or f'轨迹结果 {value.error_code}'}
finally:
self._goals.pop(arm['id'], None)
if not retained: client.destroy()
def close(self):
self.executor.shutdown(timeout_sec=3)
self._thread.join(timeout=3)
for client in list(self._late_clients): client.destroy()
self._late_clients.clear()
self.node.destroy_node()
self.rclpy.shutdown()
station/src/station/rpc.py
+54
View File
@@ -0,0 +1,54 @@
"""Bounded JSON RPC over a private Unix socket; no Python object deserialization."""
import json
import socket
import socketserver
LIMIT = 4 * 1024 * 1024
PROTOCOL = 1
def call(path, method, params=None, timeout=3):
request = json.dumps({"version": PROTOCOL, "method": method, "params": params or {}}, allow_nan=False).encode() + b'\n'
if len(request) > LIMIT:
raise ValueError("请求过大")
with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
sock.settimeout(timeout)
sock.connect(str(path))
sock.sendall(request)
with sock.makefile('rb') as stream:
raw = stream.readline(LIMIT + 1)
if not raw.endswith(b'\n') or len(raw) > LIMIT:
raise RuntimeError("后端响应超过限制或连接中断")
result = json.loads(raw)
if not result.get('ok'):
raise RuntimeError(result.get('error') or '后端请求失败')
return result['result']
class Handler(socketserver.StreamRequestHandler):
def handle(self):
self.connection.settimeout(5)
try:
raw = self.rfile.readline(LIMIT + 1)
if len(raw) > LIMIT or not raw.endswith(b'\n'):
raise ValueError("无效请求长度")
request = json.loads(raw)
if request.get('version') != PROTOCOL or not isinstance(request.get('params'), dict):
raise ValueError("协议版本或参数错误")
result = self.server.dispatch(request['method'], request['params'])
reply = {"ok": True, "result": result}
except Exception as exc:
reply = {"ok": False, "error": str(exc)}
raw = json.dumps(reply, ensure_ascii=False, allow_nan=False).encode() + b'\n'
if len(raw) > LIMIT:
raw = b'{"ok":false,"error":"response too large"}\n'
try:
self.wfile.write(raw)
except (BrokenPipeError, ConnectionResetError):
pass
class Server(socketserver.ThreadingUnixStreamServer):
daemon_threads = True
block_on_close = False
request_queue_size = 16
station/src/station/runtime_client.py
+190
View File
@@ -0,0 +1,190 @@
"""Qt-facing station API. Commands use RPC; paint/refresh only read local snapshots."""
from __future__ import annotations
import atexit
import base64
import copy
import fcntl
import hashlib
import os
import subprocess
import sys
import tempfile
import threading
import time
from pathlib import Path
from arm_application.site_io import ros2_root
from station import rpc
_runtime = None
class RuntimeClient:
def __init__(self, offline=False):
root=ros2_root()
folder=Path(os.environ.get('XDG_CACHE_HOME',str(Path.home()/'.cache')))/'orisys_arm'
folder.mkdir(parents=True,exist_ok=True)
key=hashlib.sha256(str(root.resolve()).encode()).hexdigest()[:12]
self._lease=(folder/f'station-{key}.lock').open('a')
try: fcntl.flock(self._lease,fcntl.LOCK_EX|fcntl.LOCK_NB)
except BlockingIOError:
self._lease.close();raise RuntimeError('这个项目已有工位窗口运行，请先关闭它')
self.directory=Path(tempfile.mkdtemp(prefix='orisys_control_'))
self.socket=self.directory/'service.sock'
self._lock=threading.RLock();self._stop=threading.Event()
self._snapshot={};self._frames={};self._catalog={};self.error='后端启动中'
with (self.directory/'backend.log').open('ab') as output:
args=[sys.executable,'-m','station.control_service','--socket',str(self.socket),'--parent-pid',str(os.getpid())]
if offline: args.append('--offline')
self.proc=subprocess.Popen(args,stdout=output,stderr=subprocess.STDOUT,stdin=subprocess.DEVNULL,start_new_session=True)
try:
deadline=time.monotonic()+15
while True:
if self.proc.poll() is not None: raise RuntimeError(f'工位后端启动失败：{self.directory}/backend.log')
try:
rpc.call(self.socket,'ping',timeout=.5)
self._catalog=rpc.call(self.socket,'catalog')
break
except (OSError,RuntimeError):
if time.monotonic()>deadline: raise RuntimeError(f'工位后端启动超时：{self.directory}/backend.log')
time.sleep(.05)
self._thread=threading.Thread(target=self._poll,daemon=True,name='station-state-client');self._thread.start()
except Exception:
self.close();raise
def _poll(self):
while not self._stop.is_set():
try:
snap=rpc.call(self.socket,'snapshot',timeout=2)
# Preview data is a separate bounded channel and never part of a command reply.
raw=rpc.call(self.socket,'tactile_frames',timeout=2) if (snap.get('view') or {}).get('on') else {}
frames={key:base64.b64decode(value,validate=True) for key,value in raw.items()}
with self._lock:
self._snapshot=snap;self._frames=frames;self.error=''
except Exception as exc:
with self._lock: self.error=f'工位后端连接异常：{exc}'
self._stop.wait(.2)
def snapshot(self):
with self._lock:
snap=copy.deepcopy(self._snapshot)
error=self.error
if not snap: return {'backend':{'error':error or '后端状态初始化中'}}
age=time.time()-float(snap.get('now',0))
if error or age>3:
snap.setdefault('backend',{})['error']=error or '后端状态超过 3 秒未更新'
# Never display stale samples as live/connected after a backend failure.
for row in snap.get('arm_live',{}).values(): row.update(pose_live=False,enabled=None,stack_up=False,summary='状态已过期')
for row in snap.get('gripper_live',{}).values(): row.update(connected=False,pose_live=False,summary='状态已过期')
return snap
def command(self, action, arm, mode='eye_in_hand', extras=None):
response=rpc.call(self.socket,'submit',{'action':action,'arm':arm,'mode':mode,'extras':extras or {}},timeout=3)
token=response['task_id']
while not self._stop.is_set():
try:
row=rpc.call(self.socket,'task',{'task_id':token},timeout=3)
except (OSError,RuntimeError) as exc:
return {'ok':False,'message':f'后端连接中断，任务 {token} 结果未知，不会自动重发：{exc}',
'task_id':token,'task_state':'unknown'}
if not row: raise RuntimeError('后端任务记录不存在')
if row['state'] not in ('queued','running'):
result=row['result'] or {'ok':False,'message':'任务没有结果'}
return {**result,'task_id':token,'task_state':row['state']}
self._stop.wait(.1)
return {'ok':False,'message':'工位连接已关闭','task_id':token}
def close(self):
self._stop.set()
if hasattr(self,'proc') and self.proc.poll() is None:
try: rpc.call(self.socket,'stop',timeout=5)
except Exception: pass
self.proc.terminate()
try: self.proc.wait(timeout=12)
except subprocess.TimeoutExpired:
# Do not SIGKILL a backend while it is handling a device response.
pass
thread=getattr(self,'_thread',None)
if thread and thread is not threading.current_thread(): thread.join(timeout=3)
if hasattr(self,'_lease') and not self._lease.closed: self._lease.close()
def start(offline=False):
global _runtime
if _runtime is None:
_runtime=RuntimeClient(offline=offline)
atexit.register(close)
return _runtime
def close():
global _runtime
if _runtime is not None:
runtime=_runtime;_runtime=None;runtime.close()
def status():
return _runtime.snapshot() if _runtime else {'backend':{'error':'后端未启动'}}
def catalog():
return copy.deepcopy(_runtime._catalog) if _runtime else {'actions':[],'stages':[]}
def run_action(action, arm, confirm=False, mode='eye_in_hand', extras=None):
# Compatibility signature for the UI; the backend validates every command itself.
if not _runtime: return {'ok':False,'message':'后端未启动'}
return _runtime.command(action,arm,mode,extras)
def run_gripper_tune(arm,kind,numbers):
return run_action('gripper_tune',arm,extras={**numbers,'kind':kind})
def stop_started(arm=None):
return rpc.call(_runtime.socket,'stop',{'arm':arm},timeout=45)
def notify_gauge_finished(): return rpc.call(_runtime.socket,'gauge_finished')
def reset_gauge_pass(): return rpc.call(_runtime.socket,'gauge_reset')
def command_lane(action):
if action.startswith('gripper'): return 'grip'
if action.startswith('camera'): return 'camera'
if action.startswith('tactile'): return 'tactile'
if action=='stop_commands': return 'stop'
return 'teach' if action=='handeye_teach' else 'arm'
def command_blocked_message(running,lane):
if lane=='stop': return ''
if lane in running or (lane in ('arm','teach') and running.intersection({'arm','teach'})):
return '设备正在执行上一条任务'
return ''
class TeachClient:
def _arm(self,sn):
from arm_application.site_io import arm_short_id
return arm_short_id(sn)
def drag_on(self,sn): return run_action('teach_drag_on',self._arm(sn))
def drag_off(self,sn): return run_action('teach_drag_off',self._arm(sn))
def record(self,sn,arm,name): return run_action('teach_record',arm,extras={'name':name})
def delete_point(self,arm,name): return run_action('teach_delete',arm,extras={'name':name})
def goto(self,sn,arm,name,speed): return run_action('teach_goto',arm,extras={'name':name,'speed':speed})
def list_points(self,arm): return status().get('teach_points',{}).get(arm,{'ok':True,'points':[]})
class TactileClient:
def state(self):
if not _runtime: return {}
with _runtime._lock: return copy.deepcopy(_runtime._snapshot.get("view",{}))
def frame(self,side,kind):
if _runtime:
with _runtime._lock: return _runtime._frames.get(f'{side}_{kind}')
return None
teach = TeachClient()
tactile_view = TactileClient()
station/src/station/site_catalog.py
+285
View File
@@ -0,0 +1,285 @@
"""Read-only station metadata and the tactile pass judge. No process/device control."""
from __future__ import annotations
import copy
import math
import re
import threading
import time
from collections import deque
from functools import lru_cache
from pathlib import Path
import yaml
from arm_application.gauge_pass import GaugePassJudge, shear_direction
from arm_application.site_io import eye_to_hand_result_name, listed_arms, overhead_camera, usb_expectations, ros2_root, absolutize_site_paths
ROOT = ros2_root()
_LOCK = threading.Lock()
_TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
_PASS = GaugePassJudge()
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 all(isinstance(point[k], (int, float)) and math.isfinite(point[k]) 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,
"stamp": point["t"],
}
_consider_gauge_pass_locked(point["t"])
@lru_cache(maxsize=1)
def _read_site(path: Path, mtime_ns: int, size: int) -> dict:
"""只在配置变化时解析；一次状态刷新会多次查询同一份配置。"""
data = yaml.safe_load(path.read_text()) or {}
return absolutize_site_paths(data, path.parent.parent) if isinstance(data, dict) else {}
def _site() -> dict:
path = ROOT / "config" / "site.yaml"
try:
info = path.stat()
except FileNotFoundError:
return {}
# 调用方可以修改自己的副本，不污染其他线程读取的缓存。
return copy.deepcopy(_read_site(path, info.st_mtime_ns, info.st_size))
def _arms() -> list[dict]:
"""真机臂来自 site.yaml。没有清单就空着，不补本机的 89/91。"""
return [
{
"id": row["id"],
"label": row["label"],
"sn": row["sn"],
"tactile": row["tactile"],
"camera": row["camera"],
"gripper": row["gripper"],
"rotary": row["rotary"],
}
for row in listed_arms(_site())
]
def _usb() -> list[dict]:
found = {"d405": [], "d435": [], "orisys": []}
want = {
("8086", "0b5b"): "d405",
("8086", "0b07"): "d435",
("1bcf", "2281"): "orisys",
}
base = Path("/sys/bus/usb/devices")
if not base.is_dir():
return []
for dev in base.iterdir():
vid = dev / "idVendor"
pid = dev / "idProduct"
if not vid.is_file() or not pid.is_file():
continue
key = want.get((vid.read_text().strip(), pid.read_text().strip()))
if not key:
continue
speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
found[key].append({"port": dev.name, "speed": speed})
expect = usb_expectations(_site())
return [
{"id": "d405", "label": "腕部 D405", "expect": expect["d405"], "items": found["d405"]},
{"id": "d435", "label": "手外 D435", "expect": expect["d435"], "items": found["d435"]},
{"id": "orisys", "label": "视触觉", "expect": expect["orisys"], "items": found["orisys"]},
]
def _grippers() -> list[str]:
return sorted(p.name for p in Path("/dev").glob("ttyUSB*"))
def _consider_gauge_pass_locked(now: float) -> None:
"""用当前切向更大的那一指更新通/不通。调用方必须已经拿着 ``_LOCK``。"""
best_angle = 0.0
best_mag = -1.0
saw = False
for side in ("left", "right"):
row = _TACTILE.get(side) or {}
x = row.get("fshearx")
y = row.get("fsheary")
if not isinstance(x, (int, float)) or not isinstance(y, (int, float)):
continue
angle, mag = shear_direction(float(x), float(y))
saw = True
if mag >= best_mag:
best_mag = mag
best_angle = angle
if saw:
_PASS.update(best_angle, max(best_mag, 0.0), now)
def notify_gauge_finished() -> dict:
"""结束通知的入口。对面说拧完了就调这个，目前没有接外部信号。
正在拧时收到 → 通。还没开始拧 → 只记下，等它停住再判。
已经判成不通之后，迟到的通知不翻案。
"""
with _LOCK:
verdict = _PASS.notify_finished(time.time())
snap = _PASS.snapshot()
return {"ok": True, "message": snap["label"], "verdict": verdict, "gauge_pass": snap}
def reset_gauge_pass() -> dict:
"""清掉这一次的通/不通，下一次拧入重新看方向。"""
with _LOCK:
_PASS.reset()
snap = _PASS.snapshot()
return {"ok": True, "message": snap["label"], "gauge_pass": snap}
def _robot(arm: str) -> dict:
from arm_application.site_io import arm_short_id
robots = (_site().get("robot") or {}).get("robots") or []
for robot in robots:
if not isinstance(robot, dict):
continue
sn = str(robot.get("sn") or "")
if sn.lower() == "sim":
continue
if arm_short_id(sn) == arm:
return robot
return {}
def _comment(text: str, key: str) -> str:
match = re.search(rf"^#\s*{re.escape(key)}:\s*(.+)$", text, re.M)
return match.group(1).strip() if match else ""
def _pose_count(path: Path) -> int | None:
if not path.is_file():
return None
match = re.search(r"num_poses:\s*(\d+)", path.read_text(errors="replace"))
return int(match.group(1)) if match else None
def _result_card(path: Path, pose_path: Path, board: dict) -> dict:
text = path.read_text(errors="replace") if path.is_file() else ""
trans = None
tool = ""
if text:
try:
data = yaml.safe_load(text) or {}
except yaml.YAMLError:
data = {}
if isinstance(data, dict):
raw = data.get("translation_m")
if isinstance(raw, list) and len(raw) >= 3:
trans = [float(raw[0]), float(raw[1]), float(raw[2])]
tool = str(data.get("gripper_tool") or "")
meaning = ""
for line in text.splitlines():
if line.startswith("# T_4x4"):
meaning = line[2:].strip()
break
return {
"file": str(path),
"exists": path.is_file(),
"views": _comment(text, "views"),
"utc": _comment(text, "utc"),
"board_note": _comment(text, "board"),
"camera": _comment(text, "camera_source"),
"tool": tool,
"meaning": meaning,
"translation_m": trans,
"poses": _pose_count(pose_path),
"pose_file": str(pose_path),
"board": {
"cols": board.get("cols"),
"rows": board.get("rows"),
"square_m": board.get("square_size_m"),
},
}
def _handeye(arm: str) -> dict:
site = _site()
robot = _robot(arm)
paths = robot.get("paths") if isinstance(robot.get("paths"), dict) else {}
shared = site.get("paths") if isinstance(site.get("paths"), dict) else {}
boards = ((site.get("handeye") or {}).get("boards") or {})
wrist_file = Path(str(
paths.get("handeye_file")
or shared.get("handeye_file")
or (ROOT / "handeye" / "results" / f"{arm}_eye_in_hand.yaml")
))
wrist_pose = Path(str(
paths.get("handeye_waypoints")
or shared.get("handeye_waypoints")
or (ROOT / "handeye" / "poses" / f"{arm}_eye_in_hand.yaml")
))
cam, _others = overhead_camera(site, robot or None)
cam_name = str((cam or {}).get("name") or "")
e2h_name = eye_to_hand_result_name(arm, cam_name)
e2h_file = Path(str(
paths.get("handeye_file_eye_to_hand")
or shared.get("handeye_file_eye_to_hand")
or (ROOT / "handeye" / "results" / e2h_name)
))
e2h_pose = Path(str(
paths.get("handeye_waypoints_eye_to_hand")
or shared.get("handeye_waypoints_eye_to_hand")
or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
))
wrist = _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {})
wrist_name = ""
if isinstance(robot.get("camera"), dict):
wrist_name = str(robot["camera"].get("name") or "").strip().strip("/")
wrist["preview_camera"] = wrist_name
wrist["preview_label"] = "腕部相机"
outside = _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {})
outside["preview_camera"] = cam_name.strip().strip("/")
outside["preview_label"] = "手外相机"
return {
"eye_in_hand": wrist,
"eye_to_hand": outside,
}
_ACTIONS = {'real_up': {'title': '上电控制栈', 'moves': True, 'blurb': '连接机械臂并拉起 ros2_control。'},
'camera_d405': {'title': '本臂腕部相机', 'moves': False, 'blurb': '只拉起本臂腕部相机。彩色和深度显示在程序里，不看左右红外，不另开窗口。'},
'camera_d435': {'title': '手外 D435', 'moves': False, 'blurb': '只拉起顶视 D435。彩色和深度显示在程序里，不另开窗口。'},
'tactile_up': {'title': '视触觉三力', 'moves': False, 'blurb': '发布左右指法向力与两路切向力。'},
'gripper_up': {'title': '夹爪节点', 'moves': False, 'blurb': '只拉起夹爪驱动，不主动张合。'},
'gripper_probe': {'title': '夹爪探测', 'moves': False, 'blurb': '读夹爪是否在线，不动手指。驱动没在跑时先拉起，不回零。'},
'gripper_open': {'title': '夹爪张开', 'moves': True, 'blurb': '张开当前臂夹爪。驱动没在跑时先拉起，不回零。'},
'gripper_close': {'title': '夹爪闭合', 'moves': True, 'blurb': '闭合当前臂夹爪。驱动没在跑时先拉起，不回零。'},
'gripper_rotate': {'title': '右爪转 90°', 'moves': True, 'blurb': '右臂旋转爪转到 90°。左臂平行爪没有这一轴。'},
'pipeline': {'title': '跟踪并对孔', 'moves': True, 'blurb': '跟踪和对孔一次跑完。搜孔轻压已取消，这一步不含旋入。会动臂。'},
'stage_track': {'title': '只测跟踪', 'moves': True, 'blurb': '只跑视觉跟踪。从当前位姿开始，会动臂。要先有控制栈和相机。'},
'stage_locate': {'title': '只测对孔', 'moves': True, 'blurb': '只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。'},
'stage_screw': {'title': '只测旋转装配', 'moves': True, 'blurb': '按现场配置的拧入深度和螺距把螺纹规旋进去。会动臂。臂应已在孔口。'},
'reset_home': {'title': '复位到 home', 'moves': True, 'blurb': '先停跟踪，再回到命名位姿 home。要控制栈已经起来。会动臂。'},
'gauge_dry': {'title': '装配干跑', 'moves': False, 'blurb': 'S00 到 S11 只打印计划，不发运动指令。'},
'gauge_run': {'title': '装配真跑', 'moves': True, 'blurb': '通止规状态机。会动臂、会拧。'},
'handeye_show': {'title': '查看标定配置', 'moves': False, 'blurb': '打印这块板、结果文件和已有点数，不动臂。'},
'handeye_teach': {'title': '示教采点', 'moves': True, 'blurb': '软拖拽示教。点数和是否接着采由手眼页上的控件决定。要先停控制栈，会动臂。'},
'handeye_merge': {'title': '合并示教点', 'moves': False, 'blurb': '把刚采的点写入该模式的位姿文件，不动臂。'},
'handeye_calibrate': {'title': '求解手眼', '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", "回零结束"),
],
}
station/src/station/subscriptions.py
+16
View File
@@ -0,0 +1,16 @@
"""Reconcile subscriptions with actual consumers; usable without ROS in tests."""
class SubscriptionSet:
def __init__(self):
self.items = {}
def sync(self, wanted, create, destroy):
wanted = set(wanted)
for key in set(self.items) - wanted:
destroy(self.items.pop(key))
for key in wanted - set(self.items):
self.items[key] = create(key)
def demanded_streams(cameras, preview, jpeg):
valid = {f"{cam['id']}_{kind}" for cam in cameras for kind in ('color','depth')}
return (set(preview) | {key for key in jpeg if key.endswith('_color')}) & valid
station/src/station/tasks.py
+79
View File
@@ -0,0 +1,79 @@
"""Device-independent task lifecycle and resource ownership. No Qt, ROS or shell."""
from __future__ import annotations
import copy
import threading
import time
import uuid
from concurrent.futures import ThreadPoolExecutor
class Cancelled(RuntimeError):
pass
class TaskManager:
def __init__(self, workers=8):
self._lock = threading.RLock()
self._pool = ThreadPoolExecutor(max_workers=workers, thread_name_prefix="station-command")
self._tasks = {}
self._events = {}
self._owners = {}
self._closed = False
def submit(self, name, resources, fn):
with self._lock:
if self._closed:
raise RuntimeError("工位后端正在退出")
resources = set(resources)
busy = resources.intersection(self._owners)
if busy:
raise RuntimeError("设备正在执行任务：" + ", ".join(sorted(busy)))
token = uuid.uuid4().hex
event = threading.Event()
self._events[token] = event
self._tasks[token] = {"id": token, "name": name, "state": "queued", "created": time.time(), "result": None}
for resource in resources:
self._owners[resource] = token
self._pool.submit(self._run, token, resources, event, fn)
return token
def _run(self, token, resources, event, fn):
with self._lock:
self._tasks[token]["state"] = "running"
try:
if event.is_set():
raise Cancelled("任务已取消")
result = fn(event)
state = "succeeded" if result.get("ok") else "cancelled" if event.is_set() else "failed"
except Cancelled as exc:
state, result = "cancelled", {"ok": False, "message": str(exc)}
except Exception as exc:
state, result = "failed", {"ok": False, "message": str(exc)}
with self._lock:
self._tasks[token].update(state=state, result=result, finished=time.time())
for resource in resources:
if self._owners.get(resource) == token:
del self._owners[resource]
# Retain a bounded command history; never evict an active command.
finished = [key for key, row in self._tasks.items() if row["state"] not in ("queued", "running")]
for key in finished[:-100]:
self._tasks.pop(key, None)
self._events.pop(key, None)
def snapshot(self, token=None):
with self._lock:
return copy.deepcopy(self._tasks.get(token) if token else list(self._tasks.values()))
def cancel(self, token=None):
with self._lock:
keys = [token] if token else list(self._events)
for key in keys:
if key in self._events:
self._events[key].set()
def close(self):
with self._lock:
self._closed = True
self.cancel()
self._pool.shutdown(wait=True, cancel_futures=False)
station/test/gui_fixture.py
+67
View File
@@ -0,0 +1,67 @@
"""Exercise Qt and backend shutdown with no device transport in the UI."""
import sys
import os
import time
from station import runtime_client as runtime
runtime.start(offline=True)
import station.operator_app as ui
app=ui.QApplication.instance();app.setFont(ui.QFont('Noto Sans SC',11));app.setStyleSheet(ui._STYLE)
window=ui.StationApp();window.resize(1440,900);window.show()
window._apply_theme('dark',persist=False)
assert app.property('darkTheme') is True
assert window._theme_selector.currentData()=='dark'
window._apply_theme('light',persist=False)
assert app.property('darkTheme') is False
assert window._theme_selector.currentData()=='light'
assert 'rclpy' not in sys.modules
assert 'serial' not in sys.modules
assert 'station.operator_console' not in sys.modules
assert set(window._pages) == {'main','calibration','settings','ros_monitor'}
# A photo must preserve the current RGB frame; no capture/device operation is needed.
import tempfile
from unittest.mock import patch
sample=bytes([12,84,192])*(32*18)
selected=next(cam for _page,cam,_label in window._camera_labels if cam.get('arm')==window._arm)
with tempfile.TemporaryDirectory() as pictures:
with patch.object(ui.Path,'home',return_value=ui.Path(pictures)), patch.object(ui.camera_view,'previews',return_value={selected['id']+'_color':(sample,32,18)}):
window._take_photo()
photos=list(ui.Path(pictures).rglob('*.png'))
assert len(photos)==1
image=ui.QImage(str(photos[0]))
assert (image.width(),image.height())==(32,18)
assert image.pixelColor(0,0).getRgb()[:3]==(12,84,192)
window._open_camera(selected['id'])
assert selected['id'] in window._camera_windows
window._camera_windows[selected['id']].close()
assert not window._camera_windows
errors=[];ticks=[]
duration=float(os.environ.get("ORISYS_GUI_TEST_SECONDS","2.2"))
started=time.monotonic()
frames=[bytes([v,40,80])*(320*180) for v in (30,70,120)]
def tick():
ticks.append(time.monotonic())
if duration>10:
if len(ticks)%100==0:
window._show_page(("main","calibration")[(len(ticks)//100)%2])
for key in ui.camera_view._PREVIEW_WANT:
ui.camera_view._PREVIEW[key]=(frames[len(ticks)%3],320,180)
ui.camera_view._META[key]=(320,180,20.)
original=sys.excepthook
def fail(*args): errors.append(str(args[1]));original(*args)
sys.excepthook=fail
beat=ui.QTimer();beat.timeout.connect(tick);beat.start(50)
ui.QTimer.singleShot(200,lambda:window._start(lambda:(time.sleep(.7),{'ok':True,'message':'test'})[1],'test'))
ui.QTimer.singleShot(500,lambda:window._show_page('calibration'))
ui.QTimer.singleShot(1000,lambda:window._show_page('calibration'))
ui.QTimer.singleShot(1500,lambda:window._show_page('ros_monitor'))
ui.QTimer.singleShot(1900,lambda:window._show_page('main'))
ui.QTimer.singleShot(round(duration*1000),window.close)
ui.QTimer.singleShot(round((duration+15)*1000),app.quit)
app.exec()
assert len(ticks)>20,len(ticks)
assert not errors,errors
assert runtime._runtime is None
assert not any(job.isRunning() for job in window._jobs.values())
print('Qt responsive, no device modules, async task and graceful shutdown PASS',
{'seconds':round(time.monotonic()-started,2),'ticks':len(ticks),
'max_gap':round(max((b-a for a,b in zip(ticks,ticks[1:])),default=0),3),'errors':errors},flush=True)
station/test/ros_fixture.py
+184
View File
@@ -0,0 +1,184 @@
"""Isolated DDS-domain integration fixture. All hardware transports are fake."""
import threading
import time
from pathlib import Path
# The caller assigns a private ROS_DOMAIN_ID before importing rclpy.
import rclpy
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from arm_msgs.srv import GripperExecute
from arm_application.dh_pgea import GripperState
import arm_application.dh_pgea_node as driver
class FakeGripper:
writes=[]
def __init__(self,**kwargs):
self.state=GripperState(init_state=1,grip_state=1,position=300,target_position=300,target_force=33)
def open(self): pass
def close(self): pass
def probe(self): return 1
def get_run_states(self,**kwargs): return self.state
def initialize(self): self.writes.append('INIT')
def move(self,pos,force,speed):
self.writes.append((pos,force,speed));self.state.position=pos;self.state.target_position=pos;return True
def main():
driver.DhPgeaGripper=FakeGripper
rclpy.init(args=['--ros-args','-r','__ns:=/station/grippers/arm_89','-p','auto_init:=false'])
device=driver.DhPgeaNode()
assert device.get_namespace()=='/station/grippers/arm_89',device.get_namespace()
client_node=Node('station_test_client')
executor=MultiThreadedExecutor(num_threads=2);executor.add_node(device);executor.add_node(client_node)
thread=threading.Thread(target=executor.spin,daemon=True);thread.start()
try:
client=client_node.create_client(GripperExecute,'/station/grippers/arm_89/execute')
assert client.wait_for_service(timeout_sec=3)
def command(name,position=0,force=33,speed=25):
req=GripperExecute.Request();req.command.command=name;req.command.position=float(position)
req.command.force=float(force);req.command.speed=float(speed)
future=client.call_async(req);deadline=time.monotonic()+3
while not future.done() and time.monotonic()<deadline: time.sleep(.01)
assert future.done(),'service did not respond'
return future.result()
assert command('connect').success
assert FakeGripper.writes==[],FakeGripper.writes
assert not command('move',1,force=999).success
assert not command('move',float('nan')).success
assert not command('rotate_to',90).success
assert FakeGripper.writes==[]
assert command('move',10).success
assert FakeGripper.writes==[(500,33,25)],FakeGripper.writes
assert command('connect').status.finger_position_per_mille==500.
print('ROS typed service, namespace, bounds and no-auto-init PASS',flush=True)
finally:
executor.shutdown(timeout_sec=3);thread.join(timeout=3)
device.destroy_node();client_node.destroy_node();rclpy.shutdown()
# Camera subscriptions must follow consumers, not all configured cameras.
from station import operator_camera_view as camera
cams=[{'id':'left','role':'wrist'},{'id':'right','role':'wrist'},{'id':'sim_wrist','role':'wrist'}]
camera.watch(set(),set());node=camera._open_node(cams)
try:
node.sync_streams();assert node._streams.items=={}
camera.watch({'left_color'},set());node.sync_streams()
assert set(node._streams.items)=={'left_color'}
from sensor_msgs.msg import Image
from rclpy.qos import qos_profile_sensor_data
publisher=node.create_publisher(Image,'/left/color/image_raw',qos_profile_sensor_data)
message=Image();message.width=4;message.height=4;message.step=12;message.encoding='rgb8';message.data=bytes([90,20,10])*16
deadline=time.monotonic()+3
while 'left_color' not in camera.previews() and time.monotonic()<deadline:
publisher.publish(message);rclpy.spin_once(node,timeout_sec=.05);camera.publish_latest()
blob,width,height=camera.previews()['left_color']
assert (width,height)==(4,4) and blob[:3]==bytes([90,20,10])
import tempfile
from station.cam_bridge import _export
shms={}
try:
with tempfile.TemporaryDirectory() as directory:
_export(Path(directory),shms,{})
assert shms
first=next(iter(shms.values()))[1]
assert camera._shm_read(first)[3]==blob
finally:
for _,shm in shms.values(): shm.close();shm.unlink()
camera.watch({'right_depth'},set());node.sync_streams()
assert set(node._streams.items)=={'right_depth'}
camera.watch(set(),{'left_color'});node.sync_streams()
assert set(node._streams.items)=={'left_color'}
camera.watch(set(),set());node.sync_streams()
assert not node._streams.items and not node._detections.items
print('ROS dynamic camera subscriptions PASS',flush=True)
finally:
node.destroy_node();rclpy.shutdown()
# Exercise native Action feedback/cancellation without a robot controller.
from station.ros_gateway import RosGateway
from arm_application.site_io import load_site_dict
from sensor_msgs.msg import JointState
from controller_manager_msgs.srv import ListControllers, SwitchController
from controller_manager_msgs.msg import ControllerState
from rclpy.action import ActionServer, CancelResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from control_msgs.action import FollowJointTrajectory
from station.tasks import Cancelled
gateway=RosGateway(load_site_dict())
fake=Node('fake_controller')
controller_state = ['active']
switches = []
def controllers(req,resp):
state=ControllerState();state.name='rizon_arm_controller';state.state=controller_state[0]
state.claimed_interfaces=[f"{arm['sn']}_joint{i}/position" for i in range(1,8)] if state.state=='active' else []
resp.controller=[state];return resp
fake.create_service(ListControllers,'/controller_manager/list_controllers',controllers)
def switch(req,resp):
switches.append((list(req.activate_controllers),list(req.deactivate_controllers)))
controller_state[0]='inactive' if req.deactivate_controllers else 'active'
resp.ok=True;return resp
fake.create_service(SwitchController,'/controller_manager/switch_controller',switch)
group=ReentrantCallbackGroup()
def execute(handle):
end=time.monotonic()+.7
while time.monotonic()<end:
if handle.is_cancel_requested:
handle.canceled();return FollowJointTrajectory.Result()
time.sleep(.01)
handle.succeed();return FollowJointTrajectory.Result()
action=ActionServer(fake,FollowJointTrajectory,'/rizon_arm_controller/follow_joint_trajectory',
execute_callback=execute,cancel_callback=lambda request:CancelResponse.ACCEPT,callback_group=group)
gateway.executor.add_node(fake)
arm=next(a for a in gateway.arms if a['id']==gateway.active_arm)
msg=JointState();msg.name=[f"{arm['sn']}_joint{i}" for i in range(1,8)];msg.position=[0.]*7
try:
gateway._joint(msg)
assert gateway.move_joints(arm,[.01]*7,.15,threading.Event())['ok']
gateway._joint(msg)
cancel=threading.Event();timer=threading.Timer(.3,cancel.set);timer.start()
try:
gateway.move_joints(arm,[.01]*7,.15,cancel)
raise AssertionError('expected action cancellation')
except Cancelled: pass
timer.join()
assert not gateway._goals
joint_stop=threading.Event()
def joint_feedback():
while not joint_stop.wait(.03): gateway._joint(msg)
joint_thread=threading.Thread(target=joint_feedback);joint_thread.start()
try:
assert gateway.stop_arm(arm)['ok']
assert gateway.confirm_stopped(arm)['ok']
assert controller_state[0]=='inactive'
assert switches[-1]==([],['rizon_arm_controller'])
# Only the next explicit command restores the controller.
gateway.require_arm(arm['id'],threading.Event())
assert controller_state[0]=='active'
assert switches[-1]==(['rizon_arm_controller'],[])
wrong={**arm,'sn':'OtherRobot'}
try:
gateway.stop_arm(wrong)
raise AssertionError('wrong-arm stop accepted')
except RuntimeError as exc: assert '不属于' in str(exc)
finally:
joint_stop.set();joint_thread.join()
print('ROS stop deactivation, stationary confirmation, explicit resume and arm ownership PASS',flush=True)
from arm_msgs.msg import TactileStatus
from station import site_catalog
force=TactileStatus();force.connected=True;force.fnormal=1.25;force.fshearx=-.5;force.fsheary=.75
gateway._tactile('left',force)
assert site_catalog._TACTILE['left']['fnormal']==1.25
assert site_catalog._TACTILE['left']['fshearx']==-.5
gateway.watch_tactile(True)
gateway._tactile_image('left_thermal',message)
assert 'left_thermal' in gateway.tactile_frames()
gateway.watch_tactile(False)
assert not gateway.tactile_frames() and not gateway._tactile_images.items
print('ROS tactile driver reuse, unchanged force values and preview lifetime PASS',flush=True)
print('ROS persistent gateway, arm identity and trajectory cancellation PASS',flush=True)
finally:
action.destroy();gateway.executor.remove_node(fake);fake.destroy_node();gateway.close()
if __name__=='__main__': main()
station/test/test_backend.py
+51
View File
@@ -0,0 +1,51 @@
import os
import subprocess
import sys
import tempfile
import time
from pathlib import Path
from unittest import TestCase
from station.rpc import call
class BackendTest(TestCase):
def test_offline_rpc_task_lifecycle_and_shutdown(self):
with tempfile.TemporaryDirectory() as folder:
socket=Path(folder)/'backend.sock'
proc=subprocess.Popen([sys.executable,'-m','station.control_service','--socket',str(socket),'--offline'],
stdout=subprocess.PIPE,stderr=subprocess.STDOUT,text=True)
try:
deadline=time.monotonic()+10
while not socket.exists() and proc.poll() is None and time.monotonic()<deadline: time.sleep(.05)
self.assertIsNone(proc.poll())
self.assertEqual(call(socket,'ping')['protocol'],1)
catalog=call(socket,'catalog')
unavailable={r['id'] for r in catalog['actions'] if not r['available']}
self.assertIn('gauge_run',unavailable)
with self.assertRaises(RuntimeError): call(socket,'submit',{'action':'__import__','arm':'89'})
arms=[]
while time.monotonic()<deadline:
snap=call(socket,'snapshot');arms=snap.get('arms',[])
if arms: break
time.sleep(.05)
self.assertTrue(arms)
aid=arms[0]['id']
token=call(socket,'submit',{'action':'handeye_show','arm':aid})['task_id']
while time.monotonic()<deadline:
row=call(socket,'task',{'task_id':token})
if row['state'] not in ('queued','running'): break
time.sleep(.05)
self.assertEqual(row['state'],'succeeded')
token=call(socket,'submit',{'action':'gripper_up','arm':aid})['task_id']
while time.monotonic()<deadline:
row=call(socket,'task',{'task_id':token})
if row['state'] not in ('queued','running'): break
time.sleep(.05)
self.assertEqual(row['state'],'failed');self.assertIn('离线',row['result']['message'])
finally:
proc.terminate()
try: output,_=proc.communicate(timeout=10)
except subprocess.TimeoutExpired:
proc.kill();output,_=proc.communicate();self.fail(output)
self.assertEqual(proc.returncode,0,output)
self.assertFalse(socket.exists())
station/test/test_client_state.py
+19
View File
@@ -0,0 +1,19 @@
import threading
import time
from unittest import TestCase
from unittest.mock import patch
from station.runtime_client import RuntimeClient
class ClientStateTest(TestCase):
def test_paint_snapshot_is_local_and_stale_state_is_not_live(self):
client=RuntimeClient.__new__(RuntimeClient)
client._lock=threading.RLock();client.error=''
client._snapshot={'now':time.time()-10,'arm_live':{'89':{'enabled':True,'pose_live':True}},
'gripper_live':{'89':{'connected':True,'pose_live':True}}}
with patch('station.rpc.call',side_effect=AssertionError('paint path must not perform RPC')):
snapshot=client.snapshot()
self.assertFalse(snapshot['arm_live']['89']['pose_live'])
self.assertIsNone(snapshot['arm_live']['89']['enabled'])
self.assertFalse(snapshot['gripper_live']['89']['connected'])
self.assertTrue(client._snapshot['gripper_live']['89']['connected'])
station/test/test_gripper_commands.py
+51
View File
@@ -0,0 +1,51 @@
import tempfile
import threading
import time
from unittest import TestCase
from station.control_service import StationService
class Gateway:
active_arm='89'
def __init__(self): self.started=None;self.requests=[]
def graph(self): return {'nodes':[],'topics':[],'services':['/station/grippers/arm_89/execute']}
def states(self):
elapsed=time.monotonic()-self.started if self.started else 0
return {},{'89':{'connected':True,'moving':bool(self.started and elapsed<.25),
'grip_state':0 if self.started and elapsed<.25 else 1,'permille':500.}}
def call(self,typ,name,request,event,timeout):
self.requests.append(request);self.started=time.monotonic()
return type('Reply',(),{'success':True,'message':'accepted'})()
def close(self): pass
class GripperCommandsTest(TestCase):
def test_waits_for_physical_completion_and_rejects_bounds(self):
gateway=Gateway()
with tempfile.TemporaryDirectory() as root:
service=StationService(root=root,gateway=gateway)
try:
arm=service.arms['89'];start=time.monotonic()
result=service._gripper('gripper_tune',arm,{'kind':'pos','pos':500,'force':33,'speed':25},threading.Event())
self.assertTrue(result['ok']);self.assertGreater(time.monotonic()-start,.2)
self.assertEqual(gateway.requests[0].command.position,arm['stroke_mm']/2)
count=len(gateway.requests)
with self.assertRaises(ValueError):
service._gripper('gripper_tune',arm,{'kind':'pos','pos':500,'force':999,'speed':25},threading.Event())
self.assertEqual(len(gateway.requests),count)
finally: service.close()
def test_configuration_changes_never_retarget_commands(self):
from pathlib import Path
gateway=Gateway()
with tempfile.TemporaryDirectory() as root:
service=StationService(root=root,gateway=gateway)
try:
path=Path(root)/'site.yaml';path.write_text('robot: before')
service.config_path=path;service._config_signature=service._signature()
path.write_text('robot: different-device')
with self.assertRaisesRegex(RuntimeError,'配置已变化'):
service.submit('gripper_probe','89')
self.assertEqual(gateway.requests,[])
self.assertTrue(service._frozen_config.is_file())
finally: service.close()
station/test/test_gui.py
+18
View File
@@ -0,0 +1,18 @@
import os
import subprocess
import sys
import tempfile
from pathlib import Path
from unittest import TestCase, skipUnless
class GuiTest(TestCase):
@skipUnless(os.environ.get('DISPLAY'), 'requires X11 for OpenGL widgets')
def test_gui_isolated_from_hardware_and_closes_cleanly(self):
env={**os.environ,'QT_QPA_PLATFORM':'xcb','ROS_DOMAIN_ID':'231','ROS_LOCALHOST_ONLY':'1'}
with tempfile.TemporaryDirectory() as cache:
env['XDG_CACHE_HOME']=cache
result=subprocess.run([sys.executable,str(Path(__file__).with_name('gui_fixture.py'))],env=env,
capture_output=True,text=True,timeout=20)
self.assertEqual(result.returncode,0,result.stdout+'\n'+result.stderr)
self.assertIn('shutdown PASS',result.stdout)
station/test/test_image_preview.py
+16
View File
@@ -0,0 +1,16 @@
from types import SimpleNamespace
from unittest import TestCase
import numpy as np
from station.image_preview import jpeg_preview
class ImagePreviewTest(TestCase):
def test_padding_vector_and_invalid_buffers(self):
rgb=SimpleNamespace(encoding='rgb8',width=2,height=2,step=8,is_bigendian=False,data=(bytes([90,10,30])*2+b'xx')*2)
self.assertTrue(jpeg_preview(rgb).startswith(b'\xff\xd8'))
field=np.ones((4,4,2),np.float32)
msg=SimpleNamespace(encoding='32FC2',width=4,height=4,step=32,is_bigendian=False,data=field.tobytes())
self.assertTrue(jpeg_preview(msg).startswith(b'\xff\xd8'))
field[0,0,0]=np.nan;msg.data=field.tobytes()
self.assertIsNone(jpeg_preview(msg))
rgb.data=b'bad';self.assertIsNone(jpeg_preview(rgb))
station/test/test_motion_commands.py
+74
View File
@@ -0,0 +1,74 @@
"""Exercise routing and stop semantics without launching ROS or moving hardware."""
import threading
from pathlib import Path
from unittest import TestCase
from unittest.mock import patch
from station.control_service import StationService
class Gateway:
active_arm = '89'
def __init__(self): self.calls=[];self.fail_stop=False
def graph(self): return {'nodes': [], 'topics': [], 'services': []}
def states(self): return {}, {}
def require_arm(self, aid, event): self.calls.append(('require',aid))
def move_joints(self, arm, joints, speed, event):
self.calls.append(('home',arm['id'],list(joints),speed));return {'ok':True}
def stop_arm(self, arm):
self.calls.append(('stop',arm['id']))
if self.fail_stop: raise RuntimeError('no acknowledgement')
def confirm_stopped(self, arm):
self.calls.append(('confirmed',arm['id']));return {'ok':True,'message':'stopped'}
def close(self): pass
class MotionCommandsTest(TestCase):
def setUp(self):
self.gateway=Gateway();self.service=StationService(gateway=self.gateway)
def tearDown(self): self.service.close()
def test_camera_refresh_checks_both_cameras_without_robot_control(self):
with patch.object(self.service,'_camera',side_effect=[RuntimeError('D405 missing'),{'ok':True,'message':'ready'}]) as camera:
result=self.service.execute('camera_refresh','89','eye_in_hand',{},threading.Event())
self.assertFalse(result['ok']);self.assertEqual(camera.call_count,2)
self.assertFalse(camera.call_args_list[0].args[1]);self.assertTrue(camera.call_args_list[1].args[1])
self.assertEqual(self.gateway.calls,[])
self.assertTrue(result['cameras'][1]['ok'])
def test_enable_reuses_running_stack_without_second_driver(self):
with patch.object(self.gateway,'graph',return_value={'nodes':['controller_manager'],'topics':[],'services':[]}),patch.object(self.service.processes,'start') as start:
result=self.service.execute('real_up','89','eye_in_hand',{},threading.Event())
self.assertTrue(result['ok']);start.assert_not_called()
self.assertEqual(self.gateway.calls,[('require','89')])
def test_detection_and_alignment_are_separate_stages(self):
commands=[]
def run(key, argv, event, env, cwd):
commands.append(argv);return {'ok':True}
with patch.object(self.service,'_camera'),patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',side_effect=run):
self.service.execute('stage_track','89','eye_in_hand',{},threading.Event())
self.service.execute('stage_locate','89','eye_in_hand',{},threading.Event())
self.assertIn('skip_stage1:=false',commands[0]);self.assertIn('skip_stage2:=true',commands[0])
self.assertIn('skip_stage3:=true',commands[0]);self.assertIn('d435_move_only:=false',commands[0])
self.assertIn('skip_stage1:=true',commands[1]);self.assertIn('skip_stage2:=false',commands[1])
self.assertIn('no_descent:=true',commands[1]);self.assertIn('skip_stage3:=true',commands[1])
def test_home_uses_each_arms_named_initial_pose(self):
import yaml
for aid,arm in self.service.arms.items():
robot=next(row for row in self.service.site['robot']['robots'] if row['sn']==arm['sn'])
path=Path(robot['named_poses_file'])
if not path.is_absolute():path=self.service.root/path
expected=yaml.safe_load(path.read_text())['named_pose_server']['ros__parameters']['named_poses.home']
self.service.execute('reset_home',aid,'eye_in_hand',{},threading.Event())
self.assertEqual(self.gateway.calls[-1][1:3],(aid,expected))
def test_stop_targets_running_arm_even_after_ui_selection_changes(self):
self.service._motion_arm='89'
result=self.service.stop_commands('91')
self.assertTrue(result['ok']);self.assertEqual(self.gateway.calls,[('stop','89'),('confirmed','89')])
def test_failed_stop_blocks_motion_until_stop_confirmed(self):
self.gateway.fail_stop=True
self.assertFalse(self.service.stop_commands('89')['ok'])
with self.assertRaisesRegex(RuntimeError,'停止未确认'):self.service.submit('reset_home','89')
self.gateway.fail_stop=False
self.assertTrue(self.service.stop_commands('89')['ok']);self.assertEqual(self.service._stop_error,'')
def test_shutdown_cancel_does_not_stop_unrelated_controller(self):
self.assertTrue(self.service.stop_commands()['ok']);self.assertEqual(self.gateway.calls,[])
station/test/test_parent_exit.py
+27
View File
@@ -0,0 +1,27 @@
import os
import subprocess
import sys
import tempfile
import time
from pathlib import Path
from unittest import TestCase
class ParentExitTest(TestCase):
def test_backend_shuts_down_when_ui_owner_exits(self):
with tempfile.TemporaryDirectory() as directory:
path=Path(directory)/'service.sock'
source='''import os,subprocess,sys,time
p=subprocess.Popen([sys.executable,'-m','station.control_service','--socket',sys.argv[1],'--offline','--parent-pid',str(os.getpid())],stdout=subprocess.DEVNULL,stderr=subprocess.DEVNULL)
print(p.pid,flush=True)
end=time.monotonic()+8
while not os.path.exists(sys.argv[1]) and p.poll() is None and time.monotonic()<end:time.sleep(.02)
assert os.path.exists(sys.argv[1])
'''
parent=subprocess.run([sys.executable,'-c',source,str(path)],capture_output=True,text=True,timeout=12)
self.assertEqual(parent.returncode,0,parent.stderr)
end=time.monotonic()+8
while path.exists() and time.monotonic()<end: time.sleep(.1)
if path.exists():
os.kill(int(parent.stdout.strip()),15)
self.fail('orphan backend did not stop')
station/test/test_points.py
+18
View File
@@ -0,0 +1,18 @@
from pathlib import Path
from tempfile import TemporaryDirectory
from unittest import TestCase
from unittest.mock import patch
from station import operator_teach as points
class PointsTest(TestCase):
def test_atomic_save_read_delete(self):
with TemporaryDirectory() as folder:
path=Path(folder)/'points.yaml'
with patch.object(points,'_file',return_value=path):
points._write('89',[{'name':'p1','utc':'test','joints':[.1]*7}])
self.assertTrue(path.is_file())
self.assertFalse(path.with_suffix('.yaml.tmp').exists())
self.assertEqual(points.list_points('89')['points'][0]['name'],'p1')
self.assertTrue(points.delete_point('89','p1')['ok'])
self.assertEqual(points.list_points('89')['points'],[])
station/test/test_preview_cache.py
+27
View File
@@ -0,0 +1,27 @@
import json
import tempfile
import time
from pathlib import Path
from unittest import TestCase
from unittest.mock import patch
from station import operator_camera_view as preview
class PreviewCacheTest(TestCase):
def tearDown(self):
preview.watch(set(),set())
preview._PREVIEW.clear();preview._META.clear()
def test_switch_and_stale_worker_clear_frames(self):
preview.watch({'left_color'},set())
preview._PREVIEW['left_color']=(b'123',1,1)
preview.watch({'right_color'},set())
self.assertNotIn('left_color',preview.previews())
with tempfile.TemporaryDirectory() as folder:
preview._PREVIEW['right_color']=(b'123',1,1)
payload={'frames':{},'exported_monotonic':time.monotonic()-10,'meta':{},'ages':{}}
(Path(folder)/'meta.json').write_text(json.dumps(payload))
with patch.object(preview,'_BRIDGE_DIR',Path(folder)):
preview._ingest()
self.assertEqual(preview.previews(),{})
self.assertIn('未更新',preview.state()['error'])
station/test/test_process_tree.py
+31
View File
@@ -0,0 +1,31 @@
import os
import subprocess
import sys
import tempfile
import time
from pathlib import Path
from unittest import TestCase
from arm_application.process_lifecycle import stop_process_tree,_stat
class ProcessTreeTest(TestCase):
def test_descendants_in_other_sessions_exit_without_touching_unrelated_process(self):
with tempfile.TemporaryDirectory() as directory:
script=Path(directory)/'child.py'
script.write_text("import subprocess,sys,time,os\nfrom pathlib import Path\np=subprocess.Popen([sys.executable,'-c','import time;time.sleep(120)'],start_new_session=True)\nPath(sys.argv[1]).write_text(str(p.pid))\ntime.sleep(120)\n")
pidfile=Path(directory)/'pid'
unrelated=subprocess.Popen([sys.executable,'-c','import time;time.sleep(120)'],stdout=subprocess.DEVNULL,stderr=subprocess.DEVNULL)
root=subprocess.Popen([sys.executable,str(script),str(pidfile)],start_new_session=True,stdout=subprocess.DEVNULL,stderr=subprocess.DEVNULL)
try:
deadline=time.monotonic()+3
while not pidfile.exists() and time.monotonic()<deadline:time.sleep(.02)
self.assertTrue(pidfile.exists());child=int(pidfile.read_text())
self.assertNotEqual(os.getpgid(child),os.getpgid(root.pid))
stop_process_tree(root,interrupt_timeout=1,terminate_timeout=1)
self.assertIsNotNone(root.poll())
state=_stat(child)
self.assertTrue(state is None or state[2]=='Z')
self.assertIsNone(unrelated.poll())
finally:
if root.poll() is None:stop_process_tree(root,interrupt_timeout=.2,terminate_timeout=.2)
unrelated.terminate();unrelated.wait(timeout=2)
station/test/test_processes.py
+21
View File
@@ -0,0 +1,21 @@
import sys
import tempfile
import threading
import time
from unittest import TestCase
from station.processes import ProcessSupervisor
from station.tasks import Cancelled
class ProcessesTest(TestCase):
def test_owned_process_cancel_and_exit(self):
with tempfile.TemporaryDirectory() as directory:
processes=ProcessSupervisor(directory)
try:
result=processes.run('echo',[sys.executable,'-c','print("completed")'],threading.Event())
self.assertTrue(result['ok']);self.assertIn('completed',result['detail'])
event=threading.Event();timer=threading.Timer(.2,event.set);timer.start()
with self.assertRaises(Cancelled):
processes.run('long',[sys.executable,'-c','import time;time.sleep(30)'],event)
self.assertFalse(processes.snapshot()['long']['running'])
finally: processes.close()
station/test/test_qt_refcount.py
+39
View File
@@ -0,0 +1,39 @@
"""工位 Qt 运行时回归：无设备、无 ROS，检测导致一分钟崩溃的引用计数错误。
运行：bash station/run_station.sh --self-test
"""
import sys
import unittest
import PySide6
from PySide6.QtWidgets import QApplication, QLabel
class QtRuntimeTest(unittest.TestCase):
def test_void_calls_preserve_none(self):
# 先检查版本，防止在有缺陷的绑定上运行压力循环导致解释器直接 abort。
self.assertEqual(PySide6.__version__, "6.8.3")
app = QApplication.instance() or QApplication([])
label = QLabel("warmup")
def refresh():
label.setText("updated")
label.update()
label.setVisible(False)
label.style().unpolish(label)
label.style().polish(label)
for _ in range(20):
refresh()
app.processEvents()
before = sys.getrefcount(None)
for _ in range(4000):
refresh()
after = sys.getrefcount(None)
self.assertGreater(after - before, -50, "Qt 调用持续递减 None 引用计数")
label.deleteLater()
app.processEvents()
if __name__ == "__main__":
unittest.main()
station/test/test_ros_interfaces.py
+14
View File
@@ -0,0 +1,14 @@
import os
import subprocess
import sys
from pathlib import Path
from unittest import TestCase
class RosInterfacesTest(TestCase):
def test_fake_driver_on_private_dds_domain(self):
env={**os.environ,'ROS_DOMAIN_ID':'231','ROS_LOCALHOST_ONLY':'1'}
result=subprocess.run([sys.executable,str(Path(__file__).with_name('ros_fixture.py'))],env=env,
capture_output=True,text=True,timeout=25)
self.assertEqual(result.returncode,0,result.stdout+'\n'+result.stderr)
self.assertIn('dynamic camera subscriptions PASS',result.stdout)
station/test/test_site_cache.py
+23
View File
@@ -0,0 +1,23 @@
"""配置缓存必须保留热更新和调用者之间的数据隔离。"""
from pathlib import Path
from tempfile import TemporaryDirectory
from unittest import TestCase
from unittest.mock import patch
from station import site_catalog as console
class SiteConfigTest(TestCase):
def test_edit_and_delete_config(self):
with TemporaryDirectory() as folder, patch.object(console, "ROOT", Path(folder)):
path = Path(folder) / "config" / "site.yaml"
path.parent.mkdir()
self.assertEqual(console._site(), {})
path.write_text("robot:\n sn: original\n")
first = console._site()
first["robot"]["sn"] = "local change"
self.assertEqual(console._site()["robot"]["sn"], "original")
path.write_text("robot:\n sn: updated-robot\n")
self.assertEqual(console._site()["robot"]["sn"], "updated-robot")
path.unlink()
self.assertEqual(console._site(), {})
station/test/test_subscriptions.py
+22
View File
@@ -0,0 +1,22 @@
from unittest import TestCase
from station.subscriptions import SubscriptionSet, demanded_streams
class SubscriptionsTest(TestCase):
def test_switches_release_unused_real_sim_depth(self):
cams=[{'id':'left'},{'id':'right'},{'id':'sim_wrist'},{'id':'overhead'}]
subscriptions=SubscriptionSet();created=[];destroyed=[]
def create(key): created.append(key);return key
def sync(preview=(),jpeg=()):
subscriptions.sync(demanded_streams(cams,preview,jpeg),create,destroyed.append)
sync(['left_color','overhead_color'])
self.assertEqual(set(created),{'left_color','overhead_color'})
sync(['right_depth','overhead_depth'])
self.assertEqual(set(subscriptions.items),{'right_depth','overhead_depth'})
self.assertEqual(set(destroyed),{'left_color','overhead_color'})
sync([],['left_color'])
self.assertEqual(set(subscriptions.items),{'left_color'})
sync(['sim_wrist_color'])
self.assertEqual(set(subscriptions.items),{'sim_wrist_color'})
sync();self.assertEqual(subscriptions.items,{})
sync(['unknown_color']);self.assertEqual(subscriptions.items,{})
station/test/test_tasks.py
+35
View File
@@ -0,0 +1,35 @@
import threading
import time
from unittest import TestCase
from station.tasks import TaskManager
class TasksTest(TestCase):
def setUp(self): self.manager=TaskManager()
def tearDown(self): self.manager.close()
def wait(self,token):
until=time.monotonic()+3
while time.monotonic()<until:
row=self.manager.snapshot(token)
if row['state'] not in ('running','queued'): return row
time.sleep(.01)
self.fail('task did not complete')
def test_resources_held_until_completion(self):
gate=threading.Event()
one=self.manager.submit('move',{'arm:89'},lambda cancel:(gate.wait(2),{'ok':True})[1])
with self.assertRaises(RuntimeError): self.manager.submit('move-again',{'arm:89'},lambda e:{'ok':True})
other=self.manager.submit('camera',{'camera'},lambda e:{'ok':True})
self.assertEqual(self.wait(other)['state'],'succeeded')
gate.set();self.assertEqual(self.wait(one)['state'],'succeeded')
next_task=self.manager.submit('next',{'arm:89'},lambda e:{'ok':True})
self.assertEqual(self.wait(next_task)['state'],'succeeded')
def test_cancel_and_exception_release_ownership(self):
token=self.manager.submit('long',{'arm'},lambda event:(event.wait(2),{'ok':False})[1])
self.manager.cancel(token)
self.assertEqual(self.wait(token)['state'],'cancelled')
def fail(event): raise ValueError('driver rejected')
token=self.manager.submit('reject',{'arm'},fail)
result=self.wait(token)
self.assertEqual(result['state'],'failed');self.assertIn('driver rejected',result['result']['message'])