diff --git a/config/site.example.yaml b/config/site.example.yaml index d0a65d3..116f291 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -812,11 +812,9 @@ hole_detect: max_depth_m: 0.60 linger_sec: 8.0 # 完成后继续发布结果/叠图的时长 - # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）===== - # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。 - # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座 - # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入 - # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入 + # ===== 旋转装配仍读这里的力阈值（stage3_search_node）===== + # 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。 + # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw stage3: # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移， # 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）—— @@ -843,9 +841,12 @@ hole_detect: drop_frames: 2 # 跌落需持续的帧数（防毛刺） center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏） center_scan_mm: 0.3 - insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth + insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入。旋转装配不读这项 insert_retry: 3 - screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规） + screw_pitch_mm: 0.0 # 搜孔轻压不旋。旋转装配不读这项 + # 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。 + screw_stage_depth_mm: 6.0 + screw_stage_pitch_mm: 1.0 search_speed: 0.03 # 搜孔笛卡尔速度 [m/s] settle_sec: 0.25 # 每个微步后的静止时间 tactile_timeout_s: 3.0 # 触觉话题断流多久中止 @@ -1128,7 +1129,8 @@ go_home: network: # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179） - # MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enxXXXXXXXXXXXX）。 + # MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enxXXXXXXXXXXXX 之间变， + # NetworkManager 按 MAC 绑定，不按名字。 # 本机 ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100 # NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。 # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。 diff --git a/config/site.yaml b/config/site.yaml index 25dc048..1a84117 100644 --- a/config/site.yaml +++ b/config/site.yaml @@ -796,11 +796,9 @@ hole_detect: max_depth_m: 0.60 linger_sec: 8.0 # 完成后继续发布结果/叠图的时长 - # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）===== - # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。 - # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座 - # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入 - # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入 + # ===== 旋转装配仍读这里的力阈值（stage3_search_node）===== + # 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。 + # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw stage3: # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移， # 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）—— @@ -827,9 +825,12 @@ hole_detect: drop_frames: 2 # 跌落需持续的帧数（防毛刺） center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏） center_scan_mm: 0.3 - insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth + insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入。旋转装配不读这项 insert_retry: 3 - screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规） + screw_pitch_mm: 0.0 # 搜孔轻压不旋。旋转装配不读这项 + # 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。 + screw_stage_depth_mm: 6.0 + screw_stage_pitch_mm: 1.0 search_speed: 0.03 # 搜孔笛卡尔速度 [m/s] settle_sec: 0.25 # 每个微步后的静止时间 tactile_timeout_s: 3.0 # 触觉话题断流多久中止 @@ -1112,7 +1113,8 @@ go_home: network: # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179） - # MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。 + # MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enx6c1ff7d06f17 之间变， + # NetworkManager 按 MAC 绑定，不按名字。 # 本机 192.168.2.10 ↔ 063989(左89)=.101 / 063991(右91)=.100 # NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。 # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。 diff --git a/docs/ENVIRONMENT.md b/docs/ENVIRONMENT.md index 79a9fc6..c2cb789 100644 --- a/docs/ENVIRONMENT.md +++ b/docs/ENVIRONMENT.md @@ -38,16 +38,16 @@ ## 2. 新机器先对齐的路径 -下面这些路径被脚本写死为用户 `cc` 的家目录。新机器用**同一个用户名** `cc`（家目录 `/home/cc`）时，后面的命令可以原样复制。 +脚本跟**当前用户的家目录**走，不写死 `/home/cc`。用户名可以不是 `cc`。只支持 Linux。 | 路径 | 谁在用 | |------|--------| -| `/home/cc/anaconda3` | `scripts/run_real.sh`、`scripts/run_mock_demo.sh` 直接 `source` 这个 `conda.sh`。`scripts/env.sh` 用的是 `$HOME/anaconda3`，两边要是同一个目录 | -| `/home/cc/rdk_py310/bin/python` | `run_real.sh`、`test_rdk_python.sh`、`rdk_switch_tool.sh`、`scripts/lib/real_preflight.py` 的默认 `RDK_PYTHON` | -| `/home/cc/flexiv_install` | `run_real.sh` 用 `$HOME/flexiv_install`（随用户走） | -| `/home/cc/ros2` | 工作区。`colcon build` 必须在这条 ASCII 路径下，不要在中文目录里编 | +| `$HOME/anaconda3` 或 `$HOME/miniconda3` | `scripts/lib/dev_tools.sh`。环境名默认 `ros_flexiv`，可用 `REAL_ENV_NAME` 改 | +| `$HOME/rdk_py310/bin/python` | 默认的 RDK 解释器。已经在别的目录时，启动前 `export RDK_PYTHON=...` | +| `$HOME/flexiv_install` | `run_real.sh` / `env.sh` 的 `LD_LIBRARY_PATH` | +| 仓库目录 | 克隆到哪就从哪启动。`colcon build` 必须在 ASCII 路径下，不要在中文目录里编 | -用户名不是 `cc` 时，至少要改 `scripts/run_real.sh` 里的 conda 路径，并在启动前 `export RDK_PYTHON=$HOME/rdk_py310/bin/python`。否则真机入口会去找 `/home/cc`。 +机械臂、相机、夹爪、网卡和 IP 只写在 `config/site.yaml`。臂号是序列号末两位，换一台臂改 SN，不要改程序里的 89/91。工位窗口的显示器跟当前登录会话（`who` 里的 `:0` / `:1`），不写死某一块屏。 建议磁盘空闲 **40GB 以上**（两个 conda 环境加下载缓存）。 @@ -82,7 +82,7 @@ ACTION=="add|bind", SUBSYSTEM=="usb", ATTR{idVendor}=="1bcf", ATTR{idProduct}==" ## 4. Anaconda -装到 **`$HOME/anaconda3`**（安装器里选 Just Me、路径填这个）。不要装成 `~/miniconda3` 或其他目录：`run_real.sh` 写死了 `/home/cc/anaconda3/etc/profile.d/conda.sh`。 +装到 **`$HOME/anaconda3`**（安装器里选 Just Me、路径填这个）。`$HOME/miniconda3` 也能被 `scripts/lib/dev_tools.sh` 找到。不要装到这两个目录以外的路径，除非自己改 `dev_source_conda`。 本机 conda 版本是 25.11.1。装完新开一个终端： diff --git a/docs/GRIPPER.md b/docs/GRIPPER.md index 3cd977c..f1f84c7 100644 --- a/docs/GRIPPER.md +++ b/docs/GRIPPER.md @@ -30,6 +30,8 @@ bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2 PGEA 力/速：`gripper.force_pct`（最低 20）`speed_pct` `open_position` `close_position`。RGI 不要 `auto_init: true`（`robots[]` 里已关）。 +平行爪的 `--cli` 在驱动没跑时会先把节点拉起来，再把命令发到 `/gripper/ctrl`。`auto_init` 仍用这一臂在 `site.yaml` 里的值（现场 `false`，不写 `0xA5`，不回零）。串口被别的进程占着时，报的是占用。 + ## ROS 节点 单爪： diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index 9da9c49..ab2832b 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -34,7 +34,7 @@ ### 1.3 一次完整任务分几步 -代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完： +代码里跟踪和对孔串成一条命令。搜孔轻压已取消，`run_all.sh` 不会再进入那一步，也不能用参数打开。旋入是单独的 `run_stage3_search.sh --screw`。 ``` run_all.sh @@ -46,18 +46,13 @@ run_all.sh │ YOLO 找工件外框 → Hough 找左右两个孔心 → │ 深度 + 内参 + 手眼 → 换算出孔的基座坐标 → │ 移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定 - │ - └─ stage3_search 柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里， - 再十字微扫定心，最后在力限内下压（可选按螺距旋入） ``` -三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。 -盯日志就能知道跑到哪了 —— 三行关键输出： +跟踪和对孔在同一个进程里交接。盯日志： ``` [pipeline] === stage1_track === → 在等跟踪锁定 [pipeline] === stage2_locate === → 在拍照对孔 -[pipeline] === stage3_search === → 触面 → ★ 入孔 → 轻压到位 ``` 日常入口（三个终端，**必须同一个 `--arm`**）： @@ -65,7 +60,7 @@ run_all.sh ```bash bash ~/ros2/scripts/run_real.sh --arm 89 # T1 控制栈，一直开着 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # T2 相机预览，一直开着 -bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（默认不拧入） +bash ~/ros2/scripts/run_all.sh --arm 89 # T3 跟踪 + 对孔（不含搜孔、不含旋入） bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34 ``` @@ -112,8 +107,8 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control | 能力 | 状态 | 怎么自己确认 | |------|------|--------------| -| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` | -| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` | +| 跟踪 → 对孔 | ✅ 真机跑通过。搜孔轻压已取消，这条命令不再下压搜孔 | `run_all.sh --arm 89` | +| 按螺距旋入（螺纹规） | ✅ 单独阶段。不带 `--screw` 的旧搜孔命令会直接拒绝 | `run_stage3_search.sh --arm 89 --screw` | | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) | | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 | | D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` | @@ -510,7 +505,7 @@ bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的 | **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** | | **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 | | **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） | -| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 | +| **stage1/2** | 跟踪 / 对孔，见 1.3。搜孔轻压已取消 | | **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 | | **基线** | 触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值 | | **R1.1 / M1 …** | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 里的规则编号，文档和提交信息里直接引用 | diff --git a/docs/HOLE_DETECT.md b/docs/HOLE_DETECT.md index fba4c2a..5c984a3 100644 --- a/docs/HOLE_DETECT.md +++ b/docs/HOLE_DETECT.md @@ -57,8 +57,7 @@ hole_detect: ## 三阶段串联（推荐日常入口） -一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入** -（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。 +一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。 ```bash # 终端 1 —— 控制栈（一直开着） @@ -67,12 +66,12 @@ bash ~/ros2/scripts/run_real.sh --arm 89 # 终端 2 —— 相机预览（一直开着，看跟踪画面） bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview -# 终端 3 —— 串联（搜到孔后轻压保持，不拧） -bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔 + 搜孔轻压 +# 终端 3 —— 跟踪 + 对孔（到这里停，不搜孔） +bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔 bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 换 k2 bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪 -# 若这一次就要拧入（一般留给后续阶段）： -bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0 +# 旋入不写在这条命令里： +bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw ``` `run_pipeline.sh` 是 `run_all.sh` 的别名。 @@ -85,9 +84,9 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0 1. `[pipeline] === stage1_track ===` → 等跟踪锁定 2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段` 3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准 -4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入） +对孔结束后臂停在当前高度。搜孔轻压不再接着跑。 -急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看 +急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看 `/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。 高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、 @@ -149,7 +148,10 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY，规 | `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 | | `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） | -## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋） +## 旋转装配（原三阶段节点；搜孔轻压入口已取消） + +不带 `--screw` 时 `run_stage3_search.sh` 直接退出，不会触面，也不会螺旋搜孔。 +下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。 前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程： @@ -167,12 +169,9 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY，规 7. **hold** — 锁定关节，发布结果 ```bash -# 前置：T1 run_real.sh --arm 89；二阶段已完成（臂保持不动） -bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座 -bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入 -bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # M6 螺纹规旋入 -bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径 -bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准一次 +# 前置：T1 run_real.sh --arm 89；对孔已完成（臂保持不动） +# 不带 --screw 会直接退出，不会搜孔。 +bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw ``` **力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且 diff --git a/install/arm_bringup/share/arm_bringup/config/bringup.yaml b/install/arm_bringup/share/arm_bringup/config/bringup.yaml index 3bbe47a..cc98a0f 100644 --- a/install/arm_bringup/share/arm_bringup/config/bringup.yaml +++ b/install/arm_bringup/share/arm_bringup/config/bringup.yaml @@ -4,7 +4,7 @@ rizon_type: Rizon4 # Flexiv Studio 夹爪（GN01）—— 保持 false。DH PGEA 使用 run_gripper.sh。 load_gripper: false use_fake_hardware: false -start_rviz: true +start_rviz: false # Flexiv 官方关节命名：Rizon4-063989_joint1 ... joint7 joint_prefix: Rizon4-063989_ diff --git a/install/arm_bringup/share/arm_bringup/launch/real.launch.py b/install/arm_bringup/share/arm_bringup/launch/real.launch.py index ab05017..11425de 100644 --- a/install/arm_bringup/share/arm_bringup/launch/real.launch.py +++ b/install/arm_bringup/share/arm_bringup/launch/real.launch.py @@ -74,6 +74,6 @@ def generate_launch_description(): DeclareLaunchArgument('use_fake_hardware', default_value='false'), # 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh DeclareLaunchArgument('load_gripper', default_value='false'), - DeclareLaunchArgument('start_rviz', default_value='true'), + DeclareLaunchArgument('start_rviz', default_value='false'), OpaqueFunction(function=_launch_setup), ]) diff --git a/install/arm_bringup/share/arm_bringup/launch/sim.launch.py b/install/arm_bringup/share/arm_bringup/launch/sim.launch.py index 9961860..7a24856 100644 --- a/install/arm_bringup/share/arm_bringup/launch/sim.launch.py +++ b/install/arm_bringup/share/arm_bringup/launch/sim.launch.py @@ -35,7 +35,7 @@ def generate_launch_description(): ) return LaunchDescription([ - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_rviz', default_value='false'), DeclareLaunchArgument('use_moveit', default_value='false'), # 仿真**故意**不用真机的 SN 前缀：两边同时跑时，同名关节会让 TF 与 # 控制器互相打架（症状是 RViz 里模型乱跳）。见 devices/sim/gazebo/README.md。 diff --git a/install/arm_bringup/share/arm_bringup/launch/stack.launch.py b/install/arm_bringup/share/arm_bringup/launch/stack.launch.py index 7b39d13..e137c19 100644 --- a/install/arm_bringup/share/arm_bringup/launch/stack.launch.py +++ b/install/arm_bringup/share/arm_bringup/launch/stack.launch.py @@ -201,7 +201,7 @@ def generate_launch_description(): default_value='mock', description='mock | moveit | real | isaac', ), - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_rviz', default_value='false'), DeclareLaunchArgument('robot_sn', default_value=SITE_SN), DeclareLaunchArgument('load_gripper', default_value='false'), OpaqueFunction(function=_setup), diff --git a/install/arm_control/share/arm_control/launch/mock_control.launch.py b/install/arm_control/share/arm_control/launch/mock_control.launch.py index 69fd959..e097ec7 100644 --- a/install/arm_control/share/arm_control/launch/mock_control.launch.py +++ b/install/arm_control/share/arm_control/launch/mock_control.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): ) return LaunchDescription([ - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_rviz', default_value='false'), DeclareLaunchArgument( 'robot_sn', default_value=SITE_ROBOT_SN, diff --git a/install/arm_description/share/arm_description/launch/display.launch.py b/install/arm_description/share/arm_description/launch/display.launch.py index e01fb43..600e7cb 100644 --- a/install/arm_description/share/arm_description/launch/display.launch.py +++ b/install/arm_description/share/arm_description/launch/display.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_gui', default_value='true'), - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_rviz', default_value='false'), # 默认取当前臂（scripts/env.sh 已导出）；不再写死某个 SN。 DeclareLaunchArgument( 'robot_sn', diff --git a/install/arm_moveit_config/share/arm_moveit_config/launch/move_group.launch.py b/install/arm_moveit_config/share/arm_moveit_config/launch/move_group.launch.py index 6f453bb..e882a5b 100644 --- a/install/arm_moveit_config/share/arm_moveit_config/launch/move_group.launch.py +++ b/install/arm_moveit_config/share/arm_moveit_config/launch/move_group.launch.py @@ -69,12 +69,21 @@ def _launch_setup(context, *args, **kwargs): ], ) - return [move_group, rviz] + # 模型在工位程序里看。只有显式 start_rviz:=true 才再开 RViz。 + nodes = [move_group] + if LaunchConfiguration('start_rviz').perform(context).strip().lower() in ('1', 'true', 'yes'): + nodes.append(rviz) + return nodes def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_fake_hardware', default_value='true'), + DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), # 从环境取当前臂 SN，避免靠 xacro 里的写死默认值"蒙对"。 DeclareLaunchArgument( 'robot_sn', diff --git a/scripts/env.sh b/scripts/env.sh index 47bcdca..ed16771 100755 --- a/scripts/env.sh +++ b/scripts/env.sh @@ -11,23 +11,24 @@ elif [[ -f /opt/ros/jazzy/setup.bash ]]; then source /opt/ros/jazzy/setup.bash fi -if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then - # shellcheck disable=SC1091 - source "${HOME}/anaconda3/etc/profile.d/conda.sh" +# conda 可能在 ~/anaconda3 或 ~/miniconda3。不写死用户名。 +# shellcheck disable=SC1091 +source "${ROOT}/scripts/lib/dev_tools.sh" +dev_source_conda || true +if command -v conda >/dev/null 2>&1; then # 干净切换：先把堆叠的激活退到 base，再激活真机环境。 - # 同一 shell 里从仿真环境(ros_gz_sim)切过来时，不清干净会残留 PATH/PYTHONPATH， - # 导致 python/pip 解析到另一个环境的包，构建报 "--editable not recognized" - # 之类与真实原因无关的错。详见 scripts/lib/conda_switch.sh。 + # 同一 shell 里从仿真环境切过来时，不清干净会残留 PATH/PYTHONPATH。 _env_helper="${ROOT}/scripts/lib/conda_switch.sh" + _real_env="${REAL_ENV_NAME:-ros_flexiv}" if [[ -f "${_env_helper}" ]]; then # shellcheck disable=SC1090 source "${_env_helper}" - conda_switch_clean "ros_flexiv" "${SIM_ENV_NAME:-ros_gz_sim}" || \ - conda activate ros_flexiv 2>/dev/null || true + conda_switch_clean "${_real_env}" "${SIM_ENV_NAME:-ros_gz_sim}" || \ + conda activate "${_real_env}" 2>/dev/null || true else - conda activate ros_flexiv 2>/dev/null || true + conda activate "${_real_env}" 2>/dev/null || true fi - unset _env_helper + unset _env_helper _real_env fi if [[ -f "${ROOT}/install/setup.bash" ]]; then @@ -38,13 +39,14 @@ else fi export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}" # pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它 -_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/site-packages" +_pyver="$(python3 -c 'import sys; print(f"{sys.version_info[0]}.{sys.version_info[1]}")' 2>/dev/null || echo 3.11)" +_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/${REAL_ENV_NAME:-ros_flexiv}}/lib/python${_pyver}/site-packages" for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do if [[ -d "${_d}" ]]; then export LD_LIBRARY_PATH="${_d}:${LD_LIBRARY_PATH}" fi done -unset _d _TRT_SITE +unset _d _TRT_SITE _pyver export YOLO_AUTOINSTALL=false export ROS2_WS="${ROOT}" diff --git a/scripts/fetch_flexiv.sh b/scripts/fetch_flexiv.sh index 19f8dca..734e376 100755 --- a/scripts/fetch_flexiv.sh +++ b/scripts/fetch_flexiv.sh @@ -106,11 +106,11 @@ Done. Next steps (Ubuntu 22.04 / Humble + Rizon): source install/setup.bash 4) Real robot (site SN, no gripper yet): - ros2 launch arm_bringup real.launch.py + ros2 launch arm_bringup real.launch.py start_rviz:=false # or explicitly: - ros2 launch arm_bringup real.launch.py robot_sn:= load_gripper:=false + ros2 launch arm_bringup real.launch.py robot_sn:= load_gripper:=false start_rviz:=false Official fake hardware: - ros2 launch flexiv_bringup rizon.launch.py robot_sn:= use_fake_hardware:=true load_gripper:=false + ros2 launch flexiv_bringup rizon.launch.py robot_sn:= use_fake_hardware:=true load_gripper:=false start_rviz:=false EOF diff --git a/scripts/lib/rdk_status.py b/scripts/lib/rdk_status.py index b0194cf..11c6603 100644 --- a/scripts/lib/rdk_status.py +++ b/scripts/lib/rdk_status.py @@ -117,10 +117,48 @@ def classify_state( ) -def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]: - """连一次机器人并读状态。返回给 classify_state 的关键字参数。""" - import flexivrdk # 延迟导入：让本模块在没有 flexivrdk 的环境里也能被单测导入 +def pack_status(state: Dict[str, object]) -> Dict[str, object]: + """把探针结果收成给界面的 JSON。 + ``q`` 不能传进 ``classify_state``：那个函数只认使能字段。 + 曾经用 ``classify_state(**state)``，关节角一读到就抛 TypeError， + JSON 根本没打出来，界面就永远停在“还没连上”。 + """ + fields = dict(state) + q = fields.pop("q", None) + level, summary, actions = classify_state(**fields) # type: ignore[arg-type] + payload: Dict[str, object] = { + **fields, + "level": level, + "summary": summary, + "actions": actions, + } + if isinstance(q, list) and len(q) == 7: + payload["q"] = q + return payload + + +def _open_robot(sn: str, iface: str, pc_ip: str): + """按 IPv4 白名单连机器人。网卡名不能直接交给 RDK。""" + import flexivrdk + from rdk_helpers import rdk_whitelist_lists + + # 空名单才退回不指定网卡。指定了 eth0 这种名字时，FastDDS 会把真地址滤掉， + # 表现是 ping 得通、这里一直搜到超时。 + candidates = rdk_whitelist_lists(iface, pc_ip) or [[]] + last = "" + for listen in candidates: + try: + if listen: + return flexivrdk.Robot(sn, list(listen), True, False), "" + return flexivrdk.Robot(sn), "" + except Exception as exc: # noqa: BLE001 + last = str(exc) + return None, last or "连不上 RDK" + + +def _probe(sn: str, iface: str, enable: bool, pc_ip: str = "") -> Dict[str, object]: + """连一次机器人并读状态。返回给 classify_state 的关键字参数，另加关节角 ``q``。""" out: Dict[str, object] = { "connected": False, "operational": False, @@ -130,13 +168,9 @@ def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]: "estop_released": True, "error": "", } - try: - if iface: - robot = flexivrdk.Robot(sn, [iface], True, False) - else: - robot = flexivrdk.Robot(sn) - except Exception as exc: # noqa: BLE001 - out["error"] = str(exc) + robot, error = _open_robot(sn, iface, pc_ip) + if robot is None: + out["error"] = error return out out["connected"] = True @@ -157,6 +191,13 @@ def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]: out["op_status"] = str(_safe(robot.operational_status, "")) out["fault"] = bool(_safe(robot.fault, False)) out["estop_released"] = bool(_safe(robot.estop_released, True)) + # 只读关节角，给界面画姿态。读不到就空着，绝不在这里 Enable。 + try: + q = [float(v) for v in list(robot.states().q)[:7]] + except Exception: # noqa: BLE001 + q = [] + if len(q) == 7 and all(v == v for v in q): + out["q"] = q return out @@ -164,18 +205,20 @@ def main(argv: Optional[Sequence[str]] = None) -> int: ap = argparse.ArgumentParser(description="读一次机械臂状态并给结论") ap.add_argument("--sn", required=True) ap.add_argument("--iface", default="") + ap.add_argument("--pc-ip", default="", help="本机机器人网卡的 IPv4，用来做 RDK 白名单") ap.add_argument("--enable", action="store_true", help="顺手尝试 Enable（会真的使能）") ap.add_argument("--out", default="", help="把 KEY=VALUE 写到这个文件") ap.add_argument("--json", action="store_true", help="把结果打到 stdout（JSON）") args = ap.parse_args(argv) - state = _probe(args.sn, args.iface, args.enable) - level, summary, actions = classify_state(**state) # type: ignore[arg-type] - payload = {**state, "level": level, "summary": summary, "actions": actions} + state = _probe(args.sn, args.iface, args.enable, args.pc_ip) + payload = pack_status(state) + level = str(payload["level"]) + actions = list(payload["actions"]) if args.out: lines = [f"{k}={v}" for k, v in payload.items() if k != "actions"] - lines.append("actions=" + " | ".join(actions)) + lines.append("actions=" + " | ".join(str(item) for item in actions)) Path(args.out).write_text("\n".join(lines) + "\n", encoding="utf-8") if args.json: print(json.dumps(payload, ensure_ascii=False, indent=2)) diff --git a/scripts/run_all.sh b/scripts/run_all.sh index 9e20e89..ef771a3 100755 --- a/scripts/run_all.sh +++ b/scripts/run_all.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# 三阶段装配流程的完整入口：一阶段跟踪 → 二阶段对孔 → 三阶段搜孔轻压。 +# 跟踪 → 对孔。搜孔轻压已取消，本脚本不再进入那一步，也不能用参数打开。 # # 为什么要单独有这个脚本： # pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。 @@ -13,10 +13,10 @@ # 4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数 # # 用法： -# bash ~/ros2/scripts/run_all.sh --arm 89 # 全流程 +# bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔 # bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 指定对 k2 孔 # bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪 -# bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0 +# 旋入不在这里。用 run_stage3_search.sh --arm 89 --screw # bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗 # bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了 # bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务 @@ -37,7 +37,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" # ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ---- SKIP_STAGE1=false SKIP_STAGE2=false -SKIP_STAGE3=false +SKIP_STAGE3=true # 搜孔已取消。--skip-stage3 仍然接受，但没有开关能把它重新打开。 HOLE="" DEPTH="" PITCH="" @@ -83,6 +83,12 @@ while [[ $# -gt 0 ]]; do esac done +if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then + echo "ERROR: 搜孔轻压已取消，整体流程不再搜孔，也不在这里旋入。" >&2 + echo " 旋入请用: bash ~/ros2/scripts/run_stage3_search.sh --arm ${SITE_ROBOT_ARM:-89} --screw" >&2 + exit 1 +fi + if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2 exit 1 @@ -241,7 +247,7 @@ fi # 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。 _SKIP_TACTILE_REASON="" if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then - echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ===" + echo "=== 搜孔已取消：跟踪和对孔不用触觉，跳过拉起 ===" WITH_TACTILE=false _SKIP_TACTILE_REASON="stage3" fi diff --git a/scripts/run_camera_d405.sh b/scripts/run_camera_d405.sh index 4821e9a..829337c 100755 --- a/scripts/run_camera_d405.sh +++ b/scripts/run_camera_d405.sh @@ -31,6 +31,8 @@ done # shellcheck disable=SC1091 source "${ROOT}/scripts/env.sh" +# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。 +export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" @@ -127,12 +129,14 @@ DO_PREVIEW="${SITE_CAM_PREVIEW:-1}" if [[ "${PREVIEW}" -ge 0 ]]; then DO_PREVIEW="${PREVIEW}" fi -if [[ "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then +# 程序界面只看彩色和深度。红外是旧的四窗格查看器才要的。 +# 彩色要等红外齐帧时，场景一动，整组帧对不齐，彩色就被丢掉。 +if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then CAM_ARGS+=(-p enable_infra:=true) else CAM_ARGS+=(-p enable_infra:=false) fi -if [[ "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then +if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then CAM_ARGS+=(-p preview_stereo:=true) else CAM_ARGS+=(-p preview_stereo:=false) diff --git a/scripts/run_camera_d435.sh b/scripts/run_camera_d435.sh index af6a5ca..937a425 100755 --- a/scripts/run_camera_d435.sh +++ b/scripts/run_camera_d435.sh @@ -17,6 +17,8 @@ while [[ $# -gt 0 ]]; do done # shellcheck disable=SC1091 source "${ROOT}/scripts/env.sh" +# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。 +export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" SN="${SITE_D435_SERIAL:-}" diff --git a/scripts/run_display.sh b/scripts/run_display.sh index 1594a38..9cf882b 100755 --- a/scripts/run_display.sh +++ b/scripts/run_display.sh @@ -1,7 +1,7 @@ #!/usr/bin/env bash -# 只看 URDF（joint_state_publisher_gui + RViz） +# 发布 URDF。模型在工位程序里看，不再弹出 RViz 和关节滑条窗。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" # shellcheck disable=SC1091 source "${ROOT}/install/setup.bash" -ros2 launch arm_description display.launch.py +ros2 launch arm_description display.launch.py use_rviz:=false use_gui:=false diff --git a/scripts/run_gripper.sh b/scripts/run_gripper.sh index 3b53d7a..f156c78 100755 --- a/scripts/run_gripper.sh +++ b/scripts/run_gripper.sh @@ -61,6 +61,107 @@ fi # shellcheck disable=SC1091 source "${ROOT}/scripts/env.sh" +# 平行爪节点起来之后串口是它的。再开一次会报占用，开合就失效。 +# 这时把命令交给节点已经在听的 /gripper/ctrl。 +pgea_node_up() { + local line + while IFS= read -r line; do + [[ "${line}" == *pgrep* ]] && continue + [[ -n "${line}" ]] && return 0 + done < <(pgrep -af 'dh_pgea_node.py|lib/arm_application/dh_pgea' 2>/dev/null || true) + return 1 +} + +# 控制前把平行爪驱动拉起来。不 pkill 已经在跑的节点，也不写 0xA5。 +# auto_init 用 site 里这一臂的值（现场是 false，回零会把夹着的件丢掉）。 +# setsid：命令脚本退出后驱动还在，不会跟着会话一起被挂断。 +ensure_pgea_node() { + if pgea_node_up; then + return 0 + fi + local port="${SITE_GRIP_PORT:-/dev/ttyUSB0}" + if [[ ! -e "${port}" ]]; then + echo "ERROR: ${port} 不在。USB–RS485 没插上，夹爪驱动起不来。" >&2 + return 1 + fi + local node_py="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py" + local short="${SITE_ROBOT_SHORT_ID:-89}" + local log_dir="/tmp/orisys_console" + local log="${log_dir}/gripper_node_${short}.log" + local auto="${SITE_GRIP_AUTO_INIT:-true}" + mkdir -p "${log_dir}" + echo "=== 平行爪驱动没在跑，先拉起来（auto_init=${auto}，不另写回零） ${port} ===" + setsid python3 "${node_py}" --ros-args \ + -p port:="${port}" \ + -p baud:="${SITE_GRIP_BAUD:-115200}" \ + -p slave_id:="${SITE_GRIP_SLAVE:-1}" \ + -p stroke_mm:="${SITE_GRIP_STROKE_MM:-20.0}" \ + -p force_pct:="${SITE_GRIP_FORCE:-50}" \ + -p speed_pct:="${SITE_GRIP_SPEED:-50}" \ + -p auto_init:="${auto}" \ + -p model:="${MODEL}" \ + -p "serial_number:='${SITE_GRIP_SN:-}'" \ + -p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'" \ + >>"${log}" 2>&1 < /dev/null & + local pid=$! + local i + for i in $(seq 1 24); do + if ! kill -0 "${pid}" 2>/dev/null; then + echo "ERROR: 夹爪驱动起来就退出了。" >&2 + tail -n 30 "${log}" >&2 || true + return 1 + fi + if timeout 2 ros2 topic echo /gripper/states --once >/dev/null 2>&1; then + echo "=== 夹爪驱动已在发 /gripper/states（pid ${pid}）===" + return 0 + fi + done + echo "ERROR: 夹爪驱动没有发出 /gripper/states。" >&2 + tail -n 30 "${log}" >&2 || true + kill "${pid}" 2>/dev/null || true + return 1 +} + +if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then + ensure_pgea_node || exit 1 +fi + +if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]] && pgea_node_up; then + export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}" + export GRIP_CMD="${CLI[0]}" + export GRIP_VAL="${CLI[1]:-}" + export GRIP_FORCE="${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}" + export GRIP_SPEED="${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}" + echo "=== 平行爪节点已占 ${SITE_GRIP_PORT:-/dev/ttyUSB0}，命令走 /gripper/ctrl ===" + CTRL="$(python3 - <<'PY' +import os +from arm_application.dh_pgea import ctrl_values +vals = ctrl_values( + os.environ.get("GRIP_CMD", ""), + os.environ.get("GRIP_VAL", ""), + int(os.environ.get("GRIP_FORCE") or "50"), + int(os.environ.get("GRIP_SPEED") or "50"), +) +if vals is None: + raise SystemExit(2) +print(", ".join(str(v) for v in vals)) +PY +)" && rc=0 || rc=$? + if [[ "${rc}" -eq 2 ]]; then + exec timeout 4 ros2 topic echo /gripper/states --once + fi + if [[ "${rc}" -ne 0 ]]; then + echo "ERROR: 无法把夹爪命令转成 /gripper/ctrl" >&2 + exit 1 + fi + exec timeout 8 ros2 topic pub --once /gripper/ctrl std_msgs/msg/Float32MultiArray "{data: [${CTRL}]}" +fi + +if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then + echo "ERROR: 平行爪命令没有送到 /gripper/ctrl。" >&2 + exit 1 +fi + if [[ ${#CLI[@]} -gt 0 ]]; then if [[ "${IS_ROTARY}" -eq 1 ]]; then CMD="${CLI[0]}" diff --git a/scripts/run_real.sh b/scripts/run_real.sh index e5f7fe1..fc45245 100755 --- a/scripts/run_real.sh +++ b/scripts/run_real.sh @@ -19,7 +19,8 @@ while [[ $# -gt 0 ]]; do --list|-l) LIST_ONLY=true; shift ;; -h|--help) sed -n '2,8p' "$0" - echo " Extra args are passed to ros2 launch (e.g. use_rviz:=false)." + echo " 机械臂模型在工位程序里看，这里默认不开 RViz。" + echo " 其余参数传给 ros2 launch。要临时打开 RViz：use_rviz:=true" exit 0 ;; *) break ;; @@ -127,4 +128,4 @@ if ! "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${RDK_TOOL_ARGS[@]}"; exit 1 fi -exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" "$@" +exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" use_rviz:=false "$@" diff --git a/scripts/run_sim.sh b/scripts/run_sim.sh index 525cffc..06b31ba 100755 --- a/scripts/run_sim.sh +++ b/scripts/run_sim.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -# 快速启动：mock 机械臂 + RViz +# 快速启动 mock 机械臂。模型在工位程序里看，不再弹出 RViz。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" # shellcheck disable=SC1091 @@ -16,4 +16,4 @@ source "${ROOT}/install/setup.bash" # shellcheck source=lib/ros_domain.sh source "${ROOT}/scripts/lib/ros_domain.sh" ros_domain_force_real -ros2 launch arm_bringup sim.launch.py +ros2 launch arm_bringup sim.launch.py use_rviz:=false diff --git a/scripts/run_sim_gz.sh b/scripts/run_sim_gz.sh index 0623003..94e42ca 100755 --- a/scripts/run_sim_gz.sh +++ b/scripts/run_sim_gz.sh @@ -13,7 +13,7 @@ # bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源的运动学验证 # # 与 scripts/run_sim.sh 的区别（两个名字容易混，这里说清）： -# run_sim.sh ← 走 arm_bringup 的 **mock** 驱动：假数据 + RViz，无物理 +# run_sim.sh ← 走 arm_bringup 的 **mock** 驱动：假数据，无物理，模型在工位程序里看 # run_sim_gz.sh ← 走 Gazebo：真物理、真接触 # 两者都不需要真机。要验证力控/接触类逻辑只能用后者。 # diff --git a/scripts/run_stage3_search.sh b/scripts/run_stage3_search.sh index bbd7a96..901f83e 100755 --- a/scripts/run_stage3_search.sh +++ b/scripts/run_stage3_search.sh @@ -1,18 +1,16 @@ #!/usr/bin/env bash -# 三阶段柔顺搜孔：触面 → 力闭环螺旋搜孔 → 十字定心 → 力限插入/旋入 → 锁定。 +# 旋转装配入口。不带 --screw 会直接拒绝，不再单独做搜孔轻压。 # 前置（各占一个终端）： # T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 控制器 # —— 二阶段已完成（run_stage2_locate.sh --arm 89），臂保持悬停不动 # 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了 Orisys 传感器）。 # # 用法： -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座（site 默认深度） -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 # 对 k2 孔 -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入 -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入(M6=1.0) -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径 -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准 -# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile # 不起触觉服务（调试用） +# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw # 深度和螺距只读 site +# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 --screw +# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 --screw # 临时改搜索半径 +# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap --screw # 旋入前用 D405 复拍 +# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile --screw # 不起触觉服务（调试用） # # 参数固定接口：config/site.yaml → hole_detect.stage3（力阈值/螺旋/插入全部可调） # 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看 Orisys 读数量级 @@ -22,6 +20,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" HOLE="" DEPTH="" PITCH="" +SCREW=false RMAX="" RESNAP=false WITH_TACTILE=true @@ -39,7 +38,8 @@ while [[ $# -gt 0 ]]; do --r-max) RMAX="${2:?}"; shift 2 ;; --resnap) RESNAP=true; shift ;; --no-tactile) WITH_TACTILE=false; shift ;; - -h|--help) sed -n '2,24p' "$0"; exit 0 ;; + --screw) SCREW=true; shift ;; + -h|--help) sed -n '2,19p' "$0"; exit 0 ;; *) echo "Unknown: $1" >&2; exit 1 ;; esac done @@ -49,6 +49,44 @@ source "${ROOT}/scripts/env.sh" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/load_site_config.sh" +if [[ "${SCREW}" != true ]]; then + echo "ERROR: 搜孔轻压已取消。这一步不再单独跑，也不再跟在跟踪和对孔后面。" >&2 + echo " 要旋入请加 --screw，深度和螺距只读 site.yaml。" >&2 + exit 1 +fi + +if [[ "${SCREW}" == true ]]; then + if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then + echo "ERROR: --screw 不要再写 --depth 或 --pitch。深度和螺距只在 site.yaml。" >&2 + exit 1 + fi + _screw_line="$(python3 - "${SITE_CONFIG}" << 'PY' +import sys +import yaml + +path = sys.argv[1] +with open(path, encoding="utf-8") as handle: + data = yaml.safe_load(handle) or {} +stage3 = ((data.get("hole_detect") or {}).get("stage3") or {}) +depth = stage3.get("screw_stage_depth_mm") +pitch = stage3.get("screw_stage_pitch_mm") +if not isinstance(depth, (int, float)) or not isinstance(pitch, (int, float)): + sys.stderr.write( + "ERROR: site.yaml 的 hole_detect.stage3 缺少 screw_stage_depth_mm 或 screw_stage_pitch_mm。\n" + ) + sys.exit(1) +if float(depth) <= 0 or float(pitch) == 0: + sys.stderr.write( + f"ERROR: 旋转装配深度必须大于 0、螺距不能为 0，现在是 depth={depth} pitch={pitch}，这样旋不进去。\n" + ) + sys.exit(1) +print(f"{float(depth)} {float(pitch)}") +PY +)" + DEPTH="${_screw_line%% *}" + PITCH="${_screw_line#* }" +fi + if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}" export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}" diff --git a/src/arm_application/arm_application/dh_pgea.py b/src/arm_application/arm_application/dh_pgea.py index 3cfbbbf..117d46a 100644 --- a/src/arm_application/arm_application/dh_pgea.py +++ b/src/arm_application/arm_application/dh_pgea.py @@ -213,6 +213,34 @@ WIRING_HINT = ( ) +def open_failure_text(port: str, exc: BaseException) -> str: + """串口打不开时把「被占用」和「没应答」分开。 + + 独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」， + 界面上看起来像缺自动上电，实际是别的进程还握着这个口。 + """ + text = str(exc).lower() + errno = getattr(exc, "errno", None) + busy = errno in (11, 16) or any( + token in text + for token in ( + "temporarily unavailable", + "device or resource busy", + "could not exclusively lock", + "resource busy", + ) + ) + if busy: + return ( + f"无法独占打开 {port}：{exc}。" + "串口已被别的进程占用（夹爪驱动或状态读取）。" + ) + return ( + f"无法打开 {port}：{exc}。" + "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。" + ) + + class DhPgeaGripper: """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。""" @@ -257,10 +285,7 @@ class DhPgeaGripper: exclusive=True, ) except Exception as exc: - raise DhPgeaError( - f"无法打开 {self.port}：{exc}。" - "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。" - ) from exc + raise DhPgeaError(open_failure_text(self.port, exc)) from exc try: self._ser.dtr = True except Exception: @@ -562,6 +587,36 @@ def _scan_bus(port: str) -> int: return 1 +def ctrl_values( + cmd: str, + value: str = "", + force: int = 50, + speed: int = 50, +) -> Optional[tuple[float, float, float, float]]: + """节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。 + + 顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。 + """ + name = str(cmd or "").strip().lower() + if name in ("probe", "status"): + return None + if name == "init": + return (1.0, 0.0, float(force), float(speed)) + if name == "open": + pos = 1000 + elif name == "close": + pos = 0 + elif name in ("pos", "move"): + if str(value).strip() == "": + raise ValueError("pos 需要 0–1000") + pos = int(round(float(value))) + if pos < 0 or pos > 1000: + raise ValueError("位置只接受 0–1000") + else: + raise ValueError(f"节点在线时不支持 {name}") + return (0.0, float(pos), float(force), float(speed)) + + def _cli(argv: Optional[list[str]] = None) -> int: site = _site_defaults() ap = argparse.ArgumentParser( diff --git a/src/arm_application/arm_application/operator_app.py b/src/arm_application/arm_application/operator_app.py index 34d6e32..9cc7712 100644 --- a/src/arm_application/arm_application/operator_app.py +++ b/src/arm_application/arm_application/operator_app.py @@ -11,14 +11,17 @@ import sys import threading from pathlib import Path -from PySide6.QtCore import QPoint, QRect, QSize, Qt, QThread, QTimer, Signal +from PySide6.QtCore import QPoint, QRect, QRectF, QSize, Qt, QThread, QTimer, Signal from PySide6.QtGui import ( QBrush, QColor, QFont, + QFontMetrics, + QIcon, QImage, QMatrix3x3, QPainter, + QPainterPath, QPen, QPixmap, QPolygon, @@ -46,6 +49,8 @@ from PySide6.QtWidgets import ( QScrollArea, QSizePolicy, QSlider, + QStyle, + QStyleOptionButton, QStackedWidget, QVBoxLayout, QWidget, @@ -114,7 +119,12 @@ class ValueTune(QWidget): row.setContentsMargins(0, 0, 0, 0) row.setSpacing(4) label = QLabel(title) - label.setFixedWidth(52 if compact else 88) + label.setFixedWidth(36 if compact else 88) + label.setAlignment( + Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter + if compact + else Qt.AlignmentFlag.AlignLeft | Qt.AlignmentFlag.AlignVCenter + ) self.slider = QSlider(Qt.Orientation.Horizontal) slo = low if slider_low is None else slider_low shi = high if slider_high is None else slider_high @@ -179,9 +189,9 @@ class _PictureRow(QWidget): return True def heightForWidth(self, width: int) -> int: # noqa: N802 - gap = 4 * max(0, self._columns - 1) + gap = 6 * max(0, self._columns - 1) cell = max(1, (max(1, width) - gap) // self._columns) - return 16 + int(round(cell * 9 / 16)) + return max(96, int(round(cell * 9 / 16))) def resizeEvent(self, event) -> None: # noqa: N802 super().resizeEvent(event) @@ -215,6 +225,8 @@ class FrameLabel(QLabel): self._token = 0 self._drawn = QSize() self._busy = False + self._badge_title = "" + self._badge_detail = "" def sizeHint(self) -> QSize: return QSize(200, 112) if self._compact else QSize(480, 270) @@ -229,6 +241,16 @@ class FrameLabel(QLabel): self._drawn = QSize() super().setText(text) + def set_badge(self, title: str, detail: str = "") -> None: + """画在画面底部的名字和分辨率、帧率。文字变了才重画。""" + title = str(title or "") + detail = str(detail or "") + if title == self._badge_title and detail == self._badge_detail: + return + self._badge_title = title + self._badge_detail = detail + self.update() + def show_jpeg(self, blob: bytes) -> None: token = id(blob) if token == self._token and not self._source.isNull(): @@ -266,17 +288,56 @@ class FrameLabel(QLabel): def paintEvent(self, event) -> None: # noqa: N802 if self._scaled.isNull(): super().paintEvent(event) + if self._badge_title or self._badge_detail: + painter = QPainter(self) + self._clip_round(painter) + self._paint_badge(painter) + painter.end() return painter = QPainter(self) - painter.fillRect(self.rect(), QColor("#0c0e12")) + self._clip_round(painter) + painter.fillRect(self.rect(), QColor("#090c11")) pix = self._scaled painter.drawPixmap( (self.width() - pix.width()) // 2, (self.height() - pix.height()) // 2, pix, ) + self._paint_badge(painter) + painter.end() - def _draw(self, smooth: bool) -> None: + def _clip_round(self, painter: QPainter) -> None: + painter.setRenderHint(QPainter.RenderHint.Antialiasing) + path = QPainterPath() + path.addRoundedRect(QRectF(self.rect()), 10, 10) + painter.setClipPath(path) + + def _paint_badge(self, painter: QPainter) -> None: + if not self._badge_title and not self._badge_detail: + return + bar = 24 + top = max(0, self.height() - bar) + painter.fillRect(0, top, self.width(), bar, QColor(8, 10, 14, 176)) + font = QFont(painter.font()) + font.setPixelSize(12) + painter.setFont(font) + painter.setPen(QColor("#f4f7fb")) + painter.drawText( + 10, top, max(0, self.width() // 2 - 8), bar, + int(Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignLeft), + self._badge_title, + ) + detail = QFont("DejaVu Sans Mono") + detail.setPixelSize(11) + painter.setFont(detail) + painter.setPen(QColor("#d5deea")) + painter.drawText( + self.width() // 2, top, max(0, self.width() // 2 - 10), bar, + int(Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignRight), + self._badge_detail, + ) + + def _draw(self, _smooth: bool) -> None: if self._busy or self._source.isNull(): return size = self.contentsRect().size() @@ -284,11 +345,8 @@ class FrameLabel(QLabel): return self._busy = True try: - mode = ( - Qt.TransformationMode.SmoothTransformation - if smooth - else Qt.TransformationMode.FastTransformation - ) + # 缩放用平滑。快缩放会把原图重新弄糊，帧率并不靠它省出来。 + mode = Qt.TransformationMode.SmoothTransformation self._scaled = self._source.scaled( size, Qt.AspectRatioMode.KeepAspectRatio, @@ -382,9 +440,49 @@ def _title(text: str) -> QLabel: return label +def _section(text: str) -> QWidget: + """一小节的标题。左侧一条色标，用来把相机、触觉、操作分开。""" + row = QWidget() + row.setFixedHeight(36) + lay = QHBoxLayout(row) + lay.setContentsMargins(0, 0, 0, 0) + lay.setSpacing(6) + lay.setAlignment(Qt.AlignmentFlag.AlignVCenter) + mark = QFrame() + mark.setObjectName("sectionMark") + mark.setFixedSize(3, 12) + lay.addWidget(mark) + label = QLabel(text) + label.setObjectName("section") + lay.addWidget(label) + lay.addStretch(1) + return row + + +def _segment_box() -> QWidget: + """一对开关。外框固定高度，里面两颗按钮不再各自被拉成一条。""" + box = QWidget() + box.setObjectName("switch") + box.setFixedHeight(36) + box.setSizePolicy(QSizePolicy.Policy.Fixed, QSizePolicy.Policy.Fixed) + lay = QHBoxLayout(box) + lay.setContentsMargins(3, 3, 3, 3) + lay.setSpacing(2) + return box + + +def _segment_button(button: QPushButton) -> None: + button.setObjectName("seg") + button.setFixedHeight(30) + button.setMinimumWidth(52) + button.setSizePolicy(QSizePolicy.Policy.Fixed, QSizePolicy.Policy.Fixed) + button.setCursor(Qt.CursorShape.PointingHandCursor) + + def _btn(text: str, danger: bool = False) -> QPushButton: button = QPushButton(text) button.setObjectName("danger" if danger else "action") + button.setCursor(Qt.CursorShape.PointingHandCursor) return button @@ -395,25 +493,232 @@ def _fit_text(label: QLabel) -> QLabel: return label -# 操作键上的图案。用字符，不依赖这台机器有没有图标主题。 -_NODE_MARKS = { - "real_up": "⏻", - "camera_d405": "◉", - "camera_d435": "◎", - "tactile_up": "▦", - "gripper_up": "▣", - "gripper_probe": "⌕", - "gripper_open": "◁", - "gripper_close": "▷", - "gripper_rotate": "↻", - "pipeline": "▶", - "stage_track": "⌖", - "stage_locate": "⊕", - "stage_screw": "⟳", - "reset_home": "⌂", - "gauge_dry": "◻", - "gauge_run": "■", -} +def _action_icon(name: str) -> QPixmap: + """操作键上的图案。自己画，不靠系统图标主题，也比一个符号字清楚。""" + pix = QPixmap(64, 64) + pix.setDevicePixelRatio(2.0) + pix.fill(Qt.GlobalColor.transparent) + painter = QPainter(pix) + painter.setRenderHint(QPainter.RenderHint.Antialiasing) + ink = QColor("#e8eef6") + accent = QColor("#8eb7ff") + painter.setPen(QPen(ink, 1.8, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap, Qt.PenJoinStyle.RoundJoin)) + painter.setBrush(Qt.BrushStyle.NoBrush) + if name in ("real_up",): + painter.drawEllipse(QRectF(7, 7, 18, 18)) + painter.setPen(QPen(accent, 2.2, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawLine(16, 8, 16, 16) + elif name in ("camera_d405", "camera_d435"): + painter.setBrush(QColor(94, 168, 255, 56)) + painter.drawRoundedRect(QRectF(4, 10, 24, 14), 3, 3) + painter.setBrush(QColor("#10141b")) + painter.drawEllipse(QRectF(11, 12, 10, 10)) + painter.setBrush(accent) + painter.drawEllipse(QRectF(14.2, 15.2, 3.6, 3.6)) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.setPen(QPen(ink, 1.6, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap, Qt.PenJoinStyle.RoundJoin)) + if name == "camera_d435": + painter.drawLine(10, 24, 8, 28) + painter.drawLine(22, 24, 24, 28) + painter.drawLine(8, 28, 24, 28) + else: + painter.drawRoundedRect(QRectF(12, 6, 8, 4), 1.2, 1.2) + elif name == "tactile_up": + painter.setBrush(QColor(94, 168, 255, 56)) + painter.drawRoundedRect(QRectF(10, 4, 12, 18), 6, 6) + painter.setBrush(accent) + painter.setPen(Qt.PenStyle.NoPen) + for row in (9, 15): + for col in (13, 18): + painter.drawEllipse(QRectF(col, row, 2.4, 2.4)) + painter.setPen(QPen(ink, 1.6, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.drawLine(12, 22, 12, 28) + painter.drawLine(20, 22, 20, 28) + painter.drawLine(10, 28, 22, 28) + elif name in ("gripper_up", "gripper_open", "gripper_close", "gripper_probe", "gripper_rotate"): + painter.drawRoundedRect(QRectF(6, 13, 20, 6), 1.5, 1.5) + left = 8 if name != "gripper_open" else 5 + right = 20 if name != "gripper_open" else 23 + if name == "gripper_close": + left, right = 12, 16 + painter.setBrush(QColor(94, 168, 255, 80)) + painter.drawRoundedRect(QRectF(left, 6, 4, 16), 1.2, 1.2) + painter.drawRoundedRect(QRectF(right, 6, 4, 16), 1.2, 1.2) + painter.setBrush(Qt.BrushStyle.NoBrush) + if name == "gripper_probe": + painter.setPen(QPen(accent, 1.6, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawArc(QRectF(18, 18, 10, 10), 40 * 16, 220 * 16) + elif name == "gripper_rotate": + painter.setPen(QPen(accent, 1.8, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawArc(QRectF(8, 20, 16, 10), 20 * 16, 200 * 16) + painter.drawLine(22, 24, 25, 22) + painter.drawLine(22, 24, 24, 27) + elif name == "pipeline": + painter.setBrush(accent) + painter.setPen(Qt.PenStyle.NoPen) + path = QPainterPath() + path.moveTo(11, 7) + path.lineTo(24, 16) + path.lineTo(11, 25) + path.closeSubpath() + painter.drawPath(path) + elif name == "stage_track": + painter.drawEllipse(QRectF(6, 6, 20, 20)) + painter.drawLine(16, 4, 16, 10) + painter.drawLine(16, 22, 16, 28) + painter.drawLine(4, 16, 10, 16) + painter.drawLine(22, 16, 28, 16) + painter.setBrush(accent) + painter.setPen(Qt.PenStyle.NoPen) + painter.drawEllipse(QRectF(14, 14, 4, 4)) + elif name == "stage_locate": + painter.drawEllipse(QRectF(5, 5, 22, 22)) + painter.drawEllipse(QRectF(10, 10, 12, 12)) + painter.setBrush(accent) + painter.setPen(Qt.PenStyle.NoPen) + painter.drawEllipse(QRectF(14, 14, 4, 4)) + elif name == "stage_screw": + painter.setPen(QPen(ink, 1.8, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawArc(QRectF(6, 6, 20, 20), 40 * 16, 280 * 16) + painter.setPen(QPen(accent, 1.8, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawLine(23, 8, 26, 6) + painter.drawLine(23, 8, 20, 5) + painter.setPen(QPen(ink, 1.6, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawLine(16, 12, 16, 20) + elif name in ("reset_home", "nav_main"): + path = QPainterPath() + path.moveTo(16, 5) + path.lineTo(27, 14) + path.lineTo(24, 14) + path.lineTo(24, 26) + path.lineTo(8, 26) + path.lineTo(8, 14) + path.lineTo(5, 14) + path.closeSubpath() + painter.setBrush(QColor(94, 168, 255, 56)) + painter.drawPath(path) + painter.setBrush(accent) + painter.drawRect(QRectF(13, 18, 6, 8)) + elif name == "nav_eye": + painter.drawEllipse(QRectF(4, 10, 24, 12)) + painter.setBrush(accent) + painter.drawEllipse(QRectF(12, 12, 8, 8)) + painter.setBrush(QColor("#10141b")) + painter.drawEllipse(QRectF(14.5, 14.5, 3, 3)) + elif name == "nav_pen": + painter.drawLine(8, 24, 20, 8) + painter.setPen(QPen(accent, 1.8, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawLine(20, 8, 24, 6) + painter.setPen(QPen(ink, 1.8, Qt.PenStyle.SolidLine, Qt.PenCapStyle.RoundCap)) + painter.drawLine(6, 26, 12, 22) + elif name == "gauge_dry": + painter.drawRoundedRect(QRectF(6, 6, 20, 20), 3, 3) + painter.setBrush(accent) + painter.setPen(Qt.PenStyle.NoPen) + path = QPainterPath() + path.moveTo(13, 11) + path.lineTo(21, 16) + path.lineTo(13, 21) + path.closeSubpath() + painter.drawPath(path) + elif name == "gauge_run": + painter.setBrush(accent) + painter.setPen(Qt.PenStyle.NoPen) + painter.drawRoundedRect(QRectF(6, 6, 20, 20), 4, 4) + painter.setBrush(QColor("#10141b")) + path = QPainterPath() + path.moveTo(13, 11) + path.lineTo(21, 16) + path.lineTo(13, 21) + path.closeSubpath() + painter.drawPath(path) + else: + painter.drawEllipse(QRectF(8, 8, 16, 16)) + painter.end() + return pix + + +class CommandButton(QPushButton): + """操作键。背景自己画成圆角矩形，图标和文字都落在这块矩形里面。 + + ``bar`` 用在相机、触觉那一行：图标在左、字在右，高度跟旁边的开关一样。 + 操作区用上下排，避免和系统按钮样式叠出两层边、再把邻居挤变形。 + """ + + def __init__(self, text: str, kind: str, mark: str, *, bar: bool = False) -> None: + super().__init__(text) + self._bar = bar + self._pix = _action_icon(mark) + self.setIcon(QIcon(self._pix)) + self.setObjectName(kind) + self.setCursor(Qt.CursorShape.PointingHandCursor) + self.setMinimumWidth(0) + self.setFixedHeight(36 if bar else 60) + self.setSizePolicy( + QSizePolicy.Policy.Expanding if not bar else QSizePolicy.Policy.Preferred, + QSizePolicy.Policy.Fixed, + ) + + def sizeHint(self) -> QSize: + if self._bar: + width = QFontMetrics(self.font()).horizontalAdvance(self.text()) + 46 + return QSize(max(96, width), 36) + return QSize(108, 60) + + def minimumSizeHint(self) -> QSize: + return QSize(88 if self._bar else 96, self.height()) + + def paintEvent(self, _event) -> None: # noqa: N802 + painter = QPainter(self) + painter.setRenderHint(QPainter.RenderHint.Antialiasing) + option = QStyleOptionButton() + self.initStyleOption(option) + rect = QRectF(self.rect()).adjusted(0.5, 0.5, -0.5, -0.5) + path = QPainterPath() + path.addRoundedRect(rect, 8, 8) + painter.setClipPath(path) + hover = bool(option.state & QStyle.StateFlag.State_MouseOver) + down = bool(option.state & QStyle.StateFlag.State_Sunken) + kind = self.objectName() + if kind == "primary": + fill = QColor("#2460ad" if hover else "#1d4f91") + border = QColor("#6ea2ea") + ink = QColor("#f4f7fb") + elif kind == "danger": + fill = QColor("#3a1c20" if hover else "#241416") + border = QColor("#c46a6a" if hover else "#8a4a4a") + ink = QColor("#f6d5d5") + else: + fill = QColor("#263041" if hover else "#1c2430") + border = QColor("#5c6b82" if hover else "#3d4b60") + ink = QColor("#e7ebf2") + if down: + fill = fill.darker(115) + painter.fillPath(path, fill) + painter.setPen(QPen(border, 1)) + painter.setBrush(Qt.BrushStyle.NoBrush) + painter.drawPath(path) + font = QFont(self.font()) + if self._bar: + icon_box = QRect(8, (self.height() - 18) // 2, 18, 18) + painter.drawPixmap(icon_box, self._pix) + font.setPixelSize(13) + text_rect = QRect(32, 0, max(0, self.width() - 40), self.height()) + align = Qt.AlignmentFlag.AlignVCenter | Qt.AlignmentFlag.AlignLeft + else: + icon_box = QRect((self.width() - 22) // 2, 6, 22, 22) + painter.drawPixmap(icon_box, self._pix) + font.setPixelSize(12) + text_rect = QRect(6, 32, max(0, self.width() - 12), 22) + align = Qt.AlignmentFlag.AlignHCenter | Qt.AlignmentFlag.AlignVCenter + painter.setFont(font) + painter.setPen(ink) + shown = QFontMetrics(font).elidedText( + self.text(), Qt.TextElideMode.ElideRight, text_rect.width(), + ) + painter.drawText(text_rect, int(align), shown) + painter.end() # Qt 的 QOpenGLFunctions 只带函数，不带这些枚举。数值是 OpenGL 规范里的固定值。 @@ -892,7 +1197,7 @@ class StationApp(QMainWindow): self._images = QTimer(self) self._images.timeout.connect(self._paint_images) # 和相机预览同一档：大约 20 帧，推理框才跟得上画面。 - self._images.start(50) + self._images.start(16) self._stack.currentChanged.connect(lambda _index: self._paint_images()) self._refresh() @@ -922,6 +1227,7 @@ class StationApp(QMainWindow): self._stack.addWidget(page) right.addWidget(self._stack, 1) self._detail = QPlainTextEdit() + self._detail.setObjectName("log") self._detail.setReadOnly(True) self._detail.setFixedHeight(56) self._detail.setPlaceholderText("命令输出显示在这里") @@ -949,23 +1255,31 @@ class StationApp(QMainWindow): def _sidebar(self) -> QWidget: bar = QWidget() bar.setObjectName("side") - bar.setFixedWidth(112) + bar.setFixedWidth(120) lay = QVBoxLayout(bar) - lay.setContentsMargins(8, 8, 8, 8) + lay.setContentsMargins(10, 12, 10, 12) + lay.setSpacing(6) logo = QLabel() if _LOGO.is_file(): pix = QPixmap(str(_LOGO)) logo.setPixmap(pix.scaledToWidth(88, Qt.TransformationMode.SmoothTransformation)) logo.setAlignment(Qt.AlignmentFlag.AlignCenter) lay.addWidget(logo) + brand = QLabel("通止规工位") + brand.setObjectName("brand") + brand.setAlignment(Qt.AlignmentFlag.AlignCenter) + lay.addWidget(brand) items = ( - ("main", "⌂ 主界面"), - ("handeye", "✦ 手眼"), - ("teach", "✎ 示教"), + ("main", "主界面", "nav_main"), + ("handeye", "手眼", "nav_eye"), + ("teach", "示教", "nav_pen"), ) - for key, name in items: + for key, name, mark in items: button = QPushButton(name) + button.setIcon(QIcon(_action_icon(mark))) + button.setIconSize(QSize(18, 18)) button.setObjectName("nav") + button.setFixedHeight(36) button.clicked.connect(lambda _checked=False, k=key: self._show_page(k)) self._nav.append((key, button)) lay.addWidget(button) @@ -989,6 +1303,7 @@ class StationApp(QMainWindow): self._arm_buttons: dict[str, QPushButton] = {} for row in self._catalog: button = _btn(row["label"]) + button.setObjectName("seg") button.clicked.connect(lambda _checked=False, a=row["id"]: self._set_arm(a)) self._arm_buttons[row["id"]] = button lay.addWidget(button) @@ -1018,138 +1333,102 @@ class StationApp(QMainWindow): page = QWidget() lay = QVBoxLayout(page) lay.setContentsMargins(0, 0, 0, 0) - lay.setSpacing(4) - self._lead = _fit_text(QLabel("")) - self._lead.setObjectName("lead") - self._lead.setMaximumHeight(20) - lay.addWidget(self._lead) - self._arm_banner = _fit_text(QLabel(arm_state.REMOTE_PROMPT)) + lay.setSpacing(6) + self._arm_banner = _fit_text(QLabel("")) self._arm_banner.setObjectName("warn") - self._arm_banner.setMaximumHeight(20) + self._arm_banner.setFixedHeight(22) lay.addWidget(self._arm_banner) usb = QHBoxLayout() - usb.setSpacing(4) - self._usb_labels: list[tuple[QLabel, QLabel, QLabel]] = [] + usb.setSpacing(6) + self._usb_labels: list[tuple[QLabel, QLabel]] = [] for name in ("腕部", "手外", "触觉", "串口"): card = _card() - card.setMaximumHeight(52) + card.setFixedHeight(48) box = QVBoxLayout(card) - box.setContentsMargins(6, 2, 6, 2) + box.setContentsMargins(8, 4, 8, 4) box.setSpacing(0) - box.addWidget(_title(name)) + title = _title(name) + title.setAlignment(Qt.AlignmentFlag.AlignCenter) + box.addWidget(title) count = QLabel("—") + count.setObjectName("count") + count.setAlignment(Qt.AlignmentFlag.AlignCenter) flag = QLabel("") - detail = _fit_text(QLabel("")) - detail.setObjectName("muted") - detail.setMaximumHeight(16) + flag.setAlignment(Qt.AlignmentFlag.AlignCenter) line = QHBoxLayout() + line.setSpacing(4) + line.addStretch(1) line.addWidget(count) line.addWidget(flag) line.addStretch(1) box.addLayout(line) - box.addWidget(detail) - usb.addWidget(card) - self._usb_labels.append((count, flag, detail)) + usb.addWidget(card, 1) + self._usb_labels.append((count, flag)) lay.addLayout(usb) - self._arm_lead = _fit_text(QLabel("只读机械臂。Remote 只能在示教器上开。")) - self._arm_lead.setObjectName("muted") - self._arm_lead.setMaximumHeight(18) - lay.addWidget(self._arm_lead) - self._arm_prompt = _fit_text(QLabel(arm_state.REMOTE_PROMPT)) + self._arm_prompt = _fit_text(QLabel("")) self._arm_prompt.setObjectName("warn") - self._arm_prompt.setWordWrap(True) - self._arm_prompt.setMaximumHeight(36) + self._arm_prompt.setFixedHeight(22) + self._arm_prompt.setVisible(False) lay.addWidget(self._arm_prompt) self._arm_sketch = ArmSketch() lay.addWidget(self._arm_sketch, 1) cards = QHBoxLayout() - cards.setSpacing(4) + cards.setSpacing(6) self._arm_cards: dict[str, QLabel] = {} for key, title in ( ("enable", "使能"), ("mode", "模式"), - ("fault", "故障 / 急停"), + ("fault", "故障"), ("source", "数据"), ): card = _card() - card.setMaximumHeight(58) + card.setFixedHeight(48) box = QVBoxLayout(card) - box.setContentsMargins(6, 2, 6, 2) + box.setContentsMargins(6, 4, 6, 4) box.setSpacing(0) - box.addWidget(_title(title)) - label = QLabel("—") - label.setObjectName("muted") - label.setWordWrap(True) + cap = _title(title) + cap.setAlignment(Qt.AlignmentFlag.AlignCenter) + box.addWidget(cap) + label = _fit_text(QLabel("—")) + label.setObjectName("count") + label.setAlignment(Qt.AlignmentFlag.AlignCenter) box.addWidget(label) self._arm_cards[key] = label - cards.addWidget(card) + cards.addWidget(card, 1) lay.addLayout(cards) - self._arm_summary = _fit_text(QLabel("")) - self._arm_summary.setObjectName("muted") - self._arm_summary.setMaximumHeight(18) - lay.addWidget(self._arm_summary) self._arm_joints = _fit_text(QLabel("")) self._arm_joints.setObjectName("muted") - self._arm_joints.setWordWrap(True) - self._arm_joints.setMaximumHeight(34) + self._arm_joints.setAlignment(Qt.AlignmentFlag.AlignCenter) + self._arm_joints.setFixedHeight(22) lay.addWidget(self._arm_joints) lay.addWidget(self._grip_panel()) return page def _grip_panel(self) -> QWidget: - """主界面上的夹爪仿真和下发。放在臂这一列，不跟相机、触觉抢高度。""" + """主界面上的夹爪仿真和下发。状态只留一行，滑条上下对齐。""" panel = _card() lay = QVBoxLayout(panel) - lay.setContentsMargins(6, 4, 6, 4) - lay.setSpacing(4) + lay.setContentsMargins(8, 6, 8, 8) + lay.setSpacing(6) head = QHBoxLayout() - head.addWidget(_title("夹爪")) - self._grip_summary = _fit_text(QLabel("还没读到夹爪")) + head.setSpacing(8) + head.addWidget(_section("夹爪")) + self._grip_summary = _fit_text(QLabel("还没读到")) self._grip_summary.setObjectName("warn") + self._grip_summary.setAlignment(Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter) head.addWidget(self._grip_summary, 1) lay.addLayout(head) - self._grip_lead = _fit_text(QLabel("")) - self._grip_lead.setObjectName("muted") - self._grip_lead.setWordWrap(True) - self._grip_lead.setMaximumHeight(32) - lay.addWidget(self._grip_lead) - body = QHBoxLayout() - body.setSpacing(6) - self._grip_sketch = GripperSketch(120, 96) - self._grip_sketch.setFixedHeight(168) + self._grip_sketch = GripperSketch(160, 96) + self._grip_sketch.setFixedHeight(128) self._grip_sketch.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) - body.addWidget(self._grip_sketch, 2) - ctrl = QVBoxLayout() - ctrl.setSpacing(2) - cards = QHBoxLayout() - cards.setSpacing(4) - self._grip_cards: dict[str, QLabel] = {} - for key, title in ( - ("link", "连接"), - ("opening", "开口"), - ("hold", "夹持"), - ("spin", "旋转"), - ): - card = _card() - card.setMaximumHeight(46) - box = QVBoxLayout(card) - box.setContentsMargins(4, 2, 4, 2) - box.setSpacing(0) - box.addWidget(_title(title)) - label = _fit_text(QLabel("—")) - label.setObjectName("muted") - box.addWidget(label) - self._grip_cards[key] = label - cards.addWidget(card) - ctrl.addLayout(cards) + lay.addWidget(self._grip_sketch) self._pos = ValueTune("位置", 0, 1000, 500, compact=True) - self._force = ValueTune("力度%", 20, 100, 33, compact=True) - self._speed = ValueTune("速度%", 1, 100, 25, compact=True) - ctrl.addWidget(self._pos) - ctrl.addWidget(self._force) - ctrl.addWidget(self._speed) + self._force = ValueTune("力度", 20, 100, 33, compact=True) + self._speed = ValueTune("速度", 1, 100, 25, compact=True) + for widget in (self._pos, self._force, self._speed): + lay.addWidget(widget) row = QHBoxLayout() - row.setSpacing(4) + row.setSpacing(6) apply_btn = _btn("按此夹持", True) open_btn = _btn("张开", True) close_btn = _btn("闭合", True) @@ -1157,22 +1436,17 @@ class StationApp(QMainWindow): open_btn.clicked.connect(lambda: self._grip_preset(1000)) close_btn.clicked.connect(lambda: self._grip_preset(0)) for button in (apply_btn, open_btn, close_btn): + button.setFixedHeight(36) button.setMinimumWidth(0) - button.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Fixed) - row.addWidget(button) - ctrl.addLayout(row) - body.addLayout(ctrl, 3) - lay.addLayout(body) - self._grip_draw = _fit_text(QLabel("")) - self._grip_draw.setObjectName("muted") - self._grip_draw.setMaximumHeight(16) - lay.addWidget(self._grip_draw) + button.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) + row.addWidget(button, 1) + lay.addLayout(row) - self._rotary = _card() + self._rotary = QWidget() rot = QVBoxLayout(self._rotary) - rot.setContentsMargins(6, 4, 6, 4) - rot.setSpacing(2) - rot.addWidget(_title("旋转")) + rot.setContentsMargins(0, 0, 0, 0) + rot.setSpacing(4) + rot.addWidget(_section("旋转")) self._angle = ValueTune( "绝对角", -32767, 32767, 0, step=0.1, decimals=1, slider_low=-3600, slider_high=3600, compact=True, @@ -1185,24 +1459,25 @@ class StationApp(QMainWindow): "圈数", -50, 50, 0.25, step=0.01, decimals=2, slider_low=-10, slider_high=10, compact=True, ) - self._rot_speed = ValueTune("转速%", 1, 100, 25, compact=True) - self._torque = ValueTune("力矩%", 20, 100, 33, compact=True) + self._rot_speed = ValueTune("转速", 1, 100, 25, compact=True) + self._torque = ValueTune("力矩", 20, 100, 33, compact=True) for widget in (self._angle, self._delta, self._turns, self._rot_speed, self._torque): rot.addWidget(widget) - rot_row = QHBoxLayout() - rot_row.setSpacing(4) - for title, kind in ( + rot_grid = QGridLayout() + rot_grid.setSpacing(6) + for index, (title, kind) in enumerate(( ("转到绝对角", "rotate"), ("相对转动", "rotate_by"), ("按圈数转", "revolutions"), ("旋转回零", "rotate_home"), - ): + )): button = _btn(title, True) + button.setFixedHeight(36) button.setMinimumWidth(0) - button.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Fixed) + button.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) button.clicked.connect(lambda _checked=False, k=kind: self._grip_move(k)) - rot_row.addWidget(button) - rot.addLayout(rot_row) + rot_grid.addWidget(button, index // 2, index % 2) + rot.addLayout(rot_grid) self._rotary.setVisible(False) lay.addWidget(self._rotary) return panel @@ -1211,159 +1486,199 @@ class StationApp(QMainWindow): page = QWidget() lay = QVBoxLayout(page) lay.setContentsMargins(0, 0, 0, 0) - lay.setSpacing(4) - self._infer_line = _fit_text(QLabel("推理 腕部 没有框 手外 没有框")) - self._infer_line.setObjectName("ok") - self._infer_line.setMaximumHeight(20) - lay.addWidget(self._infer_line) - self._cam_note = _fit_text(QLabel("")) - self._cam_note.setObjectName("muted") - self._cam_note.setMaximumHeight(18) - lay.addWidget(self._cam_note) + lay.setSpacing(6) cam_bar = QHBoxLayout() - cam_bar.addWidget(_title("相机")) - depth = _btn("◎ 深度") - depth.clicked.connect(self._toggle_depth) - self._depth_btn = depth + cam_bar.setSpacing(6) + cam_bar.addWidget(_section("相机")) + source = _segment_box() + self._real_mode = QPushButton("真机") + self._sim_mode = QPushButton("仿真") + self._cam_source = "real" + for button, sim in ((self._real_mode, False), (self._sim_mode, True)): + _segment_button(button) + button.clicked.connect(lambda _checked=False, use_sim=sim: self._set_source(use_sim)) + source.layout().addWidget(button) + self._real_mode.setProperty("active", True) + cam_bar.addWidget(source) + switch = _segment_box() + self._color_mode = QPushButton("彩色") + self._depth_mode = QPushButton("深度") self._depth_on = False - cam_bar.addWidget(depth) + for button, on in ((self._color_mode, False), (self._depth_mode, True)): + _segment_button(button) + button.clicked.connect(lambda _checked=False, depth=on: self._set_depth(depth)) + switch.layout().addWidget(button) + self._color_mode.setProperty("active", True) + cam_bar.addWidget(switch) for action in ("camera_d405", "camera_d435"): - cam_bar.addWidget(self._node_button(action)) + button = self._node_button(action, bar=True) + cam_bar.addWidget(button) cam_bar.addStretch(1) + self._infer_line = _fit_text(QLabel("腕部没有框 手外没有框")) + self._infer_line.setObjectName("ok") + self._infer_line.setAlignment(Qt.AlignmentFlag.AlignRight | Qt.AlignmentFlag.AlignVCenter) + cam_bar.addWidget(self._infer_line) lay.addLayout(cam_bar) + self._cam_note = _fit_text(QLabel("")) + self._cam_note.setObjectName("warn") + self._cam_note.setFixedHeight(22) + self._cam_note.setVisible(False) + lay.addWidget(self._cam_note) self._cam_pic: dict[str, FrameLabel] = {} cams = camera_view.targets() + self._slot_role = {cam["id"]: cam["role"] for cam in cams} + sim_rows = camera_view.sim_targets() + self._sim_id = {item["role"]: item["id"] for item in sim_rows} + self._sim_title = { + item["role"]: {"color": item["label"], "depth": f"{item['label']} 深度"} + for item in sim_rows + } colors = QHBoxLayout() - colors.setSpacing(4) + colors.setSpacing(6) colors.setContentsMargins(0, 0, 0, 0) - depths = QHBoxLayout() - depths.setSpacing(4) - depths.setContentsMargins(0, 0, 0, 0) + self._cam_title: dict[str, str] = {} for cam in cams: - col = QVBoxLayout() - col.setSpacing(0) - cap = QLabel(cam["label"]) - cap.setObjectName("muted") - cap.setMaximumHeight(16) - shot = FrameLabel("没有画面", compact=True) + shot = FrameLabel("", compact=True) + shot.set_badge(cam["label"], "等待画面") self._cam_pic[f"{cam['id']}_color"] = shot - col.addWidget(cap) - col.addWidget(shot, 1) - colors.addLayout(col) - dcol = QVBoxLayout() - dcol.setSpacing(0) - dcap = QLabel(f"{cam['label']} 深度") - dcap.setObjectName("muted") - dcap.setMaximumHeight(16) - dshot = FrameLabel("没有深度", compact=True) - self._cam_pic[f"{cam['id']}_depth"] = dshot - dcol.addWidget(dcap) - dcol.addWidget(dshot, 1) - depths.addLayout(dcol) + self._cam_pic[f"{cam['id']}_depth"] = shot + self._cam_title[f"{cam['id']}_color"] = cam["label"] + self._cam_title[f"{cam['id']}_depth"] = f"{cam['label']} 深度" + colors.addWidget(shot, 1) color_box = _PictureRow(len(cams) or 1) color_box.setLayout(colors) self._color_box = color_box lay.addWidget(color_box, 0) - self._depth_box = _PictureRow(len(cams) or 1) - self._depth_box.setLayout(depths) - self._depth_box.setVisible(False) - lay.addWidget(self._depth_box, 0) tac_bar = QHBoxLayout() - tac_bar.addWidget(_title("视触觉")) - see = _btn("▶ 开始成像") - hide = _btn("■ 停止成像") + tac_bar.setSpacing(6) + tac_bar.addWidget(_section("视触觉")) + see = _btn("开始成像") + hide = _btn("停止成像") + for button in (see, hide): + button.setFixedHeight(36) see.clicked.connect(lambda: self._run("tactile_view_on", False)) hide.clicked.connect(lambda: self._run("tactile_view_off", False)) tac_bar.addWidget(see) tac_bar.addWidget(hide) - tac_bar.addWidget(self._node_button("tactile_up")) + tac_bar.addWidget(self._node_button("tactile_up", bar=True)) tac_bar.addStretch(1) lay.addLayout(tac_bar) self._view_note = _fit_text(QLabel("")) - self._view_note.setObjectName("muted") - self._view_note.setMaximumHeight(18) + self._view_note.setObjectName("warn") + self._view_note.setFixedHeight(22) + self._view_note.setVisible(False) lay.addWidget(self._view_note) self._charts: dict[str, ForceChart] = {} self._pic: dict[str, FrameLabel] = {} - self._mini = [QLabel("左指 —"), QLabel("右指 —")] - pics = QHBoxLayout() - pics.setSpacing(4) - for index, (side, name) in enumerate((("left", "左指"), ("right", "右指"))): - for kind, caption in (("shear", "切向力"), ("thermal", "热成像")): - col = QVBoxLayout() - col.setSpacing(0) - cap = QLabel(f"{name} {caption}") - cap.setObjectName("muted") - cap.setMaximumHeight(16) - shot = FrameLabel("没有画面", compact=True) + self._mini = [] + pics = QGridLayout() + pics.setSpacing(6) + pics.setContentsMargins(0, 0, 0, 0) + for side_index, (side, name) in enumerate((("left", "左指"), ("right", "右指"))): + for kind_index, (kind, caption) in enumerate((("shear", "切向"), ("thermal", "热像"))): + column = side_index * 2 + kind_index + shot = FrameLabel("", compact=True) + shot.set_badge(f"{name}{caption}", "") self._pic[f"{side}_{kind}"] = shot - col.addWidget(cap) - col.addWidget(shot, 1) - pics.addLayout(col) - self._mini[index].setObjectName("muted") - self._mini[index].setMaximumHeight(16) + pics.addWidget(shot, 0, column) + force = _fit_text(QLabel("—")) + force.setObjectName("muted") + force.setFixedHeight(22) + force.setAlignment(Qt.AlignmentFlag.AlignCenter) + self._mini.append(force) + pics.addWidget(force, 1, side_index * 2, 1, 2) + for column in range(4): + pics.setColumnStretch(column, 1) + pics.setRowStretch(0, 1) tac_box = QWidget() tac_box.setLayout(pics) - tac_box.setMinimumHeight(200) + tac_box.setMinimumHeight(160) tac_box.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) self._tac_box = tac_box lay.addWidget(tac_box, 1) - forces = QHBoxLayout() - for label in self._mini: - forces.addWidget(label) - lay.addLayout(forces) self._flow_labels: list[QLabel] = [] flow = QHBoxLayout() - for name in ("跟踪", "对孔", "旋转装配"): - label = _fit_text(QLabel(name)) - label.setObjectName("muted") - label.setMaximumHeight(18) + flow.setSpacing(6) + for index, name in enumerate(("跟踪", "对孔", "旋转装配"), start=1): + label = _fit_text(QLabel(f"{index} {name}")) + label.setObjectName("step") + label.setAlignment(Qt.AlignmentFlag.AlignCenter) + label.setFixedHeight(34) self._flow_labels.append(label) flow.addWidget(label, 1) lay.addLayout(flow) - stages = " ".join(f"{sid} {name}" for sid, name in catalog()["stages"]) - stage_line = _fit_text(QLabel(stages)) - stage_line.setObjectName("muted") - stage_line.setWordWrap(True) - stage_line.setMaximumHeight(32) - lay.addWidget(stage_line) - self._op_lead = _fit_text(QLabel("")) - self._op_lead.setObjectName("lead") - self._op_lead.setMaximumHeight(18) - lay.addWidget(self._op_lead) + ops = _card() + ops_lay = QVBoxLayout(ops) + ops_lay.setContentsMargins(8, 6, 8, 8) + ops_lay.setSpacing(6) + ops_lay.addWidget(_section("操作")) grid = QGridLayout() - grid.setSpacing(4) - groups = ( - ("设备", ("real_up", "gripper_up")), - ("夹爪", ("gripper_probe", "gripper_open", "gripper_close", "gripper_rotate")), - ("作业", ( - "pipeline", "stage_track", "stage_locate", "stage_screw", - "reset_home", "gauge_dry", "gauge_run", - )), + grid.setSpacing(6) + actions = ( + "real_up", "gripper_up", "gripper_probe", "gripper_open", + "gripper_close", "gripper_rotate", "pipeline", "stage_track", + "stage_locate", "stage_screw", "reset_home", "gauge_dry", + "gauge_run", ) - index = 0 - for _heading, actions in groups: - for action in actions: - grid.addWidget(self._node_button(action), index // 4, index % 4) - index += 1 - lay.addLayout(grid) + for index, action in enumerate(actions): + grid.addWidget(self._node_button(action), index // 4, index % 4) + for column in range(4): + grid.setColumnStretch(column, 1) + ops_lay.addLayout(grid) + lay.addWidget(ops) return page - def _node_button(self, action: str) -> QPushButton: + def _node_button(self, action: str, *, bar: bool = False) -> QPushButton: spec = self._spec(action) or {} - mark = _NODE_MARKS.get(action, "•") - button = QPushButton(f"{mark} {spec.get('title') or action}") - button.setObjectName("danger" if spec.get("moves") else "node") - button.setMinimumWidth(0) - button.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Fixed) + if action == "pipeline": + kind = "primary" + elif spec.get("moves"): + kind = "danger" + else: + kind = "node" + button = CommandButton(str(spec.get("title") or action), kind, action, bar=bar) button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) return button - def _toggle_depth(self) -> None: - self._depth_on = not self._depth_on - self._depth_box.setVisible(self._depth_on) - self._depth_btn.setText("◉ 收起深度" if self._depth_on else "◎ 深度") + def _set_depth(self, on: bool) -> None: + """彩色和深度共用同一排画面，点过去就换，不往下再展开一排。""" + self._depth_on = bool(on) + for button, active in ((self._color_mode, not on), (self._depth_mode, on)): + button.setProperty("active", active) + button.style().unpolish(button) + button.style().polish(button) + suffix = "depth" if on else "color" + seen: set[int] = set() + for key, label in self._cam_pic.items(): + if not key.endswith("_" + suffix) or id(label) in seen: + continue + seen.add(id(label)) + _view_key, title = self._view_key(key) + label.setText("") + label.set_badge(title or self._cam_title.get(key, ""), "等待画面") self._sync_camera_watch() + self._paint_images() + + def _set_source(self, sim: bool) -> None: + """真机和仿真共用这一排画面。仿真图像也进这里，不另开窗口。""" + self._cam_source = "sim" if sim else "real" + for button, active in ((self._real_mode, not sim), (self._sim_mode, sim)): + button.setProperty("active", active) + button.style().unpolish(button) + button.style().polish(button) + self._set_depth(self._depth_on) + + def _view_key(self, real_key: str) -> tuple[str, str]: + """这一格现在该订哪路话题，以及画面上写的名字。""" + suffix = "_depth" if real_key.endswith("_depth") else "_color" + real_id = real_key[: -len(suffix)] + role = self._slot_role.get(real_id, "") + if self._cam_source == "sim": + sim_id = self._sim_id.get(role) + if sim_id: + kind = "depth" if suffix == "_depth" else "color" + return f"{sim_id}{suffix}", self._sim_title.get(role, {}).get(kind, "") + return real_key, self._cam_title.get(real_key, "") def _page_arm(self) -> QWidget: page = QWidget() @@ -1763,9 +2078,14 @@ class StationApp(QMainWindow): """只有正在看的那一页才处理图像。别的页上相机回调直接丢掉。""" page = self._stack.currentWidget() if page is self._pages.get("main"): - keys = {key for key in self._cam_pic if key.endswith("_color")} - if self._depth_on: - keys |= {key for key in self._cam_pic if key.endswith("_depth")} + suffix = "_depth" if self._depth_on else "_color" + keys = set() + seen: set[int] = set() + for key, label in self._cam_pic.items(): + if not key.endswith(suffix) or id(label) in seen: + continue + seen.add(id(label)) + keys.add(self._view_key(key)[0]) camera_view.watch(keys, set()) elif page is self._pages.get("handeye"): name = str(self._handeye_card().get("preview_camera") or "") @@ -1944,15 +2264,9 @@ class StationApp(QMainWindow): self._sync_camera_watch() self._fill_gripper(grip) self._fill_teach() - self._op_lead.setText(f"当前臂 {self._arm}。红色按钮会动设备，点下去就执行。") def _fill_overview(self, current: dict | None) -> None: - if current: - tactile = "本臂有视触觉" if current.get("tactile") else "本臂无视触觉" - self._lead.setText( - f"{current.get('label')} · {current.get('sn') or ''} · 相机 {current.get('camera') or '—'} · " - f"夹爪 {current.get('gripper') or '—'} · {tactile}" - ) + del current live = (self._snap.get("arm_live") or {}).get(self._arm) or {} enable_word = "已使能" if live.get("enabled") else "未使能" self._arm_banner.setText(f"{enable_word} {live.get('summary') or ''}".strip()) @@ -1962,68 +2276,66 @@ class StationApp(QMainWindow): usb = list(self._snap.get("usb") or []) ports = self._snap.get("grippers") or [] usb.append({"id": "tty", "items": [{"port": name, "speed": ""} for name in ports], "expect": 1}) - for (count, flag, detail), dev in zip(self._usb_labels, usb): + for (count, flag), dev in zip(self._usb_labels, usb): items = dev.get("items") or [] if dev.get("id") == "tty": count.setText(str(len(items))) ok = len(items) > 0 else: - count.setText(f"{len(items)} / {dev.get('expect')}") + count.setText(f"{len(items)}/{dev.get('expect')}") ok = len(items) >= int(dev.get("expect") or 0) - flag.setText("齐" if ok else ("缺一" if items else "缺失")) - detail.setText(" ".join( - f"{item['port']} · {item['speed']} Mbps" if item.get("speed") else item["port"] - for item in items - ) or "未检测到") + flag.setText("齐" if ok else "缺") + flag.setObjectName("ok" if ok else "warn") + flag.style().unpolish(flag) + flag.style().polish(flag) tactile = self._snap.get("tactile") or {} for label, side, name in ((self._mini[0], "left", "左指"), (self._mini[1], "right", "右指")): row = tactile.get(side) or {} label.setText(self._force_line(row)) pipe = next((s for s in self._snap.get("services") or [] if s["id"] == "pipeline"), None) - texts = ( - "D405 看住工件，锁住悬停", - "拍照找孔，移到孔上方", - "按现场配置的深度和螺距旋进去", - ) - state = "进程在跑，具体阶段看终端日志。" if pipe and pipe.get("up") else "流程未启动。" - for label, text in zip(self._flow_labels, texts): - label.setText(f"{text} {state}") + running = bool(pipe and pipe.get("up")) + for index, (label, name) in enumerate( + zip(self._flow_labels, ("跟踪", "对孔", "旋转装配")), start=1 + ): + label.setText(f"{index} {name}") + label.setProperty("on", running) + label.style().unpolish(label) + label.style().polish(label) def _fill_arm(self, live: dict) -> None: enabled = bool(live.get("enabled")) self._arm_cards["enable"].setText("已使能" if enabled else "未使能") - mode = live.get("mode") or "—" - op_status = live.get("op_status") or "还没读到" - self._arm_cards["mode"].setText(f"mode {mode}\nop_status {op_status}") + self._arm_cards["mode"].setText(str(live.get("mode") or "—")) fault = live.get("fault") estop = live.get("estop_released") - fault_text = "有故障" if fault else ("无故障" if fault is False else "故障未知") if estop is False: - estop_text = "急停没松开" - elif estop is True: - estop_text = "急停已松开" + fault_text = "急停" + elif fault: + fault_text = "有故障" + elif fault is False: + fault_text = "正常" else: - estop_text = "急停未知" - self._arm_cards["fault"].setText(f"{fault_text}\n{estop_text}") - self._arm_cards["source"].setText(live.get("source") or "还没有") - self._arm_prompt.setText(live.get("prompt") or arm_state.REMOTE_PROMPT) - self._arm_prompt.setObjectName("warn" if live.get("prompt_hot", True) else "ok") - self._arm_prompt.style().unpolish(self._arm_prompt) - self._arm_prompt.style().polish(self._arm_prompt) - self._arm_summary.setText(live.get("summary") or "") + fault_text = "—" + self._arm_cards["fault"].setText(fault_text) + source = str(live.get("source") or "—") + if source.startswith("RDK"): + source = "RDK" + elif source == "拖拽桥": + source = "拖拽" + self._arm_cards["source"].setText(source) + hot = bool(live.get("prompt_hot", True)) + prompt = str(live.get("prompt") or "") + sentence = prompt.split("。")[0] + self._arm_prompt.setVisible(hot and bool(sentence)) + self._arm_prompt.setText(f"{sentence}。" if sentence else "") q = live.get("q") - points = live.get("points") or [] if isinstance(q, list) and len(q) >= 7: degrees = " ".join( - f"J{i + 1} {v * 180.0 / 3.141592653589793:.1f}°" for i, v in enumerate(q[:7]) + f"J{i + 1} {v * 180.0 / 3.141592653589793:.0f}°" for i, v in enumerate(q[:7]) ) else: - degrees = "关节角还没有。图上是零位，用来看外形，不是这台臂现在的姿势。" - tip = "" - if points: - x, y, z = points[-1] - tip = f"\n法兰约 x {x * 1000:.0f} y {y * 1000:.0f} z {z * 1000:.0f} mm" - self._arm_joints.setText(degrees + tip) + degrees = "零位" + self._arm_joints.setText(degrees) self._arm_sketch.set_pose( q if isinstance(q, list) else None, bool(live.get("pose_live")), @@ -2034,15 +2346,13 @@ class StationApp(QMainWindow): def num(key: str) -> str: value = row.get(key) return f"{value:.1f}" if isinstance(value, (int, float)) else "—" - return f"法向 {num('fnormal')} 切向 X {num('fshearx')} 切向 Y {num('fsheary')} {num('fps')} fps" + return f"法向 {num('fnormal')} 切向 {num('fshearx')} / {num('fsheary')}" def _fill_tactile(self) -> None: view = self._snap.get("view") or {} - self._view_note.setText( - view.get("error") - or view.get("note") - or ("成像已开，等第一帧。" if view.get("on") else "成像没开。点开始成像后这里出切向力和热成像。") - ) + err = str(view.get("error") or "") + self._view_note.setVisible(bool(err)) + self._view_note.setText(err) history = self._snap.get("history") or {} for side, chart in self._charts.items(): chart.set_samples(history.get(side)) @@ -2061,10 +2371,25 @@ class StationApp(QMainWindow): if blob: label.show_jpeg(blob) shots = camera_view.previews() + meta = camera_view.preview_meta() + suffix = "_depth" if self._depth_on else "_color" + seen: set[int] = set() for key, label in self._cam_pic.items(): - if key.endswith("_depth") and not self._depth_on: + if not key.endswith(suffix) or id(label) in seen: continue - item = shots.get(key) + seen.add(id(label)) + view_key, title = self._view_key(key) + info = meta.get(view_key) + if not title: + title = self._cam_title.get(key, "") + if info and info[2] >= 1.0: + detail = f"{info[0]}×{info[1]} {info[2]:.0f} 帧/秒" + elif info and info[0] > 0: + detail = f"{info[0]}×{info[1]}" + else: + detail = "等待画面" + label.set_badge(title, detail) + item = shots.get(view_key) if not item: continue blob, width, height = item @@ -2133,61 +2458,20 @@ class StationApp(QMainWindow): def _fill_camera(self) -> None: view = camera_view.state() - usb = {item["id"]: item for item in self._snap.get("usb") or []} - bits = [] - for key, name in (("d405", "腕部"), ("d435", "手外")): - dev = usb.get(key) or {"items": []} - bits.append(f"{name} {len(dev.get('items') or [])} 台") - err = view.get("error") or "" - ready = "、".join(key for key, ok in (view.get("frames") or {}).items() if ok) or "还没有帧" - text = f"{' '.join(bits)}。画面：{ready}。{err}" - if text != self._cam_note.text(): - self._cam_note.setText(text) + err = str(view.get("error") or "") + self._cam_note.setVisible(bool(err)) + if err != self._cam_note.text(): + self._cam_note.setText(err) def _fill_gripper(self, live: dict) -> None: - ports = " ".join(self._snap.get("grippers") or []) or "没有 ttyUSB" current = next((row for row in self._catalog if row["id"] == self._arm), None) rotary = bool(current and current.get("rotary")) self._rotary.setVisible(rotary) - kind = "旋转爪" if rotary else "平行爪" - self._grip_lead.setText( - f"{kind} {ports}。滑条是要下发的命令，0 合拢、1000 张开。读状态不初始化、不张合。" - ) connected = bool(live.get("connected")) - self._grip_summary.setText(live.get("summary") or "还没读到夹爪") + self._grip_summary.setText(live.get("summary") or "没连上") self._grip_summary.setObjectName("ok" if connected else "warn") self._grip_summary.style().unpolish(self._grip_summary) self._grip_summary.style().polish(self._grip_summary) - link = "已连接" if connected else "未连接" - if connected: - link += " · 已初始化" if live.get("initialized") else " · 未初始化" - if live.get("source"): - link += f" · {live['source']}" - err = live.get("error") or "" - if err and not connected: - link += f" · {err}" - self._grip_cards["link"].setText(link) - opening = live.get("opening_mm") - permille = live.get("permille") - if opening is None or permille is None: - self._grip_cards["opening"].setText("—") - else: - self._grip_cards["opening"].setText( - f"{opening:.1f}/{float(live.get('stroke_mm') or 0):.0f} mm {permille:.0f}‰" - ) - force = live.get("force") - speed = live.get("speed") - force_text = f"{force:.0f}%" if isinstance(force, (int, float)) else "—" - speed_text = f"{speed:.0f}%" if isinstance(speed, (int, float)) else "—" - self._grip_cards["hold"].setText( - f"{live.get('grip_text') or '—'} {force_text}/{speed_text}" - ) - if live.get("rotary"): - angle = live.get("rotation_deg") - angle_text = f"{angle:.1f}°" if isinstance(angle, (int, float)) else "—" - self._grip_cards["spin"].setText(f"{angle_text} {live.get('rotation_text') or '—'}") - else: - self._grip_cards["spin"].setText("平行爪") self._grip_sketch.set_pose( live.get("permille"), live.get("rotation_deg"), @@ -2195,9 +2479,6 @@ class StationApp(QMainWindow): bool(live.get("pose_live")), live.get("stroke_mm"), ) - draw = getattr(self, "_grip_draw", None) - if draw is not None: - draw.setText(self._grip_sketch._note) def _fill_teach(self) -> None: listed = teach.list_points(self._arm) @@ -2242,34 +2523,64 @@ def main() -> None: _STYLE = """ -QMainWindow, QWidget { background: #070708; color: #f3f4f6; } -QWidget#side, QWidget#head { background: #0b0c0e; } -QWidget#side { border-right: 1px solid #2a2e36; } -QWidget#head { border-bottom: 1px solid #2a2e36; } -QLabel#sideNote, QLabel#lead, QLabel#muted, QLabel#chips, QLabel#clock { color: #8b929d; } -QLabel#warn { color: #f0c674; } -QLabel#ok { color: #8fce8f; } -QLabel#armLine { font-size: 16px; } -QLabel#cardTitle { color: #9aa3b2; letter-spacing: 1px; } -QLabel#stat { font-size: 28px; } -QLabel#shot { background: #0c0e12; border: 1px solid #243044; border-radius: 8px; color: #8b929d; } -QFrame#card { background: #101216; border: 1px solid #2a2e36; border-radius: 12px; } +QMainWindow, QWidget { background: #0c0f14; color: #e7ebf2; } +QWidget#side { background: #10141b; border-right: 1px solid #2a3240; } +QWidget#head { background: #10141b; border-bottom: 1px solid #2a3240; } +QLabel#brand { color: #8b95a7; font-size: 11px; letter-spacing: 1px; } +QLabel#sideNote, QLabel#lead, QLabel#muted, QLabel#chips, QLabel#clock { color: #8b95a7; } +QLabel#clock { padding: 0 8px; } +QLabel#warn { color: #e2b657; } +QLabel#ok { color: #7dcea0; } +QLabel#armLine { font-size: 16px; font-weight: 600; color: #f4f7fb; } +QLabel#count { color: #f4f7fb; font-weight: 600; } +QLabel#cardTitle { color: #8b95a7; font-size: 11px; } +QLabel#section { color: #d5dbe6; font-weight: 600; } +QFrame#sectionMark { background: #5b9bff; border: none; border-radius: 1px; } +QLabel#step { + background: #151a22; color: #9aa3b2; border: 1px solid #2a3240; + border-radius: 8px; padding: 2px 8px; +} +QLabel#step[on="true"] { background: #15263c; color: #d6e6ff; border-color: #2f5f99; } +QLabel#shot { background: #090c11; border: 1px solid #243044; border-radius: 10px; color: #8b95a7; } +QFrame#card { background: #161b24; border: 1px solid #2c3544; border-radius: 12px; } QPushButton { - background: #171a20; color: #f3f4f6; border: 1px solid #2a2e36; - border-radius: 8px; padding: 8px 12px; text-align: left; + background: #1a1f27; color: #e7ebf2; border: 1px solid #313844; + border-radius: 8px; padding: 0px 12px; font-size: 13px; text-align: left; } -QPushButton#nav, QPushButton#node { text-align: left; background: transparent; border: 1px solid transparent; } -QPushButton#node { background: #171a20; border-color: #2a2e36; padding: 6px 8px; } -QPushButton#nav[active="true"], QPushButton[active="true"] { background: #12315d; border-color: #1a4580; } -QPushButton#danger { background: #6e2a2a; border-color: #8a3a3a; } +QWidget#switch { background: #12171f; border: 1px solid #2c3544; border-radius: 8px; } +QPushButton:hover { background: #222833; border-color: #4a5568; } +QPushButton#nav { + text-align: left; background: transparent; border: none; border-radius: 8px; + padding: 0px 8px; color: #b7c0cc; font-size: 13px; +} +QPushButton#nav:hover { background: #1a2030; } +QPushButton#nav[active="true"] { + background: #182438; color: #f4f7fb; border-left: 3px solid #5b9bff; +} +QPushButton#node, QPushButton#danger, QPushButton#primary, QPushButton#action { text-align: center; padding: 0; } +QPushButton#node { background: #1c222c; border-color: #3a4558; } +QPushButton#seg { + text-align: center; padding: 0 12px; border: none; border-radius: 6px; + background: transparent; min-width: 0; min-height: 0; +} +QPushButton#seg[active="true"] { background: #1d4f91; color: #f4f7fb; } +QPushButton#primary { background: #1d4f91; border-color: #3d7ed4; color: #f4f7fb; font-weight: 600; } +QPushButton#primary:hover { background: #2460ad; } +QPushButton#danger { background: #241416; border: 1px solid #6e3a3a; color: #f0d0d0; } +QPushButton#danger:hover { background: #3a1c20; border-color: #a05050; } QPlainTextEdit, QLineEdit, QListWidget, QDoubleSpinBox { - background: #0c0e12; color: #f3f4f6; border: 1px solid #2a2e36; border-radius: 8px; padding: 4px 8px; + background: #10141a; color: #e7ebf2; border: 1px solid #2a3240; border-radius: 8px; padding: 4px 8px; } -QSlider::groove:horizontal { height: 6px; background: #2a2e36; border-radius: 3px; } +QPlainTextEdit#log { + background: #10141a; color: #b7c0cc; border: none; border-top: 1px solid #2a3240; border-radius: 0; +} +QDoubleSpinBox:focus, QLineEdit:focus { border-color: #3d7ed4; } +QSlider::groove:horizontal { height: 4px; background: #2a3240; border-radius: 2px; } QSlider::handle:horizontal { - width: 16px; margin: -6px 0; background: #4c86d4; border-radius: 8px; + width: 14px; margin: -6px 0; background: #5b9bff; border-radius: 7px; } -QScrollArea { background: transparent; } +QScrollArea { background: transparent; border: none; } +QStatusBar { background: #10141b; color: #8b95a7; } """ diff --git a/src/arm_application/arm_application/operator_camera_view.py b/src/arm_application/arm_application/operator_camera_view.py index a602ee0..217a4ed 100644 --- a/src/arm_application/arm_application/operator_camera_view.py +++ b/src/arm_application/arm_application/operator_camera_view.py @@ -2,9 +2,11 @@ 只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。 -回调里先丢掉当前页没在看的流。彩色和深度都缩成宽不超过 480 的 RGB， -界面直接贴这块小图。不再每帧把整幅图拷进 Python，也不在界面线程里解 JPEG。 -手眼页单独要一张更宽的 JPEG，只在那一页、那一路彩色上编码。 +回调里先丢掉当前页没在看的流。彩色按相机原分辨率收进来，不按时钟抽帧， +也不在收图的线程里做颜色转换。另一条线只画最新的一张，所以帧率跟相机走， +848 和 1280 都不会为了帧率被缩成 480。深度是伪彩色，仍然抽稀， +而且没打开深度时不订阅，避免大图挤占彩色。 +手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。 """ from __future__ import annotations @@ -15,12 +17,15 @@ import time import numpy as np -# 主界面同时看几路彩色。480 宽够看清，界面缩放的代价也小。 -_PREVIEW_WIDTH = 480 -# 手眼要在这张图上找棋盘，比预览宽一档，但仍然不把 1280 原图编码。 -_JPEG_WIDTH = 960 -# 主界面要看清运动和推理框，10Hz 太顿。0.05s 约 20 帧。 -_INTERVAL = 0.05 +# 彩色上限。D405 是 848×480，D435 是 1280×720，都不超过它，所以不会被缩小。 +_COLOR_WIDTH = 1280 +# 深度上色不是要看的画质。全幅每帧上色会把刷新占满，所以这一路仍抽稀。 +_DEPTH_WIDTH = 480 +# 手眼只在正在看的那一页编码，宽度跟彩色预览一样，不另缩一档。 +_JPEG_WIDTH = 1280 +# 只留给还在用 ``accept_frame`` 的调用。彩色实况不再用它抽帧： +# 相机大约 30 帧，再按这个间隔丢一次，画面会掉到十几帧。 +_INTERVAL = 1.0 / 30.0 _DET_HOLD_S = 1.5 _LOCK = threading.Lock() @@ -34,6 +39,16 @@ _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} @@ -63,13 +78,60 @@ def preview_targets(site: dict) -> list[dict]: return found +def sim_preview_targets(site: dict) -> list[dict]: + """仿真腕部和仿真手外。画面进主界面，不另开 Gazebo 的相机窗。""" + from arm_application.site_io import overhead_camera + + found: list[dict] = [] + robots = ((site or {}).get("robot") or {}).get("robots") or [] + cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {} + for robot in robots: + if not isinstance(robot, dict): + continue + if str(robot.get("sn") or "").strip().lower() != "sim": + continue + cam = robot.get("camera") if isinstance(robot.get("camera"), dict) else {} + name = str((cam or {}).get("name") or "").strip().strip("/") + if not name: + continue + cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {} + tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405" + found.append({"id": name, "label": f"仿真左腕 {tag}", "role": "wrist", "source": "sim"}) + break + overhead, _others = overhead_camera(site or {}, {"arm": "sim"}) + oname = str((overhead or {}).get("name") or "").strip().strip("/") + if oname.startswith("sim"): + tag = str((overhead or {}).get("model") or "D435").strip().upper() or "D435" + found.append({"id": oname, "label": f"仿真手外 {tag}", "role": "overhead", "source": "sim"}) + return found + + +def stream_targets(site: dict | None = None) -> list[dict]: + """真机和仿真都订上。主界面切换看哪一组，没在看的那组回调里直接丢掉。""" + if site is None: + from arm_application.operator_console import _site + + site = _site() + real = preview_targets(site) + for item in real: + item["source"] = "real" + seen = {item["id"] for item in real} + return real + [item for item in sim_preview_targets(site) if item["id"] not in seen] + + def targets() -> list[dict]: - """当前站点的主界面相机。规则在 ``preview_targets``。""" + """当前站点的主界面真机相机。规则在 ``preview_targets``。""" from arm_application.operator_console import _site return preview_targets(_site()) +def sim_targets() -> list[dict]: + from arm_application.operator_console import _site + + return sim_preview_targets(_site()) + + def state() -> dict: with _LOCK: return { @@ -91,10 +153,32 @@ def previews() -> dict[str, tuple[bytes, int, int]]: return dict(_PREVIEW) +def preview_meta() -> dict[str, tuple[int, int, float]]: + """每路相机原图的宽、高，以及最近一秒的帧率。还没来帧就是空。""" + with _LOCK: + return dict(_META) + + +def _mark_rate(key: str, width: int, height: int) -> None: + """记下这一帧的原图尺寸，并用最近一秒的到达间隔算帧率。""" + now = time.monotonic() + with _LOCK: + stamps = _STAMPS.setdefault(key, []) + stamps.append(now) + cut = now - 1.0 + while len(stamps) > 1 and stamps[0] < cut: + stamps.pop(0) + if len(stamps) >= 2 and stamps[-1] > stamps[0]: + fps = (len(stamps) - 1) / (stamps[-1] - stamps[0]) + else: + fps = 0.0 + _META[key] = (int(width), int(height), float(fps)) + + def watch(preview: set[str], jpeg: set[str] | None = None) -> None: """当前页要哪几路。没点名的流在回调里直接丢掉，不拷图像。 - ``preview`` 走 RGB 小图，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。 + ``preview`` 走原分辨率 RGB，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。 """ global _PREVIEW_WANT, _JPEG_WANT jpeg = set(jpeg or ()) @@ -114,8 +198,112 @@ def wants(key: str) -> tuple[bool, bool]: 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。""" + """这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。 + + 彩色实况不再走这里。固定间隔会把相机自己的帧率再砍掉一截。 + """ now = time.monotonic() with _LOCK: if key not in _PREVIEW_WANT and key not in _JPEG_WANT: @@ -253,7 +441,7 @@ def _stamp_role(role: str) -> None: _STATE["stamp"] = time.time() -def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _PREVIEW_WIDTH) -> np.ndarray: +def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _COLOR_WIDTH) -> np.ndarray: """缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。""" import cv2 @@ -271,7 +459,7 @@ def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _PREVIEW_WIDTH) -> return view -def depth_preview(depth: np.ndarray, limit: int = _PREVIEW_WIDTH) -> np.ndarray: +def depth_preview(depth: np.ndarray, limit: int = _DEPTH_WIDTH) -> np.ndarray: """uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。""" import cv2 @@ -374,6 +562,7 @@ def _loop(cams: list[dict]) -> None: self._det("overhead"), det_qos, ) + self._depth_subs: dict[str, object] = {} for cam in cams: name = cam["id"] role = str(cam.get("role") or "wrist") @@ -383,12 +572,26 @@ def _loop(cams: list[dict]) -> None: self._color(name, role), qos_profile_sensor_data, ) - self.create_subscription( - Image, - f"/{name}/aligned_depth_to_color/image_raw", - self._depth(name), - qos_profile_sensor_data, - ) + + def sync_depth(self) -> None: + """深度没打开就不订。订上之后每一帧都会整幅拷进进程，彩色帧率会被拖下去。""" + with _LOCK: + want = { + cam["id"] + for cam in cams + if f"{cam['id']}_depth" in _PREVIEW_WANT + } + for cam in cams: + name = cam["id"] + if name in want and name not in self._depth_subs: + self._depth_subs[name] = self.create_subscription( + Image, + f"/{name}/aligned_depth_to_color/image_raw", + self._depth(name), + qos_profile_sensor_data, + ) + elif name not in want and name in self._depth_subs: + self.destroy_subscription(self._depth_subs.pop(name)) def _det(self, role: str): def _cb(msg: Float32MultiArray) -> None: @@ -399,24 +602,12 @@ def _loop(cams: list[dict]) -> None: key = f"{name}_color" def _cb(msg: Image) -> None: - # 先看这路有没有人要。30Hz 的原图不再整幅拷进 Python。 + # 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。 try: - if not accept_frame(key): - return view = _color_view(msg) if view is None: return - preview, jpeg = wants(key) - limit = _JPEG_WIDTH if jpeg else _PREVIEW_WIDTH - rgb = rgb_preview(view, bgr=(msg.encoding == "bgr8"), limit=limit) - shown = rgb - if preview and int(rgb.shape[1]) > _PREVIEW_WIDTH: - shown = rgb_preview(rgb, bgr=False, limit=_PREVIEW_WIDTH) - if preview: - src_h, src_w = view.shape[:2] - _remember_clean(key, shown, role, int(src_w), int(src_h)) - shown = _overlay_detection(shown, role, int(src_w), int(src_h)) - _store(key, rgb, preview=shown if preview else None, jpeg=jpeg) + offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role) except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停 with _LOCK: _STATE["error"] = str(exc) @@ -427,14 +618,10 @@ def _loop(cams: list[dict]) -> None: def _cb(msg: Image) -> None: try: - if not accept_frame(key): - return view = _depth_view(msg) if view is None: return - preview, _jpeg_wanted = wants(key) - colored = depth_preview(view) - _store(key, colored, preview=colored if preview else None, jpeg=False) + offer_depth(key, view) except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停 with _LOCK: _STATE["error"] = str(exc) @@ -444,7 +631,8 @@ def _loop(cams: list[dict]) -> None: node = _Node() try: while not _STOP.is_set(): - rclpy.spin_once(node, timeout_sec=0.05) + node.sync_depth() + rclpy.spin_once(node, timeout_sec=0.01) except Exception as exc: with _LOCK: _STATE["error"] = str(exc) @@ -457,8 +645,8 @@ def _loop(cams: list[dict]) -> None: def start() -> str: - global _THREAD - cams = targets() + global _THREAD, _PAINT_THREAD + cams = stream_targets() if not cams: with _LOCK: _STATE["error"] = "site 里没有可预览的真机相机" @@ -470,14 +658,18 @@ def start() -> str: _STATE["on"] = True _STATE["error"] = "" _STOP.clear() + _WAKE.clear() _THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view") _THREAD.start() + _PAINT_THREAD = threading.Thread(target=_paint_loop, daemon=True, name="cam-paint") + _PAINT_THREAD.start() names = "、".join(cam["label"] for cam in cams) - return f"预览订了 {names} 的彩色和深度，不含左右红外。" + return f"预览订了 {names} 的彩色原图，深度点开才订，不含左右红外。" def stop() -> None: _STOP.set() - thread = _THREAD - if thread is not None: - thread.join(timeout=1.5) + _WAKE.set() + for thread in (_THREAD, _PAINT_THREAD): + if thread is not None: + thread.join(timeout=1.5) diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index fe4f9f8..fab29f0 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 -"""三阶段串联：视觉跟踪锁定 → 孔定位 → 搜孔并轻轻按压（默认不拧入）。 +"""跟踪锁定 → 孔定位。搜孔轻压已取消，本节点不再下压搜孔。 流畅性（相对 bash 杀进程再起节点）： - 相机 / 触觉 / 手眼 TF 全程只起一次 - 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂 - - 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完立刻下压搜孔 - - 找到孔位后力闭环轻压保持；拧入是后续阶段（--depth 才插入） + - 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完就停，不再下压搜孔 + - 旋入不在这个节点里 """ from __future__ import annotations @@ -125,7 +125,7 @@ class AssemblyPipeline(Stage3Search): super().__init__(node_name=node_name) self.declare_parameter("skip_stage1", False) self.declare_parameter("skip_stage2", False) - self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth + self.declare_parameter("skip_stage3", True) # 搜孔轻压已取消，写成 false 也不再下压 self.declare_parameter("d435_move_only", False) self.declare_parameter("init_z_m", -1.0) # >0：启动后先到这个法兰高度 self.declare_parameter("stage1_timeout_sec", 150.0) @@ -333,7 +333,9 @@ class AssemblyPipeline(Stage3Search): def run(self) -> int: skip1 = bool(self.get_parameter("skip_stage1").value) skip2 = bool(self.get_parameter("skip_stage2").value) - skip3 = bool(self.get_parameter("skip_stage3").value) + if not bool(self.get_parameter("skip_stage3").value): + self.get_logger().info("搜孔轻压已取消，忽略要求重新打开它的参数") + skip3 = True if self.d435_move_only: skip2 = True skip3 = True @@ -365,11 +367,7 @@ class AssemblyPipeline(Stage3Search): return code self.get_logger().info( f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm" - + ( - " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）" - if skip3 - else " — 立刻下压搜孔并轻压（不拧入）" - ) + " — 搜孔已取消，停在这里。旋入请另跑。" ) else: self._state("skip_stage2") @@ -383,7 +381,7 @@ class AssemblyPipeline(Stage3Search): ) else: self.get_logger().info( - "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；" + "搜孔已取消 —— 臂停在当前高度。" "15s 后本进程退出，臂保持不动" ) if final: diff --git a/src/arm_application/arm_application/realsense_color_pub.py b/src/arm_application/arm_application/realsense_color_pub.py index 1d8464a..c95b46a 100644 --- a/src/arm_application/arm_application/realsense_color_pub.py +++ b/src/arm_application/arm_application/realsense_color_pub.py @@ -176,6 +176,10 @@ class RealsenseColorPub(TolerantNode): self._overlay: Optional[list] = None self._overlay_stamp = 0.0 self._capture_started = False + self._cap_lock = threading.Lock() + self._cap_event = threading.Event() + self._pending = None + self._last_frame_mono = 0.0 self._usb_desc = '' self._last_ok = None self._site_yaml = _site_yaml_path() @@ -309,23 +313,26 @@ class RealsenseColorPub(TolerantNode): self._info_pub.publish(info) if depth_u16 is not None: - dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1]) - dmsg = Image() - dmsg.header.stamp = stamp - dmsg.header.frame_id = self._frame_id - dmsg.height = dh - dmsg.width = dw - dmsg.encoding = '16UC1' - dmsg.is_bigendian = 0 - dmsg.step = dw * 2 - dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes()) - self._depth_pub.publish(dmsg) - if not self._logged_depth: - self._logged_depth = True - self.get_logger().info( - f'首个对齐深度 {dw}x{dh} → ' - f'/{self._name}/aligned_depth_to_color/image_raw' - ) + self._publish_depth(depth_u16, stamp) + + def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None: + dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1]) + dmsg = Image() + dmsg.header.stamp = stamp or self.get_clock().now().to_msg() + dmsg.header.frame_id = self._frame_id + dmsg.height = dh + dmsg.width = dw + dmsg.encoding = '16UC1' + dmsg.is_bigendian = 0 + dmsg.step = dw * 2 + dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes()) + self._depth_pub.publish(dmsg) + if not self._logged_depth: + self._logged_depth = True + self.get_logger().info( + f'首个对齐深度 {dw}x{dh} → ' + f'/{self._name}/aligned_depth_to_color/image_raw' + ) def _start_pipeline(self, reconnect: bool = False) -> None: try: @@ -457,7 +464,9 @@ class RealsenseColorPub(TolerantNode): trial.enable_stream( rs.stream.infrared, 2, w, h, rs.format.y8, fps ) - profile = pipe.start(trial) + # 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套， + # 场景一动红外对不齐，彩色帧就在这里被丢掉。 + profile = pipe.start(trial, self._on_sdk_frame) have_depth = with_depth have_infra = infra_mode != 'none' self._infra_mode = infra_mode @@ -508,6 +517,13 @@ class RealsenseColorPub(TolerantNode): self._pipe = pipe self._profile = profile + try: + sensor = profile.get_device().first_color_sensor() + if sensor.supports(rs.option.frames_queue_size): + # 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。 + sensor.set_option(rs.option.frames_queue_size, 1) + except Exception: # noqa: BLE001 + pass self._have_depth = have_depth self._have_infra = have_infra self._logged_depth = False @@ -609,7 +625,7 @@ class RealsenseColorPub(TolerantNode): if not getattr(self, '_capture_started', False): self._capture_started = True - self._thread = threading.Thread(target=self._capture_loop, daemon=True) + self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish") self._thread.start() def _enable_color_ae_fast(self, profile, rs) -> None: @@ -702,18 +718,60 @@ class RealsenseColorPub(TolerantNode): ) return False - def _capture_loop(self) -> None: + def _on_sdk_frame(self, frame) -> None: + """SDK 回调。只拷走彩色并握住这一组帧，立刻返回。 + + 对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取， + 动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。 + """ + try: + frames = frame.as_frameset() + color = frames.get_color_frame() + if not color: + return + bgr = np.array(np.asanyarray(color.get_data()), copy=True) + frames.keep() + with self._cap_lock: + self._pending = (bgr, frames) + self._last_frame_mono = time.monotonic() + self._cap_event.set() + except Exception as exc: # noqa: BLE001 + try: + self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0) + except Exception: # noqa: BLE001 + pass + + def _publish_loop(self) -> None: got = False - misses = 0 while self._running and rclpy.ok(): if self._pipe is None: if not self._reconnect(): time.sleep(2.0) continue + if not self._cap_event.wait(0.2): + silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0 + if silent: + usb = getattr(self, '_usb_desc', '?') + self.get_logger().error( + f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— ' + '请给 USB 线缆留点余量，别让机械臂把它拽掉。' + ) + self._last_frame_mono = 0.0 + got = False + self._reconnect() + continue + self._cap_event.clear() + with self._cap_lock: + item = self._pending + self._pending = None + if item is None: + continue + bgr, frames = item try: - t_wait0 = time.monotonic() - frames = self._pipe.wait_for_frames(timeout_ms=2000) - self._t_wait = time.monotonic() - t_wait0 + t_pub = time.monotonic() + # 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。 + self._publish_frame(bgr, None) + self._t_pub = time.monotonic() - t_pub ir_l = ( frames.get_infrared_frame(1) if self._have_infra and self._infra_mode in ('both', 'left') @@ -724,44 +782,45 @@ class RealsenseColorPub(TolerantNode): if self._have_infra and self._infra_mode in ('both', 'right') else None ) + aligned = frames if self._align is not None: t_al = time.monotonic() - frames = self._align.process(frames) + aligned = self._align.process(frames) self._t_align = time.monotonic() - t_al + depth = aligned.get_depth_frame() color = frames.get_color_frame() except Exception as exc: # noqa: BLE001 - misses += 1 text = str(exc) dropped = ( 'disconnect' in text.lower() or 'ioctl' in text.lower() or 'errno=5' in text.lower() ) - if dropped or misses >= 4: + if dropped: usb = getattr(self, '_usb_desc', '?') self.get_logger().error( f'相机数据流中断（USB={usb}）：{text[:120]}。' '正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。' ) got = False - misses = 0 + self._last_frame_mono = 0.0 self._reconnect() - continue - if misses == 1: - self.get_logger().warn(f'wait_for_frames：{exc}') + else: + self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0) continue - if not color: - misses += 1 - continue - misses = 0 - bgr = np.asanyarray(color.get_data()) - depth = frames.get_depth_frame() depth_u16 = np.asanyarray(depth.get_data()) if depth else None if depth_u16 is not None and self._depth_to_mm != 1.0: # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位） depth_u16 = np.clip( depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535 ).astype(np.uint16) + if depth_u16 is not None: + try: + self._publish_depth(depth_u16) + except Exception as exc: # noqa: BLE001 + if not self._running: + break + self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0) self._frame_i += 1 self._fps_meter(color, depth, depth_u16) # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次 @@ -794,7 +853,6 @@ class RealsenseColorPub(TolerantNode): ) try: t_pub = time.monotonic() - self._publish_frame(bgr, depth_u16) if self._ir1_pub is not None and ir_l is not None: self._publish_ir(ir_l, self._ir1_pub) if self._ir2_pub is not None and ir_r is not None: diff --git a/src/arm_application/launch/pipeline.launch.py b/src/arm_application/launch/pipeline.launch.py index ea18aff..fee00c6 100644 --- a/src/arm_application/launch/pipeline.launch.py +++ b/src/arm_application/launch/pipeline.launch.py @@ -159,7 +159,7 @@ def generate_launch_description() -> LaunchDescription: [ DeclareLaunchArgument("skip_stage1", default_value="false"), DeclareLaunchArgument("skip_stage2", default_value="false"), - DeclareLaunchArgument("skip_stage3", default_value="false"), + DeclareLaunchArgument("skip_stage3", default_value="true"), DeclareLaunchArgument("target_hole", default_value=""), DeclareLaunchArgument("insert_depth_mm", default_value=""), DeclareLaunchArgument("screw_pitch_mm", default_value=""), diff --git a/src/arm_application/test/test_camera_preview.py b/src/arm_application/test/test_camera_preview.py index b8c8d7b..54379ac 100644 --- a/src/arm_application/test/test_camera_preview.py +++ b/src/arm_application/test/test_camera_preview.py @@ -51,8 +51,11 @@ def test_color_preview_is_rgb_and_not_wider_than_the_widget_budget() -> None: wide = np.zeros((720, 1280, 3), np.uint8) wide[:, :, 0] = 10 fitted = camera_view.rgb_preview(wide, bgr=True) - assert fitted.shape[1] == 480 - assert fitted.shape[2] == 3 + assert fitted.shape == (720, 1280, 3) + wrist = np.zeros((480, 848, 3), np.uint8) + assert camera_view.rgb_preview(wrist, bgr=False).shape == (480, 848, 3) + assert camera_view._COLOR_WIDTH >= 1280 + assert camera_view._INTERVAL <= 1.0 / 30.0 + 1e-9 with pytest.raises(ValueError, match="彩色预览"): camera_view.rgb_preview(np.zeros((8, 8), np.uint8), bgr=True) @@ -110,7 +113,7 @@ def test_frame_label_paints_rgb_without_set_pixmap() -> None: def test_preview_interval_is_fast_enough_for_the_main_view() -> None: """主界面要看运动和推理框。退回 0.10s 就是大约 10 帧，这里会红。""" - assert camera_view._INTERVAL <= 0.05 + assert camera_view._INTERVAL <= 1.0 / 30.0 + 1e-9 def test_detection_box_scales_onto_the_preview_and_empty_clears() -> None: @@ -157,6 +160,76 @@ def test_detection_is_drawn_on_the_camera_frame() -> None: camera_view._PREVIEW.pop("cam_color", None) +def test_color_burst_keeps_native_resolution() -> None: + """两帧紧挨着来也不能抽掉，预览必须仍是相机原分辨率。""" + camera_view._RAW.clear() + camera_view._PREVIEW.clear() + camera_view._META.clear() + camera_view._STAMPS.clear() + camera_view._PAINTED.clear() + try: + camera_view.watch({"cam_color"}, set()) + image = np.zeros((720, 1280, 3), np.uint8) + camera_view.offer_color("cam_color", image, bgr=True, role="wrist") + camera_view.offer_color("cam_color", image, bgr=True, role="wrist") + camera_view.publish_latest() + blob, width, height = camera_view.previews()["cam_color"] + assert (height, width) == (720, 1280) + assert len(blob) == 720 * 1280 * 3 + assert camera_view.preview_meta()["cam_color"][:2] == (1280, 720) + finally: + camera_view.watch(set(), set()) + camera_view._RAW.clear() + camera_view._PREVIEW.clear() + camera_view._META.clear() + camera_view._STAMPS.clear() + camera_view._PAINTED.clear() + + +def test_rate_uses_the_last_second_and_keeps_the_source_size() -> None: + camera_view._META.clear() + camera_view._STAMPS.clear() + try: + camera_view._mark_rate("cam_color", 1280, 720) + time.sleep(0.05) + camera_view._mark_rate("cam_color", 1280, 720) + width, height, fps = camera_view.preview_meta()["cam_color"] + assert (width, height) == (1280, 720) + assert fps > 1.0 + finally: + camera_view._META.clear() + camera_view._STAMPS.clear() + + +def test_sim_cameras_are_a_separate_pair_for_the_same_view() -> None: + """仿真腕部和手外要能在主界面里看，但不能把真机那两路挤掉。""" + site = { + "robot": { + "robots": [ + {"sn": "Rizon4-063989", "arm": "left", "camera": {"name": "d405_left"}}, + {"sn": "sim", "arm": "sim", "camera": {"name": "sim_d405"}}, + ] + }, + "cameras": { + "d405_left": {"name": "d405_left", "model": "d405", "role": "left_wrist"}, + "sim_d405": {"name": "sim_d405", "model": "d405", "role": "sim_wrist", "arm": "sim"}, + "d435": {"name": "d435", "model": "d435", "role": "overhead_eye_to_hand"}, + "sim_d435": { + "name": "sim_d435", + "model": "d435", + "role": "overhead_eye_to_hand", + "arm": "sim", + }, + }, + } + real = camera_view.preview_targets(site) + sim = camera_view.sim_preview_targets(site) + both = camera_view.stream_targets(site) + assert [cam["id"] for cam in real] == ["d405_left", "d435"] + assert [cam["id"] for cam in sim] == ["sim_d405", "sim_d435"] + assert [cam["id"] for cam in both] == ["d405_left", "d435", "sim_d405", "sim_d435"] + + def test_main_preview_is_left_wrist_and_shared_overhead_only() -> None: """右臂腕部和仿真手外不进主界面。左腕和共用 D435 才进。""" site = { diff --git a/src/arm_application/test/test_dh_pgea_latch.py b/src/arm_application/test/test_dh_pgea_latch.py index 755c94c..bd13fef 100644 --- a/src/arm_application/test/test_dh_pgea_latch.py +++ b/src/arm_application/test/test_dh_pgea_latch.py @@ -14,6 +14,8 @@ from __future__ import annotations from pathlib import Path from arm_application.dh_pgea import ( + ctrl_values, + open_failure_text, REG_POSITION, S_GRIP_ARRIVED, S_GRIP_CAUGHT, @@ -94,6 +96,62 @@ def test_force_init_does_not_return_on_the_stale_finished_bit(): assert wrote["n"] == 1 +def test_node_owned_port_uses_ctrl_topic_not_a_second_open() -> None: + assert ctrl_values("open", force=30, speed=40) == (0.0, 1000.0, 30.0, 40.0) + assert ctrl_values("close", force=20, speed=10) == (0.0, 0.0, 20.0, 10.0) + assert ctrl_values("pos", "250", force=33, speed=25) == (0.0, 250.0, 33.0, 25.0) + assert ctrl_values("init") == (1.0, 0.0, 50.0, 50.0) + assert ctrl_values("status") is None + try: + ctrl_values("pos", "1200") + except ValueError as exc: + assert "0–1000" in str(exc) + else: + raise AssertionError("超范围位置应当失败") + + +def test_exclusive_lock_is_not_called_a_power_failure(): + exc = OSError(11, "Could not exclusively lock port /dev/ttyUSB0: Resource temporarily unavailable") + text = open_failure_text("/dev/ttyUSB0", exc) + assert "占用" in text + assert "24 V" not in text + + +def test_a_real_open_failure_still_names_power_and_wiring(): + exc = FileNotFoundError(2, "No such file or directory") + text = open_failure_text("/dev/ttyUSB9", exc) + assert "24 V" in text + assert "RS485" in text + + +def test_parallel_cli_stops_before_start_when_the_port_is_missing(tmp_path): + """缺串口必须在拉起驱动之前失败，不能去开真口，也不能写回零。""" + import os + import subprocess + + root = Path(__file__).resolve().parents[3] + site = (root / "config" / "site.yaml").read_text(encoding="utf-8") + cfg = tmp_path / "site.yaml" + cfg.write_text(site.replace("/dev/ttyUSB0", "/dev/ttyUSB-missing-test"), encoding="utf-8") + env = os.environ.copy() + env["SITE_CONFIG"] = str(cfg) + proc = subprocess.run( + ["bash", str(root / "scripts" / "run_gripper.sh"), "--arm", "89", "--cli", "probe"], + cwd=str(root), + env=env, + capture_output=True, + text=True, + timeout=90, + ) + out = (proc.stdout or "") + (proc.stderr or "") + assert proc.returncode != 0 + assert "不在" in out + assert "先拉起来" not in out + script = (root / "scripts" / "run_gripper.sh").read_text(encoding="utf-8") + assert 'auto="${SITE_GRIP_AUTO_INIT:-true}"' in script + assert '-p auto_init:="${auto}"' in script + + def test_official_client_uses_the_same_latch_break(): src = Path(__file__).resolve().parents[1] / "arm_application" / "dh_official.py" text = src.read_text(encoding="utf-8") diff --git a/src/arm_application/test/test_listed_arms.py b/src/arm_application/test/test_listed_arms.py index fe237c1..31b627c 100644 --- a/src/arm_application/test/test_listed_arms.py +++ b/src/arm_application/test/test_listed_arms.py @@ -113,7 +113,8 @@ def test_main_page_holds_status_sim_cameras_tactile_and_actions(): texts = [button.text() for button in main.findChildren(QPushButton)] for name in ("跟踪并对孔", "只测旋转装配", "复位到 home", "本臂腕部相机"): assert any(name in text for text in texts) - assert any(text.startswith("⏻") for text in texts) + power = [button for button in main.findChildren(QPushButton) if "上电控制栈" in button.text()] + assert power and not power[0].icon().isNull() assert win._cam_pic assert all(label.minimumHeight() <= 96 for label in win._cam_pic.values()) from arm_application.operator_camera_view import targets diff --git a/src/arm_application/test/test_real_preflight.py b/src/arm_application/test/test_real_preflight.py index 8d06d11..c1fe7cd 100644 --- a/src/arm_application/test/test_real_preflight.py +++ b/src/arm_application/test/test_real_preflight.py @@ -402,6 +402,27 @@ def test_cameras_skips_sim_arm(tmp_path: Path) -> None: # --------------------------------------------------------------------------- +def test_joint_angles_survive_status_packing() -> None: + """关节角曾经让 classify_state(**state) 抛 TypeError，JSON 出不来，界面不更新。""" + import rdk_status + from rdk_helpers import rdk_whitelist_lists + + payload = rdk_status.pack_status({ + "connected": True, + "operational": True, + "op_status": "READY", + "mode": "IDLE", + "fault": False, + "estop_released": True, + "error": "", + "q": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7], + }) + assert payload["q"] == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7] + assert payload["level"] == rdk_status.LEVEL_OK + # 内核把 USB 网卡从 eth0 改名后，白名单必须仍是本机机器人网段地址。 + assert rdk_whitelist_lists("eth0", "192.168.2.10")[0] == ["192.168.2.10"] + + def test_network_action_appears_once_in_the_manual_list() -> None: """check_network 与 rdk_status 给的"去查网线"必须是**同一串字**。 diff --git a/src/arm_application/test/test_site_arm_sections.py b/src/arm_application/test/test_site_arm_sections.py index 29f32af..5629910 100644 --- a/src/arm_application/test/test_site_arm_sections.py +++ b/src/arm_application/test/test_site_arm_sections.py @@ -15,6 +15,7 @@ site.yaml 有两层：顶层是站点配置，``robots[]`` 里每台臂可以覆 from __future__ import annotations +import os import sys from pathlib import Path from typing import Iterable, List, Mapping @@ -94,3 +95,55 @@ def test_sim_robot_carries_the_truth_backend_override(arm_section_keys): assert sim, "site.yaml 里没有 sim 机器人 —— 仿真流程会挑不到配置" src = (sim[0].get("perception") or {}).get("source") assert src == "truth", f"sim 的 perception.source 应为 truth，现在是 {src!r}" + + +def _flatten_for(sn: str) -> dict: + import export_site_env # type: ignore + + previous = os.environ.get("ROBOT_SN") + os.environ["ROBOT_SN"] = sn + try: + return export_site_env.flatten(export_site_env.load_site()) + finally: + if previous is None: + os.environ.pop("ROBOT_SN", None) + else: + os.environ["ROBOT_SN"] = previous + + +def test_live_left_gripper_does_not_auto_home(arm_section_keys): + """左臂现场 auto_init 是 false。拉起驱动若把它弄成 true，会写 0xA5 回零。""" + flat = _flatten_for("89") + assert flat["SITE_GRIP_AUTO_INIT"] == "false" + assert flat["SITE_GRIP_PORT"] == "/dev/ttyUSB0" + + +def test_auto_init_override_is_not_ignored(arm_section_keys, tmp_path): + """把现场改成 true 必须真的变成 true。否则上面那条是 exporter 写死的假通过。""" + import export_site_env # type: ignore + + doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8")) + changed = False + for robot in robot_entries(doc): + if str(robot.get("sn") or "").endswith("063989"): + robot["gripper"]["auto_init"] = True + changed = True + assert changed + cfg = tmp_path / "site.yaml" + cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + previous_cfg = os.environ.get("SITE_CONFIG") + previous_sn = os.environ.get("ROBOT_SN") + os.environ["SITE_CONFIG"] = str(cfg) + os.environ["ROBOT_SN"] = "89" + try: + flat = export_site_env.flatten(export_site_env.load_site()) + finally: + if previous_cfg is None: + os.environ.pop("SITE_CONFIG", None) + else: + os.environ["SITE_CONFIG"] = previous_cfg + if previous_sn is None: + os.environ.pop("ROBOT_SN", None) + else: + os.environ["ROBOT_SN"] = previous_sn + assert flat["SITE_GRIP_AUTO_INIT"] == "true" diff --git a/src/arm_bringup/config/bringup.yaml b/src/arm_bringup/config/bringup.yaml index 3bbe47a..c69c46a 100644 --- a/src/arm_bringup/config/bringup.yaml +++ b/src/arm_bringup/config/bringup.yaml @@ -4,7 +4,8 @@ rizon_type: Rizon4 # Flexiv Studio 夹爪（GN01）—— 保持 false。DH PGEA 使用 run_gripper.sh。 load_gripper: false use_fake_hardware: false -start_rviz: true +# 模型在工位程序的仿真里看，启动栈不再弹出 RViz。 +start_rviz: false # Flexiv 官方关节命名：Rizon4-063989_joint1 ... joint7 joint_prefix: Rizon4-063989_ diff --git a/src/arm_bringup/launch/real.launch.py b/src/arm_bringup/launch/real.launch.py index ab05017..3c16fea 100644 --- a/src/arm_bringup/launch/real.launch.py +++ b/src/arm_bringup/launch/real.launch.py @@ -74,6 +74,10 @@ def generate_launch_description(): DeclareLaunchArgument('use_fake_hardware', default_value='false'), # 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh DeclareLaunchArgument('load_gripper', default_value='false'), - DeclareLaunchArgument('start_rviz', default_value='true'), + DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), OpaqueFunction(function=_launch_setup), ]) diff --git a/src/arm_bringup/launch/sim.launch.py b/src/arm_bringup/launch/sim.launch.py index 9961860..e942898 100644 --- a/src/arm_bringup/launch/sim.launch.py +++ b/src/arm_bringup/launch/sim.launch.py @@ -35,7 +35,11 @@ def generate_launch_description(): ) return LaunchDescription([ - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument( + 'use_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), DeclareLaunchArgument('use_moveit', default_value='false'), # 仿真**故意**不用真机的 SN 前缀：两边同时跑时，同名关节会让 TF 与 # 控制器互相打架（症状是 RViz 里模型乱跳）。见 devices/sim/gazebo/README.md。 diff --git a/src/arm_bringup/launch/stack.launch.py b/src/arm_bringup/launch/stack.launch.py index 7b39d13..22fcf03 100644 --- a/src/arm_bringup/launch/stack.launch.py +++ b/src/arm_bringup/launch/stack.launch.py @@ -201,7 +201,11 @@ def generate_launch_description(): default_value='mock', description='mock | moveit | real | isaac', ), - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument( + 'use_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), DeclareLaunchArgument('robot_sn', default_value=SITE_SN), DeclareLaunchArgument('load_gripper', default_value='false'), OpaqueFunction(function=_setup), diff --git a/src/arm_control/launch/mock_control.launch.py b/src/arm_control/launch/mock_control.launch.py index 69fd959..4fcd8f4 100644 --- a/src/arm_control/launch/mock_control.launch.py +++ b/src/arm_control/launch/mock_control.launch.py @@ -47,7 +47,11 @@ def generate_launch_description(): ) return LaunchDescription([ - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument( + 'use_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), DeclareLaunchArgument( 'robot_sn', default_value=SITE_ROBOT_SN, diff --git a/src/arm_description/launch/display.launch.py b/src/arm_description/launch/display.launch.py index e01fb43..1e86473 100644 --- a/src/arm_description/launch/display.launch.py +++ b/src/arm_description/launch/display.launch.py @@ -34,7 +34,11 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_gui', default_value='true'), - DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument( + 'use_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), # 默认取当前臂（scripts/env.sh 已导出）；不再写死某个 SN。 DeclareLaunchArgument( 'robot_sn', diff --git a/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py b/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py index 572cfca..5b020af 100644 --- a/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py +++ b/src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py @@ -70,13 +70,22 @@ def default_rdk_python() -> str: def default_bridge_script() -> Path: - """桥脚本路径。可用 ``SITE_ROS2_ROOT`` / ``ROS2_WS`` 指向别的工作区。""" + """桥脚本路径。可用 ``SITE_ROS2_ROOT`` / ``ROS2_WS`` 指向别的工作区。 + + 仓库不在 ``~/ros2`` 时，从本文件往上找，避免换目录之后去连一个空路径。 + """ root = ( os.environ.get("SITE_ROS2_ROOT", "").strip() or os.environ.get("ROS2_WS", "").strip() ) - base = Path(root).expanduser() if root else Path.home() / "ros2" - return base / BRIDGE_SCRIPT_REL + if root: + return Path(root).expanduser() / BRIDGE_SCRIPT_REL + here = Path(__file__).resolve() + for parent in here.parents: + candidate = parent / BRIDGE_SCRIPT_REL + if candidate.is_file(): + return candidate + return Path.home() / "ros2" / BRIDGE_SCRIPT_REL class RdkClient: diff --git a/src/arm_moveit_config/launch/move_group.launch.py b/src/arm_moveit_config/launch/move_group.launch.py index 6f453bb..e882a5b 100644 --- a/src/arm_moveit_config/launch/move_group.launch.py +++ b/src/arm_moveit_config/launch/move_group.launch.py @@ -69,12 +69,21 @@ def _launch_setup(context, *args, **kwargs): ], ) - return [move_group, rviz] + # 模型在工位程序里看。只有显式 start_rviz:=true 才再开 RViz。 + nodes = [move_group] + if LaunchConfiguration('start_rviz').perform(context).strip().lower() in ('1', 'true', 'yes'): + nodes.append(rviz) + return nodes def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_fake_hardware', default_value='true'), + DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='机械臂模型在工位程序里看，默认不再弹出 RViz', + ), # 从环境取当前臂 SN，避免靠 xacro 里的写死默认值"蒙对"。 DeclareLaunchArgument( 'robot_sn',