主界面一屏要同时看臂、夹爪、相机和触觉，按钮再被拉高就会叠到画面上。相机按原分辨率走回调取最新帧，场景一动不再等红外齐帧把彩色丢掉。模型和相机都留在工位窗口里，启动默认不再弹 RViz。
验收：test_listed_arms 与 test_camera_preview 共 20 项通过。D435 1280×720 发布约 29 帧/秒。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -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 绑定，不按名字。
|
||||
# 本机 <PC_IP> ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100
|
||||
# NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
|
||||
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
|
||||
|
||||
+10
-8
@@ -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 上。
|
||||
|
||||
+7
-7
@@ -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。装完新开一个终端：
|
||||
|
||||
|
||||
@@ -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 节点
|
||||
|
||||
单爪：
|
||||
|
||||
+6
-11
@@ -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) 里的规则编号，文档和提交信息里直接引用 |
|
||||
|
||||
+14
-15
@@ -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 读数是形变任意单位且
|
||||
|
||||
@@ -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_
|
||||
|
||||
@@ -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),
|
||||
])
|
||||
|
||||
@@ -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。
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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',
|
||||
|
||||
+14
-12
@@ -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}"
|
||||
|
||||
|
||||
@@ -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:=<site.yaml 里该臂的 robot.sn> load_gripper:=false
|
||||
ros2 launch arm_bringup real.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> load_gripper:=false start_rviz:=false
|
||||
|
||||
Official fake hardware:
|
||||
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false
|
||||
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false start_rviz:=false
|
||||
|
||||
EOF
|
||||
|
||||
+57
-14
@@ -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))
|
||||
|
||||
+11
-5
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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:-}"
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]}"
|
||||
|
||||
+3
-2
@@ -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 "$@"
|
||||
|
||||
+2
-2
@@ -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
|
||||
|
||||
@@ -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：真物理、真接触
|
||||
# 两者都不需要真机。要验证力控/接触类逻辑只能用后者。
|
||||
#
|
||||
|
||||
@@ -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}"
|
||||
|
||||
@@ -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(
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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(
|
||||
|
||||
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
|
||||
_WAKE.set()
|
||||
for thread in (_THREAD, _PAINT_THREAD):
|
||||
if thread is not None:
|
||||
thread.join(timeout=1.5)
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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,9 +313,12 @@ class RealsenseColorPub(TolerantNode):
|
||||
self._info_pub.publish(info)
|
||||
|
||||
if depth_u16 is not None:
|
||||
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
|
||||
dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
@@ -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()
|
||||
else:
|
||||
self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
|
||||
continue
|
||||
if misses == 1:
|
||||
self.get_logger().warn(f'wait_for_frames：{exc}')
|
||||
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:
|
||||
|
||||
@@ -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=""),
|
||||
|
||||
@@ -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 = {
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 给的"去查网线"必须是**同一串字**。
|
||||
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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_
|
||||
|
||||
@@ -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),
|
||||
])
|
||||
|
||||
@@ -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。
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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',
|
||||
|
||||
Reference in New Issue
Block a user