Initial import of ORISYS arm workstation
This commit is contained in:
cheng
@@ -0,0 +1,283 @@
|
||||
# handeye/ —— 手眼标定
|
||||
|
||||
这个文件夹放**手眼标定的入口、程序、配置、采点和结果**。四个程序在 `src/`，采点和结果在旁边。启动不要走 `scripts/`：
|
||||
|
||||
```bash
|
||||
bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 89 teach
|
||||
bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 89 merge
|
||||
bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 89 calibrate
|
||||
```
|
||||
|
||||
眼在手外把命令里的模式改成 `--mode eye_to_hand`。
|
||||
|
||||
---
|
||||
|
||||
## 1. 先搞懂"手眼标定"在干什么
|
||||
|
||||
机械臂要"看着东西去操作"，就必须知道**相机看到的位置**和**机械臂自己认为的位置**
|
||||
之间的对应关系。这个对应关系就是一个 4×4 的变换矩阵，标定过程就是把它解出来。
|
||||
|
||||
两种装法，决定了解出来的矩阵含义不同：
|
||||
|
||||
| 模式 | 相机装在哪 | 解出的 X 是什么 | 本项目的用法 |
|
||||
|------|-----------|----------------|-------------|
|
||||
| `eye_in_hand` | **相机装在机械臂末端（法兰/手腕）上** | 相机 → 法兰 | **D405 腕部相机（当前使用）** |
|
||||
| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机 |
|
||||
|
||||
**怎么选**：看相机是不是跟着机械臂动。
|
||||
- 跟着动 → `eye_in_hand`
|
||||
- 不动 → `eye_to_hand`
|
||||
|
||||
选错模式的后果：标定能算完、不报错，但结果**完全不对**（点云/抓取会系统性偏掉）。
|
||||
所以模式在文件名里就标出来了（见第 4 节），避免拿错文件。
|
||||
|
||||
---
|
||||
|
||||
## 2. 文件夹结构
|
||||
|
||||
```
|
||||
handeye/
|
||||
├── README.md ← 你正在看的这份
|
||||
├── calibrate.yaml ← 标定节点参数（板尺寸、运动、稳定性）
|
||||
├── waypoints.yaml ← 旧的"巡航记点"配置（legacy，一般用不到）
|
||||
├── poses/ ← 【输入】示教采点（每个点一组关节角）
|
||||
│ ├── 89_eye_in_hand.yaml
|
||||
│ └── 91_eye_in_hand.yaml
|
||||
├── results/ ← 【输出】标定结果（4×4 变换矩阵）
|
||||
│ ├── 89_eye_in_hand.yaml
|
||||
│ └── 91_eye_in_hand.yaml
|
||||
└── tmp/ ← 临时产物（采点图、taught_poses.txt、captures.json）
|
||||
├── 89/ **按臂号分目录**：两条臂同时标定不会互相覆盖
|
||||
└── 91/ 不进 git
|
||||
```
|
||||
|
||||
⚠️ `tmp/` 下是**按臂号分目录**的（`tmp/89`、`tmp/91`），不是所有臂共用一个。
|
||||
真实生效的值来自 `site.yaml` 的 `arms.<臂>.handeye_workdir` 与顶层
|
||||
`handeye_workdir`。两条臂若共用一个工作目录，先标定的那份采点会被后
|
||||
标定的覆盖，而症状是"标定结果突然变得很差"，不容易联想到是目录冲突。
|
||||
|
||||
**命名约定**：
|
||||
- 眼在手上：`<臂号>_eye_in_hand.yaml`（`paths.handeye_file` 指向它）
|
||||
- 眼在手外：`<臂号>_<相机>_eye_to_hand.yaml`（S03 按这个名字自己找，
|
||||
不要改 `paths.handeye_file`）
|
||||
- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`）
|
||||
- 示教点：`<臂号>_<模式>.yaml`（两种模式的关节角完全不同，分开存）
|
||||
|
||||
---
|
||||
|
||||
## 3. 标定板（最容易出错的地方）
|
||||
|
||||
两种装法用**两块不同的物理板**，尺寸按模式分开写，不能共用一组键：
|
||||
|
||||
| 模式 | 物理板 | 内角点（列 × 行） | 方格边长 | `site.yaml` 里的位置 |
|
||||
|------|--------|-------------------|----------|----------------------|
|
||||
| `eye_in_hand` | A5（腕部相机看台面） | 11 × 8 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` |
|
||||
| `eye_to_hand` | A4（板夹在 `tool10120938` 上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` |
|
||||
|
||||
权威只有 `config/site.yaml` 的 `handeye.boards.<模式>`。`run_handeye.sh` 按
|
||||
`--mode` 取对应那一组，用 `-p` 传给节点。`handeye/calibrate.yaml` 和节点默认值
|
||||
只是兜底（眼在手上那块板），**没配过的模式会被脚本当场拦下**，不会静默用兜底
|
||||
尺寸去标（那会让结果系统性偏，还不报错）。
|
||||
|
||||
怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
|
||||
是「方格数 − 1」。A5 板上画了 12×9 个格子 → 内角点 11×8；
|
||||
A4 板上画了 12×9 个格子 → 内角点 11×8（方格 20 mm）。列/行顺序不用纠结，检测端两种顺序都会试。
|
||||
|
||||
> ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
|
||||
> `8×11/0.015`，谁生效取决于覆盖顺序。后来又把两块板合成一组键，于是标眼在手外
|
||||
> 时静默用了 A5 的 15 mm。症状都是"标定流程跑得很顺、不报错，但结果明显不对"。
|
||||
|
||||
---
|
||||
|
||||
## 4. 完整流程
|
||||
|
||||
三条命令，按顺序执行。**先决条件**：标定板摆在相机视野里，机械臂能自由拖动。
|
||||
|
||||
### 第一步：采点（拖动示教）
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_handeye.sh teach --arm 89
|
||||
```
|
||||
|
||||
会弹出实时预览窗口（**只显示画面，不做棋盘检测**——真正的角点稳定判定在
|
||||
`calibrate` 那一步）。操作：
|
||||
|
||||
| 按键 | 作用 |
|
||||
|------|------|
|
||||
| `d` | 打开软拖拽（此时可以用手推机械臂） |
|
||||
| `s` | 关闭软拖拽（锁住） |
|
||||
| `Enter` / `空格` | **拍照记点**（记录当前关节角 + 图像） |
|
||||
| `q` / `Esc` | 结束采点 |
|
||||
|
||||
要点：
|
||||
- **点位要分散**：让标定板在画面里出现在不同位置、不同角度（远/近/左/右/倾斜）。
|
||||
全挤在一个角落会让求解退化，结果不可信。
|
||||
- **至少 5 个点**，推荐 **10～15 个**。数量在 `poses/*.yaml` 的 `num_poses` 里，
|
||||
也可以直接多按几次 `Enter`。
|
||||
- 采点数据写在 `handeye/tmp/<臂号>/`（如 `handeye/tmp/89/`），**确认无误后再 merge**（下一步）。
|
||||
|
||||
### 第二步：合并点位
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_handeye.sh merge --arm 89
|
||||
```
|
||||
|
||||
把临时采到的关节角写入 `handeye/poses/89_eye_in_hand.yaml`，
|
||||
并自动同步 `num_poses`。这一步不做数学计算，只是搬运+格式化。
|
||||
|
||||
### 第三步：自动标定
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_handeye.sh calibrate --arm 89
|
||||
```
|
||||
|
||||
机械臂会**自动**走一遍刚才记下的所有点位，每个点：
|
||||
|
||||
1. 移动到目标位姿
|
||||
2. 校验确实到位了（防止控制器假报成功）
|
||||
3. **等机械臂真的停下来**（看关节速度，不是干等固定秒数）
|
||||
4. 等画面里的标定板**连续多帧稳定**（角点抖动小于阈值）
|
||||
5. 拍照、记录法兰位姿 + 板位姿
|
||||
6. 屏幕上打一条进度条
|
||||
|
||||
全部走完后一次性解算出结果，写入
|
||||
`handeye/results/89_eye_in_hand.yaml`。
|
||||
|
||||
```
|
||||
[ 4/12] [████████░░░░░░░░░░░░░░░░] 33.3% moving...
|
||||
```
|
||||
|
||||
某个点看不到板时会**跳过该点**继续（`skip_unstable_poses: true`），
|
||||
不会整轮中止 —— 但如果跳掉的点很多，说明采点时板的位置不好，建议重采。
|
||||
|
||||
### 查看当前状态
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_handeye.sh show --arm 89
|
||||
```
|
||||
|
||||
打印臂号、配置文件路径、板尺寸、已记录点数，以及当前的标定结果是否存在。
|
||||
|
||||
---
|
||||
|
||||
## 5. 两种模式
|
||||
|
||||
- **自动模式** = 上面的 `calibrate`。机械臂自己走点位，人不用管，
|
||||
适合"点位已经采好、只想重算一遍"。
|
||||
- **手动模式** = 上面的 `teach`。人拖着机械臂摆姿势、手动决定什么时候记这个点。
|
||||
适合第一次标定，或者想补几个点位。
|
||||
|
||||
两者共用同一份 `poses/*.yaml`，顺序是 `teach`（采）→ `merge`（存）→ `calibrate`（算）。
|
||||
|
||||
### 一条命令跑完（手动模式 + 自动求解）
|
||||
|
||||
```bash
|
||||
# 需要先在另一个终端跑 run_real.sh
|
||||
bash ~/ros2/scripts/run_handeye.sh teach --ros --auto-solve --arm 89
|
||||
```
|
||||
|
||||
采完点按 `q` 之后，脚本会自动 `merge`、重启相机、接着 `calibrate`，
|
||||
不用再敲第二条命令。
|
||||
|
||||
> ⚠️ **`--auto-solve` 只在 `--ros`（示教器）模式能全程跑通。**
|
||||
>
|
||||
> 原因是一个硬件约束：标定必须通过 `run_real.sh` 的控制器驱动机械臂走点位，
|
||||
> 而 `--rdk` 软拖拽要求 **RDK 独占**，两者无法同时存在。
|
||||
> 所以 `--rdk --auto-solve` 会照常自动 merge，然后明确告诉你剩下那两条命令 ——
|
||||
> **这是物理限制，不是脚本能绕过的**。
|
||||
>
|
||||
> 想要真正一条命令，就用示教器 Free Drive：先在示教器上打开 Free Drive，
|
||||
> 再用 `--ros --auto-solve`。
|
||||
|
||||
|
||||
### 切换 eye-in-hand / eye-to-hand
|
||||
|
||||
```bash
|
||||
# 用腕部相机 + A5 板（默认，板放台面不动）
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
|
||||
|
||||
# 用固定相机 + A4 板（板必须装到法兰上，相机不动）
|
||||
# 前置：另开终端 bash ~/ros2/scripts/run_camera_d435.sh --preview
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand
|
||||
```
|
||||
|
||||
`--mode` 会同时切：标定板、相机、示教点文件、结果文件。所以：
|
||||
|
||||
- 两种模式**互不覆盖**，可以分别标定、共存
|
||||
- 眼在手上结果：`handeye/results/<臂号>_eye_in_hand.yaml`（`T` = 相机 → 法兰）
|
||||
- 眼在手外结果：`handeye/results/<臂号>_<相机>_eye_to_hand.yaml`
|
||||
（`T` = 相机 → 这条臂的基座；双臂共用一台 D435 时必须按臂分开）
|
||||
- **不要**把 `paths.handeye_file` 改成眼在手外那份 —— 那条路径是腕部相机
|
||||
（S05 精拍定位）用的。手外那份由 `gauge_assembly.launch` 按名字自己找，
|
||||
并以 **基座 ← 相机** 发布 TF（不是法兰 ← 相机）
|
||||
|
||||
传了非法模式会立刻报错并退出（不会悄悄退回默认值 —— 那会导致"以为标的是
|
||||
eye-to-hand、其实算的是 eye-in-hand"这种最难查的错）。
|
||||
|
||||
### 设定采集点数
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --points 12
|
||||
```
|
||||
|
||||
采点过程中会显示 `[3/12] ... 还差 9 个点`，采够时提示可以收工。
|
||||
**不会强制关窗** —— 常常采到一半发现某个角度还没覆盖，强制退出反而要重开；
|
||||
想多采就继续按 `Enter`，想结束按 `q`。
|
||||
|
||||
不加 `--points` 则不限数量（默认），建议 8～12 个点，至少 5 个。
|
||||
|
||||
|
||||
---
|
||||
|
||||
## 6. 结果怎么被用起来
|
||||
|
||||
`handeye/results/89_eye_in_hand.yaml` 的内容长这样：
|
||||
|
||||
```yaml
|
||||
translation_m: [-0.06085181, -0.00864320, 0.12364093]
|
||||
T_4x4:
|
||||
- [0.01784499, -0.86878381, 0.49486994, -0.06085181]
|
||||
- [0.99977570, 0.02115147, 0.00108124, -0.00864320]
|
||||
- [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
```
|
||||
|
||||
它被以下地方读取，最终以**静态 TF** 的形式发布出去：
|
||||
|
||||
| 谁读 | 在哪 |
|
||||
|------|------|
|
||||
| `pipeline.launch.py` | 发布 `{法兰}` → `{相机}_color_optical_frame` 静态 TF |
|
||||
| `visual_servo.launch.py` | 同上，并把路径传给 `servo_node` |
|
||||
| `stage2_locate.launch.py` | 同上；文件缺失会直接报错 |
|
||||
| `stage3_search.launch.py` | 同上 |
|
||||
| `servo_node.py` | 用 `T_4x4` 算相机倾角、做 hover/lookat |
|
||||
|
||||
也就是说：**标定结果不对，上面这些全会跟着错**。改完结果建议重启相关节点。
|
||||
|
||||
---
|
||||
|
||||
## 7. 出了问题先看这里
|
||||
|
||||
| 现象 | 多半是 |
|
||||
|------|--------|
|
||||
| 标定跑完但抓取/定位系统性偏 | 板尺寸三处不一致；或模式选错（eye-in-hand / eye-to-hand 弄反） |
|
||||
| 一直"棋盘不稳定"、点位被跳过 | 标定板没完整入镜 / 反光过曝 / 太远太小；调低 `stable_pixel_std` 或改善光照 |
|
||||
| `ERROR: 机械臂控制器未激活` | 先另开终端跑 `bash ~/ros2/scripts/run_real.sh --arm 89` |
|
||||
| `hand-eye result missing` | `site.yaml` 里的 `paths.handeye_file` 指到了不存在的文件 |
|
||||
| 拖动时臂不动 | 软拖拽没开（按 `d`）；或 RDK 与 `run_real.sh` 抢占，见 `docs/INTERFACES.md` |
|
||||
| 采的点少于 5 个 | `merge` 会拒绝（求解至少需要 5 组，且姿态要有区分度） |
|
||||
|
||||
---
|
||||
|
||||
## 8. 相关文件
|
||||
|
||||
- 拖动引擎：`scripts/rdk_drag_bridge.py`
|
||||
- 拖拽的 ROS 服务：`/soft_drag`（在 `arm_driver_flexiv` 里）
|
||||
- 数学与视觉：`src/arm_application/arm_application/handeye_lib.py`
|
||||
- 采点节点：`src/arm_application/arm_application/handeye_teach.py`
|
||||
- 求解节点：`src/arm_application/arm_application/handeye_calibrate.py`
|
||||
- 单元测试：`src/arm_application/test/test_handeye_lib.py`
|
||||
（用合成数据验证求解正确性，无需硬件：
|
||||
`python3 -m pytest src/arm_application/test/test_handeye_lib.py`）
|
||||
@@ -0,0 +1,72 @@
|
||||
# 手眼自动标定配置。
|
||||
#
|
||||
# 工作空间：~/ros2
|
||||
#
|
||||
# 先示教位姿，再求解（推荐用封装脚本）：
|
||||
# bash ~/ros2/scripts/run_handeye.sh teach
|
||||
# bash ~/ros2/scripts/run_handeye.sh merge
|
||||
# bash ~/ros2/scripts/run_handeye.sh calibrate
|
||||
|
||||
handeye_calibrate:
|
||||
ros__parameters:
|
||||
mode: "eye_in_hand"
|
||||
|
||||
# ⚠️ 站点相关值**故意不写在这里**（joint_prefix / output_dir / num_poses /
|
||||
# waypoint_positions），它们由 run_handeye.sh 在运行时提供：
|
||||
# joint_prefix ← 由 arms.yaml 按臂号解析，-p 传入
|
||||
# output_dir ← 按"臂号 + 模式"算出的临时目录，-p 传入
|
||||
# num_poses / waypoint_positions ← 来自 handeye/poses/<臂号>_<模式>.yaml
|
||||
# （作为后一个 --params-file，会覆盖本文件）
|
||||
# 它们此前在这里各留了一份副本，与 poses/ 里的实际数据并不一致
|
||||
# （本文件是 2026-08-28 的 13 点，poses/89 是 2026-09-02 的 12 点），
|
||||
# 只是恰好被覆盖顺序压住。留着就是给"改了脚本顺序 / 直连节点"埋雷：
|
||||
# 会静默用上旧数据，而症状只是标定结果变差，很难联想到这份副本。
|
||||
# 注：waypoint_positions 的节点默认值是全 0，所以**直连节点是跑不了的**，
|
||||
# 必须经 run_handeye.sh（或自己传 --params-file）—— 这是有意为之。
|
||||
#
|
||||
# 设备ID 相关的东西同理，不写死在这里，见 config/site.yaml 与 arms.yaml。
|
||||
|
||||
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
|
||||
velocity_scale: 0.10
|
||||
action_name: "follow_joint_trajectory"
|
||||
move_time_sec: 4.0
|
||||
start_index: 0
|
||||
min_poses: 5
|
||||
|
||||
# 相机装在法兰上（eye-in-hand）：D405
|
||||
camera_source: "d405" # d405 | d435 | manual
|
||||
camera_serial: ""
|
||||
camera_namespace: ""
|
||||
realsense_width: 1280
|
||||
realsense_height: 720
|
||||
image_topic: ""
|
||||
camera_info_topic: ""
|
||||
|
||||
camera_fx: 0.0
|
||||
camera_fy: 0.0
|
||||
camera_cx: 0.0
|
||||
camera_cy: 0.0
|
||||
camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
# 板尺寸的权威在 site.yaml → handeye.boards.<模式>。
|
||||
# 这里只写眼在手上那块板的兜底（A5 / 11×8 / 15mm）；run_handeye.sh 会按
|
||||
# --mode 用 -p 覆盖成对应模式的值。没配过的模式会被脚本当场拦下，
|
||||
# 不会静默用这份兜底去标另一块板。
|
||||
board_cols: 11
|
||||
board_rows: 8
|
||||
square_size_m: 0.015
|
||||
|
||||
settle_sec: 0.4
|
||||
stable_window: 8
|
||||
stable_pixel_std: 0.45
|
||||
stable_timeout_sec: 25.0
|
||||
|
||||
# 临时产物（采点图、taught_poses.txt、captures.json）落在 handeye/tmp/ 下。
|
||||
# 注意 handeye/tmp/ 不进 git。
|
||||
# output_dir 不写在这里：由 run_handeye.sh 按"臂号 + 模式"算出击（handeye/tmp/89 等）。
|
||||
# 留一个共享的 output_dir 会让多条臂互相覆盖采点，症状是"标定结果突然变差"。
|
||||
#
|
||||
# 留空 = 由 run_handeye.sh 按"设备ID + 模式"给出路径，
|
||||
# 即 handeye/results/<臂号>_<模式>.yaml（如 89_eye_in_hand.yaml）。
|
||||
result_file: ""
|
||||
cleanup_images: true
|
||||
@@ -0,0 +1,56 @@
|
||||
# 手眼标定示教的关节位姿（7 关节 × N）。由 calibrate + waypoints 共用。
|
||||
#
|
||||
# 在这里编辑，或者：
|
||||
# bash ~/ros2/scripts/run_handeye.sh teach
|
||||
# bash ~/ros2/scripts/run_handeye.sh merge
|
||||
#
|
||||
# /** 这样任意节点名都能收到这些参数。
|
||||
|
||||
/**:
|
||||
ros__parameters:
|
||||
num_poses: 21
|
||||
# 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
|
||||
waypoint_positions: [
|
||||
# pose 0
|
||||
-0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
|
||||
# pose 1
|
||||
-0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
|
||||
# pose 2
|
||||
-0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
|
||||
# pose 3
|
||||
-1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
|
||||
# pose 4
|
||||
-0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
|
||||
# pose 5
|
||||
-0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
|
||||
# pose 6
|
||||
-0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
|
||||
# pose 7
|
||||
-0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
|
||||
# pose 8
|
||||
-0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
|
||||
# pose 9
|
||||
-0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
|
||||
# pose 10
|
||||
-0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
|
||||
# pose 11
|
||||
-0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
|
||||
# pose 12
|
||||
-0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
|
||||
# pose 13
|
||||
-0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
|
||||
# pose 14
|
||||
-1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
|
||||
# pose 15
|
||||
-0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
|
||||
# pose 16
|
||||
-0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
|
||||
# pose 17
|
||||
-0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
|
||||
# pose 18
|
||||
-1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
|
||||
# pose 19
|
||||
-1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
|
||||
# pose 20
|
||||
-1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
|
||||
]
|
||||
@@ -0,0 +1,54 @@
|
||||
# 眼在手外示教关节（7 关节 × N）。板夹在 tool10120938 上，顶置 D435 看它。
|
||||
#
|
||||
# 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。
|
||||
#
|
||||
# bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
|
||||
# bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
|
||||
|
||||
/**:
|
||||
ros__parameters:
|
||||
num_poses: 21
|
||||
waypoint_positions: [
|
||||
# pose 0
|
||||
-0.798667, -0.611079, 0.054912, 2.246988, 0.049199, 1.389219, 0.196135,
|
||||
# pose 1
|
||||
-0.787304, -0.531383, 0.240108, 2.183440, 0.367026, 1.370311, 0.472489,
|
||||
# pose 2
|
||||
-1.615896, -0.687070, 1.187600, 2.243693, -0.162789, 1.650045, 1.026121,
|
||||
# pose 3
|
||||
-1.787431, -0.668004, 1.408195, 2.025431, -0.064718, 0.565415, 1.324635,
|
||||
# pose 4
|
||||
-2.024343, -0.863793, 1.349640, 2.359469, -0.720629, 0.084467, 0.821412,
|
||||
# pose 5
|
||||
-1.884100, -1.117584, 1.183093, 2.127937, -1.258013, 0.151934, 0.985555,
|
||||
# pose 6
|
||||
-1.866465, -1.194137, 1.143263, 1.872811, -1.320028, 0.280719, 0.777663,
|
||||
# pose 7
|
||||
-1.729402, -1.139685, 1.127574, 1.913544, -1.329630, -0.194162, 1.383834,
|
||||
# pose 8
|
||||
-1.611937, -1.158158, 1.064049, 1.918313, -1.480676, -0.392994, 1.833894,
|
||||
# pose 9
|
||||
-2.252978, -1.387014, 1.686186, 2.101298, -1.333349, -0.111994, 0.036817,
|
||||
# pose 10
|
||||
-2.179722, -1.297659, 1.497171, 2.167675, -1.272158, 0.100548, 0.673603,
|
||||
# pose 11
|
||||
-1.921897, -1.543024, 1.000863, 1.897273, -1.455972, 1.437388, -0.140968,
|
||||
# pose 12
|
||||
-1.884059, -1.063347, 1.439248, 2.253200, -1.164963, -0.423306, 0.907197,
|
||||
# pose 13
|
||||
-1.987022, -1.235459, 1.354108, 2.370862, -1.354044, 0.065378, 0.923079,
|
||||
# pose 14
|
||||
-1.770869, -1.333861, 1.241996, 1.800723, -1.395819, -0.255590, 1.389660,
|
||||
# pose 15
|
||||
-1.761759, -1.369860, 1.213137, 1.791639, -1.467956, -0.170537, 1.473557,
|
||||
# pose 16
|
||||
-1.504294, -1.346227, 1.206550, 1.660186, -1.320782, -0.393676, 1.657215,
|
||||
# pose 17
|
||||
-1.846546, -1.476930, 0.946255, 1.910615, -2.435429, 1.535168, 2.151544,
|
||||
# pose 18
|
||||
-1.700067, -1.452760, 0.909035, 1.758222, -2.103930, 1.445249, 2.174743,
|
||||
# pose 19
|
||||
-2.173713, -0.983647, 1.399849, 2.384939, -1.379509, -0.216471, 0.253561,
|
||||
# pose 20
|
||||
-2.160187, -0.967904, 1.376288, 2.451713, -1.393524, -0.212278, 0.473606,
|
||||
]
|
||||
@@ -0,0 +1,36 @@
|
||||
# 右臂 91 手眼标定示教的关节位姿。
|
||||
# 用以下命令填充：
|
||||
# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
|
||||
# bash ~/ros2/scripts/run_handeye.sh --arm 91 merge
|
||||
#
|
||||
# 不要复用 89 的 waypoint_positions。
|
||||
|
||||
/**:
|
||||
ros__parameters:
|
||||
num_poses: 12
|
||||
waypoint_positions: [
|
||||
# 位姿 0
|
||||
0.387144, -0.514770, -0.072695, 1.399918, 0.326501, 0.308501, -0.298212,
|
||||
# 位姿 1
|
||||
0.459205, -0.747691, 0.042380, 0.908430, 0.436933, -0.099413, -0.398336,
|
||||
# 位姿 2
|
||||
0.113162, -0.929267, -0.132969, 0.776759, 0.579499, -0.081271, -0.588715,
|
||||
# 位姿 3
|
||||
0.823056, -1.181803, -0.157161, 0.344860, 0.073450, -0.436449, -0.333484,
|
||||
# 位姿 4
|
||||
0.561660, 0.126479, -0.217063, 2.400061, 1.116363, 1.205544, -0.955972,
|
||||
# 位姿 5
|
||||
0.749011, 0.058548, -0.125615, 1.951037, 0.399065, 0.388437, -1.057397,
|
||||
# 位姿 6
|
||||
0.707611, -0.557010, -0.302672, 1.263980, 0.333093, 0.208229, -0.938372,
|
||||
# 位姿 7
|
||||
0.197846, -0.400530, 0.263064, 1.879643, 0.062586, 1.133715, -0.791285,
|
||||
# 位姿 8
|
||||
0.804027, -0.518162, 0.353947, 1.985246, -0.941195, 0.481238, -1.551113,
|
||||
# 位姿 9
|
||||
0.296161, -0.030159, 0.317117, 2.335652, 1.072158, 1.524893, -0.591372,
|
||||
# 位姿 10
|
||||
0.346492, -0.906418, -0.118014, 0.810448, 0.420788, -0.122104, -0.197859,
|
||||
# 位姿 11
|
||||
1.031802, -0.422436, -0.640939, 1.656629, 0.375163, 0.470365, -0.328041,
|
||||
]
|
||||
@@ -0,0 +1,22 @@
|
||||
# 手眼标定结果
|
||||
# mode: eye_to_hand
|
||||
# camera_source: d435
|
||||
# views: 24
|
||||
# utc: 2026-10-12T05:20:30.137123+00:00
|
||||
# board: 11x8 square_m=0.02
|
||||
# gripper_tool: tool10120938
|
||||
# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
|
||||
# 旧结果备份在 results/backup/
|
||||
gripper_tool: tool10120938
|
||||
T_flange_to_tool:
|
||||
- [1.00000000, 0.00000000, 0.00000000, -0.00050000]
|
||||
- [0.00000000, 1.00000000, 0.00000000, 0.00670000]
|
||||
- [0.00000000, 0.00000000, 1.00000000, 0.22450000]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
# T_cam_to_base (eye_to_hand)
|
||||
translation_m: [0.37094529, -0.52805928, 0.73745585]
|
||||
T_4x4:
|
||||
- [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
|
||||
- [0.99991380, 0.01024832, 0.00820703, -0.52805928]
|
||||
- [0.00862304, -0.04123150, -0.99911241, 0.73745585]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
@@ -0,0 +1,28 @@
|
||||
# 手眼标定结果
|
||||
# mode: eye_in_hand
|
||||
# camera_source: d405_left
|
||||
# views: 21
|
||||
# utc: 2026-10-12T03:29:51.243487+00:00
|
||||
# board: 11x8 square_m=0.015
|
||||
# gripper_tool: tool10120938
|
||||
# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
|
||||
# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
|
||||
# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
|
||||
gripper_tool: tool10120938
|
||||
T_flange_to_tool:
|
||||
- [1.00000000, 0.00000000, 0.00000000, -0.00050000]
|
||||
- [0.00000000, 1.00000000, 0.00000000, 0.00670000]
|
||||
- [0.00000000, 0.00000000, 1.00000000, 0.22450000]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
T_cam_to_tool:
|
||||
- [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
|
||||
- [0.99932767, -0.03620511, 0.00577974, -0.01523625]
|
||||
- [0.01281276, 0.49256977, 0.87017863, -0.09761517]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
# T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
|
||||
translation_m: [-0.06011995, -0.00853625, 0.12688483]
|
||||
T_4x4:
|
||||
- [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
|
||||
- [0.99932767, -0.03620511, 0.00577974, -0.00853625]
|
||||
- [0.01281276, 0.49256977, 0.87017863, 0.12688483]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
@@ -0,0 +1,13 @@
|
||||
# Hand-eye result
|
||||
# mode: eye_in_hand
|
||||
# camera_source: d405
|
||||
# views: 11
|
||||
# utc: 2026-09-15T08:44:59.380472+00:00
|
||||
# board: 8x11 square_m=0.015
|
||||
# T_cam_to_gripper (eye_in_hand)
|
||||
translation_m: [-0.06856274, -0.00768546, 0.13773073]
|
||||
T_4x4:
|
||||
- [0.00611182, -0.87098536, 0.49127096, -0.06856274]
|
||||
- [0.99997742, 0.00395004, -0.00543744, -0.00768546]
|
||||
- [0.00279539, 0.49129310, 0.87098983, 0.13773073]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
@@ -0,0 +1,19 @@
|
||||
# 备份：不要当现行结果用。
|
||||
# 原文件：handeye/results/89_d435_eye_to_hand.yaml
|
||||
# 备份原因：2026-10-12 起左臂 89 眼在手外改按 Studio 工具 tool10120938
|
||||
# 的 TCP 做 AX=XB（板夹在该工具上）。本份是 2026-10-09、18 视角、
|
||||
# A4 11×8 / 20 mm 的结果，当时激活工具未经这次对齐。
|
||||
# T_4x4 仍是 cam→基座。现行结果仍写回 89_d435_eye_to_hand.yaml。
|
||||
# 手眼标定结果
|
||||
# mode: eye_to_hand
|
||||
# camera_source: d435
|
||||
# views: 18
|
||||
# utc: 2026-10-09T02:56:24.621687+00:00
|
||||
# board: 11x8 square_m=0.02
|
||||
# T_cam_to_base (eye_to_hand)
|
||||
translation_m: [0.37495201, -0.52156937, 0.72589194]
|
||||
T_4x4:
|
||||
- [-0.04575158, 0.99819969, -0.03878355, 0.37495201]
|
||||
- [0.99893621, 0.04594039, 0.00399073, -0.52156937]
|
||||
- [0.00576527, -0.03855971, -0.99923967, 0.72589194]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
@@ -0,0 +1,20 @@
|
||||
# 备份：不要当现行结果用。
|
||||
# 原文件：handeye/results/89_eye_in_hand.yaml
|
||||
# 备份原因：2026-10-12 起左臂 89 眼在手上改按 Studio 工具 tool10120938 求解。
|
||||
# 本份是 2026-09-04 那次标定：AX=XB 的 gripper 按当时激活的 tool1 / 法兰口径，
|
||||
# 板为内角点 8×11、方格 15 mm。T_4x4 当时标注为 T_cam_to_gripper。
|
||||
# 现行结果仍写回 89_eye_in_hand.yaml；要比对旧外参请打开本文件。
|
||||
# 左臂 89 eye-in-hand. 不要用于 91。
|
||||
# 手眼标定结果
|
||||
# mode: eye_in_hand
|
||||
# camera_source: d405
|
||||
# views: 12
|
||||
# utc: 2026-09-04T10:15:27.480727+00:00
|
||||
# board: 8x11 square_m=0.015
|
||||
# T_cam_to_gripper (eye_in_hand)
|
||||
translation_m: [-0.06085181, -0.00864320, 0.12364093]
|
||||
T_4x4:
|
||||
- [0.01784499, -0.86878381, 0.49486994, -0.06085181]
|
||||
- [0.99977570, 0.02115147, 0.00108124, -0.00864320]
|
||||
- [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
@@ -0,0 +1,11 @@
|
||||
# 仿真手眼真值 —— **自动生成，不要手改**
|
||||
# mode: eye_to_hand
|
||||
# 来源: Gazebo/URDF 的 TF 真值 `sim_base_link` ← `sim_d435_color_optical_frame`（仿真不需要标定）
|
||||
# 生成: 2026-09-28T14:05:19+0800 生成器: scripts/gen_sim_handeye.py
|
||||
# 仿真台：手外相机固定在工位/世界上（不随臂动）
|
||||
translation_m: [-0.45000000, 0.00000000, 1.35000000]
|
||||
T_4x4:
|
||||
- [-0.00000236, -0.76604704, 0.64278451, -0.45000000]
|
||||
- [-1.00000000, 0.00000000, -0.00000367, 0.00000000]
|
||||
- [0.00000281, -0.64278451, -0.76604704, 1.35000000]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
@@ -0,0 +1,11 @@
|
||||
# 仿真手眼真值 —— **自动生成，不要手改**
|
||||
# mode: eye_in_hand
|
||||
# 来源: Gazebo/URDF 的 TF 真值 `sim_flange` ← `sim_d405_color_optical_frame`（仿真不需要标定）
|
||||
# 生成: 2026-09-28T14:05:19+0800 生成器: scripts/gen_sim_handeye.py
|
||||
# 仿真台：腕部相机装在法兰上，位姿来自 URDF
|
||||
translation_m: [0.00000000, -0.06000000, 0.05000000]
|
||||
T_4x4:
|
||||
- [0.00000000, 1.00000000, -0.00000000, 0.00000000]
|
||||
- [-1.00000000, 0.00000000, -0.00000367, -0.06000000]
|
||||
- [-0.00000367, 0.00000000, 1.00000000, 0.05000000]
|
||||
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
|
||||
Executable
+482
@@ -0,0 +1,482 @@
|
||||
#!/usr/bin/env bash
|
||||
# 手眼标定辅助脚本。
|
||||
# 脚本：~/桌面/orisys_arm/handeye/run_handeye.sh
|
||||
# 配置与结果：~/ros2/handeye/ ← 见 handeye/README.md
|
||||
# poses/<臂号>_<模式>.yaml 采点（输入）
|
||||
# results/<臂号>_<模式>.yaml 标定结果（输出）
|
||||
# calibrate.yaml 板尺寸 / 运动 / 稳定性参数
|
||||
# tmp/ 临时产物
|
||||
# 站点覆盖项：~/ros2/config/site.yaml → handeye:
|
||||
#
|
||||
# 用法：
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh calibrate
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh waypoints
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh teach # RDK 软拖拽（推荐）
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh teach --ros --auto-solve
|
||||
# # 示教器模式 + 采完自动 merge 并直接标定（一条命令跑完）
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh teach --points 12 # 目标采 12 个点
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh teach --append # 接着上次点位继续采
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh teach --ros # 示教器 Free Drive
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh merge [taught.txt]
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh show # 看当前配置与点数
|
||||
#
|
||||
# 关于 --auto-solve（采完自动标定）的重要限制:
|
||||
# 只有 --ros（示教器）模式能真正一条命令跑完。因为标定必须在 run_real.sh 的
|
||||
# 控制器上走点位，而 --rdk 软拖拽要 RDK 独占，两者互斥 —— 这是硬件约束。
|
||||
# --rdk --auto-solve 会照常自动 merge，然后明确告诉你接下来那两条命令。
|
||||
#
|
||||
# 全局选项（放在子命令前或后都可以）:
|
||||
# --arm 89|91|left|right 选臂（默认 89）
|
||||
# --mode eye_in_hand|eye_to_hand
|
||||
# 选模式。会切换到 <臂号>_<模式>.yaml，
|
||||
# 所以 eye_in_hand 与 eye_to_hand 的结果互不覆盖
|
||||
# --points N 仅 teach：目标采集点数（0=不限）
|
||||
#
|
||||
# 例：
|
||||
# bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 91 teach --mode eye_to_hand --points 15
|
||||
#
|
||||
# calibrate / waypoints 需要:
|
||||
# T1: bash ~/桌面/orisys_arm/scripts/run_real.sh --arm 89|91
|
||||
# T2: bash ~/桌面/orisys_arm/scripts/run_camera_d405.sh --arm 89|91 --preview
|
||||
|
||||
# teach（拖拽）需要: 先停掉 run_real.sh 和相机（RDK + D405 都独占）
|
||||
# 89 与 91 的结果文件、示教点、workdir 完全分开，禁止混用。
|
||||
|
||||
set -eo pipefail
|
||||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/parse_arm_args.sh"
|
||||
parse_arm_args "$@"
|
||||
set -- "${PARSE_ARM_REST[@]}"
|
||||
CMD="${1:-}"
|
||||
shift || true
|
||||
|
||||
usage() {
|
||||
sed -n '2,40p' "$0"
|
||||
}
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/lib/load_site_config.sh"
|
||||
# 标定节点就在本目录，直接用源码启动，不经过 ros2 run。
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
handeye_node() {
|
||||
python3 "${ROOT}/handeye/src/${1}.py" --ros-args "${@:2}"
|
||||
}
|
||||
|
||||
# 优先用包的源码树（editable 模式）；退而用 install 里的 share
|
||||
APP_CFG="${ROOT}/src/arm_application/config"
|
||||
if [[ ! -d "${APP_CFG}" ]]; then
|
||||
APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true
|
||||
fi
|
||||
|
||||
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。
|
||||
# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号与相机名**
|
||||
# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算；相机名来自 cameras 注册表）。
|
||||
#
|
||||
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id`、`HANDEYE_MODE` 与相机名
|
||||
# 在下面几行之后才算得出来，在这里拼会拼出 `results/_.yaml` 这种空文件名，
|
||||
# 而症状是"标定写完了、用的时候读不到"。
|
||||
HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
|
||||
CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
|
||||
TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
|
||||
WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
|
||||
PREFIX="${SITE_JOINT_PREFIX:?SITE_JOINT_PREFIX 为空 —— site.yaml 里需要该臂的 robot.sn}"
|
||||
|
||||
# ---- --mode：切换 eye-in-hand / eye-to-hand ----
|
||||
# 必须在 WP/RES/板/相机定下来之前处理：模式决定了读哪个结果文件、用哪块板、
|
||||
# 开哪台相机、采点落到哪个文件。其余参数原样留给子命令（teach 自己有 --rdk 等）。
|
||||
HANDEYE_MODE="${SITE_HANDEYE_MODE:-eye_in_hand}"
|
||||
_rest=()
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--mode)
|
||||
HANDEYE_MODE="${2:?--mode 需要 eye_in_hand 或 eye_to_hand}"
|
||||
if [[ "${HANDEYE_MODE}" != "eye_in_hand" && "${HANDEYE_MODE}" != "eye_to_hand" ]]; then
|
||||
echo "ERROR: --mode 只能是 eye_in_hand 或 eye_to_hand，收到 '${HANDEYE_MODE}'" >&2
|
||||
exit 1
|
||||
fi
|
||||
shift 2
|
||||
;;
|
||||
*)
|
||||
_rest+=("$1")
|
||||
shift
|
||||
;;
|
||||
esac
|
||||
done
|
||||
set -- "${_rest[@]}"
|
||||
|
||||
# 臂号取 Flexiv 序列号后两位：Rizon4-063989 → 89
|
||||
_arm_id="${SITE_ROBOT_SN: -2}"
|
||||
|
||||
# 取"当前模式"的那一份值。名字由 export_site_env 按模式导出成
|
||||
# SITE_HANDEYE_<字段>_EYE_IN_HAND / _EYE_TO_HAND
|
||||
# 这样"模式 → 结果文件 / 示教点 / 标定板"的对应关系只有 site.yaml 一处定义，
|
||||
# 脚本不再自己拼文件名（拼错/漏拼都是**静默**用错文件，最难查）。
|
||||
_HE_TAG="$(echo "${HANDEYE_MODE}" | tr '[:lower:]' '[:upper:]')"
|
||||
_he() {
|
||||
local name="SITE_HANDEYE_$1_${_HE_TAG}"
|
||||
printf '%s' "${!name-}"
|
||||
}
|
||||
|
||||
WP="$(_he WAYPOINTS)"
|
||||
WP="${WP:-${HE_DIR}/poses/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
|
||||
RES="$(_he FILE)"
|
||||
RES="${RES:-${HE_DIR}/results/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
|
||||
# 示教点临时文件也按模式分开：两个模式共用一份时，后一个 teach 会清掉前一个的采点，
|
||||
# 而症状只是"merge 出来的点位不对"，很难联想到是文件串了。
|
||||
TAUGHT="${WORKDIR}/taught_poses_${HANDEYE_MODE}.txt"
|
||||
|
||||
# ---- 相机：两种模式用的是**两台不同的相机** ----
|
||||
# 眼在手上 = 腕部相机（跟着臂动）；眼在手外 = 固定在工位外的相机（不随臂动）。
|
||||
# 原来这里恒用 SITE_CAM_NAME（腕部），于是 `--mode eye_to_hand` 会去开腕部相机 ——
|
||||
# 板跟着臂动、相机也跟着臂动，"标定"能跑完，但解出的是个毫无意义的错外参。
|
||||
if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
|
||||
CAM="${SITE_OVERHEAD_CAM_NAME:-}"
|
||||
if [[ -z "${CAM}" ]]; then
|
||||
echo "ERROR: eye_to_hand 需要一台固定相机，但 site.yaml 的 cameras 段里没有登记的。" >&2
|
||||
echo " 加一条 role: overhead_eye_to_hand（眼在手外、不随臂动）的相机即可。" >&2
|
||||
exit 2
|
||||
fi
|
||||
CAM_SERIAL="${SITE_OVERHEAD_CAM_SERIAL:-}"
|
||||
CAM_WIDTH="${SITE_OVERHEAD_CAM_WIDTH:-}"
|
||||
CAM_HEIGHT="${SITE_OVERHEAD_CAM_HEIGHT:-}"
|
||||
CAM_FPS="${SITE_OVERHEAD_CAM_FPS:-30}"
|
||||
CAM_AE="${SITE_OVERHEAD_CAM_AUTO_EXPOSURE:-1}"
|
||||
CAM_AE_LIMIT="${SITE_OVERHEAD_CAM_AE_LIMIT_US:-8000}"
|
||||
CAM_EMITTER="${SITE_OVERHEAD_CAM_EMITTER:-1}"
|
||||
CAM_EXPOSURE="${SITE_OVERHEAD_CAM_EXPOSURE_US:-0}"
|
||||
CAM_GAIN="${SITE_OVERHEAD_CAM_GAIN:-0}"
|
||||
else
|
||||
CAM="${SITE_CAM_NAME:-d405}"
|
||||
CAM_SERIAL="${SITE_CAM_SERIAL:-}"
|
||||
CAM_WIDTH="${SITE_CAM_WIDTH:-}"
|
||||
CAM_HEIGHT="${SITE_CAM_HEIGHT:-}"
|
||||
CAM_FPS="${SITE_CAM_FPS:-30}"
|
||||
CAM_AE="${SITE_CAM_AUTO_EXPOSURE:-1}"
|
||||
CAM_AE_LIMIT="${SITE_CAM_AE_LIMIT_US:-1800}"
|
||||
CAM_EMITTER="${SITE_CAM_EMITTER:-0}"
|
||||
CAM_EXPOSURE="${SITE_CAM_EXPOSURE_US:-0}"
|
||||
CAM_GAIN="${SITE_CAM_GAIN:-0}"
|
||||
fi
|
||||
echo "[handeye] mode=${HANDEYE_MODE} poses=${WP}"
|
||||
echo "[handeye] result=${RES}"
|
||||
echo "[handeye] camera=${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
|
||||
|
||||
_kill_servo() {
|
||||
pkill -f 'visual_servo.launch' 2>/dev/null || true
|
||||
pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
|
||||
pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
|
||||
sleep 1
|
||||
}
|
||||
|
||||
_need_files() {
|
||||
local f
|
||||
for f in "$@"; do
|
||||
if [[ ! -f "$f" ]]; then
|
||||
echo "ERROR: missing $f" >&2
|
||||
exit 2
|
||||
fi
|
||||
done
|
||||
}
|
||||
|
||||
_need_robot() {
|
||||
if ! ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
|
||||
echo "ERROR: start run_real.sh first" >&2
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
_need_camera() {
|
||||
if ! ros2 topic list 2>/dev/null | grep -q "/${CAM}/color/image_raw"; then
|
||||
echo "ERROR: no /${CAM}/color/image_raw — start run_camera_d405.sh" >&2
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
cmd_show() {
|
||||
echo "=== hand-eye (package + site) ==="
|
||||
echo " arm : ${SITE_ROBOT_SN:-} ${SITE_ROBOT_LABEL:-}"
|
||||
echo " package cfg : ${APP_CFG}"
|
||||
echo " calibrate : ${CAL}"
|
||||
echo " poses : ${WP}"
|
||||
echo " tour : ${TOUR}"
|
||||
echo " result : ${RES}"
|
||||
echo " workdir : ${WORKDIR}"
|
||||
echo " camera : ${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
echo " tool : ${SITE_ROBOT_TOOL_NAME:-（不切换）} gripper_tool=${SITE_HANDEYE_GRIPPER_TOOL:-（无，ee_pose 原样）}"
|
||||
echo " board : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m (来源=$(_he BOARD_SRC))"
|
||||
echo " （另一种模式的板见下面 export 的 标定板[...] 两行）"
|
||||
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
|
||||
if [[ -f "${WP}" ]]; then
|
||||
python3 - <<PY
|
||||
import re
|
||||
from pathlib import Path
|
||||
t = Path("${WP}").read_text(encoding="utf-8")
|
||||
m = re.search(r"num_poses:\s*(\d+)", t)
|
||||
n = int(m.group(1)) if m else "?"
|
||||
print(f" num_poses in poses yaml: {n}")
|
||||
PY
|
||||
fi
|
||||
}
|
||||
|
||||
cmd_merge() {
|
||||
local src="${1:-${TAUGHT}}"
|
||||
_need_files "${src}" "${WP}"
|
||||
python3 "${ROOT}/handeye/src/merge_handeye_poses.py" "${src}" "${WP}"
|
||||
echo "=== next: bash ~/桌面/orisys_arm/handeye/run_handeye.sh calibrate ==="
|
||||
}
|
||||
|
||||
cmd_calibrate() {
|
||||
# 先查配置（板尺寸）再看硬件：配置没配好的话，连硬件都不该去动。
|
||||
local cols="$(_he BOARD_COLS)"
|
||||
local rows="$(_he BOARD_ROWS)"
|
||||
local sq="$(_he SQUARE)"
|
||||
local src="$(_he BOARD_SRC)"
|
||||
# 板尺寸必须**真的配过**。没配就开标定 = 赌一个方格边长，而错了不会报错，
|
||||
# 只会让结果系统性偏（见 handeye/README.md 第 3 节）。所以这里直接拦下。
|
||||
if [[ -z "${src}" || "${src}" == "default" ]]; then
|
||||
echo "ERROR: 没有配置 [${HANDEYE_MODE}] 用的标定板尺寸。" >&2
|
||||
echo " 在 config/site.yaml 的 handeye.boards.${HANDEYE_MODE} 里写：" >&2
|
||||
echo " cols / rows / square_size_m" >&2
|
||||
echo " （内角点 = 方格数 − 1；测一下实际方格边长，别照抄另一块板。）" >&2
|
||||
exit 2
|
||||
fi
|
||||
_need_files "${CAL}" "${WP}"
|
||||
local grip_tool="${SITE_HANDEYE_GRIPPER_TOOL:-}"
|
||||
local grip_xyz="${SITE_HANDEYE_GRIPPER_TCP_XYZ:-}"
|
||||
local grip_q="${SITE_HANDEYE_GRIPPER_TCP_QWXYZ:-}"
|
||||
if [[ -n "${grip_tool}" ]]; then
|
||||
if [[ "${SITE_ROBOT_TOOL_NAME:-}" != "${grip_tool}" ]]; then
|
||||
echo "ERROR: 手眼要按工具 ${grip_tool} 求解，但 site 当前激活工具是 '${SITE_ROBOT_TOOL_NAME:-}'。" >&2
|
||||
echo " robots[].tool_name 必须和 handeye.gripper_tool 同一份名字。" >&2
|
||||
exit 2
|
||||
fi
|
||||
if [[ -z "${grip_xyz}" || -z "${grip_q}" ]]; then
|
||||
echo "ERROR: 配了 gripper_tool=${grip_tool} 但没写 gripper_tcp_xyz / gripper_tcp_qwxyz。" >&2
|
||||
exit 2
|
||||
fi
|
||||
fi
|
||||
_need_robot
|
||||
_need_camera
|
||||
echo "=== kill visual_servo (avoid traj fight) ==="
|
||||
_kill_servo
|
||||
mkdir -p "${WORKDIR}"
|
||||
local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
|
||||
local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
|
||||
local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"
|
||||
local qd="${SITE_HANDEYE_SETTLE_QD:-0.015}"
|
||||
local ss="${SITE_HANDEYE_SETTLE_STABLE:-0.6}"
|
||||
local sto="${SITE_HANDEYE_SETTLE_TO:-8.0}"
|
||||
local ph="${SITE_HANDEYE_PIN_HOLDS:-2}"
|
||||
local ps="${SITE_HANDEYE_PIN_SEC:-0.45}"
|
||||
local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}"
|
||||
echo "=== calibrate mode=${HANDEYE_MODE} ==="
|
||||
echo " board ${cols}x${rows} inner, square=${sq} m (site.yaml handeye.boards.${HANDEYE_MODE}，来源=${src})"
|
||||
echo " camera ${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
|
||||
echo " poses=${WP}"
|
||||
echo " result → ${RES}"
|
||||
local grip_args=()
|
||||
if [[ -n "${grip_tool}" ]]; then
|
||||
if [[ "${HANDEYE_MODE}" == "eye_in_hand" ]]; then
|
||||
echo " gripper_tool=${grip_tool} tcp_xyz=[${grip_xyz}] （AX=XB 用 TCP，T_4x4 折成 cam→法兰）"
|
||||
else
|
||||
echo " gripper_tool=${grip_tool} tcp_xyz=[${grip_xyz}] （AX=XB 用 TCP，T_4x4 仍是 cam→基座，不折）"
|
||||
fi
|
||||
grip_args+=(
|
||||
-p gripper_tool:="${grip_tool}"
|
||||
-p gripper_tcp_xyz:="[${grip_xyz}]"
|
||||
-p gripper_tcp_qwxyz:="[${grip_q}]"
|
||||
)
|
||||
fi
|
||||
exec python3 "${ROOT}/handeye/src/handeye_calibrate.py" --ros-args \
|
||||
--params-file "${CAL}" \
|
||||
--params-file "${WP}" \
|
||||
-p joint_prefix:="${PREFIX}" \
|
||||
-p camera_source:="${CAM}" \
|
||||
${CAM_SERIAL:+-p camera_serial:="${CAM_SERIAL}"} \
|
||||
-p output_dir:="${WORKDIR}" \
|
||||
-p result_file:="${RES}" \
|
||||
-p mode:="${HANDEYE_MODE}" \
|
||||
-p board_cols:="${cols}" \
|
||||
-p board_rows:="${rows}" \
|
||||
-p square_size_m:="${sq}" \
|
||||
-p velocity_scale:="${vel}" \
|
||||
-p move_time_sec:="${mt}" \
|
||||
-p settle_sec:="${st}" \
|
||||
-p settle_qd_max:="${qd}" \
|
||||
-p settle_stable_sec:="${ss}" \
|
||||
-p settle_timeout_sec:="${sto}" \
|
||||
-p pin_holds:="${ph}" \
|
||||
-p pin_hold_sec:="${ps}" \
|
||||
-p skip_unstable_poses:="${skip}" \
|
||||
"${grip_args[@]}"
|
||||
}
|
||||
|
||||
cmd_waypoints() {
|
||||
_need_files "${TOUR}" "${WP}"
|
||||
_need_robot
|
||||
_need_camera
|
||||
echo "=== kill visual_servo ==="
|
||||
_kill_servo
|
||||
mkdir -p "${WORKDIR}"
|
||||
echo "=== waypoint tour (Enter=confirm, q=quit) ==="
|
||||
echo " saves confirmed joints → ${TAUGHT}"
|
||||
echo " then: bash ~/桌面/orisys_arm/handeye/run_handeye.sh merge"
|
||||
handeye_node handeye_waypoints \
|
||||
--params-file "${TOUR}" \
|
||||
--params-file "${WP}" \
|
||||
-p joint_prefix:="${PREFIX}" \
|
||||
-p camera_source:="${CAM}" \
|
||||
-p append_poses_file:="${TAUGHT}"
|
||||
}
|
||||
|
||||
cmd_teach() {
|
||||
local backend="rdk"
|
||||
local append=false
|
||||
local points=0
|
||||
local auto_solve=false
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
--rdk) backend="rdk"; shift ;;
|
||||
--ros|--pendant) backend="ros"; shift ;;
|
||||
--append|--keep) append=true; shift ;;
|
||||
--points) points="${2:?--points 需要一个数字}"; shift 2 ;;
|
||||
--auto-solve) auto_solve=true; shift ;;
|
||||
*) echo "Unknown teach arg: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
echo "=== kill visual_servo + camera (D405 exclusive) ==="
|
||||
_kill_servo
|
||||
pkill -f 'run_camera_d405' 2>/dev/null || true
|
||||
pkill -f 'realsense_color_pub' 2>/dev/null || true
|
||||
pkill -f 'realsense2_camera_node' 2>/dev/null || true
|
||||
sleep 1
|
||||
|
||||
if [[ "${backend}" == "rdk" ]]; then
|
||||
if ros2 service list 2>/dev/null | grep -q '/set_named_pose'; then
|
||||
echo "ERROR: RDK 拖拽与 run_real.sh 互斥。请先在那个终端 Ctrl-C 停掉 run_real.sh" >&2
|
||||
exit 1
|
||||
fi
|
||||
else
|
||||
_need_robot
|
||||
fi
|
||||
|
||||
mkdir -p "${WORKDIR}/images"
|
||||
if [[ "${append}" == true ]]; then
|
||||
echo "=== append: keep existing ${TAUGHT} ==="
|
||||
else
|
||||
# 重新设点：丢掉上次 taught，避免 merge 混进旧位姿
|
||||
: > "${TAUGHT}"
|
||||
fi
|
||||
echo "=== interactive teach backend=${backend} → ${WORKDIR} ==="
|
||||
echo " 窗口按键: d=拖拽开 s=拖拽关 Enter=拍照记点 q=结束"
|
||||
if [[ "${points}" -gt 0 ]] 2>/dev/null; then
|
||||
echo " 目标 ${points} 个点（采够会有提示，仍可继续多采）"
|
||||
else
|
||||
echo " 建议 8–12 个姿态，棋盘占画面约 30–70%"
|
||||
echo " （也可用 --points N 设定目标点数）"
|
||||
fi
|
||||
echo " 结束后: bash ~/桌面/orisys_arm/handeye/run_handeye.sh merge"
|
||||
echo " bash ~/桌面/orisys_arm/handeye/run_handeye.sh calibrate"
|
||||
if [[ "${auto_solve}" == true ]]; then
|
||||
echo " --auto-solve: 采完会自动 merge，并视后端决定是否直接标定"
|
||||
fi
|
||||
TEACH_ARGS=(
|
||||
-p backend:="${backend}"
|
||||
-p robot_sn:="${SITE_ROBOT_SN:?手眼标定必须知道是哪台臂}"
|
||||
-p joint_prefix:="${PREFIX}"
|
||||
-p camera_source:="${CAM}"
|
||||
-p output_dir:="${WORKDIR}"
|
||||
-p poses_file:="${TAUGHT}"
|
||||
-p target_points:="${points}"
|
||||
)
|
||||
if [[ -n "${CAM_SERIAL}" ]]; then
|
||||
TEACH_ARGS+=(-p camera_serial:="${CAM_SERIAL}")
|
||||
fi
|
||||
if [[ -n "${CAM_WIDTH}" ]]; then
|
||||
TEACH_ARGS+=(-p width:="${CAM_WIDTH}" -p height:="${CAM_HEIGHT:-480}" -p fps:="${CAM_FPS:-30}")
|
||||
fi
|
||||
TEACH_ARGS+=(
|
||||
-p auto_exposure:="$([ "${CAM_AE:-1}" = "1" ] && echo true || echo false)"
|
||||
-p ae_limit_us:="${CAM_AE_LIMIT:-1800}"
|
||||
-p emitter_enabled:="$([ "${CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
|
||||
-p exposure_us:="${CAM_EXPOSURE:-0}"
|
||||
-p gain:="${CAM_GAIN:-0}"
|
||||
)
|
||||
python3 "${ROOT}/handeye/src/handeye_teach.py" --ros-args "${TEACH_ARGS[@]}" || true
|
||||
|
||||
# ---- 采点结束 ----
|
||||
if [[ "${auto_solve}" != true ]]; then
|
||||
echo
|
||||
echo "采点结束。接着跑（或下次加 --auto-solve 让它自动接）："
|
||||
echo " bash ~/桌面/orisys_arm/handeye/run_handeye.sh merge --arm ${SITE_ROBOT_ARM:-89}"
|
||||
echo " bash ~/桌面/orisys_arm/handeye/run_handeye.sh calibrate --arm ${SITE_ROBOT_ARM:-89}"
|
||||
return 0
|
||||
fi
|
||||
|
||||
echo
|
||||
echo "=== --auto-solve: 自动 merge ==="
|
||||
if [[ ! -s "${TAUGHT}" ]]; then
|
||||
echo "ERROR: 没采到任何点（${TAUGHT} 为空），中止。" >&2
|
||||
return 1
|
||||
fi
|
||||
if ! python3 "${ROOT}/handeye/src/merge_handeye_poses.py" "${TAUGHT}" "${WP}"; then
|
||||
echo "ERROR: merge 失败，点还在 ${TAUGHT}，可手动重跑 merge。" >&2
|
||||
return 1
|
||||
fi
|
||||
echo " 已写入 ${WP}"
|
||||
|
||||
# RDK 软拖拽与控制器互斥：软拖拽要 RDK 独占，标定必须在 run_real.sh 的
|
||||
# 控制器上走点位，两者无法同时存在。这是硬件约束，脚本绕不过去，
|
||||
# 所以这里如实停下并给出下一步，而不是硬跑一个必然失败的 calibrate。
|
||||
if [[ "${backend}" == "rdk" ]]; then
|
||||
echo
|
||||
echo "⚠️ 当前是 RDK 拖拽模式（--rdk），无法自动接着求解。"
|
||||
echo " 原因：软拖拽需要 RDK 独占，而标定需要通过 run_real.sh 的控制器走点位，"
|
||||
echo " 两者不能同时存在。"
|
||||
echo
|
||||
echo " 继续两步即可："
|
||||
echo " 1) 另开终端: bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-89}"
|
||||
echo " 2) 回到这里: bash ~/桌面/orisys_arm/handeye/run_handeye.sh calibrate --arm ${SITE_ROBOT_ARM:-89}"
|
||||
echo
|
||||
echo " 想真正一条命令跑完，请改用示教器 Free Drive 模式："
|
||||
echo " bash ~/桌面/orisys_arm/handeye/run_handeye.sh teach --ros --auto-solve"
|
||||
return 0
|
||||
fi
|
||||
|
||||
# --ros（示教器）后端：控制器本来就在跑，可以无缝接上标定。
|
||||
# 但 teach 阶段把相机杀掉了（USB 独占），标定拍照要用，得先拉回来。
|
||||
echo
|
||||
echo "=== 重启相机（teach 阶段杀掉了它，标定拍照需要）==="
|
||||
if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
|
||||
# 眼在手外用的是固定相机（D435），不是腕部 D405 —— 拉错相机的话，
|
||||
# 标定会去订一个没人发布的图，表现是"相机没数据"。
|
||||
_cam_cmd=(bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview)
|
||||
else
|
||||
_cam_cmd=(bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" --no-preview)
|
||||
fi
|
||||
nohup "${_cam_cmd[@]}" > /tmp/run_handeye_camera.log 2>&1 &
|
||||
echo " pid=$! log=/tmp/run_handeye_camera.log"
|
||||
# _need_camera 内部失败时会自己打印原因并 exit，不需要在这里再判一次。
|
||||
_need_camera
|
||||
|
||||
echo
|
||||
echo "=== --auto-solve: 自动 calibrate ==="
|
||||
cmd_calibrate
|
||||
}
|
||||
|
||||
case "${CMD}" in
|
||||
calibrate|calib) cmd_calibrate ;;
|
||||
waypoints|tour) cmd_waypoints "$@" ;;
|
||||
teach) cmd_teach "$@" ;;
|
||||
merge) cmd_merge "$@" ;;
|
||||
show) cmd_show ;;
|
||||
-h|--help|help|"") usage; [[ -n "${CMD}" ]] || exit 0; exit 0 ;;
|
||||
*) echo "Unknown: ${CMD}" >&2; usage >&2; exit 1 ;;
|
||||
esac
|
||||
@@ -0,0 +1,771 @@
|
||||
#!/usr/bin/env python3
|
||||
"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。
|
||||
|
||||
支持 eye_in_hand / eye_to_hand。
|
||||
|
||||
相机内参优先级：
|
||||
1) ROS 的 CameraInfo 话题（若正在发布）
|
||||
2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source）
|
||||
3) YAML 中手写的 camera_fx/fy/cx/cy
|
||||
|
||||
棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import shutil
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image, JointState
|
||||
from std_msgs.msg import String
|
||||
|
||||
from arm_application.handeye_lib import (
|
||||
BoardConfig,
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
T_from_flexiv_tcp,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
detect_chessboard,
|
||||
format_matrix_yaml,
|
||||
format_transform_yaml,
|
||||
is_pose_filled,
|
||||
pose_to_Rt,
|
||||
solve_board_pose,
|
||||
)
|
||||
from arm_application.handeye_realsense import (
|
||||
is_realsense_source,
|
||||
query_realsense_color_intrinsics,
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
|
||||
|
||||
def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or '').lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
|
||||
if enc in ('bgr8', 'rgb8'):
|
||||
img = data.reshape(h, w, 3)
|
||||
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
|
||||
if enc in ('mono8', '8uc1'):
|
||||
gray = data.reshape(h, w)
|
||||
return np.stack([gray, gray, gray], axis=-1)
|
||||
if enc in ('bgra8', 'rgba8'):
|
||||
img = data.reshape(h, w, 4)
|
||||
bgr = img[:, :, :3]
|
||||
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
|
||||
K = np.array(msg.k, dtype=float).reshape(3, 3)
|
||||
dist = np.array(msg.d, dtype=float).reshape(-1, 1) if msg.d else np.zeros((5, 1))
|
||||
return CameraIntrinsics(K=K, dist=dist)
|
||||
|
||||
|
||||
class HandeyeCalibrate(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_calibrate')
|
||||
|
||||
self.declare_parameter('mode', 'eye_in_hand') # eye_in_hand | eye_to_hand
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.10)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('move_time_sec', 4.0)
|
||||
self.declare_parameter('start_index', 0)
|
||||
|
||||
self.declare_parameter('num_poses', 12)
|
||||
self.declare_parameter('waypoint_positions', [0.0] * (12 * 7))
|
||||
self.declare_parameter('min_poses', 5)
|
||||
|
||||
# 相机来源：manual | d405 | d435
|
||||
self.declare_parameter('camera_source', 'manual')
|
||||
self.declare_parameter('camera_serial', '')
|
||||
self.declare_parameter('camera_namespace', '')
|
||||
self.declare_parameter('realsense_width', 1280)
|
||||
self.declare_parameter('realsense_height', 720)
|
||||
self.declare_parameter('image_topic', '')
|
||||
self.declare_parameter('camera_info_topic', '')
|
||||
self.declare_parameter('camera_fx', 0.0)
|
||||
self.declare_parameter('camera_fy', 0.0)
|
||||
self.declare_parameter('camera_cx', 0.0)
|
||||
self.declare_parameter('camera_cy', 0.0)
|
||||
self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
|
||||
# 棋盘（可自定义）
|
||||
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
|
||||
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
|
||||
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
|
||||
self.declare_parameter('board_cols', 11) # 内角点
|
||||
self.declare_parameter('board_rows', 8)
|
||||
self.declare_parameter('square_size_m', 0.015)
|
||||
|
||||
self.declare_parameter('stable_window', 8)
|
||||
self.declare_parameter('stable_pixel_std', 0.45)
|
||||
self.declare_parameter('stable_timeout_sec', 25.0)
|
||||
self.declare_parameter('settle_sec', 0.4)
|
||||
# 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。
|
||||
# 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明，
|
||||
# ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现，
|
||||
# 实际只靠一次固定 settle_sec。现在补上。
|
||||
self.declare_parameter('settle_qd_max', 0.015) # |qd|_max [rad/s] 低于此值算静
|
||||
self.declare_parameter('settle_stable_sec', 0.6) # 需要连续静多久
|
||||
self.declare_parameter('settle_timeout_sec', 8.0) # 等静止的总超时
|
||||
self.declare_parameter('pin_holds', 2) # 静止后再保持几拍
|
||||
self.declare_parameter('pin_hold_sec', 0.45) # 每拍多长
|
||||
self.declare_parameter('skip_unstable_poses', True)
|
||||
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
|
||||
self.declare_parameter('result_file', '')
|
||||
self.declare_parameter('cleanup_images', True)
|
||||
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
|
||||
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
|
||||
self.declare_parameter('gripper_tool', '')
|
||||
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
|
||||
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.mode = str(self.get_parameter('mode').value).strip().lower()
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.action_name = str(self.get_parameter('action_name').value)
|
||||
self.move_time_sec = float(self.get_parameter('move_time_sec').value)
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.min_poses = int(self.get_parameter('min_poses').value)
|
||||
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
|
||||
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
|
||||
tcp_xyz = list(
|
||||
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
|
||||
or [0.0, 0.0, 0.0]
|
||||
)
|
||||
tcp_q = list(
|
||||
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
|
||||
or [1.0, 0.0, 0.0, 0.0]
|
||||
)
|
||||
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
|
||||
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
|
||||
raise RuntimeError(
|
||||
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
|
||||
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
|
||||
)
|
||||
|
||||
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
|
||||
self.camera_namespace = str(
|
||||
self.get_parameter('camera_namespace').value or ''
|
||||
).strip()
|
||||
self.rs_width = int(self.get_parameter('realsense_width').value)
|
||||
self.rs_height = int(self.get_parameter('realsense_height').value)
|
||||
|
||||
self.board = BoardConfig(
|
||||
cols=int(self.get_parameter('board_cols').value),
|
||||
rows=int(self.get_parameter('board_rows').value),
|
||||
square_size=float(self.get_parameter('square_size_m').value),
|
||||
)
|
||||
self.stabilizer = ChessboardStabilizer(
|
||||
window=int(self.get_parameter('stable_window').value),
|
||||
max_pixel_std=float(self.get_parameter('stable_pixel_std').value),
|
||||
)
|
||||
self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value)
|
||||
self.settle_sec = float(self.get_parameter('settle_sec').value)
|
||||
self.settle_qd_max = float(self.get_parameter('settle_qd_max').value)
|
||||
self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value)
|
||||
self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value)
|
||||
self.pin_holds = max(0, int(self.get_parameter('pin_holds').value))
|
||||
self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value))
|
||||
self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value)
|
||||
|
||||
out = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.output_dir = out
|
||||
self.images_dir = out / 'images'
|
||||
rf = str(self.get_parameter('result_file').value or '').strip()
|
||||
self.result_file = Path(rf).expanduser() if rf else (out / 'handeye_result.yaml')
|
||||
|
||||
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
self._capture_pub = self.create_publisher(String, 'handeye/capture', 10)
|
||||
|
||||
self._latest_image: Optional[Image] = None
|
||||
self._latest_info: Optional[CameraInfo] = None
|
||||
self._latest_js: Optional[JointState] = None
|
||||
self._intrinsics: Optional[CameraIntrinsics] = None
|
||||
|
||||
img_topic = str(self.get_parameter('image_topic').value or '').strip()
|
||||
info_topic = str(self.get_parameter('camera_info_topic').value or '').strip()
|
||||
if is_realsense_source(self.camera_source):
|
||||
img_topic, info_topic = resolve_realsense_topics(
|
||||
self.camera_source,
|
||||
camera_namespace=self.camera_namespace,
|
||||
image_topic=img_topic,
|
||||
camera_info_topic=info_topic,
|
||||
)
|
||||
self._info_topic = info_topic
|
||||
|
||||
self.create_subscription(JointState, '/joint_states', self._on_js, 10)
|
||||
|
||||
if not img_topic:
|
||||
self.get_logger().error(
|
||||
'image_topic 为空。请设置 camera_source:=d405|d435，或填写 image_topic。'
|
||||
)
|
||||
else:
|
||||
self.create_subscription(
|
||||
Image, img_topic, self._on_image, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'已订阅图像：{img_topic}（BEST_EFFORT）')
|
||||
if info_topic:
|
||||
self.create_subscription(
|
||||
CameraInfo, info_topic, self._on_info, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）')
|
||||
|
||||
def _on_js(self, msg: JointState) -> None:
|
||||
self._latest_js = msg
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
self._latest_image = msg
|
||||
|
||||
def _on_info(self, msg: CameraInfo) -> None:
|
||||
self._latest_info = msg
|
||||
|
||||
def run(self) -> int:
|
||||
if self.mode not in ('eye_in_hand', 'eye_to_hand'):
|
||||
self.get_logger().error(
|
||||
f'未知的 mode={self.mode!r}；请使用 eye_in_hand 或 eye_to_hand'
|
||||
)
|
||||
return 1
|
||||
|
||||
poses = self._load_poses()
|
||||
if len(poses) < self.min_poses:
|
||||
self.get_logger().error(
|
||||
f'只有 {len(poses)} 个已填写位姿（需要 >= {self.min_poses}）。'
|
||||
'示教命令：bash ~/ros2/scripts/run_handeye.sh teach && merge。'
|
||||
)
|
||||
return 1
|
||||
if self.start_index >= len(poses):
|
||||
self.get_logger().error(
|
||||
f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}'
|
||||
)
|
||||
return 1
|
||||
|
||||
if not self._resolve_intrinsics():
|
||||
return 1
|
||||
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self.action_name}] 不可用。'
|
||||
'请启动：bash ~/ros2/scripts/run_real.sh'
|
||||
)
|
||||
return 2
|
||||
|
||||
self.output_dir.mkdir(parents=True, exist_ok=True)
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
eye_to_hand = self.mode == 'eye_to_hand'
|
||||
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
|
||||
self.get_logger().info(
|
||||
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
|
||||
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale} '
|
||||
f'gripper_tool={grip}'
|
||||
)
|
||||
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
|
||||
|
||||
Rg: List[np.ndarray] = []
|
||||
tg: List[np.ndarray] = []
|
||||
Rc: List[np.ndarray] = []
|
||||
tc: List[np.ndarray] = []
|
||||
meta = []
|
||||
|
||||
try:
|
||||
for i in range(self.start_index, len(poses)):
|
||||
q = poses[i]
|
||||
self._log_progress(i, len(poses), '移动中...')
|
||||
code = self._move_one(q, self.move_time_sec)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
if not self._verify_joints(q, tol=0.08):
|
||||
self.get_logger().error(
|
||||
f'[{i + 1}] 机械臂未到达目标关节位置 '
|
||||
'（控制器报告成功，但关节没有变化）。'
|
||||
'请重启 run_real.sh；示教器切 AUTO Remote；清除故障。'
|
||||
'不要相信这次标定结果。'
|
||||
)
|
||||
return 10
|
||||
|
||||
end = time.monotonic() + max(0.0, self.settle_sec)
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
|
||||
# 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。
|
||||
self._wait_settled()
|
||||
self._pin_hold()
|
||||
|
||||
ok, std, corners, pattern, bgr = self._wait_stable()
|
||||
if not ok or corners is None or bgr is None:
|
||||
msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）'
|
||||
if self.skip_unstable:
|
||||
self.get_logger().warn(f'{msg} —— 跳过')
|
||||
continue
|
||||
self.get_logger().error(f'{msg}。中止。')
|
||||
return 7
|
||||
|
||||
ee = self._read_ee_pose()
|
||||
if ee is None:
|
||||
self.get_logger().error('读取 ee_pose 失败')
|
||||
return 8
|
||||
|
||||
assert self._intrinsics is not None
|
||||
R_t2c, t_t2c = solve_board_pose(
|
||||
corners, self.board, self._intrinsics, pattern=pattern
|
||||
)
|
||||
xyz = [ee.pose.position.x, ee.pose.position.y, ee.pose.position.z]
|
||||
quat = [
|
||||
ee.pose.orientation.x,
|
||||
ee.pose.orientation.y,
|
||||
ee.pose.orientation.z,
|
||||
ee.pose.orientation.w,
|
||||
]
|
||||
R_g2b, t_g2b = pose_to_Rt(xyz, quat)
|
||||
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
img_path = self.images_dir / f'pose_{i:02d}_{stamp}.png'
|
||||
self._save_png(bgr, img_path)
|
||||
|
||||
msg = String()
|
||||
msg.data = f'capture:{i}:{len(poses)}'
|
||||
self._capture_pub.publish(msg)
|
||||
|
||||
Rg.append(R_g2b)
|
||||
tg.append(t_g2b)
|
||||
Rc.append(R_t2c)
|
||||
tc.append(t_t2c)
|
||||
meta.append(
|
||||
{
|
||||
'index': i,
|
||||
'joints': list(q),
|
||||
'ee_xyz': xyz,
|
||||
'ee_quat_xyzw': quat,
|
||||
'image': str(img_path),
|
||||
'corner_std_px': std,
|
||||
}
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{len(poses)}] 已采集 '
|
||||
f'(corner_std={std:.3f}px) -> {img_path.name}'
|
||||
)
|
||||
except KeyboardInterrupt:
|
||||
self.get_logger().info('已中断')
|
||||
return 0
|
||||
|
||||
if len(Rg) < self.min_poses:
|
||||
self.get_logger().error(
|
||||
f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次'
|
||||
)
|
||||
return 9
|
||||
|
||||
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
|
||||
extra = ''
|
||||
if eye_to_hand:
|
||||
name = 'T_cam_to_base (eye_to_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
if self.gripper_tool:
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
|
||||
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, :3] = R
|
||||
T_cam_tool[:3, 3] = t.reshape(3)
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
|
||||
if tool_len > 0.05 and delta < 1e-4:
|
||||
self.get_logger().error(
|
||||
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
|
||||
'折算没生效。结果作废。'
|
||||
)
|
||||
return 11
|
||||
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
|
||||
if (
|
||||
self.T_flange_to_tool[2, 3] > 0.05
|
||||
and float(T_cam_flange[2, 3]) < 0.0
|
||||
):
|
||||
self.get_logger().error(
|
||||
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
|
||||
'在法兰后方，乘序反了。结果作废。'
|
||||
)
|
||||
return 12
|
||||
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
|
||||
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：'
|
||||
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
|
||||
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
|
||||
)
|
||||
else:
|
||||
name = 'T_cam_to_gripper (eye_in_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
header = (
|
||||
f'# 手眼标定结果\n'
|
||||
f'# mode: {self.mode}\n'
|
||||
f'# camera_source: {self.camera_source}\n'
|
||||
f'# views: {len(Rg)}\n'
|
||||
f'# utc: {datetime.now(timezone.utc).isoformat()}\n'
|
||||
f'# board: {self.board.cols}x{self.board.rows} '
|
||||
f'square_m={self.board.square_size}\n'
|
||||
)
|
||||
if self.gripper_tool and eye_to_hand:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
|
||||
f'# 旧结果备份在 results/backup/\n'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
|
||||
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
|
||||
)
|
||||
self.result_file.parent.mkdir(parents=True, exist_ok=True)
|
||||
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
|
||||
(self.output_dir / 'captures.json').write_text(
|
||||
json.dumps(meta, indent=2), encoding='utf-8'
|
||||
)
|
||||
|
||||
self.get_logger().info('======== 手眼标定结果 ========')
|
||||
for line in (header + yaml_txt).strip().splitlines():
|
||||
self.get_logger().info(line)
|
||||
self.get_logger().info(f'已保存 -> {self.result_file}')
|
||||
|
||||
if self.cleanup_images:
|
||||
if self.images_dir.exists():
|
||||
shutil.rmtree(self.images_dir, ignore_errors=True)
|
||||
self.get_logger().info('标定图像已删除')
|
||||
else:
|
||||
self.get_logger().info(f'图像保留在 {self.images_dir}')
|
||||
return 0
|
||||
|
||||
def _load_poses(self) -> List[List[float]]:
|
||||
flat = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_cfg = int(self.get_parameter('num_poses').value)
|
||||
if n_cfg < 1:
|
||||
return []
|
||||
need = n_cfg * n_j
|
||||
if len(flat) < need:
|
||||
self.get_logger().warn(
|
||||
f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；'
|
||||
'用零补齐'
|
||||
)
|
||||
flat = flat + [0.0] * (need - len(flat))
|
||||
poses: List[List[float]] = []
|
||||
for i in range(n_cfg):
|
||||
q = [float(v) for v in flat[i * n_j : (i + 1) * n_j]]
|
||||
if is_pose_filled(q):
|
||||
poses.append(q)
|
||||
else:
|
||||
self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过')
|
||||
return poses
|
||||
|
||||
def _resolve_intrinsics(self) -> bool:
|
||||
# 1) CameraInfo
|
||||
deadline = time.monotonic() + 3.0
|
||||
while rclpy.ok() and self._latest_info is None and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.1)
|
||||
|
||||
if self._latest_info is not None:
|
||||
self._intrinsics = _camera_info_to_intrinsics(self._latest_info)
|
||||
K = self._intrinsics.K
|
||||
self.get_logger().info(
|
||||
f'内参来自 CameraInfo（{self._info_topic}）：'
|
||||
f'fx={K[0, 0]:.2f} fy={K[1, 1]:.2f} cx={K[0, 2]:.2f} cy={K[1, 2]:.2f}'
|
||||
)
|
||||
return True
|
||||
|
||||
# 2) RealSense EEPROM
|
||||
if is_realsense_source(self.camera_source):
|
||||
try:
|
||||
intr = query_realsense_color_intrinsics(
|
||||
self.camera_source,
|
||||
serial=self.camera_serial,
|
||||
width=self.rs_width,
|
||||
height=self.rs_height,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'RealSense 内参查询失败：{exc}')
|
||||
else:
|
||||
self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
|
||||
self.get_logger().info(
|
||||
f'内参来自 RealSense {intr.model.upper()} '
|
||||
f'serial={intr.serial} {intr.width}x{intr.height}：'
|
||||
f'fx={intr.fx:.2f} fy={intr.fy:.2f} '
|
||||
f'cx={intr.cx:.2f} cy={intr.cy:.2f}'
|
||||
)
|
||||
if (intr.width, intr.height) != (self.rs_width, self.rs_height):
|
||||
self.get_logger().warn(
|
||||
f'请求 {self.rs_width}x{self.rs_height}，'
|
||||
f'实际得到 {intr.width}x{intr.height}（最接近的 profile）'
|
||||
)
|
||||
return True
|
||||
|
||||
# 3) Manual YAML
|
||||
fx = float(self.get_parameter('camera_fx').value)
|
||||
fy = float(self.get_parameter('camera_fy').value)
|
||||
cx = float(self.get_parameter('camera_cx').value)
|
||||
cy = float(self.get_parameter('camera_cy').value)
|
||||
if fx <= 0 or fy <= 0:
|
||||
self.get_logger().error(
|
||||
'没有内参。请设置 camera_source:=d405|d435，'
|
||||
'或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。'
|
||||
)
|
||||
return False
|
||||
dist = np.array(
|
||||
list(self.get_parameter('camera_dist').value), dtype=float
|
||||
).reshape(-1, 1)
|
||||
K = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], dtype=float)
|
||||
self._intrinsics = CameraIntrinsics(K=K, dist=dist)
|
||||
self.get_logger().info(
|
||||
f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
|
||||
)
|
||||
return True
|
||||
|
||||
def _log_progress(self, i: int, total: int, suffix: str = '') -> None:
|
||||
"""带进度条的日志。
|
||||
|
||||
自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照），
|
||||
单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。
|
||||
"""
|
||||
frac = (i + 1) / max(1, total)
|
||||
width = 24
|
||||
filled = int(round(frac * width))
|
||||
bar = '█' * filled + '░' * (width - filled)
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}% {suffix}'
|
||||
)
|
||||
|
||||
def _wait_settled(self) -> bool:
|
||||
"""等机械臂真的停下来，再进入拍照。
|
||||
|
||||
判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。
|
||||
为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同，
|
||||
刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动），
|
||||
等太长则整轮标定被无谓拉长。直接看速度更准。
|
||||
|
||||
超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。
|
||||
"""
|
||||
if self.settle_qd_max <= 0.0:
|
||||
return True
|
||||
deadline = time.monotonic() + self.settle_timeout
|
||||
stable_since: Optional[float] = None
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
js = self._latest_js
|
||||
if js is None or not list(js.velocity):
|
||||
continue
|
||||
try:
|
||||
vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float))))
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
now = time.monotonic()
|
||||
if vmax <= self.settle_qd_max:
|
||||
if stable_since is None:
|
||||
stable_since = now
|
||||
elif now - stable_since >= self.settle_stable_sec:
|
||||
return True
|
||||
else:
|
||||
stable_since = None # 又动了，重新计时
|
||||
self.get_logger().warn(
|
||||
f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；'
|
||||
'交给棋盘稳定性检查兜底'
|
||||
)
|
||||
return False
|
||||
|
||||
def _pin_hold(self) -> None:
|
||||
"""静止后再保持几拍，抑制到位后的微小回弹/漂移。
|
||||
|
||||
语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹，
|
||||
而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。
|
||||
"""
|
||||
total = self.pin_holds * self.pin_hold_sec
|
||||
end = time.monotonic() + total
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
|
||||
def _wait_stable(
|
||||
self,
|
||||
) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]:
|
||||
self.stabilizer.reset()
|
||||
deadline = time.monotonic() + self.stable_timeout
|
||||
last_std = float('inf')
|
||||
last_corners = None
|
||||
last_pattern = None
|
||||
last_bgr = None
|
||||
last_log = 0.0
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if self._latest_image is None:
|
||||
continue
|
||||
try:
|
||||
bgr = _image_to_bgr(self._latest_image)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'图像转换失败：{exc}')
|
||||
return False, last_std, None, None, None
|
||||
detected = detect_chessboard(bgr, self.board)
|
||||
corners = detected[0] if detected is not None else None
|
||||
pattern = detected[1] if detected is not None else None
|
||||
ok, std = self.stabilizer.push(corners)
|
||||
last_std = std
|
||||
if corners is not None:
|
||||
last_corners = corners
|
||||
last_pattern = pattern
|
||||
last_bgr = bgr
|
||||
now = time.monotonic()
|
||||
if now - last_log > 1.0:
|
||||
seen = '有' if corners is not None else '无'
|
||||
self.get_logger().info(
|
||||
f' 棋盘={seen} 角点标准差='
|
||||
f'{std if std < 1e8 else float("nan"):.3f}px'
|
||||
)
|
||||
last_log = now
|
||||
if ok:
|
||||
return True, std, last_corners, last_pattern, last_bgr
|
||||
return False, last_std, last_corners, last_pattern, last_bgr
|
||||
|
||||
|
||||
def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool:
|
||||
"""确认 ``joint_states`` 真的走到了目标位置。
|
||||
|
||||
用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动
|
||||
（或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度），
|
||||
只有全部落在容差内才算真到位。
|
||||
"""
|
||||
deadline = time.monotonic() + 2.0
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if self._latest_js is None:
|
||||
continue
|
||||
name_to_q = dict(zip(list(self._latest_js.name), list(self._latest_js.position)))
|
||||
actual = []
|
||||
for n in self.joint_names:
|
||||
if n not in name_to_q:
|
||||
break
|
||||
actual.append(float(name_to_q[n]))
|
||||
if len(actual) != len(target):
|
||||
continue
|
||||
err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)])))
|
||||
self.get_logger().info(
|
||||
f' 关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）'
|
||||
)
|
||||
return err <= tol
|
||||
self.get_logger().error('校验运动时没有 joint_states')
|
||||
return False
|
||||
|
||||
def _move_one(self, positions: List[float], duration: float) -> int:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = 1
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = list(positions)
|
||||
goal.time_from_start = [float(duration)]
|
||||
|
||||
send = self._client.send_goal_async(goal)
|
||||
if not self._wait_future(send, 10.0, '发送目标'):
|
||||
return 3
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 4
|
||||
res_fut = handle.get_result_async()
|
||||
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
|
||||
return 5
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'移动失败：{result.message}')
|
||||
return 6
|
||||
return 0
|
||||
|
||||
def _wait_future(self, fut, timeout: float, label: str) -> bool:
|
||||
deadline = time.monotonic() + timeout
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'等待 {label} 超时')
|
||||
return False
|
||||
return fut.done()
|
||||
|
||||
def _read_ee_pose(self) -> Optional[PoseStamped]:
|
||||
if not self._get_state.wait_for_service(timeout_sec=2.0):
|
||||
return None
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
if not self._wait_future(fut, 2.0, 'get_arm_state'):
|
||||
return None
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
return res.ee_pose
|
||||
|
||||
def _save_png(self, bgr: np.ndarray, path: Path) -> None:
|
||||
import cv2
|
||||
|
||||
if not cv2.imwrite(str(path), bgr):
|
||||
raise RuntimeError(f'cv2.imwrite 失败：{path}')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HandeyeCalibrate()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,679 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。
|
||||
|
||||
两种后端
|
||||
------------
|
||||
ros（默认，整套栈正在运行）：
|
||||
- 关节来自 ``/get_arm_state``
|
||||
- 拖拽：示教器 Free Drive + 按住使能键
|
||||
需保持：bash ~/ros2/scripts/run_real.sh
|
||||
|
||||
rdk（软件 Free Drive）：
|
||||
- 先停掉 run_real.sh（RDK 独占）
|
||||
- 按 'd' 通过 RDK 启用 FloatingJoint 拖拽
|
||||
- 关节来自 RDK 的 robot.states
|
||||
|
||||
source ~/ros2/scripts/env.sh
|
||||
ros2 run arm_application handeye_teach --ros-args -p backend:=rdk
|
||||
|
||||
界面按键（预览窗口）
|
||||
Enter / Space 拍照 + 记录关节
|
||||
d 启用 Free Drive（rdk）/ 显示示教器提示（ros）
|
||||
s 停止 Free Drive（rdk）/ 保持
|
||||
q / Esc 结束
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
HELP = """
|
||||
================================================
|
||||
手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
|
||||
================================================
|
||||
窗口按键：
|
||||
Enter / Space = 拍照 + 保存关节
|
||||
d = 开启 Free Drive
|
||||
s = 关闭 Free Drive / 保持
|
||||
q / Esc = 结束
|
||||
提示：让棋盘清晰，并占到画面的约 30-70%
|
||||
建议采 8-12 个角度有变化的位姿
|
||||
================================================
|
||||
"""
|
||||
|
||||
|
||||
def _setup_matplotlib_font():
|
||||
"""避免中文乱码：优先选用存在的中日韩（CJK）字体。"""
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use('QtAgg')
|
||||
from matplotlib import font_manager, rcParams
|
||||
|
||||
candidates = [
|
||||
'Noto Sans CJK SC',
|
||||
'Noto Sans CJK JP',
|
||||
'Noto Serif CJK SC',
|
||||
'AR PL UMing CN',
|
||||
'AR PL UKai CN',
|
||||
'WenQuanYi Micro Hei',
|
||||
'Source Han Sans CN',
|
||||
'DejaVu Sans',
|
||||
]
|
||||
available = {f.name for f in font_manager.fontManager.ttflist}
|
||||
chosen = None
|
||||
for name in candidates:
|
||||
if name in available:
|
||||
chosen = name
|
||||
break
|
||||
if chosen is None:
|
||||
# 按文件路径再试一次
|
||||
for path in (
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc',
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc',
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-DemiLight.ttc',
|
||||
'/usr/share/fonts/truetype/arphic/uming.ttc',
|
||||
):
|
||||
if os.path.isfile(path):
|
||||
font_manager.fontManager.addfont(path)
|
||||
prop = font_manager.FontProperties(fname=path)
|
||||
chosen = prop.get_name()
|
||||
break
|
||||
if chosen:
|
||||
rcParams['font.family'] = chosen
|
||||
rcParams['axes.unicode_minus'] = False
|
||||
return chosen
|
||||
|
||||
|
||||
class RdkBridge:
|
||||
"""通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
|
||||
|
||||
def __init__(self, robot_sn: str, logger=None) -> None:
|
||||
import json
|
||||
import subprocess
|
||||
|
||||
self._json = json
|
||||
self._logger = logger
|
||||
self.robot_sn = robot_sn
|
||||
self._last_q: Optional[List[float]] = None
|
||||
self._io = threading.Lock()
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
py = os.environ.get('RDK_PYTHON', '').strip() or str(
|
||||
Path.home() / 'rdk_py310' / 'bin' / 'python'
|
||||
)
|
||||
bridge = ros2_root() / 'scripts' / 'rdk_drag_bridge.py'
|
||||
if not bridge.is_file():
|
||||
raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
|
||||
|
||||
# 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。
|
||||
flexiv_lib = os.path.expanduser('~/flexiv_install/lib')
|
||||
env = {
|
||||
'HOME': os.environ.get('HOME', ''),
|
||||
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
|
||||
'LD_LIBRARY_PATH': flexiv_lib,
|
||||
'LANG': 'C.UTF-8',
|
||||
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
|
||||
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
|
||||
}
|
||||
# 不要继承 conda 的 PYTHONPATH。
|
||||
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
|
||||
# （stdout 永远不返回）。改成写入日志文件。
|
||||
log_path = Path('/tmp/rdk_drag_bridge.log')
|
||||
self._err_file = open(log_path, 'w', encoding='utf-8')
|
||||
self._proc = subprocess.Popen(
|
||||
[py, str(bridge)],
|
||||
stdin=subprocess.PIPE,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=self._err_file,
|
||||
text=True,
|
||||
bufsize=1,
|
||||
env=env,
|
||||
)
|
||||
if logger:
|
||||
logger.info(f'RDK 桥接已启动；日志 → {log_path}')
|
||||
pong = self._ask({'cmd': 'ping'}, timeout=8.0)
|
||||
if not pong.get('ok'):
|
||||
raise RuntimeError(
|
||||
f'RDK 桥接 ping 失败：{pong}。参见 {log_path}'
|
||||
)
|
||||
if logger:
|
||||
logger.info('RDK 桥接 ping 正常，正在连接机器人...')
|
||||
conn = self._ask(
|
||||
{
|
||||
'cmd': 'connect',
|
||||
'sn': robot_sn,
|
||||
'iface': os.environ.get('SITE_NET_IFACE', ''),
|
||||
},
|
||||
timeout=45.0,
|
||||
)
|
||||
if not conn.get('ok'):
|
||||
raise RuntimeError(
|
||||
f'RDK 连接失败：{conn.get("error")}。'
|
||||
f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}'
|
||||
)
|
||||
if logger:
|
||||
logger.info(f'RDK 桥接就绪（py={pong.get("py")}）sn={robot_sn}')
|
||||
q0 = conn.get('q') or []
|
||||
if len(q0) >= 7:
|
||||
self._last_q = [float(v) for v in q0[:7]]
|
||||
|
||||
def _ask(self, msg: dict, timeout: float = 10.0) -> dict:
|
||||
import select
|
||||
|
||||
# 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。
|
||||
if not self._io.acquire(timeout=max(timeout, 0.2)):
|
||||
return {'ok': False, 'error': '桥接忙'}
|
||||
try:
|
||||
assert self._proc.stdin and self._proc.stdout
|
||||
self._proc.stdin.write(self._json.dumps(msg) + '\n')
|
||||
self._proc.stdin.flush()
|
||||
end = time.monotonic() + timeout
|
||||
while time.monotonic() < end:
|
||||
if self._proc.poll() is not None:
|
||||
tip = '/tmp/rdk_drag_bridge.log'
|
||||
return {
|
||||
'ok': False,
|
||||
'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}',
|
||||
}
|
||||
ready, _, _ = select.select([self._proc.stdout], [], [], 0.05)
|
||||
if not ready:
|
||||
continue
|
||||
line = self._proc.stdout.readline()
|
||||
if not line:
|
||||
continue
|
||||
line = line.strip()
|
||||
if not line.startswith('{'):
|
||||
continue
|
||||
try:
|
||||
return self._json.loads(line)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return {'ok': False, 'error': f'应答异常：{exc} {line!r}'}
|
||||
return {'ok': False, 'error': '桥接超时'}
|
||||
finally:
|
||||
self._io.release()
|
||||
|
||||
def drag_on(self, primitive: str = 'FloatingJoint', mode: str = 'soft') -> dict:
|
||||
return self._ask(
|
||||
{'cmd': 'drag_on', 'primitive': primitive, 'mode': mode},
|
||||
timeout=20.0,
|
||||
)
|
||||
|
||||
def drag_off(self) -> dict:
|
||||
return self._ask({'cmd': 'drag_off'}, timeout=6.0)
|
||||
|
||||
def joints(self) -> Optional[List[float]]:
|
||||
res = self._ask({'cmd': 'joints'}, timeout=2.0)
|
||||
if res.get('ok'):
|
||||
q = res.get('q') or []
|
||||
if len(q) >= 7:
|
||||
self._last_q = [float(v) for v in q[:7]]
|
||||
return self._last_q
|
||||
return self._last_q
|
||||
|
||||
def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
|
||||
"""按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
|
||||
return self._ask(
|
||||
{'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
|
||||
timeout=180.0,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
try:
|
||||
self._ask({'cmd': 'quit'}, timeout=3.0)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self._proc.terminate()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if getattr(self, '_err_file', None) is not None:
|
||||
self._err_file.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class HandeyeTeach(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_teach')
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
|
||||
self.declare_parameter('width', 1280)
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
self.declare_parameter('backend', 'ros') # ros | rdk
|
||||
self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
|
||||
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('auto_exposure', True)
|
||||
self.declare_parameter('ae_limit_us', 1800.0)
|
||||
self.declare_parameter('emitter_enabled', False)
|
||||
self.declare_parameter('exposure_us', 0.0)
|
||||
self.declare_parameter('gain', 0.0)
|
||||
# 目标采集点数。0 = 不限（按 q 结束）。
|
||||
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
|
||||
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
|
||||
self.declare_parameter('target_points', 0)
|
||||
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
|
||||
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
|
||||
self.declare_parameter('poses_file', '')
|
||||
|
||||
self.backend = str(self.get_parameter('backend').value).strip().lower()
|
||||
self.robot_sn = str(self.get_parameter('robot_sn').value)
|
||||
self.drag_primitive = str(self.get_parameter('drag_primitive').value).strip()
|
||||
|
||||
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.images_dir = self.output_dir / 'images'
|
||||
_pf = str(self.get_parameter('poses_file').value).strip()
|
||||
self.poses_file = (
|
||||
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
|
||||
)
|
||||
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
|
||||
self._last_capture_t = 0.0
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self._captures: List[dict] = []
|
||||
self._latest_bgr: Optional[np.ndarray] = None
|
||||
self._frame_lock = threading.Lock()
|
||||
self._quit = False
|
||||
self._busy = False
|
||||
self._cmd_pending = False
|
||||
self._drag_on = False
|
||||
self._robot = None
|
||||
self._rdk = None
|
||||
self._get_state = None
|
||||
self._cam_thread: Optional[threading.Thread] = None
|
||||
self._pipe = None
|
||||
self._fps = 15
|
||||
|
||||
if self.backend == 'rdk':
|
||||
self._init_rdk()
|
||||
else:
|
||||
self._init_ros_state()
|
||||
|
||||
self._start_camera()
|
||||
font = _setup_matplotlib_font()
|
||||
self._start_gui()
|
||||
print(HELP)
|
||||
if self.backend == 'rdk':
|
||||
print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）')
|
||||
print(' run_real.sh 必须已停止。')
|
||||
print(' 拖着就能按 Enter 拍照，不必先按 s')
|
||||
else:
|
||||
print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。')
|
||||
print(' 或停掉 run_real.sh 后改用：-p backend:=rdk')
|
||||
if font:
|
||||
self.get_logger().info(f'界面字体：{font}')
|
||||
self.create_timer(1.0 / max(1, self._fps), self._tick)
|
||||
|
||||
def _init_ros_state(self) -> None:
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
if not self._get_state.wait_for_service(timeout_sec=10.0):
|
||||
raise RuntimeError(
|
||||
'get_arm_state 不可用。请启动 run_real.sh，'
|
||||
'或改用：-p backend:=rdk（需先停掉整套栈）'
|
||||
)
|
||||
|
||||
def _init_rdk(self) -> None:
|
||||
self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...')
|
||||
self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
|
||||
self._robot = True # 标记：RDK 后端已就绪
|
||||
self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。')
|
||||
|
||||
def _start_camera(self) -> None:
|
||||
import pyrealsense2 as rs
|
||||
|
||||
w = int(self.get_parameter('width').value)
|
||||
h = int(self.get_parameter('height').value)
|
||||
fps = int(self.get_parameter('fps').value)
|
||||
pipe = rs.pipeline()
|
||||
profile = None
|
||||
last_err = None
|
||||
for ww, hh, ff in [
|
||||
(w, h, fps), (w, h, 15), (w, h, 10),
|
||||
(640, 480, 30), (640, 480, 15),
|
||||
]:
|
||||
try:
|
||||
cfg = rs.config()
|
||||
if self._serial:
|
||||
cfg.enable_device(self._serial)
|
||||
cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff)
|
||||
profile = pipe.start(cfg)
|
||||
self._fps = ff
|
||||
self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz')
|
||||
break
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last_err = exc
|
||||
try:
|
||||
pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
pipe = rs.pipeline()
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）'
|
||||
)
|
||||
self._pipe = pipe
|
||||
try:
|
||||
from arm_application.handeye_realsense import apply_realsense_exposure
|
||||
|
||||
apply_realsense_exposure(
|
||||
profile.get_device(),
|
||||
auto_exposure=bool(self.get_parameter('auto_exposure').value),
|
||||
ae_limit_us=float(self.get_parameter('ae_limit_us').value),
|
||||
emitter_enabled=bool(self.get_parameter('emitter_enabled').value),
|
||||
exposure_us=(
|
||||
float(self.get_parameter('exposure_us').value) or None
|
||||
),
|
||||
gain=(float(self.get_parameter('gain').value) or None),
|
||||
log=self.get_logger().info,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'曝光设置应用失败：{exc}')
|
||||
self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True)
|
||||
self._cam_thread.start()
|
||||
|
||||
def _cam_loop(self) -> None:
|
||||
while not self._quit:
|
||||
try:
|
||||
frames = self._pipe.wait_for_frames(timeout_ms=400)
|
||||
color = frames.get_color_frame()
|
||||
except Exception: # noqa: BLE001
|
||||
time.sleep(0.02)
|
||||
continue
|
||||
if not color:
|
||||
continue
|
||||
bgr = np.asanyarray(color.get_data()).copy()
|
||||
with self._frame_lock:
|
||||
self._latest_bgr = bgr
|
||||
|
||||
def _start_gui(self) -> None:
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
self._plt = plt
|
||||
self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7))
|
||||
self._fig.canvas.manager.set_window_title(
|
||||
'handeye_teach | Enter=拍照 d=拖拽 s=停止 q=退出'
|
||||
)
|
||||
self._im = None
|
||||
self._status = self._ax.text(
|
||||
0.02,
|
||||
0.98,
|
||||
self._status_text(),
|
||||
transform=self._ax.transAxes,
|
||||
color='lime',
|
||||
fontsize=13,
|
||||
va='top',
|
||||
fontweight='bold',
|
||||
bbox=dict(boxstyle='round', facecolor='black', alpha=0.6),
|
||||
)
|
||||
self._ax.axis('off')
|
||||
self._fig.canvas.mpl_connect('key_press_event', self._on_key)
|
||||
plt.ion()
|
||||
plt.show(block=False)
|
||||
|
||||
def _status_text(self) -> str:
|
||||
drag = '拖拽开启' if self._drag_on else '拖拽关闭'
|
||||
if self.backend == 'rdk':
|
||||
tip = 'd=软拖拽 s=保持'
|
||||
else:
|
||||
tip = '示教器 FreeDrive+使能'
|
||||
return (
|
||||
f'已采集:{len(self._captures)} [{drag}] '
|
||||
f'{tip} | Enter=拍照 q=退出'
|
||||
)
|
||||
|
||||
def _on_key(self, event) -> None:
|
||||
key = (event.key or '').lower()
|
||||
if key in ('q', 'escape'):
|
||||
self._quit = True
|
||||
self.get_logger().info('收到退出请求')
|
||||
return
|
||||
if self._cmd_pending:
|
||||
return
|
||||
if key in ('enter', ' ', 'space', 'd', 's'):
|
||||
self._cmd_pending = True
|
||||
threading.Thread(target=self._run_key, args=(key,), daemon=True).start()
|
||||
|
||||
def _run_key(self, key: str) -> None:
|
||||
try:
|
||||
if key in ('enter', ' ', 'space'):
|
||||
self._capture_once()
|
||||
elif key == 'd':
|
||||
self._enable_drag()
|
||||
elif key == 's':
|
||||
self._disable_drag()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'按键 {key}：{exc}')
|
||||
finally:
|
||||
self._cmd_pending = False
|
||||
|
||||
def _enable_drag(self) -> None:
|
||||
if self.backend != 'rdk':
|
||||
self._drag_on = True
|
||||
self.get_logger().warn(
|
||||
'ROS 后端：请用示教器的 Free Drive + 按住使能键。'
|
||||
'想用软件拖拽：先停掉 run_real.sh，然后执行 '
|
||||
'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk'
|
||||
)
|
||||
self._status.set_text(self._status_text())
|
||||
return
|
||||
self.get_logger().info('正在启动软拖拽（预览保持实时刷新）...')
|
||||
# soft = AUTO Remote 模式下的关节阻抗拖拽（不需要 Floating 许可）
|
||||
res = self._rdk.drag_on(self.drag_primitive, mode='soft')
|
||||
if res.get('ok'):
|
||||
self._drag_on = True
|
||||
self.get_logger().info(
|
||||
f'拖拽已开启 mode={res.get("mode")} —— 推着机械臂走，Enter=拍照（s=保持，可选）'
|
||||
)
|
||||
if res.get('hint'):
|
||||
self.get_logger().info(str(res.get('hint')))
|
||||
else:
|
||||
self._drag_on = False
|
||||
self.get_logger().error(f'拖拽失败：{res.get("error")}')
|
||||
# 状态文字由 GUI 线程里的 _tick 刷新
|
||||
|
||||
def _disable_drag(self) -> None:
|
||||
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
|
||||
res = self._rdk.drag_off()
|
||||
if not res.get('ok'):
|
||||
self.get_logger().warn(f'停止拖拽：{res.get("error")}')
|
||||
self._drag_on = False
|
||||
self.get_logger().info('Free Drive 已关闭 / 保持')
|
||||
|
||||
def _tick(self) -> None:
|
||||
if self._quit:
|
||||
return
|
||||
with self._frame_lock:
|
||||
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
|
||||
if bgr is None:
|
||||
try:
|
||||
self._fig.canvas.flush_events()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
return
|
||||
rgb = bgr[:, :, ::-1]
|
||||
if self._im is None:
|
||||
self._im = self._ax.imshow(rgb)
|
||||
else:
|
||||
self._im.set_data(rgb)
|
||||
try:
|
||||
self._status.set_text(self._status_text())
|
||||
self._fig.canvas.draw_idle()
|
||||
self._fig.canvas.flush_events()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _read_joints(self) -> Optional[List[float]]:
|
||||
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
|
||||
q = self._rdk.joints()
|
||||
if q is None:
|
||||
self.get_logger().error('读取 RDK 关节失败')
|
||||
return q
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
end = time.monotonic() + 2.0
|
||||
while rclpy.ok() and not fut.done() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.02)
|
||||
res = fut.result() if fut.done() else None
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
q = list(res.joint_positions)
|
||||
return [float(v) for v in q[:7]] if len(q) >= 7 else None
|
||||
|
||||
def _capture_once(self) -> None:
|
||||
if self._busy:
|
||||
return
|
||||
self._busy = True
|
||||
try:
|
||||
with self._frame_lock:
|
||||
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
|
||||
if bgr is None:
|
||||
self.get_logger().warn('还没有相机帧')
|
||||
return
|
||||
q = self._read_joints()
|
||||
if q is None:
|
||||
self.get_logger().error('读取关节失败')
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self._last_capture_t < 0.8:
|
||||
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
|
||||
return
|
||||
if self._captures:
|
||||
last = self._captures[-1]['joints']
|
||||
delta = max(abs(a - b) for a, b in zip(q, last))
|
||||
if delta < 0.015:
|
||||
self.get_logger().warn(
|
||||
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
|
||||
'先拖到另一个姿态再按 Enter。'
|
||||
)
|
||||
return
|
||||
self._last_capture_t = now
|
||||
|
||||
idx = len(self._captures)
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
img_path = self.images_dir / f'teach_{idx:02d}_{stamp}.png'
|
||||
try:
|
||||
import cv2
|
||||
|
||||
cv2.imwrite(str(img_path), bgr)
|
||||
except Exception: # noqa: BLE001
|
||||
from PIL import Image as PILImage
|
||||
|
||||
PILImage.fromarray(bgr[:, :, ::-1]).save(str(img_path))
|
||||
|
||||
line = ', '.join(f'{v:.6f}' for v in q) + ','
|
||||
self._captures.append(
|
||||
{'index': idx, 'joints': q, 'image': str(img_path), 'line': line}
|
||||
)
|
||||
with self.poses_file.open('a', encoding='utf-8') as f:
|
||||
f.write(f'# pose {idx} {stamp}\n')
|
||||
f.write(line + '\n')
|
||||
f.write(f'# image {img_path}\n')
|
||||
print(f'\n# pose {idx} {img_path.name}')
|
||||
print(line)
|
||||
n = idx + 1
|
||||
try:
|
||||
target = int(self.get_parameter('target_points').value)
|
||||
except Exception: # noqa: BLE001
|
||||
target = 0
|
||||
if target > 0:
|
||||
if n < target:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已保存照片+关节 —— 还差 {target - n} 个点'
|
||||
)
|
||||
elif n == target:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已采够 {target} 个点。'
|
||||
' 建议按 q 结束，然后跑 merge + calibrate；'
|
||||
' 想多采几个点也可以继续按 Enter。'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已保存照片+关节 —— 已超出目标 '
|
||||
f'{n - target} 个点（会用全部点求解）'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(f'[{n}] 已保存照片+关节')
|
||||
self._write_yaml_snippet()
|
||||
finally:
|
||||
self._busy = False
|
||||
|
||||
def _write_yaml_snippet(self) -> None:
|
||||
lines = [
|
||||
'# 粘贴到 handeye_calibrate.yaml -> waypoint_positions',
|
||||
f'# num_poses: {len(self._captures)}',
|
||||
'waypoint_positions: [',
|
||||
]
|
||||
for c in self._captures:
|
||||
lines.append(f' # pose {c["index"]}')
|
||||
lines.append(f' {c["line"]}')
|
||||
lines.append(']')
|
||||
self.yaml_file.write_text('\n'.join(lines) + '\n', encoding='utf-8')
|
||||
|
||||
def finish(self) -> None:
|
||||
self._quit = True
|
||||
if self._cam_thread is not None:
|
||||
self._cam_thread.join(timeout=1.5)
|
||||
if self._drag_on:
|
||||
self._disable_drag()
|
||||
if getattr(self, '_rdk', None) is not None:
|
||||
try:
|
||||
self._rdk.close()
|
||||
except Exception:
|
||||
pass
|
||||
n = len(self._captures)
|
||||
print('\n========== 示教完成 ==========')
|
||||
print(f'位姿: {n}')
|
||||
print(f'关节: {self.poses_file}')
|
||||
print(f'yaml: {self.yaml_file}')
|
||||
print(f'图像: {self.images_dir}')
|
||||
if n >= 1:
|
||||
print('\n--- 粘贴到 handeye_calibrate.yaml ---\n')
|
||||
print(self.yaml_file.read_text(encoding='utf-8'))
|
||||
print('同时设置 num_poses:', n)
|
||||
if n < 5:
|
||||
self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = HandeyeTeach()
|
||||
while rclpy.ok() and not node._quit:
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
except SystemExit:
|
||||
pass
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.finish()
|
||||
try:
|
||||
node._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,525 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼标定路点执行器。
|
||||
|
||||
逐个移动到每个关节路点，等到位后停留一段时间，
|
||||
让相机（或操作者）采集一张标定图像。
|
||||
|
||||
capture_mode:
|
||||
auto — 停留 dwell_sec 后进入下一个位姿
|
||||
keypress — 在终端按 Enter 继续
|
||||
preview — OpenCV 实时相机 + Enter/Space 确认（示教时推荐）
|
||||
|
||||
处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive**
|
||||
拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import array
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import String
|
||||
|
||||
from arm_application.handeye_realsense import (
|
||||
is_realsense_source,
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
cv2 = None # type: ignore
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
|
||||
DEFAULT_TIMES = [4.0] * 12
|
||||
DEFAULT_POSITIONS = [
|
||||
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
-0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
0.00, -0.90, 0.00, 1.70, 0.00, 0.85, 0.00,
|
||||
0.00, -0.50, 0.00, 1.40, 0.00, 0.55, 0.00,
|
||||
0.20, -0.70, 0.15, 1.57, 0.40, 0.70, 0.30,
|
||||
-0.20, -0.70, -0.15, 1.57, -0.40, 0.70, -0.30,
|
||||
0.25, -0.80, 0.10, 1.65, 0.25, 0.90, -0.40,
|
||||
-0.25, -0.80, -0.10, 1.65, -0.25, 0.90, 0.40,
|
||||
0.15, -0.60, 0.20, 1.50, -0.35, 0.60, 0.50,
|
||||
-0.15, -0.60, -0.20, 1.50, 0.35, 0.60, -0.50,
|
||||
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
]
|
||||
|
||||
PREVIEW_HELP_GUI = """
|
||||
=== 位姿预览 / 拖拽调整（OpenCV 窗口）===
|
||||
1. 示教器：Free Drive + 按住使能键拖拽微调
|
||||
2. 预览窗口：确认棋盘在画面内且清晰
|
||||
3. 键盘：Enter / Space = 下一个位姿；q / Esc = 退出
|
||||
"""
|
||||
|
||||
PREVIEW_HELP_HEADLESS = """
|
||||
=== 位姿预览 / 拖拽调整（无 OpenCV 窗口）===
|
||||
1. 另开终端查看画面：
|
||||
ros2 run rqt_image_view rqt_image_view /handeye/preview_image
|
||||
2. 示教器：Free Drive + 按住使能键拖拽微调
|
||||
3. 本终端：Enter = 确认下一个位姿；输入 q 回车 = 退出
|
||||
"""
|
||||
|
||||
|
||||
def _cv_gui_available() -> bool:
|
||||
if cv2 is None:
|
||||
return False
|
||||
try:
|
||||
cv2.namedWindow('_handeye_gui_probe', cv2.WINDOW_NORMAL)
|
||||
cv2.destroyWindow('_handeye_gui_probe')
|
||||
return True
|
||||
except Exception: # noqa: BLE001
|
||||
return False
|
||||
|
||||
|
||||
def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or '').lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
if enc in ('bgr8', 'rgb8'):
|
||||
img = data.reshape(h, w, 3)
|
||||
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
|
||||
if enc in ('mono8', '8uc1'):
|
||||
gray = data.reshape(h, w)
|
||||
return np.stack([gray, gray, gray], axis=-1)
|
||||
if enc in ('bgra8', 'rgba8'):
|
||||
img = data.reshape(h, w, 4)
|
||||
bgr = img[:, :, :3]
|
||||
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
class HandeyeWaypoints(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_waypoints')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.12)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
|
||||
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
|
||||
self.declare_parameter('dwell_sec', 2.5)
|
||||
# auto | keypress | preview
|
||||
self.declare_parameter('capture_mode', 'auto')
|
||||
self.declare_parameter('start_index', 0)
|
||||
self.declare_parameter('log_csv', '')
|
||||
self.declare_parameter('capture_topic', 'handeye/capture')
|
||||
self.declare_parameter('camera_source', 'd405')
|
||||
self.declare_parameter('camera_namespace', '')
|
||||
self.declare_parameter('image_topic', '')
|
||||
self.declare_parameter('preview_window', True)
|
||||
self.declare_parameter('append_poses_file', '/tmp/handeye_taught_poses.txt')
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.action_name = str(self.get_parameter('action_name').value)
|
||||
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
|
||||
self.capture_mode = str(self.get_parameter('capture_mode').value).strip().lower()
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.log_csv = str(self.get_parameter('log_csv').value or '').strip()
|
||||
topic = str(self.get_parameter('capture_topic').value)
|
||||
self.preview_window = bool(self.get_parameter('preview_window').value)
|
||||
self.append_poses_file = str(
|
||||
self.get_parameter('append_poses_file').value or ''
|
||||
).strip()
|
||||
|
||||
cam_src = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
img_topic = str(self.get_parameter('image_topic').value or '').strip()
|
||||
ns = str(self.get_parameter('camera_namespace').value or '').strip()
|
||||
if is_realsense_source(cam_src) and not img_topic:
|
||||
img_topic, _ = resolve_realsense_topics(
|
||||
cam_src, camera_namespace=ns, image_topic='', camera_info_topic=''
|
||||
)
|
||||
self.image_topic = img_topic
|
||||
|
||||
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
self._capture_pub = self.create_publisher(String, topic, 10)
|
||||
self._preview_pub = self.create_publisher(Image, 'handeye/preview_image', 10)
|
||||
self._latest_bgr: Optional[np.ndarray] = None
|
||||
self._cv_gui = _cv_gui_available() and self.preview_window
|
||||
if self.capture_mode == 'preview' and not self._cv_gui:
|
||||
self.get_logger().warn(
|
||||
'OpenCV 没有 GUI（无头环境）。请在 /handeye/preview_image 上预览，'
|
||||
'在本终端按 Enter 确认。'
|
||||
)
|
||||
if self.image_topic and self.capture_mode == 'preview':
|
||||
self.create_subscription(
|
||||
Image, self.image_topic, self._on_image, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'预览图像：{self.image_topic}')
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
try:
|
||||
self._latest_bgr = _image_to_bgr(msg)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def run(self) -> int:
|
||||
if self.capture_mode == 'preview':
|
||||
if cv2 is None:
|
||||
self.get_logger().error('preview 模式需要 OpenCV（cv2）')
|
||||
return 1
|
||||
if not self.image_topic:
|
||||
self.get_logger().error('preview 模式需要 image_topic 或 camera_source')
|
||||
return 1
|
||||
|
||||
times = list(self.get_parameter('waypoint_times').value)
|
||||
positions = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_pts = len(times)
|
||||
if n_pts < 1:
|
||||
self.get_logger().error('waypoint_times 为空')
|
||||
return 1
|
||||
if len(positions) != n_pts * n_j:
|
||||
self.get_logger().error(
|
||||
f'waypoint_positions 长度 {len(positions)} != {n_pts}*{n_j}'
|
||||
)
|
||||
return 1
|
||||
if self.start_index >= n_pts:
|
||||
self.get_logger().error(
|
||||
f'start_index {self.start_index} >= 路点总数 {n_pts}'
|
||||
)
|
||||
return 1
|
||||
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self.action_name}] 不可用。'
|
||||
'请先启动整套栈：bash scripts/run_real.sh'
|
||||
)
|
||||
return 2
|
||||
|
||||
csv_file = None
|
||||
csv_writer: Optional[csv.writer] = None
|
||||
if self.log_csv:
|
||||
path = Path(self.log_csv).expanduser()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
csv_file = path.open('w', newline='', encoding='utf-8')
|
||||
csv_writer = csv.writer(csv_file)
|
||||
csv_writer.writerow(
|
||||
['index', 'utc_iso', 'move_duration_s']
|
||||
+ [f'target_{n}' for n in self.joint_names]
|
||||
+ [f'measured_{n}' for n in self.joint_names]
|
||||
)
|
||||
self.get_logger().info(f'正在记录采集数据 → {path}')
|
||||
|
||||
if self.capture_mode == 'preview':
|
||||
print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS)
|
||||
|
||||
self.get_logger().info(
|
||||
f'手眼标定：{n_pts} 个位姿，start={self.start_index}，'
|
||||
f'v_scale={self.velocity_scale}，dwell={self.dwell_sec}s，'
|
||||
f'mode={self.capture_mode}'
|
||||
)
|
||||
self.get_logger().warn(
|
||||
'清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。'
|
||||
)
|
||||
|
||||
win = 'handeye_preview'
|
||||
try:
|
||||
for i in range(self.start_index, n_pts):
|
||||
q = positions[i * n_j : (i + 1) * n_j]
|
||||
move_t = max(1.0, float(times[i]))
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...'
|
||||
)
|
||||
code = self._move_one(q, move_t)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
measured = self._read_joints()
|
||||
self._announce_capture(i, n_pts, q, measured)
|
||||
|
||||
if csv_writer is not None and csv_file is not None:
|
||||
csv_writer.writerow(
|
||||
[i, datetime.now(timezone.utc).isoformat(), move_t]
|
||||
+ list(q)
|
||||
+ list(measured if measured else [float('nan')] * n_j)
|
||||
)
|
||||
csv_file.flush()
|
||||
|
||||
if not self._wait_capture(i, n_pts):
|
||||
self.get_logger().info('用户已中止')
|
||||
return 0
|
||||
|
||||
# 在 preview 模式下确认后，保存示教关节
|
||||
if self.capture_mode == 'preview':
|
||||
taught = self._read_joints()
|
||||
if taught:
|
||||
self._save_taught_pose(i, taught)
|
||||
finally:
|
||||
if csv_file is not None:
|
||||
csv_file.close()
|
||||
if cv2 is not None:
|
||||
try:
|
||||
cv2.destroyWindow(win)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
self.get_logger().info('手眼标定路点序列执行完毕')
|
||||
if self.append_poses_file and self.capture_mode == 'preview':
|
||||
self.get_logger().info(
|
||||
f'示教位姿已保存 → {self.append_poses_file}'
|
||||
)
|
||||
return 0
|
||||
|
||||
def _save_taught_pose(self, index: int, joints: List[float]) -> None:
|
||||
line = ', '.join(f'{v:.6f}' for v in joints[:7]) + ','
|
||||
print(f'# pose {index}')
|
||||
print(line)
|
||||
if not self.append_poses_file:
|
||||
return
|
||||
path = Path(self.append_poses_file).expanduser()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open('a', encoding='utf-8') as f:
|
||||
f.write(f'# pose {index}\n')
|
||||
f.write(line + '\n')
|
||||
self.get_logger().info(f'已保存位姿 {index} → {path}')
|
||||
|
||||
def _move_one(self, positions: List[float], duration: float) -> int:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = 1
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = list(positions)
|
||||
goal.time_from_start = [float(duration)]
|
||||
|
||||
send = self._client.send_goal_async(goal)
|
||||
if not self._wait_future(send, 10.0, '发送目标'):
|
||||
return 3
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 4
|
||||
|
||||
res_fut = handle.get_result_async()
|
||||
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
|
||||
return 5
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'移动失败：{result.message}')
|
||||
return 6
|
||||
return 0
|
||||
|
||||
def _wait_future(self, fut, timeout: float, label: str) -> bool:
|
||||
deadline = time.monotonic() + timeout
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'等待 {label} 超时')
|
||||
return False
|
||||
return fut.done()
|
||||
|
||||
def _read_joints(self) -> Optional[List[float]]:
|
||||
if not self._get_state.wait_for_service(timeout_sec=0.5):
|
||||
return None
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
if not self._wait_future(fut, 2.0, 'get_arm_state'):
|
||||
return None
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
return list(res.joint_positions)
|
||||
|
||||
def _announce_capture(
|
||||
self,
|
||||
index: int,
|
||||
n_pts: int,
|
||||
target: List[float],
|
||||
measured: Optional[List[float]],
|
||||
) -> None:
|
||||
msg = String()
|
||||
msg.data = f'capture:{index}:{n_pts}'
|
||||
self._capture_pub.publish(msg)
|
||||
q_str = ', '.join(f'{v:.4f}' for v in (measured or target))
|
||||
if self.capture_mode == 'preview':
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 已到位 —— 现在采集 q=[{q_str}]'
|
||||
)
|
||||
|
||||
def _wait_capture(self, index: int, n_pts: int) -> bool:
|
||||
if self.capture_mode == 'preview':
|
||||
return self._wait_preview(index, n_pts)
|
||||
|
||||
if self.capture_mode in ('key', 'keypress', 'manual'):
|
||||
self.get_logger().info(
|
||||
'采集完成后按 Enter（q+Enter 退出）...'
|
||||
)
|
||||
for _ in range(5):
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
line = sys.stdin.readline()
|
||||
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
|
||||
return False
|
||||
return True
|
||||
|
||||
end = time.monotonic() + max(0.0, self.dwell_sec)
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
return rclpy.ok()
|
||||
|
||||
def _annotate_preview(
|
||||
self, bgr: np.ndarray, index: int, n_pts: int
|
||||
) -> np.ndarray:
|
||||
vis = bgr.copy()
|
||||
if cv2 is None:
|
||||
return vis
|
||||
cv2.putText(
|
||||
vis,
|
||||
f'位姿 {index + 1}/{n_pts}',
|
||||
(12, 28),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.7,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
cv2.putText(
|
||||
vis,
|
||||
'Free Drive 拖拽 | Enter=下一个 q=退出',
|
||||
(12, 56),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.6,
|
||||
(0, 255, 255),
|
||||
2,
|
||||
)
|
||||
return vis
|
||||
|
||||
def _publish_preview(self, bgr: np.ndarray) -> None:
|
||||
msg = Image()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = 'camera'
|
||||
msg.height, msg.width = bgr.shape[0], bgr.shape[1]
|
||||
msg.encoding = 'bgr8'
|
||||
msg.is_bigendian = 0
|
||||
msg.step = msg.width * 3
|
||||
msg.data = array.array('B', bgr.tobytes()) # 快速路径；用 bytes 会多花约 40 ms
|
||||
self._preview_pub.publish(msg)
|
||||
|
||||
def _wait_preview(self, index: int, n_pts: int) -> bool:
|
||||
if self._cv_gui:
|
||||
return self._wait_preview_opencv(index, n_pts)
|
||||
return self._wait_preview_terminal(index, n_pts)
|
||||
|
||||
def _wait_preview_terminal(self, index: int, n_pts: int) -> bool:
|
||||
confirmed = threading.Event()
|
||||
quit_req = threading.Event()
|
||||
|
||||
def _stdin_reader() -> None:
|
||||
try:
|
||||
line = sys.stdin.readline()
|
||||
except Exception: # noqa: BLE001
|
||||
quit_req.set()
|
||||
return
|
||||
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
|
||||
quit_req.set()
|
||||
else:
|
||||
confirmed.set()
|
||||
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter '
|
||||
'（q+Enter 退出）'
|
||||
)
|
||||
reader = threading.Thread(target=_stdin_reader, daemon=True)
|
||||
reader.start()
|
||||
|
||||
no_frame_warn = False
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if confirmed.is_set():
|
||||
return True
|
||||
if quit_req.is_set():
|
||||
return False
|
||||
if not reader.is_alive() and not confirmed.is_set() and not quit_req.is_set():
|
||||
return False
|
||||
|
||||
bgr = self._latest_bgr
|
||||
if bgr is None:
|
||||
if not no_frame_warn:
|
||||
self.get_logger().warn(
|
||||
f'正在等待 {self.image_topic} ...',
|
||||
throttle_duration_sec=3.0,
|
||||
)
|
||||
no_frame_warn = True
|
||||
continue
|
||||
vis = self._annotate_preview(bgr, index, n_pts)
|
||||
self._publish_preview(vis)
|
||||
return False
|
||||
|
||||
def _wait_preview_opencv(self, index: int, n_pts: int) -> bool:
|
||||
assert cv2 is not None
|
||||
win = 'handeye_preview'
|
||||
cv2.namedWindow(win, cv2.WINDOW_NORMAL)
|
||||
|
||||
no_frame_warn = False
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(self, timeout_sec=0.02)
|
||||
bgr = self._latest_bgr
|
||||
if bgr is None:
|
||||
if not no_frame_warn:
|
||||
self.get_logger().warn(
|
||||
f'正在等待 {self.image_topic} ...',
|
||||
throttle_duration_sec=3.0,
|
||||
)
|
||||
no_frame_warn = True
|
||||
key = cv2.waitKey(30) & 0xFF
|
||||
else:
|
||||
vis = self._annotate_preview(bgr, index, n_pts)
|
||||
self._publish_preview(vis)
|
||||
cv2.imshow(win, vis)
|
||||
key = cv2.waitKey(30) & 0xFF
|
||||
|
||||
if key in (13, 10, 32):
|
||||
return True
|
||||
if key in (27, ord('q'), ord('Q')):
|
||||
return False
|
||||
return False
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HandeyeWaypoints()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Executable
+163
@@ -0,0 +1,163 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Merge taught joint lines into handeye waypoints YAML.
|
||||
|
||||
Input (from handeye_teach / handeye_waypoints append file):
|
||||
# pose 0
|
||||
0.123456, -0.700000, ...
|
||||
|
||||
Target can be either:
|
||||
- handeye/poses/<臂号>_<模式>.yaml (/** ros__parameters)
|
||||
- handeye/calibrate.yaml (handeye_calibrate: ros__parameters)
|
||||
|
||||
Usage:
|
||||
python3 scripts/merge_handeye_poses.py \\
|
||||
~/ros2/handeye/tmp/89/taught_poses.txt \\
|
||||
~/ros2/handeye/poses/89_eye_in_hand.yaml
|
||||
|
||||
# or via wrapper:
|
||||
bash ~/ros2/scripts/run_handeye.sh merge
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def parse_poses(path: Path) -> list[list[float]]:
|
||||
lines = path.read_text(encoding="utf-8").splitlines()
|
||||
poses: list[list[float]] = []
|
||||
buf: list[float] = []
|
||||
|
||||
num_re = re.compile(r"[-+]?(?:\d+\.\d*|\.\d+|\d+)(?:[eE][-+]?\d+)?")
|
||||
|
||||
for raw in lines:
|
||||
line = raw.split("#", 1)[0].strip()
|
||||
if not line:
|
||||
if buf:
|
||||
if len(buf) != 7:
|
||||
raise ValueError(f"Expected 7 joints, got {len(buf)}: {buf}")
|
||||
poses.append(buf)
|
||||
buf = []
|
||||
continue
|
||||
vals = [float(x) for x in num_re.findall(line)]
|
||||
if not vals:
|
||||
continue
|
||||
if len(vals) == 7:
|
||||
poses.append(vals)
|
||||
elif len(vals) < 7:
|
||||
buf.extend(vals)
|
||||
if len(buf) > 7:
|
||||
raise ValueError(f"Too many joint values in line: {line!r}")
|
||||
else:
|
||||
poses.append(vals[:7])
|
||||
|
||||
if buf:
|
||||
if len(buf) != 7:
|
||||
raise ValueError(f"Expected 7 joints at EOF, got {len(buf)}")
|
||||
poses.append(buf)
|
||||
|
||||
return poses
|
||||
|
||||
|
||||
def unique_poses(poses: list[list[float]], tol: float = 0.015) -> list[list[float]]:
|
||||
"""关节几乎不动的连拍只留一条。阈值 0.015 rad ≈ 0.86°。"""
|
||||
kept: list[list[float]] = []
|
||||
for pose in poses:
|
||||
if any(max(abs(a - b) for a, b in zip(pose, old)) < tol for old in kept):
|
||||
continue
|
||||
kept.append(pose)
|
||||
return kept
|
||||
|
||||
|
||||
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
|
||||
rows = [f"{indent}waypoint_positions: ["]
|
||||
for i, q in enumerate(poses):
|
||||
rows.append(f"{indent} # pose {i}")
|
||||
s = ", ".join(f"{v:.6f}" for v in q) + ","
|
||||
rows.append(f"{indent} {s}")
|
||||
rows.append(f"{indent}]")
|
||||
return "\n".join(rows)
|
||||
|
||||
|
||||
def merge(yaml_path: Path, poses: list[list[float]]) -> None:
|
||||
text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n")
|
||||
# 探测 waypoint_positions 的缩进（通常 2 空格或 4 空格）
|
||||
m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE)
|
||||
indent = m_ind.group(1) if m_ind else " "
|
||||
block = format_positions_block(poses, indent=indent)
|
||||
|
||||
pattern = re.compile(
|
||||
rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]",
|
||||
re.MULTILINE,
|
||||
)
|
||||
# 也要匹配"单行空列表"的写法：waypoint_positions: []
|
||||
one_line = re.compile(
|
||||
rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]",
|
||||
re.MULTILINE,
|
||||
)
|
||||
if pattern.search(text):
|
||||
new_text = pattern.sub(block, text, count=1)
|
||||
elif one_line.search(text):
|
||||
new_text = one_line.sub(block, text, count=1)
|
||||
else:
|
||||
raise RuntimeError("waypoint_positions block not found in YAML")
|
||||
|
||||
# 顺手把 num_poses 同步成实际点数（如果原本就有这个键）
|
||||
n = len(poses)
|
||||
if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE):
|
||||
new_text = re.sub(
|
||||
r"^(\s*num_poses:\s*)\d+",
|
||||
rf"\g<1>{n}",
|
||||
new_text,
|
||||
count=1,
|
||||
flags=re.MULTILINE,
|
||||
)
|
||||
else:
|
||||
# 原本没有 num_poses，就插在 waypoint_positions 前面
|
||||
new_text = new_text.replace(
|
||||
block,
|
||||
f"{indent}num_poses: {n}\n{block}",
|
||||
1,
|
||||
)
|
||||
|
||||
yaml_path.write_text(new_text, encoding="utf-8")
|
||||
|
||||
|
||||
def main(argv: list[str]) -> int:
|
||||
if len(argv) != 3:
|
||||
print(
|
||||
"用法：merge_handeye_poses.py <taught_poses.txt> <waypoints.yaml>",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
src = Path(argv[1]).expanduser()
|
||||
dst = Path(argv[2]).expanduser()
|
||||
if not src.is_file():
|
||||
print(f"Not found: {src}", file=sys.stderr)
|
||||
return 2
|
||||
if not dst.is_file():
|
||||
print(f"Not found: {dst}", file=sys.stderr)
|
||||
return 3
|
||||
poses = parse_poses(src)
|
||||
uniq = unique_poses(poses)
|
||||
if len(uniq) < 5:
|
||||
print(
|
||||
f"Need >=5 unique poses, got {len(poses)} lines / {len(uniq)} unique",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 4
|
||||
if len(uniq) < len(poses):
|
||||
print(
|
||||
f"警告: {len(poses)} 条里只有 {len(uniq)} 个姿态不同，已丢掉连按重复",
|
||||
file=sys.stderr,
|
||||
)
|
||||
poses = uniq
|
||||
merge(dst, poses)
|
||||
print(f"Merged {len(poses)} poses -> {dst}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main(sys.argv))
|
||||
@@ -0,0 +1,25 @@
|
||||
# 手眼标定交互式路点巡回（预览 + 自由驱动确认）。
|
||||
# 位姿来源：同目录的 handeye_poses.yaml
|
||||
#
|
||||
# 运行：
|
||||
# bash ~/ros2/scripts/run_handeye.sh waypoints
|
||||
|
||||
handeye_waypoints:
|
||||
ros__parameters:
|
||||
joint_prefix: "Rizon4-063989_"
|
||||
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
|
||||
velocity_scale: 0.05
|
||||
action_name: "follow_joint_trajectory"
|
||||
dwell_sec: 2.5
|
||||
capture_mode: "preview" # auto | keypress | preview
|
||||
start_index: 0
|
||||
log_csv: ""
|
||||
capture_topic: "handeye/capture"
|
||||
|
||||
camera_source: "d405"
|
||||
camera_namespace: ""
|
||||
image_topic: ""
|
||||
preview_window: true
|
||||
append_poses_file: "handeye/tmp/taught_poses.txt"
|
||||
|
||||
waypoint_times: [4.0, 4.0, 4.0, 4.0, 4.0, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.0, 4.0]
|
||||
Reference in New Issue
Block a user